#include "types.h"
#include "vecmath.h"
#include <math.h>
#include <stdio.h>

void vector_assign (vector* v, coord x, coord y, coord z)
{
  v->x = x;
  v->y = y;
  v->z = z;
}

coord vector_scalar (vector v1, vector v2)
{
  return (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
}

void vector_product (vector* vectres, vector vect1, vector vect2)
{
  vectres->x=vect1.y*vect2.z - vect2.y*vect1.z;
  vectres->y=vect1.z*vect2.x - vect2.z*vect1.x;
  vectres->z=vect1.x*vect2.y - vect2.x*vect1.y;
}

void vector_add (vector* v1, vector v2)
{
  v1->x += v2.x ;
  v1->y += v2.y ;
  v1->z += v2.z ;
}

void vector_sub (vector* v1, vector v2)
{
  v1->x -= v2.x ;
  v1->y -= v2.y ;
  v1->z -= v2.z ;
}

void vector_scale (vector* v, coord k)
{
  v->x *= k ;
  v->y *= k ;
  v->z *= k ;
}

coord vector_norm (vector* vect)
{
  return sqrt(vect->x*vect->x + vect->y*vect->y + vect->z*vect->z);
}

void vector_normalize (vector* vect) 
{
  coord length = vector_norm (vect) ;
  vector_scale (vect, 1/length) ;
}

void vector_computenormal (vector* normal, vector a, vector b, vector c)
{
  vector d,e ;
  d.x = a.x - b.x ;         e.x = b.x - c.x ;
  d.y = a.y - b.y ;         e.y = b.y - c.y ;
  d.z = a.z - b.z ;         e.z = b.z - c.z ;
  vector_product (normal, d, e) ;
  vector_normalize (normal) ;
}

void matrix_identity (matrix* M)
{
  (*M)[0][0]=1.0; (*M)[0][1]=0.0; (*M)[0][2]=0.0; (*M)[0][3]=0.0;
  (*M)[1][0]=0.0; (*M)[1][1]=1.0; (*M)[1][2]=0.0; (*M)[1][3]=0.0;
  (*M)[2][0]=0.0; (*M)[2][1]=0.0; (*M)[2][2]=1.0; (*M)[2][3]=0.0;
  (*M)[3][0]=0.0; (*M)[3][1]=0.0; (*M)[3][2]=0.0; (*M)[3][3]=1.0;
}

void matrix_display (matrix M)
{
  int i, j;
  for(i=0; i<4; i++) {
    for(j=0; j<4; j++)
      printf("%f ",M[i][j]);
    printf("\n");
  }
  printf("\n");
}

void vector_applymatrix (vector* vect, matrix m, coord h)
{
  vector tmp;

  tmp.x=vect->x*m[0][0]+vect->y*m[0][1]+vect->z*m[0][2]+h*m[0][3];
  tmp.y=vect->x*m[1][0]+vect->y*m[1][1]+vect->z*m[1][2]+h*m[1][3];
  tmp.z=vect->x*m[2][0]+vect->y*m[2][1]+vect->z*m[2][2]+h*m[2][3];

  vect->x=tmp.x;
  vect->y=tmp.y;
  vect->z=tmp.z;
}

void matrix_inverse(matrix* inverse, matrix m)
{
  coord det_1;
  coord pos, neg, temp;

  //matrix_display(m);

  //  * Calculate the determinant of submatrix A
  pos = neg = 0.0;
  temp =  m[0][0] * m[1][1] * m[2][2];
  if (temp >= 0.0) pos += temp;  else  neg += temp;
  temp =  m[0][1] * m[1][2] * m[2][0];
  if (temp >= 0.0) pos += temp;  else  neg += temp;
  temp =  m[0][2] * m[1][0] * m[2][1];
  if (temp >= 0.0) pos += temp;  else  neg += temp;
  temp = -m[0][2] * m[1][1] * m[2][0];
  if (temp >= 0.0) pos += temp;  else  neg += temp;
  temp = -m[0][1] * m[1][0] * m[2][2];
  if (temp >= 0.0) pos += temp;  else  neg += temp;
  temp = -m[0][0] * m[1][2] * m[2][1];
  if (temp >= 0.0) pos += temp;  else  neg += temp;
  det_1 = pos + neg;

  //matrix_display(m);
  
  // Is Singular ?
  if ((det_1 == 0.0) || (fabs(det_1 / (pos - neg)) < 1e-15))  return ;
  
  
  //  Calculate inverse(A) = adj(A) / det(A)
  det_1 = 1.0f / det_1;

  //printf("det = %f\n", det_1);

  (*inverse)[0][0] =(   ( m[1][1] * m[2][2] - m[1][2] * m[2][1] ) * det_1);
  (*inverse)[1][0] =( - ( m[1][0] * m[2][2] - m[1][2] * m[2][0] ) * det_1);
  (*inverse)[2][0] =(   ( m[1][0] * m[2][1] - m[1][1] * m[2][0] ) * det_1);
  (*inverse)[0][1] =( - ( m[0][1] * m[2][2] - m[0][2] * m[2][1] ) * det_1);
  (*inverse)[1][1] =(   ( m[0][0] * m[2][2] - m[0][2] * m[2][0] ) * det_1);
  (*inverse)[2][1] =( - ( m[0][0] * m[2][1] - m[0][1] * m[2][0] ) * det_1);
  (*inverse)[0][2] =(   ( m[0][1] * m[1][2] - m[0][2] * m[1][1] ) * det_1);
  (*inverse)[1][2] =( - ( m[0][0] * m[1][2] - m[0][2] * m[1][0] ) * det_1);
  (*inverse)[2][2] =(   ( m[0][0] * m[1][1] - m[0][1] * m[1][0] ) * det_1);
  
  (*inverse)[0][3]=-((*inverse)[0][0] * m[0][3] + (*inverse)[1][0] * m[1][3]+ (*inverse)[2][0] * m[2][3]);
  (*inverse)[1][3]=-((*inverse)[0][1] * m[0][3] + (*inverse)[1][1] * m[1][3]+ (*inverse)[2][1] * m[2][3]);
  (*inverse)[2][3]=-((*inverse)[0][2] * m[0][3] + (*inverse)[1][2] * m[1][3]+ (*inverse)[2][2] * m[2][3]);
  (*inverse)[3][3]=1; 
}

void matrix_translation (matrix* m, coord x, coord y, coord z)
{
  matrix_identity (m) ;
  (*m)[0][3]=x;
  (*m)[1][3]=y;
  (*m)[2][3]=z;
}


void matrix_rotation (matrix* m, coord angle,coord x,coord y,coord z)
{
  coord sin_a,cos_a,n1,n2,n3;
  matrix_identity (m) ;
  sin_a = sin (angle) ;
  cos_a = cos (angle) ;
  n1 = x; n2 = y; n3 = z;
  
  (*m)[0][0] = (n1*n1 + (1. - n1*n1)*cos_a);
  (*m)[1][0] = (n1*n2*(1. - cos_a) + n3*sin_a);
  (*m)[2][0] = (n1*n3*(1. - cos_a) - n2*sin_a);

  (*m)[0][1] = (n1*n2*(1. - cos_a) - n3*sin_a);
  (*m)[1][1] = (n2*n2 + (1. - n2*n2)*cos_a);
  (*m)[2][1] = (n2*n3*(1. - cos_a) + n1*sin_a);

  (*m)[0][2] = (n1*n3*(1. - cos_a) + n2*sin_a);
  (*m)[1][2] = (n2*n3*(1. - cos_a) - n1*sin_a);
  (*m)[2][2] = (n3*n3 + (1. - n3*n3)*cos_a);
}