// leave this line at the top for all g_xxxx.cpp files...
#include "g_headers.h"
#include <algorithm>

#include "b_local.h"
#include "g_navigator.h"
#include "g_nav.h"
#include <time.h>



extern int NAVNEW_ClearPathBetweenPoints(vec3_t start, vec3_t end, vec3_t mins, vec3_t maxs, int ignore, int clipmask);
extern qboolean G_EntIsUnlockedDoor( int entityNum );
extern qboolean G_EntIsDoor( int entityNum );
extern qboolean G_EntIsBreakable( int entityNum );
extern qboolean G_EntIsRemovableUsable( int entNum );

extern	cvar_t		*d_altRoutes;


static vec3_t	wpMaxs = {  16,  16, 32 };
static vec3_t	wpMins = { -16, -16, -24+STEPSIZE };//WTF:  was 16??!!!


static byte CHECKED_NO = 0;
static byte CHECKED_FAILED = 1;
static byte CHECKED_PASSED = 2;
/*
-------------------------
CEdge
-------------------------
*/

CEdge::CEdge( void )
{
	CEdge( -1, -1, -1 );
}

CEdge::CEdge( int first, int second, int cost )
{
	m_first		= first;
	m_second	= second;
	m_cost		= cost;
}

CEdge::~CEdge( void )
{
}

/*
-------------------------
CNode
-------------------------
*/

CNode::CNode( void )
{
	m_numEdges		= 0;
	m_radius		= 0;
	m_ranks			= NULL;
}

CNode::~CNode( void )
{
	m_edges.clear();

	if ( m_ranks )
		delete [] m_ranks;
}

/*
-------------------------
Create
-------------------------
*/

CNode *CNode::Create( vec3_t position, int flags, int radius, int ID )
{
	CNode	*node = new CNode;

	VectorCopy( position, node->m_position );
	
	node->m_flags = flags;
	node->m_ID = ID;
	node->m_radius = radius;

	return node;
}

/*
-------------------------
Create
-------------------------
*/

CNode *CNode::Create( void )
{
	return new CNode;
}

/*
-------------------------
AddEdge
-------------------------
*/

void CNode::AddEdge( int ID, int cost, int flags )
{
	edge_t	edge;

	edge.ID		= ID;
	edge.cost	= cost;
	edge.flags	= flags;

	STL_INSERT( m_edges, edge );

	m_numEdges++;
}

/*
-------------------------
GetEdge
-------------------------
*/

int CNode::GetEdgeNumToNode( int ID )
{
	int count = 0;

	edge_v::iterator	ei;
	STL_ITERATE( ei, m_edges )
	{
		if ( (*ei).ID == ID )
		{
			return count;
		}

		count++;
	}
	return -1;
}

/*
-------------------------
AddRank
-------------------------
*/

void CNode::AddRank( int ID, int rank )
{
	assert( m_ranks );

	m_ranks[ ID ] = rank;
}

/*
-------------------------
Draw
-------------------------
*/

void CNode::Draw( qboolean showRadius )
{
	CG_DrawNode( m_position, NODE_NORMAL );
	if( showRadius )
	{
		CG_DrawRadius( m_position, m_radius, NODE_NORMAL );
	}
}

/*
-------------------------
GetEdge
-------------------------
*/

int CNode::GetEdge( int edgeNum )
{
	if ( edgeNum > m_numEdges )
		return -1;

	int count = 0;

	edge_v::iterator	ei;
	STL_ITERATE( ei, m_edges )
	{
		if ( count == edgeNum )
		{
			return (*ei).ID;
		}

		count++;
	}

	return -1;
}

/*
-------------------------
GetEdgeCost
-------------------------
*/

int CNode::GetEdgeCost( int edgeNum )
{
	if ( edgeNum > m_numEdges )
		return Q3_INFINITE; // return -1;

	int count = 0;

	edge_v::iterator	ei;
	STL_ITERATE( ei, m_edges )
	{
		if ( count == edgeNum )
		{
			return (*ei).cost;
		}

		count++;
	}

	return Q3_INFINITE; // return -1;
}

/*
-------------------------
GetEdgeFlags
-------------------------
*/

BYTE CNode::GetEdgeFlags( int edgeNum )
{
	if ( edgeNum > m_numEdges )
		return 0;

	int count = 0;

	edge_v::iterator	ei;
	STL_ITERATE( ei, m_edges )
	{
		if ( count == edgeNum )
		{
			return (*ei).flags;
		}

		count++;
	}
	
	return 0;
}

/*
-------------------------
SetEdgeFlags
-------------------------
*/

void CNode::SetEdgeFlags( int edgeNum, int newFlags )
{
	if ( edgeNum > m_numEdges )
		return;

	int count = 0;

	edge_v::iterator	ei;
	STL_ITERATE( ei, m_edges )
	{
		if ( count == edgeNum )
		{
			(*ei).flags = newFlags;
			return;
		}

		count++;
	}

}
/*
-------------------------
InitRanks
-------------------------
*/

void CNode::InitRanks( int size )
{
	//Clear it if it's already allocated
	if ( m_ranks != NULL )
	{
		delete [] m_ranks;
		m_ranks = NULL;
	}

	m_ranks = new int[size];

	memset( m_ranks, -1, sizeof(int)*size );
}

/*
-------------------------
GetRank
-------------------------
*/

int CNode::GetRank( int ID )
{
	assert( m_ranks );

	return m_ranks[ ID ];
}


/*
-------------------------
Save
-------------------------
*/

int	CNode::Save( int numNodes, fileHandle_t file )
{
	//Write out the header
	unsigned long header = NODE_HEADER_ID;
	gi.FS_Write( &header, sizeof( header ), file );

	//Write out the basic information
	for ( int i = 0; i < 3; i++ )
		gi.FS_Write( &m_position[i], sizeof( float ), file );

	gi.FS_Write( &m_flags, sizeof( m_flags ), file );
	gi.FS_Write( &m_ID, sizeof( m_ID ), file );
	gi.FS_Write( &m_radius, sizeof( m_radius ), file );

	//Write out the edge information
	gi.FS_Write( &m_numEdges, sizeof( m_numEdges ), file );

	edge_v::iterator	ei;
	STL_ITERATE( ei, m_edges )
	{
		gi.FS_Write( &(*ei), sizeof( edge_t ), file );
	}

	//Write out the node ranks
	gi.FS_Write( &numNodes, sizeof( numNodes ), file );

	for ( i = 0; i < numNodes; i++ )
	{
		gi.FS_Write( &m_ranks[i], sizeof( int ), file );
	}

	return true;
}

/*
-------------------------
Load
-------------------------
*/

int CNode::Load( int numNodes, fileHandle_t file )
{
	unsigned long header;
	gi.FS_Read( &header, sizeof(header), file );

	//Validate the header
	if ( header != NODE_HEADER_ID )
		return false;

	//Get the basic information
	for ( int i = 0; i < 3; i++ )
		gi.FS_Read( &m_position[i], sizeof( float ), file );

	gi.FS_Read( &m_flags, sizeof( m_flags ), file );
	gi.FS_Read( &m_ID, sizeof( m_ID ), file );
	gi.FS_Read( &m_radius, sizeof( m_radius ), file );

	//Get the edge information
	gi.FS_Read( &m_numEdges, sizeof( m_numEdges ), file );

	for ( i = 0; i < m_numEdges; i++ )
	{
		edge_t	edge;

		gi.FS_Read( &edge, sizeof( edge_t ), file );

		STL_INSERT( m_edges, edge );
	}

	//Read the node ranks
	int	numRanks;

	gi.FS_Read( &numRanks, sizeof( numRanks ), file );

	//Allocate the memory
	InitRanks( numRanks );

	for ( i = 0; i < numRanks; i++ )
	{
		gi.FS_Read( &m_ranks[i], sizeof( int ), file );
	}

	return true;
}

/*
-------------------------
CNavigator
-------------------------
*/

CNavigator::CNavigator( void )
{
}

CNavigator::~CNavigator( void )
{
}

/*
-------------------------
FlagAllNodes
-------------------------
*/

void CNavigator::FlagAllNodes( int newFlag )
{
	node_v::iterator	ni;

	STL_ITERATE( ni, m_nodes )
	{
		(*ni)->AddFlag( newFlag );
	}
}

/*
-------------------------
GetChar
-------------------------
*/

char CNavigator::GetChar( fileHandle_t file )
{
	char value;

	gi.FS_Read( &value, sizeof( value ), file );

	return value;
}

/*
-------------------------
GetInt
-------------------------
*/

int	CNavigator::GetInt( fileHandle_t file )
{
	int value;

	gi.FS_Read( &value, sizeof( value ), file );

	return value;
}

/*
-------------------------
GetFloat
-------------------------
*/

