#define  BREAK		raise(SIGTRAP);

#include "for_toni/Toni.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/model/WiFiModels.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 "EvalWiFiSigStrength.h"
#include "pf/EvalWalk.h"
#include "EvalWifiOptResult.h"
#include "wifi/EvalWiFiConvex.h"
#include "wifi/EvalWiFiGround.h"
#include "wifi/EvalWiFi.h"
#include "wifi/EvalWiFiPaths.h"
#include "wifi/EvalWiFiPathMethods.h"
#include "pf/MarkusB.h"


#include "plots/PlotErrFunc.h"


void rebuildAllModels(Floorplan::IndoorMap* map, const int skip = 0, bool ignoreStaircases = 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),
//		};
//		EvalCompareOpt2 opt1(Settings::fMap, calibWalks);

	int skipCnt = 0;
	auto skipper = [&] (const WiFiFingerprint& fp) -> bool {
		if (skip == 0) {return false;}
		++skipCnt;
		return ((skipCnt % skip) != 0);
	};

	// all combined
	auto removeNone = [] (const WiFiFingerprint& fp) -> bool {return false;};

	// per floor
	auto only0th = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || std::abs(fp.pos_m.z - (1.3)) > 0.1;};
	auto only1st = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || std::abs(fp.pos_m.z - (4+1.3)) > 0.1;};
	auto only2nd = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || std::abs(fp.pos_m.z - (4+3.4+1.3)) > 0.1;};
	auto only3rd = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || std::abs(fp.pos_m.z - (4+3.4+3.4+1.3)) > 0.1;};

	// per bbox
	#include "bboxes.h"
	auto only0H = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || !bboxes0H.contains(fp.pos_m);};
	auto only0O = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || !bboxes0O.contains(fp.pos_m);};
	auto only0I = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || !bboxes0I.contains(fp.pos_m);};

	auto only1H = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || !bboxes1H.contains(fp.pos_m);};
	auto only1O = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || !bboxes1O.contains(fp.pos_m);};
	auto only1I = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || !bboxes1I.contains(fp.pos_m);};

	auto only2H = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || !bboxes2H.contains(fp.pos_m);};

	auto only3H = [&] (const WiFiFingerprint& fp) -> bool {return skipper(fp) || !bboxes3H.contains(fp.pos_m);};


	// use fingerprints?
	EvalCompareOpt2 opt1(Settings::fMap, Settings::fCalib, skipper, ignoreStaircases);


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


	// optimize per floor
	EvalCompareOpt2 opt_f0(Settings::fMap, Settings::fCalib, only0th, ignoreStaircases);
	EvalCompareOpt2::Result sf0 = opt_f0.optPosOptParamsForEach();
	EvalCompareOpt2 opt_f1(Settings::fMap, Settings::fCalib, only1st, ignoreStaircases);
	EvalCompareOpt2::Result sf1 = opt_f1.optPosOptParamsForEach();
	EvalCompareOpt2 opt_f2(Settings::fMap, Settings::fCalib, only2nd, ignoreStaircases);
	EvalCompareOpt2::Result sf2 = opt_f2.optPosOptParamsForEach();
	EvalCompareOpt2 opt_f3(Settings::fMap, Settings::fCalib, only3rd, ignoreStaircases);
	EvalCompareOpt2::Result sf3 = opt_f3.optPosOptParamsForEach();


	// optimize per bbox
	EvalCompareOpt2 opt_0H(Settings::fMap, Settings::fCalib, only0H, ignoreStaircases);
	EvalCompareOpt2::Result sf0H = opt_0H.optPosOptParamsForEach();
	EvalCompareOpt2 opt_0O(Settings::fMap, Settings::fCalib, only0O, ignoreStaircases);
	EvalCompareOpt2::Result sf0O = opt_0O.optPosOptParamsForEach();
	EvalCompareOpt2 opt_0I(Settings::fMap, Settings::fCalib, only0I, ignoreStaircases);
	EvalCompareOpt2::Result sf0I = opt_0I.optPosOptParamsForEach();

	EvalCompareOpt2 opt_1H(Settings::fMap, Settings::fCalib, only1H, ignoreStaircases);
	EvalCompareOpt2::Result sf1H = opt_1H.optPosOptParamsForEach();
	EvalCompareOpt2 opt_1O(Settings::fMap, Settings::fCalib, only1O, ignoreStaircases);
	EvalCompareOpt2::Result sf1O = opt_1O.optPosOptParamsForEach();
	EvalCompareOpt2 opt_1I(Settings::fMap, Settings::fCalib, only1I, ignoreStaircases);
	EvalCompareOpt2::Result sf1I = opt_1I.optPosOptParamsForEach();

	EvalCompareOpt2 opt_2H(Settings::fMap, Settings::fCalib, only2H, ignoreStaircases);
	EvalCompareOpt2::Result sf2H = opt_2H.optPosOptParamsForEach();

	EvalCompareOpt2 opt_3H(Settings::fMap, Settings::fCalib, only3H, ignoreStaircases);
	EvalCompareOpt2::Result sf3H = opt_3H.optPosOptParamsForEach();



	// save models to file
	s1.model.saveXML(Settings::wifiAllFixed);
	s2.model.saveXML(Settings::wifiAllOptPar);
	s3.model.saveXML(Settings::wifiEachOptPar);
	s4.model.saveXML(Settings::wifiEachOptParPos);
	sf0.model.saveXML(Settings::wifiEachOptParPos_only0th);
	sf1.model.saveXML(Settings::wifiEachOptParPos_only1st);
	sf2.model.saveXML(Settings::wifiEachOptParPos_only2nd);
	sf3.model.saveXML(Settings::wifiEachOptParPos_only3rd);

	// fancy combined model
	WiFiModelPerFloor wmpf(map);
	wmpf.add(&sf0.model, map->floors[0]);
	wmpf.add(&sf1.model, map->floors[1]);
	wmpf.add(&sf2.model, map->floors[2]);
	wmpf.add(&sf3.model, map->floors[3]);
	wmpf.saveXML(Settings::wifiEachOptParPos_multimodel);

	// ultra fancy combined model
	WiFiModelPerBBox wmbb(map);
	wmbb.add(&sf0H.model, bboxes0H);
	wmbb.add(&sf0O.model, bboxes0O);
	wmbb.add(&sf0I.model, bboxes0I);
	wmbb.add(&sf1H.model, bboxes1H);
	wmbb.add(&sf1O.model, bboxes1O);
	wmbb.add(&sf1I.model, bboxes1I);
	wmbb.add(&sf2H.model, bboxes2H);
	wmbb.add(&sf3H.model, bboxes3H);
	wmbb.saveXML(Settings::wifiEachOptParPos_perBBox);

	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.writePlotToFile(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();

	int i = 0; (void) i;
	//return;
	//sleep(1000);

}

