#ifndef EVALWIFISIGSTRENGTH_H
#define EVALWIFISIGSTRENGTH_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/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 "Structs.h"
#include "plots/Plotty.h"
#include "CSV.h"
#include "Helper.h"

#include <unordered_set>


/**
 * read path
 * fetch wifi
 * use given model to estimate the most likely location
 * -> WIFI ONLY
 */
class EvalWiFiSigStrength {

private:

	Floorplan::IndoorMap* map;
	BBox3 mapBBox;

	WiFiFingerprints calib;

public:

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

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

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

		// load fingerprints
		calib = WiFiFingerprints(fpFile);

		// remove some stuff
		LeHelper::removeNonFHWS(calib);

	}

	float scale(const float val, const float min, const float max) {
		return (val - min) / (max-min);
	}


	void forAP_avg(Plotty* p, const MACAddress& mac) {

		VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

		// get AP/floor
		std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> _ap = FloorplanHelper::getAP(map, mac);
		const Floorplan::Floor* floor = _ap.second;
		const Floorplan::AccessPoint* ap = _ap.first;

		// copy
		WiFiFingerprints calib = this->calib;
		for (WiFiFingerprint& fp : calib.getFingerprints()) {
			fp.measurements = vap.group(fp.measurements);
		}

		// plot


		std::cout << ap->mac << std::endl;

		const Point3 apPos3 = ap->getPos(floor);
		K::GnuplotPoint3 apPos(apPos3.x, apPos3.y, apPos3.z);
		p->points.add(apPos);

		// process every fingerprint location
		for (const WiFiFingerprint& fp : calib.getFingerprints()) {

			// either 0 entries [ap not seen here] or 1 entry due to vap grouping [ap seen here]
			std::vector<WiFiMeasurement> mes = fp.getAllForMAC(mac);

			if (!mes.empty()) {
				const float rssi = mes.front().getRSSI();
				const float s = scale(rssi, -100, -40);
				const Color c = Color::fromHSV(s*100, 255, 255);
				p->addFloorRect(fp.pos_m, 1, c);
			}

		}

	}

	/** SPECIAL PAPER_PLOT OUTPUT */
	void forPaperNN(Plotty* p, const MACAddress& mac) {

		VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

		// get AP/floor
		std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> _ap = FloorplanHelper::getAP(map, mac);
		const Floorplan::Floor* floor = _ap.second;
		const Floorplan::AccessPoint* ap = _ap.first;

		// copy
		WiFiFingerprints calib = this->calib;
		for (WiFiFingerprint& fp : calib.getFingerprints()) {
			fp.measurements = vap.group(fp.measurements);
		}

		const Point3 apPos3 = ap->getPos(floor);
		K::GnuplotPoint3 apPos(apPos3.x, apPos3.y, apPos3.z);
		p->points.add(apPos);
		p->points.setPointSize(1.5);		// The AP

		// get all positions
		struct Entry {
			Point3 pos;
			float rssi;
			Entry(Point3 pos, float rssi) : pos(pos), rssi(rssi) {;}
		};
		std::vector<Entry> entries;

		// process every fingerprint location
		for (const WiFiFingerprint& fp : calib.getFingerprints()) {

			// either 0 entries [ap not seen here] or 1 entry due to vap grouping [ap seen here]
			std::vector<WiFiMeasurement> mes = fp.getAllForMAC(mac);

			if (!mes.empty()) {

				const float rssi = mes.front().getRSSI();
				Entry e(fp.pos_m - Point3(0,0,1.3), rssi);

				// only floor 0!!!!!
				if (e.pos.z != 0) {continue;}

				entries.push_back(e);

			}

		}

		const float ss = 1.0;

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

			// only floor 0!!!!!
			if (f->atHeight != 0) {continue;}

			BBox3 bb = FloorplanHelper::getBBox(f);
			for (float y = bb.getMin().y; y < bb.getMax().y; y += ss) {
				for (float x = bb.getMin().x; x < bb.getMax().x; x += ss) {


					const Point3 pos(x,y,f->atHeight);
//					float rssi = 0; float distSum = 0.01;
//					for (const Entry& e : entries) {
//						const float dist = e.pos.getDistance(pos);
//						const float imp = 1.0 / std::pow((dist), 4);
//						rssi += e.rssi * imp;
//						distSum += imp;
//						if (dist < 5) {rssi = e.rssi; distSum = 1;}
//					}
//					rssi /= distSum;

					auto comp = [&] (const Entry& e1, const Entry& e2) {return e1.pos.getDistance(pos) < e2.pos.getDistance(pos);};
					const auto& it = std::min_element(entries.begin(), entries.end(), comp);

					if (it->pos.getDistance(pos) > 2.99) {continue;}

//					const float rssi = it->rssi;
//					const float s = scale(rssi, -100, -40);
//					const Color c = Color::fromHSV(s*100, 255, 255);
//					p->addFloorRect(pos, ss/2, c);

				}
			}
		}

		for (Entry e : entries) {

			// FIX ONE FP NEAR THE AP
			if (e.pos.xy().getDistance(Point2(67.2,32.1)) < 0.1) {
				e.pos.y += 0.75;
			}

			p->gp << "set label '\\scriptsize{" << std::round(e.rssi) << "}' front at " << (e.pos.x-2) << "," << e.pos.y << "\n";

			const float rssi = e.rssi;
			const float s = scale(rssi, -100, -40);
			const Color c = Color::fromHSV(s*100, 255, 255);
			p->addFloorRect(e.pos, 2.6, c, 1.4);

		}

	}

	void perAP_avg() {

		std::vector<std::pair<Floorplan::AccessPoint*, Floorplan::Floor*>> aps = FloorplanHelper::getAPs(map);

		for (const auto& it : aps) {

			const Floorplan::Floor* floor = it.second;
			const Floorplan::AccessPoint* ap = it.first;
			const MACAddress mac(ap->mac);

			Plotty* p = new Plotty(map);
			p->setTitle(ap->mac);
			p->buildFloorplan();
			forAP_avg(p, mac);

			p->plot();

			int i = 0; (void) i;

			// MACs:
			// d8:84:66:4a:23:d0 <<<< BEST
			// d8:84:66:4a:25:c0
			// d8:84:66:4a:4c:60

		}

	}



};


#endif // EVALWIFISIGSTRENGTH_H