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

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

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

// header
#include "Behaviour/BehaviourTypes/Predator/AVP_Behaviour_Predator_Movement.h"

// asura
#include "Blueprint/Asura_Blueprint_System.h"
#ifndef ASURA_NO_DEBUG_OUTPUT
#include "Debugging/Asura_DebugLineRender.h"
#include "Rendering/Asura_Font_System.h"
#endif
#include "Network/Asura_Message.h"

// axon
#include "Behaviour/BehaviourTypes/AVP_Behaviour_Types.h"
#include "Core/BrainTypes/AVP_Brain_Actor_Predator.h"

// avp
#include "Actor/Vertebrates/AVP_Vertebrate_AIAnimStateMachine.h"
#include "Actor/Vertebrates/AVP_Vertebrate_Description.h"
#include "Blueprint/AVP_Blueprint_HashIDs.h"
#include "Core/AVP_Plane.h"
#include "Core/AVP_LOS.h"
#include "Entity/PhysicalObject/Actor/Vertebrates/Predator/AVP_ServerActor_Predator.h"
#include "Entity/PhysicalObject/Actor/Vertebrates/Predator/AVP_Messages_Predator.h"
#include "Inventory/AVP_Inventory_HashIDs.h"
#include "Inventory/AVP_Inventory_Item.h"
#include "Inventory/AVP_Inventory_System.h"
#include "Mesh/AVP_PolygonFlags.h"
#include "Moves/Vertebrates/Predator/AVP_Predator_MovesJump.h"

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Behaviour_Predator_Movement
//
//////////////////////////////////////////////////////////////////////////////

const float AVP_Behaviour_Predator_Movement::s_fCollisionAvoidMinimumAngleDeg = 120.0f;	
const float AVP_Behaviour_Predator_Movement::s_fCollisionAvoidLocalScalar = 20.0f;
const float AVP_Behaviour_Predator_Movement::s_fCollisionAvoidRangePower = 2.5f;
const float AVP_Behaviour_Predator_Movement::s_fCollisionAvoidOverallScalar = 0.4f;

DEBUGVAR float dbg_fCloseToJumpPointSquared = 5.0f * 5.0f;
DEBUGVAR float dbg_fMoveAndJumpDistanceSquared = 2.0f * 2.0f;
DEBUGVAR float dbg_fFollowPointDistance = 2.0f;

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

AVP_Behaviour_Predator_Movement::AVP_Behaviour_Predator_Movement( AVP_Brain_Actor_Predator* pxBrain )
: PARENT( pxBrain, AVP_BEHAVIOUR_TYPE_PREDATOR_MOVEMENT )
, m_eCurrentState( UNSET )
, m_fTimeInState( 0.0f )
, m_fArrivalRange( 0.75f )
, m_xPrevPositionOnRoute( Asura_Maths::ZeroVector3 )
, m_xPositionOnRoute( Asura_Maths::ZeroVector3 )
, m_xFollowPoint( Asura_Maths::ZeroVector3 )
, m_usNextNextValidForJumpIndex(static_cast<u_short>(-1))
, m_usNextValidForJumpIndex(static_cast<u_short>(-1))
, m_xNextJumpPoint( Asura_Maths::ZeroVector3 )
, m_usLastValidForJumpIndex(static_cast<u_short>(-1))
, m_uMoveHash(ASURA_HASH_ID_UNSET)
, m_bStuck( false )
, m_xWPDistanceFromStartCache(WaypointIDCompareFunction)
, m_fRouteTraversedDistance(0.0f)
, m_fRouteTotalDistance(0.0f)
, m_bFreeAim(true)
, m_xAimTarget( Asura_Maths::ZeroVector3 )
, m_eLimitMoveMode( EMM_FAST )
, m_eRequestedMoveMode( EMM_IDLE )
, m_eActualMoveMode( EMM_IDLE )
, 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_bTryToUnstuckByJumping(false)
, m_iNextJumpVariation(-1)
, m_ePostJumpMoveMode(EMM_IDLE)
#ifndef ASURA_NO_DEBUG_OUTPUT
, m_xFollowSteering( Asura_Maths::ZeroVector3 )
, m_xAvoidanceSteering( Asura_Maths::ZeroVector3 )
, m_xFinalSteering( Asura_Maths::ZeroVector3 )
#endif
{
	Asura_Assert(GetPredatorBrain()->CanPlanMove(), "Starting a move not allowed now");
}

AVP_Behaviour_Predator_Movement::~AVP_Behaviour_Predator_Movement()
{
}



void AVP_Behaviour_Predator_Movement::Stop()
{
	AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetPredatorBrain()->GetActor()->GetAIAnimStateMachine() );
	pxVertStateMachine->RequestIdle();
	SwitchToState(FINISHED);
}

void AVP_Behaviour_Predator_Movement::AimAt(const Asura_Vector_3& xAimTarget)
{
	m_xAimTarget = xAimTarget;
	m_xAimTarget.Normalise();
	m_bFreeAim = false;
}
void AVP_Behaviour_Predator_Movement::AimWhereYouMove()
{
	m_bFreeAim = true;
}

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

bool AVP_Behaviour_Predator_Movement::HandleMessage( const Asura_Message* pxMessage )
{
	if (m_eCurrentState == TRAVERSING_MOVE_AND_JUMP_LINK )
	{
		if ( pxMessage->GetMsgID() == ASURA_MSG_MOVE_FINISHED && static_cast<const Asura_Msg_Move*>(pxMessage->GetData())->GetMoveID() == m_uMoveHash )
		{
			u_int eAnimStateMachineMove = AVP_Actor_AIAnimStateMachine::ACTOR_STATE_IDLE;
			switch(m_ePostJumpMoveMode)
			{
			case EMM_IDLE:
				eAnimStateMachineMove = AVP_Actor_AIAnimStateMachine::ACTOR_STATE_IDLE;
				break;
			case EMM_SLOW:
				eAnimStateMachineMove = AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_SLOW;
				break;
			case EMM_MEDIUM:
				eAnimStateMachineMove = AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_MEDIUM;
				break;
			case EMM_FAST:
				eAnimStateMachineMove = AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_FAST;
				break;
			}

			GetPredatorBrain()->GetActor()->GetAIAnimStateMachine()->RequestInMove(eAnimStateMachineMove);
		}
	}

	if ( m_eCurrentState == TRAVERSING_JUMP_LINK || m_eCurrentState == TRAVERSING_MOVE_AND_JUMP_LINK  )
	{
		switch (pxMessage->GetMsgID())
		{
		case ASURA_MSG_MOVECHAINCOMPLETE: // end of jump
		case ASURA_MSG_MOVE_ABORT:
			SwitchToState(TURN_AFTER_JUMP);
			return true;
		default:
			break;
		}
	}
	if (m_eCurrentState == RESOLVE_STUCK_BY_JUMPING)
	{
		switch (pxMessage->GetMsgID())
		{
		case ASURA_MSG_MOVECHAINCOMPLETE: // end of jump
		case ASURA_MSG_MOVE_ABORT:
			SetInError(true);
			SwitchToState(FINISHED);
			return true;
		default:
			break;
		}
	}

	return PARENT::HandleMessage( pxMessage );
}

bool AVP_Behaviour_Predator_Movement::Activate()
{
#ifndef ASURA_NO_DEBUG_OUTPUT
	GetPredatorBrain()->SetRenderRoute( true );
#endif

	return PARENT::Activate();
}

