#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "gauss.h"

static void swap_array(int n, double *p, double *q);

/* to solve a[][] x[] = b[] using gauss method                  */
/*  | a[0]  a[1]   a[2]   ... a[n-1]   |  | x[0] |     | b[0] | */
/*  | a[n]  a[n+1] a[n+2] ... a[2n-1]  |  | x[1] |     | b[1] | */
/*  |    ......                        |  |  ..  |  =  |  ..  | */
/*  |    .....                a[n^2-1] |  |x[n-1]|     |b[n-1]| */
Boolean gauss(int ndim, double *a, double *b){
   int i, j, k, pivot;
   double max, tmp;
   double *ori, *new;
   const double eps=1e-10;
   for(i=0;i<ndim;i++){
      /* find j for a[j][i] = max(a[i][i], a[i+1][i], ..., a[n-1][i]) */
      /* swap a[i][] with a[j][]                                      */
      max=fabs(a[i*ndim+i]); pivot=i;
      for(j=i+1;j<ndim;j++) if(fabs(a[j*ndim+i])>max){
         pivot=j; max=fabs(a[j*ndim+i]);
      }
      if(pivot!=i){
         ori=a+(i*ndim);
         new=a+(pivot*ndim);
         swap_array(ndim,ori,new);
         tmp=b[i]; b[i]=b[pivot]; b[pivot]=tmp;
      }
      for(j=i+1;j<ndim;j++){
         a[j*ndim+i] /= a[i*ndim+i];
         for(k=i+1;k<ndim;k++){
            a[j*ndim+k]-=(a[i*ndim+k]*a[j*ndim+i]);
         }
         b[j]-=(b[i]*a[j*ndim+i]);
      }
   }

   for(i=ndim-1;i>=0;i--){
      for(j=i+1;j<ndim;j++){
         b[i]-=a[i*ndim+j]*b[j];
      }
      b[i] /= a[i*ndim+i];
   }
   if(fabs(a[ndim*ndim-1])>eps) return TRUE;
   else return FALSE;
}

static void swap_array(int n, double *p, double *q){
   int i;
   double tmp;
   for(i=0;i<n;i++){
      tmp=p[i]; p[i]=q[i]; q[i]=tmp;
   }
}
