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

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

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

// header
#include "Behaviour/BehaviourTypes/Dropship/AVP_Behaviour_Dropship_Movement.h"

// asura
#ifndef ASURA_NO_DEBUG_OUTPUT
#include "Rendering/Asura_Font_System.h"
#include "Debugging/Asura_DebugLineRender.h"
#endif

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

// avp
#include "Entity/PhysicalObject/Actor/Vehicles/Dropship/AVP_ServerVehicle_Dropship.h"

//////////////////////////////////////////////////////////////////////////////
//
// AVP_Behaviour_Dropship_Movement
//
//////////////////////////////////////////////////////////////////////////////

AVP_Behaviour_Dropship_Movement::AVP_Behaviour_Dropship_Movement( AVP_Brain_Actor_Dropship* pxBrain )
: PARENT( pxBrain, AVP_BEHAVIOUR_TYPE_DROPSHIP_MOVEMENT )
, m_eCurrentState( UNSET )
, m_fTimeInState( 0.0f )
, m_fTotalPathDistance(0.0)
, m_xPathSegments()
, m_xDestinationSegmentIt(m_xPathSegments)
, m_xDestinationOnPath(Asura_Maths::ZeroVector3)
, m_xDestination(Asura_Maths::ZeroVector3)
, m_bAimAtTarget(false)
{
}


void AVP_Behaviour_Dropship_Movement::FollowPath()
{
	switch ( m_eCurrentState )
	{
	case UNSET:
	case HOVER:
	case STOP_TO_HOVER:
		SwitchToState(FOLLOW_PATH);
		break;
	case FOLLOW_PATH:
		break;
	default:
		Asura_Assert(false, "Unexpected state");
	}
}

void AVP_Behaviour_Dropship_Movement::Hover()
{
	switch ( m_eCurrentState )
	{
	case HOVER:
	case STOP_TO_HOVER:
		break;
	case UNSET:
	case FOLLOW_PATH:
		SwitchToState(STOP_TO_HOVER);
		break;
	default:
		Asura_Assert(false, "Unexpected state");
	}

}

DEBUGVAR float dbg_fMinHoverTime = 4.0f;

 bool AVP_Behaviour_Dropship_Movement::IsHovering() const
 {
	 return (m_eCurrentState == HOVER) && (m_fTimeInState > dbg_fMinHoverTime); 
 }

void AVP_Behaviour_Dropship_Movement::AimAtTargetMode(bool bAimAtTarget)
{
	m_bAimAtTarget = bAimAtTarget;
}


void AVP_Behaviour_Dropship_Movement::Deactivate()
{
	Axon_Behaviour* pxChild = GetChild();
	while ( pxChild )
	{
		UnlinkChild( pxChild );
		GetProjectBrain()->DestroyBehaviour( pxChild );
		pxChild = GetChild();
	}

    PARENT::Deactivate();
}

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

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

    m_fTimeInState += fTimeslice;

	UpdateAim();

	switch ( m_eCurrentState )
	{
	case UNSET:
		SwitchToState(STOP_TO_HOVER);
		break;
	case HOVER:
		StayAtPoint();
		break;
	case STOP_TO_HOVER:
		if (Stop())
		{
			SwitchToState(HOVER);
		}
		break;
	case FOLLOW_PATH:
		UpdatePath(fTimeslice);
		break;
	default:
		Asura_Assert(false, "Unexpected state");
	}
}

#ifndef ASURA_NO_DEBUG_OUTPUT
static const char* s_aszStateStrings[] =
{
	"UNSET",
	"FOLLOW_PATH",
	"STOP_TO_HOVER",
	"HOVER",
	"NUM_STATES"

};

void AVP_Behaviour_Dropship_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_DebugLineRender::AddCross(m_xDestination);

}
#endif

AVP_Brain_Actor_Dropship* AVP_Behaviour_Dropship_Movement::GetDropshipBrain() const
{
    return static_cast< AVP_Brain_Actor_Dropship* >( GetProjectBrain() );
}


