/***
*
*	Copyright (c) 1996-2002, Valve LLC. All rights reserved.
*	
*	This product contains software technology licensed from Id 
*	Software, Inc. ("Id Technology").  Id Technology (c) 1996 Id Software, Inc. 
*	All Rights Reserved.
*
*   Use, distribution, and modification of this source code and/or resulting
*   object code is restricted to non-commercial enhancements to products from
*   Valve LLC.  All other use, distribution, or modification is prohibited
*   without written permission from Valve LLC.
*
****/
// pm_math.c -- math primitives
#include <math.h>
#include "mathlib.h"
#ifdef HAVE_TGMATH_H
#include <tgmath.h>
#endif
#include "const.h"

// up / down
#define	PITCH	0
// left / right
#define	YAW	1
// fall over
#define	ROLL	2 

#ifdef _MSC_VER
#pragma warning(disable : 4244)
#endif

vec3_t vec3_origin = { 0, 0, 0 };
int nanmask = 255 << 23;

float anglemod( float a )
{
	a = ( 360.0f / 65536.0f ) * ( (int)( a * ( 65536.0f / 360.0f ) ) & 65535 );
	return a;
}

void AngleVectors( const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up )
{
	float angle;
	float sr, sp, sy, cr, cp, cy;

	angle = angles[YAW] * ( M_PI_F * 2.0f / 360.0f );
	sy = sin( angle );
	cy = cos( angle );
	angle = angles[PITCH] * ( M_PI_F * 2.0f / 360.0f );
	sp = sin( angle );
	cp = cos( angle );
	angle = angles[ROLL] * ( M_PI_F * 2.0f / 360.0f );
	sr = sin( angle );
	cr = cos( angle );

	if( forward )
	{
		forward[0] = cp * cy;
		forward[1] = cp * sy;
		forward[2] = -sp;
	}
	if( right )
	{
		right[0] = ( -1 * sr * sp * cy + -1 * cr * -sy );
		right[1] = ( -1 * sr * sp * sy + -1 * cr * cy );
		right[2] = -1 * sr * cp;
	}
	if( up )
	{
		up[0] = ( cr * sp * cy + -sr * -sy );
		up[1] = ( cr * sp * sy + -sr * cy );
		up[2] = cr * cp;
	}
}

void AngleVectorsTranspose( const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up )
{
	float angle;
	float sr, sp, sy, cr, cp, cy;
	
	angle = angles[YAW] * ( M_PI_F * 2.0f / 360.0f );
	sy = sin( angle );
	cy = cos( angle );
	angle = angles[PITCH] * ( M_PI_F * 2.0f / 360.0f );
	sp = sin( angle );
	cp = cos( angle );
	angle = angles[ROLL] * ( M_PI_F * 2.0f / 360.0f );
	sr = sin( angle );
	cr = cos( angle );

	if( forward )
	{
		forward[0] = cp * cy;
		forward[1] = ( sr * sp * cy + cr * -sy );
		forward[2] = ( cr * sp * cy + -sr * -sy );
	}
	if( right )
	{
		right[0] = cp * sy;
		right[1] = ( sr * sp * sy + cr * cy );
		right[2] = ( cr * sp * sy + -sr * cy );
	}
	if( up )
	{
		up[0] = -sp;
		up[1] = sr * cp;
		up[2] = cr * cp;
	}
}

