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

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

// support for nav mesh
#include "nav.h"
#include "nav_mesh.h"
#include "nav_pathfind.h"
#include "nav_area.h"

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

CBotPathfinder::CBotPathfinder( CBot *pOuter ) : BaseClass(pOuter)
{
}

//-----------------------------------------------------------------------------
// Purpose: Builds a route to the given vecGoal using the navigation mesh result
//			(the nav mesh is using the A* pathfinding algoritme )
//-----------------------------------------------------------------------------
void CBotPathfinder::BuildNavRoute( CNavArea *startArea, CNavArea *goalArea, const Vector &from, Navigation_t curNavType )
{
	if( !goalArea )
		return;

	Vector center, center_portal, delta;
	float hwidth, hwidthhull;
	NavDirType dir;

	Vector closestpoint;
	if( goalArea->GetParent() )
	{
		center = goalArea->GetParent()->GetCenter();
		dir = goalArea->ComputeDirection(&center);	
		goalArea->ComputePortal( goalArea->GetParent(), dir, &center_portal, &hwidth );
		goalArea->ComputeClosestPointInPortal( goalArea->GetParent(), dir, goalArea->GetParent()->GetCenter(), &closestpoint );

		// this point would be the closest route. But does or our hull fits?
		trace_t trace;
		CTraceFilterWorldOnly traceFilter;
		AI_TraceHull( closestpoint, closestpoint, GetOuter()->WorldAlignMins(), GetOuter()->WorldAlignMaxs(), MASK_SOLID, &traceFilter, &trace );
		if ( trace.fraction != 1.0f )
		{
			// move a bit to the center
			delta = closestpoint - center_portal;
			hwidthhull = NAI_Hull::Width(GetOuter()->GetHullType()) / 2.0f;
			if( delta.IsLengthGreaterThan( hwidthhull ) )
			{
				closestpoint = closestpoint + ( (hwidthhull / delta.Length() ) * delta  );	
			}
		}

		// TODO: try to make the connection look less forced by randomly moving it a bit.


		closestpoint.z = goalArea->GetZ( closestpoint );
		BuildNavRoute( startArea, goalArea->GetParent(), closestpoint, curNavType );
		
		m_Waypoints.AddToTail( closestpoint );
	}

}

bool CBotPathfinder::FindPath( Vector goal )
{
	CNavArea *closestArea = NULL;
	CNavArea *startArea = TheNavMesh->GetNearestNavAreaB(GetAbsOrigin() );
	CNavArea *goalArea = TheNavMesh->GetNearestNavAreaB(goal);

	ShortestPathCost costfunc;

	if( NavAreaBuildPath( startArea, goalArea, &goal, costfunc, &closestArea) )
	{
		m_Waypoints.Purge();
		BuildNavRoute(startArea,closestArea,GetAbsOrigin(), NAV_GROUND);
		return true;
	}

	return false;
}

bool CBotPathfinder::GetNextWayPoint( Vector &waypoint )
{
	if( m_Waypoints.Count() == 0 )
		return false;

	VectorCopy( m_Waypoints.Head(), waypoint);
	m_Waypoints.Remove(0);

	return true;
}