//========= Copyright Valve Corporation, All rights reserved. ============//
//
// Purpose:
//
//=============================================================================//

#include "cbase.h"

#include "ai_localnavigator.h"

#include "ai_basenpc.h"
#include "ai_planesolver.h"
#include "ai_moveprobe.h"
#include "ai_motor.h"

// memdbgon must be the last include file in a .cpp file!!!
#include "tier0/memdbgon.h"

ConVar ai_debug_directnavprobe("ai_debug_directnavprobe", "0");

const float TIME_DELAY_FULL_DIRECT_PROBE[2] = { 0.25, 0.35 };

//-----------------------------------------------------------------------------

BEGIN_SIMPLE_DATADESC(CAI_LocalNavigator)
	//						m_fLastWasClear	(not saved)
	//						m_LastMoveGoal	(not saved)
	//						m_FullDirectTimer	(not saved)
	//						m_pPlaneSolver	(not saved)
	//						m_pMoveProbe	(not saved)
END_DATADESC();

//-------------------------------------

CAI_LocalNavigator::CAI_LocalNavigator(CAI_BaseNPC *pOuter) : CAI_Component( pOuter ) 
{
	m_pMoveProbe = NULL;
	m_pPlaneSolver = new CAI_PlaneSolver( pOuter );

	m_fLastWasClear = false;
	memset( &m_LastMoveGoal, 0, sizeof(m_LastMoveGoal) );
}

//-------------------------------------

CAI_LocalNavigator::~CAI_LocalNavigator() 
{
	delete m_pPlaneSolver;
}

//-------------------------------------

void CAI_LocalNavigator::Init( IAI_MovementSink *pMovementServices )	
{ 
	CAI_ProxyMovementSink::Init( pMovementServices );
	m_pMoveProbe = GetOuter()->GetMoveProbe(); // @TODO (toml 03-30-03): this is a "bad" way to grab this pointer. Components should have an explcit "init" phase.
}

//-------------------------------------

void CAI_LocalNavigator::ResetMoveCalculations()
{
	m_FullDirectTimer.Force();
	m_pPlaneSolver->Reset();
}

//-------------------------------------

void CAI_LocalNavigator::AddObstacle( const Vector &pos, float radius, AI_MoveSuggType_t type )
{
	m_pPlaneSolver->AddObstacle( pos, radius, NULL, type );
}

//-------------------------------------

bool CAI_LocalNavigator::HaveObstacles()
{
	return m_pPlaneSolver->HaveObstacles();
}

//-------------------------------------

