#include <iostream>
#include <vector>
#include <stack>
#include <cmath>
#include <algorithm> 
#include <math.h>
#include "kdTree.h"

using namespace std;


bool cmp1(const coordinate& a, const coordinate& b) {
	return a.x > b.x;
}

bool cmp2(const coordinate& a, const coordinate& b) {
	return a.y > b.y;
}

bool cmp3(const coordinate& a, const coordinate& b) {
	return a.z > b.z;
}

bool equal(const coordinate& a, const coordinate& b) {
	return (a.x == b.x && a.y == b.y && a.z == b.z);
}

void ChooseSplit(coordinate exm_set[], int size, UL &split, coordinate &SplitChoice) {
	/*compute the variance on every dimension. Set split as the dismension that have the biggest
	 variance. Then choose the instance which is the median on this split dimension.*/
	 /*compute variance on the x,y dimension. DX=EX^2-(EX)^2*/
	double tmp1, tmp2;
	tmp1 = tmp2 = 0;
	for (int i = 0; i < size; ++i)
	{
		tmp1 += 1.0 / (double)size * exm_set[i].x * exm_set[i].x;
		tmp2 += 1.0 / (double)size * exm_set[i].x;
	}
	double v1 = tmp1 - tmp2 * tmp2;  //compute variance on the x dimension ¼ÆËãx·½Ïò·½²î

	tmp1 = tmp2 = 0;
	for (int i = 0; i < size; ++i)
	{
		tmp1 += 1.0 / (double)size * exm_set[i].y * exm_set[i].y;
		tmp2 += 1.0 / (double)size * exm_set[i].y;
	}
	double v2 = tmp1 - tmp2 * tmp2;  //compute variance on the y dimension ¼ÆËãy·½Ïò·½²î

	tmp1 = tmp2 = 0;
	for (int i = 0; i < size; ++i)
	{
		tmp1 += 1.0 / (double)size * exm_set[i].z * exm_set[i].z;
		tmp2 += 1.0 / (double)size * exm_set[i].z;
	}
	double v3 = tmp1 - tmp2 * tmp2;  //compute variance on the z dimension ¼ÆËãz·½Ïò·½²î
	

	//split = v1 > v2 ? 0 : 1; //set the split dimension


	if (v1 >= v2 && v1 >= v3)
	{
		split = 0;
		std::nth_element(exm_set, exm_set + size / 2, exm_set + size, cmp1);
	}
	else if (v2 >= v1 && v2 >= v3)
	{
		split = 1;
		std::nth_element(exm_set, exm_set + size / 2, exm_set + size, cmp2);
	}
	else
	{
		split = 2;
		std::nth_element(exm_set, exm_set + size / 2, exm_set + size, cmp3);
	} 

	//set the split point value
	SplitChoice.x = exm_set[size / 2 ].x;
	SplitChoice.y = exm_set[size / 2 ].y;
	SplitChoice.z = exm_set[size / 2 ].z;

}

