/*
 * pointkdtree.cpp
 *
 *  Created on: May 22, 2014
 *      Author: gregor
 */

#include <limits>
#include <vector>

#include <gtest/gtest.h>

#include <glm/glm.hpp>

#include <GLTB/refptr.h>
#include <GLTB/rng.h>
#include <GLTB/geom/aabb.h>
#include <GLTB/geom/pointkdtree.h>

template<typename vectorType>
vectorType generateRandomPoint(vectorType min, vectorType max, gltb::Random &rng)
{
	vectorType randomPoint;
	for(size_t j = 0; j < randomPoint.length(); j++) {
		randomPoint[j] = min[j] + rng.genrandReal1() * (max[j] - min[j]);
	}
	return randomPoint;
}

template<typename vectorType>
std::vector<vectorType> generatePointCloud(vectorType min, vectorType max, size_t size, size_t seed)
{
	std::vector<vectorType> pointCloud;
	pointCloud.reserve(size);

	gltb::Random rng;
	rng.init(seed);

	for(size_t i = 0; i < size; i++) {
		vectorType randomPoint = generateRandomPoint(min, max, rng);

		pointCloud.push_back(randomPoint);
	}

	return pointCloud;
}

std::vector<glm::vec3> getRegressionPointCloud()
{
	std::vector<glm::vec3> regressionPointCloud = {
		glm::vec3(18.38578033, -8.344664574, -15.83588505  ), // minimal bad subset
		glm::vec3(17.63107872, -11.74000168, -16.49318314  ),
		glm::vec3(31.69170761, 6.453565598, 0.9569984674   ),
		glm::vec3(22.7297287, 21.92994118, -6.922272205    ),
		glm::vec3(22.68516541, 21.87231064, -6.905409813   ),
		glm::vec3(22.64706612, 21.89398384, -6.844979286   ),
		glm::vec3(22.63414001, 21.83197594, -6.827338219   ),
		glm::vec3(22.60970688, 21.85610199, -6.763553619   ),
		glm::vec3(22.55711555, 21.80702972, -6.779047012   ),
		glm::vec3(22.59361649, 21.84462738, -6.738883972   ),
		glm::vec3(22.58619881, 21.79201889, -6.749990463   ),
		glm::vec3(22.54808807, 21.81546783, -6.676200867   ),
		glm::vec3(22.5122776, 21.74060631, -6.650472641    ),
		glm::vec3(22.5279808, 21.78425407, -6.609106064    ),
		glm::vec3(-22.15300179, 12.55769253, -2.653868198  ),
		glm::vec3(-18.6264019, 21.83626175, -6.593967438   ),
		glm::vec3(-22.05000496, 21.04808807, -14.84287834  ),
		glm::vec3(-15.53989697, 20.48262978, -24.44785881  ),
		glm::vec3(-22.86063766, 7.89083147, -10.91486931   ),
		glm::vec3(-23.3200016, 8.153411865, -10.85140991   ),
		glm::vec3(-5.297859192, 24.45112228, -11.6481781   ),
		glm::vec3(10.23478127, 12.99800682, -27.34999847   ),
		glm::vec3(12.8154068, 25.09553146, -13.84242725    ),
		glm::vec3(25.26787949, -3.021432877, 0.900001049   ),
		glm::vec3(25.8363018, -10.67462063, -6.388171196   ),
		glm::vec3(16.07927704, 17.20549011, -0.03749865294 ),
		glm::vec3(7.510120392, 15.527915, -28.24974823     ),
		glm::vec3(7.370000362, 15.51401615, -27.9516449    ),
		glm::vec3(11.15295696, 12.78959942, -26.03301811   ),
		glm::vec3(11.52888298, 12.94948673, -27.25121117   ),
		glm::vec3(11.14412594, 12.29574203, -26.92747116   ),
		glm::vec3(11.25496674, 13.35984039, -27.13185501   ),
		glm::vec3(11.28000259, 13.44843674, -27.05823898   ),
		glm::vec3(2.417857647, 24.92568398, -12.66446018   ),
		glm::vec3(18.38578033, -8.344664574, -15.83588505  ),
		glm::vec3(40.02767181, -9.928408623, -21.88554192  ),
		glm::vec3(22.91165924, -6.619273663, -26.61206436  ),
		glm::vec3(14.99468136, -11.55579948, 12.38000107   ),
		glm::vec3(11.6999979, -9.694328308, 9.621116638    ),
		glm::vec3(17.75688553, -2.828799486, 2.499999285   ),
		glm::vec3(17.96217346, -2.539999962, 2.680579424   ),
		glm::vec3(21.29892731, -9.462649345, 12.38000202   ),
		glm::vec3(20.27580452, -11.73999977, 10.57239628   ),
		glm::vec3(17.87080383, -2.539999962, 7.966829777   ),
		glm::vec3(14.19841766, -11.73999977, 1.971774101   ),
		glm::vec3(14.95048332, -3.915930271, 1.446022511   ),
		glm::vec3(12.78989029, -11.74000072, -9.849265099  ),
		glm::vec3(12.78805828, -11.72936344, -9.862934113  ),
		glm::vec3(49.27405167, 22.4728775, -7.79232502     ),
		glm::vec3(46.34999466, 22.0879612, -8.694431305    ),
		glm::vec3(55.3500061, 21.46413422, -5.986223221    ),
		glm::vec3(54.6462326, 21.66368484, -5.674738884    ),
		glm::vec3(50.74647141, 4.868478298, -8.34867382    ),
		glm::vec3(51.06338501, 3.370008945, -8.443130493   ),
		glm::vec3(57.4645462, 16.19517899, -2.467965603    ),
		glm::vec3(64.56685638, 19.54151917, 1.047251344    ),
		glm::vec3(64.62295532, 19.58805847, 1.064882994    ),
		glm::vec3(64.62259674, 19.58426666, 1.067184806    ),
		glm::vec3(64.62686157, 19.58891106, 1.063055515    ),
		glm::vec3(66.6499939, 18.90224838, -0.9479496479   ),
		glm::vec3(49.48072433, 0.9700080156, -25.12588882  ),
		glm::vec3(45.99999237, 7.064331055, -33.55169296   ),
		glm::vec3(49.52087402, 7.890001774, -32.45668411   ),
		glm::vec3(48.61169434, 6.278262138, -28.86000061   ),
		glm::vec3(47.31914139, -1.910001874, -38.77470016  ),
		glm::vec3(52.08469391, 2.411520958, -39.38000107   ),
		glm::vec3(56.21793365, 5.282331944, -37.41402435   ),
		glm::vec3(18.38578033, -8.344664574, -15.83588505  ),
		glm::vec3(17.37005997, -11.74000168, -13.99993515  ),
		glm::vec3(38.93935776, 23.11469841, -8.77694416    ),
		glm::vec3(53.21497345, 2.410007954, -20.99378395   ),
		glm::vec3(38.37310028, 24.29595566, -11.30552864   ),
		glm::vec3(33.8180275, 21.06549072, -18.77635956    ),
		glm::vec3(28.99860954, 16.53063965, -27.48033142   ),
		glm::vec3(3.581580639, 5.073527336, 1.25           ),
		glm::vec3(1.634493589, 11, 0.325879395             ),
		glm::vec3(1.149999976, 9.698101997, 0.3353615999   ),
		glm::vec3(21.97685051, 23.5679245, -10.21457291    ),
		glm::vec3(21.61866188, 23.47776794, -9.567873955   ),
		glm::vec3(22.58000183, 22.08619499, -11.19774532   ),
		glm::vec3(20.1974411, 25.12233925, -13.08371544    ),
		glm::vec3(-22.05000496, -2.044675112, -26.13888931 ),
		glm::vec3(-21.77615738, -2.20000267, -27.40245247  ),
		glm::vec3(-22.05000305, -1.817121029, -27.54447556 ),
		glm::vec3(-14.9399662, 6.896911144, -28.93207741   ),
		glm::vec3(-21.13986778, -0.1623981595, -29.29464531),
		glm::vec3(-18.63527679, 1.871761799, -39.38000107  ),
		glm::vec3(-14.09618568, 6.173213959, -28.47637367  ),
		glm::vec3(-16.38034821, -1.910001993, -32.74248123 ), // minimal bad subset
		glm::vec3(-16.08251572, 6.7317729, -39.81214523    ), // minimal bad subset
		glm::vec3(18.38578033, -8.344664574, -15.83588505  ), // minimal bad subset
		glm::vec3(11.65162659, -11.74000168, -16.6216774   ),
		glm::vec3(-22.41000366, 6.911560059, -22.37236214  ),
		glm::vec3(-11.1721096, 13.93139744, -25.59954834   ),
		glm::vec3(13.77903938, 9.759469986, 0.9499993324   ),
		glm::vec3(32.75, -2.389415741, -27.64692688        ),
		glm::vec3(23.09811211, -0.5893048048, 0.0191592779 ),
		glm::vec3(23.16884995, -0.5653026104, 0.1070288941 ),
		glm::vec3(23.09673691, -0.5822393894, 0.03313709423),
		glm::vec3(23.17610168, -0.568313539, 0.1021827906  ),
		glm::vec3(9.744159698, -0.3827288747, -39.38000107 ),
		glm::vec3(-11.19858932, -5.379834175, 11.07300282  ),
		glm::vec3(-12.89468384, -11.74000072, 9.321731567  ),
		glm::vec3(-9.962309837, -2.539999962, 6.432519913  ),
		glm::vec3(4.400217056, -11.74000168, -9.872331619  ),
		glm::vec3(-17.31947517, 16.89769363, 0.1986806095  ),
		glm::vec3(-18.47000122, 17.49173737, -0.6389849186 ),
		glm::vec3(-0.01999999955, 22.33585548, -13.48770714)   // minimal bad subset
	};

	return regressionPointCloud;
}

