#ifndef EVALCOMPAREOPT_H
#define EVALCOMPAREOPT_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"

using APAtFloor = std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>;


/**
 * compare different optimzation levels
 * fixed ap pos / fixed params
 * fixed ap pos / optimized params
 * optimized ap pos / optimized params [+/- WAF]
 */
class EvalCompareOpt {

protected:

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

public:

	/** ctor with map and fingerprints */
	EvalCompareOpt(const std::string& mapFile, const std::string& fpFile, const bool ignoreStaircases, const bool ignoreOutdoor, const bool ignoreIndoor) {

		setup(mapFile);

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

		cleanup();

	}

	/** ctor with map and live-walk [ground-truth] */
	EvalCompareOpt(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)), -120);
			mes.entries.push_back(m);
		}
		WiFiFingerprint fpDummy(Point3(-99, -99, -99), 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);
		//LeHelper::plot(map, calib);

		// used to aggreagate fingerprints
		base = new WiFiOptimizer::Base(*vap);
		base->addFingerprints(calib);

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

		// plot
		LeHelper::plot(map, calib);

	}

protected:

	/** get the error for the given APs */
	K::Statistics<float> analyzeErrorForAPs(const std::vector<WiFiOptimizer::LogDistCeiling::APParamsMAC>& aps) {

		// overall error
		K::Statistics<float> statsAbs;

		// process each AP
		for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : aps) {
			analyzeErrorForAP(ap, statsAbs);
		}

		// done
		return statsAbs;

	}

	/** get the error for the given AP at the provided location */
	void analyzeErrorForAP(const MACAddress& mac, const Point3 pos, const float txp, const float exp, const float waf, K::Statistics<float>& dstAbs) {

		WiFiOptimizer::LogDistCeiling::APParams params;
		params.exp = exp;
		params.txp = txp;
		params.waf = waf;
		params.x = pos.x;
		params.y = pos.y;
		params.z = pos.z;

		const WiFiOptimizer::LogDistCeiling::APParamsMAC ap(mac, params);

		analyzeErrorForAP(ap, dstAbs);

	}

	/** get the error for the given AP at the provided location */
	void analyzeErrorForAP(const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap, K::Statistics<float>& dstAbs) {

		//
		const WiFiOptimizer::LogDistCeiling::APParams& params = ap.params;
		const MACAddress& mac = ap.mac;

		// empty model
		WiFiModelLogDistCeiling model(map);

		// add configured AP
		model.addAP(mac, WiFiModelLogDistCeiling::APEntry(params.getPos(), params.txp, params.exp, params.waf), false);

		// get all fingerprints for the given AP
		const std::vector<WiFiOptimizer::RSSIatPosition> entries = base->getFingerprintsFor(mac);

		// sanity check. seems ok
//		const std::vector<WiFiFingerprint> entries2 = calib.getFingerprintsFor(mac);
//		if (entries.size() != entries2.size()) {
//			throw Exception("detected size mismatch");
//		}

		// stats
		K::Statistics<float> stats;

		// process each fingerprint for this ap to estimate the error
		for (const WiFiOptimizer::RSSIatPosition& reading : entries) {

			// get the model-estimation for the fingerprint's position
			const float rssiModel = model.getRSSI(mac, reading.pos_m);

			// difference between estimation and measurement
			const float diff = std::abs(rssiModel - reading.rssi);

			// adjust
			stats.add(std::pow(std::abs(diff), power));
			dstAbs.add(std::pow(std::abs(diff), power));

		}

		// show
		//std::cout << " --- " << mac.asString() << ": " << stats.asString() << std::endl;

	}

private:

	// looks good
	float txp = -40;
	float exp = 2.65;
	float waf = -6.5;

public:

