#include "b_local.h"
#include "g_nav.h"

qboolean NAV_CheckAhead( gentity_t *self, vec3_t end, trace_t *trace, int clipmask );
qboolean NAV_TestForBlocked( gentity_t *self, gentity_t *goal, gentity_t *blocker, float distance, int *flags );

void G_Line( vec3_t start, vec3_t end, vec3_t color, float alpha );
void G_Cube( vec3_t mins, vec3_t maxs, vec3_t color, float alpha );
void G_CubeOutline( vec3_t mins, vec3_t maxs, int time, unsigned int color, float alpha );
void G_DrawEdge( vec3_t start, vec3_t end, int type );
void G_DrawNode( vec3_t origin, int type );
void G_DrawCombatPoint( vec3_t origin, int type );
void TAG_ShowTags( int flags );

qboolean NAV_CheckNodeFailedForEnt( gentity_t *ent, int nodeNum )
{
	int j;

	//FIXME: must be a better way to do this
	for ( j = 0; j < MAX_FAILED_NODES; j++ )
	{
		if ( ent->failedWaypoints[j] == nodeNum+1 )//+1 because 0 is a valid nodeNum, but also the default
		{//we failed against this node
			return qtrue;
		}
	}
	return qfalse;
}
/*
-------------------------
NPC_UnBlocked
-------------------------
*/
void NPC_ClearBlocked( gentity_t *self )
{
	if ( self->NPC == NULL )
		return;

	//self->NPC->aiFlags &= ~NPCAI_BLOCKED;
	self->NPC->blockingEntNum = ENTITYNUM_NONE;
}

void NPC_SetBlocked( gentity_t *self, gentity_t *blocker )
{
	if ( self->NPC == NULL )
		return;

	//self->NPC->aiFlags |= NPCAI_BLOCKED;
	self->NPC->blockedSpeechDebounceTime = level.time + MIN_BLOCKED_SPEECH_TIME + ( random() * 4000 );
	self->NPC->blockingEntNum = blocker->s.number;
}

/*
-------------------------
NAVNEW_ClearPathBetweenPoints
-------------------------
*/
int NAVNEW_ClearPathBetweenPoints(vec3_t start, vec3_t end, vec3_t mins, vec3_t maxs, int ignore, int clipmask)
{
	trace_t	trace;

	//Test if they're even conceivably close to one another
	if ( !trap_InPVS( start, end ) )
	{
		return ENTITYNUM_WORLD;
	}

	trap_Trace( &trace, start, mins, maxs, end, ignore, clipmask );

	//if( ( ( trace.startsolid == false ) && ( trace.allsolid == false ) ) && ( trace.fraction < 1.0f ) )
	//{//FIXME: check for drops?
	//FIXME: if startsolid or allsolid, then the path isn't clear... but returning ENTITYNUM_NONE indicates to CheckFailedEdge that is is clear...?
		return trace.entityNum;
	//}

	//return ENTITYNUM_NONE;
}

