/*********************************************************************

Name:		Raytracer
Author:		Kamil Szatkowski "Netrix" (C)
Website:	http://www.netrix.org.pl/

*********************************************************************/
#include "KDTree.hpp"
#include "TriBoxIntersection.hpp"
using namespace NIne;
#include "NLib/nLogger.hpp"
#include <stdio.h>
#pragma warning(disable: 4996)

namespace
{
	const NIne::NUint32 MAX_DEPTH = 20;

	const NIne::NUint32 DATA_VECTOR_REALLOC_SIZE = 4;
	const NIne::NUint32 ALIGNMENT = 32;
	
	const float BOX_ENLARGEMENT = 1.02f;

	typedef NIne::NAlignedDynamicTable<FaceData> FaceDataArray;
}

void KDTree::buildTree(Scene *pScene, bool bResetCache)
{
	m_pScene = pScene;
	m_uObjectListPool = NMAX_SIZE_T;

	// at build time pointers are indices
	KDTreeNodeVector nodes;

#ifndef _WIN64
	printf("Loading kdtree from file...");
	if(bResetCache || !loadTree("kdtree.tmp", &nodes))
	{
		if(bResetCache)	{ printf(" RESET\n"); }
		else			{ printf(" FAILED\n"); }
#endif
		printf("Building kdtree...\n");

		m_dvObjectLists.Create(DATA_VECTOR_REALLOC_SIZE, ALIGNMENT, DATA_VECTOR_REALLOC_SIZE);
		nodes.Create(DATA_VECTOR_REALLOC_SIZE, DATA_VECTOR_REALLOC_SIZE);

		// Creating face list
		FaceDataArray& faceData = pScene->getFaceData();
		for(NSize_t uIndex = 0; uIndex < faceData.GetSize(); ++uIndex)
		{
			FacesListNode fln = { &faceData[uIndex], (FacesListNode*)(uIndex + 1) };
			m_dvObjectLists.PushElement(fln);
		}
		m_dvObjectLists[m_dvObjectLists.GetSize() - 1].pNext = (FacesListNode*)(NMAX_SIZE_T);
		
		// Setting root
		KDTreeNode root;
		root.bSetPrimitiveList(0);
		nodes.PushElement(root);

		// Dividing tree
		NDynamicTable<SplitCandidate> aSCPool;
		aSCPool.Create(faceData.GetSize() * 2 + 8);
		for(NSize_t uIndex = 1; uIndex < aSCPool.GetSize(); ++uIndex)
		{
			aSCPool[uIndex - 1].pNext = &aSCPool[uIndex];
		}
		aSCPool[aSCPool.GetSize() - 1].pNext = null;

		BoxAABB aabb = { pScene->getDomainMin(), pScene->getDomainMax() };
		subdivide(&nodes, 0, aSCPool.GetBuffer(), aabb, 0, faceData.GetSize());

		aSCPool.Release();

#ifndef _WIN64
		// Saving to file
		saveTree("kdtree.tmp", nodes);
	}
	else
	{
		printf(" OK\n");
	}
#endif
	
	// At the end indices are changed to pointers (after assigning to aligned array)
	m_aKDNodes.Create(nodes.GetSize(), ALIGNMENT);
	for(NSize_t uIndex = 0; uIndex < nodes.GetSize(); ++uIndex)
	{
		m_aKDNodes[uIndex] = nodes[uIndex];
		
		if(nodes[uIndex].isLeaf()) 
		{
			NSize_t uPrim = nodes[uIndex].bGetPrimitiveList();
			m_aKDNodes[uIndex].setPrimitiveList(uPrim != NMAX_SIZE_T ? &m_dvObjectLists[uPrim] : null); 
		}
		else					   
		{
			m_aKDNodes[uIndex].setChildren(&m_aKDNodes[nodes[uIndex].bGetLeftChild()]); 
		}
	}

	for(NSize_t uIndex = 0; uIndex < m_dvObjectLists.GetSize(); ++uIndex)
	{
		m_dvObjectLists[uIndex].pNext = ((NSize_t)m_dvObjectLists[uIndex].pNext != NMAX_SIZE_T) ? &m_dvObjectLists[(NSize_t)m_dvObjectLists[uIndex].pNext] : null;
	}

	nodes.Release();

	m_pRoot = m_aKDNodes.GetBuffer();

	// Stack initialization
	m_aKDStack.Create(64, ALIGNMENT);
}
/********************************************************/
void KDTree::saveTree(const char* szFile, const KDTreeNodeVector& nodes)
{
	FILE* pFile = fopen(szFile, "w");

	fprintf(pFile, "SceneFile %s\n", m_pScene->getSceneName());
	fprintf(pFile, "KDNodes %u\n", nodes.GetSize());

	for(NUint32 i = 0; i < nodes.GetSize(); ++i)
	{
		fprintf(pFile, "%u %f\n", nodes[i].m_uData, nodes[i].m_fSplit);
	}
	fprintf(pFile, "\n");

	fprintf(pFile, "ObjectList %u\n", m_dvObjectLists.GetSize());

	for(NUint32 i = 0; i < m_dvObjectLists.GetSize(); ++i)
	{
		fprintf(pFile, "%u %u\n", m_dvObjectLists[i].pFace - m_pScene->getFaceData().GetBuffer(), m_dvObjectLists[i].pNext);
	}

	fclose(pFile);
}
/********************************************************/
bool KDTree::loadTree(const char* szFile, KDTreeNodeVector* pNodes)
{
	KDTreeNodeVector& nodes = *pNodes;

	FILE* pFile = fopen(szFile, "r");
	if(pFile == null) { return false; }

	char szName[255];
	fscanf(pFile, "SceneFile %s\n", szName);
	if(strcmp(szName, m_pScene->getSceneName()) != 0)	{ return false; }

	NUint32 uSize;
	fscanf(pFile, "KDNodes %u\n", &uSize);
	nodes.Create(uSize);
	nodes.Resize(uSize);

	for(NSize_t i = 0; i < nodes.GetSize(); ++i)
	{
		fscanf(pFile, "%u %f\n", &nodes[i].m_uData, &nodes[i].m_fSplit);
	}
	fscanf(pFile, "\n");

	fscanf(pFile, "ObjectList %u\n", &uSize);
	m_dvObjectLists.Create(uSize, ALIGNMENT);
	m_dvObjectLists.Resize(uSize);

	for(NSize_t i = 0; i < m_dvObjectLists.GetSize(); ++i)
	{
		fscanf(pFile, "%u %u\n", &m_dvObjectLists[i].pFace, &m_dvObjectLists[i].pNext);
		m_dvObjectLists[i].pFace = m_pScene->getFaceData().GetBuffer() + (NSize_t)m_dvObjectLists[i].pFace;
	}

	fclose(pFile);
	return true;
}
/********************************************************/
void KDTree::release()
{
	m_pRoot = null;
	m_aKDNodes.Release();
	m_aKDStack.Release();
	m_dvObjectLists.Release();
}
/********************************************************/
void KDTree::subdivide(KDTreeNodeVector* pKdNodeVector, NSize_t uNode, SplitCandidate* pSCPool, const BoxAABB& aabb, NUint32 uDepth, NSize_t uFaces)
{
	KDTreeNode* pNode = pKdNodeVector->GetBuffer() + uNode;

	// Determine split axis
	NMVector3f v3Size = aabb.vMax - aabb.vMin;

	// Default is Z_AXIS
	if((v3Size.x >= v3Size.y) && (v3Size.x >= v3Size.z))
	{
		pNode->setAxis(X_AXIS);
	}
	else if((v3Size.y >= v3Size.x) && (v3Size.y >= v3Size.z))
	{
		pNode->setAxis(Y_AXIS);
	}

	NUint32 uAxis = pNode->getAxis();

	// Make a list of the split position candidates
	NSize_t uPrimList = pNode->bGetPrimitiveList();
	float p;
	float fMinPos = aabb.vMin.v[uAxis];
	float fMaxPos = aabb.vMax.v[uAxis];

	NDynamicTable<bool> abRight;			abRight.Create(uFaces);
	NDynamicTable<float> afLeft;			afLeft.Create(uFaces);
	NDynamicTable<float> afRight;			afRight.Create(uFaces);
	NDynamicTable<FaceData*> aFaces;		aFaces.Create(uFaces);

	SplitCandidate *pList = null;

	NUint32 uAIDX = 0;
	while(uPrimList != NMAX_SIZE_T)
	{
		const FaceData* pFace = aFaces[uAIDX] = m_dvObjectLists[uPrimList].pFace;
		abRight[uAIDX] = true;
		calculateRange(pFace, uAxis, afLeft.GetBuffer() + uAIDX, afRight.GetBuffer() + uAIDX);
		uAIDX++;
		
		for(int i = 0; i < 3; i++)
		{
			p = pFace->pVertices[i]->v3Position.v[uAxis];

			if((p >= fMinPos) && (p <= fMaxPos))
			{
				insertSplitPos(&pSCPool, &pList, p);
			}
		}

		uPrimList = (NSize_t)m_dvObjectLists[uPrimList].pNext;
	}

	// Determining primitive counts for each splits
	BoxAABB b1, b2,
			b3 = { aabb.vMin, aabb.vMax },
			b4 = { aabb.vMin, aabb.vMax };

	SplitCandidate* pListWalk = pList;
	float b3Min = b3.vMin.v[uAxis];
	float b4Max = b4.vMax.v[uAxis];

	while(pListWalk != null)
	{
		b4.vMin.v[uAxis] = pListWalk->fSplitPosition;
		b3.vMax.v[uAxis] = pListWalk->fSplitPosition;

		float b3Max = b3.vMax.v[uAxis];
		float b4Min = b4.vMin.v[uAxis];

		for(NUint32 i = 0; i < uFaces; ++i)
		{
			if (abRight[i])
			{
				const FaceData* pFace = aFaces[i];

				if((afLeft[i] <= b3Max) && (afRight[i] >= b3Min))
				{
					if(TriBoxIntersection((b3.vMin + b3.vMax) * 0.5f, 
										  (b3.vMax - b3.vMin) * 0.5f * BOX_ENLARGEMENT, 
										  pFace->pVertices[0]->v3Position, 
										  pFace->pVertices[1]->v3Position, 
										  pFace->pVertices[2]->v3Position))
					{
						pListWalk->uLNCount++;
					}
				}
				if((afLeft[i] <= b4Max) && (afRight[i] >= b4Min))
				{
					if(TriBoxIntersection((b4.vMin + b4.vMax) * 0.5f, 
										  (b4.vMax - b4.vMin) * 0.5f * BOX_ENLARGEMENT, 
										  pFace->pVertices[0]->v3Position,
										  pFace->pVertices[1]->v3Position, 
										  pFace->pVertices[2]->v3Position))
					{
						pListWalk->uRNCount++;
					}
					else
					{
						abRight[i] = false;
					}
				}
			}
			else
			{
				pListWalk->uLNCount++;
			}
		}

		pListWalk = pListWalk->pNext;
	}

	abRight.Release();

	// Calculate surface area for current node
	float fSAV = 0.5f / (v3Size.x * v3Size.z + v3Size.x * v3Size.y + v3Size.z * v3Size.y);
	// calculate cost for not splitting
	float fCLeaf = uFaces * 1.0f;
	// determine optimal split plane position
	pListWalk = pList;
	float fLowCost = 100000.0f;
	float fBestPos = 0.0f;

	while(pListWalk != null)
	{
		// Calculate child node extends
		b4.vMin.v[uAxis] = pListWalk->fSplitPosition;
		b3.vMax.v[uAxis] = pListWalk->fSplitPosition;

		// Calculate child node cost
		float w = b3.vMax.x - b3.vMin.x,
			  h = b3.vMax.y - b3.vMin.y, 
			  d = b3.vMax.z - b3.vMin.z;
		float SA1 = 2 * (w * d + w * h + d * h);
		w = b4.vMax.x - b4.vMin.x;
		h = b4.vMax.y - b4.vMin.y;
		d = b4.vMax.z - b4.vMin.z;
		float SA2 = 2 * (w * d + w * h + d * h);
		float fSplitcost = 0.3f + 1.0f * (SA1 * fSAV * pListWalk->uLNCount + SA2 * fSAV * pListWalk->uRNCount);

		// Update best cost tracking variables
		if (fSplitcost < fLowCost)
		{
			fLowCost = fSplitcost;
			fBestPos = pListWalk->fSplitPosition;
			b1 = b3;
			b2 = b4;
		}

		pListWalk = pListWalk->pNext;
	}

	if(fLowCost > fCLeaf)
	{
		if(pList != null)
		{
			SplitCandidate* pSplit = pList;
			while(pSplit->pNext != null)
			{
				pSplit = pSplit->pNext;
			}
			pSplit->pNext = pSCPool;
			pSCPool = pList;
		}

		afLeft.Release();
		afRight.Release();
		aFaces.Release();
		return;
	}
	
	pNode->setSplit(fBestPos);

	// Construct child nodes
	KDTreeNode nodes[2];
	NUint32 uNLCount = 0, uNRCount = 0;

	// Assign primitives to both sides
	float b1p1 = b1.vMin.v[uAxis];
	float b1p2 = b1.vMax.v[uAxis];
	float b2p1 = b2.vMin.v[uAxis];
	float b2p2 = b2.vMax.v[uAxis];

	for(NUint32 i = 0; i < uFaces; i++)
	{
		FaceData* pFace = aFaces[i];
		bool bAdded = false;

 		if (afLeft[i] <= b1p2 && afRight[i] >= b1p1)
		{
			if(TriBoxIntersection((b1.vMin + b1.vMax) * 0.5f, 
								  (b1.vMax - b1.vMin) * 0.5f * BOX_ENLARGEMENT, // 0.51f dac
								  pFace->pVertices[0]->v3Position,
								  pFace->pVertices[1]->v3Position, 
								  pFace->pVertices[2]->v3Position))
			{
				NSize_t uPrim = nodes[0].bGetPrimitiveList();
				NSize_t uNew = getNewObjectListNode();
				m_dvObjectLists[uNew].pFace = pFace;
				m_dvObjectLists[uNew].pNext = (FacesListNode*)uPrim;
				nodes[0].bSetPrimitiveList(uNew);
				uNLCount++;
				bAdded = true;
			}
		}
		if (afLeft[i] <= b2p2 && afRight[i] >= b2p1) 
		{
			if(TriBoxIntersection((b2.vMin + b2.vMax) * 0.5f, 
								  (b2.vMax - b2.vMin) * 0.5f * BOX_ENLARGEMENT, // 0.51f dac
								  pFace->pVertices[0]->v3Position, 
								  pFace->pVertices[1]->v3Position, 
								  pFace->pVertices[2]->v3Position))
			{
				NSize_t uPrim = nodes[1].bGetPrimitiveList();
				NSize_t uNew = getNewObjectListNode();
				m_dvObjectLists[uNew].pFace = pFace;
				m_dvObjectLists[uNew].pNext = (FacesListNode*)uPrim;
				nodes[1].bSetPrimitiveList(uNew);
				uNRCount++;
				bAdded = true;
			}
		}
	}

	// Making list of primitives for current node free
	freeObjectListNodes(pNode->bGetPrimitiveList());

	afLeft.Release();
	afRight.Release();
	aFaces.Release();

	NSize_t uIndex = pKdNodeVector->GetSize();
	pNode->bSetChildren(uIndex);
	pNode->setLeaf(false);

	pKdNodeVector->PushElement(nodes[0]);
	pKdNodeVector->PushElement(nodes[1]);

	// Tiding SCPool
	SplitCandidate* pSplit = pList;
	while(pSplit->pNext != null)
	{
		pSplit = pSplit->pNext;
	}
	pSplit->pNext = pSCPool;
	pSCPool = pList;
	
	if (uDepth < MAX_DEPTH)
	{
		if (uNLCount > 2) { subdivide(pKdNodeVector, uIndex, pSCPool, b1, uDepth + 1, uNLCount); }
		if (uNRCount > 2) { subdivide(pKdNodeVector, uIndex + 1, pSCPool, b2, uDepth + 1, uNRCount); };
	}
}
/********************************************************/
void KDTree::calculateRange(const FaceData* pFace, NIne::NUint32 uAxis, float* a, float* b)
{
	const Vertex* pVert = pFace->pVertices[0];
	*a = pVert->v3Position.v[uAxis];
	*b = pVert->v3Position.v[uAxis];

	for (int i = 1; i < 3; i++)
	{
		pVert = pFace->pVertices[i];
		*a = min(*a, pVert->v3Position.v[uAxis]);
		*b = max(*b, pVert->v3Position.v[uAxis]);
	}
}
/********************************************************/
void KDTree::insertSplitPos(SplitCandidate** pSCPool, SplitCandidate** pSCList, float fSplitPos)
{
	// Insert a split position candidate in the list if unique
	SplitCandidate* pEntry = *pSCPool;
	*pSCPool = (*pSCPool)->pNext;
	pEntry->pNext = null;
	pEntry->fSplitPosition = fSplitPos;
	pEntry->uLNCount = 0;
	pEntry->uRNCount = 0;

	if(!*pSCList)
	{
		*pSCList = pEntry;
	}
	else
	{
		if(fSplitPos < (*pSCList)->fSplitPosition)
		{
			pEntry->pNext = *pSCList;
			*pSCList = pEntry;
		}
		else if(fSplitPos == (*pSCList)->fSplitPosition)
		{
			pEntry->pNext = *pSCPool;
			*pSCPool = pEntry;
		}
		else
		{
			SplitCandidate* pList = *pSCList;

			while(pList->pNext != null && fSplitPos >= pList->pNext->fSplitPosition)
			{
				if(fSplitPos == pList->pNext->fSplitPosition)
				{
					pEntry->pNext = *pSCPool;
					*pSCPool = pEntry;
					return;
				}
				pList = pList->pNext;
			}

			pEntry->pNext = pList->pNext;
			pList->pNext = pEntry;
		}
	}
}
/********************************************************/
NSize_t KDTree::getNewObjectListNode()
{
	NSize_t uRetVal;
	if(m_uObjectListPool == NMAX_SIZE_T)
	{
		FacesListNode node;
		node.pNext = (FacesListNode*)(NMAX_SIZE_T);
		uRetVal = m_dvObjectLists.GetSize();
		m_dvObjectLists.PushElement(node);
	}
	else
	{
		uRetVal = m_uObjectListPool;
		m_uObjectListPool = (NSize_t)m_dvObjectLists[m_uObjectListPool].pNext;
		m_dvObjectLists[uRetVal].pNext = (FacesListNode*)(NMAX_SIZE_T);
	}

	return uRetVal;
}
/********************************************************/
void KDTree::freeObjectListNodes(NSize_t uIndex)
{
	NSize_t uTemp = m_uObjectListPool;
	m_uObjectListPool = uIndex;

	NSize_t uActual = uIndex;
	while(m_dvObjectLists[uActual].pNext != (FacesListNode*)(NMAX_SIZE_T))
	{
		uActual = (NSize_t)m_dvObjectLists[uActual].pNext;
	}

	m_dvObjectLists[uActual].pNext = (FacesListNode*)uTemp;
}
/********************************************************/
const NUint32 g_modulo[] = { 0, 1, 2, 0, 1 };

