#ifndef EVALWIFI_H
#define EVALWIFI_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 "Plotty.h"
#include "CSV.h"

#include <unordered_set>

template <typename T> class Line {

private:

	std::vector<T> elements;

public:

	void add(const T& elem) {
		elements.push_back(elem);
	}

	std::vector<T> getAverage(const int size) {
		std::vector<T> res;
		for (int i = 0; i < (int)elements.size(); ++i) {

			T sum;
			int cnt = 0;

			// calculate sume of all elements around i
			for (int j = -size; j <= +size; ++j) {
				int idx = i+j;
				if (idx < 0) {continue;}
				if (idx >= elements.size()) {continue;}
				sum += elements[idx];
				++cnt;
			}

			// calculate average
			T avg = sum / cnt;
			res.push_back(avg);

		}
		return res;
	}

};

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

private:

	Floorplan::IndoorMap* map;
	BBox3 mapBBox;

	//WiFiFingerprints* calib;
	VAPGrouper* vap = nullptr;
	//WiFiOptimizer::LogDistCeiling* opt;

	Offline::FileReader reader;
	WiFiModel* wiModel = nullptr;
	std::vector<int> gtIndices;

public:

	/** ctor with map and fingerprints */
	EvalWiFi(const std::string& mapFile, const std::string& fPath, const std::vector<int> gtIndices) : reader(fPath), gtIndices(gtIndices) {

		std::cout << "EvalWiFi for " << fPath << std::endl;

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

		// the optimizer
	//	opt = new WiFiOptimizer::LogDistCeiling(map, *vap, *calib, WiFiOptimizer::LogDistCeiling::Mode::MEDIUM);

	}

	void fixedParams(const float txp, const float exp, const float waf) {

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

		// setup the model
		WiFiModelLogDistCeiling* wiModel = new WiFiModelLogDistCeiling(map);
		wiModel->loadAPs(map, *vap, txp, exp, waf, false);
		this->wiModel = wiModel;

		// fire
		run();

	}

private:

	/** is the given mac one of a FHWS ap? */
	bool isFHWS_AP(const MACAddress& mac) {
		return mac.asString().substr(0,5) == "D8:84";
	}

	void run() {

		Plotty* plot = new Plotty(map);
		Line<Point3> path;
		plot->setGroundTruth(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;

			// debug output
			std::cout << wifi.ts << ":" << _mes.entries.size() << std::endl;

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


			// error calculation
			auto func = [&] (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 = 6.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 (!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);

					// adjust
					prob *= p;

				}

				const double err = -prob;
				return err;

			};

			std::minstd_rand gen;
			std::uniform_real_distribution<float> distX(mapBBox.getMin().x, mapBBox.getMax().x);
			std::uniform_real_distribution<float> distY(mapBBox.getMin().y, mapBBox.getMax().y);
			std::uniform_real_distribution<float> distZ(mapBBox.getMin().z, mapBBox.getMax().z);

			// initializer for the optimizer: random position within the map's bbox
			auto init = [&] (const int childIdx, float* params) {
				params[0] = distX(gen);
				params[1] = distY(gen);
				params[2] = distZ(gen);
			};


			float params[3];
			K::NumOptAlgoGenetic<float> opt(3);
			opt.setPopulationSize(200);
			opt.setMaxIterations(20);
			opt.calculateOptimum(func, params, init);

			std::cout << params[0] << "," << params[1] << "," << params[2] << std::endl;
			path.add(Point3(params[0], params[1], params[2]));

			// draw a smoothed version of th epath
			plot->pathEst.clear();
			for (const Point3 p : path.getAverage(1)) {
				const K::GnuplotPoint3 gp3(p.x, p.y, p.z);
				plot->pathEst.add(gp3);
			}
			plot->plot();


		}

	}

//	// TODO
//	void abc(const std::string& fpFile) {

//		// load fingerprints
//		calib = new WiFiFingerprints(fpFile);

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

//		// the optimizer
//		opt = new WiFiOptimizer::LogDistCeiling(map, *vap, *calib, WiFiOptimizer::LogDistCeiling::Mode::MEDIUM);

//	}


//	static void dumpWiFiCenterForPath(coconst std::string& fPath) {

//		std::cout << "dump WiFi for " << fPath << std::endl;

//		Offline::FileReader fr(fPath);

//		WiFiModel logDistC

//		for (const auto wifi : fr.wifi) {
//			std::cout << wifi.ts << ":" << wifi.data.entries.size() << std::endl;
//		}


//	}


};

#endif // EVALWIFI_H