/*
This code is based on source provided under the terms of the Id Software 
LIMITED USE SOFTWARE LICENSE AGREEMENT, a copy of which is included with the
GtkRadiant sources (see LICENSE_ID). If you did not receive a copy of 
LICENSE_ID, please contact Id Software immediately at info@idsoftware.com.

All changes and additions to the original source which have been developed by
other contributors (see CONTRIBUTORS) are provided under the terms of the
license the contributors choose (see LICENSE), to the extent permitted by the
LICENSE_ID. If you did not receive a copy of the contributor license,
please contact the GtkRadiant maintainers at info@gtkradiant.com immediately.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef __MATH_VECTOR_H__
#define __MATH_VECTOR_H__

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

#include <math.h>
#include <assert.h>

//#define DotProduct(a,b)			((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
//#define VectorSubtract(a,b,c)	((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2])
//#define VectorAdd(a,b,c)		((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2])
//#define VectorCopy(a,b)			((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2])
//#define VectorCopy(a,b)			((b).x=(a).x,(b).y=(a).y,(b).z=(a).z])

//#define	VectorScale(v, s, o)	((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s))
#define	__VectorMA(v, s, b, o)	((o)[0]=(v)[0]+(b)[0]*(s),(o)[1]=(v)[1]+(b)[1]*(s),(o)[2]=(v)[2]+(b)[2]*(s))
//#define CrossProduct(a,b,c)		((c)[0]=(a)[1]*(b)[2]-(a)[2]*(b)[1],(c)[1]=(a)[2]*(b)[0]-(a)[0]*(b)[2],(c)[2]=(a)[0]*(b)[1]-(a)[1]*(b)[0])

#define DotProduct4(x,y)		((x)[0]*(y)[0]+(x)[1]*(y)[1]+(x)[2]*(y)[2]+(x)[3]*(y)[3])
#define VectorSubtract4(a,b,c)	((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2],(c)[3]=(a)[3]-(b)[3])
#define VectorAdd4(a,b,c)		((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2],(c)[3]=(a)[3]+(b)[3])
#define VectorCopy4(a,b)		((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
#define	VectorScale4(v, s, o)	((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s),(o)[3]=(v)[3]*(s))
#define	VectorMA4(v, s, b, o)	((o)[0]=(v)[0]+(b)[0]*(s),(o)[1]=(v)[1]+(b)[1]*(s),(o)[2]=(v)[2]+(b)[2]*(s),(o)[3]=(v)[3]+(b)[3]*(s))


//#define VectorClear(a)			((a)[0]=(a)[1]=(a)[2]=0)
#define VectorNegate(a,b)		((b)[0]=-(a)[0],(b)[1]=-(a)[1],(b)[2]=-(a)[2])
//#define VectorSet(v, x, y, z)	((v)[0]=(x), (v)[1]=(y), (v)[2]=(z))
#define Vector4Copy(a,b)		((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])

#define	SnapVector(v) {v[0]=(int)v[0];v[1]=(int)v[1];v[2]=(int)v[2];}


//#include "util_heap.h"

#ifndef EQUAL_EPSILON
#define EQUAL_EPSILON	0.001
#endif

float Q_fabs( float f );

#ifndef ID_INLINE
#ifdef _WIN32
#define ID_INLINE __inline 
#else
#define ID_INLINE inline
#endif
#endif

// if this is defined, vec3 will take four elements, which may allow
// easier SIMD optimizations
//#define	FAT_VEC3
//#ifdef __ppc__
//#pragma align(16)
//#endif

class angles_t;
#ifdef __ppc__
// Vanilla PPC code, but since PPC has a reciprocal square root estimate instruction, 
// runs *much* faster than calling sqrt(). We'll use two Newton-Raphson 
// refinement steps to get bunch more precision in the 1/sqrt() value for very little cost. 
// We'll then multiply 1/sqrt times the original value to get the sqrt. 
// This is about 12.4 times faster than sqrt() and according to my testing (not exhaustive) 
// it returns fairly accurate results (error below 1.0e-5 up to 100000.0 in 0.1 increments). 

static inline float idSqrt(float x) {
    const float half = 0.5;
    const float one = 1.0;
    float B, y0, y1;

    // This'll NaN if it hits frsqrte. Handle both +0.0 and -0.0
    if (fabs(x) == 0.0)
        return x;
    B = x;
    
#ifdef __GNUC__
    asm("frsqrte %0,%1" : "=f" (y0) : "f" (B));
#else
    y0 = __frsqrte(B);
#endif
    /* First refinement step */
    
    y1 = y0 + half*y0*(one - B*y0*y0);
    
    /* Second refinement step -- copy the output of the last step to the input of this step */
    
    y0 = y1;
    y1 = y0 + half*y0*(one - B*y0*y0);
    
    /* Get sqrt(x) from x * 1/sqrt(x) */
    return x * y1;
}
#else
static inline double idSqrt(double x) {
    return sqrt(x);
}
#endif


