/************************************************************************
 *                                                                      *
 *  Program package         T O O L D I A G                             *
 *                                                                      *
 *  Version   1.5                                                       *
 *  Date:     Tue Feb  8 13:39:03 1994                                  *
 *                                                                      *
 *  NOTE: This program package is copyrighted in the sense that it      *
 *  may be used for scientific purposes. The package as a whole, or     *
 *  parts thereof, cannot be included or used in any commercial         *
 *  application without written permission granted by the author.       *
 *  No programs contained in this package may be copied for commercial  *
 *  distribution.                                                       *
 *                                                                      *
 *  All comments  concerning this program package may be sent to the    *
 *  e-mail address 'tr@fct.unl.pt'.                                     *
 *                                                                      *
 ************************************************************************/



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

static char buf[80];

/*
 *  Print Matrix
*/
void print_Matrix( M )
Matrix *M;
{ 
 int i, j;

 for( i = 0; i < M->row; i++ )
 {
   for( j = 0; j < M->col; j++)
     printf(" %7.5f ", (float)M->Elem[i*M->col+j]);
   printf("\n");
 }
 printf("\n");
}


/*
 *  Allocate memory for the elements of the matrix
 *   the row and column values must be set
*/
void Matrix_Alloc( M )
Matrix *M;
{
 if( (M->row == 0) || (M->col == 0) )
   fprintf(stderr,"Warning: trying allocate space for an empty matrix!!!\n");
 if( M->Elem != NULL )
   fprintf(stderr,"Warning: trying allocate space for a non-empty pointer!\n");
 M->Elem = (MatElem*) malloc(M->row * M->col * sizeof(MatElem));
 if( M->Elem == NULL )
 {
   fprintf(stderr,"No memory allocated for matrix with dimensions %d x %d ...",
                    M->row, M->col); gets( buf );
 }
}


/*
 *  Free the memory for the elements of a matrix
*/
void Matrix_Free( M )
Matrix *M;
{ if( M->Elem != NULL ) { free(M->Elem); M->Elem = NULL; }
  /* else fprintf(stderr,"Warning: trying to free already free space !\n");/**/
}


/*
 *  Initializiation of a matrix: must be called before any use
*/
void init_Matrix( M )
Matrix *M;
{ M->row = 0; M->col = 0; M->Elem = NULL; }

/*
 * Return the unit matrix of dimension 'dim'
*/
void Unit_Matrix( M, dim )
Matrix *M;
int dim;
{
 int i, j;

 Matrix_Free( M );
 M->row = dim; M->col = dim;
 Matrix_Alloc( M );
 for( i = 0; i < M->row; i++ )
   for( j = 0; j < M->col; j++)
     if( i == j )
       M->Elem[i*M->col+j] = 1.0;
     else
       M->Elem[i*M->col+j] = 0.0;
}

/*
 * Copy A to B
*/
void Copy_Matrix( A, B )
Matrix *A, *B;
{
 long int index;

 Matrix_Free( B );
 B->row = A->row; B->col = A->col;
 Matrix_Alloc( B );
 for( index = 0; index < (long int)(B->row*B->col); index++)
   B->Elem[index] = A->Elem[index];
}

/*
 *  Zero all elements of a matrix
*/
void Reset_Matrix( M )
Matrix *M;                         /*  M=0  */
{
 long int index;

 for( index = 0; index < (long int)(M->row*M->col); index++)
   M->Elem[index] = 0.0;
}

/*
 *  Adding two matrices
*/
void Add_Matrix( A, B, C )
Matrix *A, *B, *C;                  /*  C=A+B  */
{
 long int index;

 if( (A->row != B->row) || (A->col != B->col) )
 {
   fprintf( stderr, "Add_Matrix> Cannot add matrices !!!..."); gets(buf);
 }
 else
 {
   Matrix_Free( C );
   C->row = A->row; C->col = A->col;
   Matrix_Alloc( C );
   for( index = 0; index < (long int)(C->row*C->col); index++)
     C->Elem[index] = A->Elem[index] + B->Elem[index];
 }
}


