#ifdef WITH_TESTS

#include "../Tests.h"

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

#include "Plot.h"

TEST(TestAll, Nav) {


	Grid<GP> g(20);

	// 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.5);
			//if (d > 20) {d*= 1.30;}
			return d / std::pow(n2.imp, 3);
		}
	} tmp(g);

	GridFactory<GP> gf(g);

	// 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);
	gf.addFloor(f2, 340);
	gf.addStairs(s1_2, 20, 340);
	gf.removeIsolated();

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

	// calculate dijkstra using aforementioned importance
	Dijkstra<GP> d;
	const GP& start = g.getNodeFor(GridPoint(500,200,20));
	const GP& end = g.getNodeFor(GridPoint(1200,200,340));
	//const GP& end = g.getNodeFor(GridPoint(1300,1300,20));
	d.build(&start, &end, tmp);

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

	// plot path
	K::GnuplotSplotElementLines path; path.getStroke().getColor().setHexStr("#0000ff"); path.getStroke().setWidth(2);
	const DijkstraNode<GP>* dn = d.getNode(end);
	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
//	GridWalkWeighted<GP> walk;
//	Log::add("test", "walking");
//	for (int i = 0; i < 5000; ++i) {
//		walk.getDestination(g, &start, 1.0, Headings::UP);
//	}
//	Log::add("test", "done");

	Plot p;
	p.build(g);
	//p.showGrid(g);
	p.addFloor(f1, 20);
	p.addFloor(f2, 340);
	p.splot.add(&path);
	p.fire();

}

#endif