/*
-------------------------
NAVNEW_PushBlocker
-------------------------
*/
void NAVNEW_PushBlocker( gentity_t *self, gentity_t *blocker, vec3_t right, qboolean setBlockedInfo )
{//try pushing blocker to one side
	trace_t	tr;
	vec3_t	mins, end;
	float	rightSucc, leftSucc, moveamt;

	if ( self->NPC->shoveCount > 30 )
	{//don't push for more than 3 seconds;
		return;
	}

	if ( !blocker->s.number )
	{//never push the player
		return;
	}

	if ( !blocker->client || !VectorCompare( blocker->client->pushVec, vec3_origin ) )
	{//someone else is pushing him, wait until they give up?
		return;
	}

	VectorCopy( blocker->r.mins, mins );
	mins[2] += STEPSIZE;

	moveamt = (self->r.maxs[1] + blocker->r.maxs[1]) * 1.2;//yes, magic number

	VectorMA( blocker->r.currentOrigin, -moveamt, right, end );
	trap_Trace( &tr, blocker->r.currentOrigin, mins, blocker->r.maxs, end, blocker->s.number, blocker->clipmask|CONTENTS_BOTCLIP);
	if ( !tr.startsolid && !tr.allsolid )
	{
		leftSucc = tr.fraction;
	}
	else
	{
		leftSucc = 0.0f;
	}
	
	if ( leftSucc >= 1.0f )
	{//it's clear, shove him that way
		VectorScale( right, -moveamt, blocker->client->pushVec );
		blocker->client->pushVecTime = level.time + 2000;
	}
	else
	{
		VectorMA( blocker->r.currentOrigin, moveamt, right, end );
		trap_Trace( &tr, blocker->r.currentOrigin, mins, blocker->r.maxs, end, blocker->s.number, blocker->clipmask|CONTENTS_BOTCLIP );
		if ( !tr.startsolid && !tr.allsolid )
		{
			rightSucc = tr.fraction;
		}
		else
		{
			rightSucc = 0.0f;
		}
		
		if ( leftSucc == 0.0f && rightSucc == 0.0f )
		{//both sides failed
			if ( d_patched.integer )
			{//use patch-style navigation
				blocker->client->pushVecTime = 0;
			}
			return;
		}

		if ( rightSucc >= 1.0f )
		{//it's clear, shove him that way
			VectorScale( right, moveamt, blocker->client->pushVec );
			blocker->client->pushVecTime = level.time + 2000;
		}
		//if neither are enough, we probably can't get around him, but keep trying
		else if ( leftSucc >= rightSucc )
		{//favor the left, all things being equal
			VectorScale( right, -moveamt, blocker->client->pushVec );
			blocker->client->pushVecTime = level.time + 2000;
		}
		else
		{
			VectorScale( right, moveamt, blocker->client->pushVec );
			blocker->client->pushVecTime = level.time + 2000;
		}
	}

	if ( setBlockedInfo )
	{
		//we tried pushing
		self->NPC->shoveCount++;
	}
}

/*
-------------------------
NAVNEW_DanceWithBlocker
-------------------------
*/
qboolean NAVNEW_DanceWithBlocker( gentity_t *self, gentity_t *blocker, vec3_t movedir, vec3_t right )
{//sees if blocker has any lateral movement
	if ( blocker->client && !VectorCompare( blocker->client->ps.velocity, vec3_origin ) )
	{
		vec3_t blocker_movedir;
		float dot;

		VectorCopy( blocker->client->ps.velocity, blocker_movedir );
		blocker_movedir[2] = 0;//cancel any vertical motion
		dot = DotProduct( blocker_movedir, right );
		if ( dot > 50.0f )
		{//he's moving to the right of me at a relatively good speed
			//go to my left
			VectorMA( movedir, -1, right, movedir );
			VectorNormalize( movedir );
			return qtrue;
		}
		else if ( dot > -50.0f )
		{//he's moving to the left of me at a relatively good speed
			//go to my right
			VectorAdd( right, movedir, movedir );
			VectorNormalize( movedir );
			return qtrue;
		}
		/*
		vec3_t	block_pos;
		trace_t	tr;
		VectorScale( blocker_movedir, -1, blocker_movedir );
		VectorMA( self->r.currentOrigin, blocked_dist, blocker_movedir, block_pos );
		if ( NAVNEW_CheckAhead( self, block_pos, tr, ( self->clipmask & ~CONTENTS_BODY )|CONTENTS_BOTCLIP ) )
		{
			VectorCopy( blocker_movedir, movedir );
			return qtrue;
		}
		*/
	}
	return qfalse;
}

