#ifndef PROBVIZ_H
#define PROBVIZ_H

#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/math/Interpolator.h>

#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementHeatMap.h>

#include <Indoor/math/distribution/Normal.h>
#include <vector>

class ProbViz {


	K::Gnuplot gp;
	K::GnuplotSplot plot;

	K::GnuplotSplotElementHeatMap heat;

public:

	ProbViz() {
		plot.add(&heat);
	}

	template <typename T> void show(std::vector<T>& particles) {

		heat.clear();

		float x1 = -0;
		float x2 = +20;
		float y1 = -0;
		float y2 = +20;
		float s = 0.5;

		plot.getAxisX().setRange(x1,x2);
		plot.getAxisY().setRange(y1,y2);

		Distribution::Normal<double> nd(0.0, 1.0);

		for (float y = y1; y < y2; y+=s) {
			for (float x = x1; x < x2; x+=s) {

				const Point2 p1(x,y);
				double prob = 1;

				for (const T& p : particles) {
					const Point2 p2 = p.pos.xy();
					const float dist = p1.getDistance(p2);
					prob += nd.getProbability(dist);
				}
				prob /= particles.size();

				K::GnuplotPoint3 p3(x,y,prob);
				heat.add(p3);

			}
		}

		gp.draw(plot);
		gp.flush();

	}

};

#endif // PROBVIZ_H