#ifndef __M_MATRIX3_H
#define __M_MATRIX3_H

#include "maths_defs.h"
#include "Vector3.h"


class Matrix3
{
public:
	real m11,m12,m13;
	real m21,m22,m23;
	real m31,m32,m33;

public:

	Matrix3()
		: m11(0), m12(0), m13(0),
		  m21(0), m22(0), m23(0),
		  m31(0), m32(0), m33(0)
	{};

	Matrix3 (real am11, real am12, real am13,
				  real am21, real am22, real am23,
				  real am31, real am32, real am33)
	:	m11(am11), m12(am12), m13(am13),
		m21(am21), m22(am22), m23(am23),
		m31(am31), m32(am32), m33(am33)
		{}
		
	Matrix3 (real *M)
	{
	  for (short i=0;i<9;i++)
	  {
		  *((&m11)+i) = *(M + i);
	  }	    
	}

	Matrix3(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
		:	m11(v1.x), m12(v2.x), m13(v3.x),
			m21(v1.y), m22(v2.y), m23(v3.y),
			m31(v1.z), m32(v2.z), m33(v3.z)
		{}
	
	Matrix3(real diag_1, real diag_2, real diag_3)
		: m11(diag_1), m12(0), m13(0),
		  m21(0), m22(diag_2), m23(0),
		  m31(0), m32(0), m33(diag_3)
	{};


    void		Set (real m11, real m12, real m13,
			     	 real m21, real m22, real m23,
			     	 real m31, real m32, real m33);

	void		Identity();
	void		Nulle();
	Vector3	    Diag() const;

	void		Diag2(real &diag_1, real &diag_2, real &diag_3);

	FORCEINLINE Vector3	Row1() const { return Vector3 (m11,m12,m13); }
	FORCEINLINE Vector3	Row2() const { return Vector3 (m21,m22,m23); }
	FORCEINLINE Vector3	Row3() const { return Vector3 (m31,m32,m33); }
	 
	FORCEINLINE Vector3	Col1() const { return Vector3 (m11,m21,m31); }
	FORCEINLINE Vector3	Col2() const { return Vector3 (m12,m22,m32); }
	FORCEINLINE Vector3	Col3() const { return Vector3 (m13,m23,m33); }


  FORCEINLINE bool operator== (const Matrix3& m) const
  {
	  if (this == &m)
		return true;
	  for (short i=0;i<9;i++)
	  {
		  if (*((&m11)+i) != *((&m.m11)+i))
			  return false;
	  }
	  return true;
  }
/*
 j'ai benché ces deux version et la deuxième apparaît un poil plus rapide 
 surtout quand la matrice est largement différente

  FORCEINLINE bool equal(const Matrix3& m) const
  {
	  return (Matrix3::m11 == m.m11 && Matrix3::m12 == m.m12 && Matrix3::m13 == m.m13 && 
				 Matrix3::m21 == m.m21 && Matrix3::m22 == m.m22 && Matrix3::m23 == m.m23 &&
				 Matrix3::m31 == m.m31 && Matrix3::m32 == m.m32 && Matrix3::m33 == m.m33);
  }

  FORCEINLINE bool equal_b(const Matrix3& m) const
  {
	  for (short i=0;i<9;i++)
	  {
		  if (*((&m11)+i) != *((&m.m11)+i))
			  return false;
	  }
	  return true;
  }
*/

  Matrix3 operator+ (const Matrix3& m) const
  {
	  return Matrix3(Matrix3::m11 + m.m11, Matrix3::m12 + m.m12, Matrix3::m13 + m.m13, 
					 Matrix3::m21 + m.m21, Matrix3::m22 + m.m22, Matrix3::m23 + m.m23,
					 Matrix3::m31 + m.m31, Matrix3::m32 + m.m32, Matrix3::m33 + m.m33);
  }

  Matrix3 operator- (const Matrix3& m) const
  {
	  return Matrix3(Matrix3::m11 - m.m11, Matrix3::m12 - m.m12, Matrix3::m13 - m.m13, 
							 Matrix3::m21 - m.m21, Matrix3::m22 - m.m22, Matrix3::m23 - m.m23,
							 Matrix3::m31 - m.m31, Matrix3::m32 - m.m32, Matrix3::m33 - m.m33);
  }

  // +mat = mat .. c'est trivial mais on pourra optimiser
  // grace à cela 0 + a =)> a
  FORCEINLINE Matrix3 operator+ () const { return *this; }

  FORCEINLINE Matrix3 operator- () const
  {
    return Matrix3( -m11,-m12,-m13,
					-m21,-m22,-m23,
					-m31,-m32,-m33);
  }

  // vect = mat * vect 
  FORCEINLINE Vector3 operator* (const Vector3& vect) const
  {
	  real a = m11+m12+m13,
		   b = m21+m22+m23,
		   c = m31+m32+m33;
	  return Vector3(vect.x * a, vect.y * b, vect.z * c);
  }

  // vect = vect * mat | le vecteur est un "vecteur ligne"
  FORCEINLINE Vector3 ProdVectMat (const Vector3& vect, const Matrix3& mat) const
  {
		return Vector3(vect.x*(mat.m11)+vect.y*(mat.m21)+vect.z*(mat.m31), 
					  vect.x*(mat.m12)+vect.y*(mat.m22)+vect.z*(mat.m32),
					  vect.x*(mat.m13)+vect.y*(mat.m23)+vect.z*(mat.m33));
  }

	FORCEINLINE Matrix3 operator* (const real fact) const
	{
		return Matrix3(fact * m11,fact * m12,fact * m13,
					   fact * m21,fact * m22,fact * m23,
					   fact * m31,fact * m32,fact * m33);
	}

    // Produit matriciel
    // matrice = obj * mat
	FORCEINLINE Matrix3 operator* (const Matrix3& mat) const
	{
			return Matrix3(	m11*(mat.m11) + m12*(mat.m21) + m13*(mat.m31),
							m11*(mat.m12) + m12*(mat.m22) + m13*(mat.m32),
							m11*(mat.m13) + m12*(mat.m23) + m13*(mat.m33),
							m21*(mat.m11) + m22*(mat.m21) + m23*(mat.m31),
							m21*(mat.m12) + m22*(mat.m22) + m23*(mat.m32),
							m21*(mat.m13) + m22*(mat.m23) + m23*(mat.m33),
							m31*(mat.m11) + m32*(mat.m21) + m33*(mat.m31),
							m31*(mat.m12) + m32*(mat.m22) + m33*(mat.m32),
							m31*(mat.m13) + m32*(mat.m23) + m33*(mat.m33));
	}


	FORCEINLINE Matrix3 &operator*=(real f)           { return *this = *this * f; }
	FORCEINLINE Matrix3 &operator*=(const Matrix3 &m) { return *this = *this * m; }
	FORCEINLINE Matrix3 &operator+=(const Matrix3 &m) { return *this = *this + m; }
	FORCEINLINE Matrix3 &operator-=(const Matrix3 &m) { return *this = *this - m; }



	// Inversion de matrice en utilsant les cofacteurs
	FORCEINLINE Matrix3 Inverse() const
	{
		// matrice des cofacteurs
		Matrix3 inv( (m22*m33 - m23*m32), -(m12*m33 - m13*m32),  (m12*m23 - m13*m22),
					-(m21*m33 - m23*m31),  (m11*m33 - m13*m31), -(m11*m23 - m13*m21),
					 (m21*m32 - m22*m31), -(m11*m32 - m12*m31),  (m11*m22 - m12*m21));

		real s = (real)1.0/(m11*inv.m11 + m12*inv.m21 + m13*inv.m31);
		return inv * s;
	}

	FORCEINLINE Matrix3 Transpose() const
	{
		return Matrix3(m11, m21, m31, m12, m22, m32, m13, m23, m33);
	}
	// Calcul du déterminant en utilsant les
	// cofacteurs.
	FORCEINLINE real Det() const
	{
		real  fCofactor00 = m22*m33 - m23*m32;
		real  fCofactor10 = m23*m32 - m21*m33;
		real  fCofactor20 = m21*m32 - m22*m31;
		
        return  m11*fCofactor00 + m12*fCofactor10 + m13*fCofactor20;
	}

	
	// Calcule la trace de la matrice
	FORCEINLINE real Trace() const
	{
		return m11 + m22 + m33;
	}
	
	// On utilise IsIdentity(0) pour avoir une identité stricte,
	// une calculatoire est basée sur SMALL_EPSILON
	FORCEINLINE bool IsIdentity(real precision = SMALL_EPSILON) const
	{
		switch((bool)(precision == 0.0))
		{
			case true:
			{
				Matrix3 id(1,1,1);
				return (*this == id);
			}
			default:
			{
				real lim1 = 1 - precision;
				real lim2 = 1 + precision;
				real val;
				for (short i=0;i<9;i++)
				{
					val = *((&m11) + i);
					// diagonale
					// i == {0,4,8}
					// on teste i % 4 soit i & 3
					if (!( i & 3 ))
					{
						// inférieur à la limite basse et 
                        // supérieur a la limite haute
						if (val <= lim1 && val >= lim2)
							return false;
						else
							continue;
					}
					else if (ABS(val) >= precision)
						return false;
				}
			}
		}
		return true;
	}
	//
	// Diverses transformations
	// Rotations, translations etc.
	//	
	void Rotate(real angle, const Vector3 &around)
	{
		real c = Cos(angle);
		real s = Sin(angle);
		Vector3 v = around;
		v.Normalize();
		real xy = v.x * v.y;
		real yz = v.y * v.z;
		real zx = v.z * v.x;
		real xs = v.x * s;
		real ys = v.y * s;
		real zs = v.z * s;
		
		m11 = (1.0 - c) * v.x * v.x + c; 
        m12 = (1.0 - c) * xy - zs; 
        m13 = (1.0 - c) * zx + ys;
		m21 = (1.0 - c) * xy + zs; 
        m22 = (1.0 - c) * v.y * v.y + c; 
        m23 = (1.0 - c) * yz - xs;
		m31 = (1.0 - c) * zx - ys; 
        m32 = (1.0 - c) * yz + xs; 
        m33 = (1.0 - c) * v.z * v.z + c;
	}
	
	Matrix3 Rotate(real angle, const Vector3 &around) const
	{
		real mat[9] = {0};
        real c = Cos(angle);
		real s = Sin(angle);
		Vector3 v = around;
		v.Normalize();
		real xy = v.x * v.y;
		real yz = v.y * v.z;
		real zx = v.z * v.x;
		real xs = v.x * s;
		real ys = v.y * s;
		real zs = v.z * s;
		mat[0] = (1.0 - c) * v.x * v.x + c; 
        mat[3] = (1.0 - c) * xy - zs; 
        mat[6] = (1.0 - c) * zx + ys;
		mat[1] = (1.0 - c) * xy + zs; 
        mat[4] = (1.0 - c) * v.y * v.y + c; 
        mat[7] = (1.0 - c) * yz - xs;
		mat[2] = (1.0 - c) * zx - ys; 
        mat[5] = (1.0 - c) * yz + xs; 
        mat[8] = (1.0 - c) * v.z * v.z + c;
        return Matrix3(mat);	  
	}

	void Rotate_X(real angle)
	{
	  real s = Sin(angle),
	       c = Cos(angle); 
      m11 = 1.0; m12 = 0.0 ; m13 =  0.0;
      m21 = 0.0; m22 =   c;  m23 = -s;
      m31 = 0.0; m32 =   s;  m33 =  c;
	}
	
	void Rotate_Y(real angle)
	{
	  real s = Sin(angle),
	       c = Cos(angle); 
      m11 =   c; m12 = 0.0 ; m13 =   s;
      m21 = 0.0; m22 = 1.0;  m23 = 0.0;
      m31 =  -s; m32 = 0.0;  m33 =   c;
	}

	void Rotate_Z(real angle)
	{
	  real s = Sin(angle),
	       c = Cos(angle); 
      m11 = c;   m12 =  -s;  m13 = 0.0;
      m21 = s;   m22 =   c;  m23 = 0.0;
      m31 = 0.0; m32 = 0.0;  m33 = 1.0;
	}

	void Translate(const Vector3 &to)
	{
	  m11 = to[0];
	  m22 = to[1];
	  m33 = to[2];
	  m12=m13=m21=m23=m31=m32=0;	  
	}

	void OrthoNormalize()
	{
	   Vector3 a(m11,m21,m31),
	          b(m12,m22,m32);
       a.Normalize();
       b.Normalize();
       Vector3 c = a ^ b; // a ^ b : prod vect
       c.Normalize();
       m11 = a[0]; m12 = b[0]; m13 = c[0];
       m21 = a[1]; m22 = b[1]; m23 = c[1];
       m31 = a[2]; m32 = b[2]; m33 = c[2];
	}
	
	Matrix3 OrthoNormalize() const
	{
	   
       Vector3 a(m11,m21,m31),
	          b(m12,m22,m32);
       a.Normalize();
       b.Normalize();
       Vector3 c = a ^ b;
       c.Normalize();
       return Matrix3(a,b,c);
    }	

    // Sortie dans le log
	void out() const;
	
    // Permettre de faire une sortie sur n'importe quel
	// type de flux (fichier, sortie std etc.)
	//void out(std::ofstream &stream) const;
};


/// à faire
  // ----> mettre tout ca dans une classe spéciale
  //         avec décomposition LU,QR de choleswky ..
  // -> fonction map(matrix, fenetre, fonction)
  //             matrice contenant des valeurs de fonctions
  //             ... 
  // -> produit tensoriel
  // -> autres saloperies



#endif