/*
 *  Subtracting two matrices
*/
void Subtract_Matrix( A, B, C )
Matrix *A, *B, *C;                  /*  C=A-B  */
{
 long int index;

 if( (A->row != B->row) || (A->col != B->col) )
 {
   fprintf( stderr, "Subtract_Matrix> Cannot subtract matrices !!!...");
   gets(buf);
 }
 else
 {
   Matrix_Free( C );
   C->row = A->row; C->col = A->col;
   Matrix_Alloc( C );
   for( index = 0; index < (long int)(C->row*C->col); index++)
     C->Elem[index] = A->Elem[index] - B->Elem[index];
 }
}


/*
 *  Multiplying a matrix with a scalar
*/
void ScalarMult_Matrix( Scalar, M )
MatElem Scalar;
Matrix *M;
{
 long int index;

 for( index = 0; index < (long int)(M->row*M->col); index++)
   M->Elem[index] *= Scalar;
}


/*
 *  Multiply two matrices
*/
void Mult_Matrix( A, B, C )
Matrix *A, *B, *C;                  /*  C=A*B  */
{
 int i, j, k;

 if( A->col != B->row )
 {
   fprintf( stderr, "Mult_Matrix> Cannot multiply matrices !!!..."); gets(buf);
 }
 else
 {
   Matrix_Free( C );
   C->row = A->row; C->col = B->col;
   Matrix_Alloc( C );
   /* Multiply */
   for( i = 0; i < C->row; i++ )
   {
     for( j = 0; j < C->col; j++)
     {
       C->Elem[i*C->col+j] = 0.0;
       for( k = 0; k < A->col; k++ )
       {
         C->Elem[i*C->col+j] += A->Elem[i*A->col+k] * B->Elem[k*B->col+j];
       }
     }
   }
 }
}

/*
 *  Transpose Matrix
*/
void Transpose_Matrix( M, T )
Matrix *M, *T;
{
 int i, j;

 Matrix_Free( T );
 T->row = M->col; T->col = M->row;
 Matrix_Alloc( T );
 /* Transpose */
 for( i = 0; i < M->row; i++ )
 {
   for( j = 0; j < M->col; j++) {
     T->Elem[j*T->col+i] = M->Elem[i*M->col+j];
   }
 }
}


/*
 *  Splitting a square matrix in four parts
 *                  p               q
 *		(         |			)
 *  	p	(   M11   |   M12		)
 *		(         |			)
 *		(   ------|---------------------)
 *		(         |			)
 *		(         |			)
 *      q	(         |			)
 *		(   M21   |    M22		)
 *		(         |			)
 *		(         |			)
 *		(         |			)
 *		(         |			)
 *		(         |			)
*/
void Split_Matrix( M, M11, M12, M21, M22, p, q )
Matrix *M, *M11, *M12, *M21, *M22;
int p, q;
{
 int i, j;

 if( (M->row != M->col) || (M->row != (p+q)) )
 {
   fprintf( stderr, "Split_Matrix> problems !!!..."); gets(buf);
 }
 else
 {
   Matrix_Free( M11 ); M11->row = p; M11->col = p; Matrix_Alloc( M11 );
   Matrix_Free( M12 ); M12->row = p; M12->col = q; Matrix_Alloc( M12 );
   Matrix_Free( M21 ); M21->row = q; M21->col = p; Matrix_Alloc( M21 );
   Matrix_Free( M22 ); M22->row = q; M22->col = q; Matrix_Alloc( M22 );

   for( i = 0; i < M11->row; i++ )
     for( j = 0; j < M11->col; j++)
       M11->Elem[i*M11->col+j] = M->Elem[i*M->col+j];

   for( i = 0; i < M12->row; i++ )
     for( j = 0; j < M12->col; j++)
       M12->Elem[i*M12->col+j] = M->Elem[i*M->col+j+p];

   for( i = 0; i < M21->row; i++ )
     for( j = 0; j < M21->col; j++)
       M21->Elem[i*M21->col+j] = M->Elem[(i+p)*M->col+j];

   for( i = 0; i < M22->row; i++ )
     for( j = 0; j < M22->col; j++)
       M22->Elem[i*M22->col+j] = M->Elem[(i+p)*M->col+j+p];
 }
}


