//========= Copyright © 1996-2005, Valve Corporation, All rights reserved. ============//
//
// Purpose: 
//
// $NoKeywords: $
//
//=============================================================================//
// nav_node.cpp
// AI Navigation Nodes
// Author: Michael S. Booth (mike@turtlerockstudios.com), January 2003

#include "cbase.h"
#include "nav_node.h"
#include "nav_colors.h"
#include "nav_mesh.h"
#include "tier1/utlhash.h"
#include "tier1/generichash.h"

// NOTE: This has to be the last file included!
#include "tier0/memdbgon.h"


NavDirType Opposite[ NUM_DIRECTIONS ] = { SOUTH, WEST, NORTH, EAST };

CNavNode *CNavNode::m_list = NULL;
unsigned int CNavNode::m_listLength = 0;
unsigned int CNavNode::m_nextID = 1;

extern Vector NavTraceMins;
extern Vector NavTraceMaxs;

//--------------------------------------------------------------------------------------------------------------
// Node hash

class CNodeHashFuncs
{
public:
	CNodeHashFuncs( int ) {}

	bool operator()( const CNavNode *pLhs, const CNavNode *pRhs ) const
	{
		return pRhs->GetPosition()->AsVector2D() == pLhs->GetPosition()->AsVector2D();
	}

	unsigned int operator()( const CNavNode *pItem ) const
	{
		return Hash8( &pItem->GetPosition()->AsVector2D() );	
	}
};

CUtlHash<CNavNode *, CNodeHashFuncs, CNodeHashFuncs> *g_pNavNodeHash;


//--------------------------------------------------------------------------------------------------------------
/**
 * Constructor
 */
CNavNode::CNavNode( const Vector &pos, const Vector &normal, CNavNode *parent, bool isOnDisplacement )
{
	m_pos = pos;
	m_normal = normal;

	m_id = m_nextID++;

	int i;
	for( i=0; i<NUM_DIRECTIONS; ++i )
	{
		m_to[ i ] = NULL;
		m_obstacleHeight[ i ] = 0;
		m_obstacleStartDist[ i ] = 0;
		m_obstacleEndDist[ i ] = 0;
	}

	for ( i=0; i<NUM_CORNERS; ++i )
	{
		m_crouch[ i ] = false;
		m_isBlocked[ i ] = false;
	}

	m_visited = 0;
	m_parent = parent;

	m_next = m_list;
	m_list = this;
	m_listLength++;

	m_isCovered = false;
	m_area = NULL;

	m_attributeFlags = 0;

	m_isOnDisplacement = isOnDisplacement;

	if ( !g_pNavNodeHash )
	{
		g_pNavNodeHash = new CUtlHash<CNavNode *, CNodeHashFuncs, CNodeHashFuncs>( 16*1024 );
	}

	bool bDidInsert;
	UtlHashHandle_t hHash = g_pNavNodeHash->Insert( this, &bDidInsert );
	if ( !bDidInsert )
	{
		CNavNode *pExistingNode = g_pNavNodeHash->Element( hHash );
		m_nextAtXY = pExistingNode;
		g_pNavNodeHash->Element( hHash ) = this;
	}
	else
	{
		m_nextAtXY = NULL;
	}
}

CNavNode::~CNavNode()
{
}


//--------------------------------------------------------------------------------------------------------------
void CNavNode::CleanupGeneration()
{
	delete g_pNavNodeHash;
	g_pNavNodeHash = NULL;

	CNavNode *node, *next;
	for( node = CNavNode::m_list; node; node = next )
	{
		next = node->m_next;
		delete node;
	}
	CNavNode::m_list = NULL;
	CNavNode::m_listLength = 0;
	CNavNode::m_nextID = 1;
}

