#ifndef MYINITIALIZER3_H
#define MYINITIALIZER3_H

#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include "MyState.h"

#include <Indoor/grid/Grid.h>

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

private:

	Grid<MyGridNode>& grid;

	int x_cm;
	int y_cm;
	int z_cm;

	int heading;



public:

	/** q0 = random */
	MyInitializer(Grid<MyGridNode>& grid) : grid(grid), heading(0) {

	}

	/** q0 = given */
	MyInitializer(Grid<MyGridNode>& grid, int x_cm, int y_cm, int z_cm, int heading) :
		grid(grid), x_cm(x_cm), y_cm(y_cm), z_cm(z_cm), heading(heading) {

	}

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

		std::minstd_rand gen;
		std::uniform_int_distribution<> dist(0, grid.getNumNodes());

		for (K::Particle<MyState>& p : particles) {

			MyGridNode& n = grid[dist(gen)];

			//p.state.pCur = Point3(x_cm, y_cm, z_cm);
			//GridPoint gp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
			//p.state.walkState.node = &grid.getNodeFor(gp);

			p.state.pCur = (Point3) n;
			p.state.walkState.node = &n;

			p.state.pOld = p.state.pCur;
			p.state.walkState.heading = Heading::rnd();
			p.state.hPa = 0;

		}

	}

};

#endif // MYINITIALIZER_H