//	EvalCompareOptAllFixed(const std::string& mapFile, const std::string& fpFile, const bool ignoreStaircases, const bool ignoreOutdoor) :
//		EvalCompareOpt(mapFile, fpFile, ignoreStaircases, ignoreOutdoor) {
//		;
//	}


	struct Result {

		// the configured model
		WiFiModelLogDistCeiling model;

		// the resulting error
		K::Statistics<float> errAbs;

		Result(const WiFiModelLogDistCeiling& model, const K::Statistics<float>& errAbs) : model(model), errAbs(errAbs) {;}

	};

	WiFiModelLogDistCeiling::APEntry toModel(const WiFiOptimizer::LogDistCeiling::APParams& p) {
		return WiFiModelLogDistCeiling::APEntry(
			Point3(p.x, p.y, p.z),
			p.txp,
			p.exp,
			p.waf
		);
	}

	/** get the error when using the given 3 params for ALL aps */
	Result getStatsAll(const float txp, const float exp, const float waf) {

		WiFiModelLogDistCeiling model(map);

		// all access points with params
		std::vector<WiFiOptimizer::LogDistCeiling::APParamsMAC> aps;

		// construct vector containing each AP within the map + add fixed parameters
		for (const APAtFloor& mapAP : mapAPs) {

			WiFiOptimizer::LogDistCeiling::APParams params;
			params.exp = exp;
			params.txp = txp;
			params.waf = waf;
			params.x = mapAP.first->getPos(mapAP.second).x;
			params.y = mapAP.first->getPos(mapAP.second).y;
			params.z = mapAP.first->getPos(mapAP.second).z;

			const MACAddress mac = MACAddress(mapAP.first->mac);

			WiFiOptimizer::LogDistCeiling::APParamsMAC ap(mac, params);
			aps.push_back(ap);
			model.addAP(mac, toModel(params), false);

		}

		K::Statistics<float> errAbs = analyzeErrorForAPs(aps);
		return Result(model, errAbs);

	}

	/** calculate error for fixed positions and fixed constants */
	Result fixedPosFixedParamsForAll() {

		// fire
		Result res = getStatsAll(txp, exp, waf);
		std::cout << "----------------------------------------------------" << std::endl;
		std::cout << "AP POS FROM MAP, FIXED TXP/EXP/WAF FOR ALL APS" << std::endl;
		std::cout << res.errAbs.asString() << std::endl;
		std::cout << std::endl;
		return res;

	}

	/** calculate error for fixed positions and optimized constants, but the same 3 for all APs */
	Result fixedPosOptParamsForAll() {

		auto func = [&] (const float* params) {
			return getStatsAll(params[0], params[1], params[2]).errAbs.getAvg();
		};

		auto callback = [&] (const int run, const int iteration, const float , const float* ) {
			const int percent = ((run*40+iteration)*100) / (40*11);
			if (iteration == 0) {
				std::cout << percent << "," << std::flush;
			}
		};

		// use simplex
		float params[3] = {-40, 2, -8};
		K::NumOptAlgoDownhillSimplex<float> opt(3);
		opt.setMaxIterations(40);
		opt.setNumRestarts(10);
		opt.setCallback(callback);
		opt.calculateOptimum(func, params);

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

		Result res = getStatsAll(params[0], params[1], params[2]);
		std::cout << "----------------------------------------------------" << std::endl;
		std::cout << "AP POS FROM MAP, OPTIMIZING TXP/EXP/WAF: THE SAME FOR ALL APS" << std::endl;
		std::cout << "params: " << params[0] << "," << params[1] << "," << params[2] << std::endl;
		std::cout << res.errAbs.asString() << std::endl;
		std::cout << std::endl;
		return res;

	}

	/** calculate error for fixed positions and optimized constants, each AP on its own */
	Result fixedPosOptParamsForEach() {

		WiFiModelLogDistCeiling model(map);

		K::Statistics<float> _dstAbs;

		// construct vector containing each AP within the map + add fixed parameters
		for (const APAtFloor& mapAP : mapAPs) {

			// fixed
			const MACAddress mac(mapAP.first->mac);
			const Point3 pos = mapAP.first->getPos(mapAP.second);

			// opt-func for one AP
			auto func = [&] (const float* params) {
				K::Statistics<float> dstAbs;
				analyzeErrorForAP(mac, pos, params[0], params[1], params[2], dstAbs);
				return dstAbs.getAvg();
			};

			// use simplex
			float params[3] = {-40, 2, -8};
			K::NumOptAlgoDownhillSimplex<float> opt(3);
			opt.setMaxIterations(50);
			opt.setNumRestarts(10);
			opt.calculateOptimum(func, params);

			// use genetic [usually not better!]
//			K::NumOptAlgoGenetic<float> opt(3);
//			opt.setPopulationSize(100);
//			opt.setMaxIterations(50);
//			opt.setValRange({1, 0.1, 0.2});
//			opt.setElitism(0.05f);
//			opt.setMutation(0.25);
//			opt.calculateOptimum(func, params);

			// local stats
			K::Statistics<float> tmp;
			analyzeErrorForAP(mac, pos, params[0], params[1], params[2], tmp);

			// adjust global error with the resulting params
			std::cout << "--" << mac.asString() << " params: " << params[0] << ",\t" << params[1] << ",\t" << params[2] << "\terr: " << tmp.getAvg() << std::endl;
			analyzeErrorForAP(mac, pos, params[0], params[1], params[2], _dstAbs);

			// add the optimized AP to the final model
			model.addAP(mac, pos, params[0], params[1], params[2], false);

		}


		std::cout << "----------------------------------------------------" << std::endl;
		std::cout << "AP POS FROM MAP, OPTIMIZING TXP/EXP/WAF INDIVIDUALLY FOR EACH AP" << std::endl;
		std::cout << _dstAbs.asString() << std::endl;
		std::cout << std::endl;
		return Result(model, _dstAbs);

	}

	/** calculate error for fixed positions and optimized constants, each AP on its own */
	Result optPosOptParamsForEach() {

		// output only
		WiFiModelLogDistCeiling model(map);

		K::Statistics<float> _dstAbs;

		std::cout << "NOTE" << std::endl;
		std::cout << "INCREASE ITERATIONS!" << std::endl;
		std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;

		// construct vector containing each AP within the map + add fixed parameters
		for (const APAtFloor& mapAP : mapAPs) {

			// fixed
			const MACAddress mac(mapAP.first->mac);
			//const Point3 pos = mapAP.first->getPos(mapAP.second);

			if (calib.getFingerprintsFor(mac).empty()) {
				std::cout << "skipping AP " << mac.asString() << "!!! not seen during calibration" << std::endl;
				continue;
			}

			// opt-func for one AP
			auto func = [&] (const float* params) {
				K::Statistics<float> dstAbs;
				analyzeErrorForAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], dstAbs);
				return dstAbs.getAvg();
			};

			// use simplex
			float params[6] = {40, 40, 5, -40, 2, -8};
