//////////////////////////////////////////////////////////////////////////////
//
// AVP_RoomNode_AStar.cpp
//
// Created by AJK on 16-01-2009 from NJD Axon_Gamescene_AStar
//
// Copyright (C) 2009 Rebellion, All Rights Reserved.
//
//////////////////////////////////////////////////////////////////////////////

// core
#include "Core/Asura.h"

// header
#include "Entity/ObjectiveSystem/RoomNode/AVP_RoomNode_AStar.h"

// AVP
#include "Entity/ObjectiveSystem/RoomNode/AVP_RoomNode_ExitLink.h"
#include "Entity/ObjectiveSystem/RoomNode/AVP_ServerEntity_RoomNode.h"

Asura_Guid AVP_RoomNode_AStar::AStarNode::GetGuid() const
{
	return m_pxNode->GetGuid();
}

Asura_LinkList< AVP_RoomNode_AStar::AStarNode* > AVP_RoomNode_AStar::s_xOpenList;
Asura_Hashed_List< AVP_RoomNode_AStar::AStarNode > AVP_RoomNode_AStar::s_xAStarNodeList;
Asura_ResizingMempool AVP_RoomNode_AStar::s_xAStarNodeMempool( 32, sizeof( AStarNode ), __FILE__, __LINE__ );

void AVP_RoomNode_AStar::GetRouteTo( const Asura_Guid uDestNodeGuid, const Asura_Guid uCurrNodeGuid, const AVP_ServerEntity_Actor& xActor, Asura_LinkList< Asura_Guid >& xRoute )
{
	xRoute.Clear();

	if ( uDestNodeGuid == uCurrNodeGuid ) { return; }

	const AVP_ServerEntity_RoomNode* pxDestRoomNodeNode = AVP_ServerEntity_RoomNode::FindEntity( uDestNodeGuid );
	if ( !pxDestRoomNodeNode ) { return; }

	Asura_Guid uStartNodeGuid = uCurrNodeGuid;

	// Open the first node and add it to the open list.
	AStarNode* pxCurrAStarNode = GetAStarNode( uStartNodeGuid );
	if ( !pxCurrAStarNode ) { return; }
	pxCurrAStarNode->m_fCostToGoal  = EstCostToGoal( pxCurrAStarNode->m_pxNode, pxDestRoomNodeNode );
	pxCurrAStarNode->m_fTotalCost   = pxCurrAStarNode->m_fCostToGoal;
	pxCurrAStarNode->m_pxParent     = NULL;
	pxCurrAStarNode->m_pxNodeToHere = NULL;
	pxCurrAStarNode->m_bOpen = true;
	InsertNodeInOpenList( pxCurrAStarNode );

	while ( ( pxCurrAStarNode = GetOpenListHead() ) != NULL )
	{
		if ( pxCurrAStarNode->GetGuid() == uDestNodeGuid )
		{
			// We've reached our destination.
			break;
		}

		// Open all of the linked nodes.
		const AVP_ServerEntity_RoomNode* pxCurrRoomNodeNode = pxCurrAStarNode->m_pxNode;
		
		Asura_LinkList_ConstIt< AVP_RoomNode_ExitLink* > xLinkListIter( pxCurrRoomNodeNode->GetLinkList() );

		while ( !xLinkListIter.Done() )
		{
			const AVP_ServerEntity_RoomNode* pxNode = AVP_ServerEntity_RoomNode::FindEntity( (*xLinkListIter.GetCurrent())->GetLinkedRoomGuid() );
			if ( !pxNode || !pxNode->IsEntityActive() || !pxNode->HasExitLinkForSpecies( xActor) ) 
			{ 
				xLinkListIter.Next();
				continue; 
			}

			AStarNode* pxNextAStarNode = GetAStarNode( pxNode->GetGuid() );
			if ( !pxNextAStarNode ) 
			{ 
				xLinkListIter.Next();
				continue; 
			}

			const float fNewCostToHere = pxCurrAStarNode->m_fCostToHere + EstCostToGoal( pxCurrAStarNode->m_pxNode, pxNextAStarNode->m_pxNode );

			// If we've already visited this node before && we had a better cost previously then move on.
			if ( ( pxNextAStarNode->m_bOpen || pxNextAStarNode->m_bClosed ) && pxNextAStarNode->m_fCostToHere <= fNewCostToHere ) 
			{ 
				xLinkListIter.Next();
				continue; 
			}

			pxNextAStarNode->m_fCostToHere  = fNewCostToHere;
			pxNextAStarNode->m_fCostToGoal  = EstCostToGoal( pxNextAStarNode->m_pxNode, pxDestRoomNodeNode );
			pxNextAStarNode->m_fTotalCost   = pxNextAStarNode->m_fCostToHere + pxNextAStarNode->m_fCostToGoal;
			pxNextAStarNode->m_pxParent     = pxCurrAStarNode;
			pxNextAStarNode->m_pxNodeToHere = pxNode;
			pxNextAStarNode->m_bClosed = false;
			pxNextAStarNode->m_bOpen   = true;

			InsertNodeInOpenList( pxNextAStarNode );
			xLinkListIter.Next();
		}

		// Close our current node.
		pxCurrAStarNode->m_bOpen   = false;
		pxCurrAStarNode->m_bClosed = true;
	}

	while ( pxCurrAStarNode )
	{
		// Wind back up the nodes, writing our route out.
		if ( pxCurrAStarNode->m_pxNodeToHere )
		{
			xRoute.Push_Front( pxCurrAStarNode->m_pxNodeToHere->GetGuid() );
		}
		pxCurrAStarNode = pxCurrAStarNode->m_pxParent;
	}

	// Push the start node if a route was found
	if ( xRoute.Size() > 0 )
	{
		xRoute.Push_Front(uStartNodeGuid);
	}

	// Clear all the lists/mempool storage.
	s_xOpenList.Clear();
	s_xAStarNodeList.Clear();
	s_xAStarNodeMempool.ForceReset();
}

