YASMIN/nav/mesh/FilterMesh.h

#ifndef FILTERMESH_
#define FILTERMESH_

#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/navMesh/NavMesh.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>

#include "State.h"
#include "../../Settings.h"

#include <omp.h>
#include <future>

namespace MeshBased {

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

	private:

		NM::NavMesh<NM::NavMeshTriangle>* mesh;

	public:

		PFInit(NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {

		}

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

			std::minstd_rand gen;
			std::uniform_real_distribution<float> distHead(0, 2*M_PI);

			NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();

			for (K::Particle<MyState>& p : particles) {
				p.state.loc = rnd.draw();
				p.state.heading = 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);

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

			//for (K::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));

				K::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 K::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 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 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) {

				K::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 // FILTERMESH_