#ifndef OPTIMIZER_H
#define OPTIMIZER_H

#include "../../../floorplan/v2/Floorplan.h"
#include "../../../floorplan/v2/FloorplanHelper.h"

#include "../VAPGrouper.h"
#include "../../../geo/BBox3.h"
#include "../../../misc/Debug.h"

#include "WiFiFingerprint.h"
#include "../model/WiFiModel.h"
#include "../model/WiFiModelLogDistCeiling.h"

#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
#include <KLib/math/optimization/NumOptAlgoGenetic.h>
#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>

#include <string>
#include <sstream>

struct WiFiOptimizer {

private:

	/** combine one RSSI measurement with the position the signal was measured at */
	struct RSSIatPosition {

		/** real-world position (in meter) */
		const Point3 pos_m;

		/** measured signal strength (for one AP) */
		const float rssi;

		/** ctor */
		RSSIatPosition(const Point3 pos_m, const float rssi) : pos_m(pos_m), rssi(rssi) {;}

	};

public:

	struct APParams {
		float x;
		float y;
		float z;
		float txp;
		float exp;
		float waf;
		Point3 getPos() const {return Point3(x,y,z);}
		APParams() {;}
		APParams(float x, float y, float z, float txp, float exp, float waf) : x(x), y(y), z(z), txp(txp), exp(exp), waf(waf) {;}
		std::string asString() {
			std::stringstream ss;
			ss << "Pos:" << getPos().asString() << " TXP:" << txp << " EXP:" << exp << " WAF:" << waf;
			return ss.str();
		}
	};

	/** add MAC-info to params */
	struct APParamsMAC {
		MACAddress mac;
		APParams params;
		APParamsMAC(const MACAddress mac, const APParams& params) : mac(mac), params(params) {;}
	};

private:

	Floorplan::IndoorMap* map;
	const VAPGrouper vg;

	/** each MAC-Adress has several position->rssi entries */
	std::unordered_map<MACAddress, std::vector<RSSIatPosition>> apMap;

	const char* name = "WiFiOptimizer";

public:

	/** ctor */
	WiFiOptimizer(Floorplan::IndoorMap* map, const VAPGrouper& vg) : map(map), vg(vg) {
		;
	}


	/** add a new fingerprint to the optimizers data-source */
	void addFingerprint(const WiFiFingerprint& fp) {

		// group the fingerprint's measurements by VAP (if configured)
		const WiFiMeasurements measurements = vg.group(fp.measurements);

		// add each available AP to its slot (lookup map)
		for (const WiFiMeasurement& m : measurements.entries) {
			const RSSIatPosition rap(fp.pos_m, m.rssi);
			apMap[m.getAP().getMAC()].push_back(rap);
		}

	}

	/** get a list of all to-be-optimized access-points (given by their mac-address) */
	std::vector<MACAddress> getAllMACs() const {
		std::vector<MACAddress> res;
		for (const auto& it : apMap) {res.push_back(it.first);}
		return res;
	}

	/** optimize all known APs */
	std::vector<APParamsMAC> optimizeAll() const {

		// sanity chekc
		Assert::isFalse(getAllMACs().empty(), "no APs found for optimization!");

		float errSum = 0;
		std::vector<APParamsMAC> res;
		for (const MACAddress& mac : getAllMACs()) {
			float err;
			const APParams params = optimize(mac, err);
			res.push_back(APParamsMAC(mac, params));
			errSum += err;
		}

		const float avgErr = errSum / getAllMACs().size();
		Log::add(name, "average AP error is: " + std::to_string(avgErr) + " dB");

		return res;

	}

	/** optimize the given AP */
	APParams optimize(const MACAddress& mac, float& errResult) const {

		// starting parameters do not matter for the current optimizer!
		APParams params(0,0,0, -40, 2.5, -4.0);
		constexpr float hugeError = 1e10;

		// get all position->rssi measurements for this AP to compare them with the corresponding model estimations
		const std::vector<RSSIatPosition>& entries = apMap.find(mac)->second;


		// signal-strength-prediction-model...
		WiFiModelLogDistCeiling model(map);

		auto func = [&] (const float* data) {

			const APParams* params = (APParams*) data;

			// some sanity checks
			if (params->waf > 0)	{return hugeError;}

			if (params->txp < -50)	{return hugeError;}
			if (params->txp > -30)	{return hugeError;}

			if (params->exp > 4)	{return hugeError;}
			if (params->exp < 1)	{return hugeError;}

			// current position guess for the AP;
			const Point3 apPos_m = params->getPos();

			// add the AP [described by the current guess] to the signal-strength-prediction model
			model.clear();
			model.addAP(mac, WiFiModelLogDistCeiling::APEntry(apPos_m, params->txp, params->exp, params->waf));

			float err = 0;
			int cnt = 0;

			// process each measurement
			for (const 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 the error
				err += diff*diff;
				++cnt;

				// max distance penality
				// [unlikely to get a reading for this AP here!]
				if (apPos_m.getDistance(reading.pos_m) > 150) {err += 999999;}

			}

			err /= cnt;
			err = std::sqrt(err);

			if (params->txp < -50) {err += 999999;}
			if (params->txp > -35) {err += 999999;}

			if (params->exp > 3.5) {err += 999999;}
			if (params->exp < 1.0) {err += 999999;}

			return err;

		};


		//
		const BBox3 mapBBox = FloorplanHelper::getBBox(map);

		using LeOpt = K::NumOptAlgoRangeRandom<float>;
		const std::vector<LeOpt::MinMax> valRegion = {
			 LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20),	// x
			 LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20),	// y
			 LeOpt::MinMax(mapBBox.getMin().z -  5, mapBBox.getMax().z +  5),	// z
			 LeOpt::MinMax(-50,-30),	// txp
			 LeOpt::MinMax(1,3),		// exp
			 LeOpt::MinMax(-10,-4),		// waf
		 };


		// log
		Log::add(name, "optimizing parameters for AP " + mac.asString() + " by using " + std::to_string(entries.size()) + " fingerprints", false);
		Log::tick();

		LeOpt opt(valRegion);
		opt.setPopulationSize(500);	// USE MORE FOR PRODUCTION
		opt.setNumIerations(150);
		opt.calculateOptimum(func, (float*) &params);

//		using LeOpt = K::NumOptAlgoGenetic<float>;
//		LeOpt opt(6);
//		opt.setPopulationSize(750);
//		opt.setMaxIterations(50);
//		opt.setElitism(0.05f);
//		opt.setMutation(0.75f);
//		//opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
//		opt.setValRegion(valRegion);

//		K::NumOptAlgoDownhillSimplex<float, 6> opt;
//		opt.setMaxIterations(100);
//		opt.setNumRestarts(10);

		opt.calculateOptimum(func, (float*) &params);
		errResult = func((float*)&params);

		Log::tock();
		Log::add(name, mac.asString() + ": " + params.asString() + " @ " + std::to_string(errResult) +" dB err");

		return params;

	}


};


#endif // OPTIMIZER_H