#ifndef EVALWIFIPATHS_H
#define EVALWIFIPATHS_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/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>

#include <KLib/math/statistics/Statistics.h>
#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/MovingMedian.h>

#include "../Structs.h"
#include "../plots/Plotty.h"
#include "../plots/PlotErrTime.h"
#include "../plots/PlotErrFunc.h"
#include "../plots/PlotWiFiGroundProb.h"
//#include "CSV.h"

#include <unordered_set>
#include <thread>
#include "../Settings.h"

/** evaluate just the wifi error for several given paths */
class EvalWiFiPaths {

private:

	Floorplan::IndoorMap* map;
	BBox3 mapBBox;

	VAPGrouper* vap = nullptr;

	K::Statistics<float>* stats_m;
	PlotErrFunc* pef_m;

	K::Statistics<float>* stats_p;
	PlotErrFunc* pef_p;

	WiFiModel* wiModel = nullptr;

	int idx = -1;

public:

	/** ctor with map and fingerprints */
	EvalWiFiPaths(const std::string& mapFile) {

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

		// estimate bbox
		mapBBox = FloorplanHelper::getBBox(map);

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

		pef_m = new PlotErrFunc("error (m)", "measurements (%)");
		pef_m->showMarkers(false, false);

		pef_p = new PlotErrFunc("-log(p(..))", "measurements (%)");
		pef_p->showMarkers(false, false);
		pef_p->getPlot().getAxisX().setRange(K::GnuplotAxis::Range(K::GnuplotAxis::Range::AUTO, K::GnuplotAxis::Range::AUTO));

	}


	void writeGP(const std::string& path, const std::string& name) {
		writeGP(*pef_m, K::GnuplotSize(8.6, 3.3), path + "/" + name + "_meter");
		writeGP(*pef_p, K::GnuplotSize(8.6, 3.3), path + "/" + name + "_logprob");
	}

	void writeGP(PlotErrFunc& pef, K::GnuplotSize size, const std::string& file) {
		pef.getGP().setTerminal("epslatex", size);
		pef.getGP().setOutput(file+".tex");
		pef.getPlot().getKey().setVisible(true);
		pef.getPlot().getKey().setSampleLength(0.5);
		pef.getPlot().getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::BOTTOM);
		pef.getPlot().getKey().setWidthIncrement(7);
		pef.getPlot().getAxisY().setLabelOffset(2.5, 0);
		//pef.getPlot().getAxisY().setTicsStep(0, 25, 95);
		pef.getPlot().getAxisY().setTicsStep({0, 25, 50, 75, 95});
		pef.getPlot().getAxisX().setTicsLabelFormat("%h m");		// CHECK!

		// MANUAL AXIS RANGE SETTINGS
		pef.getPlot().getAxisX().setRange(K::GnuplotAxis::Range(0,25));
		pef.getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0,95));

		pef.getPlot().getAxisX().setLabel("");
		pef.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
		pef.getPlot().getMargin().set(4.5, 0.4, 0.1, 2.0);
		pef.writePlotToFile(file+".gp");
		pef.plot();
		pef.writePlotToFile("");
	}



	void loadModel(const std::string& xmlFile, const std::string& name) {

		++idx;

		WiFiModelFactory fac(map);

		// setup the model
		this->wiModel = fac.loadXML(xmlFile);

		stats_m = new K::Statistics<float>();
		stats_p = new K::Statistics<float>();

		pef_m->add(name, stats_m);
		pef_p->add(name, stats_p);

	}

	void walks (const std::vector<std::string> files, const std::vector<std::vector<int>> gtIndicies) {
		for (size_t i = 0; i < files.size(); ++i) {
			walk(files[i], gtIndicies[i]);
		}
	}

	void walk(const std::string& fPath, const std::vector<int> gtIndices) {

		static PlotErrTime pet_m("","",""); pet_m.clear();
		static PlotErrTime pet_p("","",""); pet_p.clear();
		Offline::FileReader reader(fPath);

		const Offline::FileReader::GroundTruth gtp = reader.getGroundTruth(map, gtIndices);

		// process each wifi entry within the offline file
		for (const auto wifi : reader.wifi) {

			// all seen APs at one timestamp
			const WiFiMeasurements& _mes = wifi.data;

			// perform vap grouping
			const WiFiMeasurements mes = vap->group(_mes);

			// error calculation
			auto func = [&] (const float* params) -> double {

				// crop z to 1 meter
				//params[2] = std::round(params[2]);

				// suggested position
				const Point3 pos_m(params[0], params[1], params[2]);

				const float sigma = 8.0;
				double prob = 1.0;

				// calculate error for above position using the currently available measurements
				for (const WiFiMeasurement& m : mes.entries) {

					// skip non-FHWS APs
					if (!LeHelper::isFHWS_AP(m.getAP().getMAC())) {continue;}

					// get model's rssi for the given location
					const float rssi_model = wiModel->getRSSI(m.getAP().getMAC(), pos_m);

					// skip APs unknown to the model
					if (rssi_model != rssi_model) {
						std::cout << "unknown ap: " << m.getAP().getMAC().asString() << std::endl;
						continue;
					}

					// get scan's rssi
					const float rssi_scan = m.getRSSI();

					// likelyhood
					const double p = Distribution::Normal<double>::getProbability(rssi_model, sigma, rssi_scan);
					//const double p = Distribution::Region<double>::getProbability(rssi_model, sigma, rssi_scan);

					// adjust
					prob *= p;

				}

				const double err = -prob;
				return err;

			};

			// parameters (x,y,z);
			float params[3];

			// USE RANGE RANDOM WITH COOLING
			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
			 };

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

			// estimation
			std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
			const Point3 curEst(params[0], params[1], params[2]);
			const Timestamp ts = mes.entries.front().getTimestamp();

			// groud-truth
			const Point3 gt = gtp.get(ts);

			// error in meter
			//const float err_m = gt.xy().getDistance(curEst.xy());		// 2D
			const float err_m = gt.getDistance(curEst);				// 3D
			stats_m->add(err_m);
			pet_m.addErr(ts, err_m, idx);

			// error in -log(p)
			float gtFloat[3] = {gt.x, gt.y, gt.z};
			const double probOnGT = -func(gtFloat);
			const double logProbOnGT = -std::log10(probOnGT);
			const double logProbOnGTNorm = (logProbOnGT/mes.entries.size());
			stats_p->add(logProbOnGTNorm);
			pet_p.addErr(ts, logProbOnGTNorm, idx);

			static int xx = 0; ++xx;

			if (xx % 10 == 0) {
				pet_p.plot();
				pet_m.plot();

				pef_p->plot();
				pef_m->plot();
			}

			std::this_thread::sleep_for(std::chrono::milliseconds(1));

		}

	}

};

#endif // EVALWIFIPATHS_H