#define  BREAK		raise(SIGTRAP);

#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 <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <Indoor/geo/EarthMapping.h>

#include "Settings.h"

#include "EvalCompareOpt.h"
#include "EvalCompareOpt2.h"

#include "EvalApOpt.h"
#include "EvalData.h"
#include "EvalWiFi.h"
#include "EvalWiFiSigStrength.h"
#include "pf/EvalWalk.h"

#include "plots/PlotErrFunc.h"

// build plots for the paper
void paperOutputs() {

	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap);

	// show fingerprints as plot
	if (1==0){
		EvalWiFiSigStrength sig(Settings::fMap, Settings::fCalib);
		Plotty* p = new Plotty(map);
		p->writeCodeTo(Settings::fPathGFX + "compare-wifi-in-out.gp");
		p->writeEpsTex(Settings::fPathGFX + "compare-wifi-in-out.tex");
		p->settings.floors = {0};
		p->settings.maxZ = 1;
		p->buildFloorplan();
		sig.forAP_avg(p, MACAddress("d8:84:66:4a:23:d0"));
		p->equalXY();
		p->setView(0,0);
		p->setScale(3.1, 3.1);
		p->addRectangle(Point3(62, 24, 0), Point3(72, 34, 0), Color::fromRGB(0,0,0), false, false);
		p->noFrame();
		p->plot();
		delete p;
	}

	// perform varios AP-param optimizations
	// generate error plot showing the performance of each
	// save the resulting wifi-models to XML for later re-use during the walk-eval  <<<<<< !!!!
	if (1 == 1) {

//		// use walks?
//		std::vector<std::pair<std::string, std::vector<int>>> calibWalks = {
//			std::make_pair(Settings::path1a, Settings::GroundTruth::path1),
//			std::make_pair(Settings::path1b, Settings::GroundTruth::path1),
//			std::make_pair(Settings::path2a, Settings::GroundTruth::path2),
//			std::make_pair(Settings::path2b, Settings::GroundTruth::path2),
//		};
//		EvalCompareOpt2 opt1(Settings::fMap, calibWalks);

		auto remove = [] (const WiFiFingerprint& fp) -> bool {
			return fp.pos_m.z != 4;
		};

		// use fingerprints?
		bool ignoreStaircases = false;
		bool ignoreOutdoor = false;
		bool ignoreIndoor = false;
		EvalCompareOpt2 opt1(Settings::fMap, Settings::fCalib, remove);


		EvalCompareOpt2::Result s1 = opt1.fixedPosFixedParamsForAll(); //BREAK;
		EvalCompareOpt2::Result s2 = opt1.fixedPosOptParamsForAll(); //BREAK;
		EvalCompareOpt2::Result s3 = opt1.fixedPosOptParamsForEach(); //BREAK;
		EvalCompareOpt2::Result s4 = opt1.optPosOptParamsForEach(); //BREAK;


		// save models to file
		s1.model.saveXML(Settings::wifiAllFixed);
		s2.model.saveXML(Settings::wifiAllOptPar);
		s3.model.saveXML(Settings::wifiEachOptPar);
		s4.model.saveXML(Settings::wifiEachOptParPos);

		PlotErrFunc pef("\\small{error (dB)}", "\\small{fingerprints (\\%)}");
		pef.add("\\small{empiric}", &s1.errSingle);
		pef.add("\\small{real pos, opt params}", &s2.errSingle);
		pef.add("\\small{real pos, opt params [per AP]}", &s3.errSingle);
		pef.add("\\small{opt pos, opt params [per AP]}", &s4.errSingle);

		pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.5, 5));
		pef.getGP().setOutput(Settings::fPathGFX + "wifi-opt-error-hist-methods.tex");
		pef.writeCodeTo(Settings::fPathGFX + "wifi-opt-error-hist-methods.gp");
		pef.getGP() << "set key right bottom width -4 samplen 0.5\n";
		pef.getGP() << "set rmargin 0.4\n";
		pef.getGP() << "set tmargin 0.4\n";
		pef.plot();

		return;
		//sleep(1000);



	}

	// REPLACED BY EVAL OPT 2
//	// perform varios AP-param optimizations
//	// generate error plot showing the performance of each
//	// save the resulting wifi-models to XML for later re-use during the walk-eval  <<<<<< !!!!
//	if (1 == 0) {

