#ifdef WITH_TESTS

#include "../Tests.h"

#include "Plot.h"
#include "../../grid/factory/GridImportance.h"
#include "../../grid/factory/GridFactory.h"
#include "../../floorplan/FloorplanFactorySVG.h"
#include "../../nav/dijkstra/Dijkstra.h"

#include "../../grid/walk/GridWalkState.h"
#include "../../grid/walk/GridWalkWeighted.h"
#include "../../grid/walk/GridWalkLightAtTheEndOfTheTunnel.h"


//TEST(Walk, plot) {
TEST(Walk, DISABLED_plot) {

	Grid<GP> g(20);
	GridFactory<GP> gf(g);

	bool use3D = true;

	// load floorplan
	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);
	Floor f1 = fpf.getFloor("1");
	Floor f2 = fpf.getFloor("2");
	Stairs s1_2 = fpf.getStairs("1_2");



	// build the grid
	gf.addFloor(f1, 20);
	if (use3D) {
		gf.addFloor(f2, 340);
		gf.addStairs(s1_2, 20, 340);
	}



	//TODO: write as something simpler? e.g. as box with rotation??
	PlatformStair pStair;
	pStair.platform = BBox2(Point2(200, 200), Point2(400,500));
	pStair.s1 = Stair(Line2( 600,200, 600,350 ), Point2(-200,0));
	pStair.s2 = Stair(Line2( 600,400, 600,500 ), Point2(-250,0));
	//gf.buildPlatformStair(pStair, 20, 340);

	gf.removeIsolated();

	// calculate node importance based on the floorplan (walls, ...)
	GridImportance gi;
	gi.addImportance(g, 20);
	gi.addImportance(g, 340);

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



//	// calculate shortest path
//	Dijkstra<GP> d;
	const GP* start = &g.getNodeFor(GridPoint(700,200,20));
	const GP* end;
	if (use3D) {
		end = &g.getNodeFor(GridPoint(1200,200,340));
	} else {
		end = &g.getNodeFor(GridPoint(1300,1300,20));
	}
//	d.build(end, start, tmp);

//	gi.addDistanceToTarget(g, d);

//	// add the path's importance to the grid
//	gi.addImportance(g, d.getNode(start), d.getNode(end));

//	// plot path
//	K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
//	DijkstraNode<GP>* dn = d.getNode(start);
//	while (dn->previous != nullptr) {
//		path.add(K::GnuplotPoint3(dn->element->x_cm, dn->element->y_cm, dn->element->z_cm+50));
//		dn = dn->previous;
//	}



	// walk
	GridWalkLightAtTheEndOfTheTunnel<GP> walk(g, tmp, *end);

	std::minstd_rand gen;
	std::normal_distribution<float> dWalk(0.3, 0.3);
	std::normal_distribution<float> dTurn(0.3, 0.3);

	K::GnuplotSplotElementPoints pStates; pStates.getColor().setHexStr("#880000");

	// setup starting states
	std::vector<GridWalkState<GP>> states;
	for (int i = 0; i < 1000; ++i) {
		states.push_back( GridWalkState<GP>( start, Headings::UP ) );
	}

	Plot p;

	// show the grid
	//p.build(g);

	p.addFloor(f1, 20);
	if (use3D) {
		p.addFloor(f2, 340);
	}
	//p.splot.add(&path);
	p.splot.add(&pStates);
	p.nodes.setPointSize(0.2);

	if (!use3D) {
		p.gp << "set view 0,0\n";
	}
	p.gp << "set view equal xy\n";

	// walk random distances
	for (int i = 0; i < 5000; ++i) {
		pStates.clear();
		for (GridWalkState<GP>& state : states) {
			state = walk.getDestination(g, state, std::abs(dWalk(gen)), dTurn(gen), Activity::UNKNOWN);
			pStates.add(K::GnuplotPoint3(state.node->x_cm, state.node->y_cm, state.node->z_cm+10));
		}
		p.gp.draw(p.splot);
		p.gp.flush();
		usleep(1000*80);
	}


}

#endif