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

/* forward declarations of private routines */
static int LUdecomposition( double **a, int rows, int *index, double *d );
static int LUbacksub( double **a, int n, int *index, double *b );

double *
vecCreate( int r )
{
    int i;
    double *v;

    v = (double *) malloc( r * sizeof( double ));
    for( i = 0; i < r; i++ )
      v[ i ] = 0.0;

    return v;
}

void
vecPrint( double *v, int r )
{
    int i;

    for( i = 0; i < r; i++ )
      printf( "  %20.12lf\n", v[ i ] );

    return;
}

double
vecDistance( double *v1, double *v2, int n )
{
    int i;
    double d;
    for( i = 0, d = 0.0; i < n; i++ )
      d += SQR( v1[ i ] - v2[ i ] );

    return sqrt( d );
}

void
vecScale( double *res, double s, double *v, int n )
{
    int i;

    for( i = 0; i < n; i++ )
      res[ i ] = s * v[ i ];

    return;
}

double **
mtxCreate( int r, int c )
{
    double **m;
    int i, j;

    m = (double **) malloc( r * sizeof( double * ));
    for( i = 0; i < r; i++ ) {
      m[ i ] = (double *) malloc( c * sizeof( double ));
      for( j = 0; j < c; j++ )
	m[ i ][ j ] = 0.0;
    }

    return m;
}

void
mtxDestroy( double **m, int r, int c )
{
    int i;

    for( i = 0; i < r; i++ )
      free( m[ i ] );
    free( m );

    return;
}

void
mtxDuplicate( double ***r, double **a, int m, int n )
{
    int i, j;

    *r = (double **) malloc( m * sizeof( double * ));
    for( i = 0; i < m; i++ ) {
      (*r)[ i ] = (double *) malloc( n * sizeof( double ));
      for( j = 0; j < n; j++ )
	(*r)[ i ][ j ] = a[ i ][ j ];
    }

    return;
}

void
mtxCopy( double **dst, double **src, int m, int n )
{
    int i, j;

    for( i = 0; i < m; i++ ) {
      for( j = 0; j < n; j++ )
	dst[ i ][ j ] = src[ i ][ j ];
    }

    return;
}

void
mtxMultiply( double **r, int r1, int c1, int c2, double **a, double **b )
{
    int loop, xloop, yloop;
    double val;

    /* Now do actual multiplication */
    for( yloop = 0; yloop < r1; yloop++ )
      for( xloop = 0; xloop < c2; xloop++ ) {
	for( val = 0.0, loop = 0; loop < c1; loop++ )
	  val += a[ yloop ][ loop ] * b[ loop ][ xloop ];
        r[ yloop ][ xloop ] = val;
      }

    return;
}

void
mtxTranspose( double **m, int i, int j )
{
    int r, c;
    double temp;

    for( r = 0; r < i; r++ )
      for( c = r + 1; c < j; c++ ) {
	temp = m[ r ][ c ];
	m[ r ][ c ] = m[ c ][ r ];
	m[ c ][ r ] = temp;
      }

    return;
}

void
mtxTransposeCopy( double **mt, double **m, int i, int j )
{
    int r, c;
    double temp;

    for( r = 0; r < i; r++ )
      for( c = 0; c < j; c++ )
	mt[ c ][ r ] = m[ r ][ c ];

    return;
}

int
mtxMultColVec( double *res, int i, int j, double **m, double *v )
{
    int r, c;
    double sum;

    for( r = 0; r < i; r++ ) {
      for( sum = 0.0, c = 0; c < j; c++ ) {
	sum += m[ r ][ c ] * v[ c ];
      }
      res[ r ] = sum;
    }

    return;
}

void
mtxPrint( double **p, int m, int n )
{
    int i, j;

    for( i = 0; i < m; i++ ) {
      for( j = 0; j < n; j++ )
	printf( "   %20.12lf", p[i][j] );
      printf( "\n" );
    }

    return;
}