AVP_RoomNode_AStar::AStarNode* AVP_RoomNode_AStar::GetAStarNode( const Asura_Guid uNodeGuid )
{
	AStarNode* pxAStarNode = s_xAStarNodeList.FindEntity( uNodeGuid );
	if ( !pxAStarNode )
	{
		const AVP_ServerEntity_RoomNode* pxRoomNodeNode = AVP_ServerEntity_RoomNode::FindEntity( uNodeGuid );
		if ( pxRoomNodeNode )
		{
			pxAStarNode = static_cast< AStarNode* >( s_xAStarNodeMempool.Allocate() );
			if ( pxAStarNode )
			{
				pxAStarNode->m_pxNode      = pxRoomNodeNode;
				pxAStarNode->m_fCostToHere = 0.0f;
				pxAStarNode->m_bOpen       = false;
				pxAStarNode->m_bClosed     = false;
				s_xAStarNodeList.AddToList( pxAStarNode );
			}
		}
	}

	return pxAStarNode;
}

int AVP_RoomNode_AStar::CompareAStarNodes( AStarNode* const & pxAStarNodeOne, AStarNode* const & pxAStarNodeTwo )
{
	// Can use simplified method as the numbers will always be positive.
	return Asura_Maths::FloatToInt_Near( pxAStarNodeOne->m_fTotalCost - pxAStarNodeTwo->m_fTotalCost );
}

void AVP_RoomNode_AStar::InsertNodeInOpenList( AStarNode* pxAStarNode )
{
	s_xOpenList.Insert( pxAStarNode, CompareAStarNodes );
}

AVP_RoomNode_AStar::AStarNode* AVP_RoomNode_AStar::GetOpenListHead()
{
	Asura_ListElt< AStarNode* >* pxHead = s_xOpenList.Front();
	if ( pxHead )
	{
		AStarNode* pxNode = pxHead->Data();
		s_xOpenList.PopFront();
		return pxNode;
	}
	else
	{
		return NULL;
	}
}

float AVP_RoomNode_AStar::EstCostToGoal( const AVP_ServerEntity_RoomNode* pxCurrNode, const AVP_ServerEntity_RoomNode* pxDestNode )
{
	return ( pxDestNode->GetPosition() - pxCurrNode->GetPosition() ).MagnitudeSquared();
}