void AVP_Behaviour_Predator_Movement::Deactivate()
{
	// If the brain is not active this indicates we're shutting down. We don't access the actor/etc if this is the case
	// as the virtual table is unlikely to be valid any more.
	if ( GetPredatorBrain()->IsActive() )
	{
		GetPredatorBrain()->GetActor()->GetAIAnimStateMachine()->RequestIdle();
	}

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

	PARENT::Deactivate();
}

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

	m_fTimeInState += fTimeslice;

	if ( IsInError() || IsFinished() )
	{
		return;
	}

	UpdatePositionOnRoute();
	UpdateCurrentWaypoint();

	switch ( m_eCurrentState )
	{
	case TRAVERSING_STANDARD_LINK:
		UpdateStandardLinkTraverse(fTimeslice);
		break;
	case PREPARE_TO_JUMP:
		UpdatePrepareToJump(fTimeslice);
		break;
	case MOVING_TO_DESTINATION:
		UpdateMoveToDestination(fTimeslice);
		break;
	case TURN_AFTER_JUMP:
		UpdateTurnAfterJump(fTimeslice);
		break;
	case TRAVERSING_MOVE_AND_JUMP_LINK:
	case TRAVERSING_JUMP_LINK:
		UpdateJump(fTimeslice);
		break;
	case RESOLVE_STUCK_BY_JUMPING:
		break;
	case FINISHED:
		break;
	case UNSET:
		{
			ClearWPDistanceCache();
			AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
			if (xNavData.m_usRouteLength > 0)
			{
				m_fRouteTotalDistance = FindDistanceOfWPFromStart(xNavData.m_usRouteLength-1);
			}
			else
			{
				m_fRouteTotalDistance = 0.0f;
			}
		}
		SwitchToState(TRAVERSING_STANDARD_LINK);
		break;
	case NUM_STATES:
	default:
		Asura_Assert( false, "AVP_Behaviour_Predator_Movement::PerFrameUpdate - bad state" );
		SetInError();
		SetFinished();
		break;
	}
}

#ifndef ASURA_NO_DEBUG_OUTPUT
static const char* s_aszStateStrings[] =
{
	"UNSET",	
	"TRAVERSING_STANDARD_LINK",
	"PREPARE_TO_JUMP",
	"TRAVERSING_JUMP_LINK",
	"TRAVERSING_MOVE_AND_JUMP_LINK",
	"TURN_AFTER_JUMP",
	"MOVING_TO_DESTINATION",
	"RESOLVE_STUCK_BY_JUMPING",
	"FINISHED"
};

void AVP_Behaviour_Predator_Movement::RenderDebugInfo()
{
	PARENT::RenderDebugInfo();

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

	switch (m_eLimitMoveMode)
	{
	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( GetProjectBrain()->GetDebugOutputPos().x, GetProjectBrain()->GetDebugOutputPos().y, 0.5f, 0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING, "%s %c %c %c", s_aszStateStrings[ m_eCurrentState ], 
		cActualEMM, cRequestedEMM, cLimitedEMM);
	GetProjectBrain()->GetDebugOutputPos().y += 10.0f;


	Asura_DebugLineRender::AddSphere( m_xPositionOnRoute, 0.1f, 0xff0000ff, 4, 2 );
	Asura_DebugLineRender::AddSphere( m_xFollowPoint, 0.1f, 0xff00ff00, 4, 2 );

	Asura_Vector_3 xStart = GetPredatorBrain()->GetPredatorActor()->GetPosition();
	xStart.y -= 2.5f;
	Asura_Vector_3 xEnd;
	xEnd = xStart + m_xFollowSteering;
	Asura_DebugLineRender::AddArrow(xStart, xEnd, 0xff0000ff );
	xEnd = xStart + m_xAvoidanceSteering;
	Asura_DebugLineRender::AddArrow(xStart, xEnd, 0xff00ffff );
	xEnd = xStart + m_xFinalSteering;
	Asura_DebugLineRender::AddArrow(xStart, xEnd, 0xff00ff00 );
	u_int uCurrentAimColour = 0xff00ff00;
	u_int uDesiredAimColour = 0xff00aa00;
	if (m_eCurrentState != TRAVERSING_STANDARD_LINK && m_eCurrentState != TRAVERSING_STANDARD_LINK)
	{
		uCurrentAimColour = 0xff0000ff;
		uDesiredAimColour = 0xff0000aa;
	} 
	else if (!m_bFreeAim)
	{
		uCurrentAimColour = 0xffff0000;
		uDesiredAimColour = 0xffaa0000;
	}
	xStart.y -= 0.5f;
	xEnd = xStart + GetPredatorBrain()->GetAimDirection();
	Asura_DebugLineRender::AddArrow(xStart, xEnd, uCurrentAimColour);
	xEnd = xStart + GetPredatorBrain()->GetDesiredAimDirection();
	Asura_DebugLineRender::AddArrow(xStart, xEnd, uDesiredAimColour);
}
#endif

AVP_Brain_Actor_Predator* AVP_Behaviour_Predator_Movement::GetPredatorBrain() const
{
	return static_cast< AVP_Brain_Actor_Predator* >( GetProjectBrain() );
}

void AVP_Behaviour_Predator_Movement::SwitchToState( const State eNextState )
{
	// Do any work we need to exit our current state.
	switch ( m_eCurrentState )
	{
	case TRAVERSING_MOVE_AND_JUMP_LINK:
	case TRAVERSING_JUMP_LINK:
		GetPredatorBrain()->SetJumping(false);
		FindNextJumpIndex();
		m_uMoveHash = ASURA_HASH_ID_UNSET;
		break;
	case RESOLVE_STUCK_BY_JUMPING:
		GetPredatorBrain()->SetJumping(false);
		break;
	case TRAVERSING_STANDARD_LINK:
	case PREPARE_TO_JUMP:
	case TURN_AFTER_JUMP:
	case MOVING_TO_DESTINATION:
		break;
	case UNSET:
		FindNextJumpIndex();
		break;
	case FINISHED:
	case NUM_STATES:
	default:
		break;
	}

	// Do any work we need to change into our next state.
	m_eCurrentState = eNextState;
	m_fTimeInState  = 0.0f;
	switch ( m_eCurrentState )
	{
	case MOVING_TO_DESTINATION:
	case TRAVERSING_STANDARD_LINK:
		break;
	case PREPARE_TO_JUMP:
		break;
	case TURN_AFTER_JUMP:
		break;
	case FINISHED:
		SetFinished();
		break;
	case TRAVERSING_JUMP_LINK:
		GetPredatorBrain()->SetJumping(true);
		RequestJump();
		break;
	case RESOLVE_STUCK_BY_JUMPING:
		GetPredatorBrain()->SetJumping(true);
		break;
	case TRAVERSING_MOVE_AND_JUMP_LINK:
		GetPredatorBrain()->SetJumping(true);
		RequestMoveAndJump();
		break;
	case NUM_STATES:
	case UNSET:
	default:
		break;
	}
}

void AVP_Behaviour_Predator_Movement::UpdateFollowPoint()
{
	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	const Asura_Vector_3& xCurrentPosition = GetPredatorBrain()->GetActor()->GetPosition();
	u_short usFollowRouteIndex;
	FindNearestPointOnRoute(xNavData, xCurrentPosition, m_usNextValidForJumpIndex, dbg_fFollowPointDistance, m_xFollowPoint, usFollowRouteIndex );

	// if the follow point end up on the next jump segment then snap the follow point to the waypoint
	if (usFollowRouteIndex >= m_usNextValidForJumpIndex)
	{
		const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex] );
		Asura_Assert(pxWP, "Waypoint in route not found");
		if (pxWP)
		{
			m_xFollowPoint = pxWP->GetPosition();
		}
	}
}

void AVP_Behaviour_Predator_Movement::UpdatePositionOnRoute()
{
	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();

	m_xPrevPositionOnRoute = m_xPositionOnRoute;
	FindNearestPointOnRoute( xNavData, GetPredatorBrain()->GetActor()->GetPosition(), m_usNextValidForJumpIndex, 0.0f, m_xPositionOnRoute, xNavData.m_usRouteSegmentIndex );
	Asura_Assert( xNavData.m_usRouteSegmentIndex < xNavData.m_usRouteLength, "AVP_Behaviour_Predator_Movement::PerFrameUpdate - bug in FindNearestPointOnRoute" );

	m_fMaxRouteTraversedDistance = Asura_Maths::Max( m_fMaxRouteTraversedDistance, m_fRouteTraversedDistance);
}

void AVP_Behaviour_Predator_Movement::UpdateCurrentWaypoint()
{
	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	AVP_Navigation::TestParams xTestParams;
	xTestParams.m_uPathfinderFlags = xNavData.m_uPathfinderFlags;
	xTestParams.m_bRejectNonalignedWPs = true;
	xTestParams.m_usZone = ASURA_NAVVERTEX_MAX_ZONE_ID;
	u_short usCurrentWaypointID = xNavData.m_usCurrentWPID;
	AVP_Navigation::UpdateCurrentWaypointID( GetPredatorBrain()->GetActor()->GetPosition(), usCurrentWaypointID, xTestParams );
	if ( usCurrentWaypointID != ASURA_INVALID_WAYPOINT_ID )
	{
		xNavData.m_usCurrentWPID = usCurrentWaypointID;
	}
}


