// Author: Tom M
// Description: A matrix class that handles basic matrix operations, such as
// 		adding, multiplying, transposing, eliminating, permuting,
//		gaussian techniques, as well as reading and printing a matrix.
// $Id: matrix.c,v 1.2 98/10/22 12:34:24 tommut Exp Locker: tommut $
// Revisions:
//Revision 1.7  1998/10/15 21:35:12  tommut
// added Vandermonde matrices, Cholesky factorization, and Normal Equations
// method
//
//Revision 1.6  98/09/19  23:37:34  tommut
// modified gaussian algorithm to use pivoting
//
//Revision 1.5  98/09/15  15:37:20  tommut
// Added permutations
//
//Revision 1.4  98/09/15  15:37:16  tommut
// implemented the back substitution in gaussian function
  
#include 
#include 
    
typedef struct Mat_Cell {
    int rows, cols;
    float **elements;
} Matrix;	     

Matrix *newMatrix( int rows, int cols ) ;
Matrix *readMatrix( char *filename );
Matrix *transpose( Matrix *m );
Matrix *permutationMatrix( int size, int *map );
Matrix *eliminationMatrix( int k, Matrix *m );
Matrix *multiplyMatrix( Matrix *m1, Matrix *m2 );
Matrix *addMatrix( Matrix *m1, Matrix *m2 );
Matrix *gaussMatrix( Matrix *A, Matrix *B );
Matrix *vandermondeMatrix( Matrix *A, int cols );
Matrix *choleskyMatrix( Matrix *A );
Matrix *normalMatrix( Matrix *A, Matrix *B );
float errorAnalysis( Matrix *Lt, Matrix *ret, Matrix *y ); 
void printMatrix( Matrix *m, FILE *f );
void freeMatrix( Matrix *m );

// the main function just tests all of the functions of this class
int main() {
    FILE *fp = fopen( "output", "a" );    

    int map[] = {1, 3, 0, 2};
    Matrix *m1 = readMatrix( "m1" );
    Matrix *m2 = readMatrix( "m" );
    Matrix *m3 = normalMatrix( m1, m2 );
    
    printMatrix( m3 );

    freeMatrix( m1 ); freeMatrix( m2 ); freeMatrix( m3 );
    
    return ( 0 );
}

// Name . newMatrix
// Description . this function allocates memory for a new Matrix structure
// 		 - it runs through each element or the arrays and allocates
//		 memory for it.

Matrix *newMatrix( int rows, int cols ) {
    int i, j;
    Matrix *temp = (Matrix*)malloc( sizeof( Matrix ) );

    temp->rows = rows;
    temp->cols = cols;
    temp->elements = (float**)malloc( (unsigned)rows * sizeof( float* ) );
    for ( i = 0; i < rows; i++ ) {
 	temp->elements[i] = (float*)malloc( (unsigned)cols * sizeof( float )); 
    }
    for ( i = 0; i < rows; i++ ) {
 	for ( j = 0; j < rows; j++ ) {
	    temp->elements[i][j] = 0.0; 
	}
    }
    
    return (temp);
}

// Name . readMatrix
// Description . reads in a matrix from a given filename. 
// the file must be in the following format:
// row column
// 1.0 2.0 3.4 ...
// 3.0 -.1 9.7 ...
// ...  ... ...
  
Matrix *readMatrix( char *filename ) {
    Matrix *temp;
    int rows, cols;
    int i, j;
    FILE *fp = fopen( filename, "r" );

    fscanf( fp, "%d %d", &rows, &cols ) ;
    temp = newMatrix( rows, cols );

    for ( i = 0; i < rows; i++ ) {
	for ( j = 0; j < cols; j++ ) {
	    fscanf( fp, "%f", &temp->elements[i][j] );
        }
    }
    return temp;
}
 
// Name . printMatrix
// Description . just runs through each row and column and prints out the
// 		 individual element
 
void printMatrix( Matrix *m, FILE *f ) {
    int i, j;
    for ( i = 0; i < m->rows; i++ ) {
 	for ( j = 0; j < m->cols; j++ ) {
    	    fprintf( f, "%16.7f", m->elements[i][j] );
	}
	fprintf( f, "\n" );

    }
}

// Name . addMatrix
// Description . does matrix addition on two matrices.  It just adds up each
//		 corresponding matrices elements and stores the result in a 
//		 new matrix.

Matrix *addMatrix( Matrix *m1, Matrix *m2 ){
    int i, j;
    Matrix *temp = newMatrix( m1->rows, m1->cols );
    
    for ( i = 0; i < m1->rows; i++ ) {
	for ( j = 0; j< m1->cols; j++ ) {
	    temp->elements[i][j] = m1->elements[i][j] + m2->elements[i][j];
  	}
    }
    return temp;
} 
    
// Name . multiplyMatrix
// Description . does matrix multiplication on two matrices
// 		 the result[i][k] is the summation of the multiplication of 
//		 the m1->row * m2->cols

