#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 "../gl/GLTriangles.h"
#include "../Renderable.h"

#include "../../../../Settings.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();

		// do not display?
		if (grid->getNumNodes() > Settings::MapView3D::maxColorPoints) {return;}

		float min = +INFINITY;
		float max = -INFINITY;

		for (const MyGridNode& n : *grid) {
			const float f = n.getWalkImportance();
			if (f < min) {min = f;}
			if (f > max) {max = f;}
		}

		max = 1.2;

		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.getWalkImportance();
			float h = 0.66 - ((f-min)/(max-min)) * 0.66;				// 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 = 8.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();

		// do not display?
		if (particles.size() > Settings::MapView3D::maxColorPoints) {return;}

		// group particles by grid-point
		std::unordered_map<GridPoint, float> weights;
		for (const K::Particle<T>& p : particles) {
			const GridPoint gp = p.state.position;
			if (weights.find(gp) != weights.end()) {continue;}
			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 = 8.0f;
		points.rebuild();

	}

//	void addPoint(const QVector3D pt, const QColor color) {

//		const float s = 5 / 100.0f;
//		const QVector3D col(color.redF(), color.greenF(), color.blueF());
//		const VertColor vc1(pt + QVector3D(-s, -s, 0), col);
//		const VertColor vc2(pt + QVector3D(+s, -s, 0), col);
//		const VertColor vc3(pt + QVector3D(+s, +s, 0), col);
//		const VertColor vc4(pt + QVector3D(-s, +s, 0), col);

//		points.addQuad(vc1, vc2, vc3, vc4);

//	}

	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(const RenderParams& params) override {

		(void) params;
		//glDisable(GL_DEPTH_TEST);

#ifndef ANDROID
		glPointSize(size);
#endif
		points.render(&program);
		//glEnable(GL_DEPTH_TEST);
	}


};


#endif // GL_PARTICLES_H
