#include <math.h>
#include <stdio.h>
#include <stdarg.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/timeb.h>
#include "invert_matrix.h"

#define FZERO(f)     (fabs(f) < 0.0000001F)

MATRIX *
MatrixAlloc(int rows, int cols, int type)
{
  MATRIX *mat ;
  int    row, nelts;
#ifdef _POSIX_MAPPED_FILES
  int    i;
  float  f;
#endif

  mat = (MATRIX *)calloc(1, sizeof(MATRIX)) ;
  if (!mat)
    ErrorExit(ERROR_NO_MEMORY, 
              "MatrixAlloc(%d, %d, %d): could not allocate mat", 
              rows, cols, type) ;

  mat->rows = rows ;
  mat->cols = cols ;
  mat->type = type ;

/*
  allocate a single array the size of the matrix, then initialize
  row pointers to the proper locations.
*/

  nelts = rows*cols ;
  if (type == MATRIX_COMPLEX)
    nelts *= 2 ;

/*
  because NRC is one-based, we must leave room for a few unused
  (but valid) addresses before the start of the actual data so
  that mat->rptr[0][0] is a valid address even though it wont
  be used.
*/
  mat->data = (float *)calloc(nelts+2, sizeof(float)) ;
  if (!mat->data)
    printf ("allocation for %d elements failed\n", nelts+2);

  mat->mmapfile = NULL;
  
#ifdef _POSIX_MAPPED_FILES
  if (!mat->data) /* First try to allocate a mmap'd tmpfile */
  {
    printf("MatrixAlloc(%d, %d): Using mmap'd tmpfile\n",
           rows, cols) ;
    if((mat->mmapfile = tmpfile()))
    {
      /* This maintains identical behavior with calloc */
      f = 0;
      for(i = 0; i < nelts+2; ++i) {
        if(!(fwrite(&f, sizeof(float), 1, mat->mmapfile))) {
          printf("MatrixAlloc(%d, %d): fwrite failed", rows, cols) ;
          exit(1) ;
        }
      }
      /* This seems to matter with some implementations of mmap */
      fseek(mat->mmapfile, 0, 0) ;
      /* lseek(fileno(mat->mapfile), (nelts+2) * sizeof(float), 0) ;*/
      fflush(mat->mmapfile) ;
      
      mat->data = (float *)mmap(0, (nelts+2) * sizeof(float),
                                PROT_READ | PROT_WRITE, MAP_SHARED,
                                fileno(mat->mmapfile), 0) ;

      if(mat->data == MAP_FAILED) {
        mat->data = 0 ;
      }
    }
  }
#endif
  
  if (!mat->data) /* we _still_ couldn't get it! */
  {
    fprintf(stderr, "MatrixAlloc(%d, %d): allocation failed\n",
      rows, cols) ;
    exit(1) ;
  }
  mat->data += 2 ;  

/* 
  silly numerical recipes in C requires 1-based stuff. The full
  data array is zero based, point the first row to the zeroth
  element, and so on.
*/
  mat->rptr = (float **)calloc(rows+1, sizeof(float *)) ;
  if (!mat->rptr)
  {
    free(mat->data) ;
    free(mat) ;
    ErrorExit(ERROR_NO_MEMORY, "MatrixAlloc(%d, %d): could not allocate rptr",
              rows, cols) ;
  }
  for (row = 1 ; row <= rows ; row++)
  {
    switch (type)
    {
      case MATRIX_REAL:
        mat->rptr[row] = mat->data + (row-1)*cols - 1 ;
        break ;
      case MATRIX_COMPLEX:
        mat->rptr[row] = (float *)(((CPTR)mat->data) + 
            (row-1)*cols - 1) ;
        break ; 
      default:
        ErrorReturn(NULL,
                    (ERROR_BADPARM, "MatrixAlloc: unknown type %d\n",type)) ;
    }
  }

  return(mat) ;
}

