#ifndef NODERESAMPLING_H
#define NODERESAMPLING_H


#include <algorithm>
#include <random>

#include <Indoor/grid/Grid.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResampling.h>


	/**
	 * uses simple probability resampling by drawing particles according
	 * to their current weight.
	 * HOWEVER: after drawing them, do NOT use them directly, but replace them with a neighbor
	 * O(log(n)) per particle
	 */
	template <typename State, typename Node>
	class NodeResampling : public K::ParticleFilterResampling<State> {

	private:

		/** this is a copy of the particle-set to draw from it */
		std::vector<K::Particle<State>> particlesCopy;

		/** random number generator */
		std::minstd_rand gen;

		Grid<Node>& grid;

	public:

		/** ctor */
		NodeResampling(Grid<Node>& grid) : grid(grid) {
			gen.seed(1234);
		}

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

			// compile-time sanity checks
			// TODO: this solution requires EXPLICIT overloading which is bad...
			//static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );

			const uint32_t cnt = (uint32_t) particles.size();

			// equal weight for all particles. sums up to 1.0
			const double equalWeight = 1.0 / (double) cnt;

			// ensure the copy vector has the same size as the real particle vector
			particlesCopy.resize(cnt);

			// swap both vectors
			particlesCopy.swap(particles);

			// calculate cumulative weight
			double cumWeight = 0;
			for (uint32_t i = 0; i < cnt; ++i) {
				cumWeight += particlesCopy[i].weight;
				particlesCopy[i].weight = cumWeight;
			}

			std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
			std::uniform_int_distribution<int> distRndNode(0, grid.getNumNodes()-1);
			std::normal_distribution<float> distTurn(0.0, +0.03);

			// now draw from the copy vector and fill the original one
			// with the resampled particle-set
			for (uint32_t i = 0; i < cnt; ++i) {

				// slight chance to get a truely random node as particle
				// mainly for testing
				if (distNewOne(gen) < 0.005) {
					particles[i].state.position = grid[distRndNode(gen)];
					particles[i].weight = equalWeight;
					continue;
				}

				// normal redraw procedure
				particles[i] = draw(cumWeight);
				particles[i].weight = equalWeight;

				const Node* n = grid.getNodePtrFor(particles[i].state.position);
				if (n == nullptr) {continue;}	// should not happen!

				for (int j = 0; j < 2; ++j) {
					std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
					const int idx = distIdx(gen);
					n = &grid.getNeighbor(*n, idx);
				}


				particles[i].state.position = *n;
				particles[i].state.heading.direction += distTurn(gen);

			}

		}

	private:

		/** draw one particle according to its weight from the copy vector */
		const K::Particle<State>& draw(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<State>& s, const float d) {return s.weight < d;};
			auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
			return *it;

		}



	};



#endif // NODERESAMPLING_H