//////////////////////////////////////////////////////////////////////////////
//
// AVP_Behaviour_Shared_RouteHelper.cpp
//
// Created by NJD - 07 December 2006
//
// Copyright (C) 2006 Rebellion, All Rights Reserved.
//
//////////////////////////////////////////////////////////////////////////////

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

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

// This files header
#include "Behaviour\BehaviourTypes\AVP_Behaviour_Shared_RouteHelper.h"

// Asura
//#include "AI/Asura_AI_Navigation.h"
#include "Core/Asura_LOS.h"
#include "Core/Memory/Asura_Mem.h"
#ifndef ASURA_NO_DEBUG_OUTPUT
#include "Debugging/Asura_DebugLineRender.h"
#endif

// Axon
#include "Behaviour/BehaviourTypes/AVP_Behaviour_Types.h"
#include "Core/BrainTypes/Axon_Project_Brain.h"

// AVP
#include "Entity/PhysicalObject/Actor/AVP_ServerEntity_Actor.h"
#include "Actor/AVP_Actor_PhysicsObjectHandler.h"
#include "Entity/AVP_Entity_Classification.h"

// Waypoint weighting
unsigned char AVP_Behaviour_Shared_RouteHelper::s_ucFloorWeight;
unsigned char AVP_Behaviour_Shared_RouteHelper::s_ucWallWeight;
unsigned char AVP_Behaviour_Shared_RouteHelper::s_ucCeilingWeight;
unsigned char AVP_Behaviour_Shared_RouteHelper::s_ucStepsWeight;
unsigned char AVP_Behaviour_Shared_RouteHelper::s_ucSlopeWeight;

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Behaviour_Shared_RouteHelper
//
//////////////////////////////////////////////////////////////////////////////

const float AVP_Behaviour_Shared_RouteHelper::s_fUnableToMoveDuration = 4.0f;

AVP_Behaviour_Shared_RouteHelper::AVP_Behaviour_Shared_RouteHelper( Axon_Project_Brain* pxBrain, u_int uBehaviourType )
: PARENT( pxBrain, uBehaviourType )
, m_pusRoute( NULL )
, m_iRouteLength( 0 )
, m_iRouteIndex( 0 )
, m_xCurrentDestination( Asura_Maths::ZeroVector3 )
, m_xFinalDestination( Asura_Maths::ZeroVector3 )
, m_usCurrentWaypoint( USHRT_MAX )
, m_usFinalWaypoint( USHRT_MAX )
#ifndef ASURA_NO_DEBUG_OUTPUT
, m_iPreSmoothedRouteLength( 0 )
, m_pusPreSmoothedRoute( NULL )
#endif
, m_eRouteTraversalState( RTS_UNSET )
#ifdef ASURA_USE_STREAMING
, m_uCurrentZoneHash( ASURA_HASH_ID_RTS_UNSET )
#endif
, m_fUnableToMoveTimer( 0.0f )
, m_fDestinationRange4SecsAgo( 0.0f )
, m_xPosition4SecsAgo( Asura_Maths::ZeroVector3 )
, m_fAtTargetYTol( 0.75f )
, m_usFlags( 0 )
{
}

AVP_Behaviour_Shared_RouteHelper::~AVP_Behaviour_Shared_RouteHelper()
{

}

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

void AVP_Behaviour_Shared_RouteHelper::Deactivate()
{
	if ( m_pusRoute )
	{
		Axon_Project_Brain::DestroyRouteBuffer( m_pusRoute );
	}
	m_iRouteLength = 0;

#ifndef ASURA_NO_DEBUG_OUTPUT
	if ( m_pusPreSmoothedRoute )
	{
		Axon_Project_Brain::DestroyRouteBuffer( m_pusPreSmoothedRoute );
	}
	m_iPreSmoothedRouteLength = 0;
#endif
}

////////////////////////////////////////////////////////////////////////////////
// Function     : AVP_Behaviour_Shared_RouteHelper::DestroyRoute
// Author       : Adam Law
// Date         : 07-Jul-2007
////////////////////////////////////////////////////////////////////////////////
void AVP_Behaviour_Shared_RouteHelper::DestroyRoute()
{
	if ( m_pusRoute )
	{
		Axon_Project_Brain::DestroyRouteBuffer( m_pusRoute );
		m_pusRoute     = NULL;
		m_iRouteLength = 0;		
		m_eRouteTraversalState = RTS_UNSET;
	}
#ifndef ASURA_NO_DEBUG_OUTPUT
	if ( m_pusPreSmoothedRoute )
	{
		Axon_Project_Brain::DestroyRouteBuffer( m_pusPreSmoothedRoute );
		m_pusPreSmoothedRoute     = NULL;
		m_iPreSmoothedRouteLength = 0;
	}
#endif	
}

