/** \file Matrix.cpp
 * \brief The implementation of the 'Matrix' class.
 */

#include "Matrix.h"
#include <string>
#include <math.h>

namespace OSkA
{
	Matrix::Matrix()
	{
		SetIdentity();
	}

	Matrix::~Matrix()
	{
	}

	Matrix::Matrix(const Matrix &matrix)
	{
		memcpy(mCell, matrix.mCell, 16*sizeof(float));
	}

	Matrix::Matrix(float values[16])
	{
		memcpy(mCell, values, 16*sizeof(float));
	}

	Matrix::Matrix(float a11, float a12, float a13, float a14, 
						float a21, float a22, float a23, float a24, 
						float a31, float a32, float a33, float a34, 
						float a41, float a42, float a43, float a44)
	{
		mCell[0]=a11;
		mCell[1]=a12;
		mCell[2]=a13;
		mCell[3]=a14;
		mCell[4]=a21;
		mCell[5]=a22;
		mCell[6]=a23;
		mCell[7]=a24;
		mCell[8]=a31;
		mCell[9]=a32;
		mCell[10]=a33;
		mCell[11]=a34;
		mCell[12]=a41;
		mCell[13]=a42;
		mCell[14]=a43;
		mCell[15]=a44;
	}

	void Matrix::Transpose()
	{
		float tmp[16];
		memcpy(tmp, mCell, 16*sizeof(float));

		mCell[1] = tmp[4];
		mCell[2] = tmp[8];
		mCell[3] = tmp[12];

		mCell[4] = tmp[1];
		mCell[6] = tmp[9];
		mCell[7] = tmp[13];

		mCell[8] = tmp[2];
		mCell[9] = tmp[6];
		mCell[11] = tmp[14];

		mCell[12] = tmp[3];
		mCell[13] = tmp[7];
		mCell[14] = tmp[11];
	}

	void Matrix::SetIdentity()
	{
		memset(mCell, 0, 16*sizeof(float)); 
		mCell[0]=1.0f;
		mCell[5]=1.0f;
		mCell[10]=1.0f;
		mCell[15]=1.0f;
	}

	Matrix& Matrix::operator=(const Matrix &matrix)
	{
		memcpy(mCell, matrix.mCell, 16*sizeof(float));
		return *this;
	}

	Matrix Matrix::operator*(const Matrix &matrix)const
	{
		float a[16];
	 
		for(int i=0; i<4; i++)
		for(int j=0; j<4; j++)
		{
			a[i*4 + j]=mCell[j] * matrix.mCell[i*4] +
						 mCell[j+4] * matrix.mCell[i*4+1] + 
						 mCell[j+8] * matrix.mCell[i*4+2] + 
						 mCell[j+12]* matrix.mCell[i*4+3]; 
		}
		return Matrix(a);
	}

	Matrix& Matrix::operator *=(const Matrix &matrix)
	{
		Matrix temp=*this * matrix;
		memcpy(mCell, temp.mCell, 16 * sizeof(float));
		return *this;
	}

	Matrix Matrix::operator *(const float &factor)const
	{
		float a[16];
		for(int i=0; i<16; i++)
			a[i]=mCell[i]*factor;
		return Matrix(a);
	}

	Matrix& Matrix::operator *=(const float &factor)
	{
		Matrix temp=*this * factor;
		memcpy(mCell, temp.mCell, 16 * sizeof(float));
		return *this;
	}

	Matrix Matrix::operator +(const Matrix &matrix)const
	{
		float a[16];
		for(int i=0; i<16; i++)
			a[i]=mCell[i]+matrix.mCell[i];
		return Matrix(a);
	}

	Matrix& Matrix::operator +=(const Matrix &matrix)
	{
		Matrix temp=*this + matrix;
		memcpy(mCell, temp.mCell, 16 * sizeof(float));
		return *this;
	}

	void Matrix::SetTranslate(float x, float y, float z)
	{
		mCell[12]=x;
		mCell[13]=y;
		mCell[14]=z;
	}

	void Matrix::SetRotateX(float angle)
	{
		SetIdentity();
		mCell[5]=cos(angle);
		mCell[9]=-sin(angle);
		mCell[6]=-mCell[9];
		mCell[10]=mCell[5];
	}

	void Matrix::SetRotateY(float angle)
	{
		SetIdentity();
		mCell[0]=cos(angle);
		mCell[2]=-sin(angle);
		mCell[8]=-mCell[2];
		mCell[10]=mCell[0];
	}