float CNavigator::GetFloat( fileHandle_t file )
{
	float value;

	gi.FS_Read( &value, sizeof( value ), file );

	return value;
}

/*
-------------------------
GetLong
-------------------------
*/

long CNavigator::GetLong( fileHandle_t file )
{
	long value;

	gi.FS_Read( &value, sizeof( value ), file );

	return value;
}

/*
-------------------------
Init
-------------------------
*/

void CNavigator::Init( void )
{
	Free();
}

/*
-------------------------
Free
-------------------------
*/

void CNavigator::Free( void )
{
	node_v::iterator	ni;

	STL_ITERATE( ni, m_nodes )
	{
		delete (*ni);
	}
}

/*
-------------------------
Load
-------------------------
*/

bool CNavigator::Load( const char *filename, int checksum )
{
	fileHandle_t	file;

	//Attempt to load the file
	gi.FS_FOpenFile( va( "maps/%s.nav", filename ), &file, FS_READ );

	//See if we succeeded
	if ( file == NULL )
		return false;

	//Check the header id
	long navID = GetLong( file );

	if ( navID != NAV_HEADER_ID )
	{
		gi.FS_FCloseFile( file );
		return false;
	}

	//Check the checksum to see if this file is out of date
	int check = GetInt( file );

	if ( check != checksum )
	{
		gi.FS_FCloseFile( file );
		return false;
	}

	int numNodes = GetInt( file );
	
	for ( int i = 0; i < numNodes; i++ )
	{
		CNode	*node = CNode::Create();

		if ( node->Load( numNodes, file ) == false )
		{
			gi.FS_FCloseFile( file );
			return false;
		}

		STL_INSERT( m_nodes, node );
	}

	//read in the failed edges
	gi.FS_Read( &failedEdges, sizeof( failedEdges ), file );

	gi.FS_FCloseFile( file );

	return true;
}

/*
-------------------------
Save
-------------------------
*/

bool CNavigator::Save( const char *filename, int checksum )
{
	fileHandle_t	file;

	//Attempt to load the file
	gi.FS_FOpenFile( va( "maps/%s.nav", filename ), &file, FS_WRITE );

	if ( file == NULL )
		return false;

	//Write out the header id
	unsigned long id = NAV_HEADER_ID;

	gi.FS_Write( &id, sizeof (id), file );

	//Write out the checksum
	gi.FS_Write( &checksum, sizeof( checksum ), file );

	int	numNodes = m_nodes.size();

	//Write out the number of nodes to follow
	gi.FS_Write( &numNodes, sizeof(numNodes), file );

	//Write out all the nodes
	node_v::iterator	ni;

	STL_ITERATE( ni, m_nodes )
	{
		(*ni)->Save( numNodes, file );
	}

	//write out failed edges
	gi.FS_Write( &failedEdges, sizeof( failedEdges ), file );

	gi.FS_FCloseFile( file );

	return true;
}

/*
-------------------------
AddRawPoint
-------------------------
*/

int CNavigator::AddRawPoint( vec3_t point, int flags, int radius )
{
	CNode	*node	= CNode::Create( point, flags, radius, m_nodes.size() );

	if ( node == NULL )
	{
		Com_Error( ERR_DROP, "Error adding node!\n" );
		return -1;
	}
	
	//TODO: Validate the position
	//TODO: Correct stuck waypoints

	STL_INSERT( m_nodes, node );

	return node->GetID();
}

/*
-------------------------
GetEdgeCost
-------------------------
*/

int	CNavigator::GetEdgeCost( CNode *first, CNode *second )
{
	trace_t	trace;
	vec3_t	start, end;
	vec3_t	mins, maxs;

	//Setup the player size
	VectorSet( mins, -8, -8, -8 );
	VectorSet( maxs,  8,  8,  8 );

	//Setup the points
	first->GetPosition( start );
	second->GetPosition( end );

	gi.trace( &trace, start, mins, maxs, end, ENTITYNUM_NONE, MASK_SOLID );

	if ( trace.fraction < 1.0f || trace.allsolid || trace.startsolid )
		return Q3_INFINITE; // return -1;

	//Connection successful, return the cost
	return Distance( start, end );
}

/*
-------------------------
ConnectNodes
-------------------------
*/

void CNavigator::ConnectNodes( void )
{
	node_v::iterator	ni, ni2;
	int					id = 0;
	int					cost;

	//Attempt to connect all nodes
	STL_ITERATE( ni, m_nodes )
	{
		//Attempt connection against all nodes in the system
		//TODO: Culling routines could speed this up
		STL_ITERATE( ni2, m_nodes )
		{
			if ( (*ni) == (*ni2 ) )
				continue;

			if ( EdgeFailed((*ni)->GetID(), (*ni2)->GetID()) )
			{
				continue;
			}
			cost = GetEdgeCost( (*ni), (*ni2) );

			//No connection was made
			if( cost >= Q3_INFINITE )	// GetEdgeCost used to return -1 as a fail, now it returns Q3_INFINITE
				continue;
		//	if ( cost < 0 )
		//		continue;

			//Connect the edges
			(*ni)->AddEdge( (*ni2)->GetID(), cost );
			(*ni2)->AddEdge( (*ni)->GetID(), cost );
		}

		id++;
	}
}

/*
-------------------------
AddNodeEdges
-------------------------
*/

void CNavigator::AddNodeEdges( CNode *node, int addDist, edge_l	&edgeList, bool *checkedNodes )
{
	//Add all edge
	for ( int i = 0; i < node->GetNumEdges(); i++ )
	{
		//Make sure we don't add an old edge twice
		if ( checkedNodes[ node->GetEdge( i ) ] == true )
			continue;

		//Get the node
		CNode	*nextNode = m_nodes[ node->GetEdge( i ) ];

		//This node has now been checked
		checkedNodes[ nextNode->GetID() ] = true;

		//Add it to the list
		STL_INSERT( edgeList, CEdge( nextNode->GetID(), node->GetID(), addDist + ( node->GetEdgeCost( i ) ) ) );
	}
}

/*
-------------------------
CalculatePath
-------------------------
*/

void CNavigator::CalculatePath( CNode *node )
{
	int	curRank = 0;

	CPriorityQueue	*pathList = new CPriorityQueue();
	BYTE			*checked;

	//Init the completion table
	checked = new BYTE[ m_nodes.size() ];
	memset( checked, 0, m_nodes.size() );

	//Mark this node as checked
	checked[ node->GetID() ] = true;
	node->AddRank( node->GetID(), curRank++ );

	//Add all initial nodes
	for ( int i = 0; i < node->GetNumEdges(); i++ )
	{
		CNode	*nextNode = m_nodes[ node->GetEdge(i) ];
		assert(nextNode);

		checked[ nextNode->GetID() ] = true;

		pathList->Push( new CEdge( nextNode->GetID(), nextNode->GetID(), node->GetEdgeCost(i) ) );
	}

	float				minDist;
	CEdge				*test;	

	//Now flood fill all the others
	while ( !pathList->Empty() )
	{
		minDist = Q3_INFINITE;			
		test	 = pathList->Pop();
		
		CNode	*testNode = m_nodes[ (*test).m_first ];
		assert( testNode );

		node->AddRank( testNode->GetID(), curRank++ );

		//Add in all the new edges
		for ( i = 0; i < testNode->GetNumEdges(); i++ )
		{
			CNode	*addNode = m_nodes[ testNode->GetEdge(i) ];
			assert( addNode );

			if ( checked[ addNode->GetID() ] )
				continue;

			int	newDist = (*test).m_cost + testNode->GetEdgeCost(i);
			pathList->Push( new CEdge( addNode->GetID(), (*test).m_second, newDist ) );

			checked[ addNode->GetID() ] = true;
		}
		delete test;

	}

	node->RemoveFlag( NF_RECALC );

	delete pathList;
	delete [] checked;
}

/*
-------------------------
CalculatePaths
-------------------------
*/
int GetTime ( int lastTime )
{
	int			curtime;
	static int	timeBase = 0;
	static qboolean	initialized = qfalse;

	if (!initialized) {
		timeBase = timeGetTime();
		initialized = qtrue;
	}
	curtime = timeGetTime() - timeBase - lastTime;

	return curtime;
}

extern void CP_FindCombatPointWaypoints( void );
void CNavigator::CalculatePaths( bool	recalc )
{
#ifndef FINAL_BUILD
	int	startTime = GetTime(0);
#endif
#if _HARD_CONNECT

#else

	ConnectNodes();

#endif	

	if (recalc)
	{
		ConnectNodes();
	}
	for ( int i = 0; i < m_nodes.size(); i++ )
	{
		//Allocate the needed memory
		m_nodes[i]->InitRanks( m_nodes.size() );
	}

	for ( i = 0; i < m_nodes.size(); i++ )
	{
		CalculatePath( m_nodes[i] );
	}
	
#ifndef FINAL_BUILD
	if ( pathsCalculated )
	{
		gi.Printf( S_COLOR_CYAN"%s recalced paths in %d ms\n", (NPC!=NULL?NPC->targetname:"NULL"), GetTime(startTime) );
	}
#endif
	
	if(!recalc)	//Mike says doesn't need to happen on recalc
	{
		CP_FindCombatPointWaypoints();
	}

	pathsCalculated = qtrue;
}