template<typename vectorType>
gltb::RefPtr<gltb::PointKDTree<vectorType>> buildPointKDTree(std::vector<vectorType> pointCloud, bool balanced)
{
	std::vector<int> pointData(pointCloud.size());

	for(size_t i = 0; i < pointData.size(); i++) {
		pointData[i] = i;
	}
	return new gltb::PointKDTree<vectorType>(pointCloud, pointData, balanced);
}

template<typename vectorType>
int findClosestPointReference(std::vector<vectorType> &pointCloud, vectorType point)
{
	typedef typename vectorType::value_type valueType;
	size_t result = 0;
	valueType distance = std::numeric_limits<valueType>::max();

	for(size_t i = 0; i < pointCloud.size(); i++) {
		valueType distanceToPoint = glm::length(pointCloud[i] - point);
		if(distanceToPoint < distance) {
			result = i;
			distance = distanceToPoint;
		}
	}

	return result;
}

template<typename vectorType>
std::vector<int> findPointsInRadiusReference(std::vector<vectorType> &pointCloud, vectorType point, typename vectorType::value_type radius)
{
	std::vector<int> result;

	for(size_t i = 0; i < pointCloud.size(); i++) {
		if(glm::distance(pointCloud[i], point) <= radius) {
			result.push_back(i);
		}
	}

	return result;
}

