#ifndef FLOGIC_H
#define FLOGIC_H

#include <Indoor/grid/Grid.h>

#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/GridWalkerMulti.h>

#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingVonMises.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>

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

#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>

#include <Indoor/math/distribution/Uniform.h>

#include <KLib/math/filter/particles/ParticleFilterMixing.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>

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


#include  <random>
#include  <iterator>

template <typename RandomGenerator = std::default_random_engine>
struct random_selector
{
    //On most platforms, you probably want to use std::random_device("/dev/urandom")()
    random_selector(RandomGenerator g = RandomGenerator(std::random_device()()))
        : gen(g) {}

    template <typename Iter>
    Iter select(Iter start, Iter end) {
        std::uniform_int_distribution<> dis(0, std::distance(start, end) - 1);
        std::advance(start, dis(gen));
        return start;
    }

    //convenience function
    template <typename Iter>
    Iter operator()(Iter start, Iter end) {
        return select(start, end);
    }

    //convenience function that works on anything with a sensible begin() and end(), and returns with a ref to the value type
    template <typename Container>
    auto operator()(const Container& c) -> decltype(*begin(c))& {
        return *select(begin(c), end(c));
    }

private:
    RandomGenerator gen;
};


/** particle-filter init randomly distributed within the building*/
struct PFInit : public K::ParticleFilterInitializer<MyState> {

	Grid<MyNode>& grid;
    int mode;

    PFInit(Grid<MyNode>& grid, int mode) : grid(grid), mode(mode) {;}

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

			int idx = rand() % grid.getNumNodes();
			p.state.position = grid[idx];									// random position
			p.state.heading.direction = (rand() % 360) / 180.0 * M_PI;		// random heading
			p.state.heading.error = 0;
            p.state.relativePressure = 0;                                   // start with a relative pressure of 0
            p.weight = 1.0 / particles.size();                              // equal weight

            //for debugging
            p.state.curMode = mode;

		}
	}

};

/** particle-filter init with fixed position*/
struct PFInitFixed : public K::ParticleFilterInitializer<MyState> {

    Grid<MyNode>& grid;
    GridPoint startPos;
    float headingDeg;
    int mode;

    PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg, int mode) :
        grid(grid), startPos(startPos), headingDeg(headingDeg), mode(mode) {;}

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

        Distribution::Normal<float> norm(0.0f, 1.5f);

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

            GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f);

            GridPoint startPos = grid.getNodeFor(pos);
            p.state.position = startPos;								// scatter arround the start position
            p.state.heading.direction = headingDeg / 180.0 * M_PI;		// fixed heading
            p.state.heading.error = 0;
            p.state.relativePressure = 0;                               // start with a relative pressure of 0
            p.weight = 1.0 / particles.size();						// equal weight

            //for debugging
            p.state.curMode = mode;
        }
    }

};

/** very simple transition model, just scatter normal distributed  */
struct PFTransSimple : public K::ParticleFilterTransition<MyState, MyControl>{

    Grid<MyNode>& grid;

    // define the noise
    Distribution::Normal<float> noise_cm = Distribution::Normal<float>(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
    Distribution::Normal<float> height_m = Distribution::Normal<float>(0.0, 6.0);

    // draw randomly from a vector
    random_selector<> rand;

    // draw from 0 - 1
    Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);

    /** ctor */
    PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}

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

        int noNewPositionCounter = 0;

        #pragma omp parallel for num_threads(6)
        for (int i = 0; i < Settings::numParticles; ++i) {
            K::Particle<MyState>& p = particles[i];

            // update the baromter
            float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
            p.state.relativePressure += deltaZ_cm * 0.105f;

            double diffHeight = p.state.position.inMeter().z + height_m.draw();
            double newHeight_cm = p.state.position.z_cm;
            if(diffHeight > 9.1){
                newHeight_cm = 10.8 * 100.0;
            } else if (diffHeight < 9.1 && diffHeight > 5.7){
                newHeight_cm = 7.4 * 100.0;
            } else if (diffHeight < 5.7 && diffHeight > 2.0) {
                newHeight_cm = 4.0 * 100.0;
            } else {
                newHeight_cm = 0.0;
            }

            GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), 0.0);
            GridPoint newPosition = p.state.position + noisePt;
            newPosition.z_cm = newHeight_cm;

//            p.state.position = grid.getNearestNode(newPosition);

            if(grid.hasNodeFor(newPosition)){
                p.state.position = newPosition;
            }else{
                //no new position!
                //#pragma omp atomic
                //noNewPositionCounter++;
            }

        }

        //std::cout << noNewPositionCounter << std::endl;
    }
};

