/*!
	@project Rune
	@file RuPathEngine_Base.cpp

	Copyright (c) 2004-2005 All rights reserved

	@author John Tang
	@date 2005/12/21
*/

#include "../PathEngine/RuPathEngine_Base.h"

#include "../Helper/RuHelper_StreamIO.h"

#pragma unmanaged

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

CRuPathNode::CRuPathNode()
{
}

CRuPathNode::CRuPathNode(const CRuVector3 &position)
:	m_position(position)
{
}

INT32 CRuPathNode::InsertNeighbor(INT32 neighborID, REAL neighborCost)
{
	m_neighbors.Add(neighborID);
	m_neighborCosts.Add(neighborCost);

	return m_neighbors.Count() - 1;
}

BOOL CRuPathNode::ClearAllNeighbors()
{
	m_neighbors.Clear();
	m_neighborCosts.Clear();

	return TRUE;
}

void CRuPathNode::SerializeTo(IRuStream *stream)
{
	// Write data
	::SerializeTo(stream, &m_position);

	// Write number of neighbors
	INT32 numNeighbors = m_neighbors.Count();
	stream->Write(&numNeighbors, sizeof(INT32));

	// Write neighbor data
	for(INT32 i = 0; i < numNeighbors; ++i)
	{
		stream->Write(&m_neighbors[i], sizeof(INT32));
	}

	// Write neighbor cost data
	for(INT32 i = 0; i < numNeighbors; ++i)
	{
		stream->Write(&m_neighborCosts[i], sizeof(REAL));
	}
}

void CRuPathNode::SerializeFrom(IRuStream *stream)
{
	// Read data
	::SerializeFrom(stream, &m_position);

	// Read number of neighbors
	INT32 numNeighbors;
	stream->Read(&numNeighbors, sizeof(INT32));

	// Read neighbor data
	m_neighbors.SetArraySize(numNeighbors);
	m_neighbors.SetNumEntries(numNeighbors);
	for(INT32 i = 0; i < numNeighbors; ++i)
	{
		stream->Read(&m_neighbors[i], sizeof(INT32));
	}

	// Read neighbor cost data
	m_neighborCosts.SetArraySize(numNeighbors);
	m_neighborCosts.SetNumEntries(numNeighbors);
	for(INT32 i = 0; i < numNeighbors; ++i)
	{
		stream->Read(&m_neighborCosts[i], sizeof(REAL));
	}
}

const CRuArrayList<INT32> &CRuPathNode::Neighbors() const
{
	return m_neighbors;
}

const CRuArrayList<REAL> &CRuPathNode::NeighborCosts() const
{
	return m_neighborCosts;
}

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

INT32 CRuPathNodePartition::s_partitionDensity = 25;

CRuPathNodePartition::CRuPathNodePartition(const CRuAABB &bounds)
:	m_partitionBounds(bounds)
{
	// Ignore Y parameter, since this is a quadtree
	m_partitionBounds.Minima().m_y = -100000.0f;
	m_partitionBounds.Maxima().m_y = 100000.0f;

	// Initialize children to null
	for(INT32 i = 0; i < 4; ++i)
	{
		m_children[i] = NULL;
	}
}

CRuPathNodePartition::~CRuPathNodePartition()
{
	// Destroy children
	for(INT32 i = 0; i < 4; ++i)
	{
		if(m_children[i])
		{
			delete m_children[i];
		}
	}
}

BOOL CRuPathNodePartition::InsertPathNode(INT32 nodeID, CRuPathNode *pathNode)
{
	// If the partition cannot contain the path node, abort
	if(!m_partitionBounds.Contains(pathNode->Position()))
	{
		return FALSE;
	}

	if(IsLeaf())
	{
		// Insert node ID into the list
		m_pathNodes.Add(PathNodeDesc(nodeID, pathNode));

		// Subdivide
		Subdivide(s_partitionDensity);

		return TRUE;
	}
	else
	{
		// Try adding path node into one of the child partitions
		for(INT32 i = 0; i < 4; ++i)
		{
			if(m_children[i]->InsertPathNode(nodeID, pathNode))
			{
				return TRUE;
			}
		}
	}

	return FALSE;
}

BOOL CRuPathNodePartition::DeletePathNode(INT32 nodeID)
{
	if(DeletePathNodeImpl(nodeID))
	{
		ShiftNodeIDFrom(nodeID);
		return TRUE;
	}

	return FALSE;
}

void CRuPathNodePartition::Enumerate(const CRuSphere &bounds, CRuArrayList<INT32> &pathNodesOut)
{
	if(Intersects(m_partitionBounds, bounds))
	{
		if(IsLeaf())
		{
			for(INT32 i = 0; i < m_pathNodes.Count(); ++i)
			{
				if(bounds.Contains(m_pathNodes[i].m_pathNode->Position()))
				{
					pathNodesOut.Add(m_pathNodes[i].m_nodeID);
				}
			}
		}
		else
		{
			for(INT32 i = 0; i < 4; ++i)
			{
				m_children[i]->Enumerate(bounds, pathNodesOut);
			}
		}
	}
}

BOOL CRuPathNodePartition::DeletePathNodeImpl(INT32 nodeID)
{
	if(IsLeaf())
	{
		for(INT32 i = 0; i < m_pathNodes.Count(); ++i)
		{
			if(m_pathNodes[i].m_nodeID == nodeID)
			{
				// Remove path node from array
				m_pathNodes.RemoveAt(i);

				return TRUE;
			}
		}
	}
	else
	{
		// Try removing path node from one of the child partitions
		for(INT32 i = 0; i < 4; ++i)
		{
			if(m_children[i]->DeletePathNodeImpl(nodeID))
			{
				return TRUE;
			}
		}
	}

	return FALSE;
}

void CRuPathNodePartition::ShiftNodeIDFrom(INT32 nodeID)
{
	if(IsLeaf())
	{
		for(INT32 i = 0; i < m_pathNodes.Count(); ++i)
		{
			if(m_pathNodes[i].m_nodeID > nodeID)
			{
				m_pathNodes[i].m_nodeID = m_pathNodes[i].m_nodeID - 1;
			}
		}
	}
	else
	{
		for(INT32 i = 0; i < 4; ++i)
		{
			m_children[i]->ShiftNodeIDFrom(nodeID);
		}
	}
}

void CRuPathNodePartition::Subdivide(INT32 targetDensity)
{
	if(targetDensity < m_pathNodes.Count())
	{
		// Initialize child partitions
		for(INT32 i = 0; i < 4; ++i)
		{
			CRuAABB childBounds;
			childBounds.SetBoundsToQuadrantXZ(m_partitionBounds, i);
			m_children[i] = ruNEW CRuPathNodePartition(childBounds);
		}

		// Distribute path nodes to children
		for(INT32 i = 0; i < m_pathNodes.Count(); ++i)
		{
			for(INT32 j = 0; j < 4; ++j)
			{
				m_children[j]->InsertPathNode(m_pathNodes[i].m_nodeID, m_pathNodes[i].m_pathNode);
			}
		}

		// Clear path nodes list
		m_pathNodes.Clear();
		m_pathNodes.SetArraySize(0);
	}
}

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

#pragma managed
