' ******************************************************************
' Singular value decomposition
' ******************************************************************

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

#DEFINE MatOk 0
' No error

#DEFINE MatNonConv -1
' Non-convergence

#DEFINE MatErrDim -3
' Non-compatible dimensions

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

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

' ------------------------------------------------------------------
' External functions
' ------------------------------------------------------------------

DECLARE FUNCTION Max(X AS DOUBLE, Y AS DOUBLE) AS DOUBLE
' Maximum of 2 reals

DECLARE FUNCTION IMin(X AS INTEGER, Y AS INTEGER) AS INTEGER
' Minimum of 2 integers

DECLARE FUNCTION Pythag(X AS DOUBLE, Y AS DOUBLE) AS DOUBLE
' Sqrt(X^2 + Y^2)

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

FUNCTION DSgn(A AS DOUBLE, B AS DOUBLE) AS DOUBLE
  IF B < 0 THEN DSgn = -ABS(A) ELSE DSgn = ABS(A)
END FUNCTION

SUB SV_Decomp(A() AS DOUBLE, S() AS DOUBLE, V() AS DOUBLE)
' ------------------------------------------------------------------
' Singular value decomposition. Factors the matrix A (n x m, 
' with n >= m) as a product U * S * V' where U is a (n x m) 
' column-orthogonal matrix, S is a (m x m) diagonal matrix with 
' elements >= 0 (the singular values) and V is a (m x m) orthogonal 
' matrix. This routine is used in conjunction with SV_Solve to
' solve a system of equations.
' ------------------------------------------------------------------
' Input parameters  : A = matrix to be transformed
' Output parameters : A = contains the elements of U
'                     S = vector of singular values
'                     V = orthogonal matrix
' ------------------------------------------------------------------
' The error code is returned in the global variable ErrCode:
'   ErrCode = MatOk      ==> no error
'   ErrCode = MatErrDim  ==> non-compatible dimensions (N < M)
'   ErrCode = MatNonConv ==> non-convergence
' ------------------------------------------------------------------
' NB : This procedure destroys the original matrix A
' ------------------------------------------------------------------
' This subroutine is a translation of the EISPACK subroutine SVD.
' Householder bidiagonalization and a variant of the QR algorithm 
' are used.
'
' This is a crude translation. Many of the original goto's
' have been kept!
' ------------------------------------------------------------------

  DIM AS INTEGER I, J, K, L, I1, K1, L1, Lb, Ub1, Ub2, Mn, Its
  DIM AS DOUBLE  C, F, G, H, T, X, Y, Z, Tst1, Tst2, Scale

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

  IF Lb <> LBOUND(A, 2) OR Ub1 < Ub2 THEN
    ErrCode = MatErrDim
    EXIT SUB
  END IF

  DIM AS DOUBLE R(Lb TO Ub2)

  Scale = 0
  G = 0
  X = 0

  ' Householder reduction to bidiagonal form 

  FOR I = Lb TO Ub2
    L = I + 1
    R(I) = Scale * G
    G = 0
    T = 0
    Scale = 0
    IF I > Ub1 THEN GOTO L210

    FOR K = I TO Ub1
      Scale = Scale + ABS(A(K, I))
    NEXT K
        
    IF Scale = 0 THEN GOTO L210

    FOR K = I TO Ub1
      A(K, I) = A(K, I) / Scale
      T = T + A(K, I) * A(K, I)
    NEXT K

    F = A(I, I)
    G = - DSgn(SQR(T), F)
    H = F * G - T
    A(I, I) = F - G
    IF I = Ub2 THEN GOTO L190

    FOR J = L TO Ub2
      T = 0
      FOR K = I TO Ub1
        T = T + A(K, I) * A(K, J)
      NEXT K
      F = T / H
      FOR K = I TO Ub1
        A(K, J) = A(K, J) + F * A(K, I)
      NEXT K
    NEXT J

L190:

   FOR K = I TO Ub1
      A(K, I) = Scale * A(K, I)
    NEXT K

L210:

    S(I) = Scale * G
    G = 0
    T = 0
    Scale = 0
    IF (I > Ub1) OR (I = Ub2) THEN GOTO L290

    FOR K = L TO Ub2
      Scale = Scale + ABS(A(I, K))
    NEXT K

    IF Scale = 0 THEN GOTO L290

    FOR K = L TO Ub2
      A(I, K) = A(I, K) / Scale
      T = T + A(I, K) * A(I, K)
    NEXT K

    F = A(I, L)
    G = - DSgn(SQR(T), F)
    H = F * G - T
    A(I, L) = F - G

    FOR K = L TO Ub2
      R(K) = A(I, K) / H
    NEXT K

    IF I = Ub1 THEN GOTO L270

    FOR J = L TO Ub1
      T = 0
      FOR K = L TO Ub2
        T = T + A(J, K) * A(I, K)
      NEXT K
      FOR K = L TO Ub2
        A(J, K) = A(J, K) + T * R(K)
      NEXT K
    NEXT J

