#ifndef PAPERVISIMPORTANCE_H
#define PAPERVISIMPORTANCE_H

#include <Indoor/grid/Grid.h>
#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/grid/factory/GridImportance.h>

#include <Indoor/floorplan/FloorplanFactorySVG.h>

#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>



#include <Indoor/nav/dijkstra/Dijkstra.h>
#include <Indoor/nav/dijkstra/DijkstraPath.h>

#include "PaperPlot.h"
#include "PaperPlot2D.h"

#include "../MyGridNode.h"
#include "../Settings.h"
#include "../DijkstraMapper.h"

PaperPlot2D::Size s1 = PaperPlot2D::Size(4,1.5);
PaperPlot2D::Size s2 = PaperPlot2D::Size(4,1.7);

class PaperVisImportance {

public:



	static float clamp(const float in, const float min, const float max) {
		if (in < min) {return min;}
		if (in > max) {return max;}
		return in;
	}

	// use node-importance as grid-color
	struct ColorizerImp {
		float get(const MyGridNode& n) const {return n.imp;}
		bool skip(const MyGridNode& n) const {(void) n; return false;}
	};

	// use node-distance as grid-color
	struct ColorizerDist {
		float get(const MyGridNode& n) const {return n.distToTarget;}
		bool skip(const MyGridNode& n) const {(void) n; return false;}
	};

	// use num-visited as grid-color
	struct ColorizeHeat {
		int maxCnt; int cutoff;
		ColorizeHeat(const int maxCnt, const int cutoff) : maxCnt(maxCnt), cutoff(cutoff) {;}
		float get(const MyGridNode& n) const {return (n.cnt > maxCnt) ? (maxCnt) : (n.cnt);}
		bool skip(const MyGridNode& n) const {return n.cnt < cutoff;}		// skip to reduce plot size
	};


