//////////////////////////////////////////////////////////////////////////////
//
// AVP_Behaviour_Alien_Movement.cpp
//
// Copyright (C) 2007 Rebellion. All Rights Reserved.
//
//////////////////////////////////////////////////////////////////////////////

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

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

// header
#include "Behaviour/BehaviourTypes/Alien/AVP_Behaviour_Alien_Movement.h"

// asura
#include "AI/Asura_AI_Navigation.h"
#include "Blueprint/Asura_Blueprint_System.h"
#include "Core/Asura_Timers.h"
#ifndef ASURA_NO_DEBUG_OUTPUT
#include "Debugging/Asura_DebugLineRender.h"
#include "Rendering/Asura_Font_System.h"
#endif
#include "Entity/Controller/AVP_ServerEntity_Squad_Alien.h"
#include "Entity/Locations/Spline/GamesceneSpline/Asura_ServerNode_GS2Spline.h"
#include "Network/Asura_Message.h"
#include "Network/Asura_Network.h"

// avp
#include "Actor/Vertebrates/Alien/AVP_Alien_AIAnimStateMachine.h"
#include "Blueprint/AVP_Blueprint_HashIDs.h"
#include "Core/AVP_LOS.h"
#include "Core/AVP_Plane.h"
#include "Entity/AVP_Messages.h"
#include "Entity/Controller/AVP_ServerEntity_Team_Alien.h"
#include "Entity/Locations/OrientedNode/GamesceneNode/AVP_ServerNode_GS2Node.h"
#include "Entity/PhysicalObject/Actor/AVP_ServerEntity_Actor.h"
#include "Entity/PhysicalObject/Projectile/AVP_ServerProjectile_GenericProjectile.h"
#include "Mesh/AVP_PolygonFlags.h"
#include "Moves/Vertebrates/Alien/AVP_Alien_MovesJump.h"
#include "Moves/Vertebrates/Alien/AVP_Alien_MovesVent.h"

// axon
#include "AnimTemplates/Axon_Gamescene_AnimTemplates.h"
#include "AnimTemplates/Axon_Gamescene_AnimTemplate_System.h"
#include "Behaviour/BehaviourParamsTypes/Axon_BehaviourParams_GS2Spline.h"
#include "Behaviour/BehaviourTypes/AVP_Behaviour_Types.h"
#include "Behaviour/BehaviourTypes/Axon_Behaviour_GS2Spline.h"
#include "Behaviour/BehaviourTypes/Axon_Behaviour_GS2Actor.h"
#include "Core/Axon_Messages.h"
#include "Core/AVP_Waypoint_Manager.h"
#include "Core/BrainTypes/AVP_Brain_Actor_Alien.h"
#include "Knowledge/AVP_Knowledge_System_AlienTeam.h"

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

// We're only allowed to jump every now and then.
static const float fJUMP_PERIOD = 2.5f;
// We can only jump if we've not been damaged recently.
static const float fJUMP_DAMAGE_DISABLE_PERIOD = 2.0f;
// We'll only jump if there is danger within a certain range.
static const float fJUMP_TRIGGER_RANGE = 4.0f;

static const float fHIT_DELAY_PERIOD = 2.0f;

DEBUGVAR float dbg_fCollisionAvoidOverallTightMultiplier    = 0.15f;
DEBUGVAR float dbg_fCollisionAvoidOverallScalar     = 1.5f;
DEBUGVAR float dbg_fCollisionAvoidLocalScalar       = 2.0f;
DEBUGVAR float dbg_fLightAvoidLocalScalar			= 0.5f;
DEBUGVAR float dbg_fCollisionAvoidRangeBias         = 0.8f;
DEBUGVAR float dbg_fCollisionAvoidMinimumAngleDeg   = 35.0f;
DEBUGVAR float dbg_fCollisionAlwayRightMinimumAngleDeg = 15.0f;
DEBUGVAR float dbg_fCollisionPredictionTime		    = 0.5f;
DEBUGVAR float dbg_fCollisionMaxVelocityEffect		= 1.0f;
DEBUGVAR float dbg_fCollisionMaxFrontalMultiplier	= 2.0f;
DEBUGVAR float dbg_fCollisionMinRange				= 0.5f;
DEBUGVAR float dbg_fLightMinRange					= 1.0f;
DEBUGVAR float dbg_fCollisionAlienExtraRange		= 1.5f;
DEBUGVAR float dbg_fTargetForwardCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(30.0f));
DEBUGVAR float  dbg_fLightRangeFactor = 1.5f;
DEBUGVAR bool  dbg_bCollisionAvoidDoGlobalAngleHack = true;

DEBUGVAR float dbg_fMaxArrivalRange = 3.0f;
DEBUGVAR float dbg_fMinArrivalRange = 0.75f;
DEBUGVAR float dbg_fTimeToIdle = 0.1f;

//////////////////////////////////////////////////////////////////////////////
//
// Link Traversers
//
//////////////////////////////////////////////////////////////////////////////

AVP_Brain_Actor_Alien* LinkTraverser::GetAlienBrain()
{
	return GetMovement()->GetAlienBrain();
}

StandardLinkTraverser::StandardLinkTraverser()
: m_eState(NORMAL)
, m_fTimeInState(0.0f)
, m_xFollowPoint( Asura_Maths::ZeroVector3 )
, m_usFollowPointWPIndex(ASURA_INVALID_WAYPOINT_ID)
, m_usFlags( 0 )
, m_xFirstSplinePosition( Asura_Maths::ZeroVector3 )
, m_xLastSplinePosition( Asura_Maths::ZeroVector3 )
, m_usPrevRouteIndex( 0 )
, m_fLengthUntilNonStandardLink( 0.0f )
, m_fTimeInStuckOnPath( 0.0f )
, m_fFramesInStuckOnPath( 0.0f )
, m_fTimeCheckOrientationAndPositionStuck( 0.0f )
, m_fFramesCheckOrientationAndPositionStuck( 0.0f )
, m_fMaxRouteTraversedDistance( 0.0f )
, m_fLastMaxRouteFraction( 0.0f )
, m_xLastOrientation( Asura_Maths::IdentityMatrix3x3 )
, m_xLastPosition( Asura_Maths::ZeroVector3 )
#ifndef ASURA_NO_DEBUG_OUTPUT
, m_xFollowSteering( Asura_Maths::ZeroVector3 )
, m_xAvoidanceSteering( Asura_Maths::ZeroVector3 )
, m_xFinalSteering( Asura_Maths::ZeroVector3 )
#endif
{
}

void StandardLinkTraverser::OnNewRoute()
{
	PARENT::OnNewRoute();

	m_bStuck = false; 
	m_bTryToUnstuckByJumping = false;

	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	// Calculate the first and last spline positions for our interpolation.
	const Asura_Matrix_3x3& xOurOrientM = GetAlienBrain()->GetActor()->GetOrientation();
	const Asura_Vector_3    xOurForward( xOurOrientM.e13, xOurOrientM.e23, xOurOrientM.e33 );
	if ( xNavData.m_usRouteLength > 1 )
	{
		const Asura_AI_NavVertex* pxWPZero = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ 0 ] );
		m_xFirstSplinePosition = pxWPZero->m_xPosition - ( xOurForward * 5.0f );

		const Asura_AI_NavVertex* pxWPNMinusOne = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ xNavData.m_usRouteLength - 2 ] );
		const Asura_AI_NavVertex* pxWPN			= Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ xNavData.m_usRouteLength - 1 ] );
		m_xLastSplinePosition = ( pxWPN->m_xPosition * 2.0f ) - pxWPNMinusOne->m_xPosition;

		//Asura_Vector_3 xToWPN = ( pxWPN->m_xPosition - pxWPNMinusOne->m_xPosition );
		//// Resolve the vector into the plane at the final waypoint.

	}
	else
	{
		const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();

		m_xFirstSplinePosition = xOurPosition - ( xOurForward * 5.0f );
		m_xLastSplinePosition  = ( xNavData.m_xDestinationPosition * 2.0f ) - xOurPosition;
	}
}

bool StandardLinkTraverser::ShouldStart()
{
	// We can always start because we're the fall-back.
	return true;
}

bool StandardLinkTraverser::CanBeInterrupted()
{
	return m_eState != SHARP_TURN;
}

void StandardLinkTraverser::Start()
{
	PARENT::Start();

	m_fTimeInStuckOnPath = 0.0f;
	m_fFramesInStuckOnPath = 0.0f;
	m_fTimeCheckOrientationAndPositionStuck = 0.0f;
	m_fFramesCheckOrientationAndPositionStuck = 0.0f;
	m_fMaxRouteTraversedDistance = 0.0f;
	m_fLastMaxRouteFraction = 0.0f;
	m_xLastOrientation = GetAlienBrain()->GetActor()->GetOrientation();
	m_xLastPosition = GetAlienBrain()->GetActor()->GetPosition();
	m_usFlags = 0;
	m_fTimeInState = 0.0f;

	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	m_usPrevRouteIndex = xNavData.m_usRouteSegmentIndex;
	CalculateLengthUntilNonStandardLink();
	CalculateIsConvexRouteSegment();
	CalculateSplineSegmentParams();
	UpdateFollowPoint();

 	m_eState = NORMAL;
  
//   	float fTimeInIdle = 0.0f;
//   	bool bInIdle = GetMovement()->IsInIdle(fTimeInIdle);
//   	if ( xNavData.m_usRouteSegmentIndex == 0 && xNavData.m_usRouteLength > 1 && bInIdle )
//   	{
//   		SwitchToState(PREPARE_TO_SHARP_TURN);
//   	}
}

bool StandardLinkTraverser::HandleMessage( const Asura_Message* pxMessage )
{
	if ( pxMessage->GetMsgID() == ASURA_MSG_MOVECHAINCOMPLETE )
	{
		if (m_eState == SHARP_TURN)
		{
			SwitchToState(NORMAL);
			return true;
		}
	}

	return PARENT::HandleMessage( pxMessage );
}


DEBUGVAR float dbg_fMediumTurnCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(50.0f));
DEBUGVAR float dbg_fSharpTurnCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(90.0f));
DEBUGVAR float dbg_fLargeTurnCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(70.0f));
DEBUGVAR float dbg_fLargeReorientCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(30.0f));

void StandardLinkTraverser::PerFrameUpdate( const float fTimeslice )
{
	m_fTimeInState += fTimeslice;

	PARENT::PerFrameUpdate( fTimeslice );
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	if (AVP_Navigation::IsRouteBlocked(xNavData) )
	{
		m_bStuck = true;
		return;
	}

	UpdateFollowPoint();

	const Asura_Vector_3 &xNextWPNormal = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ m_usFollowPointWPIndex ] )->GetNormal(); 
	const Asura_Vector_3 xOurNormal = Asura_Vector_3(-GetAlienBrain()->GetActor()->GetCurrentGravityDirection());

	bool bLargeAvoidanceSteering = false;
	switch (m_eState)
	{		
	case NORMAL:
		{
			//Asura_Vector_3 xRouteDirection = ( m_xFollowPoint - GetAlienBrain()->GetActor()->GetPosition() );
			//GetAlienBrain()->ResolveToOurPlane(xRouteDirection, xRouteDirection);
			//xRouteDirection.Normalise();
			Asura_Vector_3 xCurrentDirection;
			GetAlienBrain()->GetActor()->GetHeadFowardVector(xCurrentDirection);

			Asura_Vector_3 xFollowPointSteering;
			GetMovement()->CalculatePointSteering( m_xFollowPoint, xFollowPointSteering );

			Asura_Vector_3 xAvoidanceSteering = Asura_Maths::ZeroVector3;
			CalculateAvoidanceSteering( xFollowPointSteering, xAvoidanceSteering );
			Asura_Vector_3 xFinalSteering = xFollowPointSteering + xAvoidanceSteering;

			Asura_Vector_3 xFinalDirection = xFinalSteering; 
			GetAlienBrain()->ResolveToOurPlane(xFinalDirection, xFinalDirection);

			xFinalDirection.Normalise();

			float fTurnCosine = xCurrentDirection * xFinalDirection;

// 			if (fTurnCosine < dbg_fSharpTurnCosine)
// 			{
// 				Asura_Hash_ID uTurnMoveHashID;
// 				Asura_Matrix_3x3 xTurnOrientation;
// 				GetMovement()->CalculateTurn(xFinalDirection, uTurnMoveHashID, xTurnOrientation);
// 				GetMovement()->RequestTurn(uTurnMoveHashID, xTurnOrientation);
// 				SwitchToState(SHARP_TURN);
// 				return;
// 			}
// 			else 
			if (fTurnCosine < dbg_fLargeTurnCosine )
			{
				GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_SLOW);
			}		
			else if (xNextWPNormal * xOurNormal < dbg_fLargeReorientCosine ) // large reorient soon
			{
				GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_MEDIUM);
			}
			else if (fTurnCosine < dbg_fMediumTurnCosine)
			{
				GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_MEDIUM);
			}
			else
			{
				GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_FAST);
			}


			if (xAvoidanceSteering.MagnitudeSquared() > 0.25 * xFollowPointSteering.MagnitudeSquared())
			{
				bLargeAvoidanceSteering = true;
			}

			if (!xFinalSteering.ApproximatelyEquals(Asura_Maths::ZeroVector3))
			{
				xFinalSteering.Normalise();
				GetMovement()->SetAllDirections( xFinalSteering );
			}
#ifndef ASURA_NO_DEBUG_OUTPUT
			m_xFollowSteering = xFollowPointSteering;
			m_xAvoidanceSteering = xAvoidanceSteering;
			m_xFinalSteering = xFinalSteering;
#endif

			// If we've moved on to the next segment then reset the stuck detection stuff.
			const u_short usRouteIndex = GetAlienBrain()->GetNavData().m_usRouteSegmentIndex;
			if ( usRouteIndex > m_usPrevRouteIndex )
			{
				CalculateIsConvexRouteSegment();				
				CalculateLengthUntilNonStandardLink();
				CalculateSplineSegmentParams();
				m_usPrevRouteIndex = usRouteIndex;
			}
			UpdateLengthsWithAnimSpeed( fTimeslice );
		}
		break;
	case PREPARE_TO_SHARP_TURN:
		{
			float fTimeInIdle;
			bLargeAvoidanceSteering = true;
			bool bIsIdle = GetMovement()->IsInIdle(fTimeInIdle);
			// Asura_Assert(bIsIdle, "Idle state mismatch");
			if (bIsIdle && fTimeInIdle > dbg_fTimeToIdle || m_fTimeInState > 4.0f * dbg_fTimeToIdle )
			{
				SwitchToState(SHARP_TURN);
			}
		}
		break;
	case SHARP_TURN:
		{
// 			bLargeAvoidanceSteering = true;
// 			AVP_Vertebrate_AIAnimStateMachine* pxVertStateHandler = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetAlienBrain()->GetActor()->GetAIAnimStateMachine() );
// 			if ( pxVertStateHandler->GetCurrentState() ==  AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_MEDIUM)
// 			{
// 				SwitchToState(NORMAL);
// 			}
// 			else if (m_fTimeInState > 2.0f)
// 			{
// 				Asura_Assert(false, "too much time in sharp turn");
// 				SwitchToState(NORMAL);
// 			}
// 			break;

		}
		break;
	}


	UpdateStuckDetection( fTimeslice, bLargeAvoidanceSteering );

}


void StandardLinkTraverser::Stop()
{
	m_usFlags = 0;

	if (m_eState != NORMAL)
	{
		GetAlienBrain()->SetSharpTurn(false);
	}

	PARENT::Stop();
}

#ifndef ASURA_NO_DEBUG_OUTPUT
void StandardLinkTraverser::RenderDebugInfo()
{
	static const char* s_aszStrings[] =
	{
		"NORMAL",
		"PREPARE_TO_SHARP_TURN",
		"SHARP_TURN",
	};

	PARENT::RenderDebugInfo();

	Asura_Font_System::Print( GetAlienBrain()->GetDebugOutputPos().x, GetAlienBrain()->GetDebugOutputPos().y, 0.5f, 0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING,
		"StandardLinkTraverser - %s standard length: %0.2f", s_aszStrings[m_eState], m_fLengthUntilNonStandardLink);
	GetAlienBrain()->GetDebugOutputPos().y += 10.0f;

	Asura_DebugLineRender::AddSphere( m_xFollowPoint, 0.25f, 0xff00ff00, 4, 2 );
	Asura_Vector_3 xEyePosition = GetAlienBrain()->GetActor()->GetPosition();
	GetAlienBrain()->GetActor()->GetWorldSpaceMarkerPoint( 0xe5a85d04, &xEyePosition, NULL );	// target_2
	xEyePosition -= Asura_Vector_3(GetAlienBrain()->GetActor()->GetCurrentGravityDirection()) * .5f;

	Asura_DebugLineRender::AddArrow( xEyePosition, xEyePosition + m_xFollowSteering * 2.0f, 0xff00f000 );		// green
	Asura_DebugLineRender::AddArrow( xEyePosition, xEyePosition + m_xAvoidanceSteering * 2.0f, 0xfff00000 );		// red
	Asura_DebugLineRender::AddArrow( xEyePosition, xEyePosition + m_xFinalSteering * 2.0f, 0xfff0f0f0 );		// white


}
#endif

void StandardLinkTraverser::CalculateIsConvexRouteSegment()
{
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	m_bIsConvexRouteSegment = false;

	for ( Asura_LinkList_ConstIt< AVP_Alien_Movement::TransitionData > xTransitionIt( GetMovement()->GetTransitionDataList() );
		!xTransitionIt.Done();
		xTransitionIt.Next() )
	{
		const AVP_Alien_Movement::TransitionData* pxTransition = xTransitionIt.GetCurrent();
		if ( pxTransition->m_usRouteIndex == xNavData.m_usRouteSegmentIndex )
		{
			m_bIsConvexRouteSegment = pxTransition->IsConvex();
			break;
		}
		else if ( pxTransition->m_usRouteIndex > xNavData.m_usRouteSegmentIndex )
		{
			break;
		}
	}
}

void StandardLinkTraverser::CalculateLengthUntilNonStandardLink()
{
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	m_fLengthUntilNonStandardLink = 0.0f;

	for ( u_short usSegmentIndex = xNavData.m_usRouteSegmentIndex; usSegmentIndex < xNavData.m_usRouteLength - 1; ++usSegmentIndex )
	{
		const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usSegmentIndex ] );
		const Asura_AI_NavConnection* pxLink = pxFromWP->GetLinkTo( xNavData.m_pusRoute[ usSegmentIndex + 1 ] );
		if ( pxLink )
		{
			if ( AVP_Navigation::IsStandardLink( static_cast< u_short >( pxLink->GetFlags() ) ) )
			{
				m_fLengthUntilNonStandardLink += pxLink->GetLength();
			}
			else
			{
				break;
			}
		}
		else
		{
			// If we can't find a link then this will be a standard route segment, as we don't smooth over transitions/etc.
			const Asura_AI_NavVertex* pxToWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usSegmentIndex + 1 ] );
			m_fLengthUntilNonStandardLink += ( pxToWP->GetPosition() - pxFromWP->GetPosition() ).Magnitude();
		}
	}
}

void StandardLinkTraverser::CalculateSplineSegmentParams()
{
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex ] );

	// Find the relevant links.
	const Asura_AI_NavConnection* pxLinkBefore = NULL;
	const Asura_AI_NavConnection* pxLink	   = NULL;
	const Asura_AI_NavConnection* pxLinkAfter  = NULL;
	if ( xNavData.m_usRouteSegmentIndex > 0 )
	{
		const Asura_AI_NavVertex* pxBeforeWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex - 1 ] );
		pxLinkBefore = pxBeforeWP->GetLinkTo( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex ] );
	}
	if ( xNavData.m_usRouteSegmentIndex < xNavData.m_usRouteLength - 1 )
	{
		pxLink = pxCurrentWP->GetLinkTo( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex + 1 ] );
	}
	if ( xNavData.m_usRouteSegmentIndex < xNavData.m_usRouteLength - 2 )
	{
		const Asura_AI_NavVertex* pxAfterWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex + 1 ] );
		pxLinkAfter = pxAfterWP->GetLinkTo( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex + 2 ] );
	}

	// Determine whether the segments should be straight or not.
	// We do a bit of jiggery-pokery to get the spline to straighten up before and after a transition but remain curved once we're on a transition.
	// Straight before if:
	// - at the beginning of the route.
	m_bStraightBefore = ( xNavData.m_usRouteSegmentIndex == 0 );
	// - was a transition.
	m_bStraightBefore |= ( pxLinkBefore && AVP_Navigation::IsTransitionLink( static_cast< u_short >( pxLinkBefore->GetFlags() ) ) );
	// - is a transition.
	m_bStraightBefore |= ( pxLink && AVP_Navigation::IsTransitionLink( static_cast< u_short >( pxLink->GetFlags() ) ) );

	// Straight if:
	// - is a straight route.
	m_bStraight = ( xNavData.m_usRouteLength < 2 );

	// Straight after if:
	// - at the end of the route.
	m_bStraightAfter = ( xNavData.m_usRouteSegmentIndex == xNavData.m_usRouteLength - 2 );
	// - is a transition.
	m_bStraightAfter |= ( pxLink && AVP_Navigation::IsTransitionLink( static_cast< u_short >( pxLink->GetFlags() ) ) );
	// - will be a transition.
	m_bStraightAfter |= ( pxLinkAfter && AVP_Navigation::IsTransitionLink( static_cast< u_short >( pxLinkAfter->GetFlags() ) ) );
}

void StandardLinkTraverser::UpdateFollowPoint()
{
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	const float fLookAheadDistance = ( m_bIsConvexRouteSegment ? 1.5f : 2.5f );

	const Asura_Vector_3& xCurrentPosition = GetAlienBrain()->GetActor()->GetPosition();
	Asura_Vector_3 xActorGravityDirection;
	GetAlienBrain()->GetActor()->GetPhysicsObjectHandler()->GetCurrentGravityDirection( xActorGravityDirection );

	GetMovement()->FindNearestPointOnRoute( xNavData, xCurrentPosition,	-xActorGravityDirection, fLookAheadDistance, m_xFollowPoint, m_usFollowPointWPIndex, true);

	m_fMaxRouteTraversedDistance = Asura_Maths::Max( m_fMaxRouteTraversedDistance, GetMovement()->m_fRouteTraversedDistance);
}


void StandardLinkTraverser::CalculateAvoidanceSteering( const Asura_Vector_3& xFollowPointSteering, Asura_Vector_3& xSteering )
{
	// Avoiding both nearby peeps and lights.

	const Asura_Matrix_3x3& xOurOrientation = GetAlienBrain()->GetActor()->GetOrientation();
	const Asura_Vector_3    xOurNormal( -GetAlienBrain()->GetActor()->GetCurrentGravityDirection() );
	const Asura_Vector_3 xRight(xOurOrientation.e11, xOurOrientation.e21, xOurOrientation.e31);
	const Asura_Vector_3 xForward(xOurOrientation.e13, xOurOrientation.e23, xOurOrientation.e33);

	Asura_Vector_3 xNormalisedFollowPointSteering = xFollowPointSteering;
	xNormalisedFollowPointSteering.Normalise();
	const Asura_Vector_3 & xOurPosition = GetAlienBrain()->GetActor()->GetPosition();
	u_int uNumAvoidVectors = 0;

	const float fAngleHackMinAngle	  = Asura_Maths::DegToRad( dbg_fCollisionAvoidMinimumAngleDeg );
	const float fAngleHackMinAngleDot = Asura_Maths::Cosine( fAngleHackMinAngle );
	const float fAngleHackAlwaysRightAngle  = Asura_Maths::DegToRad( dbg_fCollisionAlwayRightMinimumAngleDeg );
	const float fAngleHackAlwaysRightAngleDot = Asura_Maths::Cosine( fAngleHackAlwaysRightAngle );

	float fEnvMultiplier = 1.0f;

	if (GetAlienBrain()->IsTightAvoidance())
	{
		fEnvMultiplier = dbg_fCollisionAvoidOverallTightMultiplier;
	}


	const u_int uNumNearbyGuids = GetAlienBrain()->GetNumNearbyGuids();
	for ( u_int uNearbyGuid = 0; uNearbyGuid < uNumNearbyGuids; ++uNearbyGuid )
	{
		if (uNearbyGuid == GetMovement()->GetIgnoreTargetForAvoidCalcuation())
		{
			continue;
		}

		const AVP_Brain_Actor::NearbyGuidData& xNearbyGuidData = GetAlienBrain()->GetNearbyGuidData()[ uNearbyGuid ];

		for (int i = 0; i < 1; ++i) // Should be 2 - for now don't use velocity 
		{
			Asura_Vector_3 xVelocityEffect = i == 0 ? Asura_Maths::ZeroVector3 : xNearbyGuidData.m_xVelocity * dbg_fCollisionPredictionTime;
			if (xVelocityEffect.MagnitudeSquared() > dbg_fCollisionMaxVelocityEffect * dbg_fCollisionMaxVelocityEffect)
			{
				xVelocityEffect.Normalise(); xVelocityEffect *= dbg_fCollisionMaxVelocityEffect;
			}

			Asura_Vector_3 xFromVector = xOurPosition - (xNearbyGuidData.m_xPosition + xVelocityEffect );
			float fRange = xFromVector.Normalise();
			fRange = Asura_Maths::Max( fRange - dbg_fCollisionAvoidRangeBias, dbg_fCollisionMinRange );

			// If this is a fellow Alien and it's taking damage then we avoid it even more.
			if ( xNearbyGuidData.m_uClassification == AVP_ENTITYCLASS_ALIEN && xNearbyGuidData.m_bTakingDamage )
			{
				fRange = Asura_Maths::Max( fRange - dbg_fCollisionAlienExtraRange, dbg_fCollisionMinRange );
			}


			// We need to resolve the from vector into our plane using our normal.
			Asura_Vector_3 xPlaneVector = xFromVector ^ xOurNormal;
			xPlaneVector = xOurNormal ^ xPlaneVector;
			xPlaneVector.Normalise();

			float fMultiplier = 0.5f * ( 1.0f + (-xPlaneVector * xForward) );
			if ((-xPlaneVector * xForward) > dbg_fTargetForwardCosine)
			{
				fMultiplier *= dbg_fCollisionMaxFrontalMultiplier;
			}
			else
			{
				fMultiplier *= fEnvMultiplier;
			}

			xSteering += xPlaneVector * fMultiplier * ( dbg_fCollisionAvoidLocalScalar / ( fRange ) );
			++uNumAvoidVectors;
		}

	}

 	if (GetMovement()->GetAvoidLight() && !GetAlienBrain()->IsTightAvoidance())
	{
		const u_int uNumLights = AVP_ServerEntity_Team_Alien::GetInstance()->GetNumLights();
		for ( u_int uLight = 0; uLight < uNumLights; ++uLight )
		{

			const AVP_ServerEntity_Team_Alien::LightInfo& xLightInfo = AVP_ServerEntity_Team_Alien::GetInstance()->GetLightInfo( uLight );
			Asura_Vector_3 xClosestPositionOnLightDirection;
			Asura_Maths::GetNearestPointOnLineSegment(xLightInfo.m_xPosition, xLightInfo.m_xSourcePosition, xOurPosition, xClosestPositionOnLightDirection);
			if ((xOurPosition-xClosestPositionOnLightDirection).MagnitudeSquared() < xLightInfo.m_fRadius * xLightInfo.m_fRadius) // if we already inside the light then just ignore it ...
			{
				continue;
			}
			Asura_Vector_3 xLightDirection = xLightInfo.m_xPosition - xLightInfo.m_xSourcePosition;
			xLightDirection.Normalise();
			for (float fDistanceFromSource = 0.0f; fDistanceFromSource < xLightInfo.m_fDistance + xLightInfo.m_fRadius * dbg_fLightRangeFactor; fDistanceFromSource += 2.0f )
			{
				Asura_Vector_3 xPosition = xLightInfo.m_xSourcePosition + xLightDirection * fDistanceFromSource;
				Asura_Vector_3 xFromVector = xOurPosition - xPosition;
				float fRange= xFromVector.Normalise();
				fRange = Asura_Maths::Max(fRange - xLightInfo.m_fRadius - dbg_fCollisionAlienExtraRange, dbg_fLightMinRange);

				// We need to resolve the from vector into our plane using our normal.
				Asura_Vector_3 xPlaneVector = xFromVector ^ xOurNormal;
				xPlaneVector = xOurNormal ^ xPlaneVector;
				xPlaneVector.Normalise();

				float fMultiplier = 0.5f * ( 1.0f + (-xPlaneVector * xForward) );
				if ((-xPlaneVector * xForward) > dbg_fTargetForwardCosine)
				{
					fMultiplier *= dbg_fCollisionMaxFrontalMultiplier;
				}

				xSteering += xPlaneVector * fMultiplier * ( dbg_fLightAvoidLocalScalar / ( fRange ) ) * fEnvMultiplier;
				++uNumAvoidVectors;

			}
		}
	}

	if ( uNumAvoidVectors > 1 )
	{
		xSteering /= static_cast< float >( uNumAvoidVectors );
	}

	xSteering *= dbg_fCollisionAvoidOverallScalar;

	if ( dbg_bCollisionAvoidDoGlobalAngleHack )
	{
		Asura_Vector_3 xGlobalPlaneVector = xSteering;
		float          fGlobalPlaneVector = xGlobalPlaneVector.Normalise();

		// Make sure the from vector meets the follow point steering at a minimum angle,
		// so that we don't end up with sudden movements when meeting an obstacle head on.
		const float fDot = -xGlobalPlaneVector * xNormalisedFollowPointSteering;
		if ( fDot > fAngleHackMinAngleDot )
		{

			float fTurnSign = 1.0f;

			if(fDot < fAngleHackAlwaysRightAngleDot && xRight * xGlobalPlaneVector < 0.0f ) // turn left 
			{
				fTurnSign = -1.0f;
 			}

			// Work out the direction of the rotation using the plane vector in our local space.
// 			const Asura_Vector_3 xLocalPlaneVector = xOurOrientation.ApplyTranspose( xGlobalPlaneVector );
// 
// 			// Rotate our backward direction by the required amount (in the required direction).
// 			const Asura_Vector_3 xOurUp( xOurOrientation.e12, xOurOrientation.e22, xOurOrientation.e32 );
			Asura_Quat xRotationQuat;
			xRotationQuat.FromRotation( fTurnSign * fAngleHackMinAngle, xOurNormal );
			xGlobalPlaneVector = xRotationQuat.RotateVector( -xNormalisedFollowPointSteering);
		}

		xGlobalPlaneVector *= fGlobalPlaneVector;
		xSteering = xGlobalPlaneVector;
	}

}

