#ifdef WITH_TESTS

#include "../../Tests.h"

#include "../../../grid/Grid.h"
#include "../../../nav/dijkstra/Dijkstra.h"
#include "../../grid/Plot.h"

TEST(Dijkstra, build) {

	Grid<GP> grid(1);

	int idx1 = grid.add(GP( 0, 0, 0));
	int idx2 = grid.add(GP( 0, 1, 0));
	int idx3 = grid.add(GP( 0,-1, 0));
	int idx4 = grid.add(GP( 1, 0, 0));
	int idx5 = grid.add(GP(-1, 0, 0));

	grid.connectBiDir(idx1, idx2);
	grid.connectBiDir(idx1, idx3);
	grid.connectBiDir(idx1, idx4);
	grid.connectBiDir(idx1, idx5);

	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 {return ((Point3)n1 - (Point3)n2).length();}
	} tmp(grid);

	Dijkstra<GP> d;
	d.build(grid[idx5], grid[idx3], tmp);

	// start node must be "idx5"
	DijkstraNode<GP>* n = d.getNode(grid[idx5]);
	ASSERT_EQ(&grid[idx5], n->element);	ASSERT_EQ(nullptr, n->previous); ASSERT_EQ(0, n->cumWeight);

	// "idx1" (the center) is reached via idx5
	DijkstraNode<GP>* n2 = d.getNode(grid[idx1]);
	ASSERT_EQ(&grid[idx1], n2->element);	ASSERT_EQ(&grid[idx5], n2->previous->element);

	// "idx3" (the target) is reached via idx1 (the center)
	DijkstraNode<GP>* n3 = d.getNode(grid[idx3]);
	ASSERT_EQ(&grid[idx3], n3->element);	ASSERT_EQ(&grid[idx1], n3->previous->element);


}

#endif