#ifndef PRESENTATION_H
#define PRESENTATION_H

#include "../eval/EvalBase.h"
#include "../DijkstraMapper.h"
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>

#include <Indoor/grid/walk/GridWalkSimpleControl.h>
#include <Indoor/grid/walk/GridWalkPathControl.h>
#include <Indoor/grid/walk/GridWalkShortestPathControl.h>

#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>

#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>

#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>


class Presentation {

	struct PlotErr {

		K::GnuplotPlot plot;
		K::GnuplotPlotElementLines lErr1;
		K::GnuplotPlotElementLines lErr2;

		PlotErr() {
			plot.add(&lErr1);
			plot.add(&lErr2);
			lErr1.setLineWidth(2);	lErr1.setColorHex("#000000");
			lErr2.setLineWidth(2);	lErr2.setColorHex("#666666");
			plot.addCustom("set xrange[0:160]");
			plot.addCustom("set yrange[0:12]");
		}

		void add(float ts, float err1, float err2) {
			ts /= 1000;
			lErr1.add(K::GnuplotPoint2(ts, err1));
			lErr2.add(K::GnuplotPoint2(ts, err2));
		}

	};

	struct PlotEST {

		K::Gnuplot gp;
		K::GnuplotSplot plot;
		K::GnuplotSplotElementLines walls;
		K::GnuplotSplotElementLines path1;
		K::GnuplotSplotElementLines path2;
		K::GnuplotSplotElementLines groundtruth;
		K::GnuplotSplotElementPoints particles;
		std::stringstream misc;

		PlotEST(Helper::FHWSFloors& floors, GroundTruthWay& gtw) {
			plot.addCustom("unset border");
			plot.addCustom("unset tics");
			plot.addCustom("unset cbrange");
			plot.add(&walls);
			plot.add(&particles);
			plot.add(&groundtruth);
			plot.add(&path1);
			plot.add(&path2);
			walls.setColorHex("#666666");
			//groundtruth.setCustomAttr("dashtype 3");
			groundtruth.setLineWidth(2);
			groundtruth.setColorHex("#009900");
			particles.setPointType(7);
			particles.setPointSize(0.25f);
			particles.setColorHex("#3333ff");
			path1.setLineWidth(3);	path1.setColorHex("#000000");
			path2.setLineWidth(3);	path2.setColorHex("#666666");
			addFloor(floors.f0, floors.h0.cm());
			addFloor(floors.f1, floors.h1.cm());
			addFloor(floors.f2, floors.h2.cm());
			addFloor(floors.f3, floors.h3.cm());
			addGT(gtw);
		}

		void addFloor(const Floor& f, const float height) {
			for (const Line2& l : f.getObstacles()) {
				const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height);
				const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height);
				walls.addSegment(p1, p2);
			}
		}

		void addEst1(const std::vector<Point3>& way) {
			path1.clear();
			for (Point3 p : way) {
				K::GnuplotPoint3 gp(p.x, p.y, p.z);
				path1.add(gp);
			}
		}

		void addEst2(const std::vector<Point3>& way) {
			path2.clear();
			for (Point3 p : way) {
				K::GnuplotPoint3 gp(p.x, p.y, p.z);
				path2.add(gp);
			}
		}

		void addGT(GroundTruthWay& gtw) {
			for (auto it : gtw.getWay()) {
				K::GnuplotPoint3 gp(it.value.x, it.value.y, it.value.z);
				groundtruth.add(gp);
			}
		}

		void setCur(Point3 est, Point3 gt) {
			misc.str("");
			misc << "set arrow 1 from " << est.x << "," << est.y << "," << est.z+150 << " to " << est.x << "," << est.y << "," << est.z << " lw 3\n";
			misc << "set arrow 2 from " << gt.x << "," << gt.y << "," << gt.z+150 << " to " << gt.x << "," << gt.y << "," << gt.z << "\n";
		}



		template <typename T> void addParticles(const T& lst) {
			particles.clear();
			for (int i = 0; i < (int) lst.size(); i+=15) {
				const K::Particle<MyState>& p = lst[i];
				const K::GnuplotPoint3 pp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
				particles.add(pp);
			}
		}

	};

	template <typename T> struct LeShortestPath {

		Grid<T>& grid;
		DijkstraMapper acc;
		Dijkstra<T> dijkstra;
		Point3 dst;

		LeShortestPath(Grid<T>& grid, Point3 dst) : grid(grid), acc(grid), dst(dst) {
			const T& nEnd = grid.getNearestNode( GridPoint(dst.x, dst.y, dst.z) );
			dijkstra.build(&nEnd, acc);
		}

		void addShortestPath(Point3 start, PlotEST& plot) {

			const T& nStart = grid.getNearestNode( GridPoint(start.x, start.y, start.z) );
			const T& nEnd = grid.getNearestNode( GridPoint(dst.x, dst.y, dst.z) );

			DijkstraNode<T>* dnStart = dijkstra.getNode(nStart);
			DijkstraNode<T>* dnEnd = dijkstra.getNode(nEnd);

			plot.path1.setColorHex("#0000ee");
			plot.path1.clear();
			DijkstraPath<T> dp(dnStart, dnEnd);
			for (DijkstraNode<T>* dn : dp) {
				K::GnuplotPoint3 gp(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm);
				plot.path1.add(gp);
			}

		}

	};