/*
** Replaces given matrix with its inverse.  Uses LU decomposition, then finds
** inverse column-by-column.  Taken from "Numerical Recipies in C."  Currently
** handles up to a 10x10 Matrix, although this can be easily changed.
** Returns 0 on success, -1 on error.
*/
int
mtxInvert( double **m, int rows )
{
    int i, j;
    double **LUmat, d, *col;
    int *index, err;

    if( rows == 2 ) {
      /***********************************************
      ** For 2x2 uses the following formula:
      **  Original    Inverse     1   [d  -b]
      **   [a  b]                ---
      **   [c  d]               da-bc [-c  a]
      *************************************************/
      double a, b, c, d, one_over_det;

      a = m[0][0], b = m[0][1], c = m[1][0], d = m[1][1];
      one_over_det = 1.0 / ( d * a - b * c );
      m[0][0] =  d * one_over_det;
      m[0][1] = -b * one_over_det;
      m[1][0] = -c * one_over_det;
      m[1][1] =  a * one_over_det;

    } else { /* general inversion procedure, from Numerical Recipes */
      /* allocate space */
      col = vecCreate( rows );
      index = (int *) malloc( rows * sizeof( int ));

      /* Copy our matrix into the holding area */
      mtxDuplicate( &LUmat, m, rows, rows );

      /* Do inversion via LU decomposition */
      err = LUdecomposition( LUmat, rows, index, &d );
      if( err != 0 ) {
	free( index );
	mtxDestroy( LUmat, rows, rows );
	vecDestroy( col );
	return -1;
      }

      for( j = 0; j < rows; j++ ) {
	for( i = 0; i < rows; i++ )
	  col[ i ] = 0.0;
	col[ j ] = 1.0;
	LUbacksub( LUmat, rows, index, col );

	for( i = 0; i < rows; i++ )
	  m[ j ][ i ] = col[ i ];
      }

      free( index );
      mtxDestroy( LUmat, rows, rows );
      vecDestroy( col );
    }

    return 0;
}

double
mtxTrace( double **m, int r )
{
    int i;
    double tr;

    for( i = 1, tr = m[ 0 ][ 0 ]; i < r; i++ )
      tr += m[ i ][ i ];

    return tr;
}

double
mtxDeterminant( double **m, int r )
{
    switch( r ) {
    case 1:
      return m[0][0];
      break;

    case 2:
      return m[0][0] * m[1][1] - m[1][0] * m[0][1];
      break;

    case 3:
      return m[0][0] * ( m[1][1] * m[2][2] - m[2][1] * m[1][2] ) -
             m[0][1] * ( m[1][0] * m[2][2] - m[2][0] * m[1][2] ) +
             m[0][2] * ( m[1][0] * m[2][1] - m[2][0] * m[1][1] );

    default:
      printf( "mtxDeterminant not implemented for 4x4 or larger matrix\n" );
      return 0.0;
    } /* end switch */
}

void
mtxScale( double **m, int r, int c, double s )
{
    int i, j;

    for( i = 0; i < r; i++ )
      for( j = 0; j < c; j++ )
	m[ i ][ j ] *= s;

    return;
}

/*****************************************************************************
**  Name:    LUdecomposition
**  Descrip: Replaces A with its LU decomposition.
**           Taken from "Numerical Recipies in C," p.43
**  Returns: 0 on success, -1 on error. 
*****************************************************************************/
#define ArrayElem( m, rows, r, c ) m[ r ][ c ]
static int
LUdecomposition( double **a, int rows, int *index, double *d )
{
    register int i, j, k;
    int          imax;
    double       big, dum, sum, temp;
    double       *vv;

    vv = (double *) malloc( rows * sizeof( double ));
    *d = 1.0;
    for( i = 0; i < rows; i++ ) {
      big = 0.0;
      for( j = 0; j < rows; j++ ) {
	if(( temp = fabs( ArrayElem( a, rows, j, i ))) > big )
	  big = temp;
      }
      if( big == 0.0 ) {
	fprintf( stderr, "LUdecomp: singular Matrix!\n" );
	return -1;
      }
      vv[i] = 1.0 / big;
    }

    for( j = 0; j < rows; j++ ) {
      for( i = 0; i < j; i++ ) {
	sum = ArrayElem( a, rows, j, i );
	for( k = 0; k < i; k++ )
	  sum -= ArrayElem( a, rows, k, i ) * ArrayElem( a, rows, j, k );
	ArrayElem( a, rows, j, i ) = sum;
      }

      big = 0.0;
      for( i = j; i < rows; i++ ) {
	sum = ArrayElem( a, rows, j, i );
	for( k = 0; k < j; k++ )
	  sum -= ArrayElem( a, rows, k, i ) * ArrayElem( a, rows, j, k );
	ArrayElem( a, rows, j, i ) = sum;
	if(( dum = vv[i] * fabs( sum )) >= big ) {
	  big = dum;
	  imax = i;
	}
      }

      if( j != imax ) {
	for( k = 0; k < rows; k++ ) {
	  dum = ArrayElem( a, rows, k, imax );
	  ArrayElem( a, rows, k, imax ) = ArrayElem( a, rows, k, j );
	  ArrayElem( a, rows, k, j ) = dum;
	}

	*d = -( *d );
	vv[ imax ] = vv[ j ];
      }

      index[ j ] = imax;
      if( ArrayElem( a, rows, j, j ) == 0.0 ) {
	fprintf( stderr, "LUdecomp: hit singular pivot!\n" );
	return -1;
      }

      if( j != ( rows - 1 ) ) {
	dum = 1.0 / ArrayElem( a, rows, j, j );
	for( i = j + 1; i < rows; i++ )
	  ArrayElem( a, rows, j, i ) *= dum;
      }
    }

    free( vv );
    return 0;
}


