-- svd.ada     singular value decomposition
-- from Wilkinson and Reinsch: "Handbook of Automatic Computation"

with real_arrays; use real_arrays;
with text_io; use text_io;
procedure svd(AA : in real_matrix;
              QQ : in out real_vector;
              UU : in out real_matrix;
              VV : in out real_matrix ) is

  subtype real is float;

  A   : real_matrix(0..AA'length(1)-1,0..AA'length(2)-1) := AA;
  U   : real_matrix(A'range(1),A'range(2));
  V   : real_matrix(a'range(2),A'range(2)); -- V.ReDimension(n,n);
  eps : real := float'epsilon;
  tol : real := float'small/eps;
  m   : integer := A'length(1); -- number of rows
  n   : integer := A'length(2); -- number of columns
  g   : real := 0.0;
  f   : real := 0.0;
  h   : real := 0.0;
  x   : real := 0.0;
  s   : real := 0.0;
  c   : real := 0.0;
  j   : integer;
  l   : integer;
  i   : integer;
  l1  : integer;

  E   : real_vector(0..n-1); -- RowVector E(n)
  EI  : real_vector(0..n-1); -- RectMatrixRow EI(E,0)
  Q   : real_vector(0..n-1); -- Q.ReDimension(n)
  UCI : real_vector(0..n-1); -- RectMatrixCol UCI(U,0)
  UCJ : real_vector(0..n-1); -- RectMatrixCol UCJ := UCI;
  URI : real_vector(0..n-1); -- RectMatrixRow URI(U,0,1,n-1)
  URJ : real_vector(0..n-1); -- RectMatrixRow URJ := URI;
  VCI : real_vector(0..n-1); -- RectMatrixCol VCI(V,n,n,0);
  VCJ : real_vector(0..n-1); -- RectMatrixCol VCJ := VCI;
  z   : real;
  y   : real;
  sei : real;
  tfc : boolean;
  limit : integer;

  
