#ifndef EVALCOMPAREOPT2_H
#define EVALCOMPAREOPT2_H

#include <KLib/math/statistics/Statistics.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/setup/WiFiOptimizerLogDistCeiling.h"

#include "Indoor/sensors/radio/VAPGrouper.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 "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h"
#include "Indoor/sensors/offline/FileReader.h"

#include "Helper.h"

#include <vector>

// FOR TESTING
#define FAST	1
#define QUALITY 2
#define MODE QUALITY

//#define DO_SHOW_FINGERPRINTS

class EvalCompareOpt2 {

	int power = 1;
	Floorplan::IndoorMap* map;
	WiFiFingerprints calib;
	VAPGrouper* vap;
	Floorplan::Ceilings ceilings;
	std::vector<APAtFloor> mapAPs;

public:

	/** ctor with map and fingerprints */
	EvalCompareOpt2(const std::string& mapFile, const std::string& fpFile, std::function<bool(const WiFiFingerprint& fp)> remove, bool removeStaircases = false) {

		setup(mapFile);

		// load fingerprints
		calib = WiFiFingerprints(fpFile);
//		if (ignoreOutdoor) {calib = LeHelper::removeOutdoor(calib);}
		if (removeStaircases) {calib = LeHelper::removeStaircases(calib);}
//		if (ignoreIndoor) {calib = LeHelper::removeIndoor(calib);}
		calib = LeHelper::removeIf(calib, remove);

		cleanup();

	}

	/** ctor with map and live-walk [ground-truth] */
	EvalCompareOpt2(const std::string& mapFile, std::vector<std::pair<std::string, std::vector<int>>> walks) {

		setup(mapFile);


		// ensure each AP is present at least once
		WiFiMeasurements mes;
		for (const APAtFloor& apf : mapAPs) {
			const Floorplan::AccessPoint* ap = apf.first;
			WiFiMeasurement m(AccessPoint(MACAddress(ap->mac)), -100);
			mes.entries.push_back(m);
		}
		WiFiFingerprint fpDummy(Point3(-50, -50, -50), mes);
		calib.add(fpDummy);

		for (const auto& it : walks) {

			const std::string walkFile = it.first;
			const std::vector<int> gtPathIndices = it.second;

			// load "fingerprints" by matching ground-truth against live walk
			Offline::FileReader reader(walkFile);
			Offline::FileReader::GroundTruth gt = reader.getGroundTruth(map, gtPathIndices);

			for (const auto& it : reader.getWiFiGroupedByTime()) {
				const Timestamp ts = Timestamp::fromMS(it.ts);	// time of the measurement
				const WiFiMeasurements& mes = it.data;			// wifi measurement
				const Point3 gtPos_m = gt.get(ts);				// location on the ground-truth during time of measuring
				const WiFiFingerprint fp(gtPos_m, mes);			// location + measurement = fingerprint
				calib.add(fp);
			}

		}

		cleanup();

	}


private:

	void setup(const std::string& mapFile) {

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

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

		// some ceiling calculations
		ceilings = Floorplan::Ceilings(map);

		// all APs within the map
		mapAPs = FloorplanHelper::getAPs(map);


	}

	void cleanup() {

		LeHelper::removeNonFHWS(calib);

		// VAP-group fingerprints
		for (WiFiFingerprint& fp : calib.getFingerprints()) {
			fp.measurements = vap->group(fp.measurements);
		}

		// plot
#ifdef DO_SHOW_FINGERPRINTS
		LeHelper::plot(map, calib);
#endif

	}

public:

	struct Result {

		// the configured model
		WiFiModelLogDistCeiling model;

		// the resulting error
		K::Statistics<float> errAvg;
		K::Statistics<float> errSingle;

		Result(Floorplan::IndoorMap* map) : model(map) {;}

	};


	Result fixedPosFixedParamsForAll() {

		const float txp = -40;
		const float exp = 2.65;
		const float waf = -6.5;

		Result res(map);

		// process each AP
		for (const auto _ap : mapAPs) {
			const Floorplan::AccessPoint* ap = _ap.first;
			const Floorplan::Floor* floor = _ap.second;
			const MACAddress mac(ap->mac);
			const Point3 pos_m = ap->getPos(floor);
			res.model.addAP(mac, pos_m, txp, exp, waf, true);
			K::Statistics<float> err = getError(mac, res.model);
			res.errAvg.add(err.getAvg());
			res.errSingle.add(err);
		}

		dump(res);
		return res;

	}

