#ifdef WITH_TESTS

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

#include "../../../grid/Grid.h"
#include "../../../nav/a-star/AStar.h"
#include "../../grid/Plot.h"

TEST(AStar, 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);}
		const std::vector<GP>& getAllNodes() const {return grid.getNodes();}
		decltype(grid.neighbors(GP())) getNeighbors(const GP& node) const {return grid.neighbors(node);}
		float getWeightBetween(const GP& n1, const GP& n2) const {return (n1.inCentimeter() - n2.inCentimeter()).length();}
		float getHeuristic(const GP& n1, const GP& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);}
	} tmp(grid);

	AStar<GP> nav;
//	std::vector<const GP*> vec = nav.get(grid[0], grid[4], tmp);

//	for (const GP* g : vec) {
//		std::cout << g << std::endl;
//	}


}



#endif