/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	matrixLib
 *
 * DESCRIPTION:	support routines for the various matrix operations.
 * 
 * AUTHOR/DATE:	John Lloyd, Summer 1987
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include "cdefs.h"
#include "types.h"

static ecode;

/* An error routine for the fortran matrix inversion routine */

error_ (i)
int *i;
{
	ecode = *i;
}

#ifdef NOFORTRAN

/* If we have a fortran compiler, then we compile up the matrix inversion
   routine in invert.f. This contains a good inversion algorithm taken from
   LINPACK. Otherwise, we use Gaussian elimination written in C.

   'mr' - pointer to preallocated result matrix (size must be OK)
   'm1' - pointer to source matrix 
*/

#define EPSILON 1.0e-14

matrix *invert_matrix (mr, m1)
matrix *mr, *m1;
{
	/* Simple Gaussian elimination inversion routine */

	int n, i, j, k, imax;
	double *ar;
	double *ai;
	double det;
	double max;
	double val;

	n = m1->row;
	ai = (double *) talloc (n * n * sizeof(double));
	ar = (double *) talloc (n * n * sizeof(double));

	det = 1.0;
	for (i=0; i<n; i++)
	 { for (j=0; j<n; j++)
	    { if (i == j)
	       { ar [i*n+j] = 1.0;
	       }
	      else
	       { ar [i*n+j] = 0.0;
	       }
	      ai[i*n+j] = ELEMENT(m1, i, j);
	    }
	 }

	for (k=0; k<n; k++)
	 {
	   /* find max element in column k */

	   imax = k;
	   max = ai[imax*n+k];
	   for (i=k; i<n; i++)
	    { if (FABS(ai[i*n+k]) > max)
	       { imax = i;
		 max = FABS(ai[imax*n+k]);
	       }
	    }

	   val = ai[imax*n+k];

	   if (imax != k)
	    { /* interchange rows in both matrices */

	      double temp;

	      for (j=0; j<n; j++)
	       { temp = ai[imax*n+j];
		 ai[imax*n+j] = ai[k*n+j];
		 ai[k*n+j] = temp;
	       }
	      for (j=0; j<n; j++)
	       { temp = ar[imax*n+j];
		 ar[imax*n+j] = ar[k*n+j];
		 ar[k*n+j] = temp;
	       }
	      det = -det;
	    }
	   
	   
	   if (max < EPSILON)
	    { error (0, "matrix is singular to working precision\n");
	      mr = NULL;
	      break;
	    }

	   /* reduce row k */
	  
	   for (j=0; j<n; j++)
	    { ai[k*n+j] /= val;
	      ar[k*n+j] /= val;
	    }
	   det *= val;
	   
	   /* reduce rows other than k */

	   for (i=0; i<n; i++)
	    { if (i != k)
	       { double mult = ai[i*n+k];
		 for (j=0; j<n; j++)
		  { ai[i*n+j] -= mult*ai[k*n+j];
		    ar[i*n+j] -= mult*ar[k*n+j];
		  }
	       }
	    }
	 }

	if (mr != NULL)
	 { for (i=0; i<n; i++)
	    { for (j=0; j<n; j++)
	       { mr->buf[i*n+j] = ar[i*n+j];
	       }
	    }
	 }
	return (mr);
}
#else

/* an absolutely gross piece of code which is required to interface to the
   matrix inversion routine. This routine is written in fortran, and is
   culled from the LINPACK source code used in MATLAB. The grossness, of course,
   is a consequence of the fortran interface.

   'mr' - pointer to preallocated result matrix (size must be OK)
   'm1' - pointer to source matrix 
*/
matrix *invert_matrix (mr, m1)
matrix *mr, *m1;
{
	int lda, n, *ipvt, job;
	double *ar, *ai, *zr, *zi;
	double rcond;

	int i, j;

	n = lda = m1->row;

	ar = (double *) talloc (n * n * sizeof(double));
	ai = (double *) talloc (n * n * sizeof(double));
	zr = (double *) talloc (n * sizeof(double));
	zi = (double *) talloc (n * sizeof(double));
	ipvt = (int *)  talloc (n * sizeof(int));

       	for (i=0; i<n; i++)
	 { for (j=0; j<n; j++)
	    { ar[i*n+j] = m1->buf[j*n+i];
	      ai[i*n+j] = 0.0;
	    }
	 }

	ecode = 0;
	wgeco_ (ar, ai, &lda, &n, ipvt, &rcond, zr, zi);
	if (ecode != 0)
	 { error (0, "divide by zero\n");
	   mr = NULL;
	 }
	else if ((rcond + 1.0) == 1.0)
	 { error (0, "matrix is singular to working precsion\n");
	   mr = NULL;
	 }
	else
  	 { job = 1;
	   ecode = 0;
	   wgedi_ (ar, ai, &lda, &n, ipvt, NULL, NULL, zr, zi, &job);
	   if (ecode != 0)
	    { error (0, "divide by zero\n");
	      mr = NULL;
	    }
	   else
	    { for (i=0; i<n; i++)
	       { for (j=0; j<n; j++)
	          { mr->buf[i*n+j] = ar[j*n+i];
	          }
	       }
	    }
	 }

	return (mr);
}

#endif

/* returns the transpose of a matrix. Assumes that space passed down in
   mr is correct.

   'mr' - pointer to preallocated result matrix (size must be OK)
   'm1' - pointer to source matrix
*/
matrix *transpose_matrix (mr, m1)
register matrix *mr, *m1;
{
	register int i, j;
	double d;

	mr->row = m1->col;
	mr->col = m1->row;
	for (i=0; i<mr->row; i++)
 	 { for (j=0; j<mr->col; j++)
	    { ELEMENT (mr, i, j) = ELEMENT (m1, j, i);
	    }
	 }
}
