#ifndef __MATHLIB_H__
#define __MATHLIB_H__
#if _MSC_VER > 1000
#	pragma once
#endif	// #if _MSC_VER > 1000

#include <math.h>


#define m_PI			(3.14159265358979323846f)
#define m_DEG2RAD		(m_PI / 180.0f)
#define m_RAD2DEG		(180.0f / m_PI)
#define m_EPS			(1e-6f)


// evil-magic Reciprocal Square Root calculation lifted from Q3 source code
inline float Rsqrt( float x )
{
	int i;
	float y, r;

	y = x * 0.5f;
	i = *reinterpret_cast<int *>( &x );
	i = 0x5f3759df - ( i >> 1 );
	r = *reinterpret_cast<float *>( &i );
	r = r * ( 1.5f - r * r * y );

	return r;
}

template<typename T>
inline T lerp( const T& a, const T& b, const float t )
{
	if ( t <= 0.0f )
		return a;
	else if ( t >= 1.0f )
		return b;
	else
		return a + ( b - a ) * t;
}



template <typename T>
class _vec2
{
public:

	union {
		struct {
			T x, y;
		};
		T v[2];
	};

	_vec2() : x(T(0)), y(T(0)) {}
	_vec2( const _vec2& v ) : x(v.x), y(v.y) {}
	_vec2( T _x, T _y, T _z ) : x(_x), y(_y) {}

	inline const _vec2 operator * ( T f ) const { return _vec2(x * f, y * f); }
	inline const _vec2 operator / ( T f ) const { return _vec2(x / f, y / f); }
	inline const _vec2 operator + ( const _vec2& v ) const { return _vec3(x + v.x, y + v.y); }
	inline const _vec2 operator - () const { return _vec2(-x, -y); }
	inline const _vec2 operator - ( const _vec2& v ) const { return _vec2(x - v.x, y - v.y); }

	inline _vec2& operator *= ( T f ) { return *this = *this * f; }
	inline _vec2& operator /= ( T f ) { return *this = *this / f; }
	inline _vec2& operator += ( const _vec2& v ) { return *this = *this + v; }
	inline _vec2& operator -= ( const _vec2& v ) { return *this = *this - v; }

	inline operator T*() { return v; }
	inline operator const T*() const { return v; }

	inline T& operator[](int i) { return v[i]; }
	inline const T operator[](int i) const { return v[i]; }

	inline T length( void ) const { return (T)sqrt(x * x + y * y); }

	inline void normalize( void )
	{
		T inv = (T)Rsqrt(x * x + y * y);
		x *= inv;
		y *= inv;
	}
};

typedef _vec2<float>	vec2, vec2f;
typedef _vec2<double>	vec2d;


template <typename T>
class _vec3
{
public:

	union {
		struct {
			T x, y, z;
		};
		T v[3];
	};

	_vec3() : x(T(0)), y(T(0)), z(T(0)) {}
	_vec3( const _vec3& v ) : x(v.x), y(v.y), z(v.z) {}
	_vec3( T _x, T _y, T _z ) : x(_x), y(_y), z(_z) {}

	inline const _vec3 operator * ( T f ) const { return _vec3(x * f,y * f,z * f); }
	inline const _vec3 operator / ( T f ) const { return _vec3(x / f,y / f,z / f); }
	inline const _vec3 operator + ( const _vec3& v ) const { return _vec3(x + v.x,y + v.y,z + v.z); }
	inline const _vec3 operator - () const { return _vec3(-x,-y,-z); }
	inline const _vec3 operator - ( const _vec3& v ) const { return _vec3(x - v.x,y - v.y,z - v.z); }

	inline _vec3& operator *= ( T f ) { return *this = *this * f; }
	inline _vec3& operator /= ( T f ) { return *this = *this / f; }
	inline _vec3& operator += ( const _vec3& v ) { return *this = *this + v; }
	inline _vec3& operator -= ( const _vec3& v ) { return *this = *this - v; }

	inline operator T*() { return v; }
	inline operator const T*() const { return v; }

	inline T& operator[](int i) { return v[i]; }
	inline const T operator[](int i) const { return v[i]; }

