#ifndef VIS_H
#define VIS_H

#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>

#include <Indoor/geo/Length.h>
#include <Indoor/floorplan/Floor.h>

class Vis {

public:

	K::Gnuplot gp;
	K::GnuplotSplot splot;
	K::GnuplotSplotElementLines floors;
	K::GnuplotSplotElementColorPoints gridNodes;
	K::GnuplotSplotElementLines gridEdges;
	K::GnuplotSplotElementPoints particles;

public:

	Vis() {

		gp << "set hidden3d front\n";
		//gp << "set view equal xy\n";
		gp << "set ticslevel 0\n";
		gp << "set cbrange[0.8:2.0]\n";

		gp << "unset xtics\n";
		gp << "unset ytics\n";
		gp << "unset ztics\n";
		gp << "unset border\n";

		// attach all layers
		splot.add(&floors);
		splot.add(&gridNodes);
		splot.add(&gridEdges);
		splot.add(&particles);

	}

	/** add all obstacles of the given floor to the provided height */
	Vis& addFloor(const Floor& f, const LengthF height) {

		// add each wall
		for (const Line2& l : f.getObstacles()) {
			const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
			const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
			floors.addSegment(p1, p2);
		}

		return *this;

	}

	/** add the grid to the plot */
	template <typename T> Vis& addGrid(Grid<T>& grid) {

		float max = 0;
		for (const T& n1 : grid) {
			if (n1.distToTarget > max) {max = n1.distToTarget;}
		}
		gp << "set cbrange[0.0:1.0]\n";
		//gp << "set cbrange[0.8:1.3]\n";

		for (const T& n1 : grid) {
			const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
			//const float color = n1.imp;
			//const float color = n1.distToTarget/max;
			const float color = 0;
			gridNodes.add(p1, color);
//			for (const T& n2 : grid.neighbors(n1)) {
//				const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
//				gridEdges.addSegment(p1, p2);
//			}
		}
		return *this;
	}

	void removeGrid() {
		gridNodes.clear();;
	}

	void clearStates() {
		particles.clear();
	}

	template <typename T> void addState(const GridWalkState<T>& n) {
		particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
	}

	template <typename T> Vis& showStates(std::vector<GridWalkState<T>>& states) {
		particles.clear();;
		for (const GridWalkState<T>& n : states) {
			particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
		}
		return *this;
	}

	/** show (plot) the current setup */
	void show() {
		gp.draw(splot);
		gp.flush();
	}


};

#endif // VIS_H