#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/WiFiQualityAnalyzer.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/sensors/activity/ActivityDetector.h>

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

#include <Indoor/smc/smoothing/BackwardFilterTransition.h>
#include <Indoor/smc/smoothing/BackwardSimulation.h>
#include <Indoor/smc/sampling/CumulativeSampler.h>

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

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

	Grid<MyNode>& grid;

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

	virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
		for (SMC::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

		}
	}

};

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

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

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

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

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

        for (SMC::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
        }
    }

};

/** very simple transition model, just scatter normal distributed  */
struct PFTransSimple : public SMC::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<SMC::Particle<MyState>>& particles, const MyControl* control) override {

        //int noNewPositionCounter = 0;

        #pragma omp parallel for num_threads(6)
        for (int i = 0; i < particles.size(); ++i) {
            SMC::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 SMC::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);            // fürn arsch und geht net
        //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<SMC::Particle<MyState>>& particles, const MyControl* control) override {

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

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

            //this is just for the smoothing transition... quick and dirty
            p.state.headingChangeMeasured_rad = control->turnSinceLastTransition_rad;

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

		}
	}

};

/**
*   particle-filter transition
*   Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox
*/
struct PFTransKLDSampling : public SMC::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;

    /** upper bound epsilon of the kld distance - the particle size is not allowed to exceed epsilon*/
    double epsilon;

    /** the upper 1 - delta quantil of the normal distribution. something like 0.01 */
    double delta;

    /** the bins */
    Binning<MyState> bins;

    /** max particle size */
    uint32_t N_max;


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


        epsilon = 0.15;
        delta = 0.01;
        N_max = 5000;
        bins.setBinSizes({0.01, 0.01, 0.2, 0.3});
        bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)});
    }

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

        std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
        Distribution::Uniform<int> getParticle(0, particles.size()-1);

        //init stuff
        uint32_t n = 0;
        uint32_t k = 1;
        double N = 0;

        //clear the bins
        bins.clearUsed();

        //create new particle set
        std::vector<SMC::Particle<MyState>> particlesNew;

        do{

            //draw equally from the particle set
            int particleIdx = getParticle.draw();
            SMC::Particle<MyState>& p = particles[particleIdx];

            //sample new particles based on the transition step
            // 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;

            //if it falls into an empty bin then draw another particle
            //is bin free?
            if(bins.isFree(p.state)){
                k++;
                bins.markUsed(p.state);

                //calculate the new N
                double z_delta = Distribution::NormalCDF<double>::getProbit(1 - delta);
                double front = (k - 1) / (2 * epsilon);
                double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta );

                N = front * std::pow(back, 3.0);
            }
            ++n;

            //add particle to new particleset
            particlesNew.push_back(p);


        } while (n < N && n < N_max);


        //write new particleset
        particles.clear();
        particles.swap(particlesNew);
    }

};

struct BFTrans : public SMC::BackwardFilterTransition<MyState, MyControl>{


public:

    /**
     * ctor
     * @param choice the choice to use for randomly drawing nodes
     * @param fp the underlying floorplan
     */
    BFTrans()
    {
        //nothin
    }

    uint64_t ts = 0;
    uint64_t deltaMS = 0;

    /** set the current time in millisconds */
    void setCurrentTime(const uint64_t ts) {
        if (this->ts == 0) {
            this->ts = ts;
            deltaMS = 0;
        } else {
            deltaMS = this->ts - ts;
            this->ts = ts;
        }
    }

    std::vector<SMC::Particle<MyState>> transition(std::vector<SMC::Particle<MyState>> const& toBeSmoothedParticles_qt, std::vector<MyControl> const& controls_1T) override{
        Assert::doThrow( "Wrong transition function. Use the other one!");

        std::vector<SMC::Particle<MyState>> dummyReturn;
        return dummyReturn;
    }


    /**
     * smoothing transition starting at T with t, t-1,...0
     * @param particles_qt q_t (Forward Filter) p2
     * @param particles_qt1 q_t+1 (Smoothed Particles from Step before) p1
     */
    std::vector<std::vector<double>> transition(std::vector<SMC::Particle<MyState>>const& particles_qt,
                                                std::vector<SMC::Particle<MyState>>const& particles_qt1) override {


        // calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
        // this means, predict all possible states q_t+1 with all passible states q_t
        // e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
        // p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
        std::vector<std::vector<double>> predictionProbabilities;

        omp_set_dynamic(0);     // Explicitly disable dynamic teams
        omp_set_num_threads(7);
        #pragma omp parallel for shared(predictionProbabilities)
        for (int i = 0; i < particles_qt1.size(); ++i) {
            std::vector<double> innerVector;
            auto p1 = &particles_qt1[i];

            for(int j = 0; j < particles_qt.size(); ++j){

                auto p2 = &particles_qt[j];

                const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0;

                //TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental

                const double distProb = Distribution::Normal<double>::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);

                // TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement?
                double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD());
                double diffDeg = Angle::radToDeg(diffRad);
                double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad);
                double measurementDeg = Angle::radToDeg(measurementRad);
                const double headingProb = Distribution::Normal<double>::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);

                // does the angle between two particles positions is similiar to the measurement
                //double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y);

                //check how near we are to the measurement
                double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0;
                const double floorProb = Distribution::Normal<double>::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);

                //combine the probabilities
                double prob = distProb;// * floorProb * headingProb;
                innerVector.push_back(prob);

            }
            #pragma omp critical
            predictionProbabilities.push_back(innerVector);
        }

        return predictionProbabilities;

    }
};

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

    WiFiModel& wifiModel;
    WiFiObserverFree wiFiProbability;             // free-calculation
    //WiFiObserverGrid<MyNode> wiFiProbability;		// grid-calculation
    WiFiQualityAnalyzer wqa;

    BeaconModelLogDistCeiling& beaconModel;
    BeaconObserverFree beaconProbability;

    Grid<MyNode>& grid;

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

	}

    /** probability step-distance */
    //TODO: add number of recognized steps
    inline double getStepDistanceProb(const Point3 particle1, const Point3 particle2){
        double distance = particle1.getDistance(particle2);
        return Distribution::Normal<double>::getProbability(Settings::IMU::stepLength, Settings::IMU::stepSigma + 0.4, distance);
    }

    //TODO: combinied evaluation heading and distance

	/** 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(node, 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 SMC::Particle<MyState>& p, const Activity act) {

        const float kappa = 0.65;

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

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

        case Activity::WALKING_UP:
        case Activity::WALKING_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<SMC::Particle<MyState>>& particles, const MyObs& observation) override {

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

        wqa.add(wifiObs);
        float quality = wqa.getQuality();

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

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

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

            //small checks
            Assert::isNotNaN(pWifi, "Wifi prob is nan");
            Assert::isNot0(pBaroPressure,"pBaroPressure is null");

            const bool volatile init = observation.currentTime.sec() < 25;
            //double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));
            //double pWiFiMod = (init) ? (std::pow(pWifi, 0.5)) : (std::pow(pWifi, 0.9));

            // bad wifi? -> we have no idea where we are!
            if (quality < 0.25 && !init) {
                //pWifi = 1;
                //p.weight = std::pow(p.weight, 0.5);
            }

            const double prob = pWifi;// * pStairProb;

            p.weight = prob;

            #pragma omp atomic
            sum += (prob);

		}

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

		return sum;

	}

};

#endif // FLOGIC_H