#include "deObjectTree.hpp"
#include "deFunctors.hpp"
#include "deScene.hpp"
#include "deBoundingVolumes.hpp"

deObjectTree::deObjectTree()
:	m_ObjectHash(97),
	mObjectThreshold(5),
	mDepthMax(50),
	m_Root(NULL)
{
}

deObjectTree::~deObjectTree()
{
	DestroyPartition(m_Root);
}

// add an object into the tree, at the deepest level possible
deBoolean deObjectTree::AddObject(IdeSceneObject* object)
{
	RemoveObject(object);
	if (!m_Root)
	{
		m_Root = CreatePartition(NULL, 0, 0);
		if (!m_Root)
			DEBUGRETURN(deFALSE, "deObjectTree::AddObject - error creating root partition");
		m_Root->BBox.Min = deZeroVec;
		m_Root->BBox.Max = deOneVec;
	}
	return InsertObject(m_Root, object, 0);
}

// totally remove the object from the tree
void deObjectTree::RemoveObject(IdeSceneObject* object)
{
	ObjectHash_t::iterator it = m_ObjectHash.find((DWORD)object);
	if (it != m_ObjectHash.end())
	{
		ABTPartition* oldpart = *it;
		if (oldpart)
		{
			deTList<IdeSceneObject*>::iterator end = oldpart->Objects.end();
			deFindErase(oldpart->Objects.begin(), end, object, oldpart->Objects);
			if (oldpart->Objects.size() == 0)
				PrunePartition(oldpart, 0);
		}
		m_ObjectHash.erase(it);
	}
}

deBoolean deObjectTree::RepositionObject(IdeSceneObject* object)
{
	// find the object in the hash, so we know which partition it was in
	// if it's still within the same one, then nothing needs to be done
	ObjectHash_t::iterator it = m_ObjectHash.find((DWORD)object);
	if (it != m_ObjectHash.end())
	{
		ABTPartition* oldpart = *it;
		if (oldpart)
		{
			deTList<IdeSceneObject*>::iterator end = oldpart->Objects.end();
			deTList<IdeSceneObject*>::iterator oldobj = deFind(oldpart->Objects.begin(), end, object);
			if (oldobj != end)
			{
				deAABB objectbox;
				deTransformInfo tform;
				object->GetWorldAABB(objectbox);
				object->GetWorldTransform(tform);
				objectbox.Min += tform.Translation;
				objectbox.Max += tform.Translation;
				if (deGeometry::AABBInAABB(objectbox, oldpart->BBox))
				{
					int Depth = 0;
					ABTPartition* temp = oldpart->Parent;
					while (temp)
					{
						++Depth;
						temp = temp->Parent;
					}
					// see if it fits into any existing children now
					if (oldpart->Children[0] && deGeometry::AABBInAABB(objectbox, oldpart->Children[0]->BBox))
					{
						oldpart->Objects.erase(oldobj);
						m_ObjectHash.erase(it);
						return InsertObject(oldpart->Children[0], object, Depth);
					}
					if (oldpart->Children[1] && deGeometry::AABBInAABB(objectbox, oldpart->Children[1]->BBox))
					{
						oldpart->Objects.erase(oldobj);
						m_ObjectHash.erase(it);
						return InsertObject(oldpart->Children[1], object, Depth);
					}
					// object still inside this bbox, nothing needs to be done
					return deTRUE;
				}
				oldpart->Objects.erase(oldobj);
				if (oldpart->Objects.size() == 0)
					PrunePartition(oldpart, 0);
			}
		}
		m_ObjectHash.erase(it);
		// we've removed the object from this partition, and we need to re-insert it somewhere else
		return InsertObject(m_Root, object, 0);
	}
	return AddObject(object);
}