int
MatrixFree(MATRIX **pmat)
{
  MATRIX *mat ;

  mat = *pmat ;
  *pmat = NULL;

  if (!mat)
    ErrorReturn(ERROR_BADPARM, (ERROR_BADPARM, "MatrixFree: NULL POINTER!\n"));

  /* silly numerical recipes in C requires 1-based stuff */
  mat->data -= 2 ;
  if (mat->mmapfile)
  {
    int nelts ;

    nelts = mat->rows*mat->cols ;
    if (mat->type == MATRIX_COMPLEX)
      nelts *= 2 ;

#ifdef _POSIX_MAPPED_FILES
    munmap((void *) mat->data, (nelts+2) * sizeof(float)) ;
#endif
    fclose(mat->mmapfile) ;
  }
  else{
    free(mat->data) ;
  }

  free(mat->rptr) ;
  free(mat) ;

  return(0) ;
}

MATRIX *
MatrixCopy(MATRIX *mIn, MATRIX *mOut)
{
  int row, rows, cols ;

  if(mIn == NULL)
    return(NULL);

  rows = mIn->rows ;
  cols = mIn->cols ;

  if (!mOut)
    mOut = MatrixAlloc(rows, cols, mIn->type) ;

  if (!mOut)
    ErrorReturn(NULL,
		(ERROR_NO_MEMORY,
		 "MatrixCopy: couldn't allocate mOut")) ;

  for (row = 1 ; row <= rows ; row++)
    memcpy((char *)(mOut->rptr[row]), (char *)mIn->rptr[row], 
           (cols+1)*sizeof(float)) ;

  return(mOut) ;
}

MATRIX *
MatrixCopyRegion(MATRIX *mSrc, MATRIX *mDst, int start_row, int start_col,
                 int rows, int cols, int dest_row, int dest_col)
{
  int srow, scol, drow, dcol, srows, scols, drows, dcols, end_row, end_col ;

  if ((dest_col < 1) || (dest_row < 1))
    ErrorReturn(NULL,
                (ERROR_BADPARM, "MatrixCopyRegion: bad destination (%d,%d)\n",
                 dest_row, dest_col)) ;

  srows = mSrc->rows ;
  scols = mSrc->cols ;
  end_row = start_row + rows - 1 ;
  end_col = start_col + cols - 1 ;
  if ((start_row < 1) || (start_row > srows) || (start_col < 1) ||
      (start_col > scols) || (end_row > srows) || (end_col > scols))
    ErrorReturn(NULL,
                (ERROR_BADPARM,
                 "MatrixCopyRegion: bad source region (%d,%d) --> (%d,%d)\n",
                 start_row, start_col, end_row, end_col)) ;

  if (!mDst)
    mDst = MatrixAlloc(rows, cols, mSrc->type) ;

  drows = mDst->rows ;
  dcols = mDst->cols ;
  if ((rows > drows) || (cols > dcols))
    ErrorReturn(NULL,
            (ERROR_BADPARM, 
           "MatrixCopyRegion: destination matrix not large enough (%dx%d)\n",
            rows, cols)) ;

  for (drow = dest_row, srow = start_row ; srow <= end_row ; srow++, drow++)
  {
    for (dcol = dest_col, scol = start_col ; scol <= end_col ; scol++, dcol++)
    {
      switch (mDst->type)
      {
      case MATRIX_REAL:
        *MATRIX_RELT(mDst, drow, dcol) = *MATRIX_RELT(mSrc, srow, scol) ;
        break ;
      case MATRIX_COMPLEX:
        *MATRIX_CELT(mDst, drow, dcol) = *MATRIX_CELT(mSrc,srow,scol);
        break ;
      }
    }
  }
  return(mDst) ;
}

