/* ========================== */
/* MATRIX ROUTINES */
/* ========================== */

#include "matrix.h"
#include <math.h>
#include <stdio.h>

// ================================================================================
// 3x3 matrix routines
//
// routines that return new matrices: allocate, transpose, duplicate, inverse
// routines modify existing matrices: set_identity, load, copy,
//		set_x_rotate, set_y_rotate, set_z_rotate, multiply
// routnes that return a float: cofactor, determinant
// ================================================================================
	
// -------------------------------------------------------
// MATRIX 3x3 ALLOCATE
// -------------------------------------------------------
matrix3x3 matrix3x3_allocate()
{
	int			i;
	matrix3x3	m;
	
	m = (float **)malloc(sizeof(float *)*3);
	for (i=0; i<3; i++) m[i] = (float *)malloc(sizeof(float)*3);
	return m;
}

// -------------------------------------------------------
// MATRIX 3x3 DEALLOCATE
// -------------------------------------------------------
void matrix3x3_deallocate(matrix3x3 m)
{
	int		i;
	
	for (i=0; i<3; i++) free(m[i]);
	free(m);
}

// -------------------------------------------------------
// MATRIX 3x3 SET IDENTITY */
// -------------------------------------------------------
void matrix3x3_set_identity(matrix3x3 m)
{
	m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0;
	m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0;
	m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0;
}

// -------------------------------------------------------
// MATRIX 3x3 LOAD */
// -------------------------------------------------------
void matrix3x3_load(matrix3x3 m,
	float a00,float a01,float a02,
	float a10,float a11,float a12,
	float a20,float a21,float a22  )
{
	m[0][0] = a00; m[0][1] = a01; m[0][2] = a02;
	m[1][0] = a10; m[1][1] = a11; m[1][2] = a12;
	m[2][0] = a20; m[2][1] = a21; m[2][2] = a22;
}