void AVP_Behaviour_Shared_RouteHelper::Update( const float fTimeslice )
{
	// If we don't have a route, or aren't moving then don't bother with this stuff.
	const bool bNotMoving = ( m_eRouteTraversalState == RTS_UNSET || m_eRouteTraversalState == RTS_AT_DESTINATION || m_eRouteTraversalState == RTS_NUM_STATES );
	if ( m_iRouteLength == 0 || bNotMoving )
	{
		m_fUnableToMoveTimer = 0.0f;
	}
	else
	{
		// Calculate unable to move stuff.
		m_fUnableToMoveTimer += fTimeslice;
		if ( m_fUnableToMoveTimer > s_fUnableToMoveDuration )
		{
			m_fUnableToMoveTimer = 0.0f;

			const Asura_Vector_3& xCurrentPosition = m_pxBrain->GetActor()->GetPosition();
			const float           fTargetRange     = ( xCurrentPosition - m_xCurrentDestination ).Magnitude();
			if ( Asura_Maths::Abs( fTargetRange - m_fDestinationRange4SecsAgo ) < 1.0f && xCurrentPosition.ApproximatelyEquals( m_xPosition4SecsAgo, 4.0f ) )
			{
				Asura_OutputToDebugger( ( "%u: Obstructed from moving along a route: %f %f %f\n",
					m_pxBrain->GetOwnerGuid(),
					xCurrentPosition.x, xCurrentPosition.y, xCurrentPosition.z ) );

				// We've not closed in on the target in the last 4 seconds - we are probably orbiting.
				m_bIsStuck = true;
			}
			else
			{
				m_bIsStuck = false;
			}

			m_xPosition4SecsAgo         = xCurrentPosition;
			m_fDestinationRange4SecsAgo = fTargetRange;
		}
	}
}

#ifndef ASURA_NO_DEBUG_OUTPUT
void AVP_Behaviour_Shared_RouteHelper::RenderDebugInfo()
{
	PARENT::RenderDebugInfo();

	// Only render our route & waypoints if we have a valid route
	if ( m_pusRoute )
	{
		for ( int i = 0; i < m_iRouteLength - 1; ++i )
		{
			const Asura_AI_NavVertex* pxWP1 = Asura_AI_Navigation_System::GetWaypoint( m_pusRoute[ i ] );
			const Asura_AI_NavVertex* pxWP2 = Asura_AI_Navigation_System::GetWaypoint( m_pusRoute[ i + 1 ] );

			static const Asura_Vector_3 s_xALittleBitUp( 0.0f, -0.125f, 0.0f );
			Asura_DebugLineRender::AddLine( pxWP1->m_xPosition + s_xALittleBitUp, pxWP2->m_xPosition + s_xALittleBitUp, 0xffffffff );
		}
		// Render the pre-smoothed route.
		if ( m_pusPreSmoothedRoute )
		{
			for ( int i = 0; i < m_iPreSmoothedRouteLength - 1; ++i )
			{
				const Asura_AI_NavVertex* pxWP1 = Asura_AI_Navigation_System::GetWaypoint( m_pusPreSmoothedRoute[ i ] );
				const Asura_AI_NavVertex* pxWP2 = Asura_AI_Navigation_System::GetWaypoint( m_pusPreSmoothedRoute[ i + 1 ] );

				static const Asura_Vector_3 s_xALittleBitUp( 0.0f, -0.125f, 0.0f );
				Asura_DebugLineRender::AddLine( pxWP1->m_xPosition + s_xALittleBitUp, pxWP2->m_xPosition + s_xALittleBitUp, 0xffff0000 );
			}
		}
		Asura_DebugLineRender::AddSphere( m_xCurrentDestination, 0.5f, 0xff00b0ff );
		Asura_DebugLineRender::AddSphere( m_xFinalDestination,   1.0f, 0xffffb0ff );
	}
}
#endif

bool AVP_Behaviour_Shared_RouteHelper::GetRouteTo( const Asura_Vector_3& xPosition, const bool bValidatePosition, const bool bStraightenRoute, const bool bDestroyOld )
{
	// Destroy any previously calculated routes
	if ( bDestroyOld )
		DestroyRoute();   

	const Asura_Vector_3& xCurrentPosition = m_pxBrain->GetActor()->GetPosition();
	UpdateCurrentWaypoint( xCurrentPosition );

	m_eRouteTraversalState	= RTS_BEGINNING_ROUTE;
	m_iRouteIndex			= 0;
	m_xFinalDestination		= xPosition;

	// First off, validate the position if necessary.
	if ( bValidatePosition )
	{
		m_usFinalWaypoint = ResolveToValidPosition( m_xFinalDestination );
	}
	else
	{
		m_usFinalWaypoint = Asura_AI_Navigation_System::FindWaypointNearestTo( xPosition );
	}

	// If our intended destination is within our current waypoint there is no need to do a route search.
	if ( Asura_AI_Navigation_System::IsPositionInWaypoint( m_xFinalDestination, m_usCurrentWaypoint ) )
	{
		// Make up a tiny route with the destination waypoint in it.
		m_pusRoute = Axon_Project_Brain::CreateRouteBuffer( m_iRouteLength );
		if ( m_pusRoute )
		{
			m_iRouteLength  = 1;
			m_pusRoute[ 0 ] = m_usFinalWaypoint;
		}
	}
	else
	{
		// Full A* search.
		static const u_short usPathfindingFlags = ASURA_AI_PATHFINDER_FLAGS_CANFALLFAR | ASURA_AI_PATHFINDER_FLAGS_CANFALL;
		Asura_AI_Navigation_System::FindRoute( Axon_Project_Brain::GetWorkingRouteBuffer(), m_iRouteLength, Axon_Project_Brain::GetWorkingRouteBufferLength(),
			m_usCurrentWaypoint, m_usFinalWaypoint, usPathfindingFlags );

		if ( m_iRouteLength )
		{
			m_pusRoute = Axon_Project_Brain::CreateRouteBuffer( m_iRouteLength );
			if ( m_pusRoute )
			{
				Asura_Mem::Copy( m_pusRoute, Axon_Project_Brain::GetWorkingRouteBuffer(), sizeof( u_short ) * m_iRouteLength );
			}
			else
			{
				m_iRouteLength = 0;
			}
		}
	}

	if ( bStraightenRoute )
	{
		RemoveRedundantWaypoints();
	}

	return ( m_iRouteLength > 0 );
}


