/*
   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

//==========================================
// AI_NPCPhysStepUp
// Stepping helper to AI_NPCPhysMove
//==========================================
#if 0
static 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 );
	G_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;

	G_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;
}
#endif

//==========================================
// 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
	G_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 = trace.ent;
		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 );
	}
}

//==========================================
// M_Phys_Momentum_AddFriction
//==========================================
void M_Phys_Momentum_AddFriction2( float classfriction, float class_stopspeed, vec3_t origin, vec3_t vel, float timestep, vec3_t mins, vec3_t maxs )
{
	float speed, newspeed, control;
	float friction;
	float drop;

	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;
	friction = classfriction; //jalfixme
	control = speed < class_stopspeed ? class_stopspeed : speed;
	drop += control*friction*timestep;

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