#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 "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.loc = 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::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<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* _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.loc;

                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.loc = 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.75;

            switch (act) {

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

            case Activity::WALKING_UP:
            case Activity::WALKING_DOWN:
                if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED)		{return kappa;}
                if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED)    {return kappa;}
                if (p.state.loc.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.loc.pos, observation.currentTime, wifiObs);
                const double pStair = getStairProb(p, observation.activity);
                const double pGPS = 1;

                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_