// create a partition with the specified parent
deObjectTree::ABTPartition* deObjectTree::CreatePartition(ABTPartition* parent, 
														  int ChildNum, 
														  int AxisNum)
{
	ABTPartition* newpart = new ABTPartition;
	newpart->BBox.Min = deZeroVec;
	newpart->BBox.Max = deZeroVec;
	newpart->Split.Axis = -1;
	newpart->Split.Percent = 0.5;
	newpart->Parent = parent;
	newpart->Children[0] = NULL;
	newpart->Children[1] = NULL;
	if (parent && (ChildNum == 0 || ChildNum == 1))
	{
		DE_ASSERT(AxisNum >= 0 && AxisNum < 3);
		parent->Children[ChildNum] = newpart;
		newpart->BBox.Min = parent->BBox.Min;
		newpart->BBox.Max = parent->BBox.Max;
		deVec3d* midpt;
		if (ChildNum == 0)
			midpt = &newpart->BBox.Max;
		else
			midpt = &newpart->BBox.Min;
		switch (AxisNum)
		{
		case 0:
			midpt->x = parent->BBox.Min.x + 
				newpart->Split.Percent * (parent->BBox.Max.x - parent->BBox.Min.x);
			break;
		case 1:
			midpt->y = parent->BBox.Min.y + 
				newpart->Split.Percent * (parent->BBox.Max.y - parent->BBox.Min.y);
			break;
		case 2:
			midpt->z = parent->BBox.Min.z + 
				newpart->Split.Percent * (parent->BBox.Max.z - parent->BBox.Min.z);
			break;
		}
	}
	return newpart;
}

// create children for a partition, and place its objects inside the children
deBoolean deObjectTree::SplitPartition(ABTPartition* part, 
									   int Depth)
{
	if (!part)
		return deFALSE;
	if (Depth > mDepthMax)
		return deTRUE;
	if (!part->Children[0] && !part->Children[1])
	{
		deVec3d axisscores =
		{	part->BBox.Max.x-part->BBox.Min.x, 
			part->BBox.Max.y-part->BBox.Min.y, 
			part->BBox.Max.z-part->BBox.Min.z}; // amount of volume on each axis
		deDouble largest = max(axisscores.y, axisscores.z);
		largest = max(axisscores.x, largest);
		if (largest != 0)
			axisscores /= largest; // normalize

		deVec3d splitdists;
		deGeometry::AABBCenter(part->BBox, splitdists);
		deVec3d splitscores = deZeroVec; // number of split objects
		deVec3d balancescores = deZeroVec; // balance of objects on each side
		deTList<IdeSceneObject*>::iterator it = part->Objects.begin(),
			end = part->Objects.end();
		for (; it != end; ++it)
		{
			deAABB objectbox;
			IdeSceneObject* object = *it;
			if (object)
			{
				deTransformInfo tform;
				object->GetWorldAABB(objectbox);
				object->GetWorldTransform(tform);
				objectbox.Min += tform.Translation;
				objectbox.Max += tform.Translation;
			}
			int classify;
			classify = deGeometry::AABBPlaneClassify(objectbox, deVecX, splitdists.x);
			balancescores.x += classify;
			if (!classify)
				splitscores.x += 1;
			classify = deGeometry::AABBPlaneClassify(objectbox, deVecY, splitdists.y);
			balancescores.y += classify;
			if (!classify)
				splitscores.y += 1;
			classify = deGeometry::AABBPlaneClassify(objectbox, deVecZ, splitdists.z);
			balancescores.z += classify;
			if (!classify)
				splitscores.z += 1;
		}
		balancescores.x = fabs(balancescores.x);
		balancescores.y = fabs(balancescores.y);
		balancescores.z = fabs(balancescores.z);
		largest = max(balancescores.y, balancescores.z);
		largest = max(balancescores.x, largest);
		if (largest != 0)
			balancescores /= largest; // normalize
		balancescores -= deOneVec;
		balancescores = -balancescores; // bigger is better
		if (splitscores.x == part->Objects.size() && 
			splitscores.x == splitscores.y && 
			splitscores.x == splitscores.z)
		{
			// early-out if all objects straddle the splitting plane
			return deTRUE;
		}
		largest = max(splitscores.y, splitscores.z);
		largest = max(splitscores.x, largest);
		if (largest != 0)
			splitscores /= largest; // normalize
		splitscores -= deOneVec;
		splitscores = -splitscores; // bigger is better

		deVec3d TotalScore = (axisscores*0.1 + splitscores*0.4 + balancescores*0.5);

		if (TotalScore.x >= TotalScore.y)
		{
			if (TotalScore.x >= TotalScore.z)
				part->Split.Axis = 0;
			else
				part->Split.Axis = 2;
		}
		else
		{
			if (TotalScore.y > TotalScore.z)
				part->Split.Axis = 1;
			else
				part->Split.Axis = 2;
		}
	}

	// create the child partitions now that we've decided the splitting plane
	if (!part->Children[0])
		part->Children[0] = CreatePartition(part, 0, part->Split.Axis);
	if (!part->Children[1])
		part->Children[1] = CreatePartition(part, 1, part->Split.Axis);

	// insert the objects into the appropriate child nodes, if possible
	deTList<IdeSceneObject*>::iterator it = part->Objects.begin(),
		end = part->Objects.end();
	for (; it != end;)
	{
		IdeSceneObject* object = *it;
		if (object)
		{
			deAABB objectbox;
			deTransformInfo tform;
			object->GetWorldAABB(objectbox);
			object->GetWorldTransform(tform);
			objectbox.Min += tform.Translation;
			objectbox.Max += tform.Translation;
			if (part->Children[0] && deGeometry::AABBInAABB(objectbox, part->Children[0]->BBox))
			{
				if (!InsertObject(part->Children[0], object, Depth + 1))
					return deFALSE;
			}
			else if (part->Children[1] && deGeometry::AABBInAABB(objectbox, part->Children[1]->BBox))
			{
				if (!InsertObject(part->Children[1], object, Depth + 1))
					return deFALSE;
			}
			else
			{
				// object didn't fit into a child partition, just keep it here
				++it;
				continue;
			}
		}
		// remove from current
		it = part->Objects.erase(it);
	}

	return deTRUE;
}