/*
-------------------------
RecalculatePaths
-------------------------
*/
/*
void CNavigator::RecalculatePaths( void )
{//Basically just verify the edges, ranks and paths?
	ConnectNodes();

	for ( int i = 0; i < m_nodes.size(); i++ )
	{
		//Allocate the needed memory
		m_nodes[i]->InitRanks( m_nodes.size() );
	}

	for ( i = 0; i < m_nodes.size(); i++ )
	{
		CalculatePath( m_nodes[i] );
	}
}
*/
/*
-------------------------
ShowNodes
-------------------------
*/

void CNavigator::ShowNodes( void )
{
	node_v::iterator	ni;

	vec3_t	position;
	qboolean showRadius;
	float	dist,
			radius;

	STL_ITERATE( ni, m_nodes )
	{
		(*ni)->GetPosition( position );

		if ( gi.inPVS( g_entities[0].currentOrigin, position ) )
		{
			showRadius = qfalse;
			if( NAVDEBUG_showRadius )
			{	
				
				dist = DistanceSquared( g_entities[0].currentOrigin, position );
				radius = (*ni)->GetRadius();
				// if player within node radius or 256, draw radius (sometimes the radius is really small, so we check for 256 to catch everything)
				if( (dist <= radius*radius) || dist <= 65536 ) 
				{
					showRadius = qtrue;
				}
			}
			(*ni)->Draw(showRadius);
		}
	}
}

/*
-------------------------
ShowEdges
-------------------------
*/

typedef	map < int, bool >		drawMap_m;

void CNavigator::ShowEdges( void )
{
	node_v::iterator	ni;
	vec3_t	start, end;

	drawMap_m	*drawMap;

	drawMap = new drawMap_m[ m_nodes.size() ];

	STL_ITERATE( ni, m_nodes )
	{
		//Attempt to draw each connection
		for ( int i = 0; i < (*ni)->GetNumEdges(); i++ )
		{
			int id = (*ni)->GetEdge( i );

			if ( id == -1 )
				continue;

			//Already drawn?
			if ( drawMap[(*ni)->GetID()].find( id ) != drawMap[(*ni)->GetID()].end() )
				continue;

			BYTE flags = (*ni)->GetEdgeFlags( i );

			CNode	*node = m_nodes[id];

			node->GetPosition( end );
			(*ni)->GetPosition( start );

			//Set this as drawn
			drawMap[id][(*ni)->GetID()] = true;

			if ( ( gi.inPVS( g_entities[0].currentOrigin, start ) == false ) && ( gi.inPVS( g_entities[0].currentOrigin, end ) == false ) )
				continue;

			if ( EdgeFailed( id, (*ni)->GetID() ) != -1 )//flags & EFLAG_FAILED )
				CG_DrawEdge( start, end, EDGE_FAILED );
			else if ( flags & EFLAG_BLOCKED )
				CG_DrawEdge( start, end, EDGE_BLOCKED );
			else
				CG_DrawEdge( start, end, EDGE_NORMAL );
		}
	}

	delete [] drawMap;
}

int CNavigator::GetNodeRadius( int nodeID )
{
	if ( m_nodes.size() == 0 )
		return 0;
	return m_nodes[nodeID]->GetRadius();
}

void CNavigator::CheckBlockedEdges( void )
{
	CNode	*start, *end;
	vec3_t	p1, p2;
	int		flags, first, second;
	trace_t	trace;
	qboolean failed;
	int		edgeNum;
	node_v::iterator	ni;

	//Go through all edges and test the ones that were blocked
	STL_ITERATE( ni, m_nodes )
	{
		//Attempt to draw each connection
		for ( edgeNum = 0; edgeNum < (*ni)->GetNumEdges(); edgeNum++ )
		{
			flags = (*ni)->GetEdgeFlags( edgeNum );
			if ( (flags&EFLAG_BLOCKED) )
			{
				first	= (*ni)->GetID();
				second	= (*ni)->GetEdge( edgeNum );
				start	= m_nodes[first];
				end		= m_nodes[second];
				failed	= qfalse;

				start->GetPosition( p1 );
				end->GetPosition( p2 );

				//FIXME: can't we just store the trace.entityNum from the HardConnect trace?  So we don't have to do another trace here...
				gi.trace( &trace, p1, wpMins, wpMaxs, p2, ENTITYNUM_NONE, MASK_SOLID|CONTENTS_MONSTERCLIP|CONTENTS_BOTCLIP );

				if ( trace.entityNum < ENTITYNUM_WORLD && (trace.fraction < 1.0f || trace.startsolid == qtrue || trace.allsolid == qtrue) )
				{//could be assumed, since failed before
					if ( G_EntIsDoor( trace.entityNum ) )
					{//door
						if ( !G_EntIsUnlockedDoor( trace.entityNum ) )
						{//locked door
							failed = qtrue;
						}
					}
					else
					{
						if ( G_EntIsBreakable( trace.entityNum ) )
						{//do same for breakable brushes/models/glass?
							failed = qtrue;
						}
						else if ( G_EntIsRemovableUsable( trace.entityNum ) )
						{
							failed = qtrue;
						}
						else if ( trace.allsolid || trace.startsolid )
						{//FIXME: the entitynum would be none here, so how do we know if this is stuck inside an ent or the world?
						}
						else
						{//FIXME: what about func_plats and scripted movers?
						}
					}
				}

				if ( failed )
				{
					//could add the EFLAG_FAILED to the two edges, but we stopped doing that since it was pointless
					AddFailedEdge( ENTITYNUM_NONE, first, second );
				}
			}
		}
	}
}

#if _HARD_CONNECT

/*
-------------------------
HardConnect
-------------------------
*/

void CNavigator::HardConnect( int first, int second )
{
	CNode	*start, *end;

	start	= m_nodes[first];
	end		= m_nodes[second];

	vec3_t	p1, p2;

	start->GetPosition( p1 );
	end->GetPosition( p2 );

	trace_t	trace;
	
	int		flags = EFLAG_NONE;

	gi.trace( &trace, p1, wpMins, wpMaxs, p2, ENTITYNUM_NONE, MASK_SOLID|CONTENTS_BOTCLIP|CONTENTS_MONSTERCLIP );

	int cost = Distance( p1, p2 );

	if ( trace.fraction != 1.0f || trace.startsolid == qtrue || trace.allsolid == qtrue )
	{
		flags |= EFLAG_BLOCKED;
	}

	start->AddEdge( second, cost, flags );
	end->AddEdge( first, cost, flags );
}

#endif

/*
-------------------------
TestNodePath
-------------------------
*/

int CNavigator::TestNodePath( gentity_t *ent, int okToHitEntNum, vec3_t position, qboolean includeEnts )
{
	int clipmask = ent->clipmask;

	if ( !includeEnts )
	{
		clipmask &= ~CONTENTS_BODY;
	}
	//Check the path
	if ( NAV_ClearPathToPoint( ent, ent->mins, ent->maxs, position, clipmask, okToHitEntNum ) == false )
		return false;
	
	return true;
}

/*
-------------------------
TestNodeLOS
-------------------------
*/

int CNavigator::TestNodeLOS( gentity_t *ent, vec3_t position )
{
	return NPC_ClearLOS( ent, position );
}

/*
-------------------------
TestBestFirst
-------------------------
*/

int	CNavigator::TestBestFirst( gentity_t *ent, int lastID, int flags )
{
	//Must be a valid one to begin with
	if ( lastID == NODE_NONE )
		return NODE_NONE;

	if ( lastID >= m_nodes.size() )
		return NODE_NONE;

	//Get the info
	vec3_t	nodePos;
	CNode	*node = m_nodes[ lastID ];
	CNode	*testNode;
	int		numEdges = node->GetNumEdges();
	float	dist;

	node->GetPosition( nodePos );

	//Setup our last node as our root, and search for a closer one according to its edges	
	int bestNode = ( TestNodePath( ent, ENTITYNUM_NONE, nodePos, qtrue ) ) ? lastID : NODE_NONE;
	float bestDist = ( bestNode == NODE_NONE ) ? Q3_INFINITE : DistanceSquared( ent->currentOrigin, nodePos );

	//Test all these edges first
	for ( int i = 0; i < numEdges; i++ )
	{
		//Get this node and its distance
		testNode = m_nodes[ node->GetEdge(i) ];

		if ( NodeFailed( ent, testNode->GetID() ) )
		{
			continue;
		}

		testNode->GetPosition( nodePos );

		dist = DistanceSquared( ent->currentOrigin, nodePos );

		//Test against current best
		if ( dist < bestDist )
		{
			//See if this node is valid
			if ( CheckedNode(testNode->GetID(),ent->s.number) == CHECKED_PASSED || TestNodePath( ent, ENTITYNUM_NONE, nodePos, qtrue ) )
			{
				bestDist = dist;
				bestNode = testNode->GetID();
				SetCheckedNode(testNode->GetID(),ent->s.number,CHECKED_PASSED);
			}
			else
			{
				SetCheckedNode(testNode->GetID(),ent->s.number,CHECKED_FAILED);
			}
		}	
	}

	return bestNode;
}

