#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 "EvalApOpt.h"
#include "EvalData.h"
#include "EvalWiFi.h"
#include "EvalWiFiSigStrength.h"
#include "pf/EvalWalk.h"

#include "PlotErrFunc.h"

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

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

	if (1==1){
		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;
	}

	if (1==1) {

		const bool ignoreStaircases = false;
		const bool ignoreOutdoor = false;
		EvalCompareOptAllFixed allFixed(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor);

		K::Statistics<float> s1 = allFixed.fixedPosFixedParamsForAll(); //BREAK;
		K::Statistics<float> s2 = allFixed.fixedPosOptParamsForAll(); //BREAK;
		K::Statistics<float> s3 = allFixed.fixedPosOptParamsForEach(); //BREAK;
		K::Statistics<float> s4 = allFixed.optPosOptParamsForEach(); //BREAK;

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

		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==1){
		EvalCompareOptAllFixed e1(Settings::fMap, Settings::fCalib, false, false);
		EvalCompareOptAllFixed e2(Settings::fMap, Settings::fCalib, true, false);
		EvalCompareOptAllFixed e3(Settings::fMap, Settings::fCalib, false, true);
		EvalCompareOptAllFixed e4(Settings::fMap, Settings::fCalib, true, true);

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

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

	}

}


int main(void) {

//	paperOutputs();		return 0;

	if (1 == 1) {
		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 == 0) {
		//EvalWiFi ew1(Settings::fMap, Settings::path2a, Settings::GroundTruth::path2);
		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;

	}




	// run earthmapping
	//testMapEarthReg(fMap);

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


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

		const bool ignoreStaircases = false;
		const bool ignoreOutdoor = false;
		EvalCompareOptAllFixed allFixed(Settings::fMap, Settings::fCalib, ignoreStaircases, ignoreOutdoor);

		K::Statistics<float> s1 = allFixed.fixedPosFixedParamsForAll(); //BREAK;
		K::Statistics<float> s2 = allFixed.fixedPosOptParamsForAll(); //BREAK;
		K::Statistics<float> s3 = allFixed.fixedPosOptParamsForEach(); //BREAK;
		K::Statistics<float> s4 = allFixed.optPosOptParamsForEach(); //BREAK;

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

	}

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

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

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

		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;

}