// destroy this partition and all its children
void deObjectTree::DestroyPartition(ABTPartition* part)
{
	if (!part)
		return;
	// destroy the children
	if (part->Children[0])
		DestroyPartition(part->Children[0]);
	if (part->Children[1])
		DestroyPartition(part->Children[1]);
	// make sure the parent no longer points to us
	if (part->Parent)
	{
		if (part->Parent->Children[0] == part)
			part->Parent->Children[0] = NULL;
		else if (part->Parent->Children[1] == part)
			part->Parent->Children[1] = NULL;
	}
	// delete this partition
	delete part;
	if (m_Root == part)
		m_Root = NULL;
}

void deObjectTree::PrunePartition(ABTPartition* part, long Depth)
{
#pragma fixme(Pruning causes the tree to get messed up)
//	return ;
	if (!part)
		return;
	PrunePartition(part->Children[0], 1);
	PrunePartition(part->Children[1], 1);
	if (part->Children[0] || part->Children[1])
		return;
	if (part->Objects.size() != 0)
		return;
	// make sure the parent no longer points to us
	if (part->Parent)
	{
		if (part->Parent->Children[0] == part)
			part->Parent->Children[0] = NULL;
		else if (part->Parent->Children[1] == part)
			part->Parent->Children[1] = NULL;
	}
	// delete this partition
	ABTPartition* parent = part->Parent;
	delete part;
	if (m_Root == part)
		m_Root = NULL;
	if (Depth <= 0 && 0)
		PrunePartition(parent, -1);
}

// insert an object into a partition, or into its children if it has children and the object fits
deBoolean deObjectTree::InsertObject(ABTPartition* part, 
									 IdeSceneObject* object,
									 int Depth)
{
	if (part && object)
	{
		// add the object to a child partition if possible
		deAABB objectbox;
		deTransformInfo tform;
		object->GetWorldAABB(objectbox);
		object->GetWorldTransform(tform);
		objectbox.Min += tform.Translation;
		objectbox.Max += tform.Translation;
		// make sure the object fits inside this partition
		if (!deGeometry::AABBInAABB(objectbox, part->BBox))
		{
			if (part == m_Root)
				return ExpandRoot(object);
			return InsertObject(part->Parent, object, Depth-1);
		}
		if (part->Children[0] && deGeometry::AABBInAABB(objectbox, part->Children[0]->BBox))
		{
			return InsertObject(part->Children[0], object, Depth + 1);
		}
		if (part->Children[1] && deGeometry::AABBInAABB(objectbox, part->Children[1]->BBox))
		{
			return InsertObject(part->Children[1], object, Depth + 1);
		}
		// didnt have children or object was too big, so store in this partition
		part->Objects.push_back(object);
		// rebind the mapping if it's already there
		ObjectHash_t::iterator it = m_ObjectHash.find((DWORD)object);
		if (it == m_ObjectHash.end())
			m_ObjectHash.AddElement(part, (DWORD)object);
		else
			*it = part;
		// if the newly added object breaks the threshold, split the partition
		if (part->Objects.size() > mObjectThreshold)
			return SplitPartition(part, Depth);
		return deTRUE;
	}
	return deFALSE;
}

