#ifndef FILTER_H
#define FILTER_H

#include <KLib/math/filter/particles/ParticleFilter.h>

#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>

#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.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 "State.h"
#include "Node.h"
#include "NodeResampling.h"
#include "../Settings.h"

#include <omp.h>

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

private:

	Grid<MyGridNode>* grid;

public:

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

	}

	virtual void initialize(std::vector<K::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 (K::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 (K::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 K::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<K::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);
		double probSum = 0;




		// seems OK
//		float sum = 0;
//		for (int i = 0; i < 1000; ++i) {
//			float val = noise(gen);
//			sum += std::abs(val);
//		}
		//Log::add("123", "sum: " + std::to_string(sum));
		//Log::add("123", std::to_string(Timestamp::fromRunningTime().ms()) + ": " + std::to_string(ctrl.numStepsSinceLastTransition));

		//for (K::Particle<MyState>& p : particles) {
		#pragma omp parallel for num_threads(2)
		for (int i = 0; i < (int) particles.size(); ++i) {
			K::Particle<MyState>& p = particles[i];
			const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
			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
			if (p.weight != p.weight) {throw Exception("nan");}
			probSum += prob;
			//p.weight = Distribution::Exponential<double>::getProbability(5.0, prob);
		}

//		const double avgProb = probSum / particles.size();
//		const double threshold = avgProb * 0.15;
//		for (int i = 0; i < (int) particles.size(); ++i) {
//			K::Particle<MyState>& p = particles[i];
//			p.weight = (p.weight > threshold) ? (1.0) : (0.01);		// downvote all transitions below the threshold
//			//p.weight = 1;
//		}

	}

};

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

	Grid<MyGridNode>* grid;

	WiFiModelLogDistCeiling& wifiModel;


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

	// how to perform VAP grouping. also see calibration in Controller.cpp
	VAPGrouper vg = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);

	// 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 K::Particle<MyState>& p, const ActivityButterPressure::Activity act) {

		const float kappa = 0.75;

		const MyGridNode& 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;

	}

	double evaluation(std::vector<K::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 = vg.group(_observation.wifi);

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


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

			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
			sum += p.weight;

			if (p.weight != p.weight) {throw Exception("nan");}

		}

		return sum;

	}

};



#endif // FILTER_H