//////////////////////////////////////////////////////////////////////////////
//
// AVP_Navigation_Types.cpp
//
// Copyright (C) 2008 Rebellion. All Rights Reserved.
//
//////////////////////////////////////////////////////////////////////////////

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

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

// header
#include "Core/AVP_Navigation_Types.h"
#include "Core/AVP_Navigation.h"

// asura
#include "ChunkSystem/Asura_Chunk_Stream.h"
#include "Interface/Asura_CommandConsole.h"

// AVP
#include "Entity/GamePlayVolume/AVP_ServerEntity_GamePlayVolume.h"
#include "Entity/PhysicalObject/Actor/AVP_ServerEntity_Actor.h"

// axon
#include "Core/AVP_Waypoint_Manager.h"
#include "Core/BrainTypes/Axon_Project_Brain.h"

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Navigation
//
//////////////////////////////////////////////////////////////////////////////

namespace AVP_Navigation
{
	float s_fMismatchedMobilityAdditionalCostFactor   = 5.0f;
	float s_fTransitionLinkAdditionalCostFactor		  = 2.0f;
	float s_fMismatchedLightingAdditionalCostFactor	  = 2.0f;
	float s_fMismatchedVisibilityAdditionalCostFactor = 2.0f;
	float s_fAwayFromInDirLinkCostFactor			  = 5.0f;
	float s_fAwayFromInDirGoalCostFactor			  = 0.25f;
	float s_fWaypointWeightScalar					  = 0.392156862f;	// 100.0f / 255
}