/*
-------------------------
NAVNEW_SidestepBlocker
-------------------------
*/
qboolean NAVNEW_SidestepBlocker( gentity_t *self, gentity_t *blocker, vec3_t blocked_dir, float blocked_dist, vec3_t movedir, vec3_t right )
{//trace to sides of blocker and see if either is clear
	trace_t	tr;
	vec3_t	avoidAngles;
	vec3_t	avoidRight_dir, avoidLeft_dir, block_pos, mins;
	float	rightSucc, leftSucc, yaw, avoidRadius, arcAngle;

	VectorCopy( self->r.mins, mins );
	mins[2] += STEPSIZE;

	//Get the blocked direction
	yaw = vectoyaw( blocked_dir );

	//Get the avoid radius
	avoidRadius = sqrt( ( blocker->r.maxs[0] * blocker->r.maxs[0] ) + ( blocker->r.maxs[1] * blocker->r.maxs[1] ) ) + 
						sqrt( ( self->r.maxs[0] * self->r.maxs[0] ) + ( self->r.maxs[1] * self->r.maxs[1] ) );

	//See if we're inside our avoidance radius
	arcAngle = ( blocked_dist <= avoidRadius ) ? 135 : ( ( avoidRadius / blocked_dist ) * 90 );

	/*
	float dot = DotProduct( blocked_dir, right );

	//Go right on the first try if that works better
	if ( dot < 0.0f )
		arcAngle *= -1;
	*/

	VectorClear( avoidAngles );

	//need to stop it from ping-ponging, so we have a bit of a debounce time on which side you try
	if ( self->NPC->sideStepHoldTime > level.time )
	{
		if ( self->NPC->lastSideStepSide == -1 )//left
		{
			arcAngle *= -1;
		}//else right
		avoidAngles[YAW] = AngleNormalize360( yaw + arcAngle );
		AngleVectors( avoidAngles, movedir, NULL, NULL );
		VectorMA( self->r.currentOrigin, blocked_dist, movedir, block_pos );
		trap_Trace( &tr, self->r.currentOrigin, mins, self->r.maxs, block_pos, self->s.number, self->clipmask|CONTENTS_BOTCLIP );
		return (tr.fraction==1.0&&!tr.allsolid&&!tr.startsolid);
	}

	//test right
	avoidAngles[YAW] = AngleNormalize360( yaw + arcAngle );
	AngleVectors( avoidAngles, avoidRight_dir, NULL, NULL );

	VectorMA( self->r.currentOrigin, blocked_dist, avoidRight_dir, block_pos );
		
	trap_Trace( &tr, self->r.currentOrigin, mins, self->r.maxs, block_pos, self->s.number, self->clipmask|CONTENTS_BOTCLIP );

	if ( !tr.allsolid && !tr.startsolid )
	{
		if ( tr.fraction >= 1.0f )
		{//all clear, go for it (favor the right if both are equal)
			VectorCopy( avoidRight_dir, movedir );
			self->NPC->lastSideStepSide = 1;
			self->NPC->sideStepHoldTime = level.time + 2000;
			return qtrue;
		}
		rightSucc = tr.fraction;
	}
	else
	{
		rightSucc = 0.0f;
	}

	//now test left
	arcAngle *= -1;

	avoidAngles[YAW] = AngleNormalize360( yaw + arcAngle );
	AngleVectors( avoidAngles, avoidLeft_dir, NULL, NULL );

	VectorMA( self->r.currentOrigin, blocked_dist, avoidLeft_dir, block_pos );
		
	trap_Trace( &tr, self->r.currentOrigin, mins, self->r.maxs, block_pos, self->s.number, self->clipmask|CONTENTS_BOTCLIP );

	if ( !tr.allsolid && !tr.startsolid )
	{
		if ( tr.fraction >= 1.0f )
		{//all clear, go for it (right side would have already succeeded if as good as this)
			VectorCopy( avoidLeft_dir, movedir );
			self->NPC->lastSideStepSide = -1;
			self->NPC->sideStepHoldTime = level.time + 2000;
			return qtrue;
		}
		leftSucc = tr.fraction;
	}
	else
	{
		leftSucc = 0.0f;
	}

	if ( leftSucc == 0.0f && rightSucc == 0.0f )
	{//both sides failed
		return qfalse;
	}

	if ( rightSucc*blocked_dist >= avoidRadius || leftSucc*blocked_dist >= avoidRadius ) 
	{//the traces hit something, but got a relatively good distance
		if ( rightSucc >= leftSucc )
		{//favor the right, all things being equal
			VectorCopy( avoidRight_dir, movedir );
			self->NPC->lastSideStepSide = 1;
			self->NPC->sideStepHoldTime = level.time + 2000;
		}
		else
		{
			VectorCopy( avoidLeft_dir, movedir );
			self->NPC->lastSideStepSide = -1;
			self->NPC->sideStepHoldTime = level.time + 2000;
		}
		return qtrue;
	}

	//if neither are enough, we probably can't get around him
	return qfalse;
}

