#include "Matrix44.h"

using namespace std;

Matrix44::Matrix44(void)
{
	data[0] = 1;
	data[1] = 0;
	data[2] = 0;
	data[3] = 0;
	data[4] = 0;
	data[5] = 1;
	data[6] = 0;
	data[7] = 0;
	data[8] = 0;
	data[9] = 0;
	data[10] = 1;
	data[11] = 0;
	data[12] = 0;
	data[13] = 0;
	data[14] = 0;
	data[15] = 1;
}

Matrix44::Matrix44(float _mat[16])
{
	for(int i=0; i<16; i++)
		data[i] = _mat[i];
}

Matrix44::~Matrix44(void)
{
}

void Matrix44::print() const
{
	cout << data[0] << " " << data[4] << " " << data[8] << " " << data[12] << endl;
	cout << data[1] << " " << data[5] << " " << data[9] << " " << data[13] << endl;
	cout << data[2] << " " << data[6] << " " << data[10] << " " << data[14] << endl;
	cout << data[3] << " " << data[7] << " " << data[11] << " " << data[15] << endl << endl;
}

float* Matrix44::getData()
{
	return (float*)data;
}

void Matrix44::setPosition(Vector3 position)
{
	data[0] = 1;
	data[1] = 0;
	data[2] = 0;
	data[3] = 0;
	data[4] = 0;
	data[5] = 1;
	data[6] = 0;
	data[7] = 0;
	data[8] = 0;
	data[9] = 0;
	data[10] = 1;
	data[11] = 0;
	data[12] = position[0];
	data[13] = position[1];
	data[14] = position[2];
	data[15] = 1;
}

void Matrix44::setScale(Vector3 scale)
{
	data[0] = scale[0];
	data[1] = 0;
	data[2] = 0;
	data[3] = 0;
	data[4] = 0;
	data[5] = scale[1];
	data[6] = 0;
	data[7] = 0;
	data[8] = 0;
	data[9] = 0;
	data[10] = scale[2];
	data[11] = 0;
	data[12] = 0;
	data[13] = 0;
	data[14] = 0;
	data[15] = 1;
}

void Matrix44::setRotationFromQuaternion(float x, float y, float z, float w)
{
	data[ 0] = 1.0f - 2.0f * ( y * y + z * z );
	data[ 1] = 2.0f * ( x * y + w * z );
	data[ 2] = 2.0f * ( x * z - w * y );
	data[ 3] = 0.0f;
	
	data[ 4] = 2.0f * ( x * y - w * z );
	data[ 5] = 1.0f - 2.0f * ( x * x + z * z );
	data[ 6] = 2.0f * ( y * z + w * x );
	data[ 7] = 0.0f;
	
	data[ 8] = 2.0f * ( x * z + w * y );
	data[ 9] = 2.0f * ( y * z - w * x );
	data[10] = 1.0f - 2.0f * ( x * x + y * y );
	data[11] = 0.0f;
	
	data[12] = 0.0f;
	data[13] = 0.0f;
	data[14] = 0.0f;
	data[15] = 1.0f;
}

float& Matrix44::operator [](int index)
{
	return data[index];
}

//Matrix44 Matrix44::operator=(Matrix44 other) const
//{
//	Matrix44 temp;
//	for(int i=0; i<16; i++)
//		temp[i] = other[i]
//	return temp;
//}

bool Matrix44::operator==(Matrix44 other) const
{
	bool result = true;
	for(int i=0; i<16; i++)
		result &= (data[i] == other[i]);
	return result;
}

