#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>
#include <Indoor/geo/Angle.h>
#include <Indoor/grid/walk/GridWalkState.h>

#include "eval/GroundTruthWay.h"


class Vis {

public:

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

	K::GnuplotSplotElementLines estPath;
	K::GnuplotSplotElementLines groundTruth;

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";

		groundTruth.setLineWidth(2);
		groundTruth.setColorHex("#666666");

		particles.setColorHex("#0000ff");
		particles.setPointSize(0.3);
		particleDir.setColorHex("#444444");

		estPath.setLineWidth(2);

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

	}

	/** 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) {

		std::set<uint64_t> used;

		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 uint64_t idx = n1.getIdx() * n2.getIdx();
				if (used.find(idx) == used.end()) {
					const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
					gridEdges.addSegment(p1, p2);
					used.insert(idx);
				}
			}
		}
		return *this;
	}

	void addGroundTruth(GroundTruthWay& gtw) {
		groundTruth.clear();
		for (auto it : gtw.getWay()) {
			K::GnuplotPoint3 gp(it.value.x, it.value.y, it.value.z);
			groundTruth.add(gp);
		}
	}

	void addEstPath(std::vector<Point3>& est) {
		estPath.clear();;
		for (const Point3& p : est) {
			K::GnuplotPoint3 gp(p.x, p.y, p.z);
			estPath.add(gp);
		}
	}

	void setTimestamp(uint64_t ts) {
		static uint64_t firstTs = ts;
		gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n";
	}

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

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

	void addObject(const int idx, const Point3& p) {
		gp << "set object " << idx << " polygon ";
		gp << "from " << p.x << "," << p.y << "," << p.z;
		gp << "  to " << p.x << "," << p.y << "," << p.z + 200;
		gp << "  to " << p.x << "," << p.y << "," << p.z;				// close
		gp << " lw 2 ";
		gp << "\n";
	}

	void setEstAndShould(const Point3& est, const Point3& should) {
		addObject(2,est);
		addObject(3,should);
	}

	template <typename T> void addState(const GridWalkState<T>& n) {
		Point2 dir = Angle::getPointer(n.heading.getRAD());
		K::GnuplotPoint3 p1(n.node->x_cm, n.node->y_cm, n.node->z_cm);
		K::GnuplotPoint3 p2 = p1 + K::GnuplotPoint3(dir.x, dir.y, 0) * 85;
		particles.add(p1);
		particleDir.addSegment(p1, p2);
	}


	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