//////////////////////////////////////////////////////////////////////////////
//
// Axon_Gamescene_AStar.cpp
//
// Created by NJD - 24 October 2006
//
// Copyright (C) 2006 Rebellion, All Rights Reserved.
//
//////////////////////////////////////////////////////////////////////////////

// I N C L U D E S ///////////////////////////////////////////////////////////

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

#ifdef ASURA_USE_AXON

// header
#include "Behaviour/BehaviourTypes/Axon_Gamescene_AStar.h"

// asura
#include "Core/Asura_PrimeSearch.h"
#include "Entity/Locations/OrientedNode/GamesceneNode/Asura_ServerNode_GS2Node.h"
#include "Entity/Locations/Spline/GamesceneSpline/Asura_ServerNode_GS2Spline.h"

//////////////////////////////////////////////////////////////////////////////
//
// Axon_Gamescene_AStar_System
//
//////////////////////////////////////////////////////////////////////////////

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

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

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

	if ( uDestNodeGuid == uCurrNodeGuid ) { return; }

	const Asura_ServerNode_GS2Node* pxDestGS2Node = Asura_ServerNode_GS2Node::FindEntity( uDestNodeGuid );
	if ( !pxDestGS2Node ) { return; }

	// We may be at a link.
	Asura_Guid uStartNodeGuid = uCurrNodeGuid;
	const Asura_ServerNode_GS2Spline* pxStartGS2Link = Asura_ServerNode_GS2Spline::FindEntity( uStartNodeGuid );
	if ( pxStartGS2Link )
	{
		uStartNodeGuid = pxStartGS2Link->GetTargetGuid();
	}

	// 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, pxDestGS2Node );
    pxCurrAStarNode->m_fTotalCost   = pxCurrAStarNode->m_fCostToGoal;
    pxCurrAStarNode->m_pxParent     = NULL;
	pxCurrAStarNode->m_pxLinkToHere = 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 Asura_ServerNode_GS2Node* pxCurrGS2Node = pxCurrAStarNode->m_pxNode;
		for ( u_int u = 0; u < pxCurrGS2Node->GetNumberLinks(); ++u )
		{
			const Asura_ServerNode_GS2Spline* pxLink = Asura_ServerNode_GS2Spline::FindEntity( pxCurrGS2Node->GetLink( u ) );
			if ( !pxLink || !pxLink->IsEntityActive() ) { continue; }

			const float fNewCostToHere = pxCurrAStarNode->m_fCostToHere + EstCostOfTraversal( pxLink );

			AStarNode* pxNextAStarNode = GetAStarNode( pxLink->GetTargetGuid() );
			if ( !pxNextAStarNode ) { continue; }

			// 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 ) { continue; }

            pxNextAStarNode->m_fCostToHere  = fNewCostToHere;
            pxNextAStarNode->m_fCostToGoal  = EstCostToGoal( pxNextAStarNode->m_pxNode, pxDestGS2Node );
            pxNextAStarNode->m_fTotalCost   = pxNextAStarNode->m_fCostToHere + pxNextAStarNode->m_fCostToGoal;
            pxNextAStarNode->m_pxParent     = pxCurrAStarNode;
			pxNextAStarNode->m_pxLinkToHere = pxLink;
			pxNextAStarNode->m_bClosed = false;
	        pxNextAStarNode->m_bOpen   = true;

			InsertNodeInOpenList( pxNextAStarNode );
		}

		// 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_pxLinkToHere )
		{
			xRoute.Push_Front( pxCurrAStarNode->m_pxLinkToHere->GetGuid() );
		}
		pxCurrAStarNode = pxCurrAStarNode->m_pxParent;
	}

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

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

	return pxAStarNode;
}

int Axon_Gamescene_AStar_System::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 Axon_Gamescene_AStar_System::InsertNodeInOpenList( AStarNode* pxAStarNode )
{
	s_xOpenList.Insert( pxAStarNode, CompareAStarNodes );
}

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

float Axon_Gamescene_AStar_System::EstCostToGoal( const Asura_ServerNode_GS2Node* pxCurrNode, const Asura_ServerNode_GS2Node* pxDestNode )
{
	return ( pxDestNode->GetPosition() - pxCurrNode->GetPosition() ).Magnitude();
}

float Axon_Gamescene_AStar_System::EstCostOfTraversal( const Asura_ServerNode_GS2Spline* pxLink )
{
	return pxLink->GetTotalLength();
}

//////////////////////////////////////////////////////////////////////////////
//
// 
//
//////////////////////////////////////////////////////////////////////////////

#endif // ASURA_USE_AXON