L270:

    FOR K = L TO Ub2
      A(I, K) = Scale * A(I, K)
    NEXT K

L290:

    X = Max(X, ABS(S(I)) + ABS(R(I)))
  NEXT I

  ' Accumulation of right-hand transformations 

  FOR I = Ub2 TO Lb STEP -1
    IF I = Ub2 THEN GOTO L390
    IF G = 0 THEN GOTO L360

    FOR J = L TO Ub2
      ' Double division avoids possible underflow 
      V(J, I) = (A(I, J) / A(I, L)) / G
    NEXT J

    FOR J = L TO Ub2
      T = 0
      FOR K = L TO Ub2
        T = T + A(I, K) * V(K, J)
      NEXT K
      FOR K = L TO Ub2
        V(K, J) = V(K, J) + T * V(K, I)
      NEXT K
    NEXT J

L360:

    FOR J = L TO Ub2
      V(I, J) = 0
      V(J, I) = 0
    NEXT J

L390:

    V(I, I) = 1
    G = R(I)
    L = I
  NEXT I

L410: ' Accumulation of left-hand transformations 

  Mn = IMin(Ub1, Ub2)

  FOR I = Mn TO Lb STEP -1
    L = I + 1
    G = S(I)
    IF I = Ub2 THEN GOTO L430

    FOR J = L TO Ub2
      A(I, J) = 0
    NEXT J

L430:

    IF G = 0 THEN GOTO L475
    IF I = Mn THEN GOTO L460

    FOR J = L TO Ub2
      T = 0

      FOR K = L TO Ub1
        T = T + A(K, I) * A(K, J)
      NEXT K

      ' Double division avoids possible underflow 
      F = (T / A(I, I)) / G

      FOR K = I TO Ub1
        A(K, J) = A(K, J) + F * A(K, I)
      NEXT K
    NEXT J

L460:

    FOR J = I TO Ub1
      A(J, I) = A(J, I) / G
    NEXT J

    GOTO L490

L475:

    FOR J = I TO Ub1
      A(J, I) = 0
    NEXT J
        
L490:

    A(I, I) = A(I, I) + 1
  NEXT I

L510: ' Diagonalization of the bidiagonal form

  Tst1 = X
  FOR K = Ub2 TO Lb STEP -1
    K1 = K - 1
    Its = 0

L520: ' Test for splitting

    FOR L = K TO Lb STEP -1
      L1 = L - 1
      Tst2 = Tst1 + ABS(R(L))
      IF Tst2 = Tst1 THEN GOTO L565
      ' R(Lb) is always zero, so there is no exit
      ' through the bottom of the loop
      Tst2 = Tst1 + ABS(S(L1))
      IF Tst2 = Tst1 THEN GOTO L540
    NEXT L

L540: ' Cancellation of R(L) if L greater than 1

    C = 0
    T = 1

    FOR I = L TO K
      F = T * R(I)
      R(I) = C * R(I)
      Tst2 = Tst1 + ABS(F)
      IF Tst2 = Tst1 THEN GOTO L565
      G = S(I)
      H = Pythag(F, G)
      S(I) = H
      C = G / H
      T = - F / H

      FOR J = Lb TO Ub1
        Y = A(J, L1)
        Z = A(J, I)
        A(J, L1) = Y * C + Z * T
        A(J, I) = - Y * T + Z * C
      NEXT J
    NEXT I