const FaceData* KDTree::findNearestFace(const NIne::NMVector3f& vOrigin, const NIne::NMVector3f& vDirection, float fMaxDistance, RayTestOutput* pRTO)
{
	float fTNear = 0, 
		  fTFar = fMaxDistance, // mozna to przekazac w prto
		  t;

	const NMVector3f& v3Min = m_pScene->getDomainMin();
	const NMVector3f& v3Max = m_pScene->getDomainMax();

	// Testing if ray is directing into area
	for( int i = 0; i < 3; i++)
	{
		if(vDirection.v[i] < 0.0f) 
		{
			if (vOrigin.v[i] < v3Min.v[i]) { return null; }
		}
		else if(vOrigin.v[i] > v3Max.v[i]) { return null; }
	}

	// Clipping ray to box
	for(int i = 0; i < 3; i++)
	{
		float fPos = vOrigin.v[i] + fTFar * vDirection.v[i];

		if(vDirection.v[i] < 0.0f)
		{
			// Clip end point
			if(fPos < v3Min.v[i]) 
			{
				//fTFar = fTNear + (fTFar - fTNear) * ((vOrigin.v[i] - v3Min.v[i]) / (vOrigin.v[i] - fPos));
				fTFar *= ((vOrigin.v[i] - v3Min.v[i]) / (vOrigin.v[i] - fPos));
			}
			// clip start point
			if(vOrigin.v[i] > v3Max.v[i]) 
			{
				fTNear += (fTFar - fTNear) * ((v3Max.v[i] - vOrigin.v[i]) / (fTFar * vDirection.v[i]));
			}
		}
		else
		{
			// clip end point
			if (fPos > v3Max.v[i])
			{
				//fTFar = fTNear + (fTFar - fTNear) * ((v3Max.v[i] - vOrigin.v[i]) / (fPos - vOrigin.v[i]));
				fTFar *= ((v3Max.v[i] - vOrigin.v[i]) / (fPos - vOrigin.v[i]));
			}
			// clip start point
			if (vOrigin.v[i] < v3Min.v[i]) 
			{
				fTNear += (fTFar - fTNear) * ((v3Min.v[i] - vOrigin.v[i]) / (fTFar * vDirection.v[i]));
			}
		}
		if (fTNear > fTFar) { return null; }
	}

	// Traverse initialization
	NUint32 uEntryPoint = 0,
			uExitPoint = 1;

	KDTreeNode* pFarChild,
			  * pCurrNode;

	pCurrNode = m_pRoot;
	m_aKDStack[uEntryPoint].t = fTNear;

	if(fTNear > 0.0f)	{ m_aKDStack[uEntryPoint].v3pb = vOrigin + vDirection * fTNear; }
	else				{ m_aKDStack[uEntryPoint].v3pb = vOrigin; }

	m_aKDStack[uExitPoint].t = fTFar;
	m_aKDStack[uExitPoint].v3pb = vOrigin + vDirection * fTFar;
	m_aKDStack[uExitPoint].node = 0;

	_NALIGN_16 RayTestOutput rto;

	// Traverse kd-tree
	while(pCurrNode != null)
	{
		while(!pCurrNode->isLeaf())
		{
			float fSplitPos = pCurrNode->getSplit();
			NUint32 uAxis = pCurrNode->getAxis();

			if(m_aKDStack[uEntryPoint].v3pb.v[uAxis] <= fSplitPos)
			{
				if(m_aKDStack[uExitPoint].v3pb.v[uAxis] < fSplitPos)	// <= - tak tu byly, ale by nie doszlo nizej, wiec poprawilem
				{
					pCurrNode = pCurrNode->getLeftChild();
					continue;
				}
				if(m_aKDStack[uExitPoint].v3pb.v[uAxis] == fSplitPos)
				{
					pCurrNode = pCurrNode->getRightChild();
					continue;
				}
				pCurrNode = pCurrNode->getLeftChild();
				pFarChild = pCurrNode + 1;
			}
			else
			{
				if(m_aKDStack[uExitPoint].v3pb.v[uAxis] > fSplitPos)
				{
					pCurrNode = pCurrNode->getRightChild();
					continue;
				}
				pFarChild = pCurrNode->getLeftChild();
				pCurrNode = pFarChild + 1;
			}

			t = (fSplitPos - vOrigin.v[uAxis]) / vDirection.v[uAxis];
			NUint32 uTemp = uExitPoint++;

			if(uExitPoint == uEntryPoint)	{ uExitPoint++; }

			m_aKDStack[uExitPoint].uPrev = uTemp;
			m_aKDStack[uExitPoint].t = t;
			m_aKDStack[uExitPoint].node = pFarChild;
			m_aKDStack[uExitPoint].v3pb.v[uAxis] = fSplitPos;
			
			NUint32 uNextAxis = g_modulo[uAxis + 1];
			NUint32 uPrevAxis = g_modulo[uAxis + 2];

			m_aKDStack[uExitPoint].v3pb.v[uNextAxis] = vOrigin.v[uNextAxis] + t * vDirection.v[uNextAxis];
			m_aKDStack[uExitPoint].v3pb.v[uPrevAxis] = vOrigin.v[uPrevAxis] + t * vDirection.v[uPrevAxis];
		}

		const FacesListNode* pFaceList = pCurrNode->getPrimitiveList();
		float fDist = m_aKDStack[uExitPoint].t;
		
		const FaceData* pRetFace = null;

		while(pFaceList != null)
		{
			const FaceData* pFace = pFaceList->pFace;

			if(m_pScene->testTriangle(pFace, vOrigin, vDirection, fDist, &rto))
			{
				pRetFace = pFace;
				*pRTO = rto;
				fMaxDistance = fDist = rto.t;
			}

			pFaceList = pFaceList->pNext;
		}

		if(pRetFace != null)
		{
			return pRetFace;
		}

		uEntryPoint = uExitPoint;
		pCurrNode = m_aKDStack[uExitPoint].node;
		uExitPoint = m_aKDStack[uEntryPoint].uPrev;
	}

	return null;
}

