/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include <cmath>
		#include <cfloat>
		#include <cstring>
		#include "core/path_kdtree.h"
		#include "core/geometry.h"
		#include "log/log.h"
		#include "core/renderer_toolbox.h"

		using namespace GS;
		using namespace GS::Core;
		using GS::Render::Renderer;


PathKdtree::KDTreeNode::KDTreeNode():m_KDTREE_NODE_ID_SEGMENT(NULL){ memset(m_KDTREE_NODE_ID_ROPE, -1, sizeof(int)*6);};

#define KDTREE_MAX_DEPTH			20
#define KDTREE_MAX_POLY_PER_NODE	5

void PathKdtree::DrawKdtreeNode(Renderer &render, int _CurrentNode, Matrix4& m)
{	
	MinMax min_max;
		
	min_max.mn.x = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_LEFT];
	min_max.mn.y = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_BOTTOM];
	min_max.mn.z = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_BACK];
	min_max.mx.x = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_RIGHT];
	min_max.mx.y = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_TOP];
	min_max.mx.z = m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB[KDTREE_SIDE_FRONT];		

	min_max.mn = min_max.mn*m;
	min_max.mx = min_max.mx*m;

	RendererToolbox::DrawAABB(render, min_max);
	
	if(!m_NodeTree[_CurrentNode].m_KDTREE_NODE_IS_LEAF)
 	{
		DrawKdtreeNode(render,  m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT, m);
		DrawKdtreeNode(render,  m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT, m);
	}
}

void PathKdtree::draw_scene_debug(Renderer &render, Matrix4& m)
{
	DrawKdtreeNode(render, 0, m);			
}