/** particle-filter transition */
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {

	Grid<MyNode>& grid;

	GridWalker<MyNode, MyState> walker;

	WalkModuleHeading<MyNode, MyState> modHeadUgly;							// stupid
	WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
    WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
	WalkModuleNodeImportance<MyNode, MyState> modImportance;
	WalkModuleSpread<MyNode, MyState> modSpread;
	WalkModuleFavorZ<MyNode, MyState> modFavorZ;
    //WalkModulePreventVisited<MyNode, MyState> modPreventVisited;

    //WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;

    std::minstd_rand gen;


    PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {

        walker.addModule(&modHead);
        //walker.addModule(&modHeadMises);
        //walker.addModule(&modSpread);				// might help in some situations! keep in mind!
        //walker.addModule(&modActivity);
        //walker.addModule(&modHeadUgly);
        //walker.addModule(&modImportance);
        //walker.addModule(&modFavorZ);
        //walker.addModule(&modButterAct);
        //walker.addModule(&modWifi);
        //walker.addModule(&modPreventVisited);
	}

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

        std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);

        #pragma omp parallel for num_threads(6)
        for (int i = 0; i < Settings::numParticles; ++i) {
            K::Particle<MyState>& p = particles[i];

            // save old position
            p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);

            // update steps
            const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));

			// update the particle in-place
            p.state = walker.getDestination(grid, p.state, dist_m);

            // update the baromter
            float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
            p.state.relativePressure += deltaZ_cm * 0.105f;

		}
	}

};


struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {

    WiFiModelLogDistCeiling& wifiModel;
    WiFiObserverFree wiFiProbability;             // free-calculation
    //WiFiObserverGrid<MyNode> wiFiProbability;		// grid-calculation

    BeaconModelLogDistCeiling& beaconModel;
    BeaconObserverFree beaconProbability;

    Grid<MyNode>& grid;

    PFEval(WiFiModelLogDistCeiling& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid<MyNode>& grid) :
        wifiModel(wifiModel),
        beaconModel(beaconModel),
        grid(grid),
        wiFiProbability(Settings::WiFiModel::sigma, wifiModel),
        beaconProbability(Settings::BeaconModel::sigma, beaconModel){

	}

	/** probability for WIFI */
    inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {

       //const MyNode& node = grid.getNodeFor(point);
       return wiFiProbability.getProbability(point.inMeter() + Point3(0,0,1.3), observation.currentTime, vapWifi);
    }

    /** probability for BEACONS */
    inline double getBEACON(const MyObs& observation, const GridPoint& point){

        //consider adding the persons height
        Point3 p = point.inMeter() + Point3(0,0,1.3);
        return beaconProbability.getProbability(p, observation.currentTime, observation.beacons);
    }

    /** probability for Barometer */
    inline double getBaroPressure(const MyObs& observation, const float hPa) const{
        return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
    }

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

        const float kappa = 0.75;

        const MyNode& gn = grid.getNodeFor(p.state.position);
        switch (act) {

        case ActivityButterPressure::Activity::STAY:
            if (gn.getType() == GridNode::TYPE_FLOOR)		{return kappa;}
            if (gn.getType() == GridNode::TYPE_DOOR)		{return kappa;}
                                                            {return 1-kappa;}

        case ActivityButterPressure::Activity::UP:
        case ActivityButterPressure::Activity::DOWN:
            if (gn.getType() == GridNode::TYPE_STAIR)		{return kappa;}
            if (gn.getType() == GridNode::TYPE_ELEVATOR)	{return kappa;}
                                                            {return 1-kappa;}

        }

        return 1.0;

    }

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

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

        #pragma omp parallel for num_threads(6)
        for (int i = 0; i < Settings::numParticles; ++i) {
            K::Particle<MyState>& p = particles[i];

            // Point3 pos_m = p.state.position.inMeter();
            // Point3 posOld_m = p.state.positionOld.inMeter();

            const double pWifi = getWIFI(observation, wifiObs, p.state.position);
            //const double pBaroPressure = getStairProb(p, observation.activity);
            const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
            //const double pBeacon = getBEACON(observation, p.state.position);

            //small checks
            _assertNotNAN(pWifi, "Wifi prob is nan");
            //_assertNot0(pBaroPressure,"pBaroPressure is null");

            const double prob = pWifi * pBaroPressure;

            p.weight = prob;

            #pragma omp atomic
            sum += (prob);

		}

        if(sum == 0.0){
            return 1.0;
        }

		return sum;

	}

};

#endif // FLOGIC_H