/*
-------------------------
CollectNearestNodes
-------------------------
*/

#define	NODE_COLLECT_MAX	16		//Maximum # of nodes collected at any time
#define NODE_COLLECT_RADIUS	512		//Default radius to search for nodes in
#define NODE_COLLECT_RADIUS_SQR		( NODE_COLLECT_RADIUS * NODE_COLLECT_RADIUS )

int CNavigator::CollectNearestNodes( vec3_t origin, int radius, int maxCollect, nodeChain_l &nodeChain )
{
	node_v::iterator	ni;
	float				dist;
	vec3_t				position;
	int					collected = 0;
	bool				added = false;

	//Get a distance rating for each node in the system
	STL_ITERATE( ni, m_nodes )
	{
		//If we've got our quota, then stop looking
		//Get the distance to the node
		(*ni)->GetPosition( position );
		dist = DistanceSquared( position, origin );

		//Must be within our radius range
		if ( dist > (float) ( radius * radius ) )
			continue;
		
		nodeList_t				nChain;
		nodeChain_l::iterator	nci;

		//Always add the first node
		if ( nodeChain.size() == 0 )
		{
			nChain.nodeID = (*ni)->GetID();
			nChain.distance = dist;

			nodeChain.insert( nodeChain.begin(), nChain );
			continue;
		}

		added = false;

		//Compare it to what we already have
		STL_ITERATE( nci, nodeChain )
		{
			//If we're less, than this entry, then insert before it
			if ( dist < (*nci).distance )
			{
				nChain.nodeID = (*ni)->GetID();
				nChain.distance = dist;

				nodeChain.insert( nci, nChain );
				collected = nodeChain.size();
				added = true;
				
				//If we've hit our collection limit, throw off the oldest one
				if ( nodeChain.size() > maxCollect )
				{
					nodeChain.pop_back();
				}

				break;
			}
		}

		//Otherwise, always pad out the collection if possible so we don't miss anything
		if ( ( added == false ) && ( nodeChain.size() < maxCollect ) )
		{
			nChain.nodeID = (*ni)->GetID();
			nChain.distance = dist;

			nodeChain.insert( nodeChain.end(), nChain );
		}
	}

	return collected;
}

int CNavigator::GetBestPathBetweenEnts( gentity_t *ent, gentity_t *goal, int flags )
{
	//Must have nodes
	if ( m_nodes.size() == 0 )
		return qfalse;

#define	MAX_Z_DELTA	18

	nodeChain_l				nodeChain;
	nodeChain_l::iterator	nci;
	nodeChain_l				nodeChain2;
	nodeChain_l::iterator	nci2;

	//Collect all nodes within a certain radius
	CollectNearestNodes( ent->currentOrigin, NODE_COLLECT_RADIUS, NODE_COLLECT_MAX, nodeChain );
	CollectNearestNodes( goal->currentOrigin, NODE_COLLECT_RADIUS, NODE_COLLECT_MAX, nodeChain2 );

	vec3_t				position;
	vec3_t				position2;
	int					radius;
	int					cost, pathCost, bestCost = Q3_INFINITE;
	CNode				*node, *node2;
	int					nodeNum, nodeNum2;
	int					nextNode = NODE_NONE, bestNode = NODE_NONE;
	int					nodeFlags = 0;
//	bool				recalc = false;

	ent->waypoint = NODE_NONE;
	goal->waypoint = NODE_NONE;

	//Look through all nodes
	STL_ITERATE( nci, nodeChain )
	{
		node = m_nodes[(*nci).nodeID];
		nodeNum = (*nci).nodeID;
		if ( d_altRoutes->integer )
		{
			//calc the paths for this node if they're out of date
			nodeFlags = node->GetFlags();
			if ( (nodeFlags&NF_RECALC) )
			{
				//gi.Printf( S_COLOR_CYAN"%d recalcing paths from node %d\n", level.time, nodeNum );
				CalculatePath( node );
			}
		}

		STL_ITERATE( nci2, nodeChain2 )
		{
			node2 = m_nodes[(*nci2).nodeID];
			nodeNum2 = (*nci2).nodeID;
			if ( d_altRoutes->integer )
			{
				//calc the paths for this node if they're out of date
				nodeFlags = node2->GetFlags();
				if ( (nodeFlags&NF_RECALC) )
				{
					//gi.Printf( S_COLOR_CYAN"%d recalcing paths from node %d\n", level.time, nodeNum2 );
					CalculatePath( node2 );
				}
			}

			node->GetPosition( position );
			node2->GetPosition( position2 );
			//Okay, first get the entire path cost, including distance to first node from ents' positions
			cost = floor(Distance( ent->currentOrigin, position ) + Distance( goal->currentOrigin, position2 ));

			if ( d_altRoutes->integer )
			{
				nextNode = GetBestNodeAltRoute( (*nci).nodeID, (*nci2).nodeID, &pathCost, bestNode );
				cost += pathCost;
			}
			else
			{
				cost += GetPathCost( (*nci).nodeID, (*nci2).nodeID );
			}

			if ( cost >= bestCost )
			{
				continue;
			}
			//okay, this is the shortest path we've found yet, check clear path, etc.

			if ( CheckedNode(nodeNum,ent->s.number) == CHECKED_FAILED )
			{//already checked this node against goal and it failed
//				recalc = true;
				continue;
			}
			if ( CheckedNode(nodeNum,ent->s.number) == CHECKED_PASSED )
			{//already checked this node against goal and it passed
			}
			else
			{//haven't checked this node against goal yet
				radius	= node->GetRadius();
				
				//If we're not within the known clear radius of this node OR out of Z height range...
				if ( (*nci).distance >= (radius*radius) || ( fabs( position[2] - ent->currentOrigin[2] ) >= MAX_Z_DELTA ) )
				{
					//We're not *within* this node, so check clear path, etc.

					//FIXME: any way to call G_FindClosestPointOnLineSegment and see if I can at least get to the waypoint's path
					if ( flags & NF_CLEAR_PATH )//|| flags & NF_CLEAR_LOS )
					{//need a clear path or LOS
						if ( !gi.inPVS( ent->currentOrigin, position ) )
						{//not even potentially clear
							SetCheckedNode(nodeNum,ent->s.number,CHECKED_FAILED);
							continue;
						}
					}

					//Do we need a clear path?
					if ( flags & NF_CLEAR_PATH )
					{
						if ( TestNodePath( ent, goal->s.number, position, qtrue ) == false )
						{
							SetCheckedNode(nodeNum,ent->s.number,CHECKED_FAILED);
							continue;
						}
					}

					//Do we need a clear line of sight?
					/*
					if ( flags & NF_CLEAR_LOS )
					{
						if ( TestNodeLOS( ent, position ) == false )
						{
							nodeChecked[nodeNum][ent->s.number] = CHECKED_FAILED;
							continue;
						}
					}
					*/
				}//otherwise, inside the node so it must be clear (?)
				SetCheckedNode(nodeNum,ent->s.number,CHECKED_PASSED);
			}

			if ( CheckedNode(nodeNum2,goal->s.number) == CHECKED_FAILED )
			{//already checked this node against goal and it failed
//				recalc = true;
				continue;
			}
			if ( CheckedNode(nodeNum2,goal->s.number) == CHECKED_PASSED )
			{//already checked this node against goal and it passed
			}
			else
			{//haven't checked this node against goal yet
				radius	= node2->GetRadius();
				
				//If we're not within the known clear radius of this node OR out of Z height range...
				if ( (*nci2).distance >= (radius*radius) || ( fabs( position2[2] - goal->currentOrigin[2] ) >= MAX_Z_DELTA ) )
				{
					//We're not *within* this node, so check clear path, etc.

					if ( flags & NF_CLEAR_PATH )//|| flags & NF_CLEAR_LOS )
					{//need a clear path or LOS
						if ( !gi.inPVS( goal->currentOrigin, position2 ) )
						{//not even potentially clear
							SetCheckedNode(nodeNum2,goal->s.number,CHECKED_FAILED);
							continue;
						}
					}
					//Do we need a clear path?
					if ( flags & NF_CLEAR_PATH )
					{
						if ( TestNodePath( goal, ent->s.number, position2, qfalse ) == false )//qtrue?
						{
							SetCheckedNode(nodeNum2,goal->s.number,CHECKED_FAILED);
							continue;
						}
					}

					//Do we need a clear line of sight?
					/*
					if ( flags & NF_CLEAR_LOS )
					{
						if ( TestNodeLOS( goal, position2 ) == false )
						{
							nodeChecked[nodeNum2][goal->s.number] = CHECKED_FAILED;
							continue;
						}
					}
					*/
				}//otherwise, inside the node so it must be clear (?)
				SetCheckedNode(nodeNum2,goal->s.number,CHECKED_PASSED);
			}

			bestCost = cost;
			bestNode = nextNode;
			ent->waypoint = (*nci).nodeID;
			goal->waypoint = (*nci2).nodeID;
		}
	}

	if ( !d_altRoutes->integer )
	{//bestNode would not have been set by GetBestNodeAltRoute above, so get it here
		if ( ent->waypoint != NODE_NONE && goal->waypoint != NODE_NONE )
		{//have 2 valid waypoints which means a valid path
			bestNode = GetBestNodeAltRoute( ent->waypoint, goal->waypoint, &bestCost, NODE_NONE );
		}
	}
	return bestNode;

/*
	if(recalc)
	{

		int	startTime = GetTime(0);
		if ( m_nodes[ent->waypoint] && (ent->waypoint!=-1) )
		{
			CalculatePath(m_nodes[ent->waypoint]);
		}
		if ( m_nodes[goal->waypoint] && (goal->waypoint!=-1) )
		{
			CalculatePath(m_nodes[goal->waypoint]);
		}	
		gi.Printf( S_COLOR_CYAN"%s recalced two paths in %d ms\n", (NPC!=NULL?NPC->targetname:"NULL"), GetTime(startTime) );
	}
	return (ent->waypoint != NODE_NONE && goal->waypoint != NODE_NONE);
*/
}