//			K::NumOptAlgoDownhillSimplex<float> opt(6);
//			opt.setMaxIterations(50);
//			opt.setNumRestarts(10);
//			opt.calculateOptimum(func, params);

			using LeOpt = K::NumOptAlgoRangeRandom<float>;
			const std::vector<LeOpt::MinMax> valRegion = {
				 LeOpt::MinMax(-20, 120),	// x
				 LeOpt::MinMax(-20, 120),	// y
				 LeOpt::MinMax( -5,  17),	// z
				 LeOpt::MinMax(-50, -30),	// txp
				 LeOpt::MinMax(  1,   4),	// exp
				 LeOpt::MinMax(-15,  -0),	// waf
			 };



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

			// local stats
			K::Statistics<float> tmp;
			analyzeErrorForAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], tmp);

			// adjust global error with the resulting params
			std::cout << "--" << mac.asString() << " params: " << params[0] << ",\t" << params[1] << ",\t" << params[2] << ",\t" << params[3] << ",\t" << params[4] << ",\t" << params[5] << "\terr: " << tmp.getAvg() << std::endl;
			analyzeErrorForAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], _dstAbs);

			// add to model for later reuse
			model.addAP(mac, Point3(params[0], params[1], params[2]), params[3], params[4], params[5], false);

		}

		std::cout << "----------------------------------------------------" << std::endl;
		std::cout << "OPTIMIZING POS/TXP/EXP/WAF INDIVIDUALLY FOR EACH AP" << std::endl;
		std::cout << _dstAbs.asString() << std::endl;
		std::cout << std::endl;
		return Result(model, _dstAbs);

	}



};



///** fixed ap pos, fixed ap params */
//class EvalCompareOptAllFixed : public EvalCompareOpt {



//};




#endif // EVALCOMPAREOPT_H