#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(2,4);

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,2100, 0,5100);
			plot.addFloor(f0);
			plot.addGrid(grid, ColorizerImp());
			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));

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

		// path without importance
		dijkstra.build(gnStart, 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, gnStart, accImp);
		DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));

		// build plot
		K::GnuplotPlotElementLines gpPath1;		gpPath1.setLineWidth(2);	gpPath1.setColorHex("#444444");
		K::GnuplotPlotElementLines gpPath2;		gpPath2.setLineWidth(2);	gpPath2.setColorHex("#000000");

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

		// plot the 2 paths
		{
			PaperPlot2D plot("floorplan_paths.eps", s1);
			plot.setRanges(0,2100, 0,5100);
			plot.addFloor(f0);
			plot.plot.add(&gpPath1);		gpPath1.setCustomAttr("dashtype 3");
			plot.plot.add(&gpPath2);
			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);

		}

		// plot the heat-map
		{
			PaperPlot2D plot("floorplan_dijkstra_heatmap.eps", s1);
			plot.setRanges(0,2100, 0,5100);
			plot.gp << "set palette gray negative\n";
			plot.addFloor(f0);
			plot.addGrid(grid, ColorizeHeat(7000, 50));
			plot.show();
		}


	}



};

#endif // PAPERVISIMPORTANCE_H