// -------------------------------------------------------
// MATRIX 3x3 TRANSPOSE 
// carfully in case mt is the same as m
// -------------------------------------------------------
void matrix3x3_transpose(matrix3x3 mt,matrix3x3 m)
{
	int			i,j;
	float 		t;

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

// -------------------------------------------------------
// MATRIX 3x3 DUPLICATE 
// -------------------------------------------------------
matrix3x3 matrix3x3_duplicate(matrix3x3 m)
{
	matrix3x3	m1;
	int			i,j;

	m1 = (float **)malloc(sizeof(float *)*3);
	for (i=0; i<3; i++) m1[i] = (float *)malloc(sizeof(float)*3);

	for (i=0; i<3; i++)
		for (j=0; j<3; j++)
			m1[i][j] = m[i][j];
	return m1;
}

// -------------------------------------------------------
// MATRIX 3x3 COPY 
// note: this does not create a new matrix
// -------------------------------------------------------
void matrix3x3_copy(matrix3x3 m1,matrix3x3 m2)
{
	int			i,j;

	for (i=0; i<3; i++)
		for (j=0; j<3; j++)
			m1[i][j] = m2[i][j];
}

// -------------------------------------------------------
// MATRIX 3x3 COFACTOR 
// -------------------------------------------------------
float matrix3x3_cofactor(matrix3x3 m,int i,int j)
{
	float	a00,a01,a10,a11;
	int		c;
	
	c = ( 2*((i+j)/2) == (i+j) ) ? 1 : -1;
	
	if (i==0) {
		if (j==0) {
			a00 = m[1][1]; a01 = m[1][2];
			a10 = m[2][1]; a11 = m[2][2];
		}
		else if (j==1) {
			a00 = m[1][0]; a01 = m[1][2];
			a10 = m[2][0]; a11 = m[2][2];
		}
		else {
			a00 = m[1][0]; a01 = m[1][1];
			a10 = m[2][0]; a11 = m[2][1];
		}
	}
	else if (i==1) {
		if (j==0) {
			a00 = m[0][1]; a01 = m[0][2];
			a10 = m[2][1]; a11 = m[2][2];
		}
		else if (j==1) {
			a00 = m[0][0]; a01 = m[0][2];
			a10 = m[2][0]; a11 = m[2][2];
		}
		else {
			a00 = m[0][0]; a01 = m[0][1];
			a10 = m[2][0]; a11 = m[2][1];
		}
	}
	else {
		if (j==0) {
			a00 = m[0][1]; a01 = m[0][2];
			a10 = m[1][1]; a11 = m[1][2];
		}
		else if (j==1) {
			a00 = m[0][0]; a01 = m[0][2];
			a10 = m[1][0]; a11 = m[1][2];
		}
		else {
			a00 = m[0][0]; a01 = m[0][1];
			a10 = m[1][0]; a11 = m[1][1];
		}
	}
	
	return (c*(a00*a11 - a01*a10));

}

// -------------------------------------------------------
// MATRIX 3x3 DETERMINANT 
// -------------------------------------------------------
float matrix3x3_determinant(matrix3x3 m)
{ 
	float	c1,c2,c3,d;
	
	c1 = m[0][0]*(m[1][1]*m[2][2]-m[1][2]*m[2][1]);
	c2 = m[0][1]*(m[1][0]*m[2][2]-m[1][2]*m[2][0]);
	c3 = m[0][2]*(m[1][0]*m[2][1]-m[1][1]*m[2][0]);
	printf("deteminant terms: %f %f %f\n",c1,c2,c3);
	return( c1 - c2 + c3);
	
}

// -------------------------------------------------------
// MATRIX 3x3 INVERSE 
// -------------------------------------------------------
void matrix3x3_inverse(matrix3x3 minv,matrix3x3 m)
{
	float		detm;
	int			i,j;

	matrix3x3_print(m);
	detm = matrix3x3_determinant(m);
	
	printf("determinant: %f\n",detm);
	
	for (i=0; i<3; i++) 
		for (j=0; j<3; j++)
			minv[j][i] = matrix3x3_cofactor(m,i,j)/detm;
	
}

// -------------------------------------------------------
// MATRIX 3x3 SET X ROTATE */
// -------------------------------------------------------
void matrix3x3_set_x_rotate(matrix3x3 m,float angle)
{
	double radians,ca,sa;
	
	radians = angle*3.1415926535/180;
	ca = cos(radians);
	sa = sin(radians);
	
	m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0;
	m[1][0] = 0.0; m[1][1] =  ca; m[1][2] = -sa;
	m[2][0] = 0.0; m[2][1] =  sa; m[2][2] =  ca;
}

// -------------------------------------------------------
/* MATRIX 3x3 SET Y ROTATE */
// -------------------------------------------------------
void matrix3x3_set_y_rotate(matrix3x3 m,float angle)
{
	float radians,ca,sa;
	
	radians = angle*3.1415926535/180;
	ca = cos(radians);
	sa = sin(radians);
	
	m[0][0] = ca;  m[0][1] = 0.0; m[0][2] = sa;
	m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0;
	m[2][0] = -sa; m[2][1] = 0.0; m[2][2] = ca;
}

// -------------------------------------------------------
/* MATRIX 3x3 SET Z ROTATE */
// -------------------------------------------------------
void matrix3x3_set_z_rotate(matrix3x3 m,float angle)
{
	float radians,ca,sa;
	
	radians = angle*3.1415926535/180;
	ca = cos(radians);
	sa = sin(radians);
	
	m[0][0] = ca;  m[0][1] = -sa; m[0][2] = 0.0;
	m[1][0] = sa;  m[1][1] =  ca; m[1][2] = 0.0;
	m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0;
}

// -------------------------------------------------------
// MATRIX 3x3 MULTIPLY: A = B.C */
// -------------------------------------------------------
void matrix3x3_multiply(matrix3x3 a,matrix3x3 b,matrix3x3 c)
{
	matrix3x3	t;
	int		i,j,k;
	
	t = matrix3x3_allocate();
	
	for (i=0; i<3; i++) {
		for (j=0; j<3; j++) {
			t[i][j] = 0.0;
			for (k=0; k<3; k++) {
				t[i][j] += b[i][k]*c[k][j];
			}
		}
	}
	for (i=0; i<3; i++)  for (j=0; j<3; j++) a[i][j] = t[i][j];
	matrix3x3_deallocate(t);
}

// -------------------------------------------------------
// MATRIX 3x3 ADD: A = B + C */
// -------------------------------------------------------
void matrix3x3_add(matrix3x3 a,matrix3x3 b,matrix3x3 c)
{
	int		i,j;
	
	for (i=0; i<3; i++) {
		for (j=0; j<3; j++) {
			a[i][j] = b[i][j]+c[i][j];
		}
	}
}

// -------------------------------------------------------
// MATRIX 3x3 SUBTRACT: A = B - C */
// -------------------------------------------------------
void matrix3x3_subtract(matrix3x3 a,matrix3x3 b,matrix3x3 c)
{
	int		i,j;
	
	for (i=0; i<3; i++) {
		for (j=0; j<3; j++) {
			a[i][j] = b[i][j]-c[i][j];
		}
	}
}

// -------------------------------------------------------
// MATRIX 3x3 MULTIPLY VECTOR: A = B.C
// -------------------------------------------------------
void matrix3x3_multiply_vector(vector3 w,matrix3x3 a,vector3 v)
{
	float	t[3];
	
	t[0] = a[0][0]*v[0]+a[0][1]*v[1]+a[0][2]*v[2];
	t[1] = a[1][0]*v[0]+a[1][1]*v[1]+a[1][2]*v[2];
	t[2] = a[2][0]*v[0]+a[2][1]*v[1]+a[2][2]*v[2];
	
	w[0] = t[0]; w[1] = t[1]; w[2] = t[2];
}

// -------------------------------------------------------
// MATRIX 3x3 MULTIPLY VECTOR: A = B.C
// -------------------------------------------------------
void matrix3x3_multiply_constant(matrix3x3 a,float c)
{
	a[0][0] *= c; a[0][1] *= c; a[0][2] *= c;
	a[1][0] *= c; a[1][1] *= c; a[1][2] *= c;
	a[2][0] *= c; a[2][1] *= c; a[2][2] *= c;
}

// -------------------------------------------------------
// MATRIX 3x3 PRINT
// -------------------------------------------------------
void matrix3x3_print(matrix3x3 a)
{
	printf("%f %f %f\n",a[0][0],a[0][1],a[0][2]);
	printf("%f %f %f\n",a[1][0],a[1][1],a[1][2]);
	printf("%f %f %f\n",a[2][0],a[2][1],a[2][2]);
}

// -------------------------------------------------------
// VECTOR 3 RIGID TRANFORM
// -------------------------------------------------------
void vector3_rigid_transform(vector3 pp,vector3 p,matrix3x3 R, vector3 T)
{
	
	matrix3x3_multiply_vector(pp,R,p);
	pp[0] += T[0];
	pp[1] += T[1];
	pp[2] += T[2];
	
}
	
// -------------------------------------------------------
// VECTOR 3 NORMALIZE
// -------------------------------------------------------
void  vector3_normallize(vector3 A) 
{ 
	float	len;
	
	len = vector3_norm(A);
	A[0] /= len;
	A[1] /= len;
	A[2] /= len;
}

	
// -------------------------------------------------------
// VECTOR 3 AXIS ANGLE ROTATE
// -------------------------------------------------------
void  vector3_axisangle_rotate(vector3 pp,vector3 p,vector3 a,float angle) 
{
	matrix3x3	Abar,Astar;
	matrix3x3	identity;
	matrix3x3	m;
	float		ca,sa;
	float		d;
	
	if (angle>360) { d = (int)(angle/360); angle = angle - d*360; }
	
	ca = cos(angle); sa = sin(angle);
	Abar = matrix3x3_allocate();
	Astar = matrix3x3_allocate();
	identity = matrix3x3_allocate();
	matrix3x3_set_identity(identity);
	m = matrix3x3_allocate();
	
	Abar[0][0] = a[0]*a[0]; Abar[0][1] = a[0]*a[1]; Abar[0][2] = a[0]*a[2];
	Abar[1][0] = a[1]*a[0]; Abar[1][1] = a[1]*a[1]; Abar[1][2] = a[1]*a[2];
	Abar[2][0] = a[2]*a[0]; Abar[2][1] = a[2]*a[1]; Abar[2][2] = a[2]*a[2];
	
	Astar[0][0] =   0.0; Astar[0][1] = -a[2]; Astar[0][2] =  a[1];
	Astar[1][0] =  a[2]; Astar[1][1] =   0.0; Astar[1][2] = -a[0];
	Astar[2][0] = -a[1]; Astar[2][1] =  a[0]; Astar[2][2] =   0.0;
	
	// M = ABar + cos*(I-Abar) + sin*Astar
	matrix3x3_subtract(m,identity,Abar);
	matrix3x3_multiply_constant(m,ca);
	matrix3x3_multiply_constant(Astar,sa);
	matrix3x3_add(m,Abar,m);
	matrix3x3_add(m,Astar,m);
	
	// pp = M P
	matrix3x3_multiply_vector(pp,m,p);
	
	matrix3x3_deallocate(identity);
	matrix3x3_deallocate(m);
}

// ================================================================================
// 4x4 matrix routines
// ================================================================================
	
// -----------------------------------------------------------
// MATRIX SET IDENTITY 
// -----------------------------------------------------------
void matrix_set_identity(matrix m)
{
	m[ 0] = 1.0; m[ 4] = 0.0; m[ 8] = 0.0; m[12] = 0.0;
	m[ 1] = 0.0; m[ 5] = 1.0; m[ 9] = 0.0; m[13] = 0.0;
	m[ 2] = 0.0; m[ 6] = 0.0; m[10] = 1.0; m[14] = 0.0;
	m[ 3] = 0.0; m[ 7] = 0.0; m[11] = 0.0; m[15] = 1.0;
}

// -----------------------------------------------------------
/* MATRIX SET SCALE */
// -----------------------------------------------------------
void matrix_set_scale(matrix m,float sx,float sy,float sz)
{
	m[ 0] =  sx; m[ 4] = 0.0; m[ 8] = 0.0; m[12] = 0.0;
	m[ 1] = 0.0; m[ 5] =  sy; m[ 9] = 0.0; m[13] = 0.0;
	m[ 2] = 0.0; m[ 6] = 0.0; m[10] =  sz; m[14] = 0.0;
	m[ 3] = 0.0; m[ 7] = 0.0; m[11] = 0.0; m[15] = 1.0;
}

// -----------------------------------------------------------
/* MATRIX SET TRANSLATE */
// -----------------------------------------------------------
void matrix_set_translate(matrix m,float tx,float ty,float tz)
{
	m[ 0] = 1.0; m[ 4] = 0.0; m[ 8] = 0.0; m[12] =  tx;
	m[ 1] = 0.0; m[ 5] = 1.0; m[ 9] = 0.0; m[13] =  ty;
	m[ 2] = 0.0; m[ 6] = 0.0; m[10] = 1.0; m[14] =  tz;
	m[ 3] = 0.0; m[ 7] = 0.0; m[11] = 0.0; m[15] = 1.0;
}

// -----------------------------------------------------------
/* MATRIX SET TRANSLATE */
// -----------------------------------------------------------
void matrix_concatenate_translate(matrix m,float tx,float ty,float tz)
{
	m[12] =  m[0]*tx + m[4]*ty + m[8]*tz  + m[12]; 
	m[13] =  m[1]*tx + m[5]*ty + m[9]*tz  + m[13]; 
	m[14] =  m[2]*tx + m[6]*ty + m[10]*tz + m[14];
	m[15] =  m[3]*tx + m[7]*ty + m[11]*tz + m[15];
}

// -----------------------------------------------------------
/* MATRIX SET X ROTATE */
// -----------------------------------------------------------
void matrix_set_x_rotate(matrix m,float angle)
{
	double radians,ca,sa;
	
	radians = angle*3.1415926535/180;
	
	ca = cos(radians);
	sa = sin(radians);
	
	m[ 0] = 1.0; m[ 4] = 0.0; m[ 8] = 0.0; m[12] = 0.0;
	m[ 1] = 0.0; m[ 5] =  ca; m[ 9] = -sa; m[13] = 0.0;
	m[ 2] = 0.0; m[ 6] =  sa; m[10] =  ca; m[14] = 0.0;
	m[ 3] = 0.0; m[ 7] = 0.0; m[11] = 0.0; m[15] = 1.0;
}

// -----------------------------------------------------------
/* MATRIX SET Y ROTATE */
// -----------------------------------------------------------
void matrix_set_y_rotate(matrix m,float angle)
{
	float radians;
	
	radians = angle*3.1415926535/180;
	m[ 0] = cos(radians);  m[ 4] = 0.0; m[ 8] = sin(radians); m[12] = 0.0;
	m[ 1] = 0.0;           m[ 5] = 1.0; m[ 9] = 0.0;          m[13] = 0.0;
	m[ 2] = -sin(radians); m[ 6] = 0.0; m[10] = cos(radians); m[14] = 0.0;
	m[ 3] = 0.0;           m[ 7] = 0.0; m[11] = 0.0;          m[15] = 1.0;
}

// -----------------------------------------------------------
/* MATRIX SET Z ROTATE */
// -----------------------------------------------------------
void matrix_set_z_rotate(matrix m,float angle)
{
	float radians;
	
	radians = angle*3.1415926535/180;
	m[ 0] = cos(radians); m[ 4] = -sin(radians); m[ 8] = 0.0; m[12] = 0.0;
	m[ 1] = sin(radians); m[ 5] =  cos(radians); m[ 9] = 0.0; m[13] = 0.0;
	m[ 2] = 0.0;          m[ 6] = 0.0;           m[10] = 1.0; m[14] = 0.0;
	m[ 3] = 0.0;          m[ 7] = 0.0;           m[11] = 0.0; m[15] = 1.0;
}


// -----------------------------------------------------------
/* MATRIX MULTIPLY A = B.C */
// -----------------------------------------------------------
void matrix_multiply(matrix a,matrix b,matrix c)
{
	matrix	t;
	int		i,j,k;
	
	for (i=0; i<4; i++) {
		for (j=0; j<4; j++) {
			t[4*i+j] = 0.0;
			for (k=0; k<4; k++) {
				t[4*i+j] += b[4*i+k]*c[4*k+j];
			}
		}
	}
	for (i=0; i<16; i++) a[i] = t[i];
}

// -----------------------------------------------------------
// PRINT MATRIX 
// -----------------------------------------------------------
void matrix_print(float **A)
{
	int	i,j;
	
	for (i=0; i<4; i++) {
		for (j=0; j<4; j++) {
			printf("%f ",A[i][j]);
		}
		printf("\n");
	}
}

// ================================================================================
// NxM matrix routines
// ================================================================================
	
// -------------------------------------------------------
// MATRIX NxM ALLOCATE
// -------------------------------------------------------
matrixNxM matrix_allocate_NxM(int n,int m)
{
	int			i;
	matrixNxM	m1;
	
	m1 = (float **)malloc(sizeof(float *)*n);
	for (i=0; i<n; i++) m1[i] = (float *)malloc(sizeof(float)*m);
	return m1;
}

// -----------------------------------------------------------
// MATRIX MULTIPLY NxMxP: A = B.C 
// A is nXp; B is nXm; C is mXp
// -----------------------------------------------------------
void matrixNxMxP_multiply(matrixNxM a,matrixNxM b,matrixNxM c,int n, int m,int p)
{
	matrixNxM	t;
	int			i,j,k;
	
	t = (float **)malloc(sizeof(float *)*n);
	for (i=0; i<n; i++) t[i] = (float *)malloc(sizeof(float)*p);
	
	for (i=0; i<n; i++) {
		for (j=0; j<p; j++) {
			t[i][j] = 0.0;
			for (k=0; k<m; k++) {
				t[i][j] += b[i][k]*c[k][j];
			}
		}
	}
	for (i=0; i<n; i++)  for (j=0; j<p; j++) a[i][j] = t[i][j];
}

/* --------------------------------------------------------------------------- */
/* PRINT MATRIX NxM */
/* --------------------------------------------------------------------------- */
void matrixNxM_print(matrixNxM A,int n,int m)
{
	int	i,j;
	
	for (i=0; i<n; i++) {
		for (j=0; j<m; j++) {
			printf("%f ",A[i][j]);
		}
		printf("\n");
	}
}