Indoor/tests/grid/Plot.h

#ifndef PLOT_H
#define PLOT_H

#include "../../grid/Grid.h"
#include <set>

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

#include "../../floorplan/Floor.h"

class GP : public GridNode, public GridPoint {
public:
	float imp = 1.0f;
	float impPath = 1.0f;
	float distToTarget = 1.0;
	int cnt = 0;
public:
	GP() : GridNode(), GridPoint() {;}
	GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}
};

class Plot {

public:

	K::Gnuplot gp;
	K::GnuplotSplot splot;

	K::GnuplotSplotElementColorPoints nodes;
	K::GnuplotSplotElementLines lines;
	K::GnuplotSplotElementLines floors;
	K::GnuplotSplotElementPoints particles;

	/** ctor */
	Plot() {

		gp << "set ticslevel 0\n";
		gp << "set view equal xyz\n";
		gp << "set cbrange[0.5:1.5]\n";
		gp << "set palette gray negative\n";

		gp << "set xlabel 'x'\n";
		gp << "set ylabel 'y'\n";

		//gp << "set hidden3d front\n";

		splot.add(&nodes);		nodes.setPointSize(0.5);
		splot.add(&lines);		lines.getStroke().getColor().setHexStr("#999999");
		splot.add(&floors);
		splot.add(&particles);	particles.getColor().setHexStr("#0000ff"); particles.setPointSize(0.5);

		floors.getStroke().setWidth(2);
		floors.getStroke().getColor().setHexStr("#008800");

	}

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

	void addParticle(const Point3 p) {
		particles.add(K::GnuplotPoint3(p.x, p.y, p.z));
	}

	template <typename T> Plot& showGrid(Grid<T>& g) {
		addEdges(g);
		addNodes(g);
		return *this;
	}

	template <typename T> Plot& addEdges(Grid<T>& g) {

		// prevent adding edges twice
		std::set<std::string> done;

		for (T& n1 : g) {
			for (const T& n2 : g.neighbors(n1)) {
				const size_t uid1 = g.getUID(n1);
				const size_t uid2 = g.getUID(n2);
				//size_t edge = uid1+uid2;
				std::string edge = std::to_string(std::max(uid1,uid2)) + std::to_string(std::min(uid1,uid2));
				if (done.find(edge) == done.end()) {
					K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
					K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
					lines.addSegment(p1, p2);
					done.insert(edge);
				}
			}
		}

		return *this;

	}

	template <typename T> Plot& addNodes(Grid<T>& g) {

		for (T& n1 : g) {
			K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
			nodes.add(p1, 1.0);
		}

		return *this;

	}

	template <typename T> Plot& build(Grid<T>& g) {

		std::set<uint64_t> done;

		int cnt = 0;
		for (int i = 0; i < g.getNumNodes(); ++i) {
			const GP& n1 = g[i];
			//const float color = std::pow(n1.distToTarget,0.1);
			const float color = n1.imp * n1.impPath;
			nodes.add(K::GnuplotPoint3(n1.x_cm, n1.y_cm, n1.z_cm), color);

			for (const T& n2 : g.neighbors(n1)) {
			//for (int n = 0; n < n1.getNumNeighbors(); ++n) {
			//	const GP& n2 = n1.getNeighbor(n, g);
				if (done.find(g.getUID(n2)) == done.end()) {
					K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
					K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
					lines.addSegment(p1, p2);
					++cnt;
				}
			}

			done.insert(g.getUID(n1));

		}

		nodes.setPointSize(1.0);

		return *this;

	}

	/** remove all floors from the plot */
	Plot& resetFloors() {
		floors.clear();
		return *this;
	}

	/** add the given floor to the plot */
	Plot& addFloor(Floor& f, const float z_cm) {
		for (const Line2& l : f.getObstacles()) {
			K::GnuplotPoint3 p1(l.p1.x, l.p1.y, z_cm+10);
			K::GnuplotPoint3 p2(l.p2.x, l.p2.y, z_cm+10);
			floors.addSegment(p1, p2);
		}
		return *this;
	}

	Plot& fire() {
		gp.draw(splot);
		gp.flush();
		//sleep(1000);
		return *this;
	}

};


#endif // PLOT_H