/*** These are from numerical_recipes  in  C. */
/*** PROBLEM: Thier counts went from 1 to N. **/
/*** I Changed that. ***/
#include <stdio.h>
#include <math.h>
#include "scconst.h"

#define MAX_MATRIX_SIZE MAX_NUM_SCORE_DIM

/*** gaussj replaces a by its inverse?? If b starts as identity matrix?? ***/
#define SWAP(a,b) {double temp=(a);(a)=(b);(b)=temp;}

void gaussj(a,n,b,m)
double a[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],b[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE];
int n,m;
{
	int *indxc,*indxr,*ipiv;
	int i,icol,irow,j,k,l,ll,*ivector();
	double big,dum,pivinv;
	void nrerror(),free_ivector();

	indxc=ivector(1,n);
	indxr=ivector(1,n);
	ipiv=ivector(1,n);
	for (j=0;j<n;j++) ipiv[j]=0;
	for (i=0;i<n;i++) {
		big=0.0;
		for (j=0;j<n;j++)
			if (ipiv[j] != 1)
				for (k=0;k<n;k++) {
					if (ipiv[k] == 0) {
						if (fabs(a[j][k]) >= big) {
							big=fabs(a[j][k]);
							irow=j;
							icol=k;
						}
					} else if (ipiv[k] > 1) nrerror("GAUSSJ: Singular Matrix-1");
				}
		++(ipiv[icol]);
		if (irow != icol) {
			for (l=0;l<n;l++) SWAP(a[irow][l],a[icol][l])
			for (l=0;l<m;l++) SWAP(b[irow][l],b[icol][l])
		}
		indxr[i]=irow;
		indxc[i]=icol;
		if (a[icol][icol] == 0.0) nrerror("GAUSSJ: Singular Matrix-2");
		pivinv=1.0/a[icol][icol];
		a[icol][icol]=1.0;
		for (l=0;l<n;l++) a[icol][l] *= pivinv;
		for (l=0;l<m;l++) b[icol][l] *= pivinv;
		for (ll=0;ll<n;ll++)
			if (ll != icol) {
				dum=a[ll][icol];
				a[ll][icol]=0.0;
				for (l=0;l<n;l++) a[ll][l] -= a[icol][l]*dum;
				for (l=0;l<m;l++) b[ll][l] -= b[icol][l]*dum;
			}
	}
	for (l=n-1;l>=0;l--) {
		if (indxr[l] != indxc[l])
			for (k=0;k<n;k++)
				SWAP(a[k][indxr[l]],a[k][indxc[l]]);
	}
	free_ivector(ipiv,1,n);
	free_ivector(indxr,1,n);
	free_ivector(indxc,1,n);
}

#undef SWAP



/***************************************************************/


#define TINY 1.0e-20;