bool CAI_LocalNavigator::MoveCalcDirect( AILocalMoveGoal_t *pMoveGoal, bool bOnlyCurThink, float *pDistClear, AIMoveResult_t *pResult )
{
	AI_PROFILE_SCOPE(CAI_LocalNavigator_MoveCalcDirect);

	bool bRetVal = false;
	
	if ( pMoveGoal->speed )
	{
		CAI_Motor *pMotor = GetOuter()->GetMotor();
		float  minCheckDist = pMotor->MinCheckDist();
		float  probeDist	= m_pPlaneSolver->CalcProbeDist( pMoveGoal->speed ); // having this match steering allows one fewer traces
		float  checkDist	= MAX( minCheckDist, probeDist );
		float  checkStepDist = MAX( 16.0, probeDist * 0.5 );

		if ( pMoveGoal->flags & ( AILMG_TARGET_IS_TRANSITION | AILMG_TARGET_IS_GOAL ) )
		{
			// clamp checkDist to be no farther than max distance to goal
			checkDist = MIN( checkDist, pMoveGoal->maxDist );
		}

		if ( checkDist <= 0.0 )
		{
			*pResult = AIMR_OK;
			return true;
		}

		float moveThisInterval = pMotor->CalcIntervalMove();
		bool bExpectingArrival = (moveThisInterval >= checkDist);

		if ( !m_FullDirectTimer.Expired() )
		{
			if ( !m_fLastWasClear || 
				 ( !VectorsAreEqual(pMoveGoal->target, m_LastMoveGoal.target, 0.1) || 
				   !VectorsAreEqual(pMoveGoal->dir, m_LastMoveGoal.dir, 0.1) ) ||
				 bExpectingArrival )
			{
				m_FullDirectTimer.Force();
			}
		}

		if ( bOnlyCurThink ) // Outer code claims to have done a validation (probably a simplify operation)
		{
			m_FullDirectTimer.Set( TIME_DELAY_FULL_DIRECT_PROBE[AIStrongOpt()] );
		}

		// First, check the probable move for this cycle
		bool bTraceClear = true;
		Vector testPos;

		if ( !bExpectingArrival )
		{
			testPos = GetLocalOrigin() + pMoveGoal->dir * moveThisInterval;
			bTraceClear = GetMoveProbe()->MoveLimit( pMoveGoal->navType, GetLocalOrigin(), testPos, 
													 MASK_NPCSOLID, pMoveGoal->pMoveTarget, 
													 100.0, 
													 ( pMoveGoal->navType == NAV_GROUND ) ? AIMLF_2D : AIMLF_DEFAULT, 
													 &pMoveGoal->directTrace );

			if ( !bTraceClear )
			{
				// Adjust probe top match expected probe dist (relied on later in process)
				pMoveGoal->directTrace.flDistObstructed = (checkDist - moveThisInterval) + pMoveGoal->directTrace.flDistObstructed;

			}

			if ( !IsRetail() && ai_debug_directnavprobe.GetBool() )
			{
				if ( !bTraceClear )
				{
					DevMsg( GetOuter(), "Close obstruction %f\n", checkDist - pMoveGoal->directTrace.flDistObstructed );
					NDebugOverlay::Line( WorldSpaceCenter(), Vector( testPos.x, testPos.y, WorldSpaceCenter().z ), 255, 0, 0, false, 0.1 );
					if ( pMoveGoal->directTrace.pObstruction )
						NDebugOverlay::Line( WorldSpaceCenter(), pMoveGoal->directTrace.pObstruction->WorldSpaceCenter(), 255, 0, 255, false, 0.1 );

				}
				else
				{
					NDebugOverlay::Line( WorldSpaceCenter(), Vector( testPos.x, testPos.y, WorldSpaceCenter().z ), 0, 255, 0, false, 0.1 );
				}
			}

			pMoveGoal->thinkTrace = pMoveGoal->directTrace;
		}

		// Now project out for future obstructions
		if ( bTraceClear )
		{
			if ( m_FullDirectTimer.Expired() )
			{
				testPos = GetLocalOrigin() + pMoveGoal->dir * checkDist;
				float checkStepPct = (checkStepDist / checkDist) * 100.0;
				if ( checkStepPct > 100.0 )
					checkStepPct = 100.0;
				
				bTraceClear = GetMoveProbe()->MoveLimit( pMoveGoal->navType, GetLocalOrigin(), testPos, 
														 MASK_NPCSOLID, pMoveGoal->pMoveTarget, 
														 checkStepPct, 
														 ( pMoveGoal->navType == NAV_GROUND ) ? AIMLF_2D : AIMLF_DEFAULT, 
														 &pMoveGoal->directTrace );
				if ( bExpectingArrival )
					pMoveGoal->thinkTrace = pMoveGoal->directTrace;

				if (ai_debug_directnavprobe.GetBool() )
				{
					if ( !bTraceClear )
					{
						NDebugOverlay::Line( GetOuter()->EyePosition(), Vector( testPos.x, testPos.y, GetOuter()->EyePosition().z ), 255, 0, 0, false, 0.1 );
						DevMsg( GetOuter(), "Obstruction %f\n", checkDist - pMoveGoal->directTrace.flDistObstructed );
					}
					else
					{
						NDebugOverlay::Line( GetOuter()->EyePosition(), Vector( testPos.x, testPos.y, GetOuter()->EyePosition().z ), 0, 255, 0, false, 0.1 );
						DevMsg( GetOuter(), "No obstruction\n" );
					}
				}
			}
			else
			{
				if ( ai_debug_directnavprobe.GetBool() )
					DevMsg( GetOuter(), "No obstruction (Near probe only)\n" );
			}
		}

		pMoveGoal->bHasTraced = true;
	
		float distClear = checkDist - pMoveGoal->directTrace.flDistObstructed;
		if (distClear < 0.001)
			distClear = 0;
		
		if ( bTraceClear )
		{
			*pResult = AIMR_OK;
			bRetVal = true;
			m_fLastWasClear = true;
		}
		else if ( ( pMoveGoal->flags & ( AILMG_TARGET_IS_TRANSITION | AILMG_TARGET_IS_GOAL ) ) && 
			 pMoveGoal->maxDist < distClear )
		{
			*pResult = AIMR_OK;
			bRetVal = true;
			m_fLastWasClear = true;
		}
		else
		{
			*pDistClear = distClear;
			m_fLastWasClear = false;
		}
	}
	else
	{
		// Should never end up in this function with speed of zero. Probably an activity problem.
		*pResult = AIMR_ILLEGAL;
		bRetVal = true;
	}

	m_LastMoveGoal = *pMoveGoal;
	if ( bRetVal && m_FullDirectTimer.Expired() )
		m_FullDirectTimer.Set( TIME_DELAY_FULL_DIRECT_PROBE[AIStrongOpt()] );

	return bRetVal;
}