TreeNode* build_kdtree(coordinate exm_set[], int size, TreeNode* T) {
	//call function ChooseSplit to choose the split dimension and split point
	if (size == 0) {
		return nullptr;
	}
	else {
		UL split;
		coordinate dom_elt;
		ChooseSplit(exm_set, size, split, dom_elt);
		coordinate *exm_set_right = new coordinate[size * 9 / 16 + 10];
		coordinate *exm_set_left = new coordinate[size * 9 / 16 + 10];
		int size_left, size_right;
		size_left = size_right = 0;

		switch (split)
		{
		case 0:
			for (UL i = 0; i < size; ++i)
			{

				if (!equal(exm_set[i], dom_elt) && exm_set[i].x <= dom_elt.x)
				{
					
					exm_set_left[size_left].x = exm_set[i].x;
					exm_set_left[size_left].y = exm_set[i].y;
					exm_set_left[size_left].z = exm_set[i].z;
					size_left++;
				}
				else if (!equal(exm_set[i], dom_elt) && exm_set[i].x > dom_elt.x)
				{
					
					exm_set_right[size_right].x = exm_set[i].x;
					exm_set_right[size_right].y = exm_set[i].y;
					exm_set_right[size_right].z = exm_set[i].z;
					size_right++;
				}
			}
			break;
		case 1:
			for (UL i = 0; i < size; ++i)
			{
				if (!equal(exm_set[i], dom_elt) && exm_set[i].y <= dom_elt.y)
				{
					
					exm_set_left[size_left].x = exm_set[i].x;
					exm_set_left[size_left].y = exm_set[i].y;
					exm_set_left[size_left].z = exm_set[i].z;
					size_left++;
				}
				else if (!equal(exm_set[i], dom_elt) && exm_set[i].y > dom_elt.y)
				{
					
					exm_set_right[size_right].x = exm_set[i].x;
					exm_set_right[size_right].y = exm_set[i].y;
					exm_set_right[size_right].z = exm_set[i].z;
					size_right++;
				}
			}
			break;
		default:
			for (UL i = 0; i < size; ++i)
			{
				if (!equal(exm_set[i], dom_elt) && exm_set[i].z <= dom_elt.z)
				{
					
					exm_set_left[size_left].x = exm_set[i].x;
					exm_set_left[size_left].y = exm_set[i].y;
					exm_set_left[size_left].z = exm_set[i].z;
					size_left++;
				}
				else if (!equal(exm_set[i], dom_elt) && exm_set[i].z > dom_elt.z)
				{
					
					exm_set_right[size_right].x = exm_set[i].x;
					exm_set_right[size_right].y = exm_set[i].y;
					exm_set_right[size_right].z = exm_set[i].z;
					size_right++;
				}
			}
			break;
		}

		
		T = new TreeNode;
		T->dom_elt.x = dom_elt.x;
		T->dom_elt.y = dom_elt.y;
		T->dom_elt.z = dom_elt.z;
		T->split = split;
		T->left = build_kdtree(exm_set_left, size_left, T->left);
		T->right = build_kdtree(exm_set_right, size_right, T->right);

		delete[] exm_set_left;
		delete[] exm_set_right;

		return T;

	}
}

double Distance(coordinate a, coordinate b) {
	double tmp = (a.x - b.x) * (a.x - b.x) 
		+ (a.y - b.y) * (a.y - b.y) 
		+ (a.z - b.z) * (a.z - b.z);
	return sqrt(tmp);
}

