#include <Spectre/Math/Math.h>
#include <Spectre/Math/Transform.h>

Transform::Transform() :
m_matrix	(Matrix4f::Identity)
{
}

Transform::Transform(	float a00, float a01, float a02,
			float a10, float a11, float a12,
			float a20, float a21, float a22) :
m_matrix		(a00,  a01,  0.0f, a02,
			 a10,  a11,  0.0f, a12,
			 0.0f, 0.0f, 1.0f, 0.0f,
			 a20,  a21,  0.0f, a22)
{
}

Transform::Transform(const Matrix4f& matrix) :
m_matrix	(matrix)
{
}

void Transform::set(Vector2f _translate, float _rotate, Vector2f _scale)
{
	reset();
	translate(_translate).rotate(_rotate).scale(_scale);
}

void Transform::setMatrix(const Matrix4f& matrix)
{
	m_matrix = matrix;
}

void Transform::reset()
{
	m_matrix = Matrix4f::Identity;
}

Vector2f Transform::getUpVector() const
{
	return Vector2f(m_matrix.e[4], m_matrix.e[5]).normalize();
}

Vector2f Transform::getRightVector() const
{
	return Vector2f(m_matrix.e[0], m_matrix.e[1]).normalize();
}

Transform& Transform::translate(float x, float y)
{
	Transform t(	1, 0, x,
			0, 1, y,
			0, 0, 1);
	return multiply(t);
}

Transform& Transform::translate(Vector2f vec)
{
	return translate(vec.x, vec.y);
}

Transform& Transform::rotate(float theta)
{
	float r = math::deg2rad(theta);
	float c = std::cos(r);
	float s = std::sin(r);

	// Always rotate along z-axis in 2D.
	Transform rot(  c, -s, 0,
			s,  c, 0,
			0,  0, 1);

	return multiply(rot);
}

Transform& Transform::scale(float x, float y)
{
	Transform s(	x, 0, 0,
			0, y, 0,
			0, 0, 1);
	return multiply(s);
}

Transform& Transform::scale(Vector2f vec)
{
	return scale(vec.x, vec.y);
}

Transform& Transform::scale(float s)
{
	return scale(s, s);
}

Transform& Transform::multiply(const Transform& other)
{
	m_matrix *= other.m_matrix;
	return *this;
}

Vector2f Transform::transformPoint(float x, float y) const
{
	return Vector2f(m_matrix.e[0] * x + m_matrix.e[4] * y + m_matrix.e[12],
			m_matrix.e[1] * x + m_matrix.e[5] * y + m_matrix.e[13]);
}

Vector2f Transform::transformPoint(const Vector2f& vec) const
{
	return transformPoint(vec.x, vec.y);
}

Matrix4f& Transform::getMatrix()
{
	return m_matrix;
}

const Matrix4f& Transform::getMatrix() const
{
	return m_matrix;
}


float* Transform::getRawMatrix()
{
	return m_matrix.e;
}

const float* Transform::getRawMatrix() const
{
	return m_matrix.e;
}

Matrix4f Transform::getRotationMatrix() const
{
	return Matrix4f(
		m_matrix.e[0], m_matrix.e[4], 0.0f, 0.0f,
		m_matrix.e[1], m_matrix.e[5], 0.0f, 0.0f,
		0.0f,		0.0f,		1.0f, 0.0f,
		0.0f,		0.0f,		0.0f, 1.0f);
}


Transform operator*(const Transform& a, const Transform& b)
{
	return Transform(a.getMatrix() * b.getMatrix());
}

Transform& operator*=(Transform& a, const Transform& other)
{
	a.multiply(other);
	return a;
}

Vector2f operator*(const Transform& transform, const Vector2f& vec) 
{
	return transform.transformPoint(vec);
}