/*
 */


#include "../game/q_shared.h"
#include "gs_public.h"

//#define CHECK_TRAPPED
#define GS_SLIDEMOVE_CLAMPING

#define	STOP_EPSILON	0.1

#define IsGroundPlane( normal, gravityDir ) ( DotProduct( normal, gravityDir ) < -0.45f )

//==================================================
// SNAP AND CLIP ORIGIN AND VELOCITY
//==================================================

//=================
//GS_GoodPosition
//=================
static qboolean GS_GoodPosition( int snaptorigin[3], vec3_t mins, vec3_t maxs, int passent, int contentmask )
{
	trace_t	trace;
	vec3_t point;
	int i;

	if( !( contentmask & MASK_SOLID ) )
		return qtrue;

	for( i = 0; i < 3; i++ )
		point[i] = (float)snaptorigin[i] * ( 1.0/PM_VECTOR_SNAP );

	GS_Trace( &trace, point, mins, maxs, point, passent, contentmask );
	return !trace.allsolid;
}

//=================
//GS_SnapInitialPosition
//=================
qboolean GS_SnapInitialPosition( vec3_t origin, vec3_t mins, vec3_t maxs, int passent, int contentmask )
{
	int x, y, z;
	int base[3];
	static const int offset[3] = { 0, -1, 1 };
	int originInt[3];

	VectorScale( origin, PM_VECTOR_SNAP, originInt );
	VectorCopy( originInt, base );

	for( z = 0; z < 3; z++ )
	{
		originInt[2] = base[2] + offset[z];
		for( y = 0; y < 3; y++ )
		{
			originInt[1] = base[1] + offset[y];
			for( x = 0; x < 3; x++ )
			{
				originInt[0] = base[0] + offset[x];
				if( GS_GoodPosition( originInt, mins, maxs, passent, contentmask ) )
				{
					origin[0] = originInt[0]*( 1.0/PM_VECTOR_SNAP );
					origin[1] = originInt[1]*( 1.0/PM_VECTOR_SNAP );
					origin[2] = originInt[2]*( 1.0/PM_VECTOR_SNAP );
					return qtrue;
				}
			}
		}
	}

	return qfalse;
}

//=================
//GS_SnapPosition
//=================
qboolean GS_SnapPosition( vec3_t origin, vec3_t mins, vec3_t maxs, int passent, int contentmask )
{
	int sign[3];
	int i, j, bits;
	int base[3];
	int originInt[3];
	// try all single bits first
	static const int jitterbits[8] = { 0, 4, 1, 2, 3, 5, 6, 7 };

	for( i = 0; i < 3; i++ )
	{
		if( origin[i] >= 0 )
			sign[i] = 1;
		else
			sign[i] = -1;
		originInt[i] = (int)( origin[i] * PM_VECTOR_SNAP );
		if( (float)originInt[i] * ( 1.0/PM_VECTOR_SNAP ) == origin[i] )
			sign[i] = 0;
	}

	VectorCopy( originInt, base );

	// try all combinations
	for( j = 0; j < 8; j++ )
	{
		bits = jitterbits[j];
		VectorCopy( base, originInt );
		for( i = 0; i < 3; i++ )
		{
			if( bits & ( 1<<i ) )
				originInt[i] += sign[i];
		}

		if( GS_GoodPosition( originInt, mins, maxs, passent, contentmask ) )
		{
			VectorScale( originInt, ( 1.0/PM_VECTOR_SNAP ), origin );
			return qtrue;
		}
	}

	return qfalse;
}

//=================
//GS_SnapVelocity
//=================
void GS_SnapVelocity( vec3_t velocity )
{
	int i, velocityInt[3];
	// snap velocity to sixteenths
	for( i = 0; i < 3; i++ )
	{
		velocityInt[i] = (int)( velocity[i]*PM_VECTOR_SNAP );
		velocity[i] = (float)velocityInt[i] * ( 1.0/PM_VECTOR_SNAP );
	}
}