MATRIX *
MatrixCopyRealRegion(MATRIX *mSrc, MATRIX *mDst, int start_row, int start_col,
                 int rows, int cols, int dest_row, int dest_col)
{
  int srow, scol, drow, dcol, srows, scols, drows, dcols, end_row, end_col ;

  if ((dest_col < 1) || (dest_row < 1))
    ErrorReturn(NULL,
                (ERROR_BADPARM, 
                 "MatrixCopyRealRegion: bad destination (%d,%d)\n",
                 dest_row, dest_col)) ;

  srows = mSrc->rows ;
  scols = mSrc->cols ;
  end_row = start_row + rows - 1 ;
  end_col = start_col + cols - 1 ;
  if ((start_row < 1) || (start_row > srows) || (start_col < 1) ||
      (start_col > scols) || (end_row > srows) || (end_col > scols))
    ErrorReturn(NULL,
            (ERROR_BADPARM,
             "MatrixCopyRealRegion: bad source region (%d,%d) --> (%d,%d)\n",
             start_row, start_col, end_row, end_col)) ;

  if (!mDst)
    mDst = MatrixAlloc(rows, cols, mSrc->type) ;

  drows = mDst->rows ;
  dcols = mDst->cols ;
  if ((rows > drows) || (cols > dcols))
    ErrorReturn(NULL,
          (ERROR_BADPARM, 
         "MatrixCopyRealRegion: destination matrix not large enough (%dx%d)\n",
           rows, cols)) ;

  for (drow = dest_row, srow = start_row ; srow <= end_row ; srow++, drow++)
  {
    for (dcol = dest_col, scol = start_col ; scol <= end_col ; scol++, dcol++)
    {
      switch (mDst->type)
      {
      case MATRIX_REAL:
        *MATRIX_RELT(mDst, drow, dcol) = MATRIX_CELT_REAL(mSrc, srow, scol) ;
        break ;
      case MATRIX_COMPLEX:
        MATRIX_CELT_IMAG(mDst, drow, dcol) = MATRIX_CELT_IMAG(mSrc,srow,scol);
        break ;
      }
    }
  }
  return(mDst) ;
}


MATRIX *
MatrixCopyImagRegion(MATRIX *mSrc, MATRIX *mDst, int start_row, int start_col,
                 int rows, int cols, int dest_row, int dest_col)
{
  int srow, scol, drow, dcol, srows, scols, drows, dcols, end_row, end_col ;

  if ((dest_col < 1) || (dest_row < 1))
    ErrorReturn(NULL,
                (ERROR_BADPARM, 
                 "MatrixCopyImagRegion: bad destination (%d,%d)\n",
                 dest_row, dest_col)) ;

  srows = mSrc->rows ;
  scols = mSrc->cols ;
  end_row = start_row + rows - 1 ;
  end_col = start_col + cols - 1 ;
  if ((start_row < 1) || (start_row > srows) || (start_col < 1) ||
      (start_col > scols) || (end_row > srows) || (end_col > scols))
    ErrorReturn(NULL,
              (ERROR_BADPARM,
               "MatrixCopyImagRegion: bad source region (%d,%d) --> (%d,%d)\n",
               start_row, start_col, end_row, end_col)) ;

  if (!mDst)
    mDst = MatrixAlloc(rows, cols, mSrc->type) ;

  drows = mDst->rows ;
  dcols = mDst->cols ;
  if ((rows > drows) || (cols > dcols))
    ErrorReturn(NULL,
          (ERROR_BADPARM, 
         "MatrixCopyImagRegion: destination matrix not large enough (%dx%d)\n",
           rows, cols)) ;

  for (drow = dest_row, srow = start_row ; srow <= end_row ; srow++, drow++)
  {
    for (dcol = dest_col, scol = start_col ; scol <= end_col ; scol++, dcol++)
    {
      switch (mDst->type)
      {
      case MATRIX_REAL:
        *MATRIX_RELT(mDst, drow, dcol) = MATRIX_CELT_IMAG(mSrc, srow, scol) ;
        break ;
      case MATRIX_COMPLEX:
        MATRIX_CELT_IMAG(mDst, drow, dcol) = MATRIX_CELT_IMAG(mSrc,srow,scol);
        break ;
      }
    }
  }
  return(mDst) ;
}

MATRIX  *
MatrixScalarMul(MATRIX *mIn, float val, MATRIX *mOut)
{
  int  row, col, rows, cols ;

  if (!mOut)
  {
    mOut = MatrixAlloc(mIn->rows, mIn->cols, mIn->type) ;
    if (!mOut)
      return(NULL) ;
  }

  rows = mIn->rows ;
  cols = mIn->cols ;

  if ((rows != mOut->rows) || (cols != mOut->cols))
    ErrorReturn(NULL,
                (ERROR_BADPARM,
                 "MatrixScalarMul: incompatable matrices %d x %d != %d x %d\n",
                 rows, cols, mOut->rows, mOut->cols)) ;

  for (row = 1 ; row <= rows ; row++)
  {
    for (col = 1 ; col <= cols ; col++)
      mOut->rptr[row][col] = mIn->rptr[row][col] * val ;
  }
  return(mOut) ;
}


