/* Some simple mathematical functions. Don't look for some logic in
   the function names :-) */

#include "TinyGL/gl_zmath.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>


#define X v[0]
#define Y v[1]
#define Z v[2]
#define W v[3]


/* ******* Gestion des matrices 4x4 ****** */

void gl_M4_Id( M4 *a ){
	int i,j;

	for( i=0; i<4; i++ ){
		for( j=0; j<4; j++ ){
			if( i==j ){
				a->m[i][j] = 1;
			}else{
				a->m[i][j] = 0;
			}
		}
	}

}

int gl_M4_IsId( M4 *a ){
	int i,j;

	for( i=0; i<4; i++ ){
		for( j=0; j<4; j++ ) {
			if( i==j ){ 
				if( a->m[i][j] != 1 ){
					return 0;
				}
			}else if( a->m[i][j] != 0 ){
				return 0;
			}
		}
	}
	return 1;
}

void gl_M4_Mul( M4 *c, M4 *a, M4 *b ){
  int i,j,k;
  float s;
  for(i=0;i<4;i++)
    for(j=0;j<4;j++) {
      s=0.0;
      for(k=0;k<4;k++) s+=a->m[i][k]*b->m[k][j];
      c->m[i][j]=s;
    }
}

/* c=c*a */
void gl_M4_MulLeft( M4 *c, M4 *b ){
  int   i,j,k;
  float s;
  M4    a;

  /*memcpy(&a, c, 16*sizeof(float));
  */
  a=*c;

  for(i=0;i<4;i++)
    for(j=0;j<4;j++) {
      s=0.0;
      for(k=0;k<4;k++) s+=a.m[i][k]*b->m[k][j];
      c->m[i][j]=s;
    }
}

void gl_M4_Move(M4 *a,M4 *b){
	memcpy(a,b,sizeof(M4));
}

void gl_MoveV3(V3 *a,V3 *b){
	memcpy(a,b,sizeof(V3));
}


void gl_MulM4V3(V3 *a,M4 *b,V3 *c){
	 a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z+b->m[0][3];
	 a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z+b->m[1][3];
	 a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z+b->m[2][3];
}

void gl_MulM3V3(V3 *a,M4 *b,V3 *c)
{
	 a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z;
	 a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z;
	 a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z;
}

void gl_M4_MulV4( V4 *a,M4 *b,V4 *c ){
	 a->X=b->m[0][0]*c->X+b->m[0][1]*c->Y+b->m[0][2]*c->Z+b->m[0][3]*c->W;
	 a->Y=b->m[1][0]*c->X+b->m[1][1]*c->Y+b->m[1][2]*c->Z+b->m[1][3]*c->W;
	 a->Z=b->m[2][0]*c->X+b->m[2][1]*c->Y+b->m[2][2]*c->Z+b->m[2][3]*c->W;
	 a->W=b->m[3][0]*c->X+b->m[3][1]*c->Y+b->m[3][2]*c->Z+b->m[3][3]*c->W;
}
	
/* transposition of a 4x4 matrix */
void gl_M4_Transpose( M4 *a, M4 *b ){
	a->m[0][0] = b->m[0][0]; 
	a->m[0][1] = b->m[1][0]; 
	a->m[0][2] = b->m[2][0]; 
	a->m[0][3] = b->m[3][0]; 
	
	a->m[1][0] = b->m[0][1]; 
	a->m[1][1] = b->m[1][1]; 
	a->m[1][2] = b->m[2][1]; 
	a->m[1][3] = b->m[3][1]; 
	
	a->m[2][0] = b->m[0][2]; 
	a->m[2][1] = b->m[1][2]; 
	a->m[2][2] = b->m[2][2]; 
	a->m[2][3] = b->m[3][2]; 
	
	a->m[3][0] = b->m[0][3]; 
	a->m[3][1] = b->m[1][3]; 
	a->m[3][2] = b->m[2][3]; 
	a->m[3][3] = b->m[3][3]; 
}

/* inversion of an orthogonal matrix of type Y=M.X+P */ 
void gl_M4_InvOrtho( M4 *a,M4 b ){
	int   i,j;
	float s;

	for( i=0; i<3; i++ ){
		for( j=0; j<3; j++ ){
			a->m[i][j] = b.m[j][i];
		}
		a->m[3][0] = 0; a->m[3][1] = 0; a->m[3][2] = 0; a->m[3][3] = 1;
		for( i=0; i<3; i++ ){
			s = 0;
			for( j=0; j<3; j++ ){
				s -= b.m[j][i] * b.m[j][3];
			}
			a->m[i][3] = s;
		}
	}
}

/* Inversion of a general nxn matrix.
   Note : m is destroyed */

