#ifndef WALKVIZ_H
#define WALKVIZ_H

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

#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementPoints.h>

struct WalkViz {

	K::Gnuplot gp;
	K::GnuplotPlot plot;

	K::GnuplotPlotElementPoints nodes;
	K::GnuplotPlotElementLines outline;
	K::GnuplotPlotElementLines obstacles;
	K::GnuplotPlotElementPoints particles;
	K::GnuplotPlotElementLines path;

	WalkViz() {
		plot.add(&obstacles);
		plot.add(&outline);
		plot.add(&nodes); nodes.setPointType(7); nodes.setPointSize(1); nodes.getColor().setHexStr("#999999");
		plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.1);
		plot.add(&path); path.getStroke().setWidth(2);
		gp << "set size ratio -1\n";
	}

	void add(Floorplan::FloorOutlinePolygon* poly) {
		Floorplan::Polygon2 pol2 = poly->poly;
		const int num = pol2.points.size();
		for (int i = 0; i < num; ++i) {
			const Point2 pt1 = pol2.points[(i+0)];
			const Point2 pt2 = pol2.points[(i+1)%num];
			K::GnuplotPoint2 gp1(pt1.x*100, pt1.y*100);
			K::GnuplotPoint2 gp2(pt2.x*100, pt2.y*100);
			outline.addSegment(gp1, gp2);
		}
	}

	void addObstacle(Floorplan::FloorObstacle* obs) {

		Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
		Floorplan::FloorObstacleCircle* circle = dynamic_cast<Floorplan::FloorObstacleCircle*>(obs);

		if (line) {
			K::GnuplotPoint2 gp1(line->from.x*100, line->from.y*100);
			K::GnuplotPoint2 gp2(line->to.x*100, line->to.y*100);
			obstacles.addSegment(gp1,gp2);
		}

	}

	void addFloor(Floorplan::Floor* floor) {
		for (Floorplan::FloorObstacle* obs : floor->obstacles) {
			addObstacle(obs);
		}
	}

	void addMap(Floorplan::IndoorMap* map) {
		for (Floorplan::Floor* floor : map->floors) {
			addFloor(floor);
		}
	}

	template <typename Node> void addGrid(Grid<Node>& grid) {
		for (const Node& n : grid) {
			K::GnuplotPoint2 gp(n.x_cm, n.y_cm);
			nodes.add(gp);
		}
	}

	void showPath(const Interpolator<float, Point3>& interpol) {
		for (const auto& entry : interpol.getEntries()) {
			const Point3 p = entry.value;
			K::GnuplotPoint2 gp(p.x * 100 , p.y*100);
			path.add(gp);
		}
	}

	void draw() {
		gp.draw(plot);
		gp.flush();
	}


};


#endif // WALKVIZ_H