#include <math.h>
#include <stdio.h>
#include <string.h>
#include "gr_matrix.h"

static GrMatrix _identity = {
  {
    {1.0, 0.0, 0.0, 0.0},
    {0.0, 1.0, 0.0, 0.0},
    {0.0, 0.0, 1.0, 0.0},
    {0.0, 0.0, 0.0, 1.0},
  }
};
GrMatrix *gr_matrix_identity = &_identity;


void gr_matrix_mult_vertex (GrMatrix *m, GrVertex *v, GrVertex *d)
{
  GrVertex tmp, *p = &tmp;

  if (v != d)
    p = d;

  p->c.x = m->f[0][0]*v->c.x +
    	   m->f[0][1]*v->c.y +
	   m->f[0][2]*v->c.z +
	   m->f[0][3];

  p->c.y = m->f[1][0]*v->c.x +
	   m->f[1][1]*v->c.y +
	   m->f[1][2]*v->c.z +
	   m->f[1][3];

  p->c.z = m->f[2][0]*v->c.x +
	   m->f[2][1]*v->c.y +
	   m->f[2][2]*v->c.z +
	   m->f[2][3];

  if (v == d)
    memcpy (d, p, sizeof(GrVertex));
}


void gr_matrix_mult_vertex_without_translate (GrMatrix *m,
					      GrVertex *v, GrVertex *d)
{
  GrVertex tmp, *p = &tmp;

  if (v != d)
    p = d;

  p->c.x = m->f[0][0]*v->c.x +
    	   m->f[0][1]*v->c.y +
	   m->f[0][2]*v->c.z;

  p->c.y = m->f[1][0]*v->c.x +
	   m->f[1][1]*v->c.y +
	   m->f[1][2]*v->c.z;

  p->c.z = m->f[2][0]*v->c.x +
	   m->f[2][1]*v->c.y +
	   m->f[2][2]*v->c.z;

  if (v == d)
    memcpy (d, p, sizeof(GrVertex));
}


void gr_matrix_copy (GrMatrix *s, GrMatrix *d)
{
  memcpy (d, s, sizeof(GrMatrix));
}


void gr_matrix_mult (GrMatrix *m1, GrMatrix *m2, GrMatrix *d)
{
  int i, j, k;
  float acc;
  GrMatrix tmp;

  for (i=0; i<3; i++) {
    for (j=0; j<3; j++) {
      acc = 0;
      for (k=0; k<3; k++)
	acc += m1->f[i][k] * m2->f[k][j];
      tmp.f[i][j] = acc;
    }
  }
  for (i=0; i<3; i++) {
    acc = m1->f[i][3];
    for (k=0; k<3; k++)
      acc += m1->f[i][k] * m2->f[k][3];
    tmp.f[i][3] = acc;
  }

  memcpy (d, &tmp, sizeof(float) * 4 * 3);
}

void gr_matrix_rotate (GrMatrix *m, GrMatrix *d,
		    float s, float c,
		    float x, float y, float z)
{
  GrMatrix tmp;
  float sx, sy, sz;
  float c2 = 1.0 - c;
  int i;

  sx = x * s;
  sy = y * s;
  sz = z * s;

  tmp.f[0][0] = c2 * x * x + c;
  tmp.f[0][1] = c2 * x * y - sz;
  tmp.f[0][2] = c2 * x * z + sy;

  tmp.f[1][0] = c2 * y * x + sz;
  tmp.f[1][1] = c2 * y * y + c;
  tmp.f[1][2] = c2 * y * z - sx;

  tmp.f[2][0] = c2 * z * x - sy;
  tmp.f[2][1] = c2 * z * y + sx;
  tmp.f[2][2] = c2 * z * z + c;

  for (i=0; i<3; i++) {
    tmp.f[3][i] = 0;
    tmp.f[i][3] = 0;
  }
  tmp.f[3][3] = 1;

  gr_matrix_mult (&tmp, m, d);
}


void gr_matrix_rotate_x (GrMatrix *m, GrMatrix *d, float s, float c)
{
  GrMatrix tmp;

  tmp.f[1][0] = c * m->f[1][0] - s * m->f[2][0];
  tmp.f[1][1] = c * m->f[1][1] - s * m->f[2][1];
  tmp.f[1][2] = c * m->f[1][2] - s * m->f[2][2];
  tmp.f[1][3] = c * m->f[1][3] - s * m->f[2][3];

  tmp.f[2][0] = s * m->f[1][0] + c * m->f[2][0];
  tmp.f[2][1] = s * m->f[1][1] + c * m->f[2][1];
  tmp.f[2][2] = s * m->f[1][2] + c * m->f[2][2];
  tmp.f[2][3] = s * m->f[1][3] + c * m->f[2][3];

  memcpy (d->f[1], tmp.f[1], sizeof(float) * 4 * 2);
  if (m != d)
    memcpy (d->f[0], m->f[0], sizeof(float) * 4);
}


void gr_matrix_rotate_y (GrMatrix *m, GrMatrix *d, float s, float c)
{
  GrMatrix tmp;

  tmp.f[0][0] = c * m->f[0][0] + s * m->f[2][0];
  tmp.f[0][1] = c * m->f[0][1] + s * m->f[2][1];
  tmp.f[0][2] = c * m->f[0][2] + s * m->f[2][2];
  tmp.f[0][3] = c * m->f[0][3] + s * m->f[2][3];

  tmp.f[2][0] = -s * m->f[0][0] + c * m->f[2][0];
  tmp.f[2][1] = -s * m->f[0][1] + c * m->f[2][1];
  tmp.f[2][2] = -s * m->f[0][2] + c * m->f[2][2];
  tmp.f[2][3] = -s * m->f[0][3] + c * m->f[2][3];

  memcpy (d->f[0], tmp.f[0], sizeof(float) * 4);
  memcpy (d->f[2], tmp.f[2], sizeof(float) * 4);
  if (m != d)
    memcpy (d->f[1], m->f[1], sizeof(float) * 4);
}