////////////////////////////////////////////////////////////////////////////////
// Function     : AVP_Behaviour_Shared_RouteHelper::GetCustomRouteTo
// Author       : Adam Law
// Date         : 07-Jun-2007
////////////////////////////////////////////////////////////////////////////////

bool AVP_Behaviour_Shared_RouteHelper::GetCustomRouteTo(const Asura_Vector_3& xPosition, const bool bUseFloorOnly /* = true */, const bool bValidatePosition /* = true */, const bool bStraightenRoute, const bool bDestroyOld /* = true */)
{
	const Asura_Vector_3&	xCurrentPosition	= m_pxBrain->GetActor()->GetPosition();

	// Destroy any previously calculated routes
	if (bDestroyOld)
	{
		DestroyRoute();	
	}

	// Update our Current Waypoint
	UpdateCurrentWaypoint(xCurrentPosition);
	// Give waypoints their respective weightings
	//AddWaypointWeighting(); // *** GBT Maybe redundant ***

	// Reset all the route finding variables
	m_eRouteTraversalState = RTS_BEGINNING_ROUTE;
	m_iRouteIndex          = 0;
	m_xFinalDestination    = xPosition;

	// First off, validate the position if necessary.
	if (bValidatePosition)
	{
		m_usFinalWaypoint = ResolveToValidPosition(m_xFinalDestination);
	}
	else
	{
		m_usFinalWaypoint = Asura_AI_Navigation_System::FindWaypointNearestTo(xPosition);
	}

	// If our intended destination is within our current waypoint there is no need to do a route search.
	if (Asura_AI_Navigation_System::IsPositionInWaypoint( m_xFinalDestination, m_usCurrentWaypoint))
	{
		// Make up a tiny route with the destination waypoint in it.
		m_pusRoute = Axon_Project_Brain::CreateRouteBuffer( m_iRouteLength );

		if ( m_pusRoute )
		{
			m_iRouteLength  = 1;
			m_pusRoute[ 0 ] = m_usFinalWaypoint;
		}
	}
	else
	{
		u_int uPathfindingFlags	= 0xffffffff;

		if (m_pxBrain->GetActor()->GetClassification() == AVP_ENTITYCLASS_HUMAN)
		{
			uPathfindingFlags = AVP_AI_PATHFINDER_FLAG_HUMANOID;
		}
		else if ((m_pxBrain->GetActor()->GetClassification() == AVP_ENTITYCLASS_ALIEN) || (m_pxBrain->GetActor()->GetClassification() == AVP_ENTITYCLASS_ALIENFACEHUGGER))
		{
			uPathfindingFlags = AVP_AI_PATHFINDER_FLAG_ALIEN | AVP_AI_PATHFINDER_FLAG_HUMANOID;
		}


		// Full A* search.
		Asura_AI_Navigation_System::FindRouteCustom
		(
			Axon_Project_Brain::GetWorkingRouteBuffer(), 
			m_iRouteLength, 
			Axon_Project_Brain::GetWorkingRouteBufferLength(),
			m_usCurrentWaypoint, 
			m_usFinalWaypoint, 
			uPathfindingFlags, 
			Asura_AI_Navigation_System::DefaultGoalCostEstimateFunction, 
			Asura_AI_Navigation_System::DefaultTraversalCostFunction,
			Asura_AI_Navigation_System::DefaultGoalAchievedFunction, 
			AVP_Behaviour_Shared_RouteHelper::AVP_LinkFlagsTestFn
		);

		// we have a route greater length than zero
		if (m_iRouteLength)
		{
			m_pusRoute = Axon_Project_Brain::CreateRouteBuffer(m_iRouteLength);

			if (m_pusRoute)
			{
				Asura_Mem::Copy(m_pusRoute, Axon_Project_Brain::GetWorkingRouteBuffer(), sizeof(u_short) * m_iRouteLength);
			}
			else
			{
				m_iRouteLength = 0;
			}
		}
	}

	if (bStraightenRoute)
	{
		RemoveRedundantWaypoints();
	}

	return (m_iRouteLength > 0);
}

