//////////////////////////////////////////////////////////////////////////////
//
// AVP_Behaviour_Human_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/Human/AVP_Behaviour_Human_Movement.h"

// asura
#ifndef ASURA_NO_DEBUG_OUTPUT
#include "Rendering/Asura_Font_System.h"
#include "Debugging/Asura_DebugLineRender.h"
#endif
#include "Entity/Locations/Spline/GamesceneSpline/Asura_ServerNode_GS2Spline.h"
#include "Entity/Locations/OrientedNode/GamesceneNode/Asura_ServerNode_GS2Node.h"
#include "Entity/PhysicalObject/Moves/Asura_Movement_System.h"

// axon
#include "Behaviour/BehaviourTypes/AVP_Behaviour_Types.h"
#include "Behaviour/BehaviourParamsTypes/Axon_BehaviourParams_GS2Node.h"
#include "Core/BrainTypes/AVP_Brain_Actor.h"

// avp
#include "Actor/AVP_Actor_AnimStateData.h"
#include "Actor/Vertebrates/Human/AVP_Human_AIAnimStateData.h"
#include "Actor/Vertebrates/Human/AVP_Human_AIAnimStateMachine.h"
#include "Entity/PhysicalObject/Actor/Vertebrates/Human/AVP_ServerActor_Human.h"
#include "Inventory/AVP_Inventory_Group.h"
#include "Inventory/AVP_Inventory_HashIDs.h"
#include "Inventory/AVP_Inventory_Item.h"
#include "Inventory/AVP_Inventory_System.h"
#include "Moves/Vertebrates/Human/AVP_Human_MovesGamescene.h"


//////////////////////////////////////////////////////////////////////////////
//
// AVP_Behaviour_Human_Movement
//
//////////////////////////////////////////////////////////////////////////////
AVP_Behaviour_Human_Movement::AVP_Behaviour_Human_Movement( AVP_Brain_Actor* pxBrain )
: PARENT( pxBrain, AVP_BEHAVIOUR_TYPE_HUMAN_MOVEMENT )
, m_eCurrentState( UNSET )
, m_fTimeInState( 0.0f )
, m_fArrivalRange( 0.75f )
, m_xPrevPositionOnRoute( Asura_Maths::ZeroVector3 )
, m_fSmoothedPORVelocity( 0.0f )
, m_xPositionOnRoute( Asura_Maths::ZeroVector3 )
, m_xFollowPoint( Asura_Maths::ZeroVector3 )
, m_bFirstTime( false )
, m_uGS2NodeGuid( ASURA_GUID_UNREGISTERED )
, m_xPostGS2Direction( Asura_Maths::ZeroVector3 )
, m_xEndGS2Position( Asura_Maths::ZeroVector3 )
, m_bNeedToEnterNode(false)
, m_bStopAtEnd(true)
, m_bMoveModeOverriden( false )
{
}

void AVP_Behaviour_Human_Movement::SetDestinationGS2NodeGuid( Asura_Guid uGS2NodeGuid )
{
	m_uGS2NodeGuid = uGS2NodeGuid;

	const Asura_ServerNode_GS2Node* pxEndNode = Asura_ServerNode_GS2Node::FindEntity(uGS2NodeGuid);
	Asura_Assert(pxEndNode, "GS2 Node Not Found");
	if (!pxEndNode || pxEndNode->GetNumberLinks() == 0 )  // if end node is the last node on the GS2 series
	{
		m_bNeedToEnterNode = true;
		return;
	}

	if (pxEndNode->GetBehaviourParams() && // if the node has a behaviour and its not the core one or it is the core one but marked as stop here
		( (pxEndNode->GetBehaviourParams()->GetBehaviourType() != AXON_BEHAVIOUR_TYPE_GS2_NODE) ||
		  static_cast< const Axon_BehaviourParams_GS2Node*>(pxEndNode->GetBehaviourParams())->m_bStopHere) ) 
	{
		m_bNeedToEnterNode = true;
		return;
	}

	Asura_Assert(pxEndNode->GetNumberLinks() == 1, "Expected only 1 link over smoeth over GS2 node" );

	const Asura_ServerNode_GS2Spline* pxEndSpline = Asura_ServerNode_GS2Spline::FindEntity(pxEndNode->GetLink(0));
	Asura_Assert(pxEndSpline, "GS2 Spline Not Found");
	if (!pxEndSpline)
	{
		m_bNeedToEnterNode = true;
		return;
	}

	const Asura_ServerNode_GS2Node* pxPostEndNode = Asura_ServerNode_GS2Node::FindEntity(pxEndSpline->GetTargetGuid());
	Asura_Assert(pxEndSpline, "GS2 Node Not Found");
	if (!pxEndSpline)
	{
		m_bNeedToEnterNode = true;
		return;
	}

	m_bNeedToEnterNode = false;
	m_bStopAtEnd = false;
	m_xPostGS2Direction = pxPostEndNode->GetPosition() - pxEndNode->GetPosition();
	m_xPostGS2Direction.Normalise();
	m_xEndGS2Position = pxEndNode->GetPosition();
	m_xPostGS2Direction.Normalise();
}

