/*****************************************************************************
 *
 *  Copyright (C) 2003 Cédric Brégardis <cedric.bregardis@free.fr>
 *
 *  This file is part of BRIQUOLO
 *
 *  BRIQUOLO is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  BRIQUOLO is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with BRIQUOLO; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *****************************************************************************/
#include "MOGL_MatriceTransformation.h"
#include <iostream>

#ifndef M_PI
#define M_PI 3.14159265359
#endif

using namespace std;

MOGL_MatriceTransformation::MOGL_MatriceTransformation()
{
  _Matrice=new GLfloat[16];
  Reinitialiser();
}

MOGL_MatriceTransformation::~MOGL_MatriceTransformation()
{
  delete [] _Matrice;
}

MOGL_MatriceTransformation::MOGL_MatriceTransformation(const MOGL_MatriceTransformation & UneMatrice)
{
  _Matrice=new GLfloat[16];
  for(int i=0;i<16;i++)
  {
    _Matrice[i]=UneMatrice._Matrice[i];
  }
}

MOGL_MatriceTransformation & MOGL_MatriceTransformation::operator = (const MOGL_MatriceTransformation & UneMatrice)
{
  for(int i=0;i<16;i++)
  {
    _Matrice[i]=UneMatrice._Matrice[i];
  }
  return * this;
}

GLfloat & MOGL_MatriceTransformation::operator () (int IndiceX,int IndiceY)
{
  return _Matrice[4*IndiceX+IndiceY];
}

GLfloat MOGL_MatriceTransformation::Lit(int IndiceX,int IndiceY) const
{
  return _Matrice[4*IndiceX+IndiceY];
}


void MOGL_MatriceTransformation::Multiplier(const MOGL_MatriceTransformation & UneMatrice)
{
  GLfloat * mat_temp=new GLfloat[16];
  for(int ch=0;ch<4;ch++)
  {
    for(int lg=0;lg<4;lg++)
    {
      mat_temp[4*ch+lg]=0;
      for(int cg=0;cg<4;cg++)
      {
        mat_temp[4*ch+lg]=mat_temp[4*ch+lg] + (UneMatrice._Matrice[4*cg+lg] * _Matrice[4*ch+cg]);
      }
    }
  }
  delete [] _Matrice;
  _Matrice=mat_temp;
}

/*void MOGL_MatriceTransformation::Inverser();
{
        GLfloat * mat_temp=new GLfloat[16];
        float det;

        det=
        

        delete [] _Matrice;
        _Matrice=mat_temp;
}*/

void MOGL_MatriceTransformation::Inverser()
{
  float * mat_temp=new float[16];
  float * mat3=new float[9];
  float inv_det=1/Determinant();

  for(int x=0;x<4;x++)
  {
    for(int y=0;y<4;y++)
    {
      int indicex=0;
      int indicey=0;
      for(int i=0;i<4;i++)
      {
        if (i!=x)
        {
          for(int j=0;j<4;j++)
          {
            if (j!=y)
            {
              mat3[3*indicex+indicey]=_Matrice[4*i+j];
              indicey++;
            }
          }
          indicey=0;
          indicex++;
        }
      }

      mat_temp[4*y+x]=pow(static_cast<float>(-1),x+y)*inv_det*_DeterminantMat3(mat3);
    }
  }
  delete [] _Matrice;
  delete [] mat3;
  
  _Matrice=mat_temp;
}

float MOGL_MatriceTransformation::Determinant()
{
  float det=0;
  float Mat[9];
  int indicex;

  for(int x=0;x<4;x++)
  {
    indicex=0;
    for(int i=0;i<4;i++)
    {
      if (i!=x)
      {
        for(int j=1;j<4;j++)
        {
          Mat[indicex*3+j-1]=_Matrice[4*i+j];
        }
        indicex++;
      }
    }
    det+=pow(static_cast<float>(-1),x)*_Matrice[4*x]*_DeterminantMat3(Mat);
  }

  return det;
}

float MOGL_MatriceTransformation::_DeterminantMat3(float * Mat)
{
  float det=Mat[0]*Mat[4]*Mat[8]
    +Mat[1]*Mat[5]*Mat[6]
    +Mat[2]*Mat[3]*Mat[7]

    -Mat[6]*Mat[4]*Mat[2]
    -Mat[7]*Mat[5]*Mat[0]
    -Mat[8]*Mat[3]*Mat[1];

  return det;
}

