#ifndef EVALWIFIOPTRESULT_H
#define EVALWIFIOPTRESULT_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/VAPGrouper.h"
#include "Indoor/sensors/offline/FileReader.h"
#include "Indoor/sensors/radio/WiFiQualityAnalyzer.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/grid/factory/v2/Helper.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/misc/gnuplot/objects/GnuplotObjectRectangle.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"

/** plot results of the wifi optimization */
class EvalWiFiOptResult {

	Floorplan::IndoorMap* map;
	VAPGrouper vap;

public:

	/** ctor */
	EvalWiFiOptResult(const std::string& mapFile) : vap(Settings::WiFiModel::vg_eval) {
		map = Floorplan::Reader::readFromFile(mapFile);
	}

	/**
	 * plot the error for every fingerprint
	 */
	template <typename Model> Plotty* showErrorPerFingerprint(const std::string& modelParamsXML, const WiFiFingerprints& fps) {

		Plotty* plot = new Plotty(map);

		plot->settings.obstacles = true;
		plot->settings.outline = false;
		plot->settings.stairs = false;
		plot->settings.outlineColorCustom = true;
		plot->settings.outlineColor = K::GnuplotColor::fromRGB(0,0,0);
		plot->buildFloorplan();
		plot->equalXY();

		plot->gp << "unset border\n";
		plot->splot.getAxisX().setTicsVisible(false);
		plot->splot.getAxisY().setTicsVisible(false);
		plot->splot.getAxisZ().setTicsVisible(false);
		plot->setScale(1.5, 1.5);

		// the sig strength model
		Model model(map);
		model.loadXML(modelParamsXML);
//		WiFiModelLogDistCeiling model(map);
//		model.loadXML(modelParamsXML);

		// combine position and error
		struct ErrPoint {
			Point3 pos;
			float err;
			ErrPoint(const Point3 pos, const float err) : pos(pos), err(err) {;}
		};

		std::vector<ErrPoint> errors;

		// process each location
		for (const WiFiFingerprint& fp : fps.getFingerprints()) {

			// estimate error for this location
			K::Statistics<float> errAbs;

			// vap grouping
			const WiFiMeasurements mes = vap.group(fp.measurements);

			// process each measured AP
			for (const WiFiMeasurement& m : mes.entries) {

				// fingerprint RSSI
				const float scan_rssi = m.getRSSI();

				// model RSSI
				const float model_rssi = model.getRSSI(m.getAP().getMAC(), fp.pos_m);

				// AP unknown -> skip
				if (model_rssi != model_rssi) {continue;}

				// update error
				const float err = model_rssi - scan_rssi;
				errAbs.add(std::abs(err));

			}

			// enqueue AVG error
			//errors.push_back(ErrPoint(fp.pos_m, errAbs.getAvg()));

			// enqueue MAX error
			errors.push_back(ErrPoint(fp.pos_m, errAbs.getMax()));

		}

		// interpolated error points
		std::vector<ErrPoint> errorsInt;
		K::Statistics<float> errStats;
		const float oz = 1.3;
		const float ss = 4.0;

		for (Floorplan::Floor* floor : map->floors) {

			for (float y = -20; y < 70; y+=ss) {
				for (float x = -10; x < 130; x+=ss) {

					const Point3 pos(x,y,floor->atHeight+oz);
					float errSum = 0;
					float distSum = 0;
					int cnt = 0;

					float minDist = 99999;

					for (const ErrPoint& ep : errors) {
						if (ep.pos.z != pos.z) {continue;}
						const float dist = ep.pos.getDistance(pos);
						//if (dist > 4.0) {continue;}
						//const float imp = 1.0 / (std::pow((dist+0.01)/4.0, 3));
						//errSum += ep.err * imp;
						//distSum += imp;
						if (dist < minDist) {
							errSum = ep.err;
							distSum = 1.0;
							minDist = dist;
						}


						++cnt;
					}

					// limit estimations to the floorplan's outline
					bool contained = false;
					for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
						HelperPoly hp(*poly);
						if (hp.contains(pos.xy()*100)) {
							if (poly->method == Floorplan::OutlineMethod::ADD) {
								contained = true;
							} else {
								//contained = false; break;
							}
						}
					}

					if (distSum == 0) {continue;}
					if (!contained) {continue;}
					const float err = errSum/distSum;

					// add
					errStats.add(err);
					errorsInt.push_back(ErrPoint(pos, err));

				}
			}

		}

		const float minErr = 0.0;//errStats.getMin();
		const float maxErr = 20.0;//errStats.getQuantile(0.9);

		//plot->splot.setTitle("max error: " + std::to_string(errStats.getMax()) + " dB");

		// draw interpolated errors between min/max
		for (const ErrPoint& ep : errorsInt) {

			float factor = (ep.err - minErr) / (maxErr-minErr);
			if (factor > 1) {factor = 1;}
			if (factor < 0) {factor = 0;}

			// hsv green->yellow->red
//			const float h = 90 * (1 - std::pow(factor, 1.0));
//			K::GnuplotColor color = K::GnuplotColor::fromHSV(h, 255, 255);

			// scale
			//factor = std::pow(factor, 2.0);

			// hsv white->red
			const float sat = 255 * factor;
			const float val = 255;//255 - 128 * factor;
			K::GnuplotColor color = K::GnuplotColor::fromHSV(0, sat, val);

			K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(
				K::GnuplotFill(K::GnuplotFillStyle::SOLID, color),
				K::GnuplotStroke::NONE()
			);

			// slightly overlapping??
			const float s = ss * 0.50;

			poly->add(K::GnuplotCoordinate3(ep.pos.x-s, ep.pos.y-s, ep.pos.z-oz, K::GnuplotCoordinateSystem::FIRST));
			poly->add(K::GnuplotCoordinate3(ep.pos.x+s, ep.pos.y-s, ep.pos.z-oz, K::GnuplotCoordinateSystem::FIRST));
			poly->add(K::GnuplotCoordinate3(ep.pos.x+s, ep.pos.y+s, ep.pos.z-oz, K::GnuplotCoordinateSystem::FIRST));
			poly->add(K::GnuplotCoordinate3(ep.pos.x-s, ep.pos.y+s, ep.pos.z-oz, K::GnuplotCoordinateSystem::FIRST));
			poly->close();
			poly->setZIndex(ep.pos.z - 1.5);
			plot->splot.getObjects().add(poly);

		}

