#ifndef PARTICLEWALKNAVMESH_H
#define PARTICLEWALKNAVMESH_H


#include "file.h"

#include <Indoor/math/distribution/Const.h>

#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>

#include <Indoor/navMesh/NavMeshTriangle.h>

#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/NavMeshFactory.h>
//#include <Indoor/navMesh/NavMeshDebug.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkRandom.h>
#include <Indoor/navMesh/walk/NavMeshWalkSemiRandom.h>
#include <Indoor/navMesh/walk/NavMeshWalkSemiDirected.h>
#include <Indoor/navMesh/meta/NavMeshDijkstra.h>

#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/offline/FilePlayer.h>

#include <Indoor/math/Interpolator.h>
#include <Indoor/synthetic/SyntheticWalker.h>
#include <Indoor/synthetic/SyntheticSteps.h>
#include <Indoor/synthetic/SyntheticTurns.h>

#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/StepDetection2.h>
#include <Indoor/sensors/imu/TurnDetection.h>

#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>

#include <KLib/math/statistics/Statistics.h>


#include "SlimParticleFilter.h"

#include <thread>

#include "WalkViz.h"
#include "WalkViz3.h"
#include "ProbViz.h"


class ParticleWalkNavMesh {

public:

	struct MyNavMeshTria : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra {

	public:

		MyNavMeshTria(Point3 p1, Point3 p2, Point3 p3, uint8_t t) : NM::NavMeshTriangle(p1,p2,p3,t) {;}

	};

	struct MyWalkState {
		GridPoint position;
	};

	struct MyControl {
		int steps = 0;
		float headChange_rad = 0;
	};

	struct MyState {

		//GridPoint pos;
		NM::NavMeshLocation<MyNavMeshTria> pos;
		Heading head = Heading(0);

		MyState operator * (const double w) const {
			MyState res; res.pos.pos = this->pos.pos*w; return res;
		}
		MyState& operator += (const MyState& s) {
			this->pos.pos += s.pos.pos;
			return *this;
		}
		MyState& operator /= (const double d) {
			this->pos.pos /= d;
			return *this;
		}

	};

	struct MyObservation {

	};


	struct Test : public SyntheticSteps::Listener, public SyntheticTurns::Listener, public Offline::Listener {

		MyControl ctrl;
		PoseDetection* poseDetect = new PoseDetection();
		StepDetection* stepDetect = new StepDetection();
		//StepDetection2* stepDetect = new StepDetection2();
		TurnDetection* turnDetect = new TurnDetection(poseDetect);

	public:

		void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) override {
			const bool step = stepDetect->add(ts, acc);
			if (step) {++ctrl.steps;}
		}

		void onSyntheticTurnData(const Timestamp ts, const AccelerometerData acc, const GyroscopeData gyro) override {
			poseDetect->addAccelerometer(ts, acc);
			const float rad = turnDetect->addGyroscope(ts, gyro);
			ctrl.headChange_rad += rad;
		}

		virtual void onGyroscope(const Timestamp ts, const GyroscopeData gyro) {
			const float rad = turnDetect->addGyroscope(ts, gyro);
			ctrl.headChange_rad += rad;
		}

		virtual void onAccelerometer(const Timestamp ts, const AccelerometerData acc) {
			poseDetect->addAccelerometer(ts, acc);
			const bool step = stepDetect->add(ts, acc);
			if (step) {++ctrl.steps;}
		}

