#include <Indoor/grid/factory/GridFactory.h>
#include <Indoor/floorplan/FloorplanFactorySVG.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/factory/GridImportance.h>

#include "Vis.h"

namespace Settings {
	const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
	const int gridSize_cm = 20;
}

struct MyNode : public GridNode, public GridPoint {
	float distToTarget = 1.0;
	float imp = 1.0;
public:
	MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
};


int align(const int val) {
	return val / Settings::gridSize_cm * Settings::gridSize_cm;
}

// dijkstra mapper
class DijkstraMapper {
	Grid<MyNode>& grid;
public:
	DijkstraMapper(Grid<MyNode>& grid) : grid(grid) {;}
	int getNumNeighbors(const MyNode& node) const {return node.getNumNeighbors();}
	const MyNode* getNeighbor(const MyNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
	float getWeightBetween(const MyNode& n1, const MyNode& n2) const {
		float d = ((Point3)n1 - (Point3)n2).length(2.0);
		//if (d > 20) {d*= 1.30;}
		return d / std::pow(n2.imp, 3);
	}
};

int main(void) {

	Grid<MyNode> grid(Settings::gridSize_cm);
	GridFactory<MyNode> gridFac(grid);


	FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222);

	Floor f0 = fpFac.getFloor("floor_0");
	Floor f1 = fpFac.getFloor("floor_1");
	Floor f2 = fpFac.getFloor("floor_2");
	Floor f3 = fpFac.getFloor("floor_3");

	Stairs s01 = fpFac.getStairs("staircase_0_1");
	Stairs s12 = fpFac.getStairs("staircase_1_2");
	Stairs s23 = fpFac.getStairs("staircase_2_3");

	const LengthF h0 = LengthF::cm(align(0));
	const LengthF h1 = LengthF::cm(align(360));
	const LengthF h2 = LengthF::cm(align(360+340));
	const LengthF h3 = LengthF::cm(align(360+340+340));

	gridFac.addFloor(f0, h0.cm());
	gridFac.addFloor(f1, h1.cm());
	gridFac.addFloor(f2, h2.cm());
	gridFac.addFloor(f3, h3.cm());

	gridFac.addStairs(s01, h0.cm(), h1.cm());
	gridFac.addStairs(s12, h1.cm(), h2.cm());
	gridFac.addStairs(s23, h2.cm(), h3.cm());

	// maybe the two sides are wrong?
	PlatformStair psUpperLeft;
	psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
	psUpperLeft.s1 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
	psUpperLeft.s2 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
	gridFac.buildPlatformStair(psUpperLeft, h0.cm(), h1.cm());
	gridFac.buildPlatformStair(psUpperLeft, h1.cm(), h2.cm());
	gridFac.buildPlatformStair(psUpperLeft, h2.cm(), h3.cm());
//	vis.gp << "set xrange [1100:1800]\n";
//	vis.gp << "set yrange [4500:5200]\n";

	PlatformStair psUpperRight;
	psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
	psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
	psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
	gridFac.buildPlatformStair(psUpperRight, h0.cm(), h1.cm());
	gridFac.buildPlatformStair(psUpperRight, h1.cm(), h2.cm());
	gridFac.buildPlatformStair(psUpperRight, h2.cm(), h3.cm());
//	vis.gp << "set xrange [6100:6900]\n";
//	vis.gp << "set yrange [4500:5200]\n";

	PlatformStair psLowerLeft;
	psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
	psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
	psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
	gridFac.buildPlatformStair(psLowerLeft, h0.cm(), h1.cm());
	gridFac.buildPlatformStair(psLowerLeft, h1.cm(), h2.cm());
	gridFac.buildPlatformStair(psLowerLeft, h2.cm(), h3.cm());
//	vis.gp << "set xrange [1300:2100]\n";
//	vis.gp << "set yrange [400:1400]\n";

	gridFac.removeIsolated( (MyNode&)grid.getNodeFor(GridPoint(300,300,h0.cm())) );

	GridImportance gridImp;
	gridImp.addImportance(grid, h0.cm());
	gridImp.addImportance(grid, h1.cm());
	gridImp.addImportance(grid, h2.cm());
	gridImp.addImportance(grid, h3.cm());

	MyNode& start = (MyNode&)grid.getNodeFor(GridPoint(500,300,h0.cm()));
	MyNode& end = (MyNode&)grid.getNodeFor(GridPoint(7000,5000,h3.cm()));

	Vis vis;
	vis.addFloor(f0, h0);
	vis.addFloor(f1, h1);
	vis.addFloor(f2, h2);
	vis.addFloor(f3, h3);




	//GridWalkRandomHeadingUpdate<MyNode> walk;
	//GridWalkRandomHeadingUpdateAdv<MyNode> walk;
	//GridWalkPushForward<MyNode> walk;
	GridWalkLightAtTheEndOfTheTunnel<MyNode> walk(grid, DijkstraMapper(grid), end);


	std::vector<GridWalkState<MyNode>> states;
	for (int i = 0; i < 2000; ++i) { states.push_back(GridWalkState<MyNode>(&start, Heading::rnd())); }

//	vis.addGrid(grid);
//	vis.show();
//	sleep(100);
//	vis.removeGrid();

	Distribution::Normal<float> wDist(0.3, 0.3);
	//std::minstd_rand gen(1234);
	//std::normal_distribution<float> dist(0.6, 0.3);

	while(true) {
		for (GridWalkState<MyNode>& state : states) {
			state = walk.getDestination(grid, state, std::abs(wDist.draw()) );
		}
		usleep(1000*80);
		vis.showStates(states);
		vis.show();
	}



	sleep(1000);

	return 0;

}