/*!
	@project Rune
	@file RuPathGraph_AStar.cpp

	Copyright (c) 2004-2005 All rights reserved

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

#include "../PathEngine/RuPathGraph_AStar.h"

#pragma unmanaged

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

ruRTTI_DEFINE_SUBCLASS(CRuPathGraph_AStar, IRuPathGraph_Base)
ruHEAP_DEFINE_SUBCLASS(CRuPathGraph_AStar, "CRuPathGraph_AStar", "IRuPathGraph_Base")
ruCLASSGUID_DEFINE(CRuPathGraph_AStar, 0xB527F358,0x5D534157, 0xAEB3CD42, 0x212076CC)

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

CRuPathGraph_AStar::CRuPathGraph_AStar()
:	m_pathNodePartition(NULL),
	m_pathFinder(NULL),
	m_pathRover(NULL)
{
}

CRuPathGraph_AStar::CRuPathGraph_AStar(const CRuAABB &bounds)
:	m_bounds(bounds),
	m_pathNodePartition(NULL),
	m_pathFinder(NULL),
	m_pathRover(NULL)
{
}

CRuPathGraph_AStar::~CRuPathGraph_AStar()
{
	// Delete path nodes
	for(INT32 i = 0; i < m_pathNodes.Count(); ++i)
	{
		delete m_pathNodes[i];
	}
	m_pathNodes.Clear();

	// Destroy the path node partition
	delete m_pathNodePartition;

	// Release path finder
	ruSAFE_RELEASE(m_pathFinder);

	// Release path rover
	ruSAFE_RELEASE(m_pathRover);
}

BOOL CRuPathGraph_AStar::SerializeFrom(IRuStream *stream)
{
	CRuGUID classGUID;

	// Read class GUID
	if(RuReadGUID(stream, classGUID) == FALSE)
	{
		return FALSE;
	}

	// Read version
	INT32 version = 1;
	stream->Read(&version, sizeof(INT32));

	// Chain serialization to base class
	IRuPathGraph_Base::SerializeFrom(stream);

	// Read graph key
	stream->Read(&m_graphKey, sizeof(INT32));

	// Read number of path nodes
	INT32 numPathNodes = 0;
	stream->Read(&numPathNodes, sizeof(UINT32));

	// Read path nodes
	m_pathNodes.SetArraySize(numPathNodes);
	m_pathNodes.SetNumEntries(numPathNodes);
	for(INT32 i = 0; i < numPathNodes; ++i)
	{
		m_pathNodes[i] = ruNEW CRuPathNode();
		m_pathNodes[i]->SerializeFrom(stream);
	}

	// Read number of path node links
	INT32 numPathNodeLinks = 0;
	stream->Read(&numPathNodeLinks, sizeof(INT32));

	// Read path node links
	m_pathNodeLinks.SetArraySize(numPathNodeLinks);
	m_pathNodeLinks.SetNumEntries(numPathNodeLinks);
	for(INT32 i = 0; i < numPathNodeLinks; ++i)
	{
		stream->Read(&m_pathNodeLinks[i].m_nodeID, sizeof(INT32));
		stream->Read(&m_pathNodeLinks[i].m_linkType, sizeof(RuPathNodeLinkType));
		stream->Read(&m_pathNodeLinks[i].m_linkDistance, sizeof(REAL));
		stream->Read(&m_pathNodeLinks[i].m_linkData, sizeof(INT32));
	}

	// Read block bounds
	stream->Read(&m_bounds.Minima().x, sizeof(REAL));
	stream->Read(&m_bounds.Minima().y, sizeof(REAL));
	stream->Read(&m_bounds.Minima().z, sizeof(REAL));
	stream->Read(&m_bounds.Maxima().x, sizeof(REAL));
	stream->Read(&m_bounds.Maxima().y, sizeof(REAL));
	stream->Read(&m_bounds.Maxima().z, sizeof(REAL));

	// Construct path node partition if it does not already exist
	if(m_pathNodePartition == NULL)
	{
		m_pathNodePartition = ruNEW CRuPathNodePartition(m_bounds);
	}

	// Insert path nodes into the partitioning structure
	for(INT32 i = 0; i < numPathNodes; ++i)
	{
		m_pathNodePartition->InsertPathNode(i, m_pathNodes[i]);
	}

	return TRUE;
}

BOOL CRuPathGraph_AStar::SerializeTo(IRuStream *stream)
{
	// Write class GUID
	stream->Write(GetClassGUID().Data(), GetClassGUID().Size());

	// Write version
	INT32 version = 1;
	stream->Write(&version, sizeof(INT32));

	// Chain serialization to base class
	IRuPathGraph_Base::SerializeTo(stream);

	// Write graph key
	stream->Write(&m_graphKey, sizeof(INT32));

	// Save path nodes
	INT32 numPathNodes = m_pathNodes.Count();
	stream->Write(&numPathNodes, sizeof(UINT32));
	for(INT32 i = 0; i < numPathNodes; ++i)
	{
		m_pathNodes[i]->SerializeTo(stream);
	}

	// Save path node links
	INT32 numPathNodeLinks = m_pathNodeLinks.Count();
	stream->Write(&numPathNodeLinks, sizeof(INT32));
	for(INT32 i = 0; i < numPathNodeLinks; ++i)
	{
		stream->Write(&m_pathNodeLinks[i].m_nodeID, sizeof(INT32));
		stream->Write(&m_pathNodeLinks[i].m_linkType, sizeof(RuPathNodeLinkType));
		stream->Write(&m_pathNodeLinks[i].m_linkDistance, sizeof(REAL));
		stream->Write(&m_pathNodeLinks[i].m_linkData, sizeof(INT32));
	}

	// Save block bounds
	stream->Write(&m_bounds.Minima().x, sizeof(REAL));
	stream->Write(&m_bounds.Minima().y, sizeof(REAL));
	stream->Write(&m_bounds.Minima().z, sizeof(REAL));
	stream->Write(&m_bounds.Maxima().x, sizeof(REAL));
	stream->Write(&m_bounds.Maxima().y, sizeof(REAL));
	stream->Write(&m_bounds.Maxima().z, sizeof(REAL));

	return TRUE;
}

BOOL CRuPathGraph_AStar::SetPathFinder(IRuPathFinder_Base *pathFinder)
{
	if(pathFinder)
	{
		// Increment reference on the new path finder
		pathFinder->AddRef();

		// Release old path finder and store reference to new path finder
		ruSAFE_RELEASE(m_pathFinder);
		m_pathFinder = pathFinder;
	}

	return TRUE;
}

BOOL CRuPathGraph_AStar::SetPathRover(IRuPathRover_Base *pathRover)
{
	pathRover->AddRef();
	ruSAFE_RELEASE(m_pathRover);
	m_pathRover = pathRover;

	return TRUE;
}

void CRuPathGraph_AStar::SetGraphKey(INT32 graphKey)
{
	m_graphKey = graphKey;
}

INT32 CRuPathGraph_AStar::GetGraphKey() const
{
	return m_graphKey;
}

const CRuAABB &CRuPathGraph_AStar::Bounds()
{
	return m_bounds;
}

INT32 CRuPathGraph_AStar::GetNumPathNodes()
{
	return m_pathNodes.Count();
}

const CRuPathNode *CRuPathGraph_AStar::GetPathNode(INT32 nodeID)
{
	return m_pathNodes[nodeID];
}

void CRuPathGraph_AStar::ConnectNodeToNeighbors(INT32 nodeID)
{
	// Clear old neighbors
	m_pathNodes[nodeID]->ClearAllNeighbors();

	// Connect node to neighbors
	const CRuVector3 &position = m_pathNodes[nodeID]->Position();
	CRuSphere connectionBounds(position, 80.0f);	// TODO: Change radius to an adjustable parameter
	CRuArrayList<INT32> potentialNeighbors;
	m_pathNodePartition->Enumerate(connectionBounds, potentialNeighbors);
	for(INT32 i = 0; i < potentialNeighbors.Count(); ++i)
	{
		if(potentialNeighbors[i] != nodeID)
		{
			CRuVector3 neighborPosition = m_pathNodes[potentialNeighbors[i]]->Position();
			BOOL pathOK = m_pathRover ? m_pathRover->IsPathValid(position, neighborPosition) : TRUE;

			if(pathOK)
			{
				REAL distanceToNeighbor = (position - neighborPosition).SquaredMagnitude();
				m_pathNodes[nodeID]->InsertNeighbor(potentialNeighbors[i], distanceToNeighbor);
			}
		}
	}
}

INT32 CRuPathGraph_AStar::InsertPathNode(const CRuVector3 &position)
{
	// Construct path node partition if it does not already exist
	if(m_pathNodePartition == NULL)
	{
		m_pathNodePartition = ruNEW CRuPathNodePartition(m_bounds);
	}

	// Construct new path node
	CRuPathNode *pathNode = ruNEW CRuPathNode(position);

	// Insert node into the path node list
	INT32 nodeID = m_pathNodes.Count();
	m_pathNodes.Add(pathNode);

	// Insert node into the partitioning structure
	m_pathNodePartition->InsertPathNode(nodeID, pathNode);

	// Connect node to neighbors
	CRuSphere connectionBounds(position, 80.0f);	// TODO: Change radius to an adjustable parameter
	CRuArrayList<INT32> potentialNeighbors;
	m_pathNodePartition->Enumerate(connectionBounds, potentialNeighbors);

	ConnectNodeToNeighbors(nodeID);
	for(INT32 i = 0; i < potentialNeighbors.Count(); ++i)
	{
		ConnectNodeToNeighbors(potentialNeighbors[i]);
	}

	return nodeID;
}

BOOL CRuPathGraph_AStar::DeletePathNode(INT32 nodeID)
{
	if(nodeID >= 0 && nodeID < m_pathNodes.Count())
	{
		CRuPathNode *deletedPathNode = m_pathNodes[nodeID];

		// Delete node from the partitioning structure
		m_pathNodePartition->DeletePathNode(nodeID);

		// Delete node from the path node list
		m_pathNodes.RemoveAt(nodeID);
		delete deletedPathNode;

		// TODO: Neighbors are no longer valid....

		return TRUE;
	}

	return FALSE;
}

INT32 CRuPathGraph_AStar::GetNumPathNodeLinks()
{
	return m_pathNodeLinks.Count();
}

BOOL CRuPathGraph_AStar::GetPathNodeLink(INT32 linkID, INT32 &nodeIDOut, RuPathNodeLinkType &linkTypeOut, REAL &linkDistanceOut, INT32 &linkDataOut)
{
	if(linkID >= 0 && linkID < m_pathNodeLinks.Count())
	{
		nodeIDOut = m_pathNodeLinks[linkID].m_nodeID;
		linkTypeOut = m_pathNodeLinks[linkID].m_linkType;
		linkDistanceOut = m_pathNodeLinks[linkID].m_linkDistance;
		linkDataOut = m_pathNodeLinks[linkID].m_linkData;

		return TRUE;
	}

	return FALSE;
}

BOOL CRuPathGraph_AStar::SetPathNodeLink(INT32 nodeID, RuPathNodeLinkType linkType, REAL linkDistance, INT32 linkData)
{
	// Attempt to look for the specified node ID in the link list
	for(INT32 i = 0; i < m_pathNodeLinks.Count(); ++i)
	{
		if(m_pathNodeLinks[i].m_nodeID == nodeID)
		{
			m_pathNodeLinks[i].m_linkType = linkType;
			m_pathNodeLinks[i].m_linkDistance = linkDistance;
			m_pathNodeLinks[i].m_linkData = linkData;

			return TRUE;
		}
	}

	// The specified node ID does not exist in the link list, so create a new link
	INT32 pathNodeLinkID = m_pathNodeLinks.Count();
	m_pathNodeLinks.Add(PathNodeLinkDesc());

	// Set link properties
	m_pathNodeLinks[pathNodeLinkID].m_nodeID = nodeID;
	m_pathNodeLinks[pathNodeLinkID].m_linkType = linkType;
	m_pathNodeLinks[pathNodeLinkID].m_linkDistance = linkDistance;
	m_pathNodeLinks[pathNodeLinkID].m_linkData = linkData;

	return TRUE;
}

INT32 CRuPathGraph_AStar::FindPathNodeLink(INT32 nodeID)
{
	// Attempt to look for the specified node ID in the link list
	for(INT32 i = 0; i < m_pathNodeLinks.Count(); ++i)
	{
		if(m_pathNodeLinks[i].m_nodeID == nodeID)
		{
			return i;
		}
	}

	return -1;
}

BOOL CRuPathGraph_AStar::EnumeratePathNodes(const CRuSphere &bounds, CRuArrayList<INT32> &pathNodesOut)
{
	m_pathNodePartition->Enumerate(bounds, pathNodesOut);

	return TRUE;
}

BOOL CRuPathGraph_AStar::FindPath(INT32 startNodeID, INT32 endNodeID, CRuArrayList<INT32> &pathOut)
{
	if(m_pathFinder == NULL)
	{
		m_pathFinder = ruNEW CRuPathFinder_AStar();
	}

	return m_pathFinder->FindPath(m_pathNodes, startNodeID, endNodeID, pathOut);
}

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

#pragma managed