bool AVP_Behaviour_Human_Movement::GetToPosition(const Asura_Vector_3& xDestination, MovementMode m_eMovementMode /*= AVP_Navigation::MM_MEDIUM*/, bool bOverrideMovementMode /*= false*/)
{
	SetInError( false );
	SetFinished( false );

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

	m_bMoveModeOverriden = bOverrideMovementMode;

	if (m_bMoveModeOverriden)
	{
		xNavData.m_eMovementMode = m_eMovementMode;
	}

	xNavData.m_xStartPosition       = GetActorBrain()->GetActor()->GetPosition();
	xNavData.m_xDestinationPosition = xDestination;
	// Our current waypoint ID should already be up to date, as we keep it up to date when moving.
	// Update our destination waypoint ID.
	const AVP_Navigation::TestParams xTestParams( xNavData.m_uPathfinderFlags, xNavData.m_usCurrentZone );
	if ( !AVP_Navigation::GetNearestWaypointID( xDestination, xNavData.m_usDestinationWPID, xTestParams ) )
	{
		SetInError();
		SwitchToState( FINISHED );
		return false;
	}

	if ( ( xNavData.m_xStartPosition - xNavData.m_xDestinationPosition ).MagnitudeSquared() < m_fArrivalRange * m_fArrivalRange )
	{
		// If we're already near enough the destination then there's nothing to do.
		SwitchToState( FINISHED );
	}
	else
	{
		// Clear out the old route and find the new one.
		xNavData.ResetRoute();
		if ( !AVP_Navigation::FindRoute( xNavData ) )
		{
			SetInError();
			SwitchToState( FINISHED );
			return false;
		}

		m_xPrevPositionOnRoute = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ 0 ] )->m_xPosition;
		m_fSmoothedPORVelocity = 5.0f;

		SwitchToState( TRAVERSING_STANDARD_LINK );
	}

	m_bFirstTime = true;

	return true;
}

bool AVP_Behaviour_Human_Movement::Activate()
{
#ifndef ASURA_NO_DEBUG_OUTPUT
	GetActorBrain()->SetRenderRoute( true );
#endif

	return PARENT::Activate();
}

void AVP_Behaviour_Human_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 ( GetActorBrain()->IsActive() )
	{
		if ( m_bStopAtEnd )
		{
			GetActorBrain()->GetActor()->GetAIAnimStateMachine()->RequestIdle();
		}
	}

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

	PARENT::Deactivate();
}

void AVP_Behaviour_Human_Movement::PostLoadInitialise()
{
	PARENT::PostLoadInitialise();

	// Unfortunately the anim state machine is not created until PLI time.
	// This is far from ideal, and must be compensated for by moving this code here from the end of the chunk loading function.
	AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetActorBrain()->GetActor()->GetAIAnimStateMachine() );
	switch ( GetActorBrain()->GetNavData().m_eMovementMode )
	{
		case AVP_Navigation::MM_SLOW:
		{
			pxVertStateMachine->RequestMoveSlow();
			break;
		}

		default:
		case AVP_Navigation::MM_MEDIUM:
		{
			pxVertStateMachine->RequestMoveMedium();
			break;
		}

		case AVP_Navigation::MM_FAST:
		{
			pxVertStateMachine->RequestMoveFast();
			break;
		}
	}
}

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

	switch ( m_eCurrentState )
	{
	case TRAVERSING_STANDARD_LINK:
	case MOVING_TO_DESTINATION:
	case MOUNT_GAMESCENE:
	case FINISHED:
		break;

	case UNSET:
	case NUM_STATES:
	default:
		{
			Asura_Assert( false, "AVP_Behaviour_Human_Movement::PerFrameUpdate - bad state" );
			SetInError();
			SetFinished();
			break;
		}
	}
}

