/*      FILE: ism.c
 *   PURPOSE: Invert a symmetric matrix
 *    AUTHOR: Kenneth J. Mighell (mighell@noao.edu)
 *  LANGUAGE: ANSI C
 *      DATE: 2001SEP10
 * COPYRIGHT: (C) 2001 Assoc. of Universities for Research in Astronomy Inc.
 */

#include "mx.h"
#include "inc.h"

#define OFFSET(j,k) (j+(k*MXNORDER))

void InvertSymmetricMatrix_v3(
  double *matrix, 
  int norder,
  double *det_p
){

  double det;
  double x;
  double amax;
  double save;
  int n;
  int m;
  int i;
  int j;
  int k;
  int l;
  int ik[MXNORDER];
  int jk[MXNORDER];
  int kk;

  n = norder;

  /* Parameter adjustments  */
  matrix -= MXNORDER + 1;

  det = 1.0;

  for (k = 1; k <= n; ++k) {
    
    kk = k - 1;

    /* Find largest element MATRIX(I,J) in rest of matrix */

    amax = 0.0; /* initialization */

start:

    for (i = k; i <= n; ++i) {
      for (j = k; j <= n; ++j) {
	x = abs(matrix[OFFSET(i,j)]);
	if ( (abs(amax) - x) <= 0.) {
	  amax = matrix[OFFSET(i,j)];
	  ik[kk] = i;
	  jk[kk] = j;
	}
      }
    }

    /* Interchange rows and columns to put AMAX in MATRIX(K,K) */
    if (amax == 0.) { 
      det = 0.0;
      goto bye;
    }
    i = ik[kk];
    m = i - k;        
    if (m < 0) goto start;
    if (m != 0) {
      for (j = 1; j <= n; ++j) {
	save = matrix[OFFSET(k,j)];
	matrix[OFFSET(k,j)] = matrix[OFFSET(i,j)];
	matrix[OFFSET(i,j)] = -save;
      }
    }
    j = jk[kk];
    m = j - k;
    if (m < 0) goto start;
    if (m != 0) {
      for (i = 1; i <= n; ++i) {
	save = matrix[OFFSET(i,k)];
	matrix[OFFSET(i,k)] = matrix[OFFSET(i,j)];
	matrix[OFFSET(i,j)] = -save;
      }
    }

    /* Accumulate elements of inverse matrix */
    for (i = 1; i <= n; ++i) {
      if (i - k != 0) {
	matrix[OFFSET(i,k)] = -matrix[OFFSET(i,k)] / amax;
      }
    }
    for (i = 1; i <= n; ++i) {
      for (j = 1; j <= n; ++j) {
	if (i - k != 0) {
	  if (j - k != 0) {
	    matrix[OFFSET(i,j)] += matrix[OFFSET(i,k)] * matrix[OFFSET(k,j)];
	  }
	}
      }
    }
    for (j = 1; j <= n; ++j) {
      if (j - k != 0) {
	matrix[OFFSET(k,j)] /= amax;
      }
    }
    matrix[OFFSET(k,k)] = (float)1. / amax;
    det *= amax;
  }

  /* Restore ordering of matrix */
  for (l = 1; l <= n; ++l) {
    k = n - l + 1;
    kk = k - 1;
    j = ik[kk];
    if (j - k > 0) {
      for (i = 1; i <= n; ++i) {
	save = matrix[OFFSET(i,k)];
	matrix[OFFSET(i,k)] = -matrix[OFFSET(i,j)];
	matrix[OFFSET(i,j)] = save;
      }
    }
    i = jk[kk];
    if (i - k > 0) {
      for (j = 1; j <= n; ++j) {
	save = matrix[OFFSET(k,j)];
	matrix[OFFSET(k,j)] = -matrix[OFFSET(i,j)];
	matrix[OFFSET(i,j)] = save;
      }
    }
  }

bye:
  *det_p = det;
  return;
} 

#ifdef NOGO
/*
 * ===== demo program =========================================================
 */
#include <math.h>
void main() {

#define BIG
#ifdef SMALL
#define SIZE (9)
  double matrix[SIZE] = {1,2,3,2,1,1,3,1,2};
#endif
#ifdef BIG
#define SIZE (16)
  double matrix[SIZE] = {1,-2,3,-4,-2,-3,4,-5,3,4,5,6,-4,-5,6,-7};
#endif

  double 
    /* inverse[SIZE], */
    inverse[MXNORDER*MXNORDER],
    det,
    value;
  int 
    norder,
    n,
    i,
    j,
    k,
    row,
    col,
    index;

  extern void InvertSymmetricMatrix( 
    double *matrix, int norder, double *det_p
  );

  n = sqrt(SIZE); 
  norder = n;

  for (j=0; j<(MXNORDER*MXNORDER); j++) inverse[j] = 0.0;

  for ( j=0; j<SIZE; j++) {
    printf( "matrix[%d] is %e\n",j,matrix[j] );
    row = j/n;
    col = j%n;
    index = OFFSET(col,row);
    inverse[index] = matrix[j];
    printf ("row,col,index,j: %d %d %d %d\n",row,col,index,j);
    printf( "--> inverse[%d] is %e\n",index,inverse[index] );
  }

  printf("i is %d\n",i);
  printf("n is %d\n",n);
  printf("norder is %d\n",norder);

  InvertSymmetricMatrix_v3( inverse, norder, &det);

  printf("det is %e\n",det);

  for ( j=0; j<SIZE; j++) {
    row = j/n;
    col = j%n;
    index = OFFSET(col,row);
    printf( "inverse[%d] is %e\n",index,inverse[index] );
  }

  printf("\n");
  for ( i=0; i<n; i++) {
    for ( j=0; j<n; j++) {
      value = 0;
      for ( k=0; k<n; k++) {
	row = k;
	col = j;
	index = OFFSET(col,row);
	value += matrix[k + i*n] * inverse[index];
      }
      if (fabs(value)<1e-14) value = 0.0;
      printf("i=%d j=%d matrix*inverse=%g\n",i,j,value);
    }
  }
}
/*
 * ===== end of demo program ==================================================
 */
#endif
#undef OFFSET
/* end-of-file */