//--------------------------------------------------------------------------------------------------------------
#if DEBUG_NAV_NODES
ConVar nav_show_nodes( "nav_show_nodes", "0", FCVAR_CHEAT );
ConVar nav_show_node_id( "nav_show_node_id", "0", FCVAR_CHEAT );
ConVar nav_test_node( "nav_test_node", "0", FCVAR_CHEAT );
ConVar nav_test_node_crouch( "nav_test_node_crouch", "0", FCVAR_CHEAT );
ConVar nav_test_node_crouch_dir( "nav_test_node_crouch_dir", "4", FCVAR_CHEAT );
ConVar nav_show_node_grid( "nav_show_node_grid", "0", FCVAR_CHEAT );
#endif // DEBUG_NAV_NODES


//--------------------------------------------------------------------------------------------------------------
void CNavNode::Draw( void )
{
#if DEBUG_NAV_NODES

	if ( !nav_show_nodes.GetBool() )
		return;

	int r = 0, g = 0, b = 0;

	if ( m_isCovered )
	{
		if ( GetAttributes() & NAV_MESH_CROUCH )
		{
			b = 255;
		}
		else
		{
			r = 255;
		}
	}
	else
	{
		if ( GetAttributes() & NAV_MESH_CROUCH )
		{
			b = 255;
		}
		g = 255;
	}

	NDebugOverlay::Cross3D( m_pos, 2, r, g, b, true, 0.1f );

	if ( (!m_isCovered && nav_show_node_id.GetBool()) || (m_isCovered && nav_show_node_id.GetInt() < 0) )
	{
		char text[16];
		Q_snprintf( text, sizeof( text ), "%d", m_id );
		NDebugOverlay::Text( m_pos, text, true, 0.1f );
	}

	if ( (unsigned int)(nav_test_node.GetInt()) == m_id )
	{
		TheNavMesh->TestArea( this, 1, 1 );
		nav_test_node.SetValue( 0 );
	}

	if ( (unsigned int)(nav_test_node_crouch.GetInt()) == m_id )
	{
		CheckCrouch();
		nav_test_node_crouch.SetValue( 0 );
	}

	if ( GetAttributes() & NAV_MESH_CROUCH )
	{
		int i;
		for( i=0; i<NUM_CORNERS; i++ )
		{
			if ( m_isBlocked[i] || m_crouch[i] )
			{
				Vector2D dir;
				CornerToVector2D( (NavCornerType)i, &dir );

				const float scale = 3.0f;
				Vector scaled( dir.x * scale, dir.y * scale, 0 );

				if ( m_isBlocked[i] )
				{
					NDebugOverlay::HorzArrow( m_pos, m_pos + scaled, 0.5, 255, 0, 0, 255, true, 0.1f );
				}
				else
				{
					NDebugOverlay::HorzArrow( m_pos, m_pos + scaled, 0.5, 0, 0, 255, 255, true, 0.1f );
				}
			}
		}
	}

	if ( nav_show_node_grid.GetBool() )
	{
		for ( int i = NORTH; i < NUM_DIRECTIONS; i++ )
		{
			CNavNode *nodeNext = GetConnectedNode( (NavDirType) i );
			if ( nodeNext )
			{
				NDebugOverlay::Line( *GetPosition(), *nodeNext->GetPosition(), 255, 255, 0, false, 0.1f );

				float obstacleHeight = m_obstacleHeight[i];
				if ( obstacleHeight > 0 )
				{
					float z = GetPosition()->z + obstacleHeight;
					Vector from = *GetPosition();
					Vector to = from;
					AddDirectionVector( &to, (NavDirType) i, m_obstacleStartDist[i] );
					NDebugOverlay::Line( from, to, 255, 0, 255, false, 0.1f );
					from = to;
					to.z = z;
					NDebugOverlay::Line( from, to, 255, 0, 255, false, 0.1f );
					from = to;
					to = *GetPosition();
					to.z = z;
					AddDirectionVector( &to, (NavDirType) i, m_obstacleEndDist[i] );
					NDebugOverlay::Line( from, to, 255, 0, 255, false, 0.1f );
				}				
			}
		}
	}
	

#endif // DEBUG_NAV_NODES
}


