#ifndef EVALWIFIPATHMETHODS_H
#define EVALWIFIPATHMETHODS_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"

/** evaluate just the wifi error by using the same model but various probability functions */
class EvalWiFiPathMethods {

private:

	Floorplan::IndoorMap* map;
	BBox3 mapBBox;

	VAPGrouper* vap = nullptr;

	K::Statistics<float>* statsOrig_m;
	K::Statistics<float>* statsOther_m;
	PlotErrFunc* pef_m;

	K::Statistics<float>* statsOrig_p;
	K::Statistics<float>* statsOther_p;
	PlotErrFunc* pef_p;

	K::Statistics<float>* statsOrig_c;
	K::Statistics<float>* statsOther_c;
	PlotErrFunc* pef_c;

	WiFiModel* wiModel = nullptr;

	std::vector<Point3> randomLoc;
	std::vector<AccessPoint> allAPs;

	int idx = -2;

public:

	/** ctor with map and fingerprints */
	EvalWiFiPathMethods(const std::string& mapFile) {

		// 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);

		pef_m = new PlotErrFunc("error (meter)", "measurements (%)");
		pef_m->showMarkers(false, false);

		pef_p = new PlotErrFunc("-log(p(..))", "measurements (%)");
		pef_p->showMarkers(false, false);
		pef_p->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));


		pef_c = new PlotErrFunc("cross error", "measurements (%)");
		pef_c->showMarkers(false, false);
		pef_c->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));




		// generate some random locations within the walkable area
		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);
		for (const Floorplan::Floor* f : map->floors) {

			std::vector<Point3> floorLoc;

			while (floorLoc.size() < 100) {

				const Point3 pt(distX(gen), distY(gen), f->atHeight + 1.3);	// human above ground


				for (const Floorplan::FloorOutlinePolygon* poly : f->outline) {

					// ensure the random samples are nicely placed along the floor
					for (const Point3& p : floorLoc) {
						if (p.getDistance(pt) < 4) {continue;}
					}

					HelperPoly hp(*poly);
					if (hp.contains(pt.xy()*100)) {
						floorLoc.push_back(pt);
					}
				}
			}

			for (const Point3 p : floorLoc) {randomLoc.push_back(p);}

		}

//		Plotty p(map);
//		p.buildFloorplan();
//		for (const Point3 pt :randomLoc) {
//			p.points.add({pt.x, pt.y, pt.z});
//		}
//		p.plot();
//		sleep(1000);