std::vector<K::Statistics<float>> errorStatAllModels(Floorplan::IndoorMap* map) {

	WiFiModelFactory fac(map);
	WiFiFingerprints calib(Settings::fCalib);
	VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

	for (WiFiFingerprint& fp : calib.getFingerprints()) {
		fp.measurements = vap.group(fp.measurements);
	}

	// calculate the error (model vs scan) for each fingerprint using the given model
	auto calc = [&] (const WiFiModel* model, K::Statistics<float>* stats) {
		for (const WiFiFingerprint& fp : calib.getFingerprints()) {
			for (const WiFiMeasurement& m : fp.measurements.entries) {

				const float scanRSSI = m.getRSSI();
				const float modelRSSI = model->getRSSI(m.getAP().getMAC(), fp.pos_m);

				if (modelRSSI != modelRSSI) {
					std::cout << "unknown AP: " << m.getAP().getMAC().asString() << std::endl;
					continue;
				}

				const float errAbs = std::abs(modelRSSI-scanRSSI);
				stats->add(errAbs);

			}
		}
	};

	// models
	WiFiModel* mdlAllFixed = fac.loadXML(Settings::wifiAllFixed);
	K::Statistics<float> statsAllFixed;
	calc(mdlAllFixed, &statsAllFixed);

	WiFiModel* mdlAllOptPar = fac.loadXML(Settings::wifiAllOptPar);
	K::Statistics<float> statsAllOptPar;
	calc(mdlAllOptPar, &statsAllOptPar);

	WiFiModel* mdlEachOptPar = fac.loadXML(Settings::wifiEachOptPar);
	K::Statistics<float> statsEachOptPar;
	calc(mdlEachOptPar, &statsEachOptPar);

	WiFiModel* mdlEachOptParPos = fac.loadXML(Settings::wifiEachOptParPos);
	K::Statistics<float> statsEachOptParPos;
	calc(mdlEachOptParPos, &statsEachOptParPos);

	WiFiModel* mdlFloor = fac.loadXML(Settings::wifiEachOptParPos_multimodel);
	K::Statistics<float> statsFloor;
	calc(mdlFloor, &statsFloor);

	WiFiModel* mdlBbox = fac.loadXML(Settings::wifiEachOptParPos_perBBox);
	K::Statistics<float> statsBbox;
	calc(mdlBbox, &statsBbox);

	return {
		statsAllFixed,
		statsAllOptPar,
		statsEachOptPar,
		statsEachOptParPos,
		statsFloor,
		statsBbox,
	};

}

/** error plot for all used optimization models */
void errorPlotAllModels(Floorplan::IndoorMap* map) {

	std::vector<K::Statistics<float>> errors = errorStatAllModels(map);

	for (K::Statistics<float> stats : errors) {
		std::cout << stats.getQuantile(0.25)  << "\t&\t" <<
					 stats.getMedian() << "\t&\t" <<
					 stats.getQuantile(0.75) << "\t&\t" <<
					 stats.getAvg() <<
					 "\\\hline" << std::endl;
	}

	PlotErrFunc plot("", "reference measurements (%)");
	plot.getPlot().getAxisX().setTicsLabelFormat("%h dB");
	plot.add("\\noOptEmpiric{}", &errors[0]);
	plot.add("\\optParamsAllAP{}", &errors[1]);
	plot.add("\\optParamsEachAP{}", &errors[2]);
	plot.add("\\optParamsPosEachAP{}", &errors[3]);
	plot.add("\\optPerFloor{}", &errors[4]);
	plot.add("\\optPerRegion{}", &errors[5]);
	plot.getPlot().getKey().setVisible(true);
	plot.getPlot().getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::BOTTOM);

	// debug view
	plot.setYRange(0, 95, 5);
	plot.plot();
	sleep(2);

	plot.setYRange(95, 100, 1);
	plot.plot();
	sleep(2);


	// LATEX
//	plot.getGP() << "set lmargin 4.2\n";
//	plot.getGP() << "set tmargin 0.1\n";
//	plot.getGP() << "set rmargin 0.2\n";
//	plot.getGP() << "set bmargin 1.9\n";
	plot.getPlot().getMargin().set(4.2, 0.2, 0.1, 1.9);
	plot.getPlot().getAxisY().setLabelOffset(3.0 - 0.75, 0);
	plot.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
	plot.getPlot().getKey().setSampleLength(0.5);
	plot.getPlot().getKey().setWidthIncrement(+7.0);

	plot.setYRange(0, 95, 5);
	plot.getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0, 95));
	//plot.getPlot().getAxisY().setTicsStep(0, 25, 100);
	plot.getPlot().getAxisY().setTicsStep({0,25,50,75,95});
	plot.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 4.0));
	plot.getPlot().getAxisX().setTicsStep(4);	// 4dB
	plot.getPlot().getAxisX().setRange(K::GnuplotAxis::Range(0, 16));
	plot.getGP().setOutput(Settings::fPathGFX + "/wifi_model_error_0_95.tex");
	plot.writePlotToFile(Settings::fPathGFX + "/wifi_model_error_0_95.gp");
	plot.plot();

	plot.getPlot().getKey().setVisible(false);
	plot.setYRange(97, 100, 1);
	plot.getPlot().getAxisY().setTicsStep(1);
	plot.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 2.6));
	plot.getPlot().getAxisX().setRange(K::GnuplotAxis::Range(5, 35));
	plot.getPlot().getAxisX().setTicsStep(5);	// 5dB
	plot.getGP().setOutput(Settings::fPathGFX + "/wifi_model_error_95_100.tex");
	plot.writePlotToFile(Settings::fPathGFX + "/wifi_model_error_95_100.gp");
	plot.plot();

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

}

/** error plot for the given stats. used for fingerprint errors */
void errorPlotNumFingerprints(const std::vector<K::Statistics<float>>& stats, const std::vector<std::string>& titles, const std::string& name) {

	PlotErrFunc plot("", "ref. measurements (%)");
	plot.getPlot().getAxisX().setTicsLabelFormat("%h dB");

	plot.getPlot().getKey().setVisible(true);
	plot.getPlot().getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::BOTTOM);


	plot.clear();
	for (int j = 0; j < titles.size(); ++j) {
		plot.add(titles[j], &stats[j]);
	}

//		// debug view
//		plot.setYRange(0, 90, 5);
//		plot.plot();
//		sleep(5);

//		plot.setYRange(90, 100, 5);
//		plot.plot();
//		sleep(5);

	// LATEX