/*
-------------------------
GetNearestWaypoint
-------------------------
*/

int CNavigator::GetNearestNode( gentity_t *ent, int lastID, int flags, int targetID )
{
	int	bestNode = NODE_NONE;
	//Must have nodes
	if ( m_nodes.size() == 0 )
		return NODE_NONE;

	if ( targetID == NODE_NONE )
	{
		//Try and find an early match using our last node
		bestNode = TestBestFirst( ent, lastID, flags );

		if ( bestNode != NODE_NONE )
			return bestNode;
	}//else can't rely on testing last, we want best to targetID

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

#define	MAX_Z_DELTA	18

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

	nodeChain_l				nodeChain;
	nodeChain_l::iterator	nci;

	//Collect all nodes within a certain radius
	CollectNearestNodes( ent->currentOrigin, NODE_COLLECT_RADIUS, NODE_COLLECT_MAX, nodeChain );

	vec3_t				position;
	int					radius;
	int					dist, bestDist = Q3_INFINITE;
	CNode				*node;

	//Look through all nodes
	STL_ITERATE( nci, nodeChain )
	{
		node = m_nodes[(*nci).nodeID];

		node->GetPosition( position );

		radius	= node->GetRadius();
		
		if ( NodeFailed( ent, (*nci).nodeID ) )
		{
			continue;
		}
		//Are we within the known clear radius of this node?
		if ( (*nci).distance < (radius*radius) )
		{
			//Do a z-difference sanity check
			if ( fabs( position[2] - ent->currentOrigin[2] ) < MAX_Z_DELTA )
			{
				//Found one		
				return (*nci).nodeID;
			}
		}

		//We're not *within* this node, so...
		if ( CheckedNode((*nci).nodeID,ent->s.number) == CHECKED_FAILED )
		{
			continue;
		}
		else if ( CheckedNode((*nci).nodeID,ent->s.number) == CHECKED_FAILED )
		{
			continue;
		}
		else
		{
			//Do we need a clear path?
			if ( flags & NF_CLEAR_PATH )
			{
				if ( TestNodePath( ent, ENTITYNUM_NONE, position, qfalse ) == false )//qtrue?
				{
					SetCheckedNode((*nci).nodeID,ent->s.number,CHECKED_FAILED);
					continue;
				}
			}

			//Do we need a clear line of sight?
			/*
			if ( flags & NF_CLEAR_LOS )
			{
				if ( TestNodeLOS( ent, position ) == false )
				{
					nodeChecked[(*nci).nodeID][ent->s.number] = CHECKED_FAILED;
					continue;
				}
			}
			*/
			SetCheckedNode((*nci).nodeID,ent->s.number,CHECKED_PASSED);
		}

		if ( targetID != WAYPOINT_NONE )
		{//we want to find the one with the shortest route here
			dist = GetPathCost( (*nci).nodeID, targetID );
			if ( dist < bestDist )
			{
				bestDist = dist;
				bestNode = (*nci).nodeID;
			}
		}
		else
		{//first one we find is fine
			bestNode = (*nci).nodeID;
			break;
		}
	}

	//Found one, we're done
	return bestNode;
}

/*
-------------------------
ShowPath
-------------------------
*/

void CNavigator::ShowPath( int start, int end )
{	
	//Validate the start position
	if ( ( start < 0 ) || ( start >= m_nodes.size() ) )
		return;

	//Validate the end position
	if ( ( end < 0 ) || ( end >= m_nodes.size() ) )
		return;

	CNode	*startNode	= m_nodes[ start ];
	CNode	*endNode	= m_nodes[ end ];
	
	CNode	*moveNode = startNode;
	CNode	*testNode = NULL;

	int		bestNode;
	vec3_t	startPos, endPos;

	int		runAway = 0;

	//Draw out our path
	while ( moveNode != endNode )
	{
		bestNode = GetBestNode( moveNode->GetID(), end );
		
		//Some nodes may be fragmented
		if ( bestNode == -1 )
		{
			Com_Printf("No connection possible between node %d and %d\n", start, end );
			return;
		}

		//This is our next node on the path
		testNode = m_nodes[ bestNode ];

		//Get their origins
		moveNode->GetPosition( startPos );
		testNode->GetPosition( endPos );
		
		//Draw the edge
		CG_DrawEdge( startPos, endPos, EDGE_PATH );

		//Take a new best node
		moveNode = testNode;

		if ( runAway++ > 64 )
		{
			Com_Printf("Potential Run-away path!\n");
			return;
		}
	}
}

static map<int,byte> CheckedNodes;
void CNavigator::ClearCheckedNodes( void )
{
	CheckedNodes.clear();
}

byte CNavigator::CheckedNode(int wayPoint,int ent)
{
	assert(wayPoint>=0&&wayPoint<MAX_STORED_WAYPOINTS);
	assert(ent>=0&&ent<MAX_GENTITIES);
	map<int,byte>::iterator f=CheckedNodes.find(wayPoint*MAX_GENTITIES+ent);
	if (f!=CheckedNodes.end())
	{
		return (*f).second;
	}
	return CHECKED_NO;
}

void CNavigator::SetCheckedNode(int wayPoint,int ent,byte value)
{
	assert(wayPoint>=0&&wayPoint<MAX_STORED_WAYPOINTS);
	assert(ent>=0&&ent<MAX_GENTITIES);
	assert(value==CHECKED_FAILED||value==CHECKED_PASSED);
	CheckedNodes[wayPoint*MAX_GENTITIES+ent]=value;
}

#define	CHECK_FAILED_EDGE_INTERVAL	1000
#define	CHECK_FAILED_EDGE_INTITIAL	10000

void CNavigator::CheckFailedNodes( gentity_t *ent )
{
	vec3_t	nodePos;
	int		j;

	//Must have nodes
	if ( m_nodes.size() == 0 )
		return;

	if ( ent->failedWaypointCheckTime && ent->failedWaypointCheckTime < level.time )
	{
		int failed = 0;
		//do this only once every 1 second
		for ( j = 0; j < MAX_FAILED_NODES; j++ )
		{
			if ( ent->failedWaypoints[j] != 0 )
			{
				failed++;
				//-1 because 0 is a valid node but also the default, so we add one when we add one
				m_nodes[ent->failedWaypoints[j]-1]->GetPosition( nodePos );
				if ( !NAV_ClearPathToPoint( ent, ent->mins, ent->maxs, nodePos, (CONTENTS_SOLID|CONTENTS_MONSTERCLIP|CONTENTS_BOTCLIP), ENTITYNUM_NONE ) )
				{//no path clear of architecture, so clear this since we can't check against entities
					ent->failedWaypoints[j] = 0;
					failed--;
				}
				//have clear architectural path, now check against ents only
				else if ( NAV_ClearPathToPoint( ent, ent->mins, ent->maxs, nodePos, CONTENTS_BODY, ENTITYNUM_NONE ) )
				{//clear of ents, too, so all clear, clear this one out
					ent->failedWaypoints[j] = 0;
					failed--;
				}
			}
		}
		if ( !failed )
		{
			ent->failedWaypointCheckTime = 0;
		}
		else
		{
			ent->failedWaypointCheckTime = level.time + CHECK_FAILED_EDGE_INTERVAL + Q_irand( 0, 1000 );
		}
	}
}

