//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
//
// Purpose: 
// $NoKeywords: $
//=============================================================================//

#include "cbase.h"
#include "bot/bot_navigator.h"
#include "bot/bot_pathfinder.h"
#include "in_buttons.h"

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

CBotNavigator::CBotNavigator( CBot *pOuter ) :	BaseClass(pOuter)
{
	m_fGoalTolerance = 32.0f;
	m_fIdealMoveSpeed = 420.0f;
	m_bGoalReached = true;
	m_bNewGoal = false;
	m_bReachedInternGoal = true;
}

bool CBotNavigator::Move( float flInterval )
{
	// clear old
	GetOuter()->m_flForwardMove = 0.0f;
	GetOuter()->m_flSideMove = 0.0f;
	GetOuter()->m_iButtons = 0;

	// move
	Vector substract = GetGoal() - GetAbsOrigin();
	if( !m_bGoalReached && substract.Length() < m_fGoalTolerance )
	{
		m_bGoalReached = true;
	}

	if( m_bNewGoal )
	{
		m_bNewGoal = false;
		GetOuter()->GetPathfinder()->FindPath(GetGoal());
	}

	if( !m_bGoalReached )
	{
		// if reached, say we reached it. Otherwise move in the direction of the point.
		substract = m_vInternNextGoal - GetAbsOrigin();
		if( !m_bReachedInternGoal && substract.Length() < m_fGoalTolerance )
		{
			m_bReachedInternGoal = true;
		}
		else
		{
			MoveTo(m_vInternNextGoal );
		}

		// get a new point
		if( m_bReachedInternGoal )
		{
			if( GetOuter()->GetPathfinder()->GetNextWayPoint( m_vInternNextGoal ) )
			{
				m_bReachedInternGoal = false;
				GetOuter()->SetLookAtTarget( m_vInternNextGoal );
			}
			else
			{
				MoveTo(GetGoal() );
				GetOuter()->SetLookAtTarget( GetGoal() );
			}
		}
	}

	UpdateAngles();

	GetOuter()->m_iButtons |= IN_SPEED;

	return true;
}

void CBotNavigator::UpdateAngles()
{
	float realYaw;
	QAngle idealAngles;
	QAngle angles = GetOuter()->GetLocalAngles();
	VectorAngles(GetOuter()->GetLookAtTarget()-GetOuter()->EyePosition(),idealAngles);

	realYaw = angles[YAW];
	angles[YAW] = 0.0f;
	if( fabs( idealAngles[YAW] - realYaw )  < fabs( realYaw + idealAngles[YAW] ) )
		idealAngles[YAW] -= realYaw;
	else
		idealAngles[YAW] += realYaw;

	// check difference
	if (fabs(idealAngles[YAW]) <= 0.1 )
		return;

	//Msg("angle: %f, ideal: %f\n", angles[YAW], idealAngles[YAW]);

	if( idealAngles[YAW] > 8.0f )
		angles[YAW] -= 8.0f;
	else if( idealAngles[YAW] < -8.0f )
		angles[YAW] += 8.0f;
	else
		angles[YAW] -= idealAngles[YAW];

	angles[YAW] += realYaw;
	GetOuter()->m_vViewAngles = angles;
}

void CBotNavigator::MoveTo( Vector goal )
{
	Vector substract = goal - GetAbsOrigin();
	QAngle angles = GetAbsAngles();
	QAngle qAngles;
	float fAngle;
	float flMove = 0.0;
	float flSide = 0.0;
	// we need to move to the goal!
	VectorAngles(substract, qAngles);

	// NEED to put 0-360 angles from -180 to 180 to get negative and positive values
	NormalizeFloatAngle(&angles.y);
	NormalizeFloatAngle(&angles.y);
	fAngle = angles.y - qAngles.y;

	NormalizeFloatAngle(&fAngle);

	/////////
	float radians = fAngle * 3.141592f / 180.0f; // degrees to radians
	// fl Move is percentage (0 to 1) of forward speed,
	// flSide is percentage (0 to 1) of side speed.
	flMove = cos(radians);
	flSide = sin(radians);

	GetOuter()->m_flForwardMove = m_fIdealMoveSpeed * flMove;
	GetOuter()->m_flSideMove = m_fIdealMoveSpeed * flSide;
}

void CBotNavigator::SetGoal( Vector goal, float tolerance )
{
	m_vGoalPosition = goal;
	m_fGoalTolerance = tolerance;
	m_bGoalReached = false;
	m_bNewGoal = true;
}

void CBotNavigator::NormalizeFloatAngle ( float *fAngle ) 
{ 
	if ( *fAngle > 180 ) 
	{
		*fAngle = *fAngle - 360;
	} 
	else if ( *fAngle < -180 )
	{
		*fAngle = *fAngle + 360;
	}
}