//	plot.getGP() << "set lmargin 4.2\n";
//	plot.getGP() << "set tmargin 0.1\n";
//	plot.getGP() << "set rmargin 0.2\n";
//	plot.getGP() << "set bmargin 1.9\n";
	plot.getPlot().getMargin().set(4.2, 0.2, 0.1, 1.9);
	plot.getPlot().getAxisY().setLabelOffset(3.0 - 0.7,0);
	plot.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
	plot.getPlot().getKey().setSampleLength(0.5);
	plot.getPlot().getKey().setWidthIncrement(-5.0);

	plot.setYRange(0, 90, 5);
	plot.getPlot().getAxisY().setRange(K::GnuplotAxis::Range(0, 90));
	plot.getPlot().getAxisY().setTicsStep({0, 25, 50, 75, 90});
	//lot.getPlot().getAxisY().setTicsStep(0, 25, 100);
	plot.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.3));
	plot.getPlot().getAxisX().setTicsStep(4);	// 4dB
	plot.getPlot().getAxisX().setRange(K::GnuplotAxis::Range(0, 14));
	plot.getGP().setOutput(Settings::fPathGFX + "/" + name + "_0_90.tex");
	plot.writePlotToFile(Settings::fPathGFX + "/" + name + "_0_90.gp");
	plot.plot();

	// changes
	plot.getPlot().getKey().setVisible(false);
	plot.getPlot().getAxisY().setLabel("");

	plot.setYRange(90, 100, 1);
	plot.getPlot().getAxisY().setTicsStep(2);
	plot.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 2.6));
	plot.getPlot().getAxisX().setRange(K::GnuplotAxis::Range(5, 35));
	plot.getPlot().getAxisX().setTicsStep(5);	// 5dB
	plot.getGP().setOutput(Settings::fPathGFX + "/" + name + "_90_100.tex");
	plot.writePlotToFile(Settings::fPathGFX + "/" + name +  "_90_100.gp");
	plot.plot();

}

/** show all fingerprints within the building */
void plotAllFingerprints(Floorplan::IndoorMap* map) {

	WiFiFingerprints calib(Settings::fCalib);
	LeHelper::removeNonFHWS(calib);

	VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
	for (WiFiFingerprint& fp : calib.getFingerprints()) {
		fp.measurements = vap.group(fp.measurements);
	}

	// estimate stats
	K::Statistics<float> fpVisible;
	for (const WiFiFingerprint& fp : calib.getFingerprints()) {
		fpVisible.add(fp.measurements.entries.size());
	}

	Plotty* p = new Plotty(map);
	p->buildFloorplan();
	p->cpoints.clear();
	for (const WiFiFingerprint& fp : calib.getFingerprints()) {
		const K::GnuplotPoint3 gp(fp.pos_m.x, fp.pos_m.y, fp.pos_m.z);
		const float size = fp.measurements.entries.size() / 10.0 * 1.0;
		const Point3 pos = fp.pos_m - Point3(0,0,1.2);
		Color c;

//		if (pos.z < 4) {c = Color::fromRGB(128,128,128);}
//		else if (pos.z < 6) {c = Color::fromRGB(255,96,96);}
//		else if (pos.z < 9) {c = Color::fromRGB(128,255,128);}
//		else {c = Color::fromRGB(128,128,255);}
		c = Color::fromRGB(0,0,0);

		K::GnuplotObjectPolygon* poly = p->addFloorRect(pos, size, c);
		poly->setZIndex(pos.z + 0.1);	// above the floor

		const int visibleAPs = fp.measurements.entries.size();
		if (visibleAPs == fpVisible.getMin() || visibleAPs == fpVisible.getMax()) {
			p->addLabel("\\footnotesize{" + std::to_string(fp.measurements.entries.size()) + "}", pos+Point3(0,1,1)*0.25);
		}

	}

	std::cout << fpVisible.asString() << std::endl;

	p->splot.getCustom() << "set view equal xy\n";
	p->splot.getCustom() << "unset border\n";
	p->splot.getMargin().set(1, 0,0,0);
	p->splot.getAxisX().setTicsVisible(false);
	p->splot.getAxisY().setTicsVisible(false);
	p->splot.getAxisZ().setTicsVisible(false);
	p->splot.getAxisZ().setRange(K::GnuplotAxis::Range(-8, 19.5));
	p->splot.getView().setCamera(74,30);
	p->splot.getView().setScaleAll(3.8);

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

	p->gp.setTerminal("epslatex", K::GnuplotSize(8.6, 5.0));
	p->gp.setOutput(Settings::fPathGFX + "/all_fingerprints.tex");
	p->gp.writePlotToFile(Settings::fPathGFX + "/all_fingerprints.gp");
	p->plot();

	sleep(100);

}


using Walk = std::vector<Point3>;

float getLength(const Walk& walk) {

	float len = 0;
	for (size_t i = 0; i < walk.size()-1; ++i) {
		const Point3 p1 = walk[i+0];
		const Point3 p2 = walk[i+1];
		const float dist = p1.getDistance(p2);
		len += dist;
	}
	return len;

}

/** show all walked paths */
void plotAllWalks(Floorplan::IndoorMap* map) {



	Walk path1 = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path1);
	Walk path2 = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path2);
	//Walk path_toni_all_1 = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path_toni_all_1);
	//Walk path_toni_all_2 = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path_toni_all_2);
	Walk path_toni_inst_1 = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path_toni_inst_1);
	Walk path_toni_inst_2 = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path_toni_inst_2);
	Walk path_toni_inst_3 = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path_toni_inst_3);

	std::cout << "path1: " << getLength(path1) << std::endl;
	std::cout << "path2: " << getLength(path2) << std::endl;
	std::cout << "inst_1: " << getLength(path_toni_inst_1) << std::endl;
	std::cout << "inst_2: " << getLength(path_toni_inst_2) << std::endl;
	std::cout << "inst_3: " << getLength(path_toni_inst_3) << std::endl;

	const std::vector<Walk> walks = {
		path1,
		path2,
		//path_toni_all_1,	same as path1
		//path_toni_all_2,	same as path2
		path_toni_inst_1,
		path_toni_inst_2,
		path_toni_inst_3
	};

	const std::vector<std::string> titles = {
		"path 1", "path 2", "path 3", "path 4", "path 5"
	};

	Plotty* p = new Plotty(map);
	p->buildFloorplan();

	std::vector<std::string> colors = {"#000000", "#ff0000", "#00cc00", "#0000ff", "#009999", "#aa00aa"};

	int i = 0;
	for (const Walk& walk : walks) {

		p->addStartIndicator(walk[0], colors[i]);

		K::GnuplotSplotElementLines* line = new K::GnuplotSplotElementLines();
		line->getStroke().setWidth(4);
		line->getStroke().getColor().setHexStr(colors[i]);
		line->setTitle(titles[i]);

		if (i == 4) {
			line->getStroke().setType(K::GnuplotDashtype::DOTTED);
		}

		//line->getStroke().setType( (i > 3) ? K::GnuplotDashtype::DASHED : K::GnuplotDashtype::SOLID );

		float oy = 0;
		if (i == 2) {oy -= 0.5;}
		if (i == 3) {oy += 0.5;}

		for (const Point3& pt : walk) {
			line->add(K::GnuplotPoint3(pt.x, pt.y+oy, pt.z));
		}
		p->splot.add(line);
		++i;
	}

	p->splot.getKey().setVisible(true);
	//p->splot.getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::TOP);
	p->splot.getKey().setPosition(K::GnuplotCoordinate2(0.99, 0.99, K::GnuplotCoordinateSystem::SCREEN));
	p->splot.getKey().setSampleLength(1.0);
	//p->splot.getKey().setWidthIncrement(-4);
	p->splot.setStringMod(new K::GnuplotStringModLaTeX());

	p->splot.getCustom() << "set view equal xy\n";
	p->splot.getCustom() << "unset border\n";
	p->splot.getMargin().set(1, 0,0,0);
	p->splot.getAxisX().setTicsVisible(false);
	p->splot.getAxisY().setTicsVisible(false);
	p->splot.getAxisZ().setTicsVisible(false);
	p->splot.getAxisZ().setRange(K::GnuplotAxis::Range(-8, 19.5));
	p->splot.getView().setCamera(74,30);
	p->splot.getView().setScaleAll(3.8);
	p->gp.setTerminal("epslatex", K::GnuplotSize(8.6, 5.0));

	// modified paper
	p->splot.getView().setCamera(79,30);
	p->splot.getView().setScaleAll(5.5);
	p->splot.getAxisZ().setRange(K::GnuplotAxis::Range(-14, 26.0));
	p->gp.setTerminal("epslatex", K::GnuplotSize(15.24, 5.08));
	//p->splot.getObjects().reOrderByZIndex();
	//p->plot();


	p->gp.setOutput(Settings::fPathGFX + "/all_walks.tex");
	p->gp.writePlotToFile(Settings::fPathGFX + "/all_walks.gp");
	p->plot();

	sleep(100);

}

