#include <math.h>
#include "lupack.h"

#define ORDER 1000 /* maksimalna mo"zna velikost linearnega sistema */
#define TINY 1e-5

/**************************************************************************/
/*+
   This function replaces the matrix m by the LU decomposition of a rowwise
   permutation of itself.  indx is an output vector which records the
   row permutation effected by the partial pivoting.  d is output as 1 or -1
   depending on whether the number of row interchanges was even or odd.
   Vrne -1 ob napaki;
-*/
/**************************************************************************/

int ludcmp(
float     *m,                 /* (I/O) input matrix and its decomp.*/
int       n,          	      /* velikost sistema */
int       indx[],             /* (O) permutation vector */
float     *d)                 /* (O) 1 or -1 */
{
int        i, imax=0, j, k;
float   big, dum, sum, temp;
float   vv[ORDER];

   *d = 1.0;
   for (i=0; i<n; i++) {
      big = 0.0;
      for (j=0; j<n; j++)
         if ((temp=fabs(m[i*n+j]))>big) big = temp;
      if (big==0.0) return -1;
      vv[i] = 1.0/big;
   }
   for (j=0; j<n; j++) {
      for (i=0; i<j; i++) {
         sum = m[i*n+j];
         for (k=0; k<i; k++) sum -= m[i*n+k]*m[k*n+j];
         m[i*n+j] = sum;
      }
      big = 0.0;
      for (i=j; i<n; i++) {
         sum = m[i*n+j];
         for (k=0; k<j; k++) sum -= m[i*n+k]*m[k*n+j];
         m[i*n+j] = sum;
         if ((dum=vv[i]*fabs(sum))>=big) {
            big = dum;
            imax = i;
         }
      }
      if (j!=imax) {
	 for (k=0; k<n; k++) {
            dum = m[imax*n+k];
            m[imax*n+k] = m[j*n+k];
            m[j*n+k] = dum;
         }
         *d = -(*d);
         vv[imax] = vv[j];
      }
      indx[j] = imax;
      if (m[j*n+j]==0.0) m[j*n+j] = TINY;
      if (j!=n-1) {
         dum = 1.0/(m[j*n+j]);
	 for (i=j+1; i<n; i++) m[i*n+j] *= dum;
      }
   }
   return 0;

/*  END GL_LUDCMP FUNCTION  */
}


/**************************************************************************/
/*+
   This function solves the set of n (ORDER) linear equations MX=B.  The
   matrix m is not as the matrix M but rather as its LU decomposition,
   returned by ludcmp.  indx is the permutation vector also returned by
   ludcmp.  b is the right-hand side vector B and returns the solution
   vector X.
-*/
/**************************************************************************/
int lubksb(
float   *m,                 /* (I) LU decomposition coef. matrix */
int     n, 		     /* velikost sistema */
int     indx[],              /* (I) permutation vector */
float   b[])                 /* (I) RHS vector and solution vector */
{
int        i, id=0, ii = 0, ip, j;
float   sum;

   for (i=0; i<n; i++) {
      ip = indx[i];
      sum = b[ip];
      b[ip] = b[i];
      if (id)
         for (j=ii; j<=i-1; j++) sum -= m[i*n+j]*b[j];
      else
         if (sum) {
            ii = i;
            id = 1;
         }
      b[i] = sum;
   }
   for (i=n-1; i>=0; i--) {
      sum = b[i];
      for (j=i+1; j<n; j++) sum -= m[i*n+j]*b[j];
      b[i] = sum/m[i*n+i];
   }
   return 0;

/*  END GL_LUBKSB FUNCTION  */
}