#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 "Plot.h"

TEST(TestAll, Nav) {


	Grid<20, GP> g;

	// dijkstra mapper
	class TMP {
		Grid<20, GP>& grid;
	public:
		TMP(Grid<20, 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 = ((Point3)n1 - (Point3)n2).length(2.5);
			//if (d > 20) {d*= 1.30;}
			return d / std::pow(n2.imp, 3);
		}
	} tmp(g);

	GridFactory<20, GP> gf(g);
	FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6);

	Floor f1 = fpf.getFloor("1");
	Floor f2 = fpf.getFloor("2");
	Stairs s1_2 = fpf.getStairs("1_2");

	gf.addFloor(f1, 20);
	gf.addFloor(f2, 340);
	gf.addStairs(s1_2, 20, 340);
	gf.removeIsolated();


	GridImportance gi;
	gi.addImportance(g, 20);
	gi.addImportance(g, 340);

	Dijkstra<GP> d;
	const GP& start = g.getNodeFor(GridPoint(500,200,20));
	//const GP& end = g.getNodeFor(GridPoint(1400,1400,20));
	const GP& end = g.getNodeFor(GridPoint(1200,200,340));
	d.build(start, end, tmp);

	// plot path
	K::GnuplotSplotElementLines path; path.setColorHex("#0000ff"); path.setLineWidth(2);
	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));
		dn = dn->previous;
	}

	Plot p;
	p.build(g);
	p.splot.add(&path);
	p.fire();

}

#endif