#ifndef __GEMATRIX_H
#define __GEMATRIX_H

#include "ge_math.h"
#include "ge_vector.h"

struct Matrix
{
	Vector off, v1, v2, v3;

	Matrix(void)
	{
		off = Vector(0.0, 0.0, 0.0);
		v1 = Vector(1.0, 0.0, 0.0);
		v2 = Vector(0.0, 1.0, 0.0);
		v3 = Vector(0.0, 0.0, 1.0);
	}
	Matrix(const Vector &off_in, const Vector &v1_in, const Vector &v2_in, const Vector &v3_in)
	{
		off = off_in;
		v1 = v1_in;
		v2 = v2_in;
		v3 = v3_in;
	}
	Matrix(_DONTCONSTRUCT v): off(v), v1(v), v2(v), v3(v) {}

	friend const Matrix operator * (const Matrix &m1, const Matrix &m2)
	{
		Matrix erg(DC);

		erg.off.x = m1.off.x + m1.v1.x * m2.off.x + m1.v2.x * m2.off.y + m1.v3.x * m2.off.z;
		erg.off.y = m1.off.y + m1.v1.y * m2.off.x + m1.v2.y * m2.off.y + m1.v3.y * m2.off.z;
		erg.off.z = m1.off.z + m1.v1.z * m2.off.x + m1.v2.z * m2.off.y + m1.v3.z * m2.off.z;

		erg.v1.x = m1.v1.x * m2.v1.x + m1.v2.x * m2.v1.y + m1.v3.x * m2.v1.z;
		erg.v1.y = m1.v1.y * m2.v1.x + m1.v2.y * m2.v1.y + m1.v3.y * m2.v1.z;
		erg.v1.z = m1.v1.z * m2.v1.x + m1.v2.z * m2.v1.y + m1.v3.z * m2.v1.z;

		erg.v2.x = m1.v1.x * m2.v2.x + m1.v2.x * m2.v2.y + m1.v3.x * m2.v2.z;
		erg.v2.y = m1.v1.y * m2.v2.x + m1.v2.y * m2.v2.y + m1.v3.y * m2.v2.z;
		erg.v2.z = m1.v1.z * m2.v2.x + m1.v2.z * m2.v2.y + m1.v3.z * m2.v2.z;

		erg.v3.x = m1.v1.x * m2.v3.x + m1.v2.x * m2.v3.y + m1.v3.x * m2.v3.z;
		erg.v3.y = m1.v1.y * m2.v3.x + m1.v2.y * m2.v3.y + m1.v3.y * m2.v3.z;
		erg.v3.z = m1.v1.z * m2.v3.x + m1.v2.z * m2.v3.y + m1.v3.z * m2.v3.z;
		return erg;
	}

	friend const Vector operator * (const Matrix &m, const Vector &v)
	{
		return Vector(m.off.x + m.v1.x*v.x + m.v2.x*v.y + m.v3.x*v.z,
		              m.off.y + m.v1.y*v.x + m.v2.y*v.y + m.v3.y*v.z,
		              m.off.z + m.v1.z*v.x + m.v2.z*v.y + m.v3.z*v.z);
	}

	friend const Vector operator ^ (const Vector &v, const Matrix &m)
	{
		return Vector(m.v1.x*v.x + m.v2.x*v.y + m.v3.x*v.z,
		              m.v1.y*v.x + m.v2.y*v.y + m.v3.y*v.z,
		              m.v1.z*v.x + m.v2.z*v.y + m.v3.z*v.z);
	}

	friend const Vector operator * (const Vector &v, const Matrix &m)
	{
		return Vector(m.off.x + m.v1.x*v.x + m.v2.x*v.y + m.v3.x*v.z,
		              m.off.y + m.v1.y*v.x + m.v2.y*v.y + m.v3.y*v.z,
		              m.off.z + m.v1.z*v.x + m.v2.z*v.y + m.v3.z*v.z);
	}

	friend const Matrix operator * (const Real s, const Matrix &m)
	{
		return Matrix(s*m.off, s*m.v1, s*m.v2, s*m.v3);
	}

	friend const Matrix operator / (const Matrix &m, const Real s)
	{
		return Matrix(m.off / s, m.v1 / s, m.v2 / s, m.v3 / s);
	}

