#ifndef COLORPOINTS2D_H
#define COLORPOINTS2D_H

#include <Indoor/floorplan/v2/Floorplan.h>
#include "Renderable2D.h"
#include <Indoor/geo/Point3.h>

#include <Indoor/grid/Grid.h>

#include <Indoor/smc/Particle.h>
#include "../nav/grid/Node.h"
#include "../nav/Observation.h"

/**
 * debug color points
 */
class ColorPoints2D : public Renderable2D {

private:

	struct PT {
		Point3 pos;
		QColor color;
		PT(const Point3 pos, const QColor color) : pos(pos), color(color) {;}
	};

	std::vector<PT> points;

public:

	/** ctor */
	ColorPoints2D() {

	}

	void showGridImportance(Grid<MyGridNode>* grid) {

		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 Point3 pt(n.x_cm/100.0f, n.y_cm/100.0f + 0.1f, n.z_cm/100.0f);
			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.push_back(PT(pt, color));
		}

	}

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

		points.clear();

		// group particles by grid-point
		std::unordered_map<GridPoint, float> weights;
        for (const SMC::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 Point3 pt(gp.x_cm/100.0f, gp.y_cm/100.0f + 0.1f, gp.z_cm/100.0f);
			float h = 0.66 - (p*0.66);				// 0.66 is blue on the HSV-scale
			const QColor color = QColor::fromHsvF(h, 1, 1);
			points.push_back(PT(pt, color));
		}

	}

protected:

	void doRender(QPainter& qp, const Scaler2D& s, const RenderParams2D& r) override {

		QPen pen;
		pen.setWidth(4);

		for (const PT& pt : points) {

			if (pt.pos.z < r.clip.belowHeight_m) {continue;}
			if (pt.pos.z > r.clip.aboveHeight_m) {continue;}

			const Point2 p2 = s.mapToScreen(pt.pos.xy());

			pen.setColor(pt.color);
			qp.setPen(pen);
			qp.drawPoint(p2.x, p2.y);

		}

	}


};

#endif // COLORPOINTS2D_H