void AVP_Behaviour_Predator_Movement::UpdateStandardLinkTraverse(float fTimeslice)
{
	// If we're on the last link of our route and in our last waypoint then head towards our destination.
	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	if (AVP_Navigation::IsRouteBlocked(xNavData) )
	{
		SetInError(true);
		SwitchToState(FINISHED);
		return;
	}

	const Asura_AI_NavVertex* pxDestWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usDestinationWPID );
	Asura_Vector_3 xCurrentPosition = GetPredatorBrain()->GetActor()->GetPosition();
	if (pxDestWP->Contains(xCurrentPosition) || xNavData.m_usRouteLength <= 1)
	{
		SwitchToState(MOVING_TO_DESTINATION);
	}
	else
	{
		UpdateFollowPoint();

		if (NeedToRestartAfterFalling(fTimeslice))
		{
			SetInError(true);
			SwitchToState(FINISHED);
			return;
		}
	
		// if that segment wasn't jumped already
		if (m_usNextValidForJumpIndex < xNavData.m_usRouteLength )
		{
			const Asura_AI_NavVertex* pxNextJumpWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex] );

			if (xNavData.m_usRouteSegmentIndex == m_usNextValidForJumpIndex || ( xCurrentPosition - m_xNextJumpPoint).MagnitudeSquared() < dbg_fCloseToJumpPointSquared || pxNextJumpWP->Contains(xCurrentPosition)  )
			{
				SwitchToState(PREPARE_TO_JUMP);
				return;		
			}

		}
		// otherwise
		// calculate the steering direction
		// first the direction to the followpoint
		Asura_Vector_3 xFollowSteering = m_xFollowPoint - xCurrentPosition;
		xFollowSteering.Normalise();
		// next the avoidance steering
		Asura_Vector_3 xAvoidanceSteering = Asura_Maths::ZeroVector3;
		CalculateAvoidanceSteering(xAvoidanceSteering );
		bool bLargeAvoidanceSteering = false;
		if (xAvoidanceSteering.MagnitudeSquared() > 0.25 * xFollowSteering.MagnitudeSquared())
		{
			bLargeAvoidanceSteering = true;
		}

		// merge the two
		Asura_Vector_3 xFinalSteering = MergeSteering(xFollowSteering, xAvoidanceSteering);
		if (!xFinalSteering.ApproximatelyEquals( Asura_Maths::ZeroVector3 ))
		{
			xFinalSteering.Normalise();
			// for now look in the direction of the move
			Asura_Vector_3 xAim = xFinalSteering;
			if (xAim.y > 0.0f)
			{
				xAim.y = 0.0f;
			}
			xAim.Normalise();
			m_eLimitMoveMode = CalcLimitDueToTurn(xFinalSteering);

			if (m_bFreeAim)
			{
				SetDirectionAndSpeed(xFinalSteering, xAim );
			}
			else
			{
				SetDirectionAndSpeed(xFinalSteering, m_xAimTarget );
			}
		}

#ifndef ASURA_NO_DEBUG_OUTPUT
		m_xFollowSteering = xFollowSteering;
		m_xAvoidanceSteering = xAvoidanceSteering;
		m_xFinalSteering = xFinalSteering;
#endif

		UpdateStuckDetection(fTimeslice, bLargeAvoidanceSteering);
		if (m_bStuck && ! (m_bTryToUnstuckByJumping && AttemptJumpToUnStuck() ))
		{
			SetInError();
			SwitchToState( FINISHED );
		}
	}
}

DEBUGVAR float dbg_fSmallDistanceToDestination = 1.0f;

void AVP_Behaviour_Predator_Movement::UpdateMoveToDestination(float fTimeslice)
{
	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	Asura_Vector_3 xCurrentPosition =GetPredatorBrain()->GetActor()->GetPosition();
	// If we've arrived we're done.
	AVP_Navigation::APParams xAPParams;
	xAPParams.m_fMaxCoplanarOffset = m_fArrivalRange;
	xAPParams.m_xDestinationNormal = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usDestinationWPID )->GetNormal();
	if ( AVP_Navigation::AtPosition( xCurrentPosition, xNavData.m_xDestinationPosition, xAPParams ) )
	{
		SwitchToState( FINISHED );
		return;
	}

	// we were falling and now landed
	if (GetPredatorBrain()->JustFinishedFalling())
	{
		const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usDestinationWPID );
		Asura_Assert(pxWP, "Could not find waypoint while moving");
		if (!pxWP->Contains(xCurrentPosition))
		{
			SetInError(true);
			SwitchToState(FINISHED);
			return;
		}
	}

	Asura_Vector_3 xToDestination = xNavData.m_xDestinationPosition - xCurrentPosition;
	if (xToDestination.y > 0.0f)
	{
		xToDestination.y = 0.0f;
	}

	float fMagnitute = xToDestination.Normalise();

	if (fMagnitute < dbg_fSmallDistanceToDestination)
	{
		m_eLimitMoveMode = EMM_SLOW;
	}
	else
	{
		m_eLimitMoveMode = CalcLimitDueToTurn(xToDestination);
	}

	if (m_bFreeAim)
	{
		SetDirectionAndSpeed(xToDestination, xToDestination);
	}
	else
	{
		SetDirectionAndSpeed(xToDestination, m_xAimTarget);
	}


	UpdateStuckDetection(fTimeslice, false);
	if (m_bStuck && ! (m_bTryToUnstuckByJumping && AttemptJumpToUnStuck() ))
	{
		SetInError();
		SwitchToState( FINISHED );
		return;
	}


}


DEBUGVAR float dbg_fSmallTurnCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(5.0f));
DEBUGVAR float dbg_fBigTurnCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(45.0f));
DEBUGVAR float dbg_fVeryBigTurnCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(80.0f));

void AVP_Behaviour_Predator_Movement::UpdateTurnAfterJump(float fTimeslice)
{
	if (NeedToRestartAfterFalling(fTimeslice))
	{
		SetInError(true);
		SwitchToState(FINISHED);
		return;
	}

	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	const Asura_Vector_3 xCurrentPosition =GetPredatorBrain()->GetActor()->GetPosition();

	// if we arrived at the last waypoint and the move is not exact then end here
	if (m_usLastValidForJumpIndex > xNavData.m_usRouteLength - 3 )
	{
		SwitchToState(MOVING_TO_DESTINATION);
		return;
	}

	UpdateFollowPoint();

	Asura_Vector_3 xToDirection = m_xFollowPoint - xCurrentPosition;
	if (xToDirection.y > 0.0f)
	{
		xToDirection.y = 0.0f;
	}
	xToDirection.Normalise();
//	Asura_Matrix_3x3 xOrientation = GetPredatorBrain()->GetActor()->GetOrientation();
//	Asura_Vector_3 xForwardDirection(xOrientation.e13, xOrientation.e23, xOrientation.e33); // Z component of orientation point forward
	const Asura_Vector_3& xForwardDirection = GetPredatorBrain()->GetAimDirection();

	bool bigDifference = xForwardDirection * xToDirection < dbg_fSmallTurnCosine;

	if (!bigDifference || xToDirection.ApproximatelyEquals(Asura_Maths::ZeroVector3, 0.1f))
	{
		SwitchToState(TRAVERSING_STANDARD_LINK);
	}
	else
	{
		m_eLimitMoveMode = EMM_IDLE;
		SetDirectionAndSpeed(xToDirection, xToDirection);
	}
}

void AVP_Behaviour_Predator_Movement::UpdateJump(float)
{
	// for now update the look direction 
	LookToNextWaypoint();
}


DEBUGVAR float dbg_fMaxTurnOnJump = Asura_Maths::DegToRad(70.0f);
DEBUGVAR float dbg_fMaxTurnOnJumpCos = Asura_Maths::Cosine(dbg_fMaxTurnOnJump);