#if defined(ASURA_SUPPORT_COMMAND_CONSOLE) && !defined(ASURA_NO_DEBUG_OUTPUT)
void AVP_Navigation::AddCommandConsoleVars()
{
	Asura_CommandConsole::AddVar( L"AVP.AI.MismatchedMobilityLinkCostFactor", &s_fMismatchedMobilityAdditionalCostFactor, 0.0f, FLT_MAX, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
	Asura_CommandConsole::AddVar( L"AVP.AI.TransitionLinkCostFactor", &s_fTransitionLinkAdditionalCostFactor, 0.0f, FLT_MAX, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
	Asura_CommandConsole::AddVar( L"AVP.AI.MismatchedLightingLinkCostFactor", &s_fMismatchedLightingAdditionalCostFactor, 0.0f, FLT_MAX, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
	Asura_CommandConsole::AddVar( L"AVP.AI.MismatchedVisibilityLinkCostFactor", &s_fMismatchedVisibilityAdditionalCostFactor, 0.0f, FLT_MAX, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
}
#endif


/*const char* s_aszMoveModeStrings[ AVP_Navigation::NUM_MOVEMENT_MODES ] = 
{
	"MOVE SLOW",
	"MOVE MEDIUM",
	"MOVE FAST",
};*/

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Navigation::NavData
//
//////////////////////////////////////////////////////////////////////////////

void AVP_Navigation::NavData::ReadFromChunkStream( Asura_Chunk_Stream& xStream )
{
    int iVersion;
    xStream >> iVersion;

	if ( iVersion == 6 )
	{
		int iWorkingInt;
		xStream >> iWorkingInt;
		m_eMovementMode = static_cast< MovementMode >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteType = static_cast< RouteType >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eTraverseStyle = static_cast< TraverseStyle >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteAgilityRating = static_cast< AgilityRating >( iWorkingInt );

		xStream >> m_xStartPosition;
		xStream >> m_xDestinationPosition;

		xStream >> m_uPathfinderFlags;
		xStream >> m_usRouteLength;
		if ( m_usRouteLength )
		{
			m_pusRoute = new u_short[ m_usRouteLength ];
			for ( u_short usIndex = 0; m_pusRoute && usIndex < m_usRouteLength; ++usIndex )
			{
				xStream >> m_pusRoute[ usIndex ];
			}
		}
		xStream >> m_usRouteSegmentIndex;
		xStream >> m_usDestinationWPID;
		xStream >> m_usCurrentWPID;
		xStream >> m_usCurrentZone;

		xStream >> m_fCharacterWidth;
		xStream >> m_fCharacterHeight;

		xStream >> m_usRouteLength;
		if ( m_usRouteLength )
		{
			m_pusRoute = new u_short[ m_usRouteLength ];
			for ( u_short usIndex = 0; m_pusRoute && usIndex < m_usRouteLength; ++usIndex )
			{
				xStream >> m_pusRoute[ usIndex ];
			}
		}
	}
	else if ( iVersion == 5 )
	{
		int iWorkingInt;
		xStream >> iWorkingInt;
		m_eMovementMode = static_cast< MovementMode >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteType = static_cast< RouteType >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eTraverseStyle = static_cast< TraverseStyle >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteAgilityRating = static_cast< AgilityRating >( iWorkingInt );

		xStream >> m_xStartPosition;
		xStream >> m_xDestinationPosition;

		xStream >> m_uPathfinderFlags;
		xStream >> m_usRouteLength;
		if ( m_usRouteLength )
		{
			m_pusRoute = new u_short[ m_usRouteLength ];
			for ( u_short usIndex = 0; m_pusRoute && usIndex < m_usRouteLength; ++usIndex )
			{
				xStream >> m_pusRoute[ usIndex ];
			}
		}
		xStream >> m_usRouteSegmentIndex;
		xStream >> m_usDestinationWPID;
		xStream >> m_usCurrentWPID;
		xStream >> m_usCurrentZone;

		xStream >> m_fCharacterWidth;
		xStream >> m_fCharacterHeight;

		xStream >> m_usOrigRouteLength;
		if ( m_usOrigRouteLength )
		{
			m_pusOrigRoute = new u_short[ m_usOrigRouteLength ];
			for ( u_short usIndex = 0; m_usOrigRouteLength && usIndex < m_usOrigRouteLength; ++usIndex )
			{
				xStream >> m_pusOrigRoute[ usIndex ];
			}
		}

		xStream >> m_uConnectionGeneration;

	}
	else if ( iVersion == 4 )
	{
		int iWorkingInt;
		xStream >> iWorkingInt;
		m_eMovementMode = static_cast< MovementMode >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteType = static_cast< RouteType >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteAgilityRating = static_cast< AgilityRating >( iWorkingInt );

		xStream >> m_xStartPosition;
		xStream >> m_xDestinationPosition;

		xStream >> m_uPathfinderFlags;
		xStream >> m_usRouteLength;
		if ( m_usRouteLength )
		{
			m_pusRoute = new u_short[ m_usRouteLength ];
			for ( u_short usIndex = 0; m_pusRoute && usIndex < m_usRouteLength; ++usIndex )
			{
				xStream >> m_pusRoute[ usIndex ];
			}
		}
		xStream >> m_usRouteSegmentIndex;
		xStream >> m_usDestinationWPID;
		xStream >> m_usCurrentWPID;
		xStream >> m_usCurrentZone;

		xStream >> m_fCharacterWidth;
		xStream >> m_fCharacterHeight;
	}
	else if ( iVersion == 3 )
	{
		int iWorkingInt;
		xStream >> iWorkingInt;
		m_eMovementMode = static_cast< MovementMode >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteType = static_cast< RouteType >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteAgilityRating = static_cast< AgilityRating >( iWorkingInt );

		xStream >> m_xStartPosition;
		xStream >> m_xDestinationPosition;

		xStream >> m_uPathfinderFlags;
		xStream >> m_usRouteLength;
		if ( m_usRouteLength )
		{
			m_pusRoute = new u_short[ m_usRouteLength ];
			for ( u_short usIndex = 0; m_pusRoute && usIndex < m_usRouteLength; ++usIndex )
			{
				xStream >> m_pusRoute[ usIndex ];
			}
		}
		xStream >> m_usRouteSegmentIndex;
		xStream >> m_usDestinationWPID;
		xStream >> m_usCurrentWPID;
		u_char ucWorking;
		xStream >> ucWorking;
		m_usCurrentZone = ucWorking;

		xStream >> m_fCharacterWidth;
		xStream >> m_fCharacterHeight;
	}
#ifdef ASURA_TOOLS
	else if ( iVersion == 2 )
	{
		int iWorkingInt;
		xStream >> iWorkingInt;
		m_eMovementMode = static_cast< MovementMode >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteType = static_cast< RouteType >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteAgilityRating = static_cast< AgilityRating >( iWorkingInt );

		xStream >> m_xStartPosition;
		xStream >> m_xDestinationPosition;

		xStream >> m_uPathfinderFlags;
		xStream >> m_usRouteLength;
		if ( m_usRouteLength )
		{
			m_pusRoute = new u_short[ m_usRouteLength ];
			for ( u_short usIndex = 0; m_pusRoute && usIndex < m_usRouteLength; ++usIndex )
			{
				xStream >> m_pusRoute[ usIndex ];
			}
		}
		xStream >> m_usRouteSegmentIndex;
		u_short usStartWPID;
		xStream >> usStartWPID;
		xStream >> m_usDestinationWPID;
		xStream >> m_usCurrentWPID;
		u_char ucWorking;
		xStream >> ucWorking;
		m_usCurrentZone = ucWorking;

		xStream >> m_fCharacterWidth;
		xStream >> m_fCharacterHeight;
	}
	else if ( iVersion == 1 )
	{
		int iWorkingInt;
		xStream >> iWorkingInt;
		m_eMovementMode = static_cast< MovementMode >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteType = static_cast< RouteType >( iWorkingInt );
		xStream >> iWorkingInt;
		m_eRouteAgilityRating = static_cast< AgilityRating >( iWorkingInt );

		xStream >> m_xStartPosition;
		xStream >> m_xDestinationPosition;

		xStream >> m_uPathfinderFlags;
		xStream >> m_usRouteLength;
		if ( m_usRouteLength )
		{
			m_pusRoute = new u_short[ m_usRouteLength ];
			for ( u_short usIndex = 0; m_pusRoute && usIndex < m_usRouteLength; ++usIndex )
			{
				xStream >> m_pusRoute[ usIndex ];
			}
		}
		xStream >> m_usRouteSegmentIndex;
		u_short usStartWPID;
		xStream >> usStartWPID;
		xStream >> m_usDestinationWPID;
		xStream >> m_usCurrentWPID;
		u_char ucWorking;
		xStream >> ucWorking;
		m_usCurrentZone = ucWorking;

		float fDummyFloat;
		xStream >> fDummyFloat;
	}
    else if ( iVersion == 0 )
    {
        int iWorkingInt;
        xStream >> iWorkingInt;
        m_eMovementMode = static_cast< MovementMode >( iWorkingInt );
        xStream >> iWorkingInt;
        m_eRouteType = static_cast< RouteType >( iWorkingInt );
        xStream >> iWorkingInt;
        m_eRouteAgilityRating = static_cast< AgilityRating >( iWorkingInt );

        xStream >> m_xStartPosition;
        xStream >> m_xDestinationPosition;

        xStream >> m_uPathfinderFlags;
        xStream >> m_usRouteLength;
        if ( m_usRouteLength )
        {
            m_pusRoute = new u_short[ m_usRouteLength ];
            for ( u_short usIndex = 0; m_pusRoute && usIndex < m_usRouteLength; ++usIndex )
            {
                xStream >> m_pusRoute[ usIndex ];
            }
        }
        xStream >> m_usRouteSegmentIndex;
		u_short usStartWPID;
		xStream >> usStartWPID;
        xStream >> m_usDestinationWPID;
        xStream >> m_usCurrentWPID;
		u_char ucWorking;
		xStream >> ucWorking;
		m_usCurrentZone = ucWorking;

        float fDummyFloat;
		xStream >> fDummyFloat;
        xStream >> fDummyFloat;
    }
#endif // ASURA_TOOLS
    else
    {
        Asura_Assertf( false, ( "AVP_Navigation::NavData::ReadFromChunkStream - bad chunk version number (%d)", iVersion ) );
    }

	xStream.Skip4BytePadding();
}

void AVP_Navigation::NavData::WriteToChunkStream( Asura_Chunk_Stream& xStream ) const
{
    xStream << CURRENT_CHUNK_VERSION;
    xStream << static_cast< int >( m_eMovementMode );
    xStream << static_cast< int >( m_eRouteType );
	xStream << static_cast< int >( m_eTraverseStyle );
    xStream << static_cast< int >( m_eRouteAgilityRating );

    xStream << m_xStartPosition;
    xStream << m_xDestinationPosition;

    xStream << m_uPathfinderFlags;
    xStream << m_usRouteLength;
    for ( u_short usIndex = 0; usIndex < m_usRouteLength; ++usIndex )
    {
        xStream << m_pusRoute[ usIndex ];
    }
    xStream << m_usRouteSegmentIndex;
    xStream << m_usDestinationWPID;
    xStream << m_usCurrentWPID;
    xStream << m_usCurrentZone;

	xStream << m_fCharacterWidth;
	xStream << m_fCharacterHeight;

	xStream << m_usOrigRouteLength;
	for ( u_short usIndex = 0; usIndex < m_usOrigRouteLength; ++usIndex )
	{
		xStream << m_pusOrigRoute[ usIndex ];
	}
	xStream << m_uConnectionGeneration;

    xStream.Write4BytePadding();
}

void AVP_Navigation::NavData::ResetRoute()
{
    if ( m_pusRoute )
    {
        DestroyRouteBuffer( m_pusRoute );
		m_pusRoute = NULL;
    }
    m_usRouteLength = 0;
    m_usRouteSegmentIndex  = 0;

	if ( m_pusOrigRoute )
	{
		DestroyRouteBuffer( m_pusOrigRoute );
		m_pusOrigRoute = NULL;
	}
	m_usOrigRouteLength = 0;
}

#ifdef ASURA_TOOLS
bool AVP_Navigation::NavData::Verify() const
{
    Asura_Assert( m_xStartPosition       != Asura_Maths::ZeroVector3, "AVP_Navigation::NavData::Verify - start position unset" );
    Asura_Assert( m_xDestinationPosition != Asura_Maths::ZeroVector3, "AVP_Navigation::NavData::Verify - destination position unset" );
    Asura_Assert( m_usDestinationWPID != ASURA_INVALID_WAYPOINT_ID, "AVP_Navigation::NavData::Verify - destination WPID unset" );
    Asura_Assert( m_usCurrentWPID     != ASURA_INVALID_WAYPOINT_ID, "AVP_Navigation::NavData::Verify - current WPID unset" );

	if ( !AVP_FlagIsSet( m_uPathfinderFlags, ASURA_AI_PATHFINDER_FLAGS_IGNOREZONES ) )
	{
		Asura_Assert( m_usCurrentZone != ASURA_NAVVERTEX_MAX_ZONE_ID, "AVP_Navigation::NavData::Verify - current zone unset" );
		if ( m_usCurrentZone == ASURA_NAVVERTEX_MAX_ZONE_ID )
		{
	        return false;
		}
	}

    if ( m_xStartPosition == Asura_Maths::ZeroVector3 ||
        m_xDestinationPosition == Asura_Maths::ZeroVector3 ||
        m_usDestinationWPID == ASURA_INVALID_WAYPOINT_ID ||
        m_usCurrentWPID == ASURA_INVALID_WAYPOINT_ID )
    {
        return false;
    }
    else
    {
        return true;
    }
}
#endif

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Navigation::AStarFunctorBase
//
//////////////////////////////////////////////////////////////////////////////

float AVP_Navigation::AwayFromFunctor::EstimateCostToGoal( const int iWaypointIndex )
{
	const Asura_AI_NavVertex* pxWaypoint = Asura_AI_Navigation_System::GetWaypoint( static_cast< unsigned int >( iWaypointIndex ) );
	const float fRangeSq = ( pxWaypoint->GetPosition() - m_xStartPosition ).MagnitudeSquared();
	if ( m_fRangeSq > fRangeSq )
	{
		return m_fRange - Asura_Maths::Sqrt( fRangeSq );
	}
	else
	{
		return 0.0f;
	}
}

float AVP_Navigation::AwayFromFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const Asura_AI_NavVertex* pxWaypoint = Asura_AI_Navigation_System::GetWaypoint( static_cast< unsigned int >( iWaypointIndex ) );
	return ( Asura_AI_Navigation_System::GetWaypointWeight( static_cast< u_short >( pxWaypoint->GetLink( iLinkIndex ).GetIndex() ) ) * s_fWaypointWeightScalar );
}

bool AVP_Navigation::AwayFromFunctor::IsGoalAchieved( const int iWaypointIndex )
{
	const Asura_AI_NavVertex* pxWaypoint = Asura_AI_Navigation_System::GetWaypoint( static_cast< unsigned int >( iWaypointIndex ) );
	const Asura_Vector_3 xDiff    = pxWaypoint->GetPosition() - m_xStartPosition;
	const float          fRangeSq = xDiff.MagnitudeSquared();
	return ( fRangeSq > m_fRangeSq );
}

float AVP_Navigation::AwayFromInDirFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const float fCost = AwayFromFunctor::EstimateTraversalCost( iWaypointIndex, iLinkIndex, uPathfinderFlags );

	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = pxFromWP->GetLinkedWaypoint( iLinkIndex );

	Asura_Vector_3 xStartToToWP = pxToWP->GetPosition() - m_xStartPosition;
	const float    fStartToToWPSq = xStartToToWP.MagnitudeSquared();

	// If we traverse back to the start position for whatever reason still return a valid cost.
	if ( fStartToToWPSq < 0.0001f )
	{
		Asura_Vector_3 xFromWPToToWP = pxToWP->GetPosition() - pxFromWP->GetPosition();
		xFromWPToToWP.Normalise();
		return ( 1.0f - xFromWPToToWP * m_xDirection );
	}

	// Cost for travelling in the incorrect/correct direction.
	xStartToToWP /= Asura_Maths::Sqrt( fStartToToWPSq );
	return fCost + ( 1.0f - xStartToToWP * m_xDirection );
}

bool AVP_Navigation::AlienLinkFlagsTest( const Asura_AI_NavConnection* pxLink, const u_int uPathfinderFlags )
{
	// In here return whether to ignore the link or not.
	u_int uLinkFlags = pxLink->GetFlags();

	// Ignore blocked waypoints.
	if ( uLinkFlags & ASURA_AI_CONNECTION_FLAGS_BLOCKED && AVP_Waypoint_Manager::IsLinkBlockedForAliens(pxLink))
	{

		return true;
	}

	const bool bIsAlienLink = AVP_FlagIsSet( uLinkFlags, AVP_Navigation::AVP_AI_CONNECTION_FLAG_ALIEN );
	if ( !bIsAlienLink )
	{
		return true;
	}

	// for now dont do that - i think this might really upset all the jumping from danger/falling code
	// Make sure that the mobility of the pathfinder is valid for the mobility of the link.
// 	if ( AVP_Navigation::IsHighMobilityLink( static_cast<short>(uLinkFlags) ) )
// 	{
// 		const bool bIsHighMobilityPathfinder = AVP_FlagIsSet( uPathfinderFlags, AVP_Navigation::AVP_AI_PATHFINDER_FLAG_ALIEN_HIGH_MOBILITY );
// 		return !bIsHighMobilityPathfinder;
// 	}
// 	else if ( AVP_Navigation::IsMedMobilityLink( static_cast<short>(uLinkFlags) ) )
// 	{
// 		const bool bIsMedOrHighMobilityPathfinder = AVP_FlagIsSet( uPathfinderFlags, AVP_Navigation::AVP_AI_PATHFINDER_FLAG_ALIEN_MED_MOBILITY ) ||
// 													AVP_FlagIsSet( uPathfinderFlags, AVP_Navigation::AVP_AI_PATHFINDER_FLAG_ALIEN_HIGH_MOBILITY );
// 		return !bIsMedOrHighMobilityPathfinder;
// 	}

	return false;
}

bool AVP_Navigation::HumanoidLinkFlagsTest(  const Asura_AI_NavConnection* pxLink, const u_int uPathfinderFlags  )
{
	// In here return whether to ignore the link or not.
	u_int uLinkFlags = pxLink->GetFlags();
	// Ignore blocked waypoints.
	if ( uLinkFlags & ASURA_AI_CONNECTION_FLAGS_BLOCKED && AVP_Waypoint_Manager::IsLinkBlockedForHumans(pxLink) )
	{
		return true;
	}

	const bool bIsHumanoidLink = AVP_FlagIsSet( uLinkFlags, AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID );
	if ( !bIsHumanoidLink )
	{
		return true;
	}

	return false;
}

bool AVP_Navigation::AlienQueenLinkFlagsTest(  const Asura_AI_NavConnection* pxLink, const u_int uPathfinderFlags  )
{
	// In here return whether to ignore the link or not.
	u_int uLinkFlags = pxLink->GetFlags();

	// Ignore blocked waypoints.
	if ( uLinkFlags & ASURA_AI_CONNECTION_FLAGS_BLOCKED && AVP_Waypoint_Manager::IsLinkBlockedForHumans(pxLink) )
	{
		return true;
	}

	// get the next waypoint, if it is smaller than 6m then dont allow the link to it to be used. 
	//const Asura_AI_NavVertex* pxToWP = Asura_AI_Navigation_System::GetWaypoint( pxLink->GetIndex() );
	//if( pxToWP )
	//{
	//	if( pxToWP->GetRadius() <= 3.0f )
	//	{
	//		return true;
	//	}
	//}

	const bool bIsHumanoidLink = AVP_FlagIsSet( uLinkFlags, AVP_Navigation::AVP_AI_CONNECTION_FLAG_HUMANOID );
	if ( !bIsHumanoidLink )
	{
		return true;
	}

	return false;
}

bool AVP_Navigation::PredatorLinkFlagsTest(  const Asura_AI_NavConnection* pxLink, const u_int uPathfinderFlags )
{
	// In here return whether to ignore the link or not.
	u_int uLinkFlags = pxLink->GetFlags();

	// Ignore blocked waypoints.
	if ( uLinkFlags & ASURA_AI_CONNECTION_FLAGS_BLOCKED && AVP_Waypoint_Manager::IsLinkBlockedForPredator(pxLink) )
	{
		return true;
	}

	const bool bIsPredatorLink = AVP_FlagIsSet( uLinkFlags, AVP_Navigation::AVP_AI_CONNECTION_FLAG_PREDATOR );
	return !bIsPredatorLink;
}

//////////////////////////////////////////////////////////////////////////////
//
// Alien Functors
//
//////////////////////////////////////////////////////////////////////////////
DEBUGVAR float dbg_fMobilitySensitivity = 1.0f;

float AVP_Navigation::AlienCalculateCostMultiplierDueToMobility( const Asura_AI_NavVertex* pxToWP, const u_int uPathfinderFlags, bool bForceLowMobility )
{
	float fMultiplier = 0.0f;
	AVP_AI_PATHFINDER_FLAGS eMobility; 
	if (AVP_FlagIsSet( uPathfinderFlags, AVP_AI_PATHFINDER_FLAG_ALIEN_HIGH_MOBILITY ))
	{
		eMobility = AVP_AI_PATHFINDER_FLAG_ALIEN_HIGH_MOBILITY;
	}
	else if (AVP_FlagIsSet( uPathfinderFlags, AVP_AI_PATHFINDER_FLAG_ALIEN_MED_MOBILITY ))
	{
		eMobility = AVP_AI_PATHFINDER_FLAG_ALIEN_MED_MOBILITY;
	}
	else
	{
		eMobility = AVP_AI_PATHFINDER_FLAG_ALIEN_LOW_MOBILITY;
	}

	if ( bForceLowMobility )
	{
		eMobility = AVP_AI_PATHFINDER_FLAG_ALIEN_LOW_MOBILITY;
	}

	if ( eMobility == AVP_AI_PATHFINDER_FLAG_ALIEN_HIGH_MOBILITY ) 
	{
		if ( ! ( pxToWP->IsCeilingWaypoint() || pxToWP->IsWallWaypoint()) ) // don't like to be on the floor
		{
			fMultiplier = 0.75f;
		}
		else if ( pxToWP->IsWallWaypoint() ) // and slightly prefer to be on ceiling then on wall
		{
			fMultiplier = 0.15f;
		}
	}
	else if (  eMobility == AVP_AI_PATHFINDER_FLAG_ALIEN_MED_MOBILITY )
	{
		if ( ( pxToWP->IsCeilingWaypoint()) ) // small preference to be not be on ceiling
		{
			fMultiplier = 0.30f;
		}
		else if ( !( pxToWP->IsWallWaypoint()) ) // small preference to be on the walls
		{
			fMultiplier = 0.15f;
 		}
	}
	else // m_eMobility == AVP_AI_PATHFINDER_FLAG_ALIEN_LOW_MOBILITY
	{
		if ( pxToWP->IsWallWaypoint() ) // hate to be on the wall or the ceiling
		{
			fMultiplier = 6.0f;
		}
		else if ( pxToWP->IsCeilingWaypoint() ) 
		{
			fMultiplier = 3.0f;
		}

	}

	return dbg_fMobilitySensitivity * fMultiplier;
}


float AVP_Navigation::AlienAStarFunctor::EstimateCostToGoal( const int iWaypointIndex )
{
	const Asura_AI_NavVertex* pxWP   = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );

	if (pxWP && (pxWP->GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2) )
	{
		return 0.0; // in vents ignore the distance
	}


	return PARENT::EstimateCostToGoal( iWaypointIndex );

}

float AVP_Navigation::AlienAStarFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const float fBaseCost = Asura_AI_Navigation_System::DefaultTraversalCostFunction( iWaypointIndex, iLinkIndex, uPathfinderFlags, NULL );

	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = Asura_AI_Navigation_System::GetWaypoint( pxFromWP->m_pxConnections[ iLinkIndex ].m_usIndex );

    // Add a cost based on the waypoint type and the pathfinder's mobility (high mobility guys should try and stick to ceilings, etc).
    // If this guy is high mobility then add a cost if either of the waypoints is not ceiling.
    // If this guy is med mobility then add a cost if either of the waypoints is not wall.
    // No need to add a cost for low mobility guys as links to wall and ceiling should have been ignored.

	bool bFromVent = (pxFromWP->GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2) != 0;
	bool bToVent = (pxFromWP->GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2) != 0;

	float fVentMultiplier = 1.0f;

	if (bFromVent && bToVent) // inside a vent cost is zero
	{
		return 0.0f;
	}
	else if (bFromVent || bToVent)
	{
		fVentMultiplier = 0.5f; // prefer vents
	}

	bool bForceLowMobility = m_eMovementLimitation == LIMIT_LOW_MOBILITY && (pxToWP->GetPosition() - m_xStartPosition).MagnitudeSquared() < m_fDistanceSq;

	float fMismatchedAgilityCost = fBaseCost * AlienCalculateCostMultiplierDueToMobility(pxToWP, uPathfinderFlags, bForceLowMobility) * s_fMismatchedMobilityAdditionalCostFactor;

	// Add a cost for transition links to stop unnecessary hopping on and off walls, etc.
	float fTransitionCost = 0.0f;
	const u_short usLinkFlags = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex )->m_pxConnections[ iLinkIndex ].m_usFlags;
	if ( IsTransitionLink( usLinkFlags ) )
	{
		fTransitionCost = fBaseCost * s_fTransitionLinkAdditionalCostFactor;

		if ( m_eMovementLimitation == LIMIT_AVOID_JUMPING && (pxToWP->GetPosition() - m_xStartPosition).MagnitudeSquared() < m_fDistanceSq )
		{
			fTransitionCost *= 8.0f;
		}
	}

	// Aliens hate light waypoints.
	const float fMismatchedLightingCost = Asura_Maths::ClampToRange( ( AVP_Waypoint_Manager::GetWPLuminance( pxFromWP->m_pxConnections[ iLinkIndex ].m_usIndex ) - AVP_Waypoint_Manager::GetDefaultMinHideLuminance() ) / ( AVP_Waypoint_Manager::GetDefaultMaxHideLuminance() - AVP_Waypoint_Manager::GetDefaultMinHideLuminance() ), 0.0f, 1.0f )
											 * s_fMismatchedLightingAdditionalCostFactor;

	// Add a cost for waypoints with LOF to our threat if the route type demands this.
	float fMismatchedVisibilityCost = 0.0f;
	if ( m_eRouteType == RT_SNEAKY )
	{
		Asura_Assert( m_bThreatWPIDValid, "AVP_Navigation::AlienAStarFunctor::EstimateTraversalCost - threat WPID must be valid" );
		if ( !AVP_Waypoint_Manager::HasCoverFrom( iWaypointIndex, m_uThreatWPID ) )
		{
			fMismatchedVisibilityCost = fBaseCost * s_fMismatchedVisibilityAdditionalCostFactor;
		}
	}

	return fVentMultiplier * ( fBaseCost + fMismatchedAgilityCost + fTransitionCost + fMismatchedLightingCost + fMismatchedVisibilityCost );
}