void AVP_Behaviour_Shared_RouteHelper::UpdateCurrentWaypoint( const Asura_Vector_3& xCurrentPosition )
{
#ifdef ASURA_USE_STREAMING
	if ( !m_uCurrentZoneHash )
	{
		// Have yet to determine which streaming zone we are in, so do a full search on all handlers.
		int iWaypoint = 0;
		Asura_AI_Navigation_System::FindWaypointAndZoneNearestTo( xCurrentPosition, m_uCurrentZoneHash, iWaypoint );
		m_usCurrentWaypoint = static_cast< u_short >( iWaypoint );
		return;
	}
#endif


	static float					fMaxWpRadiusSq	= 100.0f;
	Asura_AI_Navigation_Handler*	pxNavHandler	= GetCurrentNavHandler();
	int								iBestWpIndex	= -1;
	float							fBestWpDistSq	= 0.0f;

	for (int i = 0; i < pxNavHandler->GetNumberOfWaypoints(); i += 1)
	{
		const Asura_AI_NavVertex* pxWaypoint = pxNavHandler->GetWaypoint(i);

		if (pxWaypoint)
		{
			Asura_Vector_3	xDirectionToWp	= pxWaypoint->m_xPosition - xCurrentPosition;
			float			fDistToWpSq		= (xDirectionToWp).MagnitudeSquared();

			if (fDistToWpSq > fMaxWpRadiusSq)
			{
				//	Distance is greater than that of the maximum radius of a waypoint so go to the next
				continue;
			}
			else
			{
#ifdef ASURA_USE_NAV_VERTEX_NORMALS
				Asura_Matrix_3x3	xOrientation	= m_pxBrain->GetActor()->GetOrientation();
				Asura_Vector_3		xActorUp		= -Asura_Vector_3(xOrientation.e12, xOrientation.e22, xOrientation.e32);
				Asura_Vector_3		xWpNormal		= Asura_Vector_3(0.0f, 0.0f, 0.0f);

				pxWaypoint->GetNormal(xWpNormal);

				// Do a normal test, as the normal needs to be roughly the same as the -
				// actors up vector to be considered valid to be the current waypoint
				if (!xActorUp.ApproximatelyEquals(xWpNormal, 0.2f))
				{
					continue;
				}
				else
#endif
				{
					if ((iBestWpIndex == -1) || (fDistToWpSq < fBestWpDistSq))
					{
						iBestWpIndex = i;
						fBestWpDistSq = fDistToWpSq;
					}
				}
			}
		}
	}

	if (iBestWpIndex != -1)
	{
		m_usCurrentWaypoint = (unsigned short)iBestWpIndex;
	}
	else
	{
		// we didnt find one our way do it the old way
		m_usCurrentWaypoint = pxNavHandler->FindWaypointNearestTo(xCurrentPosition);
	}

#if 0
	Asura_AI_Navigation_Handler* pxNavHandler = GetCurrentNavHandler();
	if( !pxNavHandler ) return;

	if ( m_usCurrentWaypoint == USHRT_MAX )
	{
		// We'll have to do an exhaustive search.
		m_usCurrentWaypoint = pxNavHandler->FindWaypointNearestTo( xCurrentPosition );
		return;
	}

	// We can use our last value to attempt a best guess.
	float   fNearestDistanceSq = Asura_Maths::LargeFloat;
	u_short usNearestWaypoint  = USHRT_MAX;

	const Asura_AI_NavVertex* pxNavVertex = pxNavHandler->GetWaypoint( m_usCurrentWaypoint );

	Asura_Vector_3	xDirectionToCurrentWP	= pxNavVertex->m_xPosition - xCurrentPosition;
	float			fDistanceToWp			= xDirectionToCurrentWP.Magnitude();
	xDirectionToCurrentWP.Normalise();

	const Asura_LOS_InputData	xLOSInput	= Asura_LOS_InputData(xCurrentPosition, xDirectionToCurrentWP, m_pxBrain->GetActor()->GetGuid(), fDistanceToWp);
	const Asura_LOS_OutputData*	pxLOSOuput	= Asura_LOS::TestAgainstEnvironment(&xLOSInput);

	if (pxLOSOuput->m_fDistanceToIntersection < (fDistanceToWp - 0.0075f))
	{
		//		int i = 0;
	}

	if ( pxNavVertex )
	{			
		float fDistSq = ( pxNavVertex->m_xPosition - xCurrentPosition ).MagnitudeSquared();
		if ( fDistSq < pxNavVertex->m_fRadius * pxNavVertex->m_fRadius )
		{
			fNearestDistanceSq = fDistSq;
			usNearestWaypoint  = m_usCurrentWaypoint;
		}

		// Go through the current waypoint's connections too.
		for ( int i = 0; i < pxNavVertex->m_ucNumConnections; ++i )
		{
			const Asura_AI_NavConnection* pxConnection    = &pxNavVertex->m_pxConnections[ i ];
			const Asura_AI_NavVertex*     pxNextNavVertex = pxNavHandler->GetWaypoint( pxConnection->m_usIndex );

			fDistSq = ( pxNextNavVertex->m_xPosition - xCurrentPosition ).MagnitudeSquared();
			if ( fDistSq < fNearestDistanceSq && fDistSq < pxNextNavVertex->m_fRadius * pxNextNavVertex->m_fRadius )
			{
				fNearestDistanceSq = fDistSq;
				usNearestWaypoint  = pxConnection->m_usIndex;
			}
		}
	}

	if ( usNearestWaypoint < USHRT_MAX )
	{
		m_usCurrentWaypoint = usNearestWaypoint;
	}
	else
	{
		// No good, we'll have to do an exhaustive search.
		m_usCurrentWaypoint = pxNavHandler->FindWaypointNearestTo( xCurrentPosition );
	}
#endif
}

////////////////////////////////////////////////////////////////////////////////
// Function     : AVP_Behaviour_Shared_RouteHelper::AddWaypointWeighting
// Author       : Adam Law
// Date         : 15-Jun-2007
////////////////////////////////////////////////////////////////////////////////