void AngleMatrix( const vec3_t angles, float (*matrix)[4] )
{
	float angle;
	float sr, sp, sy, cr, cp, cy;

	angle = angles[YAW] * ( M_PI_F * 2.0f / 360.0f );
	sy = sin( angle );
	cy = cos( angle );
	angle = angles[PITCH] * ( M_PI_F * 2.0f / 360.0f );
	sp = sin( angle );
	cp = cos( angle );
	angle = angles[ROLL] * ( M_PI_F * 2.0f / 360.0f );
	sr = sin( angle );
	cr = cos( angle );

	// matrix = ( YAW * PITCH ) * ROLL
	matrix[0][0] = cp * cy;
	matrix[1][0] = cp * sy;
	matrix[2][0] = -sp;
	matrix[0][1] = sr * sp * cy + cr * -sy;
	matrix[1][1] = sr * sp * sy + cr * cy;
	matrix[2][1] = sr * cp;
	matrix[0][2] = ( cr * sp * cy + -sr * -sy );
	matrix[1][2] = ( cr * sp * sy + -sr * cy );
	matrix[2][2] = cr * cp;
	matrix[0][3] = 0.0f;
	matrix[1][3] = 0.0f;
	matrix[2][3] = 0.0f;
}

void AngleIMatrix( const vec3_t angles, float matrix[3][4] )
{
	float angle;
	float sr, sp, sy, cr, cp, cy;

	angle = angles[YAW] * ( M_PI_F * 2.0f / 360.0f );
	sy = sin( angle );
	cy = cos( angle );
	angle = angles[PITCH] * ( M_PI_F * 2.0f / 360.0f );
	sp = sin( angle );
	cp = cos( angle );
	angle = angles[ROLL] * ( M_PI_F * 2.0f / 360.0f );
	sr = sin( angle );
	cr = cos( angle );

	// matrix = ( YAW * PITCH ) * ROLL
	matrix[0][0] = cp * cy;
	matrix[0][1] = cp * sy;
	matrix[0][2] = -sp;
	matrix[1][0] = sr * sp * cy + cr * -sy;
	matrix[1][1] = sr * sp * sy + cr * cy;
	matrix[1][2] = sr * cp;
	matrix[2][0] = ( cr * sp * cy + -sr * -sy );
	matrix[2][1] = ( cr * sp * sy + -sr * cy );
	matrix[2][2] = cr * cp;
	matrix[0][3] = 0.0;
	matrix[1][3] = 0.0;
	matrix[2][3] = 0.0;
}

void NormalizeAngles( float *angles )
{
	int i;
	// Normalize angles
	for( i = 0; i < 3; i++ )
	{
		if( angles[i] > 180.0f )
		{
			angles[i] -= 360.0f;
		}
		else if( angles[i] < -180.0f )
		{
			angles[i] += 360.0f;
		}
	}
}

/*
===================
InterpolateAngles

Interpolate Euler angles.
FIXME:  Use Quaternions to avoid discontinuities
Frac is 0.0 to 1.0 ( i.e., should probably be clamped, but doesn't have to be )
===================
*/
void InterpolateAngles( float *start, float *end, float *output, float frac )
{
	int i;
	float ang1, ang2;
	float d;

	NormalizeAngles( start );
	NormalizeAngles( end );

	for( i = 0; i < 3; i++ )
	{
		ang1 = start[i];
		ang2 = end[i];

		d = ang2 - ang1;
		if( d > 180.0f )
		{
			d -= 360.0f;
		}
		else if( d < -180.0f )
		{	
			d += 360.0f;
		}

		output[i] = ang1 + d * frac;
	}

	NormalizeAngles( output );
}

/*
===================
AngleBetweenVectors

===================
*/
float AngleBetweenVectors( const vec3_t v1, const vec3_t v2 )
{
	float angle;
	float l1 = Length( v1 );
	float l2 = Length( v2 );

	if( !l1 || !l2 )
		return 0.0f;

	angle = acos( DotProduct( v1, v2 ) / ( l1 * l2 ) );
	angle = ( angle  * 180.0f ) / M_PI_F;

	return angle;
}

void VectorTransform( const vec3_t in1, float in2[3][4], vec3_t out )
{
	out[0] = DotProduct( in1, in2[0] ) + in2[0][3];
	out[1] = DotProduct( in1, in2[1] ) + in2[1][3];
	out[2] = DotProduct( in1, in2[2] ) + in2[2][3];
}

int VectorCompare( const vec3_t v1, const vec3_t v2 )
{
	int i;

	for( i = 0; i < 3; i++ )
		if( v1[i] != v2[i] )
			return 0;

	return 1;
}