void CNavigator::AddFailedNode( gentity_t *ent, int nodeID )
{
	int j;
	for ( j = 0; j < MAX_FAILED_NODES; j++ )
	{
		if ( ent->failedWaypoints[j] == 0 )
		{
			ent->failedWaypoints[j] = nodeID+1;//+1 because 0 is the default value and that's a valid node, so we take the +1 out when we check the node above
			if ( !ent->failedWaypointCheckTime )
			{
				ent->failedWaypointCheckTime = level.time + CHECK_FAILED_EDGE_INTITIAL;
			}
			return;
		}
		if ( ent->failedWaypoints[j] == nodeID+1 )
		{//already have this one marked as failed
			return;
		}
	}
	if ( j == MAX_FAILED_NODES )//check not needed, but...
	{//ran out of failed nodes, get rid of first one, shift rest up
		for ( j = 0; j < MAX_FAILED_NODES-1; j++ )
		{
			ent->failedWaypoints[j] = ent->failedWaypoints[j+1];
		}
	}
	ent->failedWaypoints[MAX_FAILED_NODES-1] = nodeID+1;
	if ( !ent->failedWaypointCheckTime )
	{
		ent->failedWaypointCheckTime = level.time + CHECK_FAILED_EDGE_INTITIAL;
	}
}

qboolean CNavigator::NodeFailed( gentity_t *ent, int nodeID )
{
	for ( int j = 0; j < MAX_FAILED_NODES; j++ )
	{
		if ( (ent->failedWaypoints[j]-1) == nodeID )
		{
			return qtrue;
		}
	}
	return qfalse;
}

void CNavigator::ClearFailedEdge( failedEdge_t *failedEdge )
{
	if ( !failedEdge )
	{
		return;
	}

	//clear the edge failed flags
	/*
	CNode *node = m_nodes[failedEdge->startID];
	int edgeNum = node->GetEdgeNumToNode( failedEdge->endID );
	int	flags;
	if ( edgeNum != -1 )
	{
		flags = node->GetEdgeFlags( edgeNum )&~EFLAG_FAILED;
		node->SetEdgeFlags( edgeNum, flags );
	}
	node = m_nodes[failedEdge->endID];
	edgeNum = node->GetEdgeNumToNode( failedEdge->startID );
	if ( edgeNum != -1 )
	{
		flags = node->GetEdgeFlags( edgeNum )&~EFLAG_FAILED;
		node->SetEdgeFlags( edgeNum, flags );
	}
	*/
	//clear failedEdge info
	failedEdge->startID = failedEdge->endID = WAYPOINT_NONE;
	failedEdge->checkTime = 0;
}

void CNavigator::ClearAllFailedEdges( void )
{
	for ( int j = 0; j < MAX_FAILED_EDGES; j++ )
	{
		ClearFailedEdge( &failedEdges[j] );
	}
}

int CNavigator::EdgeFailed( int startID, int endID )
{
	for ( int j = 0; j < MAX_FAILED_EDGES; j++ )
	{
		if ( failedEdges[j].startID == startID )
		{
			if ( failedEdges[j].endID == endID )
			{
				return j;
			}
		}
		else if ( failedEdges[j].startID == endID )
		{
			if ( failedEdges[j].endID == startID )
			{
				return j;
			}
		}
	}

	return -1;
}

void CNavigator::AddFailedEdge( int entID, int startID, int endID )
{
	int	j;//, nextID;

	//Must have nodes
	if ( m_nodes.size() == 0 )
		return;

	//Validate the ent number
	if ( ( entID < 0 ) || ( entID > ENTITYNUM_NONE ) )
	{
#ifndef FINAL_BUILD
		gi.Printf( S_COLOR_RED"NAV ERROR: envalid ent %d\n", entID );
		assert(0&&"invalid entID");
#endif
		return;
	}

	//Validate the start position
	if ( ( startID < 0 ) || ( startID >= m_nodes.size() ) )
	{
#ifndef FINAL_BUILD
		gi.Printf( S_COLOR_RED"NAV ERROR: tried to fail invalid waypoint %d\n", startID );
		assert(0&&"invalid failed edge");
#endif
		return;
	}

	//Validate the end position
	if ( ( endID < 0 ) || ( endID >= m_nodes.size() ) )
	{
#ifndef FINAL_BUILD
		gi.Printf( S_COLOR_RED"NAV ERROR: tried to fail invalid waypoint %d\n", endID );
		assert(0&&"invalid failed edge");
#endif
		return;
	}

	//First see if we already have this one
	if ( (j = EdgeFailed( startID, endID )) != -1 )
	{
		//just remember this guy instead
		failedEdges[j].entID = entID;
		return;
	}

	//Okay, new one, find an empty slot
	for ( j = 0; j < MAX_FAILED_EDGES; j++ )
	{
		if ( failedEdges[j].startID == WAYPOINT_NONE )
		{
			failedEdges[j].startID = startID;
			failedEdges[j].endID = endID;
			//Check one second from now to see if it's clear
			failedEdges[j].checkTime = level.time + CHECK_FAILED_EDGE_INTERVAL + Q_irand( 0, 1000 );

			/*
			//DISABLED this for now, makes people stand around too long when
			//			collision avoidance just wasn't at it's best but path is clear
			CNode *start = m_nodes[startID];
			CNode *end = m_nodes[endID];

			for ( int i = 0; i < start->GetNumEdges(); i++ )
			{
				nextID = start->GetEdge(i);

				if ( EdgeFailed( startID, nextID ) != -1 )
				{
					//This edge blocked, check next
					continue;
				}

				if ( nextID == endID || end->GetRank( nextID ) >= 0 )
				{//neighbor of or route to end
					//There's an alternate route, so don't check this one for 10 seconds
					failedEdges[j].checkTime = level.time + CHECK_FAILED_EDGE_INTITIAL;
					break;
				}
			}
			*/
			
			//Remember who needed it
			failedEdges[j].entID = entID;

			//set the edge failed flags
			/*
			CNode *node = m_nodes[startID];
			int edgeNum = node->GetEdgeNumToNode( endID );
			int	flags;
			if ( edgeNum != -1 )
			{
				flags = node->GetEdgeFlags( edgeNum )|EFLAG_FAILED;
				node->SetEdgeFlags( edgeNum, flags );
			}
			node = m_nodes[endID];
			edgeNum = node->GetEdgeNumToNode( startID );
			if ( edgeNum != -1 )
			{
				flags = node->GetEdgeFlags( edgeNum )|EFLAG_FAILED;
				node->SetEdgeFlags( edgeNum, flags );
			}
			*/

			//now recalc all the paths!
			if ( pathsCalculated )
			{
				//reconnect the nodes and mark every node's flag NF_RECALC
				//gi.Printf( S_COLOR_CYAN"%d marking all nodes for recalc\n", level.time );
				ConnectNodes();
				FlagAllNodes( NF_RECALC );
				//CalculatePaths(true);
			}
			return;
		}
	}

#ifndef FINAL_BUILD
	gi.Printf( S_COLOR_RED"NAV ERROR: too many blocked waypoint connections (%d)!!!\n", j );
#endif
}