	friend const Matrix operator + (const Matrix &m1, const Matrix &m2)
	{
		return Matrix(m1.off + m2.off, m1.v1 + m2.v1, m1.v2 + m2.v2, m1.v3 + m2.v3);
	}

	friend const Matrix operator - (const Matrix &m1, const Matrix &m2)
	{
		return Matrix(m1.off -m2.off, m1.v1 - m2.v1, m1.v2 - m2.v2, m1.v3 - m2.v3);
	}

	friend Bool operator == (const Matrix &m1, const Matrix &m2)
	{
		return ((m1.off == m2.off) && (m1.v1 == m2.v1) && (m1.v2 == m2.v2) && (m1.v3 == m2.v3));
	}

	friend Bool operator != (const Matrix &m1, const Matrix &m2)
	{
		return !(m1 == m2);
	}

	friend const Matrix operator ! (const Matrix &m)
	{
		Matrix mi(DC);

		Real det = (m.v1.x * (m.v2.y * m.v3.z - m.v3.y * m.v2.z) +
		            m.v2.x * (m.v3.y * m.v1.z - m.v1.y * m.v3.z) +
		            m.v3.x * (m.v1.y * m.v2.z - m.v2.y * m.v1.z));
		if (det == 0.0) return Matrix();

		det = 1.0 / det;
		mi.off.x = (m.v2.x * (m.off.y * m.v3.z - m.v3.y * m.off.z) +
		            m.v3.x * (m.off.z * m.v2.y - m.off.y * m.v2.z) +
		            m.off.x * (m.v3.y * m.v2.z - m.v2.y * m.v3.z)) * det;
		mi.off.y = (m.v3.x * (m.off.y * m.v1.z - m.v1.y * m.off.z) +
		            m.off.x * (m.v1.y * m.v3.z - m.v3.y * m.v1.z) +
		            m.v1.x * (m.v3.y * m.off.z - m.off.y * m.v3.z)) * det;
		mi.off.z = (m.off.x * (m.v2.y * m.v1.z - m.v1.y * m.v2.z) +
		            m.v1.x * (m.v2.z * m.off.y - m.v2.y * m.off.z) +
		            m.v2.x * (m.off.z * m.v1.y - m.off.y * m.v1.z)) * det;

		mi.v1.x = (m.v2.y * m.v3.z - m.v3.y * m.v2.z) * det;
		mi.v1.y = (m.v3.y * m.v1.z - m.v1.y * m.v3.z) * det;
		mi.v1.z = (m.v1.y * m.v2.z - m.v2.y * m.v1.z) * det;

		mi.v2.x = (m.v2.z * m.v3.x - m.v3.z * m.v2.x) * det;
		mi.v2.y = (m.v3.z * m.v1.x - m.v1.z * m.v3.x) * det;
		mi.v2.z = (m.v1.z * m.v2.x - m.v2.z * m.v1.x) * det;

		mi.v3.x = (m.v2.x * m.v3.y - m.v3.x * m.v2.y) * det;
		mi.v3.y = (m.v3.x * m.v1.y - m.v1.x * m.v3.y) * det;
		mi.v3.z = (m.v1.x * m.v2.y - m.v2.x * m.v1.y) * det;

		return mi;
	}
}; // Matrix

inline const Vector & Vector::operator *= (const Matrix &m)
{
	Real xx = m.off.x + m.v1.x * x + m.v2.x * y + m.v3.x * z;
	Real yy = m.off.y + m.v1.y * x + m.v2.y * y + m.v3.y * z;
	Real zz = m.off.z + m.v1.z * x + m.v2.z * y + m.v3.z * z;
	x = xx;
	y = yy;
	z = zz;
	return *this;
}

inline const Vector & Vector::operator ^= (const Matrix &m)
{
	Real xx = m.v1.x * x + m.v2.x * y + m.v3.x * z;
	Real yy = m.v1.y * x + m.v2.y * y + m.v3.y * z;
	Real zz = m.v1.z * x + m.v2.z * y + m.v3.z * z;
	x = xx;
	y = yy;
	z = zz;
	return *this;
}


#endif