void AVP_Behaviour_Predator_Movement::UpdatePrepareToJump(float fTimeslice)
{

	if (GetPredatorBrain()->IsFalling())
	{
		return;
	}

	if (NeedToRestartAfterFalling(fTimeslice))
	{
		SetInError(true);
		SwitchToState(FINISHED);
		return;
	}


	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	const Asura_AI_NavVertex* pxJumpDestinationWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex + 1] );
	Asura_Assert(pxJumpDestinationWP, "Waypoint not found while traveling route");
	if (!pxJumpDestinationWP)
	{
		SetInError(true);
		SwitchToState(FINISHED);
	}

	const Asura_Vector_3 xStart = GetPredatorBrain()->GetActor()->GetPosition();

	if (( xStart- m_xNextJumpPoint).MagnitudeSquared() < dbg_fMoveAndJumpDistanceSquared)
	{
		SwitchToState(TRAVERSING_MOVE_AND_JUMP_LINK);
		return;
	}
	const Asura_Vector_3 xDestination = pxJumpDestinationWP->GetPosition();
	Asura_Vector_3 xDirection = xDestination - xStart;
	xDirection.SetY(0.0f); // ignore hight 
	xDirection.Normalise();
	// using orientation was suppose to be the correct test but because of Will it does not work!
	//Asura_Matrix_3x3 xOrientation = GetPredatorBrain()->GetActor()->GetOrientation(); 
	//Asura_Vector_3 xForwardDirection(xOrientation.e13, xOrientation.e23, xOrientation.e33); // Z component of orientation point forward
	Asura_Vector_3 xForwardDirection(GetPredatorBrain()->GetAimDirection()); 
	xForwardDirection.SetY(0.0f); // ignore hight 
	xForwardDirection.Normalise();

	bool bigDifference = xForwardDirection * xDirection < dbg_fSmallTurnCosine;

 	if (!bigDifference && GetJumpAnimVariation(xStart, xDestination, m_iNextJumpVariation))
 	{
 		SwitchToState(TRAVERSING_JUMP_LINK);
 	}
 	else
	{
		Asura_Vector_3 xMoveDirection = m_xNextJumpPoint - xStart;
		xMoveDirection.y = 0.0f;
		xMoveDirection.Normalise();
		m_eLimitMoveMode = CalcLimitDueToTurn(xMoveDirection);
		SetDirectionAndSpeed(xMoveDirection, xDirection);
	}
}

void AVP_Behaviour_Predator_Movement::RequestJump()
{
	Asura_Assert(m_uMoveHash == ASURA_HASH_ID_UNSET, "Starting a jump before the last jump finished");
	m_uMoveHash = ASURA_HASH_ID_UNSET;

	const AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex] );
	const Asura_AI_NavVertex* pxDestinationWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex + 1] );
	const Asura_Vector_3 xStartActual = GetPredatorBrain()->GetActor()->GetPosition();
	const Asura_Vector_3 xStartWP = pxCurrentWP->GetPosition();
	const Asura_Vector_3 xDestination = pxDestinationWP->GetPosition();

	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Predator_Move_JumpStart ) );

	Asura_Assert(pxMessage, "Unable to create message");
	if ( pxMessage != NULL )
	{
		m_uMoveHash = AVP_Move_Predator_JumpTarget::GetStaticHash();
		u_int uOwnerGuid = GetPredatorBrain()->GetActor()->GetGuid();


		AVP_Message_Predator_Move_JumpStart* pxData = static_cast< AVP_Message_Predator_Move_JumpStart* >( pxMessage->GetData() );

		// Fill-in the message-body...
		pxMessage->SetFromGuid(uOwnerGuid);
		pxMessage->SetToGuid(uOwnerGuid);

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

		const float fJumpDistSq = ( xDestination - xStartActual ).MagnitudeSquared();
		const AVP_ClientActor_Vertebrate::JumpLandType eJumpLandType = fJumpDistSq <= ( GetPredatorBrain()->GetClientActor()->GetMinDistanceForJumpLandNormalEffect() * GetPredatorBrain()->GetClientActor()->GetMinDistanceForJumpLandNormalEffect() )
																		? AVP_ClientActor_Vertebrate::JUMPLAND_LIGHT
																		: AVP_ClientActor_Vertebrate::JUMPLAND_NORMAL;

		pxData->SetJumpData(xDestination, eJumpLandType, m_iNextJumpVariation, 1.0f); // this is assumed to be correct as we should not get here without a succesfull calculation of jump variation

		ExMovementMode ePostJumpMM = CalcPostTurnMovementMode();
		u_int eAnimStateMachineMove = AVP_Actor_AIAnimStateMachine::ACTOR_STATE_IDLE;
		switch(ePostJumpMM)
		{
			case EMM_IDLE:
				eAnimStateMachineMove = AVP_Actor_AIAnimStateMachine::ACTOR_STATE_IDLE;
				break;
			case EMM_SLOW:
				eAnimStateMachineMove = AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_SLOW;
				break;
			case EMM_MEDIUM:
				eAnimStateMachineMove = AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_MEDIUM;
				break;
			case EMM_FAST:
				eAnimStateMachineMove = AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_FAST;
				break;

		}

		GetPredatorBrain()->GetActor()->GetAIAnimStateMachine()->RequestInMove(pxMessage, eAnimStateMachineMove);
	}
	else
	{
		SetInError(true);
		SwitchToState(FINISHED);
	}
}

void AVP_Behaviour_Predator_Movement::RequestMoveAndJump()
{
	Asura_Assert(m_uMoveHash == ASURA_HASH_ID_UNSET, "Starting a jump before the last jump finished");
	m_uMoveHash = ASURA_HASH_ID_UNSET;

	const AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex] );
	const Asura_AI_NavVertex* pxDestinationWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex + 1] );
	const Asura_Vector_3 xStartActual = GetPredatorBrain()->GetActor()->GetPosition();
	const Asura_Vector_3 xStartWP = pxCurrentWP->GetPosition();
	const Asura_Vector_3 xDestination = pxDestinationWP->GetPosition();

	int iTargetedJumpVariation;

	// try to find a jump from the waypoint - this should fail only if there is some dynamic object in the way
	if(!GetJumpAnimVariation(xStartWP, xDestination, iTargetedJumpVariation)) // failure to jump might happened if some dynamic object get in the way.
	{
		SetInError(true);
		SwitchToState(FINISHED);
		return;
	}

	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Predator_Move_WalkAndJumpStart ) );

	Asura_Assert(pxMessage, "Unable to create message");
	if ( pxMessage != NULL )
	{

		float fMoveSpeed = 1.0f;
		float fTurnSpeed = Asura_Maths::Pi;

		// force a move animation -
		AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetPredatorBrain()->GetActor()->GetAIAnimStateMachine() );
		switch ( GetPredatorBrain()->GetActor()->GetAIAnimStateMachine()->GetTargetState() )
		{
		case AVP_Actor_AIAnimStateMachine::ACTOR_STATE_IDLE:
			pxVertStateMachine->RequestMoveSlow();
			fMoveSpeed = 2.0f;
			fTurnSpeed = 1.5f * Asura_Maths::Pi;
			break;
		case AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_SLOW:
			pxVertStateMachine->RequestMoveSlow();
			fMoveSpeed = 2.0f;
			fTurnSpeed = 1.0f * Asura_Maths::Pi;
			break;
		default:
		case AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_MEDIUM:
		case AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_FAST:
			pxVertStateMachine->RequestMoveMedium();
			fMoveSpeed = 4.0f;
			fTurnSpeed = 0.75f * Asura_Maths::Pi;
			break;
		}

		m_uMoveHash = AVP_Move_Predator_PreJumpTarget::GetStaticHash();
		u_int uOwnerGuid = GetPredatorBrain()->GetActor()->GetGuid();

		AVP_Message_Predator_Move_WalkAndJumpStart* pxData = static_cast< AVP_Message_Predator_Move_WalkAndJumpStart* >( pxMessage->GetData() );

		// Fill-in the message-body...
		pxMessage->SetFromGuid(uOwnerGuid);
		pxMessage->SetToGuid(uOwnerGuid);

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

		const float fJumpDistSq = ( xDestination - xStartActual ).MagnitudeSquared();
		const AVP_ClientActor_Vertebrate::JumpLandType eJumpLandType = fJumpDistSq <= ( GetPredatorBrain()->GetClientActor()->GetMinDistanceForJumpLandNormalEffect() * GetPredatorBrain()->GetClientActor()->GetMinDistanceForJumpLandNormalEffect() )
																		? AVP_ClientActor_Vertebrate::JUMPLAND_LIGHT
																		: AVP_ClientActor_Vertebrate::JUMPLAND_NORMAL;

		pxData->SetJumpData(xStartWP, xDestination, eJumpLandType, iTargetedJumpVariation, 1.0f, fMoveSpeed, fTurnSpeed);

		m_ePostJumpMoveMode = CalcPostTurnMovementMode();

		// setting the state machine will start after the jump part start to execute

		//GetPredatorBrain()->GetActor()->GetAIAnimStateMachine()->RequestInMove(pxMessage, eAnimStateMachineMove);
		Asura_Network::Send( pxMessage );
	}
	else
	{
		SetInError(true);
		SwitchToState(FINISHED);
	}
}