//		bool ignoreStaircases = false;
//		bool ignoreOutdoor = false;
//		bool ignoreIndoor = false;

//		// use walks?
////		std::vector<std::pair<std::string, std::vector<int>>> calibWalks = {
////			std::make_pair(Settings::path1a, Settings::GroundTruth::path1),
////			std::make_pair(Settings::path1b, Settings::GroundTruth::path1),
////			std::make_pair(Settings::path2a, Settings::GroundTruth::path2),
////			std::make_pair(Settings::path2b, Settings::GroundTruth::path2),
////		};
////		EvalCompareOpt opt1(Settings::fMap, calibWalks);

//		// use fingerprints?
//		EvalCompareOpt opt1(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor);

//		EvalCompareOpt::Result s1 = opt1.fixedPosFixedParamsForAll(); //BREAK;
//		EvalCompareOpt::Result s2 = opt1.fixedPosOptParamsForAll(); //BREAK;
//		EvalCompareOpt::Result s3 = opt1.fixedPosOptParamsForEach(); //BREAK;
//		EvalCompareOpt::Result s4 = opt1.optPosOptParamsForEach(); //BREAK;

//		ignoreStaircases = true;
//		ignoreOutdoor = true;
//		EvalCompareOpt opt2(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor);
//		EvalCompareOpt::Result s4b = opt2.optPosOptParamsForEach(); //BREAK;

//		// save models to file
//		s1.model.saveXML(Settings::wifiAllFixed);
//		s2.model.saveXML(Settings::wifiAllOptPar);
//		s3.model.saveXML(Settings::wifiEachOptPar);
//		s4.model.saveXML(Settings::wifiEachOptParPos);
//		s4b.model.saveXML(Settings::wifiEachOptParPosNoStairNoOut);

//		PlotErrFunc pef("\\small{error (dB)}", "\\small{fingerprints (\\%)}");
//		pef.add("\\small{empiric}", s1.errAbs);
//		pef.add("\\small{real pos, opt params}", s2.errAbs);
//		pef.add("\\small{real pos, opt params [per AP]}", s3.errAbs);
//		pef.add("\\small{opt pos, opt params [per AP]}", s4.errAbs);
//		pef.add("\\small{opt pos, opt params [per AP, no stair, no out]}", s4b.errAbs);

//		pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.5, 5));
//		pef.getGP().setOutput(Settings::fPathGFX + "wifi-opt-error-hist-methods.tex");
//		pef.writeCodeTo(Settings::fPathGFX + "wifi-opt-error-hist-methods.gp");
//		pef.getGP() << "set key right bottom width -4 samplen 0.5\n";
//		pef.getGP() << "set rmargin 0.4\n";
//		pef.getGP() << "set tmargin 0.4\n";
//		pef.plot();

//	}

	// error histogram all pos, all params, between in/out/stair, in/out, in/stair, in
	if(1==0){

		EvalCompareOpt e1(Settings::fMap, Settings::fCalib, false, false, false);
		EvalCompareOpt e2(Settings::fMap, Settings::fCalib, true, false, false);
		EvalCompareOpt e3(Settings::fMap, Settings::fCalib, false, true, false);
		EvalCompareOpt e4(Settings::fMap, Settings::fCalib, true, true, false);

		K::Statistics<float> s1 = e1.optPosOptParamsForEach().errAbs;
		K::Statistics<float> s2 = e2.optPosOptParamsForEach().errAbs;
		K::Statistics<float> s3 = e3.optPosOptParamsForEach().errAbs;
		K::Statistics<float> s4 = e4.optPosOptParamsForEach().errAbs;

		PlotErrFunc pef("\\small{error (dB)}", "\\small{fingerprints (\\%)}");
		pef.add("\\small{floor + stairs + out}", &s1);
		pef.add("\\small{floor + out}", &s2);
		pef.add("\\small{floor + stairs}", &s3);
		pef.add("\\small{floor}", &s4);

		pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.5, 5));
		pef.getGP().setOutput(Settings::fPathGFX + "wifi-opt-error-hist-stair-outdoor.tex");
		pef.writeCodeTo(Settings::fPathGFX + "wifi-opt-error-hist-stair-outdoor.gp");
		pef.getGP() << "set key right bottom width -3\n";
		pef.getGP() << "set rmargin 0.4\n";
		pef.getGP() << "set tmargin 0.4\n";
		pef.plot();

	}

	// wifi issue for path1
	if (1 == 0) {

		Offline::FileReader reader(Settings::path1a);
		PlotWifiMeasurements plot;

		for (int i = 0; i < 60; ++i) {
			const WiFiMeasurements mes = reader.getWiFiGroupedByTime().at(i).data;
			const WiFiMeasurements mes2 = Settings::WiFiModel::vg_eval.group(mes);
			plot.add(mes2);
		}

		K::GnuplotObjectRectangle rect(
			K::GnuplotCoordinate2(0, K::GnuplotCoordinateSystem::FIRST, 0, K::GnuplotCoordinateSystem::GRAPH),
			K::GnuplotCoordinate2(10, K::GnuplotCoordinateSystem::FIRST, 1, K::GnuplotCoordinateSystem::GRAPH),
			K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromRGB(128,128,128), 0.5),
			K::GnuplotStroke()
		);

		plot.getPlot().setGrid(true);
		plot.getPlot().setLegend(false);
		plot.getPlot().getObjects().add(&rect);
		plot.plot();

		sleep(100);

	}

}