void plotEstAndRealApPosDistance(Floorplan::IndoorMap* map) {

	WiFiModelLogDistCeiling mdl(map);
	mdl.loadXML(Settings::wifiEachOptParPos);

	K::Statistics<float> statsTxp;
	K::Statistics<float> statsExp;
	K::Statistics<float> statsWaf;
	K::Statistics<float> statsPosErr;
	int numAPs = 0;
	int numWrongZ = 0;


	for (const AccessPoint& ap : mdl.getAllAPs()) {

		// param range
		const WiFiModelLogDistCeiling::APEntry params = mdl.getAP(ap.getMAC());
		statsTxp.add(params.txp);
		statsExp.add(params.exp);
		statsWaf.add(params.waf);

		// position error
		const Point3 mdlPos = params.position_m;

		const auto& it = FloorplanHelper::getAP(map, ap.getMAC());
		const Floorplan::AccessPoint* fap = it.first;
		const Floorplan::Floor* floor = it.second;

		const Point3 realPos = fap->getPos(floor);


		const float posErr = mdlPos.getDistance(realPos);
		statsPosErr.add(posErr);

		// wrong z?
		if (mdlPos.z < floor->atHeight || mdlPos.z > (floor->atHeight+floor->height)) {
			++numWrongZ;
		}

		++numAPs;

	}

	PlotErrFunc pef("", "\\docAP{}s (%)");
	pef.getPlot().getAxisX().setTicsLabelFormat("%h m");
	pef.add("oo", &statsPosErr);
	pef.plot();

	const std::string file = Settings::fPathGFX + "/" + "wifiOptApPosDifference";
	pef.getGP().setOutput(file + ".tex");
	pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 2.8));
	pef.getPlot().getMargin().set(3.5, 0.2, 0.1, 2.0);
	pef.writePlotToFile(file + ".gp");
	pef.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
	pef.plot();

	std::cout << "TXP:\t" << statsTxp.asString() << std::endl;
	std::cout << "EXP:\t" << statsExp.asString() << std::endl;
	std::cout << "WAF:\t" << statsWaf.asString() << std::endl;
	std::cout << "Pos:\t" << statsPosErr.asString() << std::endl;
	std::cout << "WrongZ:\t" << numWrongZ << " (" << (numWrongZ*100.0f/numAPs) << "%) "<< std::endl;

	int i = 0; (void) i;

}