qboolean CNavigator::CheckFailedEdge( failedEdge_t *failedEdge )
{
	if ( !failedEdge )
	{
		return qfalse;
	}

	//Every 1 second, see if our failed edges are clear
	if ( failedEdge->checkTime < level.time )
	{
		if ( failedEdge->startID != WAYPOINT_NONE )
		{
			vec3_t		start, end, mins, maxs;
			int			ignore, clipmask;
			gentity_t	*ent = (failedEdge->entID<ENTITYNUM_WORLD)?&g_entities[failedEdge->entID]:NULL;
			int			hitEntNum;

			if ( !ent )
			{
				VectorSet( mins, DEFAULT_MINS_0, DEFAULT_MINS_1, DEFAULT_MINS_2+STEPSIZE );
				VectorSet( maxs, DEFAULT_MAXS_0, DEFAULT_MAXS_1, DEFAULT_MAXS_2 );
				ignore = ENTITYNUM_NONE;
				clipmask = MASK_NPCSOLID;
			}
			else
			{
				VectorCopy( ent->mins, mins );
				mins[2] += STEPSIZE;
				VectorCopy( ent->maxs, maxs );
				ignore = failedEdge->entID;
				clipmask = ent->clipmask;
			}

			if ( maxs[2] < mins[2] )
			{//don't invert bounding box
				maxs[2] = mins[2];
			}

			m_nodes[failedEdge->startID]->GetPosition( start );
			m_nodes[failedEdge->endID]->GetPosition( end );

			//See if it's NAV_ClearPath...
#if 0
			hitEntNum = NAVNEW_ClearPathBetweenPoints( start, end, mins, maxs, ignore, clipmask|CONTENTS_MONSTERCLIP|CONTENTS_BOTCLIP );//NOTE: should we really always include monsterclip (physically blocks NPCs) and botclip (do not enter)?
#else
			trace_t	trace;

			//Test if they're even conceivably close to one another
			if ( !gi.inPVSIgnorePortals( start, end ) )
			{
				return qfalse;
			}

			gi.trace( &trace, start, mins, maxs, end, ignore, clipmask|CONTENTS_MONSTERCLIP|CONTENTS_BOTCLIP );//NOTE: should we really always include monsterclip (physically blocks NPCs) and botclip (do not enter)?

			if( trace.startsolid == qtrue || trace.allsolid == qtrue )
			{
				return qfalse;
			}
			hitEntNum = trace.entityNum;
#endif
			//if we did hit something, see if it's just an auto-door and allow it
			if ( hitEntNum != ENTITYNUM_NONE && G_EntIsUnlockedDoor( hitEntNum ) )
			{
				hitEntNum = ENTITYNUM_NONE;
			}
			if ( hitEntNum == ENTITYNUM_NONE )
			{
				//If so, clear it
				ClearFailedEdge( failedEdge );
				return qtrue;
			}
			else
			{
				//Check again in one second
				failedEdge->checkTime = level.time + CHECK_FAILED_EDGE_INTERVAL + Q_irand( 0, 1000 );
			}
		}
	}
	return qfalse;
}

void CNavigator::CheckAllFailedEdges( void )
{
	failedEdge_t	*failedEdge;
	qboolean		clearedAny = qfalse;

	//Must have nodes
	if ( m_nodes.size() == 0 )
		return;

	for ( int j = 0; j < MAX_FAILED_EDGES; j++ )
	{
		failedEdge = &failedEdges[j];

		clearedAny = CheckFailedEdge( failedEdge )?qtrue:clearedAny;
	}
	if ( clearedAny )
	{//need to recalc the paths
		if ( pathsCalculated )
		{
			//reconnect the nodes and mark every node's flag NF_RECALC
			//gi.Printf( S_COLOR_CYAN"%d marking all nodes for recalc\n", level.time );
			ConnectNodes();
			FlagAllNodes( NF_RECALC );
			//CalculatePaths(true);
		}
	}
}

qboolean CNavigator::RouteBlocked( int startID, int testEdgeID, int endID, int rejectRank )
{
	int		nextID, edgeID, lastID, bestNextID = NODE_NONE;
	int		bestRank = rejectRank;
	int		testRank;
	qboolean	allEdgesFailed;
	CNode	*end;
	CNode	*next;

	if ( EdgeFailed( startID, testEdgeID ) != -1 )
	{
		return qtrue;
	}

	if ( testEdgeID == endID )
	{//Neighbors, checked out, all clear
		return qfalse;
	}
	
	//Okay, first edge is clear, now check rest of route!
	end	= m_nodes[ endID ];
	nextID = testEdgeID;
	lastID = startID;

	while( 1 )
	{
		next = m_nodes[ nextID ];
		allEdgesFailed = qtrue;

		for ( int i = 0; i < next->GetNumEdges(); i++ )
		{
			edgeID = next->GetEdge(i);

			if ( edgeID == lastID )
			{//Don't backtrack
				continue;
			}

			if ( edgeID == startID )
			{//Don't loop around
				continue;
			}

			if ( EdgeFailed( nextID, edgeID ) != -1 )
			{
				//This edge blocked, check next
				continue;
			}

			if ( edgeID == endID )
			{//We got there all clear!
				return qfalse;
			}

			//Still going...
			testRank = end->GetRank( edgeID );

			if ( testRank < 0 )
			{//No route this way
				continue;
			}

			//Is the rank good enough?
			if ( testRank < bestRank )
			{
				bestNextID = edgeID;
				bestRank = testRank;
				allEdgesFailed = qfalse;
			}
		}
		
		if ( allEdgesFailed )
		{
			//This route has no clear way of getting to end
			return qtrue;
		}
		else
		{
			lastID = nextID;
			nextID = bestNextID;
		}
	}
}

/*
-------------------------
GetBestNodeAltRoute
-------------------------
*/

int CNavigator::GetBestNodeAltRoute( int startID, int endID, int *pathCost, int rejectID )
{
	//Must have nodes
	if ( m_nodes.size() == 0 )
		return WAYPOINT_NONE;

	//Validate the start position
	if ( ( startID < 0 ) || ( startID >= m_nodes.size() ) )
		return WAYPOINT_NONE;

	//Validate the end position
	if ( ( endID < 0 ) || ( endID >= m_nodes.size() ) )
		return WAYPOINT_NONE;

	//Is it the same node?
	if ( startID == endID )
	{
		if ( !d_altRoutes->integer || EdgeFailed( startID, endID ) == -1 )
		{
			return startID;
		}
		else
		{
			return WAYPOINT_NONE;
		}
	}

	CNode	*start	= m_nodes[ startID ];

	int		bestNode = -1;
	int		bestRank = Q3_INFINITE;
	int		testRank, rejectRank = Q3_INFINITE;
	int		bestCost = Q3_INFINITE;

	*pathCost = Q3_INFINITE;

	//Find the minimum rank of the edge(s) we want to reject as paths
	if ( rejectID != WAYPOINT_NONE )
	{
		for ( int i = 0; i < start->GetNumEdges(); i++ )
		{
			if ( start->GetEdge(i) == rejectID )
			{
				rejectRank = GetPathCost( startID, endID );//end->GetRank( start->GetEdge(i) );
				break;
			}
		}
	}

	for ( int i = 0; i < start->GetNumEdges(); i++ )
	{
		int	edgeID = start->GetEdge(i);

		testRank = GetPathCost( edgeID, endID );//end->GetRank( edgeID );

		//Make sure it's not worse than our reject rank
		if ( testRank >= rejectRank )
			continue;

		//Found one
		if ( edgeID == endID )
		{
			if ( !d_altRoutes->integer || !RouteBlocked( startID, edgeID, endID, rejectRank ) )
			{
				*pathCost += start->GetEdgeCost( i );
				return edgeID;
			}
			else
			{//this is blocked, can't consider it
				continue;
			}
		}

		//No possible connection
		if ( testRank == NODE_NONE )
		{
			*pathCost = Q3_INFINITE;
			return NODE_NONE;
		}

		//Found a better one
		if ( testRank < bestRank )
		{
			//FIXME: make sure all the edges down from startID through edgeID to endID
			//		does NOT include a failedEdge...
			if ( !d_altRoutes->integer || !RouteBlocked( startID, edgeID, endID, rejectRank ) )
			{
				bestNode = edgeID;
				bestRank = testRank;
				bestCost = start->GetEdgeCost(i)+testRank;
			}
		}
	}

	*pathCost = bestCost;

	return bestNode;
}
/*
-------------------------
GetBestNodeAltRoute
overloaded so you don't have to pass a pathCost int pointer in
-------------------------
*/

int CNavigator::GetBestNodeAltRoute( int startID, int endID, int rejectID )
{
	int	junk;
	return GetBestNodeAltRoute( startID, endID, &junk, rejectID );
}
/*
-------------------------
GetBestNode
-------------------------
*/

int CNavigator::GetBestNode( int startID, int endID, int rejectID )
{
	//Validate the start position
	if ( ( startID < 0 ) || ( startID >= m_nodes.size() ) )
		return WAYPOINT_NONE;

	//Validate the end position
	if ( ( endID < 0 ) || ( endID >= m_nodes.size() ) )
		return WAYPOINT_NONE;

	if ( startID == endID )
		return startID;

	CNode	*start	= m_nodes[ startID ];
	CNode	*end	= m_nodes[ endID ];

	int		bestNode = -1;
	int		bestRank = Q3_INFINITE;
	int		testRank, rejectRank = 0;

	if ( rejectID != WAYPOINT_NONE )
	{
		for ( int i = 0; i < start->GetNumEdges(); i++ )
		{
			if ( start->GetEdge(i) == rejectID )
			{
				rejectRank = end->GetRank( start->GetEdge(i) );
				break;
			}
		}
	}

	for ( int i = 0; i < start->GetNumEdges(); i++ )
	{
		int	edgeID = start->GetEdge(i);

		//Found one
		if ( edgeID == endID )
			return edgeID;

		testRank = end->GetRank( edgeID );

		//Found one
		if ( testRank <= rejectRank )
			continue;

		//No possible connection
		if ( testRank == NODE_NONE )
			return NODE_NONE;

		//Found a better one
		if ( testRank < bestRank )
		{
			bestNode = edgeID;
			bestRank = testRank;
		}
	}

	return bestNode;
}