void searchNearest(TreeNode * Kd, coordinate target, coordinate &nearestpoint, double & distance) {

	//1. Èç¹ûKdÊǿյģ¬ÔòÉèdistΪÎÞÇî´ó·µ»Ø

	//2. ÏòÏÂËÑË÷Ö±µ½Ò¶×Ó½áµã

	stack<TreeNode*> search_path;
	TreeNode* pSearch = Kd;
	coordinate nearest;
	double dist;

	while (pSearch != nullptr)
	{
		//pSearch¼ÓÈëµ½search_pathÖÐ;
		search_path.push(pSearch);

		switch (pSearch->split)
		{
		case 0:
			if (target.x <= pSearch->dom_elt.x) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
			{
				pSearch = pSearch->left;
			}
			else
			{
				pSearch = pSearch->right;
			}
			break;
		case 1:
			if (target.y <= pSearch->dom_elt.y) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
			{
				pSearch = pSearch->left;
			}
			else
			{
				pSearch = pSearch->right;
			}
			break;
		default:
			if (target.z <= pSearch->dom_elt.z) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
			{
				pSearch = pSearch->left;
			}
			else
			{
				pSearch = pSearch->right;
			}
			break;
		}

		
	}
	//È¡³ösearch_path×îºóÒ»¸ö¸³¸ønearest
	nearest.x = search_path.top()->dom_elt.x;
	nearest.y = search_path.top()->dom_elt.y;
	nearest.z = search_path.top()->dom_elt.z;
	search_path.pop();


	dist = Distance(nearest, target);
	//3. »ØËÝËÑË÷·¾¶

	TreeNode* pBack;

	while (!search_path.empty())
	{
		//È¡³ösearch_path×îºóÒ»¸ö½áµã¸³¸øpBack
		pBack = search_path.top();
		search_path.pop();

		if (pBack->left == nullptr && pBack->right == nullptr) /* Èç¹ûpBackΪҶ×Ó½áµã */
		{

			if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
			{
				nearest = pBack->dom_elt;
				dist = Distance(pBack->dom_elt, target);
			}

		}

		else

		{

			UL s = pBack->split;

			switch (s)
			{
			case 0:
				if (fabs(pBack->dom_elt.x - target.x) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
				{
					if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
					{
						nearest = pBack->dom_elt;
						dist = Distance(pBack->dom_elt, target);
					}
					if (target.x <= pBack->dom_elt.x) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
						pSearch = pBack->right;
					else
						pSearch = pBack->left; /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
					if (pSearch != nullptr)
						//pSearch¼ÓÈëµ½search_pathÖÐ
						search_path.push(pSearch);
				}
				break;
			case 1:
				if (fabs(pBack->dom_elt.y - target.y) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
				{
					if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
					{
						nearest = pBack->dom_elt;
						dist = Distance(pBack->dom_elt, target);
					}
					if (target.y <= pBack->dom_elt.y) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
						pSearch = pBack->right;
					else
						pSearch = pBack->left; /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
					if (pSearch != nullptr)
						// pSearch¼ÓÈëµ½search_pathÖÐ
						search_path.push(pSearch);
				}
				break;
			default:
				if (fabs(pBack->dom_elt.z - target.z) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
				{
					if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
					{
						nearest = pBack->dom_elt;
						dist = Distance(pBack->dom_elt, target);
					}
					if (target.z <= pBack->dom_elt.z) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
						pSearch = pBack->right;
					else
						pSearch = pBack->left; /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
					if (pSearch != nullptr)
						// pSearch¼ÓÈëµ½search_pathÖÐ
						search_path.push(pSearch);
				}
				break;
			}
		}
	}

	nearestpoint.x = nearest.x;
	nearestpoint.y = nearest.y;
	nearestpoint.z = nearest.z;
	distance = dist;

}

void searchNearest(const TreeElement* treeArray,int size, coordinate target, coordinate &nearestpoint, double & distance)
{
	stack<int> search_path;

	int index = 0;
	TreeElement pSearch = treeArray[index];

	coordinate nearest;
	double dist;

	while (!pSearch.isEmpty)
	{
		search_path.push(index);

		if ((2 * index + 1) >= size)
		{
			break;
		}

		switch (pSearch.split)
		{
		case 0:
			if (target.x <= pSearch.dom_elt.x) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
			{
				index = 2 * index + 1;
				pSearch = treeArray[index];
			}
			else
			{
				index = 2 * index + 2;
				pSearch = treeArray[index];
			}
			break;
		case 1:
			if (target.y <= pSearch.dom_elt.y) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
			{
				index = 2 * index + 1;
				pSearch = treeArray[index];
			}
			else
			{
				index = 2 * index + 2;
				pSearch = treeArray[index];
			}
			break;
		default:
			if (target.z <= pSearch.dom_elt.z) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
			{
				index = 2 * index + 1;
				pSearch = treeArray[index];
			}
			else
			{
				index = 2 * index + 2;
				pSearch = treeArray[index];
			}
			break;
		}
	}
	//È¡³ösearch_path×îºóÒ»¸ö¸³¸ønearest
	index = search_path.top();
	nearest.x = treeArray[index].dom_elt.x;
	nearest.y = treeArray[index].dom_elt.y;
	nearest.z = treeArray[index].dom_elt.z;
	search_path.pop();
	dist = Distance(nearest, target);

	//3. »ØËÝËÑË÷·¾¶
	TreeElement pBack;

	while (!search_path.empty())
	{
		//È¡³ösearch_path×îºóÒ»¸ö½áµã¸³¸øpBack
		index = search_path.top();
		pBack = treeArray[index];
		search_path.pop();

		if ((2 * index + 1) >= size) /* Èç¹ûpBackΪҶ×Ó½áµã */
		{
			if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
			{
				nearest = pBack.dom_elt;
				dist = Distance(pBack.dom_elt, target);
			}
		}
		else if (treeArray[2 * index + 1].isEmpty == true && treeArray[2 * index + 2].isEmpty == true) /* Èç¹ûpBackΪҶ×Ó½áµã */
		{
			if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
			{
				nearest = pBack.dom_elt;
				dist = Distance(pBack.dom_elt, target);
			}
		}
		else
		{
			UL s = pBack.split;
			switch (s)
			{
			case 0:
				if (fabs(pBack.dom_elt.x - target.x) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
				{
					if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
					{
						nearest = pBack.dom_elt;
						dist = Distance(pBack.dom_elt, target);
					}

					if (target.x <= pBack.dom_elt.x) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
					{
						index = 2 * index + 2;
						pSearch = treeArray[index];
					}
					else                            /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
					{
						index = 2 * index + 1;
						pSearch = treeArray[index];
					}
					if (!pSearch.isEmpty)
						//pSearch¼ÓÈëµ½search_pathÖÐ
						search_path.push(index);
				}
				break;
			case 1:
				if (fabs(pBack.dom_elt.y - target.y) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
				{
					if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
					{
						nearest = pBack.dom_elt;
						dist = Distance(pBack.dom_elt, target);
					}
					if (target.y <= pBack.dom_elt.y) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
					{
						index = 2 * index + 2;
						pSearch = treeArray[index];
					}
					else                             /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
					{
						index = 2 * index + 1;
						pSearch = treeArray[index];
					}
					if (!pSearch.isEmpty)
						//pSearch¼ÓÈëµ½search_pathÖÐ
						search_path.push(index);
				}
				break;
			default:
				if (fabs(pBack.dom_elt.z - target.z) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
				{
					if (Distance(nearest, target) > Distance(pBack.dom_elt, target))
					{
						nearest = pBack.dom_elt;
						dist = Distance(pBack.dom_elt, target);
					}
					if (target.z <= pBack.dom_elt.z) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
					{
						index = 2 * index + 2;
						pSearch = treeArray[index];
					}
					else                            /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
					{
						index = 2 * index + 1;
						pSearch = treeArray[index];
					}
					if (!pSearch.isEmpty)
						//pSearch¼ÓÈëµ½search_pathÖÐ
						search_path.push(index);
				}
				break;
			}
		}
		
	}

	nearestpoint.x = nearest.x;
	nearestpoint.y = nearest.y;
	nearestpoint.z = nearest.z;
	distance = dist;
}

int getHeight(TreeNode * T)
{
	if (T == nullptr)
	{
		return 0;
	}

	int leftheight = getHeight(T->left);

	int rightheight = getHeight(T->right);

	return max(leftheight, rightheight) + 1;

}

void TreeToArray(TreeNode* T, TreeElement* treeArray, int index)
{
	//TreeElement *treeArray = new TreeElement[size];

	if (T != nullptr)
	{
		treeArray[index].dom_elt = T->dom_elt;
		treeArray[index].split = T->split;
		treeArray[index].isEmpty = false;
		TreeToArray(T->left, treeArray, 2 * index + 1);
		TreeToArray(T->right, treeArray, 2 * index + 2);
	}
	/*else 
	{
		treeArray[index].dom_elt = *(new coordinate);
		treeArray[index].split = 0;
		treeArray[index].isEmpty = true;
	}*/
}