#ifndef REGIONALRESAMPLING_H
#define REGIONALRESAMPLING_H

#include <Indoor/smc/filtering/resampling/ParticleFilterResampling.h>
#include "State.h"

namespace GridBased {

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

	public:

		float maxDist = 12.5;

		RegionalResampling() {;}

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

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

            std::vector<SMC::Particle<MyState>> next;
            for (const SMC::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<SMC::Particle<MyState>> copy = particles;
			double cumWeight = 0;
            for ( SMC::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 SMC::Particle<MyState>& draw(std::vector<SMC::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 SMC::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