void StandardLinkTraverser::UpdateLengthsWithAnimSpeed( const float fTimeslice )
{
	const float fAnimSpeed = GetAlienBrain()->GetAnimationMovementSpeed();
	m_fLengthUntilNonStandardLink -= fTimeslice * fAnimSpeed;
}


DEBUGVAR float dbg_fTestTimeScaleAtStartOfPath = 4.0f;
DEBUGVAR float dbg_fTestTimeScaleAtAvoidanceOfPath = 4.0f;
DEBUGVAR float dbg_fTestTimeForMovementOnPath = 1.0f;
DEBUGVAR float dbg_fTestFramesForMovementOnPath = dbg_fTestTimeForMovementOnPath * 20.0f; // assume 20 frames per seconds
DEBUGVAR float dbg_fTestTimeForMovementAndReorintation = 0.3f;
DEBUGVAR float dbg_fTestFramesForMovementAndReorintation = dbg_fTestTimeForMovementAndReorintation * 20.0f; // assume 20 frames per seconds
DEBUGVAR float dbg_fTestVelocityScale = 0.5f;
DEBUGVAR float dbg_fTestMinimalOrientationCos = Asura_Maths::Cosine(Asura_Maths::DegToRad(2.0f));


void StandardLinkTraverser::UpdateStuckDetection( const float fTimeslice, bool bLargeAvoidanceSteering )
{
	if (m_bStuck)
	{
		return;
	}

	// if we in the middle of a melee or a flinch then we are not stuck
	if (GetAlienBrain()->IsMelee())
	{
		return;
	}
	if (GetAlienBrain()->GetAlienActor()->IsFlinching())
	{
		return;
	}

	// check movement on the path
	if (m_fMaxRouteTraversedDistance > m_fLastMaxRouteFraction + 1.0f * fTimeslice )
	{
		m_fTimeInStuckOnPath = 0.0f;
		m_fFramesInStuckOnPath = 0.0f;
		m_fLastMaxRouteFraction = m_fMaxRouteTraversedDistance;
	}
	else
	{
		if (m_fMaxRouteTraversedDistance == 0.0f)
		{
			m_fTimeInStuckOnPath += fTimeslice / dbg_fTestTimeScaleAtStartOfPath;
			m_fFramesInStuckOnPath += 1.0f / dbg_fTestTimeScaleAtStartOfPath;
		}
		else if (bLargeAvoidanceSteering)
		{
			m_fTimeInStuckOnPath += fTimeslice / dbg_fTestTimeScaleAtAvoidanceOfPath;
			m_fFramesInStuckOnPath += 1.0f / dbg_fTestTimeScaleAtAvoidanceOfPath;
		}
		else
		{
			m_fTimeInStuckOnPath += fTimeslice;
			m_fFramesInStuckOnPath += 1.0f;
		}

		if ( m_fTimeInStuckOnPath > dbg_fTestTimeForMovementOnPath && m_fFramesInStuckOnPath > dbg_fTestFramesForMovementOnPath )
		{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
			Asura_Vector_3 xPosition = GetAlienBrain()->GetActor()->GetPosition();
			Asura_OutputToDebugger(("%d Alien %d stuck - did not advance on its path for %f seconds and %f frames, position %.2f,%.2f,%.2f\n",
				Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetActor()->GetGuid(), m_fTimeInStuckOnPath, m_fFramesInStuckOnPath, 
				xPosition.x, xPosition.y, xPosition.z));
#endif
			m_bStuck = true;
			return;
		}
	}

	// check reorientation and position
	m_fTimeCheckOrientationAndPositionStuck += fTimeslice;
	m_fFramesCheckOrientationAndPositionStuck += 1.0f;

	if (m_fTimeCheckOrientationAndPositionStuck > dbg_fTestTimeForMovementAndReorintation && 
		m_fFramesCheckOrientationAndPositionStuck > dbg_fTestFramesForMovementAndReorintation)
	{
		bool bStuckPosition = true;
		bool bStuckOrientation = true;

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


		float fAvarageVelocity = (xCurrentPosition - m_xLastPosition).Magnitude() / m_fTimeCheckOrientationAndPositionStuck;
		m_xLastPosition = xCurrentPosition;

		if (fAvarageVelocity  > 1.0f * dbg_fTestVelocityScale )
		{
			bStuckPosition = false;
		}

		const Asura_Matrix_3x3& xCurentOrientation = GetAlienBrain()->GetActor()->GetOrientation();
		Asura_Vector_3 xCurrVector(xCurentOrientation.e13, xCurentOrientation.e23, xCurentOrientation.e33);
		Asura_Vector_3 xLastVector(m_xLastOrientation.e13, m_xLastOrientation.e23, m_xLastOrientation.e33);

		if (xCurrVector * xLastVector < dbg_fTestMinimalOrientationCos)
		{
			bStuckOrientation = false;
		}
		else
		{
			xCurrVector = Asura_Vector_3(xCurentOrientation.e12, xCurentOrientation.e22, xCurentOrientation.e32);
			xLastVector = Asura_Vector_3(m_xLastOrientation.e12, m_xLastOrientation.e22, m_xLastOrientation.e32);
			if (xCurrVector * xLastVector < dbg_fTestMinimalOrientationCos)
			{
				bStuckOrientation = false;
			}
		}

		m_xLastOrientation = xCurentOrientation;
		m_bStuck = bStuckOrientation & bStuckPosition;
		m_bTryToUnstuckByJumping = bStuckOrientation & bStuckPosition; // if we really got stuck because lack of movement - can try to resolve this by jumping to the next waypoint

#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
		if (m_bStuck)
		{
			Asura_Vector_3 xPosition = GetAlienBrain()->GetActor()->GetPosition();
			Asura_OutputToDebugger(("%d Alien %d stuck - did not orientation and position did not change for %f seconds and %f frames, position %.2f,%.2f,%.2f (try to unstuck by jumping)\n",
				Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetActor()->GetGuid(), m_fTimeCheckOrientationAndPositionStuck, m_fFramesCheckOrientationAndPositionStuck,
				xPosition.x, xPosition.y, xPosition.z));
		}
#endif
		m_fFramesCheckOrientationAndPositionStuck = 0.0f;
		m_fTimeCheckOrientationAndPositionStuck = 0.0f;
	}
}


void StandardLinkTraverser::SwitchToState( const State eState )
{
	switch ( m_eState )
	{
	case NORMAL:
	case PREPARE_TO_SHARP_TURN:
		break;
	case SHARP_TURN:
		GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_FAST); 
		GetAlienBrain()->SetSharpTurn(false);
		break;
	default:
		break;
	}

	// Enter new state.
	m_eState       = eState;
	m_fTimeInState = 0.0f;
	switch ( m_eState )
	{
	case NORMAL:
		break;
	case PREPARE_TO_SHARP_TURN:
		GetAlienBrain()->SetSharpTurn(true);
		GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_SLOW); 		
		break;
	case SHARP_TURN:
		{ 	
			GetAlienBrain()->SetSharpTurn(true);
			GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_SLOW); 		

// 			Asura_Vector_3 xFollowPointSteering;
// 			GetMovement()->CalculatePointSteering( m_xFollowPoint, xFollowPointSteering );
// 			GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_MEDIUM); 
// 
// 
// 			Asura_Vector_3 xAvoidanceSteering = Asura_Maths::ZeroVector3;
// 			CalculateAvoidanceSteering( xFollowPointSteering, xAvoidanceSteering );
// 			if ( xAvoidanceSteering != Asura_Maths::ZeroVector3 )
// 			{
// 				AVP_Vertebrate_AIAnimStateMachine* pxVertStateHandler = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetAlienBrain()->GetActor()->GetAIAnimStateMachine() );
// 				Asura_Vector_3 xFinalSteering = xFollowPointSteering + xAvoidanceSteering;
// 				xFinalSteering.Normalise();				
// 				GetMovement()->ForceMoveDirection( xFinalSteering );
// 				pxVertStateHandler->RequestMoveMedium();
// 				GetMovement()->SetAllDirections(xFinalSteering);
// //				GetAlienBrain()->SetLookAtPosition( GetAlienBrain()->GetActor()->GetPosition() + xFinalSteering * 10.0f );
// 
// 			
// 			}
// 			else
// 			{
// 				AVP_Vertebrate_AIAnimStateMachine* pxVertStateHandler = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetAlienBrain()->GetActor()->GetAIAnimStateMachine() );
// 				xFollowPointSteering.Normalise();
// 				GetMovement()->ForceMoveDirection( xFollowPointSteering );
// 				pxVertStateHandler->RequestMoveMedium();
// 				GetMovement()->SetAllDirections(xFollowPointSteering);
// 
// 			}
		}
		break;
	}
}


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

	Asura_Font_System::Print( GetAlienBrain()->GetDebugOutputPos().x, GetAlienBrain()->GetDebugOutputPos().y, 0.5f, 0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "SlowLinkTraverser" );
	GetAlienBrain()->GetDebugOutputPos().y += 10.0f;
}
#endif

void JumpLinkTraverser::OnNewRoute()
{
	PARENT::OnNewRoute();

	// Work out the next transition data index we're interested in.
	GetNextJumpTransitionIndex( true );
}

DEBUGVAR float dbg_fBigUpDiffCos = Asura_Maths::Cosine(Asura_Maths::DegToRad(100.0f));

bool JumpLinkTraverser::ShouldStart()
{
	if ( PARENT::ShouldStart() )
	{
		return true;
	}

	// If there's a transition link coming up then we should handle it.
	if ( m_usNextJumpTransitionIndex < GetMovement()->GetNumTransitions() )
	{
		const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
		const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();
		const AVP_Alien_Movement::TransitionData* pxNextTransition = GetMovement()->GetTransitionDataElt( m_usNextJumpTransitionIndex );
		float fLookahead = 1.5f + + Asura_Random::GetFloat(0.0f, 1.5f);
		if (AVP_Navigation::LTT_ENVHOOK_JUMPOVER == pxNextTransition->m_eLinkTransitionType)
		{
			fLookahead = 1.25f;
		}
		if (AVP_Navigation::LTT_ENVHOOK == pxNextTransition->m_eLinkTransitionType)
		{
			fLookahead = 1.5f;
		}


		// The transition look-ahead point is 1.5-3.0m further along the route than us. We use this to switch to the transition slightly before time.
		Asura_Vector_3 xDummy;
		u_short        usLookAheadRouteIndex;
		Asura_Vector_3 xActorGravityDirection;
		GetAlienBrain()->GetActor()->GetPhysicsObjectHandler()->GetCurrentGravityDirection( xActorGravityDirection );

		GetMovement()->FindNearestPointOnRoute( xNavData, xOurPosition, xActorGravityDirection, fLookahead, xDummy, usLookAheadRouteIndex, false );
		// If it's due.
		if ( !pxNextTransition || usLookAheadRouteIndex != pxNextTransition->m_usRouteIndex )
		{
			return false;
		}

		const Asura_Matrix_3x3 xOurOrientation = GetAlienBrain()->GetActor()->GetOrientation();
		const Asura_Vector_3 xOurUp(-xOurOrientation.e12, -xOurOrientation.e22, -xOurOrientation.e32);
		const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usLookAheadRouteIndex ] );
		const Asura_Vector_3& xWPNormal = pxCurrentWP->GetNormal();

		if (xWPNormal * xOurUp < dbg_fBigUpDiffCos)
		{
			return false;
		}
		return true;

	}

	return false;
}

void JumpLinkTraverser::Start()
{
	PARENT::Start();
	GetAlienBrain()->SetJumping(true);

	// Fire off the jump.
	const AVP_Alien_Movement::TransitionData* pxTransitionData = GetMovement()->GetTransitionDataElt( m_usNextJumpTransitionIndex );
	const AVP_Alien_Movement::TransitionData* pxNextTransitionData = GetMovement()->GetTransitionDataElt( m_usPostNextJumpTransitionIndex );
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	Asura_Assert(pxTransitionData, "Transition data not found");
	if (!pxTransitionData)
	{
		m_bFinished = true;
		return;
	}

	Asura_Hash_ID uJumpMoveHashID;
	switch ( pxTransitionData->m_eLinkTransitionType )
	{
		case AVP_Navigation::LTT_FLOOR_TO_CEILING:
        case AVP_Navigation::LTT_CEILING_TO_FLOOR:
        case AVP_Navigation::LTT_WALL_TO_WALL_180:
			uJumpMoveHashID = AVP_Move_Alien_JumpSurface180::GetStaticHash();
			GetMovement()->RequestJumpMove( uJumpMoveHashID, pxTransitionData->m_xDestinationPosition, pxTransitionData->m_xDestinationNormal );
			break;


		default:
			
			bool bJumpOver = AVP_Navigation::LTT_ENVHOOK_JUMPOVER == pxTransitionData->m_eLinkTransitionType;
			bool bNeedToMoveOrTurn;
			Asura_Matrix_3x3 xJumpOrientation;
			Asura_Matrix_3x3 xTurnOrientation;
			Asura_Vector_3 xJumpDestination;
			Asura_Vector_3 xJumpStart;
			AVP_Alien_Movement::ExMovementMode eExMoveMode;
			Asura_Hash_ID uJumpHashID;
			Asura_Hash_ID uJumpAnimHashID;
			u_short usNextTransitionRouteIndex = pxNextTransitionData ? pxNextTransitionData->m_usRouteIndex : xNavData.m_usRouteLength;

			GetMovement()->CalculateJump(bNeedToMoveOrTurn, uJumpMoveHashID, uJumpHashID, uJumpAnimHashID, xTurnOrientation, xJumpStart, xJumpDestination, xJumpOrientation, eExMoveMode, 
										 pxTransitionData->m_usRouteIndex, 
										 usNextTransitionRouteIndex,
										 bJumpOver);
			if (bNeedToMoveOrTurn)
			{
				GetMovement()->RequestMoveAndJumpMove( uJumpMoveHashID, uJumpHashID, uJumpAnimHashID, xTurnOrientation, xJumpStart, xJumpDestination, xJumpOrientation, eExMoveMode );
			}
			else
			{
				GetMovement()->RequestJumpMove( uJumpMoveHashID, uJumpHashID, uJumpAnimHashID,  xJumpDestination, xJumpOrientation, eExMoveMode );
			}
			break;
	}

	
	m_fTimeInState = 0.0f;
	m_usFlags      = 0;
}

bool JumpLinkTraverser::HandleMessage( const Asura_Message* pxMessage )
{
	if ( pxMessage->GetMsgID() == ASURA_MSG_MOVECHAINCOMPLETE )
	{
		// The move chain complete message means we have no more moves to play.
		// We don't check the hash of the move because we may have been interrupted by a flinch move or whatever.
		GetAlienBrain()->ResetMovementDirection();
		m_bFinished = true;
		return true;
	}

	return PARENT::HandleMessage( pxMessage );
}

void JumpLinkTraverser::PerFrameUpdate( const float fTimeslice )
{
	PARENT::PerFrameUpdate( fTimeslice );

	GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_FAST);

	// If we've been at this too long then we should finish.
	m_fTimeInState += fTimeslice;
// 	if ( m_fTimeInState > 2.0f && !m_bFinished)
// 	{
// 		GetAlienBrain()->SetJumping(false);
// 		m_bFinished = true;
// 		GetMovement()->m_fTimeSinceLastJump = 0.0f;
// 	}
}

void JumpLinkTraverser::Stop()
{
	GetAlienBrain()->SetJumping(false);
	GetMovement()->m_fTimeSinceLastJump = 0.0f;
 	GetNextJumpTransitionIndex( false );
	m_fTimeInState = 0.0f;
	m_usFlags      = 0;

	PARENT::Stop();
}

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

	Asura_Font_System::Print( GetAlienBrain()->GetDebugOutputPos().x, GetAlienBrain()->GetDebugOutputPos().y, 0.5f, 0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "JumpLinkTraverser" );
	GetAlienBrain()->GetDebugOutputPos().y += 10.0f;
}
#endif

void JumpLinkTraverser::GetNextJumpTransitionIndex( const bool bNewRoute )
{

	if (bNewRoute)
	{
		m_usNextJumpTransitionIndex = GetNextJumpTransitionIndexStartingAt(0);
	}
	else
	{
		m_usNextJumpTransitionIndex = m_usPostNextJumpTransitionIndex;
	}

	m_usPostNextJumpTransitionIndex = GetNextJumpTransitionIndexStartingAt(m_usNextJumpTransitionIndex + 1);
}

u_short JumpLinkTraverser::GetNextJumpTransitionIndexStartingAt( u_short usStartIndex )
{
	u_short usResultIndex = usStartIndex;
	Asura_LinkList_ConstIt< AVP_Alien_Movement::TransitionData > xTransitionDataIt( GetMovement()->GetTransitionDataList() );
	for ( u_short usIndex = 0; usIndex < usStartIndex; ++usIndex )
	{
		xTransitionDataIt.Next();
	}

	// Find the next jump transition.
	for (  ; !xTransitionDataIt.Done(); xTransitionDataIt.Next() )
	{
		const AVP_Alien_Movement::TransitionData* pxTransition = xTransitionDataIt.GetCurrent();
		if ( pxTransition->m_bPotentialJump )
		{
			return usResultIndex;
		}
		++usResultIndex;
	}

	return usResultIndex;

}


GS2LinkTraverser::GS2LinkTraverser()
: m_eState( UNSET )
, m_fTimeInState( 0.0f )
, m_usFlags( 0 )
, m_usInitialRouteIndex( USHRT_MAX )
, m_usFinalWPIndex( USHRT_MAX )
, m_uVentInStartGuid( ASURA_GUID_UNREGISTERED )
, m_uVentInEndGuid( ASURA_GUID_UNREGISTERED )
, m_uVentOutStartGuid( ASURA_GUID_UNREGISTERED )
, m_uVentOutEndGuid( ASURA_GUID_UNREGISTERED )
, m_xGS2LinkGuidList()
, m_pxGS2Behaviour(NULL)
{
}

bool GS2LinkTraverser::ShouldStart()
{
	if ( PARENT::ShouldStart() )
	{
		return true;
	}

	// If there's a GS2 link coming up then we should handle it.
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();

	// Look ever so slightly ahead.
	Asura_Vector_3 xDummy;
	Asura_Vector_3 xActorGravityDirection;
	GetAlienBrain()->GetActor()->GetPhysicsObjectHandler()->GetCurrentGravityDirection( xActorGravityDirection );

	GetMovement()->FindNearestPointOnRoute( xNavData, xOurPosition, xActorGravityDirection, 0.1f, xDummy, m_usInitialRouteIndex, false );

	if ( m_usInitialRouteIndex < xNavData.m_usRouteLength - 1 )
	{
		// Test the link for the GS2 flag.
		// We set the initial WP index ready for the time we return true and start.
		u_short usInitialWPIndex = xNavData.m_pusRoute[ m_usInitialRouteIndex ];

		// If the initial WP index is the same as our final WP index it means we've been a little inaccurate at the end of our route,
		// and we've not quite cleared the GS2 structure. We don't want to switch back on to GS2 though.
		if ( usInitialWPIndex == m_usFinalWPIndex )
		{
			return false;
		}

		// if this node is actually exit node then dont start.
		u_int uDummy;
		const AVP_ServerNode_GS2Node* pxDummy = NULL;
		bool bDummy;
		if (!SelectNextGS2Node(NULL, m_usInitialRouteIndex, pxDummy, uDummy, bDummy))
		{
			return false;
		}

		const Asura_AI_NavVertex* pxSourceWP = Asura_AI_Navigation_System::GetWaypoint( usInitialWPIndex );
		const Asura_AI_NavConnection* pxLink = pxSourceWP->GetLinkTo( xNavData.m_pusRoute[ m_usInitialRouteIndex + 1 ] );
		return ( pxLink && ( ( pxLink->GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_GS2 ) != 0 ) );
	}

	return false;
}

void GS2LinkTraverser::Start()
{
	PARENT::Start();

	m_bFinished = false;
	m_bInError  = false;

	GetAlienBrain()->SetGameScene(true);
	GetAlienBrain()->GetAlienActor()->SetAllowDamageLimb(false);
	m_xGS2LinkGuidList.Clear();

	// The waypoint we are at should correspond to one and only one GS2 node that has an outgoing link and no incoming links.
	//## For the moment only test for the outgoing link. The incoming link test requires iterating over all GS2 nodes.
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	const AVP_ServerNode_GS2Node* pxNextNode = NULL;
	const AVP_ServerNode_GS2Node* pxPrevNode = NULL;
	bool bIsNextNodeEnd = false;
	Asura_Guid uLink = ASURA_GUID_UNREGISTERED;

	if(!SelectNextGS2Node(NULL, m_usInitialRouteIndex, pxNextNode, uLink, bIsNextNodeEnd))
	{
		m_bInError = true;
		SwitchToState(FINISHED);
		return;
	}

	// Determine what type of GS2 structure we are.
	// (Currently we'll only have to deal with Alien Vent structures, but this traverser could end up being used for Predator movement also.
	if ( pxNextNode->GetStructureType() != AVP_GS_ST_ALIEN_VENT )
	{
		m_bInError = true;
		SwitchToState(FINISHED);
		return;
	}

	pxPrevNode = pxNextNode;
	m_uVentInStartGuid = pxPrevNode->GetGuid();
	m_uVentInEndGuid = ASURA_GUID_UNREGISTERED;
	m_uVentOutStartGuid = ASURA_GUID_UNREGISTERED;
	m_uVentOutEndGuid = ASURA_GUID_UNREGISTERED;

	u_short usRouteIndex =  m_usInitialRouteIndex + 1;
	bool bStart = true;

	while (usRouteIndex < xNavData.m_usRouteLength && !bIsNextNodeEnd)
	{
		if(!SelectNextGS2Node(pxPrevNode, usRouteIndex, pxNextNode, uLink, bIsNextNodeEnd))
		{
			m_bInError = true;
			SwitchToState(FINISHED);
			return;
		}

		if (bStart)
		{
			bStart = false;
			m_uVentInEndGuid = pxNextNode->GetGuid();
		}
		else if (!bIsNextNodeEnd)
		{
			m_xGS2LinkGuidList.Push_Back(uLink);
		}
		else
		{
			m_uVentOutStartGuid = pxPrevNode->GetGuid();
			m_uVentOutEndGuid = pxNextNode->GetGuid();
		}

		pxPrevNode = pxNextNode;
		++usRouteIndex;
	}

	Asura_Assert(m_uVentInStartGuid != ASURA_GUID_UNREGISTERED, "Vent Entrance GS node not found");
	Asura_Assert(m_uVentInEndGuid != ASURA_GUID_UNREGISTERED, "Vent Entrance GS node not found");
	Asura_Assert(m_uVentOutStartGuid != ASURA_GUID_UNREGISTERED, "Vent Exit GS node not found");
	Asura_Assert(m_uVentOutEndGuid != ASURA_GUID_UNREGISTERED, "Vent Exit GS node not found");
	Asura_Assert(bIsNextNodeEnd, "Route ended in the middle of GS");
	if (!bIsNextNodeEnd ||
		m_uVentInStartGuid == ASURA_GUID_UNREGISTERED || m_uVentInEndGuid == ASURA_GUID_UNREGISTERED ||
		m_uVentOutStartGuid == ASURA_GUID_UNREGISTERED || m_uVentOutEndGuid == ASURA_GUID_UNREGISTERED )
	{
		m_bInError = true;
		SwitchToState(FINISHED);
		return;
	}

	SwitchToState( VENT_IN );
}

bool GS2LinkTraverser::HandleMessage( const Asura_Message* pxMessage )
{
	if ( m_pxGS2Behaviour )
	{
		m_pxGS2Behaviour->DoHandleMessage( pxMessage );
	}

	if ( pxMessage->GetMsgID() == ASURA_MSG_MOVECHAINCOMPLETE )
	{
		const Asura_Msg_MoveChainComplete* pxData = static_cast< const Asura_Msg_MoveChainComplete* >( pxMessage->GetData() );
		if ( pxData )
		{
			if ( m_eState == VENT_IN )
			{
				SwitchToState( ON_GS2 );
				return true;
			}
			if ( m_eState == VENT_OUT )
			{
				SwitchToState( FINISHED );
				return true;
			}
		}
	}

	return PARENT::HandleMessage( pxMessage );
}

void GS2LinkTraverser::PerFrameUpdate( const float fTimeslice )
{
	m_fTimeInState += fTimeslice;

	GetMovement()->SetSharpTurnSpeedLimit(AVP_Alien_Movement::EMM_FAST);

	switch ( m_eState )
	{
		case VENT_IN:
			break;
		case VENT_OUT:
			break;
		case ON_GS2:
		{
			// If our GS2 behaviour has finished then we're done.
			if ( m_pxGS2Behaviour )
			{
				m_pxGS2Behaviour->DoPerFrameUpdate( fTimeslice );
			}

			if ( !m_pxGS2Behaviour || m_pxGS2Behaviour->IsInError() )
			{
                m_bInError       = true;				
				SwitchToState( FINISHED );
			}
			else if ( m_pxGS2Behaviour->IsFinished() )
			{
				SwitchToState( VENT_OUT );
			}

			break;
		}

		case FINISHED:
		case UNSET:
		default:
		{
			break;
		}
	}
}

