/// Copyright (c) 2010 Ruben Penalva. All rights reserved.
/// Redistribution and use in source and binary forms, with or without modification, 
/// are permitted provided that the following conditions are met:
///    1. Redistributions of source code must retain the above copyright notice, this 
///    list of conditions and the following disclaimer.
///    2. Redistributions in binary form must reproduce the above copyright notice,
///    this list of conditions and the following disclaimer in the documentation and/or
///    other materials provided with the distribution.
///    3. The name of the author may not be used to endorse or promote products derived
///    from this software without specific prior written permission.
///
/// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.

#include "Matrix4x4.h"

#include <math.h>

#include <string>

#include <boost\math\constants\constants.hpp>

using namespace TestFrameWork;

namespace
{

inline float Degree2Radians(void)
{
	return boost::math::constants::pi<float>() / 180.0f;
}

}
Matrix4x4::Matrix4x4(void)
{
	BuildIdentity();
}

Matrix4x4::~Matrix4x4(void)
{
}

Matrix4x4& Matrix4x4::operator=(const Matrix4x4& m)
{
	memcpy(m_data, m.m_data, sizeof(float) * 16);

	return *this;
}
	
Matrix4x4& Matrix4x4::operator*=(const Matrix4x4& m)
{
	*this = *this * m;
	
	return *this;
}

Vector3 Matrix4x4::operator*(const Vector3& vector) const
{
	return Vector3(m_data[0] * vector.m_x + m_data[1] * vector.m_y + m_data[2] * vector.m_z + m_data[3],
		m_data[4] * vector.m_x + m_data[5] * vector.m_y + m_data[6] * vector.m_z + m_data[7],
		m_data[8] * vector.m_x + m_data[9] * vector.m_y + m_data[10] * vector.m_z + m_data[11]);
}

Matrix4x4 Matrix4x4::operator*(const Matrix4x4& matrix) const
{
	Matrix4x4 result;

	result.m_data[0] = m_data[0] * matrix.m_data[0] + m_data[1] * matrix.m_data[4] + m_data[2] * matrix.m_data[8] + m_data[3] * matrix.m_data[12];	
	result.m_data[1] = m_data[0] * matrix.m_data[1] + m_data[1] * matrix.m_data[5] + m_data[2] * matrix.m_data[9] + m_data[3] * matrix.m_data[13];		
	result.m_data[2] = m_data[0] * matrix.m_data[2] + m_data[1] * matrix.m_data[6] + m_data[2] * matrix.m_data[10] + m_data[3] * matrix.m_data[14];	
	result.m_data[3] = m_data[0] * matrix.m_data[3] + m_data[1] * matrix.m_data[7] + m_data[2] * matrix.m_data[11] + m_data[3] * matrix.m_data[15];

	result.m_data[4] = m_data[4] * matrix.m_data[0] + m_data[5] * matrix.m_data[4] + m_data[6] * matrix.m_data[8] + m_data[7] * matrix.m_data[12];	
	result.m_data[5] = m_data[4] * matrix.m_data[1] + m_data[5] * matrix.m_data[5] + m_data[6] * matrix.m_data[9] + m_data[7] * matrix.m_data[13];		
	result.m_data[6] = m_data[4] * matrix.m_data[2] + m_data[5] * matrix.m_data[6] + m_data[6] * matrix.m_data[10] + m_data[7] * matrix.m_data[14];	
	result.m_data[7] = m_data[4] * matrix.m_data[3] + m_data[5] * matrix.m_data[7] + m_data[6] * matrix.m_data[11] + m_data[7] * matrix.m_data[15];

	result.m_data[8] = m_data[8] * matrix.m_data[0] + m_data[9] * matrix.m_data[4] + m_data[10] * matrix.m_data[8] + m_data[11] * matrix.m_data[12];	
	result.m_data[9] = m_data[8] * matrix.m_data[1] + m_data[9] * matrix.m_data[5] + m_data[10] * matrix.m_data[9] + m_data[11] * matrix.m_data[13];		
	result.m_data[10] = m_data[8] * matrix.m_data[2] + m_data[9] * matrix.m_data[6] + m_data[10] * matrix.m_data[10] + m_data[11] * matrix.m_data[14];	
	result.m_data[11] = m_data[8] * matrix.m_data[3] + m_data[9] * matrix.m_data[7] + m_data[10] * matrix.m_data[11] + m_data[11] * matrix.m_data[15];

	result.m_data[12] = m_data[12] * matrix.m_data[0] + m_data[13] * matrix.m_data[4] + m_data[14] * matrix.m_data[8] + m_data[15] * matrix.m_data[12];	
	result.m_data[13] = m_data[12] * matrix.m_data[1] + m_data[13] * matrix.m_data[5] + m_data[14] * matrix.m_data[9] + m_data[15] * matrix.m_data[13];		
	result.m_data[14] = m_data[12] * matrix.m_data[2] + m_data[13] * matrix.m_data[6] + m_data[14] * matrix.m_data[10] + m_data[15] * matrix.m_data[14];	
	result.m_data[15] = m_data[12] * matrix.m_data[3] + m_data[13] * matrix.m_data[7] + m_data[14] * matrix.m_data[11] + m_data[15] * matrix.m_data[15];

	return result;
}