void VectorMA( const vec3_t veca, float scale, const vec3_t vecb, vec3_t vecc )
{
	vecc[0] = veca[0] + scale * vecb[0];
	vecc[1] = veca[1] + scale * vecb[1];
	vecc[2] = veca[2] + scale * vecb[2];
}

vec_t _DotProduct( vec3_t v1, vec3_t v2 )
{
	return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}

void _VectorSubtract( vec3_t veca, vec3_t vecb, vec3_t out )
{
	out[0] = veca[0] - vecb[0];
	out[1] = veca[1] - vecb[1];
	out[2] = veca[2] - vecb[2];
}

void _VectorAdd( vec3_t veca, vec3_t vecb, vec3_t out )
{
	out[0] = veca[0] + vecb[0];
	out[1] = veca[1] + vecb[1];
	out[2] = veca[2] + vecb[2];
}

void _VectorCopy( vec3_t in, vec3_t out )
{
	out[0] = in[0];
	out[1] = in[1];
	out[2] = in[2];
}

void CrossProduct( const vec3_t v1, const vec3_t v2, vec3_t cross )
{
	cross[0] = v1[1] * v2[2] - v1[2] * v2[1];
	cross[1] = v1[2] * v2[0] - v1[0] * v2[2];
	cross[2] = v1[0] * v2[1] - v1[1] * v2[0];
}

float Length( const vec3_t v )
{
	int i;
	float length = 0.0f;

	for( i = 0; i < 3; i++ )
		length += v[i] * v[i];
	length = sqrt( length );		// FIXME

	return length;
}

float Distance( const vec3_t v1, const vec3_t v2 )
{
	vec3_t d;
	VectorSubtract( v2, v1, d );
	return Length( d );
}

float VectorNormalize( vec3_t v )
{
	float length, ilength;

	length = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
	length = sqrt( length );		// FIXME

	if( length )
	{
		ilength = 1.0f / length;
		v[0] *= ilength;
		v[1] *= ilength;
		v[2] *= ilength;
	}

	return length;
}

void VectorInverse( vec3_t v )
{
	v[0] = -v[0];
	v[1] = -v[1];
	v[2] = -v[2];
}

void VectorScale( const vec3_t in, vec_t scale, vec3_t out )
{
	out[0] = in[0] * scale;
	out[1] = in[1] * scale;
	out[2] = in[2] * scale;
}

void VectorMatrix( vec3_t forward, vec3_t right, vec3_t up )
{
	vec3_t tmp;

	if( forward[0] == 0.0f && forward[1] == 0.0f )
	{
		right[0] = 1.0f;
		right[1] = 0.0f;
		right[2] = 0.0f;
		up[0] = -forward[2]; 
		up[1] = 0.0f;
		up[2] = 0.0f;
		return;
	}

	tmp[0] = 0.0f; tmp[1] = 0.0f; tmp[2] = 1.0f;
	CrossProduct( forward, tmp, right );
	VectorNormalize( right );
	CrossProduct( right, forward, up );
	VectorNormalize( up );
}

void VectorAngles( const vec3_t forward, vec3_t angles )
{
	float tmp, yaw, pitch;

	if( forward[1] == 0.0f && forward[0] == 0.0f )
	{
		yaw = 0.0f;
		if( forward[2] > 0.0f )
			pitch = 90.0f;
		else
			pitch = 270.0f;
	}
	else
	{
		yaw = ( atan2( forward[1], forward[0] ) * 180.0f / M_PI_F );
		if( yaw < 0.0f )
			yaw += 360.0f;

		tmp = sqrt( forward[0] * forward[0] + forward[1] * forward[1] );
		pitch = ( atan2( forward[2], tmp ) * 180.0f / M_PI_F );
		if( pitch < 0.0f )
			pitch += 360.0f;
	}

	angles[0] = pitch;
	angles[1] = yaw;
	angles[2] = 0.0f;
}