/* test_simeq.c   test:  simeq, inverse, determinant, mat_mul, mat_vec etc */
/* double determinant(int n, double A[]);                       d=determinant(n,A); */
/* void inverse(int n, double A[], double AA[]);                inverse(n,A,AA); */
/* void mat_mul(int n, double A[], double B[], double C[]);     mat_mul(n,A,B,C); */
/* void mat_vec_mul(int n, double A[], double B[], double C[]); mat_vec_mul(n,A,X,Y); */
/* void mat_add(int n, double A[], double B[], double C[]);     mat_add(n,A,B,C); */
/* void mat_sub(int n, double A[], double B[], double C[]);     mat_sub(n,A,B,C); */
/* void vec_sub(int n, double A[], double B[], double C[]);     vec_sub(n,X,Y,Z); */
/* void mat_put(int n, double A[]);                             mat_put(n,A); */
/* void vec_put(int n, double A[]);                             vec_put(n,X); */

#include "simeq.h"
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <math.h>

#undef abs
#define abs(a) ((a)<0.0?(-(a)):(a))
#undef max
#define max(a,b) ((a)>(b)?(a):(b))

double determinant(int n, double A[])
{
    double D = 1.0;  /* DETERMINANT */
    double *B;  /* WORKING MATRIX */
    int *ROW;  /* ROW INTERCHANGE INDICIES */
    int HOLD , I_PIVOT;  /* PIVOT INDICIES */
    double PIVOT;  /* PIVOT ELEMENT VALUE */
    double ABS_PIVOT;
    int i, j, k;

    B = (double *)calloc(n*n, sizeof(double));
    ROW = (int *)calloc(n, sizeof(int));
    memcpy(B,A,n*n*sizeof(double));

/*                        SET UP ROW  INTERCHANGE VECTORS */
    for(k=0; k<n; k++){
      ROW[k]= k;
    }

/*                         BEGIN MAIN REDUCTION LOOP  */
    for(k=0; k<n-1; k++){

/*                             FIND LARGEST ELEMENT FOR PIVOT */
      PIVOT = B[ROW[k]*n+k];
      ABS_PIVOT = abs ( PIVOT );
      I_PIVOT = k;
      for(i=k; i<n; i++){
        if( abs(B[ROW[i]*n+k]) > ABS_PIVOT ){
          I_PIVOT = i;
          PIVOT = B[ROW[i]*n+k];
          ABS_PIVOT = abs(PIVOT);
        }
      }

/*                         HAVE PIVOT, INTERCHANGE ROW POINTERS */
      if(I_PIVOT != k){
        HOLD = ROW[k];
        ROW[k] = ROW[I_PIVOT];
        ROW[I_PIVOT] = HOLD;
        D = - D;
      }

/*                                CHECK FOR NEAR SINGULAR  */
      if(ABS_PIVOT < 1.0E-10){
        free(B);
        free(ROW);
        return 0.0;
      }
      else
      {
        D = D * PIVOT;

/*                                REDUCE ABOUT PIVOT */
        for(j=k+1; j<n; j++){
          B[ROW[k]*n+j] = B[ROW[k]*n+j] / B[ROW[k]*n+k];
        }

/*                               INNER REDUCTION LOOP */
        for(i=0; i<n; i++){
          if(i != k){
            for(j=k+1; j<n; j++){
              B[ROW[i]*n+j] = B[ROW[i]*n+j] - B[ROW[i]*n+k]* B[ROW[k]*n+j];
            }
          }
        }
      }

/*                               FINISHED INNER REDUCTION */
    }

/*                               END OF MAIN REDUCTION LOOP */
    return D * B[ROW[n-1]*n+n-1];
} /* end determinant */