void testWAF() {

	Floorplan::Ceilings ceilings;
	ceilings.addCeiling(3);
	ceilings.addCeiling(6);
	ceilings.addCeiling(9);
	ceilings.addCeiling(12);

	K::Gnuplot gp;
	K::GnuplotPlot gplot;
	K::GnuplotPlotElementLines lines; gplot.add(&lines);

	const Point3 posAP(0, 0, 8);

	for (float z = 0; z < 15; z += 0.1) {

		const Point3 posMe(0, 0, z);
		float factor = ceilings.numCeilingsBetweenFloat(posAP, posMe);
		lines.add({z, factor});

	}

	gp.draw(gplot);
	gp.flush();

	sleep(1000);


}


int main(void) {

	//testWAF();

	//paperOutputs();	//	return 0;

	// calib error in/out
	if (1 == 0) {

		// outdoor only
		bool ignoreStaircases = true;
		bool ignoreOutdoor = false;
		bool ignoreIndoor = true;
		EvalCompareOpt opt1(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor);
		EvalCompareOpt::Result s1 = opt1.optPosOptParamsForEach();
		std::cout << s1.errAbs.asString() << std::endl;

		// stairs only
		ignoreStaircases = false;
		ignoreOutdoor = true;
		ignoreIndoor = true;
		EvalCompareOpt opt3(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor);
		EvalCompareOpt::Result s3 = opt3.optPosOptParamsForEach();
		std::cout << s3.errAbs.asString() << std::endl;

		// indoor only
		ignoreStaircases = true;
		ignoreOutdoor = true;
		ignoreIndoor = false;
		EvalCompareOpt opt2(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor);
		EvalCompareOpt::Result s2 = opt2.optPosOptParamsForEach();
		std::cout << s2.errAbs.asString() << std::endl;



	}

	// walks
	if (1 == 0) {
		Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap);;
		EvalWalk walk(map);
		walk.walk1();
		return 0;
	}

	// test gps within data files
	if (1 == 0) {
		EvalData::dumpGPSforPath(Settings::path1a); BREAK;
		EvalData::dumpGPSforPath(Settings::path1b); BREAK;
		EvalData::dumpGPSforPath(Settings::path2a); BREAK;
		EvalData::dumpGPSforPath(Settings::path2b); BREAK;
	}

	// eval average signal-strength per AP
	if (1 == 0) {

		EvalWiFiSigStrength ewss(Settings::fMap, Settings::fCalib);
		ewss.perAP_avg();

	}


	// test wifi within data files
	if (1 == 1) {
		EvalWiFi ew1(Settings::fMap, Settings::path1a, Settings::GroundTruth::path1);
		//EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path2);
		//ew1.fixedParams(-40, 2.5, -8); BREAK;
		//ew1.fixedParams(-64.5905, 1.25988, -2.47863); BREAK;
		//ew1.fixedParams(-59.4903,1.52411,-3.25077); BREAK;
		//ew1.load(Settings::wifiEachOptParPos);
		ew1.load(Settings::wifiAllFixed, "empirc");
		ew1.load(Settings::wifiAllOptPar, "opt params all APs");
		ew1.load(Settings::wifiEachOptPar, "opt params each AP");
		ew1.load(Settings::wifiEachOptParPos, "everything opt");

		sleep(1000);

	}




	// run earthmapping
	//testMapEarthReg(fMap);

	if (1 == 0) {
		EvalApOpt eval(Settings::fMap, Settings::fCalib);
		eval.optAll();
	}


	// compare wifi opt methods
	if (1 == 0) {

		const bool ignoreStaircases = false;
		const bool ignoreOutdoor = false;
		const bool ignoreIndoor = false;
		EvalCompareOpt opt(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor, ignoreIndoor);

		EvalCompareOpt::Result s1 = opt.fixedPosFixedParamsForAll(); //BREAK;
		EvalCompareOpt::Result s2 = opt.fixedPosOptParamsForAll(); //BREAK;
		EvalCompareOpt::Result s3 = opt.fixedPosOptParamsForEach(); //BREAK;
		EvalCompareOpt::Result s4 = opt.optPosOptParamsForEach(); //BREAK;

		PlotErrFunc pef("error (dB)", "fingerprints (%)");
		pef.add("empiric", &s1.errAbs);
		pef.add("real pos, opt params [same for all]", &s2.errAbs);
		pef.add("real pos, opt params [for each]", &s3.errAbs);
		pef.add("opt pos, opt params [for each]", &s4.errAbs);
		pef.plot();

	}

	// compare leaving out fingerprints
	if (1 == 0) {

		EvalCompareOpt e1(Settings::fMap, Settings::fCalib, false, false, false);
		EvalCompareOpt e2(Settings::fMap, Settings::fCalib, true, false, false);
		EvalCompareOpt e3(Settings::fMap, Settings::fCalib, false, true, false);
		EvalCompareOpt e4(Settings::fMap, Settings::fCalib, true, true, false);

		K::Statistics<float> s1 = e1.optPosOptParamsForEach().errAbs;
		K::Statistics<float> s2 = e2.optPosOptParamsForEach().errAbs;
		K::Statistics<float> s3 = e3.optPosOptParamsForEach().errAbs;
		K::Statistics<float> s4 = e4.optPosOptParamsForEach().errAbs;

		PlotErrFunc pef("error (dB)", "fingerprints (%)");
		pef.add("floor + stairs + out", &s1);
		pef.add("floor + out", &s2);
		pef.add("floor + stairs", &s3);
		pef.add("floor", &s4);
		pef.plot();

	}





