#ifndef BLUECORE_MATRIX_H
#define BLUECORE_MATRIX_H

// system includes
#include <algorithm>
#include <cmath>

// local includes
#include "Scalar.h"
#include "Vector.h"
#include "Quaternion.h"

namespace BlueCore
{
template<class T> class Matrix3x3Template
{
public:
	T m[9];

	inline Matrix3x3Template()
	{
		identity();
	}

	template<class S> inline Matrix3x3Template(const S a[9])
	{
		m[0] = static_cast<T>(a[0]);
		m[1] = static_cast<T>(a[1]);
		m[2] = static_cast<T>(a[2]);

		m[3] = static_cast<T>(a[3]);
		m[4] = static_cast<T>(a[4]);
		m[5] = static_cast<T>(a[5]);

		m[6] = static_cast<T>(a[6]);
		m[7] = static_cast<T>(a[7]);
		m[8] = static_cast<T>(a[8]);
	}

	template<class S> inline Matrix3x3Template(const Matrix3x3Template<S> &a)
	{
		m[0] = static_cast<T>(a.m[0]);
		m[1] = static_cast<T>(a.m[1]);
		m[2] = static_cast<T>(a.m[2]);

		m[3] = static_cast<T>(a.m[3]);
		m[4] = static_cast<T>(a.m[4]);
		m[5] = static_cast<T>(a.m[5]);

		m[6] = static_cast<T>(a.m[6]);
		m[7] = static_cast<T>(a.m[7]);
		m[8] = static_cast<T>(a.m[8]);
	}

	inline Matrix3x3Template(const Vector3Template<T> &h,
			const Vector3Template<T> &u)
	{
		Vector3Template<T> a, b, c;
		c = h.normalized();
		a = h.cross( u ).normalized();
		b = c.cross( a ).normalized();

		m[0] = a.x;
		m[1] = b.x;
		m[2] = c.x;

		m[3] = a.y;
		m[4] = b.y;
		m[5] = c.y;

		m[6] = a.z;
		m[7] = b.z;
		m[8] = c.z;
	}

	inline Matrix3x3Template(T a, T e, T t)
	{
		T ch = cos(a);
		T sh = sin(a);
		T ca = cos(e);
		T sa = sin(e);
		T cb = cos(t);
		T sb = sin(t);

		m[0] = ch * ca;
		m[1] = sh*sb - ch*sa*cb;
		m[2] = ch*sa*sb + sh*cb;

		m[3] = sa;
		m[4] = ca*cb;
		m[5] = -ca*sb;

		m[6] = -sh*ca;
		m[7] = sh*sa*cb + ch*sb;
		m[8] = -sh*sa*sb + ch*cb;
	}

	template<class S> inline Matrix3x3Template(const QuaternionTemplate<S> &q)
	{
		T xx = static_cast<T>(q.x*q.x), xy = static_cast<T>(q.x*q.y), xz =
				static_cast<T>(q.x*q.z), xw = static_cast<T>(q.x*q.w),

		yy = static_cast<T>(q.y*q.y), yz = static_cast<T>(q.y*q.z), yw =
				static_cast<T>(q.y*q.w),

		zz = static_cast<T>(q.z*q.z), zw = static_cast<T>(q.z*q.w);

		m[0] = 1 - yy - yy - zz - zz;
		m[1] = xy + xy + zw + zw;
		m[2] = xz + xz - yw - yw;

		m[3] = xy + xy - zw - zw;
		m[4] = 1 - xx - xx - zz - zz;
		m[5] = yz + yz + xw + xw;

		m[6] = xz + xz + yw + yw;
		m[7] = yz + yz - xw - xw;
		m[8] = 1 - xx - xx - yy - yy;
	}

	void set(size_t i, Vector3Template<T>& v)
	{
		m[i] = v.x;
		m[i+3] = v.y;
		m[i+6] = v.z;
	}

	inline void identity()
	{
		m[0] = static_cast<T>(1);
		m[1] = static_cast<T>(0);
		m[2] = static_cast<T>(0);

		m[3] = static_cast<T>(0);
		m[4] = static_cast<T>(1);
		m[5] = static_cast<T>(0);

		m[6] = static_cast<T>(0);
		m[7] = static_cast<T>(0);
		m[8] = static_cast<T>(1);
	}

	inline Matrix3x3Template<T> transposed() const
	{
		Matrix3x3Template<T> a;

		a.m[0] = m[0];
		a.m[3] = m[1];
		a.m[6] = m[2];
		a.m[1] = m[3];
		a.m[4] = m[4];
		a.m[7] = m[5];
		a.m[2] = m[6];
		a.m[5] = m[7];
		a.m[8] = m[8];

		return a;
	}

	inline void transpose()
	{
		swap(m[3], m[1]);
		swap(m[6], m[2]);
		swap(m[7], m[5]);
	}

	inline T determinant() const
	{
		return (m[0]*m[4]*m[8] + m[1]*m[5]*m[6] + m[2]*m[3]*m[7] ) - (m[6]*m[4]
				*m[2] + m[7]*m[5]*m[0] + m[8]*m[3]*m[1] );
	}

	inline Matrix3x3Template<T> inverted()
	{
		T det = determinant();
		T one_over_det = 1.0f / det;

		Matrix3x3Template<T> result;
		result.m[0] = +(m[4] * m[8] - m[5] * m[7]) * one_over_det;
		result.m[1] = -(m[1] * m[8] - m[2] * m[7]) * one_over_det;
		result.m[2] = +(m[1] * m[5] - m[2] * m[4]) * one_over_det;
		result.m[3] = -(m[3] * m[8] - m[5] * m[6]) * one_over_det;
		result.m[4] = +(m[0] * m[8] - m[2] * m[6]) * one_over_det;
		result.m[5] = -(m[0] * m[5] - m[2] * m[3]) * one_over_det;
		result.m[6] = +(m[3] * m[7] - m[4] * m[6]) * one_over_det;
		result.m[7] = -(m[0] * m[7] - m[1] * m[6]) * one_over_det;
		result.m[8] = +(m[0] * m[4] - m[1] * m[3]) * one_over_det;

		return result;
	}

	template<class S> inline Matrix3x3Template<T> &operator +=(
			const Matrix3x3Template<S> &a)
	{
		m[0] += static_cast<T>(a.m[0]);
		m[1] += static_cast<T>(a.m[1]);
		m[2] += static_cast<T>(a.m[2]);

		m[3] += static_cast<T>(a.m[3]);
		m[4] += static_cast<T>(a.m[4]);
		m[5] += static_cast<T>(a.m[5]);

		m[6] += static_cast<T>(a.m[6]);
		m[7] += static_cast<T>(a.m[7]);
		m[8] += static_cast<T>(a.m[8]);

		return *this;
	}

	template<class S> inline Matrix3x3Template<T> operator *(
			const Matrix3x3Template<S> &a)
	{
		Matrix3x3Template<T> result;

		result.m[0] = m[0] * static_cast<T>(a.m[0]) + m[3]
				* static_cast<T>(a.m[1]) + m[6] * static_cast<T>(a.m[2]);

		result.m[1] = m[1] * static_cast<T>(a.m[0]) + m[4]
				* static_cast<T>(a.m[1]) + m[7] * static_cast<T>(a.m[2]);

		result.m[2] = m[2] * static_cast<T>(a.m[0]) + m[5]
				* static_cast<T>(a.m[1]) + m[8] * static_cast<T>(a.m[2]);

		result.m[3] = m[0] * static_cast<T>(a.m[3]) + m[3]
				* static_cast<T>(a.m[4]) + m[6] * static_cast<T>(a.m[5]);

		result.m[4] = m[1] * static_cast<T>(a.m[3]) + m[4]
				* static_cast<T>(a.m[4]) + m[7] * static_cast<T>(a.m[5]);

		result.m[5] = m[2] * static_cast<T>(a.m[3]) + m[5]
				* static_cast<T>(a.m[4]) + m[8] * static_cast<T>(a.m[5]);

		result.m[6] = m[0] * static_cast<T>(a.m[6]) + m[3]
				* static_cast<T>(a.m[7]) + m[6] * static_cast<T>(a.m[8]);

		result.m[7] = m[1] * static_cast<T>(a.m[6]) + m[4]
				* static_cast<T>(a.m[7]) + m[7] * static_cast<T>(a.m[8]);

		result.m[8] = m[2] * static_cast<T>(a.m[6]) + m[5]
				* static_cast<T>(a.m[7]) + m[8] * static_cast<T>(a.m[8]);

		return result;
	}

	template<class S> Vector3Template<T> operator *(const Vector3Template<S>& a)
	{
		return Vector3Template<T>(m[0] * static_cast<T>(a.x) + m[3]
				* static_cast<T>(a.y) + m[6] * static_cast<T>(a.z), m[1]
				* static_cast<T>(a.x) + m[4] * static_cast<T>(a.y) + m[7]
				* static_cast<T>(a.z), m[2] * static_cast<T>(a.x) + m[5]
				* static_cast<T>(a.y) + m[8] * static_cast<T>(a.z) );
	}

	template<class S> Matrix3x3Template<T> operator *(const S a)
	{
		Matrix3x3Template<T> result;

		result.m[0] = m[0] * static_cast<T>(a);
		result.m[1] = m[1] * static_cast<T>(a);
		result.m[2] = m[2] * static_cast<T>(a);
		result.m[3] = m[3] * static_cast<T>(a);
		result.m[4] = m[4] * static_cast<T>(a);
		result.m[5] = m[5] * static_cast<T>(a);
		result.m[6] = m[6] * static_cast<T>(a);
		result.m[7] = m[7] * static_cast<T>(a);
		result.m[8] = m[8] * static_cast<T>(a);

		return result;
	}

	friend std::ostream &operator <<(std::ostream& os, Matrix3x3Template<T> m)
	{
		os << "( " << m.m[0] << ", " << m.m[1] << ", " << m.m[2] << " )"
				<< std::endl;
		os << "( " << m.m[3] << ", " << m.m[4] << ", " << m.m[5] << " )"
				<< std::endl;
		os << "( " << m.m[6] << ", " << m.m[7] << ", " << m.m[8] << " )"
				<< std::endl;
		return os;
	}

	QuaternionTemplate<T> toQuaternion() const
	{
		// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
		// article "Quaternion Calculus and Fast Animation".

		T fTrace = m[0]+m[4]+m[8];
		T fRoot;
		QuaternionTemplate<T> q;

		if (fTrace > 0.0)
		{
			// |w| > 1/2, may as well choose w > 1/2
			fRoot = std::sqrt(fTrace + 1.0); // 2w
			q.w = 0.5*fRoot;
			fRoot = 0.5/fRoot; // 1/(4w)
			q.x = (m[7]-m[5])*fRoot;
			q.y = (m[2]-m[6])*fRoot;
			q.z = (m[3]-m[1])*fRoot;
		}
		else
		{
			// |w| <= 1/2
			static size_t s_iNext[3] =
			{ 1, 2, 0 };
			size_t i = 0;
			if (m[4] > m[0])
				i = 1;
			if (m[8] > m[i*3+i])
				i = 2;
			size_t j = s_iNext[i];
			size_t k = s_iNext[j];

			fRoot = std::sqrt(m[i*3+i]-m[j*3+j]-m[k*k+1] + 1.0);
			T* apkQuat[3] =
			{ &q.x, &q.y, &q.z };
			*apkQuat[i] = 0.5*fRoot;
			fRoot = 0.5/fRoot;
			q.w = (m[k*3+j]-m[j*3+k])*fRoot;
			*apkQuat[j] = (m[j*3+i]+m[i*3+j])*fRoot;
			*apkQuat[k] = (m[k*3+i]+m[i*3+k])*fRoot;
		}

		return q;
	}
};

typedef Matrix3x3Template<float> Matrix3x3Float;
typedef Matrix3x3Template<double> Matrix3x3Double;
typedef Matrix3x3Template<Scalar> Matrix3x3;

template<class T> class Matrix4x4Template
{

public:

	T m[16];

	inline Matrix4x4Template()
	{
		identity();
	}

	inline void identity()
	{
		m[0] = static_cast<T>(1);
		m[1] = static_cast<T>(0);
		m[2] = static_cast<T>(0);
		m[3] = static_cast<T>(0);

		m[4] = static_cast<T>(0);
		m[5] = static_cast<T>(1);
		m[6] = static_cast<T>(0);
		m[7] = static_cast<T>(0);

		m[8] = static_cast<T>(0);
		m[9] = static_cast<T>(0);
		m[10] = static_cast<T>(1);
		m[11] = static_cast<T>(0);

		m[12] = static_cast<T>(0);
		m[13] = static_cast<T>(0);
		m[14] = static_cast<T>(0);
		m[15] = static_cast<T>(1);
	}

	inline Matrix4x4Template(const T a[16])
	{
		m[0] = a[0];
		m[1] = a[1];
		m[2] = a[2];
		m[3] = a[3];
		m[4] = a[4];
		m[5] = a[5];
		m[6] = a[6];
		m[7] = a[7];
		m[8] = a[8];
		m[9] = a[9];
		m[10] = a[10];
		m[11] = a[11];
		m[12] = a[12];
		m[13] = a[13];
		m[14] = a[14];
		m[15] = a[15];
	}

	inline Matrix4x4Template(const Matrix3x3Template<T> &a)
	{
		m[0] = a[0];
		m[1] = a[1];
		m[2] = a[2];
		m[3] = static_cast<T>(0);

		m[4] = a[3];
		m[5] = a[4];
		m[6] = a[5];
		m[7] = static_cast<T>(0);

		m[8] = a[6];
		m[9] = a[7];
		m[10] = a[8];
		m[11] = static_cast<T>(0);

		m[12] = static_cast<T>(0);
		m[13] = static_cast<T>(0);
		m[14] = static_cast<T>(0);
		m[15] = static_cast<T>(1);
	}

	inline Matrix4x4Template(const Matrix3x3Template<T> &a,
			const Vector3Template<T> &b)
	{
		m[0] = a[0];
		m[1] = a[1];
		m[2] = a[2];
		m[3] = static_cast<T>(0);

		m[4] = a[3];
		m[5] = a[4];
		m[6] = a[5];
		m[7] = static_cast<T>(0);

		m[8] = a[6];
		m[9] = a[7];
		m[10] = a[8];
		m[11] = static_cast<T>(0);

		m[12] = b.x;
		m[13] = b.y;
		m[14] = b.z;
		m[15] = static_cast<T>(1);
	}

	inline Matrix4x4Template(const Matrix4x4Template<T> &a)
	{
		m[0] = a.m[0];
		m[1] = a.m[1];
		m[2] = a.m[2];
		m[3] = a.m[3];

		m[4] = a.m[4];
		m[5] = a.m[5];
		m[6] = a.m[6];
		m[7] = a.m[7];

		m[8] = a.m[8];
		m[9] = a.m[9];
		m[10] = a.m[10];
		m[11] = a.m[11];

		m[12] = a.m[12];
		m[13] = a.m[13];
		m[14] = a.m[14];
		m[15] = a.m[15];
	}

	inline Matrix4x4Template(const QuaternionTemplate<T> &q)
	{
		m[0] = 1 - 2*q.y*q.y - 2*q.z*q.z;
		m[1] = 2*q.x*q.y - 2*q.z*q.w;
		m[2] = 2*q.x*q.z + 2*q.y*q.w;
		m[3] = static_cast<T>(0);

		m[4] = 2*q.x*q.y + 2*q.z*q.w;
		m[5] = 1 - 2*q.x*q.x - 2*q.z*q.z;
		m[6] = 2*q.y*q.z - 2*q.x*q.w;
		m[7] = static_cast<T>(0);

		m[8] = 2*q.x*q.z - 2*q.y*q.w;
		m[9] = 2*q.y*q.z + 2*q.x*q.w;
		m[10] = 1 - 2*q.x*q.x - 2*q.y*q.y;
		m[11] = static_cast<T>(0);

		m[12] = static_cast<T>(0);
		m[13] = static_cast<T>(0);
		m[14] = static_cast<T>(0);
		m[15] = static_cast<T>(1);
	}

	inline Matrix4x4Template(const QuaternionTemplate<T> &q,
			const Vector3Template<T> &a)
	{
		m[0] = 1 - 2*q.y*q.y - 2*q.z*q.z;
		m[1] = 2*q.x*q.y - 2*q.z*q.w;
		m[2] = 2*q.x*q.z + 2*q.y*q.w;
		m[3] = static_cast<T>(0);

		m[4] = 2*q.x*q.y + 2*q.z*q.w;
		m[5] = 1 - 2*q.x*q.x - 2*q.z*q.z;
		m[6] = 2*q.y*q.z - 2*q.x*q.w;
		m[7] = static_cast<T>(0);

		m[8] = 2*q.x*q.z - 2*q.y*q.w;
		m[9] = 2*q.y*q.z + 2*q.x*q.w;
		m[10] = 1 - 2*q.x*q.x - 2*q.y*q.y;
		m[11] = static_cast<T>(0);

		m[12] = a.x;
		m[13] = a.y;
		m[14] = a.z;
		m[15] = static_cast<T>(1);
	}

	inline Matrix4x4Template(const Vector3Template<T> &h,
			const Vector3Template<T> &u, const Vector3Template<T> &t)
	{
		Vector3Template<T> a, b, c;
		c = h.unit();
		a = h.crossProduct( u ).unit();
		b = c.crossProduct( a ).unit();

		m[0] = a.x;
		m[1] = b.x;
		m[2] = c.x;
		m[3] = static_cast<T>(0);

		m[4] = a.y;
		m[5] = b.y;
		m[6] = c.y;
		m[7] = static_cast<T>(0);

		m[8] = a.z;
		m[9] = b.z;
		m[10] = c.z;
		m[11] = static_cast<T>(0);

		m[12] = t.x;
		m[13] = t.y;
		m[14] = t.z;
		m[15] = static_cast<T>(1);
	}

	inline Matrix4x4Template(T a, T e, T t, const Vector3Template<T> &tr)
	{
		T ch = cos(a);
		T sh = sin(a);
		T ca = cos(e);
		T sa = sin(e);
		T cb = cos(t);
		T sb = sin(t);

		m[0] = ch * ca;
		m[1] = sh*sb - ch*sa*cb;
		m[2] = ch*sa*sb + sh*cb;
		m[3] = static_cast<T>(0);

		m[4] = sa;
		m[5] = ca*cb;
		m[6] = -ca*sb;
		m[7] = static_cast<T>(0);

		m[8] = -sh*ca;
		m[9] = sh*sa*cb + ch*sb;
		m[10] = -sh*sa*sb + ch*cb;
		m[11] = static_cast<T>(0);

		m[12] = tr.x;
		m[13] = tr.y;
		m[14] = tr.z;
		m[15] = static_cast<T>(1);
	}

	inline void transpose()
	{
		std::swap(m[4], m[1]);
		std::swap(m[8], m[2]);
		std::swap(m[12], m[3]);

		std::swap(m[9], m[6]);
		std::swap(m[13], m[7]);
		std::swap(m[14], m[11]);
	}

	inline Matrix4x4Template<T> transposed()
	{
		Matrix4x4Template<T> a;

		a.m[0] = m[0];
		a.m[4] = m[1];
		a.m[8] = m[2];
		a.m[12] = m[3];
		a.m[1] = m[4];
		a.m[5] = m[5];
		a.m[9] = m[6];
		a.m[13] = m[7];
		a.m[2] = m[8];
		a.m[6] = m[9];
		a.m[10] = m[10];
		a.m[14] = m[11];
		a.m[3] = m[12];
		a.m[7] = m[13];
		a.m[11] = m[14];
		a.m[15] = m[15];

		return a;
	}

	template<class S> inline Matrix4x4Template<T> operator *(
			const Matrix4x4Template<S> &a)
	{
		Matrix4x4Template<T> result;

		result.m[0] = m[0] * static_cast<T>(a.m[0]) + m[4]
				* static_cast<T>(a.m[1]) + m[8] * static_cast<T>(a.m[2])
				+ m[12] * static_cast<T>(a.m[3]);

		result.m[1] = m[1] * static_cast<T>(a.m[0]) + m[5]
				* static_cast<T>(a.m[1]) + m[9] * static_cast<T>(a.m[2])
				+ m[13] * static_cast<T>(a.m[3]);

		result.m[2] = m[2] * static_cast<T>(a.m[0]) + m[6]
				* static_cast<T>(a.m[1]) + m[10] * static_cast<T>(a.m[2])
				+ m[14] * static_cast<T>(a.m[3]);

		result.m[3] = m[3] * static_cast<T>(a.m[0]) + m[7]
				* static_cast<T>(a.m[1]) + m[11] * static_cast<T>(a.m[2])
				+ m[15] * static_cast<T>(a.m[3]);

		result.m[4] = m[0] * static_cast<T>(a.m[4]) + m[4]
				* static_cast<T>(a.m[5]) + m[8] * static_cast<T>(a.m[6])
				+ m[12] * static_cast<T>(a.m[7]);

		result.m[5] = m[1] * static_cast<T>(a.m[4]) + m[5]
				* static_cast<T>(a.m[5]) + m[9] * static_cast<T>(a.m[6])
				+ m[13] * static_cast<T>(a.m[7]);

		result.m[6] = m[2] * static_cast<T>(a.m[4]) + m[6]
				* static_cast<T>(a.m[5]) + m[10] * static_cast<T>(a.m[6])
				+ m[14] * static_cast<T>(a.m[7]);

		result.m[7] = m[3] * static_cast<T>(a.m[4]) + m[7]
				* static_cast<T>(a.m[5]) + m[11] * static_cast<T>(a.m[6])
				+ m[15] * static_cast<T>(a.m[7]);

		result.m[8] = m[0] * static_cast<T>(a.m[8]) + m[4]
				* static_cast<T>(a.m[9]) + m[8] * static_cast<T>(a.m[10])
				+ m[12] * static_cast<T>(a.m[11]);

		result.m[9] = m[1] * static_cast<T>(a.m[8]) + m[5]
				* static_cast<T>(a.m[9]) + m[9] * static_cast<T>(a.m[10])
				+ m[13] * static_cast<T>(a.m[11]);

		result.m[10] = m[2] * static_cast<T>(a.m[8]) + m[6]
				* static_cast<T>(a.m[9]) + m[10] * static_cast<T>(a.m[10])
				+ m[14] * static_cast<T>(a.m[11]);

		result.m[11] = m[3] * static_cast<T>(a.m[8]) + m[7]
				* static_cast<T>(a.m[9]) + m[11] * static_cast<T>(a.m[10])
				+ m[15] * static_cast<T>(a.m[11]);

		result.m[12] = m[0] * static_cast<T>(a.m[12]) + m[4]
				* static_cast<T>(a.m[13]) + m[8] * static_cast<T>(a.m[14])
				+ m[12] * static_cast<T>(a.m[15]);

		result.m[13] = m[1] * static_cast<T>(a.m[12]) + m[5]
				* static_cast<T>(a.m[13]) + m[9] * static_cast<T>(a.m[14])
				+ m[13] * static_cast<T>(a.m[15]);

		result.m[14] = m[2] * static_cast<T>(a.m[12]) + m[6]
				* static_cast<T>(a.m[13]) + m[10] * static_cast<T>(a.m[14])
				+ m[14] * static_cast<T>(a.m[15]);

		result.m[15] = m[3] * static_cast<T>(a.m[12]) + m[7]
				* static_cast<T>(a.m[13]) + m[11] * static_cast<T>(a.m[14])
				+ m[15] * static_cast<T>(a.m[15]);

		return result;
	}

	Vector3Template<T> operator *(const Vector3Template<T> &a)
	{
		Vector3Template<T> result;

		result.x = m[0] * a.x + m[4] * a.y + m[8] * a.z + m[12];
		result.y = m[1] * a.x + m[5] * a.y + m[9] * a.z + m[13];
		result.z = m[2] * a.x + m[6] * a.y + m[11] * a.z + m[14];

		return result;
	}

	T *data()
	{
		return &m[0];
	}
};

typedef Matrix4x4Template<float> Matrix4x4Float;
typedef Matrix4x4Template<double> Matrix4x4Double;
typedef Matrix4x4Template<Scalar> Matrix4x4;

} // namespace bc

#endif // BLUECORE_MATRIX_H