// This algorithm is a copy of the one used to generate the jump links.
// Not very nice but it use different data.
bool AVP_Behaviour_Predator_Movement::GetJumpAnimVariation(const Asura_Vector_3& xFromPosition, const Asura_Vector_3& xToPosition, int& iVariation ) const
{

	iVariation = -1;

	const Asura_Blueprint* pxBlueprint = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESPREDATOR, 0x49c653e6 ); // "TargetedJumpSetWristblades"
	Asura_Assert(pxBlueprint, "Predator Blueprint not found");
	if (!pxBlueprint) 
	{
		return false; 
	}

	// To simplify our approximation, just use the y part of the head marker offset. Should still be good enough.
	// Since the jump may change our orientation and look values, attempting to be exact here is pretty much always going to fail.
// 	const AVP_ClientEntity_PhysicalObject* pxClient = AVP_ClientEntity_PhysicalObject::FindEntity(GetPredatorBrain()->GetActor()->GetGuid());
// 	Asura_Vector_3 xHeadOffset;
// 	if (!pxClient || !pxClient->GetMarkerPositionInRestPose(AVP_Vertebrate_Description::GetMarkerPointHashID(AVP_Vertebrate_Description::MPID_TARGET_2), &xHeadOffset, NULL))
// 	{
// 		Asura_Assert(false, "Head marker position not found");
// 		xHeadOffset.SetX(0.0f); xHeadOffset.SetY(-2.0f); xHeadOffset.SetZ(0.0f);  
// 	}
	Asura_Vector_3 xHeadOffset( 0.0f, -1.9f, 0.0f);  

	Asura_Vector_3 xStartPosition[2];
	xStartPosition[0] = xFromPosition  + Asura_Vector_3(0.0f, -0.2, 0.0f); // add small value to not hit the ground.
	xStartPosition[1] = xFromPosition + Asura_Vector_3(0.0f, xHeadOffset.y, 0.0f);
	Asura_Vector_3 xEndPosition[2];
	xEndPosition[0] = xToPosition  + Asura_Vector_3(0.0f, -0.2, 0.0f);
	xEndPosition[1] = xToPosition + Asura_Vector_3(0.0f, xHeadOffset.y, 0.0f);

	Asura_Value xValue;
	float fBestMaxFlatDistanceSquared = Asura_Maths::LargeFloat;
	float fBestMinHeightChange = 0.0f; // This will always be positive, regardless of the sign of the actual change.
	float fBestHeadroom = 0.0f;
	int iBestVariation = -1;

	Asura_Vector_3 xFlatOffset = xToPosition - xFromPosition;
	const float fHeightOffset = Asura_Maths::Abs(xFlatOffset.y);
	const float fHeightOffsetSign = Asura_Maths::Sign(xFlatOffset.y);
	xFlatOffset.y = 0.0f;
	const float fFlatDistanceSquared = xFlatOffset.MagnitudeSquared();

	// do the nominal test
	for (int i=0; i<pxBlueprint->GetNumParameters(); ++i)
	{
		if (pxBlueprint->GetParameterValueByIndex(i, xValue))
		{

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

			float fMaxFlatDistanceSquared;
			float fMinHeightChange;
			float fHeadroom;

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_MAXDISTANCE, xValue)) { continue; }
			fMaxFlatDistanceSquared = xValue.GetFloatValue();
			fMaxFlatDistanceSquared *= fMaxFlatDistanceSquared;

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_HEIGHTCHANGE, xValue)) { continue; }
			fMinHeightChange = xValue.GetFloatValue();

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_HEADROOM, xValue)) { continue; }
			fHeadroom = xValue.GetFloatValue();

			// First, make sure its height change is appropriate. 0 height change is always a valid option.
			if ((fMinHeightChange != 0.0f) && (fHeightOffsetSign != Asura_Maths::Sign(fMinHeightChange))) { continue; }
			fMinHeightChange = Asura_Maths::Abs(fMinHeightChange);
			if (fMinHeightChange < fBestMinHeightChange) { continue; }
			if (fMinHeightChange > fHeightOffset) { continue; }

			// Next, make sure it's within valid distance.
			if (fMaxFlatDistanceSquared < fFlatDistanceSquared) { continue; }
			// Only need to compare with the best if our height changes are equal since that's the priority comparison.
			if ((fMinHeightChange == fBestMinHeightChange) && (fMaxFlatDistanceSquared > fBestMaxFlatDistanceSquared)) { continue; }

			// Finally, the headroom check. We want the greatest clearance we think should be safe. We're rapidly approaching needing LOS tests...
			// Headroom is only a deciding factor if the other two values are equal to the previous best.
			if ((fMinHeightChange == fBestMinHeightChange) && (fMaxFlatDistanceSquared == fBestMaxFlatDistanceSquared) && (fHeadroom < fBestHeadroom)) { continue; }
			// We need two LOS tests to make a reasonable guess - from the current head position to an approximation of head position at the midpoint, and from there to the end head position.
			bool bFailed = false;
			Asura_Vector_3 xDir1 = ((xFromPosition + xToPosition) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f)) - xFromPosition;
			Asura_Vector_3 xDir2 = xToPosition - ((xFromPosition + xToPosition) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f));
			float fDistance1 = xDir1.Normalise();
			float fDistance2 = xDir2.Normalise();

			for (int j =0; j < 2 && !bFailed; ++j)
			{
				const Asura_Vector_3 xMidpoint = (xStartPosition[j] + xEndPosition[j]) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f);

				Asura_LOS_InputData xInput(xStartPosition[j], xDir1, GetPredatorBrain()->GetActor()->GetGuid(), fDistance1, false, AVP_PolygonFlag_IgnoreForPredatorCollision | AsuraPolygonFlag_NoCollisions);
				const Asura_LOS_OutputData* pxOutput = AVP_LOS::TestAgainstEnvironmentAndMoveableObjects(&xInput);
				if (pxOutput->m_fDistanceToIntersection < fDistance1) 
				{ 
					bFailed = true; 
					continue; 
				}

				xInput.m_xOrigin = xMidpoint;
				xInput.m_xDirection = xDir2;
				xInput.m_fInitialMaximumRange = fDistance2;
				pxOutput = Asura_LOS::TestAgainstEverything(&xInput);
				if (pxOutput->m_fDistanceToIntersection < fDistance2) 
				{ 
					bFailed = true;
				}
			}

			if (bFailed) { continue; }

			// Finally, this can be considered successful. Store the values.
			fBestMaxFlatDistanceSquared = fMaxFlatDistanceSquared;
			fBestMinHeightChange = fMinHeightChange;
			fBestHeadroom = fHeadroom;
			iBestVariation = i;
		}
	}

	if (iBestVariation >= 0)
	{
		iVariation = iBestVariation;
		return true;
	}

	// since the nominal test failed do a test ignoring distance limit and choose the variation with the longest jump possible
	fBestMaxFlatDistanceSquared =0.0f;
	for (int i=0; i<pxBlueprint->GetNumParameters(); ++i)
	{
		if (pxBlueprint->GetParameterValueByIndex(i, xValue))
		{
			const Asura_Blueprint* pxVariationBlueprint = Asura_Blueprint_System::GetBlueprintByHashID(uAVP_BLUEPRINT_TYPE_MOVESPREDATOR, xValue.GetHashIDValue());
			if (!pxVariationBlueprint) { continue; }

			float fMaxFlatDistanceSquared;
			float fMinHeightChange;
			float fHeadroom;

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_MAXDISTANCE, xValue)) { continue; }
			fMaxFlatDistanceSquared = xValue.GetFloatValue();
			fMaxFlatDistanceSquared *= fMaxFlatDistanceSquared;

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_HEIGHTCHANGE, xValue)) { continue; }
			fMinHeightChange = xValue.GetFloatValue();

			if (!pxVariationBlueprint->GetParameterValueByHashID(uAVP_BLUEPRINT_PARAM_HEADROOM, xValue)) { continue; }
			fHeadroom = xValue.GetFloatValue();

			// First, make sure its height change is appropriate. 0 height change is always a valid option.
			if ((fMinHeightChange != 0.0f) && (fHeightOffsetSign != Asura_Maths::Sign(fMinHeightChange))) { continue; }
			fMinHeightChange = Asura_Maths::Abs(fMinHeightChange);
			if (fMinHeightChange < fBestMinHeightChange) { continue; }
			if (fMinHeightChange > fHeightOffset) { continue; }

			// Ignore distance check
			// Now pick the largest distance distance 
			// Only need to compare with the best if our height changes are equal since that's the priority comparison.
			if ((fMinHeightChange == fBestMinHeightChange) && (fMaxFlatDistanceSquared < fBestMaxFlatDistanceSquared)) { continue; }

			if ((fMinHeightChange == fBestMinHeightChange) && (fMaxFlatDistanceSquared == fBestMaxFlatDistanceSquared) && (fHeadroom < fBestHeadroom)) { continue; }
			// We need two LOS tests to make a reasonable guess - from the current head position to an approximation of head position at the midpoint, and from there to the end head position.
			bool bFailed = false;
			Asura_Vector_3 xDir1 = ((xFromPosition + xToPosition) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f)) - xFromPosition;
			Asura_Vector_3 xDir2 = xToPosition - ((xFromPosition + xToPosition) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f));
			float fDistance1 = xDir1.Normalise();
			float fDistance2 = xDir2.Normalise();

			for (int j =0; j < 2 && !bFailed; ++j)
			{
				const Asura_Vector_3 xMidpoint = (xStartPosition[j] + xEndPosition[j]) * 0.5f + Asura_Vector_3(0.0f, -fHeadroom, 0.0f);

				Asura_LOS_InputData xInput(xStartPosition[j], xDir1, GetPredatorBrain()->GetActor()->GetGuid(), fDistance1, false, AVP_PolygonFlag_IgnoreForPredatorCollision | AsuraPolygonFlag_NoCollisions);
				const Asura_LOS_OutputData* pxOutput = Asura_LOS::TestAgainstEverything(&xInput);
				if (pxOutput->m_fDistanceToIntersection < fDistance1) 
				{ 
					bFailed = true; 
					continue; 
				}

				xInput.m_xOrigin = xMidpoint;
				xInput.m_xDirection = xDir2;
				xInput.m_fInitialMaximumRange = fDistance2;
				pxOutput = Asura_LOS::TestAgainstEverything(&xInput);
				if (pxOutput->m_fDistanceToIntersection < fDistance2) 
				{ 
					bFailed = true;
				}
			}

			if (bFailed) { continue; }

			// Finally, this can be considered successful. Store the values.
			fBestMaxFlatDistanceSquared = fMaxFlatDistanceSquared;
			fBestMinHeightChange = fMinHeightChange;
			fBestHeadroom = fHeadroom;
			iBestVariation = i;
		}
	}

	if (iBestVariation >= 0)
	{
		iVariation = iBestVariation;
		return true;
	}
	else
	{
		return false;
	}
}