MATRIX *
MatrixRealToComplex(MATRIX *mReal, MATRIX *mImag, MATRIX *mOut)
{
  int rows, cols, row, col ;

  rows = mReal->rows ;
  cols = mReal->cols ;
  if (!mOut)
    mOut = MatrixAlloc(mReal->rows, mReal->cols, MATRIX_COMPLEX) ;

  for (row = 1 ; row <= rows ; row++)
  {
    for (col = 1 ; col <= cols ; col++)
    {
      MATRIX_CELT_REAL(mOut,row,col) = *MATRIX_RELT(mReal, row, col) ;
      MATRIX_CELT_IMAG(mOut,row,col) = *MATRIX_RELT(mImag, row, col) ;
    }
  }

  return(mOut) ;
}

#define MAX_ELTS 10*1024
  
MATRIX * 
MatrixInverse(MATRIX *mIn, MATRIX *mOut)
{
  float  **a, **y, d, col[MAX_ELTS] ;
  int    i, j, index[MAX_ELTS], rows, cols, alloced = 0 ;
  MATRIX *mTmp ;

  if (mIn->rows != mIn->cols)
    ErrorReturn(NULL,
                (ERROR_BADPARM, 
                 "MatrixInverse: matrix (%d x %d) is not square\n",
                 mIn->rows, mIn->cols)) ;

  rows = mIn->rows ;
  cols = mIn->cols ;

  if (!mOut)
  {
    alloced = 1 ;
    mOut = MatrixAlloc(rows, cols, mIn->type) ;
  }
    
  /* allocate temp matrix so as not to destory contents of mIn */
  if (mIn->type == MATRIX_COMPLEX)
  {
    MATRIX *mQuad, *mInv, *mReal, *mImag ;

    mTmp = MatrixAlloc(2*rows, 2*cols, MATRIX_REAL) ;


/*
   form square matrix of the form

   A  -C
   C  A

   where A and C are the real and imaginary components of the input
   matrix respectively.
*/
    MatrixCopyRealRegion(mIn, mTmp, 1, 1, rows, cols, 1, 1) ;
    MatrixCopyRealRegion(mIn, mTmp, 1, 1, rows, cols, rows+1, cols+1) ;
    mQuad = MatrixAlloc(rows,cols,MATRIX_REAL) ;
    MatrixCopyImagRegion(mIn, mQuad, 1, 1, rows, cols, 1, 1) ;
    MatrixScalarMul(mQuad, -1.0f, mQuad) ;
    MatrixCopyRegion(mQuad, mTmp, 1, 1, rows, cols, 1, cols+1) ;
    MatrixCopyImagRegion(mIn, mTmp, 1, 1, rows, cols, cols+1, 1) ;

    mReal = MatrixAlloc(rows, cols, MATRIX_REAL) ;
    mImag = MatrixAlloc(rows, cols, MATRIX_REAL) ;
    mInv = MatrixInverse(mTmp, NULL) ;

    MatrixCopyRegion(mInv, mReal, 1, 1, rows, cols, 1, 1) ;
    MatrixCopyRegion(mInv, mImag, rows+1, 1, rows, cols, 1, 1) ;
    MatrixRealToComplex(mReal, mImag, mOut) ;

    MatrixFree(&mQuad) ;
    MatrixFree(&mReal) ;
    MatrixFree(&mImag) ;
  }
  else
  {
    mTmp = MatrixCopy(mIn, NULL) ;   

    a = mTmp->rptr ;
    y = mOut->rptr ;

    if (ludcmp(a, rows, index, &d) < 0)
    {
      MatrixFree(&mTmp) ;
      if (alloced)
        MatrixFree(&mOut) ;
      ErrorReturn(NULL,
		  (ERROR_BADPARM,
		   "MatrixMultiply: ludcmp returned < 0\n" ));
    }
    
    for (j = 1 ; j <= rows ; j++)
    {
      for (i = 1 ; i <= rows ; i++)
        col[i] = 0.0f ;
      col[j] = 1.0f ;
      lubksb(a, rows, index, col) ;
      for (i = 1 ; i <= rows ; i++)
        y[i][j] = col[i] ;
    }
  }

  MatrixFree(&mTmp) ;

  for (j = 1 ; j <= rows ; j++)
  {
    for (i = 1 ; i <= rows ; i++)
      switch (mOut->type)
      {
      case MATRIX_REAL:
        if (!finite(*MATRIX_RELT(mOut, i, j)))
        {
          if (alloced)
            MatrixFree(&mOut) ;
          return(NULL) ;   /* was singular */
        }
        break ;
      case MATRIX_COMPLEX:
        if (!finite(MATRIX_CELT_REAL(mOut, i, j)) ||
             !finite(MATRIX_CELT_IMAG(mOut, i, j)))
        {
          if (alloced)
            MatrixFree(&mOut) ;
          return(NULL) ;   /* was singular */
        }
        break ;
      }
  }

  return(mOut) ;
}