void GS2LinkTraverser::Update( const float fTimeslice )
{
	PARENT::Update( fTimeslice );

	if ( m_pxGS2Behaviour )
	{
		m_pxGS2Behaviour->DoUpdate( fTimeslice );
	}
}

void GS2LinkTraverser::Stop()
{
	SwitchToState( UNSET );
	m_usFlags = 0;

	PARENT::Stop();
}

void GS2LinkTraverser::Pause()
{
	PARENT::Pause();

	if ( m_pxGS2Behaviour )
	{
		m_pxGS2Behaviour->DoPause();
	}
}

void GS2LinkTraverser::Unpause()
{
	PARENT::Unpause();

	if ( m_pxGS2Behaviour )
	{
		m_pxGS2Behaviour->DoUnpause();
	}
}

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

	Asura_Font_System::Print( GetAlienBrain()->GetDebugOutputPos().x, GetAlienBrain()->GetDebugOutputPos().y, 0.5f, 0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "GS2LinkTraverser" );
	GetAlienBrain()->GetDebugOutputPos().y += 10.0f;
}
#endif

void GS2LinkTraverser::SwitchToState( const State eState )
{
	// Clean up old state.
	switch ( m_eState )
	{
		case ON_GS2:
			// Destroy our GS2 child behaviour.
			if ( m_pxGS2Behaviour )
			{
				m_pxGS2Behaviour->DoDeactivate();
				GetAlienBrain()->DestroyBehaviour( m_pxGS2Behaviour );
				m_pxGS2Behaviour = NULL;
			}
			break;
		case FINISHED:
			m_bFinished = false;
			break;
		case VENT_IN:
			break;
		case VENT_OUT:
			break;
		case UNSET:
			break;
		default:
			Asura_Assert(false, "Unexpected state");			
			break;
	}

	// Enter new state.
    m_eState = eState;
	m_fTimeInState = 0.0f;
	switch ( m_eState )
	{
		case VENT_IN:
			if ( !RequestVentMove( m_uVentInStartGuid, m_uVentInEndGuid ) )
			{
				m_bInError = true;
				SwitchToState( FINISHED );
			}
			break;
		case VENT_OUT:
			if ( !RequestVentMove( m_uVentOutStartGuid, m_uVentOutEndGuid ) )
			{
				m_bInError = true;
				SwitchToState( FINISHED );
			}
			break;
		case ON_GS2:
			BuildOnGS2Behaviour();
			break;
		case FINISHED:
			m_usFinalWPIndex = GetAlienBrain()->GetNavData().m_usCurrentWPID;
			GetAlienBrain()->SetGameScene(false);
			GetAlienBrain()->GetAlienActor()->SetAllowDamageLimb(true);
			m_bFinished = true;
			break;
		case UNSET:
			break;
		default:
			Asura_Assert(false, "Unexpected state");
			break;
	}
}

bool GS2LinkTraverser::RequestVentMove( Asura_Guid uGS2StartMoveNode, Asura_Guid uGS2EndMoveNode )
{

	const AVP_ServerNode_GS2Node* pxStartNode = AVP_ServerNode_GS2Node::FindEntity( uGS2StartMoveNode );
	const AVP_ServerNode_GS2Node* pxEndNode = AVP_ServerNode_GS2Node::FindEntity( uGS2EndMoveNode );
	if (!pxStartNode || !pxEndNode) { return false; }

	Asura_ServerNode_GS2Spline* pxLink = NULL;
	
	for (u_int uLink = 0; uLink < pxStartNode->GetNumberLinks() && pxLink == NULL; ++uLink)
	{
		const Asura_Guid uCandidateLinkGuid = pxStartNode->GetLink(uLink);
		Asura_ServerNode_GS2Spline* pxCandidateLink = Asura_ServerNode_GS2Spline::FindEntity(uCandidateLinkGuid);
		if (!pxCandidateLink)
		{
			continue;
		}
		const Asura_Guid uEndNodeGuid = pxCandidateLink->GetTargetGuid();
		if (uEndNodeGuid == pxEndNode->GetGuid())
		{					
			pxLink = pxCandidateLink;
		}
	}

	Asura_Assert(pxLink, "Link from start node to end node not found");
	if (pxLink == NULL)
	{
		return false;
	}

	Asura_Assert(pxLink->GetBehaviourParams(), "Link behaviour params not found");

	if ( pxLink->GetBehaviourParams() && pxLink->GetBehaviourParams()->GetParamsType() != AXON_BP_TYPE_GS2_SPLINE ) 
	{
		return false; 
	}

	const Axon_BehaviourParams_GS2Spline* pxLinkParams = static_cast< Axon_BehaviourParams_GS2Spline* >( pxLink->GetBehaviourParams() );
	Asura_Hash_ID uAnimHash = ASURA_HASH_ID_UNSET;
	if ( !GetAnimHashFromAnimTemplateHash( pxLinkParams->m_uAnimSet, uAnimHash ) ) 
	{
		return false; 
	}

	Asura_Hash_ID uMoveHash = AVP_Move_Alien_AI_TraverseVent_Begin::GetStaticHash();
	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Alien_AI_Move_VentStart ) );
	if ( pxMessage != NULL )
	{
		AVP_Message_Alien_AI_Move_VentStart* pxData = static_cast< AVP_Message_Alien_AI_Move_VentStart* >( pxMessage->GetData() );

		pxMessage->SetFromGuid( GetAlienBrain()->GetOwnerGuid() );
		pxMessage->SetToGuid( GetAlienBrain()->GetOwnerGuid() );

		pxData->ClearFlags();
		pxData->SetMoveID( uMoveHash );
		pxData->SetIgnoredByOwner( false );

		Asura_Matrix_3x3 xOri;
		pxEndNode->GetOrientation(xOri);

		pxData->SetDestPos( pxEndNode->GetPosition() );
		pxData->SetDestOri( xOri );
		pxData->SetAnimHash( uAnimHash );
		AVP_Alien_AIAnimStateMachine* pxAlienStateHandler = static_cast< AVP_Alien_AIAnimStateMachine* >( GetAlienBrain()->GetActor()->GetAIAnimStateMachine() );
		pxAlienStateHandler->RequestInMove(pxMessage, AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_MEDIUM); // for now just medium speed
		return true;
	}
	else
	{
		Asura_Assert( false, "Unable to create message" );
		return false;
	}
}

bool GS2LinkTraverser::GetAnimHashFromAnimTemplateHash( const Asura_Hash_ID uAnimTemplateHash, Asura_Hash_ID& uAnimHash )
{
	Axon_Gamescene_AnimTemplate* pxTemplate = Axon_Gamescene_AnimTemplate_System::GetAnimTemplateByID( uAnimTemplateHash );
	if ( pxTemplate && pxTemplate->m_pxFirstEntry )
	{
		uAnimHash = pxTemplate->m_pxFirstEntry->CalcAnimHashIDToPlay();
		return true;
	}

	return false;
}

bool GS2LinkTraverser::SelectNextGS2Node(const AVP_ServerNode_GS2Node* pxCurrentNode, u_short usRouteIndex, const AVP_ServerNode_GS2Node*& pxNextNode, Asura_Guid& uLinkGuid, bool& bIsNextNodeEnd)
{
	const Asura_Guid* puNodeGuids = NULL;
	int iNumNodeGuids = 0;
	bIsNextNodeEnd = true;
	uLinkGuid = ASURA_GUID_UNREGISTERED;
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	Asura_Assert(usRouteIndex <= xNavData.m_usRouteLength - 1, "Route finished in the middle of GS");
	if (usRouteIndex > xNavData.m_usRouteLength - 1)
	{
		return false;
	}

	if (!AVP_Waypoint_Manager::GetGS2NodeGuidsFromWPIndex( xNavData.m_pusRoute[ usRouteIndex ], puNodeGuids, iNumNodeGuids ) )
	{
		return false;
	}

	if (iNumNodeGuids == 0)
	{
		return false;
	}

	Asura_Assert(iNumNodeGuids == 1, "Expected only one GS2 per waypoint");

	if (pxCurrentNode == NULL) // on the first LINK
	{			
		
		pxNextNode = AVP_ServerNode_GS2Node::FindEntity( puNodeGuids[ 0 ] );
		Asura_Assert(pxNextNode, "start GS2 node not found");
		if (pxNextNode)
		{			
			bIsNextNodeEnd = pxNextNode->GetNumberLinks() == 0;
			return true;
		}
		else
		{
			return false;
		}
	}
	else
	{
		pxNextNode = AVP_ServerNode_GS2Node::FindEntity( puNodeGuids[ 0 ] ); // Assume only one 
		Asura_Assert(pxNextNode, "GS2 node not found");
		if (!pxNextNode)
		{
			return false;
		}
		bIsNextNodeEnd = pxNextNode->GetNumberLinks() == 0;
		// verify we can get to the next GS2 node;
		for (u_int uLink = 0; uLink < pxCurrentNode->GetNumberLinks(); ++uLink)
		{
			uLinkGuid = pxCurrentNode->GetLink(uLink);
			const Asura_ServerNode_GS2Spline* pxLink = Asura_ServerNode_GS2Spline::FindEntity(uLinkGuid);
			if (!pxLink)
			{
				continue;
			}
			const Asura_Guid uNextNodeGuid = pxLink->GetTargetGuid();
			if (uNextNodeGuid == pxNextNode->GetGuid())
			{			
				return true;
			}
		}

		Asura_Assert(false, "Link to next GS2 node not found");
		return false;
	}
}

void GS2LinkTraverser::BuildOnGS2Behaviour()
{
	if (m_xGS2LinkGuidList.Front() == NULL) // no GS2 - enter and immdeiatly exit
	{
		SwitchToState( VENT_OUT	);
		return;
	}

	// Create a GS2 child behaviour and teleport to the correct node.
	m_pxGS2Behaviour = (GetAlienBrain()->CreateBehaviour( AXON_BEHAVIOUR_TYPE_GS2_ACTOR ));
	if ( m_pxGS2Behaviour )
	{
		const AVP_ServerNode_GS2Node* pxStartGSNode = AVP_ServerNode_GS2Node::FindEntity( m_uVentInEndGuid );

		static_cast<Axon_Behaviour_GS2Actor*>(m_pxGS2Behaviour)->AppendRoute(m_xGS2LinkGuidList);	
				
		m_pxGS2Behaviour->DoActivate();

		Asura_Assert(pxStartGSNode, "Start GS node not found");
		if (!pxStartGSNode)
		{
			m_bInError = true;
			SwitchToState( FINISHED );
			return;
		}

		Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_AXON_GS2_TELEPORT_TO_NODE, sizeof( Axon_Msg_GS2_Teleport_To_Node ) );
		if (pxMessage)
		{
			Axon_Msg_GS2_Teleport_To_Node* pxTeleportToNodeMsg = static_cast< Axon_Msg_GS2_Teleport_To_Node* >( pxMessage->GetData() );

			pxTeleportToNodeMsg->m_uNodeGuid = pxStartGSNode->GetGuid();
			m_pxGS2Behaviour->DoHandleMessage( pxMessage );

			Asura_Network::DestroyMessage( pxMessage );		
		}
	}
	else
	{
		m_bInError = true;
		SwitchToState( FINISHED );
	}
}

EnvHookLinkTraverser::EnvHookLinkTraverser()
: m_xFollowPoint( Asura_Maths::ZeroVector3 )
, m_usFlags( 0 )
, m_usPrevRouteIndex( 0 )
, m_fTimeInStuckOnPath( 0.0f )
, m_fFramesInStuckOnPath( 0.0f )
, m_fTimeCheckOrientationAndPositionStuck( 0.0f )
, m_fFramesCheckOrientationAndPositionStuck( 0.0f )
, m_fMaxRouteTraversedDistance( 0.0f )
, m_fLastMaxRouteFraction( 0.0f )
, m_xLastOrientation( Asura_Maths::IdentityMatrix3x3 )
, m_xLastPosition( Asura_Maths::ZeroVector3 )
, m_fAvarageVelocity(0.0f)
{
}

void EnvHookLinkTraverser::OnNewRoute()
{
	PARENT::OnNewRoute();

	m_bStuck = false; 
	m_bTryToUnstuckByJumping = false;
}

bool EnvHookLinkTraverser::ShouldStart()
{

// 	if ( PARENT::ShouldStart() )
// 	{
// 		return true;
// 	}
// 
// 	// If there's an env hook link coming up then we should handle it.
// 	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
// 	const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();
// 
// 	// Look ever so slightly ahead.
// 	Asura_Vector_3 xDummy;
// 	u_short        usRouteIndex;
// 	Asura_Vector_3 xActorGravityDirection;
// 	GetAlienBrain()->GetActor()->GetPhysicsObjectHandler()->GetCurrentGravityDirection( xActorGravityDirection );
// 
// 	GetMovement()->FindNearestPointOnRoute( xNavData, xOurPosition, xActorGravityDirection, 0.1f, xDummy, usRouteIndex, false );
// 
// 	if ( usRouteIndex < xNavData.m_usRouteLength - 1 )
// 	{
// 		// Test the link for the env hook flag.
// 		// We set the source WP index ready for the time we return true and start.
// 		const Asura_AI_NavVertex* pxSourceWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usRouteIndex ] );
// 		const Asura_AI_NavConnection* pxLink = pxSourceWP->GetLinkTo( xNavData.m_pusRoute[ usRouteIndex + 1 ] );
// 		return ( pxLink && ( ( pxLink->GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK ) != 0 ) );
// 	}

	return false;
}

void EnvHookLinkTraverser::Start()
{
	PARENT::Start();

	m_usPrevRouteIndex = GetAlienBrain()->GetNavData().m_usRouteSegmentIndex;
	GetMovement()->RequestMovementAnimState();

	m_usFlags = 0;
	m_fTimeInStuckOnPath = 0.0f;
	m_fFramesInStuckOnPath = 0.0f;
	m_fTimeCheckOrientationAndPositionStuck = 0.0f;
	m_fFramesCheckOrientationAndPositionStuck = 0.0f;
	m_fMaxRouteTraversedDistance = 0.0f;
	m_fLastMaxRouteFraction = 0.0f;
	m_xLastOrientation = GetAlienBrain()->GetActor()->GetOrientation();
	m_xLastPosition = GetAlienBrain()->GetActor()->GetPosition();
	m_fAvarageVelocity = 0.0f;

}

void EnvHookLinkTraverser::PerFrameUpdate( const float fTimeslice )
{
	PARENT::PerFrameUpdate( fTimeslice );

	UpdateFollowPoint();

	Asura_Vector_3 xFollowPointSteering;
	GetMovement()->CalculatePointSteering( m_xFollowPoint, xFollowPointSteering );
	xFollowPointSteering.Normalise();
	GetMovement()->SetAllDirections( xFollowPointSteering );

	// If we've moved on to the next segment then reset the stuck detection stuff.
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	const u_short usRouteIndex = xNavData.m_usRouteSegmentIndex;
	if ( usRouteIndex > m_usPrevRouteIndex )
	{
		// If the new segment is not an env hook segment then we've finished.
		if ( usRouteIndex < xNavData.m_usRouteLength - 1 )
		{
			// Test the link for the env hook flag.
			// We set the source WP index ready for the time we return true and start.
			const Asura_AI_NavVertex* pxSourceWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usRouteIndex ] );
			const Asura_AI_NavConnection* pxLink = pxSourceWP->GetLinkTo( xNavData.m_pusRoute[ usRouteIndex + 1 ] );
			m_bFinished = ( !pxLink || ( ( pxLink->GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_ENV_HOOK ) == 0 ) );
		}

		m_usPrevRouteIndex = usRouteIndex;
	}

	UpdateStuckDetection( fTimeslice, false );
}

void EnvHookLinkTraverser::Stop()
{
	m_usFlags = 0;

	PARENT::Stop();
}

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

	Asura_Font_System::Print( GetAlienBrain()->GetDebugOutputPos().x, GetAlienBrain()->GetDebugOutputPos().y, 0.5f, 0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "EnvHookLinkTraverser" );
	GetAlienBrain()->GetDebugOutputPos().y += 10.0f;
}
#endif

void EnvHookLinkTraverser::UpdateFollowPoint()
{
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	const float fLookAheadDistance = 2.5f;

	const Asura_Vector_3& xCurrentPosition = GetAlienBrain()->GetActor()->GetPosition();
	Asura_Vector_3 xActorGravityDirection;
	GetAlienBrain()->GetActor()->GetPhysicsObjectHandler()->GetCurrentGravityDirection( xActorGravityDirection );

	u_short usNearestRouteIndex;
	GetMovement()->FindNearestPointOnRoute( xNavData, xCurrentPosition,	-xActorGravityDirection, fLookAheadDistance, m_xFollowPoint, usNearestRouteIndex, false );
}

void EnvHookLinkTraverser::UpdateStuckDetection( const float fTimeslice, bool bLargeAvoidanceSteering )
{

	if (m_bStuck)
	{
		return;
	}

	// if we in the middle of a melee or a flinch then we are not stuck
	if (GetAlienBrain()->IsMelee())
	{
		return;
	}
	if (GetAlienBrain()->GetAlienActor()->IsFlinching())
	{
		return;
	}

	// check movement on the path
	if (m_fMaxRouteTraversedDistance > m_fLastMaxRouteFraction + 1.0f * fTimeslice )
	{
		m_fTimeInStuckOnPath = 0.0f;
		m_fFramesInStuckOnPath = 0.0f;
			m_fLastMaxRouteFraction = m_fMaxRouteTraversedDistance;
	}
	else
	{
		if (m_fMaxRouteTraversedDistance == 0.0f)
		{
			m_fTimeInStuckOnPath += fTimeslice / dbg_fTestTimeScaleAtStartOfPath;
			m_fFramesInStuckOnPath += 1.0f / dbg_fTestTimeScaleAtStartOfPath;
		}
		else if (bLargeAvoidanceSteering)
		{
			m_fTimeInStuckOnPath += fTimeslice / dbg_fTestTimeScaleAtAvoidanceOfPath;
			m_fFramesInStuckOnPath += 1.0f / dbg_fTestTimeScaleAtAvoidanceOfPath;
		}
		else
		{
			m_fTimeInStuckOnPath += fTimeslice;
			m_fFramesInStuckOnPath += 1.0f;
		}

		if ( m_fTimeInStuckOnPath > dbg_fTestTimeForMovementOnPath && m_fFramesInStuckOnPath > dbg_fTestFramesForMovementOnPath )
		{
			m_bStuck = true;
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
			Asura_Vector_3 xPosition = GetAlienBrain()->GetActor()->GetPosition();
			Asura_OutputToDebugger(("%d Alien %d stuck - did not advance on its path for %f seconds and %f frames,  position %.2f,%.2f,%.2f\n",
				Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetActor()->GetGuid(), m_fTimeInStuckOnPath, m_fFramesInStuckOnPath, 
				xPosition.x, xPosition.y, xPosition.z));

#endif
			return;
		}
	}

	// check reorientation and position
	m_fTimeCheckOrientationAndPositionStuck += fTimeslice;
	m_fFramesCheckOrientationAndPositionStuck += 1.0f;

	if (m_fTimeCheckOrientationAndPositionStuck > dbg_fTestTimeForMovementAndReorintation && 
		m_fFramesCheckOrientationAndPositionStuck > dbg_fTestFramesForMovementAndReorintation)
	{
		bool bStuckPosition = true;
		bool bStuckOrientation = true;

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


		float fAvarageVelocity = (xCurrentPosition - m_xLastPosition).Magnitude() / m_fTimeCheckOrientationAndPositionStuck;
		m_xLastPosition = xCurrentPosition;

		if (fAvarageVelocity  > 1.0f * dbg_fTestVelocityScale )
		{
			bStuckPosition = false;
		}

		const Asura_Matrix_3x3& xCurentOrientation = GetAlienBrain()->GetActor()->GetOrientation();
		Asura_Vector_3 xCurrVector(xCurentOrientation.e13, xCurentOrientation.e23, xCurentOrientation.e33);
		Asura_Vector_3 xLastVector(m_xLastOrientation.e13, m_xLastOrientation.e23, m_xLastOrientation.e33);

		if (xCurrVector * xLastVector < dbg_fTestMinimalOrientationCos)
		{
			bStuckOrientation = false;
		}
		else
		{
			xCurrVector = Asura_Vector_3(xCurentOrientation.e12, xCurentOrientation.e22, xCurentOrientation.e32);
			xLastVector = Asura_Vector_3(m_xLastOrientation.e12, m_xLastOrientation.e22, m_xLastOrientation.e32);
			if (xCurrVector * xLastVector < dbg_fTestMinimalOrientationCos)
			{
				bStuckOrientation = false;
			}
		}

		m_xLastOrientation = xCurentOrientation;
		m_bStuck = bStuckOrientation & bStuckPosition;
		m_bTryToUnstuckByJumping = bStuckOrientation & bStuckPosition; // if we really got stuck because lack of movement - can try to resolve this by jumping to the next waypoint

#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
		if (m_bStuck)
		{
			Asura_Vector_3 xPosition = GetAlienBrain()->GetActor()->GetPosition();
			Asura_OutputToDebugger(("%d Alien %d stuck - did not orientation and position did not change for %f seconds and %f frames, position %.2f,%.2f,%.2f (try to unstuck by jumping)\n",
				Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetActor()->GetGuid(), m_fTimeCheckOrientationAndPositionStuck, m_fFramesCheckOrientationAndPositionStuck,
				xPosition.x, xPosition.y, xPosition.z));
		}
#endif
		m_fFramesCheckOrientationAndPositionStuck = 0.0f;
		m_fTimeCheckOrientationAndPositionStuck = 0.0f;
	}
}

DestinationTraverser::DestinationTraverser()
: m_eState( UNSET )
, m_uFlags( 0 )
, m_fTimeInStuckOnPath( 0.0f )
, m_fFramesInStuckOnPath( 0.0f )
, m_fTimeCheckOrientationAndPositionStuck( 0.0f )
, m_fFramesCheckOrientationAndPositionStuck( 0.0f )
, m_fMaxRouteTraversedDistance( 0.0f )
, m_fLastMaxRouteFraction( 0.0f )
, m_xLastOrientation( Asura_Maths::IdentityMatrix3x3 )
, m_xLastPosition( Asura_Maths::ZeroVector3 )
, m_fAvarageVelocity(0.0f)
#ifndef ASURA_NO_DEBUG_OUTPUT
, m_xFollowSteering( Asura_Maths::ZeroVector3 )
, m_xAvoidanceSteering( Asura_Maths::ZeroVector3 )
, m_xFinalSteering( Asura_Maths::ZeroVector3 )
#endif
{
}

void DestinationTraverser::OnNewRoute()
{
	PARENT::OnNewRoute();

	m_bStuck = false; 
	m_bTryToUnstuckByJumping = false;
}

bool DestinationTraverser::ShouldStart()
{
	if ( PARENT::ShouldStart() )
	{
		return true;
	}

	// If we're not near enough the end of the route.
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	if ( xNavData.m_usRouteSegmentIndex < xNavData.m_usRouteLength - 2 )
	{
		return false;
	}

	const Asura_AI_NavVertex* pxDestinationWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usDestinationWPID );
	if ( pxDestinationWP )
	{
		const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();

		// If we're inside the final waypoint.
		if ( pxDestinationWP->Contains( xOurPosition ) )
		{
			return true;
		}

		// If we're near enough the destination. (This is not formally correct, but is ok for Aliens.)
		AVP_Navigation::APParams xAPParams;
		xAPParams.m_fMaxCoplanarOffset = Asura_Maths::Max( 5.0f, GetMovement()->m_fArrivalRange );
		xAPParams.m_xDestinationNormal = pxDestinationWP->GetNormal();
		if ( AVP_Navigation::AtPosition( xOurPosition, xNavData.m_xDestinationPosition, xAPParams ) )
		{
			return true;
		}
	}

	return false;
}

void DestinationTraverser::Start()
{
	PARENT::Start();

	m_uFlags = false;
	m_fTimeInStuckOnPath = 0.0f;
	m_fFramesInStuckOnPath = 0.0f;
	m_fTimeCheckOrientationAndPositionStuck = 0.0f;
	m_fFramesCheckOrientationAndPositionStuck = 0.0f;
	m_fMaxRouteTraversedDistance = 0.0f;
	m_fLastMaxRouteFraction = 0.0f;
	m_xLastOrientation = GetAlienBrain()->GetActor()->GetOrientation();
	m_xLastPosition = GetAlienBrain()->GetActor()->GetPosition();
	m_fAvarageVelocity = 0.0f;

	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	if ( GetAlienBrain()->GetActor()->GetAIAnimStateMachine()->IsIdle() )
	{
		// If we're not already moving then we can move straight towards the destination.
		SwitchToState( MOVING_DIRECT_TO_DESTINATION );
	}
	else if ( GetAlienBrain()->WillOrbitPosition( xNavData.m_xDestinationPosition ) )
	{
		SwitchToState( TRAVERSING_AVOIDING_ORBIT );
	}
	else
	{
		SwitchToState( TRAVERSING_TO_DESTINATION );
	}
}

void DestinationTraverser::PerFrameUpdate( const float fTimeslice )
{
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();

	UpdateRouteFraction();
	bool bLargeAvoidanceSteering = false;

	switch ( m_eState )
	{
		case MOVING_DIRECT_TO_DESTINATION:
		{
			Asura_Vector_3 xToDestination = xNavData.m_xDestinationPosition - xOurPosition;
			xToDestination.Normalise();
			GetMovement()->GetAlienBrain()->SetMovementDirection( xToDestination );
#ifndef ASURA_NO_DEBUG_OUTPUT
			m_xFollowSteering = xToDestination;
			m_xAvoidanceSteering = Asura_Maths::ZeroVector3;
			m_xFinalSteering = xToDestination;
#endif

			break;
		}

		case TRAVERSING_TO_DESTINATION:
		{
			// Steer towards the destination. No character avoidance otherwise we might miss.
			Asura_Vector_3 xDestinationSteering;
			GetMovement()->CalculatePointSteering( xNavData.m_xDestinationPosition, xDestinationSteering );
			xDestinationSteering.Normalise();
			GetMovement()->SetAllDirections( xDestinationSteering );

			if ( GetAlienBrain()->WillOrbitPosition( xNavData.m_xDestinationPosition ) )
			{
				SwitchToState( TRAVERSING_AVOIDING_ORBIT );
			}
#ifndef ASURA_NO_DEBUG_OUTPUT
			m_xFollowSteering = xDestinationSteering;
			m_xAvoidanceSteering = Asura_Maths::ZeroVector3;
			m_xFinalSteering = xDestinationSteering;
#endif

			break;
		}

		case TRAVERSING_AVOIDING_ORBIT:
		{
			// Steer away from the destination. This should only be temporary and get us to a position where we can head for the destination.
			//## During this movement we could incorporate character avoidance.
			bLargeAvoidanceSteering = true;
			Asura_Vector_3 xToSteeringPoint = xNavData.m_xDestinationPosition - xOurPosition;
			// Rotate by 180° around our y axis.
			Asura_Quat xRotation;
			xRotation.FromRotation( Asura_Maths::Pi, Asura_Vector_3( GetAlienBrain()->GetActor()->GetCurrentGravityDirection() ) );
			xToSteeringPoint = xRotation.RotateVector( xToSteeringPoint);

			Asura_Vector_3 xAwayFromDestinationSteering;
			GetMovement()->CalculatePointSteering( xOurPosition + xToSteeringPoint, xAwayFromDestinationSteering );
			xAwayFromDestinationSteering.Normalise();
			GetMovement()->SetAllDirections( xAwayFromDestinationSteering );

			if ( !GetAlienBrain()->WillOrbitPosition( xNavData.m_xDestinationPosition ) )
			{
				SwitchToState( TRAVERSING_TO_DESTINATION );
			}
#ifndef ASURA_NO_DEBUG_OUTPUT
			m_xFollowSteering = xToSteeringPoint;
			m_xAvoidanceSteering = Asura_Maths::ZeroVector3;
			m_xFinalSteering = xToSteeringPoint;
#endif

			break;
		}

		default:
		case UNSET:
		{
			m_bInError = true;
			break;
		}
	}

	// Note that the check for being at the destination is done automatically inside the main movement update, so we don't need to check here.

	UpdateStuckDetection( fTimeslice, bLargeAvoidanceSteering );
}

