-- debugging
--with text_io; use text_io;
--with Standard_Natural_Vectors_io; use Standard_Natural_Vectors_io;

package body Generic_Floating_Linear_Solvers is

  use Field,Vectors;

-- AUXILIARIES :

  function csign ( x,y : number ) return number is

    res : number;

  begin
    if y > zero
     then res := AbsVal(x);
     else res := AbsVal(x);
          Min(res);
    end if;
    return res;
   -- return AbsVal(x) * y / AbsVal(y);
  end csign;

  function Inverse_Abs_Sum ( z : Vectors.Vector ) return number is

  -- DESCRIPTION :
  --   Returns the reciprocal of the sum of the absolute values in z.

    res,sum,acc : number;

  begin
    Copy(zero,sum);
    for i in z'range loop
      acc := AbsVal(z(i));
      Add(sum,acc);
      Clear(acc);
    end loop;
    res := one/sum;
    Clear(sum);
    return res;
  end Inverse_Abs_Sum;

-- STATIC TRIANGULATORS :

  procedure lufac ( a : in out matrix; n : in natural;
                    ipvt : out Standard_Natural_Vectors.Vector;
                    info : out natural ) is

    kp1,l,nm1 : natural;
    smax,temp,acc : number;

  begin
    info := 0;
    nm1 := n - 1;
    if nm1 >= 1
     then for k in 1..nm1 loop
            kp1 := k + 1;
            l := k;
            smax := AbsVal(a(k,k));                 -- find the pivot index l
            for i in kp1..n loop
              acc := AbsVal(a(i,k));
              if acc > smax 
               then l := i;
                    Copy(acc,smax);
              end if;
              Clear(acc);
            end loop;
            ipvt(k) := l;
            if Equal(smax,zero)
             then info := k;           -- this column is already triangulated
             else if l /= k                       -- interchange if necessary
                   then Copy(a(l,k),temp);
                        Copy(a(k,k),a(l,k));
                        Copy(temp,a(k,k));
                  end if;
                  acc := one/a(k,k);                   -- compute multipliers
                  Min(acc);
                  for i in kp1..n loop
                    Mul(a(i,k),acc);
                  end loop;
                  Clear(acc);
                  for j in kp1..n loop                     -- row elimination
                    Copy(a(l,j),temp);
                    if l /= k
                     then Copy(a(k,j),a(l,j));
                          Copy(temp,a(k,j));
                    end if;
                    for i in kp1..n loop
                      acc := temp*a(i,k);
                      Add(a(i,j),acc);
                      Clear(acc);
                    end loop;
                  end loop;
                  Clear(temp);
            end if;
            Clear(smax);
          end loop;
    end if;
    ipvt(n) := n;
    if Equal(a(n,n),zero)
     then info := n;
    end if;
  end lufac;

  procedure lufco ( a : in out Matrix; n : in natural;
                    ipvt : out Standard_Natural_Vectors.Vector;
                    rcond : out number ) is

  -- NOTE :
  --   rcond = 1/(norm(a)*(estimate of norm(inverse(a))))
  --   estimate = norm(z)/norm(y) where a*z = y and ctrans(a)*y = e.
  --   ctrans(a) is the conjugate transpose of a.
  --   The components of e are chosen to cause maximum local
  --   growth in teh elements of w where ctrans(u)*w = e.
  --   The vectors are frequently rescaled to avoid overflow.

    z : Vectors.Vector(1..n);
    info,kb,kp1,l : natural;
    s,sm,sum,anorm,ynorm,ek,t,wk,wkm,acc,absacc1,absacc2 : number;
    ipvtt : Standard_Natural_Vectors.Vector(1..n);

  begin
    Copy(zero,anorm);                                 -- compute 1-norm of a
    for j in 1..n loop
      Copy(zero,sum);
      for i in 1..n loop
        acc := AbsVal(a(i,j));
        Add(sum,acc);
        Clear(acc);
      end loop;
      if sum > anorm
       then Copy(sum,anorm);
      end if; 
    end loop;
    Clear(sum);
    lufac(a,n,ipvtt,info);                                         -- factor
   -- put_line("Factorization completed");
   -- put("ipvtt : "); put(ipvtt); new_line;
    for i in 1..n loop
      ipvt(i) := ipvtt(i);
    end loop;
    Copy(one,ek);                                    -- solve ctrans(u)*w = e
    for j in 1..n loop
      Copy(zero,z(j));
    end loop;
   -- put_line("At start of first k loop");
    for k in 1..n loop
      acc := AbsVal(z(k));
      if not Equal(acc,zero)
       then Clear(acc);
            acc := -z(k);
            ek := csign(ek,acc);
            Clear(acc);
      end if;
      acc := ek-z(k);
      absacc1 := AbsVal(acc);
      absacc2 := AbsVal(a(k,k));
      if absacc1 > absacc2
       then s := absacc2/absacc1;
            Mul(z,s);
            Mul(ek,s);
            Clear(s);
      end if;
      Clear(absacc1); Clear(absacc2);
      Clear(acc);
      wk := ek - z(k);
      wkm := -ek;
      Clear(ek);
      Sub(wkm,z(k));
      s := AbsVal(wk);
      sm := AbsVal(wkm);
      acc := AbsVal(a(k,k));
      if Equal(acc,zero)
       then Copy(one,wk);
            Copy(one,wkm);
       else wk := wk / a(k,k);
            wkm := wkm / a(k,k);
      end if;
      Clear(acc);
      kp1 := k + 1;
      if kp1 <= n
       then for j in kp1..n loop
              acc := wkm*a(k,j);
              Add(acc,z(j));
              absacc1 := AbsVal(acc);
              Add(sm,absacc1);
              Clear(acc); Clear(absacc1);
              acc := wk*a(k,j);
              Add(z(j),acc);
              Clear(acc);
              absacc1 := AbsVal(z(j));
              Add(s,absacc1);
            end loop;
            if s < sm
             then t := wkm - wk;
                  Copy(wkm,wk);
                  for j in kp1..n loop
                    acc := t*a(k,j);
                    Add(z(j),acc);
                    Clear(acc);
                  end loop;
                  Clear(t);
            end if;
      end if;
      Copy(wk,z(k)); Clear(wk); Clear(wkm);
      Clear(s); Clear(sm);
    end loop;
   -- put_line("Gone through first k loop");
    s := Inverse_Abs_Sum(z);
    Mul(z,s); Clear(s);
   -- put_line("At start of second k loop");
    for k in 1..n loop                              -- solve ctrans(l)*y = w
      kb := n+1-k;
      if kb < n
       then Copy(zero,t);
            for i in (kb+1)..n loop
              acc := a(i,kb)*z(i);
              Add(t,acc);
              Clear(acc);
            end loop;
            Add(z(kb),t); Clear(t);
      end if;
     -- put_line("In the middle of second k loop");
      acc := AbsVal(z(kb));
      if acc > one 
       then s := one / acc;
            Mul(z,s); Clear(s);
      end if;
      Clear(acc);
      l := ipvtt(kb);
     -- put_line("Just before copying in second k loop");
      if l /= kb
       then Copy(z(l),t);
            Copy(z(kb),z(l));
            Copy(t,z(kb)); Clear(t);
      end if;
     -- put_line("At end of one stage of second k loop");
    end loop;
   -- put_line("At end of second k loop");
    s := Inverse_Abs_Sum(z);
    Mul(z,s); Clear(s);
    Copy(one,ynorm);
   -- put_line("At start of third k loop");
    for k in 1..n loop                                      -- solve l*v = y
      l := ipvtt(k);
      if l /= k
       then Copy(z(l),t);
            Copy(z(k),z(l));
            Copy(t,z(k));
       else Copy(z(l),t);
      end if;
      if k < n
       then for i in (k+1)..n loop
              acc := t*a(i,k);
              Add(z(i),acc);
              Clear(acc);
            end loop;
      end if;
      Clear(t);
      acc := AbsVal(z(k));
      if acc > one
       then s := one / acc;
            Mul(z,s);
            Mul(ynorm,s); Clear(s);
      end if;
      Clear(acc);
    end loop;
   -- put_line("At end of third k loop");
    s := Inverse_Abs_Sum(z);
    Mul(z,s);
    Mul(ynorm,s); Clear(s);
   -- put_line("At start of fourth k loop");
    for k in 1..n loop                                      -- solve u*z = v
      kb := n+1-k;
      absacc1 := AbsVal(z(kb));
      absacc2 := AbsVal(a(kb,kb));
      if absacc1 > absacc2
       then s := absacc2 / absacc1;
            Mul(z,s);
            Mul(ynorm,s); Clear(s);
      end if;
      Clear(absacc1); 
      if Equal(absacc2,zero)
       then Copy(one,z(kb));
       else Div(z(kb),a(kb,kb));
      end if;
      Clear(absacc2);
      t := -z(kb);
      for i in 1..(kb-1) loop
        acc := t*a(i,kb);
        Add(z(i),acc);
        Clear(acc);
      end loop;
      Clear(t);
    end loop;
    s := Inverse_Abs_Sum(z);                             -- make znorm = 1.0
    Mul(z,s);
    Mul(ynorm,s); Clear(s);
    if Equal(anorm,zero)
     then Copy(zero,rcond);
     else rcond := ynorm/anorm;
    end if;
  end lufco;

  procedure lusolve ( a : in matrix; n : in natural;
                      ipvt : in Standard_Natural_Vectors.Vector;
                      b : in out Vectors.vector ) is

    l,nm1,kb : integer;
    temp,acc : number;
 
  begin
    nm1 := n-1;
    if nm1 >= 1                                             -- solve l*y = b
     then for k in 1..nm1 loop
            l := ipvt(k);
            Copy(b(l),temp);
            if l /= k 
             then Copy(b(k),b(l));
                  Copy(temp,b(k));
            end if;
            for i in (k+1)..n loop
              acc := temp*a(i,k);
              Add(b(i),acc);
              Clear(acc);
            end loop;
            Clear(temp);
          end loop;
    end if;
    for k in 1..n loop                                      -- solve u*x = y
      kb := n+1-k;
      Div(b(kb),a(kb,kb));
      temp := -b(kb);
      for j in 1..(kb-1) loop
        acc := temp*a(j,kb);
        Add(b(j),acc);
        Clear(acc);
      end loop;
      Clear(temp);
    end loop;
  end lusolve;

  procedure Triangulate ( a : in out Matrix; n,m : in integer ) is

    pivot,k,kcolumn : integer;
    max,temp,acc : number;

  begin
    k := 1;
    kcolumn := 1;
    while (k <= n) and (kcolumn <= m) loop
      max := zero;                                            -- find pivot
      for l in k..n loop
        if AbsVal(a(l,kcolumn)) > max
         then max := AbsVal(a(l,kcolumn));
              pivot := l;
        end if;
      end loop;
      if Equal(max,zero)
       then kcolumn := kcolumn + 1;
       else if pivot /= k                       -- interchange if necessary
             then for i in 1..m loop
                    temp := a(pivot,i);
                    a(pivot,i) := a(k,i);
                    a(k,i) := temp;
                  end loop;
            end if;
            for j in (kcolumn+1)..m loop                   -- triangulate a
              Div(a(k,j),a(k,kcolumn));
            end loop;
            Copy(one,a(k,kcolumn));
            for i in (k+1)..n loop
              for j in (kcolumn+1)..m loop
                acc := a(i,kcolumn)*a(k,j);
                Sub(a(i,j),acc);
                Clear(acc);
              end loop;
            end loop;
            for j in (k+1)..n loop
              Copy(zero,a(j,kcolumn));
            end loop;
            k := k + 1;
            kcolumn := kcolumn + 1;
       end if;
    end loop;
  end Triangulate;

  procedure Diagonalize ( a : in out Matrix; n,m : in integer ) is

    max,temp,acc : number;
    pivot,k,kcolumn : integer;

  begin
    k := 1;
    kcolumn := 1;
    while (k <= n) and (kcolumn <= m) loop
      max := zero;                                              -- find pivot
      for l in k..n loop
        if AbsVal(a(l,kcolumn)) > max
         then max := AbsVal(a(l,kcolumn));
              pivot := l;
        end if;
      end loop;
      if Equal(max,zero)
       then kcolumn := kcolumn + 1;
       else if pivot /= k                       -- interchange if necessary
             then for i in 1..m loop
                    temp := a(pivot,i);
                    a(pivot,i) := a(k,i);
                    a(k,i) := temp;
                  end loop;
            end if;
            for j in (kcolumn+1)..m loop                   -- diagonalize a
              Div(a(k,j),a(k,kcolumn));
            end loop;
            Copy(one,a(k,kcolumn));
            for i in 1..(k-1) loop
              for j in (kcolumn+1)..m loop
                acc := a(i,kcolumn)*a(k,j);
                Sub(a(i,j),acc);
                Clear(acc);
              end loop;
            end loop;
            for i in (k+1)..n loop
              for j in (kcolumn+1)..m loop
                acc := a(i,kcolumn)*a(k,j);
                Sub(a(i,j),acc);
                Clear(acc);
              end loop;
            end loop;
            for j in 1..(k-1) loop
              Copy(zero,a(j,kcolumn));
            end loop;
            for j in (k+1)..n loop
              Copy(zero,a(j,kcolumn));
            end loop;
            k := k + 1;
            kcolumn := kcolumn + 1;
      end if;
    end loop;
  end Diagonalize;