//class idVec3  : public idHeap<idVec3> {
class idVec3 {
public:	
#ifndef	FAT_VEC3
	    float x,y,z;
#else
	    float x,y,z,dist;
#endif

#ifndef	FAT_VEC3
					idVec3() {};
#else
					idVec3() {dist = 0.0f;};
#endif
					idVec3( const float x, const float y, const float z );

					operator float *();

	float			operator[]( const int index ) const;
	float			&operator[]( const int index );

	void 			set( const float x, const float y, const float z );

	idVec3			operator-() const;

	idVec3			&operator=( const idVec3 &a );

	float			operator*( const idVec3 &a ) const;
	idVec3			operator*( const float a ) const;
	friend idVec3	operator*( float a, idVec3 b );

	idVec3			operator+( const idVec3 &a ) const;
	idVec3			operator-( const idVec3 &a ) const;
	
	idVec3			&operator+=( const idVec3 &a );
	idVec3			&operator-=( const idVec3 &a );
	idVec3			&operator*=( const float a );

	int				operator==(	const idVec3 &a ) const;
	int				operator!=(	const idVec3 &a ) const;

	idVec3			Cross( const idVec3 &a ) const;
	idVec3			&Cross( const idVec3 &a, const idVec3 &b );

	float			Length( void ) const;
	float			Normalize( void );

	void			Zero( void );
	void			Snap( void );
	void			SnapTowards( const idVec3 &to );

	float			toYaw( void );
	float			toPitch( void );
	angles_t		toAngles( void );
	friend idVec3	LerpVector( const idVec3 &w1, const idVec3 &w2, const float t );

	char			*string( void );
};

extern idVec3 vec_zero;

ID_INLINE idVec3::idVec3( const float x, const float y, const float z ) {
	this->x = x;
	this->y = y;
	this->z = z;
#ifdef	FAT_VEC3
	this->dist = 0.0f;
#endif
}

ID_INLINE float idVec3::operator[]( const int index ) const {
	return ( &x )[ index ];
}

ID_INLINE float &idVec3::operator[]( const int index ) {
	return ( &x )[ index ];
}

ID_INLINE idVec3::operator float *( void ) {
	return &x;
}

ID_INLINE idVec3 idVec3::operator-() const {
	return idVec3( -x, -y, -z );
}
	
ID_INLINE idVec3 &idVec3::operator=( const idVec3 &a ) { 
	x = a.x;
	y = a.y;
	z = a.z;
	
	return *this;
}

ID_INLINE void idVec3::set( const float x, const float y, const float z ) {
	this->x = x;
	this->y = y;
	this->z = z;
}

ID_INLINE idVec3 idVec3::operator-( const idVec3 &a ) const {
	return idVec3( x - a.x, y - a.y, z - a.z );
}

ID_INLINE float idVec3::operator*( const idVec3 &a ) const {
	return x * a.x + y * a.y + z * a.z;
}

ID_INLINE idVec3 idVec3::operator*( const float a ) const {
	return idVec3( x * a, y * a, z * a );
}

ID_INLINE idVec3 operator*( const float a, const idVec3 b ) {
	return idVec3( b.x * a, b.y * a, b.z * a );
}

ID_INLINE idVec3 idVec3::operator+( const idVec3 &a ) const {
	return idVec3( x + a.x, y + a.y, z + a.z );
}