//-------------------------------------

ConVar ai_no_steer( "ai_no_steer", "0" );

bool CAI_LocalNavigator::MoveCalcSteer( AILocalMoveGoal_t *pMoveGoal, float distClear, AIMoveResult_t *pResult )
{
	if ( (pMoveGoal->flags & AILMG_NO_STEER) )
		return false;

	if ( ai_no_steer.GetBool() )
		return false;

	if ( GetOuter()->IsFlaggedEfficient() )
		return false;

	AI_PROFILE_SCOPE(CAI_Motor_MoveCalcSteer);
	Vector moveSolution;
	if ( m_pPlaneSolver->Solve( *pMoveGoal, distClear, &moveSolution ) )
	{
		if ( moveSolution != pMoveGoal->dir )
		{
			float dot = moveSolution.AsVector2D().Dot( pMoveGoal->dir.AsVector2D() );

			const float COS_HALF_30 = 0.966;
			if ( dot > COS_HALF_30 )
			{
				float probeDist = m_pPlaneSolver->CalcProbeDist( pMoveGoal->speed );
				if ( pMoveGoal->maxDist < probeDist * 0.33333 && distClear > probeDist * 0.6666)
				{
					// A waypoint is coming up, but there's probably time to steer
					// away after hitting it
					*pResult = AIMR_OK;
					return true;
				}
			}

			pMoveGoal->facing = pMoveGoal->dir = moveSolution;
		}
		*pResult = AIMR_OK;
		return true;
	}
		
	return false;
}

//-------------------------------------

bool CAI_LocalNavigator::MoveCalcStop( AILocalMoveGoal_t *pMoveGoal, float distClear, AIMoveResult_t *pResult )
{
	if (distClear < pMoveGoal->maxDist)
	{
		if ( distClear < 0.1 )
		{
			DebugNoteMovementFailure();
			*pResult = AIMR_ILLEGAL;
		}
		else
		{
			pMoveGoal->maxDist = distClear;
			*pResult = AIMR_OK;
		}

		return true;
	}
	*pResult = AIMR_OK;
	return true;
}

//-------------------------------------

#ifdef DEBUG
#define SetSolveCookie() 	pMoveGoal->solveCookie = __LINE__;
#else
#define SetSolveCookie()	((void)0)
#endif