		virtual void onGravity(const Timestamp ts, const GravityData data) {};
		virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) {};
		virtual void onBarometer(const Timestamp ts, const BarometerData data) {};
		virtual void onGPS(const Timestamp ts, const GPSData data) {};
		virtual void onCompass(const Timestamp ts, const CompassData data) {};
		virtual void onMagnetometer(const Timestamp ts, const MagnetometerData data) {};


		void reset() {
			ctrl.steps = 0;
			ctrl.headChange_rad = 0;
		}

	};

	//WalkViz3 viz;
	NM::NavMeshDebug vizNM;

	const int gridSize_cm = 50;//22;
	const int numParticles = 3333; //3333;

	void run() {

		Test test;


		#define REAL

		#ifdef FAKE

			// load the map and ground-truth
			//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map2b.xml")); std::vector<int> ids = {0,1,2,3,4,5,6,7,8,9};
			//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map3b_for_nav_mesh.xml"));	std::vector<int> ids = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};
			//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::get("map3c.xml"));	std::vector<int> ids = {0,1,2,3,4,5,6,7,8,9,10};
			//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/maps/map_stair1.xml"));	std::vector<int> ids = {0,1,2,3,4,5,6};

			// get a walk
			SyntheticPath walk1;
			walk1.create(map, ids);
			walk1.smooth(1,3);
			//viz.showPath(walk1);

			// fake estimation sensor data
			SyntheticWalker synthWalker(walk1);
			SyntheticSteps synSteps(&synthWalker, 0.70, 0.35, 0.00005, 0.6);		// TODO, why does step-sigma have such a huge impact?
			SyntheticTurns synTurns(&synthWalker, 0.01,0.05, 0.4);

			synSteps.addListener(&test);
			synTurns.addListener(&test);

			const Point3 startP3 = walk1.getPosAfterDistance(0);

		#endif

		#ifdef REAL

			//float startHead = 0;

			//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_in_circles_around_hole_map.xml")); const Point3 startP3 = Point3(0,0,0); vizNM.plot.getAxisZ().setRange(0, 15);
			//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_stair_down_and_up_again_map_b.xml"));const Point3 startP3 = Point3(0,0,6);
			//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/maps/map_free.xml")); const Point3 startP3 = Point3(0,0,0); vizNM.plot.getAxisZ().setRange(0, 15);

//			Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/maps/SHL41_nm.xml"));
//			map->floors[0]->enabled = false; map->floors[1]->enabled = false; map->floors[3]->enabled = false;
//			Offline::FileReader reader(File::getData("/walks/walk_in_circles_around_hole.csv"));
//			const Point3 startP3 = Point3(61.0, 39.8, 7.4); float startHead = M_PI/2;
//			vizNM.plot.getAxisX().setRange(50, 64);
//			vizNM.plot.getAxisY().setRange(37, 47);
//			vizNM.plot.getAxisZ().setRange(7.3, 7.5);

//			Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_in_circles_around_hole_map.xml")); const Point3 startP3 = Point3(0,0,0); vizNM.plot.getAxisZ().setRange(0, 15);
//			Offline::FileReader reader(File::getData("/walks/walk_in_circles_around_hole.csv")); //const Point3 startP3 = Point3(0,0,0);
//			const float startHead = 0;

			Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("walk_stair_down_and_up_again_map_b.xml"));const Point3 startP3 = Point3(0,0,6);
			Offline::FileReader reader(File::getData("walk_stair_down_and_up_again.csv"));
			const float startHead = 0;

			Offline::FilePlayer player(&reader, &test);



		#endif



		//viz.addMap(map);

		// build the NavMesh
		NM::NavMesh<MyNavMeshTria> mesh;
		NM::NavMeshSettings settings;
		NM::NavMeshFactory<MyNavMeshTria> fac(&mesh, settings);
		fac.build(map);
		//NM::NavMeshDebug::show(mesh);

		vizNM.addMesh(mesh);

#define WALK_MODE 1
//#define WALK_USE_MANY

#if (WALK_MODE==1)

		using MeshWalker = NM::NavMeshWalkSimple<MyNavMeshTria>;
//		Distribution::Normal<float> dDistPlane(0.70, 0.13);	// walker walks straight -> we have to add noise ourselves
//		Distribution::Normal<float> dDistStair(0.35, 0.13);	// walker walks straight -> we have to add noise ourselves
//		Distribution::Normal<float> dHead(1, 0.09);

		Distribution::Normal<float> dDistPlane(0.70, 0.09);	// walker walks straight -> we have to add noise ourselves
		Distribution::Normal<float> dDistStair(0.35, 0.09);	// walker walks straight -> we have to add noise ourselves
		//Distribution::Normal<float> dHeadPercent(1, 0.09);
		Distribution::Normal<float> dHead(0, 0.08);

