#include "vector3.h"
#include "quaternion.h"

namespace stingray_plugin_foundation {

// ----------------------------------------------------------------------
// Matrix3x3
// ----------------------------------------------------------------------

inline Matrix3x3 matrix3x3_identity()
{
	Matrix3x3 m;
	set_to_identity(m);
	return m;
}

inline void set_to_identity(Matrix3x3 &m)
{
	m.x = vector3(1,0,0);
	m.y = vector3(0,1,0);
	m.z = vector3(0,0,1);
}

inline Matrix3x3 operator*(const Matrix3x3 &a, const Matrix3x3 &b)
{
	Matrix3x3 m;

	m.x.x = a.x.x*b.x.x + a.x.y*b.y.x + a.x.z*b.z.x;
	m.x.y = a.x.x*b.x.y + a.x.y*b.y.y + a.x.z*b.z.y;
	m.x.z = a.x.x*b.x.z + a.x.y*b.y.z + a.x.z*b.z.z;

	m.y.x = a.y.x*b.x.x + a.y.y*b.y.x + a.y.z*b.z.x;
	m.y.y = a.y.x*b.x.y + a.y.y*b.y.y + a.y.z*b.z.y;
	m.y.z = a.y.x*b.x.z + a.y.y*b.y.z + a.y.z*b.z.z;

	m.z.x = a.z.x*b.x.x + a.z.y*b.y.x + a.z.z*b.z.x;
	m.z.y = a.z.x*b.x.y + a.z.y*b.y.y + a.z.z*b.z.y;
	m.z.z = a.z.x*b.x.z + a.z.y*b.y.z + a.z.z*b.z.z;

	return m;
}

inline Matrix3x3 matrix3x3(const Quaternion &q)
{
	const float	d = dot(q,q);
	float s = d ? 2.0f / d : 1.0f;
	const float	xs = q.x * s;
	const float	ys = q.y * s;
	const float	zs = q.z * s;
	const float wx = q.w * xs;
	const float wy = q.w * ys;
	const float wz = q.w * zs;
	const float xx = q.x * xs;
	const float xy = q.x * ys;
	const float xz = q.x * zs;
	const float yy = q.y * ys;
	const float yz = q.y * zs;
	const float zz = q.z * zs;

	Matrix3x3 m;
	m.x = vector3((1.0f - yy - zz), (xy + wz), (xz - wy));
	m.y = vector3((xy - wz), (1.0f - xx - zz), (yz + wx));
	m.z = vector3((xz + wy), (yz - wx), (1.0f - xx - yy));
	return m;
}

inline Matrix3x3 matrix3x3(const Matrix4x4 &tm)
{
	Matrix3x3 m;
	m.x = x_axis(tm);
	m.y = y_axis(tm);
	m.z = z_axis(tm);
	return m;
}

// ----------------------------------------------------------------------
// LocalTransform
// ----------------------------------------------------------------------

inline LocalTransform local_transform(const Matrix3x3 &rot, const Vector3 &pos)
{
	LocalTransform tm;
	tm.rot = rot;
	tm.pos = pos;
	tm.scale = vector3(1,1,1);
	tm.dummy = 0.0f;
	return tm;
}

inline LocalTransform local_transform(const Matrix3x3 &rot, const Vector3 &pos, const Vector3 &scale)
{
	LocalTransform tm;
	tm.rot = rot;
	tm.pos = pos;
	tm.scale = scale;
	tm.dummy = 0.0f;
	return tm;
}

inline LocalTransform local_transform_identity()
{
	LocalTransform tm;
	set_to_identity(tm);
	return tm;
}

inline void set_to_identity(LocalTransform &tm)
{
	set_to_identity(tm.rot);
	tm.pos = vector3(0,0,0);
	tm.scale = vector3(1,1,1);
	tm.dummy = 0.0f;
}

inline Vector3 transform(const LocalTransform &tm, const Vector3 &p)
{
	return tm.pos + p.x*tm.rot.x*tm.scale.x + p.y*tm.rot.y*tm.scale.y + p.z*tm.rot.z*tm.scale.z;
}

inline Vector3 transform_without_translation(const LocalTransform &tm, const Vector3 &p)
{
	return p.x*tm.rot.x*tm.scale.x + p.y*tm.rot.y*tm.scale.y + p.z*tm.rot.z*tm.scale.z;
}

inline LocalTransform operator*(const LocalTransform &a, const LocalTransform &b)
{
	Matrix3x3 a_rs = a.rot;
	Matrix3x3 b_rs = b.rot;

	a_rs.x = a_rs.x * a.scale.x;
	a_rs.y = a_rs.y * a.scale.y;
	a_rs.z = a_rs.z * a.scale.z;

	b_rs.x = b_rs.x * b.scale.x;
	b_rs.y = b_rs.y * b.scale.y;
	b_rs.z = b_rs.z * b.scale.z;

	LocalTransform res;
	res.rot = a_rs * b_rs;
	res.scale = vector3(length(res.rot.x), length(res.rot.y), length(res.rot.z));
	res.rot.x /= res.scale.x; res.rot.y /= res.scale.y; res.rot.z /= res.scale.z;
	res.pos = transform(b, a.pos);
	return res;
}

inline void operator*=(LocalTransform &a, const LocalTransform &b)
{
	a = a*b;
}

inline LocalTransform relative(LocalTransform &child, const LocalTransform &parent)
{
	Matrix4x4 p4i = inverse(matrix4x4(parent));
	Matrix4x4 rm = matrix4x4(child) * p4i;
	return local_transform(rm);
}

inline LocalTransform local_transform(const Matrix4x4 &m4)
{
	LocalTransform tm;
	tm.rot.x = x_axis(m4);
	tm.rot.y = y_axis(m4);
	tm.rot.z = z_axis(m4);

	tm.scale = vector3( length(tm.rot.x), length(tm.rot.y), length(tm.rot.z) );
	if (tm.scale.x && tm.scale.y && tm.scale.z) {
		tm.rot.x /= tm.scale.x;
		tm.rot.y /= tm.scale.y;
		tm.rot.z /= tm.scale.z;
	} else
		tm.rot = matrix3x3_identity();

	tm.pos = translation(m4);
	tm.dummy = 0.0f;

	return tm;
}

inline Matrix4x4 matrix4x4(const LocalTransform &tm)
{
	Matrix4x4 m4 = matrix4x4_identity();
	x_axis(m4) = tm.rot.x * tm.scale.x;
	y_axis(m4) = tm.rot.y * tm.scale.y;
	z_axis(m4) = tm.rot.z * tm.scale.z;
	translation(m4) = tm.pos;
	return m4;
}

}