#ifndef PARTICLEWALK_H
#define PARTICLEWALK_H

#include "file.h"

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

#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/GridNodeImportance.h>
#include <Indoor/grid/factory/v2/Importance.h>

#include <Indoor/grid/factory/v3/GridFactory3.h>


#include <Indoor/grid/walk/v3/Walker.h>
#include <Indoor/grid/walk/GridWalkState.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/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"


//template<typename T, typename... Args>
//std::unique_ptr<T> make_unique(Args&&... args) {
//	return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
//}


class ParticleWalk {

public:


	struct MyWalkState {
		GridPoint position;
	};

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

	struct MyState {

		//GridPoint pos;
		Point3 pos;
		Heading head = Heading(0);

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

	};

	struct MyObservation {

	};

//	struct MyPfInit : public K::ParticleFilterInitializer<MyState> {
//		Point3 pos;
//		MyPfInit(Point3 pos) : pos(pos) {;}
//		virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
//			for (K::Particle<MyState>& p : particles) {
//				p.state.pos = pos;
//				p.state.head = Heading(0);
//				p.state.weight = 1;
//			}
//		}
//	};

	struct MyGridNode : public GridNode, public GridPoint, public GridNodeImportance {

	public:

		MyGridNode(int x, int y, int z) : GridPoint(x,y,z) {;}

	};

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

		MyControl ctrl;
		PoseDetection* poseDetect = new PoseDetection();
		StepDetection* stepDetect = new StepDetection();		// TODO
		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;
	WalkViz3 vizGrid;

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

	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.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.65, 0.35, 0.00005, 0.7);		// 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

			//Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(File::getData("/walks/walk_in_circles_around_hole_map.xml"));
			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);

			// data source
			//Offline::FileReader reader(File::getData("/walks/walk_in_circles_around_hole.csv")); const Point3 startP3 = Point3(0,0,0);
			Offline::FileReader reader(File::getData("/walks/walk_stair_down_and_up_again.csv"));
			Offline::FilePlayer player(&reader, &test);



		#endif

		viz.addMap(map);
		vizGrid.addMap(map);

		// build the grid
		Grid<MyGridNode> grid(gridSize_cm);
//		GridFactory<MyGridNode> gf(grid);
//		gf.build(map);
		GridFactory3<MyGridNode> gf(grid);
		gf.build(map);

		// add importance
		Importance::addImportance(grid);

		// show
		vizGrid.addGridWalkImp(grid, true);			// also debug-plot the grid
		vizGrid.draw();




#define WALK_MODE 1

#if (WALK_MODE==1)

		using GridWalker = GW3::WalkerDirectDestination<MyGridNode>;
		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);

#elif (WALK_MODE==2)

		using GridWalker = GW3::WalkerWeightedRandom<MyGridNode>;
		Distribution::Normal<float> dDistPlane(0.65, 0.001);	// walker walks straight -> we have to add noise ourselves
		Distribution::Normal<float> dDistStair(0.35, 0.001);	// walker walks straight -> we have to add noise ourselves
		Distribution::Normal<float> dHead(1, 0.00000015);

#endif

		GridWalker walker(grid);

		// eval
		const float stepSizeSigma_m = 0.1;
		const float turnSigma_rad = Angle::degToRad(2.5);
		walker.addEvaluator(new GW3::WalkEvalDistance<MyGridNode>(grid, stepSizeSigma_m));
		walker.addEvaluator(new GW3::WalkEvalHeadingStartEnd<MyGridNode>(turnSigma_rad));
		//walker.addEvaluator(new GW3::WalkEvalEndNodeProbability<MyGridNode>(&grid));

		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] (SPF::Particle<MyState>& p) {
			p.state.pos = startP3;
			p.state.head = Heading(0);
			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));



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

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

			#ifdef FAKE
				const Point3 pos = synthWalker.tick(timePassed);
				viz.setGT(pos);
			#endif

			#ifdef REAL
				player.tick();
			#endif

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

				// adjust the heading
				p.state.head = p.state.head + test.ctrl.headChange_rad * dHead.draw();

				//const GridPoint start = p.pos;
				const Point3 start = p.state.pos;

				const MyGridNode& nStart = grid.getNodeFor(GridPoint(start.x*100, start.y*100, start.z*100));

				//const GridPoint dst = walker.getDestination(grid, start, ctrl, pathDistance);
				//p.pos = dst;
				//K::GnuplotPoint2 gp(p.pos.x_cm, p.pos.y_cm);

				//kein kopieren von distribution erlauben!" das macht nicht was es soll!"

				//const float add_m = (nStart.getType() == GridNode::TYPE_STAIR) ? (dDistStair.draw()) : (dDistPlane.draw());
				//const float add_m = (grid.isPlain(nStart)) ? (dDistPlane.draw()) : (dDistStair.draw());

				//const float add_m = dDistPlane.draw();

				//const float walkDistance = test.ctrl.steps * add_m;

				GW3::WalkParams params;
				params.stepSizes.stepSizeFloor_m = dDistPlane.draw();
				params.stepSizes.stepSizeStair_m = dDistStair.draw();
				params.start = start;
				params.heading = p.state.head;
				//params.distance_m = walkDistance;
				params.lookFurther_m = 1;
				params.numSteps = 1;

				const GW3::WalkResult res = walker.getDestination(params);
				p.state.pos = res.position;
				p.state.head = res.heading;
				p.weight *= res.probability;
				K::GnuplotPoint3 gp(p.state.pos.x, p.state.pos.y, p.state.pos.z);

				//viz.particles.add(gp);
				//std::cout << node->asString() << std::endl;

			};



			// 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();
				MyState est = pf.update(ctrl, obs, fTrans, fEval);
				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.x, est.pos.y, est.pos.z));

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

				viz.setCurPos(est.pos);



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

			}

			if (cnt % 150 == 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";

				//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");
				++nr;
			}

//			for (const SPF::Particle<MyState>& p : pf.getParticles()) {
//				viz.particles.add(gp);
//			}

		}


//		// walk along the path
//		for (int i = 0; i < 30000; ++i) {

//			viz.particles.clear();;

//			// increment the walk
//			const Timestamp timePassed = Timestamp::fromMS(10);
//			const Point3 pos = synthWalker.tick(timePassed);

//			++cnt;
//			if (cnt % 50 == 0) {

//				for (MyParticle& p : particles) {

//					// adjust the heading
//					p.head = p.head + test.ctrl.headChange_rad * dHead.draw();

//					//const GridPoint start = p.pos;
//					const Point3 start = p.pos;

//					//const GridPoint dst = walker.getDestination(grid, start, ctrl, pathDistance);
//					//p.pos = dst;
//					//K::GnuplotPoint2 gp(p.pos.x_cm, p.pos.y_cm);

//					const float walkDistance = test.ctrl.steps * 0.7 * dDist.draw();

//					GridWalker::WalkParams params;
//					params.start = start;
//					params.heading = p.head;
//					params.distance_m = walkDistance;

//					const GridWalker::WalkResult res = walker.getDestination(grid, params);
//					p.pos = res.position;
//					p.head = res.heading;
//					p.weight *= res.probability;
//					K::GnuplotPoint3 gp(p.pos.x, p.pos.y, p.pos.z);

//					viz.particles.add(gp);
//					//std::cout << node->asString() << std::endl;

//				}

//				test.reset();
//	//			pViz.show(particles);

//				viz.draw();

//			}

//			std::this_thread::sleep_for(std::chrono::milliseconds(1));
//		}

	}

};

#endif // PARTICLEWALK_H