with Strings, Globals, Protected_Counters;
use Strings, Globals, Protected_Counters;

pragma Elaborate_All (Strings,Globals);

package body ScalVectors is

  procedure CheckDim(N: in Positive; V: in Vector) is
  begin
    if not (V'First=1 and V'Last=N) then
      raise Dimension_Error with "ScalVectors.CheckDim error V";
    end if;
  end CheckDim;

  procedure CheckDim(N: in Positive; A: in Matrix) is
  begin
    if not (A'First(1)=1 and A'First(2)=1 and A'Last(1)=N and A'Last(2)=N) then
      raise Dimension_Error with "ScalVectors.CheckDim error A";
    end if;
  end CheckDim;

  ------------------------- diag

  function GetDiag(A: in Matrix) return Diag is
    D: Diag(A'Range(1));
  begin
    if CheckRange and then not IsSquare(A) then
      raise Dimension_Error with "ScalVectors.GetDiag error";
    end if;
    for I in D'Range loop
      Copy(A(I,I),D(I));
    end loop;
    return D;
  end GetDiag;

  procedure Add(D: in Diag; A: in out Matrix) is
  begin
    for I in D'Range loop
      Add(D(I),A(I,I));
    end loop;
  end Add;

  procedure Sub(D: in Diag; A: in out Matrix) is
  begin
    for I in D'Range loop
      Sub(D(I),A(I,I));
    end loop;
  end Sub;

  procedure Mult(D: in Diag; W: in out Vector) is
  begin
    for I in W'Range loop
      Mult(D(I),W(I));
    end loop;
  end Mult;

  procedure Mult(D1: in Diag; D2: in out Diag) is
  begin
    for I in D2'Range loop
      Mult(D1(I),D2(I));
    end loop;
  end Mult;

  procedure Mult(D: in Diag; A: in out Matrix) is
  begin
    for I in A'Range(1) loop
      for J in A'Range(2) loop
        Mult(D(I),A(I,J));
      end loop;
    end loop;
  end Mult;

  procedure Mult(A: in out Matrix; D: in Diag) is
  begin
    for J in A'Range(2) loop
      for I in A'Range(1) loop
        Mult(D(J),A(I,J));
      end loop;
    end loop;
  end Mult;

  function "*"(D: Diag; A: Matrix) return Matrix is
    B: Matrix := A;
  begin
    Mult(D,B);
    return B;
  end "*";

  function "*"(A: Matrix; D: Diag) return Matrix is
    B: Matrix := A;
  begin
    Mult(B,D);
    return B;
  end "*";

  procedure Invert(D: in out Diag) is
    S: Scalar;
  begin
    for I in D'Range loop
      Inv(D(I),S);
      Copy(S,D(I));
    end loop;
  end Invert;

  function Inverse(D: in Diag) return Diag is
    Di: Diag(D'Range);
  begin
    for I in D'Range loop
      Inv(D(I),Di(I));
    end loop;
    return Di;
  end Inverse;
  
  procedure Invert(A: in out Matrix) is
    First: constant Integer := A'First(1);
    Last: constant Integer := A'Last(1);
    NT: constant Natural := Reserve_Tasks(Last-First,MInvert_Parallel);
  begin
    if not ((A'First(2)=First) and (A'Last(2)=Last)) then raise Dimension_Error; end if;
    if NT=0 then
      Serial_Invert(A);
    elsif First=1 then
      Parallel_Invert(NT,Last,A);
      Free_Tasks(NT);
    else
      raise Not_Implemented;
    end if;
  end Invert;

  procedure Parallel_Invert(NT,Last1: in Positive; A: in out Matrix) is
    --- do column operations in parallel
    --- assuming A is a Matrix(1..Last1,1..Last1)
    subtype Task_Range is Integer range 1 .. NT;
    subtype Range1 is Integer range 1 .. Last1;
    type Integer_Array1 is array(Range1) of Integer;
    subtype Column is Vector(Range1);
    Last2: constant Positive := 2*Last1;
    subtype Range2 is Integer range 1 .. Last2;
    type Boolean_Array2 is array(Range2) of Boolean;

    protected type Col_Counter_Type is
      procedure Update(JP: in Integer);
      procedure Next(J: out Integer);
    private
      Count: Integer := 1;
      Pivot_In_Col: Boolean_Array2 := (others => False);
    end Col_Counter_Type;

    protected body Col_Counter_Type is
      procedure Update(JP: in Integer) is
      begin
        Pivot_In_Col(JP) := True;
        Count := 1;
      end Update;
      procedure Next(J: out Integer) is
      begin
        if Count>Last1 then
          J := Count;
        else
          while Pivot_In_Col(Count) loop Count := Count+1; end loop;
          J := Count;
        end if;
        Count := Count+1;
      end Next;
    end Col_Counter_Type;

    Col_Counter: Col_Counter_Type;
    C: array(Range2) of Column;    --- global data

    procedure Col_Loop(IP: in Integer; JP: in out Integer; RP: out Flt; Tmp: in out Scalar) is
      Last_Row: constant Boolean := IP=Last1;
      IP_Minus1: constant Integer := IP-1;
      IP_Plus1: constant Integer := IP+1;
      Right_Col: Boolean;
      J: Integer;
      R: Flt;
      Pivot_Col: Column renames C(JP); --- only use of original JP
    begin
      JP := 0;    -- will become proposed new pivot column
      RP := Zero;
      loop
        Col_Counter.Next(J);
        if J>Last1 then
          exit when J>Last2;
          Right_Col := True;
        else
          Right_Col := False;
        end if;
        declare
          My_Col: Column renames C(J);
          My_Col_IP: Scalar renames My_Col(IP);
        begin
          if not IsZero(My_Col_IP) then
            --- divide row(IP) by C(IP,JP)                         -- original JP
            Quot(My_Col_IP,Pivot_Col(IP),Tmp);
            Copy(Tmp,My_Col_IP);
            --- for I /= IP, subtract C(I,JP)*row(IP) from row(I)  -- original JP
            for I in 1 .. IP_Minus1 loop
              SubProd(Pivot_Col(I),My_Col_IP,My_Col(I),Tmp);
            end loop;
            for I in IP_Plus1 .. Last1 loop
               SubProd(Pivot_Col(I),My_Col_IP,My_Col(I),Tmp);
            end loop;
          end if;
          if not (Last_Row or else Right_Col) then
            --- largest |entry| in pivot row left
            R := MaxNorm(My_Col(IP_Plus1));
            if R>RP then RP := R; JP := J; end if;
          end if;
        end;
      end loop;
    end Col_Loop;

    task type Col_Task_Type is
      entry Start(IPivot,JPivot: in Integer);
      entry Continue(MaxCol: out Integer; MaxVal: out Flt; Err: out Boolean);
    end Col_Task_Type;

    task body Col_Task_Type is
      Error: Boolean := False;
      IP,JP: Integer;
      RP: Flt;
      Tmp: Scalar;
    begin
      Proper_Rounding;
      loop
        accept Start(IPivot,JPivot: in Integer) do
          IP := IPivot;
          JP := JPivot;
        end Start;
        begin
          Col_Loop(IP,JP,RP,Tmp);
        exception
          when others => Error := True;
        end;
        accept Continue(MaxCol: out Integer; MaxVal: out Flt; Err: out Boolean) do
          MaxCol := JP;
          MaxVal := RP;
          Err := Error;
        end Continue;
        exit when IP=Last1;
      end loop;
    end Col_Task_Type;

    Col_Task: Array(Task_Range) of Col_Task_Type;

    procedure FirstPivot(JP: out Integer) is
      --- determine pivot in first row
      RP: Flt := Zero;
      R: Flt;
    begin
      for J in Range1 loop
        R := MaxNorm(A(1,J));
        if R>RP then
          JP := J;
          RP := R;
        end if;
      end loop;
    end FirstPivot;

    procedure A2C is
      --- augmented matrix C=[A|Id]
    begin
      for J in Range1 loop
        declare
          AJ: Column renames C(J);
          BJ: Column renames C(J+Last1);
        begin
          for I in Range1 loop
            Copy(A(I,J),AJ(I));
            SetZero(BJ(I));
          end loop;
          Copy(One_Scalar,BJ(J));
        end;
      end loop;
    end A2C;

    Row_Pivot: Integer_Array1 := (others => 0);

    procedure C2A is
      --- copy right half of C to A, after permuting rows
      I: Integer;
    begin
      for IP in Range1 loop
        I := Row_Pivot(IP);
        for J in Range1 loop
          Copy(C(J+Last1)(IP),A(I,J));
        end loop;
      end loop;
    end C2A;

    Error: Boolean;
    JP,J: Integer;         --- JP is pivot column
    RP,R: Flt;
    Tmp: Scalar;
  begin
    A2C;
    FirstPivot(JP);
    for IP in Range1 loop  --- IP is pivot row
      Row_Pivot(IP) := JP;
      Col_Counter.Update(JP);
      --- start other tasks
      for K in Task_Range loop Col_Task(K).Start(IP,JP); end loop;
      --- do work as well
      Col_Loop(IP,JP,RP,Tmp);
      --- wait for other tasks to finish
      for K in Task_Range loop
        Col_Task(K).Continue(J,R,Error);
        if Error then raise Sorry; end if;
        if R>RP then RP := R; JP := J; end if;
      end loop;
    end loop;
    C2A;
  end Parallel_Invert;

  ------------------------------- norms etc

  procedure Norm1(V: in Vector; S: in out Scalar) is
    T: Scalar;
  begin
    Norm(V(V'Last),S);
    for I in reverse V'First .. V'Last-1 loop
      Norm(V(I),T);
      Add(T,S);
    end loop;
  end Norm1;

  function Norm1(V: Vector) return Scalar is
    S: Scalar;
  begin
    Norm1(V,S);
    return S;
  end Norm1;

  function MaxNorm1(V: Vector) return Radius is
    R: Flt := Zero;
  begin
    for I in V'Range loop R := R+MaxNorm(V(I)); end loop;
    return R;
  end MaxNorm1;

  procedure WNorm1(W,V: in Vector; S: in out Scalar) is
    --- W(I) assumed to be nonnegative weights
    N: Scalar;
  begin
    SetZero(S);
    for I in reverse V'Range loop
      Norm(V(I),N);
      AddProd(W(I),N,S);
    end loop;
  end WNorm1;

  procedure WNorm1(W: in RadVec; V: in Vector; S: in out Scalar) is
    N,T: Scalar;
  begin
    SetZero(S);
    for I in reverse V'Range loop
      Norm(V(I),N);
      Prod(W(I),N,T);
      Add(T,S);
    end loop;
  end WNorm1;

  function WNorm1(W: RadVec; V: Vector) return Scalar is
    S: Scalar;
  begin
    WNorm1(W,V,S);
    return S;
  end WNorm1;

  procedure WBallAt0(W: in RadVec; R: in Flt; V: in out Vector) is
    --- assumes Flt ops are rounded up
  begin
    for I in V'Range loop
      BallAt0(R/W(I),V(I));
    end loop;
  end WBallAt0;

  procedure Norm1(A: in Matrix; S: in out Scalar) is
    F1: constant Integer := A'First(1);
    L1: constant Integer := A'Last(1);
    T,SJ: Scalar;
  begin
    SetZero(S);
    for J in reverse A'Range(2) loop
      Norm(A(L1,J),SJ);
      for I in reverse F1 .. L1-1 loop
        Norm(A(I,J),T);
        Add(T,SJ);
      end loop;
      Max(SJ,S);
    end loop;
  end Norm1;

  function Norm1(A: Matrix) return Scalar is
    S: Scalar;
  begin
    Norm1(A,S);
    return S;
  end Norm1;

  function MaxNorm1(A: Matrix) return Radius is
    --- assuming Flt ops are rounded up
    R: Flt := Zero;
    RJ: Flt;
  begin
    for J in reverse A'Range(2) loop
      RJ := Zero;
      for I in reverse A'Range(1) loop
        RJ := RJ+MaxNorm(A(I,J));
      end loop;
      R := RMax(RJ,R);
    end loop;
    return R;
  end MaxNorm1;

  function CheckNorm1(A: Matrix; R: Radius) return Boolean is
    --- checks if MaxNorm1(A)<=R
    RJ: Flt;
  begin
    for J in A'Range(2) loop
      RJ := Zero;
      for I in A'Range(1) loop
        RJ :=  RJ+MaxNorm(A(I,J));
      end loop;
      if RJ>R then return False; end if;
    end loop;
    return True;
  end CheckNorm1;

  procedure WNorm1(W: in Vector; A: in Matrix; S: in out Scalar) is
    --- W(I) assumed to be positive weights
    N,SJ: Scalar;
  begin
    SetZero(S);
    for J in reverse A'Range(2) loop
      SetZero(SJ);
      for I in reverse A'Range(1) loop
        Norm(A(I,J),N);
        AddProd(W(I),N,SJ);
      end loop;
      Inv(W(J),N);
      Mult(N,SJ);
      Max(SJ,S);
    end loop;
  end WNorm1;

  procedure WNorm1(W: in RadVec; A: in Matrix; S: in out Scalar) is
    --- assumes Flt ops are rounded up
    N,T,SJ: Scalar;
  begin
    SetZero(S);
    for J in reverse A'Range(2) loop
      SetZero(SJ);
      for I in reverse A'Range(1) loop
        Norm(A(I,J),N);
        Prod(W(I),N,T);
        Add(T,SJ);
      end loop;
      Prod(One/W(J),SJ,T);
      Max(T,S);
    end loop;
  end WNorm1;

  function WNorm1(W: RadVec; A: Matrix) return Scalar is
    S: Scalar;
  begin
    WNorm1(W,A,S);
    return S;
  end WNorm1;

  procedure WBallAt0(W: in RadVec; R: in Flt; A: in out Matrix) is
    --- assumes Flt ops are rounded up
    Fac: Flt;
  begin
    for I in A'Range(1) loop
      Fac := R/W(I);
      for J in A'Range(2) loop
        BallAt0(Fac*W(J),A(I,J));
      end loop;
    end loop;
  end WBallAt0;

  function NormI(V: Vector) return Scalar is
    S,N: Scalar;
  begin
    SetZero(N);
    for I in reverse V'Range loop
      Norm(V(I),S);
      Max(S,N);
    end loop;
    return N;
  end NormI;

  function Norm2(V: Vector) return Scalar is
    S,N,Tmp: Scalar;
  begin
    SetZero(N);
    for I in reverse V'Range loop
      Adjoint(V(I),S);
      AddProd(S,V(I),N,Tmp);
    end loop;
    return Sqrt(N);
  end Norm2;

  function MaxNorm2(A: Matrix; Pow4: Positive := 4) return Radius is
    S: Scalar;
    A1,A2: Matrix(A'Range(1),A'Range(2));
  begin
    Copy(A,A1);
    MaxEigen(A1,A2,S,Pow4);
    return Sup(S);
  end MaxNorm2;

  function CheckNorm2(A: Matrix; R: Radius; Pow4: Positive := 4) return Boolean is
    use Flt_EF;
    A1,A2: Matrix(A'Range(1),A'Range(2));
    S: Flt := R;
  begin
    Copy(A,A1);
    for K in 1 .. Pow4 loop
      Prod(A1,A1,A2);
      S := S*(-S);
      Prod(A2,A2,A1);
      S := S*(-S);
      if CheckNorm1(A1,-S) then return True; end if;
    end loop;
    if Verbosity>1 then
      S := MaxNorm1(A);
      for K in 1 .. Pow4 loop S := Sqrt(Sqrt(S)); end loop;
      Show1("rough radius bound:",S);
    end if;
    return False;
  end CheckNorm2;

  ----------------------- misc

  procedure Center(V: in out Vector) is
  begin
    if Not_STrunc then
      for N in V'Range loop Center(V(N)); end loop;
    end if;
  end Center;

  procedure Center(A: in out Matrix) is
  begin
    if Not_STrunc then
      for I in A'Range(1) loop
        for J in A'Range(2) loop Center(A(I,J)); end loop;
      end loop;
    end if;
  end Center;

  procedure Dot(V1,V2: in Vector; S: in out Scalar) is
    Tmp: Scalar;
  begin
    SetZero(S);
    for I in IMax(V1'First,V2'First) .. IMin(V1'Last,V2'Last) loop
      AddProd(V1(I),V2(I),S,Tmp);
    end loop;
  end Dot;

  function Dot(V1,V2: Vector) return Scalar is
    S: Scalar;
  begin
    SetZero(S);
    Dot(V1,V2,S);
    return S;
  end Dot;

  function "*"(V1,V2: Vector) return Scalar is
    A,S,Tmp: Scalar;
  begin
    SetZero(S);
    for I in IMax(V1'First,V2'First) .. IMin(V1'Last,V2'Last) loop
      Adjoint(V1(I),A);
      AddProd(A,V2(I),S,Tmp);
    end loop;
    return S;
  end "*";

  function "*"(S: Scalar; U: FltVec) return Vector is
    V: Vector(U'Range);
  begin
    for I in U'Range loop
      V(I) := U(I)*S;
    end loop;
    return V;
  end "*";

  procedure Truncate(Eps: in Radius; V: in out Vector) is
  begin
    for I in V'Range loop
      if MaxNorm(V(I))<Eps then SetZero(V(I)); end if;
    end loop;
  end Truncate;

  procedure Truncate(Eps: in Radius; A: in out Matrix) is
  begin
    for I in A'Range(1) loop
      for J in A'Range(2) loop
        if MaxNorm(A(I,J))<Eps then SetZero(A(I,J)); end if;
      end loop;
    end loop;
  end Truncate;

  procedure SetRow(I: in Integer; V: in Vector; A: in out Matrix) is
  begin
    if CheckRange and then not ((A'First(2)=V'First) and (A'Last(2)=V'Last)) then
      raise Dimension_Error with "ScalVectors.SetRow error";
    end if;
    for J in V'Range loop
      Copy(V(J),A(I,J));
    end loop;
  end SetRow;

  procedure SetColumn(J: in Integer; V: in Vector; A: in out Matrix) is
  begin
    if CheckRange and then not ((A'First(1)=V'First) and (A'Last(1)=V'Last)) then
      raise Dimension_Error with "ScalVectors.SetColumn error";
    end if;
    for I in V'Range loop
      Copy(V(I),A(I,J));
    end loop;
  end SetColumn;

  procedure GetRow(I: in Integer; A: in Matrix; V: in out Vector) is
  begin
    if CheckRange and then not ((A'First(2)=V'First) and (A'Last(2)=V'Last)) then
      raise Dimension_Error with "ScalVectors.GetRow error";
    end if;
    for J in V'Range loop
      Copy(A(I,J),V(J));
    end loop;
  end GetRow;

  function GetRow(I: Integer; A: Matrix) return Vector is
    V: Vector(A'Range(2));
  begin
    for J in V'Range loop
      Copy(A(I,J),V(J));
    end loop;
    return V;
  end GetRow;

  procedure GetColumn(J: in Integer; A: in Matrix; V: in out Vector) is
  begin
    if CheckRange and then not ((A'First(1)=V'First) and (A'Last(1)=V'Last)) then
      raise Dimension_Error with "ScalVectors.GetColumn error";
    end if;
    for I in V'Range loop
      Copy(A(I,J),V(I));
    end loop;
  end GetColumn;

  function GetColumn(J: Integer; A: Matrix) return Vector is
    V: Vector(A'Range(1));
  begin
    for I in V'Range loop
      Copy(A(I,J),V(I));
    end loop;
    return V;
  end GetColumn;

  procedure Permute(P: in Permutation; V,Tmp: in out Vector) is
    --- assuming all Ranges are the same
  begin
    for I in V'Range loop Copy(V(I),Tmp(I)); end loop;
    for I in V'Range loop Copy(Tmp(P(I)),V(I)); end loop;
  end Permute;

  procedure Row_Permute(P: in Permutation; A: in out Matrix; Tmp: in out Vector) is
    --- assuming all relevant Ranges are the same
  begin
    for J in A'Range(2) loop --- permutation in column J
      for I in A'Range(1) loop Copy(A(I,J),Tmp(I)); end loop;
      for I in A'Range(1) loop Copy(Tmp(P(I)),A(I,J)); end loop;
    end loop;
  end Row_Permute;

  procedure Column_Permute(P: in Permutation; A: in out Matrix; Tmp: in out Vector) is
    --- assuming all relevant Ranges are the same
  begin
    for I in A'Range(1) loop --- permutation in row I
      for J in A'Range(2) loop Copy(A(I,J),Tmp(J)); end loop;
      for J in A'Range(2) loop Copy(Tmp(P(J)),A(I,J)); end loop;
    end loop;
  end Column_Permute;

  procedure Symmetrize(A: in out Matrix) is
    S: Scalar;
  begin
    if CheckRange and then not IsSquare(A) then
      raise Dimension_Error with "ScalVectors.Symmetrize error";
    end if;
    for I in A'Range(1) loop
      for J in A'First(2) .. I-1 loop
        Adjoint(A(J,I),S);
        Add(S,A(I,J));
        Mult(Half_Scalar,A(I,J));
        Adjoint(A(I,J),S);
        Add(S,A(J,I));
        Mult(Half_Scalar,A(J,I));
      end loop;
      Real_Part(A(I,I));
    end loop;
  end Symmetrize;

  function AntiSymm(A: in Matrix) return Matrix is
    S: Scalar;
    B: Matrix(A'Range(1),A'Range(2));
  begin
    if CheckRange and then not IsSquare(A) then
      raise Dimension_Error with "ScalVectors.AntiSymm error";
    end if;
    for I in A'Range(1) loop
      for J in A'Range(2) loop
        Adjoint(A(J,I),S);
        Diff(A(I,J),S,B(I,J));
        Mult(Half_Scalar,B(I,J));
      end loop;
      Imag_Part(B(I,I));
    end loop;
    return B;
  end AntiSymm;

  ----------------------------

  procedure Orthogonalize(A: in out Matrix) is  -- not checked for Complex A
    subtype Column is Vector(A'Range(1));
    VJ: Column;
    F: Vector(A'Range(2));
    U: array(A'Range(2)) of Column;
  begin
    SetZero(F(F'First)); -- just to avoid a compiler warning
    for J in F'Range loop
      GetColumn(J,A,VJ);
      Copy(VJ,U(J));
      for K in F'First .. J-1 loop
        AddProd(-(VJ*U(K))*F(K),U(K),U(J));
      end loop;
      Inv(U(J)*U(J),F(J));
    end loop;
    for J in F'Range loop
      Mult(Sqrt(F(J)),U(J));
      SetColumn(J,U(J),A);
    end loop;
  end Orthogonalize;

  procedure Dgefa(A: in out Matrix; Pvt: out Pivot) is
    --- adapted from linpack routine dgefa, http://www.netlib.org/linpack/
    --- factors a matrix by gaussian elimination
    First: constant Integer := Pvt'First;
    Last: constant Integer := Pvt'Last;
    L: Integer;
    X,Y: Radius;
    T,Tmp: Scalar;
  begin
    if CheckRange and then not BothFirst(A,First) then
      raise Dimension_Error with "ScalVectors.Dgefa error";
    end if;
    for K in First .. Last-1 loop
      Y := abs(Approx(A(Last,K)));
      L := Last;
      for M in reverse K .. Last-1 loop
        X := abs(Approx(A(M,K)));
        if X >= Y then Y := X; L := M; end if;
      end loop;
      Pvt(K) := L;
      if not IsZero(A(L,K)) then
        if L /= K then
          Swap(A(L,K),A(K,K));
        end if;
        Inv(A(K,K),T);
        Neg(T);
        for M in K+1 .. Last loop
          Mult(T,A(M,K));
        end loop;
        for J in K+1 .. Last loop
          Copy(A(L,J),T);
          if L /= K then
            Swap(A(L,J),A(K,J));
          end if;
          for M in K+1 .. Last loop
            AddProd(T,A(M,K),A(M,J),Tmp);
          end loop;
        end loop;
      end if;
    end loop;
    Pvt(Last) := Last;
  end Dgefa;

  procedure Dgesl(Pvt: in Pivot; A: in Matrix; B: in out Vector) is
    --- adapted from linpack routine dgesl, http://www.netlib.org/linpack/
    --- solves the system AX=B
    First: constant Integer := Pvt'First;
    Last: constant Integer := Pvt'Last;
    L: Integer;
    T,Tmp: Scalar;
  begin
    if CheckRange and then not BothFirst(A,First) then
      raise Dimension_Error with "ScalVectors.Dgesl error";
    end if;
    for K in First .. Last-1 loop
      L := Pvt(K);
      Copy(B(L),T);
      if L /= K then
        Swap(B(L),B(K));
      end if;
      for M in K+1 .. Last loop
        AddProd(T,A(M,K),B(M),Tmp);
      end loop;
    end loop;
    for K in reverse First .. Last loop
      Inv(A(K,K),T);
      Mult(T,B(K));
      Neg(B(K),T);
      for M in First .. K-1 loop
        AddProd(T,A(M,K),B(M),Tmp);
      end loop;
    end loop;
  end Dgesl;

  procedure Dgedi(Pvt: in Pivot; A: in out Matrix; B: in out Vector) is
    --- adapted from linpack routine dgedi, http://www.netlib.org/linpack/
    --- computes the inverse of a matrix
    First: constant Integer := Pvt'First;
    Last: constant Integer := Pvt'Last;
    L: integer;
    T,Tmp: Scalar;
  begin
    if CheckRange and then not BothFirst(A,First) then
      raise Dimension_Error with "ScalVectors.Dgedi error";
    end if;
    for K in First .. Last loop
      Inv(A(K,K),T);
      Copy(T,A(K,K));
      Neg(T);
      for M in First .. K-1 loop
        Mult(T,A(M,K));
      end loop;
      for J in K+1 .. Last loop
        Copy(A(K,J),T);
        SetZero(A(K,J));
        for M in First .. K loop
          AddProd(T,A(M,K),A(M,J),Tmp);
        end loop;
      end loop;
    end loop;
    for K in reverse First .. Last-1 loop
      for I in K+1 .. Last loop
        Copy(A(I,K),B(I));
        SetZero(A(I,K));
      end loop;
      for J in K+1 .. Last loop
        for M in First .. Last loop
          AddProd(B(J),A(M,J),A(M,K),Tmp);
        end loop;
      end loop;
      L := Pvt(K);
      if L /= K  then
        for M in First .. Last loop
          Swap(A(M,K),A(M,L));
        end loop;
      end if;
    end loop;
  end Dgedi;

  function Determinant(A: Matrix) return Scalar is
    S: Scalar := Scal(One);
    Pvt: Pivot(A'Range(1));
    B: Matrix(A'Range(1),A'Range(2)) := A;
  begin
    Dgefa(B,Pvt);
    for J in A'Range(1) loop
      if Pvt(J) /= J then Neg(S); end if;
      Mult(B(J,J),S);
    end loop;
    return S;
  end Determinant;

  procedure Solve(B: in Vector; A: in Matrix; X: in out Vector) is
    Pvt: Pivot(A'Range(1));
    Ar: Matrix(A'Range(1),A'Range(2)) := A;
  begin
    Copy(B,X);
    Dgefa(Ar,Pvt);
    Dgesl(Pvt,Ar,X);
  end Solve;

  procedure Solve(A: in out Matrix; X: in out Vector) is
    Pvt: Pivot(A'Range(1));
  begin
    Dgefa(A,Pvt);
    Dgesl(Pvt,A,X);
  end Solve;

  procedure Old_Invert(A: in out Matrix) is
    Pvt: Pivot(A'Range(2));
    B: Vector(A'Range(2));
  begin
    Dgefa(A,Pvt);
    Dgedi(Pvt,A,B);
  end Old_Invert;

  procedure InvNewtonStep(A: in Matrix; Ai,B1,B2: in out Matrix; Err: out Radius) is
    --- B1 and B2 are just work space
  begin
    Err := Zero;
    Neg(Ai,B1);
    Prod(A,Ai,B2);
    for I in A'Range(1) loop
      Sub(One_Scalar,B2(I,I));
      Err := Err+MaxNorm(B2(I,I)); --- just error on the diagonal
      Sub(One_Scalar,B2(I,I));
    end loop;
    Prod(B1,B2,Ai);
  end InvNewtonStep;

  procedure InvNewton(A: in Matrix; Ai: in out Matrix; Iter: in Positive) is
    Err: Flt;
    B1,B2: Matrix(A'Range(1),A'Range(2));
  begin
    for I in 1 .. Iter loop
      InvNewtonStep(A,Ai,B1,B2,Err);
      if Verbosity>3 then Show1("ScalVectors.InvNewton: Err =",Err); end if;
    end loop;
    if Verbosity>2 then Show1("ScalVectors.InvNewton: Err =",Err); end if;
  end InvNewton;

  procedure InvNewton(A: in out Matrix; Iter: in Positive) is
    B: constant Matrix := A;
  begin
    Invert(A);
    InvNewton(B,A,Iter);
  end InvNewton;

  ------------------------------- QR stuff

  procedure HHLeftMult(U,V: in Vector; A: in out Matrix; Tmp: in out Vector) is
    --- normally V is the adjoint of U
    S: Scalar;
  begin
    TransProd(A,V,Tmp);
    Assign(2,S);
    Mult(S,Tmp);
    for J in A'Range(2) loop
      for I in A'Range(1) loop
        Prod(U(I),Tmp(J),S);
        Sub(S,A(I,J));
      end loop;
    end loop;
  end HHLeftMult;

  procedure HHRightMult(U,V: in Vector; A: in out Matrix; Tmp: in out Vector) is
    --- normally V is the adjoint of U
    S: Scalar;
  begin
    Prod(A,U,Tmp);
    Assign(2,S);
    Mult(S,Tmp);
    for I in A'Range(1) loop
      for J in A'Range(2) loop
        Prod(Tmp(I),V(J),S);
        Sub(S,A(I,J));
      end loop;
    end loop;
  end HHRightMult;

  procedure QR(A,Q: in out Matrix) is
    --- A is transformed to upper triangular via Householder reflections
    M: constant Natural := A'Last(1);
    N: constant Natural := A'Last(2);
    NonZero: Boolean;
    U,V,Tmp: Vector(1 .. M);
  begin
    if CheckRange then
      if not ((A'First(1)=1) and (A'First(2)=1)) then
        raise Dimension_Error with "ScalVectors.QR error";
      end if;
      CheckDim(M,Q);
    end if;
    Identity(Q);
    for J in 1 .. IMin(M,N-1) loop
      for I in J+1 .. M loop
        NonZero := not IsZero(A(I,J));
        exit when NonZero;
      end loop;
      if NonZero then
        for I in J .. M loop
          Copy(A(I,J),U(I));
        end loop;
        if Inf(U(J)) >= Zero then
          Add(Norm2(U),U(J));
        else
          Sub(Norm2(U),U(J));
        end if;
        Mult(Inv(Norm2(U)),U);
        Adjoint(U,V);
        HHLeftMult(U,V,A,Tmp);
        HHRightMult(U,V,Q,Tmp);
        for I in J+1 .. M loop
          SetZero(A(I,J));
        end loop;
      end if;
      SetZero(U(J));
    end loop;
  end QR;

  procedure Francis_Upper(A,Q: in out Matrix; Symm: in Boolean := False) is
    M: constant Natural := A'Last(1);
    NonZero: Boolean;
    U,V,Tmp: Vector(1 .. M);
  begin
    if CheckRange then
      CheckDim(M,A);
      CheckDim(M,Q);
    end if;
    Identity(Q);
    for J in 1 .. M-2 loop
      SetZero(U(J));
      for I in J+2 .. M loop
        NonZero := not IsZero(A(I,J));
        exit when NonZero;
      end loop;
      if NonZero then
        for I in J+1 .. M loop
          Copy(A(I,J),U(I));
        end loop;
        if Inf(U(J+1)) >= Zero then
          Add(Norm2(U),U(J+1));
        else
          Sub(Norm2(U),U(J+1));
        end if;
        Mult(Inv(Norm2(U)),U);
        Adjoint(U,V);
        HHLeftMult(U,V,A,Tmp);
        HHRightMult(U,V,A,Tmp);
        HHRightMult(U,V,Q,Tmp);
        for I in J+2 .. M loop SetZero(A(I,J)); end loop;
      end if;
      if Symm then
        for I in J+2 .. M loop SetZero(A(J,I)); end loop;
      end if;
    end loop;
  end Francis_Upper;

  procedure Francis_Lower(A,Q: in out Matrix; Symm: in Boolean := False) is
  begin
    if Symm then
      Francis_Upper(A,Q,True);
    else
      declare
        B: Matrix(A'Range(2),A'Range(1));
      begin
        Adjoint(A,B);
        Francis_Upper(B,Q,False);
        Adjoint(B,A);
      end;
    end if;
  end Francis_Lower;

  procedure DeterminantTri(A: in Matrix; D: in out Scalar) is
    M: constant Natural := A'Last(1);
    D1,D2: Scalar;
  begin
    if CheckRange then
      CheckDim(M,A);
    end if;
    Copy(A(1,1),D1);
    Assign(1,D2);
    for J in 2 .. M loop
      Prod(A(J-1,J),D2,D);
      Prod(A(J,J-1),D,D2);
      Prod(A(J,J),D1,D);
      Sub(D2,D);         -- D := A(J,J)*D1-A(J,J-1)*A(J-1,J)*D2
      Copy(D1,D2);
      Copy(D,D1);
    end loop;
  end DeterminantTri;

  function DeterminantTri(A: Matrix) return Scalar is
    D: Scalar;
  begin
    DeterminantTri(A,D);
    return D;
  end DeterminantTri;

  --------------------------- spectral etc

  procedure LDLdecomp(A: in Matrix; L: in out Matrix; D: in out Diag) is
    First: constant Integer := A'First(1);
    Last: constant Integer := A'Last(1);
    Tmp1,Tmp2,S: Scalar;
  begin
    if CheckRange and then not IsSquare(A) then
      raise Dimension_Error with "ScalVectors.LDLdecomp error";
    end if;
    Assign(1,L);
    for J in First .. Last loop
      Copy(A(J,J),D(J));
      for K in First .. J-1 loop
        Adjoint(L(J,K),Tmp2);
        Prod(L(J,K),Tmp2,Tmp1);
        Prod(Tmp1,D(K),Tmp2);
        Sub(Tmp2,D(J));
      end loop;
      for I in J+1 .. Last loop
        Copy(A(I,J),S);
        for K in First .. J-1 loop
          Adjoint(L(J,K),Tmp2);
          Prod(L(I,K),Tmp2,Tmp1);
          Prod(Tmp1,D(K),Tmp2);
          Sub(Tmp2,S);
        end loop;
        Quot(S,D(J),L(I,J));
      end loop;
    end loop;
  end LDLdecomp;

  procedure LDLTransposed(D: in Diag; L: in Matrix ; A: in out Matrix) is
  begin
    A := L;
    Transpose(A);
    Mult(D,A);
    Mult(L,A);
  end LDLTransposed;

  function IsPositive(A: Matrix) return Logical is
    --- checks for strict positivity
    D: Diag(A'Range(1));
    L: Matrix(A'Range(1),A'Range(2));
  begin
    LDLdecomp(A,L,D);
    for I in D'Range loop
      if Sup(D(I)) <= Zero then return False; end if;
    end loop;
    for I in D'Range loop
      if Inf(D(I)) <= Zero then return Uncertain; end if;
    end loop;
    return True;
  exception
    when others => return Uncertain;
  end IsPositive;

  function MaxEigen(A: Matrix; Pow4: Positive := 4) return Scalar is
    S: Scalar;
    A1,A2: Matrix(A'Range(1),A'Range(2));
  begin
    Copy(A,A1);
    MaxEigen(A1,A2,S,Pow4);
    return S;
  end MaxEigen;

  procedure MaxEigen(A,Tmp: in out Matrix; S: in out Scalar; Pow4: in Positive := 4) is
    --- A and Tmp get modified
    Fac: Flt := One;
  begin
    for K in 1 .. Pow4 loop
      Prod(A,A,Tmp);
      Prod(Tmp,Tmp,A);
      Fac := Quarter*Fac;
    end loop;
    S := Exp(Fac*Log(Scal(MaxNorm1(A))));
  end MaxEigen;

  function MinEigen(A: Matrix; Pow4: Positive := 4) return Scalar is
    Fac: Flt := NegOne;
    A1,A2: Matrix(A'Range(1),A'Range(2));
  begin
    Copy(A,A1);
    Invert(A1);
    for K in 1 .. Pow4 loop
      Prod(A1,A1,A2);
      Prod(A2,A2,A1);
      Fac := Quarter*Fac;
    end loop;
    return Exp(Fac*Log(Norm1(A1)));
  end MinEigen;

  procedure CheckSpecGap(A,Tmp: in out Matrix; Need: in Radius; Gap: out Radius) is
    Pow2: constant Positive := 8;
    Factor: Flt := NegOne;
  begin
    Invert(A);
    for K in 1 .. Pow2 loop
      Prod(A,A,Tmp);
      Factor := Half*Factor;
      Gap := Inf(Exp(Factor*Log(Norm1(Tmp))));
      if Gap>Need then return; end if;
      Swap(A,Tmp);
    end loop;
  end CheckSpecGap;

  --------------------- testing and info

  procedure RandomVec(V: in out Vector; Fac: in Flt := One) is
  begin
    for I in V'Range loop
      Assign(Fac*Simple_Random,V(I));
    end loop;
  end RandomVec;

  procedure RandomMat(A: in out Matrix; Fac: in Flt := One) is
  begin
    for I in A'Range(1) loop
      for J in A'Range(2) loop
        Assign(Fac*Simple_Random,A(I,J));
      end loop;
    end loop;
  end RandomMat;

  procedure RandomOrth(A: in out Matrix) is
  begin
    RandomMat(A,One);
    Orthogonalize(A);
    if Verbosity>4 then CheckOrth(A); end if;
  end RandomOrth;

  procedure RandomSymm(W: in out Vector; A: in out Matrix) is
    Q: Matrix(A'Range(1),A'Range(2));
  begin
    RandomVec(W);
    Default_Sort(W);
    RandomOrth(A);
    Adjoint(A,Q);
    Mult(Diag(W),A);
    Mult(Q,A);
    if Verbosity>4 then CheckSymm(A); end if;
    Symmetrize(A);
  end RandomSymm;

  procedure RandomCircle(H: in out Vector; A: in out Matrix) is
    --- the cosines H are equidistributed, not the angles
    N: constant Positive := A'Last(1);
    Nh: constant Positive := N/2;
    K1,K2: Integer;
    C,S,Tmp: Scalar;
    B,Q: Matrix(1..N,1..N);
  begin
    if (N mod 2)=1 then raise Not_Implemented; end if;
    CheckDim(N,A);
    SetZero(A);
    for K in 1 .. Nh loop
      Assign(Simple_Random,C);
      Prod(C,C,S);
      Diff(One_Scalar,S,Tmp);
      Sqrt(Tmp,S);
      Copy(C,H(K));
      K2 := 2*K;
      K1 := 2*K-1;
      Copy(C,A(K1,K1));
      Copy(C,A(K2,K2));
      Copy(S,A(K2,K1));
      Neg( S,A(K1,K2));
    end loop;
    RandomOrth(Q);
    Prod(A,Q,B);
    Adjoint(Q);
    Prod(Q,B,A);
  end RandomCircle;

  function SymmErr(A: in Matrix) return Radius is
    --- meant to be faster than CheckSymm
    First: constant Integer := A'First(1);
    Last: constant Integer := A'Last(1);
    R: Radius := Zero;
    S: Scalar;
  begin
    if A'First(2)/=First or A'Last(2)/=Last then raise Undefined; end if;
    for I in First .. Last loop
      for J in First .. I-1 loop
        Adjoint(A(I,J),S);
        Sub(A(J,I),S);
        R := RMax(MaxNorm(S),R);
      end loop;
    end loop;
    return R;
  end SymmErr;

  procedure Show_Approx(N: in String; A: in Matrix) is
    W: constant Positive := 4;
    AllZero: Boolean := True;
    R: Flt;
  begin
    for I in A'Range(1) loop
      for J in A'Range(2) loop
        R := Approx(A(I,J));
        if R/=Zero then
          Show1(N & Strng(I,W) & Strng(J,W) & " approx ",R);
          AllZero := False;
        end if;
      end loop;
    end loop;
    if AllZero then Show0(N & "0"); end if;
  end Show_Approx;

  procedure CheckSymm(A: in Matrix; ShowMat: Boolean := False) is
    E: Scalar;
    C: Matrix(A'Range(1),A'Range(2));
  begin
    if not IsSquare(A) then raise Undefined; end if;
    Adjoint(A,C);
    Sub(A,C);
    Norm1(C,E);
    Show1("|A-A^t| =",MaxNorm(E));
    if ShowMat then Show_Approx("A-A^* ",C); end if;
  end CheckSymm;

  procedure CheckInv(A,Ai: in Matrix; ShowMat: Boolean := False) is
    E: Scalar;
    C: Matrix(A'Range(1),A'Range(2));
  begin
    Prod(A,Ai,C);
    AddConst(-1,C);
    Norm1(C,E);
    Show1("|A*Ai-Id| =",MaxNorm(E));
    if ShowMat then Show_Approx("A*Ai-Id ",C); end if;
  end CheckInv;

  procedure CheckOrth(A: in Matrix; ShowMat: Boolean := False) is
    E: Scalar;
    B,C: Matrix(A'Range(1),A'Range(2));
  begin
    Adjoint(A,B);
    Prod(A,B,C);
    AddConst(-1,C);
    Norm1(C,E);
    Show1("|A*A^*| =",MaxNorm(E));
    if ShowMat then Show_Approx("A*A^*-Id ",C); end if;
  end CheckOrth;

  ---------------------- misc

  procedure Col_Normalize1(Vr,Vi: in out Matrix) is
    N: Flt;
  begin
    for J in Vr'Range(2) loop
      N := Zero;
      for I in Vr'Range(1) loop
        N := N+MaxNorm(Vr(I,J))+MaxNorm(Vi(I,J));
      end loop;
      N := Two/N;
      for I in Vr'Range(1) loop
        Mult(N,Vr(I,J));
        Mult(N,Vi(I,J));
      end loop;
    end loop;
  end Col_Normalize1;

  procedure Normalize1(V: in out Vector; N: out Radius) is
    S: Scalar;
  begin
    N := Zero;
    for I in V'Range loop N := N+MaxNorm(V(I)); end loop;
    Assign(One/N,S);
    for I in V'Range loop Mult(S,V(I)); end loop;
  end Normalize1;

  procedure NumSpecGap(A: in out Matrix; Eps: in Radius; R,Err: out Radius) is
    --- backwards iteration
    Iter: constant Positive := 128;
    R0: Radius;
    Pvt: Pivot(A'Range(1));
    V: Vector(A'Range(1));
  begin
    Dgefa(A,Pvt);
    RandomVec(V);
    Normalize1(V,R);
    for I in 1 .. Iter loop
      R0 := R;
      Center(V);
      Dgesl(Pvt,A,V);
      Normalize1(V,R);
      Err := abs(R/R0-One);
      if Err<Eps then return; end if;
    end loop;
    Show1("NumSpecGap: getting tired. Err =",Err);
  exception
    when others => R := Zero;
  end NumSpecGap;

  function NumSpecGap(A: Matrix; Eps: Radius) return Radius is
    --- backwards iteration
    R,Err: Radius;
    Ar: Matrix(A'Range(1),A'Range(2)) := A;
  begin
    NumSpecGap(Ar,Eps,R,Err);
    return R;
  end NumSpecGap;

end ScalVectors;