void DestinationTraverser::Stop()
{
	SwitchToState( UNSET );
	m_uFlags = 0;

	// Force the movement direction to be straight ahead so we don't leave the vectors in an odd state.
	const Asura_Matrix_3x3& xOurOrientM = GetAlienBrain()->GetActor()->GetOrientation();
	GetAlienBrain()->ForceMovementDirection( Asura_Vector_3( xOurOrientM.e13, xOurOrientM.e23, xOurOrientM.e33 ) );

	PARENT::Stop();
}

#ifndef ASURA_NO_DEBUG_OUTPUT
void DestinationTraverser::RenderDebugInfo()
{
	static const char* s_aszStrings[ NUM_STATES ] =
	{
		"UNSET",
		"MOVING_DIRECT_TO_DESTINATION",
		"TRAVERSING_TO_DESTINATION",
		"TRAVERSING_AVOIDING_ORBIT"
	};

	PARENT::RenderDebugInfo();

	Asura_Font_System::Print( GetAlienBrain()->GetDebugOutputPos().x, GetAlienBrain()->GetDebugOutputPos().y, 0.5f, 0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "DestinationTraverser: %s",
		s_aszStrings[ m_eState ]);
	GetAlienBrain()->GetDebugOutputPos().y += 10.0f;

	Asura_Vector_3 xEyePosition = GetAlienBrain()->GetActor()->GetPosition();
	GetAlienBrain()->GetActor()->GetWorldSpaceMarkerPoint( 0xe5a85d04, &xEyePosition, NULL );	// target_2
	xEyePosition -= Asura_Vector_3(GetAlienBrain()->GetActor()->GetCurrentGravityDirection()) * .5f;

	Asura_DebugLineRender::AddArrow( xEyePosition, xEyePosition + m_xFollowSteering * 2.0f, 0xff00f000 );		// green
	Asura_DebugLineRender::AddArrow( xEyePosition, xEyePosition + m_xAvoidanceSteering * 2.0f, 0xfff00000 );		// red
	Asura_DebugLineRender::AddArrow( xEyePosition, xEyePosition + m_xFinalSteering * 2.0f, 0xfff0f0f0 );		// white

}
#endif

void DestinationTraverser::SwitchToState( const State eState )
{
	switch ( m_eState )
	{
		default:
		case UNSET:
		case MOVING_DIRECT_TO_DESTINATION:
		case TRAVERSING_TO_DESTINATION:
			break;
	}

	
	m_eState = eState;
	switch ( m_eState )
	{
		case MOVING_DIRECT_TO_DESTINATION:
		{
			const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
			const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();
			Asura_Vector_3 xToDestination = xNavData.m_xDestinationPosition - xOurPosition;
			xToDestination.Normalise();
			GetMovement()->GetAlienBrain()->ForceMovementDirection( xToDestination );
			GetMovement()->SetAllDirections(xToDestination);
			GetMovement()->RequestMovementAnimState();
			break;
		}

		case TRAVERSING_TO_DESTINATION:
		{
			GetMovement()->RequestMovementAnimState();
			break;
		}

		case TRAVERSING_AVOIDING_ORBIT:
		{
			GetMovement()->RequestMovementAnimState();
			break;
		}

		case UNSET:
		default:
			break;
	}
}

void DestinationTraverser::UpdateRouteFraction()
{
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	const Asura_Vector_3& xCurrentPosition = GetAlienBrain()->GetActor()->GetPosition();
	Asura_Vector_3 xActorGravityDirection;
	GetAlienBrain()->GetActor()->GetPhysicsObjectHandler()->GetCurrentGravityDirection( xActorGravityDirection );

	Asura_Vector_3 xDummy;
	u_short usDummy;

	GetMovement()->FindNearestPointOnRoute( xNavData, xCurrentPosition,	-xActorGravityDirection, 0.0f , xDummy, usDummy, false);

	m_fMaxRouteTraversedDistance = Asura_Maths::Max( m_fMaxRouteTraversedDistance, GetMovement()->m_fRouteTraversedDistance);

}


void DestinationTraverser::UpdateStuckDetection( const float fTimeslice, bool bLargeAvoidanceSteering )
{
	if (m_bStuck)
	{
		return;
	}

	// if we in the middle of a melee or a flinch then we are not stuck
	if (GetAlienBrain()->IsMelee())
	{
		return;
	}
	if (GetAlienBrain()->GetAlienActor()->IsFlinching())
	{
		return;
	}

	// check movement on the path
	if (m_fMaxRouteTraversedDistance > m_fLastMaxRouteFraction + 1.0f * fTimeslice )
	{
		m_fTimeInStuckOnPath = 0.0f;
		m_fFramesInStuckOnPath = 0.0f;
			m_fLastMaxRouteFraction = m_fMaxRouteTraversedDistance;
	}
	else
	{
		if (m_fMaxRouteTraversedDistance == 0.0f)
		{
			m_fTimeInStuckOnPath += fTimeslice / dbg_fTestTimeScaleAtStartOfPath;
			m_fFramesInStuckOnPath += 1.0f / dbg_fTestTimeScaleAtStartOfPath;
		}
		else if (bLargeAvoidanceSteering)
		{
			m_fTimeInStuckOnPath += fTimeslice / dbg_fTestTimeScaleAtAvoidanceOfPath;
			m_fFramesInStuckOnPath += 1.0f / dbg_fTestTimeScaleAtAvoidanceOfPath;
		}
		else
		{
			m_fTimeInStuckOnPath += fTimeslice;
			m_fFramesInStuckOnPath += 1.0f;
		}

		if ( m_fTimeInStuckOnPath > dbg_fTestTimeForMovementOnPath && m_fFramesInStuckOnPath > dbg_fTestFramesForMovementOnPath )
		{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
			Asura_Vector_3 xPosition = GetAlienBrain()->GetActor()->GetPosition();
			Asura_OutputToDebugger(("%d Alien %d stuck - did not advance on its path for %f seconds and %f frames, position %.2f,%.2f,%.2f\n",
				Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetActor()->GetGuid(), m_fTimeInStuckOnPath, m_fFramesInStuckOnPath, 
				xPosition.x, xPosition.y, xPosition.z));
#endif
			m_bStuck = true;
			return;
		}
	}

	// check reorientation and position
	m_fTimeCheckOrientationAndPositionStuck += fTimeslice;
	m_fFramesCheckOrientationAndPositionStuck += 1.0f;

	if (m_fTimeCheckOrientationAndPositionStuck > dbg_fTestTimeForMovementAndReorintation && 
		m_fFramesCheckOrientationAndPositionStuck > dbg_fTestFramesForMovementAndReorintation)
	{
		bool bStuckPosition = true;
		bool bStuckOrientation = true;

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


		float fAvarageVelocity = (xCurrentPosition - m_xLastPosition).Magnitude() / m_fTimeCheckOrientationAndPositionStuck;
		m_xLastPosition = xCurrentPosition;

		if (fAvarageVelocity  > 1.0f * dbg_fTestVelocityScale )
		{
			bStuckPosition = false;
		}

		const Asura_Matrix_3x3& xCurentOrientation = GetAlienBrain()->GetActor()->GetOrientation();
		Asura_Vector_3 xCurrVector(xCurentOrientation.e13, xCurentOrientation.e23, xCurentOrientation.e33);
		Asura_Vector_3 xLastVector(m_xLastOrientation.e13, m_xLastOrientation.e23, m_xLastOrientation.e33);

		if (xCurrVector * xLastVector < dbg_fTestMinimalOrientationCos)
		{
			bStuckOrientation = false;
		}
		else
		{
			xCurrVector = Asura_Vector_3(xCurentOrientation.e12, xCurentOrientation.e22, xCurentOrientation.e32);
			xLastVector = Asura_Vector_3(m_xLastOrientation.e12, m_xLastOrientation.e22, m_xLastOrientation.e32);
			if (xCurrVector * xLastVector < dbg_fTestMinimalOrientationCos)
			{
				bStuckOrientation = false;
			}
		}

		m_xLastOrientation = xCurentOrientation;
		m_bStuck = bStuckOrientation & bStuckPosition;
		m_bTryToUnstuckByJumping = bStuckOrientation & bStuckPosition; // if we really got stuck because lack of movement - can try to resolve this by jumping to the next waypoint

#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
		if (m_bStuck)
		{
			Asura_Vector_3 xPosition = GetAlienBrain()->GetActor()->GetPosition();
			Asura_OutputToDebugger(("%d Alien %d stuck - did not orientation and position did not change for %f seconds and %f frames, position %.2f,%.2f,%.2f (try to unstuck by jumping)\n",
				Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetActor()->GetGuid(), m_fTimeCheckOrientationAndPositionStuck, m_fFramesCheckOrientationAndPositionStuck,
				xPosition.x, xPosition.y, xPosition.z));
		}
#endif
		m_fFramesCheckOrientationAndPositionStuck = 0.0f;
		m_fTimeCheckOrientationAndPositionStuck = 0.0f;
	}
}

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Alien_Movement
//
//////////////////////////////////////////////////////////////////////////////


static int WaypointIDCompareFunction(const u_short& xKey1, const u_short& xKey2)
{
	return static_cast<int>(xKey2 - xKey1);
}

Asura_Vector_3            AVP_Alien_Movement::s_axTestPositions[ iNUM_TEST_POSITIONS ];
const Asura_AI_NavVertex* AVP_Alien_Movement::s_pxClosestWP = NULL;
float                     AVP_Alien_Movement::s_fSmallestDistanceSq = Asura_Maths::LargeFloat;

bool AVP_Alien_Movement::TransitionData::IsConvex() const
{
	switch ( m_eLinkTransitionType )
	{
		case AVP_Navigation::LTT_FLOOR_TO_WALL_CONVEX:
		case AVP_Navigation::LTT_WALL_TO_FLOOR_CONVEX:
		case AVP_Navigation::LTT_WALL_TO_CEILING_CONVEX:
		case AVP_Navigation::LTT_CEILING_TO_WALL_CONVEX:
		case AVP_Navigation::LTT_WALL_TO_WALL_90_CONVEX:
			return true;
		default:
			return false;
	}
}

AVP_Alien_Movement::AVP_Alien_Movement()
: m_eState( FINISHED )
, m_eMovementModeState( NORMAL )
, m_fTimeInState( 0.0f )
, m_pxAlienBrain( NULL )
, m_eLimitsDueToSharpTurnMode(AVP_Alien_Movement::EMM_FAST)
, m_eRequestedMoveMode(AVP_Alien_Movement::EMM_IDLE)
, m_eActualMoveMode(AVP_Alien_Movement::EMM_IDLE)
, m_uPerRouteFlags( 0 )
, m_uPersistentFlags( 0 )
, m_fArrivalRange( dbg_fMinArrivalRange )
, m_xStandardLinkTraverser()
, m_xSlowLinkTraverser()
, m_xJumpLinkTraverser()
, m_xGS2LinkTraverser()
, m_xEnvHookLinkTraverser()
, m_pxCurrentTraverser( NULL )
, m_fTraversalFailureScore( 0.0f )
, m_fJumpUpdateTimer( 0.0f )
, m_fPrevPeriodSinceDamage( 1.0f )
, m_fTimeSinceLastFall(100.0f)
, m_fTimeSinceLastJump(100.0f)
, m_fRouteTraversedDistance(0.0f)
, m_fRouteTotalDistance(0.0f)
, m_uIgnoreTargetForAvoidanceID(ASURA_GUID_UNREGISTERED)
, m_xWPDistanceFromStartCache(WaypointIDCompareFunction)
{
	m_xStandardLinkTraverser.SetMovement( this );
	m_xSlowLinkTraverser.SetMovement( this );
	m_xJumpLinkTraverser.SetMovement( this );
	m_xGS2LinkTraverser.SetMovement( this );
	m_xEnvHookLinkTraverser.SetMovement( this );
	m_xDestinationTraverser.SetMovement( this );

	m_bJumpFromDanger = true;
}

bool AVP_Alien_Movement::GetToPosition( const Asura_Vector_3& xDestination, float fArrivalRange, AVP_Navigation::AlienAStarFunctor& xFunctor, AVP_Alien_Movement::ExMovementMode eExMoveMode, bool bRefineRoute /*= true*/ )
{


	bool bCanHandleNow = CanPlanMove();

	if ( bCanHandleNow )
	{
		AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();	
		// Find the destination waypoint ID.
		AVP_Navigation::TestParams xTestParams( xNavData.m_uPathfinderFlags, ASURA_NAVVERTEX_MAX_ZONE_ID );
		u_short usDestinationWPID;
		if ( !AVP_Navigation::GetNearestWaypointID( xDestination, usDestinationWPID, xTestParams ) )
		{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
			Asura_OutputToDebugger(("%d Alien %d GetToPosition no way point in the destination %.2f, %.2f, %.2f\n",
				Asura_Timers::GetFrameNumber(),	GetAlienBrain()->GetOwnerGuid(), xDestination.x, xDestination.y, xDestination.z ));
#endif

			SetInError();
			SwitchToState( FINISHED );
			return false;
		}
		bool result = ActionGetToPosition( xDestination, usDestinationWPID, fArrivalRange, xFunctor, eExMoveMode, bRefineRoute );
		return result;
	}
	else
	{
		Asura_Assert(false, "Requesting to move while an uniterruptable move is in progress");
		return false;
	}
}

bool AVP_Alien_Movement::GetToPosition( u_short usDestinationWPID, float fArrivalRange, AVP_Navigation::AlienAStarFunctor& xFunctor, AVP_Alien_Movement::ExMovementMode eExMoveMode, bool bRefineRoute )
{

	bool bCanHandleNow = CanPlanMove();

	if ( bCanHandleNow )
	{
		const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( usDestinationWPID );
		if (!pxWP)
		{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
			Asura_OutputToDebugger(("%d Alien %d GetToPosition way point %d not found\n",
				Asura_Timers::GetFrameNumber(),	GetAlienBrain()->GetOwnerGuid(), usDestinationWPID ));
#endif
			SetInError();
			SwitchToState( FINISHED );
			return false;
		}
		const Asura_Vector_3& xDestination = pxWP->GetPosition();
		bool result = ActionGetToPosition( xDestination, usDestinationWPID, fArrivalRange, xFunctor, eExMoveMode, bRefineRoute );
		return result;
	}
	else
	{
		Asura_Assert(false, "Requesting to move while an uniterruptable move is in progress");
		return false;
	}
}

bool AVP_Alien_Movement::GetToPosition( const Asura_Vector_3& xDestination, u_short usDestinationWPID, float fArrivalRange, AVP_Navigation::AlienAStarFunctor& xFunctor, AVP_Alien_Movement::ExMovementMode eExMoveMode, bool bRefineRoute )
{

	bool bCanHandleNow = CanPlanMove();

	if ( bCanHandleNow )
	{
		const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( usDestinationWPID );
		if (!pxWP)
		{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
			Asura_OutputToDebugger(("%d Alien %d GetToPosition way point %d not found\n",
				Asura_Timers::GetFrameNumber(),	GetAlienBrain()->GetOwnerGuid(), usDestinationWPID ));
#endif
			SetInError();
			SwitchToState( FINISHED );
			return false;
		}

		if (!pxWP->Contains(xDestination))
		{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
			Asura_OutputToDebugger(("%d Alien %d GetToPosition way point %d does not contains destination %.2f, %.2f, %.2f\n",
				Asura_Timers::GetFrameNumber(),	GetAlienBrain()->GetOwnerGuid(), usDestinationWPID, xDestination.x, xDestination.y, xDestination.z ));
#endif
			SetInError();
			SwitchToState( FINISHED );
			return false;

		}

		bool result = ActionGetToPosition( xDestination, usDestinationWPID, fArrivalRange, xFunctor, eExMoveMode, bRefineRoute );
		return result;
	}
	else
	{
		Asura_Assert(false, "Requesting to move while an uniterruptable move is in progress");
		return false;
	}
}

bool AVP_Alien_Movement::GetAwayFromPosition( AVP_Navigation::AlienAwayFromFunctor& xFunctor, AVP_Alien_Movement::ExMovementMode eExMoveMode )
{
	bool bCanHandleNow = CanPlanMove();


	if ( bCanHandleNow )
	{

		return ActionGetAwayFromPosition( xFunctor, eExMoveMode );
	}
	else
	{
		Asura_Assert(false, "Requesting to move while an uniterruptable move is in progress");
		return false;
	}
}



bool AVP_Alien_Movement::FollowSharedRoute( const Asura_Guid uRouteOwner, AVP_Navigation::AlienAStarFunctor& xFunctor, AVP_Alien_Movement::ExMovementMode eExMoveMode )
{
	bool bCanHandleNow = CanPlanMove();


	if ( bCanHandleNow )
	{
		return ActionFollowSharedRoute( uRouteOwner, xFunctor, eExMoveMode );
	}
	else
	{
		Asura_Assert(false, "Requesting to move while an uniterruptable move is in progress");
		return false;
	}
}

bool AVP_Alien_Movement::StartMoving()
{
	bool bCanHandleNow = CanPlanMove();

	if ( bCanHandleNow )
	{
		SwitchToState( MOVING );
		return true;
	}
	else
	{
		Asura_Assert(false, "Requesting to move while an uniterruptable move is in progress");
		return false;
	}
}

void AVP_Alien_Movement::Stop()
{
	bool bCanHandleNow = CanPlanMove();

	if ( bCanHandleNow )
	{
		RequestIdle();
		SwitchToState( FINISHED );
	}
	else
	{
		Asura_Assert(false, "Requesting to stop while an uniterruptable move is in progress");
	}
}

void AVP_Alien_Movement::Abort()
{
	m_bAbort = true;
}

void AVP_Alien_Movement::Fall()
{
	bool bCanHandleNow = CanPlanMove();

	if ( bCanHandleNow )
	{
		ActionFall();
	}
	else
	{
		Asura_Assert(false, "Requesting to fall while an uniterruptable move is in progress");
	}
}


void AVP_Alien_Movement::UpdateMovementModeState( const float fTimeslice )
{
	const float fPeriodSinceDamage = GetAlienBrain()->GetActor()->GetLastDamageInterval();
	const bool bShouldBeSlowDueToDamage = false; // ( 0.0f < fPeriodSinceDamage && fPeriodSinceDamage < fHIT_DELAY_PERIOD );
	
	switch ( m_eMovementModeState )
	{
		default:
		case NORMAL:
		{
			if ( bShouldBeSlowDueToDamage )
			{
				m_eMovementModeState = SLOWED_BY_HIT;
			}
			else if ( m_bIsBursting )
			{
				m_eMovementModeState = ADDING_BURST;
			}
			break;
		}

		case ADDING_BURST:
		{
			if ( bShouldBeSlowDueToDamage )
			{
				m_eMovementModeState = SLOWED_BY_HIT;
			}
			else if ( !m_bIsBursting )
			{
				m_eMovementModeState = NORMAL;
			}
			
			break;
		}

		case SLOWED_BY_HIT:
		{
			if ( fPeriodSinceDamage > fHIT_DELAY_PERIOD )
			{
				m_eMovementModeState = ( m_bIsBursting ) ? ADDING_BURST : NORMAL;
			}
			break;
		}
	}

	RequestMovementAnimState();
}

void AVP_Alien_Movement::UpdateFallingDueToDamage()
{
	// FOR NOW FALLING IS DISABLED ALWAYS
	return;

// 	if (m_bFallingDisallowed || !m_bNotGS2Mode)
// 	{
// 		return;
// 	}
// 
// 	const bool bHasEverBeenDamaged = GetAlienBrain()->GetActor()->HasEverBeenDamaged();
// 
// 	const float fPeriodSinceDamage = bHasEverBeenDamaged ? GetAlienBrain()->GetActor()->GetLastDamageInterval() : m_fPrevPeriodSinceDamage * 2.0f;
// 	if ( m_eState != FALLING && (fPeriodSinceDamage < m_fPrevPeriodSinceDamage || GetAlienBrain()->IsInjured()) )
// 	{
// 		if (GetAlienBrain()->IsGS2() || GetAlienBrain()->IsMelee())
// 		{
// 			// no falling during GS2 move or melee
// 		}
// 		else if (!IsJumping())
// 		{
// 			const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
// 			const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usCurrentWPID );
// 			Asura_Assert(pxCurrentWP, "Can not find current way point");
// 			if ( pxCurrentWP && (pxCurrentWP->IsCeilingWaypoint() || pxCurrentWP->IsWallWaypoint()) )
// 			{
// 				SwitchToState( FALLING );
// 			}
// 		}
// 		else if (!m_bFallDueToDamageDeferred)
// 		{
// 			GetAlienBrain()->SetFalling(true);
// 			m_bFallDueToDamageDeferred = true; 
// 		}
// 	}
// 	m_fPrevPeriodSinceDamage = fPeriodSinceDamage;
}

void AVP_Alien_Movement::UpdatePositionOnRoute()
{
	Asura_Assert(GetAlienBrain()->GetActor()->GetPhysicsObjectHandler(), "AVP_Alien_Movement::UpdatePositionOnRoute() relies on physics object handler which is NULL");
	if (!GetAlienBrain()->GetActor()->GetPhysicsObjectHandler()) return;

	AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	Asura_Vector_3 xNearestPositionOnRoute;
	Asura_Vector_3 xActorGravityDirection;

	GetAlienBrain()->GetActor()->GetPhysicsObjectHandler()->GetCurrentGravityDirection( xActorGravityDirection );

	FindNearestPointOnRoute( xNavData, GetAlienBrain()->GetActor()->GetPosition(), xActorGravityDirection, 0.0f, xNearestPositionOnRoute, xNavData.m_usRouteSegmentIndex, false );
	Asura_Assert( xNavData.m_usRouteSegmentIndex < xNavData.m_usRouteLength, "AVP_Alien_Movement::UpdatePositionOnRoute - bug in AVP_Alien_Movement::FindNearestPointOnRoute" );
}

float AVP_Alien_Movement::GetLengthLeftOnPath() const
{
	return m_fRouteTotalDistance > m_fRouteTraversedDistance ? m_fRouteTotalDistance - m_fRouteTraversedDistance : 0.0f;
}

float AVP_Alien_Movement::GetLengthTraversedOnPath() const
{
	return m_fRouteTraversedDistance;
}


void AVP_Alien_Movement::CalculatePointSteering( const Asura_Vector_3& xPoint, Asura_Vector_3& xSteering )
{
	GetAlienBrain()->ResolveToOurPlane( ( xPoint - GetAlienBrain()->GetActor()->GetPosition() ), xSteering );
	xSteering.Normalise();
}

void AVP_Alien_Movement::SetAllDirections( const Asura_Vector_3& xDirection )
{
	GetAlienBrain()->SetMovementDirection( xDirection );
	GetAlienBrain()->SetLookAtPosition( GetAlienBrain()->GetActor()->GetPosition() + xDirection * 10.0f );

	GetAlienBrain()->SetAimDirection( xDirection );
}

void AVP_Alien_Movement::ForceMoveDirection( const Asura_Vector_3& xDirection )
{
	GetAlienBrain()->ForceMovementDirection( xDirection );
}


bool AVP_Alien_Movement::FindNearestPointOnRoute( const AVP_Navigation::NavData& xNavData, const Asura_Vector_3& xCurrentPosition, const Asura_Vector_3& xActorUp,
												  float fLookAheadDistance, Asura_Vector_3& xNearestPosition, u_short& usNearestRouteSegmentIndex, 
												  bool bStopOnGS2AndReorientation )
{
	// We always start from a few segments back, meaning the follow point can track backwards.
	// We optionally don't follow onto special links (transitions, gs2, etc).
	// We also don't follow any further once the up vector of the follow point passes a certain angle from the actor's current up vector.

	m_fRouteTraversedDistance = 0.0f;

	if ( xNavData.m_usRouteLength == 0 )
	{
		Asura_Assert( false, "AVP_Alien_Movement::FindNearestPointOnRoute - zero route length" );
		xNearestPosition = xCurrentPosition;
		usNearestRouteSegmentIndex = 0;
		return false;
	}
	if ( xNavData.m_usRouteSegmentIndex >= xNavData.m_usRouteLength - 1 )
	{
		const Asura_AI_NavVertex* pxDestWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usDestinationWPID );
		Asura_Assert( pxDestWP, "AVP_Alien_Movement::FindNearestPointOnRoute - no destination waypoint found" );
		if ( pxDestWP )
		{
			xNearestPosition = pxDestWP->m_xPosition;
		}
		usNearestRouteSegmentIndex = 0;
		m_fRouteTraversedDistance = FindDistanceOfWPFromStart(xNavData.m_usRouteLength-1);
		return true;
	}

    Asura_Vector_3 xCurrentSegmentStart;
    Asura_Vector_3 xCurrentSegmentEnd;

	usNearestRouteSegmentIndex = xNavData.m_usRouteSegmentIndex;

    float fLeastDistanceSq = Asura_Maths::LargeFloat;
    for ( u_short usSegmentIndex = xNavData.m_usRouteSegmentIndex;//static_cast< u_short >( Asura_Maths::Max( xNavData.m_usRouteSegmentIndex - 3, 0 ) );
		usSegmentIndex < xNavData.m_usRouteLength - 1 && usSegmentIndex < xNavData.m_usRouteSegmentIndex + 5;
		++usSegmentIndex )
    {
        xCurrentSegmentStart = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usSegmentIndex ] )->m_xPosition;
        xCurrentSegmentEnd   = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usSegmentIndex + 1 ] )->m_xPosition;

        Asura_Vector_3 xPosition;
        Asura_Maths::GetNearestPointOnLineSegment( xCurrentSegmentStart, xCurrentSegmentEnd, xCurrentPosition, xPosition );
        const float fDistanceSq = ( xPosition - xCurrentPosition ).MagnitudeSquared();
        if ( fDistanceSq < fLeastDistanceSq )
        {
            fLeastDistanceSq           = fDistanceSq;
            usNearestRouteSegmentIndex = usSegmentIndex;
            xNearestPosition           = xPosition;
		}
    }

	
	const Asura_AI_NavVertex* pxFromWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usNearestRouteSegmentIndex ] );
	const Asura_AI_NavVertex* pxToWP   = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usNearestRouteSegmentIndex + 1 ] );
    xCurrentSegmentStart = pxFromWP->m_xPosition;
    xCurrentSegmentEnd   = pxToWP->m_xPosition;

	Asura_Assert(usNearestRouteSegmentIndex < xNavData.m_usRouteLength - 1, "Segment mismatch");

    if ( usNearestRouteSegmentIndex == xNavData.m_usRouteLength - 2 && xNearestPosition.ApproximatelyEquals( xCurrentSegmentEnd ) )
    {
		m_fRouteTraversedDistance = FindDistanceOfWPFromStart(xNavData.m_usRouteLength-1);
        return true;
    }

	// calculate the position in our plane and project back to the segment - this will prevent going backward.
	AVP_Plane xOurPlane(xCurrentPosition, xActorUp );
	Asura_Vector_3 xCurrentSegmentStartInOurPlane;
	Asura_Vector_3 xCurrentSegmentEndInOurPlane;
	xOurPlane.CalculateClosestPoint(xCurrentSegmentStart, xCurrentSegmentStartInOurPlane);
	xOurPlane.CalculateClosestPoint(xCurrentSegmentEnd, xCurrentSegmentEndInOurPlane);

	float fLamda;

	bool bNearsetRouteSegmentIsJump = false;
	bool bFound = false;
	for ( Asura_LinkList_ConstIt< TransitionData > xTransitionIt( &m_xTransitionDataList ); !bFound &&!xTransitionIt.Done(); xTransitionIt.Next() )
	{

		const TransitionData* pxTransitionData = xTransitionIt.GetCurrent();
		if (pxTransitionData && pxTransitionData->m_usRouteIndex == usNearestRouteSegmentIndex )
		{
			bFound = true;
			bNearsetRouteSegmentIsJump = pxTransitionData->m_bPotentialJump;
		}
	}

	if (bNearsetRouteSegmentIsJump) 
	{
		fLamda = 0.0f; // for jump links make sure we do not skip them
	}
	else // otherwise assume we at the end so we dont move backward.
	{
		if (xCurrentSegmentStartInOurPlane.ApproximatelyEquals(xCurrentSegmentEndInOurPlane)) 
		{
			fLamda = 1.0f;
		}
		else
		{
			Asura_Vector_3 xPositionInOurPlane;
			Asura_Maths::GetNearestPointOnLineSegment( xCurrentSegmentStartInOurPlane, xCurrentSegmentEndInOurPlane, xCurrentPosition, xPositionInOurPlane );

			fLamda = (xPositionInOurPlane - xCurrentSegmentStartInOurPlane).Magnitude() / (xCurrentSegmentEndInOurPlane-xCurrentSegmentStartInOurPlane).Magnitude();		
		}
	}

	xNearestPosition = xCurrentSegmentStart + (xCurrentSegmentEnd-xCurrentSegmentStart) * fLamda;

	m_fRouteTraversedDistance = FindDistanceOfWPFromStart(usNearestRouteSegmentIndex) + (xNearestPosition-xCurrentSegmentStart).Magnitude();

	const Asura_AI_NavConnection* pxSegmentLink = pxFromWP->GetLinkTo( xNavData.m_pusRoute[ usNearestRouteSegmentIndex + 1 ] );
	// These always pass first time, because we always want to move along the segment we're currently on.
	bool  bIsGS2Segment			= false;
	float fActorUpFromNormalDot = 1.0f;
    while ( fLookAheadDistance > 0.0f && (!bStopOnGS2AndReorientation || (!bIsGS2Segment && fActorUpFromNormalDot >= dbg_fLargeReorientCosine)) )
    {
        Asura_Vector_3 xSegmentDirection = xCurrentSegmentEnd - xCurrentSegmentStart;
        xSegmentDirection.Normalise();
        const float fToSegmentEnd = ( xCurrentSegmentEnd - xNearestPosition ).Magnitude();

        xNearestPosition   += xSegmentDirection * Asura_Maths::Min( fLookAheadDistance, fToSegmentEnd );
        fLookAheadDistance -= fToSegmentEnd;
        if ( fLookAheadDistance > 0.0f )
        {
            if ( ++usNearestRouteSegmentIndex < xNavData.m_usRouteLength - 1 )
            {
				pxFromWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usNearestRouteSegmentIndex ] );
				pxToWP   = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usNearestRouteSegmentIndex + 1 ] );
                xCurrentSegmentStart = pxFromWP->m_xPosition;
                xCurrentSegmentEnd   = pxToWP->m_xPosition;

				pxSegmentLink = pxFromWP->GetLinkTo( xNavData.m_pusRoute[ usNearestRouteSegmentIndex + 1 ] );
				bIsGS2Segment = ( pxSegmentLink ? AVP_Navigation::IsGS2Link( static_cast< u_short >( pxSegmentLink->GetFlags() ) ) : false );
				fActorUpFromNormalDot = xActorUp * pxFromWP->GetNormal();
            }
            else
            {
				usNearestRouteSegmentIndex = xNavData.m_usRouteLength - 1;
                xNearestPosition = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usDestinationWPID )->m_xPosition;
                return true;
            }
        }
    }

    return false;
}