DEBUGVAR float dbg_fLargetTurnCos = Asura_Maths::Cosine(Asura_Maths::DegToRad(60.0f));

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

	m_fTimeInState += fTimeslice;

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

	UpdatePositionOnRoute();
	UpdateStuckDetection( fTimeslice );

	switch ( m_eCurrentState )
	{
		case TRAVERSING_STANDARD_LINK:
		{
			// If we're on the last link of our route and in our last waypoint then head towards our destination.
			AVP_Navigation::NavData& xNavData = GetActorBrain()->GetNavData();
			if ( ( xNavData.m_usRouteLength == 1 || xNavData.m_usRouteSegmentIndex == xNavData.m_usRouteLength - 2 ) &&
				xNavData.m_usCurrentWPID == xNavData.m_usDestinationWPID )
			{
				SwitchToState( MOVING_TO_DESTINATION );
			}
			else
			{
				if ( !UpdateFollowPoint() )
				{
					SwitchToState( MOVING_TO_DESTINATION );
				}

				Asura_Vector_3 xToFollowPoint = m_xFollowPoint - GetActorBrain()->GetActor()->GetPosition();
				Asura_Vector_3 xAvoidanceSteering = Asura_Maths::ZeroVector3;
				CalculateAvoidanceSteering(xToFollowPoint, xAvoidanceSteering);
				xToFollowPoint += xAvoidanceSteering;
				xToFollowPoint.Normalise();

				if ( xNavData.m_usRouteSegmentIndex <= xNavData.m_usRouteLength - 2 )
				{
					if ( !Asura_AI_Navigation_System::IsPositionInWaypoint( GetActorBrain()->GetActor()->GetPosition() + xToFollowPoint, xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex ] ) && !Asura_AI_Navigation_System::IsPositionInWaypoint( GetActorBrain()->GetActor()->GetPosition() + xToFollowPoint, xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex + 1 ] ) )
					{
						Asura_AI_NavVertex* pxWaypoint = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex ] );
						if ( pxWaypoint )
						{
							Asura_Vector_3 xDirToCurrent = pxWaypoint->GetPosition() - GetActorBrain()->GetActor()->GetPosition();
							xDirToCurrent.Normalise();

							Asura_AI_NavVertex* pxNextWaypoint = Asura_AI_Navigation_System::GetWaypoint( xNavData.m_pusRoute[ xNavData.m_usRouteSegmentIndex + 1] );

							if ( pxNextWaypoint )
							{
								Asura_Vector_3 xDirToNext = pxNextWaypoint->GetPosition() - GetActorBrain()->GetActor()->GetPosition();
								xDirToNext.Normalise();

								float fDot = xDirToCurrent * xDirToNext;

								if ( fDot > 0.0f )
								{
									xToFollowPoint.x = xDirToCurrent.x;
									xToFollowPoint.Normalise();
								}
							}
						}
					}
				}

				if ( m_uGS2NodeGuid == ASURA_GUID_UNREGISTERED )
				{
					if ( m_bFirstTime )
					{
						m_bFirstTime = false;
						GetActorBrain()->ForceMovementDirection( xToFollowPoint );
						GetActorBrain()->ForceLookAtPosition( m_xFollowPoint );
						GetActorBrain()->ForceAimDirection( xToFollowPoint );
					}
					else
					{
						GetActorBrain()->SetMovementDirection( xToFollowPoint );
						GetActorBrain()->SetLookAtPosition(m_xFollowPoint);
						GetActorBrain()->SetAimDirection(xToFollowPoint);
					}
				}
				else
				{
					if ( m_uGS2NodeGuid != ASURA_GUID_UNREGISTERED )
					{
						Asura_ServerNode_GS2Node* pxGS2Node = Asura_ServerNode_GS2Node::FindEntity( m_uGS2NodeGuid );
						AVP_ServerActor_Human* pxHuman = (AVP_ServerActor_Human*)GetActorBrain()->GetActor();

						if ( pxGS2Node && pxHuman )
						{
							float fDistanceSq = ( pxGS2Node->GetPosition() - pxHuman->GetPosition() ).MagnitudeSquared();

							if ( !m_bNeedToEnterNode && fDistanceSq < 2.5f )
							{
								GetActorBrain()->ForceMovementDirection( m_xPostGS2Direction );
								SwitchToState( FINISHED );
								return;
							}
							else if ( !m_bNeedToEnterNode &&  fDistanceSq < 4.5f )
							{
								Asura_Vector_3 xDirToGS = m_xEndGS2Position - GetActorBrain()->GetActor()->GetPosition();
								xDirToGS.Normalise();
								if (m_xPostGS2Direction * xDirToGS < dbg_fLargetTurnCos)
								{
									AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetActorBrain()->GetActor()->GetAIAnimStateMachine() );
									pxVertStateMachine->RequestMoveSlow();
									GetActorBrain()->GetNavData().m_eMovementMode = AVP_Navigation::MM_SLOW;
								}
							}
							else if ( m_bNeedToEnterNode &&  fDistanceSq < 2.0f )
							{
								SwitchToState( MOUNT_GAMESCENE );
							}
							else if ( !m_bNeedToEnterNode && fDistanceSq < 16.0f )
							{
								Asura_Matrix_3x3 xOri; pxGS2Node->GetOrientation(xOri);
								GetActorBrain()->SetAimDirection( Asura_Vector_3( xOri.e13, xOri.e23, xOri.e33 ) );
							}
							else
							{
								GetActorBrain()->SetLookAtPosition(m_xFollowPoint);
								GetActorBrain()->SetAimDirection(xToFollowPoint);
							}
						}
					}
					else
					{
						GetActorBrain()->SetLookAtPosition(m_xFollowPoint);
						GetActorBrain()->SetAimDirection(xToFollowPoint);
					}

					GetActorBrain()->SetMovementDirection( xToFollowPoint );
				}
			}

			break;
		}

		case MOVING_TO_DESTINATION:
		{
			AVP_Navigation::NavData& xNavData = GetActorBrain()->GetNavData();

			Asura_Vector_3 xToDestination = xNavData.m_xDestinationPosition - GetActorBrain()->GetActor()->GetPosition();
			Asura_Vector_3 xAvoidanceSteering = Asura_Maths::ZeroVector3;
			CalculateAvoidanceSteering(xToDestination, xAvoidanceSteering);
			xToDestination += xAvoidanceSteering;
			xToDestination.Normalise();

			if ( m_uGS2NodeGuid != ASURA_GUID_UNREGISTERED )
			{
				Asura_ServerNode_GS2Node* pxGS2Node = Asura_ServerNode_GS2Node::FindEntity( m_uGS2NodeGuid );
				AVP_ServerActor_Human* pxHuman = (AVP_ServerActor_Human*)GetActorBrain()->GetActor();

				if ( pxGS2Node && pxHuman )
				{
					float fDistanceSq = ( pxGS2Node->GetPosition() - pxHuman->GetPosition() ).MagnitudeSquared();

					if ( !m_bNeedToEnterNode && fDistanceSq < 2.5f )
					{
						GetActorBrain()->ForceMovementDirection( m_xPostGS2Direction );
						SwitchToState( FINISHED );
						return;
					}
					else if ( !m_bNeedToEnterNode &&  fDistanceSq < 4.5f )
					{
						Asura_Vector_3 xDirToGS = m_xEndGS2Position - GetActorBrain()->GetActor()->GetPosition();
						xDirToGS.Normalise();
						if (m_xPostGS2Direction * xDirToGS < dbg_fLargetTurnCos)
						{
							AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetActorBrain()->GetActor()->GetAIAnimStateMachine() );
							pxVertStateMachine->RequestMoveSlow();
							GetActorBrain()->GetNavData().m_eMovementMode = AVP_Navigation::MM_SLOW;
						}
					}
					else if ( m_bNeedToEnterNode &&  fDistanceSq < 2.0f )
					{
						SwitchToState( MOUNT_GAMESCENE );
					}
					else if ( m_bNeedToEnterNode && fDistanceSq < 16.0f )
					{
						Asura_Matrix_3x3 xOri; pxGS2Node->GetOrientation(xOri);
						GetActorBrain()->SetAimDirection( Asura_Vector_3( xOri.e13, xOri.e23, xOri.e33 ) );
					}
					else
					{
						GetActorBrain()->SetAimDirection( xToDestination );
					}
				}
			}
			else
			{
				if ( m_bFirstTime )
				{
					m_bFirstTime = false;
					GetActorBrain()->ForceMovementDirection( xToDestination );
					GetActorBrain()->ForceLookAtPosition( xNavData.m_xDestinationPosition );
					GetActorBrain()->ForceAimDirection( xToDestination );
				}
				else
				{
					GetActorBrain()->SetLookAtPosition( xNavData.m_xDestinationPosition );
					GetActorBrain()->SetAimDirection( xToDestination );
				}

				// If we've arrived we're done.
				AVP_Navigation::APParams xAPParams;
				xAPParams.m_fMaxCoplanarOffset = m_fArrivalRange;
				if ( AVP_Navigation::AtPosition( GetActorBrain()->GetActor()->GetPosition(), xNavData.m_xDestinationPosition, xAPParams ) )
				{
					SwitchToState( FINISHED );
				}
			}

			GetActorBrain()->SetMovementDirection( xToDestination );

			break;
		}

		case MOUNT_GAMESCENE:
		{

			AVP_ClientActor_Human* pxHuman = AVP_ClientActor_Human::FindEntity( GetActorBrain()->GetOwnerGuid() );

			if ( pxHuman )
			{
				AVP_ServerActor_Human* pxOurActor = (AVP_ServerActor_Human*)GetActorBrain()->GetActor();

				if ( pxOurActor && pxOurActor->GetAIAnimStateMachine()->GetCurrentState() != AVP_Actor_AIAnimStateMachine::ACTOR_STATE_IN_MOVE )
				{
					if ( m_eCurrentState == MOUNT_GAMESCENE )
					{
						SendMountGamesceneMessage();
					}
				}
			}
		}
		break;

		case FINISHED:
		break;

		case UNSET:
		case NUM_STATES:
		default:
		{
			Asura_Assert( false, "AVP_Behaviour_Human_Movement::PerFrameUpdate - bad state" );
			SetInError();
			SetFinished();
			break;
		}
	}

	if ( GetActorBrain()->GetActor()->GetAIAnimStateMachine()->GetCurrentState() != AVP_Actor_AIAnimStateMachine::ACTOR_STATE_IN_MOVE )
	{
		SetMovementMode();
	}
}