/*
-------------------------
NAVNEW_Bypass
-------------------------
*/
qboolean NAVNEW_Bypass( gentity_t *self, gentity_t *blocker, vec3_t blocked_dir, float blocked_dist, vec3_t movedir, qboolean setBlockedInfo ) 
{
	vec3_t	moveangles, right;

	//Draw debug info if requested
	if ( NAVDEBUG_showCollision )
	{
		G_DrawEdge( self->r.currentOrigin, blocker->r.currentOrigin, EDGE_NORMAL );
	}

	vectoangles( movedir, moveangles );
	moveangles[2] = 0;
	AngleVectors( moveangles, NULL, right, NULL );

	//Check to see what dir the other guy is moving in (if any) and pick the opposite dir
	if ( NAVNEW_DanceWithBlocker( self, blocker, movedir, right ) )
	{
		return qtrue;
	}

	//Okay, so he's not moving to my side, see which side of him is most clear
	if ( NAVNEW_SidestepBlocker( self, blocker, blocked_dir, blocked_dist, movedir, right ) )
	{
		return qtrue;
	}

	//Neither side is clear, tell him to step aside
	NAVNEW_PushBlocker( self, blocker, right, setBlockedInfo );

	return qfalse;
}

/*
-------------------------
NAVNEW_CheckDoubleBlock
-------------------------
*/
qboolean NAVNEW_CheckDoubleBlock( gentity_t *self, gentity_t *blocker, vec3_t blocked_dir )
{
	//Stop double waiting
	if ( ( blocker->NPC ) && ( blocker->NPC->blockingEntNum == self->s.number ) )
		return qtrue;

	return qfalse;
}

/*
-------------------------
NAVNEW_ResolveEntityCollision
-------------------------
*/
extern void CalcTeamDoorCenter ( gentity_t *ent, vec3_t center );
qboolean NAVNEW_ResolveEntityCollision( gentity_t *self, gentity_t *blocker, vec3_t movedir, vec3_t pathDir, qboolean setBlockedInfo )
{
	vec3_t	blocked_dir;
	float blocked_dist;

	//Doors are ignored
	if ( Q_stricmp( blocker->classname, "func_door" ) == 0 )
	{
		vec3_t center;
		CalcTeamDoorCenter ( blocker, center );
		if ( DistanceSquared( self->r.currentOrigin, center ) > MIN_DOOR_BLOCK_DIST_SQR )
			return qtrue;
	}

	VectorSubtract( blocker->r.currentOrigin, self->r.currentOrigin, blocked_dir );
	blocked_dist = VectorNormalize( blocked_dir );

	//Make sure an actual collision is going to happen
//	if ( NAVNEW_PredictCollision( self, blocker, movedir, blocked_dir ) == qfalse )
//		return qtrue;
	
	//First, attempt to walk around the blocker or shove him out of the way
	if ( NAVNEW_Bypass( self, blocker, blocked_dir, blocked_dist, movedir, setBlockedInfo ) )
		return qtrue;

	//Can't get around him... see if I'm blocking him too... if so, I need to just keep moving?
	if ( NAVNEW_CheckDoubleBlock( self, blocker, blocked_dir ) )
		return qtrue;

	if ( setBlockedInfo )
	{
		//Complain about it if we can
		NPC_SetBlocked( self, blocker );
	}

	return qfalse;
}