void AVP_Behaviour_Predator_Movement::CalculateAvoidanceSteering( Asura_Vector_3& xSteering ) const
{

	xSteering = Asura_Maths::ZeroAVector3;
	const u_int uNumNearbyGuids = GetPredatorBrain()->GetNumNearbyGuids();
	if (uNumNearbyGuids == 0)
	{
		return;
	}
	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	const Asura_AI_NavVertex* pxNextWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex +1 ]);

	Asura_Assert(pxNextWP, "waypoint not found while moving on route");
	if (!pxNextWP)
	{
		return;
	}

	const float fToDestSq = ( GetPredatorBrain()->GetActor()->GetPosition() - xNavData.m_xDestinationPosition ).MagnitudeSquared();

	Asura_Vector_3 xOptimalDirection = pxNextWP->GetPosition() - m_xPositionOnRoute;
	xOptimalDirection.Normalise();
	const Asura_Vector_3 xOurNormal( -GetPredatorBrain()->GetActor()->GetCurrentGravityDirection() );

	// choose the closest obstacle 
	
	u_int uClosestGuid = uNumNearbyGuids;
	float fMinRange = Asura_Maths::LargeFloat;
	for ( u_int uNearbyGuid = 0; uNearbyGuid < uNumNearbyGuids; ++uNearbyGuid )
	{
		const AVP_Brain_Actor::NearbyGuidData& xNearbyGuidData = GetPredatorBrain()->GetNearbyGuidData()[ uNearbyGuid ];
		Asura_Vector_3 xFromGuidNormalized = xNearbyGuidData.m_xFromGuid;
		xFromGuidNormalized.Normalise();

		if (  fMinRange > xNearbyGuidData.m_fRange && 
			  xFromGuidNormalized * xOptimalDirection < Asura_Maths::Cosine(Asura_Maths::DegToRad(180.0f - s_fCollisionAvoidMinimumAngleDeg) ) )
		{
			fMinRange = GetPredatorBrain()->GetNearbyGuidData()[ uNearbyGuid ].m_fRange;
			uClosestGuid = uNearbyGuid;
		}
	}

	// nothing close and in our direction
	if (uNumNearbyGuids == uClosestGuid || fToDestSq < fMinRange * fMinRange)
	{
		return;
	}

	Asura_Vector_3 xFromVector = GetPredatorBrain()->GetNearbyGuidData()[ uClosestGuid ].m_xFromGuid;

	Asura_Vector_3 xPlaneVector = xFromVector ^ xOurNormal;
	xPlaneVector = xOurNormal ^ xPlaneVector;
	xPlaneVector.Normalise();

	// get only the perpend. part of the avoidance steering
	xPlaneVector -= xOptimalDirection * (xOptimalDirection * xPlaneVector) ;
	xPlaneVector.Normalise();

	xSteering = xPlaneVector * ( s_fCollisionAvoidLocalScalar / static_cast< float >( pow( fMinRange, s_fCollisionAvoidRangePower ) ) ) * s_fCollisionAvoidOverallScalar;
}


Asura_Vector_3 AVP_Behaviour_Predator_Movement::MergeSteering(const Asura_Vector_3& xToDirection, const Asura_Vector_3& xAvoidanceDirection) const
{

	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	if ( xAvoidanceDirection == Asura_Maths::ZeroVector3 )
	{
		return xToDirection;
	}

	Asura_Vector_3 xFinalSteering = ( xToDirection + xAvoidanceDirection ) * 0.5f;

	// check that we are not going to end outside of the waypoint map
	Asura_Vector_3 xCurrentPosition = GetPredatorBrain()->GetActor()->GetPosition();
	Asura_Vector_3 xFuturePosition = xCurrentPosition + xFinalSteering * 2.0f;
	const AVP_Navigation::TestParams xTestParams( xNavData.m_uPathfinderFlags, xNavData.m_usCurrentZone );
	u_short usWPID;
	if( !AVP_Navigation::GetNearestWaypointID( xFuturePosition, usWPID, xTestParams ) )
	{
		xFinalSteering = xToDirection;
	}
	else
	{   // if the future position is not inside the waypoint space - ignore the steering correction.
		// 
		if (!Asura_AI_Navigation_System::IsPositionInWaypoint(xFuturePosition, usWPID))
		{
			xFinalSteering = xToDirection;
		}
	}

	return xFinalSteering;
}