#elif (WALK_MODE==2)

		using MeshWalker = NM::NavMeshWalkRandom<MyNavMeshTria>;
		Distribution::Normal<float> dDistPlane(0.70, 0.0009);
		Distribution::Normal<float> dDistStair(0.35, 0.0009);
		Distribution::Normal<float> dHead(0, 0.0005);

#elif (WALK_MODE==3)

		using MeshWalker = NM::NavMeshWalkSemiRandom<MyNavMeshTria>;
		Distribution::Normal<float> dDistPlane(0.70, 0.0009);		// sieht schlecht aus mit diesen params!
		Distribution::Normal<float> dDistStair(0.35, 0.0009);
		Distribution::Normal<float> dHead(0, 0.0005);

#elif (WALK_MODE==4)

		using MeshWalker = NM::NavMeshWalkSemiDirected<MyNavMeshTria>;
		Distribution::Const<float> dDistPlane(0.65);		// no scatter needed
		Distribution::Const<float> dDistStair(0.35);
		Distribution::Const<float> dHead(0);

#endif



		MeshWalker walker(mesh);

		// eval
		const float stepSizeSigma_m = 0.15;//0.1;
		const float turnSigma_rad = Angle::degToRad(5.5); //2.5);
		walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTria>(stepSizeSigma_m));
		walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTria>(turnSigma_rad));





		// path
		NM::NavMeshDijkstra::stamp(mesh, Point3(-0.3, -0.3, 0.0)); // walkin down the stair
		walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTria>());







		std::vector<MyState> particles; particles.resize(numParticles);

		// initialize particles