ID_INLINE idVec3 &idVec3::operator+=( const idVec3 &a ) {
	x += a.x;
	y += a.y;
	z += a.z;

	return *this;
}

ID_INLINE idVec3 &idVec3::operator-=( const idVec3 &a ) {
	x -= a.x;
	y -= a.y;
	z -= a.z;

	return *this;
}

ID_INLINE idVec3 &idVec3::operator*=( const float a ) {
	x *= a;
	y *= a;
	z *= a;

	return *this;
}

ID_INLINE int idVec3::operator==( const idVec3 &a ) const {
	if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
		return false;
	}
			
	if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
		return false;
	}

	if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
		return false;
	}

	return true;
}

ID_INLINE int idVec3::operator!=( const idVec3 &a ) const {
	if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
		return true;
	}
			
	if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
		return true;
	}

	if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
		return true;
	}

	return false;
}

ID_INLINE idVec3 idVec3::Cross( const idVec3 &a ) const {
	return idVec3( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x );
}

ID_INLINE idVec3 &idVec3::Cross( const idVec3 &a, const idVec3 &b ) {
	x = a.y * b.z - a.z * b.y;
	y = a.z * b.x - a.x * b.z;
	z = a.x * b.y - a.y * b.x;

	return *this;
}

ID_INLINE float idVec3::Length( void ) const {
	float length;
	
	length = x * x + y * y + z * z;
	return ( float )idSqrt( length );
}

ID_INLINE float idVec3::Normalize( void ) {
	float length;
	float ilength;

	length = this->Length();
	if ( length ) {
		ilength = 1.0f / length;
		x *= ilength;
		y *= ilength;
		z *= ilength;
	}
		
	return length;
}

ID_INLINE void idVec3::Zero( void ) {
	x = 0.0f;
	y = 0.0f;
	z = 0.0f;
}

ID_INLINE void idVec3::Snap( void ) {
	x = float( int( x ) );
	y = float( int( y ) );
	z = float( int( z ) );
}

/*
======================
SnapTowards

Round a vector to integers for more efficient network
transmission, but make sure that it rounds towards a given point
rather than blindly truncating.  This prevents it from truncating 
into a wall.
======================
*/
ID_INLINE void idVec3::SnapTowards( const idVec3 &to ) {
	if ( to.x <= x ) {
		x = float( int( x ) );
	} else {
		x = float( int( x ) + 1 );
	}

	if ( to.y <= y ) {
		y = float( int( y ) );
	} else {
		y = float( int( y ) + 1 );
	}

	if ( to.z <= z ) {
		z = float( int( z ) );
	} else {
		z = float( int( z ) + 1 );
	}
}

//===============================================================

class Bounds {
public:
	idVec3	b[2];

			Bounds();
			Bounds( const idVec3 &mins, const idVec3 &maxs );

	void	Clear();
	void	Zero();
	float	Radius();		// radius from origin, not from center
	idVec3	Center();
	void	AddPoint( const idVec3 &v );
	void	AddBounds( const Bounds &bb );
	bool	IsCleared();
	bool	ContainsPoint( const idVec3 &p );
	bool	IntersectsBounds( const Bounds &b2 );	// touching is NOT intersecting
};

extern Bounds	boundsZero;

ID_INLINE Bounds::Bounds(){
}

ID_INLINE bool Bounds::IsCleared() {
	return b[0][0] > b[1][0];
}

ID_INLINE bool Bounds::ContainsPoint( const idVec3 &p ) {
	if ( p[0] < b[0][0] || p[1] < b[0][1] || p[2] < b[0][2]
		|| p[0] > b[1][0] || p[1] > b[1][1] || p[2] > b[1][2] ) {
		return false;
	}
	return true;
}

ID_INLINE bool Bounds::IntersectsBounds( const Bounds &b2 ) {
	if ( b2.b[1][0] < b[0][0] || b2.b[1][1] < b[0][1] || b2.b[1][2] < b[0][2]
		|| b2.b[0][0] > b[1][0] || b2.b[0][1] > b[1][1] || b2.b[0][2] > b[1][2] ) {
		return false;
	}
	return true;
}