//--------------------------------------------------------------------------------------------------------
// return ground height above node in given corner direction (NUM_CORNERS for highest in any direction)
float CNavNode::GetGroundHeightAboveNode( NavCornerType cornerType ) const
{
	if ( cornerType >= 0 && cornerType < NUM_CORNERS )
		return m_groundHeightAboveNode[ cornerType ];

	float blockedHeight = 0.0f;
	for ( int i=0; i<NUM_CORNERS; ++i )
	{
		blockedHeight = MAX( blockedHeight, m_groundHeightAboveNode[i] );
	}

	return blockedHeight;
}


//--------------------------------------------------------------------------------------------------------------
/**
 * Look up to JumpCrouchHeight in the air to see if we can fit a whole HumanHeight box
 */
bool CNavNode::TestForCrouchArea( NavCornerType cornerNum, const Vector& mins, const Vector& maxs, float *groundHeightAboveNode )
{
	CTraceFilterWalkableEntities filter( NULL, COLLISION_GROUP_PLAYER_MOVEMENT, WALK_THRU_EVERYTHING );
	trace_t tr;

	Vector start( m_pos );
	Vector end( start );
	end.z += JumpCrouchHeight;
	UTIL_TraceHull( start, end, NavTraceMins, NavTraceMaxs, MASK_NPCSOLID_BRUSHONLY, &filter, &tr );

	float maxHeight = tr.endpos.z - start.z;

	Vector realMaxs( maxs );

	for ( float height = 0; height <= maxHeight; height += 1.0f )
	{
		start = m_pos;
		start.z += height;

		realMaxs.z = HumanCrouchHeight;
		UTIL_TraceHull( start, start, mins, realMaxs, MASK_NPCSOLID_BRUSHONLY, &filter, &tr );
		if ( !tr.startsolid )
		{
			*groundHeightAboveNode = start.z - m_pos.z;

			// We found a crouch-sized space.  See if we can stand up.
			realMaxs.z = HumanHeight;
			UTIL_TraceHull( start, start, mins, realMaxs, MASK_NPCSOLID_BRUSHONLY, &filter, &tr );
			if ( !tr.startsolid )
			{
				// We found a crouch-sized space.  See if we can stand up.
#if DEBUG_NAV_NODES
				if ( (unsigned int)(nav_test_node_crouch.GetInt()) == GetID() )
				{
					NDebugOverlay::Box( start, mins, maxs, 0, 255, 255, 100, 100 );
				}
#endif // DEBUG_NAV_NODES
				return true;
			}
#if DEBUG_NAV_NODES
			if ( (unsigned int)(nav_test_node_crouch.GetInt()) == GetID() )
			{
				NDebugOverlay::Box( start, mins, maxs, 255, 0, 0, 100, 100 );
			}
#endif // DEBUG_NAV_NODES

			return false;
		}
	}

	*groundHeightAboveNode = JumpCrouchHeight;
	m_isBlocked[ cornerNum ] = true;
	return false;
}


//--------------------------------------------------------------------------------------------------------------
void CNavNode::CheckCrouch( void )
{
	// For each direction, trace upwards from our best ground height to VEC_HULL_MAX.z to see if we have standing room.
	for ( int i=0; i<NUM_CORNERS; ++i )
	{
#if DEBUG_NAV_NODES
		if ( nav_test_node_crouch_dir.GetInt() != NUM_CORNERS && i != nav_test_node_crouch_dir.GetInt() )
			continue;
#endif // DEBUG_NAV_NODES

		NavCornerType corner = (NavCornerType)i;
		Vector2D cornerVec;
		CornerToVector2D( corner, &cornerVec );

		// Build a mins/maxs pair for the HumanWidth x HalfHumanWidth box facing the appropriate direction
		Vector mins( 0, 0, 0 );
		Vector maxs( 0, 0, 0 );
		if ( cornerVec.x < 0 )
		{
			mins.x = -HalfHumanWidth;
		}
		else if ( cornerVec.x > 0 )
		{
			maxs.x = HalfHumanWidth;
		}
		if ( cornerVec.y < 0 )
		{
			mins.y = -HalfHumanWidth;
		}
		else if ( cornerVec.y > 0 )
		{
			maxs.y = HalfHumanWidth;
		}
		maxs.z = HumanHeight;

		// now make sure that mins is smaller than maxs
		for ( int j=0; j<3; ++j )
		{
			if ( mins[j] > maxs[j] )
			{
				float tmp = mins[j];
				mins[j] = maxs[j];
				maxs[j] = tmp;
			}
		}

		if ( !TestForCrouchArea( corner, mins, maxs, &m_groundHeightAboveNode[i] ) )
		{
			SetAttributes( NAV_MESH_CROUCH );
			m_crouch[corner] = true;
		}
	}
}