Matrix44 Matrix44::operator*(Matrix44 other)
{
	float _mat[16];
	_mat[0] = data[0]*other[0] + data[4]*other[1] + data[8]*other[2] + data[12]*other[3];
	_mat[1] = data[1]*other[0] + data[5]*other[1] + data[9]*other[2] + data[13]*other[3];
	_mat[2] = data[2]*other[0] + data[6]*other[1] + data[10]*other[2] + data[14]*other[3];
	_mat[3] = data[3]*other[0] + data[7]*other[1] + data[11]*other[2] + data[15]*other[3];

	_mat[4] = data[0]*other[4] + data[4]*other[5] + data[8]*other[6] + data[12]*other[7];
	_mat[5] = data[1]*other[4] + data[5]*other[5] + data[9]*other[6] + data[13]*other[7];
	_mat[6] = data[2]*other[4] + data[6]*other[5] + data[10]*other[6] + data[14]*other[7];
	_mat[7] = data[3]*other[4] + data[7]*other[5] + data[11]*other[6] + data[15]*other[7];

	_mat[8] = data[0]*other[8] + data[4]*other[9] + data[8]*other[10] + data[12]*other[11];
	_mat[9] = data[1]*other[8] + data[5]*other[9] + data[9]*other[10] + data[13]*other[11];
	_mat[10] = data[2]*other[8] + data[6]*other[9] + data[10]*other[10] + data[14]*other[11];
	_mat[11] = data[3]*other[8] + data[7]*other[9] + data[11]*other[10] + data[15]*other[11];

	_mat[12] = data[0]*other[12] + data[4]*other[13] + data[8]*other[14] + data[12]*other[15];
	_mat[13] = data[1]*other[12] + data[5]*other[13] + data[9]*other[14] + data[13]*other[15];
	_mat[14] = data[2]*other[12] + data[6]*other[13] + data[10]*other[14] + data[14]*other[15];
	_mat[15] = data[3]*other[12] + data[7]*other[13] + data[11]*other[14] + data[15]*other[15];
	return Matrix44(_mat);
}

Matrix44& Matrix44::operator*=(Matrix44 other)
{
	float _mat[16];
	_mat[0] = data[0]*other[0] + data[4]*other[1] + data[8]*other[2] + data[12]*other[3];
	_mat[1] = data[1]*other[0] + data[5]*other[1] + data[9]*other[2] + data[13]*other[3];
	_mat[2] = data[2]*other[0] + data[6]*other[1] + data[10]*other[2] + data[14]*other[3];
	_mat[3] = data[3]*other[0] + data[7]*other[1] + data[11]*other[2] + data[15]*other[3];

	_mat[4] = data[0]*other[4] + data[4]*other[5] + data[8]*other[6] + data[12]*other[7];
	_mat[5] = data[1]*other[4] + data[5]*other[5] + data[9]*other[6] + data[13]*other[7];
	_mat[6] = data[2]*other[4] + data[6]*other[5] + data[10]*other[6] + data[14]*other[7];
	_mat[7] = data[3]*other[4] + data[7]*other[5] + data[11]*other[6] + data[15]*other[7];

	_mat[8] = data[0]*other[8] + data[4]*other[9] + data[8]*other[10] + data[12]*other[11];
	_mat[9] = data[1]*other[8] + data[5]*other[9] + data[9]*other[10] + data[13]*other[11];
	_mat[10] = data[2]*other[8] + data[6]*other[9] + data[10]*other[10] + data[14]*other[11];
	_mat[11] = data[3]*other[8] + data[7]*other[9] + data[11]*other[10] + data[15]*other[11];

	_mat[12] = data[0]*other[12] + data[4]*other[13] + data[8]*other[14] + data[12]*other[15];
	_mat[13] = data[1]*other[12] + data[5]*other[13] + data[9]*other[14] + data[13]*other[15];
	_mat[14] = data[2]*other[12] + data[6]*other[13] + data[10]*other[14] + data[14]*other[15];
	_mat[15] = data[3]*other[12] + data[7]*other[13] + data[11]*other[14] + data[15]*other[15];

	for(int i=0; i<16; i++)
		data[i] = _mat[i];
	return *this;
}

Vector3 Matrix44::operator*(Vector3 other)
{
	float x = data[0]*other[0] + data[4]*other[1] + data[8]*other[2] + data[12];
	float y = data[1]*other[0] + data[5]*other[1] + data[9]*other[2] + data[13];
	float z = data[2]*other[0] + data[6]*other[1] + data[10]*other[2] + data[14];
	float w = data[3]*other[0] + data[7]*other[1] + data[11]*other[2] + data[15];

	if(w != 1)
	{
		x /= w;
		y /= w;
		z /= w;
	}
	return Vector3(x,y,z);
}

Vector4 Matrix44::operator*(Vector4 other)
{
	float x = data[0]*other[0] + data[4]*other[1] + data[8]*other[2] + data[12]*other[3];
	float y = data[1]*other[0] + data[5]*other[1] + data[9]*other[2] + data[13]*other[3];
	float z = data[2]*other[0] + data[6]*other[1] + data[10]*other[2] + data[14]*other[3];
	float w = data[3]*other[0] + data[7]*other[1] + data[11]*other[2] + data[15]*other[3];

	return Vector4(x,y,z,w);
}
