' ******************************************************************
' QR decomposition
' ******************************************************************

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

#DEFINE MatOk 0
' No error

#DEFINE MatSing -2
' Quasi-singular matrix

#DEFINE MatErrDim -3 
' Non-compatible dimensions

' ------------------------------------------------------------------
' Global variable
' ------------------------------------------------------------------

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

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

SUB QR_Decomp(A() AS DOUBLE, R() AS DOUBLE)
' ------------------------------------------------------------------
' QR decomposition. Factors the matrix A (n x m, with n >= m) as
' a product Q * R where Q is a (n x m) column-orthogonal matrix, 
' and R a (m x m) upper triangular matrix. This routine is used 
' in conjunction with QR_Solve to solve a system of equations.
' ------------------------------------------------------------------
' Input parameter   : A = matrix to be transformed
' Output parameters : A = transformed matrix (= Q)
'                     R = upper triangular matrix
' ------------------------------------------------------------------
' The error code is returned in the global variable ErrCode:
'   ErrCode = MatOk     ==> no error
'   ErrCode = MatErrDim ==> non-compatible dimensions (N < M)
'   ErrCode = MatSing   ==> quasi-singular matrix
' ------------------------------------------------------------------
' NB : This procedure destroys the original matrix A
' ------------------------------------------------------------------

  DIM AS INTEGER I, J, K, Lb, Ub1, Ub2
  DIM AS DOUBLE  Sum

  Lb  = LBOUND(A, 1)
  Ub1 = UBOUND(A, 1)
  Ub2 = UBOUND(A, 2)

  IF Ub1 < Ub2 THEN ErrCode = MatErrDim : EXIT SUB

  FOR K = Lb TO Ub2
    ' Compute the "k"th diagonal entry in R 
    Sum = 0
    FOR I = Lb TO Ub1
      Sum = Sum + A(I,K) * A(I,K)
    NEXT I

    IF Sum = 0 THEN ErrCode = MatSing : EXIT SUB

    R(K,K) = SQR(Sum)

    ' Divide the entries in the "k"th column of A by the computed
    ' "k"th diagonal element of R. This begins the process of 
    ' overwriting A with Q
    FOR I = Lb TO Ub1
      A(I,K) = A(I,K) / R(K,K)
    NEXT I

    FOR J = (K + 1) TO Ub2
      ' Complete the remainder of the row entries in R 
      Sum = 0
      FOR I = Lb TO Ub1
        Sum = Sum + A(I,K) * A(I,J)
      NEXT I
      R(K,J) = Sum

      ' Update the column entries of the Q/A matrix 
      FOR I = Lb TO Ub1
        A(I,J) = A(I,J) - A(I,K) * R(K,J)
      NEXT I
    NEXT J
  NEXT K

  ErrCode = MatOk
END SUB

SUB QR_Solve(Q() AS DOUBLE, R() AS DOUBLE, _
             B() AS DOUBLE, X() AS DOUBLE)
' ------------------------------------------------------------------
' Solves a system of equations by the QR decomposition,
' after the matrix has been transformed by QR_Decomp.
' ------------------------------------------------------------------
' Input parameters : Q, R = matrices from QR_Decomp
'                    B    = constant vector
' Output parameter : X    = solution vector
' ------------------------------------------------------------------

  DIM AS INTEGER I, J, Lb, Ub1, Ub2
  DIM AS DOUBLE  Sum

  Lb  = LBOUND(Q, 1)
  Ub1 = UBOUND(Q, 1)
  Ub2 = UBOUND(Q, 2)

  ' Form Q'B and store the result in X 
  FOR J = Lb TO Ub2
    X(J) = 0
    FOR I = Lb TO Ub1
      X(J) = X(J) + Q(I,J) * B(I)
    NEXT I
  NEXT J

  ' Update X with the solution vector 
  X(Ub2) = X(Ub2) / R(Ub2,Ub2)
  FOR I = (Ub2 - 1) TO Lb STEP -1
    Sum = 0
    FOR J = (I + 1) TO Ub2
      Sum = Sum + R(I,J) * X(J)
    NEXT J
    X(I) = (X(I) - Sum) / R(I,I)
  NEXT I
END SUB