template<typename vectorType>
std::vector<int> findNearestNeighboursReference(const std::vector<vectorType> &pointCloud, vectorType point, size_t numNeighbours)
{
	typedef typename vectorType::value_type scalarType;
	typedef std::pair<int, scalarType> pairType;

	std::vector<int> result;

	scalarType searchRadius = std::numeric_limits<scalarType>::max();

	std::vector<pairType> distances(pointCloud.size());
	for(size_t i = 0; i < pointCloud.size(); i++) {
		distances[i] = pairType(i, glm::distance(pointCloud[i], point));
	}

	// sort whole point set by closest distance
	std::sort(distances.begin(), distances.end(), [&pointCloud, point] (const pairType &left, const pairType &right) -> bool { return left.second < right.second; });

	for(size_t i = 0; i < std::min(numNeighbours, distances.size()); i++) {
		result.push_back(distances[i].first);
	}

	return result;
}

/**
 * Verify that child nodes in all possible subtrees of the kD-tree are within the bounding
 * box covered by their respective root nodes.
 */
template<typename vectorType>
void checkRangeConstraint(gltb::RefPtr<gltb::PointKDTree<vectorType>> tree, int node, gltb::AABB<vectorType> boundingBox)
{
	vectorType nodePosition = tree->getNodePosition(node);
	char splitPlaneIndex = tree->getNodeSplitPlane(node);
	
	ASSERT_LE(boundingBox.min[splitPlaneIndex], nodePosition[splitPlaneIndex]);
	ASSERT_GE(boundingBox.max[splitPlaneIndex], nodePosition[splitPlaneIndex]);

	int leftChild, rightChild;
	tree->getNodeChildren(node, leftChild, rightChild);
	if(leftChild != -1) {
		gltb::AABB<vectorType> leftChildAABB(boundingBox);
		leftChildAABB.max[splitPlaneIndex] = nodePosition[splitPlaneIndex];
		checkRangeConstraint(tree, leftChild, leftChildAABB);
	}
	if(rightChild != -1) {
		gltb::AABB<vectorType> rightChildAABB(boundingBox);
		rightChildAABB.min[splitPlaneIndex] = nodePosition[splitPlaneIndex];
		checkRangeConstraint(tree, rightChild, rightChildAABB);
	}
}