#ifndef ASURA_NO_DEBUG_OUTPUT
static const char* s_aszStateStrings[] =
{
	"MOVEMENT - UNSET",	
	"MOVEMENT - TRAVERSING STANDARD LINK",
	"MOVEMENT - MOVING TO DESTINATION",
	"MOVEMENT - MOUNT GAME SCENE",
	"MOVEMENT - FINISHED"
};

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

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

	Asura_Font_System::Print
	(
		GetProjectBrain()->GetDebugOutputPos().x,
		GetProjectBrain()->GetDebugOutputPos().y,
		0.5f,
		0xffffffff,
		ASURA_TEXTFORMAT_CENTREJUSTIFIED | ASURA_TEXTFORMAT_FORMATSTRING,
		"smoothed por vel: %f", m_fSmoothedPORVelocity
	);

	GetProjectBrain()->GetDebugOutputPos().y += 10.0f;

	if (m_xFollowPoint != Asura_Maths::ZeroVector3)
	{
		Asura_Vector_3 xOutputPos( m_xFollowPoint );

		if (Asura_Camera::ProjectWorldCoordsToScreenSpace(xOutputPos, true) || xOutputPos.z > 0.0f)
		{
			Asura_DebugLineRender::AddSphere(m_xFollowPoint, 0.5f, 0xffffff00, 3, 2);
			Asura_Font_System::Print(xOutputPos.x, xOutputPos.y, 0.5f, 0xffffff00, ASURA_TEXTFORMAT_CENTREJUSTIFIED, "Follow Point");
		}
	}
}
#endif