//		// generate some random locations within the building
//		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);
//		for (int i = 0; i < 250; ++i) {
//			const Point3 pt(distX(gen), distY(gen), distZ(gen));
//			for (const Point3& p : randomLoc) {
//				if (p.getDistance(pt) < 3) {continue;}	// ensure the random samples are nicely placed
//			}
//			randomLoc.push_back(pt);
//		}

	}



	void loadModel(const std::string& xmlFile, const std::string& modelName, const std::string& modeNameA, const std::string& modeNameB) {

		idx += 2;

		WiFiModelFactory fac(map);

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

		statsOrig_m = new K::Statistics<float>();
		statsOrig_p = new K::Statistics<float>();
		statsOrig_c = new K::Statistics<float>();

		statsOther_m = new K::Statistics<float>();
		statsOther_p = new K::Statistics<float>();
		statsOther_c = new K::Statistics<float>();

		pef_m->add(modeNameA, statsOrig_m);
		pef_m->add(modeNameB, statsOther_m);
		pef_m->getPlot().getAxisX().setTicsLabelFormat("%h m");

		pef_p->add(modeNameA, statsOrig_p);
		pef_p->add(modeNameB, statsOther_p);

		pef_c->add(modeNameA, statsOrig_c);
		pef_c->add(modeNameB, statsOther_c);
		pef_c->getPlot().getAxisX().setTicsLabelFormat("%h %%");


		this->allAPs = wiModel->getAllAPs();
		for (const AccessPoint& ap : allAPs) {
			std::cout << ap.getMAC().asString() << std::endl;
		}
		int i = 0; (void) i;

	}

	void walks (const std::vector<std::string> files, const std::vector<std::vector<int>> gtIndicies) {
		for (size_t i = 0; i < files.size(); ++i) {
			walk(files[i], gtIndicies[i]);
		}
	}

	void writeGP(const std::string& path, const std::string& name) {
		writeGP(*pef_m, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + name + "_meter");
		writeGP(*pef_c, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + name + "_cross");
		writeGP(*pef_p, K::GnuplotSize(8.6, 3.0), path + "/wifiCompare_" + name + "_logprob");
	}

	void writeGP(PlotErrFunc& pef, K::GnuplotSize size, const std::string& file) {
		pef.getGP().setTerminal("epslatex", size);
		pef.getGP().setOutput(file+".tex");
		pef.getPlot().getKey().setVisible(true);
		pef.getPlot().getKey().setSampleLength(0.5);
		pef.getPlot().getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::BOTTOM);
		pef.getPlot().getKey().setWidthIncrement(-4);
		pef.getPlot().getAxisY().setLabelOffset(2.5, 0);
		pef.getPlot().getAxisY().setTicsStep(0, 25, 95);
		pef.getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0,95));

		pef.getPlot().getAxisX().setLabel("");
		pef.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
		pef.getGP() << "set lmargin 4.5\n";
		pef.getGP() << "set tmargin 0.1\n";
		pef.getGP() << "set rmargin 0.4\n";
		pef.getGP() << "set bmargin 2.0\n";
		pef.writePlotToFile(file+".gp");
		pef.plot();
		pef.writePlotToFile("");
	}

	void walk(const std::string& fPath, const std::vector<int> gtIndices) {

		Offline::FileReader reader(fPath);

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

		// 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;

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

			// error calculation
			auto funcOrig =		[&] (const float* params) -> double { return errFuncOrig(params, mes); };
			auto funcOther =	[&] (const float* params) -> double { return errFuncSingleProb(params, mes); };	// ADJUST HERE

			// parameters (x,y,z);
			float paramsOrig[3] = {0,0,0};
			float paramsOther[3] = {0,0,0};

			// 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(400);
			opt.setNumIerations(30);

			opt.calculateOptimum(funcOrig, paramsOrig);
			opt.calculateOptimum(funcOther, paramsOther);


			// estimation
			//std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
			const Point3 curEstOrig(paramsOrig[0], paramsOrig[1], paramsOrig[2]);
			const Point3 curEstOther(paramsOther[0], paramsOther[1], paramsOther[2]);
			const Timestamp ts = mes.entries.front().getTimestamp();

			// groud-truth
			const Point3 gt = gtp.get(ts);

			// error in meter
			//const float err_m = gt.xy().getDistance(curEst.xy());		// 2D
			const float errOrig_m = gt.getDistance(curEstOrig);				// 3D
			const float errOther_m = gt.getDistance(curEstOther);			// 3D

			statsOrig_m->add(errOrig_m);
			//pet_m.addErr(ts, errOrig_m, idx+0);
			statsOther_m->add(errOther_m);
			//pet_m.addErr(ts, errOther_m, idx+1);

//			// error in -log(p)
			float gtFloat[3] = {gt.x, gt.y, gt.z + 1.3};	// human above ground

			{
				const double probOnGT_a = -errFuncOrig(gtFloat, mes);
				const double logProbOnGT_a = -std::log10(probOnGT_a);
				const double logProbOnGTNorm_a = (logProbOnGT_a/mes.entries.size());
				statsOrig_p->add(logProbOnGTNorm_a);
			}

			{
				const double probOnGT_b = -errFuncOther(gtFloat, mes);
				const double logProbOnGT_b = -std::log10(probOnGT_b);
				const double logProbOnGTNorm_b = (logProbOnGT_b/mes.entries.size());

				// break on huge errors
				if (logProbOnGTNorm_b > 2) {
//					std::cout << "-------------------------------"<< std::endl;
//					std::cout << gt.asString() << std::endl;
//					getVeto(gt, mes);
//					throw "123";
				}
				statsOther_p->add(logProbOnGTNorm_b);
			}

			checkCross(funcOrig, gt, statsOrig_c);
			checkCross(funcOther, gt, statsOther_c);

			//pet_p.addErr(ts, logProbOnGTNorm, idx);

			static int xx = 0; ++xx;

			if ( (xx % 20) == 0) {
				pef_p->plot();
				std::this_thread::sleep_for(std::chrono::milliseconds(1));
				pef_m->plot();
				std::this_thread::sleep_for(std::chrono::milliseconds(1));
				pef_c->plot();
				std::this_thread::sleep_for(std::chrono::milliseconds(1));
			}



		}

	}