void AVP_Navigation::AlienAStarFunctor::SetLowMobilityForShortDistance(Asura_Vector_3 xStartPosition, float fDistance)
{
	m_xStartPosition = xStartPosition;
	m_fDistanceSq = fDistance * fDistance;
	m_eMovementLimitation = LIMIT_LOW_MOBILITY;
}

void AVP_Navigation::AlienAStarFunctor::SetAvoidJumping(Asura_Vector_3 xStartPosition, float fDistance)
{
	m_xStartPosition = xStartPosition;
	m_fDistanceSq = fDistance * fDistance;
	m_eMovementLimitation = LIMIT_AVOID_JUMPING;
}

void AVP_Navigation::AlienAStarFunctor::SetNoLimit()
{
	m_xStartPosition = Asura_Maths::ZeroVector3;
	m_fDistanceSq = 0.0f;
	m_eMovementLimitation = LIMIT_NO_LIMIT;
}

float AVP_Navigation::AlienAwayFromFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const float fBaseCost = PARENT::EstimateTraversalCost( iWaypointIndex, iLinkIndex, uPathfinderFlags );

	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = Asura_AI_Navigation_System::GetWaypoint( pxFromWP->m_pxConnections[ iLinkIndex ].m_usIndex );

	// Add a cost based on the waypoint type and the pathfinder's mobility (high mobility guys should try and stick to ceilings, etc).
	// If this guy is high mobility then add a cost if either of the waypoints is not ceiling.
	// If this guy is med mobility then add a cost if either of the waypoints is not wall.
	// No need to add a cost for low mobility guys as links to wall and ceiling should have been ignored.

	bool bForceLowMobility = m_eMovementLimitation == LIMIT_LOW_MOBILITY && (pxToWP->GetPosition() - m_xStartPosition).MagnitudeSquared() < m_fDistanceSq;
	float fMismatchedAgilityCost = AlienCalculateCostMultiplierDueToMobility(pxToWP, uPathfinderFlags, bForceLowMobility) * s_fMismatchedMobilityAdditionalCostFactor;

	// Add a cost for transition links to stop unnecessary hopping on and off walls, etc.
	float fTransitionCost = 0.0f;
	const u_short usLinkFlags = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex )->m_pxConnections[ iLinkIndex ].m_usFlags;
	if ( IsTransitionLink( usLinkFlags ) )
	{
		fTransitionCost = fBaseCost * s_fTransitionLinkAdditionalCostFactor;

		if ( m_eMovementLimitation == LIMIT_AVOID_JUMPING && (pxToWP->GetPosition() - m_xStartPosition).MagnitudeSquared() < m_fDistanceSq )
		{
			fTransitionCost *= 8.0f;
		}
	}

	return ( fBaseCost + fMismatchedAgilityCost + fTransitionCost );

}

