/*
===========================================================================
Copyright (C) 1999-2005 Id Software, Inc.

This file is part of Quake III Arena source code.

Quake III Arena source code 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.

Quake III Arena source code 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 Foobar; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
===========================================================================
*/
//
// bg_slidemove.c -- part of bg_pmove functionality
//
// Reconstructed from Quake Live x86_64 binary.
// PM_SlideMove at 0x00155ed0, PM_StepSlideMove at 0x00156850.

#include "q_shared.h"
#include "bg_public.h"
#include "bg_local.h"

extern	float	_DAT_003cfe90;		// velocity damping factor for step-up
extern	int		DAT_003cfe30;		// edge grab enabled
extern	int		pmove_stepJumpFlag;	// step jump enabled

/*
==================
PM_SlideMove

Returns qtrue if the velocity was clipped in some way
==================
*/
#define	MAX_CLIP_PLANES	5
qboolean PM_SlideMove( qboolean gravity ) {
	int			bumpcount;
	int			numplanes;
	vec3_t		planes[MAX_CLIP_PLANES];
	vec3_t		clipVelocity;
	vec3_t		endClipVelocity;
	vec3_t		endVelocity;
	vec3_t		end;
	vec3_t		dir;
	trace_t		trace;
	float		primal_vel_x, primal_vel_y, primal_vel_z;
	float		time_left;
	float		into;
	float		d;
	int			i, j, k;

	primal_vel_x = pm->ps->velocity[0];
	primal_vel_y = pm->ps->velocity[1];
	primal_vel_z = pm->ps->velocity[2];

	if ( !gravity ) {
		endVelocity[0] = 0.0f;
		endVelocity[1] = 0.0f;
		endVelocity[2] = 0.0f;
	} else {
		endVelocity[0] = primal_vel_x;
		endVelocity[1] = pm->ps->velocity[1];
		primal_vel_z = pm->ps->velocity[2] - (float)pm->ps->gravity * pml.frametime;
		pm->ps->velocity[2] = (pm->ps->velocity[2] + primal_vel_z) * 0.5f;
		endVelocity[2] = primal_vel_z;
		if ( pml.groundPlane ) {
			// slide along the ground plane
			PM_ClipVelocity( pm->ps->velocity, pml.groundTrace.plane.normal,
				pm->ps->velocity, OVERCLIP );
		}
	}

	time_left = pml.frametime;

	// never turn against the ground plane
	if ( pml.groundPlane ) {
		numplanes = 2;
		VectorCopy( pml.groundTrace.plane.normal, planes[0] );
		VectorNormalize2( pm->ps->velocity, planes[1] );
	} else {
		numplanes = 1;
		VectorNormalize2( pm->ps->velocity, planes[0] );
	}

	for ( bumpcount = 0; bumpcount < 4; bumpcount++ ) {

		// calculate position we are trying to move to
		VectorMA( pm->ps->origin, time_left, pm->ps->velocity, end );

		// see if we can make it there
		pm->trace( &trace, pm->ps->origin, pm->mins, pm->maxs, end,
			pm->ps->clientNum, pm->tracemask );

		if ( trace.allsolid ) {
			// entity is completely trapped in another solid
			pm->ps->velocity[2] = 0.0f;
			return qtrue;
		}

		if ( trace.fraction > 0.0f ) {
			// actually covered some distance
			VectorCopy( trace.endpos, pm->ps->origin );
		}

		if ( trace.fraction == 1.0f ) {
			 break;		// moved the entire distance
		}

		// save entity for contact
		PM_AddTouchEnt( trace.entityNum );

		time_left -= time_left * trace.fraction;

		if ( numplanes >= MAX_CLIP_PLANES ) {
			// this shouldn't really happen
			VectorClear( pm->ps->velocity );
			return qtrue;
		}

		//
		// if this is the same plane we hit before, nudge velocity
		// out along it, which fixes some epsilon issues with
		// non-axial planes
		//
		for ( i = 0; i < numplanes; i++ ) {
			if ( (double)DotProduct( trace.plane.normal, planes[i] ) > 0.99 ) {
				VectorAdd( trace.plane.normal, pm->ps->velocity, pm->ps->velocity );
				break;
			}
		}
		if ( i < numplanes ) {
			continue;
		}
		VectorCopy( trace.plane.normal, planes[numplanes] );
		numplanes++;

		//
		// modify velocity so it parallels all of the clip planes
		//

		// find a plane that it enters
		for ( i = 0; i < numplanes; i++ ) {
			into = DotProduct( pm->ps->velocity, planes[i] );
			if ( (double)into >= 0.1 ) {
				continue;		// move doesn't interact with the plane
			}

			// see how hard we are hitting things
			if ( -into > pml.impactSpeed ) {
				pml.impactSpeed = -into;
			}

			// slide along the plane
			PM_ClipVelocity( pm->ps->velocity, planes[i], clipVelocity, OVERCLIP );

			// slide along the plane
			PM_ClipVelocity( endVelocity, planes[i], endClipVelocity, OVERCLIP );

			// see if there is a second plane that the new move enters
			for ( j = 0; j < numplanes; j++ ) {
				if ( j == i ) {
					continue;
				}
				if ( (double)DotProduct( clipVelocity, planes[j] ) >= 0.1 ) {
					continue;		// move doesn't interact with the plane
				}

				// try clipping the move to the plane
				PM_ClipVelocity( clipVelocity, planes[j], clipVelocity, OVERCLIP );
				PM_ClipVelocity( endClipVelocity, planes[j], endClipVelocity, OVERCLIP );

				// see if it goes back into the first clip plane
				if ( DotProduct( clipVelocity, planes[i] ) >= 0.0f ) {
					continue;
				}

				// slide the original velocity along the crease
				CrossProduct( planes[i], planes[j], dir );
				VectorNormalize( dir );
				d = DotProduct( dir, pm->ps->velocity );
				VectorScale( dir, d, clipVelocity );

				CrossProduct( planes[i], planes[j], dir );
				VectorNormalize( dir );
				d = DotProduct( dir, endVelocity );
				VectorScale( dir, d, endClipVelocity );

				// see if there is a third plane the new move enters
				for ( k = 0; k < numplanes; k++ ) {
					if ( k == i || k == j ) {
						continue;
					}
					if ( (double)DotProduct( clipVelocity, planes[k] ) >= 0.1 ) {
						continue;		// move doesn't interact with the plane
					}

					// stop dead at a triple plane interaction
					VectorClear( pm->ps->velocity );
					return qtrue;
				}
			}

			// if we have fixed all interactions, try another move
			VectorCopy( clipVelocity, pm->ps->velocity );
			VectorCopy( endClipVelocity, endVelocity );
			break;
		}
	}

	if ( gravity ) {
		VectorCopy( endVelocity, pm->ps->velocity );
	}

	// don't change velocity if in a timer (FIXME: is this correct?)
	if ( pm->ps->pm_time ) {
		pm->ps->velocity[0] = primal_vel_x;
		pm->ps->velocity[1] = primal_vel_y;
		pm->ps->velocity[2] = primal_vel_z;
	}

	return ( bumpcount != 0 );
}