public:

	void xxx() {

		float stepSize = 0.71;

		K::ParticleFilter<MyState, MyControl, MyObservation>* pf1;
		K::ParticleFilter<MyState, MyControl, MyObservation>* pf2;

		Grid<MyGridNode> grid(20);
		Helper::FHWSFloors floors = Helper::getFloors(grid);
		Helper::buildTheGrid(grid, floors);

		pf1 = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
		pf2 = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );

		std::unique_ptr<MyEvaluation> eval1 = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
		eval1.get()->setUsage(true, true, true, true, true);
		pf1->setEvaluation( std::move(eval1) );
		pf1->setNEffThreshold(1.0);
		pf1->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
		pf1->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));

		std::unique_ptr<MyEvaluation> eval2 = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
		eval2.get()->setUsage(true, true, true, true, true);
		pf2->setEvaluation( std::move(eval2) );
		pf2->setNEffThreshold(1.0);
		pf2->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
		pf2->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));



		//runName = "bergwerk_path4_nexus_shortest";

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

		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!
		BarometerEvaluation::barometerSigma = 0.10;
		sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
		srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
		srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/Steps2.txt");
		gtw = EvalBase::getGroundTruthWay(*sr, floors.gtwp, path4dbl);

		MyGridNode& end = (MyGridNode&)grid.getNodeFor( EvalBase::conv(floors.gtwp[path4dbl.back()]) );

		{
			GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
			//DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
			pf1->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
		}

		{
			GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
			pf2->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
		}






		PlotEST pEst(floors, gtw);
		PlotEST pShortest(floors, gtw);
		PlotErr pErr;
		LeShortestPath<MyGridNode> pShortestFac(grid, gtw.back().value);

		K::Gnuplot gp;
		K::GnuplotMultiplot multiplot(2,1);

		multiplot.add(&pErr.plot);
		multiplot.add(&pEst.plot);
		//multiplot.add(&pShortest.plot);




			// sensor numberspShortest
			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> pathEst1;
			std::vector<Point3> pathEst2;


			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_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*)pf1->getTransition())->setCurrentTime(lastTransitionTS);
					((MyTransition*)pf2->getTransition())->setCurrentTime(lastTransitionTS);


					// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
					const MyState est1 = pf1->update(&ctrl, obs);
					const MyState est2 = pf2->update(&ctrl, obs);

					const Point3 curEst1 = est1.pCur;
					const Point3 curEst2 = est2.pCur;

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

					const Point3 diff1 = curEst1 - curGT;
					const Point3 diff2 = curEst2 - curGT;

					const float err1 = diff1.length();
					const float err2 = diff2.length();

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

					pathEst1.push_back(curEst1);
					pathEst2.push_back(curEst2);
					//const float err = diff.length();
					//errors.push_back(err);

					// plot
					pEst.setCur(curEst1, curGT);
					pEst.addParticles(pf1->getParticles());
					pEst.addEst1(pathEst1);
					pEst.addEst2(pathEst2);
					pShortestFac.addShortestPath(curEst1, pShortest);
					pErr.add(se.ts-start_time, err1/100, err2/100);


					static int pi = 0;
					static int dspCnt = 0;
					if (++dspCnt > 0) {

						//pEst.gp.draw(pEst.plot);
						//pEst.gp.flush();

						//pShortest.gp.draw(pShortest.plot);
						//pShortest.gp.flush();

						float s = 0.75;
						gp.setTerminal("pngcairo", K::GnuplotSize(50*s,29*s));
						gp.setOutput("/tmp/1/" + std::to_string(pi++) + ".png");
						gp << "set multiplot\n";


						gp << "set border\n";
						gp << "set tics\n";
						gp << "set size 1.0, 0.25 \n";
						gp << "set origin 0.0, 0.0 \n";
						gp << "unset xtics\n";
						gp.draw(pErr.plot);

						gp << "unset xrange; unset yrange; unset zrange;\n";

						gp << "set size 0.68, 1.04 \n";

						gp << "set origin -0.08, 0.04 \n";
						gp << pEst.misc.str();
						gp.draw(pEst.plot);
						gp << "unset arrow\n";

						gp << "set origin 0.405, 0.04 \n";
						gp << pShortest.misc.str();
						gp.draw(pShortest.plot);
						gp << "unset arrow\n";

						gp << "unset multiplot\n";

						gp.flush();

						usleep(1000*33);	// prevent gnuplot errors
						dspCnt = 0;

						std::cout << pi << std::endl;

						/** GIF */
						/*
							#!/bin/sh
							palette="/tmp/palette.png"
							filters=""
							ffmpeg -i %d.png -vf "$filters,palettegen" -y $palette
							ffmpeg -i %d.png -i $palette  -lavfi "setpts=3.0*PTS,paletteuse" yy.gif

							OR
							ffmpeg -f image2 -i %d.png -vf "setpts=3.0*PTS" nyan.flv
						*/

					}



				}

			}

		}

};

#endif // PRESENTATION_H