		plot->splot.getObjects().reOrderByZIndex();
		plot->plot();

		return plot;

	}
























	Plotty* showWifiQuality(const std::vector<Offline::FileReader>& readers, const std::vector<std::vector<int>>& paths) {

		Plotty* plot = new Plotty(map);

		plot->settings.obstacles = true;
		plot->settings.outline = false;
		plot->settings.stairs = false;
		plot->settings.outlineColorCustom = true;
		//plot->settings.outlineColor = K::GnuplotColor::fromRGB(0,0,0);
		plot->buildFloorplan();
		plot->equalXY();

		plot->gp << "unset border\n";
		plot->splot.getAxisX().setTicsVisible(false);
		plot->splot.getAxisY().setTicsVisible(false);
		plot->splot.getAxisZ().setTicsVisible(false);
		plot->setScale(1.5, 1.5);

		// combine position and quality
		struct ErrPoint {
			Point3 pos;
			float err;
			ErrPoint(const Point3 pos, const float err) : pos(pos), err(err) {;}
		};

		std::vector<ErrPoint> errors;

		// process each location
		for (int i = 0; i < readers.size(); ++i) {

			const Offline::FileReader& reader = readers[i];
			Offline::FileReader::GroundTruth gt = reader.getGroundTruth(map, paths[i]);

			WiFiQualityAnalyzer analyzer;

			for (const auto& entry : reader.getWiFiGroupedByTime()) {

				analyzer.add(entry.data);

				Point3 pt = gt.get(Timestamp::fromMS(entry.ts));
				const float quality = analyzer.getQuality();

				// enqueue MAX error
				errors.push_back(ErrPoint(pt, quality));

			}
		}

		// interpolated error points
		std::vector<ErrPoint> errorsInt;
		K::Statistics<float> errStats;
		const float oz = 1.3;
		const float ss = 4.0;

		for (Floorplan::Floor* floor : map->floors) {

			for (float y = -20; y < 70; y+=ss) {
				for (float x = -10; x < 130; x+=ss) {

					const Point3 pos(x,y,floor->atHeight+oz);
					float errSum = 0;
					float distSum = 0;
					int cnt = 0;

					float minDist = 99999;

					for (const ErrPoint& ep : errors) {
						//if (ep.pos.z != pos.z) {continue;}
						const float dist = ep.pos.getDistance(pos);
						//if (dist > 4.0) {continue;}
						//const float imp = 1.0 / (std::pow((dist+0.01)/4.0, 3));
						//errSum += ep.err * imp;
						//distSum += imp;
						if (dist < minDist) {
							errSum = ep.err;
							distSum = 1.0;
							minDist = dist;
						}


						++cnt;
					}

					// limit estimations to the floorplan's outline
					bool contained = false;
					for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
						HelperPoly hp(*poly);
						if (hp.contains(pos.xy()*100)) {
							if (poly->method == Floorplan::OutlineMethod::ADD) {
								contained = true;
							} else {
								//contained = false; break;
							}
						}
					}

					if (distSum == 0) {continue;}
					if (!contained) {continue;}
					const float err = errSum/distSum;

					// add
					errStats.add(err);
					errorsInt.push_back(ErrPoint(pos, err));

				}
			}

		}

		const float minErr = 0.0;//errStats.getMin();
		const float maxErr = 20.0;//errStats.getQuantile(0.9);

		//plot->splot.setTitle("max error: " + std::to_string(errStats.getMax()) + " dB");

		// draw interpolated errors between min/max
		for (const ErrPoint& ep : errorsInt) {

			float factor = ep.err;

			factor = std::pow(factor, 0.5);

			// hsv white->red
			const float sat = 128 - 128 * factor;
			const float val = 255;//128 + 127 * factor;
			K::GnuplotColor color = K::GnuplotColor::fromHSV(0, sat, val);

			K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(
				K::GnuplotFill(K::GnuplotFillStyle::SOLID, color),
				K::GnuplotStroke::NONE()
			);

			const float s = ss/2;

			poly->add(K::GnuplotCoordinate3(ep.pos.x-s, ep.pos.y-s, ep.pos.z-oz, K::GnuplotCoordinateSystem::FIRST));
			poly->add(K::GnuplotCoordinate3(ep.pos.x+s, ep.pos.y-s, ep.pos.z-oz, K::GnuplotCoordinateSystem::FIRST));
			poly->add(K::GnuplotCoordinate3(ep.pos.x+s, ep.pos.y+s, ep.pos.z-oz, K::GnuplotCoordinateSystem::FIRST));
			poly->add(K::GnuplotCoordinate3(ep.pos.x-s, ep.pos.y+s, ep.pos.z-oz, K::GnuplotCoordinateSystem::FIRST));
			poly->close();
			poly->setZIndex(ep.pos.z - 1.5);
			plot->splot.getObjects().add(poly);

		}

		plot->splot.getObjects().reOrderByZIndex();
		plot->plot();

		return plot;

	}








};

#endif // EVALWIFIOPTRESULT_H