AVP_Brain_Actor* AVP_Behaviour_Human_Movement::GetActorBrain() const
{
	return static_cast< AVP_Brain_Actor* >( GetProjectBrain() );
}

void AVP_Behaviour_Human_Movement::SwitchToState( const State eNextState )
{
	// Do any work we need to exit our current state.
	switch ( m_eCurrentState )
	{
		case TRAVERSING_STANDARD_LINK:
		case MOVING_TO_DESTINATION:
		case UNSET:
		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:
		{
			AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetActorBrain()->GetActor()->GetAIAnimStateMachine() );
			switch ( GetActorBrain()->GetNavData().m_eMovementMode )
			{
				case AVP_Navigation::MM_SLOW:
				{
					pxVertStateMachine->RequestMoveSlow();
					break;
				}

				default:
				case AVP_Navigation::MM_MEDIUM:
				{
					pxVertStateMachine->RequestMoveMedium();
					break;
				}

				case AVP_Navigation::MM_FAST:
				{
					pxVertStateMachine->RequestMoveFast();
					break;
				}
			} // end switch MovementMode
			break;
		}

		case MOUNT_GAMESCENE:
		{
			if ( !SendMountGamesceneMessage() )
			{
				AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( GetActorBrain()->GetActor()->GetAIAnimStateMachine() );

				if ( m_bStopAtEnd )
				{
					pxVertStateMachine->RequestIdle();
				}

				SwitchToState( FINISHED );
			}

			break;
		}

		case FINISHED:
		{
			if ( m_bStopAtEnd )
			{
				GetActorBrain()->GetActor()->GetAIAnimStateMachine()->RequestIdle();
			}

			SetFinished();
			break;
		}

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

void AVP_Behaviour_Human_Movement::SetMovementMode( void )
{
	AVP_ServerActor_Human* pxHuman = (AVP_ServerActor_Human*)GetActorBrain()->GetActor();
	AVP_Vertebrate_AIAnimStateMachine* pxVertStateMachine = static_cast< AVP_Vertebrate_AIAnimStateMachine* >( pxHuman->GetAIAnimStateMachine() );

	AVP_Navigation::MovementMode eMoveMode;

	if ( m_bMoveModeOverriden )
	{
		const AVP_Navigation::NavData& xNavData = GetActorBrain()->GetNavData();
		eMoveMode = xNavData.m_eMovementMode;
	}
	else
	{
		eMoveMode = (AVP_Navigation::MovementMode)pxHuman->GetGS2MoveSpeed();
	}

	switch ( eMoveMode )
	{
		default:
		case AVP_Navigation::MM_SLOW:
		{
			if ( !pxVertStateMachine->IsMoveSlow() )
			{
				pxVertStateMachine->RequestMoveSlow();
			}
			break;
		}

		case AVP_Navigation::MM_MEDIUM:
		{
			if ( !pxVertStateMachine->IsMoveMedium() )
			{
				pxVertStateMachine->RequestMoveMedium();
			}
			break;
		}

		case AVP_Navigation::MM_FAST:
		{
			if ( !pxVertStateMachine->IsMoveFast() )
			{
				pxVertStateMachine->RequestMoveFast();
			}
			break;
		}
	} // end switch MovementMode
}

bool AVP_Behaviour_Human_Movement::HandleMessage( const Asura_Message* pxMessage )
{
	switch ( pxMessage->GetMsgID() )
	{
		case ASURA_MSG_MOVECHAINCOMPLETE:
		{
			const Asura_Msg_MoveChainComplete* pxData = static_cast< const Asura_Msg_MoveChainComplete* >( pxMessage->GetData() );

			if ( pxData && pxData->GetInitialMoveID() == AVP_Move_Human_MountGamescene::GetStaticHash() )
			{
				SwitchToState( FINISHED );
			}

			return true;
		}

		default:
		{
			return PARENT::HandleMessage( pxMessage );
		}
	}
}

bool AVP_Behaviour_Human_Movement::SendMountGamesceneMessage()
{
	Asura_Message* pxMessage = Asura_Network::CreateMessage( ASURA_MSG_MOVE_START, sizeof( AVP_Message_Human_MountGamescene ) );

	if ( pxMessage )
	{
		AVP_Message_Human_MountGamescene*		pxData		= static_cast< AVP_Message_Human_MountGamescene* >( pxMessage->GetData() );
		Asura_ServerNode_GS2Node*				pxGS2Node	= Asura_ServerNode_GS2Node::FindEntity( m_uGS2NodeGuid );

		if ( pxGS2Node )
		{
			Asura_Matrix_3x3 xNodeOri;
			pxGS2Node->GetOrientation(xNodeOri);

			pxMessage->SetFromGuid( GetActorBrain()->GetOwnerGuid() );
			pxMessage->SetToGuid( GetActorBrain()->GetOwnerGuid() );
			pxData->ClearFlags();
			pxData->SetMoveID( AVP_Move_Human_MountGamescene::GetStaticHash() );
			pxData->SetDestinationPosition( pxGS2Node->GetPosition() );
			pxData->SetDestinationOrientation( xNodeOri );
			pxData->SetAnimBlendTemplateBlueprintHash( GetMountGamesceneAnimBlendTemplateBlueprintHash() );

			Asura_Matrix_3x3	xOri;
			pxGS2Node->GetOrientation( xOri );
			Asura_Vector_3		xAim( xOri.e13, xOri.e23, xOri.e33 );

			GetActorBrain()->SetAimDirection( xAim );
			GetActorBrain()->SetLookAtPosition( GetActorBrain()->GetActor()->GetPosition() + xAim );

			if ( pxData->GetAnimBlendTemplateBlueprintHash() == ASURA_HASH_ID_UNSET )
			{
				Asura_Network::DestroyMessage(pxMessage);
				return false;
			}

			AVP_ServerActor_Human* xOurActor = (AVP_ServerActor_Human*)GetActorBrain()->GetActor();

			if ( xOurActor->GetAIAnimStateMachine() )
			{
				xOurActor->GetAIAnimStateMachine()->RequestInMove( pxMessage, AVP_Human_AIAnimStateMachine::ACTOR_STATE_IDLE );
			}
			else
			{
				Asura_Network::DestroyMessage(pxMessage);
			}
		}
		else
		{
			Asura_Network::DestroyMessage(pxMessage);
		}
	}

	return true;
}

Asura_Hash_ID AVP_Behaviour_Human_Movement::GetMountGamesceneAnimBlendTemplateBlueprintHash(void)
{
	const AVP_ServerActor_Human* pxHuman = static_cast<const AVP_ServerActor_Human*>( GetActorBrain()->GetActor() );
	
	const AVP_Inventory_System* pxInvSys = pxHuman->GetInventorySystem();
	const AVP_Inventory_Group* pxWeaponGroup = pxInvSys != NULL ? pxInvSys->GetGroup( uAVP_INVENTORY_GROUPHASH_WEAPON ) : NULL;
	Asura_Assert( pxWeaponGroup != NULL, "Unable to get wepaon group" );
	if ( pxWeaponGroup != NULL )
	{
		const AVP_Inventory_Item* pxItem = pxWeaponGroup->GetCurrentInventoryItem();
		if ( pxItem )
		{
			if ( pxHuman->GetAIAnimStateMachine()->GetCurrentState() == AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_SLOW )
			{
				const AVP_Actor_AnimStateData* pxStateData = AVP_Human_AIAnimStateData::GetAnimStateData( pxHuman->GetAnimStateBlueprintType(), pxItem->GetBlueprintHash(), AVP_Human_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_SLOW_TO_IDLE, pxHuman->IsAiming(), pxHuman->GetCoverType() );
				if ( pxStateData )
				{
					return pxStateData->GetAnimBlendTemplateBlueprintHash();
				}
			}
			else //if ( pxHuman->GetAIAnimStateMachine()->GetCurrentState() == AVP_Vertebrate_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_MEDIUM )
			{
				const AVP_Actor_AnimStateData* pxStateData = AVP_Human_AIAnimStateData::GetAnimStateData( pxHuman->GetAnimStateBlueprintType(), pxItem->GetBlueprintHash(), AVP_Human_AIAnimStateMachine::VERTEBRATE_STATE_MOVE_MEDIUM_TO_IDLE, pxHuman->IsAiming(), pxHuman->GetCoverType() );
				if ( pxStateData )
				{
					return pxStateData->GetAnimBlendTemplateBlueprintHash();
				}
			}
		}
	}

	// else
	return ASURA_HASH_ID_UNSET;
}

DEBUGVAR float dbg_fTargetForwardCosine = Asura_Maths::Cosine(Asura_Maths::DegToRad(25.0f));
DEBUGVAR float  dbg_fCollisionAvoidRangeBias = 0.5f;
DEBUGVAR float dbg_fCollisionAvoidMinimumAngleDeg   = 45.0f;
DEBUGVAR float dbg_fCollisionAvoidOverallScalar     = 1.5f;
DEBUGVAR float dbg_fCollisionAvoidLocalScalar       = 2.0f;
DEBUGVAR float dbg_fLookAheadDistance		        = 3.0f;
DEBUGVAR bool  dbg_bCollisionAvoidDoGlobalAngleHack = true;

void AVP_Behaviour_Human_Movement::CalculateAvoidanceSteering( const Asura_Vector_3& xFollowPointSteering, Asura_Vector_3& xSteering )
{
	// Don't avoid if we are close to destination ( better approach is to simply mark target to ignore it we try to get to a target
	AVP_Navigation::NavData& xNavData = GetActorBrain()->GetNavData();
 	const float fToDestSq = ( GetActorBrain()->GetActor()->GetPosition() - xNavData.m_xDestinationPosition ).MagnitudeSquared();
 	if ( fToDestSq <= 25.0f )
 	{
 		return;
 	}

	const Asura_Matrix_3x3& xOurOrientation = GetActorBrain()->GetActor()->GetOrientation();
	const Asura_Vector_3    xOurNormal( -GetActorBrain()->GetActor()->GetCurrentGravityDirection() );	
	Asura_Vector_3 xNormalisedFollowPointSteering = xFollowPointSteering;
	xNormalisedFollowPointSteering.Normalise();

	u_int uNumAvoidVectors = 0;

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

	const u_int uNumNearbyGuids = GetActorBrain()->GetNumNearbyGuids();
	for ( u_int uNearbyGuid = 0; uNearbyGuid < uNumNearbyGuids; ++uNearbyGuid )
	{

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

		float fRange = Asura_Maths::Max( xNearbyGuidData.m_fRange - dbg_fCollisionAvoidRangeBias, 0.75f );

		Asura_Vector_3 xFromVector = xNearbyGuidData.m_xFromGuid;

		// 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 * xNormalisedFollowPointSteering) );
		if ((-xPlaneVector * xNormalisedFollowPointSteering) > dbg_fTargetForwardCosine)
		{
			fMultiplier += 1.0f;
		}

		xSteering += xPlaneVector * fMultiplier * ( dbg_fCollisionAvoidLocalScalar / ( fRange ) );
		++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 )
		{
			// 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( -Asura_Maths::Sign( xLocalPlaneVector.x ) * fAngleHackMinAngle, xOurUp );
			xGlobalPlaneVector = xRotationQuat.RotateVector( -xNormalisedFollowPointSteering);
		}

		xGlobalPlaneVector *= fGlobalPlaneVector;
		xSteering = xGlobalPlaneVector;
	}

	// check if new steering will keep me in waypoint spaces
	
	bool bSafeToUseAvoidance = false;
	for (int iTests = 0; iTests < 3 && !bSafeToUseAvoidance; ++iTests)
	{
		Asura_Vector_3 xCombinedSteering = xSteering + xFollowPointSteering;
		xCombinedSteering.Normalise();
		Asura_Vector_3 xDestination = GetActorBrain()->GetActor()->GetPosition() + (xCombinedSteering * dbg_fLookAheadDistance); // check 2m ahead
		u_short usDestWPID = xNavData.m_usCurrentWPID;
		AVP_Navigation::TestParams	xTestParams;

		// Fill the test against parameters with the route finding needed information
		xTestParams.m_uPathfinderFlags		= xNavData.m_uPathfinderFlags;
		xTestParams.m_usZone				= xNavData.m_usCurrentZone;
		xTestParams.m_xNormal				= -GetActorBrain()->GetActor()->GetCurrentGravityDirection();

		AVP_Navigation::UpdateCurrentWaypointID(xDestination, usDestWPID, xTestParams);
		const Asura_AI_NavVertex* pxDestWP = usDestWPID != ASURA_INVALID_WAYPOINT_ID ? Asura_AI_Navigation_System::GetWaypoint( usDestWPID ) : NULL;
		bSafeToUseAvoidance = pxDestWP ? pxDestWP->Contains(xDestination) : false;

		if (!bSafeToUseAvoidance)
		{
			xSteering = xSteering / 1.5f;
		}
	}

	if (!bSafeToUseAvoidance)
	{
		xSteering = Asura_Maths::ZeroVector3;
	}
}


