//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
#include <main.h>
#include <matrix.h>
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////

     
//////////////////////////////////////////////////////////////
// funcao que aloca uma matriz de transformacao
double* allocate_matrix(void)
{
	double *matrix = (double*)malloc(sizeof(double)*DIM_MATRIX*DIM_MATRIX+1);

	if(matrix == NULL)
	{
		printf("\n ERRO: Alocacao da matriz - allocate_matrix - MATRIX.H \n");	
		exit(1);	
	}
	return(matrix);
}
//////////////////////////////////////////////////////////////
// funcao que desaloca uma matriz de transformacao
void deallocate_matrix(double *matrix)
{
	free(matrix);
	matrix = NULL;
}
//////////////////////////////////////////////////////////////
// funcao que aloca uma matriz de transformacao
double* allocate_vector(void)
{
	double *vector = (double*)malloc(sizeof(double)*DIM_MATRIX);

	if(vector == NULL)
	{
		printf("\n ERRO: Alocacao da matriz - allocate_vector - MATRIX.H \n");	
		exit(1);	
	}
	return(vector);
}
//////////////////////////////////////////////////////////////
// funcao que desaloca uma matriz de transformacao
void deallocate_vector(double *vector)
{
	free(vector);
	vector = NULL;
}
//////////////////////////////////////////////////////////////
// preenche a matriz de rotacao do eixo x
void set_rotation_x_matrix(double *m, double angle)
{
	m[0*DIM_MATRIX + 0] = 1.0;
	m[0*DIM_MATRIX + 1] = 0.0;
	m[0*DIM_MATRIX + 2] = 0.0;	
	m[0*DIM_MATRIX + 3] = 0.0;

	m[1*DIM_MATRIX + 0] = 0.0;
	m[1*DIM_MATRIX + 1] = 1.0;
	m[1*DIM_MATRIX + 2] = 0.0;
	m[1*DIM_MATRIX + 3] = 0.0;

	m[2*DIM_MATRIX + 0] = 0.0;
	m[2*DIM_MATRIX + 1] = 0.0;
	m[2*DIM_MATRIX + 2] = cos(angle);
	m[2*DIM_MATRIX + 3] = -sin(angle);;

	m[3*DIM_MATRIX + 0] = 0.0;
	m[3*DIM_MATRIX + 1] = 0.0;
	m[3*DIM_MATRIX + 2] = sin(angle);
	m[3*DIM_MATRIX + 3] = cos(angle);;
}
//////////////////////////////////////////////////////////////
// preenche a matriz de rotacao do eixo y
void set_rotation_y_matrix(double *m, double angle)
{
	m[0*DIM_MATRIX + 0] = 1.0;
	m[0*DIM_MATRIX + 1] = 0.0;
	m[0*DIM_MATRIX + 2] = 0.0;
	m[0*DIM_MATRIX + 3] = 0.0;

	m[1*DIM_MATRIX + 0] = 0.0;
	m[1*DIM_MATRIX + 1] = cos(angle);
	m[1*DIM_MATRIX + 2] = 0.0;
	m[1*DIM_MATRIX + 3] = sin(angle);

	m[2*DIM_MATRIX + 0] = 0.0;
	m[2*DIM_MATRIX + 1] = 0.0;
	m[2*DIM_MATRIX + 2] = 1.0;
	m[2*DIM_MATRIX + 3] = 0.0;

	m[3*DIM_MATRIX + 0] = 0.0;
	m[3*DIM_MATRIX + 1] = -sin(angle);
	m[3*DIM_MATRIX + 2] = 0.0;
	m[3*DIM_MATRIX + 3] = cos(angle);
}
//////////////////////////////////////////////////////////////
// preenche a matriz de rotacao do eixo z
void set_rotation_z_matrix(double *m, double angle)
{
	m[0*DIM_MATRIX + 0] = 1.0;
	m[0*DIM_MATRIX + 1] = 0.0;
	m[0*DIM_MATRIX + 2] = 0.0;
	m[0*DIM_MATRIX + 3] = 0.0;

	m[1*DIM_MATRIX + 0] = 0.0;
	m[1*DIM_MATRIX + 1] = cos(angle);
	m[1*DIM_MATRIX + 2] = -sin(angle);
	m[1*DIM_MATRIX + 3] = 0.0;

	m[2*DIM_MATRIX + 0] = 0.0;
	m[2*DIM_MATRIX + 1] = sin(angle);
	m[2*DIM_MATRIX + 2] = cos(angle);
	m[2*DIM_MATRIX + 3] = 0.0;

	m[3*DIM_MATRIX + 0] = 0.0;
	m[3*DIM_MATRIX + 1] = 0.0;
	m[3*DIM_MATRIX + 2] = 0.0;
	m[3*DIM_MATRIX + 3] = 1.0;
}
//////////////////////////////////////////////////////////////
// preenche a matriz de rotacao do eixo z
void set_translation_matrix(double *m, double tx, double ty, double tz)
{
	m[0*DIM_MATRIX + 0] = 1.0;
	m[0*DIM_MATRIX + 1] = 0.0;
	m[0*DIM_MATRIX + 2] = 0.0;
	m[0*DIM_MATRIX + 3] = 0.0;

	m[1*DIM_MATRIX + 0] = tx;
	m[1*DIM_MATRIX + 1] = 1.0;
	m[1*DIM_MATRIX + 2] = 0.0;
	m[1*DIM_MATRIX + 3] = 0.0;

	m[2*DIM_MATRIX + 0] = ty;
	m[2*DIM_MATRIX + 1] = 0.0;
	m[2*DIM_MATRIX + 2] = 1.0;
	m[2*DIM_MATRIX + 3] = 0.0;

	m[3*DIM_MATRIX + 0] = tz;
	m[3*DIM_MATRIX + 1] = 0.0;
	m[3*DIM_MATRIX + 2] = 0.0;
	m[3*DIM_MATRIX + 3] = 1.0;
}