	Result fixedPosOptParamsForAll() {


		// get fingerprints for each AP [once]
		std::unordered_map<MACAddress, std::vector<WiFiFingerprint>> fps;
		for (const auto _ap : mapAPs) {
			const Floorplan::AccessPoint* ap = _ap.first;
			const MACAddress mac(ap->mac);
			fps[mac] = calib.getFingerprintsFor(mac);
		}

		// resulting error
		auto errFunc = [&] (const float txp, const float exp, const float waf) -> Result {

			Result res(map);

			// process each AP
			for (const auto _ap : mapAPs) {
				const Floorplan::AccessPoint* ap = _ap.first;
				const Floorplan::Floor* floor = _ap.second;
				const MACAddress mac(ap->mac);
				const Point3 pos_m = ap->getPos(floor);
				res.model.addAP(mac, pos_m, txp, exp, waf, false);		// allow unsafe txp,exp,waf params
				K::Statistics<float> err = getError(mac, res.model, fps[mac]);
				res.errAvg.add(err.getAvg());
				res.errSingle.add(err);
			}

			return res;

		};

		// to-be-optimized function
		auto optFunc = [&] (const float* params) {
			return whatToOpt(errFunc(params[0], params[1], params[2]).errSingle);
		};

		// use simplex
		float params[3] = {-40, 2, -8};
		K::NumOptAlgoDownhillSimplex<float> opt(3);
#if MODE == FAST
		opt.setMaxIterations(50);
		opt.setNumRestarts(10);
#elif MODE == QUALITY
		opt.setMaxIterations(200);
		opt.setNumRestarts(25);
#endif
		opt.calculateOptimum(optFunc, params);

//		// use genetic
//		K::NumOptAlgoGenetic<float> opt(3);
//		opt.setPopulationSize(100);
//		opt.setMaxIterations(100);
//		opt.setValRange({1, 0.1, 0.2});
//		opt.setElitism(0.05f);
//		opt.setMutation(0.25);
//		opt.calculateOptimum(optFunc, params);

		// get the error result for all APs
		Result res = errFunc(params[0], params[1], params[2]);

		// done
		dump(res);
		return res;

	}

	Result fixedPosOptParamsForEach() {

		Result res(map);

		// process each AP
		for (const auto _ap : mapAPs) {

			// fixed AP params
			const Floorplan::AccessPoint* ap = _ap.first;
			const Floorplan::Floor* floor = _ap.second;
			const MACAddress mac(ap->mac);
			const Point3 pos_m = ap->getPos(floor);
			const std::vector<WiFiFingerprint> fps = getFingerprints(mac);

			// resulting error
			auto errFunc = [&] (const float txp, const float exp, const float waf) -> K::Statistics<float> {
				WiFiModelLogDistCeiling model(map);						// new, empty model for this AP
				model.addAP(mac, pos_m, txp, exp, waf, false);			// allow unsafe txp,exp,waf params
				K::Statistics<float> err = getError(mac, model, fps);	// calculate error among all fingerprints
				return err;
			};

			// to-be-optimized function
			auto optFunc = [&] (const float* params) {
				return whatToOpt(errFunc(params[0], params[1], params[2]));
			};

			// use simplex
			float params[3] = {-40, 2, -8};
			K::NumOptAlgoDownhillSimplex<float> opt(3);
#if MODE == FAST
		opt.setMaxIterations(50);
		opt.setNumRestarts(10);
#elif MODE == QUALITY
		opt.setMaxIterations(200);
		opt.setNumRestarts(25);
#endif
			opt.calculateOptimum(optFunc, params);

//			// use genetic
//			K::NumOptAlgoGenetic<float> opt(3);
//			opt.setPopulationSize(100);
//			opt.setMaxIterations(100);
//			opt.setValRange({1, 0.1, 0.2});
//			opt.setElitism(0.05f);
//			opt.setMutation(0.25);
//			opt.calculateOptimum(optFunc, params);

			// get the error result for the current AP
			K::Statistics<float> err = errFunc(params[0], params[1], params[2]);
			res.errAvg.add(err.getAvg());
			res.errSingle.add(err);

			// add finalized params to the model
			res.model.addAP(mac, pos_m, params[0], params[1], params[2], false);

		}

		// done
		dump(res);
		return res;

	}

	Result optPosOptParamsForEach() {

		Result res(map);

		// process each AP
		for (const auto _ap : mapAPs) {

			// fixed AP params
			const Floorplan::AccessPoint* ap = _ap.first;
			const MACAddress mac(ap->mac);
			const std::vector<WiFiFingerprint> fps = getFingerprints(mac);

			// ensure a certain number of fingerprints
			// otherwise: ignore this AP as it can not be optimized!
			if (fps.size() < 3) {
				std::cout << "IGNORING " << mac.asString() << " due to insufficient fingerprints! using fixed -100 entry" << std::endl;
				res.model.addAP(mac, Point3(0,0,0), -100, 0, 0, false);
				//res.model.addAP(mac, pos_m, params[3], params[4], params[5], false);
				continue;
			}

			// resulting error
			auto errFunc = [&] (const Point3 pos_m, const float txp, const float exp, const float waf) -> K::Statistics<float> {
				WiFiModelLogDistCeiling model(map);									// new, empty model for this AP
				model.addAP(mac, pos_m, txp, exp, waf, false);						// allow unsafe txp,exp,waf params
				K::Statistics<float> err = getError(mac, model, fps);				// calculate error among all fingerprints
				return err;
			};

			// to-be-optimized function
			auto optFunc = [&] (const float* params) {
				const Point3 pos_m(params[0], params[1], params[2]);
				return whatToOpt(errFunc(pos_m, params[3], params[4], params[5]));
			};

			// params
			float params[6] = {0};

			std::minstd_rand gen;
			BBox3 mapBBox = FloorplanHelper::getBBox(map);
			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);
				params[3] = -40;
				params[4] = 2;
				params[5] = -8;
			};

//			// use genetic
//			K::NumOptAlgoGenetic<float> opt(6);
//			opt.setPopulationSize(300);
//			opt.setMaxIterations(50);
//			opt.setValRange({1, 1, 1,   1, 0.1, 0.2});
//			opt.setElitism(0.05f);
//			opt.setMutation(0.25);
//			opt.calculateOptimum(optFunc, params, init);

			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
				 LeOpt::MinMax(-50, -30),	// txp
				 LeOpt::MinMax(  1,   4),	// exp
				 LeOpt::MinMax(-15,  -0),	// waf
			 };

			K::NumOptAlgoRangeRandom<float> opt(valRegion);