//--------------------------------------------------------------------------------------------------------------
/**
 * Create a connection FROM this node TO the given node, in the given direction
 */
void CNavNode::ConnectTo( CNavNode *node, NavDirType dir, float obstacleHeight, float obstacleStartDist, float obstacleEndDist )
{
	Assert( obstacleStartDist >= 0 && obstacleStartDist <= GenerationStepSize );
	Assert( obstacleEndDist >= 0 && obstacleStartDist <= GenerationStepSize );
	Assert( obstacleStartDist < obstacleEndDist );

	m_to[ dir ] = node;
	m_obstacleHeight[ dir ] = obstacleHeight;
	m_obstacleStartDist[ dir ] = obstacleStartDist;
	m_obstacleEndDist[ dir ] = obstacleEndDist;
}

//--------------------------------------------------------------------------------------------------------------
/**
 * Return node at given position.
 * @todo Need a hash table to make this lookup fast
 */
CNavNode *CNavNode::GetNode( const Vector &pos )
{
	const float tolerance = 0.45f * GenerationStepSize;			// 1.0f
	CNavNode *pNode = NULL;
	if ( g_pNavNodeHash )
	{
		static CNavNode lookup;
		lookup.m_pos = pos;
		UtlHashHandle_t hNode = g_pNavNodeHash->Find( &lookup );

		if ( hNode != g_pNavNodeHash->InvalidHandle() )
		{
			for( pNode = g_pNavNodeHash->Element( hNode ); pNode; pNode = pNode->m_nextAtXY )
			{
				float dz = fabs( pNode->m_pos.z - pos.z );

				if (dz < tolerance)
				{
					break;
				}
			}
		}
	}

#ifdef DEBUG_NODE_HASH
	CNavNode *pTestNode = NULL;
	for( CNavNode *node = m_list; node; node = node->m_next )
	{
		float dx = fabs( node->m_pos.x - pos.x );
		float dy = fabs( node->m_pos.y - pos.y );
		float dz = fabs( node->m_pos.z - pos.z );

		if (dx < tolerance && dy < tolerance && dz < tolerance)
		{
			pTestNode = node;
			break;
		}
	}
	AssertFatal( pTestNode == pNode );
#endif

	return pNode;
}

//--------------------------------------------------------------------------------------------------------------
/**
 * Return true if this node is bidirectionally linked to 
 * another node in the given direction
 */
BOOL CNavNode::IsBiLinked( NavDirType dir ) const
{
	if (m_to[ dir ] && m_to[ dir ]->m_to[ Opposite[dir] ] == this)
	{
		return true;
	}

	return false;
}

//--------------------------------------------------------------------------------------------------------------
/**
 * Return true if this node is the NW corner of a quad of nodes
 * that are all bidirectionally linked.
 */
BOOL CNavNode::IsClosedCell( void ) const
{
	if (IsBiLinked( SOUTH ) &&
		IsBiLinked( EAST ) &&
		m_to[ EAST ]->IsBiLinked( SOUTH ) &&
		m_to[ SOUTH ]->IsBiLinked( EAST ) &&
		m_to[ EAST ]->m_to[ SOUTH ] == m_to[ SOUTH ]->m_to[ EAST ])
	{
		return true;
	}

	return false;
}