Matrix *multiplyMatrix( Matrix *m1, Matrix *m2 ) {
    Matrix *temp = newMatrix( m1->rows, m2->cols );
    float sum1 = 0;
    int i, j, k;

    if ( m1->cols != m2->rows )  {
        printf( "Warning: M1 cols %d not equal to M2 rows %d\n",
						m1->cols, m2->rows );
    }
    for ( k = 0; k < temp->cols; k++ ) {
	for ( i = 0; i < m1->rows; i++ ) {
	    for ( j = 0; j< m1->cols; j++ ) {
		temp->elements[i][k]+=m1->elements[i][j] * 
				     m2->elements[j][k];
	    }
	}
    }
    return ( temp );
}
    
// Name . transpose
// Description . simply transposes a given matrix :
//		 element[i][j] = element[j][i]

Matrix *transpose( Matrix *m ){
    Matrix *temp = newMatrix( m->cols, m->rows );
    int i, j;

    for ( i = 0; i < m->cols; i++ ) {
	for ( j = 0; j< m->rows; j++ ) {
	    temp->elements[i][j] = m->elements[j][i];
        }
    }
    return temp;
} 
    
// Name . eliminationMatrix
// Description . forms an elimination matrix of a given matrix m, for column k.  
Matrix *eliminationMatrix( int k, Matrix *m ){
    Matrix *temp = newMatrix( m->cols, m->rows );
    int i, j = 0;

    // fill the diagonal with 1's
    for ( i = 0; i < m->cols; i++ ) {
	temp->elements[i][i] = 1; 
    }

    // compute the values for row k below the diagonal
    for ( i = k+1; i < temp->rows; i++ ) {
	temp->elements[i][k] = -m->elements[i][k] /
	    		        m->elements[k][k];
    }
    return temp;
}

// Name . gaussMatrix 	
// Description . compute a gaussMatrix with two given matrices.  Matrix A is 
//		 the original known matrix and B is the answer 
//		 This function returns the computed answer to fill in the
//		 unknown variables.

Matrix *gaussMatrix( Matrix *A, Matrix *B ){
    Matrix *ret = newMatrix( A->rows, 1);
    Matrix *Adopple = A; // a copy (doppleganger) of A to preserve the original
    Matrix *Bdopple = B; // a copy (doppleganger) of B to preserve the original
    Matrix *elim = newMatrix( A->rows, A->cols );// hold the elimination matrix
    Matrix *ans = newMatrix( A->rows, A->cols ); // answer of A*elimination
    Matrix *elimans = newMatrix( A->rows, A->cols ); // answer of B*elimination
    Matrix *permMat = newMatrix( A->rows, A->cols ); // for perm matrix
    float x;  // highest value for pivot
    int i, j;
    int xrow;
    int map[A->cols];  

    // first, reduce the matrix through elimination and pivoting
    for ( i = 0; i < Adopple->cols-1; i++ ) {
	x = Adopple->elements[i][i];
	xrow = i;

	// find the highest coefficient
	for ( j = i; j < Adopple->rows; j++ ) {
	    if ( Adopple->elements[j][i] > x ) {
		x = Adopple->elements[j][i];
		xrow = j;
	    }
	}

	// set the permutation map
	for ( j = 0; j < Adopple->cols; j++ ) {
	    map[j] = j;
	}
	j = map[i];
	map[i] = map[xrow]; 
	map[xrow] = j;

	// permute matrix B
	x = Bdopple->elements[xrow][0];
	Bdopple->elements[xrow][0] = Bdopple->elements[i][0];
	Bdopple->elements[i][0] = x;
    
	// permute Matrix A
	permMat = permutationMatrix( Adopple->cols, map );
	elim = multiplyMatrix( permMat, Adopple );
	Adopple = elim;
	    
	// Multiply A and B by their elimination matrix
	elim = eliminationMatrix( i, Adopple );
	//printf( "elim\n" );	
	//printMatrix( elim, stdout );
	elimans = multiplyMatrix( elim, Bdopple );
	//printf( "elimans\n" );	
	//printMatrix( elimans, stdout );
	ans = multiplyMatrix( elim, Adopple );
	//printf( "ans\n" );	
	//printMatrix( ans, stdout );
	Adopple = ans;
	Bdopple = elimans;
    }
    
    // now, use back substitution to get the final variables
    for( i = ret->rows - 1; i >= 0; i--){
        ret->elements[i][0] = Bdopple->elements[i][0];
        for( j = i+1; j < Adopple->cols; j++){ 
            ret->elements[i][0] -= (Adopple->elements[i][j] *
		    		    ret->elements[j][0]);
        }
        ret->elements[i][0] /= Adopple->elements[i][i];
    }

    // free all temporary matrices
    freeMatrix (Adopple); freeMatrix (Bdopple );
    freeMatrix (elim ); freeMatrix (ans );
    freeMatrix (elimans ); freeMatrix (permMat );

    return ret;
}

// Name . permutationMatrix
// Desription . create a permuted square matrix of size X size  and using the
// 		map array to place the 1's

Matrix *permutationMatrix( int size, int *map ){
    Matrix *temp = newMatrix( size, size );
    int i;
    for ( i = 0; i < temp->rows; i++ ) {
	temp->elements[i][*(map++)] = 1;
    }
    return temp;
}