void gr_matrix_rotate_z (GrMatrix *m, GrMatrix *d, float s, float c)
{
  GrMatrix tmp;

  tmp.f[0][0] = c * m->f[0][0] - s * m->f[1][0];
  tmp.f[0][1] = c * m->f[0][1] - s * m->f[1][1];
  tmp.f[0][2] = c * m->f[0][2] - s * m->f[1][2];
  tmp.f[0][3] = c * m->f[0][3] - s * m->f[1][3];

  tmp.f[1][0] = s * m->f[0][0] + c * m->f[1][0];
  tmp.f[1][1] = s * m->f[0][1] + c * m->f[1][1];
  tmp.f[1][2] = s * m->f[0][2] + c * m->f[1][2];
  tmp.f[1][3] = s * m->f[0][3] + c * m->f[1][3];

  memcpy (d->f[0], tmp.f[0], sizeof(float) * 4 * 2);
  if (m != d)
    memcpy (d->f[2], m->f[2], sizeof(float) * 4);
}


void gr_matrix_translate (GrMatrix *m, GrMatrix *d, GrVertex *v)
{
  if (m != d)
    gr_matrix_copy (m, d);

  d->f[0][3] += v->c.x;
  d->f[1][3] += v->c.y;
  d->f[2][3] += v->c.z;
}


void gr_matrix_scale (GrMatrix *m, GrMatrix *d, GrVertex *v)
{
  int i;

  for (i=0; i<4; i++) {
    d->f[0][i] = m->f[0][i] * v->c.x;
    d->f[1][i] = m->f[1][i] * v->c.y;
    d->f[2][i] = m->f[2][i] * v->c.z;
  }
}

/* Gauss Elimination */
void gr_matrix_invert (GrMatrix *m , GrMatrix *d)
{
  int i, j, k, imax, jmax;
  float  s, p, q;
  int  jun[4];
  float  sf[4];
  GrMatrix a = *m;

  /* balancing */
  for (i=0; i<4; i++) {
    p = fabs(a.f[i][0]);
    for (j=1 ; j<4 ; ++j) {
      if ((q = fabs(a.f[i][j])) > p) {
        p = q;
      }
    }
    d->f[i][3] = 0;
    d->f[3][i] = 0;
    for (j=0; j<4; ++j) {
      a.f[i][j] /= p;
      if (i == j) {
	d->f[i][j] = 1.0 / p;
      } else {
	d->f[i][j] = 0.0;
      }
    }
  }
  d->f[3][3] = 1;

  for (j=0; j<4; j++) {
    p = fabs(a.f[0][j]);
    for (i=1 ;i<4 ;i++) {
      if ((q = fabs(a.f[i][j])) > p) {
        p = q;
      }
    }
    for (i=0; i<4; i++) {
      a.f[i][j] /= p;
    }
    sf[j] = p;
  }

  for (k=0; k<4-1; k++) {
    /* complete pivotting */
    imax = k;
    jmax = k;
    p=0.0;
    for (i=k; i<4; i++) {
      for (j=k; j<4 ;j++) {
        if ((q = fabs(a.f[i][j])) > p) {
          p = q;  
          imax = i;
          jmax = j;
        }
      }
    }
    jun[k]=jmax;
    if (imax != k) {
      for (j=k; j<4; ++j) {
	s = a.f[k][j];
	a.f[k][j] = a.f[imax][j];
	a.f[imax][j] = s;
      }
      for (j=0; j<4; ++j) {
	s = d->f[k][j];
	d->f[k][j] = d->f[imax][j];
	d->f[imax][j] = s;
      }
    }
    if (jmax != k) {
      for (i=0; i<4; i++) {
	s = a.f[i][k];
	a.f[i][k] = a.f[i][jmax];
	a.f[i][jmax] = s;
      }
    }

    /*  k  */
    p = a.f[k][k];
    for (j=k+1; j<4; ++j) {
      a.f[k][j] /= p;
    }
    for (j=0; j<4; ++j) {
      d->f[k][j] /= p;
    }
    /*  i  */
    for (i=k+1; i<4; ++i) {
      q = a.f[i][k] ;
      for (j=k+1; j<4; ++j) {
        a.f[i][j] -= q*a.f[k][j];
      }
      for (j=0; j<4; ++j) {
        d->f[i][j] -= q*d->f[k][j];
      }
    } 
  } 

  p = a.f[k][4-1];
  for (j=0; j<4; ++j) {
    d->f[4-1][j] /= p;
  }

  for (k=4-1; k>0; --k) {
    for (i=0; i<k; ++i) {
      q = a.f[i][k];
      for (j=0; j<4; ++j)  {
        d->f[i][j] -= q*d->f[k][j];
      }
    }
  }

  for (k=4-2; k>=0; --k) {
    i=jun[k];
    for (j=0; j<4; ++j) {
      s = d->f[i][j];
      d->f[i][j] = d->f[k][j];
      d->f[k][j] = s;
    }
  }

  for (i=0; i<4; ++i) {
    p = sf[i];
    for (j=0; j<4; ++j) {
      d->f[i][j] /= p;
    }
  }
}