#ifndef GL_PARTICLES_H
#define GL_PARTICLES_H


#include <Indoor/floorplan/v2/Floorplan.h>
#include <KLib/math/filter/particles/ParticleFilter.h>

#include "../gl/GLHelper.h"
#include "../gl/GLPoints.h"
#include "../Renderable.h"

#include "../../../nav/Node.h"

class ColorPoints : public Renderable {

private:

	GLPoints points;
	float size = 3.0f;

public:

	/** ctor */
	ColorPoints() {

	}

	/** NOTE: must be called from Qt's main thread! */
	void setFromGridImportance(Grid<MyGridNode>* grid) {

		points.clear();

		for (const MyGridNode& n : *grid) {
			const QVector3D pt(n.x_cm/100.0f, n.z_cm/100.0f + 0.1f, n.y_cm/100.0f);		// swap z and y
			const float f = n.getNavImportance();
			float h = 0.66 - (f*0.20);				// 0.66 is blue on the HSV-scale
			if (h < 0) {h = 0;}
			if (h > 1) {h = 1;}
			const QColor color = QColor::fromHsvF(h, 1, 1);
			points.addPoint(pt, color);
		}

		size = 3.0f;
		points.rebuild();

	}

	/** NOTE: must be called from Qt's main thread! */
	template <typename T> void setFromParticles(const std::vector<K::Particle<T>>& particles) {

		points.clear();

		// group particles by grid-point
		std::unordered_map<GridPoint, float> weights;
		for (const K::Particle<T>& p : particles) {
			const GridPoint gp = p.state.position;
			weights[gp] += p.weight;
		}

		// find min/max
		float min = +INFINITY;
		float max = -INFINITY;
		for (auto it : weights) {
			if (it.second > max) {max = it.second;}
			if (it.second < min) {min = it.second;}
		}

		// draw colored
		for (auto it : weights) {
			const GridPoint gp = it.first;
			const float w = it.second;
			const float p = (w-min) / (max-min);		// [0:1]
			const QVector3D pt(gp.x_cm/100.0f, gp.z_cm/100.0f + 0.1f, gp.y_cm/100.0f);		// swap z and y
			float h = 0.66 - (p*0.66);				// 0.66 is blue on the HSV-scale
			const QColor color = QColor::fromHsvF(h, 1, 1);
			points.addPoint(pt, color);
		}


//		for (const K::Particle<T>& p : particles) {
//			const GridPoint gp = p.state.position;
//			const QVector3D pt(gp.x_cm/100.0f, gp.z_cm/100.0f + 0.1f, gp.y_cm/100.0f);		// swap z and y
//			const QColor color = Qt::blue;
//			points.addPoint(pt, color);
//		}

		size = 6.0f;
		points.rebuild();

	}


	void initGL() override {
		loadShader(":/res/gl/vertex1.glsl", ":/res/gl/fragmentColorPoint.glsl");
		//program.setUniformValue("color", QVector4D(0.5, 0.5, 0.5, 1.0));
		points.initGL();
	}

	/** render the floor */
	void _render() override {
		//glDisable(GL_DEPTH_TEST);
		//glPointSize()
#ifndef ANDROID
		glPointSize(size);
#endif
		points.render(&program);
		//glEnable(GL_DEPTH_TEST);
	}


};


#endif // GL_PARTICLES_H