#warning "TODO: code um alle pfade MEHRFACH mit allen modellen durch den filter laufen zu lassen"

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

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



	// show optimization behaviour
	if (1 == 0) {

		EvalWiFiConvex eval(map, Settings::fCalib);
		eval.showParams();
		//eval.showPos();
		eval.showPosZ();
		return;

	}


	// 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", K::GnuplotSize(8.6, 4.7));
		p->settings.floors = {0};
		p->settings.maxZ = 1;
		p->settings.outlineColor = K::GnuplotColor::fromRGB(170,170,170);
		p->buildFloorplan();
		sig.forPaperNN(p, MACAddress("d8:84:66:4a:23:d0"));
		p->equalXY();
		p->setView(0,0);
		p->setScale(3.45, 3.45, -0.005, -0.04);
		p->addRectangleW(Point3(62.5, 24, 0), Point3(72, 35, 0), K::GnuplotColor::fromRGB(0,0,0), 3, true);	// small
		p->addRectangleW(Point3(8.0, 39.75, 0), Point3(72, 43.75, 0), K::GnuplotColor::fromRGB(0,0,0), 3, true); // big
		p->noFrame();
		p->plot();
		delete p;
	}


	// plot the configured bboxes
	if (1 == 0) {

		#include "bboxes.h"

		Plotty pt(map);
		pt.settings.outline = false;
		pt.buildFloorplan();

		// coloring
		K::GnuplotColor cH0 = K::GnuplotColor::fromRGB(130,130,130);
		K::GnuplotColor cH1 = K::GnuplotColor::fromRGB(170,170,170);

		K::GnuplotColor cO = K::GnuplotColor::fromRGB(0,128,0);

		K::GnuplotColor cI0 = K::GnuplotColor::fromRGB(150,150,150);
		K::GnuplotColor cI1 = K::GnuplotColor::fromRGB(190,190,190);
		K::GnuplotColor cI2 = K::GnuplotColor::fromRGB(210,210,210);
		K::GnuplotColor cI3 = K::GnuplotColor::fromRGB(230,230,230);

		// floor 0
		pt.addBBoxes(bboxes0H, cH0);
		pt.addBBoxes(bboxes0O, cO);
		pt.addBBoxes(bboxes0I, cI0);

		// floor 1
		pt.addBBoxes(bboxes1H, cH1);
		pt.addBBoxes(bboxes1O, cO);
		pt.addBBoxes(bboxes1I, cI1);

		// floor 2
		pt.addBBoxes(_bboxes2H, cI2);

		// floor 3
		pt.addBBoxes(_bboxes3H, cI3);

		pt.splot.getObjects().reOrderByZIndex();
		pt.plot();

		pt.gp << "unset border\n";
		pt.gp << "set view equal xy\n";
		pt.splot.getView().setCamera(74, 30);
		pt.splot.getView().setScaleAll(3.8);
		pt.splot.getAxisZ().setRange(-8.000000,19.500000);
		pt.splot.getAxisX().setTicsVisible(false);
		pt.splot.getAxisY().setTicsVisible(false);
		pt.splot.getAxisZ().setTicsVisible(false);
		pt.gp.writePlotToFile(Settings::fPathGFX + "/model-bboxes.gp");
		pt.gp.setTerminal("epslatex", K::GnuplotSize(8.3, 4.5));
		pt.gp.setOutput(Settings::fPathGFX + "/model-bboxes.tex");
		pt.plot();

		int i = 0; (void) i;

	}

	// show all fingerprints
	if (1 == 0) {
		plotAllFingerprints(map);
	}

	// show all walks
	if (1 == 0) {
		plotAllWalks(map);
	}



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

		//rebuildAllModels(map, 0);

		/** detailled error analysis for above optimization routine */
		errorPlotAllModels(map);

	}


	// leaving out fingerprints and the effects on all optimization strategies
	if (1 == 0) {

		rebuildAllModels(map,4);
		std::vector<K::Statistics<float>> stats4 = errorStatAllModels(map);
		rebuildAllModels(map,2);
		std::vector<K::Statistics<float>> stats2 = errorStatAllModels(map);
		rebuildAllModels(map,0,true);
		std::vector<K::Statistics<float>> statsNoStairs = errorStatAllModels(map);
		rebuildAllModels(map,0);		// ensure all output files are overwritten with the "all fingerprints" opt!!!
		std::vector<K::Statistics<float>> stats0 = errorStatAllModels(map);

		// analyze all 5 opt strategies. skip the empiric one: stats0[0]
		for (int i = 1; i < 6; ++i) {
			std::cout << "leaving out fingerprints for model " << i << std::endl;
			std::cout << "\t 25%: " << stats4[i].asString() << std::endl;
			std::cout << "\t 50%: " << stats2[i].asString() << std::endl;
			std::cout << "\t 100%: " << stats0[i].asString() << std::endl;
			std::cout << "\t noStair%: " << statsNoStairs[i].asString() << std::endl;
			std::cout << std::endl;

			std::string name = "wifi_model_error_num_fingerprints_method_" + std::to_string(i);
			errorPlotNumFingerprints(
				{stats0[0], stats4[i], stats2[i], stats0[i], statsNoStairs[i]},
				{"empiric", "25%", "50%", "100%", "no stairs"},
				name
			);
			sleep(1);
		}

		sleep(1);

	}

	/** plot wifi eval results */
	if (1 == 0) {

		WiFiFingerprints fps;
		fps.load(Settings::fCalib);

		EvalWiFiOptResult eval2(Settings::fMap);
		Plotty* p1 = eval2.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiAllFixed, fps);

		EvalWiFiOptResult eval1(Settings::fMap);
		Plotty* p2 = eval1.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos, fps);

		// advanced model [1 model per floor]
		//EvalWiFiOptResult evalfloor(Settings::fMap);
		//evalfloor.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only0th, fps);
		//evalfloor.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only1st, fps);
		//evalfloor.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only2nd, fps);
		//evalfloor.showErrorPerFingerprint<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only3rd, fps);
		//evalfloor.showErrorPerFingerprint<WiFiModelPerFloor>(Settings::wifiEachOptParPos_multimodel, fps);

		// more advanved model [1 model per bbox-region]
		EvalWiFiOptResult evalBBox(Settings::fMap);
		Plotty* p3 = evalBBox.showErrorPerFingerprint<WiFiModelPerBBox>(Settings::wifiEachOptParPos_perBBox, fps);

		K::GnuplotSize size(5.15, 3.4);
		const float s = 4.4;

		auto adjust = [&] (Plotty* pp) {
			//pp->setScale(1,1, -0.075, 0);
			pp->gp.setTerminal("epslatex", size);
			pp->splot.setTitle("");
			pp->splot.getView().setCamera(74, 30);
			pp->splot.getView().setScaleAll(s);
			pp->splot.getAxisZ().setRange(K::GnuplotAxis::Range(-6, 18.5));
			pp->plot();
		};

		p1->gp.writePlotToFile(Settings::fPathGFX + "wifiMaxErrorNN_opt0.gp");
		p1->gp.setOutput(Settings::fPathGFX + "wifiMaxErrorNN_opt0.tex");
		adjust(p1);

		p2->gp.writePlotToFile(Settings::fPathGFX + "wifiMaxErrorNN_opt3.gp");
		p2->gp.setOutput(Settings::fPathGFX + "wifiMaxErrorNN_opt3.tex");
		p2->splot.setStringMod(new K::GnuplotStringModLaTeX());
		adjust(p2);

		p3->gp.writePlotToFile(Settings::fPathGFX + "wifiMaxErrorNN_opt5.gp");
		p3->gp.setOutput(Settings::fPathGFX + "wifiMaxErrorNN_opt5.tex");
		p3->splot.getCustom() << "set pm3d; set cbrange [0:20]; set palette defined (0 '#ffffff', 1 '#ff0000'); \n";
		//p3->splot.getCustom() << "set colorbox horizontal user origin screen -0.11, 0.08 size 0.45,0.05 front;\n";
		p3->splot.getCustom() << "set colorbox vertical user origin screen 0.77, 0.55 size 0.04,0.40 front;\n";
		p3->splot.getAxisCB().setTicsLabelFormat("%h dB");
		p3->splot.getAxisCB().setTicsStep(10);
		p3->splot.setStringMod(new K::GnuplotStringModLaTeX());

		adjust(p3);


		int i = 0; (void) i;

	}



//	// 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.writePlotToFile(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();

//	}



}

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);
	K::GnuplotPlotElementLines ceils; gplot.add(&ceils);

	const Point3 posAP(0, 0, 8);

	for (const float h : ceilings.getCeilings()) {
		ceils.addSegment(K::GnuplotPoint2(h, -2), K::GnuplotPoint2(h, +4));
	}

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

		const Point3 posMe(0, 0, z);
		//float factor = ceilings.numCeilingsBetweenLinearInt(posAP, posMe);
		float factor = ceilings.numCeilingsBetweenFloat(posAP, posMe);

		lines.add({z, factor});

	}

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

	sleep(10);


}

void showFingerprintsFor(const std::string& mapFile, const std::string& fpFile, const std::string& smac) {

	WiFiFingerprints calib(fpFile);
	VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
	for (WiFiFingerprint& fp : calib.getFingerprints()) {
		fp.measurements = vap.group(fp.measurements);
	}

	const MACAddress mac(smac);

	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);


	Plotty p(map);
	p.buildFloorplan();

	std::vector<WiFiFingerprint> fps = calib.getFingerprintsFor(mac);

	for (const WiFiFingerprint& fp : fps) {
		if (fp.measurements.entries.size() != 1) {throw "123";}
		const float rssi = fp.measurements.entries[0].getRSSI();
		const float s = (rssi-100) / (-40 - -100);
		const Color c = Color::fromHSV(s*100, 255, 255);
		p.addFloorRect(fp.pos_m, 3, c);
	}

	p.plot();
	sleep(1);

}

void showModelFor(const std::string& mapFile, const std::string& modelXml, const std::string& smac) {

	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);
	WiFiModelFactory fac(map);

	WiFiModel* model = fac.loadXML(modelXml);

	const MACAddress mac(smac);

	Plotty p(map);
	p.buildFloorplan();

	const float ss = 2.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+1.3);

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

				if (!contained) {continue;}

				const float rssi = model->getRSSI(mac, pos);
				const float s = (rssi-100) / (-40 - -100);
				const Color c = Color::fromHSV(s*100, 255, 255);
				p.addFloorRect(pos, 3, c);

			}
		}
	}

	p.plot();
	sleep(1);

}