float AVP_Alien_Movement::GetRouteFreeDistance() const
{
	if ( m_pxCurrentTraverser )
	{
		return m_pxCurrentTraverser->GetRouteFreeDistance();
	}
	else
	{
		return 0.0f;
	}
}


DEBUGVAR float dbg_fBigMinDistanceTraveled = 1.0f;
DEBUGVAR float dbg_fBigPercentageTraveled = 50.0f;
bool AVP_Alien_Movement::ConsiderateAdvanceAlongRoute()
{
	AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	if (!xNavData.m_pusRoute || xNavData.m_usRouteLength == 0 )
	{
		return false;
	}

	if (m_fRouteTraversedDistance > dbg_fBigMinDistanceTraveled)
	{
		return true;
	}

	float fTotalDistance = FindDistanceOfWPFromStart(xNavData.m_usRouteLength-1);

	if (fTotalDistance == 0.0f)
	{
		return true;
	}
	
	if (m_fRouteTraversedDistance/fTotalDistance > dbg_fBigPercentageTraveled / 100.0f)
	{
		return true;
	}

#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
	Asura_OutputToDebugger(("%d Alien %d did not advance much in last movement Traveled %f, Route %f\n",
						   Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetActor()->GetGuid(), m_fRouteTraversedDistance, fTotalDistance));
#endif

	return false;

}

float AVP_Alien_Movement::GetMinArrivalRange() 
{
	return dbg_fMinArrivalRange;
}

float AVP_Alien_Movement::GetMaxArrivalRange() 
{
	return dbg_fMaxArrivalRange;
}

void AVP_Alien_Movement::SetMovementMode( ExMovementMode eMovementMode )
{

	m_eRequestedMoveMode = eMovementMode;

	switch ( m_eMovementModeState )
	{
		default:
		case NORMAL:
		{

			m_eActualMoveMode = Asura_Maths::Min(m_eLimitsDueToSharpTurnMode, m_eRequestedMoveMode);

			RequestMovementAnimState();
			break;
		}

		case ADDING_BURST:
		{
			m_eActualMoveMode = Asura_Maths::Min(m_eLimitsDueToSharpTurnMode, m_eRequestedMoveMode);
			if ( m_eState == TRAVERSING_ROUTE || m_eState == MOVING )
			{
				RequestMovementAnimState();
			}

			break;
		}

		case SLOWED_BY_HIT:
		{
			m_eActualMoveMode = Asura_Maths::Min(EMM_SLOW, m_eRequestedMoveMode);
			break;
		}
	}
}

void AVP_Alien_Movement::SetRouteType( const AVP_Navigation::RouteType eRouteType )
{
	// Wrapper in case we need to do something with this data.
	GetAlienBrain()->GetNavData().m_eRouteType = eRouteType;
}

void AVP_Alien_Movement::SetTraverseStyle( const AVP_Navigation::TraverseStyle eTraverseStyle )
{
	// Wrapper in case we need to do something with this data.
	GetAlienBrain()->GetNavData().m_eTraverseStyle = eTraverseStyle;
}

void AVP_Alien_Movement::SetBurst( bool bBurst )
{
 	m_bIsBursting = bBurst;
	if (m_bIsBursting)
	{
		SetMovementMode(EMM_FAST);
	}
	else
	{
		SetMovementMode(EMM_MEDIUM);
	}
 	GetAlienBrain()->GetAlienSquad()->SetBursting(GetAlienBrain()->GetActor()->GetGuid(), bBurst);
}

void AVP_Alien_Movement::SetIgnoreTargetForAvoidCalcuation(Asura_Guid uTargetGuid)
{
	m_uIgnoreTargetForAvoidanceID = uTargetGuid;
}
void AVP_Alien_Movement::ResetIgnoreTargetForAvoidCalcuation()
{
	m_uIgnoreTargetForAvoidanceID = ASURA_GUID_UNREGISTERED;
}


bool AVP_Alien_Movement::HandleMessage( const Asura_Message* pxMessage )
{
	if ( pxMessage->GetMsgID() == AVP_MSGID_PHYSICS_REORIENTATION_RESET && m_eState != FALLING )
	{
		// If we've fallen off the environment we need to stop and then re-find a route once we're on the ground again.
		if (!IsJumping() && !GetAlienBrain()->IsMelee())
		{
			SwitchToState( FALLING );
		}
		else if (!m_bFallDueToDamageDeferred)
		{
			GetAlienBrain()->SetFalling(true);
			m_bFallDueToDamageDeferred = true; 
		}
		return true;
	} 
	if (pxMessage->GetMsgID() == ASURA_MSG_MOVECHAINCOMPLETE)
	{
		if ( !GetAlienBrain()->IsMelee() && (m_eState == JUMPING_FROM_DANGER || m_eState == RESOLVE_STUCK_BY_JUMPING) )
		{
			if (m_bFallDueToDamageDeferred)
			{
				SwitchToState( FALLING );
			}
			else
			{
				SwitchToState( FINISHED );
			}
			return true;
		}
		else if (m_eState == FALLING)
		{
			SetInError(true);
			SwitchToState( FINISHED );
			return true;
		}
	}

	return m_pxCurrentTraverser ? m_pxCurrentTraverser->HandleMessage( pxMessage ) : false;
}

void AVP_Alien_Movement::Initialise()
{
	m_bIsShutDown = false;
#ifndef ASURA_NO_DEBUG_OUTPUT
	GetAlienBrain()->SetRenderRoute( true );
#endif
}

void AVP_Alien_Movement::Shutdown()
{
	m_bIsShutDown = true;
	SwitchToState( FINISHED );

#ifndef ASURA_NO_DEBUG_OUTPUT
	GetAlienBrain()->SetRenderRoute( false );
#endif

	m_xTransitionDataList.Clear();
}

void AVP_Alien_Movement::Pause()
{
	if ( m_pxCurrentTraverser )
	{
		m_pxCurrentTraverser->Pause();
	}
}

void AVP_Alien_Movement::Unpause()
{
	if ( m_pxCurrentTraverser )
	{
		m_pxCurrentTraverser->Unpause();
	}
}

void AVP_Alien_Movement::PerFrameUpdate( const float fTimeslice )
{
	if ( fTimeslice == 0.0f ) { return; }
	m_fTimeInState += fTimeslice;
	m_fJumpUpdateTimer -= fTimeslice;
	m_fTimeSinceLastFall += fTimeslice;
	m_fTimeSinceLastJump += fTimeslice;
	UpdateMovementModeState( fTimeslice );
	UpdateFallingDueToDamage();

	if (GetAlienBrain()->IsMelee()) // we can not do anything while a melee is in progress
	{
		return;
	}

	if (!m_bJumpingDisallowed && GetAlienBrain()->IsInjured())
	{
		m_bJumpingDisallowed = true;
	}

	switch ( m_eState )
	{
		default:
		case FINISHED:
		{
			if (m_bFallDueToDamageDeferred && !GetAlienBrain()->IsMelee()) // we still have a deferred fall 
			{
				SwitchToState(FALLING);
			}
			break;
		}

		case FALLING:
		{
			if (!m_bFallingDisallowed && GetAlienBrain()->IsInjured())
			{
				m_bFallingDisallowed = true; // only one falling is allowed in injured
			}
			break;
		}

		case JUMPING_FROM_DANGER:
		{
			// Hard-coded timeout in case we don't receive the message we're expecting.
// 			if ( m_fTimeInState > 2.0f )
// 			{
// 				SetInError(true);
// 				SwitchToState( FINISHED );
// 			}

			break;
		}
		case RESOLVE_STUCK_BY_JUMPING:
		{
			// Hard-coded timeout in case we don't receive the message we're expecting.
// 			if ( m_fTimeInState > 2.0f )
// 			{
// 				SetInError(true);
// 				SwitchToState( FINISHED );
// 			}

			break;
		}
		case MOVING:
		{
			//## could check for moving out of waypointed space here and stop automatically
			//## could also implement character avoidance
			break;
		}

		case TRAVERSING_ROUTE:
		{
			if ( GetAlienBrain()->GetNavData().m_usRouteLength == 0 )
			{
				Asura_Assert( false, "" );
				SwitchToState( FINISHED	);
				break;
			}

			UpdatePositionOnRoute();

			// If we are at the destination then we're done.
			AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
			const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();
			AVP_Navigation::APParams xAPParams;
			xAPParams.m_fMaxCoplanarOffset = m_fArrivalRange;
			xAPParams.m_xDestinationNormal = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usDestinationWPID )->GetNormal();
			if (m_bAbort && !GetAlienBrain()->IsJumping() && !GetAlienBrain()->IsGS2() && !GetAlienBrain()->IsFalling())
			{
				SwitchToState( FINISHED );
				break;
			}

			if ( ( !GetAlienBrain()->IsJumping() && !GetAlienBrain()->IsGS2() && !GetAlienBrain()->IsFalling()) && AVP_Navigation::AtPosition( xOurPosition, xNavData.m_xDestinationPosition, xAPParams ) )
			{
				// This is in case waypoints overlap and the nav data thinks we're in a different one to our destination.
				xNavData.m_usCurrentWPID = xNavData.m_usDestinationWPID;
				SwitchToState( FINISHED );
				break;
			}

			if ( !m_pxCurrentTraverser )
			{
				Asura_Assert( false, "AVP_Alien_Movement::PerFrameUpdate - should always have a traverser when TRAVERSING_ROUTE" );
				SetInError();
				SwitchToState( FINISHED );
			}
			else
			{
				// Update the current traverser and stop it if necessary.
				if ( m_pxCurrentTraverser )
				{
					m_pxCurrentTraverser->PerFrameUpdate( fTimeslice );
					if ( m_pxCurrentTraverser->IsInError() )
					{
						if (! (m_pxCurrentTraverser->IsTryToResolveByJumping() && !m_bJumpingDisallowed && AttemptJumpToUnStuck() ))
						{
							SetInError();
							SwitchToState( FINISHED );
						}
						break;
					}
					else if ( m_pxCurrentTraverser->IsFinished() )
					{
						m_pxCurrentTraverser->Stop();
						m_pxCurrentTraverser = NULL;
					}
				}

				// Find the next traverser, if any, and start it if necessary and allowed.
				LinkTraverser* pxNextTraverser = FindNextTraverser();
				if ( pxNextTraverser && pxNextTraverser != m_pxCurrentTraverser )
				{
					if ( m_pxCurrentTraverser && m_pxCurrentTraverser->CanBeInterrupted() )
					{
						m_pxCurrentTraverser->Stop();
						m_pxCurrentTraverser = NULL;
					}
					if ( !m_pxCurrentTraverser )
					{
						m_pxCurrentTraverser = pxNextTraverser;
						m_pxCurrentTraverser->Start();
					}
				}
			}

			// This comes last as it might change our state away.
			UpdateJumping();

			break;
		}
	}
}

void AVP_Alien_Movement::Update( const float fTimeslice )
{
	if ( m_pxCurrentTraverser )
	{
		m_pxCurrentTraverser->Update( fTimeslice );
	}
}

#ifndef ASURA_NO_DEBUG_OUTPUT
void AVP_Alien_Movement::RenderDebugInfo()
{
	const char* s_aszStateStrings[ NUM_STATES ] =
	{
		"FINISHED",
		"MOVING",
		"FALLING",
		"JUMPING_FROM_DANGER",
		"TRAVERSING_ROUTE",
		"RESOLVE_STUCK_BY_JUMPING",
	};
	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	char cRequestedEMM = '?';
	char cLimitedEMM = '?';
	char cActualEMM = '?';

	switch (m_eLimitsDueToSharpTurnMode)
	{
	case EMM_IDLE:
		cLimitedEMM = 'I';
		break;
	case EMM_SLOW:
		cLimitedEMM = 'S';
		break;
	case EMM_MEDIUM:
		cLimitedEMM = 'M';
		break;
	case EMM_FAST:
		cLimitedEMM = 'F';
		break;
	}

	switch (m_eRequestedMoveMode)
	{
	case EMM_IDLE:
		cRequestedEMM = 'I';
		break;
	case EMM_SLOW:
		cRequestedEMM = 'S';
		break;
	case EMM_MEDIUM:
		cRequestedEMM = 'M';
		break;
	case EMM_FAST:
		cRequestedEMM = 'F';
		break;
	}

	switch (m_eActualMoveMode)
	{
	case EMM_IDLE:
		cActualEMM = 'I';
		break;
	case EMM_SLOW:
		cActualEMM = 'S';
		break;
	case EMM_MEDIUM:
		cActualEMM = 'M';
		break;
	case EMM_FAST:
		cActualEMM = 'F';
		break;
	}

	Asura_Font_System::Print( GetAlienBrain()->GetDebugOutputPos().x, GetAlienBrain()->GetDebugOutputPos().y, 0.5f, 0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "Alien Movement: %s: %0.2f %d %f  %c %c %c",
		s_aszStateStrings[ m_eState ], m_fTraversalFailureScore, 
		xNavData.m_usCurrentWPID, m_fRouteTraversedDistance, cActualEMM, cRequestedEMM, cLimitedEMM);
	GetAlienBrain()->GetDebugOutputPos().y += 10.0f;

	if ( m_pxCurrentTraverser )
	{
		m_pxCurrentTraverser->RenderDebugInfo();
	}
}
#endif

const AVP_Alien_Movement::TransitionData* AVP_Alien_Movement::GetTransitionDataElt( const u_int uIndex ) const
{
	u_int uSearchIndex = 0;
	for ( Asura_LinkList_ConstIt< TransitionData > xTransitionIt( &m_xTransitionDataList ); !xTransitionIt.Done(); xTransitionIt.Next() )
	{
		if ( uSearchIndex == uIndex )
		{
			return xTransitionIt.GetCurrent();
		}
		else
		{
			++uSearchIndex;
		}
	}

	return NULL;
}

DEBUGVAR float dbg_fChaceToJumpFloorToWall = 0.7f;
DEBUGVAR float dbg_fChaceToJumpWallToFloor = 0.3f;

void AVP_Alien_Movement::CalculateRouteTransitions()
{
	m_xTransitionDataList.Clear();

	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	for ( u_short usRouteIndex = 0; usRouteIndex < xNavData.m_usRouteLength - 1; ++usRouteIndex )
	{
		const Asura_AI_NavVertex* pxToWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usRouteIndex + 1 ] );
		if ( !pxToWP ) { continue; }

		const AVP_Navigation::LinkTransitionType eLTT = AVP_Navigation::GetLinkTransitionType( xNavData.m_pusRoute[ usRouteIndex ], xNavData.m_pusRoute[ usRouteIndex + 1 ] );
		if ( eLTT != AVP_Navigation::LTT_NO_TRANSITION && eLTT != AVP_Navigation::NUM_LINK_TRANSITION_TYPES )
		{
			TransitionData xTransitionData;
			xTransitionData.m_eLinkTransitionType  = eLTT;
			xTransitionData.m_usRouteIndex         = usRouteIndex;
			xTransitionData.m_xDestinationPosition = pxToWP->m_xPosition;
			xTransitionData.m_bPotentialJump       = false;
			xTransitionData.m_bMoveAfterwards      = ( ( usRouteIndex + 1 ) < xNavData.m_usRouteLength );
			xTransitionData.m_xDestinationNormal   = pxToWP->GetNormal();
			m_xTransitionDataList.Push_Back( xTransitionData );
		}
	}

	// Now traverse the route transitions and see if any can be jumped.
	for ( Asura_LinkList_It< TransitionData > xTransitionDataIt( &m_xTransitionDataList ); !xTransitionDataIt.Done(); xTransitionDataIt.Next() )
	{
		TransitionData* pxTransitionData = xTransitionDataIt.GetCurrent();

		if ( pxTransitionData->m_eLinkTransitionType == AVP_Navigation::LTT_FLOOR_TO_WALL_CONCAVE )
		{
			pxTransitionData->m_bPotentialJump = true; // Asura_Random::GetFloat(0.0f, 1.0f) < dbg_fChaceToJumpFloorToWall;
		}
		else if ( pxTransitionData->m_eLinkTransitionType == AVP_Navigation::LTT_WALL_TO_FLOOR_CONCAVE )
		{
			pxTransitionData->m_bPotentialJump = true; // Asura_Random::GetFloat(0.0f, 1.0f) < dbg_fChaceToJumpWallToFloor;
		}
		else if ( pxTransitionData->m_eLinkTransitionType == AVP_Navigation::LTT_FLOOR_TO_CEILING ||
				  pxTransitionData->m_eLinkTransitionType == AVP_Navigation::LTT_CEILING_TO_FLOOR ||
				  pxTransitionData->m_eLinkTransitionType == AVP_Navigation::LTT_WALL_TO_WALL_180 || 
				  pxTransitionData->m_eLinkTransitionType == AVP_Navigation::LTT_ENVHOOK ||
				  pxTransitionData->m_eLinkTransitionType == AVP_Navigation::LTT_ENVHOOK_JUMPOVER)
		{
			pxTransitionData->m_bPotentialJump = true;
		}
	}
}

LinkTraverser* AVP_Alien_Movement::FindNextTraverser()
{
	LinkTraverser* pxNextTraverser = NULL;
	if ( m_xDestinationTraverser.ShouldStart() )
	{
		pxNextTraverser = &m_xDestinationTraverser;
	}
	else if ( m_xGS2LinkTraverser.ShouldStart() )
	{
		pxNextTraverser = &m_xGS2LinkTraverser;
	}
	else if ( m_xEnvHookLinkTraverser.ShouldStart() )
	{
		pxNextTraverser = &m_xEnvHookLinkTraverser;
	}
	else if ( m_xJumpLinkTraverser.ShouldStart() )
	{
		pxNextTraverser = &m_xJumpLinkTraverser;
	}
	else if ( m_xSlowLinkTraverser.ShouldStart() )
	{
		pxNextTraverser = &m_xSlowLinkTraverser;
	}
	else if ( m_xStandardLinkTraverser.ShouldStart() )
	{
		pxNextTraverser = &m_xStandardLinkTraverser;
	}
	return pxNextTraverser;
}

void AVP_Alien_Movement::SwitchToState( const State eNextState )
{
    // Do any work we need to exit our current state.
    switch ( m_eState )
    {
		case TRAVERSING_ROUTE:
		{
			if ( m_pxCurrentTraverser )
			{
				Asura_Assert(m_pxCurrentTraverser->CanBeInterrupted() || 
							( eNextState == FINISHED || eNextState == FALLING ), "stopping a traverser that can not be interrupted" );
				m_pxCurrentTraverser->Stop();
				m_pxCurrentTraverser = NULL;
			}
			break;
		}

		case MOVING:
		{
			break;
		}


		case JUMPING_FROM_DANGER:
		{
			SetInError(true);
			m_fJumpUpdateTimer = fJUMP_PERIOD;
			GetAlienBrain()->SetJumping(false);			
			break;
		}
		case RESOLVE_STUCK_BY_JUMPING:
			SetInError(true);
			GetAlienBrain()->SetJumping(false);			
			break;

		case FALLING:
		{
			SetInError(true);
			m_fTimeSinceLastFall = 0.0f;
			GetAlienBrain()->SetFalling(false);			
			break;
		}
		default:
		case FINISHED:
        case NUM_STATES:
        {
            break;
        }
    }

    // Do any work we need to change into our next state.
    m_eState = eNextState;
	m_fTimeInState  = 0.0f;
    switch ( m_eState )
    {
        case FINISHED:
        {
		if (!m_bIsShutDown)
			{
				// remove the sharp turn limit since there is no more planned move anymore
				SetSharpTurnSpeedLimit(EMM_FAST);

				if (m_bFallDueToDamageDeferred && !GetAlienBrain()->IsMelee()) // we still have a deferred fall 
				{
					SwitchToState(FALLING);
					return;
				}

				// Movement not suppose to finish while we still in special movement.
				Asura_Assert(!GetAlienBrain()->IsJumping(), "Movement finished while still in jump");
				if (GetAlienBrain()->IsJumping())
				{
					GetAlienBrain()->SetJumping(false);
				}

				Asura_Assert(!GetAlienBrain()->IsGS2(), "Movement finished while still in GS2");
				if (GetAlienBrain()->IsGS2())
				{
					GetAlienBrain()->SetGameScene(false);
				}

				Asura_Assert(!GetAlienBrain()->IsSharpTurn(), "Movement finished while still in Sharp turn");
				if (GetAlienBrain()->IsSharpTurn())
				{
					GetAlienBrain()->SetSharpTurn(false);
				}

				if (m_bInError && m_bMovementInProgress)
				{
					if (!ConsiderateAdvanceAlongRoute())
					{
						++m_fTraversalFailureScore;
					}
					else
					{
						m_fTraversalFailureScore = 0.0f;
					}
				}



				// 3 times with out any meaningful advancement on a planned route - we really are stuck.
				if ( m_fTraversalFailureScore > 3.0f )
				{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
					Asura_OutputToDebugger(("%d Alien %d Terminal Failure\n", 
						Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetActor()->GetGuid()));
#endif
					Asura_Assert(false, "Terminal Failure");
					m_fTraversalFailureScore = 0.0f;
				}
			}

			SetFinished(true);

			break;
        }

		case FALLING:
		{
			if (!m_bFallDueToDamageDeferred) // if not deffered falling then set falling flag now
			{
				GetAlienBrain()->SetFalling(true);
			}
			m_bFallDueToDamageDeferred = false;
			RequestFall();
			break;
		}

		case JUMPING_FROM_DANGER:
		{
			GetAlienBrain()->SetJumping(true);			
			break;
		}

		case RESOLVE_STUCK_BY_JUMPING:
		{
			GetAlienBrain()->SetJumping(true);	
			break;
		}

		case MOVING:
		{
			m_uPerRouteFlags = 0;
			RequestMovementAnimState();
			break;
		}

        case TRAVERSING_ROUTE:
		{
			Asura_Assert( GetAlienBrain()->GetNavData().m_usRouteLength	> 0, "" );
			LinkTraverser* pxNextTraverser = FindNextTraverser();
			Asura_Assert( pxNextTraverser != NULL, "AVP_Alien_Movement::SwitchToState - could not find a suitable traverser" );
			if ( pxNextTraverser )
			{
				Asura_Assert( m_pxCurrentTraverser == NULL, "AVP_Alien_Movement::SwitchToState - shouldn't already have a traverser" );
				m_pxCurrentTraverser = pxNextTraverser;
				m_pxCurrentTraverser->Start();
			}
			break;
		}

		default:
        case NUM_STATES:
        {
            break;
        }
    }
}

DEBUGVAR float dbg_fTimeToStayLowMobilityAfterFall = 3.0f;
DEBUGVAR float dbg_fDistanceToStayLowMobilityAfterFall = 10.0f;
DEBUGVAR float dbg_fTimeToNotJumpAfterJump = 1.0f;
DEBUGVAR float dbg_fDistanceToPreferNotTojump = 10.0f;