void AVP_Behaviour_Shared_RouteHelper::AddWaypointWeighting()
{
	// Note: These values get multiplied by a scalar (m_fWaypointWeightFactor)
	// Max link length: Tools_Mode_Waypointing::s_fMaxLinkLength 
	unsigned char ucWaypointWeight = 0;

	Asura_AI_Navigation_Handler* pxNavHandler = GetCurrentNavHandler();
	Asura_Assertf( pxNavHandler, ("Unable to get current navigation handler.") );
	if( !pxNavHandler ) return;

	unsigned short usNumWaypoints = pxNavHandler->GetNumberOfWaypoints();

	for ( unsigned short int i = 0; i < usNumWaypoints; ++i )
	{
		const Asura_AI_NavVertex* pxNextNavVertex = pxNavHandler->GetWaypoint( i );

		if ( !pxNextNavVertex ) {
			Asura_Assertf( false, ("Unable to get waypoint.") );
			return;
		}

		if ( pxNextNavVertex->m_usFlags & ASURA_AI_NAVVERTEX_FLAG_CEILING )	{
			ucWaypointWeight = s_ucCeilingWeight;
		}
		else if ( pxNextNavVertex->m_usFlags & ASURA_AI_NAVVERTEX_FLAG_WALL ) {
			ucWaypointWeight = s_ucWallWeight;
		}
		else if ( pxNextNavVertex->m_usFlags & ASURA_AI_NAVVERTEX_FLAG_SLOPE ) {
			ucWaypointWeight = s_ucSlopeWeight;
		}
		else if ( pxNextNavVertex->m_usFlags & ASURA_AI_NAVVERTEX_FLAG_STEPS ) {
			ucWaypointWeight = s_ucStepsWeight;
		}
		// Floor
		else {
			ucWaypointWeight = s_ucFloorWeight;				
		}	
		Asura_AI_Navigation_System::AddWaypointWeight( i, ucWaypointWeight );
	}	
}

//-----------------------------------------------------------------------------------------------
// Function		: AVP_Behaviour_Shared_RouteHelper::PositionsAreInSameWaypointZone
// Created		: 05 / 06 / 07
// Author		: Gareth Thomas : GBT
// DESCRIPTION	: Finds the closest way points to the supplied position one and position two - 
//				  if the way points are in the same zone TRUE is returned.
//-----------------------------------------------------------------------------------------------
bool AVP_Behaviour_Shared_RouteHelper::PositionsAreInSameWaypointZone(Asura_Vector_3 pxPositionOne, Asura_Vector_3 pxPositionTwo)
{
	const Asura_AI_NavVertex* pxWaypointOne	= Asura_AI_Navigation_System::GetWaypoint(Asura_AI_Navigation_System::FindWaypointNearestTo(pxPositionOne));
	const Asura_AI_NavVertex* pxWaypointTwo	= Asura_AI_Navigation_System::GetWaypoint(Asura_AI_Navigation_System::FindWaypointNearestTo(pxPositionTwo));

	if (pxWaypointOne->m_ucZone == pxWaypointTwo->m_ucZone)
	{
		return true;
	}

	return false;
}

//--------------------------------------------------------------------------------
u_short AVP_Behaviour_Shared_RouteHelper::ResolveToValidPosition( Asura_Vector_3& xPosition )
{
	const u_short				usNearestWaypoint	= Asura_AI_Navigation_System::FindWaypointNearestTo(xPosition);
	const Asura_AI_NavVertex*	pxNearestWaypoint	= Asura_AI_Navigation_System::GetWaypoint( usNearestWaypoint );

	if(pxNearestWaypoint == NULL)
	{
		return(0);
	}
	else
	{
		const Asura_Vector_3	xWaypointToPosition(xPosition - pxNearestWaypoint->m_xPosition);

		const float				fWaypointToPosition	= xWaypointToPosition.Magnitude();

		// If the position is outside the nearest waypoint bring it 0.5m inside.
		if(fWaypointToPosition > pxNearestWaypoint->m_fRadius)
		{
			AVP_ServerEntity_Actor*	pxThisActor		= m_pxBrain->GetActor();
			const Asura_Matrix_3x3&	xOrientation	= pxThisActor->GetOrientation();

			const Asura_Vector_3	xUp(xOrientation.e12, xOrientation.e22, xOrientation.e32);
			Asura_Vector_3			xDirection(xWaypointToPosition / fWaypointToPosition);

			const float				fDotProduct	= xDirection * xUp;

			// Pick the closest point on the waypoint radius...
			xDirection	= xDirection - (xUp * fDotProduct);
			xPosition	= pxNearestWaypoint->m_xPosition + (xDirection * (pxNearestWaypoint->m_fRadius - 0.5f));
		}

		return usNearestWaypoint;
	}
}

