#include "..\Utilities\util.h"

inline DXMatrix::DXMatrix(const float * pf)
{
	memcpy(&_11, pf, sizeof(DXMatrix));
}

inline DXMatrix::DXMatrix(const DXMatrix& mat)
{
	memcpy(&_11, &mat, sizeof(DXMatrix));
}

inline DXMatrix::DXMatrix(const D3DXMATRIX& mat)
{
	memcpy(&_11, &mat, sizeof(DXMatrix));
}

inline DXMatrix::DXMatrix( const float f11, const float f12, const float f13, const float f14,
						   const float f21, const float f22, const float f23, const float f24,
						   const float f31, const float f32, const float f33, const float f34,
						   const float f41, const float f42, const float f43, const float f44)
{
	_11 = f11; _12 = f12; _13 = f13; _14 = f14;
    _21 = f21; _22 = f22; _23 = f23; _24 = f24;
    _31 = f31; _32 = f32; _33 = f33; _34 = f34;
    _41 = f41; _42 = f42; _43 = f43; _44 = f44;
}
    
// access grants
inline float& DXMatrix::operator () (unsigned iRow, unsigned iCol)
{
	return m[iRow][iCol];
}

inline float  DXMatrix::operator () (unsigned iRow, unsigned iCol) const
{
	return m[iRow][iCol];
}

// casting operators
inline DXMatrix::operator float* ()
{
	return (float*)&_11;
}

inline DXMatrix::operator const float* () const
{
	return (const float*)&_11;
}

inline DXMatrix::operator D3DXMATRIX ()
{
	return D3DXMATRIX((float*)this);
}

// assignment operators
inline DXMatrix& DXMatrix::operator =(const D3DXMATRIX& mat)
{
	memcpy(&_11, &mat, sizeof(DXMatrix));
	return *this;
}


inline DXMatrix& DXMatrix::operator *= (const DXMatrix& mat)
{
	return *(DXMatrix*)D3DXMatrixMultiply((D3DXMATRIX*)this, (D3DXMATRIX*)this, (D3DXMATRIX*)&mat);
}

inline DXMatrix& DXMatrix::operator += (const DXMatrix& mat)
{
	_11 += mat._11; _12 += mat._12; _13 += mat._13; _14 += mat._14;
    _21 += mat._21; _22 += mat._22; _23 += mat._23; _24 += mat._24;
    _31 += mat._31; _32 += mat._32; _33 += mat._33; _34 += mat._34;
    _41 += mat._41; _42 += mat._42; _43 += mat._43; _44 += mat._44;
	return *this;
}

inline DXMatrix& DXMatrix::operator -= (const DXMatrix& mat)
{
	_11 -= mat._11; _12 -= mat._12; _13 -= mat._13; _14 -= mat._14;
    _21 -= mat._21; _22 -= mat._22; _23 -= mat._23; _24 -= mat._24;
    _31 -= mat._31; _32 -= mat._32; _33 -= mat._33; _34 -= mat._34;
    _41 -= mat._41; _42 -= mat._42; _43 -= mat._43; _44 -= mat._44;
	return *this;
}

inline DXMatrix& DXMatrix::operator *= (const float s)
{
	_11 *= s; _12 *= s; _13 *= s; _14 *= s;
    _21 *= s; _22 *= s; _23 *= s; _24 *= s;
    _31 *= s; _32 *= s; _33 *= s; _34 *= s;
    _41 *= s; _42 *= s; _43 *= s; _44 *= s;
	return *this;
}

inline DXMatrix& DXMatrix::operator /= (const float r)
{
	return *this *= 1/r;
}

// unary operators
inline DXMatrix DXMatrix::operator + () const
{
	return *this;
}

inline DXMatrix DXMatrix::operator - () const
{
    return DXMatrix(-_11, -_12, -_13, -_14,
                    -_21, -_22, -_23, -_24,
                    -_31, -_32, -_33, -_34,
                    -_41, -_42, -_43, -_44);
}