// Name . freeMatrix
// Desription . frees up allocated memory used by matrix
//

void freeMatrix( Matrix *m ) {
    int i;

    for ( i = 0; i < m->rows; i++ ) {
 	free( m->elements[i] ); 
    }
    
    free ( m->elements );
    free ( m );
}

// Name . vandermondeMatrix
// Description . takes in a matrix and the desired degree and forms a 
//		 Vandermonde matrix 
//

Matrix *vandermondeMatrix( Matrix *A, int cols ){
    Matrix *result = newMatrix( A->rows, cols );
    int i, j = 0;
    for ( j = 0; j < cols; j++ ) {
	for ( i = 0; i < A->rows; i++ ) {
	    result->elements[i][j] = pow( A->elements[i][0], j ) ;
	}
    }
    return result;
}

// Name . normalMatrix
// Description . takes in two matrices and uses the normal equations method
// 		 to return the result
//

Matrix *normalMatrix( Matrix *t, Matrix *b ){
    Matrix *A = vandermondeMatrix( t, 4  );
    Matrix *result1 = multiplyMatrix( transpose( A ), A );
    Matrix *result2 = multiplyMatrix( transpose( A ), b ); 
    Matrix *L = choleskyMatrix( result1 );
    Matrix *Lt = transpose( L );

    Matrix *y  = newMatrix( A->cols, 1 );
    Matrix *ret = newMatrix( A->cols, 1 );
    int i, j = 0;

    // print out some information
    printf( "Vandermonde Matrix: A\n" );
    printMatrix( A, stdout );
    printf( "\n\nA^T * A: \n" );
    printMatrix( result1, stdout );
    printf( "\n\nA^T * b: \n" );
    printMatrix( result2, stdout );
    printf( "\n\nCholesky( A^t * A ): L\n" );
    printMatrix( L, stdout );
    printf( "\n\nL^T\n" );
    printMatrix( Lt, stdout );

    // forward subs to solve y
    y->elements[0][0] = result2->elements[0][0]/L->elements[0][0];

    for( i = 1; i < L->cols; i++){
        y->elements[i][0] = result2->elements[i][0];
        for( j = 0; j < i; j++){ 
            y->elements[i][0] -= ( L->elements[i][j] *
		    		    y->elements[j][0] );
        }
        y->elements[i][0] /= L->elements[i][i];
    }


    // and now back subs to solve the x
    for( i = ret->rows - 1; i >= 0; i--){
        ret->elements[i][0] = y->elements[i][0];
        for( j = i+1; j < Lt->cols; j++){ 
            ret->elements[i][0] -= (Lt->elements[i][j] *
		    		    ret->elements[j][0]);
        }
        ret->elements[i][0] /= Lt->elements[i][i];
    }
    
    

    // print results
    printf( "\n\nLy = A^Tb : solve for y with forward subs.:\n" );
    printMatrix( y, stdout );
    printf( "\n\nL^Tx = y : solve for x using back subs:\n" );
    printMatrix( ret, stdout );

    errorAnalysis( A, ret, b); 
    
    // free all temp matrices
    freeMatrix( A );  freeMatrix( result1 ); freeMatrix( result2 );
    freeMatrix( L );  freeMatrix( Lt ); freeMatrix( y );

    return ret;
}


// Name . choleskyMatrix
// Description . takes in a matrix and performs Choleksy factorization on it
//		 and returns the result
//

Matrix *choleskyMatrix( Matrix *A ){
    Matrix *result = newMatrix( A->rows, A->cols );
    int i, j, k;
    double total;

    for( i = 0; i < A->rows; i++){
        total=0;
        for( j = i - 1; j >= 0; j--){
            total += ( result->elements[i][j] ) * ( result->elements[i][j] );
        }
        result->elements[i][i] = pow( A->elements[i][i] - total,0.5 );
        total=0;
        for( j = i + 1; j < A->rows; j++){
            for( k = i - 1; k >= 0; k--){
                total += ( result->elements[i][k] * result->elements[j][k] );
            }
	     
	    // now scale by the square root diagonol 
            result->elements[j][i]=( (A->elements[j][i] - total) /
                                      result->elements[i][i]);
        }
    }
    return result;
}


// Name . errorAnalysis
// Description . calculates the error by finding the norm of the residual 
// 		 vector.
//

float errorAnalysis( Matrix *A, Matrix *x, Matrix *b ){ 
    Matrix *result = newMatrix( A->rows, 1 );
    Matrix *AX = multiplyMatrix( A,x ); 
    float value = 0.0;
    int i = 0;
    float sum = 0;
    
    printf( "b\n" );
    printMatrix( b, stdout );
    printf( "AX\n" );
    printMatrix( AX, stdout );

    for ( i = 0; i < AX->rows; i++ ) {
	value = b->elements[i][0] - AX->elements[i][0];
	result->elements[i][0] = value * value;
	sum += value*value; 
    }
    printf( "Residual Vector\n" );
    printMatrix( result, stdout );

    printf( "Norm of Residual Vector: %20.15f\n", sum );

    freeMatrix( AX );

    return value;
}