void AVP_Behaviour_Shared_RouteHelper::UpdateCurrentDestination()
{
	if ( m_bIsInError )
	{
		return;
	}
	if ( m_iRouteLength == 0 )
	{
		Asura_Assert( false, "AVP_Behaviour_Shared_RouteHelper::CalcMovementVector - no route" );
		return;
	}

	const Asura_Vector_3& xCurrentPosition = m_pxBrain->GetActor()->GetPosition();
	UpdateCurrentWaypoint( xCurrentPosition );

	Asura_AI_Navigation_Handler* pxNavHandler = GetCurrentNavHandler();
	if ( !pxNavHandler ) 
	{
		return;
	}

	// Once we have reached our destination we don't allow any more state changes.
	if ( m_eRouteTraversalState == RTS_AT_DESTINATION )
	{
		return;
	}

	// Always check whether we are in our last waypoint.
	if ( m_eRouteTraversalState != RTS_IN_FINAL_WAYPOINT && m_eRouteTraversalState != RTS_MOVING_TO_DESTINATION && m_eRouteTraversalState != RTS_AT_DESTINATION && 
		pxNavHandler->IsPositionInWaypoint( xCurrentPosition, m_pusRoute[ m_iRouteLength - 1 ] ) )
	{
		// Check whether the destination is in this waypoint.
		if ( pxNavHandler->IsPositionInWaypoint( m_xFinalDestination, m_pusRoute[ m_iRouteLength - 1 ] ) )
		{
			// *** GBT ***
			// Need to make this on the surface!
			// use the normal to calculate

#ifdef ASURA_USE_NAV_VERTEX_NORMALS
			Asura_AI_NavVertex* pxWaypoint = pxNavHandler->GetWaypoint(m_pusRoute[m_iRouteLength - 1]);

			Asura_Vector_3 xWaypointNorm = Asura_Vector_3(0.0f, 0.0f, 0.0f);
			pxWaypoint->GetNormal(xWaypointNorm);

			Asura_Vector_3 xWaypointPos = pxWaypoint->m_xPosition;

			float fDistanceFromSurface = (xWaypointNorm * m_xFinalDestination) - (xWaypointNorm * xWaypointPos);
			m_xFinalDestination = m_xFinalDestination - (xWaypointNorm * fDistanceFromSurface);
#endif
			// It is - head straight for it.
			m_xCurrentDestination  = m_xFinalDestination;
			m_eRouteTraversalState = RTS_MOVING_TO_DESTINATION;
		}
		else
		{
			// It's not - head for the centre of the waypoint first.
			m_xCurrentDestination  = pxNavHandler->GetWaypoint( m_pusRoute[ m_iRouteLength - 1 ] )->m_xPosition;
			m_eRouteTraversalState = RTS_IN_FINAL_WAYPOINT;
		}
	}

	// Set the tolerance on our up vector to use when checking if we're at our destination
	// This is variable so we can set it relative to our destinations waypoint encompassing sphere
	// this should help stop Aliens not being able to reach their target point
	m_fAtTargetYTol = pxNavHandler->GetWaypoint( m_iRouteIndex )->m_fRadius;	

	switch ( m_eRouteTraversalState )
	{
	case RTS_BEGINNING_ROUTE:
		{
			// We're starting off - see if we're in our first waypoint.
			if ( pxNavHandler->IsPositionInWaypoint( xCurrentPosition, m_pusRoute[ m_iRouteIndex ] ) )
			{
				// We are - we should head for the 'exit' to not look stupid.
				const Asura_AI_NavVertex* pxThisWaypoint = pxNavHandler->GetWaypoint( m_pusRoute[ m_iRouteIndex ] );
				const Asura_AI_NavVertex* pxNextWaypoint = pxNavHandler->GetWaypoint( m_pusRoute[ m_iRouteIndex + 1 ] );
				Asura_Vector_3            xLinkDirection = ( pxNextWaypoint->m_xPosition - pxThisWaypoint->m_xPosition );
				xLinkDirection.Normalise();

				m_xCurrentDestination = pxThisWaypoint->m_xPosition + xLinkDirection * ( pxThisWaypoint->m_fRadius - 0.5f );
			}
			else
			{
				// We're not - we should head for the waypoint centre.
				m_xCurrentDestination = pxNavHandler->GetWaypoint( m_pusRoute[ m_iRouteIndex ] )->m_xPosition;
			}

			m_eRouteTraversalState = RTS_EN_ROUTE;

			m_EN_ROUTE_fTimeout = 0.f;

		} break;

	case RTS_EN_ROUTE:
		{
			// See if we've got to our current destination yet.
			if ( AtTargetPosition( xCurrentPosition, m_xCurrentDestination ) )
			{
				// Don't bother checking for final waypoint stuff - this is done above.
				m_iRouteIndex = Asura_Maths::Min( ++m_iRouteIndex, m_iRouteLength - 1 );
				Asura_AI_NavVertex* pxNextWaypoint = pxNavHandler->GetWaypoint( m_pusRoute[ m_iRouteIndex ] );
				m_xCurrentDestination = pxNextWaypoint->m_xPosition;

				m_EN_ROUTE_fTimeout = 0.0f;
			}
			else if (AtTargetPosition( xCurrentPosition, m_xCurrentDestination, 1.5f))
			{
				m_EN_ROUTE_fTimeout += Asura_Timers::GetGameTimeSlice();

				if (m_EN_ROUTE_fTimeout > 1.0f)
				{
					// Don't bother checking for final waypoint stuff - this is done above.
					m_iRouteIndex = Asura_Maths::Min( ++m_iRouteIndex, m_iRouteLength - 1 );
					Asura_AI_NavVertex* pxNextWaypoint = pxNavHandler->GetWaypoint( m_pusRoute[ m_iRouteIndex ] );
					m_xCurrentDestination = pxNextWaypoint->m_xPosition;

					m_EN_ROUTE_fTimeout = 0.0f;
				}
			}
			else if (AtTargetPosition( xCurrentPosition, m_xCurrentDestination, 2.5f))
			{
				m_EN_ROUTE_fTimeout += Asura_Timers::GetGameTimeSlice();

				if (m_EN_ROUTE_fTimeout > 2.0f)
				{
					// Don't bother checking for final waypoint stuff - this is done above.
					m_iRouteIndex = Asura_Maths::Min( ++m_iRouteIndex, m_iRouteLength - 1 );
					Asura_AI_NavVertex* pxNextWaypoint = pxNavHandler->GetWaypoint( m_pusRoute[ m_iRouteIndex ] );
					m_xCurrentDestination = pxNextWaypoint->m_xPosition;

					m_EN_ROUTE_fTimeout = 0.0f;
				}
			}
		} break;

	case RTS_MOVING_TO_DESTINATION:
		{
			// See if we've got to our current destination yet.
			if ( AtTargetPosition( xCurrentPosition, m_xCurrentDestination ) )
			{
				m_eRouteTraversalState = RTS_AT_DESTINATION;
				return;
			}
		} break;

	case RTS_IN_FINAL_WAYPOINT:
		{
			// See if we've got to our current destination yet.
			if ( AtTargetPosition( xCurrentPosition, m_xCurrentDestination ) )
			{
				m_xCurrentDestination  = m_xFinalDestination;
				m_eRouteTraversalState = RTS_MOVING_TO_DESTINATION;
			}
		} break;

	case RTS_AT_DESTINATION:
	case RTS_UNSET:
	case RTS_NUM_STATES:
	default:
		{
			Asura_Assert( false, "GetToPosition::OrientAlongRoute - state error" );
			m_bIsInError = true;
			return;
		} break;
	}	
}