void AVP_Navigation::AlienAwayFromFunctor::SetLowMobilityForShortDistance(Asura_Vector_3 xStartPosition, float fDistance)
{
	m_xStartPosition = xStartPosition;
	m_fDistanceSq = fDistance * fDistance;
	m_eMovementLimitation = LIMIT_LOW_MOBILITY;
}

void AVP_Navigation::AlienAwayFromFunctor::SetAvoidJumping(Asura_Vector_3 xStartPosition, float fDistance)
{
	m_xStartPosition = xStartPosition;
	m_fDistanceSq = fDistance * fDistance;
	m_eMovementLimitation = LIMIT_AVOID_JUMPING;
}

void AVP_Navigation::AlienAwayFromFunctor::SetNoLimit()
{
	m_xStartPosition = Asura_Maths::ZeroVector3;
	m_fDistanceSq = 0.0f;
	m_eMovementLimitation = LIMIT_NO_LIMIT;
}

float AVP_Navigation::AlienAwayFromInDirFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const float fBaseCost = PARENT::EstimateTraversalCost( iWaypointIndex, iLinkIndex, uPathfinderFlags );

	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = Asura_AI_Navigation_System::GetWaypoint( pxFromWP->m_pxConnections[ iLinkIndex ].m_usIndex );

	// Add a cost based on the waypoint type and the pathfinder's mobility (high mobility guys should try and stick to ceilings, etc).
	// If this guy is high mobility then add a cost if either of the waypoints is not ceiling.
	// If this guy is med mobility then add a cost if either of the waypoints is not wall.
	// No need to add a cost for low mobility guys as links to wall and ceiling should have been ignored.
	bool bForceLowMobility = m_eMovementLimitation == LIMIT_LOW_MOBILITY && (pxToWP->GetPosition() - m_xStartPosition).MagnitudeSquared() < m_fDistanceSq;
	float fMismatchedAgilityCost = AlienCalculateCostMultiplierDueToMobility(pxToWP, uPathfinderFlags, bForceLowMobility) * s_fMismatchedMobilityAdditionalCostFactor;

	// Add a cost for transition links to stop unnecessary hopping on and off walls, etc.
	float fTransitionCost = 0.0f;
	const u_short usLinkFlags = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex )->m_pxConnections[ iLinkIndex ].m_usFlags;
	if ( IsTransitionLink( usLinkFlags ) )
	{
		fTransitionCost = fBaseCost * s_fTransitionLinkAdditionalCostFactor;

		if ( m_eMovementLimitation == LIMIT_AVOID_JUMPING && (pxToWP->GetPosition() - m_xStartPosition).MagnitudeSquared() < m_fDistanceSq )
		{
			fTransitionCost *= 8.0f;
		}
	}

	return ( fBaseCost + fMismatchedAgilityCost + fTransitionCost );

}