	static void createImportance() {

		// load the floorplan
		FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
		Floor f0 = fpFac.getFloor("test1");
		const LengthF h0 = LengthF::cm(0);

		// add the floorplan to the grid
		Grid<MyGridNode> grid(20);
		GridFactory<MyGridNode> gridFac(grid);

		gridFac.addFloor(f0, h0.cm());

		// remove all isolated nodes not attached to 300,300,floor0
		gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(400,400,h0.cm()) ) );

		// stamp importance information onto the grid-nodes
		GridImportance gridImp;
		gridImp.addImportance(grid, h0.cm());


		{
			PaperPlot2D plot("floorplan_importance.eps", s1);
			plot.setRanges(0,5100, 0,2100);
			//plot.addFloor(f0);
			plot.addGrid(grid, ColorizerImp());
			plot.gp << "set colorbox\n";
			plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0.6\n";
			plot.show();
		}


	}




	static void createPath() {

		// load the floorplan
		FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
		Floor f0 = fpFac.getFloor("test1");
		const LengthF h0 = LengthF::cm(0);

		// add the floorplan to the grid
		Grid<MyGridNode> grid(20);
		GridFactory<MyGridNode> gridFac(grid);

		gridFac.addFloor(f0, h0.cm());

		// remove all isolated nodes not attached to 300,300,floor0
		gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );



		// start and end
		//const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0));
		//const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0));
		const MyGridNode& gnStart = grid.getNodeFor(GridPoint(300, 300, 0));
		const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(4700, 1300, 0));

		// build all shortest path to reach th target
		Dijkstra<MyGridNode> dijkstra;
		DijkstraMapper accImp(grid);
		DijkstraMapperNormal accNormal(grid);

		// path without importance
		dijkstra.build(&gnStart, accNormal);
		DijkstraPath<MyGridNode> pathNormal(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));

		// stamp importance information onto the grid-nodes
		GridImportance gridImp;
		gridImp.addImportance(grid, h0.cm());

		// path WITH importance
		dijkstra.build(&gnStart, accImp);
		DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));

		// build plot
		K::GnuplotPlotElementLines gpPath1a;		gpPath1a.setLineWidth(6);	gpPath1a.setColorHex("#000000");
		K::GnuplotPlotElementLines gpPath1b;		gpPath1b.setLineWidth(2);	gpPath1b.setColorHex("#ffffff");
		K::GnuplotPlotElementLines gpPath2a;		gpPath2a.setLineWidth(7);	gpPath2a.setColorHex("#000000");
		K::GnuplotPlotElementLines gpPath2b;		gpPath2b.setLineWidth(2);	gpPath2b.setColorHex("#ffffff");

		for (DijkstraNode<MyGridNode>* dn : pathNormal) {
			gpPath1a.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
			gpPath1b.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
		}
		for (DijkstraNode<MyGridNode>* dn : pathImp) {
			gpPath2a.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
			gpPath2b.add(K::GnuplotPoint2(dn->element->x_cm, dn->element->y_cm));
		}

		// plot the 2 paths
		{
			PaperPlot2D plot("floorplan_paths.eps", s1);
			plot.floors.setColorHex("#777777");
			plot.setRanges(0,5100, 0,2100);
			plot.addFloor(f0);
			plot.plot.add(&gpPath1a);		//gpPath1a.setCustomAttr("dashtype 2");
			plot.plot.add(&gpPath1b);		gpPath1b.setCustomAttr("dashtype 2");
			plot.plot.add(&gpPath2a);
			plot.plot.add(&gpPath2b);
			plot.show();
		}

		// stamp distance information onto the grid
		// attach a corresponding weight-information to each user-grid-node
		for (MyGridNode& node : grid) {
			const DijkstraNode<MyGridNode>* dn = dijkstra.getNode(node);
			node.distToTarget = dn->cumWeight;
		}

		// walk
		GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk (grid, accImp, gnStart);

		for (int i = 0; i < 30000; ++i) {

			if (i % 250 == 0) {std::cout << i << std::endl;}
			const MyGridNode& nStart = gnEnd;
			GridWalkState<MyGridNode> sStart(&nStart, Heading::rnd());
			GridWalkState<MyGridNode> sEnd = walk.getDestination(grid, sStart, 135, 0);
			(void) sEnd;

		}

		// plot the heat-map
		{
			PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s2);
			plot.setRanges(0,5220, 0,2100);
			plot.floors.setColorHex("#000000");
			//plot.gp << "set palette gray negative\n";
			plot.gp << "set palette defined ( 0 'white', 1 'blue', 2 'orange', 3 'yellow' )\n";
			plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0\n";
			plot.addFloor(f0);
			plot.addGrid(grid, ColorizeHeat(7000, 50));
			plot.plot.add(&gpPath1a);		//gpPath1a.setCustomAttr("dashtype 2");
			plot.plot.add(&gpPath1b);		gpPath1b.setCustomAttr("dashtype 2");
			plot.plot.add(&gpPath2a);
			plot.plot.add(&gpPath2b);
			plot.gp << "set object circle at 300,300 size 40,40 front\n";
			plot.gp << "set object circle at 4700,1300 size 40,40 front\n";
			plot.show();
		}





	}


	static void showDijkstraDistance() {

		// load the floorplan
		FloorplanFactorySVG fpFac(MiscSettings::floorplanPlot, 2.822222);
		Floor f0 = fpFac.getFloor("test1");
		const LengthF h0 = LengthF::cm(0);

		// add the floorplan to the grid
		Grid<MyGridNode> grid(20);
		GridFactory<MyGridNode> gridFac(grid);

		gridFac.addFloor(f0, h0.cm());

		// remove all isolated nodes not attached to 300,300,floor0
		gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor( GridPoint(300,300,h0.cm()) ) );



		// start and end
		const MyGridNode& gnStart = grid.getNodeFor(GridPoint(300, 300, 0));
		const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(4700, 1300, 0));

		// build all shortest path to reach th target
		Dijkstra<MyGridNode> dijkstra;

		// stamp importance information onto the grid-nodes
		GridImportance gridImp;
		gridImp.addImportance(grid, h0.cm());

		// path WITH importance
		DijkstraMapper accImp(grid);
		dijkstra.build(&gnStart, accImp);
		DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));

		// stamp distance information onto the grid
		// attach a corresponding weight-information to each user-grid-node
		for (MyGridNode& node : grid) {
			const DijkstraNode<MyGridNode>* dn = dijkstra.getNode(node);
			node.distToTarget = dn->cumWeight;
		}

		// plot the heat-map
		{
			PaperPlot2D plot;
			plot.setRanges(0,5220, 0,2100);
			plot.floors.setColorHex("#000000");
			//plot.gp << "set palette gray negative\n";
			plot.gp << "set palette defined ( 0 'white', 1 'blue', 2 'orange', 3 'yellow' )\n";
			plot.gp << "set lmargin 0\n set tmargin 0\n set bmargin 0\n set rmargin 0\n";
			plot.addFloor(f0);
			plot.addGrid(grid, ColorizerDist());
			plot.gp << "set object circle at 300,300 size 40,40 front\n";
			plot.gp << "set object circle at 4700,1300 size 40,40 front\n";
			plot.show();
			sleep(1000);
		}





	}



};

#endif // PAPERVISIMPORTANCE_H
