#ifndef EVALWIFICONVEX_H
#define EVALWIFICONVEX_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/GnuplotSplotElementMesh.h>

#include <KLib/math/statistics/Statistics.h>

//#include "../Structs.h"
#include "../plots/Plotty.h"
#include "../plots/PlotErrTime.h"
#include "../plots/PlotErrFunc.h"
#include "../Settings.h"
#include "../Helper.h"

#include <unordered_set>
#include <thread>

class EvalWiFiConvex {

private:

	Floorplan::IndoorMap* map;
	std::vector<APAtFloor> mapAPs;
	WiFiFingerprints calib;

public:

	/** ctor */
	EvalWiFiConvex(Floorplan::IndoorMap* map, const std::string& fpFile) : map(map){

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

		// load fingerprints
		calib = WiFiFingerprints(fpFile);

		// VAP-group
		VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN);
		for (WiFiFingerprint& fp : calib.getFingerprints()) {
			fp.measurements = vap.group(fp.measurements);
		}

	}

	void showParams() {

		K::Gnuplot gp;
		K::GnuplotSplot splot;
		K::GnuplotSplotElementMesh mesh; splot.add(&mesh);

		//splot.setTitle("optimizing TXP and EXP}");
		splot.getAxisX().setLabel("TXP (dBm)");
		splot.getAxisX().setLabelOffset(-3.9 + 1.5, -1.9);
		splot.getAxisX().setTicsStep(2.5);
		splot.getAxisX().setTicsOffset(-1, -0.1);

		splot.getAxisY().setLabel("EXP");
		splot.getAxisY().setLabelOffset(-0.5, -0.6);
		splot.getAxisY().setTicsOffset(-0.3, -0.5);

		splot.getAxisZ().setLabel("error (dB)");

		splot.getAxisCB().setTicsStep(4);

		splot.getView().setCamera(55, 115);
		splot.getAxisZ().setLabelRotation(90);
		splot.setStringMod(new K::GnuplotStringModLaTeX());

		std::string name = Settings::fPathGFX + "/wifiop_show_optfunc_params";
		gp.setOutput(name + ".tex");
		gp.setTerminal("epslatex", K::GnuplotSize(7.7, 4.5));

		splot.getAxisZ().setTicsVisible(false);
		splot.getAxisZ().setLabel("");
		gp << "set palette defined (0 '#00ff00',  1 '#eeeeee', 9 '#222222')\n";

		gp << "set margins 0,0,0,0\n";
		gp << "set multiplot layout 1,1 scale 1.25, 1.4 offset -0.04, 0.05\n";
		gp << "set colorbox horizontal user origin 0.03, 0.95 size 0.4, 0.03\n";

		//gp << "set pm3d implicit\n";
		//gp << "set hidden3d front\n";

		//p mac="D8:84:66:4A:23:F0" px="46" py="47.400002" pz="13.8" txp="-40" exp="2.6500001" waf="-6.5"/>
		const MACAddress mac("D8:84:66:4A:23:F0");
		const Point3 pos_m(46, 47.4, 13.8);
		const float waf = -6;

		const std::vector<WiFiFingerprint> fps = getFingerprints(mac);

		for (float txp = -45; txp <= -35; txp += 0.5) {
			for (float exp = 1.5; exp <= 3.5; exp += 0.05) {

				WiFiModelLogDistCeiling model(map);
				model.addAP(mac, pos_m, txp, exp, waf);

				const K::Statistics<float> res = getError(mac, model, fps);
				const float err = res.getAvg();
				mesh.add(K::GnuplotPoint3(txp, exp, err));

			}
		}


		gp.draw(splot);
		LeHelper::writeCode(name + ".gp", gp.getBuffer());
		gp.flush();

		//sleep(100);

	}


//	void showPos() {

//		K::Gnuplot gp;
//		K::GnuplotSplot splot;
//		K::GnuplotSplotElementMesh mesh; splot.add(&mesh);

//		//splot.setTitle("optimizing position");
//		splot.getAxisX().setLabel("x-position (meter)");
//		splot.getAxisY().setLabel("y-position (meter)");
//		splot.getAxisZ().setLabel("error (dB)");

//		splot.getAxisZ().setLabelRotation(90);
//		splot.setStringMod(new K::GnuplotStringModLaTeX());
//		gp << "set palette defined (0 '#00ff00',  1 '#eeeeee', 9 '#000000')\n";

//		const MACAddress mac("D8:84:66:4A:23:F0");
//		const std::vector<WiFiFingerprint> fps = getFingerprints(mac);

//		const Point3 pos_m(46, 47.4, 13.8);
//		const float txp = -40;
//		const float exp = 2.2;
//		const float waf = -6;

//		for (float ox = -10; ox <= +10; ox += 0.5) {
//			for (float oy = -19; oy <= +8; oy += 0.5) {

//				const Point3 pos = pos_m + Point3(ox, oy, 0);

//				WiFiModelLogDistCeiling model(map);
//				model.addAP(mac, pos, txp, exp, waf);

//				const K::Statistics<float> res = getError(mac, model, fps);
//				const float err = res.getAvg();
//				mesh.add(K::GnuplotPoint3(pos.x, pos.y, err));

//			}
//		}

//		gp.draw(splot);
//		gp.flush();

//		//sleep(100);

