#ifndef EVALWIFI_H
#define EVALWIFI_H

#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"
#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
#include "Indoor/sensors/radio/model/WiFiModels.h"

#include "Indoor/sensors/radio/VAPGrouper.h"
#include "Indoor/sensors/offline/FileReader.h"

#include "Indoor/floorplan/v2/Floorplan.h"
#include "Indoor/floorplan/v2/FloorplanReader.h"
#include "Indoor/floorplan/v2/FloorplanHelper.h"
#include "Indoor/floorplan/v2/FloorplanCeilings.h"

#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>

#include <KLib/math/statistics/Statistics.h>
#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/MovingMedian.h>

#include "../Structs.h"
#include "../plots/Plotty.h"
#include "../plots/PlotErrTime.h"
#include "../plots/PlotErrFunc.h"
#include "../plots/PlotWiFiGroundProb.h"
//#include "CSV.h"

#include <unordered_set>
#include <thread>
#include "../Settings.h"

template <typename T> class Line {

private:

	std::vector<T> elements;

public:

	void add(const T& elem) {
		elements.push_back(elem);
	}

	std::vector<T> getAverage(const int size) {
		std::vector<T> res;
		for (int i = 0; i < (int)elements.size(); ++i) {

			T sum;
			int cnt = 0;

			// calculate sume of all elements around i
			for (int j = -size; j <= +size; ++j) {
				int idx = i+j;
				if (idx < 0) {continue;}
				if (idx >= elements.size()) {continue;}
				sum += elements[idx];
				++cnt;
			}

			// calculate average
			T avg = sum / cnt;
			res.push_back(avg);

		}
		return res;
	}

};

/**
 * read path
 * fetch wifi
 * use given model to estimate the most likely location
 * -> WIFI ONLY
 */
class EvalWiFi {

private:

	Floorplan::IndoorMap* map;
	BBox3 mapBBox;

	//WiFiFingerprints* calib;
	VAPGrouper* vap = nullptr;
	//WiFiOptimizer::LogDistCeiling* opt;

	Offline::FileReader reader;
	WiFiModel* wiModel = nullptr;
	std::vector<int> gtIndices;

	// error in meter
	PlotErrFunc* pef;
	PlotErrTime* pet;
	MovingAVG<float> mavgMeter = MovingAVG<float>(10);

	// error in probability
	PlotErrFunc* pef2;
	PlotErrTime* pet2;

	Plotty* plot;

	//PlotWiFiGroundProb* groundProb;

public:

	/** ctor with map and fingerprints */
	EvalWiFi(const std::string& mapFile, const std::string& fPath, const std::vector<int> gtIndices) : reader(fPath), gtIndices(gtIndices) {

		std::cout << "EvalWiFi for " << fPath << std::endl;

		// load floorplan
		map = Floorplan::Reader::readFromFile(mapFile);

		// estimate bbox
		mapBBox = FloorplanHelper::getBBox(map);

//		// how to handle VAPs
//		vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

		// the optimizer
	//	opt = new WiFiOptimizer::LogDistCeiling(map, *vap, *calib, WiFiOptimizer::LogDistCeiling::Mode::MEDIUM);

		// how to handle VAPs
		vap = new VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

		pef = new PlotErrFunc("\\small{error (m)}", "\\small{measurements (\\%)}");
		pef->showMarkers(false, false);

		pef2 = new PlotErrFunc("\\small{-log(p(..))}", "\\small{measurements (\\%)}");
		pef2->showMarkers(false, false);

		pet = new PlotErrTime("walktime (seconds)", "\\small{error (m)}", "");
		pet->getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0, 25));
		pet->getPlot().getKey().setVisible(true);

		pet2 = new PlotErrTime("walktime (seconds)", "\\small{-log(p(groundTruth | measurements))", "");

		plot = new Plotty(map);
		plot->buildFloorplan();
		plot->setGroundTruth(gtIndices);

	}

	void load(const std::string& xmlFile, const std::string& name) {

		WiFiModelFactory fac(map);

		// setup the model
		this->wiModel = fac.loadXML(xmlFile);

		// fire
		build(name);


	}

	void writeTeX(const std::string& name) {

		pet->writeEpsTex(Settings::fPathGFX + "/wifi_eval_" + name + ".tex", K::GnuplotSize(8.6, 3.8));
		pet->writeCodeTo(Settings::fPathGFX + "/wifi_eval_" + name + ".gp");
		pet->plot();

	}


