-- qrd_package.ada   QR decomposition 
--    Decompose a matrix A into an orthonormal matrix Q
--    times an Upper triangluar matrix R such that
--    A = Q * R
--    Q * Q**T = I   and    R is an upper diagonal matrix
--    A x = b   or   Q * R x = b  becomes  Q**T * Q * R x = Q**T b
--              or   R x = Q**T b   solve by back substitution
--    A**-1 = set of x vectors where set of b is unit vectors
--
--            Array_Index_Error is raised if matrix not square and also if
--            A, Q and R  row and column ranges are not the same.
--
-- algorithm (corrected) from Numerical Recipies
-- programmed 9/9/96 by Jon S. Squire

with Real_Arrays; use Real_Arrays;

package Qrd_Package is

  -- given A find Q and R
  procedure Qrd(A:Real_Matrix;
                Q: in out Real_Matrix;
                R: in out Real_Matrix);

  -- given Q and R from QRD, and given Y find X such that A x = y
  function Qrd_Solve(Q:Real_Matrix;
                     R:Real_Matrix;
                     Y:Real_Vector) return Real_Vector;

  -- given Q and R from QRD, find  inverse(A)
  function Qrd_Inverse(Q:Real_Matrix;
                       R:Real_Matrix) return Real_Matrix;

  -- the following pair of procedures model the Fortran calls that change inputs
  -- given A, modify A and find C and D  for use in QRDC_SOLVE
--  procedure QRDCD ( A : in out REAL_MATRIX ;
--                    C : in out REAL_VECTOR ;
--                    D : in out REAL_VECTOR ) ;
  -- given modified A, C and D from QRDCD, given B, find modified B
  -- that solves   origonal A * modified B = origonal B
--  procedure QRDCD_SOLVE ( A : REAL_MATRIX ;
--                          C : REAL_VECTOR ;
--                          D : REAL_VECTOR ;
--                          B : in out REAL_VECTOR) ;

  Singular_Error: exception ;
end Qrd_Package;

with Elementary_Functions; use Elementary_Functions;

package body Qrd_Package is

  subtype Real is Float;

  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;

  -- given A find Q and R
  procedure Qrd(A:Real_Matrix;
                Q: in out Real_Matrix;
                R: in out Real_Matrix) is
    subtype Real is Float;
    N:Integer := A'length(1);
    Aa:Real_Matrix(1..N,1..N) := A;
    B:Real_Vector(1..N);
    C:Real_Vector(1..N);
    D:Real_Vector(1..N);
    Scale:Real;
    Siz:Real:=0.0;
    Sigma:Real;
    Sum:Real;
    Tau:Real;
  begin
    if A'length(1)/=A'length(2) or A'length(1)/=Q'length(1) or
       A'length(1)/=Q'length(2) or A'length(1)/=R'length(1) or
       A'length(1)/=R'length(2) then
      raise Array_Index_Error;
    end if ;
    for I in A'range loop
      Siz := Amax1(Siz, abs A(A'first(1),I)); -- for singular check
    end loop;
    for K in 1..N-1 loop
      Scale := 0.0;
      for I in K..N loop
        Scale := Amax1(Scale, abs (Aa(I,K)));
      end loop ;
      if Scale < REAL'EPSILON*Siz then
        raise Singular_Error;
      else
        for I in K..N loop
          Aa(I,K) := Aa(I,K)/Scale;
        end loop ;
        Sum := 0.0;
        for I in K..N loop
          Sum := Sum+Aa(I,K)*Aa(I,K);
        end loop ;
        Sigma := Sign(Sqrt(Sum),Aa(K,K));
        Aa(K,K) := Aa(K,K)+Sigma;
        C(K) := Sigma*Aa(K,K);
        D(K) := -Scale*Sigma;
        for J in K+1..N loop
          Sum := 0.0;
          for I in K..N loop
            Sum := Sum+Aa(I,K)*Aa(I,J);
          end loop ;
          Tau := Sum/C(K);
          for I in K..N loop
            Aa(I,J) := Aa(I,J)-Tau*Aa(I,K);
          end loop ;
        end loop ;
      end if ;
    end loop ;
    D(N) := Aa(N,N);
    if abs D(N) < REAL'EPSILON*Siz then
      raise Singular_Error;
    end if ;

    -- now finish up outputting Q and R
    for I in 1..N loop
      for J in 1..N loop
        R(I-1+R'first(1),J-1+R'first(2)) := 0.0;
        if I=J then
          R(I-1+R'first(1),J-1+R'first(2)) := D(I);
        elsif I<J then
          R(I-1+R'first(1),J-1+R'first(2)) := Aa(I,J);
        end if ;
      end loop ;
    end loop ;
    for K in 1..N loop
      for J in 1..N loop
        B(J) := 0.0;
        if J=K then
          B(J) := 1.0;
        end if ;
      end loop ;
      for J in 1..N-1 loop
        Sum := 0.0;
        for I in J..N loop
          Sum := Sum+Aa(I,J)*B(I);
        end loop ;
        Tau := Sum/C(J);
        for I in J..N loop
          B(I) := B(I)-Tau*Aa(I,J);
        end loop ;
      end loop ;
      for J in 1..N loop
        Q(K-1+Q'first(1),J-1+Q'first(2)) := B(J);
      end loop ;
    end loop ;
  end Qrd;

  function Qrd_Solve(Q:Real_Matrix;
                     R:Real_Matrix;
                     Y:Real_Vector) return Real_Vector is
    subtype Real is Float;
    X:Real_Vector(Y'range);  -- A * x = y, where A = Q * R
    B:Real_Vector(Y'range);  -- temporary
    N:Integer := Q'length(1);
    Sum:Real;
  begin
    if Q'length(1)/=Q'length(2) or Q'length(1)/=R'length(1) or
       Q'length(1)/=R'length(2) or Q'length(1)/=Y'length then
      raise Array_Index_Error;
    end if ;
    for I in Q'range(2) loop
      Sum := 0.0;
      for J in Y'range loop
        Sum := Sum+Q(J,I)*Y(J);
      end loop ;
      B(I-Q'first(2)+B'first) := Sum;
    end loop ;
    for I in reverse R'range(1) loop
      Sum := B(I-R'first(1)+B'first);
      for J in I+1..R'last(2) loop
        Sum := Sum-R(I,J)*X(J-R'first(2)+X'first);
      end loop ;
      X(I-R'first(1)+X'first) := Sum/R(I,I);
    end loop ;
    return X;
  end Qrd_Solve;

  function Qrd_Inverse(Q:Real_Matrix;
                       R:Real_Matrix) return Real_Matrix is
    subtype Real is Float;
    Y:Real_Vector(Q'range(1));  -- unit vectors
    X:Real_Vector(Y'range);  -- column of inverse(A)
    Ai:Real_Matrix(Q'range(1),Q'range(2));  -- inverse(A)
  begin
    if Q'length(1)/=Q'length(2) or Q'length(1)/=R'length(1) or
       Q'length(1)/=R'length(2) or Q'length(1)/=Y'length then
      raise Array_Index_Error;
    end if ;
    for K in Q'range(1) loop
      Y := Unit_Vector(K,Q'length(1),Q'first(1));
      X := Qrd_Solve(Q,R,Y);
      for J in Q'range(1) loop
        Ai(J,K) := X(J-Q'first(1)+X'first);
      end loop ;
    end loop ;
    return Ai;
  end Qrd_Inverse;
end Qrd_Package;