with Globals, Strings, Integer_Sqrt;
use Globals, Strings;

package body Polynoms is

  --- Polynom1 --------------------------------------------------

  function EffDeg(P: Polynom1) return Natural is
  begin
    for D in reverse 1 .. P'Last loop
      if not IsZero(P(D)) then return D; end if;
    end loop;
    return 0;
  end EffDeg;

  function EffDeg0(P: Polynom1) return Boolean is
  begin
    for D in reverse 1 .. P'Last loop
      if not IsZero(P(D)) then return False; end if;
    end loop;
    return True;
  end EffDeg0;

  function IsEven(C: Polynom1) return Boolean is
    --- assuming C'First <= 1
    D: constant Integer := EffDeg(C);
    I: Integer := 1;
  begin
    while I <= D loop
      if not IsZero(C(I)) then return False; end if;
      I := I+2;
    end loop;
    return True;
  end IsEven;

  function IsOdd(C: Polynom1) return Boolean is
    --- assuming C'First = 0
    D: constant Integer := EffDeg(C);
    I: Integer := 0;
  begin
    while I <= D loop
      if not IsZero(C(I)) then return False; end if;
      I := I+2;
    end loop;
    return True;
  end IsOdd;

  procedure Copy(P1: in Polynom1; P2: in out Polynom1) is
    D: constant Natural := EffDeg(P1);
  begin
    for I in 0 .. D loop Copy(P1(I),P2(I)); end loop;
    for I in D+1 .. P2'Last loop SetZero(P2(I)); end loop;
  end Copy;

  procedure Mult(S: in Scalar; P: in out Polynom1) is
  begin
    for I in 0 .. EffDeg(P) loop Mult(S,P(I)); end loop;
  end Mult;

  procedure Prod(S: in Scalar; P1: in Polynom1; P2: in out Polynom1) is
    D: constant Natural := EffDeg(P1);
  begin
    for I in 0 .. D loop Prod(S,P1(I),P2(I)); end loop;
    for N in D+1 .. P2'Last loop SetZero(P2(N)); end loop;
  end Prod;

  procedure AddProd(S: in Scalar; P1: in Polynom1; P2: in out Polynom1) is
  begin
    if not IsZero(S) then
      for I in 0 .. EffDeg(P1) loop AddProd(S,P1(I),P2(I)); end loop;
    end if;
  end AddProd;

  procedure Evaluate(P: in Polynom1; S: in Scalar; C: in out Scalar; Deg: in Natural) is
  begin
    if Deg=0 or else IsZero(S) then
      Copy(P(0),C);
    else
      Copy(P(Deg),C);
      for I in reverse 0 .. Deg-1 loop
        Mult(S,C);
        Add(P(I),C);
      end loop;
    end if;
  end Evaluate;

  procedure Evaluate(P: in Polynom1; S: in Scalar; C: in out Scalar) is
    D: constant Natural := EffDeg(P);
  begin
    Copy(P(D),C);
    for I in reverse 0 .. D-1 loop
      Mult(S,C);
      Add(P(I),C);
    end loop;
  end Evaluate;

  function Evaluate(P: Polynom1; S: Scalar; Deg: in Natural) return Scalar is
    C: Scalar;
  begin
    Evaluate(P,S,C,Deg);
    return C;
  end Evaluate;

  function Evaluate(P: Polynom1; S: Scalar) return Scalar is
    C: Scalar;
  begin
    Evaluate(P,S,C);
    return C;
  end Evaluate;

  procedure Evaluate(P: in Polynom1; Sr,Si: in Scalar; Cr,Ci: in out Scalar) is
    --- P is assumed to be real
    D: constant Natural := EffDeg(P);
  begin
    SetZero(Ci);
    if IsZero(Si) then
      Evaluate(P,Sr,Cr,D);
    else
      Copy(P(D),Cr);
      declare
        Ctr,Cti: Scalar;
      begin
        for I in reverse 0 .. D-1 loop
          Copy(Cr,Ctr);
          Prod(Si,Ci,Cr);
          Neg(Cr);
          AddProd(Sr,Ctr,Cr); -- Cr := Sr*Cr-Si*Ci
          Copy(Ci,Cti);
          Prod(Si,Ctr,Ci);
          AddProd(Sr,Cti,Ci); -- Ci := Sr*Ci+Si*Cr
          Add(P(I),Cr);
        end loop;
      end;
    end if;
  end Evaluate;

  procedure EvalDer(P: in Polynom1; S: in Scalar; C: in out Scalar; Deg: in Natural) is
    Tmp: Scalar;
  begin
    Assign(Deg,Tmp);
    Prod(Tmp,P(Deg),C);
    for I in reverse 1 .. Deg-1 loop
      Mult(S,C);
      Assign(I,Tmp);
      AddProd(Tmp,P(I),C);
    end loop;
  end EvalDer;

  procedure EvalDer(P: in Polynom1; S: in Scalar; C: in out Scalar) is
    D: constant Natural := EffDeg(P);
    Tmp: Scalar;
  begin
    Assign(D,Tmp);
    Prod(Tmp,P(D),C);
    for I in reverse 1 .. D-1 loop
      Mult(S,C);
      Assign(I,Tmp);
      AddProd(Tmp,P(I),C);
    end loop;
  end EvalDer;

  procedure Scale(S: in Scalar; P: in out Polynom1) is
    D: constant Natural := EffDeg(P);
  begin
    if D>0 then
      Mult(S,P(1));
      if D>1 then
        declare
          SP: Scalar;
        begin
          Copy(S,SP);
          for I in 2 .. D loop
            Mult(S,SP);
            Mult(SP,P(I));
          end loop;
        end;
      end if;
    end if;
  end Scale;

  procedure Scale(P1: in Polynom1; S: in Scalar; P2: in out Polynom1) is
    D1: constant Natural := EffDeg(P1);
  begin
    Copy(P1(0),P2(0));
    if D1>0 then
      Prod(S,P1(1),P2(1));
      if D1>1 then
        declare
          SP: Scalar;
        begin
          Copy(S,SP);
          for I in 2 .. D1 loop
            Mult(S,SP);
            Prod(SP,P1(I),P2(I));
          end loop;
        end;
      end if;
    end if;
    for I in D1+1 .. P2'Last loop SetZero(P2(I)); end loop;
  end Scale;

  procedure Translate(P1: in Polynom1; S: in Scalar; P2,Tmp: in out Polynom1) is
    D1: constant Natural := EffDeg(P1);
    X: Polynom1 renames Tmp;
  begin
    Copy(P1,P2);
    Assign(1,X(0));
    for N in 1 .. D1 loop
      Assign(1,X(N));
      for K in reverse 1 .. N-1 loop  -- produce X(t)=(t+S)^N
        Mult(S,X(K));
        Add(X(K-1),X(K));
        AddProd(X(K),P1(N),P2(K));    -- and add P1(N)*X to P2
      end loop;
      Mult(S,X(0));
      AddProd(X(0),P1(N),P2(0));
    end loop;
  end Translate;

  procedure Mult1(S: in Scalar; P: in out Polynom1) is
    --- assuming P'First=0
    I: Integer := P'Last;
  begin
    while IsZero(P(I)) loop I := I-1; end loop;
    Neg(P(I),P(I+1));
    while I>0 loop
      Mult(S,P(I));
      Sub(P(I-1),P(I));
      I := I-1;
    end loop;
    Mult(S,P(0));
  end Mult1;

  procedure Prod1(S: in Scalar; P1: in Polynom1; P2: in out Polynom1) is
  begin
    Copy(P1,P2);
    Mult1(S,P2);
  end Prod1;

  procedure Prod(P1,P2: in Polynom1; P3: in out Polynom1) is
    D1: constant Natural := EffDeg(P1);
    D2: constant Natural := EffDeg(P2);
  begin
    SetZero(P3);
    for I in 0 .. D1 loop
      for J in 0 .. D2 loop
        AddProd(P1(I),P2(J),P3(I+J));
      end loop;
    end loop;
  end Prod;

  procedure LMultiply(W: in Scalar; P: in out Polynom1) is
    --- multiply with (x-W)
    Deg: constant Natural := EffDeg(P);
    B: Scalar;
  begin
    Neg(W,B);
    for I in reverse 1 .. Deg+1 loop -- P(I) := P(I-1)-W*P(I)
      Mult(B,P(I));
      Add(P(I-1),P(I));
    end loop;
    Mult(B,P(0));
  end LMultiply;

  procedure LMultiply(A,B: in Scalar; P: in out Polynom1) is
    --- multiply with (A*x+B)
    D: constant Natural := EffDeg(P);
  begin
    if IsZero(A) then
      for I in 0 .. D loop Mult(B,P(I)); end loop;
    else
      for I in reverse 1 .. D+1 loop -- P(I) := A*P(I-1)+B*P(I)
        Mult(B,P(I));
        AddProd(A,P(I-1),P(I));
      end loop;
      Mult(B,P(0));
    end if;
  end LMultiply;

  procedure QMultiply(U,V,C,D: in Scalar; P: in out Polynom1) is
    --- multiply with (x*x+U*x+V) and then add (C*x+D)
    Deg: constant Natural := EffDeg(P);
  begin
    for I in reverse 2 .. Deg+2 loop -- P(I) := P(I-2)+U*P(I-1)+V*P(I);
      Mult(V,P(I));
      AddProd(U,P(I-1),P(I));
      Add(P(I-2),P(I));
    end loop;
    Mult(V,P(1));
    AddProd(U,P(0),P(1));
    Mult(V,P(0));
    Add(D,P(0));
    Add(C,P(1));
  end QMultiply;

  procedure QProduct(U,V,C,D: in Scalar; Q: in Polynom1; P: in out Polynom1) is
  begin
    Copy(Q,P);
    QMultiply(U,V,C,D,P);
  end QProduct;

  procedure QMultiply(U,V: in Vector; A,B: in Scalar; P: in out Polynom1) is
    SZero: Scalar;
  begin
    for I in P'Range loop SetZero(P(I)); end loop;
    Assign(1,P(0));
    SetZero(SZero);
    for K in U'Range loop
      QMultiply(U(K),V(K),SZero,SZero,P);
    end loop;
    LMultiply(A,B,P);
  end QMultiply;

  procedure LDivision(P: in out Polynom1; W: in Scalar; R: in out Scalar) is
    D: constant Natural := EffDeg(P);
    Pt: Scalar renames P(D);
  begin
    if D=0 then
      Copy(W,R);
    else
      Swap(Pt,P(D-1));
      for I in reverse 1 .. D-1 loop  -- Q(I-1) := W*Q(I)+P(I)
        Swap(Pt,P(I-1));
        AddProd(W,P(I),P(I-1));
      end loop;
      Prod(W,P(0),R);
      Add(Pt,R);
      SetZero(Pt);
    end if;
  end LDivision;

  procedure LDivision(P: in Polynom1; W: in Scalar; Q: in out Polynom1; R: in out Scalar) is
  begin
    Copy(P,Q);
    LDivision(Q,W,R);
  end LDivision;

  procedure QDivision(P: in Polynom1; U,V: in Scalar; Q: in out Polynom1; C,D: in out Scalar) is
    S: constant Vector2 := (U,V);
    FS: Vector2;
  begin
    Bairstow0(P,EffDeg(P),S,Q,FS);
    Copy(FS(1),C);
    Copy(FS(2),D);
  end QDivision;

  procedure QDivision(P: in Polynom1; U,V: in Scalar; Q: in out Polynom1) is
    S: constant Vector2 := (U,V);
    FS: Vector2;
  begin
    Bairstow0(P,EffDeg(P),S,Q,FS);
  end QDivision;

  procedure Bairstow0(P: in Polynom1; Deg: in Integer; S: in Vector2; Q: in out Polynom1; FS: in out Vector2) is
    --- Bairstow's method (wikipedia notation)
    --- P(x)=(x*x+U*x+V)*Q(x)+(C*x+D)
    U: Scalar renames S(1);
    V: Scalar renames S(2);  -- x*x+U*x+V
    C: Scalar renames FS(1);
    D: Scalar renames FS(2); -- C*x+D
  begin
    if Deg<2 then
      for I in Q'Range loop SetZero(Q(I)); end loop;
      Copy(P(1),C);
      Copy(P(0),D);
    else
      for I in reverse Deg-1 .. Q'Last loop
        SetZero(Q(I));
      end loop;
      Copy(P(Deg),Q(Deg-2));
      if Deg>2 then
        Neg(P(Deg-1),Q(Deg-3));
        AddProd(U,Q(Deg-2),Q(Deg-3));
        Neg(Q(Deg-3));
        for I in reverse 0 .. Deg-4 loop
          Neg(P(I+2),Q(I));
          AddProd(V,Q(I+2),Q(I));
          AddProd(U,Q(I+1),Q(I));
          Neg(Q(I));
        end loop;
      end if;
      Neg(P(1),C);
      AddProd(U,Q(0),C);
      AddProd(V,Q(1),C);
      Neg(C);
      Neg(P(0),D);
      AddProd(V,Q(0),D);
      Neg(D);
    end if;
  end Bairstow0;

  procedure Bairstow1(Q: in Polynom1; Deg: in Integer; S: in Vector2; R: in out Polynom1; DFS: in out Matrix2) is
    --- derivative for Bairstow0
    T: Vector2;
    U: Scalar renames S(1);
    V: Scalar renames S(2);  -- x*x+U*x+V
    G: Scalar renames T(1);
    H: Scalar renames T(2);
  begin
    Bairstow0(Q,Deg,S,R,T);
    Prod(G,U,DFS(1,1));
    Sub(H,DFS(1,1));    --  GU-H
    Neg(G,DFS(1,2));    --         -G
    Prod(G,V,DFS(2,1)); --  GV
    Neg(H,DFS(2,2));    --         -H
  end Bairstow1;

  procedure Roots2Poly(W: in Vector; P: in out Polynom1) is
  begin
    for I in P'Range loop SetZero(P(I)); end loop;
    Assign(1,P(0));
    for I in W'Range loop LMultiply(W(I),P); end loop;
  end Roots2Poly;

  procedure Roots2Poly(Wr,Wi: in Vector; P: in out Polynom1) is
    --- assuming cc roots are next to each other
    I: Integer := 1;
    SZero,SNegTwo: Scalar;
    U,V,S: Scalar;
  begin
    SetZero(SZero);
    Assign(-2,SNegTwo);
    for J in P'Range loop SetZero(P(J)); end loop;
    Assign(1,P(0));
    while I <= Wr'Last loop
      if IsZero(Wi(I)) then
        LMultiply(Wr(I),P);
        I := I+1;
      else
        Diff(Wr(I),Wr(I+1),S);
        if not IsZero(S) then
          Show2("Wr ",Wr(I),Wr(I+1));
          raise Not_Implemented;
        end if;
        Sum(Wi(I),Wi(I+1),S);
        if not IsZero(S) then
          Show2("Wr ",Wi(I),Wi(I+1));
          raise Not_Implemented;
        end if;
        Prod(SNegTwo,Wr(I),U);
        Prod(Wr(I),Wr(I),V);
        AddProd(Wi(I),Wi(I),V);
        QMultiply(U,V,SZero,SZero,P);
        I := I+2;
      end if;
    end loop;
  end Roots2Poly;

  procedure Chebyshev(N: in Natural; P: in out Polynom1) is
    use SV;
  begin
    if N<2 then
      SetZero(P);
      Assign(1,P(N));
    else
      declare
        C: Cheby(0 .. N);
      begin
        SetZero(C);
        Assign(1,C(N));
        Clenshaw(C,P);
      end;
    end if;
  end Chebyshev;

  procedure Clenshaw(C: in Cheby; P: in out Polynom1) is
    --- convert a finite Cheby series to a polynomial
    use SV;
    D: constant Natural := EffLast(C);
  begin
    SetZero(P);
    if D<2 then
      for I in 0 .. D loop Copy(C(I),P(I)); end loop;
    else
      declare
        B2: Polynom1 renames P;
        B1: Polynom1(0 .. D);
        STwo: Scalar;
      begin
        Assign(2,STwo);
        SetZero(B1);
        Copy(C(D),B1(0));
        for K in reverse 1 .. D-1 loop
          for I in 1 .. D-K loop
            Neg(B2(I));
            AddProd(STwo,B1(I-1),B2(I));
          end loop;
          Neg(B2(0));
          Add(C(K),B2(0));
          for I in 0 .. D-K loop Swap(B1(I),B2(I)); end loop;
        end loop;
        for I in 1 .. D loop
          Neg(B2(I));
          Add(B1(I-1),B2(I));
        end loop;
        Neg(B2(0));
        Add(C(0),B2(0));
      end;
    end if;
  end Clenshaw;

  procedure SymPolyCheby(P: in Polynom1; C: in out Cheby) is
    D: constant Natural := EffDeg(P);
    M: constant Natural := D/2;
    STwo: Scalar;
  begin
    Assign(2,STwo);
    if 2*M /= D then raise Undefined; end if;
    Copy(P(M),C(0));
    for K in 1 .. M loop
      Prod(STwo,P(M-K),C(K));
    end loop;
    for K in M+1 .. C'Last loop
      SetZero(C(K));
    end loop;
  end SymPolyCheby;

  procedure SymCosPoly(P: in Polynom1; Q: in out Polynom1) is
    --- a factor (x*x+U*x+1) of P corresponds to a root -U/2 for Q
    M: constant Natural := EffDeg(P)/2;
    C: Cheby(0 .. M);
  begin
    SymPolyCheby(P,C);
    Clenshaw(C,Q);
  end SymCosPoly;

  --- Polynom2 ---------------------------------------------------------

  -- index = (D*(D+1))/2+J = (D*(D+3))/2-I
  -- where D=I+J
  -- for max degree D need Polynom2(0 .. D*(D+3)/2)
  -- enumeration is
  --  J
  --  |
  --  9
  --  5 8
  --  2 4 7
  --  0 1 3 6 -- I

  function MaxDeg(P: Polynom2) return Natural is
    K: constant Natural := P'Last;
    D: constant Integer := Integer_Sqrt(2*K);
  begin
    if ((D*(D+1))/2) <= K then
      return D;
    else
      return D-1;
    end if;
  end MaxDeg;

  function EffDeg(P: Polynom2) return Natural is
    D: Integer;
  begin
    for K in reverse 1 .. P'Last loop
      if not IsZero(P(K)) then
        D := Integer_Sqrt(2*K);
        if ((D*(D+1))/2) <= K then
          return D;
        else
          return D-1;
        end if;
      end if;
    end loop;
    return 0;
  end EffDeg;

  procedure SetCoeff(I,J: in Natural; C: in Scalar; P: in out Polynom2) is
    D: constant Natural := I+J;
  begin
    Copy(C,P((D*(D+1))/2+J));
  end SetCoeff;

  procedure Coefficient(P: in Polynom2; I,J: in Natural; C: in out Scalar) is
    D: constant Natural := I+J;
  begin
    Copy(P(((D*(D+1))/2+J)),C);
  end Coefficient;

  procedure Show1(N: in String; P: in Polynom2; Hide0: in Boolean := True) is
    W: constant Positive := 4;
    Deg: constant Natural := EffDeg(P);
    AllZero: Boolean := True;
    J,K: Integer;
  begin
    for D in 0 .. Deg loop
      for I in 0 .. D loop
        J := D-I;
        K := (D*(D+1))/2+J;
        if not IsZero(P(K)) then
          Show1(N & Strng(I,W) & Strng(J,W) & " ",P(K));
          AllZero := False;
        end if;
      end loop;
    end loop;
    if not Hide0 then
      if AllZero then Show0(N & "0"); end if;
      Show0;
    end if;
  end Show1;

  procedure Show2(N: in String; P1,P2: in Polynom2; Hide0: in Boolean := True) is
    W: constant Positive := 4;
    Deg: constant Natural := Natural'Max(EffDeg(P1),EffDeg(P2));
    AllZero: Boolean := True;
    J,K: Integer;
  begin
    for D in 0 .. Deg loop
      for I in 0 .. D loop
        J := D-I;
        K := (D*(D+1))/2+J;
        if not (IsZero(P1(K)) and then IsZero(P2(K))) then
          Show2(N & Strng(I,W) & Strng(J,W) & " ",P1(K),P2(K));
          AllZero := False;
        end if;
      end loop;
    end loop;
    if not Hide0 then
      if AllZero then Show0(N & "0 0"); end if;
      Show0;
    end if;
  end Show2;

  procedure Evaluate(P: in Polynom2; S1,S2: in Scalar; C: in out Scalar) is
    --- assuming P includes all coefficients for degree Deg
    Deg: constant Natural := EffDeg(P);
    C2: Scalar;
  begin
    if Deg=0 then
      Copy(P(0),C);
    else
      SetZero(C);
      for I in reverse 0 .. Deg loop
        SetZero(C2);
        for D in reverse I .. Deg loop -- J=D-I
          Mult(S2,C2);
          Add(P((D*(D+3))/2-I),C2);
        end loop;
        Mult(S1,C);
        Add(C2,C);
      end loop;
    end if;
  end Evaluate;

end Polynoms;