private:

	void build(const std::string& name) {

		K::GnuplotStroke stroke(
			K::GnuplotDashtype::SOLID, 1.5,
			K::GnuplotColor::AUTO()
		);
		static int idx = -1; ++idx;


		const Offline::FileReader::GroundTruth gtp = reader.getGroundTruth(map, gtIndices);

		Line<Point3> path;
		K::GnuplotSplotElementLines* gpPath = new K::GnuplotSplotElementLines();
		gpPath->setStroke(stroke);
		plot->splot.add(gpPath);

		K::Statistics<float>* stats = new K::Statistics<float>();
		K::Statistics<float>* statsProbOnGT = new K::Statistics<float>();
		pef->add(name, stats);
		pef2->add(name, statsProbOnGT);

		// process each wifi entry within the offline file
		for (const auto wifi : reader.wifi) {

			// all seen APs at one timestamp
			const WiFiMeasurements& _mes = wifi.data;

			// debug output
			std::cout << wifi.ts << ":" << _mes.entries.size() << std::endl;

			// perform vap grouping
			const WiFiMeasurements mes = vap->group(_mes);



			// error calculation
			auto func = [&] (const float* params) -> double {

				// crop z to 1 meter
				//params[2] = std::round(params[2]);

				// suggested position
				const Point3 pos_m(params[0], params[1], params[2]);

				const float sigma = 8.0;

				double prob = 1.0;

				if (1 == 1) {

					// calculate error for above position using the currently available measurements
					for (const WiFiMeasurement& m : mes.entries) {

						// skip non-FHWS APs
						if (!LeHelper::isFHWS_AP(m.getAP().getMAC())) {continue;}

						// get model's rssi for the given location
						const float rssi_model = wiModel->getRSSI(m.getAP().getMAC(), pos_m);

						// skip APs unknown to the model
						if (rssi_model != rssi_model) {
							std::cout << "unknown ap: " << m.getAP().getMAC().asString() << std::endl;
							continue;
						}

						// get scan's rssi
						const float rssi_scan = m.getRSSI();

						// likelyhood
						const double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);
						//const double p = Distribution::Region<double>::getProbability(rssi_model, sigma, rssi_scan);

						// adjust
						prob *= p;

					}

				} else {

//					//const float limit = -85;

//					for (const AccessPoint& ap : wiModel->getAllAPs()) {

//						// get model's rssi for the given location
//						float rssi_model = wiModel->getRSSI(ap.getMAC(), pos_m);
//						if (rssi_model < limit) {rssi_model = limit;}

//						// get scan's rssi
//						const WiFiMeasurement* mesModel = mes.getForMac(ap.getMAC());
//						float rssi_scan = (mesModel) ? (mesModel->getRSSI()) : (limit);
//						if (rssi_scan < limit) {rssi_scan = limit;}

//						// likelyhood
//						const double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);

//						// adjust
//						prob *= p;

//					}

				}

				const double err = -prob;
				return err;

			};

			// parameters
			float params[3];

			// USE GENETIC
//			std::minstd_rand gen;
//			std::uniform_real_distribution<float> distX(mapBBox.getMin().x, mapBBox.getMax().x);
//			std::uniform_real_distribution<float> distY(mapBBox.getMin().y, mapBBox.getMax().y);
//			std::uniform_real_distribution<float> distZ(mapBBox.getMin().z, mapBBox.getMax().z);
//			// initializer for the optimizer: random position within the map's bbox
//			auto init = [&] (const int childIdx, float* params) {
//				(void) childIdx;
//				params[0] = distX(gen);
//				params[1] = distY(gen);
//				params[2] = distZ(gen);
//			};
//			K::NumOptAlgoGenetic<float> opt(3);
//			opt.setPopulationSize(400);
//			opt.setMaxIterations(20);
//			opt.calculateOptimum(func, params, init);


			// USE RANGE RANDOM WITH COOLING
			using LeOpt = K::NumOptAlgoRangeRandom<float>;
			const std::vector<LeOpt::MinMax> valRegion = {
				 LeOpt::MinMax(mapBBox.getMin().x, mapBBox.getMax().x),	// x
				 LeOpt::MinMax(mapBBox.getMin().y, mapBBox.getMax().y),	// y
				 LeOpt::MinMax(mapBBox.getMin().z, mapBBox.getMax().z),	// z
			 };

			K::NumOptAlgoRangeRandom<float> opt(valRegion);
			opt.setPopulationSize(200);
			opt.setNumIerations(50);
			opt.calculateOptimum(func, params);


			std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
			const Point3 curEst(params[0], params[1], params[2]);
			path.add(curEst);

			const Timestamp ts = mes.entries.front().getTimestamp();

			// draw a smoothed version of the path
			gpPath->clear();
			for (const Point3 p : path.getAverage(2)) {
				const K::GnuplotPoint3 gp3(p.x, p.y, p.z);
				gpPath->add(gp3);
			}

			static int xxx = 0; ++xxx;

			// groud-truth
			const Point3 gt = gtp.get(ts);
			plot->gp << "set arrow 1 at " << gt.x << "," << gt.y << "," << gt.z << " to " << gt.x << "," << gt.y << "," << (gt.z+1) << "\n";

			// error in meter
			const float err_m = gt.xy().getDistance(curEst.xy());		// 2D
			//const float err_m = gt.getDistance(curEst);				// 3D
			stats->add(err_m);
			mavgMeter.add(err_m);
			if (xxx % 3 == 0) {
				pet->addErr(ts, mavgMeter.get(), idx);
			}

			// error in -log(p)
			float gtFloat[3] = {gt.x, gt.y, gt.z};
			const double probOnGT = -func(gtFloat);
			const double logProbOnGT = -std::log10(probOnGT);
			const double logProbOnGTNorm = (logProbOnGT/mes.entries.size());
			statsProbOnGT->add(logProbOnGTNorm);
			pet2->addErr(ts, logProbOnGTNorm, idx);

			if (xxx%4 == 0) {

				plot->plot();

				pet->plot();
				pef->plot();

				pet2->plot();
				pef2->plot();

			} else {
				std::this_thread::sleep_for(std::chrono::milliseconds(5));
			}



		}

	}

//	// TODO
//	void abc(const std::string& fpFile) {

//		// load fingerprints
//		calib = new WiFiFingerprints(fpFile);

//		// how to handle VAPs
//		VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

//		// the optimizer
//		opt = new WiFiOptimizer::LogDistCeiling(map, *vap, *calib, WiFiOptimizer::LogDistCeiling::Mode::MEDIUM);

//	}


//	static void dumpWiFiCenterForPath(coconst std::string& fPath) {

//		std::cout << "dump WiFi for " << fPath << std::endl;

//		Offline::FileReader fr(fPath);

//		WiFiModel logDistC

//		for (const auto wifi : fr.wifi) {
//			std::cout << wifi.ts << ":" << wifi.data.entries.size() << std::endl;
//		}


//	}


};

#endif // EVALWIFI_H