AIMoveResult_t CAI_LocalNavigator::MoveCalcRaw( AILocalMoveGoal_t *pMoveGoal, bool bOnlyCurThink )
{
	AI_PROFILE_SCOPE(CAI_Motor_MoveCalc);
	
	AIMoveResult_t result = AIMR_OK; // Assume success
	AIMoveTrace_t  directTrace;
	float	   	   distClear;
	
	// --------------------------------------------------

	bool bDirectClear = MoveCalcDirect( pMoveGoal, bOnlyCurThink, &distClear, &result);
	if ( OnCalcBaseMove( pMoveGoal, distClear, &result ) )
	{
		SetSolveCookie();
		return DbgResult( result );
	}

	bool bShouldSteer = ( !(pMoveGoal->flags & AILMG_NO_STEER) && ( !bDirectClear || HaveObstacles() ) );

	if ( bDirectClear && !bShouldSteer )
	{
		SetSolveCookie();
		return DbgResult( result );
	}
	
	// --------------------------------------------------

	if ( bShouldSteer )
	{
		if ( !bDirectClear )
		{
			if ( OnObstructionPreSteer( pMoveGoal, distClear, &result ) )
			{
				SetSolveCookie();
				return DbgResult( result );
			}
		}

		if ( MoveCalcSteer( pMoveGoal, distClear, &result ) )
		{
			SetSolveCookie();
			return DbgResult( result );			
		}
	}

	if ( OnFailedSteer( pMoveGoal, distClear, &result ) )
	{
		SetSolveCookie();
		return DbgResult( result );
	}

	// --------------------------------------------------
	
	if ( OnFailedLocalNavigation( pMoveGoal, distClear, &result ) )
	{
		SetSolveCookie();
		return DbgResult( result );
	}

	if ( distClear < GetOuter()->GetMotor()->MinStoppingDist() )
	{
		if ( OnInsufficientStopDist( pMoveGoal, distClear, &result ) )
		{
			SetSolveCookie();
			return DbgResult( result );
		}

		if ( MoveCalcStop( pMoveGoal, distClear, &result) )
		{
			SetSolveCookie();
			return DbgResult( result );
		}
	}

	// A hopeful result... may get in trouble at next waypoint and obstruction is still there
	if ( distClear > pMoveGoal->curExpectedDist )
	{
		SetSolveCookie();
		return DbgResult( AIMR_OK );
	}

	// --------------------------------------------------

	DebugNoteMovementFailure();
	SetSolveCookie();
	return DbgResult( IsMoveBlocked( pMoveGoal->directTrace.fStatus ) ? pMoveGoal->directTrace.fStatus : AIMR_ILLEGAL );
}

//-------------------------------------

AIMoveResult_t CAI_LocalNavigator::MoveCalc( AILocalMoveGoal_t *pMoveGoal, bool bPreviouslyValidated )
{
	bool bOnlyCurThink = ( bPreviouslyValidated && !HaveObstacles() );

	AIMoveResult_t result = MoveCalcRaw( pMoveGoal, bOnlyCurThink );

	if ( pMoveGoal->curExpectedDist > pMoveGoal->maxDist )
		pMoveGoal->curExpectedDist = pMoveGoal->maxDist;

	// If success, try to dampen really fast turning movement
	if ( result == AIMR_OK)
	{
		float interval = GetOuter()->GetMotor()->GetMoveInterval();
		float currentYaw = UTIL_AngleMod( GetLocalAngles().y );
		float goalYaw;
		float deltaYaw;
		float speed;
		float clampedYaw;

		// Clamp yaw
		goalYaw = UTIL_VecToYaw( pMoveGoal->facing );
		deltaYaw = fabs( UTIL_AngleDiff( goalYaw, currentYaw ) );
		if ( deltaYaw > 15 )
		{
			speed = deltaYaw * 4.0; // i.e., any maneuver takes a quarter a second
			clampedYaw = AI_ClampYaw( speed, currentYaw, goalYaw, interval );

			if ( clampedYaw != goalYaw )
			{
				pMoveGoal->facing = UTIL_YawToVector( clampedYaw );
			}
		}
	}
	
	return result;
}
//-----------------------------------------------------------------------------