/*
Copyright (C) 1997-2001 Id Software, Inc.

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  

See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

*/

#include "../g_local.h"
#include "ai_local.h"

#define	MAX_CLIP_PLANES	5

void SV_Impact (edict_t *e1, trace_t *trace);
int ClipVelocity (vec3_t in, vec3_t normal, vec3_t out, float overbounce);


//==========================================
// AI_NPCPhysStepUp
// Stepping helper to AI_NPCPhysMove
//==========================================
qboolean AI_NPCPhysStepUp( edict_t *ent, vec3_t virtual_origin, int mask )
{
	vec3_t start;
	vec3_t	end;
	vec3_t	movedir;
	trace_t	trace;

	//check for having groundentity
	VectorCopy( virtual_origin, end ); end[2] -= 0.25f;//inside floor
	//trace = gi.trace( virtual_origin, ent->mins, ent->maxs, end, ent, mask );
	trap_Trace( &trace, virtual_origin, ent->r.mins, ent->r.maxs, end, ent, mask );
	if( trace.fraction == 1.0f || trace.plane.normal[2] < 0.7 )
		return qfalse;

	VectorCopy( virtual_origin, start );
	VectorNormalize2( ent->velocity, movedir );
	movedir[2] = 0.0f;

	start[2] += AI_STEPSIZE;
	VectorMA( start, 0.3f, movedir, end );
	end[2] -= AI_STEPSIZE*2;

	trap_Trace( &trace, start, ent->r.mins, ent->r.maxs, end, ent, mask);
	//trace = gi.trace (start, ent->r.mins, ent->r.maxs, end, ent, mask);
	if( trace.startsolid || !trace.fraction )
		return qfalse;

	if( trace.fraction )
		VectorCopy( trace.endpos, ent->s.origin );

	return qtrue;
}

//==========================================
// AI_NPCPhysMove
// Move entity's box based on it's velocity vector.
// It does slide on planes, and handles stepping up
// if enabled. Doesn't add physics effects, only box movement.
// It's a modified copy of SV_FlyMove.
//==========================================
int AI_NPCPhysMove (edict_t *ent, float time, int mask, qboolean step, float bouncescale )
{
	edict_t		*hit;
	int			bumpcount, numbumps;
	vec3_t		dir;
	float		d;
	int			numplanes;
	vec3_t		planes[MAX_CLIP_PLANES];
	vec3_t		primal_velocity, original_velocity, new_velocity;
	int			i, j;
	trace_t		trace;
	vec3_t		end;
	float		time_left;
	int			blocked;
	
	numbumps = 4;
	
	blocked = 0;
	VectorCopy ( ent->velocity, original_velocity);
	VectorCopy ( ent->velocity, primal_velocity);
	numplanes = 0;
	
	time_left = time;

	ent->groundentity = NULL;
	for (bumpcount=0 ; bumpcount<numbumps ; bumpcount++)
	{
retry:
		for (i=0 ; i<3 ; i++)
			end[i] = ent->s.origin[i] + time_left * ent->velocity[i];

		//trace = gi.trace (ent->s.origin, ent->r.mins, ent->r.maxs, end, ent, mask);
		trap_Trace( &trace, ent->s.origin, ent->r.mins, ent->r.maxs, end, ent, mask);

		if (trace.allsolid)
		{	// entity is trapped in another solid
			VectorCopy (vec3_origin, ent->velocity);
			return 3;
		}

		if (trace.fraction > 0)
		{	// actually covered some distance
			VectorCopy (trace.endpos, ent->s.origin);
			VectorCopy (ent->velocity, original_velocity);
			numplanes = 0;
		}

		if (trace.fraction == 1)
			 break;		// moved the entire distance

		hit = &game.edicts[trace.ent];

		if (trace.plane.normal[2] > 0.7)
		{
			blocked |= 1;		// floor
			if ( hit->r.solid == SOLID_BSP)
			{
				ent->groundentity = hit;
				ent->groundentity_linkcount = hit->r.linkcount;
			}
		}
		if (!trace.plane.normal[2])
		{
			//try to solve the step
			if( step && AI_NPCPhysStepUp( ent, trace.endpos, mask ) )
			{
				time_left -= time_left * trace.fraction;
				VectorCopy (ent->velocity, original_velocity);
				numplanes = 0;
				goto retry;
			}
			blocked |= 2;		// step
		}

//
// run the impact function
//
		SV_Impact (ent, &trace);
		if (!ent->r.inuse)
			break;		// removed by the impact function

		
		time_left -= time_left * trace.fraction;
		
	// cliped to another plane
		if (numplanes >= MAX_CLIP_PLANES)
		{	// this shouldn't really happen
			VectorCopy (vec3_origin, ent->velocity);
			return 3;
		}

		VectorCopy (trace.plane.normal, planes[numplanes]);
		numplanes++;

//
// modify original_velocity so it parallels all of the clip planes
//
		for (i=0 ; i<numplanes ; i++)
		{
			ClipVelocity (original_velocity, planes[i], new_velocity, bouncescale);
			for (j=0 ; j<numplanes ; j++)
				if (j != i)
				{
					if (DotProduct (new_velocity, planes[j]) < 0)
						break;	// not ok
				}
			if (j == numplanes)
				break;
		}
		
		if (i != numplanes)
		{	// go along this plane
			VectorCopy (new_velocity, ent->velocity);
		}
		else
		{	// go along the crease
			if (numplanes != 2)
			{
//				gi.dprintf ("clip velocity, numplanes == %i\n",numplanes);
				VectorCopy (vec3_origin, ent->velocity);
				return 7;
			}
			CrossProduct (planes[0], planes[1], dir);
			d = DotProduct (dir, ent->velocity);
			VectorScale (dir, d, ent->velocity);
		}

//
// if original velocity is against the original velocity, stop dead
// to avoid tiny occilations in sloping corners
//
		if (DotProduct (ent->velocity, primal_velocity) <= 0)
		{
			VectorCopy (vec3_origin, ent->velocity);
			return blocked;
		}
	}

	return blocked;
}


