/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
------------------------------------------------------------------------------*/


		#include "math/matrix4.h"
		#include "math/matrix3.h"
		#include "math/quaternion.h"
		#include "metafile/nml.h"

		using namespace GS;

		Matrix4 Matrix4::static_identity(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);


//------------------------------------------------------------------------------
Matrix4			Matrix4::FromMatrix3(const Matrix3 &m)
{
	return Matrix4(
						m.m[0][0], m.m[1][0], m.m[2][0], 0,
						m.m[0][1], m.m[1][1], m.m[2][1], 0,
						m.m[0][2], m.m[1][2], m.m[2][2], 0,
						0, 0, 0, 1
					);
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
const Matrix4		&Matrix4WithInverse::Get() const
{	return matrix;	}
const Matrix4		&Matrix4WithInverse::GetInverse() const
{	return imatrix;		}
void				Matrix4WithInverse::Commit()
{	imatrix = matrix.InversedFast();		}
void				Matrix4WithInverse::Set(const Matrix4 &m)
{
	matrix = m;
	Commit();
}
Vector4				Matrix4WithInverse::GetRow(uint n, bool w_1) const
{	return matrix.GetRow(n, w_1);	}
Vector4				Matrix4WithInverse::GetColumn(uint n, bool w_1) const
{	return matrix.GetColumn(n, w_1);	}
void				Matrix4WithInverse::SetRow(uint n, const Vector4 &row, bool w_1)
{
	matrix.SetRow(n, row, w_1);
	Commit();
}
void				Matrix4WithInverse::SetColumn(uint n, const Vector4 &col, bool w_1)
{
	matrix.SetColumn(n, col, w_1);
	Commit();
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
NML::Tag			*Matrix4WithInverse::AsMetaTag(const char *id) const
{	return matrix.AsMetaTag(id);	}
bool				Matrix4WithInverse::FromMetaTag(NML::Tag &tag)
{
	if	(!matrix.FromMetaTag(tag))
		return false;
	Commit();
	return true;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix4			Matrix4::TransformationMatrix(const Vector4 &p, const Matrix3 &r, const Vector4 &s, const Vector4 *o)
{
	Matrix4 m =
				Matrix4::TranslationMatrix(p) *
				Matrix4::FromMatrix3(r) *
				Matrix4::ScaleMatrix(s);
	return o ? m * Matrix4::TranslationMatrix(*o) : m;
}
Matrix4			Matrix4::TransformationMatrix(const Vector4 &p, const Vector4 &r, const Vector4 &s, const Vector4 *o)
{
	Matrix4 m = 
				Matrix4::TranslationMatrix(p) *
				Matrix4::FromMatrix3(Matrix3::FromEuler(r.x, r.y, r.z)) *
				Matrix4::ScaleMatrix(s);
	return o ? m * Matrix4::TranslationMatrix(*o) : m;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix4			Matrix4::LerpAsOrthonormalBase(const Matrix4 &a, const Matrix4 &b, float k, bool fast)
{
	if	(fast)
	{
		Matrix4 o;
		for (int m = 0; m < 4; ++m)
			for (int n = 0; n < 4; ++n)
				o.m[m][n] = (b.m[m][n] - a.m[m][n]) * k + a.m[m][n];
		return o;
	}

	Matrix3 a_matrix3, b_matrix3;
	Vector4 a_position, b_position, a_scale, b_scale;

	a.Decompose(&a_position, &a_scale, &a_matrix3);
	b.Decompose(&b_position, &b_scale, &b_matrix3);

	Quaternion a_orientation(Quaternion::FromMatrix3(a_matrix3));
	Quaternion b_orientation(Quaternion::FromMatrix3(b_matrix3));

	return	Matrix4::TranslationMatrix((b_position - a_position) * k + a_position) *
			Matrix4::FromMatrix3(Quaternion::Slerp(k, a_orientation, b_orientation).AsMatrix3()) *
			Matrix4::ScaleMatrix((b_scale - a_scale) * k + a_scale);
}
void				Matrix4::Decompose(Vector4 *position, Vector4 *scale, Vector4 *rotation, Math::rOrder order) const
{
	Matrix3 m3;
	Decompose(position, scale, &m3);
	if	(rotation)
		*rotation = m3.AsEuler(order);
}
void				Matrix4::Decompose(Vector4 *position, Vector4 *scale, Matrix3 *rotation) const
{
	// Extract position.
	if	(position)
		*position = GetRow(3);

	// Extract scale.
	Vector4 scl;
	scl.Set(GetRow(0).Len(), GetRow(1).Len(), GetRow(2).Len());

	// Handle negative scale (permute X to preserve left-handedness).
	Vector4 left = GetRow(1).Cross(GetRow(2));
	if	(left.Dot(GetRow(0)) < 0)
		scl.x = -scl.x;
	if	(scale)
		*scale = scl;

	// Rotation 3x3 (renormalized).
	if	(rotation)
	{
		if	(scl.x)
		{
			scl.x = 1 / scl.x;
			rotation->SetRow(0, Vector4(m[0][0] * scl.x, m[1][0] * scl.x, m[2][0] * scl.x));
		}
		else	rotation->SetRow(0, Vector4(1, 0, 0));

		if	(scl.y)
		{
			scl.y = 1 / scl.y;
			rotation->SetRow(1, Vector4(m[0][1] * scl.y, m[1][1] * scl.y, m[2][1] * scl.y));
		}
		else	rotation->SetRow(1, Vector4(0, 1, 0));

		if	(scl.z)
		{
			scl.z = 1 / scl.z;
			rotation->SetRow(2, Vector4(m[0][2] * scl.z, m[1][2] * scl.z, m[2][2] * scl.z));
		}
		else	rotation->SetRow(2, Vector4(0, 0, 1));
	}
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix4				Matrix4::InversedFast() const
{
	// Extract inverse scale.
	Vector4 scl(1.f / GetRow(0).Len(), 1.f / GetRow(1).Len(), 1.f / GetRow(2).Len());

	// Inverse rotation 3x3 (renormalized).
	Matrix3 irt	(
						m[0][0] * scl.x, m[0][1] * scl.y, m[0][2] * scl.z,
						m[1][0] * scl.x, m[1][1] * scl.y, m[1][2] * scl.z,
						m[2][0] * scl.x, m[2][1] * scl.y, m[2][2] * scl.z
					);

	// Recompose as inverse matrix.
	return Matrix4::ScaleMatrix(scl) * (Matrix4::FromMatrix3(irt) * Matrix4::TranslationMatrix(GetRow(3).Reversed()));
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix4				Matrix4::AsOrthonormalBase() const
{
	Matrix3 rcp	(
						m[0][0], m[1][0], m[2][0],
						m[0][1], m[1][1], m[2][1],
						m[0][2], m[1][2], m[2][2]
					);
	rcp = rcp.AsOrthonormalBase();

	Matrix4 otb(*this);
	for	(int i = 0; i < 3; ++i)
		for	(int j = 0; j < 3; ++j)
			otb.m[i][j] = rcp.m[i][j];
	return otb;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix4				Matrix4::TranslationMatrix(const Vector4 &t)
{	return Matrix4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1);	}
Matrix4				Matrix4::ScaleMatrix(const Vector4 &s)
{	return Matrix4(s.x, 0, 0, 0, 0, s.y, 0, 0, 0, 0, s.z, 0, 0, 0, 0, 1);	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
NML::Tag			*Matrix4::AsMetaTag(const char *id) const
{
	NML::Tag *root = new NML::Tag(id ? id : "Mtx4");
	root->AddChild(GetRow(0, false).AsMetaTag("R0", true));
	root->AddChild(GetRow(1, false).AsMetaTag("R1", true));
	root->AddChild(GetRow(2, false).AsMetaTag("R2", true));
	root->AddChild(GetRow(3, false).AsMetaTag("R3", true));
	return root;
}
bool				Matrix4::FromMetaTag(NML::Tag &tag)
{
	NML::Tag *t;
	Vector4	R;

	List <NML::Tag *> ::Iterator i(tag.GetTags().GetRoot());

	t = i.ObjectPtr();
	if	(!t)	return false;
	R.FromMetaTag(*t); SetRow(0, R, false);
	++i;

	t = i.ObjectPtr();
	if	(!t)	return false;
	R.FromMetaTag(*t); SetRow(1, R, false);
	++i;

	t = i.ObjectPtr();
	if	(!t)	return false;
	R.FromMetaTag(*t); SetRow(2, R, false);
	++i;

	t = i.ObjectPtr();
	if	(!t)	return false;
	R.FromMetaTag(*t); SetRow(3, R, false);
	return true;
}
//------------------------------------------------------------------------------

bool Matrix4::Inverse(Matrix4 &out) const
{
	float inv[16], det;

	inv[0] = m[1][1] * m[2][2] * m[3][3] - m[1][1] * m[2][3] * m[3][2] - m[2][1] * m[1][2] * m[3][3] + m[2][1] * m[1][3] * m[3][2] + m[3][1] * m[1][2] * m[2][3] - m[3][1] * m[1][3] * m[2][2];
	inv[4] = -m[1][0] * m[2][2] * m[3][3] + m[1][0] * m[2][3] * m[3][2] + m[2][0] * m[1][2] * m[3][3] - m[2][0] * m[1][3] * m[3][2] - m[3][0] * m[1][2] * m[2][3] + m[3][0] * m[1][3] * m[2][2];
	inv[8] = m[1][0] * m[2][1] * m[3][3] - m[1][0] * m[2][3] * m[3][1] - m[2][0] * m[1][1] * m[3][3] + m[2][0] * m[1][3] * m[3][1] + m[3][0] * m[1][1] * m[2][3] - m[3][0] * m[1][3] * m[2][1];
	inv[12] = -m[1][0] * m[2][1] * m[3][2] + m[1][0] * m[2][2] * m[3][1] + m[2][0] * m[1][1] * m[3][2] - m[2][0] * m[1][2] * m[3][1] - m[3][0] * m[1][1] * m[2][2] + m[3][0] * m[1][2] * m[2][1];
	inv[1] = -m[0][1] * m[2][2] * m[3][3] + m[0][1] * m[2][3] * m[3][2] + m[2][1] * m[0][2] * m[3][3] - m[2][1] * m[0][3] * m[3][2] - m[3][1] * m[0][2] * m[2][3] + m[3][1] * m[0][3] * m[2][2];
	inv[5] = m[0][0] * m[2][2] * m[3][3] - m[0][0] * m[2][3] * m[3][2] - m[2][0] * m[0][2] * m[3][3] + m[2][0] * m[0][3] * m[3][2] + m[3][0] * m[0][2] * m[2][3] - m[3][0] * m[0][3] * m[2][2];
	inv[9] = -m[0][0] * m[2][1] * m[3][3] + m[0][0] * m[2][3] * m[3][1] + m[2][0] * m[0][1] * m[3][3] - m[2][0] * m[0][3] * m[3][1] - m[3][0] * m[0][1] * m[2][3] + m[3][0] * m[0][3] * m[2][1];
	inv[13] = m[0][0] * m[2][1] * m[3][2] - m[0][0] * m[2][2] * m[3][1] - m[2][0] * m[0][1] * m[3][2] + m[2][0] * m[0][2] * m[3][1] + m[3][0] * m[0][1] * m[2][2] - m[3][0] * m[0][2] * m[2][1];
	inv[2] = m[0][1] * m[1][2] * m[3][3] - m[0][1] * m[1][3] * m[3][2] - m[1][1] * m[0][2] * m[3][3] + m[1][1] * m[0][3] * m[3][2] + m[3][1] * m[0][2] * m[1][3] - m[3][1] * m[0][3] * m[1][2];
	inv[6] = -m[0][0] * m[1][2] * m[3][3] + m[0][0] * m[1][3] * m[3][2] + m[1][0] * m[0][2] * m[3][3] - m[1][0] * m[0][3] * m[3][2] - m[3][0] * m[0][2] * m[1][3] + m[3][0] * m[0][3] * m[1][2];
	inv[10] = m[0][0] * m[1][1] * m[3][3] - m[0][0] * m[1][3] * m[3][1] - m[1][0] * m[0][1] * m[3][3] + m[1][0] * m[0][3] * m[3][1] + m[3][0] * m[0][1] * m[1][3] - m[3][0] * m[0][3] * m[1][1];
	inv[14] = -m[0][0] * m[1][1] * m[3][2] + m[0][0] * m[1][2] * m[3][1] + m[1][0] * m[0][1] * m[3][2] - m[1][0] * m[0][2] * m[3][1] - m[3][0] * m[0][1] * m[1][2] + m[3][0] * m[0][2] * m[1][1];
	inv[3] = -m[0][1] * m[1][2] * m[2][3] + m[0][1] * m[1][3] * m[2][2] + m[1][1] * m[0][2] * m[2][3] - m[1][1] * m[0][3] * m[2][2] - m[2][1] * m[0][2] * m[1][3] + m[2][1] * m[0][3] * m[1][2];
	inv[7] = m[0][0] * m[1][2] * m[2][3] - m[0][0] * m[1][3] * m[2][2] - m[1][0] * m[0][2] * m[2][3] + m[1][0] * m[0][3] * m[2][2] + m[2][0] * m[0][2] * m[1][3] - m[2][0] * m[0][3] * m[1][2];
	inv[11] = -m[0][0] * m[1][1] * m[2][3] + m[0][0] * m[1][3] * m[2][1] + m[1][0] * m[0][1] * m[2][3] - m[1][0] * m[0][3] * m[2][1] - m[2][0] * m[0][1] * m[1][3] + m[2][0] * m[0][3] * m[1][1];
	inv[15] = m[0][0] * m[1][1] * m[2][2] - m[0][0] * m[1][2] * m[2][1] - m[1][0] * m[0][1] * m[2][2] + m[1][0] * m[0][2] * m[2][1] + m[2][0] * m[0][1] * m[1][2] - m[2][0] * m[0][2] * m[1][1];
	det = m[0][0] * inv[0] + m[0][1] * inv[4] + m[0][2] * inv[8] + m[0][3] * inv[12];

	if (det == 0)
		return false;

	det = 1.f / det;

	for (int i = 0; i < 16; i++)
		((float *)out.m)[i] = inv[i] * det;

	return true;
}