#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/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/WalkModuleButterActivity.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>

#include "State.h"
#include "Node.h"

#include "../Settings.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
		}

//		// fix position + heading
//		for (K::Particle<MyState>& p : particles) {
//			const int idx = 9000;
//			const MyGridNode& node = (*grid)[idx];
//			p.state.position = node;
//			p.state.heading.direction = Heading(0);
//		}

	}

};

class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {

public:

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

	WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
	WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
	WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
	WalkModuleButterActivity<MyGridNode, MyState> modBarometer;
	WalkModuleFollowDestination<MyGridNode, MyState> modDestination;

	std::minstd_rand gen;

public:

	PFTrans(Grid<MyGridNode>* grid, MyControl* ctrl) : grid(grid), modHeading(ctrl, Settings::turnSigma), modDestination(*grid) {

		walker.addModule(&modFavorZ);
		walker.addModule(&modHeading);
		walker.addModule(&modImportance);
		walker.addModule(&modBarometer);
		walker.addModule(&modDestination);

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

	}



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

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

		for (K::Particle<MyState>& p : particles) {
			const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::stepLength + noise(gen));
			p.state = walker.getDestination(*grid, p.state, dist_m);
		}

		((MyControl*)control)->resetAfterTransition();

	}

};

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

	WiFiModelLogDistCeiling& wifiModel;
	WiFiObserverFree wiFiProbability;

public:

	PFEval(WiFiModelLogDistCeiling& wifiModel) : wifiModel(wifiModel), wiFiProbability(Settings::wifiSigma, wifiModel) {

	}

	double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {

		double sum = 0;

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

		// local copy!! observation might be changed async outside!! (will really produces crashes!)
		const MyObservation observation = _observation;

		for (K::Particle<MyState>& p : particles) {
			const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, observation.wifi);
			const double pGPS = 1;
			const double prob = pWiFi * pGPS;
			p.weight = prob;
			sum += prob;
		}

		return sum;

	}

};



#endif // FILTER_H