/*
-------------------------
NAVNEW_AvoidCollision
-------------------------
*/
qboolean NAVNEW_AvoidCollision( gentity_t *self, gentity_t *goal, navInfo_t *info, qboolean setBlockedInfo, int blockedMovesLimit )
{
	vec3_t	movedir;
	vec3_t	movepos;

	//Cap our distance
	if ( info->distance > MAX_COLL_AVOID_DIST )
	{
		info->distance = MAX_COLL_AVOID_DIST;
	}

	//Get an end position
	VectorMA( self->r.currentOrigin, info->distance, info->direction, movepos );
	VectorCopy( info->direction, movedir );

	//Now test against entities
	if ( NAV_CheckAhead( self, movepos, &info->trace, CONTENTS_BODY ) == qfalse )
	{
		//Get the blocker
		info->blocker = &g_entities[ info->trace.entityNum ];
		info->flags |= NIF_COLLISION;

		//Ok to hit our goal entity
		if ( goal == info->blocker )
			return qtrue;

		if ( setBlockedInfo )
		{
			if ( self->NPC->consecutiveBlockedMoves > blockedMovesLimit  )
			{
				if ( d_patched.integer )
				{//use patch-style navigation
					self->NPC->consecutiveBlockedMoves++;
				}
				NPC_SetBlocked( self, info->blocker );
				return qfalse;
			}
			self->NPC->consecutiveBlockedMoves++;
		}
		//See if we're moving along with them
		//if ( NAVNEW_TrueCollision( self, info->blocker, movedir, info->direction ) == qfalse )
		//	return qtrue;

		//Test for blocking by standing on goal
		if ( NAV_TestForBlocked( self, goal, info->blocker, info->distance, &info->flags ) == qtrue )
			return qfalse;

		//If the above function said we're blocked, don't do the extra checks
		/*
		if ( info->flags & NIF_BLOCKED )
			return qtrue;
		*/

		//See if we can get that entity to move out of our way
		if ( NAVNEW_ResolveEntityCollision( self, info->blocker, movedir, info->pathDirection, setBlockedInfo ) == qfalse )
			return qfalse;

		VectorCopy( movedir, info->direction );
		
		return qtrue;
	}
	else
	{
		if ( setBlockedInfo )
		{
			self->NPC->consecutiveBlockedMoves = 0;
		}
	}

	//Our path is clear, just move there
	if ( NAVDEBUG_showCollision )
	{
		G_DrawEdge( self->r.currentOrigin, movepos, EDGE_MOVEDIR );
	}

	return qtrue;
}