TEST(PointKDTree2D, verifyTreeConsistency)
{
	const size_t numPoints = 1000000;

	std::vector<glm::vec2> pointCloud = generatePointCloud(glm::vec2(-150, -200), glm::vec2(200, 350), numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec2>> tree = buildPointKDTree(pointCloud, true);

	int minDepth, maxDepth;
	tree->getMinMaxDepth(minDepth, maxDepth);
	std::cout << minDepth << " " << maxDepth << std::endl;
	ASSERT_LE(maxDepth - minDepth, 1);

	ASSERT_EQ(tree->getNumNodes(), numPoints);

	gltb::AABB<glm::vec2> pointsBox(pointCloud);
	checkRangeConstraint(tree, 0, pointsBox);

	std::vector<bool> pointPresent(numPoints);
	for(size_t i = 0; i < numPoints; i++) {
		pointPresent[i] = false;
	}

	for(size_t i = 0; i < tree->getNumNodes(); i++) {
		int leftChild, rightChild;
		tree->getNodeChildren(i, leftChild, rightChild);
		glm::vec2 nodePosition = tree->getNodePosition(i);
		char splitPlaneIndex = tree->getNodeSplitPlane(i);

		int userData = tree->getNodeUserData(i);
		ASSERT_TRUE(nodePosition == pointCloud[userData]); // user data and positions were not mixed up

		ASSERT_FALSE(pointPresent[userData]); // point has not been seen before
		pointPresent[userData] = true; // mark point as seen

		if(leftChild != -1) {
			ASSERT_EQ((splitPlaneIndex + 1) % 2, tree->getNodeSplitPlane(leftChild));

			glm::vec2 leftChildPosition = tree->getNodePosition(leftChild);
			ASSERT_LE(leftChildPosition[splitPlaneIndex], nodePosition[splitPlaneIndex]);
		}
		if(rightChild != -1) {
			ASSERT_EQ((splitPlaneIndex + 1) % 2, tree->getNodeSplitPlane(rightChild));

			glm::vec2 rightChildPosition = tree->getNodePosition(rightChild);
			ASSERT_GE(rightChildPosition[splitPlaneIndex], nodePosition[splitPlaneIndex]);
		}
	}

	// all points have been seen
	for(size_t i = 0; i < numPoints; i++) {
		ASSERT_TRUE(pointPresent[i]);
	}
}

TEST(PointKDTree2D, findClosestPoint)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 10000;

	const glm::vec2 min = glm::vec2(-150, -200);
	const glm::vec2 max = glm::vec2(200, 350);

	std::vector<glm::vec2> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec2>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec2 randomPoint = generateRandomPoint(min, max, rng);

		int treeClosestPoint = tree->getNodeUserData(tree->findClosestPoint(randomPoint));
		int referenceClosestPoint = findClosestPointReference(pointCloud, randomPoint);

		if(treeClosestPoint != referenceClosestPoint) {
			float treeDistance = glm::length(pointCloud[treeClosestPoint] - randomPoint);
			float referenceDistance = glm::length(pointCloud[referenceClosestPoint] - randomPoint);

			std::cout << "iteration: " << i << std::endl;
			std::cout << "kD tree distance: " << treeDistance << std::endl;
			std::cout << "reference distance: " << referenceDistance << std::endl;
			std::cout << "difference: " << fabs(treeDistance - referenceDistance) << std::endl;
		}

		ASSERT_EQ(treeClosestPoint, referenceClosestPoint);
	}
}

