#include <iomanip>
#include <sstream>
#include <string.h>
#include "Matrix4.h"

namespace sp {

template<typename T>
const Matrix4<T> Matrix4<T>::Identity(
	1, 0, 0, 0,
	0, 1, 0, 0,
	0, 0, 1, 0,
	0, 0, 0, 1
);

template<typename T>
inline Matrix4<T>::Matrix4()
{
	e[0] = T(0); e[4] = T(0); e[8]  = T(0); e[12] = T(0);
	e[1] = T(0); e[5] = T(0); e[9]  = T(0); e[13] = T(0);
	e[2] = T(0); e[6] = T(0); e[10] = T(0); e[14] = T(0);
	e[3] = T(0); e[7] = T(0); e[11] = T(0); e[15] = T(0);
}

template<typename T>
inline Matrix4<T>::Matrix4(
	T a00, T a01, T a02, T a03,
	T a10, T a11, T a12, T a13,
	T a20, T a21, T a22, T a23,
	T a30, T a31, T a32, T a33)
{
	e[0] = a00; e[4] = a01; e[8]  = a02; e[12] = a03;
	e[1] = a10; e[5] = a11; e[9]  = a12; e[13] = a13;
	e[2] = a20; e[6] = a21; e[10] = a22; e[14] = a23;
	e[3] = a30; e[7] = a31; e[11] = a32; e[15] = a33;
}

template<typename T>
inline Matrix4<T>::Matrix4(T *a)
{
	// Note. array is passed as row-major
	m[0][0] = a[0];  m[1][0] = a[1];  m[2][0] = a[2];  m[3][0] = a[3];
	m[0][1] = a[4];  m[1][1] = a[5];  m[2][1] = a[6];  m[3][1] = a[7];
	m[0][2] = a[8];  m[1][2] = a[9];  m[2][2] = a[10]; m[3][2] = a[11];
	m[0][3] = a[12]; m[1][3] = a[13]; m[2][3] = a[14]; m[3][3] = a[15];
}

template<typename T>
inline Matrix4<T>& Matrix4<T>::operator=(const Matrix4<T>& mat)
{
	if (*this != mat) {
		memcpy(e, mat.e, sizeof(T) * 16);
	}
	return *this;
}

template<typename T>
Matrix4<T> Matrix4<T>::transpose() const
{
	return Matrix4<T>(
		m[0][0], m[1][0], m[2][0], m[3][0],
		m[0][1], m[1][1], m[2][1], m[3][1],
		m[0][2], m[1][2], m[2][2], m[3][2],
		m[0][3], m[1][3], m[2][3], m[3][3]);
}

template <typename T>
std::string Matrix4<T>::toString() const
{
	std::stringstream ss;

	ss << std::setprecision(2)
	   << "[ " << e[0 ] << "\t" << e[1 ] << "\t" << e[2 ] << "\t" << e[3 ] << " ]" << std::endl
	   << "| " << e[4 ] << "\t" << e[5 ] << "\t" << e[6 ] << "\t" << e[7 ] << " |" << std::endl
	   << "| " << e[8 ] << "\t" << e[9 ] << "\t" << e[10] << "\t" << e[11] << " |" << std::endl
	   << "[ " << e[12] << "\t" << e[13] << "\t" << e[14] << "\t" << e[15] << " ]" << std::endl;

	return ss.str();
}

template<typename T>
inline bool operator==(const Matrix4<T>& a, const Matrix4<T>& b)
{
	return     a.e[0 ] == b.e[0 ]
		&& a.e[1 ] == b.e[1 ]
		&& a.e[2 ] == b.e[2 ]
		&& a.e[3 ] == b.e[3 ]
		&& a.e[4 ] == b.e[4 ]
		&& a.e[5 ] == b.e[5 ]
		&& a.e[6 ] == b.e[6 ]
		&& a.e[7 ] == b.e[7 ]
		&& a.e[8 ] == b.e[8 ]
		&& a.e[9 ] == b.e[9 ]
		&& a.e[10] == b.e[10]
		&& a.e[11] == b.e[11]
		&& a.e[12] == b.e[12]
		&& a.e[13] == b.e[13]
		&& a.e[14] == b.e[14]
		&& a.e[15] == b.e[15];
}

template<typename T>
inline bool operator!=(const Matrix4<T>& a, const Matrix4<T>& b)
{
	return !(a == b);
}


// -----------------
// Arithmetic: Matrix-Matrix
// -----------------

template<typename T>
inline Matrix4<T> operator*(const Matrix4<T>& a, const Matrix4<T>& b)
{
	Matrix4<T> c;
	for(int i = 0; i < 4; i++) {
		for(int j = 0; j < 4; j++) {
			T s = T(0);
			for(int k = 0; k < 4; k++) {
				s += a.m[k][i] * b.m[j][k];
			}
			c.m[j][i] = s;
		}
	}
	return c;
}

template<typename T>
inline Matrix4<T>& operator*=(Matrix4<T>& a, const Matrix4<T>& b)
{
	a = a * b;
	return a;
}

// -----------------
// Arithmetic: Matrix-Vector
// -----------------

template <typename T>
inline Vector3<T> operator*(const Matrix4<T>& mat, const Vector3<T>& vec)
{
	Vector3<T> r;
	r.x = mat.m[0][0] * vec.x + mat.m[1][0] * vec.y + mat[2][0] * vec.z;
	r.y = mat.m[0][1] * vec.x + mat.m[1][1] * vec.y + mat[2][1] * vec.z;
	r.z = mat.m[0][2] * vec.x + mat.m[1][2] * vec.y + mat[2][2] * vec.z;
	return r;
}


template <typename T>
inline std::ostream& operator<<(std::ostream &s, const Matrix4<T>& mat)
{
	return s << mat.toString();
}

} // namespace sp