#include "DynOctTree.h"

DynOctTree::DynOctTree(const float CellSize, const float Epsilon)
{
	m_CellSize = CellSize;
	m_Epsilon = Epsilon;

//	m_MinX = m_MinY = m_MinZ = INT_MAX; 
//	m_MaxX = m_MaxY = m_MaxZ = -INT_MAX; 

//	m_Levels = 0;
}

DynOctTree::~DynOctTree(void)
{
	ListElement<hash_map<LONGLONG, DynOctTreeNode*>>* lvl = m_NodeIndex.GetHead();
	while(lvl)
	{
		for (hash_map<LONGLONG, DynOctTreeNode*>::const_iterator it = lvl->GetInfo()->begin(); it != lvl->GetInfo()->end(); it++)
		{
			delete it->second;
		}
		lvl = lvl->GetNext();
	}

	m_NodeIndex.Clear();
}

void DynOctTree::AddLevel(void)
{
	m_NodeIndex.AddTail(new hash_map<LONGLONG, DynOctTreeNode*>, m_NodeIndex.GetElementCount());
	//build fathers for all old last level nodes
	
	hash_map<LONGLONG, DynOctTreeNode*>* lastlevel = m_NodeIndex.GetTail()->GetPrev() ? m_NodeIndex.GetTail()->GetPrev()->GetInfo() : NULL;

	if (lastlevel && lastlevel->size())
	{
		for (hash_map<LONGLONG, DynOctTreeNode*>::const_iterator it = lastlevel->begin(); it != lastlevel->end(); it++)
		{
			DynOctTreeNode* n = it->second;

			if (n)
			{
				n->pFather = AcquireCellNode(n->CellX/2, n->CellY/2, n->CellZ/2, n->Level+1);

				int sx = n->CellX - (n->CellX / 2 * 2);
				int sy = n->CellY - (n->CellY / 2 * 2);
				int sz = n->CellZ - (n->CellZ / 2 * 2);

				n->pFather->m_Childs.SetN(n, sx + sy * 2 + sz * 4);
			}
		}
	}
}

DynOctTreeNode* DynOctTree::AcquireCellNode(const int cx, const int cy, const int cz, const int l)
{
	/*const int s = 1 << l;

	if (cx * s < m_MinX)
		m_MinX = cx * s;
	if ((cx + 1) * s > m_MaxX)
		m_MaxX = (cx + 1) * s;

	if (cy * s < m_MinY)
		m_MinY = cy * s;
	if ((cy + 1) * s > m_MaxY)
		m_MaxY = (cy + 1) * s;

	if (cz * s < m_MinZ)
		m_MinZ = cz * s;
	if ((cz + 1) * s > m_MaxZ)
		m_MaxZ = (cz + 1) * s;

	int e = _max(_max(m_MaxX - m_MinX, m_MaxY - m_MinY), m_MaxZ - m_MinZ);
	
	if (e > 0)
		m_Levels = (int)(logn(2.f, (float)e) + 1);
	else
		m_Levels = 1;*/

	while (m_NodeIndex.GetElementCount() <= (unsigned)l)
	{
		AddLevel();
	}

	LONGLONG key = BuildNodeKey(cx, cy, cz);
	DynOctTreeNode* n = (*(m_NodeIndex[l]->GetInfo()))[key];
	if (!n)
	{
		//build father if there already is at least one node in this level; operator [] already allocates this element, therefore > 1
		DynOctTreeNode* father = m_NodeIndex[l]->GetInfo()->size() > 1 || l+1 < m_NodeIndex.GetElementCount() ? AcquireCellNode(cx/2, cy/2, cz/2, l+1) : NULL;

		n = new DynOctTreeNode(this, father, cx, cy, cz, l);
		hash_map<LONGLONG, DynOctTreeNode*>* nodes = m_NodeIndex[l]->GetInfo();
		(*nodes)[key] = n;

		if (father)
		{
			int sx = cx - (cx / 2 * 2);
			int sy = cy - (cy / 2 * 2);
			int sz = cz - (cz / 2 * 2);

			father->m_Childs.SetN(n, sx + sy * 2 + sz * 4);
		}
		else
		{
			m_pRoot = n;
		}
	}

	return n;
}

void DynOctTree::FindSuitableCell(SceneObject* const obj, int& cx, int& cy, int& cz, int& l)
{
	AABB objbound = obj->GetBound().BuildAABB();
	DXVector3 pos = objbound.GetCenter() / m_CellSize;

	cx = pos.x;
	cy = pos.y;
	cz = pos.z;
	l = 0;

	DXVector3 e = DXVector3(m_Epsilon, m_Epsilon, m_Epsilon);
	AABB cellmaxbound;
	
	while (true)
	{
		cellmaxbound = CalcCellBound(cx, cy, cz, l);

		if (Cont_AABB_In_AABB(objbound, cellmaxbound) == CTR_CONTAINS)
		{
			break;
		}

		cx /= 2;
		cy /= 2;
		cz /= 2;
		l++;
	}
}
void DynOctTree::AddObject(SceneObject* const obj)
{
	//DXVector3 pos = obj->m_CombinedTransformation.GetTranslation() / m_CellSize;
	int cx, cy, cz, l;
	FindSuitableCell(obj, cx, cy, cz, l);

	DynOctTreeNode* n = AcquireCellNode(cx, cy, cz, l);

	n->m_Objects.AddTail(obj, 0);

	obj->SetSPTNode(n);
}

void DynOctTree::ObjectMoved(SceneObject* const obj)
{
	DynOctTreeNode* n = (DynOctTreeNode*)obj->GetSPTNode();

	int cx, cy, cz, l;
	FindSuitableCell(obj, cx, cy, cz, l);

	DynOctTreeNode* newcell = AcquireCellNode(cx, cy, cz, l);

	if (n != newcell)
	{
		//add object to new cell first, because maybe the node where it will be inserted might be delete when object is removed
		newcell->m_Objects.AddTail(obj, 0);
		obj->SetSPTNode(newcell);

		n->m_Objects.ReleaseElement(obj);
		if (n->m_Objects.GetElementCount() == 0 && n->GetChildCount() == 0)
		{
			//ReleaseNode(n);
		}
	}
}

void DynOctTree::RemoveObject(SceneObject* const obj)
{
}

void DynOctTree::ReleaseNode(DynOctTreeNode* const pNode)
{
	//delete reference from father
	if (pNode->pFather)
	{
		int sx = pNode->CellX - (pNode->CellX / 2 * 2);
		int sy = pNode->CellY - (pNode->CellY / 2 * 2);
		int sz = pNode->CellZ - (pNode->CellZ / 2 * 2);
		pNode->pFather->m_Childs.SetN(NULL, sx + sy * 2 + sz * 4);
	}

	//delete from hashmap
	hash_map<LONGLONG, DynOctTreeNode*>* lvl = m_NodeIndex[pNode->Level]->GetInfo();
	lvl->erase(pNode->Key);

	//int cc = pNode->GetChildCount();

	if (m_pRoot == pNode) //root was released
	{
		m_NodeIndex.GetTail()->Delete(); //root always is in last level and is the only node there

		m_pRoot = lvl->begin()->second;
		((DynOctTreeNode*)m_pRoot)->pFather = NULL;
	}
	else if (pNode->pFather->GetChildCount() == 0 && pNode->pFather->m_Objects.GetElementCount() == 0)
	{
		ReleaseNode(pNode->pFather);
	}

	delete pNode;
}