TEST(PointKDTree2D, findPointsWithinRadius)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 1000;

	const glm::vec2 min = glm::vec2(-150, -200);
	const glm::vec2 max = glm::vec2(200, 350);

	std::vector<glm::vec2> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec2>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec2 point = generateRandomPoint(min, max, rng);
		float radius = 0.5 * rng.genrandReal1() * glm::distance(min, max);

		std::vector<int> referenceNearPoints = findPointsInRadiusReference(pointCloud, point, radius);
		std::vector<int> treeNearPoints = tree->findPointsWithinRadius(point, radius);

		ASSERT_EQ(referenceNearPoints.size(), treeNearPoints.size());

		std::sort(referenceNearPoints.begin(), referenceNearPoints.end());
		std::sort(treeNearPoints.begin(), treeNearPoints.end(), [&tree] (int a, int b) { return (tree->getNodeUserData(a) < tree->getNodeUserData(b)); });

		for(size_t j = 0; j < referenceNearPoints.size(); j++) {
			ASSERT_EQ(referenceNearPoints[j], tree->getNodeUserData(treeNearPoints[j]));
		}
	}
}

TEST(PointKDTree2D, findNearestNeighboursSimple)
{
	const size_t numPoints = 10;
	const size_t numTestPoints = 1;

	const glm::vec2 min = glm::vec2(-150, -200);
	const glm::vec2 max = glm::vec2(200, 350);

	std::vector<glm::vec2> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec2>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	glm::vec2 point = generateRandomPoint(min, max, rng);


	float radius = 0.5 * rng.genrandReal1() * glm::distance(min, max);

	std::vector<int> referenceNearPoints = findNearestNeighboursReference(pointCloud, point, 2);
	std::vector<int> treeNearPoints = tree->findNearestNeighbours(point, 2);

	ASSERT_EQ(referenceNearPoints.size(), treeNearPoints.size());

	std::sort(referenceNearPoints.begin(), referenceNearPoints.end());
	std::sort(treeNearPoints.begin(), treeNearPoints.end(), [&tree] (int a, int b) { return (tree->getNodeUserData(a) < tree->getNodeUserData(b)); });

	for(size_t j = 0; j < referenceNearPoints.size(); j++) {
		ASSERT_EQ(referenceNearPoints[j], tree->getNodeUserData(treeNearPoints[j]));
	}
}

TEST(PointKDTree2D, findNearestNeighbours)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 1000;

	const glm::vec2 min = glm::vec2(-150, -200);
	const glm::vec2 max = glm::vec2(200, 350);

	std::vector<glm::vec2> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec2>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec2 point = generateRandomPoint(min, max, rng);

		float radius = 0.5 * rng.genrandReal1() * glm::distance(min, max);

		std::vector<int> referenceNearPoints = findNearestNeighboursReference(pointCloud, point, 10);
		std::vector<int> treeNearPoints = tree->findNearestNeighbours(point, 10);

		ASSERT_EQ(referenceNearPoints.size(), treeNearPoints.size());

		std::sort(referenceNearPoints.begin(), referenceNearPoints.end());
		std::sort(treeNearPoints.begin(), treeNearPoints.end(), [&tree] (int a, int b) { return (tree->getNodeUserData(a) < tree->getNodeUserData(b)); });

		for(size_t j = 0; j < referenceNearPoints.size(); j++) {
			ASSERT_EQ(referenceNearPoints[j], tree->getNodeUserData(treeNearPoints[j]));
		}
	}
}