void AVP_Behaviour_Dropship_Movement::SwitchToState( const State eNextState )
{
	
	switch ( m_eCurrentState )
	{
	case UNSET:
	case FOLLOW_PATH:
	case STOP_TO_HOVER:
	case HOVER:
		break;
	case FINISHED:
	default:
		Asura_Assert(false, "Unexpected state");
	}

	m_eCurrentState = eNextState;
	m_fTimeInState  = 0.0f;
	switch ( m_eCurrentState )
	{
	case FOLLOW_PATH:
	case STOP_TO_HOVER:
		break;
	case HOVER:
		break;
	case FINISHED:
		SetFinished(true);
		break;
	case UNSET:
	default:
		Asura_Assert(false, "Unexpected state");
	}
}

DEBUGVAR float dbg_fExpectedVelocity = 40.0f;
DEBUGVAR float dbg_fWaitDistanceSq = 40.0f * 40.0f;

void AVP_Behaviour_Dropship_Movement::UpdatePath(float fTimeslice)
{
	Asura_Vector_3 xCurrentPosition = GetDropshipBrain()->GetActor()->GetPosition();
	if (m_fTotalPathDistance == 0.0f)
	{
		ExpandPath();
		m_xDestinationSegmentIt.Restart();
		if (m_xDestinationSegmentIt.Done())
		{
			return;
		}
		else
		{
			m_xDestinationOnPath = *m_xDestinationSegmentIt.GetCurrent();
		}
	}
	else if ( (m_xDestinationOnPath - xCurrentPosition).MagnitudeSquared() < dbg_fWaitDistanceSq )
	{
		float fLookAheadDistance = dbg_fExpectedVelocity * fTimeslice;
		Asura_LinkList_It<Asura_Vector_3> xNextSegmentIt = m_xDestinationSegmentIt;
		xNextSegmentIt.Next();
		while (fLookAheadDistance > 0)
		{
			if (xNextSegmentIt.Done()) // if we got to the end expand the path 
			{
				float fDeltaDistance = GetDropshipBrain()->ExpandPath(m_xPathSegments);
				Asura_Assert(fDeltaDistance > 0, "Path expansion failed");
				if (fDeltaDistance > 0)
				{
					m_fTotalPathDistance += fDeltaDistance;
					xNextSegmentIt = m_xDestinationSegmentIt;
					xNextSegmentIt.Next();
				}
				else
				{
					SetInError(true);
					SwitchToState(FINISHED);
					return;
				}
			}

			Asura_Vector_3 xSegmentDirection = *xNextSegmentIt.GetCurrent() - *m_xDestinationSegmentIt.GetCurrent();
			xSegmentDirection.Normalise();
			const float fToSegmentEnd = ( *xNextSegmentIt.GetCurrent() - m_xDestinationOnPath ).Magnitude();
			if (fLookAheadDistance > fToSegmentEnd)
			{
				m_xDestinationOnPath += xSegmentDirection * fToSegmentEnd;
				xNextSegmentIt.Next();
				m_xDestinationSegmentIt.DeleteCurrent();
				fLookAheadDistance -= fToSegmentEnd;
			}
			else
			{
				m_xDestinationOnPath += xSegmentDirection * fLookAheadDistance;		
				fLookAheadDistance = -1.0f;
			}
		}

// 		bool bMovingAway = true;
// 		float fDistanceSq;
// 		while(bMovingAway)
// 		{
// 			fDistanceSq = (m_xDestinationOnPath-xCurrentPosition).MagnitudeSquared();
// 			fLookAheadDistance = 1.0f;
// 			while (fLookAheadDistance > 0)
// 			{
// 				if (xNextSegmentIt.Done()) // if we got to the end expand the path 
// 				{
// 					float fDeltaDistance = GetDropshipBrain()->ExpandPath(m_xPathSegments);
// 					Asura_Assert(fDeltaDistance > 0, "Path expansion failed");
// 					if (fDeltaDistance > 0)
// 					{
// 						m_fTotalPathDistance += fDeltaDistance;
// 						xNextSegmentIt = m_xDestinationSegmentIt;
// 						xNextSegmentIt.Next();
// 					}
// 					else
// 					{
// 						SetInError(true);
// 						SwitchToState(FINISHED);
// 						return;
// 					}
// 				}
// 
// 				Asura_Vector_3 xSegmentDirection = *xNextSegmentIt.GetCurrent() - *m_xDestinationSegmentIt.GetCurrent();
// 				xSegmentDirection.Normalise();
// 				const float fToSegmentEnd = ( *xNextSegmentIt.GetCurrent() - m_xDestinationOnPath ).Magnitude();
// 				if (fLookAheadDistance > fToSegmentEnd)
// 				{
// 					m_xDestinationOnPath += xSegmentDirection * fToSegmentEnd;
// 					xNextSegmentIt.Next();
// 					m_xDestinationSegmentIt.DeleteCurrent();
// 					fLookAheadDistance -= fToSegmentEnd;
// 				}
// 				else
// 				{
// 					m_xDestinationOnPath += xSegmentDirection * fLookAheadDistance;		
// 					fLookAheadDistance = -1.0f;
// 				}
// 			}
// 			if((m_xDestinationOnPath-xCurrentPosition).MagnitudeSquared() < fDistanceSq)
// 			{
// 				bMovingAway = false;
// 			}
// 		}
		
	}

	SetDestination(m_xDestinationOnPath);


}