L565: ' Test for convergence

    Z = S(K)
    IF L = K THEN GOTO L650

    IF Its = 30 THEN ErrCode = MatNonConv : EXIT SUB

    ' Shift from bottom 2 by 2 minor 
    Its = Its + 1
    X = S(L)
    Y = S(K1)
    G = R(K1)
    H = R(K)
    F = 0.5 * (((G + Z) / H) * ((G - Z) / Y) + Y / H - H / Y)
    G = Pythag(F, 1)
    F = X - (Z / X) * Z + (H / X) * (Y / (F + DSgn(G, F)) - H)

    ' Next QR transformation 
    C = 1
    T = 1

    FOR I1 = L TO K1
      I = I1 + 1
      G = R(I)
      Y = S(I)
      H = T * G
      G = C * G
      Z = Pythag(F, H)
      R(I1) = Z
      C = F / Z
      T = H / Z
      F = X * C + G * T
      G = - X * T + G * C
      H = Y * T
      Y = Y * C

      FOR J = Lb TO Ub2
        X = V(J, I1)
        Z = V(J, I)
        V(J, I1) = X * C + Z * T
        V(J, I) = - X * T + Z * C
      NEXT J

      Z = Pythag(F, H)
      S(I1) = Z
      ' Rotation can be arbitrary if Z is zero 
      IF Z = 0 THEN GOTO L580
      C = F / Z
      T = H / Z

L580:

      F = C * G + T * Y
      X = - T * G + C * Y

      FOR J = Lb TO Ub1
        Y = A(J, I1)
        Z = A(J, I)
        A(J, I1) = Y * C + Z * T
        A(J, I) = - Y * T + Z * C
      NEXT J
    NEXT I1

    R(L) = 0
    R(K) = F
    S(K) = X
    GOTO L520

L650: ' Convergence 
    IF Z >= 0 THEN GOTO L700

    ' S(K) is made non-negative 
    S(K) = - Z

    FOR J = Lb TO Ub2
      V(J, K) = - V(J, K)
    NEXT J

L700:

  NEXT K

  ErrCode = MatOk
END SUB

SUB SV_SetZero(S() AS DOUBLE, Tol AS DOUBLE)
' ------------------------------------------------------------------
' Sets the singular values to zero if they are lower 
' than a specified threshold.
' ------------------------------------------------------------------
' Input parameters : S   = vector of singular values
'                    Tol = relative tolerance
'                          Threshold value will be Tol * Max(S)
' Output parameter : S   = modified singular values
' ------------------------------------------------------------------

  DIM AS DOUBLE  Threshold
  DIM AS INTEGER I, Lb, Ub

  Lb = LBOUND(S)
  Ub = UBOUND(S)

  Threshold = S(Lb)
  FOR I = Lb + 1 TO Ub
    IF S(I) > Threshold THEN Threshold = S(I)
  NEXT I

  Threshold = Tol * Threshold
  FOR I = Lb TO Ub
    IF S(I) < Threshold THEN S(I) = 0
  NEXT I
END SUB

SUB SV_Solve(U() AS DOUBLE, S() AS DOUBLE, V() AS DOUBLE, _
             B() AS DOUBLE, X() AS DOUBLE)
' ------------------------------------------------------------------
' Solves a system of equations by singular value decomposition,
' after the matrix has been transformed by SV_Decomp, and the
' lowest singular values have been set to zero by SV_SetZero.
' ------------------------------------------------------------------
' Input parameters : U, S, V = vector and matrices from SV_Decomp
'                    B       = constant vector
'
' Output parameter : X = solution vector
'                      = V * Diag(1/s(i)) * U' * B, for s(i) <> 0
' ------------------------------------------------------------------

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

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

  DIM AS DOUBLE Tmp(Lb TO Ub2)
  
  FOR J = Lb TO Ub2
    Sum = 0
    IF S(J) > 0 THEN
      FOR I = Lb TO Ub1
        Sum = Sum + U(I,J) * B(I)
      NEXT I
      Sum = Sum / S(J)
    END IF
    Tmp(J) = Sum
  NEXT J
      
  FOR J = Lb TO Ub2
    Sum = 0
    FOR JJ = Lb TO Ub2
      Sum = Sum + V(J,JJ) * Tmp(JJ)
    NEXT JJ    
    X(J) = Sum
  NEXT J
END SUB

SUB SV_Approx(U() AS DOUBLE, S() AS DOUBLE, _
              V() AS DOUBLE, A() AS DOUBLE)
' ------------------------------------------------------------------
' Approximates a matrix A by the product USV', after the lowest
' singular values have been set to zero by SV_SetZero.
' ------------------------------------------------------------------
' Input parameters : U, S, V = vector and matrices from SV_Decomp
' Output parameter : A       = approximated matrix
' ------------------------------------------------------------------

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

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

  FOR I = Lb TO Ub1
    FOR J = Lb TO Ub2
      A(I,J) = 0
      FOR K = Lb TO Ub2
        IF S(K) > 0 THEN A(I,J) = A(I,J) + U(I,K) * V(J,K)
      NEXT K
    NEXT J
  NEXT I
END SUB