void inverse(int n, double A[], double AA[])
{
    int *ROW, *COL;
    double *TEMP;
    int HOLD , I_PIVOT , J_PIVOT;
    double PIVOT, ABS_PIVOT;
    int i, j, k;

    ROW = (int *)calloc(n, sizeof(int));
    COL = (int *)calloc(n, sizeof(int));
    TEMP = (double *)calloc(n, sizeof(double));
    memcpy(AA,A,n*n*sizeof(double));

/*                  SET UP ROW AND COLUMN INTERCHANGE VECTORS */
    for (k=0; k<n; k++) {
      ROW[k] = k ;
      COL[k] = k ;
    }

/*                         BEGIN MAIN REDUCTION LOOP */

    for (k=0; k<n; k++) {

/*            FIND LARGEST ELEMENT FOR PIVOT */
      PIVOT = AA[ROW[k]*n+COL[k]] ;
      I_PIVOT = k;
      J_PIVOT = k;
      for(i=k; i<n; i++){
        for(j=k; j<n; j++){
          ABS_PIVOT = abs(PIVOT) ;
          if( abs(AA[ROW[i]*n+COL[j]]) > ABS_PIVOT ){
            I_PIVOT = i ;
            J_PIVOT = j ;
            PIVOT = AA[ROW[i]*n+COL[j]] ;
          }
        }
      }
      if(abs(PIVOT) < 1.0E-10){
        free(ROW);
        free(COL);
        free(TEMP);
        printf("MATRIX is SINGULAR !!! \n");
        return;
      }

      HOLD = ROW[k];
      ROW[k]= ROW[I_PIVOT];
      ROW[I_PIVOT] = HOLD ;
      HOLD = COL[k];
      COL[k]= COL[J_PIVOT];
      COL[J_PIVOT] = HOLD ;

/*                                REDUCE ABOUT PIVOT */
      AA[ROW[k]*n+COL[k]] = 1.0 / PIVOT ;
      for (j=0; j<n; j++) {
        if( j != k ){
          AA[ROW[k]*n+COL[j]] = AA[ROW[k]*n+COL[j]] * AA[ROW[k]*n+COL[k]];
        }
      }

/*                       INNER REDUCTION LOOP */
      for (i=0; i<n; i++) {
        if( k != i ){
          for (j=0; j<n; j++) {
            if( k != j ){
              AA[ROW[i]*n+COL[j]] = AA[ROW[i]*n+COL[j]] - AA[ROW[i]*n+COL[k]] *
                                   AA[ROW[k]*n+COL[j]] ;
            }
          }
          AA[ROW[i]*n+COL [k]] = - AA[ROW[i]*n+COL[k]] * AA[ROW[k]*n+COL[k]] ;
        }
      }
    }
/*      END OF MAIN REDUCTION LOOP */

/*      UNSCRAMBLE ROWS */
    for (j=0; j<n; j++) {
      for (i=0; i<n; i++) {
        TEMP[COL[i]] = AA[ROW[i]*n+j];
      }
      for (i=0; i<n; i++) {
        AA[i*n+j] = TEMP[i] ;
      }
    }

/*    UNSCRAMBLE COLUMNS */
    for (i=0; i<n; i++) {
      for (j=0; j<n; j++) {
        TEMP[ROW[j]] = AA[i*n+COL[j]] ;
      }
      for (j=0; j<n; j++) {
        AA[i*n+j] = TEMP[j] ;
      }
    }
  free(ROW);
  free(COL);
  free(TEMP);
} /* end inverse */

void mat_mul(int n, double A[], double B[], double C[])
{
  int i, j, k;
  for(i=0; i<n; i++){
    for(j=0; j<n; j++){
      C[i*n+j] = 0.0;
      for(k=0; k<n; k++){
        C[i*n+j] = C[i*n+j] + A[i*n+k] * B[k*n+j];
      }
    }
  }
}

void mat_vec_mul(int n, double A[], double B[], double C[])
{
  int i, j, k;
  for(i=0; i<n; i++){
    C[i] = 0.0;
    for(j=0; j<n; j++){
      C[i] = C[i] + A[i*n+j] * B[j];
    }
  }
}

void mat_add(int n, double A[], double B[], double C[])
{
  int i, j;
  for(i=0; i<n; i++) 
    for(j=0; j<n; j++)
      C[i*n+j] = A[i*n+j] + B[i*n+j];
}

void mat_sub(int n, double A[], double B[], double C[])
{
  int i, j;
  for(i=0; i<n; i++) 
    for(j=0; j<n; j++)
      C[i*n+j] = A[i*n+j] - B[i*n+j];
}

void vec_sub(int n, double A[], double B[], double C[])
{
  int i;
  for(i=0; i<n; i++) C[i] = A[i] - B[i];
}

void mat_put(int n, double A[])
{
  int i, j;
  for(i=0; i<n; i++) 
    for(j=0; j<n; j++)
      printf("MAT[%d,%d] = %lf \n", i, j, A[i*n+j]);
}

void vec_put(int n, double A[])
{
  int i;
  for(i=0; i<n; i++) printf("VEC[%d] = %lf \n", i, A[i]);
}