bool AVP_Alien_Movement::ActionGetToPosition( const Asura_Vector_3& xDestination, u_short usDestinationWPID, float fArrivalRange, AVP_Navigation::AlienAStarFunctor& xFunctor, AVP_Alien_Movement::ExMovementMode eExMoveMode, const bool bRefineRoute )
{
//	const bool bWasFinishedOrInError = ( m_eState == FINISHED ) || IsInError();
	if (!IsInError())
	{
		m_fTraversalFailureScore =0;
	}
	SetInError( false );
	SetFinished(false);
	m_bMovementInProgress = false;
	m_fArrivalRange = Asura_Maths::ClampToRange(fArrivalRange, dbg_fMinArrivalRange, dbg_fMaxArrivalRange);
	
	AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();	

	// Do some preliminary checking to see if we really need to update the route.
// 	if ( xNavData.m_pusRoute &&
// 		xNavData.m_usDestinationWPID == usDestinationWPID &&
// 		xNavData.m_xDestinationPosition.ApproximatelyEquals( xDestination, m_fArrivalRange ) &&
// 		!bWasFinishedOrInError )
// 	{
// 		m_bMovementInProgress = true;
// 		SetMovementMode(eExMoveMode);
// 		// Our old route is fine.
// 		Asura_Assert( m_eState == TRAVERSING_ROUTE, "AVP_Alien_Movement::ActionGetToPosition - assumption about moving is incorrect" );
// 	}
// 	else
	{

		// Update with a new route.
		xNavData.m_xStartPosition       = GetAlienBrain()->GetActor()->GetPosition();
		xNavData.m_usDestinationWPID	= usDestinationWPID;
		xNavData.m_xDestinationPosition = xDestination;

		if ( xNavData.m_usCurrentWPID == usDestinationWPID &&
			( xNavData.m_xStartPosition - xNavData.m_xDestinationPosition ).MagnitudeSquared() < m_fArrivalRange * m_fArrivalRange )
		{
			// If we're already in our final waypoint and near enough the destination then there's nothing to do.
			SwitchToState( FINISHED );
			return true;
		}
		else
		{
			// Clear out the old route and find the new one.
			xNavData.ResetRoute();
			if (m_fTimeSinceLastFall < dbg_fTimeToStayLowMobilityAfterFall)
			{
				float fDistanceToStayLowMobile = (dbg_fTimeToStayLowMobilityAfterFall-m_fTimeSinceLastFall) * dbg_fDistanceToStayLowMobilityAfterFall / dbg_fTimeToStayLowMobilityAfterFall;
				xFunctor.SetLowMobilityForShortDistance(xNavData.m_xStartPosition, fDistanceToStayLowMobile);														 
			}
			else if (m_fTimeSinceLastJump < dbg_fTimeToNotJumpAfterJump)
			{
				float fDistanceToStayLowMobile = (dbg_fTimeToNotJumpAfterJump-m_fTimeSinceLastJump) * dbg_fDistanceToPreferNotTojump / dbg_fTimeToNotJumpAfterJump;
				xFunctor.SetAvoidJumping(xNavData.m_xStartPosition, fDistanceToStayLowMobile);														 
			}
			else
			{
				xFunctor.SetNoLimit();
			}

			if ( !AVP_Navigation::FindRoute( xNavData, &xFunctor, bRefineRoute ) )
			{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
				Asura_Vector_3 xPosition = GetAlienBrain()->GetAlienActor()->GetPosition();
				Asura_OutputToDebugger(("%d Alien %d GetToPosition failed to get from  %.2f, %.2f, %.2f (%d) to %.2f, %.2f, %.2f (%d)\n",
					Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetOwnerGuid(), 
					xPosition.x, xPosition.y, xPosition.z, xNavData.m_usCurrentWPID, 
					xDestination.x, xDestination.y, xDestination.z, xNavData.m_usDestinationWPID ));
#endif
				SetInError();
				SwitchToState( FINISHED );
				return false;
			}

			ClearWPDistanceCache();
			if (xNavData.m_usRouteLength > 0)
			{
				m_fRouteTotalDistance = FindDistanceOfWPFromStart(xNavData.m_usRouteLength-1);
			}
			else
			{
				m_fRouteTotalDistance = 0.0f;
			}

			AVP_Navigation::CalculateRouteAgility( xNavData );
			CalculateRouteTransitions();
			UpdatePositionOnRoute();

			m_xStandardLinkTraverser.OnNewRoute();
			m_xSlowLinkTraverser.OnNewRoute();
			m_xJumpLinkTraverser.OnNewRoute();
			m_xGS2LinkTraverser.OnNewRoute();
			m_xEnvHookLinkTraverser.OnNewRoute();
			m_xDestinationTraverser.OnNewRoute();
			m_uPerRouteFlags = 0;
		}
		m_bMovementInProgress = true;
		SetMovementMode(eExMoveMode);
		
		SwitchToState( TRAVERSING_ROUTE );
	}

	return true;
}

bool AVP_Alien_Movement::ActionGetAwayFromPosition( AVP_Navigation::AlienAwayFromFunctor& xFunctor, AVP_Alien_Movement::ExMovementMode eExMoveMode )
{
	if (!IsInError())
	{
		m_fTraversalFailureScore =0;
	}

	SetInError( false );
	SetFinished(false);
	m_fArrivalRange = dbg_fMinArrivalRange;
	m_bMovementInProgress = false;

	AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	xNavData.m_xStartPosition = GetAlienBrain()->GetActor()->GetPosition();

	// Clear out the old route and find the new one.
	xNavData.ResetRoute();
	if (m_fTimeSinceLastFall < dbg_fTimeToStayLowMobilityAfterFall)
	{
		float fDistanceToStayLowMobile = (dbg_fTimeToStayLowMobilityAfterFall-m_fTimeSinceLastFall) * dbg_fDistanceToStayLowMobilityAfterFall / dbg_fTimeToStayLowMobilityAfterFall;
		xFunctor.SetLowMobilityForShortDistance(xNavData.m_xStartPosition, fDistanceToStayLowMobile);														 
	}
	else if (m_fTimeSinceLastJump < dbg_fTimeToNotJumpAfterJump)
	{
		float fDistanceToStayLowMobile = (dbg_fTimeToNotJumpAfterJump-m_fTimeSinceLastJump) * dbg_fDistanceToPreferNotTojump / dbg_fTimeToNotJumpAfterJump;
		xFunctor.SetAvoidJumping(xNavData.m_xStartPosition, fDistanceToStayLowMobile);														 
	}
	else
	{
		xFunctor.SetNoLimit();
	}
	
	if ( !AVP_Navigation::FindRouteAwayFrom( xNavData, &xFunctor ) )
	{		
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
		Asura_Vector_3 xPosition = xFunctor.GetStartPosition();
		Asura_OutputToDebugger(("%d Alien %d GetAwayFromPosition failed to get away from %.2f, %.2f, %.2f for %f \n",
			Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetOwnerGuid(), 			
			xPosition.x, xPosition.y, xPosition.z, xFunctor.GetDistance() ));
#endif
		SetInError();
		SwitchToState( FINISHED );
		return false;
	}

	ClearWPDistanceCache();
	if (xNavData.m_usRouteLength > 0)
	{
		m_fRouteTotalDistance = FindDistanceOfWPFromStart(xNavData.m_usRouteLength-1);
	}
	else
	{
		m_fRouteTotalDistance = 0.0f;
	}

	AVP_Navigation::CalculateRouteAgility( xNavData );
	CalculateRouteTransitions();
	m_xStandardLinkTraverser.OnNewRoute();
	m_xSlowLinkTraverser.OnNewRoute();
	m_xJumpLinkTraverser.OnNewRoute();
	m_xGS2LinkTraverser.OnNewRoute();
	m_xEnvHookLinkTraverser.OnNewRoute();
	m_xDestinationTraverser.OnNewRoute();
	m_uPerRouteFlags = 0;

	m_bMovementInProgress = true;
	SetMovementMode(eExMoveMode);
	SwitchToState( TRAVERSING_ROUTE );
	return true;
}

bool AVP_Alien_Movement::ActionFollowSharedRoute( const Asura_Guid uRouteOwner, AVP_Navigation::AlienAStarFunctor& xFunctor, AVP_Alien_Movement::ExMovementMode eExMoveMode )
{
	if (!IsInError())
	{
		m_fTraversalFailureScore =0;
	}
	SetInError( false );
	SetFinished(false);
	m_fArrivalRange = dbg_fMinArrivalRange;
	m_bMovementInProgress = false;

	const AVP_ServerActor_Alien* pxAlienOwner = AVP_ServerActor_Alien::FindEntity( uRouteOwner );
	if ( !pxAlienOwner || !pxAlienOwner->GetActorBrain() )
	{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
		Asura_OutputToDebugger(("%d Alien %d FollowSharedRoute failed to get route owner %d \n",
			Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetOwnerGuid(), uRouteOwner ));
#endif
		SetInError( true );
		SwitchToState( FINISHED );
		return false;
	}

	const AVP_Navigation::NavData& xRouteOwnerNavData = pxAlienOwner->GetActorBrain()->GetNavData();
	AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	if (m_fTimeSinceLastFall < dbg_fTimeToStayLowMobilityAfterFall)
	{
		float fDistanceToStayLowMobile = (dbg_fTimeToStayLowMobilityAfterFall-m_fTimeSinceLastFall) * dbg_fDistanceToStayLowMobilityAfterFall / dbg_fTimeToStayLowMobilityAfterFall;
		xFunctor.SetLowMobilityForShortDistance(xNavData.m_xStartPosition, fDistanceToStayLowMobile);														 
	}
	else if (m_fTimeSinceLastJump < dbg_fTimeToNotJumpAfterJump)
	{
		float fDistanceToStayLowMobile = (dbg_fTimeToNotJumpAfterJump-m_fTimeSinceLastJump) * dbg_fDistanceToPreferNotTojump / dbg_fTimeToNotJumpAfterJump;
		xFunctor.SetAvoidJumping(xNavData.m_xStartPosition, fDistanceToStayLowMobile);														 
	}
	else
	{
		xFunctor.SetNoLimit();
	}
	xNavData.ResetRoute();
	if ( !AVP_Navigation::AdoptRoute( xNavData, xRouteOwnerNavData, GetAlienBrain()->GetActor()->GetPosition(), &xFunctor ) )
	{
#if !defined(ASURA_NO_DEBUG_OUTPUT) && defined(_DEBUG)
		Asura_Vector_3 xPosition = GetAlienBrain()->GetAlienActor()->GetPosition();
		Asura_OutputToDebugger(("%d Alien %d FollowSharedRoute failed to adopt route owner %d from %.2f,%.2f,%.2f (d) to %.2f,%.2f,%.2f (d) \n",
			Asura_Timers::GetFrameNumber(), GetAlienBrain()->GetOwnerGuid(), uRouteOwner, 
			xPosition.x, xPosition.y, xPosition.z, xNavData.m_usCurrentWPID,
			xRouteOwnerNavData.m_xDestinationPosition.x, xRouteOwnerNavData.m_xDestinationPosition.y, xRouteOwnerNavData.m_xDestinationPosition.z, xRouteOwnerNavData.m_usDestinationWPID ));
#endif

		SetInError( true );
		SwitchToState( FINISHED );
		return false;
	}

	ClearWPDistanceCache();
	if (xNavData.m_usRouteLength > 0)
	{
		m_fRouteTotalDistance = FindDistanceOfWPFromStart(xNavData.m_usRouteLength-1);
	}
	else
	{
		m_fRouteTotalDistance = 0.0f;
	}

	AVP_Navigation::CalculateRouteAgility( xNavData );
	CalculateRouteTransitions();
	m_xStandardLinkTraverser.OnNewRoute();
	m_xSlowLinkTraverser.OnNewRoute();
	m_xJumpLinkTraverser.OnNewRoute();
	m_xGS2LinkTraverser.OnNewRoute();
	m_xEnvHookLinkTraverser.OnNewRoute();
	m_xDestinationTraverser.OnNewRoute();
	m_uPerRouteFlags = 0;
	m_bMovementInProgress = true;
	SetMovementMode(eExMoveMode);
	SwitchToState( TRAVERSING_ROUTE );
	return true;
}

void AVP_Alien_Movement::ActionFall()
{
	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( GetAlienBrain()->GetNavData().m_usCurrentWPID );

	if (pxCurrentWP &&((pxCurrentWP->IsCeilingWaypoint() || pxCurrentWP->IsWallWaypoint())) && !m_bFallingDisallowed)
	{
		SwitchToState(FALLING);
	}
}

void AVP_Alien_Movement::RequestMovementAnimState()
{
	AVP_Alien_AIAnimStateMachine* pxVertStateHandler = static_cast< AVP_Alien_AIAnimStateMachine* >( GetAlienBrain()->GetActor()->GetAIAnimStateMachine() );
	if(pxVertStateHandler)
	{
		
		switch ( GetCurrentMovementMode() )
		{
		case EMM_IDLE:
			if (m_bAvoidLight)
			{
				pxVertStateHandler->RequestHideIdle();
			}
			else
			{ 
				pxVertStateHandler->RequestIdle();
			}
			GetAlienBrain()->SetMaxMoveDirAngularVelocity( 1.5f * Asura_Maths::Pi );
			GetAlienBrain()->SetMaxAimDirAngularVelocity( 1.5f * Asura_Maths::Pi );
			break;

		case EMM_SLOW:
			pxVertStateHandler->RequestMoveSlow();
			GetAlienBrain()->SetMaxMoveDirAngularVelocity( 1.8f * Asura_Maths::Pi );
			GetAlienBrain()->SetMaxAimDirAngularVelocity( 1.8f * Asura_Maths::Pi );
			break;


		case EMM_MEDIUM:
			pxVertStateHandler->RequestMoveMedium();
			GetAlienBrain()->SetMaxMoveDirAngularVelocity( 1.0f * Asura_Maths::Pi );
			GetAlienBrain()->SetMaxAimDirAngularVelocity( 1.0f * Asura_Maths::Pi );
			break;

		case EMM_FAST:
			pxVertStateHandler->RequestMoveFast();
			GetAlienBrain()->SetMaxMoveDirAngularVelocity( 1.0f * Asura_Maths::Pi );
			GetAlienBrain()->SetMaxAimDirAngularVelocity( 1.0f * Asura_Maths::Pi );
			break;
		}
	}
}


AVP_Alien_Movement::ExMovementMode AVP_Alien_Movement::GetCurrentMovementMode() const
{
	return m_eActualMoveMode;
}

void AVP_Alien_Movement::RequestMove( const Asura_Hash_ID uMoveHashID )
{
	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Msg_Move ) );
	if ( pxMessage != NULL )
	{
		AVP_Msg_Move* pxData = static_cast< AVP_Msg_Move* >( pxMessage->GetData() );

		pxMessage->SetFromGuid( GetAlienBrain()->GetOwnerGuid() );
		pxMessage->SetToGuid( GetAlienBrain()->GetOwnerGuid() );

		pxData->ClearFlags();
//		pxData->SetForceAbort( true );
		pxData->SetIgnoredByOwner( false );
		pxData->SetMoveID( uMoveHashID );

		AVP_Actor_AIAnimStateMachine* pxVertStateHandler = GetAlienBrain()->GetActor()->GetAIAnimStateMachine();
		pxVertStateHandler->RequestInMove( pxMessage, GetAnimStateFromMovementMode( GetCurrentMovementMode() ) );
	}
	else
	{
		// Do it without the move if something goes wrong.
		RequestMovementAnimState();
	}
}

void AVP_Alien_Movement::RequestJumpMove( Asura_Hash_ID uMoveHashID, const Asura_Vector_3& xDestPos, const Asura_Vector_3& xDestNormal )
{
	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Alien_Move_JumpSurfaceStart ) );
	if ( pxMessage != NULL )
	{
		AVP_Message_Alien_Move_JumpSurfaceStart* pxData = static_cast< AVP_Message_Alien_Move_JumpSurfaceStart* >( pxMessage->GetData() );

		pxMessage->SetFromGuid( GetAlienBrain()->GetOwnerGuid() );
		pxMessage->SetToGuid( GetAlienBrain()->GetOwnerGuid() );

		pxData->ClearFlags();
//		pxData->SetForceAbort( true );
		pxData->SetIgnoredByOwner( false );
		pxData->SetMoveID( uMoveHashID );

		pxData->SetDestinationNormal( xDestNormal );
		pxData->SetDestinationPosition( xDestPos );

		AVP_Actor_AIAnimStateMachine* pxVertStateHandler = GetAlienBrain()->GetActor()->GetAIAnimStateMachine();
		pxVertStateHandler->RequestInMove( pxMessage, GetAnimStateFromMovementMode( GetCurrentMovementMode() ) );
	}
	else
	{
		// Do it without the move if something goes wrong.
		RequestMovementAnimState();
	}
}


void AVP_Alien_Movement::RequestJumpMove( Asura_Hash_ID uMoveHashID, Asura_Hash_ID uJumpHashID, Asura_Hash_ID uJumpAnimHashID, const Asura_Vector_3& xDestPos, const Asura_Matrix_3x3& xDestOrientation, AVP_Alien_Movement::ExMovementMode eExMoveMode )
{
	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Alien_AI_Move_JumpSurfaceStart ) );
	if ( pxMessage != NULL )
	{
		AVP_Message_Alien_AI_Move_JumpSurfaceStart* pxData = static_cast< AVP_Message_Alien_AI_Move_JumpSurfaceStart* >( pxMessage->GetData() );

		pxMessage->SetFromGuid( GetAlienBrain()->GetOwnerGuid() );
		pxMessage->SetToGuid( GetAlienBrain()->GetOwnerGuid() );

		pxData->ClearFlags();
		pxData->SetIgnoredByOwner( false );
		pxData->SetMoveID( uMoveHashID );

		pxData->SetDestinationOrientation(xDestOrientation);
		pxData->SetDestinationPosition( xDestPos );
		pxData->SetJumpHash(uJumpHashID);
		pxData->SetJumpAnimHash(uJumpAnimHashID);

		AVP_Actor_AIAnimStateMachine* pxVertStateHandler = GetAlienBrain()->GetActor()->GetAIAnimStateMachine();
		pxVertStateHandler->RequestInMove( pxMessage, GetAnimStateFromMovementMode( eExMoveMode ) );
	}
	else
	{
		// Do it without the move if something goes wrong.
		RequestMovementAnimState();
	}
}

void AVP_Alien_Movement::RequestMoveAndJumpMove( const Asura_Hash_ID uMoveHashID, Asura_Hash_ID uJumpHashID, Asura_Hash_ID uJumpAnimHashID, const Asura_Matrix_3x3& xTurnOrientation, const Asura_Vector_3& xJumpStartPos, const Asura_Vector_3& xDestPos, const Asura_Matrix_3x3& xDestOrientation, AVP_Alien_Movement::ExMovementMode eExMoveMode )
{
	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Alien_AI_Move_TurnAndJumpSurfaceStart ) );
	if ( pxMessage != NULL )
	{
		AVP_Message_Alien_AI_Move_TurnAndJumpSurfaceStart* pxData = static_cast< AVP_Message_Alien_AI_Move_TurnAndJumpSurfaceStart* >( pxMessage->GetData() );

		pxMessage->SetFromGuid( GetAlienBrain()->GetOwnerGuid() );
		pxMessage->SetToGuid( GetAlienBrain()->GetOwnerGuid() );

		pxData->ClearFlags();
		pxData->SetIgnoredByOwner( false );
		pxData->SetMoveID( uMoveHashID );

		pxData->SetTurnOrientation(xTurnOrientation);
		pxData->SetDestinationOrientation(xDestOrientation);
		pxData->SetDestinationPosition( xDestPos );
		pxData->SetStartJumpPosition( xJumpStartPos); 
		pxData->SetJumpHash(uJumpHashID);
		pxData->SetJumpAnimHash(uJumpAnimHashID);

		AVP_Actor_AIAnimStateMachine* pxVertStateHandler = GetAlienBrain()->GetActor()->GetAIAnimStateMachine();
		pxVertStateHandler->RequestInMove( pxMessage, GetAnimStateFromMovementMode( eExMoveMode ) );
	}
	else
	{
		// Do it without the move if something goes wrong.
		RequestMovementAnimState();
	}
}


void AVP_Alien_Movement::RequestTurn( const Asura_Hash_ID uMoveHashID, const Asura_Matrix_3x3& xTurnOrientation )
{
	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Alien_AI_Move_TurnStart ) );
	if ( pxMessage != NULL )
	{
		AVP_Message_Alien_AI_Move_TurnStart* pxData = static_cast< AVP_Message_Alien_AI_Move_TurnStart* >( pxMessage->GetData() );

		pxMessage->SetFromGuid( GetAlienBrain()->GetOwnerGuid() );
		pxMessage->SetToGuid( GetAlienBrain()->GetOwnerGuid() );

		pxData->ClearFlags();
		pxData->SetIgnoredByOwner( false );
		pxData->SetMoveID( uMoveHashID );

		pxData->SetTurnOrientation(xTurnOrientation);

		AVP_Actor_AIAnimStateMachine* pxVertStateHandler = GetAlienBrain()->GetActor()->GetAIAnimStateMachine();
		pxVertStateHandler->RequestInMove( pxMessage, GetAnimStateFromMovementMode( GetCurrentMovementMode() ) );
	}
	else
	{
		// Do it without the move if something goes wrong.
		RequestMovementAnimState();
	}
}


DEBUGVAR float dbg_fSmallRotationToResolveAmbiguity = Asura_Maths::DegToRad(5.0f);
DEBUGVAR float dbg_fTurn50 = Asura_Maths::DegToRad(50.0f);
DEBUGVAR float dbg_fTurn50Cosine = Asura_Maths::Cosine(dbg_fTurn50);
DEBUGVAR float dbg_fTurn60Cosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(60.0f));
DEBUGVAR float dbg_fTurn135Cosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(135.0f));
DEBUGVAR float dbg_fTurn45Cosine =  Asura_Maths::Cosine(Asura_Maths::DegToRad(45.0f));
DEBUGVAR float dbg_fTurn30Cosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(30.0f));
DEBUGVAR float dbg_fStartCloseToEndCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(30.0f));
DEBUGVAR float dbg_fMinDestToCrawlSq = 0.5f * 0.5f;
DEBUGVAR float dbg_fMinDistSqToForceIdle = 2.0f * 2.0f;
DEBUGVAR float dbg_fMaxAngleToDoFlatMoveCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(15.0f));
DEBUGVAR float dbg_fMinDistSqBetweenWP = 0.5f * 0.5f;

static const Asura_Hash_ID g_uAIMovesBP		= 0xd81414b8;   // Asura_GetHashID( "Moves3rdPerson" )
static const Asura_Hash_ID g_uFlatJumpAnimParam = 0x60f38f27; //"FlatJump"
static const Asura_Hash_ID g_uFlatUpJumpAnimParam = 0xf24c7002; //"FlatJumpUp"
static const Asura_Hash_ID g_uFlatDownJumpAnimParam = 0x90e8c849; //"FlatJumpDown"


