MODULE SVD;

IMPORT Math;
FROM LongFloat IMPORT CopySign;

PROCEDURE HouseholderReflection(
    VAR a: ARRAY OF ARRAY OF LONGREAL;
    i, j: CARDINAL;
    di, dj: CARDINAL;
    n: CARDINAL;
  ): LONGREAL =
  (*
    Let "v" be the vector consisting of the elements 
    "v[k] = a[i+k*di,j+k*dj]", for "k" in "0..n-1".
    
    This procedure replaces the vector "v" by the unique vector "h"
    such that "v - (h.v) h" has the form "{s, 0, 0, ...}", where "s
    = (IF v[0] < 0 THEN L2Norm(v) ELSE -L2Norm(v))".

    The quantity "s" is returned as the result of the call.
  *)

  VAR big, s, pivot: LONGREAL; ki, kj: CARDINAL;
  BEGIN
    (* This code would be much simpler and faster if Modula-3 arrays *)
    (*   had the correct semantics... *)
    <* ASSERT n > 0 *>
    pivot := a[i,j];
    (* Find element in "v" with largest absolute value, except for "v[0]": *)
    big := 0.0d0;
    ki := i + di; kj := j + dj;
    FOR k := 1 TO n-1 DO 
      big := MAX(bix, ABS(a[ki,kj])); 
      INC(ki, di); INC(kj, dj);
    END;
    IF big = 0.0d0 THEN
      (* The vector "h" is null: *)
      s := -pivot;
      a[i,j] := 0.0d0;
    ELSE
      (* Compute "L2Norm(v)", with care to avoid overflow: *)
      big := MAX(ABS(pivot), big);  (* big = LInfNorm(v) *)
      scale := 1.0d0/big;           (* scale = 1/LInfNorm(v) *)
      sumsq := 0.0d0;
      ki := i; kj := j;
      FOR k := 0 TO n-1 DO 
        WITH vk = a[ki,kj] DO 
          IF vk # 0.0d0 THEN 
            WITH svk = vk*scale DO sumsq := sumsq + svk*svk END
          END
        END;
        INC(ki, di); INC(kj, dj);
      END;
      s := Math.sqrt(sumsq);
      (* Now "s" is "L2Norm(v)/LInfNorm(v)" *)
      (* Fix its sign: *)
      IF pivot > 0.0d0 THEN s := -s END;
      (* Now compute the vector "h = lambda*(v - {s, 0, ...})": *)
      WITH
        lambda = scale/Math.sqrt(s*(s - scale*pivot)),
        res = big*s
      DO
        a[i,j] := pivot - res;
        ki := i; kj := j;
        FOR k := 0 TO n-1 DO 
          a[ki,kj] := lambda * a[ki,kj];
          INC(ki, di); INC(kj, dj);
        END;
        RETURN res
      END
    END;
  END HouseholderReflection;

PROCEDURE Bidiagonalize(
    VAR A: ARRAY OF ARRAY OF REAL;
    VAR D: ARRAY OF LONGREAL;
    VAR E: ARRAY OF LONGREAL;
  ) =
  BEGIN
    WITH
      M = NUMBER(A),
      N = NUMBER(A[0]),
      L = MIN(M, N),
      K = MAX(M, N),
      V = NEW(REF ARRAY OF LONGREAL, K)^
    DO
      <* ASSERT NUMBER(D) = L *>
      <* ASSERT NUMBER(E) = L-1 *>
      
      i := 0; j := 0;
      IF M <= N THEN
        di := 0; dj := 1 
      ELSE
        di := 1; dj := 0
      END;
      k := 0;
      FOR irot := 0 TO L+L-1 DO
        IF di > dj THEN 
          p := i; q := M; r := j; s := N
        ELSE
          p := j; q := N; r := i; s := M
        END;
        <* ASSERT p < q *>
        WITH
          (* Compute the Householder reflection that anihilates the tail: *)
          res = HouseholderReflection(a, i, j, di, dj, q - p)
        DO
          (* Apply the Householder rotation to other vectors in "a": *)
          (* This code would be much simpler and faster if Modula-3 arrays *)
          (*   had the correct semantics... *)
          ki := i + dj; kj := j + di;
          FOR x := r+1 TO s-1 DO
            (* Compute the dot product "u . h": *)
            ui := ki; uj := kj;  hi := i; hj := j;
            dot := 0.0d0;
            FOR y := p TO q-1 DO 
              dot := dot + a[ui,uj]*a[hi,hj];
              INC(ui, di); INC(uj, dj);
              INC(hi, di); INC(hj, dj)
            END;
            (* Perform the first reflection "u := u - (u.h) h": *)
            ui := ki; uj := kj;  hi := i; hj := j;
            FOR y := p TO q-1 DO 
              a[ui,uj] := a[ui,uj] - dot * a[hi,hj];
              INC(ui, di); INC(uj, dj);
              INC(hi, di); INC(hj, dj)
            END;
            (* Perform the second reflection (negate "u[p]"): *)
            a[ki,kj] := -a[ki,kj];
            INC(ki, dj); INC(kj, di)
          END;
          (* Store the new pivot value in the bidiagonalized array: *)
          IF i = j THEN D[k] := -res ELSE E[k] := -res; INC(k) END;
        END
      END
    END 
  END Bidiagonalize;

BEGIN
END SVD.