// binary operators
inline DXMatrix DXMatrix::operator * (const DXMatrix& mat) const
{
	DXMatrix temp;
    D3DXMatrixMultiply((D3DXMATRIX*)&temp, (D3DXMATRIX*)this, (D3DXMATRIX*)&mat);
    return temp;
}

inline DXMatrix DXMatrix::Multiply(const DXMatrix& a, const DXMatrix& b)
{
	DXMatrix matT;
    D3DXMatrixMultiply((D3DXMATRIX*)&matT, (D3DXMATRIX*)&a, (D3DXMATRIX*)&b);
    return matT;
}


inline DXMatrix DXMatrix::operator + (const DXMatrix& mat) const
{
	return DXMatrix(_11 + mat._11, _12 + mat._12, _13 + mat._13, _14 + mat._14,
                    _21 + mat._21, _22 + mat._22, _23 + mat._23, _24 + mat._24,
                    _31 + mat._31, _32 + mat._32, _33 + mat._33, _34 + mat._34,
                    _41 + mat._41, _42 + mat._42, _43 + mat._43, _44 + mat._44);
}

inline DXMatrix DXMatrix::operator - (const DXMatrix& mat) const
{
	return DXMatrix(_11 - mat._11, _12 - mat._12, _13 - mat._13, _14 - mat._14,
                    _21 - mat._21, _22 - mat._22, _23 - mat._23, _24 - mat._24,
                    _31 - mat._31, _32 - mat._32, _33 - mat._33, _34 - mat._34,
                    _41 - mat._41, _42 - mat._42, _43 - mat._43, _44 - mat._44);
}

inline DXMatrix DXMatrix::operator * (const float s) const
{
	return DXMatrix(_11 * s, _12 * s, _13 * s, _14 * s,
                    _21 * s, _22 * s, _23 * s, _24 * s,
                    _31 * s, _32 * s, _33 * s, _34 * s,
                    _41 * s, _42 * s, _43 * s, _44 * s);
}

inline DXMatrix DXMatrix::operator / (const float r) const
{
	float fInv = 1.0f / r;
    return DXMatrix(_11 * fInv, _12 * fInv, _13 * fInv, _14 * fInv,
                    _21 * fInv, _22 * fInv, _23 * fInv, _24 * fInv,
                    _31 * fInv, _32 * fInv, _33 * fInv, _34 * fInv,
                    _41 * fInv, _42 * fInv, _43 * fInv, _44 * fInv);
}

inline DXMatrix operator * ( const float f, const DXMatrix& mat)
{
	return DXMatrix(f * mat._11, f * mat._12, f * mat._13, f * mat._14,
                    f * mat._21, f * mat._22, f * mat._23, f * mat._24,
                    f * mat._31, f * mat._32, f * mat._33, f * mat._34,
                    f * mat._41, f * mat._42, f * mat._43, f * mat._44);
}

inline bool DXMatrix::operator == ( const DXMatrix& mat ) const
{
	return 0 == memcmp(this, &mat, sizeof(DXMatrix));
}

inline bool DXMatrix::operator != ( const DXMatrix& mat ) const
{
	return 0 != memcmp(this, &mat, sizeof(DXMatrix));
}

//Get Set Functions
inline float DXMatrix::Get(const int row, const int col) const 
{
	return m[row][col];
}

inline void DXMatrix::Set(const int row, const int col, const float value) 
{
	m[row][col] = value;
}

inline void DXMatrix::SetAxes(const DXVector3& x, const DXVector3& y, const DXVector3& z)
{
	_11 = x.x; _12 = x.y; _13 = x.z; _14 = 0;
	_21 = y.x; _22 = y.y; _23 = y.z; _24 = 0;
	_31 = z.x; _32 = z.y; _33 = z.z; _34 = 0;
	_41 = 0; _42 = 0; _43 = 0; _44 = 1;
}