MATRIX  *
MatrixTranspose(MATRIX *mIn, MATRIX *mOut)
{
  int  row, col, rows, cols ;

  if (!mOut)
  {
    mOut = MatrixAlloc(mIn->cols, mIn->rows, mIn->type) ;
    if (!mOut)
      return(NULL) ;
  }

  rows = mIn->rows ;
  cols = mIn->cols ;

  for (row = 1 ; row <= rows ; row++)
  {
    for (col = 1 ; col <= cols ; col++)
      mOut->rptr[col][row] = mIn->rptr[row][col] ;
  }

  return(mOut) ;
}

MATRIX *
MatrixMultiply(MATRIX *m1, MATRIX *m2, MATRIX *m3)
{
  int   col, row, i, rows, cols, m1_cols ;
  float *r3 ;
  register float val, *r1, *r2 ;
  MATRIX   *m_tmp1 = NULL, *m_tmp2 = NULL ;

  if (m1->cols != m2->rows)
    ErrorReturn(NULL,
                (ERROR_BADPARM,
                "MatrixMultiply: m1 cols %d does not match m2 rows %d\n",
                m1->cols, m2->rows)) ;

  if (!m3)
  {
    /* twitzel also did something here */
    if((m1->type == MATRIX_COMPLEX) || (m2->type == MATRIX_COMPLEX)) 
      m3 = MatrixAlloc(m1->rows, m2->cols, MATRIX_COMPLEX);
    else
      m3 = MatrixAlloc(m1->rows, m2->cols, m1->type) ;
    if (!m3)
      return(NULL) ;
  }
  else if ((m3->rows != m1->rows) || (m3->cols != m2->cols))
    ErrorReturn(NULL,
                (ERROR_BADPARM, 
                 "MatrixMultiply: (%d x %d) * (%d x %d) != (%d x %d)\n",
                 m1->rows, m1->cols, m2->rows, m2->cols, m3->rows, m3->cols)) ;

  if (m3 == m2)
  {
    m_tmp1 = MatrixCopy(m2, NULL) ;
    m2 = m_tmp1 ;
  }
  if (m3 == m1)
  {
    m_tmp2 = MatrixCopy(m1, NULL) ;
    m1 = m_tmp2 ;
  }
  /*  MatrixClear(m3) ;*/
  cols = m3->cols ;
  rows = m3->rows ;
  m1_cols = m1->cols ;

  /* twitzel modified here */
  if((m1->type == MATRIX_REAL) && (m2->type == MATRIX_REAL))
  {
    for (row = 1 ; row <= rows ; row++)
    {
      r3 = &m3->rptr[row][1] ;
      for (col = 1 ; col <= cols ; col++)
      {
        val = 0.0 ;
        r1 = &m1->rptr[row][1] ;
        r2 = &m2->rptr[1][col] ;
        for (i = 1 ; i <= m1_cols ; i++, r2 += cols)
        {
#if 0
          m3->rptr[row][col] +=
            m1->rptr[row][i] * m2->rptr[i][col] ;
#else
          val += *r1++ * *r2 ;
#endif
        }
        *r3++ = val ;
      }
    }
  } else if((m1->type == MATRIX_COMPLEX) && (m2->type == MATRIX_COMPLEX)) {
 
    for (row = 1 ; row <= rows ; row++)
      {
  for (col = 1 ; col <= cols ; col++)
    {
      for (i = 1 ; i <= m1->cols ; i++)
        {
    float a, b, c, d ;  /* a + ib and c + id */
    
    a = MATRIX_CELT_REAL(m1,row,i) ;
    b = MATRIX_CELT_IMAG(m1,row,i) ;
    c = MATRIX_CELT_REAL(m2,i,col) ;
    d = MATRIX_CELT_IMAG(m2,i,col) ;
    MATRIX_CELT_REAL(m3,row,col) += a*c - b*d ;
    MATRIX_CELT_IMAG(m3,row,col) += a*d + b*c ;
        }
    }
      }
  } else if((m1->type == MATRIX_REAL) && (m2->type == MATRIX_COMPLEX)) {
    for (row = 1 ; row <= rows ; row++)
      {
  for (col = 1 ; col <= cols ; col++)
    {
      for (i = 1 ; i <= m1->cols ; i++)
        {
    float a, c, d ;  /* a + ib and c + id and b=0 here*/
    
    a = *MATRIX_RELT(m1,row,i);
    c = MATRIX_CELT_REAL(m2,i,col);
    d = MATRIX_CELT_IMAG(m2,i,col);
    MATRIX_CELT_REAL(m3,row,col) += a*c;
    MATRIX_CELT_IMAG(m3,row,col) += a*d;
        }
    }
      }
  }
  if (m_tmp1)
    MatrixFree(&m_tmp1) ;
  if (m_tmp2)
    MatrixFree(&m_tmp2) ;
  return(m3) ;
}

