#pragma once

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

#include <Indoor/geo/Heading.h>
#include <Indoor/math/distribution/Uniform.h>
#include <Indoor/math/distribution/Normal.h>
//#include <Indoor/math/distribution/Region.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/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/model/LogDistanceModel.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 "FtmKalman.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 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;
	}

    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
    std::map<MACAddress, WiFiMeasurement> wifi;

    //time
    Timestamp currentTime;

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

};

struct MyObservation {

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

    //wifi
    std::unordered_map<MACAddress, WiFiMeasurement> wifi;

    //time
    Timestamp currentTime;

    //activity
    Activity activity;

};

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

	const MyNavMesh* mesh;

public:

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

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

		/** random position and heading within the mesh */
		Distribution::Uniform<float> dHead(0, 2*M_PI);
		MyNavMeshRandom rnd = mesh->getRandom();
        for (SMC::Particle<MyState>& p : particles) {
			p.state.pos = rnd.draw();
			p.state.heading = 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 void initialize(std::vector<SMC::Particle<MyState>>& particles) override {

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

};

class MyPFTransStatic : public SMC::ParticleFilterTransition<MyState, MyControl>{
    void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
        // nop
    }
};

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::NavMeshWalkKLD<MyNavMeshTriangle>;
    using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<MyNavMeshTriangle>;

	MyNavMeshWalk walker;

    const double lambda =  0.03;

public:

    //std::vector<double> listRadiusSub;

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

        // walking and heading random
        Distribution::Normal<float> dStepSizeFloor(0.60, 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
			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.1 || params.stepSizes.stepSizeStair_m < 0.1){
                params.stepSizes.stepSizeFloor_m  = 0.1;
                params.stepSizes.stepSizeStair_m  = 0.1;
            }

            double deltaUnblockable = 0.01;

			// walk
            MyNavMeshWalk::ResultEntry res = walker.getOne(params);
            //MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);

			// 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> {

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

        const float kappa = 0.75;

        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:

	// FRANK
    MyPFEval() { };

    bool assignProps = false;

    std::shared_ptr<std::unordered_map<MACAddress, Kalman>> kalmanMap;

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

        double sum = 0;

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

            double pFtm = 1.0;

            if (observation.wifi.size() == 0)
            {
                printf("");
            }

            for (auto& wifi : observation.wifi) {

                if (   (true && wifi.second.getAP().getMAC() == Settings::NUC1)
                    || (true && wifi.second.getAP().getMAC() == Settings::NUC2) 
                    || (true && wifi.second.getAP().getMAC() == Settings::NUC3)
                    || (true && wifi.second.getAP().getMAC() == Settings::NUC4)
                    )
                {
                    float rssi_pathloss = Settings::data.CurrentPath.NUCs.at(wifi.second.getAP().getMAC()).rssi_pathloss;

                    float rssiDist = LogDistanceModel::rssiToDistance(-40, rssi_pathloss, wifi.second.getRSSI());
                    float ftmDist = wifi.second.getFtmDist();

                    Point3 apPos = Settings::data.CurrentPath.NUCs.find(wifi.first)->second.position;
                    Point3 particlePos = p.state.pos.pos;
                    particlePos.z = 1.3; // smartphone höhe
                    float apDist = particlePos.getDistance(apPos);

                    if (Settings::UseKalman)
                    {
                        auto kalman = kalmanMap->at(wifi.second.getAP().getMAC());
                        pFtm *= Distribution::Normal<float>::getProbability(ftmDist, std::sqrt(kalman.P(0,0)), apDist);
                    }
                    else
                    {
                        pFtm *= Distribution::Normal<float>::getProbability(apDist, 3.5, ftmDist);
                        //pFtm *= Distribution::Region<float>::getProbability(apDist, 3.5/2, ftmDist);
                    }
                                    }
            }

            
            double prob = pFtm;

            if (assignProps)
                p.weight = prob; // p.weight *= prob
            else          
                p.weight *= prob;

            #pragma omp atomic
            sum += prob;
        }

        return sum;

	}

};


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