void AVP_Alien_Movement::CalculateJump( bool& bNeedToMoveOrTurn,
									   Asura_Hash_ID &uMoveHashID, 
									   Asura_Hash_ID &uJumpHashID, 
									   Asura_Hash_ID &uJumpAnimHashID,
									   Asura_Matrix_3x3& xTurnOrientation, 
									   Asura_Vector_3& xStartJumpPos, 
									   Asura_Vector_3& xDestPos, Asura_Matrix_3x3& xDestOrientation, 
									   AVP_Alien_Movement::ExMovementMode& eExMoveMode, 
									   u_short usRouteIndex, 
									   u_short usNextRouteIndex, 
									   bool bJumpOver)
{

	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();

	Asura_Assert(usRouteIndex < xNavData.m_usRouteLength, "Jumping without a path");
	if (usRouteIndex >= xNavData.m_usRouteLength)
	{
		RequestMovementAnimState();
		return;
	}

	uJumpAnimHashID = ASURA_HASH_ID_UNSET; // not all moves require external animation id 
	const Asura_AI_NavVertex* pxStartWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usRouteIndex ] );
	const Asura_AI_NavVertex* pxDestWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usRouteIndex + 1 ] );
	Asura_Assert(pxDestWP, "Destination waypoint not found");

	Asura_Matrix_3x3 xCurrentOrientation = GetAlienBrain()->GetActor()->GetOrientation();
	Asura_Vector_3 xCurrentPosition = GetAlienBrain()->GetActor()->GetPosition();

	Asura_Vector_3 xStartNormal(-xCurrentOrientation.e12, -xCurrentOrientation.e22, -xCurrentOrientation.e32);
	Asura_Vector_3 xStartForward(xCurrentOrientation.e13, xCurrentOrientation.e23, xCurrentOrientation.e33);
	Asura_Vector_3 xStartRight(xCurrentOrientation.e11, xCurrentOrientation.e21, xCurrentOrientation.e31);

	const Asura_Vector_3& xDestNormal = pxDestWP->GetNormal();	

	bool bForceIdlePostJump = false;
	Asura_Vector_3 xDesiredDestForward(Asura_Maths::ZeroVector3);
	// first calculate destination position
	if (usRouteIndex == xNavData.m_usRouteLength - 2)
	{
		eExMoveMode = m_eRequestedMoveMode; // no known limitations
		xDestPos = pxDestWP->GetPosition();		
	}
	else
	{
		const Asura_AI_NavVertex* pxPostDestWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usRouteIndex + 2 ] );
		bool bJumpToWp = false;
		bool bNoDesiredForward = false;
		if (usNextRouteIndex < xNavData.m_usRouteLength)
		{
			const Asura_AI_NavVertex* pxNextJumpStartWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usNextRouteIndex ] );			
			if ( (pxNextJumpStartWP->GetPosition() - pxDestWP->GetPosition()).MagnitudeSquared() < dbg_fMinDistSqToForceIdle)
			{
				bForceIdlePostJump = true;
				bJumpToWp = true;
				bNoDesiredForward = true;
			}
		}
		else if (pxPostDestWP->GetNormal() * xDestNormal < dbg_fLargeReorientCosine) // big turn so jump do waypoint 
		{
			bJumpToWp = true;
		}

		if ((pxPostDestWP->GetPosition() - pxDestWP->GetPosition()).MagnitudeSquared() < dbg_fMinDistSqBetweenWP) // very close WP jump to dest WP and no desired direction
		{
			bJumpToWp = true;
			bNoDesiredForward = true;

		}

		if (!bNoDesiredForward && !m_bJumpOnWaypoint)
		{
			AVP_Plane xDestPlane(pxDestWP->GetPosition(), pxDestWP->GetNormal() );
			Asura_Vector_3 xPostDestPositionInDestPlane;;
			xDestPlane.CalculateClosestPoint(pxPostDestWP->GetPosition(), xPostDestPositionInDestPlane);
			xDesiredDestForward = xPostDestPositionInDestPlane - pxDestWP->GetPosition();
			const float fPostDestDistance = xDesiredDestForward.Normalise();
			if (bJumpToWp)
			{
				xDestPos = pxDestWP->GetPosition();
			}
			else
			{
				// xPostDestPositionInDestPlane = pxPostDestWP->GetPosition();
				float fDistance = Asura_Maths::ClampToRange( fPostDestDistance, 0.0f, 1.5f);
				xDestPos = pxDestWP->GetPosition() + xDesiredDestForward * Asura_Random::GetFloat(0.25f, 1.0f) * fDistance; // pick a position between the dest wp and the post dest wp (in the plane)
			}
		}
		else // no desired forward force jumping to wp
		{
			xDestPos = pxDestWP->GetPosition();
		}

		// post move mode
		if (bForceIdlePostJump)
		{
			eExMoveMode = AVP_Alien_Movement::EMM_IDLE;
		}
		else if (pxDestWP->GetNormal() * pxPostDestWP->GetNormal() < dbg_fLargeReorientCosine ) // large reorient soon
		{
			eExMoveMode = AVP_Alien_Movement::EMM_SLOW;
		}
		else
		{
			eExMoveMode = m_eRequestedMoveMode; 
		}
	}

	// now we have the exact destination and the desired forward (if any)
	Asura_Vector_3 xStartWPNormal = pxStartWP->GetNormal();
	AVP_Plane xCurrentWPPlane(pxStartWP->GetPosition(),  xStartWPNormal);
	Asura_Vector_3 xDestInCurrentWPPlane;
	Asura_Vector_3 xStartForwardInCurrentWPPlane;
	Asura_Vector_3 xStartRightInCurrentWPPlane;
	xCurrentWPPlane.CalculateClosestPoint(pxDestWP->GetPosition(), xDestInCurrentWPPlane);
	xCurrentWPPlane.CalculateClosestPoint(pxStartWP->GetPosition() + xStartForward, xStartForwardInCurrentWPPlane);
	xCurrentWPPlane.CalculateClosestPoint(pxStartWP->GetPosition() + xStartRight, xStartRightInCurrentWPPlane);

	Asura_Vector_3 xDirToDestInCurrentPlane = xDestInCurrentWPPlane - pxStartWP->GetPosition();
	xStartForwardInCurrentWPPlane -= pxStartWP->GetPosition();
	xStartRightInCurrentWPPlane -= pxStartWP->GetPosition();
	xStartForwardInCurrentWPPlane.Normalise();
	xStartRightInCurrentWPPlane.Normalise();
	float fDistanceToDestInPlane = xDirToDestInCurrentPlane.Normalise();
	float fDotForwardToJumpDirection = xDirToDestInCurrentPlane * xStartForwardInCurrentWPPlane; 
	bool bTurnRight = xDirToDestInCurrentPlane * xStartRightInCurrentWPPlane > 0; 
	bool bNeedToMove = false;
	bool bNeedToTurn = false;

	// if the jump is forward or to the sides turn requirement is determined by the destination desired forward direction
	if (fDotForwardToJumpDirection > dbg_fTurn60Cosine)
	{

		if (xDesiredDestForward.ApproximatelyEquals(Asura_Maths::ZeroVector3) ||
			xStartForward * xDirToDestInCurrentPlane > dbg_fStartCloseToEndCosine)
		{
			bNeedToTurn = false;
			xTurnOrientation = xCurrentOrientation;
		}
		else // no shortcuts - need to do a turn to the direction of jump
		{
			bNeedToTurn = true;
		}		
	}
	else
	{
		bNeedToTurn = true;
	}

	if (fDotForwardToJumpDirection > dbg_fTurn50Cosine) // no need to turn for such a small angle
	{
		bNeedToMoveOrTurn = false;
	}


	// Another factor is if our orientation is far from the start WP orientation.
	float fDistanceToStartSq = (pxStartWP->GetPosition()- xCurrentPosition).MagnitudeSquared();
	if (xStartNormal * xStartWPNormal < dbg_fTurn30Cosine || fDistanceToStartSq > dbg_fMinDestToCrawlSq)
	{
		bNeedToMove = true;
		xStartJumpPos = pxStartWP->GetPosition();
	}
	else
	{
		xStartJumpPos = xCurrentPosition;
	}

	bNeedToMoveOrTurn = bNeedToMove || bNeedToTurn;

	{
		const Asura_Vector_3 xTurnRight = xDirToDestInCurrentPlane ^ xStartWPNormal;

		xTurnOrientation.e11 = xTurnRight.x;
		xTurnOrientation.e21 = xTurnRight.y;
		xTurnOrientation.e31 = xTurnRight.z;
		xTurnOrientation.e12 = -xStartWPNormal.x;
		xTurnOrientation.e22 = -xStartWPNormal.y;
		xTurnOrientation.e32 = -xStartWPNormal.z;
		xTurnOrientation.e13 = xDirToDestInCurrentPlane.x;
		xTurnOrientation.e23 = xDirToDestInCurrentPlane.y;
		xTurnOrientation.e33 = xDirToDestInCurrentPlane.z;

	}

	if (bNeedToMoveOrTurn)
	{
		if (bNeedToMove)
		{
			if (bNeedToTurn)
			{
				if (bTurnRight)
				{
					if (fDotForwardToJumpDirection > dbg_fTurn135Cosine)
					{
						uMoveHashID = AVP_Move_Alien_AI_Turn90RightAndJumpSurface::GetStaticHash();
					}
					else
					{
						uMoveHashID = AVP_Move_Alien_AI_Turn180RightAndJumpSurface::GetStaticHash();
					}
				}
				else
				{
					if (fDotForwardToJumpDirection > dbg_fTurn135Cosine)
					{
						uMoveHashID = AVP_Move_Alien_AI_Turn90LeftAndJumpSurface::GetStaticHash();
					}
					else
					{
						uMoveHashID = AVP_Move_Alien_AI_Turn180LeftAndJumpSurface::GetStaticHash();
					}
				}
			}
			else
			{
				uMoveHashID = AVP_Move_Alien_AI_MoveForwardAndJumpSurface::GetStaticHash();
			}
		}
		else // turn on spot
		{
			if (bTurnRight)
			{
				if (fDotForwardToJumpDirection > dbg_fTurn135Cosine)
				{
					uMoveHashID = AVP_Move_Alien_AI_Turn90OnSpotRightAndJumpSurface::GetStaticHash();
				}
				else
				{
					uMoveHashID = AVP_Move_Alien_AI_Turn180OnSpotRightAndJumpSurface::GetStaticHash();
				}
			}
			else
			{
				if (fDotForwardToJumpDirection > dbg_fTurn135Cosine)
				{
					uMoveHashID = AVP_Move_Alien_AI_Turn90OnSpotLeftAndJumpSurface::GetStaticHash();
				}
				else
				{
					uMoveHashID = AVP_Move_Alien_AI_Turn180OnSpotLeftAndJumpSurface::GetStaticHash();
				}
			}

		}		
	}

	// now that the start jump oreintaion is known calculate the destination orientation using the start orienation and the desired forward
	{
		// first calculate the orientation based on start jump orientation (which is the turn orientation)
		Asura_Vector_3 xStartJumpNormal(xStartWPNormal);

		Asura_Quat xTurnQuad;

		// need to avoid 180 deg turn as in this case the turn is arbitrary 
		if (xStartNormal * xDestNormal < -0.99)
		{
			xTurnQuad.FromRotation(Asura_Random::GetInt(0,1) == 0 ? dbg_fSmallRotationToResolveAmbiguity : -dbg_fSmallRotationToResolveAmbiguity , xStartForward);
			xStartJumpNormal = xTurnQuad.RotateVector(xStartNormal);
		}

		xTurnQuad.RotationFromAToB(xStartJumpNormal, xDestNormal);
		Asura_Matrix_3x3 xRotation;
		xTurnQuad.ToMatrix(xRotation);
		xDestOrientation = xRotation * xTurnOrientation;

		// now rotate around the Y axis 
		Asura_Vector_3 xNominalDestForward(xDestOrientation.e13, xDestOrientation.e23, xDestOrientation.e33);
		if (xNominalDestForward * xDesiredDestForward > dbg_fTurn50Cosine) // small enough turn
		{
			xTurnQuad.RotationFromAToB(xNominalDestForward, xDesiredDestForward);
			xTurnQuad.ToMatrix(xRotation);
			xDestOrientation = xRotation * xDestOrientation;
		}
		else  // do the biggest allowed turn
		{
			if ((xNominalDestForward ^ xDesiredDestForward) * xDestNormal > 0.0f)
			{
				xTurnQuad.FromRotation(dbg_fTurn50, xDestNormal);
			}
			else
			{
				xTurnQuad.FromRotation(-dbg_fTurn50, xDestNormal);
			}
			xTurnQuad.ToMatrix(xRotation);
			xDestOrientation = xRotation * xDestOrientation;
		}
	}

	// now calculate the actual jump animations
	bool bFlatJump = xStartNormal * xDestNormal > dbg_fTurn45Cosine;
	Asura_Vector_3 xTurnForward(xTurnOrientation.e13, xTurnOrientation.e23, xTurnOrientation.e33);
	Asura_Vector_3 xDestForward(xDestOrientation.e13, xDestOrientation.e23, xDestOrientation.e33);
	if (bFlatJump)
	{
		bool bJumpUp = false;
		bool bJumpDown = false;
		float fDistanceToDest = (pxDestWP->GetPosition() - xCurrentPosition).Magnitude(); 
		// if the angle of the jump is big enough then use up/down jump
		if (fDistanceToDest > 0.01 && fDistanceToDestInPlane / fDistanceToDest < dbg_fMaxAngleToDoFlatMoveCosine)
		{
			bool bAbovePlane = ((pxDestWP->GetPosition() - xDestInCurrentWPPlane)) * pxStartWP->GetNormal() > 0.0f; 
			if (bAbovePlane)
			{
				bJumpUp = true;
			}
			else
			{
				bJumpDown = true;
			}
		}

		uJumpHashID = AVP_Move_Alien_AI_JumpSurfaceFlat::GetStaticHash();
		// flat jump require animation hash id
		uJumpAnimHashID = GetFlatJumpAnimHashID(xStartJumpPos, xDestPos, bJumpOver, bJumpUp, bJumpDown, !bForceIdlePostJump);
	}
	else if (xTurnForward * xDestForward > dbg_fStartCloseToEndCosine) // post turn forward is more or less destination forward and we jump on wall - we jump left or right
	{
		if (bTurnRight)
		{
			uJumpHashID = AVP_Move_Alien_AI_JumpSurfaceConcaveRight::GetStaticHash();
		}
		else
		{
			uJumpHashID = AVP_Move_Alien_AI_JumpSurfaceConcaveLeft::GetStaticHash();
		}
	}
	else
	{
		uJumpHashID = AVP_Move_Alien_AI_JumpSurfaceConcave90Begin::GetStaticHash();
	}

	if (!bNeedToMoveOrTurn) // no turn to the jump is the whole move
	{
		uMoveHashID = uJumpHashID;
	}
}


void AVP_Alien_Movement::CalculateTurn(const Asura_Vector_3& xDirection, Asura_Hash_ID &uMoveHashID, Asura_Matrix_3x3& xTurnOrientation)
{
	Asura_Matrix_3x3 xCurrentOrientation = GetAlienBrain()->GetActor()->GetOrientation();
	Asura_Vector_3 xCurrentPosition = GetAlienBrain()->GetActor()->GetPosition();

	Asura_Vector_3 xStartNormal(-xCurrentOrientation.e12, -xCurrentOrientation.e22, -xCurrentOrientation.e32);
	Asura_Vector_3 xStartForward(xCurrentOrientation.e13, xCurrentOrientation.e23, xCurrentOrientation.e33);
	Asura_Vector_3 xStartRight(xCurrentOrientation.e11, xCurrentOrientation.e21, xCurrentOrientation.e31);

	bool bTurnRight = xDirection * xStartRight > 0; 
	if (bTurnRight)
	{
		uMoveHashID = AVP_Move_Alien_AI_Turn180Right::GetStaticHash();
	}
	else
	{
		uMoveHashID = AVP_Move_Alien_AI_Turn180Left::GetStaticHash();
	}

	const Asura_Vector_3 xTurnRight = xDirection ^ xStartNormal;

	xTurnOrientation.e11 = xTurnRight.x;
	xTurnOrientation.e21 = xTurnRight.y;
	xTurnOrientation.e31 = xTurnRight.z;
	xTurnOrientation.e12 = -xStartNormal.x;
	xTurnOrientation.e22 = -xStartNormal.y;
	xTurnOrientation.e32 = -xStartNormal.z;
	xTurnOrientation.e13 = xDirection.x;
	xTurnOrientation.e23 = xDirection.y;
	xTurnOrientation.e33 = xDirection.z;
}

Asura_Hash_ID AVP_Alien_Movement::GetFlatJumpAnimHashID(const Asura_Vector_3& xStart,const Asura_Vector_3& xEnd,  bool bJumpOver, bool bJumpUp, bool bJumpDown, bool bRunAfterJump) const
{
	Asura_Hash_ID uMoveHashID = ASURA_HASH_ID_UNSET;

	if (bJumpOver)
	{
		int iVariation = 0;
		if (!GetJumpOverVariation(xStart, xEnd, GetAlienBrain()->GetOwnerGuid(), iVariation))
		{
			Asura_Assert(false, "Jump variation not found");
			iVariation = 0;
		}
		
		const Asura_Blueprint* pxBP = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESALIEN, uAVP_BLUEPRINT_PARAM_JUMPOVERSET );
		Asura_Assert(pxBP, "Alien Blueprint not found");
		Asura_Value xValue;
		if (pxBP && pxBP->GetParameterValueByIndex(iVariation, xValue)) 
		{
			if (bRunAfterJump)
			{
				const Asura_Blueprint* pxVariationBP = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESALIEN, xValue.GetHashIDValue());
				if (pxVariationBP && pxVariationBP->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_THIRDPERSONANIM_RUN, xValue))
				{
					uMoveHashID = xValue.GetHashIDValue();
				}
			}
			else
			{
				const Asura_Blueprint* pxVariationBP = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESALIEN, xValue.GetHashIDValue());
				if (pxVariationBP && pxVariationBP->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_THIRDPERSONANIM, xValue))
				{
					uMoveHashID = xValue.GetHashIDValue();
				}

			}
		}
	}
	else 
	{
		Asura_Value xValue;
		Asura_Blueprint* pxBP = Asura_Blueprint_System::GetBlueprintByHashID( uAVP_BLUEPRINT_TYPE_MOVESALIEN, g_uAIMovesBP );
		uMoveHashID = AVP_Move_Alien_AI_JumpSurfaceFlat::GetStaticHash();
		u_int uJumpAnimParam;
		if (bJumpUp)
		{
			uJumpAnimParam = g_uFlatJumpAnimParam;
		}
		else if (bJumpDown)
		{
			uJumpAnimParam = g_uFlatDownJumpAnimParam;
		}
		else
		{
			uJumpAnimParam = g_uFlatUpJumpAnimParam;
		}

		if ( pxBP && pxBP->GetParameterValueByHashID( uJumpAnimParam, xValue ) )
		{
			uMoveHashID = xValue.GetHashIDValue();
		}
	}

	return uMoveHashID;

}


// void AVP_Alien_Movement::RequestJumpMoveCalcOrientation( const Asura_Hash_ID uMoveHashID, const Asura_Vector_3& xDestPos, const Asura_Vector_3& xDestNormal )
// {
// 	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Alien_AI_Move_JumpSurfaceStart ) );
// 	if ( pxMessage != NULL )
// 	{
// 		AVP_Message_Alien_AI_Move_JumpSurfaceStart* pxData = static_cast< AVP_Message_Alien_AI_Move_JumpSurfaceStart* >( pxMessage->GetData() );
// 
// 		pxMessage->SetFromGuid( GetAlienBrain()->GetOwnerGuid() );
// 		pxMessage->SetToGuid( GetAlienBrain()->GetOwnerGuid() );
// 
// 		pxData->ClearFlags();
// 		pxData->SetIgnoredByOwner( false );
// 		pxData->SetMoveID( uMoveHashID );
// 
// 		Asura_Matrix_3x3 xOrientation = GetAlienBrain()->GetActor()->GetOrientation();
// 		Asura_Vector_3 xStartPosition = GetAlienBrain()->GetActor()->GetPosition();		
// 		Asura_Vector_3 xStartNormal(-xOrientation.e12, -xOrientation.e22, -xOrientation.e32);
// 		Asura_Vector_3 xStartForward(xOrientation.e13, xOrientation.e23, xOrientation.e33);
// 
// 		Asura_Quat xTurnQuad;
// 
// 		// need to avoid 180 deg turn as in this case the turn is arbitrary 
// 		if (xStartNormal * xDestNormal < -0.99)
// 		{
// 			xTurnQuad.FromRotation(Asura_Random::GetInt(0,1) == 0 ? dbg_fSmallRotationToResolveAmbiguity : -dbg_fSmallRotationToResolveAmbiguity , xStartForward);
// 			xStartNormal = xTurnQuad.RotateVector(xStartNormal);
// 		}
// 
// 		xTurnQuad.RotationFromAToB(xStartNormal, xDestNormal);
// 		Asura_Matrix_3x3 xRotation;
// 		xTurnQuad.ToMatrix(xRotation);
// 		xOrientation = xRotation * xOrientation;
// 
// 		pxData->SetDestinationOrientation(xOrientation);
// 		pxData->SetDestinationPosition( xDestPos );
// 
// 		AVP_Actor_AIAnimStateMachine* pxVertStateHandler = GetAlienBrain()->GetActor()->GetAIAnimStateMachine();
// 		pxVertStateHandler->RequestInMove( pxMessage, GetAnimStateFromMovementMode( GetCurrentMovementMode() ) );
// 	}
// 	else
// 	{
// 		// Do it without the move if something goes wrong.
// 		RequestMovementAnimState();
// 	}
// }
// 

void AVP_Alien_Movement::RequestIdle()
{
	SetMovementMode(EMM_IDLE);
}

bool AVP_Alien_Movement::IsInIdle(float& fTimeInIdle) const
{
	const AVP_Actor_AIAnimStateMachine* pxVertStateHandler = GetAlienBrain()->GetActor()->GetAIAnimStateMachine();
	bool bIsIdle = pxVertStateHandler->IsIdle();

	if (bIsIdle)
	{
		fTimeInIdle = pxVertStateHandler->GetTimeInState();
		return true;
	}
	else
	{
		fTimeInIdle = 0.0f;
		return pxVertStateHandler->IsStateIdle(pxVertStateHandler->GetTargetState());
	}

}

void AVP_Alien_Movement::RequestFall()
{

	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usCurrentWPID );
	Asura_Assert(pxCurrentWP, "Can not find current way point");
	if ( !pxCurrentWP || (!pxCurrentWP->IsCeilingWaypoint() && !pxCurrentWP->IsWallWaypoint()) ) // don't fall from the ground..
	{
		SwitchToState(FINISHED);
		return;
	}

	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Alien_AI_Move_FallStart ) );
	Asura_Assert(pxMessage, "RequestJump create message failed");
	if ( pxMessage != NULL )
	{
		AVP_Message_Alien_AI_Move_FallStart* pxData = static_cast< AVP_Message_Alien_AI_Move_FallStart* >( pxMessage->GetData() );

		pxMessage->SetFromGuid( GetAlienBrain()->GetOwnerGuid() );
		pxMessage->SetToGuid( GetAlienBrain()->GetOwnerGuid() );

		pxData->ClearFlags();
		// pxData->SetForceAbort( true );
		pxData->SetIgnoredByOwner( false );
		pxData->SetMoveID( AVP_Move_Alien_AI_FallStart::GetStaticHash() );

		const Asura_Vector_3 xDestinationNormal(0.0f, -1.0f, 0.0f);
		Asura_Matrix_3x3 xOrientation = GetAlienBrain()->GetActor()->GetOrientation();
		Asura_Vector_3 xStartNormal(-xOrientation.e12, -xOrientation.e22, -xOrientation.e32);
		Asura_Vector_3 xStartForward(xOrientation.e13, xOrientation.e23, xOrientation.e33);

		Asura_Quat xTurnQuad;
		// need to avoid 180 deg turn as in this case the turn is arbitrary 
		if (xStartNormal * xDestinationNormal < -0.99)
		{
			xTurnQuad.FromRotation(Asura_Random::GetInt(0,1) == 0 ? dbg_fSmallRotationToResolveAmbiguity : -dbg_fSmallRotationToResolveAmbiguity , xStartForward);
			xStartNormal = xTurnQuad.RotateVector(xStartNormal);
		}

		xTurnQuad.RotationFromAToB(xStartNormal, xDestinationNormal);
		Asura_Matrix_3x3 xRotation;
		xTurnQuad.ToMatrix(xRotation);
		xOrientation = xRotation * xOrientation;

		pxData->SetDestinationOrientation( xOrientation );

		// after fall we must be in idle to start with...
		// u_int uAnimState = GetAnimStateFromMovementMode(m_eActualMoveMode);

		AVP_Actor_AIAnimStateMachine* pxVertStateHandler = GetAlienBrain()->GetActor()->GetAIAnimStateMachine();
		pxVertStateHandler->RequestInMove( pxMessage, AVP_Vertebrate_AIAnimStateMachine::ACTOR_STATE_IDLE );
	}
	else
	{
		SetInError(true);
		SwitchToState(FINISHED);
	}


}

bool AVP_Alien_Movement::UpdateJumping()
{
	if ( !m_bJumpFromDanger ) { return false; }
	if ( m_bJumpingDisallowed) { return false; }
	if (!m_bNotGS2Mode) { return false;	}
	if (GetAlienBrain()->IsMelee()) { return false; }
	if ( m_fJumpUpdateTimer > 0.0f ) { return false; }
	const float fLastDamagedInterval = GetAlienBrain()->GetActor()->GetLastDamageInterval();
	if ( 0.0f < fLastDamagedInterval && fLastDamagedInterval < fJUMP_DAMAGE_DISABLE_PERIOD ) { return false; }
	if ( m_pxCurrentTraverser && !m_pxCurrentTraverser->CanBeInterrupted() ) { return false; }
	if ( GetAlienBrain()->IsPlayingMove() ) { return false; }

	bool bNeedToJump = false;
	Asura_Vector_3 xFromDanger;
	float		   fFromDanger;

	const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();

	if ( !bNeedToJump )
	{
		Asura_Vector_3 xFromNearestProj = Asura_Maths::ZeroVector3;
		float		   fFromNearestProjSq = Asura_Maths::LargeFloat;

		// Test to see if any projectiles are near enough.
		for ( Asura_Hashed_List_ConstIt< AVP_ServerProjectile_GenericProjectile > xProjIter( AVP_ServerProjectile_GenericProjectile::GetList() );
			!xProjIter.Done();
			xProjIter.Next() )
		{
			const AVP_ServerProjectile_GenericProjectile* pxProj = xProjIter.GetCurrent();
			if ( !pxProj->IsAvoidedByAI() || pxProj->IsForwardPredictedByAI() ) { continue; }

			Asura_Vector_3 xFromProj = xOurPosition - pxProj->GetPosition();
			const float fFromProjSq = xFromProj.MagnitudeSquared();
			if ( fFromProjSq < fFromNearestProjSq )
			{
				fFromNearestProjSq = fFromProjSq;
				xFromNearestProj = xFromProj;
			}
		}

		if ( fFromNearestProjSq < fJUMP_TRIGGER_RANGE * fJUMP_TRIGGER_RANGE )
		{
			fFromDanger = Asura_Maths::Sqrt( fFromNearestProjSq );
			xFromDanger = xFromNearestProj / fFromDanger;
			bNeedToJump = true;
		}
	}

	if ( !bNeedToJump )
	{
		// Test the forward predicted threats next.
		Asura_Vector_3 xFromNearestThreat = Asura_Maths::ZeroVector3;
		float		   fFromNearestThreatSq = Asura_Maths::LargeFloat;

		const AVP_ServerEntity_Team_Alien* pxAlienTeam = GetAlienBrain()->GetAlienTeam();
		for ( u_int u = 0 ; u < pxAlienTeam->GetNumFPThreats(); ++u )
		{
			Asura_Vector_3 xThreatP0, xThreatP1;
			if ( !pxAlienTeam->GetFPThreatData( u, xThreatP0, xThreatP1 ) ) { continue; }
			Asura_Vector_3 xNearestPointToThreat;
			Asura_Maths::GetNearestPointOnLineSegment( xThreatP0, xThreatP1, xOurPosition, xNearestPointToThreat );

			Asura_Vector_3 xFromThreat = xOurPosition - xNearestPointToThreat;
			const float fFromThreatSq = xFromThreat.MagnitudeSquared();
			if ( fFromThreatSq < fFromNearestThreatSq )
			{
				fFromNearestThreatSq = fFromThreatSq;
				xFromNearestThreat = xFromThreat;
			}
		}

		if ( fFromNearestThreatSq < fJUMP_TRIGGER_RANGE * fJUMP_TRIGGER_RANGE )
		{
			fFromDanger = Asura_Maths::Sqrt( fFromNearestThreatSq );
			xFromDanger = xFromNearestThreat / fFromDanger;
			bNeedToJump = true;
		}
	}

	if ( !bNeedToJump )
	{
		// Test to see if our threat will hit us or get near us with their fire.
		const AVP_AlienTeam_Knowledge* pxCurrentTarget = GetAlienBrain()->GetCurrentTargetKnowledge();
		if ( !pxCurrentTarget ) { return false; }
		if ( !pxCurrentTarget->IsWeaponFiring() ) { return false; }
		const Asura_Vector_3& xWeaponP0 = pxCurrentTarget->GetWeaponPosition();
		const Asura_Vector_3& xWeaponP1 = xWeaponP0 + ( pxCurrentTarget->GetWeaponDirection() * pxCurrentTarget->GetWeaponLOSLength() );
		Asura_Vector_3 xNearestPointToWeaponFire;
		Asura_Maths::GetNearestPointOnLineSegment( xWeaponP0, xWeaponP1, xOurPosition, xNearestPointToWeaponFire );

		Asura_Vector_3 xFromWeaponFire = xOurPosition - xNearestPointToWeaponFire;
		const float fFromWeaponFireSq = xFromWeaponFire.MagnitudeSquared();
		if ( fFromWeaponFireSq < fJUMP_TRIGGER_RANGE * fJUMP_TRIGGER_RANGE )
		{
			fFromDanger = Asura_Maths::Sqrt( fFromWeaponFireSq );
			xFromDanger = xFromWeaponFire / fFromDanger;
			bNeedToJump = true;
		}
	}

	if ( !bNeedToJump ) { return false; }

	// If we don't want to jump then drop out and set the timer, so we don't check again for a while.
	if ( Asura_Random::GetFloat() > GetAlienBrain()->GetDangerJumpFraction() )
	{
		m_fJumpUpdateTimer = fJUMP_PERIOD;
		return false;
	}

	// Work out which direction we need to jump in.
	JumpDirection eJumpDir;
	Asura_Vector_3 xIdealJumpDir;
	Asura_Vector_3 xFromDangerFlat;
	GetAlienBrain()->ResolveToOurPlane( xFromDanger, xFromDangerFlat );
	const Asura_Matrix_3x3& xOurOrientM = GetAlienBrain()->GetActor()->GetOrientation();
	const Asura_Vector_3 xRight( xOurOrientM.e11, xOurOrientM.e21, xOurOrientM.e31 );
	const float fFromAndRightDot = xFromDangerFlat * xRight;
	if ( fFromAndRightDot > 0.0f )
	{
		eJumpDir = JUMP_RIGHT;
		xIdealJumpDir = xRight;
	}
	else
	{
		eJumpDir = JUMP_LEFT;
		xIdealJumpDir = -xRight;
	}

	// Now work out which jumps we can do based on where we are relative to everything.
	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( GetAlienBrain()->GetNavData().m_usCurrentWPID );
	if ( !pxCurrentWP )
	{
		return false;
	}
	if ( pxCurrentWP->IsCeilingWaypoint() )
	{
		if ( AttemptCeilingToFloorJump( pxCurrentWP ) )
		{
			SwitchToState( JUMPING_FROM_DANGER );
			return true;
		}
	}
	else if ( pxCurrentWP->IsWallWaypoint() )
	{
		if ( AttemptWallToWallJump180( pxCurrentWP ) ||
			AttemptWallToFloorJump( eJumpDir, xIdealJumpDir ) ||
			//AttemptWallHop( eJumpDir, xIdealJumpDir ) ||
			AttemptWallToWallJump90( eJumpDir, xIdealJumpDir ) )
		{
			SwitchToState( JUMPING_FROM_DANGER );
			return true;
		}
	} 
	else
	{
		if ( AttemptFloorToWallJump( eJumpDir, xIdealJumpDir ) || AttemptFloorHop( eJumpDir, xIdealJumpDir ) )
		{
			SwitchToState( JUMPING_FROM_DANGER );
			return true;
		}
	}

	m_fJumpUpdateTimer = fJUMP_PERIOD;
	return false;
}

bool AVP_Alien_Movement::AttemptCeilingToFloorJump( const Asura_AI_NavVertex* pxCurrentWP )
{
	const Asura_AI_NavVertex* pxConnectedFloorWP = NULL;
	for ( u_int uLink = 0; uLink < pxCurrentWP->GetNumLinks(); ++uLink )
	{
		const Asura_AI_NavVertex* pxNextWP = pxCurrentWP->GetLinkedWaypoint( uLink );
		if ( !pxNextWP->IsCeilingWaypoint() && !pxNextWP->IsWallWaypoint() )
		{
			pxConnectedFloorWP = pxNextWP;
			break;
		}
	}

	if ( !pxConnectedFloorWP )
	{
		return false;
	}

	// Jump onto the floor.
	RequestJumpMove( AVP_Move_Alien_JumpSurface180::GetStaticHash(), pxConnectedFloorWP->GetPosition(), pxConnectedFloorWP->GetNormal() );
	return true;
}

