with Globals, Ints, EisPack1, EisPack2;
use Globals, Ints;

pragma Elaborate_All (EisPack1,EisPack2);

package body ScalVectors.RNum is

  package Eis1 is new EisPack1(Real => Scalar, Vector => Vector, Matrix => Matrix);
  package Eis2 is new EisPack2(Flt => Flt, Real => Scalar, Vector => Vector, Matrix => Matrix, Pivot => Pivot);
  use Eis1, Eis2;

  function Epsilon(R: Flt) return Flt is
  begin
    return Epsilon(R,Get_Precision(One_Scalar*One_Scalar));
  end Epsilon;

  function CountLarge(B: Vector; Cut: Scalar) return Integer is
    N: Integer := 0;
  begin
    for I in B'Range loop
      if (B(I)>Cut) then N := N+1; end if;
    end loop;
    return N;
  end CountLarge;

  procedure SortEigen(W: in out Vector; V: in out Matrix) is
    X,Y: Integer;
    A: Pivot(W'Range);
    U: Vector(W'Range);
---
    function GT(I,J: Integer) return Boolean is
    begin
      return abs(W(I))>abs(W(J));
    end GT;
---
    procedure Qsort(L,R: Integer) is
      I,J: integer;
    begin
      I:=L; J:=R; X:=A((L+R)/2);
      loop
        while GT(A(I),X) loop I:=I+1; end loop;
        while GT(X,A(J)) loop J:=J-1; end loop;
        if (I <= J) then
          Y:=A(I); A(I):=A(J); A(J):=Y;
          I:=I+1; J:=J-1;
        end if;
        exit when I>J;
      end loop;
      if (L<J) then Qsort(L,J); end if;
      if (I<R) then Qsort(I,R); end if;
    end Qsort;
---
  begin
    for J in W'Range loop A(J) := J; end loop;
    Qsort(W'First,W'Last);
    U := W;
    for J in W'Range loop W(J) := U(A(J)); end loop;
    for I in W'Range loop
      for J in W'Range loop U(J) := V(I,J); end loop;
      for J in W'Range loop V(I,J) := U(A(J)); end loop;
    end loop;
  end SortEigen;
---
  procedure Eigen(A: in Matrix; W: in out Vector) is
    pragma Suppress(Storage_Check);
    N: constant Integer := A'Last(1);
    Ierr: Integer;
    Fv1,Fv2: Vector(1 .. N);
    Ar: Matrix := A;
  begin
    CheckDim(N,A);
    CheckDim(N,W);
    Tred1(N,Ar,W,Fv1,Fv2);
    Tql1(N,W,Fv1,Ierr);
    if (Ierr>0) then
      Show1("Error: Problem with eigenvalue number ",Ierr);
    end if;
  end Eigen;

  procedure Eigen(A: in Matrix; W: in out Vector; V: in out Matrix) is
    pragma Suppress(Storage_Check);
    N: constant Integer := A'Last(1);
    Ar: constant Matrix := A;
    Ierr: Integer;
    Fv1: Vector(1 .. N);
  begin
    CheckDim(N,A);
    CheckDim(N,W);
    CheckDim(N,V);
    Tred2(N,Ar,W,Fv1,V);
    Tql2(N,W,Fv1,V,Ierr);
    if (Ierr>0) then
      Show1("Error: Problem with eigenvalue number ",Ierr);
    end if;
    SortEigen(W,V);
  end Eigen;

  procedure SortEigen(Eps: in Flt; Wr,Wi: in out Vector; Vr,Vi: in out Matrix) is
    Less1: constant Scalar := One_Scalar-Scal(Eps);
    X,Y: Integer;
    Ri,Rj: Scalar;
    A: Pivot(Wr'Range);
    U: Vector(Wr'Range);
---
    function GT(I,J: Integer) return Boolean is
    begin
      Ri := Sqr(Wr(I))+Sqr(Wi(I));
      Rj := Sqr(Wr(J))+Sqr(Wi(J));
      if (Less1*Ri > Rj) then return  True; end if;
      if (Less1*Rj > Ri) then return False; end if;
      if ((Less1*abs(Wr(I))) > abs(Wr(J))) then return  True; end if;
      if ((Less1*abs(Wr(J))) > abs(Wr(I))) then return False; end if;
      if (Wi(I) > Wi(J)) then return  True; end if;
      return False;
    end GT;
---
    procedure Qsort(L,R: Integer) is
      I,J: integer;
    begin
      I:=L; J:=R; X:=A((L+R)/2);
      loop
        while GT(A(I),X) loop I:=I+1; end loop;
        while GT(X,A(J)) loop J:=J-1; end loop;
        if (I <= J) then
          Y:=A(I); A(I):=A(J); A(J):=Y;
          I:=I+1; J:=J-1;
        end if;
        exit when (I>J);
      end loop;
      if (L<J) then Qsort(L,J); end if;
      if (I<R) then Qsort(I,R); end if;
    end Qsort;
---
  begin
    for J in Wr'Range loop A(J) := J; end loop;
    Qsort(Wr'First,Wr'Last);
    U := Wr;
    for J in Wr'Range loop Wr(J) := U(A(J)); end loop;
    U := Wi;
    for J in Wr'Range loop Wi(J) := U(A(J)); end loop;
    for I in Wr'Range loop
      for J in Wr'Range loop U(J) := Vr(I,J); end loop;
      for J in Wr'Range loop Vr(I,J) := U(A(J)); end loop;
      for J in Wr'Range loop U(J) := Vi(I,J); end loop;
      for J in Wr'Range loop Vi(I,J) := U(A(J)); end loop;
    end loop;
  end SortEigen;

  procedure FixEval(Wr,Wi: in out Vector; E: in out Scalar) is
--- assuming complex conjugate eigenvalues are next to each other
    J: Integer := Wi'First;
    E1,E2: Scalar;
  begin
    SetZero(E);
    while J < Wi'Last loop
      E1 := abs(Wi(J));
      E2 := Max(abs(Wr(J)-Wr(J+1)),abs(Wi(J)+Wi(J+1)));
      if (E1<E2) then
        SetZero(Wi(J));
        E := Max(E,E1/(abs(Wr(J))+E1));
        J := J+1;
      else
        Wr(J) := Half_Scalar*(Wr(J)+Wr(J+1));
        Wr(J+1) := Wr(J);
        Wi(J) := Half_Scalar*(Wi(J)-Wi(J+1));
        Wi(J+1) := -Wi(J);
        E := Max(E,E2/(abs(Wr(J))+E1));
        J := J+2;
      end if;
    end loop;
    if (J=Wi'Last) then
      E1 := abs(Wi(J));
      SetZero(Wi(J));
      E := Min(E,E1/(abs(Wr(J))+E1));
    end if;
  end FixEval;

  procedure FixEvec(Wi: in Vector; Vr,Vi: in out Matrix) is
--- assuming complex conjugate eigenvalues are next to each other
    J: Integer := Wi'First;
  begin
    while J <= Wi'Last loop
      if IsZero(Wi(J)) then
        for I in Wi'Range loop
          SetZero(Vi(I,J));
        end loop;
        J := J+1;
      else
        for I in Wi'Range loop
          Vr(I,J) := Half_Scalar*(Vr(I,J)+Vr(I,J+1));
          Vr(I,J+1) := Vr(I,J);
          Vi(I,J) := Half_Scalar*(Vi(I,J)-Vi(I,J+1));
          Vi(I,J+1) := -Vi(I,J);
        end loop;
        J := J+2;
      end if;
    end loop;
  end FixEvec;

  procedure NormalizeEvec(Vr,Vi: in out Matrix) is
    VF: constant Integer := Vr'First(1);
    VL: constant Integer := Vr'Last(1);
    K: Integer;
    S,E,C,Br,Bi: Scalar;
  begin
    for J in VF .. VL loop
      SetZero(S);
      SetZero(E);
      for I in VF .. VL loop
        C := Sqr(Vr(I,J))+Sqr(Vi(I,J));
        S := S+C;
        if (C>E) then E := C; K := I; end if;
      end loop;
      Br := Vr(K,J);
      Bi := Vi(K,J);
      S := Sqrt(S*(Sqr(Br)+Sqr(Bi)));
      Br := Br/S;
      Bi := Bi/S;
      for I in VF .. VL loop
        S := Vr(I,J);
        C := Vi(I,J);
        Vr(I,J) := Br*S+Bi*C;
        Vi(I,J) := Br*C-Bi*S;
      end loop;
      SetZero(Vi(K,J));
    end loop;
  end NormalizeEvec;

  procedure Eigen(Ar,Ai: in Matrix; Wr,Wi: in out Vector; Vr,Vi: in out Matrix) is
    pragma Suppress(Storage_Check);
    N: constant Integer := Ar'Last(1);
    Low,Igh,Ierr: Integer;
    Int: Pivot(1 .. N);
    Scale: Vector(1 .. N);
    Br: Matrix := Ar;
    Bi: Matrix := Ai;
  begin
    CheckDim(N,Ar); CheckDim(N,Ai);
    CheckDim(N,Wr); CheckDim(N,Wi);
    CheckDim(N,Vr); CheckDim(N,Vi);
    Cbal(N,Br,Bi,Low,Igh,Scale);
    Comhes(N,Low,Igh,Br,Bi,Int);
    Comlr2(N,Low,Igh,Int,Br,Bi,Wr,Wi,Vr,Vi,Ierr);
    if (Ierr>0) then
      Show1("Error: Problem with eigenvalue number ",Ierr);
    end if;
    Cbabk2(N,Low,Igh,N,Scale,Vr,Vi);
  end Eigen;

  procedure Eigen(A: in Matrix; Wr,Wi: in out Vector; Vr,Vi: in out Matrix) is
    Small: constant Flt := 1.0E-100;
    Tiny:  constant Flt := 1.0E-200;
    Eps: constant Flt := Epsilon(1.0E-8);
    Err: constant Flt := Epsilon(1.0E-10);
    E: Scalar;
    Ai: Matrix(A'Range(1),A'Range(2));
  begin
    SetZero(E);
    SetZero(Ai);
    Eigen(A,Ai,Wr,Wi,Vr,Vi);
    SortEigen(Eps,Wr,Wi,Vr,Vi);
    --FixEval(Wr,Wi,E);
    if (Sup(E)>Err) then
       Show1("ScalVectors.Num.Eigen warning: could not fix",E);
    end if;
    NormalizeEvec(Vr,Vi);
    --FixEvec(Wi,Vr,Vi);
    Truncate(Tiny,Wr);
    Truncate(Tiny,Wi);
    Truncate(Small,Vr);
    Truncate(Small,Vi);
  end Eigen;

  procedure QuasiEigen(A: in Matrix; Wr,Wi: in out Vector; Vr: in out Matrix) is
--- replace cc eigenvalues by their real part
    J: Integer := Wr'First;
    S: Scalar;
    Vi: Matrix(Vr'Range(1),Vr'Range(2));
  begin
    Eigen(A,Wr,Wi,Vr,Vi);
    while J <= Wi'Last loop
      if not IsZero(Wi(J)) then
        J := J+1;
        for I in Wi'Range loop
          Vr(I,J) := Vi(I,J);
        end loop;
      end if;
      J := J+1;
    end loop;
    for K in Wr'Range loop      -- normalize Vr
      SetZero(S);
      for I in Wr'Range loop
        S := S+abs(Vr(I,K));
      end loop;
      for I in Wr'Range loop
        Vr(I,K) := Vr(I,K)/S;
      end loop;
    end loop;
  end QuasiEigen;

  procedure ClosestEigen(A: in Matrix; Er,Ei: in out Scalar; Ur,Ui: in out Vector) is
    N: constant Integer := A'Last(1);
    J0: Integer := 0;
    D0: Scalar := Scal(1.0E+99);
    D: Scalar;
    Vr,Vi: Matrix(1 .. N,1 .. N);
  begin
    Eigen(A,Ur,Ui,Vr,Vi);
    for J in 1 .. N loop
      D := Sqr(Ur(J)-Er)+Sqr(Ui(J)-Ei);
      if (D<D0) then D0 := D; J0 := J; end if;
    end loop;
    Er := Ur(J0);
    Ei := Ui(J0);
    Ur := GetColumn(J0,Vr);
    Ui := GetColumn(J0,Vi);
  end ClosestEigen;

  procedure SetEigen(V: in Matrix; W: in Vector; A: in out Matrix) is
  begin
    A := V;
    Invert(A);
    for I in W'Range loop
      for J in W'Range loop
        Mult(W(I),A(I,J));
      end loop;
    end loop;
    Mult(V,A);
  end SetEigen;

  function SpecRadius(A: Matrix; Eps: Radius) return Scalar is
    Iter: constant Integer := 9999;
    InvPow: Flt := One;
    Err: Flt;
    LnFac,S,N,LastS: Scalar;
    B: Matrix := A;
    C: Matrix(A'Range(1),A'Range(2));
  begin
    SetZero(LnFac);
    SetZero(S);
    for I in 1 .. Iter loop
      Prod(B,B,C);
      Prod(C,C,B);
      Prod(B,B,C);
      Prod(C,C,B);
      InvPow := InvPow/Flt(16);
      LnFac := Flt(16)*LnFac;
      N := Norm1(B);
      if IsZero(N) then return N; end if;
      Mult(Inv(N),B);
      LnFac := LnFac+Log(N);
      LastS := S;
      S := Exp(InvPow*LnFac);
      Err := MaxNorm(S-LastS);
      if Err<Eps then return S; end if;
    end loop;
    Show1("ScalVectors.RNum.SpecRadius: getting tired. Err =",Err);
    return S;
  end SpecRadius;

  function NormSmallerOne(A: in Matrix) return Boolean is
  begin
    for K in A'Range(1) loop
      if MaxNorm(A(K,K)) >= One then return False; end if;
    end loop;
    return (MaxNorm(Norm1(A))<One);
  end NormSmallerOne;

  procedure SpecHalf(A,B,C,D: in out Matrix) is
--- iterate A -> f(A) where f(z)=z^2/[(1-z)^2+z^2]
--- f attracts Re(z)>1/2 to 1 and rest to 0
--- B,C,D are just workspace
    Eps: constant Flt := 1.0E-8;
    Err,LastErr: Flt := Flt'Last;
  begin
    Copy(A,B);
    loop
      AddConst(-Scal(1),B);
      if NormSmallerOne(A) then
        SetZero(A);
        Err := Zero;
      end if;
      if NormSmallerOne(B) then
        Identity(A);
        Err := Zero;
      end if;
      exit when Err=Zero;
      Prod(B,B,D);         -- (A-1)^2
      Prod(A,A,C);         -- A^2
      Add(C,D);           -- A^2+(A-1)^2
      InvNewton(D,2);      -- 1/[A^2+(A-1)^2]
      Prod(C,D,B);        -- A^2/[A^2+(A-1)^2]
      Diff(A,B,C);
      LastErr := Err;
      Err := MaxNorm(Norm1(C));
      exit when (Err >= LastErr) and then (Err <= Eps);
      Copy(B,A);
    end loop;
    if Verbosity>2 then
      Show1("ScalVectors.RNum.SpecHalf: Err =",Err);
    end if;
  end SpecHalf;

  procedure SpecProjPos(A,B,C,D: in out Matrix) is
--- B,C,D are just workspace
    Fac: constant Flt := Flt(63)/Flt(64);
    N: Scalar;
  begin
    Prod(A,A,C);
    Prod(C,C,B);
    Prod(B,B,C);
    Prod(C,C,B);
    N := Sqrt(Sqrt(Sqrt(Sqrt(Norm1(B))))); -- approx spectral radius
    Mult(Fac*Inv(N),A);     -- [-1,1]
    AddConst(One_Scalar,A); --  [0,2]
    Mult(Half_Scalar,A);    --  [0,1]
    SpecHalf(A,B,C,D);
  end SpecProjPos;

  procedure SpecProjPos(A: in out Matrix) is
    B,C,D: Matrix(A'Range(1),A'Range(2));
  begin
    SpecProjPos(A,B,C,D);
  end SpecProjPos;

  procedure SpecProjInterval(L,R: in Scalar; P: in out Matrix) is
    Q,B,C,D: Matrix(P'Range(1),P'Range(2));
  begin
    Neg(P,Q);
    AddConst(L,Q);
    SpecProjPos(Q,B,C,D);  --- spec<L
    AddConst(-R,P);
    SpecProjPos(P,B,C,D);  --- spec>R
    Add(Q,P);
    Neg(P);
    AddConst(One_Scalar,P);
  end SpecProjInterval;

end ScalVectors.RNum;