//		//const GridPoint startPoint(startP3.x*100, startP3.y*100, startP3.z*100);
//		for (MyState& p : particles) {
//			p.pos = startP3;
//		}




		int cnt = 0;

		//auto pfInit = make_unique<MyPfInit>(new MyPfInit(startP3));
		//K::ParticleFilter<MyState, MyControl, MyObservation> pf(1000, std::move(pfInit);

		auto fInit = [startP3,startHead,&mesh] (SPF::Particle<MyState>& p) {
			p.state.pos = mesh.getLocation(startP3);
			p.state.head = Heading(startHead);
			p.weight = 1;
		};

		auto fEval = [] (const SPF::Particle<MyState>& p, const MyObservation& observation) {
			return 1.0;
		};

		MyControl ctrl;
		MyObservation obs;

		SPF::Filter<MyState, MyControl, MyObservation, false> pf;	// OpenMP
		pf.initialize(numParticles, fInit);

		K::Statistics<float> sss;

		std::this_thread::sleep_for(std::chrono::milliseconds(500));


		//gewichten basierend auf control und nicht control + distMod(gen)!!


		for (int i = 0; i < 300000; ++i) {

			// increment the walk
			const Timestamp timePassed = Timestamp::fromMS(10);

			#ifdef FAKE
				const int updateEvery = 40;
				if (!synthWalker.done()) {
					const Point3 pos = synthWalker.tick(timePassed);
					//viz.setGT(pos);
					vizNM.setGT(pos);
				} else {
					sleep(1000);
				}
			#endif

			#ifdef REAL
				player.tick();
				const int updateEvery = 150;
			#endif

			auto fTransOne = [&] (SPF::Particle<MyState>& p, const MyControl& ctrl) {

				NM::NavMeshWalkParams<MyNavMeshTria> params;
				params.stepSizes.stepSizeFloor_m = dDistPlane.draw();
				params.stepSizes.stepSizeStair_m = dDistStair.draw();
				params.start = p.state.pos;
				params.heading = p.state.head + test.ctrl.headChange_rad + dHead.draw();
				params.numSteps = 1;

				const MeshWalker::ResultEntry e = walker.getOne(params);
				p.state.pos = e.location;
				p.state.head = e.heading;
				p.weight *= e.probability;

			};

			auto fTransMany = [&] (std::vector<SPF::Particle<MyState>>& particles, const MyControl& ctrl) {

				std::vector<SPF::Particle<MyState>> newParticles;

				for (const SPF::Particle<MyState>& _p : particles) {

					NM::NavMeshWalkParams<MyNavMeshTria> params;
					params.stepSizes.stepSizeFloor_m = dDistPlane.draw();
					params.stepSizes.stepSizeStair_m = dDistStair.draw();
					params.start = _p.state.pos;
					params.heading = _p.state.head + test.ctrl.headChange_rad + dHead.draw();
					params.numSteps = 1;

					const MeshWalker::ResultList res = walker.getMany(params);
					for (const MeshWalker::ResultEntry& e : res) {
						SPF::Particle<MyState> p = _p;	// particle copy
						p.state.pos = e.location;
						p.state.head = e.heading;
						p.weight *= e.probability;
						newParticles.push_back(p);
					}

				}

				auto comp = [] (const SPF::Particle<MyState>& p1, const SPF::Particle<MyState>& p2) {return p1.weight > p2.weight;};
				std::sort(newParticles.begin(), newParticles.end(), comp);
				newParticles.erase(newParticles.begin()+numParticles, newParticles.end());

				std::swap(particles, newParticles);

			};



			// every step
			static int cnt = 0; ++cnt;


			//++cnt;		// 500 msec
			//if (cnt % 50 == 0) {
			if (test.ctrl.steps != 0) {
				//viz.particles.clear();

				auto t1 = std::chrono::system_clock::now();

				#ifdef WALK_USE_MANY
					MyState est = pf.update(ctrl, obs, fTransMany, fEval);
				#else
					MyState est = pf.update(ctrl, obs, fTransOne, fEval);
				#endif
				auto t2 = std::chrono::system_clock::now();
				auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1);
				sss.add(duration.count());
				std::cout << sss.asString() << std::endl;

				//viz.pathEstimated.add(K::GnuplotPoint3(est.pos.pos.x, est.pos.pos.y, est.pos.pos.z));
				vizNM.pathEstimated.add(K::GnuplotPoint3(est.pos.pos.x, est.pos.pos.y, est.pos.pos.z));

				//viz.showParticles(pf.getParticles());
				test.reset();

				//viz.setCurPos(est.pos.pos);


				vizNM.showParticles(pf.getParticles());
				vizNM.setCurPos(est.pos.pos);

				PERF_DUMP();

				std::this_thread::sleep_for(std::chrono::milliseconds(35));

			}

			if (cnt % updateEvery == 0) {
				static int nr = 0;
				//viz.plot.getView().setCamera(45, (cnt/70)%360);
				//viz.plot.getView().setEnabled(true);
				//viz.draw();
				//viz.gp << "unset colorbox\n";

				vizNM.gp << "unset colorbox\n";
				vizNM.plot.getView().setCamera(60, (cnt/70)%360);
				vizNM.plot.getView().setEnabled(true);

				//viz.gp.setTerminal("pngcairo", K::GnuplotSize(20,15));
				//viz.gp.setTerminal("pngcairo", K::GnuplotSize(500*3,350*3));
				//viz.gp.setOutput("/tmp/123/" + std::to_string(nr) + ".png");

				//vizNM.gp.setTerminal("pngcairo", K::GnuplotSize(25,19));
				//vizNM.gp.setOutput("/tmp/ani/" + std::to_string(nr) + ".png");
				// ffmpeg -r 15 -i %d.png -preset slow -crf 22 -pix_fmt yuv420p -b:v 700k -filter:v "crop=496:464:140:40" out.mp4

				vizNM.draw();


				++nr;

			}


		}



	}

};

#endif // PARTICLEWALKNAVMESH_H