-- DYNAMIC TRIANGULATORS :

  procedure Upper_Triangulate
               ( row : in natural; mat : in out Matrix; tol : in number;
                 ipvt : in out Standard_Natural_Vectors.Vector;
                 pivot : out integer ) is

   factor,tmp,max,acc : number;
   piv,tpi : integer := 0;

  begin
    for j in mat'first(1)..row-1 loop
      if AbsVal(mat(row,j)) > tol                 -- make mat(row,j) zero
       then factor := mat(row,j)/mat(j,j);
            for k in j..mat'last(2) loop
              acc := factor*mat(j,k);
              Sub(mat(row,k),acc);
              Clear(acc);
            end loop;
      end if;
    end loop;
    for j in row..ipvt'last loop             -- search pivot
      tmp := AbsVal(mat(row,j));
      if tmp > tol
       then if piv = 0
             then max := tmp; piv := j;
             elsif tmp > max
                 then max := tmp; piv := j;
            end if;
      end if;
    end loop;
    pivot := piv;
    if piv /= 0                                -- zero row
     then if piv /= row                        -- interchange columns
           then for k in mat'range(1) loop
                  tmp := mat(k,row); mat(k,row) := mat(k,piv);
                  mat(k,piv) := tmp;
                end loop;
                tpi := ipvt(row); ipvt(row) := ipvt(piv); ipvt(piv) := tpi;
          end if;
    end if;
  end Upper_Triangulate;

  procedure Upper_Triangulate
               ( roweli : in natural; elim : in Matrix; tol : in number;
                 rowmat : in natural; mat : in out Matrix ) is

    factor,acc : number;

  begin
    if AbsVal(mat(rowmat,roweli)) > tol
     then factor := mat(rowmat,roweli)/elim(roweli,roweli);
          for i in roweli..mat'last(2) loop
            acc := factor*elim(roweli,i);
            Sub(mat(rowmat,i),acc);
            Clear(acc);
          end loop;
    end if;
  end Upper_Triangulate;

  procedure Upper_Triangulate
               ( roweli : in natural; elim : in Matrix; tol : in number;
                 firstrow,lastrow : in natural; mat : in out Matrix ) is
  begin
    for i in firstrow..lastrow loop
      Upper_Triangulate(roweli,elim,tol,i,mat);
    end loop;
  end Upper_Triangulate;

  procedure Switch ( ipvt : in Standard_Natural_Vectors.Vector;
                     row : in integer; mat : in out Matrix ) is

    tmp : Vectors.Vector(mat'range(2));

  begin
    for k in tmp'range loop
      tmp(k) := mat(row,k);
    end loop;
    for k in ipvt'range loop
      mat(row,k) := tmp(ipvt(k));
    end loop;
    for k in ipvt'last+1..mat'last(2) loop
      mat(row,k) := tmp(k);
    end loop;
  end Switch;

  procedure Switch ( k,pivot,first,last : in integer; mat : in out Matrix ) is

    tmp : number;

  begin
    if k /= pivot
     then for i in first..last loop
            tmp := mat(i,k);
            mat(i,k) := mat(i,pivot);
            mat(i,pivot) := tmp;
          end loop;
    end if;
  end Switch;

  function Solve ( mat : Matrix; tol : number;
                   ipvt : Standard_Natural_Vectors.Vector )
                 return Vectors.Vector is

    res,x : Vectors.Vector(mat'range(2)) := (mat'range(2) => zero);
    index : integer;
    acc : number;

  begin
    for i in mat'range(1) loop
      index := i;
      exit when i > mat'last(2);
      exit when AbsVal(mat(i,i)) < tol;
    end loop;
    if (AbsVal(mat(index,index)) > tol) and then (index < mat'last(2))
     then index := index + 1;
    end if;
    Copy(one,x(index));
    for i in reverse mat'first(1)..(index-1) loop
      x(i) := -mat(i,index);
      for j in i+1..index-1 loop
        acc := mat(i,j)*x(j);
        Sub(x(i),acc);
        Clear(acc);
      end loop;
      Div(x(i),mat(i,i));
    end loop;
    for k in ipvt'range loop
      res(ipvt(k)) := x(k);
    end loop;
    for k in ipvt'last+1..res'last loop
      res(k) := x(k);
    end loop;
    return res;
  end Solve;

  function Solve ( n,col : natural; mat : Matrix )
                 return Vectors.Vector is

    res : Vectors.Vector(1..n+1);
    acc : number;

  begin
    Copy(one,res(n+1));
    for i in reverse 1..n loop
      res(i) := -mat(i,col);
      for j in i+1..n loop
        acc := mat(i,j)*res(j);
        Sub(res(i),acc);
        Clear(acc);
      end loop;
      Div(res(i),mat(i,i));
    end loop;
    return res;
  end Solve;

end Generic_Floating_Linear_Solvers;