#ifndef FILTERMESH_
#define FILTERMESH_

#include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilter.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>

#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>

#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>

#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>

#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>

#include "State.h"
#include "../Observation.h"
#include "../../Settings.h"

#include <omp.h>
#include <future>

namespace MeshBased {

	class PFInit : public SMC::ParticleFilterInitializer<MyState> {

	private:

		const NM::NavMesh<NM::NavMeshTriangle>* mesh;

	public:

		PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
			;
		}

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

			std::minstd_rand gen;
			std::uniform_real_distribution<float> distHead(0, 2*M_PI);

			NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();

			for (SMC::Particle<MyState>& p : particles) {
				p.state.pos = rnd.draw();
				p.state.heading = Heading(distHead(gen));		// random heading
				p.weight = 1.0 / particles.size();				// equal weight
			}

	//		// fix position + heading
	//		for (SMC::Particle<MyState>& p : particles) {
	////			const int idx = 9000;
	////			const MyGridNode& node = (*grid)[idx];
	//			const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0));		// center of the testmap
	//			p.state.position = node;
	//			p.state.heading.direction = Heading(0);
	//		}

		}

	};


	class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {

	public:

		//using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
		using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>;
		//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
		//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
		MyNavMeshWalk walker;

		// local, static control-data COPY
		MyControl ctrl;

	public:

		PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){

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

		}

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

			// local copy!! observation might be changed async outside!! (will really produces crashes!)
			this->ctrl = *_ctrl;
			((MyControl*)_ctrl)->resetAfterTransition();

			// walking and heading random
			Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
			Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
			Distribution::Normal<float> dHeading(0.0, 0.1);

			#pragma omp parallel for num_threads(3)
			for (int i = 0; i < particles.size(); ++i) {
				SMC::Particle<MyState>& p = particles[i];

				// how to walk
				NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
				params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
				params.numSteps = ctrl.numStepsSinceLastTransition;
				params.start = p.state.pos;

				params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
				params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();

				if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
					params.stepSizes.stepSizeFloor_m  = 0.1;
					params.stepSizes.stepSizeStair_m  = 0.1;
				}

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

		}

	};


	class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {

		WiFiModel& wifiModel;
		WiFiObserverFree wifiProbability;

		double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {

			const float kappa = 0.9;

			switch (act) {

			case Activity::WALKING:
				if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR)		{return kappa;}
				if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR)             {return kappa;}
				if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED)    {return kappa;}
																							{return 1-kappa;}

			case Activity::WALKING_UP:
			case Activity::WALKING_DOWN:
				if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED)		{return kappa;}
				if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED)    {return kappa;}
				if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR)         {return kappa;}
																							{return 1-kappa;}
			}

			return 1.0;
		}

	public:

		//TODO: Was ist hier besser? Im Museum hatten wir das unterste.
		PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel){}
		//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
		//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}

		double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {

			double sum = 0;

			// local copy!! observation might be changed async outside!! (will really produces crashes!)
			const MyObservation observation = _observation;

			// vap-grouping
			const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);

			// sanity check
			//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");

			// assign weights
			#pragma omp parallel for num_threads(3)
			for (size_t i = 0; i < particles.size(); ++i) {
				SMC::Particle<MyState>& p = particles[i];

				const double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
				const double pStair = getStairProb(p, observation.activity);
				const double pGPS = 1;

				//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.

				const double prob = pWifi * pStair * pGPS;

				p.weight *= prob;
				if (p.weight != p.weight) {throw Exception("nan");}

				#pragma omp atomic
				sum += p.weight;
			}

			return sum;

		}

	};

}

#endif // FILTERMESH_