	inline T length( void ) const { return (T)sqrt(x * x + y * y + z * z); }

	inline void normalize( void )
	{
		T inv = (T)Rsqrt(x * x + y * y + z * z);
		x *= inv;
		y *= inv;
		z *= inv;
	}
};

template <typename T>
inline T dot( const _vec3<T>& v1,const _vec3<T>& v2 )
{
	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
template <typename T>
inline _vec3<T> cross( const _vec3<T>& v1,const _vec3<T>& v2 )
{
	return _vec3<T>( v1.y * v2.z - v1.z * v2.y,
					 v1.z * v2.x - v1.x * v2.z,
					 v1.x * v2.y - v1.y * v2.x );
}

typedef _vec3<float>	vec3, vec3f;
typedef _vec3<double>	vec3d;


template <typename T>
class _vec4
{
public:

	union {
		struct {
			T x, y, z, w;
		};
		T v[3];
	};

	_vec4() : x(T(0)), y(T(0)), z(T(0)) {}
	_vec4( const _vec4& v ) : x(v.x), y(v.y), z(v.z) {}
	_vec4( T _x, T _y, T _z ) : x(_x), y(_y), z(_z) {}

	inline operator T*() { return v; }
	inline operator const T*() const { return v; }

	inline T& operator[](int i) { return v[i]; }
	inline const T operator[](int i) const { return v[i]; }
};

typedef _vec4<float>	vec4, vec4f;
typedef _vec4<double>	vec4d;


template <typename T>
class _quat
{
public:

	union {
		struct {
			T x, y, z, w;
		};
		T v[4];
	};

	_quat() : x(T(0)), y(T(0)), z(T(0)), w(T(1)) {}
	_quat( const _quat& q ) : x(q.x), y(q.y), z(q.z), w(q.w) {}
	_quat( T _x, T _y, T _z, T _w ) : x(_x), y(_y), z(_z), w(_w) {}

	operator T*() { return v; }
	operator const T*() const { return v; }

	T &operator[]( int i ) { return v[i]; }
	const T operator[]( int i ) const { return v[i]; }

	_quat& operator = ( const _quat& other )
	{
		x = other.x;
		y = other.y;
		z = other.z;
		w = other.w;
		return (*this);
	}

	_quat operator * ( float a ) const { return _quat( x * a, y * a, z * a, w * a ); }
	_quat operator * ( const _quat& q ) const
	{
		return _quat( w * q.x + x * q.w + y * q.z - z * q.y,
					  w * q.y + y * q.w + z * q.x - x * q.z,
					  w * q.z + z * q.w + x * q.y - y * q.x,
					  w * q.w - x * q.x - y * q.y - z * q.z );
	}
	_vec3<T> operator * ( const _vec3<T>& p ) const
	{
		// original code from DooM 3 SDK
		float xxzz = x*x - z*z;
		float wwyy = w*w - y*y;

		float xw2 = x*w*2.0f;
		float xy2 = x*y*2.0f;
		float xz2 = x*z*2.0f;
		float yw2 = y*w*2.0f;
		float yz2 = y*z*2.0f;
		float zw2 = z*w*2.0f;

		return _vec3<T>(
			(xxzz + wwyy)*p.x		+ (xy2 + zw2)*p.y		+ (xz2 - yw2)*p.z,
			(xy2 - zw2)*p.x			+ (y*y+w*w-x*x-z*z)*p.y	+ (yz2 + xw2)*p.z,
			(xz2 + yw2)*p.x			+ (yz2 - xw2)*p.y		+ (wwyy - xxzz)*p.z
			);
	}

	_quat operator - () const { return _quat( -x, -y, -z, -w ); }
	_quat operator + ( const _quat& q ) const { return _quat( x + q.x, y + q.y, z + q.z, w + q.w ); }

	_quat inverse( void ) const
	{
		T lenSq = x * x + y * y + z * z + w * w;
		if ( lenSq > T(0) )
		{
			T invLenSq = T(1) / lenSq;
			return _quat<T>( -x * invLenSq, -y * invLenSq, -z * invLenSq, w * invLenSq );
		}
		else
		{
			assert( false );
			return _quat<T>(0,0,0,1);
		}
	}
	_quat& invert( void ) { return (*this = this->inverse()); }

	_quat& normalize( void )
	{
		T len = (T)sqrt( x * x + y * y + z * z + w * w );
		if ( len )
		{
			T ilength = T(1) / len;
			x *= ilength;
			y *= ilength;
			z *= ilength;
			w *= ilength;
		}
		return (*this);
	}

};

typedef _quat<float>	quat, quatf;
typedef _quat<double>	quatd;

inline quat slerp( const quat& from, const quat& to, float t )
{
	quat temp;
	float omega, cosom, sinom, scale0, scale1;

	if ( t <= 0.0f )
		return from;
	else if ( t >= 1.0f )
		return to;

	cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
	if ( cosom < 0.0f )
	{
		temp  = -to;
		cosom = -cosom;
	}
	else
		temp = to;

	if ( ( 1.0f - cosom ) > m_EPS )
	{
		omega  = acosf( cosom );
		sinom  = 1.0f / sinf( omega );
		scale0 = sinf( ( 1.0f - t ) * omega ) * sinom;
		scale1 = sinf( t * omega ) * sinom;
	}
	else
	{
		scale0 = 1.0f - t;
		scale1 = t;
	}

	return ( ( from * scale0 ) + ( temp * scale1 ) );
}



template <typename T>
class _mat4
{
public:

	union {
		struct {
			T	m00, m10, m20, m30,
				m01, m11, m21, m31,
				m02, m12, m22, m32,
				m03, m13, m23, m33;
		};
		T mat[16];
	};

	_mat4()
	{
		mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
		mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
		mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
		mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
	}
	_mat4( const _mat4& m )
	{
		mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
		mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
		mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
		mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
	}

	inline _mat4 operator * ( const _mat4& m ) const
	{
		mat4 ret;
		ret[0] = mat[0] * m[0] + mat[4] * m[1] + mat[8] * m[2] + mat[12] * m[3];
		ret[1] = mat[1] * m[0] + mat[5] * m[1] + mat[9] * m[2] + mat[13] * m[3];
		ret[2] = mat[2] * m[0] + mat[6] * m[1] + mat[10] * m[2] + mat[14] * m[3];
		ret[3] = mat[3] * m[0] + mat[7] * m[1] + mat[11] * m[2] + mat[15] * m[3];
		ret[4] = mat[0] * m[4] + mat[4] * m[5] + mat[8] * m[6] + mat[12] * m[7];
		ret[5] = mat[1] * m[4] + mat[5] * m[5] + mat[9] * m[6] + mat[13] * m[7];
		ret[6] = mat[2] * m[4] + mat[6] * m[5] + mat[10] * m[6] + mat[14] * m[7];
		ret[7] = mat[3] * m[4] + mat[7] * m[5] + mat[11] * m[6] + mat[15] * m[7];
		ret[8] = mat[0] * m[8] + mat[4] * m[9] + mat[8] * m[10] + mat[12] * m[11];
		ret[9] = mat[1] * m[8] + mat[5] * m[9] + mat[9] * m[10] + mat[13] * m[11];
		ret[10] = mat[2] * m[8] + mat[6] * m[9] + mat[10] * m[10] + mat[14] * m[11];
		ret[11] = mat[3] * m[8] + mat[7] * m[9] + mat[11] * m[10] + mat[15] * m[11];
		ret[12] = mat[0] * m[12] + mat[4] * m[13] + mat[8] * m[14] + mat[12] * m[15];
		ret[13] = mat[1] * m[12] + mat[5] * m[13] + mat[9] * m[14] + mat[13] * m[15];
		ret[14] = mat[2] * m[12] + mat[6] * m[13] + mat[10] * m[14] + mat[14] * m[15];
		ret[15] = mat[3] * m[12] + mat[7] * m[13] + mat[11] * m[14] + mat[15] * m[15]; 
		return ret;
	}

	inline _mat4& operator *= (const _mat4& m) { return *this = *this * m; }

	inline operator float*() { return mat; }
	inline operator const float*() const { return mat; }

	_mat4& SetTranslateVector(const _vec3<T>& v)
	{
		mat[12] = v.x;
		mat[13] = v.y;
		mat[14] = v.z;
		return (*this);
	}

	_vec3<T> TransformPoint(const _vec3<T>& b) const
	{
		return _vec3<T>(mat[0] * b.x + mat[4] * b.y + mat[ 8] * b.z + mat[12],
						mat[1] * b.x + mat[5] * b.y + mat[ 9] * b.z + mat[13],
						mat[2] * b.x + mat[6] * b.y + mat[10] * b.z + mat[14]);
	}
	_vec3<T> TransformDirection(const _vec3<T>& b) const
	{
		return _vec3<T>(mat[0] * b.x + mat[4] * b.y + mat[ 8] * b.z,
						mat[1] * b.x + mat[5] * b.y + mat[ 9] * b.z,
						mat[2] * b.x + mat[6] * b.y + mat[10] * b.z);
	}

	_mat4 GetInverse(void)
	{
		_mat4 ret;
		T det;
		det = mat[0] * (mat[5] * (mat[10] * mat[15] - mat[14] * mat[11]) - mat[9] * (mat[6] * mat[15] - mat[14] * mat[7]) + mat[13] * (mat[6] * mat[11] - mat[10] * mat[7]));
		det -= mat[4] * (mat[1] * (mat[10] * mat[15] - mat[14] * mat[11]) - mat[9] * (mat[2] * mat[15] - mat[14] * mat[3]) + mat[13] * (mat[2] * mat[11] - mat[10] * mat[3]));
		det += mat[8] * (mat[1] * (mat[6] * mat[15] - mat[14] * mat[7]) - mat[5] * (mat[2] * mat[15] - mat[14] * mat[3]) + mat[13] * (mat[2] * mat[7] - mat[6] * mat[3]));
		det -= mat[12] * (mat[1] * (mat[6] * mat[11] - mat[10] * mat[7]) - mat[5] * (mat[2] * mat[11] - mat[10] * mat[3]) + mat[9] * (mat[2] * mat[7] - mat[6] * mat[3]));

		if ( det < m_EPS )
			return ret;

		T idet = T(1.0) / det;
		ret[0] = (mat[5] * (mat[10] * mat[15] - mat[14] * mat[11]) - mat[9] * (mat[6] * mat[15] - mat[14] * mat[7]) + mat[13] * (mat[6] * mat[11] - mat[10] * mat[7])) * idet;
		ret[1] = -(mat[1] * (mat[10] * mat[15] - mat[14] * mat[11]) - mat[9] * (mat[2] * mat[15] - mat[14] * mat[3]) + mat[13] * (mat[2] * mat[11] - mat[10] * mat[3])) * idet;
		ret[2] = (mat[1] * (mat[6] * mat[15] - mat[14] * mat[7]) - mat[5] * (mat[2] * mat[15] - mat[14] * mat[3]) + mat[13] * (mat[2] * mat[7] - mat[6] * mat[3])) * idet;
		ret[3] = -(mat[1] * (mat[6] * mat[11] - mat[10] * mat[7]) - mat[5] * (mat[2] * mat[11] - mat[10] * mat[3]) + mat[9] * (mat[2] * mat[7] - mat[6] * mat[3])) * idet;
		ret[4] = -(mat[4] * (mat[10] * mat[15] - mat[14] * mat[11]) - mat[8] * (mat[6] * mat[15] - mat[14] * mat[7]) + mat[12] * (mat[6] * mat[11] - mat[10] * mat[7])) * idet;
		ret[5] = (mat[0] * (mat[10] * mat[15] - mat[14] * mat[11]) - mat[8] * (mat[2] * mat[15] - mat[14] * mat[3]) + mat[12] * (mat[2] * mat[11] - mat[10] * mat[3])) * idet;
		ret[6] = -(mat[0] * (mat[6] * mat[15] - mat[14] * mat[7]) - mat[4] * (mat[2] * mat[15] - mat[14] * mat[3]) + mat[12] * (mat[2] * mat[7] - mat[6] * mat[3])) * idet;
		ret[7] = (mat[0] * (mat[6] * mat[11] - mat[10] * mat[7]) - mat[4] * (mat[2] * mat[11] - mat[10] * mat[3]) + mat[8] * (mat[2] * mat[7] - mat[6] * mat[3])) * idet;
		ret[8] = (mat[4] * (mat[9] * mat[15] - mat[13] * mat[11]) - mat[8] * (mat[5] * mat[15] - mat[13] * mat[7]) + mat[12] * (mat[5] * mat[11] - mat[9] * mat[7])) * idet;
		ret[9] = -(mat[0] * (mat[9] * mat[15] - mat[13] * mat[11]) - mat[8] * (mat[1] * mat[15] - mat[13] * mat[3]) + mat[12] * (mat[1] * mat[11] - mat[9] * mat[3])) * idet;
		ret[10] = (mat[0] * (mat[5] * mat[15] - mat[13] * mat[7]) - mat[4] * (mat[1] * mat[15] - mat[13] * mat[3]) + mat[12] * (mat[1] * mat[7] - mat[5] * mat[3])) * idet;
		ret[11] = -(mat[0] * (mat[5] * mat[11] - mat[9] * mat[7]) - mat[4] * (mat[1] * mat[11] - mat[9] * mat[3]) + mat[8] * (mat[1] * mat[7] - mat[5] * mat[3])) * idet;
		ret[12] = -(mat[4] * (mat[9] * mat[14] - mat[13] * mat[10]) - mat[8] * (mat[5] * mat[14] - mat[13] * mat[6]) + mat[12] * (mat[5] * mat[10] - mat[9] * mat[6])) * idet;
		ret[13] = (mat[0] * (mat[9] * mat[14] - mat[13] * mat[10]) - mat[8] * (mat[1] * mat[14] - mat[13] * mat[2]) + mat[12] * (mat[1] * mat[10] - mat[9] * mat[2])) * idet;
		ret[14] = -(mat[0] * (mat[5] * mat[14] - mat[13] * mat[6]) - mat[4] * (mat[1] * mat[14] - mat[13] * mat[2]) + mat[12] * (mat[1] * mat[6] - mat[5] * mat[2])) * idet;
		ret[15] = (mat[0] * (mat[5] * mat[10] - mat[9] * mat[6]) - mat[4] * (mat[1] * mat[10] - mat[9] * mat[2]) + mat[8] * (mat[1] * mat[6] - mat[5] * mat[2])) * idet;

		return ret;
	}
};

typedef _mat4<float>	mat4, mat4f;
typedef _mat4<double>	mat4d;

class Transform
{
public:
	quat	q;
	vec3	v;

	Transform() {}
	Transform(const quat& _q, const vec3& _v) : q(_q), v(_v) {}
	Transform(const Transform& t) : q(t.q), v(t.v) {}

	Transform& operator = (const Transform& t)
	{
		q = t.q;
		v = t.v;
		return (*this);
	}

	Transform Mod(const Transform& t) const
	{
		Transform	ret;
		ret.q = q * t.q;
		ret.v = (t.q * v) + t.v;
		return ret;
	}
	Transform Mod2(const Transform& t) const
	{
		Transform	ret;
		ret.q = q * t.q;
		ret.v = (ret.q * v) + t.v;
		return ret;
	}

	Transform& Invert(void)
	{
		q.invert().normalize();
		v = -v;
		return (*this);
	}

	Transform GetInverse(void)
	{
		Transform t(*this);
		t.Invert();
		return t;
	}

	vec3 TransformPoint(const vec3& p) const
	{
		return (q * p) + v;
	}
	vec3 TransformDirection(const vec3& p) const
	{
		return q * p;
	}
};

static mat4 MakeOrtho2D(float left, float right, float bottom, float top, float zNear, float zFar)
{
	mat4 res;

	res.m00 = 2.0f / (right - left);	res.m01 = 0.0f;						res.m02 = 0.0f;						res.m03 = -(right + left) / (right - left);
	res.m10 = 0.0f;						res.m11 = 2.0f / (top - bottom);	res.m12 = 0.0f;						res.m13 = -(top + bottom) / (top - bottom);
	res.m20 = 0.0f;						res.m21 = 0.0f;						res.m22 = -2.0f / (zFar - zNear);	res.m23 = -(zFar + zNear) / (zFar - zNear);
	res.m30 = 0.0f;						res.m31 = 0.0f;						res.m32 = 0.0f;						res.m33 = 1.0f;

	return res;
}

static mat4 MakePerspective( float fovy, float aspect, float zNear, float zFar )
{
	mat4 m;
	float sine, cotangent, deltaZ;
	float radians = fovy * 0.5f * m_DEG2RAD;

	deltaZ = zFar - zNear;
	sine = sinf( radians );
	if ( ( deltaZ == 0 ) || ( sine == 0 ) || ( aspect == 0 ) )
		return m;
	cotangent = cosf( radians ) / sine;

	m[0 ] = cotangent / aspect; m[1 ] =      0.0f; m[2 ] =                          0.0f; m[3 ] =  0.0f;
	m[4 ] =               0.0f; m[5 ] = cotangent; m[6 ] =                          0.0f; m[7 ] =  0.0f;
	m[8 ] =               0.0f; m[9 ] =      0.0f; m[10] =      -(zFar + zNear) / deltaZ; m[11] = -1.0f;
	m[12] =               0.0f; m[13] =      0.0f; m[14] = -2.0f * zNear * zFar / deltaZ; m[15] =  0.0f;

	return m;
}

static mat4 MakeLookAt(const vec3& eye, const vec3& center, const vec3& up)
{
	vec3 dir = center - eye;
	dir.normalize();

	vec3 side = cross(dir, up);
	side.normalize();

	vec3 updir = cross(side, dir);

	mat4 m;

	m[0] = side.x;	m[4] = side.y;	m[ 8] = side.z;
	m[1] = updir.x;	m[5] = updir.y;	m[ 9] = updir.z;
	m[2] = -dir.x;	m[6] = -dir.y;	m[10] = -dir.z;

	m.SetTranslateVector(vec3(-dot(side, eye), -dot(updir, eye), -dot(-dir, eye)));

	return m;
} 

static mat4 RotateMatrixZ(float angle)
{
	mat4 res;

	float rad = angle * m_DEG2RAD;
	float c = cosf(rad);
	float s = sinf(rad);
	res.m00 = c;	res.m01 = -s;	res.m02 = 0.0f;	res.m03 = 0.0;
	res.m10 = s;	res.m11 = c;	res.m12 = 0.0f;	res.m13 = 0.0;
	res.m20 = 0.0f;	res.m21 = 0.0f;	res.m22 = 1.0f;	res.m23 = 0.0;
	res.m30 = 0.0f;	res.m31 = 0.0f;	res.m32 = 0.0f;	res.m33 = 1.0;

	return res;
}

static mat4 Quat2Mat4(const quat& q)
{
	mat4 m;
	float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

	x2 = q.x + q.x;	y2 = q.y + q.y;	z2 = q.z + q.z;
	xx = q.x * x2;	xy = q.x * y2;	xz = q.x * z2;
	yy = q.y * y2;	yz = q.y * z2;	zz = q.z * z2;
	wx = q.w * x2;	wy = q.w * y2;	wz = q.w * z2;

	m[0] = 1.0f - (yy + zz);	m[4] = xy + wz;				m[8] = xz - wy;
	m[1] = xy - wz;				m[5] = 1.0f - (xx + zz);	m[9] = yz + wx;
	m[2] = xz + wy;				m[6] = yz - wx;				m[10] = 1.0f - (xx + yy);

	m[ 3] = 0.0f;
	m[ 7] = 0.0f;
	m[11] = 0.0f;
	m[12] = 0.0f;
	m[13] = 0.0f;
	m[14] = 0.0f;
	m[15] = 1.0f;

	return m;
}


#endif // __MATHLIB_H__