#if MODE == FAST
			opt.setPopulationSize(100);
			opt.setNumIerations(50);
#elif MODE == QUALITY
			opt.setPopulationSize(500);
			opt.setNumIerations(250);
#endif

			opt.calculateOptimum(optFunc, params);



//			K::NumOptAlgoDownhillSimplex<float> opt(6);
//			opt.setMaxIterations(40);
//			opt.setNumRestarts(20);
//			opt.calculateOptimum(optFunc, params);

			// get the error result for the current AP
			const Point3 pos_m(params[0], params[1], params[2]);
			K::Statistics<float> err = errFunc(pos_m, params[3], params[4], params[5]);
			res.errAvg.add(err.getAvg());
			res.errSingle.add(err);

			// add finalized params to the model
			res.model.addAP(mac, pos_m, params[3], params[4], params[5], false);

		}

		// done
		dump(res);
		return res;

	}




private:

	// the stats to optimize
	static inline float whatToOpt(const K::Statistics<float>& s) {
		//return s.getAvg() + s.getStdDev() / 2 + s.getMax() / 4;		// for dBm
		//return s.getAvg() + s.getStdDev() * 2 + s.getMax() / 2;		// for probability
		//return s.getQuantile(0.96);
		return s.getAvg();
	}

	std::vector<WiFiFingerprint> getFingerprints(const MACAddress& mac) {

		// get all fingerprints where the given mac was seen
		const std::vector<WiFiFingerprint> fps = calib.getFingerprintsFor(mac);

		// sanity check
//		if (fps.size() < 5) {throw Exception("not enought fingerprints for AP " + mac.asString());}

		return fps;

	}



	void dump(const Result& res) {
		std::cout << res.errSingle.asString() << std::endl;
		std::cout << res.errAvg.asString() << std::endl;
		std::cout << "------------------------------------------------------" << std::endl;
	}

	/** calculate the error for the given AP [mac + configured model] for all fingerprints for this mac */
	K::Statistics<float> getError(const MACAddress& mac, const WiFiModelLogDistCeiling& model) {

		// get all fingerprints where the given mac was seen
		const std::vector<WiFiFingerprint> fps = getFingerprints(mac);

		// fire
		return getError(mac, model, fps);

	}

	/** calculate the error for the given AP [mac + configured model] for all of the given FPS */
	K::Statistics<float> getError(const MACAddress& mac, const WiFiModelLogDistCeiling& model, const std::vector<WiFiFingerprint>& fps) {

		K::Statistics<float> res;

		// calculate the model error for each fingerprint
		for (const WiFiFingerprint& fp : fps) {

			// sanity checks
			if (fp.measurements.entries.size() != 1) {throw Exception("invalid fingerprint");}
			if (fp.measurements.entries.front().getAP().getMAC() != mac) {throw Exception("invalid fingerprint");}

			// rssi prediction from the configured model
			const float model_rssi = model.getRSSI(mac, fp.pos_m);

			// sanity check
			if (model_rssi != model_rssi) {throw Exception("model rssi not available for " + mac.asString());}

			// rssi measured during scan at this location
			const float scan_rssi = fp.measurements.entries.front().getRSSI();

			// sanity check
			if (scan_rssi < -100) {throw Exception("scan rssi out of range for " + mac.asString());}
			if (scan_rssi > -35) {throw Exception("scan rssi out of range for " + mac.asString());}

			// dB error
			const float err = model_rssi - scan_rssi;

			// probability matching error
			//const float err = -std::log(Distribution::Normal<float>::getProbability(model_rssi, 8, scan_rssi));

			// quadratic
			float aErr = std::pow(std::abs(err), 2);

			// penalty
//			if (model.getAP(mac).waf > -2) {aErr += 9999;}
//			if (model.getAP(mac).txp > -30) {aErr += 9999;}
//			if (model.getAP(mac).txp < -50) {aErr += 9999;}


			res.add(aErr);

		}

		// done
		return res;

	}


};

#endif // EVALCOMPAREOPT2_H