deBoolean deObjectTree::ExpandRoot(IdeSceneObject* outlier)
{
	deAABB objectbox;
	deTransformInfo tform;
	outlier->GetWorldAABB(objectbox);
	outlier->GetWorldTransform(tform);
	objectbox.Min += tform.Translation;
	objectbox.Max += tform.Translation;
	// sanity check, nothing beyond a billion units please...
	if (Vec3d_Length(objectbox.Min) > 1000000000)
		return deFALSE;
	if (Vec3d_Length(objectbox.Max) > 1000000000)
		return deFALSE;
	while (1)
	{
		int ChildNum = 1; // the child number of the child that the root will be
		int AxisNum = 0;
		deAABB NewBox = m_Root->BBox;
		deVec3d Difference = NewBox.Max - NewBox.Min;
		// very simple growing heuristic - just grow along the first one we find
		if (objectbox.Min.x < m_Root->BBox.Min.x)
		{
			NewBox.Min.x -= Difference.x;
			AxisNum = 0;
		}
		else if (objectbox.Min.y < m_Root->BBox.Min.y)
		{
			NewBox.Min.y -= Difference.y;
			AxisNum = 1;
		}
		else if (objectbox.Min.z < m_Root->BBox.Min.z)
		{
			NewBox.Min.z -= Difference.z;
			AxisNum = 2;
		}
		else
		{
			ChildNum = 0;
			if (objectbox.Max.x > m_Root->BBox.Max.x)
			{
				NewBox.Max.x += Difference.x;
				AxisNum = 0;
			}
			else if (objectbox.Max.y > m_Root->BBox.Max.y)
			{
				NewBox.Max.y += Difference.y;
				AxisNum = 1;
			}
			else if (objectbox.Max.z > m_Root->BBox.Max.z)
			{
				NewBox.Max.z += Difference.z;
				AxisNum = 2;
			}
			else
				break; // it's inside, we're done growing
		}

		// create a new root
		ABTPartition* oldroot = m_Root;
		m_Root = CreatePartition(NULL, 0, 0);
		m_Root->Children[ChildNum] = oldroot;
		m_Root->BBox = NewBox;
		m_Root->Split.Axis = AxisNum;
		oldroot->Parent = m_Root;
		ChildNum = 1 - ChildNum;
		// create an opposite child so the object doesn't get stuck in the root
		m_Root->Children[ChildNum] = CreatePartition(m_Root, ChildNum, AxisNum);
		PrunePartition(oldroot, 0);

		// do the insert again, it should work this time
		return InsertObject(m_Root, outlier, 0);
	}
	return deTRUE;
}

void deObjectTree::IntersectionTest(IdeBoundingVolume* BV, 
									deTList<IdeSceneObject*>& outList)
{
	IntersectionHelper(m_Root, BV, outList);
}

void deObjectTree::IntersectionHelper(ABTPartition* part, 
									  IdeBoundingVolume* BV,
									  deTList<IdeSceneObject*>& outList)
{
	if (!part) // nothing here
		return;
	if (!BV->TestAABBIntersect(deZeroVec, part->BBox)) // doesn't intersect
		return;

	deTList<IdeSceneObject*>::iterator it = part->Objects.begin(),
		end = part->Objects.end();
	for (; it != end; ++it)
	{
		IdeSceneObject* object = *it;
		if (object)
		{
			deAABB objectbox;
			deTransformInfo tform;
			object->GetWorldAABB(objectbox);
			object->GetWorldTransform(tform);
			// if it "intersects" then add it to the outlist
			if (BV->TestAABBIntersect(tform.Translation, objectbox))
				outList.AddElement(object);
		}
	}
	// recurse to children
	if (part->Children[0] && BV->TestAABBIntersect(deZeroVec, part->Children[0]->BBox))
		IntersectionHelper(part->Children[0], BV, outList);
	if (part->Children[1] && BV->TestAABBIntersect(deZeroVec, part->Children[1]->BBox))
		IntersectionHelper(part->Children[1], BV, outList);
}

