#ifndef REGIONALRESAMPLING_H
#define REGIONALRESAMPLING_H

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

class RegionalResampling : public K::ParticleFilterResampling<MyState> {

public:

	float maxDist = 12.5;

	RegionalResampling() {;}

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

		Point3 sum;
		for (const K::Particle<MyState>& p : particles) {
			sum += p.state.position.inMeter();
		}
		const Point3 avg = sum / particles.size();

		std::vector<K::Particle<MyState>> next;
		for (const K::Particle<MyState>& p : particles) {
			const float dist = p.state.position.inMeter().getDistance(avg);
			if (rand() % 6 != 0) {continue;}
			if (dist < maxDist) {next.push_back(p);}
		}

		// cumulate
		std::vector<K::Particle<MyState>> copy = particles;
		double cumWeight = 0;
		for ( K::Particle<MyState>& p : copy) {
			cumWeight += p.weight;
			p.weight = cumWeight;
		}

		// draw missing particles
		const int missing = particles.size() - next.size();
		for (int i = 0; i < missing; ++i) {
			next.push_back(draw(copy, cumWeight));
		}

		std::swap(next, particles);

	}

	std::minstd_rand gen;

	/** draw one particle according to its weight from the copy vector */
	const K::Particle<MyState>& draw(std::vector<K::Particle<MyState>>& copy, const double cumWeight) {

		// generate random values between [0:cumWeight]
		std::uniform_real_distribution<float> dist(0, cumWeight);

		// draw a random value between [0:cumWeight]
		const float rand = dist(gen);

		// search comparator (cumWeight is ordered -> use binary search)
		auto comp = [] (const K::Particle<MyState>& s, const float d) {return s.weight < d;};
		auto it = std::lower_bound(copy.begin(), copy.end(), rand, comp);
		return *it;

	}

};

#endif // REGIONALRESAMPLING_H