private:



	void checkCross(std::function<double(const float* params)> func, Point3 gt, K::Statistics<float>* stats) {

		const float gtFloat[3] = {gt.x, gt.y, gt.z + 1.3};	// above ground
		const double orig_p = -func(gtFloat);
		float bigger = 0;
		int cnt = 0;

		float errSum = 0;
		int errCnt = 0;

		for (const Point3 pt : randomLoc) {

			if (pt.getDistance(gt) > 20) {continue;}
			float params[3] = {pt.x, pt.y, pt.z};
			const double other_p = -func(params);

			if (pt.getDistance(gt) >= 3) {			// at least 3 meter away
				if (other_p > orig_p*0.75) {++bigger;}
			}

			if (pt.getDistance(gt) >=5) {			// at least 5 meter away
				if (other_p > orig_p) {errSum += pt.getDistance(gt); ++errCnt;}
			}
			++cnt;
		}


//		for (float rad = 0; rad < 2*M_PI; rad += 0.1) {
//			Point3 p = gt + Point3(std::cos(rad), std::sin(rad), 0) * 5;	// 5 meters around gt
//			float params[3] = {p.x, p.y, p.z};
//			const double other_p = func(params);
//			if (other_p >orig_p) {++bigger;}
//			++cnt;
//		}

		const float factor = bigger / (float)cnt;
		//const float factor = errSum / (errCnt+0.00001f);

		// break on huge errors
		if (factor > 0.12) {
//			std::cout << "-------------------------------"<< std::endl;
//			std::cout << gt.asString() << std::endl;
//			func(gtFloat);
//			const float gtFloat2[3] = {gt.x, gt.y, gt.z-1};
//			func(gtFloat2);
//			throw "123";
		}

		stats->add(factor * 100);	// scale to "%"

	}

	double errFuncOrig(const float* params, const WiFiMeasurements& mes) {

		// 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;

		// 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;

		}

		const double err = -prob;
		return err;

	}

	double errFuncOther(const float* params, const WiFiMeasurements& _mes) {

		// 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;

		WiFiMeasurements mes = _mes;
		const auto comp = [] (const WiFiMeasurement& m1, const WiFiMeasurement& m2) {return m1.getRSSI() < m2.getRSSI();};
		//std::sort(mes.entries.begin(), mes.entries.end(), comp);
		const auto& min = std::min_element(mes.entries.begin(), mes.entries.end(), comp);

		// 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;}

			// TESTING


			if (mes.entries.size() > 8) {
				//if (m.getRSSI() < mes.entries[1].getRSSI()-10) {continue;}
				//if (m.getRSSI() > -65) {continue;}
				if (m.getAP().getMAC() == min->getAP().getMAC()) {continue;}
			}

			const float rssi = m.getRSSI();
//			const volatile float min = -100;
//			const volatile float max = -40;
//			const volatile float val = (m.getRSSI() - min) / (max-min);

			// 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
			double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);

			// adjust
			prob *= p;

		}

		const double err = -prob;
		return err;

	}


	/** USED WITHIN THE PAPER */
	double errFuncOtherExponential(const float* params, const WiFiMeasurements& mes) {

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

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

		double prob = 1.0;

		//const auto comp = [] (const WiFiMeasurement& m1, const WiFiMeasurement& m2) {return m1.getRSSI() < m2.getRSSI();};
		//const auto& min = std::min_element(mes.entries.begin(), mes.entries.end(), comp);

		// 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
			double p = Distribution::Exponential<double>::getProbability(1.0, std::abs(rssi_model-rssi_scan));

			// adjust
			prob *= p;

		}

		const double err = -prob;
		return err;

	}

	/** TESTING */
	double errFuncSingleProb(const float* params, const WiFiMeasurements& mes) {

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

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

		int cnt = 0;
		double error = 0;

		//const auto comp = [] (const WiFiMeasurement& m1, const WiFiMeasurement& m2) {return m1.getRSSI() < m2.getRSSI();};
		//const auto& min = std::min_element(mes.entries.begin(), mes.entries.end(), comp);

		// 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();

			++cnt;
			error += std::pow(std::abs(rssi_scan - rssi_model),2);

		}

		if (cnt == 0) {return 1e-50;}
		double errorAvg = std::pow((error / cnt), 1.0/2.0);
		double prob = Distribution::Normal<double>::getProbability(0, 4, errorAvg);
		const double err = -prob;
		return err;

	}


	double getVeto(const Point3& pos_m, const WiFiMeasurements& obs) const {

		struct APR {
			AccessPoint ap;
			float rssi;
			APR(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) {;}
		};

		std::vector<APR> all;
		for (const AccessPoint& ap : allAPs) {
			const float rssi = wiModel->getRSSI(ap.getMAC(), pos_m);
			if (rssi != rssi) {throw Exception("should not happen?!");}	// unknown to the model
			all.push_back(APR(ap, rssi));
		}

		// stort by RSSI
		auto comp = [&] (const APR& apr1, const APR& apr2) {return apr1.rssi > apr2.rssi;};
		std::sort(all.begin(), all.end(), comp);

		int numVetos = 0;

		for (int i = 0; i < 3; ++i) {
			const APR& apr = all[i];
			if (apr.rssi < -80) {continue;}
			const WiFiMeasurement* mes = obs.getForMac(apr.ap.getMAC());
			if (!mes) {
				++numVetos;
				continue;
				//std::cout << apr.ap.getMAC().asString() << ":" << apr.rssi << std::endl;
			}
			const float rssiScan = mes->getRSSI();
			const float rssiModel = apr.rssi;
			const float diff = std::abs(rssiScan - rssiModel);
			if (diff > 20) {++numVetos;}
		}

		return (numVetos < 1) ? (0.999) : (0.001);

//		if (numVetos == 0)	{return 0.70;}
//		if (numVetos == 1)	{return 0.70;}
//		else				{return 0.01;}

	}

};


#endif // EVALWIFIPATHMETHODS_H