void compareAll() {

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

//	std::vector<std::string> files = {
//		Settings::path1a, //Settings::path1b,
//		//Settings::path2a, Settings::path2b,
//		//Settings::path_toni_all_1a, Settings::path_toni_all_1b,
//		//Settings::path_toni_all_2a, Settings::path_toni_all_2b,
//		//Settings::path_toni_inst_1a, Settings::path_toni_inst_1b,
//		//Settings::path_toni_inst_2a, Settings::path_toni_inst_2b,
//		//Settings::path_toni_inst_3a, Settings::path_toni_inst_3b,
//	};
//	std::vector<std::vector<int>> gtIndices = {
//		Settings::GroundTruth::path1, //Settings::GroundTruth::path1,
//		//Settings::GroundTruth::path2, Settings::GroundTruth::path2,
//		//Settings::GroundTruth::path1, Settings::GroundTruth::path1,
//		//Settings::GroundTruth::path2, Settings::GroundTruth::path2,
//		//Settings::GroundTruth::path_toni_inst_1, Settings::GroundTruth::path_toni_inst_1,
//		//Settings::GroundTruth::path_toni_inst_2, Settings::GroundTruth::path_toni_inst_2,
//		//Settings::GroundTruth::path_toni_inst_3, Settings::GroundTruth::path_toni_inst_3,
//	};

	const std::string path = Settings::path1a;
	const std::vector<int> gtIndices = Settings::GroundTruth::path1;

	WalkResult res1;
	WalkResult res2;
	WalkResult res3;
	WalkResult res4;
//	{
//		EvalWiFiPaths ewp1(Settings::fMap);
//		ewp1.loadModel(Settings::wifiAllFixed, "A");
//		res1 = ewp1.walk(path, gtIndices);
//		res1.serialize("/tmp/walk1.dat");
//	}

//	{
//		EvalWiFiPaths ewp2(Settings::fMap);
//		ewp2.loadModel(Settings::wifiEachOptParPos_multimodel, "B");
//		res2 = ewp2.walk(path, gtIndices);
//		res2.serialize("/tmp/walk2.dat");
//	}

//	{
//		EvalWalk ew(map);
//		ew.walk(path, gtIndices, Settings::wifiEachOptParPos_multimodel);
//		ew.res.serialize("/tmp/walk3.dat");
//	}

//	{
//		EvalWalk ew(map);
//		ew.walk(path, gtIndices, Settings::wifiAllFixed);
//		ew.res.serialize("/tmp/walk4.dat");
//	}

	res1.deserialize("/tmp/walk1.dat");
	res2.deserialize("/tmp/walk2.dat");
	res3.deserialize("/tmp/walk3.dat");
	res4.deserialize("/tmp/walk4.dat");

	PlotErrTime pet("time", "error (m)", "");
	pet.getPlot().getAxisY().setRange(0, 25);



	Plotty p(map);
	p.buildFloorplan();

	Plotty p2(map);
	p2.settings.maxZ = 4.1;
	p2.buildFloorplan();

	K::GnuplotSplotElementLines line1;	p.splot.add(&line1);	line1.setTitle("\\noOptEmpiric{}");		line1.getStroke().getColor().setHexStr("#ff0000");	line1.getStroke().setType(K::GnuplotDashtype::DOTTED);	line1.getStroke().setWidth(2);
	K::GnuplotSplotElementLines line1b;	p.splot.add(&line1b);											line1b.getStroke().getColor().setHexStr("#ff0000");	line1b.getStroke().setWidth(5);

	K::GnuplotSplotElementLines line2;	p.splot.add(&line2);	line2.setTitle("\\optPerFloor{}");		line2.getStroke().getColor().setHexStr("#0000ff");	line2.getStroke().setType(K::GnuplotDashtype::DASHED);	line2.getStroke().setWidth(2);
	K::GnuplotSplotElementLines line2b;	p.splot.add(&line2b);											line2b.getStroke().getColor().setHexStr("#0000ff");	line2b.getStroke().setWidth(5);

	K::GnuplotSplotElementLines line3;	p.splot.add(&line3);	line3.setTitle("PF + \\optPerFloor{}");	line3.getStroke().getColor().setHexStr("#00dd00");	line3.getStroke().setWidth(2);
	K::GnuplotSplotElementLines line3b;	p.splot.add(&line3b);											line3b.getStroke().getColor().setHexStr("#00dd00");	line3b.getStroke().setWidth(6);

	K::GnuplotSplotElementLines line4;							line4.setTitle("PF + \\noOptEmpiric{}");
	K::GnuplotSplotElementLines line4b;							line4b.getStroke().setWidth(4);


	p2.splot.add(&line4);		// the failing path
	p2.splot.add(&line4b);		// the failing path

	p2.splot.add(&line1);
	p2.splot.add(&line1b);
	p2.splot.add(&line2);
	p2.splot.add(&line2b);
	p2.splot.add(&line3);
	p2.splot.add(&line3b);


	// ERROR OVER TIME
	int ms;

	// EMPIRIC
	MovingAVG<float> avga1(12);
	for (const WalkResult::Entry& e : res1.entries) {avga1.add(e.err); pet.addErr(e.ts, avga1.get(), 0); ms += 500;}
	pet.setColor(0, "#ff0000");
	pet.setLabel(0, "\\noOptEmpiric{}");
	pet.getStroke(0).setType(K::GnuplotDashtype::DOTTED);

	// OPT_PER_FLOOR
	MovingAVG<float> avga2(12);
	for (const WalkResult::Entry& e : res2.entries) {avga2.add(e.err); pet.addErr(e.ts, avga2.get(), 1); ms += 500;}
	pet.setColor(1, "#0000ff");
	pet.setLabel(1, "\\optPerFloor{}");
	pet.getStroke(1).setType(K::GnuplotDashtype::DASHED);

	// PARTICLE FILTER
	MovingAVG<float> avga3(3);
	for (const WalkResult::Entry& e : res3.entries) {avga3.add(e.err); pet.addErr(e.ts, avga3.get(), 2); ms += 500;}
	pet.setColor(2, "#00dd00");
	pet.setLabel(2, "PF + \\optPerFloor{}");

	auto isOutdoor = [] (const Timestamp ts) {
		return ts.sec() > 95 && ts.sec() < 119;
	};

	K::GnuplotCoordinate2 rFrom(92, K::GnuplotCoordinateSystem::FIRST, 0, K::GnuplotCoordinateSystem::GRAPH);
	K::GnuplotCoordinate2 rTo(121, K::GnuplotCoordinateSystem::FIRST, 1, K::GnuplotCoordinateSystem::GRAPH);
	K::GnuplotFill rFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromRGB(200,200,200));
	K::GnuplotStroke rStroke = K::GnuplotStroke::NONE();
	K::GnuplotObjectRectangle rect(rFrom, rTo, rFill, rStroke);
	pet.getPlot().getObjects().add(&rect);

	pet.getPlot().getKey().setVisible(true);
	//pet.plot();

	pet.getPlot().getKey().setOpaque(true);
	pet.getPlot().getKey().setWidthIncrement(5.5);
	pet.getPlot().getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::TOP);
	pet.getPlot().getMargin().set(5, 0.2, 0.1, 2.0);
	pet.getPlot().getAxisX().setTicsLabelFormat("%h s");
	pet.getPlot().getAxisX().setLabel("");
	pet.getPlot().getAxisY().setLabel("error (m)");
	pet.getPlot().getAxisY().setLabelOffset(2.0, 0);
	pet.getPlot().setStringMod(new K::GnuplotStringModLaTeX());
	pet.getGP().setTerminal("epslatex", K::GnuplotSize(7.80*2, 3.0));
	pet.getGP().writePlotToFile(Settings::fPathGFX + "final-error.gp");
	pet.getGP().setOutput(Settings::fPathGFX + "final-error.tex");
	pet.getGP() << "set label 1 '\\footnotesize{outdoor}' at first 98, graph 0.1 front\n";
	pet.plot();

	MovingAVG<Point3> avg1(15);
	for (const WalkResult::Entry& e : res1.entries) {
		avg1.add(e.estimation);
		const Point3 pt = avg1.get();
		const K::GnuplotPoint3 gp3(pt.x, pt.y, pt.z);
		line1.add(gp3);
		if (isOutdoor(e.ts)) {line1b.add(gp3);}
	}

	MovingAVG<Point3> avg2(15);
	for (const WalkResult::Entry& e : res2.entries) {
		avg2.add(e.estimation);
		const Point3 pt = avg2.get();
		const K::GnuplotPoint3 gp3(pt.x, pt.y, pt.z);
		line2.add(gp3);
		if (isOutdoor(e.ts)) {line2b.add(gp3);}
	}

	for (const WalkResult::Entry& e : res3.entries) {
		const Point3 pt = e.estimation;
		const K::GnuplotPoint3 gp3(pt.x, pt.y, pt.z);
		line3.add(gp3);
		if (isOutdoor(e.ts)) {line3b.add(gp3);}
	}

	// failing walk
	for (const WalkResult::Entry& e : res4.entries) {
		const Point3 pt = e.estimation;
		const K::GnuplotPoint3 gp3(pt.x, pt.y, pt.z);
		line4.add(gp3);
		if (isOutdoor(e.ts)) {line4b.add(gp3);}
	}

	p.addStartIndicator(res1.entries[0].estimation, "#ff0000");
	p.addStartIndicator(res2.entries[0].estimation, "#0000ff");
	p.addStartIndicator(res3.entries[0].estimation, "#00cc00");

	p2.addStartIndicator(res1.entries[0].estimation, "#ff0000");
	p2.addStartIndicator(res2.entries[0].estimation, "#0000ff");
	p2.addStartIndicator(res3.entries[0].estimation, "#00cc00");
	p2.addStartIndicator(res4.entries[0].estimation, "#000000");

	p.splot.getCustom() << "set view equal xy\n";
	p.splot.getCustom() << "unset border\n";
	p.splot.getMargin().set(1, 0,0,0);
	p.splot.getAxisX().setTicsVisible(false);
	p.splot.getAxisY().setTicsVisible(false);
	p.splot.getAxisZ().setTicsVisible(false);
	p.splot.getAxisZ().setRange(K::GnuplotAxis::Range(-8, 19.5));
	p.splot.getView().setCamera(74,30);
	p.splot.getView().setScaleAll(3.9);
	p.splot.getKey().setVisible(true);
	p.splot.getKey().setSampleLength(0.5);
	p.splot.getKey().setWidthIncrement(4.5);
	p.splot.setStringMod(new K::GnuplotStringModLaTeX());

	p.splot.getKey().setOpaque(true);
	//p.splot.getKey().setPosition(K::GnuplotCoordinate2(0.2, 1.0, K::GnuplotCoordinateSystem::SCREEN));
	p.splot.getKey().setPosition(K::GnuplotCoordinate2(1.0, 1.0, K::GnuplotCoordinateSystem::SCREEN));
	p.gp.setTerminal("epslatex", K::GnuplotSize(8.3, 4.8));
	p.gp.writePlotToFile(Settings::fPathGFX + "final3D.gp");
	p.gp.setOutput(Settings::fPathGFX + "final3D.tex");
	p.plot();

	p2.splot.getCustom() << "set view equal xy\n";
	p2.splot.getCustom() << "unset border\n";
	p2.splot.getMargin().set(1, 0,0,0);
	p2.splot.getAxisX().setTicsVisible(false);
	p2.splot.getAxisY().setTicsVisible(false);
	p2.splot.getAxisZ().setTicsVisible(false);
	p2.splot.getAxisZ().setRange(K::GnuplotAxis::Range(-8, 19.5));
	p2.splot.getView().setCamera(74,30);
	p2.splot.getView().setScaleAll(3.9);
	p2.splot.getKey().setVisible(true);
	p2.splot.getKey().setSampleLength(0.5);
	p2.splot.getKey().setWidthIncrement(4.5);
	p2.splot.setStringMod(new K::GnuplotStringModLaTeX());


	// only list the additional line within the key
	line1.setTitle("");
	line2.setTitle("");
	line3.setTitle("");

	p2.splot.getKey().setVisible(true);
	p2.splot.getKey().setPosition(K::GnuplotCoordinate2(0.53, 1.0, K::GnuplotCoordinateSystem::SCREEN));
	//p2.splot.getKey().setPosition(K::GnuplotCoordinate2(0.71, 0.35, K::GnuplotCoordinateSystem::SCREEN));
	p2.splot.getView().setCamera(0,0);
	p2.splot.getView().setScaleAll(4.0);
	p2.gp.setTerminal("epslatex", K::GnuplotSize(8.3, 4.8));
	p2.gp.writePlotToFile(Settings::fPathGFX + "final2D.gp");
	p2.gp.setOutput(Settings::fPathGFX + "final2D.tex");
	p2.setScale(1,1, 0.02, -0.04);
	p2.plot();

	int i = 0;
	(void) i;

}