//////////////////////////////////////////////////////////////
// funcao que preenche a matriz identidade
void identity_matrix(double *inv,int n)
{
	int	row,col;

	for (row = 0; row < n; ++row)
	{
		for (col = 0; col < n; ++col)
		{
			if (row == col)
				 inv[row*n + col] = 1.0;
			else 
				 inv[row*n + col] = 0.0;
		}
	}
}
//////////////////////////////////////////////////////////////
// 
int invert_matrix(double *matrix,double *inv, int n)
/*** Returns a value of 1 if mat is invertable. ***/
/*** Returns a value of 0 if mat is not invertable. ***/
{     

	double	m,*temp = NULL;
	int	i,j,col,finder = 0;
	
	temp = allocate_matrix();	

	for( i = 0; i < n; ++i)
		for (j = 0; j < n; ++j)
			temp[i*n + j] = matrix[i*n + j];

	identity_matrix(inv,n);

	for (i = 0; i < n; ++i)
	{	
		finder = i;
		if (temp[i*n+ i]==0.0)	
		/*** Find a row with a non-zero ***/
		/*** ith element and add it to  ***/
		/*** the ith row.               ***/
		{	
			while ( (finder < n) && (temp[finder*n + i] == 0.0) )
				++finder;
			if (finder < n)
				for (col = 0; col < n; ++col)
				{	
					temp[i*n + col] += temp[finder*n+col];
					inv[i*n+col] += inv[finder*n+col];
				}
		}
		if (finder < n)
		{	
			m = temp[i*n+i];
			for (col = 0; col < n; ++col)
			{	
				temp[i*n+col] /= m;
				inv[i*n+col] /= m;
			}
			for (j = i+1; j< n; ++j)
			{	
				m = - temp[j*n+i];
				for (col = 0; col < n; ++col)
				{	
					temp[j*n+col] += ( temp[i*n+col] * m );
					inv[j*n+col] += ( inv[i*n+col] * m );
				}
			}
		}
		else printf ("\nMatrix uninvertable.");
	}

	if (finder < n)
	for (i = 0; i < n; ++i)
		for (j = i+1; j < n; ++j)
		{
			m = - temp[i*n+j];
			for (col = 0; col < n; ++col)
			{	
				temp[i*n+col] += ( temp[j*n+col] * m );
				inv[i*n+col] += ( inv[j*n+col] * m );
			}
		}


	deallocate_matrix(temp);

	if (finder < n)
	    return (1);
	else return (0);

} 
//////////////////////////////////////////////////////////////
// funcao que multiplica duas matrizes
void mult_matrix(double *a,double *b, double *c,int n)
{
 	int i,j,k;
 	double *aux = NULL;
 
 	aux = allocate_matrix();
 
 //inicializa a matriz que vai receber o resultado 
 for(i = 0; i < n; i++) 
  for(j = 0; j < n; j++)
   aux[i*n+j] = 0.0;
 
 //realiza a multiplicacao  
 for(i = 0; i < n; i++)
  for(j = 0; j < n; j++)
   for(k = 0; k < n; k++)
    aux[i*n + j] = aux[i*n + j] + a[i*n + k]*b[k*n + j];
   
 
 //realiza a transferencia de valores 
 for(i = 0; i < n; i++) 
  for(j = 0; j < n; j++)
   c[i*n+j] = aux[i*n+j];
   
  deallocate_matrix(aux);	
}
//////////////////////////////////////////////////////////////
// funcao que realiza a inversao de modo preciso
double * inverse(double *matrix)
{
	int i = 0,
		 j = 0;
	
	double *m = NULL, 
          *n = NULL,
			 *residual = NULL,
			 *inv = NULL,	
			 *temp = NULL; 

	//aloca as matrizes
	m = allocate_matrix();	
	n = allocate_matrix();
	residual = allocate_matrix();
	temp = allocate_matrix();
	inv = allocate_matrix();
	
	
	//inverte a matriz
	invert_matrix(matrix,temp,DIM_MATRIX);
	//multiplica para calcular o residuo
	mult_matrix(matrix,temp,residual,DIM_MATRIX);
	
		
	//calcula o residuo para aumentar a precisao
	for(i = 0; i < DIM_MATRIX; i++)
	{		
		for(j = 0; j < DIM_MATRIX; j++)
		{
			if(i == j)
				residual[i*DIM_MATRIX + j] = residual[i*DIM_MATRIX + j] - 1.0;			
		}
	}

	//multiplica o residuo pela inversa
	mult_matrix(residual,temp,n,DIM_MATRIX);
	//soma os valores da matrizes calculadas
	for(i = 0; i < DIM_MATRIX; i++)
	{		
		for(j = 0; j < DIM_MATRIX; j++)
		{
				inv[i*DIM_MATRIX + j] = n[i*DIM_MATRIX + j] + temp[i*DIM_MATRIX + j]; 
		}
	}
  	
	//calcula o erro novamente
	//depois pode ser retirado
 	mult_matrix(matrix,inv,residual,DIM_MATRIX);

	
	
	return(inv);	
}
//////////////////////////////////////////////////////////////
// funcao que realiza a multiplicacao da matrix pelo vetor
void mult_matrix_vector(double *matrix, double *vector, double *result, int n)
{
 int i,j;
 
 //calcula a multiplicacao  
 for(i = 0; i < n; i++)
 {	
	result[i] = 0.0;	
  	for(j = 0; j < n; j++)
  	{
   	result[i] += matrix[i*n + j]*vector[j];
  	}
 }
}
//////////////////////////////////////////////////////////////
// funcao que realiza a multiplicacao de um vetor por uma matrix
void mult_vector_matrix(double *vector,double *matrix, double *result, int n)
{
 int i,j;
 
 //calcula a multiplicacao  
 for(i = 0; i < n; i++)
 {	
	result[i] = 0.0;	
  	for(j = 0; j < n; j++)
  	{
   	result[i] += vector[j]*matrix[j*n + i];
  	}
 }
}