/*
-------------------------
GetNodePosition
-------------------------
*/

int CNavigator::GetNodePosition( int nodeID, vec3_t out )
{
	//Validate the number
	if ( ( nodeID < 0 ) || ( nodeID >= m_nodes.size() ) )
		return false;

	CNode	*node = m_nodes[ nodeID ];

	node->GetPosition( out );

	return true;
}

/*
-------------------------
GetNodeNumEdges
-------------------------
*/

int CNavigator::GetNodeNumEdges( int nodeID )
{
	if ( ( nodeID < 0 ) || ( nodeID >=  m_nodes.size() ) )
		return -1;

	CNode	*node = m_nodes[ nodeID ];

	assert( node );

	return node->GetNumEdges();
}

/*
-------------------------
GetNodeEdge
-------------------------
*/

int CNavigator::GetNodeEdge( int nodeID, int edge )
{
	if ( ( nodeID < 0 ) || ( nodeID >=  m_nodes.size() ) )
		return -1;

	CNode	*node = m_nodes[ nodeID ];

	assert( node );

	return node->GetEdge( edge );
}

/*
-------------------------
Connected
-------------------------
*/

bool CNavigator::Connected( int startID, int endID )
{
	//Validate the start position
	if ( ( startID < 0 ) || ( startID >= m_nodes.size() ) )
		return false;

	//Validate the end position
	if ( ( endID < 0 ) || ( endID >= m_nodes.size() ) )
		return false;

	if ( startID == endID )
		return true;

	CNode	*start	= m_nodes[ startID ];
	CNode	*end	= m_nodes[ endID ];

	for ( int i = 0; i < start->GetNumEdges(); i++ )
	{
		int	edgeID = start->GetEdge(i);

		//Found one
		if ( edgeID == endID )
			return true;

		if ( ( end->GetRank( edgeID ) ) != NODE_NONE )
			return true;
	}

	return false;
}

/*
-------------------------
GetPathCost
-------------------------
*/

unsigned int CNavigator::GetPathCost( int startID, int endID )
{
	//Validate the start position
	if ( ( startID < 0 ) || ( startID >= m_nodes.size() ) )
		return Q3_INFINITE; // return 0;

	//Validate the end position
	if ( ( endID < 0 ) || ( endID >= m_nodes.size() ) )
		return Q3_INFINITE; // return 0;

	CNode	*startNode	= m_nodes[ startID ];

	if ( !startNode->GetNumEdges() )
	{//WTF?  Solitary waypoint!  Bad designer!
		return Q3_INFINITE; // return 0;
	}

	CNode	*endNode	= m_nodes[ endID ];
	
	CNode	*moveNode = startNode;

	int		bestNode;
	int		pathCost = 0;
	int		bestCost;

	int		bestRank;
	int		testRank;

	//Draw out our path
	while ( moveNode != endNode )
	{
		bestRank = WORLD_SIZE;
		bestNode = -1;
		bestCost = 0;

		for ( int i = 0; i < moveNode->GetNumEdges(); i++ )
		{
			int	edgeID = moveNode->GetEdge(i);

			//Done
			if ( edgeID == endID )
			{
				return pathCost + moveNode->GetEdgeCost( i );
			}	

			testRank = endNode->GetRank( edgeID );

			//No possible connection
			if ( testRank == NODE_NONE )
				return Q3_INFINITE; // return 0;

			//Found a better one
			if ( testRank < bestRank )
			{
				bestNode = edgeID;
				bestRank = testRank;
				bestCost = moveNode->GetEdgeCost( i );
			}
		}

		pathCost += bestCost;

		//Take a new best node
		moveNode = m_nodes[ bestNode ];
	}

	return pathCost;
}

/*
-------------------------
GetEdgeCost
-------------------------
*/

unsigned int CNavigator::GetEdgeCost( int startID, int endID )
{
	//Validate the start position
	if ( ( startID < 0 ) || ( startID >= m_nodes.size() ) )
		return Q3_INFINITE; // return 0;

	//Validate the end position
	if ( ( endID < 0 ) || ( endID >= m_nodes.size() ) )
		return Q3_INFINITE; // return 0;

	CNode	*start	= m_nodes[startID];
	CNode	*end	= m_nodes[endID];

	return GetEdgeCost( start, end );
}

/*
-------------------------
GetProjectedNode
-------------------------
*/

int CNavigator::GetProjectedNode( vec3_t origin, int nodeID )
{
	//Validate the start position
	if ( ( nodeID < 0 ) || ( nodeID >= m_nodes.size() ) )
		return NODE_NONE;

	CNode	*node = m_nodes[nodeID];
	CNode	*tempNode;

	float	bestDot		= 0.0f;
	int		bestNode	= NODE_NONE;

	vec3_t	targetDir, basePos, tempDir, tempPos;
	float	dot;

	//Setup our target direction
	node->GetPosition( basePos );

	VectorSubtract( origin, basePos, targetDir );
	VectorNormalize( targetDir );

	//Go through all the edges
	for ( int i = 0; i < node->GetNumEdges(); i++ )
	{
		tempNode = m_nodes[node->GetEdge(i)];
		tempNode->GetPosition( tempPos );

		VectorSubtract( tempPos, basePos, tempDir );
		VectorNormalize( tempDir );	//FIXME: Retain the length here if you want it

		dot = DotProduct( targetDir, tempDir );

		if ( dot < 0.0f )
			continue;

		if ( dot > bestDot )
		{
			bestDot		= dot;
			bestNode	= tempNode->GetID();
		}
	}

	return bestNode;
}

// This is the PriorityQueue stuff for lists of connections
// better than linear		(1/21/02 BJG)
//////////////////////////////////////////////////////////////////
// Helper pop_mHeap algorithm class
//////////////////////////////////////////////////////////////////
class NodeTotalGreater 
{
public:
   bool operator()( CEdge * first, CEdge * second ) const {
      return( first->m_cost > second->m_cost );
   }
};


//////////////////////////////////////////////////////////////////
// Destructor - Deallocate any remaining pointers in the queue
//////////////////////////////////////////////////////////////////
CPriorityQueue::~CPriorityQueue()
{
	while (!Empty())
	{
		delete Pop();
	}
}

//////////////////////////////////////////////////////////////////
// Standard Iterative Search
//////////////////////////////////////////////////////////////////
CEdge* CPriorityQueue::Find(int npNum)
{
	for(vector<CEdge*>::iterator HeapIter=mHeap.begin(); HeapIter!=mHeap.end(); HeapIter++)
	{
		if ((*HeapIter)->m_first == npNum)
		{
			return *HeapIter;
		}
	}
	return 0;
}

//////////////////////////////////////////////////////////////////
// Remove Node And Resort
//////////////////////////////////////////////////////////////////
CEdge* CPriorityQueue::Pop()
{  
   CEdge *edge = mHeap.front();

   //pop_mHeap will move the node at the front to the position N
   //and then sort the mHeap to make positions 1 through N-1 correct
   //(STL makes no assumptions about your data and doesn't want to change
   //the size of the container.)
   std::pop_heap(mHeap.begin(), mHeap.end(), NodeTotalGreater() );

   //pop_back() will actually remove the last element from the mHeap
   //now the mHeap is sorted for positions 1 through N
   mHeap.pop_back();

   return( edge );
}

//////////////////////////////////////////////////////////////////
// Add New Node And Resort
//////////////////////////////////////////////////////////////////
void CPriorityQueue::Push(CEdge* theEdge )
{    
   //Pushes the node onto the back of the mHeap
   mHeap.push_back( theEdge );

   //Sorts the new element into the mHeap
   std::push_heap( mHeap.begin(), mHeap.end(), NodeTotalGreater() );
}

//////////////////////////////////////////////////////////////////
// Find The Node In Question And Resort mHeap Around It
//////////////////////////////////////////////////////////////////
void CPriorityQueue::Update( CEdge* edge )
{  
   for(vector<CEdge*>::iterator i=mHeap.begin(); i!=mHeap.end(); i++)
   {
      if( (*i)->m_first == edge->m_first )
      {  //Found node - resort from this position in the mHeap
         //(its total value was changed before this function was called)
         std::push_heap( mHeap.begin(), i+1, NodeTotalGreater() );
         return;
      }
   }
}

//////////////////////////////////////////////////////////////////
// Just a wrapper for stl empty function.
//////////////////////////////////////////////////////////////////
bool CPriorityQueue::Empty()
{
   return( mHeap.empty() );
};