int main(void) {



//Toni::run();


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


	//plotAllWalks(map);

	//compareAll();

	//return 0;

	//testWAF();
	//sleep(1);

	//const float rssi = LogDistanceModel::distanceToRssi(-100, 0, 999);
	//int i = 0; (void) i;

	//	paperOutputs();		return 0;

	//showFingerprintsFor(Settings::fMap, Settings::fCalib, "D8:84:66:4A:4A:D0");
	//showModelFor(Settings::fMap, Settings::wifiEachOptParPos_multimodel, "D8:84:66:4A:4A:D0");
	//showFingerprintsFor(Settings::fMap, Settings::fCalib, "D8:84:66:4A:4A:E0");
	//showModelFor(Settings::fMap, Settings::wifiEachOptParPos_multimodel, "D8:84:66:4A:4A:E0");

	if (1 == 0) {
		plotEstAndRealApPosDistance(map);
		int i = 0; (void) i;
	}

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

//		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().getKey().setVisible(false);
//		plot.getPlot().getObjects().add(&rect);
//		plot.plot();

//		sleep(100);

//	}



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

	}

	// plot probability within the building for every wifi measurement
	if (1 == 0) {

		Plotty::Settings settings;
		settings.outline = false;
		EvalWiFiGround eval(map, Settings::wifiEachOptParPos_multimodel, settings);
		//EvalWiFiGround eval(map, Settings::wifiEachOptParPos_perBBox, settings);
		//EvalWiFiGround eval(map, Settings::wifiEachOptPar, settings);
		//EvalWiFiGround eval(map, Settings::wifiEachOptParPos, settings);

		//eval.show(Settings::path1a);
		//eval.show(Settings::path_toni_inst_2b, Settings::GroundTruth::path_toni_inst_2);
		eval.show(Settings::path_toni_inst_3a, Settings::GroundTruth::path_toni_inst_3);




		int i = 0; (void) i;

	}