void AVP_Behaviour_Predator_Movement::LookToNextWaypoint()
{
// 	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
// 	Asura_Vector_3 xCurrentPosition =GetPredatorBrain()->GetActor()->GetPosition();
// 	const Asura_AI_NavVertex* pxDestinationWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[xNavData.m_usRouteSegmentIndex + 1] );
// 	Asura_Assert(pxDestinationWP, "No waypoint in the middle of a path");
// 	if (pxDestinationWP)
// 	{
// 		const Asura_Vector_3 xDestination = pxDestinationWP->GetPosition();
// 		Asura_Vector_3 xToFollowPoint = xDestination - xCurrentPosition; 
// 		if (xToFollowPoint.MagnitudeSquared() > 0)
// 		{
// 			xToFollowPoint.Normalise();
// 			Asura_Vector_3 xAim = xToFollowPoint;
// 			xAim.Normalise();
// 			m_eLimitMovementMode = EMM_IDLE;
// 			SetDirectionAndSpeed(xToFollowPoint, xAim);
// 		}
// 	}
}

void AVP_Behaviour_Predator_Movement::FindNextJumpIndex()
{

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

	if (m_usNextNextValidForJumpIndex == static_cast<u_short>(-1))
	{
		m_usNextValidForJumpIndex = FindJumpIndexStartingAt(0);
		m_usNextNextValidForJumpIndex = FindJumpIndexStartingAt(m_usNextValidForJumpIndex + 1);
		m_usLastValidForJumpIndex= 0;
	}
	else
	{
		m_usLastValidForJumpIndex = m_usNextValidForJumpIndex ;
		m_usNextValidForJumpIndex = m_usNextNextValidForJumpIndex;
		m_usNextNextValidForJumpIndex = FindJumpIndexStartingAt(m_usNextNextValidForJumpIndex + 1);
	}

	if (m_usNextValidForJumpIndex < xNavData.m_usRouteLength)
	{
		const Asura_AI_NavVertex* pxNextJumpWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex] );
		m_xNextJumpPoint = pxNextJumpWP->GetPosition();
	}
	else
	{
		m_xNextJumpPoint = Asura_Maths::ZeroVector3;
	}
}

u_short AVP_Behaviour_Predator_Movement::FindJumpIndexStartingAt(u_short uStartIndex)
{
	const AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	u_short uIndex = uStartIndex;
	while (uIndex < xNavData.m_usRouteLength -1 )
	{
		const Asura_AI_NavVertex* pxCurentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[uIndex] );
		Asura_Assert(pxCurentWP, "Waypoint in route not found");
		if (!pxCurentWP)
		{
			SetInError(true);
			SwitchToState(FINISHED);
			return xNavData.m_usRouteLength;
		}

		const Asura_AI_NavConnection* pxCurrentLink =  pxCurentWP->GetLinkTo(xNavData.m_pusRoute[uIndex + 1]);
		// link might not exist because of smothing (but the jump links always remain.
		if (pxCurrentLink && pxCurrentLink->GetFlags() & AVP_Navigation::AVP_AI_CONNECTION_FLAG_TRANSITION_HIGH_MOBILITY)
		{				
			return uIndex;
		}
		++uIndex;
	}

	return xNavData.m_usRouteLength;
}


void AVP_Behaviour_Predator_Movement::SetDirectionAndSpeed(const Asura_Vector_3& xMoveDirection, const Asura_Vector_3& xAimDirection)
{
	AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetPredatorBrain()->GetActor()->GetAIAnimStateMachine() );
	const AVP_ServerActor_Predator* pxActor = GetPredatorBrain()->GetPredatorActor();
	bool bIsIdle = pxActor->GetAIAnimStateMachine()->IsIdle();

	GetPredatorBrain()->SetAimDirection( xAimDirection );
	GetPredatorBrain()->SetLookAtPosition( pxActor->GetPosition() + xAimDirection * 10.0f );

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

	bool bToMove = m_eRequestedMoveMode != EMM_IDLE;

	if (bToMove)
	{
		if (bIsIdle)
		{
			GetPredatorBrain()->ForceMovementDirection( xMoveDirection );
		}
		else
		{
			GetPredatorBrain()->SetMovementDirection( xMoveDirection );
		}
	}

	switch ( m_eActualMoveMode )
	{
	case EMM_SLOW:
		pxVertStateMachine->RequestMoveSlow();
		GetPredatorBrain()->SetMaxMoveDirAngularVelocity( 1.0f * Asura_Maths::Pi );
		GetPredatorBrain()->SetMaxAimDirAngularVelocity( 1.0f * Asura_Maths::Pi );
		break;
	default:
	case EMM_MEDIUM:
		GetPredatorBrain()->SetMaxMoveDirAngularVelocity( 0.9f * Asura_Maths::Pi );
		GetPredatorBrain()->SetMaxAimDirAngularVelocity( 0.9f * Asura_Maths::Pi );
		pxVertStateMachine->RequestMoveMedium();
		break;
	case EMM_FAST:
		GetPredatorBrain()->SetMaxMoveDirAngularVelocity( 0.75f * Asura_Maths::Pi );
		GetPredatorBrain()->SetMaxAimDirAngularVelocity( 0.75f * Asura_Maths::Pi );
		pxVertStateMachine->RequestMoveFast();
		break;
	case EMM_IDLE:
		pxVertStateMachine->RequestIdle();
		GetPredatorBrain()->SetMaxMoveDirAngularVelocity( 1.5f * Asura_Maths::Pi );
		GetPredatorBrain()->SetMaxAimDirAngularVelocity( 1.5f * Asura_Maths::Pi );
		break;
	} // end switch MovementMode

	
}

DEBUGVAR float dbg_fTestTimeScaleAtStartOfPath = 3.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 AVP_Behaviour_Predator_Movement::UpdateStuckDetection( const float fTimeslice, bool bLargeAvoidanceSteering )
{
	if (m_bStuck)
	{
		return;
	}

	// if we in the middle of the flinch then we are not stuck
	if (GetPredatorBrain()->GetPredatorActor()->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 = GetPredatorBrain()->GetActor()->GetPosition();
			Asura_OutputToDebugger(("%d Predator %d stuck - did not advance on its path for %f seconds and %f frames, position %.2f,%.2f,%.2f\n",
				Asura_Timers::GetFrameNumber(), GetPredatorBrain()->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 = GetPredatorBrain()->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 = GetPredatorBrain()->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 = GetPredatorBrain()->GetActor()->GetPosition();
			Asura_OutputToDebugger(("%d Predator %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(), GetPredatorBrain()->GetActor()->GetGuid(), m_fTimeCheckOrientationAndPositionStuck, m_fFramesCheckOrientationAndPositionStuck,
				xPosition.x, xPosition.y, xPosition.z));
		}
#endif
		m_fFramesCheckOrientationAndPositionStuck = 0.0f;
		m_fTimeCheckOrientationAndPositionStuck = 0.0f;
	}
}

DEBUGVAR float dbg_fMaxJumpDistanceSquared = 8.0f*8.0f;

bool AVP_Behaviour_Predator_Movement::AttemptJumpToUnStuck()
{

	const AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
	const Asura_Vector_3& xCurrentPosition = GetPredatorBrain()->GetActor()->GetPosition();

	const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usCurrentWPID );
	if (!pxCurrentWP)
	{
		return false;
	}
	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       = GetPredatorBrain()->GetActor()->GetPosition();
		xTempNavData.m_usDestinationWPID	= usDestinationWPID;
		xTempNavData.m_xDestinationPosition = pxDestinationWP->GetPosition();

		AVP_Navigation::FaceHuggerAStarFunctor xFunctor;		
		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( xNavData.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;
	}

	// Fire the jump move off.

	int iTargetedJumpVariation = 0;
	const Asura_Vector_3& xStart = GetPredatorBrain()->GetActor()->GetPosition();
	const Asura_Vector_3& xDestination = pxDestinationWP->GetPosition();

	const float fJumpDistSq = ( xDestination - xStart ).MagnitudeSquared();
	const AVP_ClientActor_Vertebrate::JumpLandType eJumpLandType = fJumpDistSq <= ( GetPredatorBrain()->GetClientActor()->GetMinDistanceForJumpLandNormalEffect() * GetPredatorBrain()->GetClientActor()->GetMinDistanceForJumpLandNormalEffect() )
																	? AVP_ClientActor_Vertebrate::JUMPLAND_LIGHT
																	: AVP_ClientActor_Vertebrate::JUMPLAND_NORMAL;

	
	if(!GetJumpAnimVariation(xStart, xDestination, iTargetedJumpVariation))  // first try to find a better jump from our actual position
	{
		// well there is no guarantee that there is any good jump - so just fall back to the good old variation 0 
		iTargetedJumpVariation = 0;
	}

	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Predator_Move_JumpStart ) );

	Asura_Assert(pxMessage, "Unable to create message");
	if ( pxMessage != NULL )
	{
		m_uMoveHash = AVP_Move_Predator_JumpTarget::GetStaticHash();
		u_int uOwnerGuid = GetPredatorBrain()->GetActor()->GetGuid();


		AVP_Message_Predator_Move_JumpStart* pxData = static_cast< AVP_Message_Predator_Move_JumpStart* >( pxMessage->GetData() );

		// Fill-in the message-body...
		pxMessage->SetFromGuid(uOwnerGuid);
		pxMessage->SetToGuid(uOwnerGuid);

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

		pxData->SetJumpData(xDestination, eJumpLandType, iTargetedJumpVariation, 1.0f);

		// Forward the message directly to the client...
		Asura_Network::Send( pxMessage );
		SwitchToState( RESOLVE_STUCK_BY_JUMPING );
		return true;
	}
	else
	{
		return false;
	}
}