//==========================================
// M_Phys_Momentum_AddPush
// add a force impulse to the acceleration
//==========================================
void M_Phys_Momentum_AddPush( vec3_t accel, vec3_t pushdir, float push, float mass, float timestep )
{
	float pushaccel;

	if( !mass )
		mass = 100;

	pushaccel = ((1000 * push) / mass) * timestep;
	VectorNormalize( pushdir );
	VectorScale( pushdir, pushaccel, pushdir );
	VectorAdd( accel, pushdir, accel );
}

//==========================================
// M_Phys_Momentum_AddFriction
//==========================================
void M_Phys_Momentum_AddFriction( float classfriction, float class_stopspeed, vec3_t origin, vec3_t vel, float timestep, vec3_t mins, vec3_t maxs, edict_t *passent, int solidmask )
{
	float	speed, newspeed, control;
	float	friction;
	float	drop;
	int		groundentity = -1;
	int		surfflags = 0;
	trace_t	trace;
	vec3_t	v1;

	//check groundentity
	VectorCopy( origin, v1 ); v1[2] -= 0.25f;//inside floor
	trap_Trace( &trace, origin, mins, maxs, v1, passent, solidmask );
	//trace = gi.trace( origin, mins, maxs, v1, passent, solidmask );
	if( trace.fraction < 1.0f && trace.plane.normal[2] >= 0.7 ) // We have ground entity
	{
		groundentity = 1;
		surfflags = trace.surfFlags;
		//surfflags = trace.surface->flags;
	}

	speed = vel[0]*vel[0] +vel[1]*vel[1] + vel[2]*vel[2];
	if (speed < 1)
	{
		vel[0] = 0;
		vel[1] = 0;
		return;
	}

	speed = sqrt(speed);
	drop = 0;

// apply ground friction
	if (((groundentity != -1) && !(surfflags & SURF_SLICK) ) /*&& (pm->waterlevel < 2) || (pml.ladder)*/ )
	{
		friction = classfriction;//jalfixme
		control = speed < class_stopspeed ? class_stopspeed : speed;
		drop += control*friction*timestep;
	}

	/*
// apply water friction
	if ((pm->waterlevel >= 2) && !pml.ladder)
		drop += speed*pm_waterfriction*pm->waterlevel*timestep;
*/

// scale the velocity
	newspeed = speed - drop;
	if (newspeed <= 0)
	{
		newspeed = 0;
		VectorClear ( vel );
	}
	else
	{
		newspeed /= speed;
		VectorScale ( vel, newspeed, vel );
	}
}