//	}

	void showPosZ() {

		K::Gnuplot gp;
		K::GnuplotSplot splot;
		K::GnuplotSplotElementMesh mesh; splot.add(&mesh);
		K::GnuplotSplotElementLines lines1; splot.add(&lines1); lines1.getStroke().setWidth(1); lines1.getStroke().getColor().setHexStr("#444444"); lines1.getStroke().setType(K::GnuplotDashtype::DOTTED);
		K::GnuplotSplotElementLines lines2; splot.add(&lines2); lines2.getStroke().setWidth(1); lines2.getStroke().getColor().setHexStr("#444444");  //lines2.getStroke().setType(K::GnuplotDashtype::DASHED);
		K::GnuplotSplotElementLines lines3; splot.add(&lines3); lines3.getStroke().setWidth(2);
		K::GnuplotSplotElementPoints points; splot.add(&points); points.setPointSize(1.5); points.setPointType(7);

		//splot.setTitle("optimizing position");
		splot.getAxisX().setLabel("y-pos (meter)");
		splot.getAxisX().setLabelOffset(-0.9 + 0.2, -1.4 - 0.5);
		splot.getAxisX().setTicsOffset(-0.9, 0);
		splot.getAxisX().setTicsStep(25);

		splot.getAxisY().setLabel("z-pos (meter)");
		splot.getAxisY().setLabelOffset(-0.3, -0.3 - 0.3);
		splot.getAxisY().setTicsOffset(0, -0.5);

		splot.getAxisY().setTicsStep(3);

		//splot.getAxisZ().setLabel("error (dB)");
		//splot.getAxisZ().setTicsStep(3);
		//splot.getAxisZ().setLabelRotation(90);
		splot.getAxisZ().setLabel("");
		splot.getAxisZ().setTicsVisible(false);
		splot.getAxisCB().setTicsStep(3);


		std::string name = Settings::fPathGFX + "/wifiop_show_optfunc_pos_yz";
		gp.setOutput(name + ".tex");
		gp.setTerminal("epslatex", K::GnuplotSize(7.7, 4.5));
		gp << "set palette defined (0 '#00ff00',  1 '#eeeeee', 9 '#222222')\n";
		gp << "set margins 0,0,0,0\n";
		gp << "set multiplot layout 1,1 scale 1.25,1.4 offset -0.04, 0.05\n";
		gp << "set colorbox horizontal user origin 0.57, 0.95 size 0.4, 0.03\n";
		gp << "set hidden3d front\n";

		// paper out
		splot.setStringMod(new K::GnuplotStringModLaTeX());
		splot.getView().setCamera(38, 109);

		const MACAddress mac("D8:84:66:4A:23:F0");
		const std::vector<WiFiFingerprint> fps = getFingerprints(mac);

		const Point3 pos_m(46-15, 47.4-15, 13.8);
		const float txp = -40;
		const float exp = 2.2;
		const float waf = -6;

		const int steps = 35;


		std::vector<std::vector<K::GnuplotPoint3>> hor;
		hor.resize(3);

		std::vector<std::vector<K::GnuplotPoint3>> ver;
		ver.resize(5);

		for (float sz = 0; sz < steps; ++sz) {
			for (int sy = 0; sy < steps; ++sy) {

				const float oz = -15.0f + 20.0f * sz / steps;
				const float oy = -35.0f + 90.0f * sy / steps;

				const Point3 pos = pos_m + Point3(0, oy, oz);

				WiFiModelLogDistCeiling model(map);
				model.addAP(mac, pos, txp, exp, waf);

				const K::Statistics<float> res = getError(mac, model, fps);
				const float err = res.getAvg();
				const K::GnuplotPoint3 gp3(pos.y, pos.z, err);
				mesh.add(gp3);

				// horizontal lines
				if (sy == 9)	{hor[0].push_back(gp3);}
				if (sy == 17)	{hor[1].push_back(gp3);}
				if (sy == 26)	{hor[2].push_back(gp3);}

				// vertical lines
				if (sz == 0)	{ver[0].push_back(gp3);}
				if (sz == 8)	{ver[1].push_back(gp3);}
				if (sz == 14)	{ver[2].push_back(gp3);}
				if (sz == 20)	{ver[3].push_back(gp3);}
				if (sz == 28)	{ver[4].push_back(gp3);}

				if (sz == 24)	{lines3.add(gp3);}

				if (sy ==  9 && sz == 24) {points.add(gp3 + K::GnuplotPoint3(0,0,0.10));}	// minor z-offset for Hidden3D to work
				if (sy == 26 && sz == 24) {points.add(gp3 + K::GnuplotPoint3(0,0,0.10));}	// minor z-offset for Hidden3D to work

			}
		}

		// horizontal lines
		for (const std::vector<K::GnuplotPoint3>& vec : hor) {
			for (const K::GnuplotPoint3 p3 : vec) {
				lines1.add(p3 + K::GnuplotPoint3(0,0,0.05));		// minor z-offset for Hidden3D to work
			}
			lines1.add(K::GnuplotPoint3::getEmpty());
			lines1.add(K::GnuplotPoint3::getEmpty());
		}

		// vertical lines
		for (const std::vector<K::GnuplotPoint3>& vec : ver) {
			for (const K::GnuplotPoint3 p3 : vec) {
				lines2.add(p3 + K::GnuplotPoint3(0,0,0.05));		// minor z-offset for Hidden3D to work
			}
			lines2.add(K::GnuplotPoint3::getEmpty());
			lines2.add(K::GnuplotPoint3::getEmpty());
		}

		gp.draw(splot);
		LeHelper::writeCode(name + ".gp", gp.getBuffer());
		gp.flush();

		int i = 0; (void) i;

	}

private:

	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;

	}


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

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

			res.add(aErr);

		}

		// done
		return res;

	}

};

#endif // EVALWIFICONVEX_H