void ludcmp(a,n,indx,d)
int n,*indx;
double a[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],*d;
{
	int i,imax,j,k;
	double big,dum,sum,temp;
	double *vv,*vector();
	void nrerror(),free_vector();

/*	fprintf(stderr,"\n Declaring space for vv"); */
/*	vv=vector(1,n); */
	vv=vector(0,n-1);
/*	fprintf(stderr,"\nSuccessfully declared vv"); */

	*d=1.0;
	for (i=0;i<n;i++) {
		big=0.0;
		for (j=0;j<n;j++) {
			if ((temp=fabs(a[i][j])) > big) big=temp;
		      }
		if (big == 0.0) nrerror("Singular matrix in routine LUDCMP");
		vv[i]=1.0/big;
	}

	for (j=0;j<n;j++) {
		for (i=0;i<j;i++) {
			sum=a[i][j];
			for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
		}
		big=0.0;
		for (i=j;i<n;i++) {
			sum=a[i][j];
			for (k=0;k<j;k++)
				sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
			if ( (dum=vv[i]*fabs(sum)) >= big) {
				big=dum;
				imax=i;
			}
		}
		if (j != imax) {
			for (k=0;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.0) a[j][j]=TINY;
		if (j != n-1) {
			dum=1.0/(a[j][j]);
			for (i=j+1;i<n;i++) a[i][j] *= dum;
		}
	}

/*	free_vector(vv,1,n);  */
	free_vector(vv,0,n-1);

}

#undef TINY




/***********************************/

void lubksb(a,n,indx,b)
double a[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],b[MAX_MATRIX_SIZE];
int n,indx[MAX_MATRIX_SIZE];
{
	int i,ii=-1,ip,j;
	double sum;

	for (i=0;i<n;i++) {
		ip=indx[i];
		sum=b[ip];
		b[ip]=b[i];
		if (ii != -1)
			for (j=ii;j<=i-1;j++) sum -= a[i][j]*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 -= a[i][j]*b[j];
		b[i]=sum/a[i][i];
	}
}



/*****************************************************/


/*** 
  matrix and inverse should be n X n arrays.
  indx should be an array of n
  col should be an array of n
****/
/*** If just want det then set inverse to NULL ***/
void matrix_inverse_and_det (double matrix[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],
			     double inverse[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],
			     double *det,int n)
{
  int i,j;
  double  temp_matrix[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE];
  double col[MAX_MATRIX_SIZE];
  int indx[MAX_MATRIX_SIZE];


  for (i=0; i<n; i++) 
    for (j=0; j<n; j++)      /** So matrix doesn't get destroyed. **/
      temp_matrix[i][j] = matrix[i][j];  

/**  fprintf(stderr,"\n Matrix to invert + find det of is:\n");
  print_matrix(matrix,n,n,stderr);
***/

/*  fprintf(stderr,"\n Into ludcmp"); */
  
  ludcmp(temp_matrix,n,indx,det);


  for (j=0; j<n; j++)
    *det *= temp_matrix[j][j];
  
/*  fprintf(stderr,"\n did det=%lf",*det); */

/*** Now convert current form of "inverse" into the real matrix inverse.  */
/** only if want to compute inverse though. ****/
  if (inverse)
    for (j=0; j<n; j++) {
      for (i=0; i<n; i++)
	col[i] = 0.0;
      col[j]=1.0;

      lubksb(temp_matrix,n,indx,col);
      for (i=0; i<n; i++)
	inverse[i][j] = col[i];
    }

}



/*************************************************************************/


double determ(double matrix[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], int n)
{
  double det;
 
  matrix_inverse_and_det (matrix, NULL,   &det,n);

  return (det);
}
    


void  matrix_mult(int num_rows1, int num_columns1_rows2, int num_columns2,
		  double matrix1[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], 
		  double matrix2[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], 
		  double matrix_product[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE])
{

  int i, j, k;

  for (i=0; i<num_rows1; i++)
    for(k=0; k<num_columns2; k++) {
      matrix_product[i][k]=0;
      for (j=0; j<num_columns1_rows2; j++)
	matrix_product[i][k] += matrix1[i][j] * matrix2[j][k];
    }

}






/** Return 0 if x had some -HUGE_VAL values in it, since that means **/
/** the normalized matrix is grabage. ***?
/*** compute x(transpose)  * M * x ******/
int normalize (int num_M_row_column, 
                int num_x_column, int num_x_row, 
                double out_matrix[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE],
		 double M[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], 
                double x[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE])

{
  double x_trans[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE]; 
  int i,k;
  double product1[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE];
  int all_x_non_infinity =1;

  for(i=0; i<num_x_column; i++)
    for (k=0; k < num_x_row; k++) {
      x_trans[i][k] = x[k][i]; 
      if (x[k][i] <= -HUGE_VAL) 
	all_x_non_infinity=0;
    }


  if (num_x_row != num_M_row_column) {   /** num_x_row = num_xtrans_column. **/
    fprintf(stderr, "\n Error:  Can't multiply transpose by matrix because number_x_row = %d, number_M_row_column=%d.", num_x_row, num_M_row_column);
    exit(-1);
  }

  matrix_mult(num_x_column, num_M_row_column, num_M_row_column, x_trans, M, 
              product1);
  matrix_mult(num_x_column, num_M_row_column, num_x_column, product1, x, 
              out_matrix);

/***********
  if (all_x_non_infinity) {
    fprintf(stderr,"\n x_trans=");
    print_matrix(x_trans, num_x_column, num_M_row_column,stderr);
    fprintf(stderr,"\n M=");
    print_matrix(M, num_M_row_column, num_M_row_column,stderr);
    fprintf(stderr,"\n x_trans*M=");
    print_matrix(product1, num_x_column, num_M_row_column,stderr);
    fprintf(stderr,"\n x_trans*M*x=");
    print_matrix(out_matrix, num_x_column,num_x_column,stderr);
  }
*************/

  if (all_x_non_infinity) return 1;
  else return 0;

}






print_matrix(double M[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE], int num_rows, 
		  int num_columns, FILE *f_out)
{
  int i,j;

 
  for (i=0; i<num_rows; i++){ 
    fprintf(f_out,"\n");
    for (j=0;j<num_columns;j++)
      fprintf(f_out," %lf",M[i][j]);
  }
  fprintf(f_out,"\n");
}


/*** For printing a matrix of size used for computing ALL dimensions. ***/
print_matrix2(double M[NUM_DIM_IN_ORIG_MATRIX][NUM_DIM_IN_ORIG_MATRIX], 
	      int num_rows, int num_columns, FILE *f_out)
{
  int i,j;

 
  for (i=0; i<num_rows; i++){ 
    fprintf(f_out,"\n");
    for (j=0;j<num_columns;j++)
      fprintf(f_out," %lf",M[i][j]);
  }
  fprintf(f_out,"\n");
}