#ifndef WALKVIZ3_H
#define WALKVIZ3_H


#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/synthetic/SyntheticPath.h>
#include <Indoor/grid/Grid.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/objects/GnuplotObjectPolygon.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>

struct WalkViz3 {

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

	K::GnuplotSplotElementPoints nodes;
	K::GnuplotSplotElementLines edges;

	K::GnuplotSplotElementColorPoints nodesCol;

	K::GnuplotSplotElementLines outline;
	K::GnuplotSplotElementLines obstacles;
	K::GnuplotSplotElementColorPoints particles;
	K::GnuplotSplotElementLines path;
	K::GnuplotSplotElementLines pathEstimated;
	K::GnuplotObjectPolygon oPos;

	WalkViz3() {
		plot.add(&obstacles);
		plot.add(&outline);

		plot.add(&edges); edges.getStroke().getColor().setHexStr("#ff0000");
		plot.add(&nodes); nodes.setPointType(7); nodes.setPointSize(0.4); nodes.getColor().setHexStr("#999999");

		plot.add(&nodesCol); nodesCol.setPointType(7); nodesCol.setPointSize(0.4);

		plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2);
		plot.add(&path); path.getStroke().setWidth(2); path.setShowPoints(true);
		plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00");

		plot.getObjects().add(&oPos);

		//gp << "set size ratio -1\n";
		//gp << "set view equal xy\n";
		//gp << "set view equal xyz\n";

		plot.getView().setEnabled(false);
	}

	void add(Floorplan::Floor* floor, 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::GnuplotPoint3 gp1(pt1.x, pt1.y, floor->atHeight);
			K::GnuplotPoint3 gp2(pt2.x, pt2.y, floor->atHeight);
			outline.addSegment(gp1, gp2);
		}
	}

	void addObstacle(Floorplan::Floor* floor, Floorplan::FloorObstacle* obs) {

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

		if (line) {
			K::GnuplotPoint3 gp1(line->from.x, line->from.y, floor->atHeight);
			K::GnuplotPoint3 gp2(line->to.x, line->to.y, floor->atHeight);
			obstacles.addSegment(gp1,gp2);
		}

	}

	void showPos(const Point3 pos) {
		oPos.clear();
		oPos.add( K::GnuplotCoordinate3(pos.x-1,pos.y-1,pos.z, K::GnuplotCoordinateSystem::FIRST) );
		oPos.add( K::GnuplotCoordinate3(pos.x+1,pos.y-1,pos.z, K::GnuplotCoordinateSystem::FIRST) );
		oPos.add( K::GnuplotCoordinate3(pos.x+0,pos.y+1,pos.z, K::GnuplotCoordinateSystem::FIRST) );
		oPos.close();
	}

	template <typename T> void showParticlesGrid(const std::vector<T>& particles) {
		this->particles.clear();
		double min = +999;
		double max = -999;
		for (const T& p : particles) {
			const K::GnuplotPoint3 p3(p.state.pos.x, p.state.pos.y, p.state.pos.z);
			const double prob = std::pow(p.weight, 0.25);
			this->particles.add(p3, prob);
			if (prob > max) {max = prob;}
			if (prob < min) {min = prob;}
		}
		plot.getAxisCB().setRange(min, max + 0.000001);
	}

	template <typename T> void showParticles(const std::vector<T>& particles) {
		this->particles.clear();
		double min = +999;
		double max = -999;
		for (const T& p : particles) {
			const K::GnuplotPoint3 p3(p.state.pos.x, p.state.pos.y, p.state.pos.z);
			const double prob = std::pow(p.weight, 0.25);
			this->particles.add(p3, prob);
			if (prob > max) {max = prob;}
			if (prob < min) {min = prob;}
		}
		plot.getAxisCB().setRange(min, max + 0.000001);
	}

	void addFloor(Floorplan::Floor* floor) {
		for (Floorplan::FloorObstacle* obs : floor->obstacles) {
			addObstacle(floor, obs);
		}
		for (Floorplan::Stair* s : floor->stairs) {
			addStair(floor, s);
		}
	}

	void addStair(Floorplan::Floor* floor, Floorplan::Stair* s) {

		std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(s->getParts(), floor);
		for (const Floorplan::Quad3& quad : quads) {
			K::GnuplotPoint3 gp1(quad.p1.x, quad.p1.y, quad.p1.z);
			K::GnuplotPoint3 gp2(quad.p2.x, quad.p2.y, quad.p2.z);
			K::GnuplotPoint3 gp3(quad.p3.x, quad.p3.y, quad.p3.z);
			K::GnuplotPoint3 gp4(quad.p4.x, quad.p4.y, quad.p4.z);
			obstacles.addSegment(gp1,gp2);
			obstacles.addSegment(gp2,gp3);
			obstacles.addSegment(gp3,gp4);
			obstacles.addSegment(gp4,gp1);
		}

	}

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

	void setGT(const Point3 pt) {
		gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.7) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n";
	}
	void setCurPos(const Point3 pt) {
		gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+1.7) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n";
	}


	template <typename Node> void addGrid(Grid<Node>& grid, bool addEdges = false) {
		nodes.clear();
		edges.clear();
		for (const Node& n : grid) {
			const K::GnuplotPoint3 gp(n.x_cm/100.0f, n.y_cm/100.0f, n.z_cm/100.0f);
			nodes.add(gp);
		}
		if (addEdges) {
			for (const Node& n : grid) {
				for (const Node& n2 : grid.neighbors(n)) {
					if (n.getIdx() < n2.getIdx()) {		// uni-directional.. prevent duplicates
						edges.addSegment(
							K::GnuplotPoint3 (n.x_cm, n.y_cm, n.z_cm) / 100.0f,
							K::GnuplotPoint3 (n2.x_cm, n2.y_cm, n2.z_cm) / 100.0f
						);
					}
				}
			}
		}
	}

	template <typename Node> void addGridWalkImp(Grid<Node>& grid, bool addEdges = false) {
		double pMin = +999;
		double pMax = -999;
		for (const Node& n : grid) {
			const K::GnuplotPoint3 gp(n.x_cm/100.0f, n.y_cm/100.0f, n.z_cm/100.0f);
			const double p = n.getWalkImportance();
			nodesCol.add(gp, p);
			if (p > pMax) {pMax = p;}
			if (p < pMin) {pMin = p;}
		}
		if (addEdges) {
			for (const Node& n : grid) {
				for (const Node& n2 : grid.neighbors(n)) {
					if (n.getIdx() < n2.getIdx()) {		// uni-directional.. prevent duplicates
						edges.addSegment(
							K::GnuplotPoint3 (n.x_cm, n.y_cm, n.z_cm) / 100.0f,
							K::GnuplotPoint3 (n2.x_cm, n2.y_cm, n2.z_cm) / 100.0f
						);
					}
				}
			}
		}
		plot.getAxisCB().setRange(pMin, pMax);
	}

	void showPath(const SyntheticPath& interpol) {
		for (const auto& entry : interpol.getEntries()) {
			const Point3 p = entry.value;
			K::GnuplotPoint3 gp(p.x, p.y, p.z);
			path.add(gp);
		}
	}

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


};



#endif // WALKVIZ3_H