void AVP_Navigation::AlienAwayFromInDirFunctor::SetLowMobilityForShortDistance(Asura_Vector_3 xStartPosition, float fDistance)
{
	m_xStartPosition = xStartPosition;
	m_fDistanceSq = fDistance * fDistance;
	m_eMovementLimitation = LIMIT_LOW_MOBILITY;
}

void AVP_Navigation::AlienAwayFromInDirFunctor::SetAvoidJumping(Asura_Vector_3 xStartPosition, float fDistance)
{
	m_xStartPosition = xStartPosition;
	m_fDistanceSq = fDistance * fDistance;
	m_eMovementLimitation = LIMIT_AVOID_JUMPING;
}

void AVP_Navigation::AlienAwayFromInDirFunctor::SetNoLimit()
{
	m_xStartPosition = Asura_Maths::ZeroVector3;
	m_fDistanceSq = 0.0f;
	m_eMovementLimitation = LIMIT_NO_LIMIT;
}



bool AVP_Navigation::AlienFindCoverInRangeFunctor::VisitNode( const int iWaypointIndex )
{
	const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_Vector_3 xToWP = pxWP->GetPosition() - GetStartPosition();
	if ( xToWP.MagnitudeSquared() < m_fMinRangeSq )
	{
		return false;
	}

	const u_short usWPID = static_cast< u_short >( iWaypointIndex );
	if ( AVP_Waypoint_Manager::HasCoverFrom( usWPID, m_uThreatWPID ) )
	{
		m_uCoverWPIDResult = usWPID;
		return true;
	}
	else
	{
		return false;
	}
}

