#ifndef NAV_MESH_FILTER_H
#define NAV_MESH_FILTER_H

#include "mesh.h"

#include <Indoor/geo/Heading.h>
#include <Indoor/math/Distributions.h>

#include <KLib/math/filter/particles/Particle.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/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>

#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkEval.h>

struct MyState {

	/** the state's position (within the mesh) */
	MyNavMeshLocation pos;

	/** the state's heading */
	Heading heading;

	MyState() : pos(), heading(0) {;}

	MyState& operator += (const MyState& o) {
		pos.tria = nullptr;	// impossible
		pos.pos += o.pos.pos;
		return *this;
	}

	MyState& operator /= (const double val) {
		pos.tria = nullptr;	// impossible
		pos.pos /= val;
		return *this;
	}

	MyState operator * (const double val) const {
		MyState res;
		res.pos.pos = pos.pos * val;
		return res;
	}

};

struct MyControl {

	int numStepsSinceLastEval = 0;
	float headingChangeSinceLastEval = 0;

	void afterEval() {
		numStepsSinceLastEval = 0;
		headingChangeSinceLastEval = 0;
	}


};

struct MyObservation {



};

class MyPFInitUniform : public K::ParticleFilterInitializer<MyState> {

	const MyNavMesh* mesh;

public:

	MyPFInitUniform(const MyNavMesh* mesh) : mesh(mesh) {
		;
	}

	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {

		/** random position and heading within the mesh */
		Distribution::Uniform<float> dHead(0, 2*M_PI);
		MyNavMeshRandom rnd = mesh->getRandom();
		for (K::Particle<MyState>& p : particles) {
			p.state.pos = rnd.draw();
			p.state.heading = dHead.draw();
			p.weight = 1.0 / particles.size();
		}
	}

};

class MyPFInitFixed : public K::ParticleFilterInitializer<MyState> {

	const MyNavMesh* mesh;
	const Point3 pos;

public:

	MyPFInitFixed(const MyNavMesh* mesh, const Point3 pos) : mesh(mesh), pos(pos) {
		;
	}

	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {

		/** random position and heading within the mesh */
		Distribution::Uniform<float> dHead(0, 2*M_PI);
		for (K::Particle<MyState>& p : particles) {
			p.state.pos = mesh->getLocation(pos);
			p.state.heading = dHead.draw();
			p.weight = 1.0 / particles.size();
		}
	}

};


class MyPFTrans : public K::ParticleFilterTransition<MyState, MyControl> {

	using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
	MyNavMeshWalk walker;

public:

	MyPFTrans(MyNavMesh& mesh) : walker(mesh) {

		// how to evaluate drawn points
		//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
		//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
		walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9));		// 90% for particles moving towards the target

	}

	void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {

		Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
		Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
		Distribution::Normal<float> dHeading(0.0, 0.10);


		for (K::Particle<MyState>& p : particles) {

			// how to walk
			MyNavMeshWalkParams params;
			params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
			params.numSteps = control->numStepsSinceLastEval;
			params.start = p.state.pos;
			params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
			params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();

			// walk
			MyNavMeshWalk::ResultEntry res = walker.getOne(params);

			// assign back to particle's state
			p.weight *= res.probability;
			p.state.pos = res.location;
			p.state.heading = res.heading;

		}

		// reset the control (0 steps, 0 delta-heading)
		//control->afterEval();

	}


};

class MyPFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {

public:

	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {

		return 1.0;

	}

};


using MyFilter = K::ParticleFilter<MyState, MyControl, MyObservation>;


#endif