bool AVP_Behaviour_Shared_RouteHelper::AtTargetPosition( const Asura_Vector_3& xCurrentPosition, const Asura_Vector_3& xTargetPosition, float fXZTolerance /* = 0.75f */ )
{
	const Asura_Vector_3 xDelta = xTargetPosition - xCurrentPosition;
	return ( Asura_Maths::Abs( xDelta.x ) < fXZTolerance && Asura_Maths::Abs( xDelta.y ) < m_fAtTargetYTol /*0.75f*/ && Asura_Maths::Abs( xDelta.z ) < fXZTolerance );
}

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

void AVP_Behaviour_Shared_RouteHelper::RemoveRedundantWaypoints()
{
#ifndef ASURA_NO_DEBUG_OUTPUT
	m_iPreSmoothedRouteLength = m_iRouteLength;
	m_pusPreSmoothedRoute     = Axon_Project_Brain::CreateRouteBuffer( m_iPreSmoothedRouteLength );
	if ( m_pusPreSmoothedRoute )
	{
		Asura_Mem::Copy( m_pusPreSmoothedRoute, m_pusRoute, m_iPreSmoothedRouteLength * sizeof( u_short ) );
	}
#endif

	for ( int iStartIndex = 0; iStartIndex < m_iRouteLength - 1; ++iStartIndex )
	{
		const Asura_Vector_3& xStartPosition = Asura_AI_Navigation_System::GetWaypoint( m_pusRoute[ iStartIndex ] )->m_xPosition;

		// Form sub-route between the start waypoint and each successive waypoint further along the route.
		int  iNumRemoved    = 0;
		bool bShortcutValid = true;
		// Note that there's no point in starting from the very next waypoint along, as we're guaranteed to be able to make it there.
		for ( int iEndIndex = iStartIndex + 2; iEndIndex < m_iRouteLength && bShortcutValid; ++iEndIndex )
		{
			const Asura_AI_NavVertex* pxEndWaypoint = Asura_AI_Navigation_System::GetWaypoint( m_pusRoute[ iEndIndex ] );
			const Asura_Vector_3      xEndLink      = ( pxEndWaypoint->m_xPosition - xStartPosition );
			const float               fEndLength    = xEndLink.Magnitude();

			// For each section of the sub-route, iterate along and ensure that we stay within the route space.
			for ( int iMidIndex = iStartIndex + 1; iMidIndex <= iEndIndex && bShortcutValid; ++iMidIndex )
			{
				if ( !WaypointValidForSmoothing( m_pusRoute[ iMidIndex ] ) )
				{
					// Waypoint cannot be shortcut. This is not a valid shortcut.
					bShortcutValid = false;
					continue;
				}

				const Asura_AI_NavVertex* pxMidWaypoint = Asura_AI_Navigation_System::GetWaypoint( m_pusRoute[ iMidIndex ] );
				const Asura_Vector_3      xMidLink      = ( pxMidWaypoint->m_xPosition - xStartPosition );
				const float               fMidLength    = xMidLink.Magnitude();

				Asura_Assert( fEndLength != 0.0f && fMidLength != 0.0f, "AVP_Behaviour_Shared_RouteHelper::RemoveRedundantWaypoints - algorithm error" );
				Asura_Assert( pxMidWaypoint->m_fRadius, "AVP_Behaviour_Shared_RouteHelper::RemoveRedundantWaypoints - bad waypoint data" );

				const float fPhi   = Asura_Maths::ArcCosine( ( xEndLink * xMidLink ) / ( fEndLength * fMidLength ) );
				const float fDelta = fMidLength * Asura_Maths::Sine( fPhi );
				if ( ( fDelta / pxMidWaypoint->m_fRadius ) > 0.8f )
				{
					// Outside the route space. This is not a valid shortcut.
					bShortcutValid = false;
				}
			}

			// If the shortcut is valid record that we can remove the waypoint.
			if ( bShortcutValid )
			{
				++iNumRemoved;
			}
		}

		// If we have managed to shortcut any of the route then remove the redundant waypoints.
		for ( int i = iStartIndex + 1; i < m_iRouteLength - iNumRemoved; ++i )
		{
			m_pusRoute[ i ] = m_pusRoute[ i + iNumRemoved ];
		}
		m_iRouteLength -= iNumRemoved;
	}
}