bool AVP_Navigation::AlienFindCoverInDirFunctor::VisitNode( const int iWaypointIndex )
{
	const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_Vector_3 xToWP = pxWP->GetPosition() - GetStartPosition();
	if ( xToWP.MagnitudeSquared() < m_fMinRangeSq )
	{
		return false;
	}

	const u_short usWPID = static_cast< u_short >( iWaypointIndex );
	if ( AVP_Waypoint_Manager::HasCoverFrom( usWPID, m_usThreatWPID ) )
	{
		m_usNearestCoverWPID = usWPID;
		return true;
	}
	else
	{
		return false;
	}
}

bool AVP_Navigation::AlienFindWallInDirFunctor::VisitNode( const int iWaypointIndex )
{
	const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	if ( pxWP && pxWP->IsWallWaypoint() )
	{
		m_usNearestWallWPID = static_cast< u_short >( iWaypointIndex );
		return true;
	}
	else
	{
		return false;
	}
}

bool AVP_Navigation::AlienFindFloorInDirFunctor::VisitNode( const int iWaypointIndex )
{
	const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	if ( pxWP && !pxWP->IsWallWaypoint() && !pxWP->IsCeilingWaypoint() )
	{
		m_usNearestFloorWPID = static_cast< u_short >( iWaypointIndex );
		return true;
	}
	else
	{
		return false;
	}
}

//////////////////////////////////////////////////////////////////////////////
//
// FaceHugger Functors
//
//////////////////////////////////////////////////////////////////////////////

float AVP_Navigation::FaceHuggerAStarFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const float fBaseCost = Asura_AI_Navigation_System::DefaultTraversalCostFunction( iWaypointIndex, iLinkIndex, uPathfinderFlags, NULL );

	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = Asura_AI_Navigation_System::GetWaypoint( pxFromWP->m_pxConnections[ iLinkIndex ].m_usIndex );

	float fMismatchedAgilityCost = 0.0f;

	// no direct move from floor to ceiling..
	if (pxToWP->IsCeilingWaypoint() && !(pxFromWP->IsWallWaypoint() || pxFromWP->IsCeilingWaypoint() ))
	{
		return -1.0f;
	}

	// no usage of GS2 (vents)
	if (pxToWP->GetFlags() & AVP_AI_NAVVERTEX_FLAG_GS2)
	{
		return -1.0f;
	}

	// allow jumping down ...
// 	if (pxFromWP->IsCeilingWaypoint() && !(pxToWP->IsWallWaypoint() || pxToWP->IsCeilingWaypoint() ))
// 	{
// 		return -1.0f;
// 	}


	if ( pxToWP->IsWallWaypoint() ) // hate to be on the wall 
	{
		fMismatchedAgilityCost = fBaseCost * s_fMismatchedMobilityAdditionalCostFactor * 0.5f;
	}
	else if ( pxToWP->IsCeilingWaypoint() ) // hate even more to be on the ceiling
	{
		fMismatchedAgilityCost = fBaseCost * s_fMismatchedMobilityAdditionalCostFactor;
	}

	// Add a cost for transition links to stop unnecessary hopping on and off walls, etc.
	float fTransitionCost = 0.0f;
	const u_short usLinkFlags = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex )->m_pxConnections[ iLinkIndex ].m_usFlags;
	if ( IsTransitionLink( usLinkFlags ) )
	{
		fTransitionCost = fBaseCost * s_fTransitionLinkAdditionalCostFactor;
	}

	return ( fBaseCost + fMismatchedAgilityCost + fTransitionCost );
}


float AVP_Navigation::FaceHuggerAwayFromFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const float fBaseCost = 1.0f + PARENT::EstimateTraversalCost( iWaypointIndex, iLinkIndex, uPathfinderFlags );

	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = Asura_AI_Navigation_System::GetWaypoint( pxFromWP->m_pxConnections[ iLinkIndex ].m_usIndex );

	// no usage of GS2 (vents)
	if (pxToWP->GetFlags() & AVP_AI_NAVVERTEX_FLAG_GS2)
	{
		return -1.0f;
	}

	float fMismatchedAgilityCost = 0.0f;

	if ( pxToWP->IsWallWaypoint() ) // hate to be on the wall 
	{
		fMismatchedAgilityCost = fBaseCost / 4.0f;
	}
	else if ( pxToWP->IsCeilingWaypoint() ) // hate even more to be on the ceiling
	{
		fMismatchedAgilityCost = fBaseCost / 2.0f;
	}

	// Add a cost for transition links to stop unnecessary hopping on and off walls, etc.
	float fTransitionCost = 0.0f;
	const u_short usLinkFlags = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex )->m_pxConnections[ iLinkIndex ].m_usFlags;
	if ( IsTransitionLink( usLinkFlags ) )
	{
		fTransitionCost = fBaseCost / 8.0f;
	}

	return ( fBaseCost + fMismatchedAgilityCost + fTransitionCost );
}


