' ******************************************************************
' Solution of a system of linear equations with a single 
' constant vector by Gauss-Jordan method
' ******************************************************************

' ------------------------------------------------------------------
' 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 variable
' ------------------------------------------------------------------

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

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

SUB LinEq(A() AS DOUBLE, B() AS DOUBLE, BYREF Det AS DOUBLE)
' ------------------------------------------------------------------
' Solves the linear system A*X = B by Gauss-Jordan algorithm
' ------------------------------------------------------------------
' On input:
'   * A(L..N, L..N) is the system matrix
'   * B(L..N) is the constant vector
'
' On output:
'   * A(L..N, L..N) contains the inverse matrix
'   * B(L..N) contains the solution vector
'   * The determinant of the system matrix is returned in Det
'   * The error code is returned in the global variable ErrCode:
'       ErrCode = MatOk     ==> no error
'       ErrCode = MatErrDim ==> non-compatible dimensions
'       ErrCode = MatSing   ==> quasi-singular matrix
' ------------------------------------------------------------------

  DIM AS INTEGER L, N     ' Bounds of A
  DIM AS INTEGER I, J, K  ' Loop variables
  DIM AS INTEGER Ik, Jk   ' Pivot coordinates
  DIM AS DOUBLE  Pvt      ' Pivot
  DIM AS DOUBLE  T        ' Auxiliary variable

  L = LBOUND(A, 1)
  N = UBOUND(A, 1)
    
  IF LBOUND(B) <> L OR UBOUND(B) <> N THEN
    ErrCode = MatErrDim
    EXIT SUB
  END IF

  DIM AS INTEGER PRow(L TO N)  ' Stores line of pivot
  DIM AS INTEGER PCol(L TO N)  ' Stores column of pivot
  DIM AS DOUBLE  MCol(L TO N)  ' Stores a column of the matrix

  Det = 1
  K = L

  DO WHILE K <= N
    ' Search for largest pivot in submatrix A[K..N, K..N]
    Pvt = A(K, K)
    Ik = K
    Jk = K
    FOR I = K TO N
      FOR J = K TO N
        IF ABS(A(I, J)) > ABS(Pvt) THEN
          Pvt = A(I, J)
          Ik = I
          Jk = J
        END IF
      NEXT J
    NEXT I

    ' Pivot too small ==> quasi-singular matrix
    IF ABS(Pvt) < MachEp THEN
      Det = 0
      ErrCode = MatSing
      EXIT SUB
    END IF

    ' Save pivot position
    PRow(K) = Ik
    PCol(K) = Jk

    ' Update determinant
    Det = Det * Pvt
    IF Ik <> K THEN Det = -Det
    IF Jk <> K THEN Det = -Det

    ' Exchange current row (K) with pivot row (Ik)
    IF Ik <> K THEN
      FOR J = L TO N
        SWAP A(K, J), A(Ik, J)
      NEXT J
      SWAP B(K), B(Ik)
    END IF

    ' Exchange current column (K) with pivot column (Jk)
    IF Jk <> K THEN
      FOR I = L TO N
        SWAP A(I, K), A(I, Jk)
      NEXT I
    END IF

    ' Store col. K of A into MCol and set this col. to 0
    FOR I = L TO N
      IF I <> K THEN
        MCol(I) = A(I, K)
        A(I, K) = 0
      ELSE
        MCol(I) = 0
        A(I, K) = 1
      END IF
    NEXT I

    ' Transform pivot row
    FOR J = L TO N
      A(K, J) = A(K, J) / Pvt
    NEXT J
    B(K) = B(K) / Pvt
    
    ' Transform other rows
    FOR I = L TO N
      IF I <> K THEN
        T = MCol(I)
        FOR J = L TO N
          A(I, J) = A(I, J) - T * A(K, J)
        NEXT J
        B(I) = B(I) - T * B(K)
      END IF
    NEXT I

    K = K + 1
  LOOP

  ' Exchange rows of inverse matrix and solution vector
  FOR I = N TO L STEP -1
    Ik = PCol(I)
    IF Ik <> I THEN
      FOR J = L TO N
        SWAP A(I, J), A(Ik, J)
      NEXT J
      SWAP B(I), B(Ik)
    END IF
  NEXT I

  ' Exchange columns of inverse matrix
  FOR J = N TO L STEP -1
    Jk = PRow(J)
    IF Jk <> J THEN
      FOR I = L TO N
        SWAP A(I, J), A(I, Jk)
      NEXT I
    END IF
  NEXT J

  ErrCode = MatOk
END SUB