void Matrix4x4::BuildIdentity(void)
{
	m_data[0] = 1.0f;	m_data[1] = 0.0f;	m_data[2] = 0.0f;	m_data[3] = 0.0f;
	m_data[4] = 0.0f;	m_data[5] = 1.0f;	m_data[6] = 0.0f;	m_data[7] = 0.0f;
	m_data[8] = 0.0f;	m_data[9] = 0.0f;	m_data[10] = 1.0f;	m_data[11] = 0.0f;
	m_data[12] = 0.0f;	m_data[13] = 0.0f;	m_data[14] = 0.0f;	m_data[15] = 1.0f;
}

void Matrix4x4::BuildPerspective(float fovy, float aspect, float zNear, float zFar)
{
	float f = 1.0f / tanf((fovy * Degree2Radians()) * 0.5f);
	
	m_data[0] = f / aspect; m_data[1] = 0.0f;	m_data[2] = 0.0f;									m_data[3] = 0.0f;
	m_data[4] = 0.0f;		m_data[5] = f;		m_data[6] = 0.0f;									m_data[7] = 0.0f;
	m_data[8] = 0.0f;		m_data[9] = 0.0f;	m_data[10] = (zFar + zNear) / (zNear - zFar);		m_data[11] = 2 * zFar * zNear / (zNear - zFar);
	m_data[12] = 0.0f;		m_data[13] = 0.0f;	m_data[14] = -1.0f;									m_data[15] = 0.0f;
}

void Matrix4x4::BuildView(const Vector3& eye, const Vector3& target, const Vector3& up)
{
	Vector3 zAxis(target - eye);
	zAxis.Normalize();

	Vector3 xAxis(zAxis.Cross(up));
	xAxis.Normalize();

	Vector3 yAxis(xAxis.Cross(zAxis));
	yAxis.Normalize();

	m_data[0] = xAxis.m_x;		m_data[1] = xAxis.m_y;	m_data[2] = xAxis.m_z;			m_data[3] = -xAxis.Dot(eye);
	m_data[4] = yAxis.m_x;		m_data[5] = yAxis.m_y;	m_data[6] = yAxis.m_z;			m_data[7] = -yAxis.Dot(eye);
	m_data[8] = -zAxis.m_x;		m_data[9] = -zAxis.m_y;	m_data[10] = -zAxis.m_z;		m_data[11] = zAxis.Dot(eye);
	m_data[12] = 0.0f;			m_data[13] = 0.0f;		m_data[14] = 0.0f;				m_data[15] = 1.0f;
}

void Matrix4x4::BuildTranslation(const Vector3& position)
{
	m_data[0] = 1.0f;	m_data[1] = 0.0f;	m_data[2] = 0.0f;	m_data[3] = position.m_x;
	m_data[4] = 0.0f;	m_data[5] = 1.0f;	m_data[6] = 0.0f;	m_data[7] = position.m_y;
	m_data[8] = 0.0f;	m_data[9] = 0.0f;	m_data[10] = 1.0f;	m_data[11] = position.m_z;
	m_data[12] = 0.0f;	m_data[13] = 0.0f;	m_data[14] = 0.0f;	m_data[15] = 1.0f;
}

void Matrix4x4::BuildRotationX(float angle)
{
	float sine = sinf(angle * Degree2Radians());
	float cosine = cosf(angle * Degree2Radians());

	m_data[0] = 1.0f;	m_data[1] = 0.0f;	m_data[2] = 0.0f;		m_data[3] = 0.0f;
	m_data[4] = 0.0f;	m_data[5] = cosine;	m_data[6] = -sine;		m_data[7] = 0.0f;
	m_data[8] = 0.0f;	m_data[9] = sine;	m_data[10] = cosine;	m_data[11] = 0.0f;
	m_data[12] = 0.0f;	m_data[13] = 0.0f;	m_data[14] = 0.0f;		m_data[15] = 1.0f;
}