int
MatrixPrint(FILE *fp, MATRIX *mat)
{
  int  row, col, rows, cols ;

  if (fp == NULL)
  {
    fp = stdout ; 
    ErrorPrintf(ERROR_BADPARM, "MatrixPrint: fp = NULL!") ;
  }
  if (mat == NULL)
    ErrorReturn(ERROR_BADPARM,(ERROR_BADPARM, "MatrixPrint: mat = NULL!")) ;

  rows = mat->rows ;
  cols = mat->cols ;

  for (row = 1 ; row <= rows ; row++)
  {
    for (col = 1 ; col <= cols ; col++)
    {
      switch (mat->type)
      {
        case MATRIX_REAL:
          fprintf(fp, "% 2.3f", mat->rptr[row][col]) ;
          break ;
        case MATRIX_COMPLEX:
          fprintf(fp, "% 2.3f + % 2.3f i", 
            MATRIX_CELT_REAL(mat,row,col), 
            MATRIX_CELT_IMAG(mat, row, col)) ;
          break ;
        default:
          ErrorReturn(ERROR_BADPARM,
                      (ERROR_BADPARM, 
                       "MatrixPrint: unknown type %d\n",mat->type)) ;
      }
#if 0
      if (col < cols)
        fprintf(fp, " | ") ;
#else
      if (col < cols)
        fprintf(fp, "  ") ;
#endif
    }
    fprintf(fp, ";\n") ;
  }
  return(NO_ERROR) ;
}

#define TINY ((float)(1.0e-20))

int ludcmp(float **a, int n, int *indx, float *d)
{
  int i,imax = 0,j,k;
  float big,dum,sum,temp;
  float *vv;

  for (j=1;j<=n;j++)
  {
    for (i=1;i<j;i++)
      if (FZERO(a[i][j]))    /* prevent underflow */
        a[i][j] = 0.0f ;
  }

  vv=vector(1,n);
  *d=1.0f;
  for (i=1;i<=n;i++)
  {
    big=0.0f;
    for (j=1;j<=n;j++)
      if ((temp=(float)fabs((double)a[i][j])) > big) big=temp;
    if (big == 0.0f) 
    {
      free_vector(vv,1,n);
      fprintf (stderr, "Singular matrix in routine LUDCMP\n");
      return(-1) ;
    }

    vv[i]=1.0f/big;
  }
  for (j=1;j<=n;j++)
  {
    for (i=1;i<j;i++)
    {
      sum=a[i][j];
      for (k=1;k<i;k++) sum -= a[i][k]*a[k][j];
      a[i][j]=sum;
    }
    big=0.0f;
    for (i=j;i<=n;i++)
    {
      sum=a[i][j];
      for (k=1;k<j;k++)
        sum -= a[i][k]*a[k][j];
      a[i][j]=sum;
      if ( (dum=vv[i]*(float)fabs((double)sum)) >= big)
      {
        big=dum;
        imax=i;
      }
    }
    if (j != imax)
    {
      for (k=1;k<=n;k++)
      {
        dum=a[imax][k];
        a[imax][k]=a[j][k];
        a[j][k]=dum;
      }
      *d = -(*d);
      vv[imax]=vv[j];
    }
    indx[j]=imax;
    if (a[j][j] == 0.0f) a[j][j]=TINY;
    if (j != n)
    {
      dum=1.0f/(a[j][j]);
      for (i=j+1;i<=n;i++) a[i][j] *= dum;
    }
  }
  free_vector(vv,1,n);

  return(0) ;
}