bool AVP_Behaviour_Predator_Movement::NeedToRestartAfterFalling( float fTimeslice )
{
	// we were falling and now landed
	// if we landed outside the next waypoint be conservative and fail the movement 
	// let higher level figure it out.
	if (GetPredatorBrain()->JustFinishedFalling())
	{
		AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
		const Asura_Vector_3& xCurrentPosition = GetPredatorBrain()->GetActor()->GetPosition();
		const Asura_AI_NavVertex* pxWP = NULL;
		if (xNavData.m_usRouteSegmentIndex+1 < xNavData.m_usRouteLength)
		{
			pxWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[xNavData.m_usRouteSegmentIndex+1] );
		}
		else
		{
			pxWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[xNavData.m_usRouteLength-1] );
		}
		Asura_Assert(pxWP, "Could not find waypoint while moving");
		if (!pxWP ||  !pxWP->Contains(xCurrentPosition))
		{
			return true;
		}
	}

	return false; 
}

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

}

float AVP_Behaviour_Predator_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 = GetPredatorBrain()->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_Behaviour_Predator_Movement::FindNearestPointOnRoute( const AVP_Navigation::NavData& xNavData, const Asura_Vector_3& xCurrentPosition,
															  u_short usBlockIndex, float fLookAheadDistance, Asura_Vector_3& xNearestPosition, u_short& usNearestRouteSegmentIndex )
{

	m_fRouteTraversedDistance = 0.0f;

	const Asura_Vector_3 xUp(0.0f, -1.0f, 0.0f);

	if ( xNavData.m_usRouteLength == 0 )
	{
		Asura_Assert( false, "AVP_Behaviour_Predator_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_Behaviour_Predator_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;
		usSegmentIndex < xNavData.m_usRouteLength - 1 && usSegmentIndex < usBlockIndex;
		++usSegmentIndex )
	{
		const Asura_AI_NavVertex* pxCurrentWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ usSegmentIndex ] );
		xCurrentSegmentStart = pxCurrentWP->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, xUp );
	Asura_Vector_3 xCurrentSegmentStartInOurPlane;
	Asura_Vector_3 xCurrentSegmentEndInOurPlane;
	xOurPlane.CalculateClosestPoint(xCurrentSegmentStart, xCurrentSegmentStartInOurPlane);
	xOurPlane.CalculateClosestPoint(xCurrentSegmentEnd, xCurrentSegmentEndInOurPlane);

	float fLamda;

	if (xCurrentSegmentStartInOurPlane.ApproximatelyEquals(xCurrentSegmentEndInOurPlane)) // if the projection is almost a point select the end segment as our position
	{
		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();

	// These always pass first time, because we always want to move along the segment we're currently on.
	bool bDone = false;
	while ( fLookAheadDistance > 0.0f && !bDone )
	{
		if (usNearestRouteSegmentIndex == usBlockIndex)
		{
			bDone = true;
		}
		else
		{
			const Asura_AI_NavConnection* pxSegmentLink = pxFromWP->GetLinkTo( xNavData.m_pusRoute[ usNearestRouteSegmentIndex + 1 ] );
			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 ] );
				}
				else
				{
					usNearestRouteSegmentIndex = xNavData.m_usRouteLength - 1;
					xNearestPosition = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_usDestinationWPID )->m_xPosition;
					return true;
				}
			}
		}
	}

	return false;
}

AVP_Behaviour_Predator_Movement::ExMovementMode AVP_Behaviour_Predator_Movement::CalcLimitDueToTurn(const Asura_Vector_3& xDesiredDirection ) const
{
	Asura_Vector_3 xCurrentDirection = GetPredatorBrain()->GetMovementDirection();
	Asura_Vector_3 xFlatDesiredDirection = xDesiredDirection; 
	xFlatDesiredDirection.y = 0.0f;
	xCurrentDirection.y = 0.0f;
	if (xCurrentDirection * xFlatDesiredDirection < dbg_fVeryBigTurnCosine) // big turn 
	{
		return EMM_SLOW;
	}
	else if (xCurrentDirection * xFlatDesiredDirection < dbg_fBigTurnCosine) // big turn 
	{
		return EMM_MEDIUM;
	}
	else
	{
		return EMM_FAST;
	}

}

AVP_Behaviour_Predator_Movement::ExMovementMode AVP_Behaviour_Predator_Movement::CalcPostTurnMovementMode()
{

	return EMM_IDLE;

// 	AVP_Navigation::NavData& xNavData = GetPredatorBrain()->GetNavData();
// 
// 	Asura_Assert(m_usNextValidForJumpIndex < xNavData.m_usRouteLength-1, " CalcPostTurnMovementMode called with no turn or invalid turn index");
// 	if (m_usNextValidForJumpIndex >= xNavData.m_usRouteLength-1)
// 	{
// 		return EMM_IDLE;
// 	}
// 
// 	if (m_usNextValidForJumpIndex + 1 == m_usNextNextValidForJumpIndex) // jump after jump
// 	{
// 		return EMM_IDLE;
// 	}
// 
// 	const Asura_AI_NavVertex* pxLandWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextValidForJumpIndex + 1 ] );
// 	const Asura_Vector_3& xLandPosition = pxLandWP->GetPosition();
// 	Asura_Vector_3 xPostLandingFollowPoint;
// 	u_short usFollowRouteIndex;
// 	FindNearestPointOnRoute(xNavData, xLandPosition, m_usNextNextValidForJumpIndex, dbg_fFollowPointDistance, xPostLandingFollowPoint, usFollowRouteIndex );
// 
// 	if (usFollowRouteIndex >= xNavData.m_usRouteLength-1)
// 	{
// 		return EMM_IDLE;
// 	}
// 
// 	if (usFollowRouteIndex >= m_usNextNextValidForJumpIndex)
// 	{
// 		const Asura_AI_NavVertex* pxWP = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[m_usNextNextValidForJumpIndex] );
// 		Asura_Assert(pxWP, "Waypoint in route not found");
// 		if (pxWP)
// 		{
// 			xPostLandingFollowPoint = pxWP->GetPosition();
// 		}
// 	}
// 
// 
// 	Asura_Vector_3 xFlatJumpDir = pxLandWP->GetPosition()-GetPredatorBrain()->GetActor()->GetPosition();
// 	xFlatJumpDir.y = 0.0f;
// 	xFlatJumpDir.Normalise();
// 	Asura_Vector_3 xFlatPostJumpDir = xPostLandingFollowPoint-xLandPosition;
// 	xFlatPostJumpDir.y = 0.0f;
// 	xFlatPostJumpDir.Normalise();
// 
// 	if (xFlatJumpDir * xFlatPostJumpDir < dbg_fVeryBigTurnCosine) // big turn 
// 	{
// 		return EMM_IDLE;
// 	}
// 	else if (xFlatJumpDir * xFlatPostJumpDir < dbg_fBigTurnCosine) // big turn 
// 	{
// 		return EMM_SLOW;
// 	}
// 	else
// 	{
// 		return EMM_MEDIUM;
// 	}
}



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