void Matrix4x4::BuildRotationY(float angle)
{
	float sine = sinf(angle * Degree2Radians());
	float cosine = cosf(angle * Degree2Radians());

	m_data[0] = cosine;	m_data[1] = 0.0f;	m_data[2] = sine;		m_data[3] = 0.0f;
	m_data[4] = 0.0f;	m_data[5] = 1.0f;	m_data[6] = 0.0f;		m_data[7] = 0.0f;
	m_data[8] = -sine;	m_data[9] = 0.0f;	m_data[10] = cosine;	m_data[11] = 0.0f;
	m_data[12] = 0.0f;	m_data[13] = 0.0f;	m_data[14] = 0.0f;		m_data[15] = 1.0f;
}

void Matrix4x4::BuildRotationZ(float angle)
{
	float sine = sinf(angle * Degree2Radians());
	float cosine = cosf(angle * Degree2Radians());

	m_data[0] = cosine;	m_data[1] = -sine;	m_data[2] = 0.0f;	m_data[3] = 0.0f;
	m_data[4] = sine;	m_data[5] = cosine;	m_data[6] = 0.0f;	m_data[7] = 0.0f;
	m_data[8] = 0.0f;	m_data[9] = 0.0f;	m_data[10] = 1.0f;	m_data[11] = 0.0f;
	m_data[12] = 0.0f;	m_data[13] = 0.0f;	m_data[14] = 0.0f;	m_data[15] = 1.0f;
}

void Matrix4x4::BuildScale(float x, float y, float z)
{
	m_data[0] = x;		m_data[1] = 0.0f;	m_data[2] = 0.0f;	m_data[3] = 0.0f;
	m_data[4] = 0.0f;	m_data[5] = y;		m_data[6] = 0.0f;	m_data[7] = 0.0f;
	m_data[8] = 0.0f;	m_data[9] = 0.0f;	m_data[10] = z;		m_data[11] = 0.0f;
	m_data[12] = 0.0f;	m_data[13] = 0.0f;	m_data[14] = 0.0f;	m_data[15] = 1.0f;
}

const float* Matrix4x4::GetData(void) const
{
	return m_data;
}   

Matrix4x4 Matrix4x4::Inverse(void) const
{
	Matrix4x4 result(*this);

	Vector3 translation(result.m_data[3], result.m_data[7], result.m_data[11]);
	Vector3 xAxis(result.m_data[0], result.m_data[4], result.m_data[8]);
	Vector3 yAxis(result.m_data[1], result.m_data[5], result.m_data[9]);
	Vector3 zAxis(result.m_data[2], result.m_data[6], result.m_data[10]);

	result.m_data[0] = xAxis.m_x;	result.m_data[1] = xAxis.m_y;	result.m_data[2] = xAxis.m_z;		result.m_data[3] = -xAxis.Dot(translation);
	result.m_data[4] = yAxis.m_x;	result.m_data[5] = yAxis.m_y;	result.m_data[6] = yAxis.m_z;		result.m_data[7] = -yAxis.Dot(translation);
	result.m_data[8] = zAxis.m_x;	result.m_data[9] = zAxis.m_y;	result.m_data[10] = zAxis.m_z;		result.m_data[11] = -zAxis.Dot(translation);
	result.m_data[12] = 0.0f;		result.m_data[13] = 0.0f;		result.m_data[14] = 0.0f;			result.m_data[15] = 1.0f;
	
	return result;
}

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

Vector3 Matrix4x4::GetTranslation(void) const
{
	return Vector3(m_data[3], m_data[7], m_data[11]);
}

Vector3 Matrix4x4::GetXAxis(void) const
{
	return Vector3(m_data[0], m_data[4], m_data[8]);
}

Vector3 Matrix4x4::GetYAxis(void) const
{
	return Vector3(m_data[1], m_data[5], m_data[9]);
}

Vector3 Matrix4x4::GetZAxis(void) const
{
	return Vector3(m_data[2], m_data[6], m_data[10]);
}

Vector3 Matrix4x4::GetPosition(void) const
{
	return Vector3(m_data[3], m_data[7], m_data[11]);
}
