with Standard_Floating_Numbers;           use Standard_Floating_Numbers;
with Standard_Complex_Numbers;            use Standard_Complex_Numbers;
with Standard_Mathematical_Functions;     use Standard_Mathematical_Functions;

package body Standard_Complex_QR_Decomposition is

-- AUXILIARIES :

  function min0 ( a,b : integer ) return integer is

  -- DESCRIPTION : returns the minimum of a and b.

  begin
    if a <= b
     then return a;
     else return b;
    end if;
  end min0;

  function dmax1 ( a,b : double_float ) return double_float is

  -- DESCRIPTION : returns the maximum of a and b.

  begin
    if a >= b
     then return a;
     else return b;
    end if;
  end dmax1;

  function cdabs ( a : Complex_Number ) return double_float is

  -- DESCRIPTION :
  --   Computes the modulus of the complex number, hopefully this
  --   corresponds to the `cdabs' fortran function.

    res : double_float := SQRT(REAL_PART(a)**2 + IMAG_PART(a)**2);

  begin
    return res;
  end cdabs;

  function csign ( a,b : Complex_Number ) return Complex_Number is

  -- DESCRIPTION : translated from
  --       csign(zdum1,zdum2) = cdabs(zdum1)*(zdum2/cdabs(zdum2)) 

  begin
    return (Create(cdabs(a)/cdabs(b))*b);
  end csign;

  procedure zswap ( a : in out Standard_Complex_Matrices.Matrix;
                    k1,k2 : in natural ) is

  -- DESCRIPTION :
  --   Swaps the columns k1 and k2 in the matrix a.

    tmp : Complex_Number;

  begin
    for i in a'range(1) loop
      tmp := a(i,k1); a(i,k1) := a(i,k2); a(i,k2) := tmp;
    end loop;
  end zswap;
 
  function znrm2 ( a : Standard_Complex_Matrices.Matrix; row,col : natural )
                 return double_float is

  -- DESCRIPTION :
  --   Computes the 2-norm of the vector in the column col of the matrix,
  --   starting at the given row.

    sum : Complex_Number := Create(0.0);

  begin
    for i in row..a'last(1) loop
      sum := sum + Conjugate(a(i,col))*a(i,col);
    end loop;
    return SQRT(REAL_PART(sum));
  end znrm2;

  function new_znrm2 ( a : Standard_Complex_Matrices.Matrix; row,col : natural )
                 return double_float is

  -- DESCRIPTION :
  --   Computes the 2-norm of the vector in the column col of the matrix,
  --   starting at the given row.

	ssq,scale,temp : double_float;

  begin
    scale := 0.0;
    ssq := 1.0;
    for i in row..a'last(1) loop
      if REAL_PART(a(i,col)) /= 0.0
       then temp := abs(REAL_PART(a(i,col)));
            if scale < temp
             then ssq := 1.0 + ssq*(scale/temp)**2;
                  scale := temp;
             else ssq :=       ssq*(scale/temp)**2;
            end if;
      end if;
      if IMAG_PART(a(i,col)) /= 0.0
       then temp := abs(IMAG_PART(a(i,col)));
            if scale < temp
             then ssq := 1.0 + ssq*(scale/temp)**2;
                  scale := temp;
             else ssq :=       ssq*(scale/temp)**2;
            end if;
      end if;
    end loop;
    return (scale*SQRT(ssq));
  end new_znrm2;

  function zdot ( a : Standard_Complex_Matrices.Matrix; row,k1,k2 : natural ) 
                return Complex_Number is

  -- DESCRIPTION :
  --   Returns the inner product of the vectors in the columns k1 and k2,
  --   starting at the given row.

    res : Complex_Number := Create(0.0);

  begin
    for i in row..a'last(1) loop
      res := res + Conjugate(a(i,k1))*a(i,k2);
    end loop;
    return res;
  end zdot;

  procedure zaxpy ( a : in out Standard_Complex_Matrices.Matrix; 
                    f : in Complex_Number; row,k1,k2 : in integer ) is

  -- DESCRIPTION :
  --   The column k2 is added with f times the column k1, starting at row.

  begin
    for i in row..a'last(1) loop
      a(i,k2) := a(i,k2) + f*a(i,k1);
    end loop;
  end zaxpy;

  procedure zscal ( a : in out Standard_Complex_Matrices.Matrix;
                    f : in Complex_Number; row,col : in integer ) is

  -- DESCRIPTION :
  --   Multiplies the column col of the matrix with f, starting at row.

  begin
    for i in row..a'last(1) loop
      a(i,col) := f*a(i,col);
    end loop;
  end zscal;

  procedure QRD ( x : in out Standard_Complex_Matrices.Matrix;
                  qraux : in out Standard_Complex_Vectors.Vector;
                  jpvt : in out Standard_Natural_Vectors.Vector;
                  piv : in boolean ) is

    n : constant natural := x'length(1);  -- number of rows
    p : constant natural := x'length(2);  -- number of columns
    work : Standard_Complex_Vectors.Vector(x'range(2));
    jj,jp,lp1,lup,maxj,pl,pu : integer;
    maxnrm,tt : double_float;
    nrmxl,t : Complex_Number;
    negj,swapj : boolean;

  begin
    pl := 1; pu := 0;
    if piv 
     then for j in x'range(2) loop       -- rearrange columns according to jpvt
            swapj := (jpvt(j) > 0);
            negj := (jpvt(j) < 0);
            jpvt(j) := j;
            if negj
             then jpvt(j) := -j;
            end if;
            if (swapj and then (j /= pl))
             then zswap(x,pl,j);
                  jpvt(j) := jpvt(pl);
                  jpvt(pl) := j;
                  pl := pl + 1;
            end if;
          end loop;
          pu := p;
          for j in 1..p loop
            jj := p - j + 1;
            if jpvt(jj) < 0
             then jpvt(jj) := -jpvt(jj);
                  if jj /= pu
                   then zswap(x,pu,jj);
                        jp := jpvt(pu);
                        jpvt(pu) := jpvt(jj);
                        jpvt(jj) := jp;
                  end if;
                  pu := pu - 1;
            end if;
          end loop; 
    end if;
    for j in pl..pu loop                   -- compute norms of the free columns
      qraux(j) := Create(znrm2(x,1,j));
      work(j) := qraux(j);
    end loop;
    lup := min0(n,p);                 -- perform the householder reduction of x
    for l in 1..lup loop
      if (l >= pl and l < pu)
       then maxnrm := 0.0;               -- locate column with largest norm and
            maxj := l;                        -- bring it in the pivot position
            for j in l..pu loop
              if REAL_PART(qraux(j)) > maxnrm
               then maxnrm := REAL_PART(qraux(j));
                    maxj := j;
              end if;
            end loop;
            if maxj /= l
             then zswap(x,l,maxj);
                  qraux(maxj) := qraux(l);
                  work(maxj) := work(l);
                  jp := jpvt(maxj);
                  jpvt(maxj) := jpvt(l);
                  jpvt(l) := jp;
           end if;
      end if;
      qraux(l) := Create(0.0);
      if l /= n
       then
	  nrmxl := Create(znrm2(x,l,l));           -- householder transformation
         if AbsVal(nrmxl) /= 0.0                                -- for column l
          then
            if AbsVal(x(l,l)) /= 0.0
             then nrmxl := csign(nrmxl,x(l,l));
            end if;
            zscal(x,Create(1.0)/nrmxl,l,l);       -- RIGHT
           -- zscal(x,1.0/nrmxl,l,l);             WRONG !
            x(l,l) := Create(1.0) + x(l,l);
            lp1 := l + 1;         --  apply the transformation to the remaining
            for j in lp1..p loop                --  columns, updating the norms
              t := -zdot(x,l,l,j)/x(l,l);
              zaxpy(x,t,l,l,j);
              if (j >= pl) and (j <= pu) and (AbsVal(qraux(j)) /= 0.0)
               then
                 tt := 1.0 - (cdabs(x(l,j))/REAL_PART(qraux(j)))**2;
                 tt := dmax1(tt,0.0);
                 t := Create(tt);
                 tt := 1.0
                    + 0.05*tt*(REAL_PART(qraux(j))/REAL_PART(work(j)))**2;
                 if tt /= 1.0
                  then qraux(j) := qraux(j)*Create(SQRT(REAL_PART(t)));
                  else qraux(j) := Create(znrm2(x,l+1,j));
                       work(j) := qraux(j);
                 end if;
              end if;
            end loop;
            qraux(l) := x(l,l);                      -- save the transformation
            x(l,l) := -nrmxl;
         end if;
      end if;
    end loop;
  end QRD;

  procedure Basis ( qr : in out Standard_Complex_Matrices.Matrix;
                    x : in Standard_Complex_Matrices.Matrix ) is

    sum : Complex_Number;
    wrk : Standard_Complex_Vectors.Vector(qr'range(2));

  begin
    for j in x'range(2) loop               -- compute jth column of q
      for i in qr'range(1) loop
        sum := x(i,j);
        for k in qr'first(2)..(j-1) loop
          sum := sum - qr(i,k)*qr(k,j);
        end loop;
        wrk(i) := sum/qr(j,j);
      end loop;
      for i in qr'range(1) loop
        qr(i,j) := wrk(i);
      end loop;
    end loop;
  end Basis;

end Standard_Complex_QR_Decomposition;