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

#define MAX(x,y) ( (x) > (y) ? (x) : (y) )
#define MIN(x,y) ( (x) > (y) ? (y) : (x) )
#define ABS(x) ( (x) > 0 ? (x) : (-(x ) ) )
#define AEPS 1.0e-15

#define ALLOCMEM( s ) calloc( s, 1 )
#define FREEMEM( p ) free( p ) 

#define A( i, j ) a[n * ( i ) + ( j )]

void ludecomp(double *,int, int *);

void mtrinv( double *a, int n )
{
    int i;
    int j;
    int k;
    int *pivot;

    double s;
  
    pivot = (int *)ALLOCMEM( n * sizeof(int) );

    /*
     *  AP = LU
     */
    ludecomp( a, n, pivot );

    for( i = 0; i < n; i++ ) {
        if ( ABS(A(i, i))<AEPS ) {
            fprintf( stderr, "Inv: Matrix is singular.\n" );
            return;
            }
        A(i, i) = 1.0 / A(i, i);
        }

    /*  
     *  INV(U)
     */
    for( i = n - 2; i >= 0; i-- ) {
        for( j = n - 1; j > i; j-- ) {
            s = 0.0;
            for( k = i + 1; k <= j; k++ ) {
                if ( k != j ) {
                    s -= A(i, k) * A(k, j);
                } else {
                    s -= A(i, k);
                    }
                }
            A(i, j) = s;
            }
        }

    /*
     * INV(L)
     */
    for( i = n - 2; i >= 0; i-- ) {
        for( j = n - 1; j > i; j-- ) {
            s = 0.0;
            for( k = i + 1; k <= j; k++ ) {
                s -= A(j, k) * A(k, i);
                }
            A(j, i) = A(i, i) * s;
            }
        }
  
    /* 
     * A  = INV(AP)
     */
    for( i = 0; i < n; i++ ) {
        for( j = 0; j < n; j++ ) {
            s = 0.0;
            for( k = MAX( i, j ); k < n; k++ ) {
                if ( k != i ) {
                    s += A(i, k) * A(k, j);
                } else {
                    s += A(k, j);
                    }
                A(i, j) = s;
                }
            }
        }

    /*
     * A = INV(A) (at last)
     */
    for( i = n - 1; i >= 0; i-- ) {
        if ( pivot[i] != i ) {
            for( j = 0; j < n; j++ ) {
                s = A(i, j);
                A(i, j) = A(pivot[i], j);
                A(pivot[i], j) = s;
                }
            }
        }

    FREEMEM( (char *)pivot );
}

/*
 * LU- decomposition by gaussian elimination. Row pivoting is used.
 * 
 * result : AP = L'U ; L' = LD; pivot[i] is the swapped column 
 * number for column i (for pivoting).
 *
 * Result is stored in place of original matrix.
 *
 */
void ludecomp( double *a, int n, int *pivot )
{
    double swap;

    int i;
    int j;
    int k;
    int l;

    for( i = 0; i < n - 1; i++ ) {
        j = i;
        for( k = i + 1; k < n; k++ ) {
            if ( ABS( A(i, k) ) > ABS( A(i, j) ) ) {
                j = k;
                }
            }

        if ( ABS(A(i, j)) < AEPS ) {
            fprintf( stderr, "LUDecomp: Matrix is singular, %d %d.\n", i, j );
            return;
            }

        pivot[i] = j;
    
        if ( j != i ) {
            for( k = 0; k <= i; k++ ) {
                swap = A(k, j);
                A(k, j) = A(k, i);
                A(k, i) = swap;
                }
            }

        for( k = i + 1; k < n; k++ ) {
            A(i, k) /= A(i, i);
            }
    
        for( k = i + 1; k < n; k++ ) {
            if ( j != i ) {
                swap = A(k, i);
                A(k, i) = A(k, j); 
                A(k, j) = swap;
                }

            for( l = i + 1; l < n; l++ ) {
                A(k, l) -= A(k, i) * A(i, l);
                }
            }
        }
  
    pivot[n-1] = n-1;
    if (ABS(A(n-1,n-1))<AEPS) {
        fprintf( stderr, "LUDecomp: Matrix is singular.\n" );
        }
}