bool AVP_Behaviour_Shared_RouteHelper::WaypointValidForSmoothing( const u_short usWaypointIndex )
{
	return true;
}

//-----------------------------------------------------------------------------------
// Function     : static AVP_Behaviour_Shared_RouteHelper::AVP_Human_LinkFlagsTestFn
// Author       : Gareth Thomas
// Date         : 23 / 08 / 07
//-----------------------------------------------------------------------------------
bool AVP_Behaviour_Shared_RouteHelper::AVP_LinkFlagsTestFn(const unsigned short usLinkFlags, const unsigned int uPathfinderFlags)
{
	unsigned short HumanAndAlienLinkFlags = AVP_AI_CONNECTION_FLAG_HUMANOID & AVP_AI_CONNECTION_FLAG_HUMANOID;

	// This is a human and alien link
	if ((usLinkFlags & HumanAndAlienLinkFlags) && (uPathfinderFlags & (AVP_AI_PATHFINDER_FLAG_HUMANOID | AVP_AI_PATHFINDER_FLAG_ALIEN)))
	{
		return false;
	}

	const bool bCanFall = (uPathfinderFlags & ASURA_AI_PATHFINDER_FLAGS_CANFALL) ? true : false;

	if ((usLinkFlags & ASURA_AI_CONNECTION_FLAGS_FALL) && !(bCanFall))
	{
		// it's a fall link and we can't fall - ignore
		return true;
	}

	const bool bCanUseHumanoidLinks = (uPathfinderFlags & AVP_AI_PATHFINDER_FLAG_HUMANOID) ? true : false;
	if ((usLinkFlags & AVP_AI_CONNECTION_FLAG_HUMANOID) && !(bCanUseHumanoidLinks))
	{
		// it's a humanoid link and we can't use that type - ignore
		return true;
	}

	const bool bCanUseAlienLinks = (uPathfinderFlags & AVP_AI_PATHFINDER_FLAG_ALIEN) ? true : false;
	if ((usLinkFlags & AVP_AI_CONNECTION_FLAG_ALIEN) && !(bCanUseAlienLinks))
	{
		// it's a alien link and we can't use that type - ignore
		return true;
	}

	if ((usLinkFlags & AVP_AI_CONNECTION_FLAG_TRANSITION) && !(bCanUseAlienLinks))
	{
		return true;
	}

	return false;
}

Asura_AI_Navigation_Handler* AVP_Behaviour_Shared_RouteHelper::GetCurrentNavHandler() const
{
#ifdef ASURA_USE_STREAMING
	Asura_AI_Navigation_Handler* pxHandler = m_uCurrentZoneHash ? Asura_AI_Navigation_System::GetHandler(m_uCurrentZoneHash) : &Asura_AI_Navigation_System::GetCurrentHandler();
	Asura_Assert(pxHandler,"Failed to get navigation handler for movement system");
	return pxHandler;
#else
	return &Asura_AI_Navigation_System::GetCurrentHandler();
#endif
}


////////////////////////////////////////////////////////////////////////////////
// Function     : AVP_Behaviour_Shared_RouteHelper::OrientAlongRoute
// Author       : Andy Tate
// Date         : 16-May-2007
////////////////////////////////////////////////////////////////////////////////

void AVP_Behaviour_Shared_RouteHelper::OrientAlongRoute(const float fTimeStep,
														const bool bBackwards,
														AVP_ServerEntity_Actor* pxOtherActor,
														const bool bCanTurnInMidAir)
{
	AVP_ServerEntity_Actor*	pxThisActor	= m_pxBrain->GetActor();
	Asura_AVector3			xToDestination;	

	if ( !pxThisActor )
	{
		Asura_Assert( false, "Invalid actor pointer." );
	}		

	// Calculate the destination...
	UpdateCurrentDestination();	

	// Only orient if we are in contact with a surface (looks a bit funny if we turn in mid air)	
	AVP_Actor_PhysicsObjectHandler* pxObjHandler;
	pxObjHandler = pxThisActor->GetPhysicsObjectHandler();

	if ( pxObjHandler )
	{
		Asura_Physics_Object* pxObject = pxObjHandler->GetPhysicsObject();		

		if( pxObject && !pxObject->IsSupported() && !bCanTurnInMidAir )
			return;
	}

	// Calculate the desired orientation...
	if(bBackwards)
	{
		xToDestination	= pxThisActor->GetPosition() - GetCurrentDestination();		
	}
	else
	{
		xToDestination	= GetCurrentDestination() - pxThisActor->GetPosition();
	}
	// Orient the actor(s)...
	pxThisActor->OrientToDirection(xToDestination, fTimeStep);

	if( pxOtherActor )
	{
		pxOtherActor->OrientToDirection(xToDestination, fTimeStep);
	}
}

/// End of function 'AVP_Behaviour_Shared_RouteHelper::OrientAlongRoute'. ///

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