bool AVP_Behaviour_Human_Movement::UpdateFollowPoint()
{
	const AVP_Navigation::NavData&	xNavData				= GetActorBrain()->GetNavData();
	const Asura_Vector_3&			xCurrentPosition		= GetActorBrain()->GetActor()->GetPosition();
	u_short							usNearestRouteIndex;
	
	AVP_Navigation::FindNearestPointOnRoute( GetActorBrain()->GetNavData(), xCurrentPosition, 2.5f, m_xFollowPoint, usNearestRouteIndex );

	if ( usNearestRouteIndex == ( xNavData.m_usRouteLength - 2 ) )
	{
		if ( Asura_AI_Navigation_System::IsPositionInWaypoint( xCurrentPosition, xNavData.m_pusRoute[ usNearestRouteIndex + 1 ] ) )
		{
			return false;
		}
	}

	return true;
}

void AVP_Behaviour_Human_Movement::UpdatePositionOnRoute()
{
	AVP_Navigation::NavData& xNavData = GetActorBrain()->GetNavData();

	m_xPrevPositionOnRoute = m_xPositionOnRoute;
	AVP_Navigation::FindNearestPointOnRoute( xNavData, GetActorBrain()->GetActor()->GetPosition(), 0.0f, m_xPositionOnRoute, xNavData.m_usRouteSegmentIndex );
	Asura_Assert( xNavData.m_usRouteSegmentIndex < xNavData.m_usRouteLength, "AVP_Behaviour_Human_Movement::PerFrameUpdate - bug in AVP_Navigation::FindNearestPointOnRoute" );
}