void set_Matrix();

/*
 *  Inversion of a square matrix.
 *  Calculates also the determinant of the matrix
*/
void Invert_Matrix( M, Det )
Matrix *M;
MatElem *Det;   /* Determinant */
{
 int k, j, i, n;
 MatElem D = 1.0;

 if( M->row != M->col)
     {fprintf(stderr,"Invert_Matrix> not square matrix !. Exit...\n"); exit(1);}

 n = M->row;
 for(k=0; k<n; k++)
 {
   for(i=0; i<n; i++)
     for(j=0; j<n; j++)
       if( i!=k && j!=k )
         M->Elem[i*n+j] -= M->Elem[k*n+j]*M->Elem[i*n+k]/M->Elem[k*n+k];

   for(i=0; i<n; i++)
     for(j=0; j<n; j++)
       if( i==k && i!=j )
         M->Elem[i*n+j]= -M->Elem[i*n+j]/M->Elem[k*n+k];

   for(i=0; i<n; i++)
     for(j=0; j<n; j++)
       if( j==k && i!=j )
         M->Elem[i*n+j]= M->Elem[i*n+j]/M->Elem[k*n+k];
   D *= M->Elem[k*n+k];
   M->Elem[k*n+k]= 1.0/M->Elem[k*n+k];
 }
 *Det = D; 
}

/*
 * Calculate the trace of a square matrix, i.e. the sum of all diagonal
 * elements of the matrix
*/
MatElem trace( M )
Matrix *M;
{
 MatElem tr = 0.0;
 int j;

 if( M->row != M->col)
     {fprintf(stderr,"trace> not square matrix !. Exit...\n"); exit(1);}

 for( j = 0; j < M->col; j++)
   tr += M->Elem[j*M->col+j];

 return( tr );
}

/* #define TEST	/**/
#ifdef TEST

void get_d( d )
int *d;
{ gets(buf); sscanf(buf,"%d",d); }

void get_f( f )
float *f;
{ gets(buf); sscanf(buf,"%f",f); }
#endif

void get_double( d )
double *d;
{ gets(buf); sscanf(buf,"%lf",d); }


/*
 *  Read Matrix from stdin
*/
void set_Matrix( M )
Matrix *M;
{
 int i, j;

 printf("Rows= "); get_d( &(M->row) ); 
 printf("Colums= "); get_d( &(M->col) ); 
 Matrix_Alloc( M );

 for( i = 0; i < M->row; i++ )
   for( j = 0; j < M->col; j++ )
   {
     printf("M[%d][%d]= ", i+1, j+1);
     get_double( &(M->Elem[i*M->col+j]) );
   }
}

#ifdef TEST
static Matrix T1, T2, T3, T4, T5, T6;

void main()
{
 MatElem Det;
 int p,q;

 init_Matrix( &T1 ); init_Matrix( &T2 ); init_Matrix( &T3 );
 init_Matrix( &T4 ); init_Matrix( &T5 ); init_Matrix( &T6 );

 set_Matrix( &T1 );
 print_Matrix( &T1 );
 set_Matrix( &T2 );
 print_Matrix( &T2 );


 Invert_Matrix( &T2, &Det );
 print_Matrix( &T2 );
 Mult_Matrix( &T2, &T1, &T3 );
 print_Matrix( &T3 );


 Matrix_Free( &T1 );
 Matrix_Free( &T2 );
 Matrix_Free( &T3 );
 Matrix_Free( &T4 );
 Matrix_Free( &T5 );
 Matrix_Free( &T6 );
}
#endif