	void Matrix::SetRotateZ(float angle)
	{
		SetIdentity();
		mCell[0]=cos(angle);
		mCell[4]=-sin(angle);
		mCell[1]=-mCell[4];
		mCell[5]=mCell[0];
	}

	void Matrix::SetScale(float x, float y, float z)
	{
		mCell[0]=(float)x;
		mCell[5]=(float)y;
		mCell[10]=(float)z;
	}

	void Matrix::SetTransform(float* pos, float rotAngle, float* rotAxis, float* scale)
	{
		Matrix rotMat, transMat, scaleMat;
		scaleMat.SetScale(scale[0], scale[1], scale[2]);
		transMat.SetTranslate(pos[0], pos[1], pos[2]);

		float c, s, t, x, y, z, w;
		c=cos(rotAngle);
		s=sin(rotAngle);
		t=1-c;
		x=rotAxis[0];
		y=rotAxis[1];
		z=rotAxis[2];
		w=sqrt(x*x + y*y + z*z);
		x /=w;
		y /=w;
		z /=w;

		rotMat.mCell[0]=t*x*x + c;
		rotMat.mCell[1]=t*x*y + s*z;
		rotMat.mCell[2]=t*x*y - s*y;

		rotMat.mCell[4]=t*x*y - s*z;
		rotMat.mCell[5]=t*y*y + c;
		rotMat.mCell[6]=t*y*z + s*x;

		rotMat.mCell[8]=t*x*z + s*y;
		rotMat.mCell[9]=t*y*z - s*x;
		rotMat.mCell[10]=t*z*z + c;

		Matrix trans=transMat * scaleMat * rotMat;

		memcpy(mCell, trans.mCell, 16*sizeof(float));
	}

	float Matrix::GetDeterminant()
	{
		double value;
		value=
			mCell[3] * mCell[6] * mCell[9] * mCell[12]-mCell[2] * mCell[7] * mCell[9] * mCell[12]-mCell[3] * mCell[5] * mCell[10] * mCell[12]+mCell[1] * mCell[7] * mCell[10] * mCell[12]+
			mCell[2] * mCell[5] * mCell[11] * mCell[12]-mCell[1] * mCell[6] * mCell[11] * mCell[12]-mCell[3] * mCell[6] * mCell[8] * mCell[13]+mCell[2] * mCell[7] * mCell[8] * mCell[13]+
			mCell[3] * mCell[4] * mCell[10] * mCell[13]-mCell[0] * mCell[7] * mCell[10] * mCell[13]-mCell[2] * mCell[4] * mCell[11] * mCell[13]+mCell[0] * mCell[6] * mCell[11] * mCell[13]+
			mCell[3] * mCell[5] * mCell[8] * mCell[14]-mCell[1] * mCell[7] * mCell[8] * mCell[14]-mCell[3] * mCell[4] * mCell[9] * mCell[14]+mCell[0] * mCell[7] * mCell[9] * mCell[14]+
			mCell[1] * mCell[4] * mCell[11] * mCell[14]-mCell[0] * mCell[5] * mCell[11] * mCell[14]-mCell[2] * mCell[5] * mCell[8] * mCell[15]+mCell[1] * mCell[6] * mCell[8] * mCell[15]+
			mCell[2] * mCell[4] * mCell[9] * mCell[15]-mCell[0] * mCell[6] * mCell[9] * mCell[15]-mCell[1] * mCell[4] * mCell[10] * mCell[15]+mCell[0] * mCell[5] * mCell[10] * mCell[15];
		return value;
	}