TEST(PointKDTree3D, verifyTreeConsistency)
{
	const size_t numPoints = 1000000;

	std::vector<glm::vec3> pointCloud = generatePointCloud(glm::vec3(-150, -200, -300), glm::vec3(200, 350, 100), numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec3>> tree = buildPointKDTree(pointCloud, true);

	int minDepth, maxDepth;
	tree->getMinMaxDepth(minDepth, maxDepth);
	std::cout << minDepth << " " << maxDepth << std::endl;
	ASSERT_LE(maxDepth - minDepth, 1);

	ASSERT_EQ(tree->getNumNodes(), numPoints);

	gltb::AABB<glm::vec3> pointsBox(pointCloud);
	checkRangeConstraint(tree, 0, pointsBox);

	for(size_t i = 0; i < tree->getNumNodes(); i++) {
		int leftChild, rightChild;
		tree->getNodeChildren(i, leftChild, rightChild);
		glm::vec3 nodePosition = tree->getNodePosition(i);
		char splitPlaneIndex = tree->getNodeSplitPlane(i);

		int userData = tree->getNodeUserData(i);
		ASSERT_TRUE(nodePosition == pointCloud[userData]); // user data and positions were not mixed up

		if(leftChild != -1) {
			ASSERT_EQ((splitPlaneIndex + 1) % 3, tree->getNodeSplitPlane(leftChild));

			glm::vec3 leftChildPosition = tree->getNodePosition(leftChild);
			ASSERT_LE(leftChildPosition[splitPlaneIndex], nodePosition[splitPlaneIndex]);
		}
		if(rightChild != -1) {
			ASSERT_EQ((splitPlaneIndex + 1) % 3, tree->getNodeSplitPlane(rightChild));

			glm::vec3 rightChildPosition = tree->getNodePosition(rightChild);
			ASSERT_GE(rightChildPosition[splitPlaneIndex], nodePosition[splitPlaneIndex]);
		}
	}
}

TEST(PointKDTree3D, verifyTreeConsistencyRegressionSet)
{
	std::vector<glm::vec3> pointCloud = getRegressionPointCloud();
	size_t numPoints = pointCloud.size();
	gltb::RefPtr<gltb::PointKDTree<glm::vec3>> tree = buildPointKDTree(pointCloud, true);

	int minDepth, maxDepth;
	tree->getMinMaxDepth(minDepth, maxDepth);
	std::cout << minDepth << " " << maxDepth << std::endl;

	ASSERT_EQ(tree->getNumNodes(), numPoints);

	gltb::AABB<glm::vec3> pointsBox(pointCloud);
	checkRangeConstraint(tree, 0, pointsBox);

	for(size_t i = 0; i < tree->getNumNodes(); i++) {
		int leftChild, rightChild;
		tree->getNodeChildren(i, leftChild, rightChild);
		glm::vec3 nodePosition = tree->getNodePosition(i);
		char splitPlaneIndex = tree->getNodeSplitPlane(i);

		int userData = tree->getNodeUserData(i);
		ASSERT_TRUE(nodePosition == pointCloud[userData]); // user data and positions were not mixed up

		if(leftChild != -1) {
			ASSERT_EQ((splitPlaneIndex + 1) % 3, tree->getNodeSplitPlane(leftChild));

			glm::vec3 leftChildPosition = tree->getNodePosition(leftChild);
			ASSERT_LE(leftChildPosition[splitPlaneIndex], nodePosition[splitPlaneIndex]);
		}
		if(rightChild != -1) {
			ASSERT_EQ((splitPlaneIndex + 1) % 3, tree->getNodeSplitPlane(rightChild));

			glm::vec3 rightChildPosition = tree->getNodePosition(rightChild);
			ASSERT_GE(rightChildPosition[splitPlaneIndex], nodePosition[splitPlaneIndex]);
		}
	}
}

TEST(PointKDTree3D, findClosestPoint)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 10000;

	const glm::vec3 min = glm::vec3(-150, -200, -300);
	const glm::vec3 max = glm::vec3(200, 350, 100);

	std::vector<glm::vec3> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec3>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec3 randomPoint = generateRandomPoint(min, max, rng);

		int treeClosestPoint = tree->getNodeUserData(tree->findClosestPoint(randomPoint));
		int referenceClosestPoint = findClosestPointReference(pointCloud, randomPoint);

		if(treeClosestPoint != referenceClosestPoint) {
			float treeDistance = glm::length(pointCloud[treeClosestPoint] - randomPoint);
			float referenceDistance = glm::length(pointCloud[referenceClosestPoint] - randomPoint);

			std::cout << "iteration: " << i << std::endl;
			std::cout << "kD tree distance: " << treeDistance << std::endl;
			std::cout << "reference distance: " << referenceDistance << std::endl;
			std::cout << "difference: " << fabs(treeDistance - referenceDistance) << std::endl;
		}

		ASSERT_EQ(treeClosestPoint, referenceClosestPoint);
	}
}