//=================
//GS_ClipVelocity
//=================
void GS_ClipVelocity( vec3_t in, vec3_t normal, vec3_t out, float overbounce )
{
	float backoff;
	float change;
	int i;

	backoff = DotProduct( in, normal );

	if( backoff <= 0 )
	{
		backoff *= overbounce;
	}
	else
	{
		backoff /= overbounce;
	}

	for( i = 0; i < 3; i++ )
	{
		change = normal[i] * backoff;
		out[i] = in[i] - change;
	}
#ifdef GS_SLIDEMOVE_CLAMPING
	{
		float oldspeed, newspeed;
		oldspeed = VectorLength( in );
		newspeed = VectorLength( out );
		if( newspeed > oldspeed )
		{
			VectorNormalize( out );
			VectorScale( out, oldspeed, out );
		}
	}
#endif
}

//==================================================
// SLIDE MOVE
//
// Note: groundentity info should be up to date when calling any slide move function
//==================================================

//=================
//GS_ClipVelocity
//=================
static void GS_AddTouchEnt( move_t *move, int entNum )
{
	int i;

	if( move->numtouch >= MAXTOUCH || entNum < 0 )
		return;

	// see if it is already added
	for( i = 0; i < move->numtouch; i++ )
	{
		if( move->touchents[i] == entNum )
			return;
	}

	// add it
	move->touchents[move->numtouch] = entNum;
	move->numtouch++;
}

//=================
//GS_ClearClippingPlanes
//=================
static void GS_ClearClippingPlanes( move_t *move )
{
	move->numClipPlanes = 0;
}

//=================
//GS_ClipVelocityToClippingPlanes
//=================
static void GS_ClipVelocityToClippingPlanes( move_t *move )
{
	int i;

	for( i = 0; i < move->numClipPlanes; i++ )
	{
		if( DotProduct( move->velocity, move->clipPlaneNormals[i] ) >= SLIDEMOVE_PLANEINTERACT_EPSILON )
			continue; // looking in the same direction than the velocity
#ifndef TRACEVICFIX
#ifndef TRACE_NOAXIAL
		// this is a hack, cause non axial planes can return invalid positions in trace endpos
		if( PlaneTypeForNormal( move->clipPlaneNormals[i] ) == PLANE_NONAXIAL )
		{
			// offset the origin a little bit along the plane normal
			VectorMA( move->origin, 0.05, move->clipPlaneNormals[i], move->origin );
		}
#endif
#endif

		GS_ClipVelocity( move->velocity, move->clipPlaneNormals[i], move->velocity, move->slideBounce );
	}
}

//=================
//GS_AddClippingPlane
//=================
static void GS_AddClippingPlane( move_t *move, const vec3_t planeNormal )
{
	int i;

	// see if we are already clipping to this plane
	for( i = 0; i < move->numClipPlanes; i++ )
	{
		if( DotProduct( planeNormal, move->clipPlaneNormals[i] ) >= ( 1.0f - SLIDEMOVE_PLANEINTERACT_EPSILON ) )
		{
			return;
		}
	}

	if( move->numClipPlanes + 1 == MAX_SLIDEMOVE_CLIP_PLANES )
		GS_Error( "GS_AddTouchPlane: MAX_SLIDEMOVE_CLIP_PLANES reached\n" );

	// add the plane
	VectorCopy( planeNormal, move->clipPlaneNormals[move->numClipPlanes] );
	move->numClipPlanes++;
}