inline DXVector3 DXMatrix::GetYawPitchRoll(void) const
{
	/*DXVector3 out;
	out.x = asin(_32); 
	float test = cos(out.x); 
	if(test > 0.001f)
	{
		out.z = atan2(_12, _22);
		out.y = atan2(_31, _33); 
	} 
	else
	{ 
		out.z = atan2(_21, _11); 
		out.y = 0.0; 
	} 
	return out;*/

	// Assuming the angles are in radians.
	DXVector3 out;

	if (_12 > 0.998) 
	{ // singularity at north pole
		out.x = atan2(_31,_33);
		out.y = D3DX_PI * 0.5f;
		out.z = 0;
		return out;
	}	
	
	if (_12 < -0.998) 
	{ // singularity at south pole
		out.x = atan2(_31,_33);		
		out.y = -D3DX_PI * 0.5f;
		out.z = 0;
		return out;
	}	
	
	out.x = atan2(-_13, _11);
	out.y = atan2(-_32, _22);
	out.z = asin(_12);
	return out;
}	

inline DXVector3 DXMatrix::GetXAxis(void) const 
{ 
	return DXVector3(_11, _21, _31); 
}

inline DXVector3 DXMatrix::GetYAxis(void) const 
{ 
	return DXVector3(_21, _22, _32); 
}

inline DXVector3 DXMatrix::GetZAxis(void) const 
{ 
	return DXVector3(_31, _23, _33); 
}

inline DXVector3 DXMatrix::GetTranslation(void) const
{
	return DXVector3(_41, _42, _43);
}

inline void DXMatrix::SetTranslation(const DXVector3& trans)
{
	_41 = trans.x;
	_42 = trans.y;
	_43 = trans.z;
}

//functions
inline float DXMatrix::Det(void) const
{
	return D3DXMatrixDeterminant((D3DXMATRIX*)this);
}

//manipulations
inline void DXMatrix::Invert(void) 
{ 
	D3DXMatrixInverse((D3DXMATRIX*)this, NULL, (D3DXMATRIX*)this); 
}

inline void DXMatrix::SetIdentity(void) 
{ 
	D3DXMatrixIdentity((D3DXMATRIX*)this); 
}

//matrix constructions
inline DXMatrix DXMatrix::IDENTITY(void) 
{
	DXMatrix temp; 
	D3DXMatrixIdentity((D3DXMATRIX*)&temp);
	return temp;
}

inline DXMatrix DXMatrix::InvertMatrix(const DXMatrix& mat) 
{ 
	DXMatrix temp; 
	D3DXMatrixInverse((D3DXMATRIX*)&temp, NULL, (D3DXMATRIX*)&mat); 
	return temp; 
} 

inline DXMatrix DXMatrix::ScalingMatrix(const DXVector3& s) 
{ 
	DXMatrix temp; 
	D3DXMatrixScaling((D3DXMATRIX*)&temp, s.x, s.y, s.z); 
	return temp;
}

inline DXMatrix DXMatrix::ScalingMatrix(const float sx, const float sy, const float sz) 
{ 
	DXMatrix temp; 
	D3DXMatrixScaling((D3DXMATRIX*)&temp, sx, sy, sz); 
	return temp; 
}

inline DXMatrix DXMatrix::TranslationMatrix(const DXVector3& t) 
{ 
	DXMatrix temp; 
	D3DXMatrixTranslation((D3DXMATRIX*)&temp, t.x, t.y, t.z); 
	return temp;
}

inline DXMatrix DXMatrix::TranslationMatrix(const float tx, const float ty, const float tz) 
{ 
	DXMatrix temp; 
	D3DXMatrixTranslation((D3DXMATRIX*)&temp, tx, ty, tz); 
	return temp;
}

inline DXMatrix DXMatrix::RotationYawPitchRoll(const DXVector3& rot)
{
	DXMatrix temp;
	D3DXMatrixRotationYawPitchRoll((D3DXMATRIX*)&temp, rot.x, rot.y, rot.z);
	return temp; 
}