TEST(PointKDTree3D, findPointsWithinRadius)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 1000;

	const glm::vec3 min = glm::vec3(-150, -200, -300);
	const glm::vec3 max = glm::vec3(200, 350, 100);

	std::vector<glm::vec3> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec3>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec3 point = generateRandomPoint(min, max, rng);
		float radius = 0.5 * rng.genrandReal1() * glm::distance(min, max);

		std::vector<int> referenceNearPoints = findPointsInRadiusReference(pointCloud, point, radius);
		std::vector<int> treeNearPoints = tree->findPointsWithinRadius(point, radius);

		ASSERT_EQ(referenceNearPoints.size(), treeNearPoints.size());

		std::sort(referenceNearPoints.begin(), referenceNearPoints.end());
		std::sort(treeNearPoints.begin(), treeNearPoints.end(), [&tree] (int a, int b) { return (tree->getNodeUserData(a) < tree->getNodeUserData(b)); });

		for(size_t j = 0; j < referenceNearPoints.size(); j++) {
			ASSERT_EQ(referenceNearPoints[j], tree->getNodeUserData(treeNearPoints[j]));
		}
	}
}

TEST(PointKDTree3D, findNearestNeighbours)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 1000;

	const glm::vec3 min = glm::vec3(-150, -200, -300);
	const glm::vec3 max = glm::vec3(200, 350, 100);

	std::vector<glm::vec3> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec3>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec3 point = generateRandomPoint(min, max, rng);

		float radius = 0.5 * rng.genrandReal1() * glm::distance(min, max);

		std::vector<int> referenceNearPoints = findNearestNeighboursReference(pointCloud, point, 10);
		std::vector<int> treeNearPoints = tree->findNearestNeighbours(point, 10);

		ASSERT_EQ(referenceNearPoints.size(), treeNearPoints.size());

		std::sort(referenceNearPoints.begin(), referenceNearPoints.end());
		std::sort(treeNearPoints.begin(), treeNearPoints.end(), [&tree] (int a, int b) { return (tree->getNodeUserData(a) < tree->getNodeUserData(b)); });

		for(size_t j = 0; j < referenceNearPoints.size(); j++) {
			ASSERT_EQ(referenceNearPoints[j], tree->getNodeUserData(treeNearPoints[j]));
		}
	}
}

TEST(PointKDTree4D, verifyTreeConsistency)
{
	const size_t numPoints = 1000000;

	std::vector<glm::vec4> pointCloud = generatePointCloud(glm::vec4(-150, -200, -300, 200), glm::vec4(200, 350, -10, 400), numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec4>> tree = buildPointKDTree(pointCloud, true);

	int minDepth, maxDepth;
	tree->getMinMaxDepth(minDepth, maxDepth);
	std::cout << minDepth << " " << maxDepth << std::endl;
	ASSERT_LE(maxDepth - minDepth, 1);

	ASSERT_EQ(tree->getNumNodes(), numPoints);

	gltb::AABB<glm::vec4> pointsBox(pointCloud);
	checkRangeConstraint(tree, 0, pointsBox);

	for(size_t i = 0; i < tree->getNumNodes(); i++) {
		int leftChild, rightChild;
		tree->getNodeChildren(i, leftChild, rightChild);
		glm::vec4 nodePosition = tree->getNodePosition(i);
		char splitPlaneIndex = tree->getNodeSplitPlane(i);

		int userData = tree->getNodeUserData(i);
		ASSERT_TRUE(nodePosition == pointCloud[userData]); // user data and positions were not mixed up

		if(leftChild != -1) {
			ASSERT_EQ((splitPlaneIndex + 1) % 4, tree->getNodeSplitPlane(leftChild));

			glm::vec4 leftChildPosition = tree->getNodePosition(leftChild);
			ASSERT_LE(leftChildPosition[splitPlaneIndex], nodePosition[splitPlaneIndex]);
		}
		if(rightChild != -1) {
			ASSERT_EQ((splitPlaneIndex + 1) % 4, tree->getNodeSplitPlane(rightChild));

			glm::vec4 rightChildPosition = tree->getNodePosition(rightChild);
			ASSERT_GE(rightChildPosition[splitPlaneIndex], nodePosition[splitPlaneIndex]);
		}
	}
}

TEST(PointKDTree4D, findClosestPoint)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 10000;

	const glm::vec4 min = glm::vec4(-150, -200, -300, 200);
	const glm::vec4 max = glm::vec4(200, 350, -10, 400);

	std::vector<glm::vec4> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec4>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec4 randomPoint = generateRandomPoint(min, max, rng);

		int treeClosestPoint = tree->getNodeUserData(tree->findClosestPoint(randomPoint));
		int referenceClosestPoint = findClosestPointReference(pointCloud, randomPoint);

		if(treeClosestPoint != referenceClosestPoint) {
			float treeDistance = glm::length(pointCloud[treeClosestPoint] - randomPoint);
			float referenceDistance = glm::length(pointCloud[referenceClosestPoint] - randomPoint);

			std::cout << "iteration: " << i << std::endl;
			std::cout << "kD tree distance: " << treeDistance << std::endl;
			std::cout << "reference distance: " << referenceDistance << std::endl;
			std::cout << "difference: " << fabs(treeDistance - referenceDistance) << std::endl;
		}

		ASSERT_EQ(treeClosestPoint, referenceClosestPoint);
	}
}