//=================
//GS_SlideMoveClipMove
//=================
static int GS_SlideMoveClipMove( move_t *move /*, const qboolean stepping*/ )
{
	vec3_t endpos, startingOrigin, startingVelocity;
	trace_t	trace;
	int blockedmask = 0;

	VectorCopy( move->origin, startingOrigin );
	VectorCopy( move->velocity, startingVelocity );

	VectorMA( move->origin, move->remainingTime, move->velocity, endpos );
	GS_Trace( &trace, move->origin, move->mins, move->maxs, endpos, move->passent, move->contentmask );
	if( trace.allsolid )
	{
		if( trace.ent > 0 )
			GS_AddTouchEnt( move, trace.ent );
		return blockedmask|SLIDEMOVEFLAG_TRAPPED;
	}

	if( trace.fraction == 1.0f )
	{                          // was able to cleanly perform the full move
		VectorCopy( trace.endpos, move->origin );
		move->remainingTime -= ( trace.fraction * move->remainingTime );
		return blockedmask|SLIDEMOVEFLAG_MOVED;
	}

	if( trace.fraction < 1.0f )
	{                         // wasn't able to make the full move
		GS_AddTouchEnt( move, trace.ent );
		blockedmask |= SLIDEMOVEFLAG_PLANE_TOUCHED;

		// move what can be moved
		if( trace.fraction > 0.0 )
		{
			VectorCopy( trace.endpos, move->origin );
			move->remainingTime -= ( trace.fraction * move->remainingTime );
			blockedmask |= SLIDEMOVEFLAG_MOVED;
		}

		// if the plane is a wall and stepping, try to step it up
		if( !IsGroundPlane( trace.plane.normal, move->gravityDir ) )
		{
			//if( stepping && GS_StepUp( move ) ) {
			//	return blockedmask;  // solved : don't add the clipping plane
			//}
			//else {
			blockedmask |= SLIDEMOVEFLAG_WALL_BLOCKED;
			//}
		}

		GS_AddClippingPlane( move, trace.plane.normal );
	}

	return blockedmask;
}

//=================
//GS_SlideMove
//=================
int GS_SlideMove( move_t *move )
{
#define MAX_SLIDEMOVE_ATTEMPTS	8
	int count;
	int blockedmask = 0;
	vec3_t lastValidOrigin, originalVelocity;

	// if the velocity is too small, just stop
	if( VectorLength( move->velocity ) < STOP_EPSILON )
	{
		VectorClear( move->velocity );
		move->remainingTime = 0;
		return 0;
	}

	VectorCopy( move->velocity, originalVelocity );
	VectorCopy( move->origin, lastValidOrigin );

	GS_ClearClippingPlanes( move );
	move->numtouch = 0;

	for( count = 0; count < MAX_SLIDEMOVE_ATTEMPTS; count++ )
	{
		// get the original velocity and clip it to all the planes we got in the list
		VectorCopy( originalVelocity, move->velocity );
		GS_ClipVelocityToClippingPlanes( move );
		blockedmask = GS_SlideMoveClipMove( move /*, stepping*/ );

#ifdef CHECK_TRAPPED
		{
			trace_t	trace;
			GS_Trace( &trace, move->origin, move->mins, move->maxs, move->origin, move->passent, move->contentmask );
			if( trace.startsolid )
			{
				blockedmask |= SLIDEMOVEFLAG_TRAPPED;
			}
		}
#endif

		// can't continue
		if( blockedmask & SLIDEMOVEFLAG_TRAPPED )
		{
#ifdef CHECK_TRAPPED
			GS_Printf( "GS_SlideMove SLIDEMOVEFLAG_TRAPPED\n" );
#endif
			move->remainingTime = 0.0f;
			VectorCopy( lastValidOrigin, move->origin );
			return blockedmask;
		}

		VectorCopy( move->origin, lastValidOrigin );

		// touched a plane, re-clip velocity and retry
		if( blockedmask & SLIDEMOVEFLAG_PLANE_TOUCHED )
		{
			continue;
		}

		// if it didn't touch anything the move should be completed
		if( move->remainingTime > 0.0f )
		{
			GS_Printf( "slidemove finished with remaining time\n" );
			move->remainingTime = 0.0f;
		}

		break;
	}

	// snap
	GS_SnapPosition( move->origin, move->mins, move->maxs, move->passent, move->contentmask );
	GS_SnapVelocity( move->velocity );

	return blockedmask;
}