/*****************************************************************************
**  Name:    LUbacksub
**  Descrip: Solves a set of linear equations AX = B where Matrix A is the LU
**           decomposition of the original Matrix A.
**           Taken from "Numerical Recipies in C," p.44
**  Returns: 0 on success, -1 on error.
*****************************************************************************/
static int
LUbacksub( double **a, int n, int *index, double *b )
{
    int i, j, ii = -1, ip;
    double sum;

    for( i = 0; i < n; i++ ) {
      ip = index[ i ];
      sum = b[ ip ];
      b[ ip ] = b[ i ];

      if( ii >= 0 ) {
	for( j = ii; j <= i - 1; j++ )
	  sum -= ArrayElem( a, n, j, i ) * b[ j ];
      } else if( sum ) {
	ii = i;
      }

      b[ i ] = sum;
    }

    for( i = n - 1; i >= 0; i-- ) {
      sum = b[ i ];
      for( j = i + 1; j < n; j++ ) {
	sum -= ArrayElem( a, n, j, i ) * b[ j ];
      }
      b[ i ] = sum / ArrayElem( a, n, i, i );
    }

    return 0;
}

#ifdef MTXTEST
int size[] = { 4, 5, 6 };
void
main( int argc, char *argv[] )
{
    int i, j;
    double **r, **a, **b, **bt, *v, *rv, **inv, **orig;

    srand48( (time_t) time( NULL ));

#if 0
    r = mtxCreate( size[0], size[2] );
    a = mtxCreate( size[0], size[1] );
    b = mtxCreate( size[1], size[2] );
    bt = mtxCreate( size[2], size[1] );
    v = vecCreate( size[2] );
    rv = vecCreate( size[0] );

    for( i = 0; i < size[0]; i++ )
      for( j = 0; j < size[1]; j++ )
	a[i][j] = drand48( );

    for( i = 0; i < size[1]; i++ )
      for( j = 0; j < size[2]; j++ )
	b[i][j] = drand48( );
    mtxTransposeCopy( bt, b, size[1], size[2] );

    printf( "b =\n" );
    mtxPrint( b, size[1], size[2] );
    printf( "bt =\n" );
    mtxPrint( bt, size[2], size[1] );

    mtxMultiply( r, size[0], size[1], size[2], a, b );
    printf( "a =\n" );
    mtxPrint( a, size[0], size[1] );
    printf( "b =\n" );
    mtxPrint( b, size[1], size[2] );
    printf( "r = a * b =\n" );
    mtxPrint( r, size[0], size[2] );

    for( i = 0; i < size[2]; i++ )
      v[i] = drand48( );

    printf( "v =\n" );
    vecPrint( v, size[2] );

    mtxMultColVec( rv, size[0], size[2], r, v );

    printf( "rv =\n" );
    vecPrint( rv, size[0] );
#endif

    inv = mtxCreate( size[0], size[0] );
    for( i = 0; i < size[0]; i++ )
      for( j = 0; j < size[0]; j++ ) {
	inv[i][j] = drand48( );
      }
    mtxDuplicate( &orig, inv, size[0], size[0] );
    
    mtxInvert( inv, size[0] );
    printf( "original =\n" );
    mtxPrint( orig, size[0], size[0] );
    printf( "inverse  =\n" );
    mtxPrint( inv, size[0], size[0] );

    return;
}
#endif