bool AVP_Alien_Movement::AttemptWallToWallJump180( const Asura_AI_NavVertex* pxCurrentWP )
{
	const Asura_Vector_3& xCurrentNormal = pxCurrentWP->GetNormal();

	const Asura_AI_NavVertex* pxConnectedOppositeWallWP = NULL;
	for ( u_int uLink = 0; uLink < pxCurrentWP->GetNumLinks(); ++uLink )
	{
		const Asura_AI_NavVertex* pxNextWP = pxCurrentWP->GetLinkedWaypoint( uLink );
		if ( !pxNextWP->IsWallWaypoint() )
		{
			continue;
		}

		// ~165°. Allowing 15° surface tolerance.
		if ( pxNextWP->GetNormal() * xCurrentNormal > -0.9659258f )
		{
			continue;
		}

		pxConnectedOppositeWallWP = pxNextWP;
		break;
	}

	if ( !pxConnectedOppositeWallWP )
	{
		return false;
	}

	// Jump onto the wall.
	RequestJumpMove( AVP_Move_Alien_JumpSurface180::GetStaticHash(), pxConnectedOppositeWallWP->GetPosition(), pxConnectedOppositeWallWP->GetNormal() );
	return true;
}

bool AVP_Alien_Movement::AttemptWallToFloorJump( const JumpDirection eJumpDir, const Asura_Vector_3& xIdealJumpDir )
{
	// Work out which jump we should use.
	const Asura_Movement_System* pxMovementSystem = GetAlienBrain()->GetClientActor()->GetMovementSystem();
	if ( !pxMovementSystem ) { return false; }
	const AVP_Move* pxJumpMove;
	Asura_Hash_ID uJumpMoveHashID;
	if ( eJumpDir == JUMP_RIGHT )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpOnToWallRight::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else if ( eJumpDir == JUMP_LEFT )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpOnToWallLeft::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else if ( eJumpDir == JUMP_FORWARDS )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpSurfaceConcave90::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else
	{
		return false;
	}
	if ( !pxJumpMove ) { return false; }
	const Asura_Hash_ID uJumpAnimHashID = pxJumpMove->GetThirdPersonAnimHash();
	if ( uJumpAnimHashID == ASURA_HASH_ID_UNSET ) { return false; }

	// Work out if we are able to use this move.
	const Asura_AI_NavVertex* pxDestinationWP = GetDestinationWaypoint( uJumpAnimHashID );
	if ( !pxDestinationWP || pxDestinationWP->IsWallWaypoint() || pxDestinationWP->IsCeilingWaypoint() ) { return false; }
	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( GetAlienBrain()->GetNavData().m_usCurrentWPID );
	if ( !pxCurrentWP || !AVP_Navigation::IsConcaveLink( pxCurrentWP->GetPosition(), pxDestinationWP->GetPosition(), pxDestinationWP->GetNormal() ) ) { return false; }

	// Fire the jump move off.
	RequestJumpMove( uJumpMoveHashID, pxDestinationWP->GetPosition(), pxDestinationWP->GetNormal() );
	return true;
}

bool AVP_Alien_Movement::AttemptWallHop( const JumpDirection eJumpDir, const Asura_Vector_3& xIdealJumpDir )
{
	// Can we just do the standard hop?
	const Asura_Movement_System* pxMovementSystem = GetAlienBrain()->GetClientActor()->GetMovementSystem();
	if ( !pxMovementSystem ) { return false; }
	const AVP_Move* pxHopMove;
	Asura_Hash_ID uHopMoveHashID;
	if ( eJumpDir == JUMP_RIGHT )
	{
		uHopMoveHashID = AVP_Move_Alien_WallHopRight::GetStaticHash();
		pxHopMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uHopMoveHashID ) );
	}
	else if ( eJumpDir == JUMP_LEFT )
	{
		uHopMoveHashID = AVP_Move_Alien_WallHopLeft::GetStaticHash();
		pxHopMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uHopMoveHashID ) );
	}
	else
	{
		return false;
	}
	const Asura_Hash_ID uHopAnimHashID = pxHopMove ? pxHopMove->GetThirdPersonAnimHash() : ASURA_HASH_ID_UNSET;
	if ( uHopAnimHashID == ASURA_HASH_ID_UNSET ) { return false; }

	// Work out if we are able to use this move.
	const Asura_AI_NavVertex* pxDestinationWP = GetDestinationWaypoint( uHopAnimHashID );
	if ( !pxDestinationWP || !pxDestinationWP->IsWallWaypoint() ) { return false; }
	if ( !GetAlienBrain()->GetActor()->HasSpaceForAnim( uHopAnimHashID ) ) { return false; }

	// Fire off the hop move.
	RequestMove( uHopMoveHashID );
	return true;
}

bool AVP_Alien_Movement::AttemptWallToWallJump90( const JumpDirection eJumpDir, const Asura_Vector_3& xIdealJumpDir )
{
	// Work out which jump we should use.
	const Asura_Movement_System* pxMovementSystem = GetAlienBrain()->GetClientActor()->GetMovementSystem();
	if ( !pxMovementSystem ) { return false; }
	const AVP_Move* pxJumpMove;
	Asura_Hash_ID uJumpMoveHashID;
	if ( eJumpDir == JUMP_RIGHT )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpOnToWallRight::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else if ( eJumpDir == JUMP_LEFT )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpOnToWallLeft::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else if ( eJumpDir == JUMP_FORWARDS )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpSurfaceConcave90::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else
	{
		return false;
	}
	const Asura_Hash_ID uJumpAnimHashID = pxJumpMove ? pxJumpMove->GetThirdPersonAnimHash() : ASURA_HASH_ID_UNSET;
	if ( uJumpAnimHashID == ASURA_HASH_ID_UNSET ) { return false; }

	// Work out if we are able to use this move.
	const Asura_AI_NavVertex* pxDestinationWP = GetDestinationWaypoint( uJumpAnimHashID );
	if ( !pxDestinationWP || !pxDestinationWP->IsWallWaypoint() ) { return false; }
	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( GetAlienBrain()->GetNavData().m_usCurrentWPID );
	if ( !pxCurrentWP || !AVP_Navigation::IsConcaveLink( pxCurrentWP->GetPosition(), pxDestinationWP->GetPosition(), pxDestinationWP->GetNormal() ) ) { return false; }

	// Fire the jump move off.
	RequestJumpMove( uJumpMoveHashID, pxDestinationWP->GetPosition(), pxDestinationWP->GetNormal() );
	return true;
}

bool AVP_Alien_Movement::AttemptFloorToWallJump( const JumpDirection eJumpDir, const Asura_Vector_3& xIdealJumpDir )
{
	// Work out which jump we should use.
	const Asura_Movement_System* pxMovementSystem = GetAlienBrain()->GetClientActor()->GetMovementSystem();
	if ( !pxMovementSystem ) { return false; }
	const AVP_Move* pxJumpMove;
	Asura_Hash_ID uJumpMoveHashID;
	if ( eJumpDir == JUMP_RIGHT )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpOnToWallRight::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else if ( eJumpDir == JUMP_LEFT )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpOnToWallLeft::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else if ( eJumpDir == JUMP_FORWARDS )
	{
		uJumpMoveHashID = AVP_Move_Alien_JumpSurfaceConcave90::GetStaticHash();
		pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );
	}
	else
	{
		return false;
	}
	const Asura_Hash_ID uJumpAnimHashID = pxJumpMove ? pxJumpMove->GetThirdPersonAnimHash() : ASURA_HASH_ID_UNSET;
	if ( uJumpAnimHashID == ASURA_HASH_ID_UNSET ) { return false; }

	// Work out if we are able to use this move.
	const Asura_AI_NavVertex* pxDestinationWP = GetDestinationWaypoint( uJumpAnimHashID );
	if ( !pxDestinationWP || !pxDestinationWP->IsWallWaypoint() ) { return false; }
	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( GetAlienBrain()->GetNavData().m_usCurrentWPID );
	if ( !pxCurrentWP || !AVP_Navigation::IsConcaveLink( pxCurrentWP->GetPosition(), pxDestinationWP->GetPosition(), pxDestinationWP->GetNormal() ) ) { return false; }

	// Fire the jump move off.
	RequestJumpMove( uJumpMoveHashID, pxDestinationWP->GetPosition(), pxDestinationWP->GetNormal() );
	return true;
}

bool AVP_Alien_Movement::AttemptFloorHop( const JumpDirection eJumpDir, const Asura_Vector_3& xIdealJumpDir )
{
	// Can we just do the standard hop?
	const Asura_Movement_System* pxMovementSystem = GetAlienBrain()->GetClientActor()->GetMovementSystem();
	if ( !pxMovementSystem ) { return false; }
	const AVP_Move* pxHopMove;
	Asura_Hash_ID uHopMoveHashID;
	if ( eJumpDir == JUMP_RIGHT )
	{
		uHopMoveHashID = AVP_Move_Alien_HopRight::GetStaticHash();
		pxHopMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uHopMoveHashID ) );
	}
	else if ( eJumpDir == JUMP_LEFT )
	{
		uHopMoveHashID = AVP_Move_Alien_HopLeft::GetStaticHash();
		pxHopMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uHopMoveHashID ) );
	}
	else
	{
		return false;
	}
	const Asura_Hash_ID uHopAnimHashID = pxHopMove ? pxHopMove->GetThirdPersonAnimHash() : ASURA_HASH_ID_UNSET;
	if ( uHopAnimHashID == ASURA_HASH_ID_UNSET ) { return false; }

	// Work out if we are able to use this move.
	const Asura_AI_NavVertex* pxDestinationWP = GetDestinationWaypoint( uHopAnimHashID );
	if ( !pxDestinationWP || pxDestinationWP->IsWallWaypoint() || pxDestinationWP->IsCeilingWaypoint() ) { return false; }
	if ( !GetAlienBrain()->GetActor()->HasSpaceForAnim( uHopAnimHashID ) ) { return false; }

	// Fire off the hop move.
	RequestJumpMove( uHopMoveHashID, pxDestinationWP->GetPosition(), pxDestinationWP->GetNormal() );
	return true;
}


DEBUGVAR float dbg_f45degCos = Asura_Maths::Cosine(Asura_Maths::DegToRad(45.0f));
DEBUGVAR float dbg_fMaxJumpDistanceSquared = 8.0f*8.0f;

bool AVP_Alien_Movement::AttemptJumpToUnStuck()
{

	const AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	const Asura_Vector_3& xCurrentPosition = GetAlienBrain()->GetActor()->GetPosition();
	
	const Asura_AI_NavVertex* pxDestinationWP;
	u_short usDestinationWPID;

	if (xNavData.m_usRouteLength == 0)
	{
		return false;
	}
	else if (xNavData.m_usRouteSegmentIndex > xNavData.m_usRouteLength - 2 )
	{
		usDestinationWPID = xNavData.m_pusRoute[xNavData.m_usRouteLength - 1];

	}
	else
	{
		usDestinationWPID = xNavData.m_pusRoute[xNavData.m_usRouteSegmentIndex + 1];
	}

	pxDestinationWP = Asura_AI_Navigation_System::GetWaypoint( usDestinationWPID );
	if ( !pxDestinationWP )
	{
		return false;	
	}

	if((pxDestinationWP->GetPosition() - xCurrentPosition).MagnitudeSquared() > dbg_fMaxJumpDistanceSquared)
	{
		AVP_Navigation::NavData xTempNavData;
		xNavData.CopyConfigTo(xTempNavData);
		xTempNavData.ResetRoute();
		xTempNavData.m_xStartPosition       = GetAlienBrain()->GetActor()->GetPosition();
		xTempNavData.m_usDestinationWPID	= usDestinationWPID;
		xTempNavData.m_xDestinationPosition = pxDestinationWP->GetPosition();

		AVP_Navigation::AlienAStarFunctor xFunctor;
		xFunctor.SetNoLimit();

		if ( !AVP_Navigation::FindRoute( xTempNavData, &xFunctor, false ) )
		{
			return false;
		}

		pxDestinationWP = NULL;
		float fMaxDistanceSquared =0.0f;

		for (u_short u = 1; u < xTempNavData.m_usRouteLength; ++u )
		{
			const Asura_AI_NavVertex* pxCandidateWP = Asura_AI_Navigation_System::GetWaypoint( xTempNavData.m_pusRoute[u] );
			if (!pxCandidateWP)
			{
				continue;
			}
			float fCandidateDistanceSquared = (pxCandidateWP->GetPosition() - xCurrentPosition).MagnitudeSquared();
			if (fCandidateDistanceSquared > fMaxDistanceSquared &&  
				fCandidateDistanceSquared < dbg_fMaxJumpDistanceSquared)
			{
				fMaxDistanceSquared = fCandidateDistanceSquared;
				pxDestinationWP = pxCandidateWP;
			}
		}
	}

	if (pxDestinationWP == NULL)
	{
		return false;
	}

	Asura_Hash_ID uJumpMoveHashID = AVP_Move_Alien_JumpSurfaceConcave90::GetStaticHash();
	const Asura_Movement_System* pxMovementSystem = GetAlienBrain()->GetClientActor()->GetMovementSystem();
	if ( !pxMovementSystem ) 
	{
		return false; 
	}
	const AVP_Move* pxJumpMove = static_cast< const AVP_Move* >( pxMovementSystem->GetMove( uJumpMoveHashID ) );

	if ( !pxJumpMove ) 
	{
		return false; 
	}

	const Asura_Hash_ID uJumpAnimHashID = pxJumpMove->GetThirdPersonAnimHash();
	if ( uJumpAnimHashID == ASURA_HASH_ID_UNSET ) { return false; }

	// Fire the jump move off.
	RequestJumpMove( uJumpMoveHashID, pxDestinationWP->GetPosition(), pxDestinationWP->GetNormal() );
	SwitchToState( RESOLVE_STUCK_BY_JUMPING );
	return true;
}



const Asura_AI_NavVertex* AVP_Alien_Movement::GetDestinationWaypoint( const Asura_Hash_ID uAnimHashID )
{
	static const float fJUMP_TEST_SCALE_1 = 1.0f;
	static const float fJUMP_TEST_SCALE_2 = 1.25f;
	static const float fJUMP_TEST_SCALE_3 = 0.75f;
	static const float fJUMP_TEST_SCALE_LARGEST = fJUMP_TEST_SCALE_2;

	// Fire a ray in waypoint space along the direction of travel of the anim and see which waypoint is nearest our ideal end position.
	Asura_Vector_3 xAnimMovementDir;
	if ( !GetAlienBrain()->GetActor()->GetAnimMovementOffset( uAnimHashID, 1.0f, &xAnimMovementDir ) ) { return false; }
	const float fAnimMovement = xAnimMovementDir.Normalise();
	xAnimMovementDir = xAnimMovementDir * GetAlienBrain()->GetActor()->GetOrientation();

	const Asura_Vector_3& xOurPosition = GetAlienBrain()->GetActor()->GetPosition();
	// These are in order of desirability.
	s_axTestPositions[ 0 ] = xOurPosition + xAnimMovementDir * fAnimMovement;
	s_axTestPositions[ 1 ] = xOurPosition + xAnimMovementDir * ( fAnimMovement * fJUMP_TEST_SCALE_1 );
	s_axTestPositions[ 2 ] = xOurPosition + xAnimMovementDir * ( fAnimMovement * fJUMP_TEST_SCALE_2 );
	s_axTestPositions[ 3 ] = xOurPosition + xAnimMovementDir * ( fAnimMovement * fJUMP_TEST_SCALE_3 );
	s_pxClosestWP = NULL;
	s_fSmallestDistanceSq = Asura_Maths::LargeFloat;
	float fSearchDistance = ( fAnimMovement * fJUMP_TEST_SCALE_LARGEST ) + 0.5f;
	Asura_AI_Navigation_System::SearchWaypoints( xOurPosition, xAnimMovementDir, &fSearchDistance, WaypointRaySearchCallback );
	return s_pxClosestWP;
}

int AVP_Alien_Movement::GetAnimStateFromMovementMode( AVP_Alien_Movement::ExMovementMode eMovementMode )
{
	switch ( eMovementMode )
	{
	case EMM_SLOW:	return AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_SLOW;
	case EMM_MEDIUM:	return AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_MEDIUM;
	case EMM_FAST:	return AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_FAST;
	case EMM_IDLE: return AVP_Vertebrate_AIAnimStateMachine::ACTOR_STATE_IDLE;
	default:
		Asura_Assert(false, "Unexpected Move mode");
		return AVP_Vertebrate_AIAnimStateMachine::ACTOR_STATE_IDLE;
	}
}



bool AVP_Alien_Movement::GetJumpOverVariation(const Asura_Vector_3& xStartPosition, const Asura_Vector_3& xEndPosition, Asura_Guid uActorGuid, int& iVariation)
{
	iVariation = -1;
	const Asura_Blueprint* pxBlueprint = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESALIEN, uAVP_BLUEPRINT_PARAM_JUMPOVERSET );
	Asura_Assert(pxBlueprint, "Alien Blueprint not found");
	if (!pxBlueprint) 
	{
		return false; 
	}

	Asura_Value xValue;

	Asura_Vector_3 xStartToEndDirection = xEndPosition - xStartPosition;
	Asura_Vector_3 xHorDirection = xStartToEndDirection; xHorDirection.y = 0.0f;
	float fHorDistance = xHorDirection.Normalise();
	xStartToEndDirection.Normalise();
	int iBestPreference = -1;

	for (int v=0; v<pxBlueprint->GetNumParameters(); ++v)
	{
		if (!pxBlueprint->GetParameterValueByIndex(v, xValue)) { continue; }

		const Asura_Blueprint* pxVariationBlueprint = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESALIEN, xValue.GetHashIDValue());
		if (!pxVariationBlueprint) { continue; }

		if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_NOMINALDISTANCE, xValue)) { continue; }
		const float fHorNominalDistance = xValue.GetFloatValue();
		if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_P1_HORIZONTAL, xValue)) { continue; }
		const float fP1Hor = xValue.GetFloatValue();
		if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_P1_VERTICAL_LOW, xValue)) { continue; }
		const float fP1VerLow = xValue.GetFloatValue();
		if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_P1_VERTICAL_HIGH, xValue)) { continue; }
		const float fP1VerHigh = xValue.GetFloatValue();
		if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_P2_HORIZONTAL, xValue)) { continue; }
		const float fP2Hor = xValue.GetFloatValue();
		if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_P2_VERTICAL_LOW, xValue)) { continue; }
		const float fP2VerLow = xValue.GetFloatValue();
		if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_P2_VERTICAL_HIGH, xValue)) { continue; }
		const float fP2VerHigh = xValue.GetFloatValue();
		if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_PREFERENCE, xValue)) { continue; }
		const int iPreference = xValue.GetIntValue();


		Asura_Vector_3 xTestPoints[4];
		xTestPoints[0] = xStartPosition;
		xTestPoints[1] = xStartPosition + xStartToEndDirection * fHorDistance / fHorNominalDistance * fP1Hor;
		xTestPoints[1].y -= fP1VerLow;
		xTestPoints[2] = xStartPosition + xStartToEndDirection * fHorDistance / fHorNominalDistance * fP2Hor;
		xTestPoints[2].y -= fP2VerLow;
		xTestPoints[3] = xEndPosition;

		// test low foundries
		bool bFailed = false;
		for (int i = 0; i < 3 && !bFailed; ++i)
		{
			Asura_Vector_3 xTestDirection = xTestPoints[i+1] - xTestPoints[i];
			float fTestDistance = xTestDirection.Normalise();
			Asura_LOS_InputData xInput(xTestPoints[i], xTestDirection, uActorGuid, fTestDistance, false, AVP_PolygonFlag_IgnoreForAlienCollision | AsuraPolygonFlag_NoCollisions);
			const Asura_LOS_OutputData* pxOutput = AVP_LOS::TestAgainstEnvironmentAndMoveableObjects(&xInput);
			if (pxOutput->m_fDistanceToIntersection < fTestDistance - 0.2f) 
			{ 
				bFailed = true;
			}
		}

		if (bFailed)
		{
			continue;
		}

		// change to high boundries
		xTestPoints[1].y -= fP1VerHigh - fP1VerLow;
		xTestPoints[2].y -= fP2VerHigh - fP2VerLow;

		// do just test of the top part 
		Asura_Vector_3 xTestDirection = xTestPoints[2] - xTestPoints[1];
		float fTestDistance = xTestDirection.Normalise();
		Asura_LOS_InputData xInput(xTestPoints[1], xTestDirection, uActorGuid, fTestDistance, false, AVP_PolygonFlag_IgnoreForPredatorCollision | AsuraPolygonFlag_NoCollisions);
		const Asura_LOS_OutputData* pxOutput = AVP_LOS::TestAgainstEnvironmentAndMoveableObjects(&xInput);
		if (pxOutput->m_fDistanceToIntersection < fTestDistance) 
		{ 
			continue; 
		}

		// select the prefered variation
		if (iBestPreference < iPreference)
		{
			iBestPreference = iPreference;
			iVariation = v;
		}

	}

	return iVariation != -1;
}


void AVP_Alien_Movement::SetSharpTurnSpeedLimit(AVP_Alien_Movement::ExMovementMode eLimit)
{
	m_eLimitsDueToSharpTurnMode = eLimit;

	SetMovementMode(m_eRequestedMoveMode);
}

bool AVP_Alien_Movement::CanPlanMove() const
{
	switch ( m_eState )
	{
	case FINISHED:
	case MOVING:
		return true;
	case TRAVERSING_ROUTE:
		return ( !m_pxCurrentTraverser || m_pxCurrentTraverser->CanBeInterrupted() );
	default:
	case FALLING:
	case JUMPING_FROM_DANGER:
	case RESOLVE_STUCK_BY_JUMPING:
		return false;
	}
}

void AVP_Alien_Movement::ClearWPDistanceCache()
{
	m_xWPDistanceFromStartCache.Reset();

}

float AVP_Alien_Movement::FindDistanceOfWPFromStart(u_short usWPNumber)
{
	if (usWPNumber < m_xWPDistanceFromStartCache.GetNumberOfNodes())
	{
		const float* pfDistance = m_xWPDistanceFromStartCache.GetData(usWPNumber);
		Asura_Assert(pfDistance, "Distance on route mismatch");
		if (pfDistance)
		{
			return *pfDistance;
		}
		else
		{
			return 0.0f;
		}
	}

	AVP_Navigation::NavData& xNavData = GetAlienBrain()->GetNavData();
	Asura_Assert(usWPNumber < xNavData.m_usRouteLength, "Requesting disatnce for waypoint not on route");

	if (m_xWPDistanceFromStartCache.GetNumberOfNodes() == 0)
	{
		u_short usZero = 0;
		const float* pfSuccess = m_xWPDistanceFromStartCache.AddNode(usZero, 0.0f);
		Asura_Assert(pfSuccess, "Updating distance cache failed");
		if (!pfSuccess)
		{
			return 0.0f;
		}
	}

	const float* pfDistance = m_xWPDistanceFromStartCache.GetData(static_cast<u_short>(m_xWPDistanceFromStartCache.GetNumberOfNodes()-1));
	Asura_Assert(pfDistance, "Updating distance cache failed");
	if (!pfDistance)
	{
		return 0.0f;
	}

	float fDistance = *pfDistance;

	u_short usCurrWPNumber = static_cast<u_short>(m_xWPDistanceFromStartCache.GetNumberOfNodes());

	while (usCurrWPNumber <= usWPNumber)
	{
		const Asura_Vector_3& xPrevWPPosition = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usCurrWPNumber-1 ] )->m_xPosition;
		const Asura_Vector_3& xCurrPosition = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usCurrWPNumber ] )->m_xPosition;

		fDistance += (xCurrPosition -  xPrevWPPosition).Magnitude();
		m_xWPDistanceFromStartCache.AddNode(usCurrWPNumber, fDistance);
		++usCurrWPNumber;
	}

	return fDistance;
}

bool AVP_Alien_Movement::WaypointRaySearchCallback( const u_int uWPID )
{
	const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( uWPID );

	for ( int i = 0; i < iNUM_TEST_POSITIONS; ++i )
	{
		if ( pxWP->Contains( s_axTestPositions[ i ] ) )
		{
			// The distance to our ideal position is used for comparison.
			const Asura_Vector_3 xToIdealPosition = s_axTestPositions[ 0 ] - pxWP->GetPosition();
			const float fDistanceSq = xToIdealPosition.MagnitudeSquared();
			if ( fDistanceSq < s_fSmallestDistanceSq )
			{
				s_pxClosestWP		  = pxWP;
				s_fSmallestDistanceSq = fDistanceSq;

				if ( s_fSmallestDistanceSq <= 1.0f )
				{
					// This is near enough for us. Terminate the search now.
					return false;
				}
			}

			// We've had a hit in this waypoint, so don't bother testing the other positions.
			break;
		}
	}

	return true;
}

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Behaviour_Alien_MovementWrapper
//
//////////////////////////////////////////////////////////////////////////////

DEBUGVAR int dbg_iMaxRetries(3); 

AVP_Behaviour_Alien_MovementWrapper::AVP_Behaviour_Alien_MovementWrapper( AVP_Brain_Actor_Alien* pxAlienBrain )
: PARENT( pxAlienBrain, AVP_BEHAVIOUR_TYPE_ALIEN_MOVEMENTWRAPPER )
, m_bIsMovePending(false)
, m_xPosition(Asura_Maths::ZeroVector3)
, m_iRetries(dbg_iMaxRetries)
{
}

bool AVP_Behaviour_Alien_MovementWrapper::GetToPosition( const Asura_Vector_3& xPosition )
{
	m_xPosition = xPosition;

	if (GetAlienBrain()->IsMelee()) // a bit flaky but if this behaviour is active that means it suspended the only behaviour that could release this flag 
	{								// so force it anyhow. Other flags are controlled by the move behaviour that is still active...
		GetAlienBrain()->SetMelee(false);
	}


	if (GetAlienBrain()->CanPlanMove())
	{
		m_bIsMovePending = false;
		bool bSuccess = GetAlienBrain()->GetMovement().GetToPosition( m_xPosition, true, AVP_Navigation::s_xAlienAStarFunctor, AVP_Alien_Movement::EMM_MEDIUM );
// 		if (bSuccess)
// 		{
// 			GetAlienBrain()->GetMovement().SetAvoidLight();
// 		}
		return bSuccess;
	}
	else
	{
		m_bIsMovePending = true;
		return true;
	}
}

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

	PARENT::ReadFromChunkStream( xStream );

	if ( iVersion == CURRENT_CHUNK_VERSION )
	{
		xStream >> m_xPosition;
		GetToPosition( m_xPosition );
	}
	else
	{
		Asura_Assertf( false, ( "AVP_Behaviour_Alien_MovementWrapper::ReadFromChunkStream - unhandled chunk version (%d)", iVersion ) );
	}
}

void AVP_Behaviour_Alien_MovementWrapper::WriteToChunkStream( Asura_Chunk_Stream& xStream ) const
{
	xStream << CURRENT_CHUNK_VERSION;

	PARENT::WriteToChunkStream( xStream );

	xStream << m_xPosition;
}

void AVP_Behaviour_Alien_MovementWrapper::PerFrameUpdate( const float fTimeslice )
{
	PARENT::PerFrameUpdate( fTimeslice );

	if (m_bIsMovePending)
	{
		GetToPosition( m_xPosition );
		return;
	}

	AVP_Alien_Movement& xMovement = GetAlienBrain()->GetMovement();
	if ( xMovement.IsInError() )
	{
		// we moved along the route but got stuck try again
		if (xMovement.ConsiderateAdvanceAlongRoute())
		{
			m_iRetries = dbg_iMaxRetries;
			GetToPosition( m_xPosition );
		}
		else
		{
			--m_iRetries;
			if (m_iRetries < 0 )
			{
				Asura_Assert(false, "Alien MovementWrapper - failed to get to position");
				SetInError();
			}
			else
			{
				GetToPosition( m_xPosition );
			}
		}
	}
	else if ( xMovement.IsFinished() )
	{
		SetFinished();
	}

}

AVP_Brain_Actor_Alien* AVP_Behaviour_Alien_MovementWrapper::GetAlienBrain() const
{
	return static_cast< AVP_Brain_Actor_Alien* >( GetBrain() );
}

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