void		PathKdtree::NearestQuadtreeTreeNode(Vector4 p, SharedArrayList<nMSegment*> &list_segment)
//------------------------------------------------------------------------------------------------------------------------
{
	// go inside the quadtree
	ArrayList<int> list_id_segment;

	if(segment_list.GetCount() <= 0)
		return;

 	int l_CurrentNode = 0;
 
 	while(!m_NodeTree[l_CurrentNode].m_KDTREE_NODE_IS_LEAF)
 	{
 		switch (m_NodeTree[l_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT)
 		{
 		case KDTREE_X_AXIS:
			{
				float l_X = p.x ;
				if(l_X == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
					l_X += 0.001f;

				if(l_X > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
					l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
				else
					l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
			}
 			break;
		case KDTREE_Y_AXIS:
			{
				float l_Y = p.y ;
				if(l_Y == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
					l_Y += 0.001f;
					
				if(l_Y > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
					l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
				else
					l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
			}
 			break;
		case KDTREE_Z_AXIS:
			{
				float l_Z = p.z;
				if(l_Z == m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
					l_Z += 0.001f;
					
				if(l_Z > m_NodeTree[l_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT)
					l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT;
				else
					l_CurrentNode = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT;
			}
 			break;					
 		}
	} 

	int* l_TempPntIdSegment = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_ID_SEGMENT;
	int l_CountSegment = m_NodeTree[l_CurrentNode].m_KDTREE_NODE_COUNT_SEGMENT;
	
	for(int i=0; i< l_CountSegment; ++i)
	{
		list_segment.Add(segment_list[*l_TempPntIdSegment]);
		++l_TempPntIdSegment;
	}
}

//-------------------------------------------------------------------
void PathKdtree::IncreaseSizeNodeKdtreeBuffer(int _IncreaseSize)
//-------------------------------------------------------------------
{
	KDTreeNode* l_tempCopy = new KDTreeNode[m_SizeTree + _IncreaseSize];
	memcpy(l_tempCopy, m_NodeTree, sizeof(KDTreeNode)*m_SizeTree);

	for(int i=0; i<m_SizeTree; ++i)
	{
		if(m_NodeTree[i].m_KDTREE_NODE_ID_SEGMENT)
		{
			l_tempCopy[i].m_KDTREE_NODE_ID_SEGMENT = new int[m_NodeTree[i].m_KDTREE_NODE_COUNT_SEGMENT];
			memcpy( l_tempCopy[i].m_KDTREE_NODE_ID_SEGMENT, m_NodeTree[i].m_KDTREE_NODE_ID_SEGMENT, sizeof(int)*m_NodeTree[i].m_KDTREE_NODE_COUNT_SEGMENT);
		}
	}

	m_SizeTree += _IncreaseSize;

	delete []m_NodeTree;
	m_NodeTree = l_tempCopy;
}

//----------------------------------------------------------------------------------------------------------------------------------------------------------------
void PathKdtree::CreateNodeKdtree(int &_CurrentNode, int *_IdSegment, int _CountSegment, int _CurrentDepth, bool _ForceCreateLeaf )
//----------------------------------------------------------------------------------------------------------------------------------------------------------------
{
	// check if there is a minimum of place for 2 child 
	if(m_SizeTree < _CurrentNode + 3)
	{
		IncreaseSizeNodeKdtreeBuffer(1000);
	}

	// set the id of the node
	m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID = _CurrentNode;

	//get the aabb
	float * l_TempAABB =  m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB;

	// check if  it's the moment to create the leaf
	if(_ForceCreateLeaf || _CountSegment < KDTREE_MAX_POLY_PER_NODE || _CurrentDepth >= KDTREE_MAX_DEPTH
		/*|| fabs(l_TempAABB[KDTREE_SIDE_LEFT] - l_TempAABB[KDTREE_SIDE_RIGHT]) < 0.1f
		|| fabs(l_TempAABB[KDTREE_SIDE_BOTTOM] - l_TempAABB[KDTREE_SIDE_TOP]) < 0.1f
		|| fabs(l_TempAABB[KDTREE_SIDE_BACK] - l_TempAABB[KDTREE_SIDE_FRONT]) < 0.1f*/)
	{
		// check if there is a minimum of place for all the poly
		if(m_SizeTree < (_CurrentNode + _CountSegment))
		{
			IncreaseSizeNodeKdtreeBuffer(10000 + _CountSegment);
		}

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_IS_LEAF = true;

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT = -1;	
		m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT = -1;	

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_COUNT_SEGMENT = _CountSegment;
		
		m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_SEGMENT = new int[_CountSegment];
		memcpy( m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_SEGMENT, _IdSegment, sizeof(int)*_CountSegment);

		// set the new id to set back
		++_CurrentNode;
	}
	else
	{
		m_NodeTree[_CurrentNode].m_KDTREE_NODE_IS_LEAF  = false;

		//find the correct split axe X or Z
		float l_TempValueSplit = 0.0f;

		int l_CountSegmentOnX = 0; 
		int l_CountSegmentOnZ = 0; 
		
		// get the middle of the aabb 
		float l_X_MiddleAABB = (l_TempAABB[KDTREE_X_AXIS*2] + l_TempAABB[KDTREE_X_AXIS*2+1])*0.5f;
		float l_Z_MiddleAABB = (l_TempAABB[KDTREE_Z_AXIS*2] + l_TempAABB[KDTREE_Z_AXIS*2+1])*0.5f;

		for(int i=0; i<_CountSegment; ++i)
		{
			if(segment_list[_IdSegment[i]]->bounding_box.GetCenter().x > l_X_MiddleAABB)
				++l_CountSegmentOnX;
			else
				--l_CountSegmentOnX;
			
			if(segment_list[_IdSegment[i]]->bounding_box.GetCenter().z > l_Z_MiddleAABB)
				++l_CountSegmentOnZ;
			else
				--l_CountSegmentOnZ;
		}

		//set new axis
		int l_NewAxis;
		if(Types::Abs(l_CountSegmentOnX) < Types::Abs(l_CountSegmentOnZ))
			l_NewAxis = KDTREE_X_AXIS;
		else
			l_NewAxis = KDTREE_Z_AXIS;

		// problem , we need absolutly leaf with some path inside, bad split function, so patch it
		if((l_NewAxis == KDTREE_X_AXIS && _CountSegment == Types::Abs(l_CountSegmentOnX)) || (l_NewAxis == KDTREE_Z_AXIS && _CountSegment == Types::Abs(l_CountSegmentOnZ)))
		{
			CreateNodeKdtree(_CurrentNode, _IdSegment, _CountSegment, _CurrentDepth, true );
			return;
		}

		// axis check with the length and width
		float diff_axis_aabb = (l_TempAABB[KDTREE_X_AXIS*2+1] - l_TempAABB[KDTREE_X_AXIS*2]) / (l_TempAABB[KDTREE_Z_AXIS*2+1] - l_TempAABB[KDTREE_Z_AXIS*2]);
		if(diff_axis_aabb > 1.5)
			l_NewAxis = KDTREE_X_AXIS;
		if(diff_axis_aabb < 0.66)
			l_NewAxis = KDTREE_Z_AXIS;
		
		m_NodeTree[_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT = l_NewAxis;

		// get the middle of the aabb 
		float l_MiddleAABB = (l_TempAABB[l_NewAxis*2] + l_TempAABB[l_NewAxis*2+1])*0.5f;
		l_TempValueSplit = l_MiddleAABB;

		m_NodeTree[_CurrentNode].m_KDTREE_NODE_VALUE_SPLIT = l_TempValueSplit;
		
		// create the 2 childs

		// create the 2 child list
		int l_IdInBigArray;

		// left node
		{
			int l_NewIdChildLeft = _CurrentNode + 1;
			m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_LEFT = l_NewIdChildLeft;		

			// set the new aabb
			float * l_TempAABBLeftChild = m_NodeTree[l_NewIdChildLeft].m_KDTREE_NODE_AABB;
		
			l_TempAABB =  m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB;
			memcpy(l_TempAABBLeftChild, l_TempAABB, sizeof(float)*6);
			l_TempAABBLeftChild[l_NewAxis*2+1] = l_TempValueSplit;

			int *l_IdLeftSegmentList = new int [_CountSegment];
			int l_IdLeftCount = 0;

			for(int i=0; i<_CountSegment; ++i)
			{
				bool l_Include = false;
				switch(l_NewAxis)
				{
				case KDTREE_X_AXIS:
					if(segment_list[_IdSegment[i]]->a.x <= l_TempValueSplit ||
						segment_list[_IdSegment[i]]->b.x <= l_TempValueSplit)
						l_Include = true;
					break;
				case KDTREE_Z_AXIS:
					if(segment_list[_IdSegment[i]]->a.z <= l_TempValueSplit ||
						segment_list[_IdSegment[i]]->b.z <= l_TempValueSplit)
						l_Include = true;
					break;
				}

				if(l_Include)
				{
					l_IdLeftSegmentList[l_IdLeftCount] = _IdSegment[i];

					++l_IdLeftCount;
				}
			}

			// copy the strict minimum, not good, because it's fragment memory, but it's just for the creation
			{
				int* l_tempCopy = new int[l_IdLeftCount];
				memcpy(l_tempCopy, l_IdLeftSegmentList, sizeof(int)*l_IdLeftCount);

				delete []l_IdLeftSegmentList;
				l_IdLeftSegmentList = l_tempCopy;
			}

			CreateNodeKdtree(l_NewIdChildLeft, l_IdLeftSegmentList, l_IdLeftCount, _CurrentDepth+1);

			l_IdInBigArray = l_NewIdChildLeft;

			delete []l_IdLeftSegmentList;
		}

		// right node
		{
			if(m_SizeTree < l_IdInBigArray + 3)
				IncreaseSizeNodeKdtreeBuffer(10000);

			int l_NewIdChildRight = l_IdInBigArray;
			m_NodeTree[_CurrentNode].m_KDTREE_NODE_ID_CHILD_RIGHT = l_NewIdChildRight;	

			// set the new aabb
			float * l_TempAABBRightChild = m_NodeTree[l_NewIdChildRight].m_KDTREE_NODE_AABB;
			
			l_TempAABB =  m_NodeTree[_CurrentNode].m_KDTREE_NODE_AABB;
			memcpy(l_TempAABBRightChild, l_TempAABB, sizeof(float)*6);
			l_TempAABBRightChild[l_NewAxis*2] = l_TempValueSplit;

			int *l_IdRightSegmentList = new int [_CountSegment];
			int l_IdRightCount = 0;
			
			for(int i=0; i<_CountSegment; ++i)
			{
				bool l_Include = false;
				switch(l_NewAxis)
				{
				case KDTREE_X_AXIS:
					if(segment_list[_IdSegment[i]]->a.x >= l_TempValueSplit ||
						segment_list[_IdSegment[i]]->b.x >= l_TempValueSplit)
						l_Include = true;
					break;
				case KDTREE_Z_AXIS:
					if(segment_list[_IdSegment[i]]->a.z >= l_TempValueSplit ||
						segment_list[_IdSegment[i]]->b.z >= l_TempValueSplit)
						l_Include = true;
					break;
				}

				if(l_Include)
				{
					l_IdRightSegmentList[l_IdRightCount] = _IdSegment[i];

					++l_IdRightCount;
				}
			}

			// copy the strict minimum, not good, because it's fragment memory, but it's just for the creation
			{
				int* l_tempCopy = new int[l_IdRightCount];
				memcpy(l_tempCopy, l_IdRightSegmentList, sizeof(int)*l_IdRightCount);

				delete []l_IdRightSegmentList;
				l_IdRightSegmentList = l_tempCopy;
			}

			CreateNodeKdtree(l_NewIdChildRight, l_IdRightSegmentList, l_IdRightCount, _CurrentDepth+1);

			//set the new id for the next node in the stack
			_CurrentNode = l_NewIdChildRight;

			delete []l_IdRightSegmentList;
		}
	}
}

//--------------------------------------------------------------------------------
void PathKdtree::BuildQuadtree()
//--------------------------------------------------------------------------------
{
	if(segment_list.GetCount() <= 0)
		return;
		
	//very not powerful kdtree construction

	m_SizeTree = segment_list.GetCount()*4;
	m_NodeTree = new KDTreeNode[m_SizeTree];

	int m_CurrentNode = 0;

	m_NodeTree[m_CurrentNode].m_KDTREE_NODE_TYPE_SPLIT = KDTREE_X_AXIS;

	// find the big bounding box
	MinMax max_min_max = segment_list[0]->GetBoundingBox();
	ArrayListForeachPtr(nMSegment*, segment, segment_list)
	{
		max_min_max.Grow(segment->GetBoundingBox());
	}
	max_min_max.mn.y -= 100.0f;
	max_min_max.mx.y += 100.0f;

	float * l_TempAABB = m_NodeTree[m_CurrentNode].m_KDTREE_NODE_AABB;

	l_TempAABB[KDTREE_SIDE_LEFT] = max_min_max.mn.x;
	l_TempAABB[KDTREE_SIDE_BOTTOM] = max_min_max.mn.y;
	l_TempAABB[KDTREE_SIDE_BACK] = max_min_max.mn.z;
	l_TempAABB[KDTREE_SIDE_RIGHT] = max_min_max.mx.x;
	l_TempAABB[KDTREE_SIDE_TOP] = max_min_max.mx.y;
	l_TempAABB[KDTREE_SIDE_FRONT] = max_min_max.mx.z;		


	// to build the kdtree: id of the poly
	int* l_IdSegment = new int[segment_list.GetCount()];
	for(uint i=0; i<segment_list.GetCount(); ++i)
		l_IdSegment[i] = i;

	CreateNodeKdtree(m_CurrentNode, l_IdSegment, segment_list.GetCount(), 0);

	_safe_delete_array(l_IdSegment);
}

//-------------------------------------------------------------------
bool				PathKdtree::AddSegment(nMSegment* segment)
//-------------------------------------------------------------------
{		
	segment_list.Add(segment);

	return true;
}

//------------------------------------------------------------------------------------------
bool				PathKdtree::AddSegment(SharedArrayList<nMSegment*> _segment_list)
//------------------------------------------------------------------------------------------
{	
	ArrayListForeachPtr(nMSegment*, segment, _segment_list)
		segment_list.Add(segment);
	
	return true;
}
//---------------------------------------------------
bool	PathKdtree::InsideKdTree(const Vector4 &s)
//---------------------------------------------------
{
	if(m_NodeTree[0].m_KDTREE_NODE_AABB[0] <= s.x && s.x <= m_NodeTree[0].m_KDTREE_NODE_AABB[1] && 
	   m_NodeTree[0].m_KDTREE_NODE_AABB[2] <= s.y && s.y <= m_NodeTree[0].m_KDTREE_NODE_AABB[3] && 
	   m_NodeTree[0].m_KDTREE_NODE_AABB[4] <= s.z && s.z <= m_NodeTree[0].m_KDTREE_NODE_AABB[5] )
		return true;
	else
		return false;
}

//-----------------------------------------
void				PathKdtree::Free()
//-----------------------------------------
{
	_safe_delete_array(m_NodeTree);
	m_count_bih = 0;
}

PathKdtree::PathKdtree()
{
	m_NodeTree = NULL;
	m_count_bih = 0;
}