void MOGL_MatriceTransformation::ChargerDansOpenGL()
{
  glLoadMatrixf(_Matrice);
}

void MOGL_MatriceTransformation::MultiplierDansOpenGL()
{
  glMultMatrixf(_Matrice);
}

void MOGL_MatriceTransformation::PrendreDeOpenGL()
{
  int val;
  
  glGetIntegerv(GL_MATRIX_MODE, &val);
  
  switch (val)
  {
    case GL_MODELVIEW:
      glGetFloatv(GL_MODELVIEW_MATRIX, _Matrice);
      break;
    case GL_PROJECTION:
      glGetFloatv(GL_PROJECTION_MATRIX, _Matrice);
      break;
    case GL_TEXTURE:
      glGetFloatv(GL_TEXTURE_MATRIX, _Matrice);
      break;
  }
}

void MOGL_MatriceTransformation::Reinitialiser()
{
  for(int i=0;i<4;i++)
  {
    for(int j=0;j<4;j++)
    {
      if (i!=j)
        _Matrice[i*4+j]=0;
      else
        _Matrice[i*4+j]=1;
    }
  }
}

void MOGL_MatriceTransformation::AjouterTranslation(GLfloat p_X, GLfloat p_Y, GLfloat p_Z)
{
  _Matrice[12]+=p_X;
  _Matrice[13]+=p_Y;
  _Matrice[14]+=p_Z;
}



MOGL_MatriceTransformation MOGL_MatriceTransformation::FabriqueTranslation(GLfloat p_X,GLfloat p_Y, GLfloat p_Z)
{
  MOGL_MatriceTransformation UneMatrice;
  UneMatrice.AjouterTranslation(p_X,p_Y,p_Z);
  return UneMatrice;
}

MOGL_MatriceTransformation MOGL_MatriceTransformation::FabriqueEchelle(GLfloat p_X,GLfloat p_Y, GLfloat p_Z)
{
  MOGL_MatriceTransformation UneMatrice;
  UneMatrice._Matrice[0]=p_X;
  UneMatrice._Matrice[5]=p_Y;
  UneMatrice._Matrice[10]=p_Z;

        
  return UneMatrice;
}


MOGL_MatriceTransformation MOGL_MatriceTransformation::FabriqueRotation(GLfloat p_AngleX, GLfloat p_AngleY, GLfloat p_AngleZ)
{
  float AngleX,AngleY,AngleZ;
  AngleX=p_AngleX*3.14159/180;
  AngleY=p_AngleY*3.14159/180;
  AngleZ=p_AngleZ*3.14159/180;

  MOGL_MatriceTransformation UneMatrice;
  UneMatrice._Matrice[0] = cos(AngleZ)*cos(AngleY);
  UneMatrice._Matrice[4] = sin(AngleZ)*cos(AngleY);
  UneMatrice._Matrice[8] = -sin(AngleY);

  UneMatrice._Matrice[1] = cos(AngleZ)*sin(AngleY)*sin(AngleX) - sin(AngleZ)*cos(AngleX);
  UneMatrice._Matrice[5] = sin(AngleZ)*sin(AngleY)*sin(AngleX) + cos(AngleX)*cos(AngleZ);
  UneMatrice._Matrice[9] = sin(AngleX)*cos(AngleY);

  UneMatrice._Matrice[2] = cos(AngleZ)*sin(AngleY)*cos(AngleX) + sin(AngleZ)*sin(AngleX);
  UneMatrice._Matrice[6] = sin(AngleZ)*sin(AngleY)*cos(AngleX) - cos(AngleZ)*sin(AngleX);
  UneMatrice._Matrice[10] = cos(AngleX)*cos(AngleY);
  return UneMatrice;
}

MOGL_MatriceTransformation MOGL_MatriceTransformation::FabriqueRotation(const MOGL_Struct_Vecteur & p_Vecteur, GLfloat p_Angle)
{
  float Angle=p_Angle*3.14159/180;
  float nx=p_Vecteur.x;
  float ny=p_Vecteur.y;
  float nz=p_Vecteur.z;
  
  MOGL_MatriceTransformation UneMatrice;
  UneMatrice._Matrice[0] = nx*nx+(1-nx*nx)*cos(Angle);
  UneMatrice._Matrice[4] = nx*ny*(1-cos(Angle))-nz*sin(Angle);
  UneMatrice._Matrice[8] = nx*nz*(1-cos(Angle))+ny*sin(Angle);
  
  UneMatrice._Matrice[1] = nx*ny*(1-cos(Angle))+nz*sin(Angle);
  UneMatrice._Matrice[5] = ny*ny+(1-ny*ny)*cos(Angle);
  UneMatrice._Matrice[9] = ny*nz*(1-cos(Angle))-nx*sin(Angle);
  
  UneMatrice._Matrice[2] = nx*nz*(1-cos(Angle))-ny*sin(Angle);
  UneMatrice._Matrice[6] = ny*nz*(1-cos(Angle))+nx*sin(Angle);
  UneMatrice._Matrice[10] = nz*nz+(1-nz*nz)*cos(Angle);
  return UneMatrice;
}