int Matrix_Inv( float *r, float *m, int n ){
	 int   i,j,k,l;
	 float max,tmp,t;

	 /* identitée dans r */
	 for(i=0;i<n*n;i++) r[i]=0;
	 for(i=0;i<n;i++) r[i*n+i]=1;
	 
	 for(j=0;j<n;j++) {
			
			/* recherche du nombre de plus grand module sur la colonne j */
			max=m[j*n+j];
			k=j;
			for(i=j+1;i<n;i++)
				if (fabs(m[i*n+j])>fabs(max)) {
					 k=i;
					 max=m[i*n+j];
				}

      /* non intersible matrix */
      if (max==0) return 1;

			
			/* permutation des lignes j et k */
			if (k!=j) {
				 for(i=0;i<n;i++) {
						tmp=m[j*n+i];
						m[j*n+i]=m[k*n+i];
						m[k*n+i]=tmp;
						
						tmp=r[j*n+i];
						r[j*n+i]=r[k*n+i];
						r[k*n+i]=tmp;
				 }
			}
			
			/* multiplication de la ligne j par 1/max */
			max=1/max;
			for(i=0;i<n;i++) {
				 m[j*n+i]*=max;
				 r[j*n+i]*=max;
			}
			
			
			for(l=0;l<n;l++) if (l!=j) {
				 t=m[l*n+j];
				 for(i=0;i<n;i++) {
						m[l*n+i]-=m[j*n+i]*t;
						r[l*n+i]-=r[j*n+i]*t;
				 }
			}
	 }

	 return 0;
}


/* inversion of a 4x4 matrix */

void gl_M4_Inv( M4 *a, M4 *b ){
	M4 tmp;
	memcpy( &tmp, b, 16*sizeof(float) );
	/*tmp=*b;*/
	Matrix_Inv( &a->m[0][0], &tmp.m[0][0], 4 );
}

void gl_M4_Rotate( M4 *a, float t, int u ){
	float s,c;
	int v,w;
	if ((v=u+1)>2) v=0;
	if ((w=v+1)>2) w=0;
	s = (float)(  sin( t )  );
	c = (float)(  cos( t )  );
	gl_M4_Id( a );
	a->m[v][v] = c; a->m[v][w] = -s;
	a->m[w][v] = s;	a->m[w][w] =  c;
}
	

/* inverse of a 3x3 matrix */
void gl_M3_Inv( M3 *a, M3 *m ){
	 float det;
	 
	 det =
		m->m[0][0]*m->m[1][1]*m->m[2][2]-m->m[0][0]*m->m[1][2]*m->m[2][1]-
		m->m[1][0]*m->m[0][1]*m->m[2][2]+m->m[1][0]*m->m[0][2]*m->m[2][1]+
		m->m[2][0]*m->m[0][1]*m->m[1][2]-m->m[2][0]*m->m[0][2]*m->m[1][1];

	 a->m[0][0] =  ( m->m[1][1]*m->m[2][2]-m->m[1][2]*m->m[2][1])/det;
	 a->m[0][1] = -( m->m[0][1]*m->m[2][2]-m->m[0][2]*m->m[2][1])/det;
	 a->m[0][2] = -(-m->m[0][1]*m->m[1][2]+m->m[0][2]*m->m[1][1])/det;
	 
	 a->m[1][0] = -( m->m[1][0]*m->m[2][2]-m->m[1][2]*m->m[2][0])/det;
	 a->m[1][1] =  ( m->m[0][0]*m->m[2][2]-m->m[0][2]*m->m[2][0])/det;
	 a->m[1][2] = -( m->m[0][0]*m->m[1][2]-m->m[0][2]*m->m[1][0])/det;

	 a->m[2][0] =  ( m->m[1][0]*m->m[2][1]-m->m[1][1]*m->m[2][0])/det;
	 a->m[2][1] = -( m->m[0][0]*m->m[2][1]-m->m[0][1]*m->m[2][0])/det;
	 a->m[2][2] =  ( m->m[0][0]*m->m[1][1]-m->m[0][1]*m->m[1][0])/det;
}

																										
/* vector arithmetic */

int gl_V3_Norm( V3 *a ){
	float n;
	n = (float)(  sqrt(a->X*a->X+a->Y*a->Y+a->Z*a->Z)  );
	if( n==0 ){
		return 1;
	}
	a->X /= n;
	a->Y /= n;
	a->Z /= n;
	return 0;
}

V3 gl_V3_New( float x, float y, float z ){
	V3 a;
	a.X = x;
	a.Y = y;
	a.Z = z;
	return a;
}

V4 gl_V4_New( float x, float y, float z, float w ){
	V4 a;
	a.X = x;
	a.Y = y;
	a.Z = z;
	a.W = w;
	return a;
}