float AVP_Navigation::FaceHuggerAwayFromInDirFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{

	const float fWightCost = PARENT::EstimateTraversalCost( iWaypointIndex, iLinkIndex, uPathfinderFlags );
	const float fBaseCost = 1.0f;
	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = pxFromWP->GetLinkedWaypoint( iLinkIndex );

	// no usage of GS2 (vents)
	if (pxToWP->GetFlags() & AVP_AI_NAVVERTEX_FLAG_GS2)
	{
		return -1.0f;
	}

	Asura_Vector_3 xStartToToWP = pxToWP->GetPosition() - m_xStartPosition;
	const float    fStartToToWPSq = xStartToToWP.MagnitudeSquared();

	float fDirectionCost = fBaseCost;

	// If we traverse back to the start position for whatever reason still return a valid cost.
	if ( fStartToToWPSq > 0.0001f )
	{
		xStartToToWP /= Asura_Maths::Sqrt( fStartToToWPSq );
		fDirectionCost = fBaseCost * ( 1.0f - xStartToToWP * m_xDirection ) / 2.0f;
	}

	float fMismatchedAgilityCost = 0.0f;

	if ( pxToWP->IsWallWaypoint() ) // hate to be on the wall 
	{
		fMismatchedAgilityCost = fBaseCost / 4.0f;
	}
	else if ( pxToWP->IsCeilingWaypoint() ) // hate even more to be on the ceiling
	{
		fMismatchedAgilityCost = fBaseCost / 2.0f;
	}

	// Add a cost for transition links to stop unnecessary hopping on and off walls, etc.
	float fTransitionCost = 0.0f;
	const u_short usLinkFlags = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex )->m_pxConnections[ iLinkIndex ].m_usFlags;
	if ( IsTransitionLink( usLinkFlags ) )
	{
		fTransitionCost = fBaseCost / 8.0f;
	}

	return ( fWightCost + fDirectionCost + fMismatchedAgilityCost + fTransitionCost );
}


//////////////////////////////////////////////////////////////////////////////
//
// Humanoid Functors
//
//////////////////////////////////////////////////////////////////////////////

float AVP_Navigation::HumanoidAStarFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	float fCost = Asura_AI_Navigation_System::DefaultTraversalCostFunction( iWaypointIndex, iLinkIndex, uPathfinderFlags, NULL );

	// Humanoids hate dark waypoints.
	const Asura_AI_NavVertex*	pxFromWP			= Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const float					fWPLuminaceFactor	= Asura_Maths::ClampToRange( ( AVP_Waypoint_Manager::GetWPLuminance( pxFromWP->m_pxConnections[ iLinkIndex ].m_usIndex ) - AVP_Waypoint_Manager::GetDefaultMinHideLuminance() ) / ( AVP_Waypoint_Manager::GetDefaultMaxHideLuminance() - AVP_Waypoint_Manager::GetDefaultMinHideLuminance() ), 0.0f, 1.0f );

	fCost *= ( 1.0f - fWPLuminaceFactor ); // s_fMismatchedLightingAdditionalCostFactor;

	return fCost;
}

DEBUGVAR float fCostModifier = 20.0f;

float AVP_Navigation::HumanoidCombatAStarFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	float fCost = PARENT::EstimateTraversalCost( iWaypointIndex, iLinkIndex, uPathfinderFlags );

	const Asura_AI_NavVertex* pxTestWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );

	if ( !pxTestWP )
	{
		return fCost;
	}

	u_short usThreatWaypointID = ASURA_INVALID_WAYPOINT_ID;

	AVP_Navigation::GetNearestWaypointID( m_xPosition, usThreatWaypointID );

	if ( usThreatWaypointID == ASURA_INVALID_WAYPOINT_ID )
	{
		return fCost;
	}

	Asura_Vector_3 xDirectionToThreat( m_xPosition - pxTestWP->GetPosition() );
	float fDistToThreatSq = xDirectionToThreat.MagnitudeSquared();
	
	float fMinDistanceSq = AVP_Waypoint_Manager::GetMinLOFValue(iWaypointIndex, xDirectionToThreat);
	fMinDistanceSq *= fMinDistanceSq;

	if ( fDistToThreatSq < fMinDistanceSq )
	{
		fCost *= fCostModifier;
		return fCost;
	}
	else
	{
		return fCost;
	}
}

float AVP_Navigation::HumanoidThreatProximityAStarFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	float fCost = PARENT::EstimateTraversalCost( iWaypointIndex, iLinkIndex, uPathfinderFlags );

	const Asura_AI_NavVertex* pxTestWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );

	if ( !pxTestWP )
	{
		return fCost;
	}

	const float fHeavyWeight	= 1000.0f;
	const float fMediumWeight	= 250.0f;
	const float fLightWeight	= 50.0f;

	float fDistanceSq = ( m_xThreatPos - pxTestWP->GetPosition() ).MagnitudeSquared();

	if ( fDistanceSq < 4.0f )
	{
		fCost *= fHeavyWeight;
	}
	else if ( fDistanceSq < 16.0f )
	{
		fCost *= fMediumWeight;
	}
	else if ( fDistanceSq < 36.0f )
	{
		fCost *= fLightWeight;
	}

	return fCost;
}

//////////////////////////////////////////////////////////////////////////////
//
// Alien Queen Functors
//
//////////////////////////////////////////////////////////////////////////////

float AVP_Navigation::AlienQueenAStarFunctor::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	float fCost = Asura_AI_Navigation_System::DefaultTraversalCostFunction( iWaypointIndex, iLinkIndex, uPathfinderFlags, NULL );

	// get the way point under question
	const Asura_AI_NavVertex* pxTestWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );

	if ( !pxTestWP )
	{
		return fCost;
	}

	const float fHeavyWeight	= 200.0f;
	const float fMediumWeight	= 50.0f;

	// find the radius of the way point and increase wieghting more if it is a small waypoint. 
	float fWPRadius = pxTestWP->GetRadius();

	if ( fWPRadius < 2.5f )
	{
		fCost *= fHeavyWeight;
	}
	else if ( fWPRadius < 5.0f )
	{
		fCost *= fMediumWeight;
	}

	return fCost;
}

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Navigation::BreadthFirstFunctor
//
//////////////////////////////////////////////////////////////////////////////

float AVP_Navigation::BreadthFirstSearch::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = Asura_AI_Navigation_System::GetWaypoint( pxFromWP->GetLink( iLinkIndex ).GetIndex() );

	// Disallow any links that travel over our max range.
	const float fStartPosToWPSq = ( pxToWP->GetPosition() - m_xStartPosition ).MagnitudeSquared();
	if ( fStartPosToWPSq > m_fMaxRangeSq )
	{
		// Any negative value stops this link being used.
		return -1.0f;
	} 
	else
	{
		return Asura_AI_Navigation_System::DefaultTraversalCostFunction( iWaypointIndex, iLinkIndex, uPathfinderFlags, NULL );
	}
}