	void Matrix::Invert()
	{
		float tmCell[16];
		tmCell[0]=mCell[6]*mCell[11]*mCell[13] - mCell[7]*mCell[10]*mCell[13] + mCell[7]*mCell[9]*mCell[14] - mCell[5]*mCell[11]*mCell[14] - mCell[6]*mCell[9]*mCell[15] + mCell[5]*mCell[10]*mCell[15];
		tmCell[1]=mCell[3]*mCell[10]*mCell[13] - mCell[2]*mCell[11]*mCell[13] - mCell[3]*mCell[9]*mCell[14] + mCell[1]*mCell[11]*mCell[14] + mCell[2]*mCell[9]*mCell[15] - mCell[1]*mCell[10]*mCell[15];
		tmCell[2]=mCell[2]*mCell[7]*mCell[13] - mCell[3]*mCell[6]*mCell[13] + mCell[3]*mCell[5]*mCell[14] - mCell[1]*mCell[7]*mCell[14] - mCell[2]*mCell[5]*mCell[15] + mCell[1]*mCell[6]*mCell[15];
		tmCell[3]=mCell[3]*mCell[6]*mCell[9] - mCell[2]*mCell[7]*mCell[9] - mCell[3]*mCell[5]*mCell[10] + mCell[1]*mCell[7]*mCell[10] + mCell[2]*mCell[5]*mCell[11] - mCell[1]*mCell[6]*mCell[11];
		tmCell[4]=mCell[7]*mCell[10]*mCell[12] - mCell[6]*mCell[11]*mCell[12] - mCell[7]*mCell[8]*mCell[14] + mCell[4]*mCell[11]*mCell[14] + mCell[6]*mCell[8]*mCell[15] - mCell[4]*mCell[10]*mCell[15];
		tmCell[5]=mCell[2]*mCell[11]*mCell[12] - mCell[3]*mCell[10]*mCell[12] + mCell[3]*mCell[8]*mCell[14] - mCell[0]*mCell[11]*mCell[14] - mCell[2]*mCell[8]*mCell[15] + mCell[0]*mCell[10]*mCell[15];
		tmCell[6]=mCell[3]*mCell[6]*mCell[12] - mCell[2]*mCell[7]*mCell[12] - mCell[3]*mCell[4]*mCell[14] + mCell[0]*mCell[7]*mCell[14] + mCell[2]*mCell[4]*mCell[15] - mCell[0]*mCell[6]*mCell[15];
		tmCell[7]=mCell[2]*mCell[7]*mCell[8] - mCell[3]*mCell[6]*mCell[8] + mCell[3]*mCell[4]*mCell[10] - mCell[0]*mCell[7]*mCell[10] - mCell[2]*mCell[4]*mCell[11] + mCell[0]*mCell[6]*mCell[11];
		tmCell[8]=mCell[5]*mCell[11]*mCell[12] - mCell[7]*mCell[9]*mCell[12] + mCell[7]*mCell[8]*mCell[13] - mCell[4]*mCell[11]*mCell[13] - mCell[5]*mCell[8]*mCell[15] + mCell[4]*mCell[9]*mCell[15];
		tmCell[9]=mCell[3]*mCell[9]*mCell[12] - mCell[1]*mCell[11]*mCell[12] - mCell[3]*mCell[8]*mCell[13] + mCell[0]*mCell[11]*mCell[13] + mCell[1]*mCell[8]*mCell[15] - mCell[0]*mCell[9]*mCell[15];
		tmCell[10]=mCell[1]*mCell[7]*mCell[12] - mCell[3]*mCell[5]*mCell[12] + mCell[3]*mCell[4]*mCell[13] - mCell[0]*mCell[7]*mCell[13] - mCell[1]*mCell[4]*mCell[15] + mCell[0]*mCell[5]*mCell[15];
		tmCell[11]=mCell[3]*mCell[5]*mCell[8] - mCell[1]*mCell[7]*mCell[8] - mCell[3]*mCell[4]*mCell[9] + mCell[0]*mCell[7]*mCell[9] + mCell[1]*mCell[4]*mCell[11] - mCell[0]*mCell[5]*mCell[11];
		tmCell[12]=mCell[6]*mCell[9]*mCell[12] - mCell[5]*mCell[10]*mCell[12] - mCell[6]*mCell[8]*mCell[13] + mCell[4]*mCell[10]*mCell[13] + mCell[5]*mCell[8]*mCell[14] - mCell[4]*mCell[9]*mCell[14];
		tmCell[13]=mCell[1]*mCell[10]*mCell[12] - mCell[2]*mCell[9]*mCell[12] + mCell[2]*mCell[8]*mCell[13] - mCell[0]*mCell[10]*mCell[13] - mCell[1]*mCell[8]*mCell[14] + mCell[0]*mCell[9]*mCell[14];
		tmCell[14]=mCell[2]*mCell[5]*mCell[12] - mCell[1]*mCell[6]*mCell[12] - mCell[2]*mCell[4]*mCell[13] + mCell[0]*mCell[6]*mCell[13] + mCell[1]*mCell[4]*mCell[14] - mCell[0]*mCell[5]*mCell[14];
		tmCell[15]=mCell[1]*mCell[6]*mCell[8] - mCell[2]*mCell[5]*mCell[8] + mCell[2]*mCell[4]*mCell[9] - mCell[0]*mCell[6]*mCell[9] - mCell[1]*mCell[4]*mCell[10] + mCell[0]*mCell[5]*mCell[10];

		float det=GetDeterminant();
		for(int i=0; i<16; i++)
		{
			mCell[i]=tmCell[i] / det;
		}
	}

	Matrix* Matrix::GetInverted()
	{
		Matrix* m=new Matrix(*this);
		m->Invert();
		return m;
	}
}