/*         TEST PROGRAM TO EXERCIZE  simeq, etc                 */
/*                                                              */
/*                                                              */
/* THIS TEST:  SETS UP AN INITIAL COEFFICIENT MATRIX            */
/*             SETS UP THE DEPENDENT VECTOR                     */
/*             SOLVES THE LINEAR EQUATIONS                      */
/*             RECOMPUTES THE DEPENDENT VECTOR                  */
/*                                                              */

/*                       ROUTINE ( USED LOCALLY )               */
static mat_init_1(int n, double B[])
{
    int i, j;

    for(i=0; i<n; i++){
      for(j=0; j<n; j++){
        if( i > j )
          B[i*n+j] = (double)(i+1);
        else
          B[i*n+j] = (double)(j+1);
      }
    }
} /* end mat_init_1 */


main()
{
  double A[4][4];
  double AI[4][4];
  static double Y[4] = { 30.0 , 31.0 , 34.0 , 40.0 };
  double X[4];
  double VECTOR_RESULT[4];
  double VECTOR_RESULT_2[4];
  static double A2[3][3] = {{ 1.0 , 2.0 , 3.0 } , { 4.0 , 5.0
     , 6.0 } , { 4.0 , 4.0 , 4.0 }};
  double A2I[3][3];
  static double Y2[3] = { 1.5 , 2.5 , 3.5 };
  double X2[3];
  double VR2[3];
  double D;

  printf ( "INITIAL MATRIX A\n" );
  mat_init_1 (4, (double *)A );
  mat_put( 4, (double *)A );
  printf ( "INITIAL VECTOR Y \n" );
  vec_put( 4, Y );
  printf ( "SOLVING EQUATIONS \n" );
  simeq( 4, (double *)A , Y , X );
  printf ( "X = SOLUTION \n" );
  vec_put(4, (double *)X );
  printf ( "CHECK IF SOLUTION GIVES BACK Y \n" );
  mat_vec_mul(4, (double *)A, X, VECTOR_RESULT);
  vec_put(4, VECTOR_RESULT );
  printf ( "CHECK FOR ZERO VECTOR \n" );
  vec_sub(4, VECTOR_RESULT, Y, VECTOR_RESULT_2);
  vec_put( 4, VECTOR_RESULT_2 );
  inverse(4, (double *)A, (double *)AI );
  printf ( "PRINT INVERSE \n" );
  mat_put(4, (double *)AI );
  printf ( "CHECK FOR SOLUTION VECTOR, USING A_INVERSE * Y \n" );
  mat_vec_mul(4, (double *)AI, Y, VECTOR_RESULT);
  vec_put( 4, VECTOR_RESULT );
  printf ( "CHECK FOR ZERO VECTOR \n" );
  vec_sub(4, VECTOR_RESULT, X, VECTOR_RESULT_2);
  vec_put( 4, VECTOR_RESULT_2 );
  inverse(4, (double *)AI, (double *)A);
  printf ( "PRINT INVERSE OF INVERSE \n" );
  mat_put(4, (double *)A);
  printf ( "INITIAL VECTOR Y, CHECK \n" );
  vec_put( 4, Y );
  printf("\n \n");

  printf ( "INITIAL MATRIX A2 \n" );
  mat_put( 3, (double *)A2 );
  printf ( "INITIAL VECTOR Y2 \n" );
  vec_put( 3, Y2 );
  printf ( "SOLVING EQUATIONS \n" );
  simeq( 3, (double *)A2 , Y2 , X2);
  printf ( "X2 = SOLUTION \n" );
  vec_put(3, X2 );
  printf ( "CHECK IF SOLUTION GIVES BACK Y2 \n" );
  mat_vec_mul(3, (double *)A2, X2, VR2);
  vec_put(3, VR2 );
  printf ( "CHECK FOR ZERO VECTOR \n" );
  vec_sub(3, VR2, Y2, VR2);
  vec_put(3, VR2 );
  inverse(3, (double *)A2, (double *)A2I );
  printf ( "expect singular matrix \n" );
  printf("\n \n");

  printf ( "DETERMINANT OF A \n" );
  D = determinant(4, (double *)A );
  printf ( "DETERMINANT = %lf \n",D );
  printf ( "DETERMINANT OF A2 \n" );
  D = determinant( 3, (double *)A2 );
  printf ( "DETERMINANT = %lf \n",D );
  printf ( "DONE \n \n" );
  return 0;
} /* end main of test_simeq.c */