#ifndef EVALBASE_H
#define EVALBASE_H

#include "../Settings.h"
#include "../Helper.h"
#include "../Vis.h"

#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/statistics/Statistics.h>

#include "GroundTruthWay.h"

#include "../particles/MyState.h"
#include "../particles/MyObservation.h"
#include "../particles/MyEvaluation.h"
#include "../particles/MyTransition.h"
#include "../particles/MyInitializer.h"

#include "../reader/SensorReader.h"
#include "../reader/SensorReaderStep.h"
#include "../reader/SensorReaderTurn.h"

#include "../lukas/TurnObservation.h"
#include "../lukas/StepObservation.h"

#include "../toni/BarometerSensorReader.h"

#include "../frank/WiFiSensorReader.h"
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"


#include <Indoor/misc/Time.h>

class EvalBase {

protected:

	Grid<MyGridNode> grid;
	Helper::FHWSFloors floors;

	Vis vis;

	K::ParticleFilter<MyState, MyControl, MyObservation>* pf;

	SensorReader* sr;
	SensorReaderTurn* srt;
	SensorReaderStep* srs;



	std::string runName;

	GroundTruthWay gtw;

	// OLD
	//std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
	//std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
	//std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};

	// NEW
	std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
	std::vector<int> path1dbl = {29, 29, 28,27,26,255,25,24,23,22,21,20};
	std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
	std::vector<int> path2dbl = {19, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
	std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
	std::vector<int> path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
	std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
	std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};	// duplicate 1st waypoint!

	float stepSize = 0.71;

public:

	EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {

		// build the grid
		Helper::buildTheGrid(grid, floors);

		// setup the visualisation
		vis.addFloor(floors.f0, floors.h0);
		vis.addFloor(floors.f1, floors.h1);
		vis.addFloor(floors.f2, floors.h2);
		vis.addFloor(floors.f3, floors.h3);

		vis.floors.setColorHex("#666666");
		vis.groundTruth.setCustomAttr("dashtype 3");
		vis.groundTruth.setColorHex("#009900");
		vis.gp << "unset cbrange\n";


	}

	static GridPoint conv(const Point3& p) {
		return GridPoint(p.x, p.y, p.z);
	}

	static GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {

		// construct the ground-truth-path by using all contained waypoint ids
		std::vector<Point3> path;
		for (int id : ids) {
			auto it = waypoints.find(id);
			if(it == waypoints.end()) {throw "not found";}
			path.push_back(it->second);
		}

		// new created the timed path
		GroundTruthWay gtw;
		int i = 0;
		while (sr.hasNext()) {
			const SensorEntry se = sr.getNext();
			if (se.data.empty()) {continue;}		// why necessary??
			if (se.idx == 99) {
				gtw.add(se.ts, path[i]);
				++i;
			}
		}

		// ensure the sensor-data contained usable timestamps for the ground-truth mapping
		assert(i>0);

		sr.rewind();
		return gtw;

	}