/*
==================
PM_StepSlideMove

QL version: significantly expanded from Q3 with air-step support,
step-jump logic, edge grab, and velocity damping on step-ups.
==================
*/
void PM_StepSlideMove( qboolean gravity ) {
	vec3_t		start_o;
	float		start_vx, start_vy, start_vz;
	vec3_t		up, down;
	vec3_t		projected;
	vec3_t		stepStart, stepEnd;
	vec3_t		edgeMins, edgeMaxs, edgeEnd;
	trace_t		trace;
	float		stepSize;
	float		delta;
	float		dotNV;

	VectorCopy( pm->ps->origin, start_o );
	start_vx = pm->ps->velocity[0];
	start_vy = pm->ps->velocity[1];
	start_vz = pm->ps->velocity[2];

	if ( PM_SlideMove( gravity ) == 0 ) {
		return;		// we got exactly where we wanted to go first try
	}

	// project where we would have gone without obstruction
	projected[0] = start_vx * pml.frametime + start_o[0];
	projected[1] = start_vy * pml.frametime + start_o[1];
	projected[2] = pml.frametime * start_vz + start_o[2];

	if ( pm_airSteps == 0 ) {
		// trace forward to projected position, then down to check for ground
		pm->trace( &trace, start_o, pm->mins, pm->maxs, projected,
			pm->ps->clientNum, pm->tracemask );
		VectorCopy( trace.endpos, projected );
		down[0] = trace.endpos[0];
		down[1] = trace.endpos[1];
		down[2] = trace.endpos[2] - (float)pm_stepHeight;
		pm->trace( &trace, projected, pm->mins, pm->maxs, down,
			pm->ps->clientNum, pm->tracemask );
		// never step up when you still have up velocity
		if ( start_vz > 0.0f ) {
			if ( trace.fraction == 1.0f ) {
				return;
			}
			if ( trace.plane.normal[2] < MIN_WALK_NORMAL ) {
				return;
			}
		}
	}

	// try stepping up
	up[0] = start_o[0];
	up[1] = start_o[1];
	up[2] = start_o[2] + (float)pm_stepHeight;

	// test the player position if they were a stepheight higher
	pm->trace( &trace, start_o, pm->mins, pm->maxs, up,
		pm->ps->clientNum, pm->tracemask );
	if ( trace.allsolid ) {
		if ( pm->debugLevel ) {
			Com_Printf( "%i:bend can't step\n", c_pmove );
		}
		return;		// can't step up
	}

	stepSize = trace.endpos[2] - start_o[2];
	// try slidemove from this position
	VectorCopy( trace.endpos, pm->ps->origin );
	pm->ps->velocity[0] = start_vx;
	pm->ps->velocity[1] = start_vy;
	pm->ps->velocity[2] = start_vz;

	PM_SlideMove( gravity );

	// push down the final amount
	VectorCopy( pm->ps->origin, down );
	down[2] -= stepSize;
	pm->trace( &trace, pm->ps->origin, pm->mins, pm->maxs, down,
		pm->ps->clientNum, pm->tracemask );
	if ( !trace.allsolid ) {
		VectorCopy( trace.endpos, pm->ps->origin );
	}
	if ( trace.fraction < 1.0f ) {
		dotNV = DotProduct( trace.plane.normal, pm->ps->velocity );
		if ( dotNV >= 0.0f && fabs( dotNV ) >= 0.001f ) {
			// velocity is moving away from the surface, skip clipping
			goto skipClip;
		}
		PM_ClipVelocity( pm->ps->velocity, trace.plane.normal,
			pm->ps->velocity, OVERCLIP );
	}

skipClip:
	// trace from start to final position to check if we actually stepped
	pm->trace( &trace, start_o, pm->mins, pm->maxs, pm->ps->origin,
		pm->ps->clientNum, pm->tracemask );
	if ( trace.fraction < 1.0f ) {
		// didn't make it all the way -- use the step move result
		delta = pm->ps->origin[2] - start_o[2];

		if ( delta > 2.0f ) {
			pm->field_0dc = delta;
			pm->field_0e0 = pm->cmd.serverTime;
		}

		// apply velocity damping when stepping up while airborne
		if ( !pml.groundPlane && delta > 0.0f && start_vz > 0.0f ) {
			float dampFactor = 1.0f - _DAT_003cfe90;
			pm->ps->velocity[0] *= dampFactor;
			pm->ps->velocity[1] *= dampFactor;
		}

		// step jump / edge grab logic
		if ( pmove_stepJumpFlag && pm->ps->pm_type == PM_NORMAL
			&& delta > 0.0f && (int)pm->waterlevel < 2 ) {
			if ( PM_CanJump() || ( DAT_003cfe30 && PM_CanEdgeGrab() ) ) {
				// check if there is walkable ground at the projected position
				stepStart[0] = projected[0];
				stepStart[1] = projected[1];
				stepStart[2] = projected[2] + (float)pm_stepHeight;
				stepEnd[0] = projected[0];
				stepEnd[1] = projected[1];
				stepEnd[2] = projected[2] - (float)pm_stepHeight;
				pm->trace( &trace, stepStart, pm->mins, pm->maxs, stepEnd,
					pm->ps->clientNum, pm->tracemask );
				if ( !trace.startsolid && !trace.allsolid
					&& trace.plane.normal[2] >= MIN_WALK_NORMAL ) {
					if ( PM_CanJump() ) {
						// normal step jump
						pml.isStepJump = 1;
						PM_Jump();
						pml.isStepJump = 0;
					} else if ( DAT_003cfe30 && PM_CanEdgeGrab() ) {
						// edge grab: check for open air below with shrunk bbox
						edgeMins[0] = pm->mins[0] + 1.0f;
						edgeMins[1] = pm->mins[1] + 1.0f;
						edgeMins[2] = pm->mins[2];
						edgeMaxs[0] = pm->maxs[0] - 1.0f;
						edgeMaxs[1] = pm->maxs[1] - 1.0f;
						edgeMaxs[2] = pm->maxs[2];
						edgeEnd[0] = pm->ps->origin[0];
						edgeEnd[1] = pm->ps->origin[1];
						edgeEnd[2] = pm->ps->origin[2] - 64.0f;
						pm->trace( &trace, pm->ps->origin, edgeMins, edgeMaxs, edgeEnd,
							pm->ps->clientNum, pm->tracemask );
						if ( trace.fraction == 1.0f ) {
							// over an edge -- do a crouch step jump
							pml.isJumppad = 1;
							PM_Jump();
							pml.isJumppad = 0;
						}
					}
				}
			}
		}

		if ( pm->debugLevel ) {
			Com_Printf( "%i:stepped %f\n", c_pmove, delta );
		}
	}
}