DEBUGVAR float dbg_fPracticalStopVelocitySq = 0.2f * 0.2f;

bool AVP_Behaviour_Dropship_Movement::Stop()
{
	// set the target our current position - will cause the ship to stop
	AVP_ServerVehicle_Dropship* pxActor = GetDropshipBrain()->GetDropshipActor();
	SetDestination(pxActor->GetPosition());

	if (pxActor->GetVelocity().MagnitudeSquared() < dbg_fPracticalStopVelocitySq)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void AVP_Behaviour_Dropship_Movement::StayAtPoint()
{
	AVP_ServerVehicle_Dropship* pxActor = GetDropshipBrain()->GetDropshipActor();
	SetDestination(pxActor->GetPosition());

}


void AVP_Behaviour_Dropship_Movement::UpdateAim()
{
	Asura_Vector_3 xTarget;

	if (!m_bAimAtTarget)
	{
		xTarget = m_xDestination;
	}
	else
	{
		if (!GetDropshipBrain()->GetTarget(xTarget))
		{
			xTarget = m_xDestination;
		}
	}

	Asura_Vector_3 xAimDirection = xTarget - GetDropshipBrain()->GetDropshipActor()->GetPosition();

	if (xAimDirection.MagnitudeSquared() > 0.01f)
	{
		xAimDirection.Normalise();
		GetDropshipBrain()->SetAimDirection(xAimDirection);
	}
}

void AVP_Behaviour_Dropship_Movement::SetDestination(const Asura_Vector_3& xDestination)
{
	m_xDestination = xDestination;
	GetDropshipBrain()->GetDropshipActor()->SetNextDestination(m_xDestination);
}


DEBUGVAR float dbg_fMaxPathDistance = 30.0f;

void AVP_Behaviour_Dropship_Movement::ExpandPath()
{
	while (m_fTotalPathDistance < dbg_fMaxPathDistance)
	{
		float fDeltaDistance = GetDropshipBrain()->ExpandPath(m_xPathSegments);
		Asura_Assert(fDeltaDistance > 0, "Path expansion failed");
		if (fDeltaDistance > 0)
		{
			m_fTotalPathDistance += fDeltaDistance;
		}
		else
		{
			SetInError(true);
			SwitchToState(FINISHED);
			return;
		}
	}
}


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