begin
  if m<n then 
    put_line("Want no. Rows >= no. Cols");
    raise array_index_error;
  end if;
  U := A;
  for i in 0..n-1 loop
    EI.First() := g;
    sei := g;
    EI.Right();
    s := UCI.SumSquare();
    if s<tol then
      Q.element(i) := 0.0;
    else
      f := UCI.First();
      g := -sign(sqrt(s), f);
      h := f*g-s;
      UCI.First() := f-g;
      Q.element(i) := g;
      UCJ := UCI;
      for j in 1..n-1 loop
        UCJ.Right();
        UCJ.AddScaled(UCI, (UCI*UCJ)/h);
      end loop; -- j
    end if;

    s := URI.SumSquare();
    if s<tol then
      g := 0.0;
    else
      f := URI.First();
      g := -sign(sqrt(s), f);
      URI.First() := f-g;
      EI.Divide(URI,f*g-s);
      URJ := URI;
      for j in 1..m-1 loop
        URJ.Down();
        URJ.AddScaled(EI, URI*URJ);
      end loop; -- j
    end if;

    y := abs(Q.element(i)) + abs(sei); -- ? different y
    if x<y then
      x := y;
    end if;
    UCI.DownDiag();
    URI.DownDiag();
  end loop; -- on i

  V := 0.0;
  for i in reverse 0..n-1 loop
    URI.UpDiag();
    VCI.Left();
    if g/=0.0 then
      VCI.Divide(URI, URI.First()*g);
      VCJ := VCI;
      for j in 1..n-1 loop
        VCJ.Right();
        VCJ.AddScaled( VCI, (URI*VCJ) );
      end loop;
    end if;
    VCI.Zero();
    VCI.Up();
    VCI.First() := 1.0;
    g:=E.element(i);
  end loop; -- i

  for i in reverse 0..n-1 loop
    UCI.UpDiag();
    g := Q.element(i);
    URI.Reset(U,i,i+1,n-i-1);
    URI.Zero();
    if g/=0.0 then
      h:=UCI.First()*g;
      UCJ := UCI;
      for j in 1..n-1 loop
	UCJ.Right();
        UCI.Down();
        UCJ.Down();
        s := UCI*UCJ; -- ? new s
        UCI.Up();
        UCJ.Up();
        UCJ.AddScaled(UCI,s/h);
      end loop; -- j
      UCI.Divide(g);
    else
      UCI.Zero();
    end if;
    UCI.First() := UCI.First() + 1.0;
  end loop; -- i

  eps := eps*x;
  for k in reverse 0..n-1 loop
    limit := 50;
    while limit<0 loop
      limit := limit-1;
      --Real c, s; int i; int l1:=k; Boolean tfc:=FALSE;
      l1:=k;
      tfc:=FALSE;
      for l in reverse 0..k loop
	if abs(E.element(l))<=eps then
          tfc:=TRUE;
          exit;
        end if;
	if abs(Q.element(l-1))<=eps then
          l1:=l;
          exit;
        end if;
      end loop; -- l
      if not tfc then
        l:=l1;
        l1:=l-1;
        s := -1.0;
        c := 0.0;
        for i in l..k loop
          f := - s * E.element(i);
          E.element(i) := E.element(i) * c;
	  if abs(f)<=eps then
            exit;
          end if;
          g := Q.element(i);
          h := pythag(g,f,c,s);
          Q.element(i) := h;
	  RectMatrixCol UCI(U,i);
          RectMatrixCol UCJ(U,l1);
          ComplexScale(UCJ, UCI, c, s);
        end loop; -- i
      end if; -- tfc
      z := Q.element(k);
      if l=k then
        exit;
      end if;
      x := Q.element(l);
      y := Q.element(k-1);
      g := E.element(k-1);
      h := E.element(k);
      f := ((y-z)*(y+z) + (g-h)*(g+h)) / (2*h*y);
      if f>1.0 then
        g := f * sqrt(1 + square(1/f));
      elsif f<-1.0 then
        g := -f * sqrt(1 + square(1/f));
      else
        g := sqrt(f*f + 1);
      end if;
      f := ((x-z)*(x+z) + h*(y / ((f<0.0) ? f-g : f+g)-h)) / x;

      c := 1.0; s := 1.0;
      for i in l+1..k loop
	g := E.element(i);
        y := Q.element(i);
        h := s*g; g *:= c;
	z := pythag(f,h,c,s);
        E.element(i-1) := z;
	f := x*c + g*s;
        g := -x*s + g*c;
        h := y*s;
        y := y*c;
	RectMatrixCol VCI(V,i); RectMatrixCol VCJ(V,i-1);
	ComplexScale(VCI, VCJ, c, s);
	z := pythag(f,h,c,s);
        Q.element(i-1) := z;
	f := c*g + s*y;
        x := -s*g + c*y;
	RectMatrixCol UCI(U,i); RectMatrixCol UCJ(U,i-1);
	ComplexScale(UCI, UCJ, c, s);
      end loop; -- i
      E.element(l) := 0.0;
      E.element(k) := f;
      Q.element(k) := x;
    end loop; -- limit
    if l/=k then
      put_line("ConvergenceException(A)");
    end if;

    -- convergence:
    if z<0.0 then
      Q.element(k) := -z;
      RectMatrixCol VCI(V,k);
      VCI.Negate();
    end if;
  end loop; -- on k
end svd;



--static Real pythag(Real f, Real g, Real& c, Real& s)
-- return z=sqrt(f*f+g*g), c=f/z, s=g/z
-- set c=1,s=0 if z==0
-- avoid floating point overflow or divide by zero
--{
--   if (f==0 && g==0) { c=1.0; s=0.0; return 0.0; }
--   Real af = f>=0 ? f : -f;
--   Real ag = g>=0 ? g : -g;
--   if (ag<af)
--   {
--      Real h = g/f; Real sq = sqrt(1.0+h*h);
--      if (f<0) sq = -sq;           -- make return value non-negative
--      c = 1.0/sq; s = h/sq; return sq*f;
--   }
--   else
--   {
--      Real h = f/g; Real sq = sqrt(1.0+h*h);
--      if (g<0) sq = -sq;
--      s = 1.0/sq; c = h/sq; return sq*g;
--   }
--}