inline DXMatrix DXMatrix::RotationYawPitchRoll(const float yaw, const float pitch, const float roll)
{ 
	DXMatrix temp;
	D3DXMatrixRotationYawPitchRoll((D3DXMATRIX*)&temp, yaw, pitch, roll);
	return temp; 
}

inline DXMatrix DXMatrix::TransposeMatrix(const DXMatrix& mat) 
{ 
	DXMatrix temp; 
	D3DXMatrixTranspose((D3DXMATRIX*)&temp, (D3DXMATRIX*)&mat);
	return temp; 
} 

inline DXMatrix DXMatrix::RotationX(const float ang) 
{ 
	DXMatrix temp; 
	D3DXMatrixRotationX((D3DXMATRIX*)&temp, ang); 
	return temp; 
}

inline DXMatrix DXMatrix::RotationY(const float ang) 
{ 
	DXMatrix temp; 
	D3DXMatrixRotationY((D3DXMATRIX*)&temp, ang); 
	return temp; 
}

inline DXMatrix DXMatrix::RotationZ(const float ang) 
{ 
	DXMatrix temp; 
	D3DXMatrixRotationZ((D3DXMATRIX*)&temp, ang); 
	return temp; 
}

inline DXMatrix DXMatrix::RotationAxis(const DXVector3& ax, const float ang) 
{ 
	DXMatrix temp; 
	D3DXMatrixRotationAxis((D3DXMATRIX*)&temp, (const D3DXVECTOR3*)&ax, ang); 
	return temp; 
}

inline DXMatrix DXMatrix::LookAtMatrix(const DXVector3& pos, const DXVector3& lookat, const DXVector3 up)
{
	DXMatrix temp; 
	D3DXMatrixLookAtLH((D3DXMATRIX*)&temp, (const D3DXVECTOR3*)&pos, (const D3DXVECTOR3*)&lookat, (const D3DXVECTOR3*)&up); 
	return temp; 
}

//vector transformations
inline DXVector3 DXMatrix::TransformCoord(const DXVector3& v) const 
{
	DXVector3 temp; 
	D3DXVec3TransformCoord((D3DXVECTOR3*)&temp, (const D3DXVECTOR3*)&v, (const D3DXMATRIX*)this); 
	return temp; 
}

inline DXVector3 DXMatrix::TransformNormal(const DXVector3& v) const 
{
	DXVector3 temp; 
	D3DXVec3TransformNormal((D3DXVECTOR3*)&temp, (const D3DXVECTOR3*)&v, (const D3DXMATRIX*)this); 
	return temp; 
}

inline DXVector4 DXMatrix::Transform(const DXVector4& v) const 
{
	DXVector4 temp; 
	D3DXVec4Transform((D3DXVECTOR4*)&temp, (const D3DXVECTOR4*)&v, (const D3DXMATRIX*)this); 
	return temp; 
}

inline DXMatrix DXMatrix::AlexaScale(const DXMatrix& A, const float s)
{
	DXMatrix mlog = A.Logarithm();
	return (s * mlog).Exponential();
}

inline DXMatrix DXMatrix::AlexaAdd(const DXMatrix& A, const DXMatrix& B)
{
	return (A.Logarithm() + B.Logarithm()).Exponential();
}

inline DXMatrix DXMatrix::Lerp(const DXMatrix& A, const DXMatrix& B, const float s)
{
	//DXMatrix a = AlexaScale(DXMatrix::TransposeMatrix(A), 1-s);
	//DXMatrix b = AlexaScale(DXMatrix::TransposeMatrix(B), s);
	//return DXMatrix::TransposeMatrix(AlexaAdd(a, b));

	DXMatrix a = AlexaScale(A, 1 - s);
	DXMatrix b = AlexaScale(B, s);
	return AlexaAdd(a, b);
}