void lubksb(float **a, int n, int *indx, float b[])
{
  int i,ii=0,ip,j;
  float sum;

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


float *vector(long nl, long nh)
/* allocate a float vector with subscript range v[nl..nh] */
{
  float *v;

  v=(float *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(float)));
  if (!v) 
    ErrorExit(ERROR_NOMEMORY, "could not allocate vector(%ld, %ld)",
              nl, nh);
  return v-nl+NR_END;
}

void free_vector(float *v, long nl, long nh)
/* free a float vector allocated with vector() */
{
  free((FREE_ARG) (v+nl-NR_END));
}



int
ErrorPrintf(int ecode, char *fmt, ...)
{
  va_list  args ;
  FILE     *fp ;

  va_start(args, fmt) ;
  vfprintf(stderr, fmt, args) ;
  vfprintf(stderr, "\n", NULL) ;
  if (errno)
    perror(NULL) ;
  
  return(ecode) ;
}

void
ErrorExit(int ecode, char *fmt, ...)
{
  va_list  args ;

  va_start(args, fmt) ;
  vfprintf(stderr, fmt, args) ;
  fprintf(stderr, "\n") ;
  if (errno)
    perror(NULL) ;

  exit(ecode) ;
}


struct timeb start_time;
void
start_timer() 
{
  struct timeval tv; 
  gettimeofday(&tv, 0);
  start_time.time = tv.tv_sec;
  start_time.millitm = (tv.tv_usec+500)/1000;
  start_time.dstflag = 0;
}

int
stop_timer() 
{
  int msec;
  struct timeval tvnow, tvthen;
  gettimeofday(&tvnow, 0); 
  tvthen.tv_sec = start_time.time;
  tvthen.tv_usec = ((long) (start_time.millitm))*1000;
  msec = 1000*(tvnow.tv_sec - tvthen.tv_sec) + (tvnow.tv_usec - tvthen.tv_usec+500)/1000;
  return msec;
}

int main ( int argc, char** argv ) {

  int nrows, ncols;
  MATRIX *m, *m_tr, *m_tr_m, *m_inv;
  int error;
  int r, c;
  int msec;

  if (argc != 1 && argc != 3) {
    printf ("USAGE: %s [ncols nrows]\n", argv[0]);
    return 1;
  }

  /* Use default nrows and ncols or get them from the command line. */
  if (argc == 1) { 
    printf ("Using nrows = 4096, ncols = nrows*2\n");
    nrows = 4096;
    ncols = nrows * 2;
  } else {
    ncols = atoi (argv[1]);
    if (ERANGE == errno) {
      printf ("error reading ncols\n");
      return 1;
    }
    nrows = atoi (argv[2]);
    if (ERANGE == errno) {
      printf ("error reading nrows\n");
      return 1;
    }
  }

  /* Create a matrix and fill it with random values. */
  m = MatrixAlloc (nrows, ncols, MATRIX_REAL);
  if (!m)
    ErrorExit(ERROR_BADPARM, "could not allocate matrix") ;
  for( r = 1; r <= m->rows; r++ ) {
    for( c = 1; c <= m->cols; c++ ) {
      m->rptr[r][c] = (float) rand() / 2147483647;
    }
  }
  


  start_timer();
  
  /* Compute the psudo inverse. */
  m_tr = MatrixTranspose (m, NULL);
  if (!m_tr)
    ErrorExit(ERROR_BADPARM, "could not transpose matrix") ;

  m_tr_m = MatrixMultiply (m_tr, m, NULL);
  if (!m_tr_m)
    ErrorExit(ERROR_BADPARM, "could not multiple matrix") ;

  m_inv = MatrixInverse (m_tr_m, NULL);
  if (!m_inv)
    ErrorExit(ERROR_BADPARM, "could not invert matrix") ;

  msec = stop_timer();



  printf ("time for %d col %d row matrix: %d msec\n", ncols, nrows, msec);

  
  MatrixFree (&m);
  MatrixFree (&m_tr);
  MatrixFree (&m_tr_m);
  MatrixFree (&m_inv);

  exit( 0 );
}