/* simeq_plus.c */
#include "simeq_plus.h"
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#undef  abs
#define abs(a) ((a)<0.0?(-(a)):(a))

void simeq(int n, double A[], double Y[], double X[])
{

/*      PURPOSE : SOLVE THE LINEAR SYSTEM OF EQUATIONS WITH REAL     */
/*                COEFFICIENTS   [A] * |X| = |Y|                     */
/*                                                                   */
/*      INPUT  : THE NUMBER OF EQUATIONS  n                          */
/*               THE REAL MATRIX  A   should be A[i][j] but A[i*n+j] */
/*               THE REAL VECTOR  Y                                  */
/*      OUTPUT : THE REAL VECTOR  X                                  */
/*                                                                   */
/*      METHOD : GAUSS-JORDAN ELIMINATION USING MAXIMUM ELEMENT      */
/*               FOR PIVOT.                                          */
/*                                                                   */
/*      USAGE  :     simeq(n,A,X,Y);                                 */
/*                                                                   */
/*                                                                   */
/*    WRITTEN BY : JON SQUIRE , 28 MAY 1983                          */
/*    ORIGONAL DEC 1959 for IBM 650, TRANSLATED TO OTHER LANGUAGES   */
/*    e.g. FORTRAN converted to Ada converted to C                   */

    double *B;           /* [n][n+1]  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,m;

    B = (double *)calloc((n+1)*(n+1), sizeof(double));
    ROW = (int *)calloc(n, sizeof(int));
    m = n+1;

/*                        BUILD WORKING DATA STRUCTURE               */
    for(i=0; i<n; i++){
      for(j=0; j<n; j++){
        B[i*m+j] = A[i*n+j];
      }
      B[i*m+n] = Y[i];
    }
/*                        SET UP ROW  INTERCHANGE VECTORS            */
    for(k=0; k<n; k++){
      ROW[k] = k;
    }

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

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

/*             HAVE PIVOT, INTERCHANGE ROW POINTERS                */
      HOLD = ROW[k];
      ROW[k] = ROW[I_PIVOT];
      ROW[I_PIVOT] = HOLD;

/*                        CHECK FOR NEAR SINGULAR                  */
      if( ABS_PIVOT < 1.0E-25 ){
        for(j=k+1; j<n+1; j++){
          B[ROW[k]*m+j] = 0.0;
        }
        printf("redundant row (singular) %d \n", ROW[k]);
      } /* singular, delete row */
      else{

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

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

/*                               FINISHED INNER REDUCTION           */
    }

/*                               END OF MAIN REDUCTION LOOP         */
/*                               BUILD  X  FOR RETURN, UNSCRAMBLING ROWS */
    for(i=0; i<n; i++){
      X[i] = B[ROW[i]*m+n];
    }
    free(B);
    free(ROW);
} /* end simeq */

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-25){
        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-25){
        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_add(int n, double A[], double B[], double C[])
{
  int i;
  for(i=0; i<n; i++) C[i] = A[i] + B[i];
}

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]);
}

/* end simeq_plus.c */