bool KDTree::walkthroughNodes(NUint32 uFace, KDTreeNode* pNode)
{
	if(pNode->isLeaf())
	{
		if(pNode->getPrimitiveList() != null && walkthroughFacesList(uFace, pNode->getPrimitiveList()))
		{
			printf("Node index: %d.\n", pNode - m_aKDNodes.GetBuffer());
			return true;
		}
	}
	else 
	{	
		bool bFirst = walkthroughNodes(uFace, pNode->getLeftChild());
		bool bSecond = walkthroughNodes(uFace, pNode->getRightChild());

		if(bFirst && bSecond)
		{
			printf("Node index: %d, with two children: %d & %d.\n", pNode - m_aKDNodes.GetBuffer(), pNode->getLeftChild() - m_aKDNodes.GetBuffer(), pNode->getRightChild() - m_aKDNodes.GetBuffer());
			return true;
		}
		else if(bFirst || bSecond)
		{
			printf("Node index: %d.\n", pNode - m_aKDNodes.GetBuffer());
			return true;
		}
	}

	return false;
}

bool KDTree::walkthroughFacesList(NUint32 uFace, const FacesListNode* pFaceListNode)
{
	if(pFaceListNode->pFace == &m_pScene->getFaceData()[uFace])
	{
		printf("-------\n");
		printf("Face list index: %d, found!\n", pFaceListNode - m_dvObjectLists.GetBuffer());
		return true;
	}
	else if(pFaceListNode->pNext != null)
	{
		if(walkthroughFacesList(uFace, pFaceListNode->pNext))
		{
			printf("Face list index: %d.\n", pFaceListNode - m_dvObjectLists.GetBuffer());
			return true;
		}
	}

	return false;
}
/********************************************************/
bool KDTree::findObstacle(const NMVector3f& vOrigin, const NMVector3f& vDirection, float fMaxDistance)
{
	float fTNear = NM_LOW_EPSILON_F, 
		  t;

	NUint32 uEntry = 0,
			uExit = 1;

	// Init traverse
	KDTreeNode* pFarChild,
			  * pCurrNode = m_pRoot;

	m_aKDStack[uEntry].t = fTNear;
	m_aKDStack[uEntry].v3pb = vOrigin;
	m_aKDStack[uExit].t = fMaxDistance;
	m_aKDStack[uExit].v3pb = vOrigin + vDirection * fMaxDistance;
	m_aKDStack[uExit].node = 0;

	_NALIGN_16 RayTestOutput rto;

	// Traversing loop
	while (pCurrNode)
	{
		while (!pCurrNode->isLeaf())
		{
			float fSplit = pCurrNode->getSplit();
			NUint32 uAxis = pCurrNode->getAxis();

			if(m_aKDStack[uEntry].v3pb.v[uAxis] <= fSplit)
			{
				if(m_aKDStack[uExit].v3pb.v[uAxis] < fSplit)
				{
					pCurrNode = pCurrNode->getLeftChild();
					continue;
				}
				if(m_aKDStack[uExit].v3pb.v[uAxis] == fSplit)
				{
					pCurrNode = pCurrNode->getRightChild();
					continue;
				}
				pCurrNode = pCurrNode->getLeftChild();
				pFarChild = pCurrNode + 1;	// getRightChild()
			}
			else
			{
				if(m_aKDStack[uExit].v3pb.v[uAxis] > fSplit)
				{
					pCurrNode = pCurrNode->getRightChild();
					continue;
				}
				pFarChild = pCurrNode->getLeftChild();
				pCurrNode = pFarChild + 1;	// getRightChild()
			}

			t = (fSplit - vOrigin.v[uAxis]) / vDirection.v[uAxis];
			NUint32 uTemp = uExit;

			if(++uExit == uEntry)	{ uExit++; }

			m_aKDStack[uExit].uPrev = uTemp;
			m_aKDStack[uExit].t = t;
			m_aKDStack[uExit].node = pFarChild;
			m_aKDStack[uExit].v3pb.v[uAxis] = fSplit;

			NUint32 uNextAxis = g_modulo[uAxis + 1];
			NUint32 uPrevAxis = g_modulo[uAxis + 2];
			m_aKDStack[uExit].v3pb.v[uNextAxis] = vOrigin.v[uNextAxis] + t * vDirection.v[uNextAxis];
			m_aKDStack[uExit].v3pb.v[uPrevAxis] = vOrigin.v[uPrevAxis] + t * vDirection.v[uPrevAxis];
		}

		const FacesListNode* pList = pCurrNode->getPrimitiveList();

		while(pList != null)
		{
			if(m_pScene->testTriangle(pList->pFace, vOrigin, vDirection, fMaxDistance, &rto))
			{
				return true;	// nie uwzglednia polcieni
			}
			pList = pList->pNext;
		}

		uEntry = uExit;
		pCurrNode = m_aKDStack[uExit].node;
		uExit = m_aKDStack[uEntry].uPrev;
	}
	return false;
}
