#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 "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;


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

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

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

		// load fingerprints
		calib = WiFiFingerprints(fpFile);

		LeHelper::removeNonFHWS(calib);
		if (ignoreOutdoor) {calib = LeHelper::removeOutdoor(calib);}
		if (ignoreStaircases) {calib = LeHelper::removeStaircases(calib);}
		//LeHelper::plot(map, calib);
		WiFiFingerprints calib2 = calib;
		for (WiFiFingerprint& fp : calib2.getFingerprints()) {
			fp.measurements = vap->group(fp.measurements);
		}
		LeHelper::plot(map, calib2);

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

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

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

	}

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

		K::Statistics<float> statsAbs;

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

		// done
		//std::cout << "overall error: " << std::endl << statsAbs.asString() << std::endl;
		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;

		// always using the same model
		WiFiModelLogDistCeiling model(map);
		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);

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

		// process each fingerprint for this ap
		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;

	}



};



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

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) {
		;
	}



	/** get the error when using the given 3 params for ALL aps */
	K::Statistics<float> getStatsAll(const float txp, const float exp, const float waf) {

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

		}

		return analyzeErrorForAPs(aps);

	}

	/** calculate error for fixed positions and fixed constants */
	K::Statistics<float> fixedPosFixedParamsForAll() {

		// fire
		K::Statistics<float> stats = 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 << stats.asString() << std::endl;
		std::cout << std::endl;
		return stats;

	}

	/** calculate error for fixed positions and optimized constants, but the same 3 for all APs */
	K::Statistics<float> fixedPosOptParamsForAll() {

		auto func = [&] (const float* params) {
			return getStatsAll(params[0], params[1], params[2]).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);

		K::Statistics<float> stats = 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 << stats.asString() << std::endl;
		std::cout << std::endl;
		return stats;

	}

	/** calculate error for fixed positions and optimized constants, each AP on its own */
	K::Statistics<float> fixedPosOptParamsForEach() {

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

		}

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

	}

	/** calculate error for fixed positions and optimized constants, each AP on its own */
	K::Statistics<float> optPosOptParamsForEach() {

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

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

		}

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

	}

};




#endif // EVALCOMPAREOPT_H