//	// markus B
//	if (1 == 0) {
//		Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(Settings::fMap);;
//		MarkusB::walkEverything(map);
//	}

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

		//75% quartil > 20 meter -> fehler? [verlaufen irgendwo ab der hälfte]
		//EvalWalk::walkEverything(map);
		EvalWalk::walkEverythingBuildStats(map);
		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 == 0) {
		//EvalWiFi ew1(Settings::fMap, Settings::path1a, Settings::GroundTruth::path1);
		EvalWiFi ew1(Settings::fMap, Settings::path1a, Settings::GroundTruth::path1);
		//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, "normal model");
		ew1.load(Settings::wifiEachOptParPos_multimodel, "model per floor");
		ew1.load(Settings::wifiEachOptParPos_perBBox, "model per region");
		//ew1.load<WiFiModelLogDistCeiling>(Settings::wifiEachOptParPos_only1st, "everything opt");
		//ew1.writeTeX("path1");

		sleep(1000);

	}

	// show wifi multimodalities
	if (1 == 0) {

		Plotty::Settings settings;
		settings.maxZ = 8;

		EvalWiFiGround ewg(map, Settings::wifiEachOptParPos_perBBox, settings);
		//ewg.show(Settings::path1a, 30);
		//ewg.show(Settings::path1a, 70);
		//ewg.show(Settings::path1a, 90);

		int hueGreen = 120*255/360;
		int hueYellow = 60*255/360;
		int hueBlue = 210*255/360;
		int hueRed = 0*255/360;

		ewg.addGT(Settings::path1a, 100, hueGreen, Settings::GroundTruth::path1);
		ewg.addGT(Settings::path1a, 170-8, hueBlue, Settings::GroundTruth::path1);
		ewg.addGT(Settings::path1a, 200, hueRed, Settings::GroundTruth::path1);


		// green
		ewg.add(Settings::path1a, 100, hueGreen);

		// yellow
		ewg.add(Settings::path1a, 170, hueBlue);

		// red
		ewg.add(Settings::path1a, 200, hueRed);

		ewg.groundProb->getPlot().splot.getObjects().reOrderByZIndex();
		ewg.groundProb->plotMe();

		ewg.writeGP(Settings::fPathGFX, "wifiMultimodality");

		sleep(1000);

	}


	// wifi vs ground-truth distance error
	if (1 == 1) {

		std::vector<std::string> files = {
			Settings::path1a, Settings::path1b,
			Settings::path2a, Settings::path2b,
			Settings::path_toni_all_1a, Settings::path_toni_all_1b,
			Settings::path_toni_all_2a, Settings::path_toni_all_2b,
			Settings::path_toni_inst_1a, Settings::path_toni_inst_1b,
			Settings::path_toni_inst_2a, Settings::path_toni_inst_2b,
			Settings::path_toni_inst_3a, Settings::path_toni_inst_3b,
		};
		std::vector<std::vector<int>> gtIndices = {
			Settings::GroundTruth::path1, Settings::GroundTruth::path1,
			Settings::GroundTruth::path2, Settings::GroundTruth::path2,
			Settings::GroundTruth::path1, Settings::GroundTruth::path1,
			Settings::GroundTruth::path2, Settings::GroundTruth::path2,
//			Settings::GroundTruth::path_toni_all_1, Settings::GroundTruth::path_toni_all_1,
//			Settings::GroundTruth::path_toni_all_2, Settings::GroundTruth::path_toni_all_2,
			Settings::GroundTruth::path_toni_inst_1, Settings::GroundTruth::path_toni_inst_1,
			Settings::GroundTruth::path_toni_inst_2, Settings::GroundTruth::path_toni_inst_2,
			Settings::GroundTruth::path_toni_inst_3, Settings::GroundTruth::path_toni_inst_3,
		};

		int numScans = 0;
		for (const std::string& file : files) {
			Offline::FileReader reader(file);
			numScans += reader.getWiFiGroupedByTime().size();
		}

		std::cout << "num scans: " << numScans << std::endl;

		// different VAP settings
		if (1 == 1) {

			EvalWiFiPaths ewp(Settings::fMap);

			ewp.vap->setMinOccurences(2);
			ewp.loadModel(Settings::wifiEachOptPar, "0");
			ewp.walks(files, gtIndices);

			ewp.vap->setMinOccurences(0);
			ewp.loadModel(Settings::wifiEachOptPar, "1");
			ewp.walks(files, gtIndices);

			sleep(10000);

		}

		// stats as table
		if (1 == 0) {

			EvalWiFiPaths ewp(Settings::fMap);
			ewp.loadModel(Settings::wifiAllFixed, "0");
			ewp.walks(files, gtIndices);

			ewp.loadModel(Settings::wifiAllOptPar, "1");
			ewp.walks(files, gtIndices);

			ewp.loadModel(Settings::wifiEachOptPar, "2");
			ewp.walks(files, gtIndices);

			ewp.loadModel(Settings::wifiEachOptParPos, "3");
			ewp.walks(files, gtIndices);

			ewp.loadModel(Settings::wifiEachOptParPos_multimodel, "4");
			ewp.walks(files, gtIndices);

			ewp.loadModel(Settings::wifiEachOptParPos_perBBox, "5");
			ewp.walks(files, gtIndices);

			// FOR PAPER
			ewp.dumpStats();

		}

		// stats as GFX
		if (1 == 0) {

			EvalWiFiPaths ewp(Settings::fMap);
			ewp.loadModel(Settings::wifiAllFixed, "\\noOptEmpiric{}");
			ewp.walks(files, gtIndices);

			ewp.loadModel(Settings::wifiEachOptParPos, "\\optParamsPosEachAP{}");
			ewp.walks(files, gtIndices);

			ewp.loadModel(Settings::wifiEachOptParPos_multimodel, "\\optPerFloor{}");
			ewp.walks(files, gtIndices);

			ewp.loadModel(Settings::wifiEachOptParPos_perBBox, "\\optPerRegion{}");
			ewp.walks(files, gtIndices);

			ewp.writeGP(Settings::fPathGFX, "modelPerformance");

		}


		// examine various modifications [did not work out.. not within the paper]
		if (1 == 0) {

			EvalWiFiPathMethods ewpm(Settings::fMap);
			ewpm.loadModel(Settings::wifiEachOptParPos_perBBox, "model per region", "original", "alternative");
			ewpm.walks(files, gtIndices);

			// export for paper
			// using errFuncOtherExponential and only path1a, path1b
			//ewpm.writeGP(Settings::fPathGFX, "normalVsExp");

		}

		sleep(10000);

	}



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

	}
}