' ******************************************************************
' LU decomposition
' ******************************************************************

' ------------------------------------------------------------------
' Error codes
' ------------------------------------------------------------------

#DEFINE MatOk 0
' No error

#DEFINE MatSing -2
' Quasi-singular matrix

#DEFINE MatErrDim -3
' Non-compatible dimensions

' ------------------------------------------------------------------
' Machine-dependent constant
' ------------------------------------------------------------------

#DEFINE MachEp 2.220446049250313D-16
' Floating point precision: 2^(-52)

' ------------------------------------------------------------------
' Global variables
' ------------------------------------------------------------------

COMMON SHARED ErrCode AS INTEGER
' Error code from the latest function evaluation

DIM SHARED AS INTEGER Index()
' Saves permutation index

' ******************************************************************

SUB LU_Decomp(A() AS DOUBLE)
' ------------------------------------------------------------------
' LU decomposition. Factors the square matrix A as a product L * U,
' where L is a lower triangular matrix (with unit diagonal terms)
' and U is an upper triangular matrix. This routine is used in 
' conjunction with LU_Solve to solve a system of equations.
' ------------------------------------------------------------------
' Input parameter  : A = matrix to be transformed
' Output parameter : A contains the elements of L and U
' ------------------------------------------------------------------
' The error code is returned by function MathErr:
'   ErrCode = MatOk     ==> no error
'   ErrCode = MatErrDim ==> non-compatible dimensions
'   ErrCode = MatSing   ==> quasi-singular matrix
' ------------------------------------------------------------------
' NB : This procedure destroys the original matrix A
' ------------------------------------------------------------------

  DIM AS INTEGER I, Imax, J, K, Lb, Ub
  DIM AS DOUBLE  Pvt, T, Sum

  Lb = LBOUND(A, 1)
  Ub = UBOUND(A, 1)

  IF LBOUND(A, 2) <> Lb AND UBOUND(A, 2) <> Ub THEN
    ErrCode = MatErrDim
    EXIT SUB
  END IF

  DIM AS DOUBLE V(Lb TO Ub)

  REDIM AS INTEGER Index(Lb TO Ub)

  FOR I = Lb TO Ub
    Pvt = 0
    FOR J = Lb TO Ub
      IF ABS(A(I,J)) > Pvt THEN Pvt = ABS(A(I,J))
      NEXT J
    IF Pvt < MachEp THEN ErrCode = MatSing : EXIT SUB
    V(I) = 1 / Pvt
  NEXT I

  FOR J = Lb TO Ub
    FOR I = Lb TO J - 1
      Sum = A(I,J)
      FOR K = Lb TO I - 1
        Sum = Sum - A(I,K) * A(K,J)
      NEXT K
      A(I,J) = Sum
    NEXT I

    Imax = 0
    Pvt = 0

    FOR I = J TO Ub
      Sum = A(I,J)
      FOR K = Lb TO J - 1
        Sum = Sum - A(I,K) * A(K,J)
      NEXT K
      A(I,J) = Sum
      T = V(I) * ABS(Sum)
      IF T > Pvt THEN Pvt = T : Imax = I
    NEXT I

    IF J <> Imax THEN
      FOR K = Lb TO Ub
        SWAP A(Imax, K), A(J, K)
      NEXT K
      V(Imax) = V(J)
    END IF

    Index(J) = Imax

    IF A(J,J) = 0 THEN A(J,J) = MachEp

    IF J <> Ub THEN
      T = 1 / A(J,J)
      FOR I = J + 1 TO Ub
        A(I,J) = A(I,J) * T
      NEXT I
    END IF
  NEXT J

  ErrCode = MatOk
END SUB

SUB LU_Solve(A() AS DOUBLE, B() AS DOUBLE, X() AS DOUBLE)
' ------------------------------------------------------------------
' Solves a system of equations whose matrix has been transformed by
' LU_Decomp
' ------------------------------------------------------------------
' Input parameters : A = result from LU_Decomp
'                    B = constant vector
' Output parameter : X = solution vector
' ------------------------------------------------------------------

  DIM AS INTEGER I, Ip, J, K, Lb, Ub
  DIM AS DOUBLE  Sum

  Lb = LBOUND(A, 1)
  Ub = UBOUND(A, 1)

  K = Lb - 1

  FOR I = Lb TO Ub
    X(I) = B(I)
  NEXT I

  FOR I = Lb TO Ub
    Ip = Index(I)
    Sum = X(Ip)
    X(Ip) = X(I)
    IF K >= Lb THEN
      FOR J = K TO I - 1
        Sum = Sum - A(I,J) * X(J)
      NEXT J
    ELSEIF Sum <> 0 THEN
      K = I
    END IF
    X(I) = Sum
  NEXT I

  FOR I = Ub TO Lb STEP -1
    Sum = X(I)
    IF I < Ub THEN
      FOR J = I + 1 TO Ub
        Sum = Sum - A(I,J) * X(J)
      NEXT J
    END IF
    X(I) = Sum / A(I,I)
  NEXT I
END SUB