void AVP_Behaviour_Human_Movement::UpdateStuckDetection( const float fTimeslice )
{
	if ( fTimeslice > 0.0f )
	{
		const float fPORVelocity = ( m_xPositionOnRoute - m_xPrevPositionOnRoute ).Magnitude() / fTimeslice;
		m_fSmoothedPORVelocity += ( fPORVelocity - m_fSmoothedPORVelocity ) * 0.5f * fTimeslice;
		if ( m_fSmoothedPORVelocity < 0.1f )
		{
			// We're stuck.
			SetInError();
			SetFinished();
		}
	}
}

void AVP_Behaviour_Human_Movement::ReadFromChunkStream( Asura_Chunk_Stream& xStream )
{
	u_int uVersion;
	xStream >> uVersion;

	PARENT::ReadFromChunkStream(xStream);

	if ( uVersion == uCURRENT_CHUNK_VERSION )
	{
		u_int uCurrentState = 0; xStream >> uCurrentState; m_eCurrentState = (State)uCurrentState;
		xStream >> m_fTimeInState;
		xStream >> m_fArrivalRange;
		xStream >> m_xPrevPositionOnRoute;
		xStream >> m_fSmoothedPORVelocity;
		xStream >> m_xPositionOnRoute;
		xStream >> m_xFollowPoint;
		xStream >> m_bFirstTime;
		xStream >> m_uGS2NodeGuid;
		xStream >> m_xPostGS2Direction;
		xStream >> m_xEndGS2Position;
		xStream >> m_bNeedToEnterNode;
		xStream >> m_bStopAtEnd;
	}
	else if ( uVersion == 1 )
	{
		u_int uCurrentState = 0; xStream >> uCurrentState; m_eCurrentState = (State)uCurrentState;
		xStream >> m_fTimeInState;
		xStream >> m_fArrivalRange;
		xStream >> m_xPrevPositionOnRoute;
		xStream >> m_fSmoothedPORVelocity;
		xStream >> m_xPositionOnRoute;
		xStream >> m_xFollowPoint;
		xStream >> m_bFirstTime;
		xStream >> m_uGS2NodeGuid;

		m_bNeedToEnterNode = true;
		m_bStopAtEnd = true;
	}
}

void AVP_Behaviour_Human_Movement::WriteToChunkStream( Asura_Chunk_Stream& xStream ) const
{
	xStream << uCURRENT_CHUNK_VERSION;

	PARENT::WriteToChunkStream( xStream );
	
	u_int uCurrentState = (u_int)m_eCurrentState; xStream << uCurrentState;

	xStream << m_fTimeInState;
	xStream << m_fArrivalRange;
	xStream << m_xPrevPositionOnRoute;
	xStream << m_fSmoothedPORVelocity;
	xStream << m_xPositionOnRoute;
	xStream << m_xFollowPoint;
	xStream << m_bFirstTime;
	xStream << m_uGS2NodeGuid;
	xStream << m_xPostGS2Direction;
	xStream << m_xEndGS2Position;
	xStream << m_bNeedToEnterNode;
	xStream << m_bStopAtEnd;
}

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