qboolean NAVNEW_TestNodeConnectionBlocked( int wp1, int wp2, gentity_t *ignoreEnt, int goalEntNum, qboolean checkWorld, qboolean checkEnts )
{//see if the direct path between 2 nodes is blocked by architecture or an ent
	vec3_t	pos1, pos2, mins, maxs;
	trace_t	trace;
	int		clipmask = MASK_NPCSOLID|CONTENTS_BOTCLIP;
	int ignoreEntNum;
	vec3_t playerMins, playerMaxs;

	if ( !checkWorld && !checkEnts )
	{//duh, nothing to trace against
		return qfalse;
	}
	VectorSet(playerMins, -15, -15, DEFAULT_MINS_2);
	VectorSet(playerMaxs, 15, 15, DEFAULT_MAXS_2);

	trap_Nav_GetNodePosition( wp1, pos1 );
	trap_Nav_GetNodePosition( wp2, pos2 );

	if ( !checkWorld )
	{
		clipmask &= ~(CONTENTS_SOLID|CONTENTS_MONSTERCLIP|CONTENTS_BOTCLIP);
	}
	if ( !checkEnts )
	{
		clipmask &= ~CONTENTS_BODY;
	}
	if ( ignoreEnt )
	{
		VectorCopy( ignoreEnt->r.mins, mins );
		VectorCopy( ignoreEnt->r.maxs, maxs );
		ignoreEntNum = ignoreEnt->s.number;
	}
	else
	{
		VectorCopy( playerMins, mins );
		VectorCopy( playerMaxs, mins );
		ignoreEntNum = ENTITYNUM_NONE;
	}
	mins[2] += STEPSIZE;
	//don't let box get inverted
	if ( mins[2] > maxs[2] )
	{	
		mins[2] = maxs[2];
	}

	trap_Trace( &trace, pos1, mins, maxs, pos2, ignoreEntNum, clipmask );
	if ( trace.fraction >= 1.0f || trace.entityNum == goalEntNum )
	{//clear or hit goal
		return qfalse;
	}
	//hit something we weren't supposed to
	return qtrue;
}
/*
-------------------------
NAVNEW_MoveToGoal
-------------------------
*/
int	NAVNEW_MoveToGoal( gentity_t *self, navInfo_t *info )
{
	int			bestNode = WAYPOINT_NONE;
	qboolean	foundClearPath = qfalse;
	vec3_t		origin;
	navInfo_t	tempInfo;
	qboolean	setBlockedInfo = qtrue;
	qboolean	inBestWP, inGoalWP, goalWPFailed = qfalse;
	int			numTries = 0;

	memcpy( &tempInfo, info, sizeof( tempInfo ) );

	//Must have a goal entity to move there
	if( self->NPC->goalEntity == NULL )
		return WAYPOINT_NONE;

	if ( self->waypoint == WAYPOINT_NONE && self->noWaypointTime > level.time )
	{//didn't have a valid one in about the past second, don't look again just yet
		return WAYPOINT_NONE;
	}
	if ( self->NPC->goalEntity->waypoint == WAYPOINT_NONE && self->NPC->goalEntity->noWaypointTime > level.time )
	{//didn't have a valid one in about the past second, don't look again just yet
		return WAYPOINT_NONE;
	}
	if ( self->noWaypointTime > level.time &&
		self->NPC->goalEntity->noWaypointTime > level.time )
	{//just use current waypoints
		bestNode = trap_Nav_GetBestNodeAltRoute2( self->waypoint, self->NPC->goalEntity->waypoint, bestNode );
	}
	//FIXME!!!!: this is making them wiggle back and forth between waypoints
	else if ( (bestNode = trap_Nav_GetBestPathBetweenEnts( self, self->NPC->goalEntity, NF_CLEAR_PATH )) == NODE_NONE )//!NAVNEW_GetWaypoints( self, qtrue ) )
	{//one of us didn't have a valid waypoint!
		if ( self->waypoint == NODE_NONE )
		{//don't even try to find one again for a bit
			self->noWaypointTime = level.time + Q_irand( 500, 1500 );
		}
		if ( self->NPC->goalEntity->waypoint == NODE_NONE )
		{//don't even try to find one again for a bit
			self->NPC->goalEntity->noWaypointTime = level.time + Q_irand( 500, 1500 );
		}
		return WAYPOINT_NONE;
	}
	else
	{
		if ( self->NPC->goalEntity->noWaypointTime < level.time )
		{
			self->NPC->goalEntity->noWaypointTime = level.time + Q_irand( 500, 1500 );
		}
	}

	while( !foundClearPath )
	{
		inBestWP = inGoalWP = qfalse;
		/*
		bestNode = trap_Nav_GetBestNodeAltRoute( self->waypoint, self->NPC->goalEntity->waypoint, bestNode );
		*/

		if ( bestNode == WAYPOINT_NONE )
		{
			goto failed;
		}

		//see if we can get directly to the next node off bestNode en route to goal's node...
		//NOTE: shouldn't be necc. now
		/*
		int oldBestNode = bestNode;
		bestNode = NAV_TestBestNode( self, self->waypoint, bestNode, qtrue );//, self->NPC->goalEntity->waypoint );// 
		//NOTE: Guaranteed to return something
		if ( bestNode != oldBestNode )
		{//we were blocked somehow
			if ( setBlockedInfo )
			{
				self->NPC->aiFlags |= NPCAI_BLOCKED;
				trap_Nav_GetNodePosition( oldBestNode, NPCInfo->blockedDest );
			}
		}
		*/
		trap_Nav_GetNodePosition( bestNode, origin );
		/*
		if ( !goalWPFailed )
		{//we haven't already tried to go straight to goal or goal's wp
			if ( bestNode == self->NPC->goalEntity->waypoint )
			{//our bestNode is the goal's wp
				if ( NAV_HitNavGoal( self->r.currentOrigin, self->r.mins, self->r.maxs, origin, trap_Nav_GetNodeRadius( bestNode ), FlyingCreature( self ) ) )
				{//we're in the goal's wp
					inGoalWP = qtrue;
					//we're in the goalEntity's waypoint already
					//so head for the goalEntity since we know it's clear of architecture
					//FIXME: this is pretty stupid because the NPCs try to go straight
					//		towards their goal before then even try macro_nav...
					VectorCopy( self->NPC->goalEntity->r.currentOrigin, origin );
				}
			}
		}
		*/
		if ( !inGoalWP )
		{//not heading straight for goal
			if ( bestNode == self->waypoint )
			{//we know it's clear or architecture
				//trap_Nav_GetNodePosition( self->waypoint, origin );
				/*
				if ( NAV_HitNavGoal( self->r.currentOrigin, self->r.mins, self->r.maxs, origin, trap_Nav_GetNodeRadius( bestNode ), FlyingCreature( self ) ) )
				{//we're in the wp we're heading for already
					inBestWP = qtrue;
				}
				*/
			}
			else
			{//heading to an edge off our confirmed clear waypoint... make sure it's clear
				//it it's not, bestNode will fall back to our waypoint
				int oldBestNode = bestNode;
				bestNode = NAV_TestBestNode( self, self->waypoint, bestNode, qtrue );
				if ( bestNode == self->waypoint )
				{//we fell back to our waypoint, reset the origin
					self->NPC->aiFlags |= NPCAI_BLOCKED;
					trap_Nav_GetNodePosition( oldBestNode, NPCInfo->blockedDest );
					trap_Nav_GetNodePosition( bestNode, origin );
				}
			}
		}
		//Com_Printf( "goalwp = %d, mywp = %d, node = %d, origin = %s\n", self->NPC->goalEntity->waypoint, self->waypoint, bestNode, vtos(origin) );

		memcpy( &tempInfo, info, sizeof( tempInfo ) );
		VectorSubtract( origin, self->r.currentOrigin, tempInfo.direction );
		VectorNormalize( tempInfo.direction );

		//NOTE: One very important thing NAVNEW_AvoidCollision does is
		//		it actually CHANGES the value of "direction" - it changes it to
		//		whatever dir you need to go in to avoid the obstacle...
		foundClearPath = NAVNEW_AvoidCollision( self, self->NPC->goalEntity, &tempInfo, setBlockedInfo, 5 );

		if ( !foundClearPath )
		{//blocked by an ent
			if ( inGoalWP )
			{//we were heading straight for the goal, head for the goal's wp instead
				trap_Nav_GetNodePosition( bestNode, origin );
				foundClearPath = NAVNEW_AvoidCollision( self, self->NPC->goalEntity, &tempInfo, setBlockedInfo, 5 );
			}
		}

		if ( foundClearPath )
		{//clear!
			//If we got set to blocked, clear it
			NPC_ClearBlocked( self );
			//Take the dir
			memcpy( info, &tempInfo, sizeof( *info ) );
			if ( self->s.weapon == WP_SABER )
			{//jedi
				if ( info->direction[2] * info->distance > 64 )
				{
					self->NPC->aiFlags |= NPCAI_BLOCKED;
					VectorCopy( origin, NPCInfo->blockedDest );
					goto failed;
				}
			}
		}
		else
		{//blocked by ent!
			if ( setBlockedInfo )
			{
				self->NPC->aiFlags |= NPCAI_BLOCKED;
				trap_Nav_GetNodePosition( bestNode, NPCInfo->blockedDest );
			}
			//Only set blocked info first time
			setBlockedInfo = qfalse;

			if ( inGoalWP )
			{//we headed for our goal and failed and our goal's WP and failed
				if ( self->waypoint == self->NPC->goalEntity->waypoint )
				{//our waypoint is our goal's waypoint, nothing we can do
					//remember that this node is blocked
					trap_Nav_AddFailedNode( self, self->waypoint );
					goto failed;
				}
				else
				{//try going for our waypoint this time
					goalWPFailed = qtrue;
					inGoalWP = qfalse;
				}
			}
			else if ( bestNode != self->waypoint )
			{//we headed toward our next waypoint (instead of our waypoint) and failed
				if ( d_altRoutes.integer )
				{//mark this edge failed and try our waypoint
					//NOTE: don't assume there is something blocking the direct path
					//			between my waypoint and the bestNode... I could be off
					//			that path because of collision avoidance...
					if ( d_patched.integer &&//use patch-style navigation
 						( !trap_Nav_NodesAreNeighbors( self->waypoint, bestNode )
						|| NAVNEW_TestNodeConnectionBlocked( self->waypoint, bestNode, self, self->NPC->goalEntity->s.number, qfalse, qtrue ) ) )
					{//the direct path between these 2 nodes is blocked by an ent
						trap_Nav_AddFailedEdge( self->s.number, self->waypoint, bestNode );
					}
					bestNode = self->waypoint;
				}
				else
				{
					//we should stop
					goto failed;
				}
			}
			else 
			{//we headed for *our* waypoint and couldn't get to it
				if ( d_altRoutes.integer )
				{
					//remember that this node is blocked
					trap_Nav_AddFailedNode( self, self->waypoint );
					//Now we should get our waypoints again
					//FIXME: cache the trace-data for subsequent calls as only the route info would have changed
					//if ( (bestNode = trap_Nav_GetBestPathBetweenEnts( self, self->NPC->goalEntity, NF_CLEAR_PATH )) == NODE_NONE )//!NAVNEW_GetWaypoints( self, qfalse ) )
					{//one of our waypoints is WAYPOINT_NONE now
						goto failed;
					}
				}
				else
				{
					//we should stop
					goto failed;
				}
			}

			if ( ++numTries >= 10 )
			{
				goto failed;
			}
		}
	}

//finish:
	//Draw any debug info, if requested
	if ( NAVDEBUG_showEnemyPath )
	{
		vec3_t	dest, start;

		//Get the positions
		trap_Nav_GetNodePosition( self->NPC->goalEntity->waypoint, dest );
		trap_Nav_GetNodePosition( bestNode, start );

		//Draw the route
		G_DrawNode( start, NODE_START );
		if ( bestNode != self->waypoint )
		{
			vec3_t	wpPos;
			trap_Nav_GetNodePosition( self->waypoint, wpPos );
			G_DrawNode( wpPos, NODE_NAVGOAL );
		}
		G_DrawNode( dest, NODE_GOAL );
		G_DrawEdge( dest, self->NPC->goalEntity->r.currentOrigin, EDGE_PATH );
		G_DrawNode( self->NPC->goalEntity->r.currentOrigin, NODE_GOAL );
		trap_Nav_ShowPath( bestNode, self->NPC->goalEntity->waypoint );
	}

	self->NPC->shoveCount = 0;

	//let me keep this waypoint for a while
	if ( self->noWaypointTime < level.time )
	{
		self->noWaypointTime = level.time + Q_irand( 500, 1500 );
	}
	return bestNode;

failed:
	//FIXME: What we should really do here is have a list of the goal's and our
	//		closest clearpath waypoints, ranked.  If the first set fails, try the rest
	//		until there are no alternatives.

	trap_Nav_GetNodePosition( self->waypoint, origin );

	//do this to avoid ping-ponging?
	return WAYPOINT_NONE;
	/*
	//this was causing ping-ponging
	if ( DistanceSquared( origin, self->r.currentOrigin ) < 16 )//woo, magic number
	{//We're right up on our waypoint, so that won't help, return none
		//Or maybe find the nextbest here?
		return WAYPOINT_NONE;
	}
	else
	{//Try going to our waypoint
		bestNode = self->waypoint;

		VectorSubtract( origin, self->r.currentOrigin, info.direction );
		VectorNormalize( info.direction );
	}
	
	goto finish;
	*/
}