//	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile("/apps/android/workspace/OTHER2017/data/SHL33a.xml");


//	WiFiFingerprints calib("/apps/android/workspace/OTHER2017/data/wifi_fp.dat");

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

//	WiFiOptimizer opt(map, vap);
//	opt.addFingerprints(calib);
//	WiFiOptimizer::APParamsList list = opt.optimizeAll();

//	for (Floorplan::Floor* floor : map->floors) {
//		for (Floorplan::AccessPoint* ap : floor->accesspoints) {
//			const WiFiOptimizer::APParamsMAC* params = list.get(ap->mac);
//			if (params) {
//				const float delta = ap->getPos(floor).getDistance(Point3(params->params.x, params->params.y, params->params.z));
//				std::cout << ap->mac << ": " << delta << "m" << std::endl;
//			}
//		}
//	}



//	static K::Gnuplot gp;
//	gp << "set view equal xy\n";

//	K::GnuplotSplot splot;
//	K::GnuplotSplotElementPoints points; splot.add(&points); points.setPointType(7); points.setPointSize(0.5);
//	K::GnuplotSplotElementLines lines; splot.add(&lines);

//	for (Floorplan::Floor* floor : map->floors) {
//		for (Floorplan::FloorObstacle* obs : floor->obstacles) {
//			Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
//			if (line) {
//				const K::GnuplotPoint3 p1(line->from.x, line->from.y, floor->atHeight);
//				const K::GnuplotPoint3 p2(line->to.x, line->to.y, floor->atHeight);
//				lines.addSegment(p1, p2);
//			}
//		}
//	}

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

//	gp.draw(splot);
//	gp.flush();

//	int i = 0; (void) i;

}