#ifndef EVALAPOPT_H
#define EVALAPOPT_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/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>

void plot(const K::SimpleHistogram& h) {

	K::Gnuplot* gp = new K::Gnuplot();

	K::GnuplotPlot plot;
	K::GnuplotPlotElementHistogram hist; plot.add(&hist);

	hist.add(h);

	gp->draw(plot);
	gp->flush();

}


class EvalApOpt {

	Floorplan::IndoorMap* map;
	WiFiFingerprints calib;
	VAPGrouper* vap;
	WiFiOptimizer::LogDistCeiling* opt;
	Floorplan::Ceilings ceilings;
	CSV csv;

public:

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

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

		// load fingerprints
		calib = WiFiFingerprints(fpFile);


		calib = LeHelper::removeOutdoor(calib);
		calib = LeHelper::removeStaircases(calib);
		LeHelper::plot(map, calib);


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

		// some ceiling calculations
		ceilings = Floorplan::Ceilings(map);

		// get average error
		//opt->optimizeAll(opt->MIN_5_FPS);
		return;

	}

	/** optimize all access-points given by the scanned fingerprints */
	std::vector<OptStats> optAll() {

		std::vector<OptStats> results;
		CSV csv;
		K::Statistics<float> statsAbs;
		K::Statistics<float> statsSigned;

		// all APs known to the map
		std::unordered_set<MACAddress> mapAPs;
		for (const Floorplan::Floor* f : map->floors) {
			for (const Floorplan::AccessPoint* ap : f->accesspoints) {
				mapAPs.insert(ap->mac);
			}
		}

		// process each AP seen during fingerprinting
		for (const MACAddress& mac : opt->getAllMACs()) {

			// but: skip non-FHWS accesspoints [smartphones, portable stuff, ..]
			if (mac.asString().substr(0,5) != "D8:84") {
				std::cerr << "skipping non FHWS AP: " << mac.asString() << std::endl << std::endl;
				continue;
			}

			// optimize
			const OptStats result = optOne(mac);

			// optimization will only work out, if there are enough fingerprints
			if (result.opt.usedFingerprins < 10) {
				csv.addNotEnoughFingerprints(result);
				std::cerr << "skipping AP due to not enough fingerprints: " << mac.asString() << std::endl << std::endl;
				continue;
			}

			plot(result);

			results.push_back(result);
			dumpStats(result);
			csv.add(result, ceilings);
			//if (mac == MACAddress("D8:84:66:4A:22:B0")) {plot(result);}		// extrema

			K::SimpleHistogram hist(3.0);
			for (const auto err : result.opt.errors) {
				//std::cout << err.getError_db() << std::endl;
				hist.add(err.getError_db(), 1.0);
				statsAbs.add( std::abs(err.getError_db()) );
				statsSigned.add( err.getError_db() );
			}
			//::plot(hist);

			int i = 0; (void) i;

		}

		// average error
		std::cout << "OVERALL RESULT: " << std::endl;
		std::cout << "error signed: " << statsSigned.asString() << std::endl;
		std::cout << "error unsigned: " << statsAbs.asString() << std::endl;
		std::cout << "--------------------------------------------------------" << std::endl;

		// all APs known to the map but invisible during fingerprinting [should not happen]
		std::unordered_set<MACAddress> mapUnseen = mapAPs;
		for (const MACAddress& mac : opt->getAllMACs()) {mapUnseen.erase(mac);}
		for (const MACAddress& mac : mapUnseen) {csv.addUnseen(mac);}

		csv.dump();

		return results;

	}

	/** analyze the given optimization result obtained by optOne(mac) */
	void dumpStats(const OptStats& stats) {

		std::cout << "stats " << stats.mac.asString() << std::endl;

		if (!stats.knownToMap()) {
			std::cerr << "\t" << "AP NOT KNOWN TO MAP!" << std::endl << std::endl;
			return;
		}

		std::cout << "\t" << "error: " << stats.getEstErrorAvg() << " dB" << std::endl;
		std::cout << "\t" << "max neg: " << stats.getEstErrorMaxNeg().getError_db() << " dB" << std::endl;
		std::cout << "\t" << "max pos: " << stats.getEstErrorMaxPos().getError_db() << " dB" << std::endl;

		std::cout << "\t" << "real position: " << stats.realPos.asString() << " m" << std::endl;
		std::cout << "\t" << "est. position: " << stats.getEstPos().asString() << " m" << std::endl;
		std::cout << "\t" << "real/est difference: " << stats.getRealEstDiff().length() << " m" << std::endl;
		std::cout << "\t" << "ceiling delta: " << ceilings.numCeilingsBetween(stats.realPos, stats.getEstPos()) << std::endl;

		std::cout << std::endl;

	}



	/** plot the given optimization result obtained by optOne(mac) */
	void plot(const OptStats& stats) {

		// plot
		Plotty plot(map);

		if (stats.name != "") {
			plot.setTitle(stats.name);
			plot.addLabel("x realPos", stats.realPos);
		} else {
			plot.setTitle(stats.mac.asString() + " ??????");
		}

		plot.addLabel("x estPos", Point3(stats.params.x, stats.params.y, stats.params.z));

		for (const WiFiOptimizer::ErrorAtPosition& err : stats.opt.errors) {

			const float delta = err.model_rssi - err.scan_rssi;
			const Point3 pos = err.pos_m;

			plot.cpoints.add(K::GnuplotPoint3(pos.x, pos.y, pos.z), delta);

		}

		plot.setPaletteRedBlue();

		//		for (const WiFiOptimizer::APParamsMAC& ap : list.get()) {
		//			const K::GnuplotPoint3 p3(ap.params.x, ap.params.y, ap.params.z);
		//			points.add(p3);
		//			const Floorplan::AccessPoint* fap = FloorplanHelper::getAP(map, ap.mac);
		//			std::string lbl = (fap) ? (fap->name) : (ap.mac.asString());
		//			if (!fap) {
		//			gp << "set label '" << lbl << "' at " << ap.params.x+1 << "," << ap.params.y+1 << "," << ap.params.z+0.3 << "\n";

		//				std::cout << "AP missing in Map: " << ap.mac.asString() << " @ " << ap.params.x << "," << ap.params.y << "," << ap.params.z << std::endl;
		//			}
		//		}

		if (1 == 0) {
			plot.plot();
		}
		return;

	}


	OptStats optOne(const MACAddress& mac) {

		OptStats optStats;
		optStats.mac = mac;

		const WiFiOptimizer::LogDistCeiling::APParams params = opt->optimize(mac, optStats.opt);
		const std::vector<WiFiFingerprint> fps = calib.getFingerprintsFor(vap->getBaseMAC(mac));

		optStats.params = params;

		// fetch AP from MAP
		std::pair<Floorplan::AccessPoint*, Floorplan::Floor*> ap = FloorplanHelper::getAP(map, mac);

		// ap known to the map?
		if (ap.first) {
			optStats.name = ap.first->name;
			optStats.realPos = ap.first->getPos(ap.second);
		}

		return optStats;

	}


};

#endif // EVALAPOPT_H