--     svd_package.ada          Singular Value Decomposition
--     Given matrix A, m by n, find matricies U, W, V such that
--     A = U * W * V**T  (possibly permuted) where:
--     W is an n by n diagonal matrix of non negative elements, "vector"
--       these are the singular values of A in decending order
--     U is an m by n column orthnormal matrix, U**T * U = I
--     V is an n by n orthonormal matrix, V * V**T = I and V**T * V= I
--     A**-1 = V * [diag(1/W(k)] * U**T
--     A * x = b solved by
--         x = V * [diag(1/W(k))] * (U**T * b)  via  SVD_SOLVE using U
--
--     The singular values are the square roots of the eigenvalues of A * A**T
--     Beware if any singular values are near zero, the Solve and Inverse fail

with Real_Arrays; use Real_Arrays;

package Svd_Package is

  procedure Svd(A: in Real_Matrix;    -- m by n
                Uu: out Real_Matrix;  -- m by n
                Vv: out Real_Matrix;  -- n by n
                Ww: out Real_Vector;  -- n
                Ierr: out Integer);

  procedure Svd(A: in Real_Matrix;    -- m by n
                Uu: out Real_Matrix;  -- m by n
                Vv: out Real_Matrix;  -- n by n
                Ww: out Real_Vector); -- n
                                      -- exception raised, no ierr

  -- A * X = B    return is X  ,uses U,V,W produced by svd(A,U,V,W)
  function Svd_Solve(U:Real_Matrix;
                     V:Real_Matrix;
                     W:Real_Vector;
                     B:Real_Vector) return Real_Vector;

  -- inverse(A) := V * 1/W * U**T, uses U, V, W produced by from svd(A,U,V,W)
  function Svd_Inverse(U:Real_Matrix;
                       V:Real_Matrix;
                       W:Real_Vector) return Real_Matrix;

  Matrix_Data_Error: exception ;
  Matrix_Solution_Error: exception ;

end Svd_Package;


with Elementary_Functions; use Elementary_Functions;

package body Svd_Package is
  --     THIS SUBROUTINE IS A TRANSLATION OF THE ALGOL PROCEDURE SVD,
  --     NUM. MATH. 14, 403-420(1970) BY GOLUB AND REINSCH.
  --     HANDBOOK FOR AUTO. COMP., VOL II-LINEAR ALGEBRA, 134-151(1971).
  --     THEN TRANSLATED FROM FORTRAN TO ADA
  --
  --            T
  --     A=U W V  OF A REAL M BY N RECTANGULAR MATRIX.  HOUSEHOLDER
  --     BIDIAGONALIZATION AND A VARIANT OF THE QR ALGORITHM ARE USED.
  --
  --        IERR IS SET TO
  --          ZERO       FOR NORMAL RETURN,
  --          K          IF THE K-TH SINGULAR VALUE HAS NOT BEEN
  --                     DETERMINED AFTER 30 ITERATIONS,
  --
  --     MACHEP IS A MACHINE DEPENDENT PARAMETER SPECIFYING
  --     THE RELATIVE PRECISION OF FLOATING POINT ARITHMETIC.

  procedure Svd(A: in Real_Matrix;    -- m by n
                Uu: out Real_Matrix;  -- m by n
                Vv: out Real_Matrix;  -- n by n
                Ww: out Real_Vector;  -- n
                Ierr: out Integer) is
    subtype Real is Float;
    M:Integer := A'length(1);
    N:Integer := A'length(2);
    U:Real_Matrix(1..M,1..N) := A;
    V:Real_Matrix(1..N,1..N);
    W:Real_Vector(1..N);
    Rv1:Real_Vector(1..N);
    Its:Integer := 0;
    L:Integer := 0;
    L1:Integer := 0;
    I:Integer := 0;
    K:Integer := 0;
    K1:Integer := 0;
    Mn:Integer := 0;
    C,F,G,H,S,X,Y,Z,Eps,Scale,Machep:Real;

    function Amax1(A,B:Real) return Real is
    begin
      if A>B then
        return A;
      end if ;
      return B;
    end Amax1;

    function Sign(Val,Sgn:Real) return Real is
    begin
      if Sgn<0.0 then
        return - abs (Val);
      end if ;
      return abs (Val);
    end Sign;
  begin
    if Uu'length(1)/=M or Uu'length(2)/=N or
       Vv'length(1)/=N or Vv'length(2)/=N or
       Ww'length/=N then
      raise Array_Index_Error;
    end if ;
    Machep := 2.0**(-23);
    Ierr := 0;
    --     HOUSEHOLDER REDUCTION TO BIDIAGONAL FORM
    G := 0.0;
    Scale := 0.0;
    X := 0.0;
    for I in 1..N loop -- 300
      L := I+1;
      Rv1(I) := Scale*G;
      G := 0.0;
      S := 0.0;
      Scale := 0.0;
      if I<=M then  -- 210
        for K in I..M loop  -- 120
          Scale := Scale+ abs (U(K,I));
        end loop ;  -- 120
        if Scale/=0.0 then  -- 210
          for K in I..M loop  -- 130
            U(K,I) := U(K,I)/Scale;
            S := S+U(K,I)**2;
          end loop ;  -- 130
          F := U(I,I);
          G := -Sign(Sqrt(S),F);
          H := F*G-S;
          U(I,I) := F-G;
          if I/=N then  -- 190
            for J in L..N loop  -- 150
              S := 0.0;
              for K in I..M loop  -- 140
                S := S+U(K,I)*U(K,J);
              end loop ;  -- 140
              F := S/H;
              for K in I..M loop  -- 150
                U(K,J) := U(K,J)+F*U(K,I);
              end loop ;  -- 150
            end loop ;  -- 150
          end if ;  -- 190
          for K in I..M loop  -- 200
            U(K,I) := Scale*U(K,I);
          end loop ;  -- 200
        end if ;  -- 210
      end if ;  -- 210
      W(I) := Scale*G;
      G := 0.0;
      S := 0.0;
      Scale := 0.0;
      --IF (I > M .OR. I = N) GO TO 290
      if I<=M and I/=N then  -- 290
        for K in L..N loop  -- 220
          Scale := Scale+ abs (U(I,K));
        end loop ;  -- 220
        if Scale/=0.0 then  -- 290
          for K in L..N loop  -- 230
            U(I,K) := U(I,K)/Scale;
            S := S+U(I,K)**2;
          end loop ;  -- 230
          F := U(I,L);
          G := -Sign(Sqrt(S),F);
          H := F*G-S;
          U(I,L) := F-G;
          for K in L..N loop  -- 240
            Rv1(K) := U(I,K)/H;
          end loop ;  -- 240
          if I/=M then  -- 270
            for J in L..M loop  -- 260
              S := 0.0;
              for K in L..N loop  -- 250
                S := S+U(J,K)*U(I,K);
              end loop ;  -- 250
              for K in L..N loop  -- 260
                U(J,K) := U(J,K)+S*Rv1(K);
              end loop ;  -- 260
            end loop ;  -- 260
          end if ;  -- 270
          for K in L..N loop  -- 280
            U(I,K) := Scale*U(I,K);
          end loop ;  -- 280
        end if ;  -- 290
      end if ;  -- 290
      X := Amax1(X, abs (W(I))+ abs (Rv1(I)));
    end loop ;  -- 300
    -- ACCUMULATION OF RIGHT-HAND TRANSFORMATIONS
    -- FOR I:=N STEP -1 UNTIL 1 DO
    for Ii in 1..N loop  -- 400
      I := N+1-Ii;
      if I/=N then  -- 390
        if G/=0.0 then  -- 360
          for J in L..N loop  -- 320
          -- DOUBLE DIVISION AVOIDS POSSIBLE UNDERFLOW
            V(J,I) := (U(I,J)/U(I,L))/G;
          end loop ;  -- 320
          for J in L..N loop  -- 350
            S := 0.0;
            for K in L..N loop  -- 340
              S := S+U(I,K)*V(K,J);
            end loop ;  -- 340
            for K in L..N loop  -- 350
              V(K,J) := V(K,J)+S*V(K,I);
            end loop ;  -- 350
          end loop ;  -- 350
        end if ;  -- 360
        for J in L..N loop  -- 380
          V(I,J) := 0.0;
          V(J,I) := 0.0;
        end loop ;  -- 380
      end if ;  -- 390
      V(I,I) := 1.0;
      G := Rv1(I);
      L := I;
    end loop ;  -- 400
    -- ACCUMULATION OF LEFT-HAND TRANSFORMATIONS
    -- FOR I:=MIN(M,N) STEP -1 UNTIL 1 DO
    Mn := N;
    if M<N then
      Mn := M;
    end if ;
    for Ii in 1..Mn loop
      -- 500
      I := Mn+1-Ii;
      L := I+1;
      G := W(I);
      if I/=N then  -- 430
        for J in L..N loop
          -- 420
          U(I,J) := 0.0;
        end loop ;  -- 420
      end if ;  -- 430
      if G/=0.0 then  -- 475
        if I/=Mn then  -- 460
          for J in L..N loop  -- 450
            S := 0.0;
            for K in L..M loop  -- 440
              S := S+U(K,I)*U(K,J);
            end loop ;  -- 440
            -- DOUBLE DIVISION AVOIDS POSSIBLE UNDERFLOW
            F := (S/U(I,I))/G;
            for K in I..M loop  -- 450
              U(K,J) := U(K,J)+F*U(K,I);
            end loop ;  -- 450
          end loop ;  -- 450
        end if ;  -- 460
        for J in I..M loop
          -- 470
          U(J,I) := U(J,I)/G;
        end loop ;  -- 470
      else  -- 475
        for J in I..M loop  -- 480
          U(J,I) := 0.0;
        end loop ;  -- 480
      end if ;  -- 475
      U(I,I) := U(I,I)+1.0;
    end loop ;  -- 500
    -- DIAGONALIZATION OF THE BIDIAGONAL FORM
    Eps := Machep*X;
    -- FOR K:=N STEP -1 UNTIL 1 DO
    for Kk in 1..N loop  -- 700
      K1 := N-Kk;
      K := K1+1;
      Its := 0;
      -- TEST FOR SPLITTING.
      -- FOR L:=K STEP -1 UNTIL 1 DO
      loop  -- 520
        for Ll in 1..K loop  -- 530
          L1 := K-Ll;
          L := L1+1;
          if abs (Rv1(L))<=Eps then
            goto L565;
          end if ;
          -- RV1(1) IS ALWAYS ZERO, SO THERE IS NO EXIT
          -- THROUGH THE BOTTOM OF THE LOOP
          if (abs W(L1))<=Eps then
            exit ;  -- GO TO 540
          end if ;
        end loop ;
        -- 530
        --  CANCELLATION OF RV1(L) IF L GREATER THAN 1
        --540
        C := 0.0;
        S := 1.0;
        for I in L..K loop  -- 560
          F := S*Rv1(I);
          Rv1(I) := C*Rv1(I);
          if abs (F)<=Eps then
            exit ;  -- GO TO 565
          end if ;
          G := W(I);
          H := Sqrt(F*F+G*G);
          W(I) := H;
          C := G/H;
          S := -F/H;
          for J in 1..M loop  -- 550
            Y := U(J,L1);
            Z := U(J,I);
            U(J,L1) := Y*C+Z*S;
            U(J,I) := -Y*S+Z*C;
          end loop ;  -- 550
        end loop ;  -- 560
         -- TEST FOR CONVERGENCE
<<L565>>
        Z := W(K);
        if L=K then
          goto L650;
        end if ;
         -- SHIFT FROM BOTTOM 2 BY 2 MINOR
        if Its=30 then
           -- SET ERROR, NO CONVERGENCE TO A SINGULAR VALUE AFTER 30 ITERATIONS
          Ierr := K;
          return ;  -- raise ???
        end if ;
        Its := Its+1;
        X := W(L);
        Y := W(K1);
        G := Rv1(K1);
        H := Rv1(K);
        F := ((Y-Z)*(Y+Z)+(G-H)*(G+H))/(2.0*H*Y);
        G := Sqrt(F*F+1.0);
        F := ((X-Z)*(X+Z)+H*(Y/(F+Sign(G,F))-H))/X;
        -- NEXT QR TRANSFORMATION
        C := 1.0;
        S := 1.0;
        for I1 in L..K1 loop  -- 600
          I := I1+1;
          G := Rv1(I);
          Y := W(I);
          H := S*G;
          G := C*G;
          Z := Sqrt(F*F+H*H);
          Rv1(I1) := Z;
          C := F/Z;
          S := H/Z;
          F := X*C+G*S;
          G := -X*S+G*C;
          H := Y*S;
          Y := Y*C;
          for J in 1..N loop  -- 570
            X := V(J,I1);
            Z := V(J,I);
            V(J,I1) := X*C+Z*S;
            V(J,I) := -X*S+Z*C;
          end loop ;  -- 570
          Z := Sqrt(F*F+H*H);
          W(I1) := Z;
           -- ROTATION CAN BE ARBITRARY IF Z IS ZERO
          if Z/=0.0 then  -- 580
            C := F/Z;
            S := H/Z;
          end if ;  -- 580
          F := C*G+S*Y;
          X := -S*G+C*Y;
          for J in 1..M loop  -- 590
            Y := U(J,I1);
            Z := U(J,I);
            U(J,I1) := Y*C+Z*S;
            U(J,I) := -Y*S+Z*C;
          end loop ;  -- 590
        end loop ;  -- 600
        Rv1(L) := 0.0;
        Rv1(K) := F;
        W(K) := X;
      end loop ;  -- 520
       -- CONVERGENCE
<<L650>>
      if Z<0.0 then
        -- W(K) IS MADE NON-NEGATIVE
        W(K) := -Z;
        for J in 1..N loop
          V(J,K) := -V(J,K);
        end loop ;
      end if ;
    end loop ;  -- 700
    Ww := W;
    Uu := U;
    Vv := V;
  end Svd;

  -- alternate call to svd
  procedure Svd(A: in Real_Matrix;       -- m by n
                Uu: out Real_Matrix;     -- m by n
                Vv: out Real_Matrix;     -- n by n
                Ww: out Real_Vector) is  -- n
                -- exception raised, no ierr
    Ierr:Integer;
  begin
    Svd(A,Uu,Vv,Ww,Ierr);
    if Ierr/=0 then
      raise Matrix_Data_Error;
    end if ;
  end Svd;


  -- uses U, V, W produced by svd(A,U,V,W)
  -- A * X = B    return is X
  function Svd_Solve(U:Real_Matrix;
                     V:Real_Matrix;
                     W:Real_Vector;
                     B:Real_Vector) return Real_Vector is
    subtype Real is Float;
    M:Integer := U'length(1);
    N:Integer := U'length(2);
    S:Real;
    X:Real_Vector(1..N);
    Tmp:Real_Vector(1..N);
  begin
    if V'length(1)/=N or V'length(2)/=N or W'length/=N or B'length/=N then
      raise Array_Index_Error;
    end if ;
    for J in 1..N loop
      S := 0.0;
      if W(J-1+W'first)/=0.0 then
        for I in 1..M loop
          S := S+U(I-1+U'first(1),J-1+U'first(2))*B(I-1+B'first);
        end loop ;
        S := S/W(J-1+W'first);
      end if ;
      Tmp(J) := S;
    end loop ;
    for J in 1..N loop
      S := 0.0;
      for Jj in 1..N loop
        S := S+V(J-1+V'first(1),Jj-1+V'first(2))*Tmp(Jj);
      end loop ;
      X(J) := S;
    end loop ;
    return X;
  end Svd_Solve;

  -- returned inverse AI := V * 1/W * transpose(U)
  -- U, V, W computed from svd(A, U, V, W)
  function Svd_Inverse(U:Real_Matrix;
                       V:Real_Matrix;
                       W:Real_Vector) return Real_Matrix is
    subtype Real is Float;
    M:Integer := U'length(1);
    N:Integer := U'length(2);
    Ai:Real_Matrix(1..N,1..M);
    S:Real;
  begin
    if V'length(1)/=N or V'length(2)/=N or W'length/=N then
      raise Array_Index_Error;
    end if ;
    for I in 1..N loop
      if W(I-1+W'first)/=0.0 then
        for J in 1..M loop
          S := 0.0;
          for K in 1..N loop
            S := S+V(I-1+V'first(1),K-1+V'first(2))*
                   U(J-1+U'first(1),K-1+U'first(2))/W(K-1+W'first);
          end loop ;
          Ai(I,J) := S;
        end loop ;
      else
        for J in 1..M loop
          Ai(I,J) := 0.0;
        end loop ;
      end if ;
    end loop ;
    return Ai;
  exception
    when others =>
      raise Matrix_Data_Error;
  end Svd_Inverse;
end Svd_Package;