ID_INLINE Bounds::Bounds( const idVec3 &mins, const idVec3 &maxs ) {
	b[0] = mins;
	b[1] = maxs;
}

ID_INLINE idVec3 Bounds::Center() {
	return idVec3( ( b[1][0] + b[0][0] ) * 0.5f, ( b[1][1] + b[0][1] ) * 0.5f, ( b[1][2] + b[0][2] ) * 0.5f );
}

ID_INLINE void Bounds::Clear() {
	b[0][0] = b[0][1] = b[0][2] = 99999;
	b[1][0] = b[1][1] = b[1][2] = -99999;
}

ID_INLINE void Bounds::Zero() {
	b[0][0] = b[0][1] = b[0][2] =
	b[1][0] = b[1][1] = b[1][2] = 0;
}

ID_INLINE void Bounds::AddPoint( const idVec3 &v ) {
	if ( v[0] < b[0][0]) {
		b[0][0] = v[0];
	}
	if ( v[0] > b[1][0]) {
		b[1][0] = v[0];
	}
	if ( v[1] < b[0][1] ) {
		b[0][1] = v[1];
	}
	if ( v[1] > b[1][1]) {
		b[1][1] = v[1];
	}
	if ( v[2] < b[0][2] ) {
		b[0][2] = v[2];
	}
	if ( v[2] > b[1][2]) {
		b[1][2] = v[2];
	}
}


ID_INLINE void Bounds::AddBounds( const Bounds &bb ) {
	if ( bb.b[0][0] < b[0][0]) {
		b[0][0] = bb.b[0][0];
	}
	if ( bb.b[0][1] < b[0][1]) {
		b[0][1] = bb.b[0][1];
	}
	if ( bb.b[0][2] < b[0][2]) {
		b[0][2] = bb.b[0][2];
	}

	if ( bb.b[1][0] > b[1][0]) {
		b[1][0] = bb.b[1][0];
	}
	if ( bb.b[1][1] > b[1][1]) {
		b[1][1] = bb.b[1][1];
	}
	if ( bb.b[1][2] > b[1][2]) {
		b[1][2] = bb.b[1][2];
	}
}

ID_INLINE float Bounds::Radius( ) {
	int		i;
	float	total;
	float	a, aa;

	total = 0;
	for (i=0 ; i<3 ; i++) {
		a = (float)fabs( b[0][i] );
		aa = (float)fabs( b[1][i] );
		if ( aa > a ) {
			a = aa;
		}
		total += a * a;
	}

	return (float)idSqrt( total );
}

//===============================================================


class idVec2 {
public:
	float			x;
	float			y;

					operator float *();
	float			operator[]( int index ) const;
	float			&operator[]( int index );
};

ID_INLINE float idVec2::operator[]( int index ) const {
	return ( &x )[ index ];
}

ID_INLINE float& idVec2::operator[]( int index ) {
	return ( &x )[ index ];
}

ID_INLINE idVec2::operator float *( void ) {
	return &x;
}

class idVec4 : public idVec3 {
public:
#ifndef	FAT_VEC3
	float			dist;
#endif
	idVec4();
	~idVec4() {};
	
	idVec4( float x, float y, float z, float dist );
	float			operator[]( int index ) const;
	float			&operator[]( int index );
};

ID_INLINE idVec4::idVec4() {}
ID_INLINE idVec4::idVec4( float x, float y, float z, float dist ) {
	this->x = x;
	this->y = y;
	this->z = z;
	this->dist = dist;
}

ID_INLINE float idVec4::operator[]( int index ) const {
	return ( &x )[ index ];
}

ID_INLINE float& idVec4::operator[]( int index ) {
	return ( &x )[ index ];
}


class idVec5_t : public idVec3 {
public:
	float			s;
	float			t;
	float			operator[]( int index ) const;
	float			&operator[]( int index );
};


ID_INLINE float idVec5_t::operator[]( int index ) const {
	return ( &x )[ index ];
}

ID_INLINE float& idVec5_t::operator[]( int index ) {
	return ( &x )[ index ];
}

#endif /* !__MATH_VECTOR_H__ */
