#ifdef WITH_TESTS

#include "../Tests.h"
#include "../../grid/Grid.h"
#include "../../grid/GridPoint.h"
#include "../../grid/GridNode.h"

class GP : public GridNode, public GridPoint {
public:
	GP() : GridNode(), GridPoint() {;}
	GP(int x, int y, int z) : GridNode(), GridPoint(x,y,z) {;}

};

TEST(Grid, add) {

	Grid<20, GP> grid;
	ASSERT_EQ(0, grid.add(GP()));
	ASSERT_EQ(1, grid.add(GP()));
	ASSERT_EQ(2, grid.add(GP()));
	ASSERT_EQ(3, grid.add(GP()));

	// not aligned? -> error
	ASSERT_THROW(grid.add(GP(10,10,10)), std::exception);
	ASSERT_THROW(grid.add(GP(10,20,20)), std::exception);
	ASSERT_THROW(grid.add(GP(20,10,20)), std::exception);
	ASSERT_THROW(grid.add(GP(20,20,10)), std::exception);
	ASSERT_EQ(4, grid.add(GP(20,20,20)));

	// access
	ASSERT_EQ(GP(20,20,20), grid[4]);

}

TEST(Grid, BBox) {

	Grid<20, GP> grid;
	int idx = grid.add(GP(40,40,40));
	ASSERT_EQ(30, grid.getBBox(idx).x1_cm);
	ASSERT_EQ(50, grid.getBBox(idx).x2_cm);
	ASSERT_EQ(30, grid.getBBox(idx).y1_cm);
	ASSERT_EQ(50, grid.getBBox(idx).y2_cm);

}


TEST(Grid, neighbors) {

	Grid<1, GP> grid;

	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.connect(idx1, idx2);
	grid.connect(idx1, idx3);
	grid.connect(idx1, idx4);
	grid.connect(idx1, idx5);

	ASSERT_EQ(4, grid.getNumNeighbors(idx1));
	ASSERT_EQ(1, grid.getNumNeighbors(idx2));
	ASSERT_EQ(1, grid.getNumNeighbors(idx3));
	ASSERT_EQ(1, grid.getNumNeighbors(idx4));
	ASSERT_EQ(1, grid.getNumNeighbors(idx5));

}

TEST(Grid, uid) {

	Grid<20, GP> grid;

	GP gp(20,40,60);
	uint64_t uid = grid.getUID(gp);

	const int mask = (1 << 20) - 1;	// 20-bit mask
	ASSERT_EQ(3, uid >> 40 & mask);	// x
	ASSERT_EQ(2, uid >> 20 & mask);	// y
	ASSERT_EQ(1, uid >> 0 & mask);	// z

}

TEST(Grid, nearest) {

	Grid<20, GP> grid;

	GP c1(20,20,20);
	GP c2(40,40,40);

	grid.add(c1);
	grid.add(c2);

	ASSERT_EQ(c1, grid.getNodeFor(GP(15,20,20)));
	ASSERT_EQ(c1, grid.getNodeFor(GP(20,15,20)));
	ASSERT_EQ(c1, grid.getNodeFor(GP(20,20,15)));

	ASSERT_EQ(c1, grid.getNodeFor(GP(25,20,20)));
	ASSERT_EQ(c1, grid.getNodeFor(GP(20,25,20)));
	ASSERT_EQ(c1, grid.getNodeFor(GP(20,20,25)));

	ASSERT_EQ(c2, grid.getNodeFor(GP(35,40,40)));
	ASSERT_EQ(c2, grid.getNodeFor(GP(40,35,40)));
	ASSERT_EQ(c2, grid.getNodeFor(GP(40,40,35)));

	ASSERT_EQ(c2, grid.getNodeFor(GP(45,40,40)));
	ASSERT_EQ(c2, grid.getNodeFor(GP(40,45,40)));
	ASSERT_EQ(c2, grid.getNodeFor(GP(40,40,45)));

}

#endif