#ifndef NAV_MESH_FILTER_H
#define NAV_MESH_FILTER_H

#include "mesh.h"
#include "../Settings.h"
#include <omp.h>

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

#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/estimation/ParticleFilterEstimationBoxKDE.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>

#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
#include <Indoor/navMesh/walk/NavMeshWalkWifi.h>
#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
#include <Indoor/navMesh/NavMeshRandom.h>

#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/activity/ActivityDetector.h>

#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>

struct MyState {

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

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

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

    MyState(Point3 p) : pos(p, nullptr), heading(0){}

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

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

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

	float getX(){
		return pos.pos.x;
	}

	float getY() {
		return pos.pos.y;
	}

	float getZ() {
		return pos.pos.z;
	}


	float getBinValue(const int dim) const {
		switch (dim) {
			case 0: return this->pos.pos.x;
			case 1: return this->pos.pos.y;
			case 2: return this->pos.pos.z;
			case 3: return this->heading.getRAD();
		}
		throw "cant find this value within the bin";
	}

};

struct MyControl {

	int numStepsSinceLastEval = 0;
	float headingChangeSinceLastEval = 0;

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

	//wifi
	WiFiMeasurements wifi;

	//time
	Timestamp currentTime;

	//last estimation
	Point3 lastEstimate = Point3(26, 43, 7.5);

};

struct MyObservation {

	// pressure
	float sigmaPressure = 0.10f;
	float relativePressure = 0;

	//wifi
	WiFiMeasurements wifi;

	//time
	Timestamp currentTime;

	//activity
	Activity activity;

};

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

	const MyNavMesh* mesh;

public:

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

    virtual ~MyPFInitUniform(){}

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

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

};

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

	const MyNavMesh* mesh;
	const Point3 pos;

public:

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

    virtual ~MyPFInitFixed(){}

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

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

};


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

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

	MyNavMeshWalk walker;

	WiFiQualityAnalyzer analyzer;
	WiFiObserverFree wifiProbability;

	SMC::ParticleFilterEstimationBoxKDE<MyState> estimator;

public:

	//std::vector<double> listRadiusSub;

	MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) :
		walker(mesh),
		wifiProbability(Settings::WiFiModel::sigma, wifiModel){

		// 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<SMC::Particle<MyState>>& particles, const MyControl* control) override {

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

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

			// 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();

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

			// 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 SMC::ParticleFilterEvaluation<MyState, MyObservation> {

	WiFiModel& wifiModel;
	WiFiObserverFree wifiProbability;

	//TODO: add this to transition probability
    float getStairProb(const SMC::Particle<MyState>& p, const Activity act) {

		const float kappa = 0.85f;

		switch (act) {

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

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

public:

	//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
	//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
	MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}

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

		double sum = 0;
		const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);

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

			double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
            double pBluetooth =
            double pStair = static_cast<double>(getStairProb(p, observation.activity));

            double prob = pWifi * pStair;

			p.weight *= prob;

			#pragma omp atomic
			sum += prob;
		}

		return sum;

	}

};


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


#endif