#include <math.h>

#include "gauss.h"

/* dokładność: jakie wielkości utożsamiać z zerem (dzielenie przez zero) */
#define EPS 1e-7
/* czy metoda Gaussa ma kontynuować obliczenia w przypadku */
/* niemożliwości znalezienia elementu podstawowego         */
#undef TRY_TO_GO_ON

/* -------------------------------------------------------------------- */
/*  PROCEDURA FGAUSS_LU                                                 */
/*                                                                      */
/*  Zastosowanie                                                        */
/*  ============                                                        */
/*                                                                      */
/*  fgauss_lu oblicza rozklad LU macierzy A metoda eliminacji Gaussa    */
/*  z czesciowym wyborem elementu podstawowego (glownego).              */
/*                                                                      */
/*  Rozklad ma postac                                                   */
/*     A = P * L * U                                                    */
/*  gdzie P jest macierza permutacji, L jest trojkatna dolna z          */
/*  jednostkowymi elementami na diagonali, zas U jest trojkatna gorna.  */
/*                                                                      */
/*  Argumenty                                                           */
/*  =========                                                           */
/*  N   (we) rozmiar macierzy A.                                        */
/*  A   (we/wy) macierz liczb typu FLOAT (wektor wskaznikow na wektory) */
/*      Na wejściu, macierz A do rozlozenia.                            */
/*      Na wyjściu, czynniki LU z rozkladu; A=P*L*U,                    */
/*      diagonalne jednostkowe elementy L nie są przechowywane.         */
/* IPIV (wy) wektor zamian. IPIV(i) == j oznacza, ze wiersz j           */
/*      powedrowal na miejsce i w i-tej iteracji.                       */
/* -------------------------------------------------------------------- */
int fgauss_lu( int N, float **A, int *IPIV)
{
  int k,idx;
  int i,j;
  float max,curr;
  float mul,tmp;
  int res;


  /* patologiczny przypadek: tablica zerowego rozmiaru */
  if( N == 0)
    return 0;
  
  for(k = 0; k < N-1; k++) {
    /* znajdujemy indeks elementu podstawowego    */
    /* t.j. max |A[i,k]|, gdzie i = k,k+1,...,N-1 */
    idx = k;
    max = -1.0;
    for(i = k; i < N; i++)  
      if((curr=fabs(A[i][k])) > max) {
	max = curr;
	idx = i;
      }	

    /* będziemy zamieniać wiersz idx-ty z k-tym                */
    /* zapamietujemy, ktory wiersz zawierał element podstawowy */
    IPIV[k] = idx;

    /* sprawdzamy, czy nie bedzie dzielenia przez zero */
    if( max < EPS ) /* A[idx][k]; po zamianie: A[k][k] */
#ifndef TRY_TO_GO_ON
      return k;
#else
    continue;
#endif
    
    /* zamieniamy wiersze idx-ty z k-tym, jeśli to potrzebne */
    if( idx != k ) 
      for(i = k; i < N; i++) {
	tmp       = A[k][i];
	A[k][i]   = A[idx][i];
	A[idx][i] = tmp;
      }
    
    /* obliczamy elementy k+1,k+2,...,n k-tego wiersza */
    mul = 1.0/A[k][k];
    for(i = k+1; i < N; i++) {
      A[i][k] *= mul;          /* L[i][k] = A[i][k]/A[k][k] */
      curr = A[i][k];
      for(j = k+1; j < N; j++)
	A[i][j] -= curr*A[k][j]; /* A[i][j] = A[i][j] - L[i][k]*A[k][j] */
    }
  }
  IPIV[N-1] = N-1;

  return 0; /* brak bledu */
}



/* -------------------------------------------------------------------- */
/*  PROCEDURA FSOLVE_LU                                                 */
/*                                                                      */
/*  Zastosowanie                                                        */
/*  ============                                                        */
/*                                                                      */
/*  fsolve_lu rozwiązuje układ równań Ax = b, mając dany rozkład LU     */
/*  macierzy A: A = P*L*U. Rozkład LU powinien być dany przez fgauss_lu */
/*  albo podobną.                                                       */
/*                                                                      */
/*  Argumenty                                                           */
/*  =========                                                           */
/*  N   (we) rozmiar macierzy A.                                        */
/*  A   (we) macierz liczb typu FLOAT rozłożona na czynniki LU.         */
/* IPIV (wy) wektor zamian. IPIV(i) == j oznacza, ze wiersz j           */
/*      powędrował na miejsce i w i-tej iteracji metody Gaussa.         */
/* B    (we/wy) na wejściu - prawa strona równania; na wyjściu          */
/*      wektor rozwiązania równania Ax = y.                             */
/* -------------------------------------------------------------------- */
int fsolve_lu( int N, float **LU, int *IPIV, float *B)
{
  float sdot,tmp;
  int k,l,idx,nm1;

  /* Najpierw rozwiązujemy L*Y = B */
  if (N - 1 > 1) {
    for (k = 1; k < N; k++) {
      idx = IPIV[k];
      tmp = B[k];
      /* zamieniamy wiersze jeśli potrzeba */
      if (idx != k) {
	B[idx] = B[k];
	B[k]   = tmp;
      }
      for (l = 0; l < k; l++)
	B[k] -= LU[k][l]*B[l];

    }
  }
  
  /* Następnie rozwiązujemy U*X = Y */
  for (k = N-1; k >= 0; k--) {
    for (l = N-1; l > k; l--) 
      B[k] -= LU[k][l]*B[l];

#ifndef TRY_TO_GO_ON
    if (fabs(LU[k][k]) < EPS)
      return k;
    else
      B[k] /= LU[k][k];
#else
    B[k] /= LU[k][k];
#endif
  }

  return 0;  
}