	void run() {

			// sensor numbers
			const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
			//const int s_linearAcceleration = 2;

			std::list<TurnObservation> turn_observations;
			std::list<StepObservation> step_observations;

			//Create an BarometerSensorReader
			BarometerSensorReader baroSensorReader;


			//Read all turn Observations
			while(srt->hasNext()) {

				SensorEntryTurn set = srt->getNext();
				TurnObservation to;

				to.ts = set.ts;
				to.delta_heading = set.delta_heading;
				to.delta_motion = set.delta_motion;

				turn_observations.push_back(to);
			}


			//Step Observations
			while(srs->hasNext()) {

				SensorEntryStep ses = srs->getNext();
				StepObservation so;

				so.ts = ses.ts;

				step_observations.push_back(so);
			}



			// the to-be-evaluated observation
			MyObservation obs;
			obs.step = new StepObservation(); obs.step->steps = 0;
			obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0;

			// control data
			MyControl ctrl;


			std::vector<Point3> pathEst;

			uint64_t lastTransitionTS = 0;
			int64_t start_time = -1;

			K::Statistics<float> statsTime;
			K::Statistics<double> stats;
			int cnt = 0;

			std::vector<float> errors;

			// process each single sensor reading
			while(sr->hasNext()) {

				// get the next sensor reading from the CSV
				const SensorEntry se = sr->getNext();

				//start_time needed for time calculation of steps and turns
				obs.latestSensorDataTS = se.ts;
				if (start_time == -1) {start_time = se.ts;}
				int64_t current_time = se.ts - start_time;

				switch(se.idx) {

					case s_wifi: {
						obs.wifi = WiFiSensorReader::readWifi(se);
						break;
					}

					case s_beacons: {
						BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
						if (!boe.mac.empty()) {
							obs.beacons.entries.push_back(boe);
						}			// add the observed beacon
						obs.beacons.removeOld(obs.latestSensorDataTS);
						break;
					}

					case s_barometer: {
						obs.barometer = baroSensorReader.readBarometer(se);
						break;
					}

//					case s_linearAcceleration:{
//						baroSensorReader.readVerticalAcceleration(se);
//						break;
//					}

					case s_orientation: {
						obs.orientation = OrientationSensorReader::read(se);
						break;
					}

				}

				// process all occurred turns
				while (!step_observations.empty() && current_time > step_observations.front().ts) {
					const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
					obs.step->steps++;
					ctrl.walked_m = obs.step->steps * stepSize;
				}

				// process all occurred steps
				while (!turn_observations.empty() && current_time > turn_observations.front().ts) {
					const TurnObservation _to = turn_observations.front(); turn_observations.pop_front();
					obs.turn->delta_heading += _to.delta_heading;
					obs.turn->delta_motion += _to.delta_motion;
					ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);

				}


				// time for a transition?
				if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {

					auto tick1 = Time::tick();

					lastTransitionTS = se.ts;

					// timed updates
					((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);


					// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
					const MyState est = pf->update(&ctrl, obs);
					const Point3 curEst = est.pCur;

					// error calculation. compare ground-truth to estimation
					const int offset = 0;
					const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
					const Point3 diff = curEst - curGT;

					auto tick2 = Time::tick();
					float diffTime = Time::diffMS(tick1, tick2) * 1.25f;
					statsTime.add(diffTime);
					std::cout << "#" << statsTime.getAvg() << "\t" << diffTime << std::endl;

					// skip the first 10 scans due to uniform distribution start

					pathEst.push_back(curEst);
					const float err = diff.length();
					errors.push_back(err);

					// skip the first 24 scans due to uniform distribution start (12 seconds)
					if (++cnt > 24) {
						stats.add(err);
						std::cout << stats.asString() << std::endl;
					}

					// plot
					vis.clearStates();
					for (int i = 0; i < (int) pf->getParticles().size(); i+=15) {
						const K::Particle<MyState>& p = pf->getParticles()[i];
						vis.addState(p.state.walkState);
					}
					vis.setTimestamp(se.ts);
					vis.addGroundTruth(gtw);
					vis.addEstPath(pathEst);
					vis.setEstAndShould(curEst, curGT);

					if (obs.barometer != nullptr) {
						vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
					}
					vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";

					//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";

					Point2 p1(0.1, 0.1);
					Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
					//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
					vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";

					static int dspCnt = 0;
					if (++dspCnt > 0) {
						vis.show();
						usleep(1000*33);	// prevent gnuplot errors
						dspCnt = 0;
					}

				}

			}

			// append error for each run to a file
			std::ofstream oTError("/tmp/errors.txt", std::ios_base::app);
			oTError << runName << "\n\t";	stats.appendTo(oTError); oTError << "\n\n";
			oTError.close();

			// detailled error-description
			std::ofstream oError("/tmp/err_" + runName + ".dat");
			for (float f : errors) {oError << f << "\n";}
			oError.close();

			// plot-data
			std::ofstream oPath("/tmp/path_" + runName + ".dat");	vis.groundTruth.addDataTo(oPath); oPath.close();
			std::ofstream oEst("/tmp/est_" + runName + ".dat");		vis.estPath.addDataTo(oEst); oEst.close();
			std::ofstream oFloor("/tmp/floors.dat");				vis.floors.addDataTo(oFloor); oFloor.close();

			std::ofstream oPlot("/tmp/plot_" + runName + ".gp");

			oPlot << "set terminal eps size 3.4,2\n";
			oPlot << "set output '" << runName << ".eps'\n";
			oPlot << "set termoption dashlength 0.5\n";

			oPlot << "set ticslevel 0\n";
			oPlot << "set view equal xy\n";
			oPlot << "set zrange [0:2200]\n";
			oPlot << "set multiplot layout 1,1 scale 2.7,2.7 offset 0,0.23\n";
			oPlot << "set view 72,33\n";

			oPlot << "unset border\n";
			oPlot << "unset xtics\n";
			oPlot << "unset ytics\n";
			oPlot << "unset ztics\n";

			oPlot << "splot \\\n";
			oPlot << "'floors.dat'								skip 21 notitle with lines lc rgb '#777777', \\\n";
			oPlot << "'path_bergwerk_path2_nexus_shortest.dat'	skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
			oPlot << "'est_bergwerk_path2_nexus_shortest.dat'	skip 21 notitle with lines lw 2.5 lc rgb '#000099' ";

			oPlot.close();

		}



};

#endif // EVALBASE_H