/*!
	@project Rune
	@file RuPathFinder_AStar.cpp

	Copyright (c) 2004-2005 All rights reserved

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

#include "../PathEngine/RuPathFinder_AStar.h"

#pragma unmanaged

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

ruRTTI_DEFINE_SUBCLASS(CRuPathFinder_AStar, IRuPathFinder_Base)
ruHEAP_DEFINE_SUBCLASS(CRuPathFinder_AStar, "CRuPathFinder_AStar", "IRuPathFinder_Base")
ruCLASSGUID_DEFINE(CRuPathFinder_AStar, 0x03A5C52A, 0xEAE743f8, 0xAE2C5A62, 0x280509CA)

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

CRuPathFinder_AStar::CRuPathFinder_AStar()
:	m_maxExpansions(1024)
{
}

CRuPathFinder_AStar::~CRuPathFinder_AStar()
{
	// Reset path finder
	ResetPathFinder();

	// Delete pre-allocated path nodes
	while(!m_freeNodeList.IsEmpty())
		delete m_freeNodeList.Pop();
}

BOOL CRuPathFinder_AStar::SetMaximumExpansions(INT32 maxExpansions)
{
	m_maxExpansions = maxExpansions;

	return TRUE;
}

BOOL CRuPathFinder_AStar::FindPath(CRuArrayList<CRuPathNode *> &pathNodes, INT32 startNodeID, INT32 endNodeID, CRuArrayList<INT32> &pathOut)
{
	REAL heuristicScale = 1.25f;

	Node *finalNode = NULL;
	CRuVector3 endCenter, startCenter, tmpVector;
	REAL newG, newH, newF;

	// Make sure open & closed bit fields are large enough to accommodate our nodes
	if(m_openFlags.GetNumBits() < pathNodes.Count())
	{
		m_openFlags.SetNumBits(pathNodes.Count());
		m_closedFlags.SetNumBits(pathNodes.Count());
	}

	// Reset pathfinder state
	ResetPathFinder();

	// Put starting node on the open list
	m_openFlags.Set(startNodeID);

	Node *node = m_freeNodeList.IsEmpty() ? ruNEW Node() : m_freeNodeList.Pop();

	tmpVector.SetToDifference(pathNodes[endNodeID]->Position(), pathNodes[startNodeID]->Position());

	node->m_previousNode = NULL;
	node->m_pathNode = startNodeID;
	node->m_g = 0.0f;
	node->m_h = tmpVector.SquaredMagnitude() * heuristicScale;
	node->m_f = node->m_g + node->m_h;
	m_openList.Push(node->m_f, node);

	// --- Actual algorithm
	INT32 openedNodes = 0;

	while(!m_openList.IsEmpty() && openedNodes < m_maxExpansions)
	{
		++openedNodes;

		// Remove the lowest cost node from open list and put it onto closed list
		Node *curNode = m_openList.Pop();
		INT32 nodeID = curNode->m_pathNode;
		CRuPathNode *curPathNode = pathNodes[nodeID];
		const CRuArrayList<INT32> &neighbors = curPathNode->Neighbors();
		const CRuArrayList<REAL> &neighborCosts = curPathNode->NeighborCosts();

		m_closedList.Add(curNode);
		m_closedFlags.Set(nodeID);

		// If the node is goal, we are done!
		if(nodeID == endNodeID)
		{
			finalNode = curNode;
			break;
		}

		// Process each of the node's neighbors
		for(INT32 i = 0; i < neighbors.Count(); ++i)
		{
			CRuPathNode *neighborNode = pathNodes[neighbors[i]];

			tmpVector.SetToDifference(pathNodes[endNodeID]->Position(), pathNodes[neighbors[i]]->Position());

			newG = neighborCosts[i];
			newH = tmpVector.SquaredMagnitude() * heuristicScale;
			newF = newG + newH;

			// Make sure the neighbor node is not on the closed list
			if(!m_closedFlags[neighbors[i]])
			{
				// Is the node already on the open list?
				if(m_openFlags[neighbors[i]])
				{
					// --- supposed to reconsider node here...
				}
				else
				{
					Node *newNode = m_freeNodeList.IsEmpty() ? ruNEW Node() : m_freeNodeList.Pop();

					// If the node is not on the open list, add it
					newNode->m_previousNode = curNode;
					newNode->m_pathNode = neighbors[i];
					newNode->m_g = newG;
					newNode->m_h = newH;
					newNode->m_f = newF;
					m_openList.Push(newNode->m_f, newNode);
					m_openFlags.Set(neighbors[i]);
				}
			}
		}
	}

	// Path reconstruction
	if(finalNode)
	{
		while(finalNode != NULL)
		{
			Node *newNode = m_freeNodeList.IsEmpty() ? ruNEW Node() : m_freeNodeList.Pop();

			newNode->m_previousNode = finalNode->m_previousNode;
			newNode->m_pathNode = finalNode->m_pathNode;

			m_pathQueue.Queue(newNode);
			finalNode = finalNode->m_previousNode;
		}

		// Construct vector path list
		while(!m_pathQueue.IsEmpty())
		{
			// Dequeue node in reverse order and push onto free list
			Node *curNode = m_pathQueue.DequeueTail();
			m_freeNodeList.Push(curNode);

			// Add path position to output list
			pathOut.Add(curNode->m_pathNode);
		}

		return TRUE;
	}

	return FALSE;
}

void CRuPathFinder_AStar::ResetPathFinder()
{
	// Clear open list
	while(!m_openList.IsEmpty())
	{
		m_freeNodeList.Push(m_openList.Pop());
	}

	// Clear path queue
	while(!m_pathQueue.IsEmpty())
	{
		m_freeNodeList.Push(m_pathQueue.Dequeue());
	}

	// Clear closed list
	for(INT32 i = 0; i < m_closedList.Count(); i++)
	{
		m_freeNodeList.Push(m_closedList[i]);
	}
	m_closedList.Clear();

	// Reset flags
	m_openFlags.ClearAll();
	m_closedFlags.ClearAll();
}

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

#pragma managed