MOGL_Struct_Vecteur MOGL_MatriceTransformation::MultiplierVecteur(const MOGL_Struct_Vecteur & p_Vecteur) const
{
  GLfloat pt3=Lit(0,3)*p_Vecteur.x+
    Lit(1,3)*p_Vecteur.y+
    Lit(2,3)*p_Vecteur.z+
    Lit(3,3);

  MOGL_Struct_Vecteur p2;       
  p2.x=(Lit(0,0)*p_Vecteur.x+
        Lit(1,0)*p_Vecteur.y+
        Lit(2,0)*p_Vecteur.z+
        Lit(3,0))/pt3;
        
  p2.y=(Lit(0,1)*p_Vecteur.x+
        Lit(1,1)*p_Vecteur.y+
        Lit(2,1)*p_Vecteur.z+
        Lit(3,1))/pt3;
                                                
  p2.z=(Lit(0,2)*p_Vecteur.x+
        Lit(1,2)*p_Vecteur.y+
        Lit(2,2)*p_Vecteur.z+
        Lit(3,2))/pt3;

  return p2;
}


MOGL_Struct_Vecteur MOGL_MatriceTransformation::EffectuerRotationVecteur(const MOGL_Struct_Vecteur & p_Vecteur) const
{
  GLfloat pt3=Lit(0,3)*p_Vecteur.x+
    Lit(1,3)*p_Vecteur.y+
    Lit(2,3)*p_Vecteur.z+Lit(3,3);
  pt3=1;

  MOGL_Struct_Vecteur p2;       
  p2.x=(Lit(0,0)*p_Vecteur.x+
        Lit(1,0)*p_Vecteur.y+
        Lit(2,0)*p_Vecteur.z)/pt3;
        
  p2.y=(Lit(0,1)*p_Vecteur.x+
        Lit(1,1)*p_Vecteur.y+
        Lit(2,1)*p_Vecteur.z)/pt3;
                                                
  p2.z=(Lit(0,2)*p_Vecteur.x+
        Lit(1,2)*p_Vecteur.y+
        Lit(2,2)*p_Vecteur.z)/pt3;


  float norm=sqrt(p2.x*p2.x+p2.y*p2.y+p2.z*p2.z);
  p2.x/=norm;
  p2.y/=norm;
  p2.z/=norm;
  return p2;
}

void MOGL_MatriceTransformation::Print() const
{
  for(int y=0; y<4; y++)
  {
    for(int x=0; x<4; x++)
    {
      cout<<Lit(x,y)<<" ";
    }
    cout<<endl;
  }
}

MOGL_Struct_Vecteur MOGL_MatriceTransformation::GetRotation()
{
  float rx,ry,rz,t;
  float Epsilon=0.000001f;
  if (1.0 - fabs(Lit(2,0)) < Epsilon)
  {
    if (Lit(2,0) < 0.0)
    {
      ry = static_cast<float>(M_PI / 2.0);
    }
    else
    {
      ry = static_cast<float>(- M_PI / 2.0);
    }
    rx = atan2(Lit(0,1), Lit(0,2));
    if (Lit(2,0) > 0.0)
    {
      rx += static_cast<float>(M_PI);
    }
    rz = 0.0;

  }
  else
  {
    rx = atan2(Lit(2,1), Lit(2,2));
    if (fabs(fabs(rx) - M_PI / 2.0) < Epsilon)
      t = Lit(2,1) / sin(rx);
    else
      t = Lit(2,2) / cos(rx);
    ry = atan2(-Lit(2,0), t);
    rz = atan2(Lit(1,0), Lit(0,0));

  }
  MOGL_Struct_Vecteur Rot;
  Rot.x=rx;
  Rot.y=ry;
  Rot.z=rz;

  return Rot;
}
