#ifndef FILTER_H
#define FILTER_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/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/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>

#include "../Observation.h"
#include "State.h"
#include "Node.h"
#include "NodeResampling.h"
#include "../Settings.h"

#include <../misc/fixc11.h>
#include <omp.h>
#include <future>

namespace GridBased {

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

    private:

        Grid<MyGridNode>* grid;

    public:

        PFInit(Grid<MyGridNode>* grid) : grid(grid) {

        }

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

            std::minstd_rand gen;
            std::uniform_int_distribution<int> distIdx(0, grid->getNumNodes()-1);
            std::uniform_real_distribution<float> distHead(0, 2*M_PI);


            for (SMC::Particle<MyState>& p : particles) {
                const int idx = distIdx(gen);
                const MyGridNode& node = (*grid)[idx];
                p.state.position = node;								// random position
                p.state.heading.direction = 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:

        /** local, static control-data COPY */
        MyControl ctrl;

        Grid<MyGridNode>* grid;
        GridWalker<MyGridNode, MyState> walker;

        WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
        WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
        WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
        WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
        WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;

        NodeResampling<MyState, MyGridNode> resampler;

        std::minstd_rand gen;

    public:

        PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {

            //walker.addModule(&modFavorZ);
            walker.addModule(&modHeading);
            //walker.addModule(&modImportance);
            walker.addModule(&modActivity);


            if (Settings::destination != GridPoint(0,0,0)) {
                //walker.addModule(&modDestination);
                modDestination.setDestination(grid->getNodeFor(Settings::destination));
            }

        }



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

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

            // sanity check
            Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");

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

                //#pragma omp atomic
                const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));

                SMC::Particle<MyState>& p = particles[i];

                double prob;
                p.state = walker.getDestination(*grid, p.state, dist_m, prob);
                //p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
                //p.weight = (prob > 0.01) ? (1.0) : (0.15);
                //p.weight = prob;
                //p.weight = 1.0;							// reset
                //p.weight = std::pow(p.weight, 0.1);		// make all particles a little more equal [less strict]
                //p.weight *= std::pow(prob, 0.1);		// add grid-walk-probability
                p.weight = prob;							// grid-walk-probability
                if (p.weight != p.weight) {throw Exception("nan");}

            }

        }

    };

    class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {

        Grid<MyGridNode>* grid;

        WiFiModelLogDistCeiling& wifiModel;


        //WiFiObserverFree wiFiProbability;					// free-calculation
        WiFiObserverGrid<MyGridNode> wiFiProbability;		// grid-calculation

        // smartphone is 1.3 meter above ground
        const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);

    public:

        PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
            grid(grid), wifiModel(wifiModel),
            //wiFiProbability(Settings::WiFiModel::sigma, wifiModel) {		// WiFi free
            wiFiProbability(Settings::WiFiModel::sigma) {					// WiFi grid


        }

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

            const float kappa = 0.75;

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

            case Activity::STANDING:
            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;

        }

        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 int numAP1 = observation.wifi.entries.size();
            const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
            const int numAP2 = wifiObs.entries.size();

            Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));

            // sanity check
            Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");

            #pragma omp parallel for num_threads(3)
            for (int i = 0; i < Settings::numParticles; ++i) {

                SMC::Particle<MyState>& p = particles[i];

                // WiFi free
                //const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));

                // WiFi grid
                const MyGridNode& node = grid->getNodeFor(p.state.position);
                const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);


                //Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));

                const double pStair = getStairProb(p, observation.activity);
                const double pGPS = 1;
                const double prob = pWiFi * pGPS * pStair;

                p.weight *= prob;		// NOTE: keeps the weight returned by the transition step!
                //p.weight = prob;		// does NOT keep the weights returned by the transition step
                if (p.weight != p.weight) {throw Exception("nan");}

                #pragma omp atomic
                sum += p.weight;

            }

            return sum;

        }

    };

}



#endif // FILTER_H