#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <graphicsMath.h>

// Prints a matrix to a stream in a nice format. Pass in stdout or
// stderr to send it to the console
void MatrixPrint(Matrix_t m, FILE *fp) {
  int i;

  for(i=0;i<4;i++) {
    fprintf(fp, "| %5.3f %5.3f %5.3f %5.3f |\n", m[i*4], m[i*4+1], m[i*4+2], m[i*4+3]);
  }
  printf("\n");
}

// Sets all entries in a matrix to a single value
void MatrixSetValue(double val, Matrix_t m) {
  long i;

  for(i=0;i<MatrixSize;i++)
    m[i] = val;
}

// Sets a matrix to identity
void MatrixSetIdentity(Matrix_t m) {
  long i;

  for(i=0;i<MatrixSize;i++)
    m[i] = 0.0;

  m[0] = m[5] = m[10] = m[15] = 1.0;
}

// Returns the value at row r, column c with bounds checking
double MatrixGet(short r, short c, Matrix_t m) {
  if(r >= 0 && r < 4) {
    if(c >= 0 && c < 4)
      return(m[(r<<2) + c]);
  }
  fprintf(stderr,"Illegal matrix get (%hd, %hd)\n", r, c);
  return(0.0);
}

// Sets the value at row r, column c with bounds checking
void MatrixSet(short r, short c, double val, Matrix_t m) {
  if(r >= 0 && r < 4) {
    if(c >= 0 && c < 4) {
      m[(r<<2) + c] = val;
      return;
    }
  }
  fprintf(stderr,"Illegal matrix set (%hd, %hd)\n", r, c);
}

// Transposes a matrix in place (modifies the input)
void MatrixTranspose(Matrix_t m) {
  Matrix_t t;
  int i, j;

  for(i=0;i<4;i++) {
    for(j=0;j<4;j++)
      t[(j<<2) + i] = m[(i<<2) + j];
  }

  for(i=0;i<MatrixSize;i++)
    m[i] = t[i];
}

// Copies the second matrix to the first
void MatrixCopy(Matrix_t to, Matrix_t from) {
  int i;

  for(i=0;i<MatrixSize;i++)
    to[i] = from[i];
}

// Executes the matrix multiplication left * right => result
// The result matrix can be one of the operands.
void MatrixMultiply(Matrix_t left, Matrix_t right, Matrix_t result) {
  Matrix_t t;
  double sum;
  int i,j,k;
    
  for(i=0;i<4;i++) {
    for(j=0;j<4;j++) {
      for(sum=0.0,k=0;k<4;k++)
	sum += left[(i<<2) + k] * right[(k<<2) + j];
      t[(i<<2) + j] = sum;
    }
  }

  for(i=0;i<MatrixSize;i++)
    result[i] = t[i];
}

// Transforms a vector by the matrix: result = M * V
void VectorTransform(Matrix_t left, Vector_t right, Vector_t result) {
  Vector_t t;
  int i,j;
	
  for(i=0;i<4;i++) {
    for(t[i]=0.0,j=0;j<4;j++)
      t[i] += left[(i<<2) + j] * right[j];
  }

  result[0] = t[0];
  result[1] = t[1];
  result[2] = t[2];
  result[3] = t[3];
}

// Applies a 2D scale to the left of the input matrix  r = S * r
void MatrixScale2D(double sx, double sy, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[0] = sx;
  m[5] = sy;

  MatrixMultiply(m, r, r);
}

// Applies a 2D rotation to the left of the input matrix r  = R * r
void MatrixRotate2D(double costh, double sinth, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[0] = costh;
  m[1] = -sinth;
  m[4] = sinth;
  m[5] = costh;

  MatrixMultiply(m, r, r);
}

// Applies a 2D translation to the left of the input matrix r = T * r
void MatrixTranslate2D(double tx, double ty, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[3] = tx;
  m[7] = ty;

  MatrixMultiply(m, r, r);
}

// Applies a 3D scale to the left of the input matrix r = S * r
void MatrixScale3D(double sx, double sy, double sz, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[0] = sx;
  m[5] = sy;
  m[10] = sz;

  MatrixMultiply(m, r, r);
}

// applies a rotation about Z to the left of the input matrix r = Rz * r;
void MatrixRotateZ(double costh, double sinth, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[0] = costh;
  m[1] = -sinth;
  m[4] = sinth;
  m[5] = costh;

  MatrixMultiply(m, r, r);
}

// applies a rotation about X to the left of the input matrix r = Rx * r;
void MatrixRotateX(double costh, double sinth, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[5] = costh;
  m[6] = -sinth;
  m[9] = sinth;
  m[10] = costh;

  MatrixMultiply(m, r, r);
}

// applies a rotation about Y to the left of the input matrix r = Ry * r;
void MatrixRotateY(double costh, double sinth, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[0] = costh;
  m[2] = sinth;
  m[8] = -sinth;
  m[10] = costh;

  MatrixMultiply(m, r, r);
}

// applies a coordinate system alignment based on the orthonormal
// vectors x, y, and z:  r = Rxyz * r
void MatrixRotateXYZ(Vector_t x, Vector_t y, Vector_t z, Matrix_t r) {
  Vector_t xbar;
  Vector_t ybar;
  Vector_t zbar;
  Matrix_t m;

  MatrixSetIdentity(m);
  VectorSetNormalized(x[0], x[1], x[2], xbar);
  VectorSetNormalized(y[0], y[1], y[2], ybar);
  VectorSetNormalized(z[0], z[1], z[2], zbar);

  m[0] = xbar[0]; m[1] = xbar[1]; m[2] = xbar[2];
  m[4] = ybar[0]; m[5] = ybar[1]; m[6] = ybar[2];
  m[8] = zbar[0]; m[9] = zbar[1]; m[10] = zbar[2];

  MatrixMultiply(m, r, r);
}

// applies a 3D translation to the left of the input matrix: r = T * r
void MatrixTranslate3D(double tx, double ty, double tz, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[3] = tx;
  m[7] = ty;
  m[11] = tz;

  MatrixMultiply(m, r, r);
}

// applies a perspective projection matrix to the left of the input matrix: r = P * r
void MatrixPerspective(double d, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[14] = 1.0/d;
  m[15] = 0.0;

  MatrixMultiply(m, r, r);
}

// applies a Z shear to the left of the input matrix: r = Shz * r
void MatrixShearZ(double a, double b, Matrix_t r) {
  Matrix_t m;

  MatrixSetIdentity(m);
  m[2] = a;
  m[6] = b;

  MatrixMultiply(m, r, r);
}