float AVP_Navigation::InDirectionSearch::EstimateTraversalCost( const int iWaypointIndex, const int iLinkIndex, const u_int uPathfinderFlags )
{
	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_AI_NavVertex* pxToWP   = pxFromWP->GetLinkedWaypoint( iLinkIndex );

	Asura_Vector_3 xStartToToWP = pxToWP->GetPosition() - m_xStartPosition;
	const float    fStartToToWPSq = xStartToToWP.MagnitudeSquared();

	// If we traverse back to the start position for whatever reason still return a valid cost.
	if ( fStartToToWPSq < 0.0001f )
	{
		Asura_Vector_3 xFromWPToToWP = pxToWP->GetPosition() - pxFromWP->GetPosition();
		xFromWPToToWP.Normalise();
		return ( 1.0f - xFromWPToToWP * m_xDirection );
	}

	// Disallow any links that travel over our max range.
	if ( fStartToToWPSq > m_fMaxRangeSq )
	{
		// Any negative value stops this link being used.
		return -1.0f;
	}

	// Cost for travelling in the incorrect/correct direction.
	xStartToToWP /= Asura_Maths::Sqrt( fStartToToWPSq );
	return ( 1.0f - xStartToToWP * m_xDirection );
}

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Navigation::PosEvalFunctor
//
//////////////////////////////////////////////////////////////////////////////

#if defined(_DEBUG) && !defined(ASURA_NO_DEBUG_OUTPUT)
Asura_List<AVP_Navigation::PosEvalFunctor::WPInfo> AVP_Navigation::PosEvalFunctor::s_xWPInfoList;
#endif

bool AVP_Navigation::PosEvalFunctor::VisitNode( const int iWaypointIndex )
{
	m_xPosEvalParams.m_uTestWPID = iWaypointIndex;

	if ( m_pxCombatZone )
	{
		Asura_AI_NavVertex* pxWaypoint = Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
		
		if ( !m_pxCombatZone->Contains( pxWaypoint->GetPosition() ) )
		{
			return false;
		}
	}

	u_int uScore = m_pfnWaypointScoringFn( m_xPosEvalParams );

#if defined(_DEBUG) && !defined(ASURA_NO_DEBUG_OUTPUT)
	WPInfo* pxWPInfo = new WPInfo;
	pxWPInfo->m_uWPID = (u_int)iWaypointIndex;
	pxWPInfo->m_uScore = uScore;
	s_xWPInfoList.AddToList(pxWPInfo);
#endif

	for ( int i = 0; i < MAX_STORED_BEST_WAYPOINTS; ++i )
	{
		if ( uScore > m_uBestScores[i] )
		{
			for ( int j = MAX_STORED_BEST_WAYPOINTS - 2; j >= i; --j )
			{
				m_uBestScores[j + 1] = m_uBestScores[ j ];
				m_uBestWaypoints[j + 1] = m_uBestWaypoints [ j ];
			}

			m_uBestScores[i] = uScore;
			m_uBestWaypoints[i] = (u_int)iWaypointIndex;
			break;
		}
	}

	return false;
}

#if defined(_DEBUG) && !defined(ASURA_NO_DEBUG_OUTPUT)
void AVP_Navigation::PosEvalFunctor::RenerWpInfo()
{
	u_int uBestWp = ASURA_INVALID_WAYPOINT_ID;
	u_int uBestScore = 0;

	{
		Asura_List_ConstIt< WPInfo > xIt( &s_xWPInfoList );
		while (!xIt.Done())
		{
			const WPInfo* pxWPInfo = xIt.GetCurrent();

			if ( pxWPInfo->m_uScore > uBestScore )
			{
				uBestScore = pxWPInfo->m_uScore;
				uBestWp = pxWPInfo->m_uWPID;
			}
			
			xIt.Next();
		}
	}

	{
		Asura_List_ConstIt< WPInfo > xIt( &s_xWPInfoList );
		while (!xIt.Done())
		{
			const WPInfo* pxWPInfo = xIt.GetCurrent();

			if ( pxWPInfo->m_uWPID == uBestWp ) { xIt.Next(); continue; }
			Asura_AI_NavVertex* pxWp = Asura_AI_Navigation_System::GetWaypoint( pxWPInfo->m_uWPID );
			if (!pxWp) { xIt.Next(); continue; }
			Asura_Vector_3 xOutputPos( pxWp->GetPosition() );
			if ( !Asura_Camera::ProjectWorldCoordsToScreenSpace( xOutputPos, true ) || xOutputPos.z <= 0.0f ) { xIt.Next(); continue; }
			Asura_Font_System::Print( xOutputPos.x, xOutputPos.y, 0.5f, 0xffff0000, ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "WP - %d : S - %d", pxWPInfo->m_uWPID, pxWPInfo->m_uScore );
			xIt.Next();
		}

		{
			Asura_AI_NavVertex* pxWp = Asura_AI_Navigation_System::GetWaypoint( uBestWp );
			if ( pxWp )
			{
				Asura_Vector_3 xOutputPos( pxWp->GetPosition() );
				if ( Asura_Camera::ProjectWorldCoordsToScreenSpace( xOutputPos, true ) || xOutputPos.z <= 0.0f )
				{
					Asura_Font_System::Print( xOutputPos.x, xOutputPos.y, 0.5f, 0xffffffff, ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "WP - %d : S - %d", uBestWp, uBestScore );
				}
			}
		}
	}
}

void AVP_Navigation::PosEvalFunctor::DestoryList()
{
	while ( s_xWPInfoList.GetNumEntries() > 0 ) 
	{
		delete ( s_xWPInfoList.PopHeadObject() );
	}
}
#endif

void AVP_Navigation::PosEvalFunctor::SetCombatZoneGamePlayVolume()
{
	if ( m_xPosEvalParams.m_uCombatZoneGPVGuid != ASURA_GUID_UNREGISTERED )
	{
		m_pxCombatZone = AVP_ServerEntity_GamePlayVolume::FindEntity( m_xPosEvalParams.m_uCombatZoneGPVGuid );
	}
}

bool AVP_Navigation::HumanoidInDirectionSearch::VisitNode( const int iWaypointIndex )
{
	const Asura_AI_NavVertex*	pxWP		= Asura_AI_Navigation_System::GetWaypoint( iWaypointIndex );
	const Asura_Vector_3		xToWP		= pxWP->GetPosition() - GetStartPosition();
	const float					fRangeSq	= xToWP.MagnitudeSquared();
	
	if ( fRangeSq > GetMaxRangeSq() )
	{
		return false;
	}

	if ( fRangeSq < GetMinRangeSq() )
	{
		return false;
	}

	Asura_Vector_3 xDirToWP = pxWP->GetPosition() - GetStartPosition();
	xDirToWP.Normalise();

	float fDot = GetDirection() * xDirToWP;

	if ( fDot < 0.707f )
	{
		return false;
	}

	return true;
}

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