TEST(PointKDTree4D, findPointsWithinRadius)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 1000;

	const glm::vec4 min = glm::vec4(-150, -200, -300, 200);
	const glm::vec4 max = glm::vec4(200, 350, -10, 400);

	std::vector<glm::vec4> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec4>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec4 point = generateRandomPoint(min, max, rng);
		float radius = 0.5 * rng.genrandReal1() * glm::distance(min, max);

		std::vector<int> referenceNearPoints = findPointsInRadiusReference(pointCloud, point, radius);
		std::vector<int> treeNearPoints = tree->findPointsWithinRadius(point, radius);

		ASSERT_EQ(referenceNearPoints.size(), treeNearPoints.size());

		std::sort(referenceNearPoints.begin(), referenceNearPoints.end());
		std::sort(treeNearPoints.begin(), treeNearPoints.end(), [&tree] (int a, int b) { return (tree->getNodeUserData(a) < tree->getNodeUserData(b)); });

		for(size_t j = 0; j < referenceNearPoints.size(); j++) {
			ASSERT_EQ(referenceNearPoints[j], tree->getNodeUserData(treeNearPoints[j]));
		}
	}
}

TEST(PointKDTree4D, findNearestNeighbours)
{
	const size_t numPoints = 100000;
	const size_t numTestPoints = 1000;

	const glm::vec4 min = glm::vec4(-150, -200, -300, 200);
	const glm::vec4 max = glm::vec4(200, 350, 100, 400);

	std::vector<glm::vec4> pointCloud = generatePointCloud(min, max, numPoints, 10);
	gltb::RefPtr<gltb::PointKDTree<glm::vec4>> tree = buildPointKDTree(pointCloud, true);

	gltb::Random rng;

	for(size_t i = 0; i < numTestPoints; i++) {
		glm::vec4 point = generateRandomPoint(min, max, rng);

		float radius = 0.5 * rng.genrandReal1() * glm::distance(min, max);

		std::vector<int> referenceNearPoints = findNearestNeighboursReference(pointCloud, point, 10);
		std::vector<int> treeNearPoints = tree->findNearestNeighbours(point, 10);

		ASSERT_EQ(referenceNearPoints.size(), treeNearPoints.size());

		std::sort(referenceNearPoints.begin(), referenceNearPoints.end());
		std::sort(treeNearPoints.begin(), treeNearPoints.end(), [&tree] (int a, int b) { return (tree->getNodeUserData(a) < tree->getNodeUserData(b)); });

		for(size_t j = 0; j < referenceNearPoints.size(); j++) {
			ASSERT_EQ(referenceNearPoints[j], tree->getNodeUserData(treeNearPoints[j]));
		}
	}
}

int main(int argc, char *argv[])
{
    ::testing::InitGoogleTest(&argc, argv);

    return RUN_ALL_TESTS();
}