with Strings, Globals, Ints;
use Strings, Globals, Ints;

pragma Elaborate_All (Strings,Globals);

package body Taylors2.Cheb.Fix is

   use Lin,Lin_Contr;

   procedure Aux(F: in Traj; Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,
                               Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff: in out TayCheb) is
   begin
      Copy(F(1),Y);
      Copy(F(2),Theta);
      Y2:= Sqr(Y);
      SinTheta:= Sin(Theta);
      SqrSinTheta:= Sqr(SinTheta);
      Sin2Theta:= Sin(Two*Theta);
      Sin4Theta:= Sin(Four*Theta);
      Cos2Theta:= Cos(Two*Theta);
      Cos4Theta:= Cos(Four*Theta);
      SixYplus2:= Flt(6)*Y;
      EightYplus6:= Flt(8)*Y;
      Add(Scal(Two),SixYplus2.C(0).C(0,0));     -- 6y+2
      Add(Scal(6),EightYplus6.C(0).C(0,0));     -- 8y+6
      SetZero(DThCoeff);
   end Aux;

   procedure FMap(F: in Traj; GF: in out Traj) is
      Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff: TayCheb;
      GY:       TayCheb renames GF(1);
      GTheta:   TayCheb renames GF(2);
      S: TSCalar;
   begin
      Aux(F,Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff);
      Copy(Y,GY);
      Add(Y2,GY);
      Add(Y*Y2,GY);
      Mult(Two,GY);                      ------   2(Y+Y^2+Y^3)
      Add(SixYplus2*SqrSinTheta,GY);     ------   2(Y+Y^2+Y^3)+(6Y+2)Sin(Theta)^2
      AntiDer2D(GY);
      Mult(MOmega2,GY);
      GTheta := Flt(8)*Y;
      Add(Scal(6),GTheta.C(0).C(0,0));
      Add(Flt(12)*Y2,GTheta);                ------  6+8Y+12Y^2
      Mult(Sin2Theta,GTheta);                ------  (6+8Y+12Y^2) Sin(2Theta)
      Sub(Sin4Theta,GTheta);                 ------  6+8Y+12Y^2  Sin(2Theta)-Sin(4Theta)
      Mult(M34Omega2,GTheta);
      AntiDer(GTheta);
      SumOdd(GTheta,S);
      Add(GTheta.C(1),S);
      AntiDerP(GTheta);
      Sum(S,Theta.C(0),GTheta.C(0));
   end FMap;

   procedure GMap(F: in Traj; GF: in out Traj) is
      S: Tscalar := Scal(0);
   begin
      FMap(F,GF);
      IdMinusProjection(GF);
      Add(LambdaFHat,GF);
   end GMap;

   procedure DGMap(Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,
                     Cos4Theta,SixYplus2,EightYplus6,DThCoeff: in TayCheb; H: in Traj; DGH: in out Traj) is
      DY:       TayCheb renames H(1);
      DTheta:   TayCheb renames H(2);
      DGY:      TayCheb renames DGH(1);
      DGTheta:  TayCheb renames DGH(2);
      S: TScalar;
   begin
      DGY := Four*Y;
      Add(Scal(Two),DGY.C(0).C(0,0));
      Add(Flt(6)*(Y2+SqrSinTheta),DGY);         ------   2+4Y+6(y^2+Sin(Theta)^2)
      Mult(DY,DGY);
      Add((SixYplus2*Sin2Theta)*DTheta,DGY);
      Mult(MOmega2,DGY);
      AntiDer2D(DGY);
      DGTheta := Four*SixYplus2;                ------  24y+8
      Mult(Sin2Theta*DY,DGTheta);               ------  (24y+8)Sin(2Theta)dY
      Add((Two*(EightYplus6+Flt(12)*Y2)*Cos2Theta-Four*Cos4Theta)*DTheta,DGTheta);
      Mult(M34Omega2,DGTheta);
      AntiDer(DGTheta);

      SumOdd(DGTheta,S);
      Add(DGTheta.C(1),S);
      AntiDerP(DGTheta);
      Sum(S,DTheta.C(0),DGTheta.C(0));

      IdMinusProjection(DGH);
   end DGMap;

   procedure DContrNorm(M: in SMatrix; F: in Traj; PackSize: in Positive; R: out Radius) is
      Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff: TayCheb;
      Q: constant TrajModes := Make(RadC,PackSize,True,False);  --- Err modes, but Extract is not used
      procedure DG(H: in Traj; DGH: in out Traj) is begin
         DGMap(Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff,H,DGH);
      end DG;
      procedure DContract is new DContr (DMap => DG);
      procedure DC(H: in Traj; DCH: in out Traj) is begin DContract(QC,M,H,DCH); end DC;
      procedure DCNorm is new Op_Norm (LinOp => DC);
   begin
      Aux(F,Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff);
      if PackSize <= Mat_PackSize then raise Not_Implemented; end if;
      if Verbosity>1 then
         Show1("DContrNorm: Q'Last = ",Q'Last);
      end if;
      DCNorm(Q,R);
   end DContrNorm;

   procedure ContrFix(M: in SMatrix; PackSize: in Positive; F: in out Traj) is
      KMax: constant Radius := Flt(7)/Flt(8);
      E,K: Radius;
      ANorm,R,Eps,Del: Scalar;
      F0: Traj;
   begin
      Show0("ContrFix: ",False);
      Copy(F,F0);
      Center(F0);
      FProd_Parallel := True;
      GMap(F0,F);
      Sub(F0,F);
      FProd_Parallel := False;
      E := MaxNorm(F);
      Assign(E,Eps);
      OpNorm(QC,M,ANorm);
      Add(SOne,ANorm);
      Show2("Eps,ANorm ",E,Sup(ANorm));
      ---
      Del := Eps/(SOne-Scal(KMax))+Scal(Tiny);
      R := ANorm*Del;
      Copy(F0,F);
      AddBall(MaxNorm(R),F);
      DContrNorm(M,F,PackSize,K);
      Show1("ContrFix: K ",K);
      if K>KMax then raise Sorry with "K too large"; end if;
      ---
      Del := Eps/(SOne-Scal(K))+Scal(Tiny);
      R := ANorm*Del;
      Show2("ContrFix: Del,R: ",Sup(Del),Sup(R));
      Copy(F0,F);
      AddBall(MaxNorm(R),F);
   end ContrFix;

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

   procedure DerMatrix(F: in Traj; M: in out SMatrix) is
      --- for numeric mode only
      Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff: TayCheb;
      procedure DG(H: in Traj; DGH: in out Traj) is begin
         DGMap(Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff,H,DGH);
      end DG;
      procedure Enclose is new Enclosure (LinOp => DG);
   begin
      if Not_STrunc then raise Sorry; end if;
      Aux(F,Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff);
      Enclose(QC,M);
   end DerMatrix;

   procedure ContrMatrix(F: in Traj; M: in out SMatrix) is
   begin
      if Verbosity>1 then Show0("running ContrMatrix"); end if;
      DerMatrix(F,M);
      Contr_Matrix(M);
   end ContrMatrix;

   procedure ApproxFix(F: in out Traj; M: in out SMatrix; Eps: in Radius := Zero) is
      --- find approximate fixed point
      procedure Contract0 is new ContrTmp0 (Map => GMap);
      Iter: constant Integer := 1024;
      MaxCredit: constant Integer := 4;
      Credit: Integer := MaxCredit;
      Err,MinErr: Radius := 1.0E+9;
      ErrMat: constant Radius := 1.0e-4;
      G,Tmp1,Tmp2: Traj;
   begin
      if Not_STrunc then raise Sorry with "only for numeric mode"; end if;
      if Verbosity>1 then Show0("running ApproxFix"); end if;
      Copy(F,G);
      for I in 1 .. Iter loop
         if Err>ErrMat then ContrMatrix(G,M); end if;
         Contract0(QC,M,G,Tmp1,Tmp2,Err);
         if Verbosity>1 then Show1("ApproxFix: step " & Strng(I) & ", Err ",Err); end if;
         if Err<MinErr then
            Copy(G,F);
            if (Err<Eps) or (Err=MinErr) then exit; end if;
            MinErr := Err;
            Credit := MaxCredit;
         else
            exit when Credit=0;
            Credit := Credit-1;
         end if;
      end loop;
      Show1("ApproxFix: Err ",Err);
      --    ContrMatrix(F,M);
   end ApproxFix;

   procedure IterContr(F: in out Traj; M: in out SMatrix) is
      --- improve approximate fixed point
      procedure Contract0 is new ContrTmp0 (Map => GMap);
      MaxCredit: constant Integer := 16;
      Credit: Integer := MaxCredit;
      Err,MinErr: Radius := One;
      G,Tmp1,Tmp2: Traj;
   begin
      if Not_STrunc then raise Sorry with "only for numeric mode"; end if;
      if Verbosity>1 then Show0("running IterContr"); end if;
      Copy(F,G);
      --ContrMatrix(G,M);
      while Credit>0 loop
         Contract0(QC,M,G,Tmp1,Tmp2,Err);
         Show1("IterContr: Err ",Err);
         if Err<MinErr then
            Copy(G,F);
            MinErr := Err;
            Credit := MaxCredit;
            --ContrMatrix(G,M);
         else
            Credit := Credit-1;
         end if;
      end loop;
      Show1("IterContr: Err ",MinErr);
      --ContrMatrix(F,M);
   end IterContr;


  procedure ScalProd(F,G: in Traj; S: in out TScalar) is
    Tmp: TScalar;
  begin
    ScalProd(F(1),G(1),S);
    ScalProd(F(2),G(2),Tmp);
    Add(Tmp,S);
  end ScalProd;

  procedure Normalize(F: in out Traj) is
    S,T: TScalar;
  begin
    ScalProd(F,F,S);
    Sqrt(S,T);
    Inv(T,S);
    Mult(S,F);
  end Normalize;

  procedure IdMinusProjection(F: in out Traj) is
    S: TScalar;
  begin
    ScalProd(F,FHat,S);
    Neg(S);
    AddProd(S,FHat,F);
  end IdMinusProjection;

  procedure SaveG(F: in Traj; FileName: in String) is
    S: TScalar;
    G: Traj;
  begin
    FMap(F,G);
    ScalProd(G,FHat,S);
    Sub(Lambda,S);
    if STrunc then
      Write(Output_Dir & FileName & ".g.approx",S);
    else
      Write(Output_Dir & FileName & ".g",S);
    end if;
    Plot(Pic_Dir & FileName & ".g.plot",S,100);
  end SaveG;

  procedure ShowDer1 is
    S: constant Radius := Flt(7)/Flt(8);
    R: Radius;
    RR: Scalar;
    C: SV.Vector(1..7);
    T,T1,T2,T12,T22,T222: Taylor2;
    OK: Boolean := True;
  begin
    if STrunc then
       Read(Input_Dir & "BIF1.g.approx",T);
    else
       Read(Input_Dir & "BIF1.g",T);
    end if;
    R := T.R*S;
    Copy(T,T1);
    Copy(T,T2);
    Der1(R,T1);
    Der2(R,T2);
    Copy(T2,T12);
    Copy(T2,T22);
    R := R*S;
    Der1(R,T12);
    Der2(R,T22);
    Copy(T22,T222);
    R := R*S;
    Der2(R,T222);
    RR := BallAt0(R);
    C(1) := -Val(T222,RR,RR);
    C(2) := -Val(T12,RR,RR);
    C(3) := -Val(T2,Scal(R),SZero)+(Half*R)*Val(T22,Scal(R),SZero);
    C(4) := -Val(T2,Scal(R),SZero)-(Half*R)*Val(T22,Scal(R),SZero);
    C(5) := -Val(T,Scal(-R),Scal(R));
    C(6) := Val(T,Scal(-R),Scal(-R));
    C(7) := Val(T2,Scal(-R),SZero);
    for I in 1..7 loop
       if Inf(C(I))<=Zero then Show1("NOT ok: ",I); OK := False; end if;
    end loop;
    if OK then Show0("All checks ok"); end if;
  end ShowDer1;

  procedure ShowDer2 is
    S: constant Radius := Flt(31)/Flt(32);
    R,Left,Right,Up,Down,D: Flt;
    RR: Scalar;
    C: SV.Vector(1..7);
    T,T1,T2,T12,T22,T222: Taylor2;
    OK: Boolean := True;
  begin
    if STrunc then
       Read(Input_Dir & "BIF2.g.approx",T);
    else
       Read(Input_Dir & "BIF2.g",T);
    end if;
    R := T.R*S;
    Copy(T,T1);
    Copy(T,T2);
    Der1(R,T1);
    Der2(R,T2);
    Copy(T2,T12);
    Copy(T2,T22);
    R := R*S;
    Der1(R,T12);
    Der2(R,T22);
    Copy(T22,T222);
    R := R*S;
    Der2(R,T222);

    -----------  pitchfork bifurcation
    RR := BallAt0(R);
    C(1) := -Val(T222,Scal(-Flt(3)/Flt(4)*R)+Quarter*RR,Quarter*RR);
    C(2) := Val(T12,RR,RR);
    C(3) := -Val(T2,Scal(-R),SZero)+(Quarter*R)*Val(T22,Scal(-R),SZero);
    C(4) := -Val(T2,Scal(-R),SZero)-(Quarter*R)*Val(T22,Scal(-R),SZero);
    C(5) := -Val(T,Scal(-Half*R),Scal(Quarter*R));
    C(6) := Val(T,Scal(-Half*R),Scal(-Quarter*R));
    C(7) := Val(T2,Scal(-Half*R),SZero);
    for I in 1..7 loop
       if Inf(C(I))<=Zero then Show1("1st NOT ok: ",I); OK := False; end if;
    end loop;

    -----------  fold bifurcation
    D := Half**4*R;
    Left := (Flt(10)*Half**4)*R;
    Right := R;
    Up := (Flt(13)*Half**4)*R;
    Down := (Flt(17)*Half**5)*R;
    for I in 10..15 loop
       for J in 9..13 loop
          C(1) := Val(T22,Scal(Flt(I)*D+Half**5)+Flt(Half**5)*RR,Scal(Flt(J)*D)+Flt(Half**5)*RR);
          if Inf(C(1))<=Zero then Show2("2nd (1) NOT ok: ",I,J); OK := False; end if;
       end loop;
    end loop;
    C(2) := Val(T1,Scal(Half*R)+(Quarter)*RR,Scal(Half*R)+Quarter*RR);
    C(3) := Val(T,Scal(Right),Scal(Half*(Up+Down)))+Half*(Up-Down)*Val(T2,Scal(Right),Scal(Half*(Up+Down)));
    C(4) := Val(T,Scal(Right),Scal(Half*(Up+Down)))-Half*(Up-Down)*Val(T2,Scal(Right),Scal(Half*(Up+Down)));
    C(5) := Val(T,Scal(Left),Scal(Up));
    C(6) := -Val(T,Scal(Left),Scal(0.6*R));
    C(7) := Val(T,Scal(Left),Scal(Down));
    for I in 2..7 loop
       if Inf(C(I))<=Zero then Show1("2nd NOT ok: ",I); OK := False; end if;
    end loop;

    -----------  branch between bifurcations
    D := Half**6*R;
    Right := (Half**6)*R;
    Left := -Half*R;
    Up := (Flt(41)*Half**6)*R;
    Down := (Flt(7)*Half**5)*R;

    C(1) := Val(T1,Enclose(Left,Right),Enclose(Down,Up));
    for I in -31..0 loop
       for J in 15..41 loop
          C(2) := -Val(T2,Scal(Flt(I)*D)+Flt(Half**6)*RR,Scal(Flt(J)*D)+Flt(Half**6)*RR);
          if Inf(C(2))<=Zero then Show2("3rd (2) NOT ok: ",I,J); OK := False; end if;
       end loop;
    end loop;
    C(3) := -Val(T,Scal(Right),Scal(Up));
    C(4) := -Val(T,Scal(Left),Scal(Up));
    C(5) := Val(T,Scal(Right),Scal(Down));
    C(6) := Val(T,Scal(Left),Scal(Down));
    for I in 1..6 loop
       if Inf(C(I))<=Zero then Show1("3rd NOT ok: ",I); OK := False; end if;
    end loop;

    Right := (Flt(10)*Half**4)*R;
    Left := Half**6*R;
    Up := (Flt(42)*Half**6)*R;
    Down := (Flt(19)*Half**6)*R;
    for I in 1..39 loop
       for J in 20..41 loop
          C(1) := -Val(T2,Scal(Flt(I)*D)+Flt(Half**6)*RR,Scal(Flt(J)*D)+Flt(Half**6)*RR);
          if Inf(C(1))<=Zero then Show2("4th (1) NOT ok: ",I,J); OK := False; end if;
       end loop;
    end loop;
    C(1) := -Val(T,Scal(Right),Scal(Up));
    C(2) := -Val(T,Scal(Left),Scal(Up));
    C(3) := Val(T,Scal(Right),Scal(Down));
    C(4) := Val(T,Scal(Left),Scal(Down));
    for I in 1..4 loop
       if Inf(C(I))<=Zero then Show1("4th NOT ok: ",I); OK := False; end if;
    end loop;

    if OK then Show0("All checks ok"); end if;

  end ShowDer2;

  function Interval(C,R: Flt; J: Positive) return TScalar is
     P: TScalar;
  begin
    SetZero(P);
    if R=Zero then
      Assign(C,P.C(0,0));
    elsif PDeg=0 then
      Enclose(-(R-C),C+R,P.C(0,0));
    else
      P.F := PDeg1;
      Assign(C,P.C(0,0));
      if J=1 then
        Assign(R,P.C(1,0));
      elsif J=2 then
        Assign(R,P.C(0,1));
      else
        raise Undefined;
      end if;
    end if;
    return P;
  end Interval;

  procedure ReadFHat(FileName: in String) is
  begin
     SetZero(FHat);
     if FileName="BIF1" then
        Read(Input_Dir & "BIF1.FHat",FHat(2));
        Lambda := Interval(Zero,Half**5,2);
     else
        Read(Input_Dir & "BIF2.FHat",FHat(1));
        Lambda := Interval(Zero,Flt(9)*Half**5,2);
     end if;
     Normalize(FHat);
     Prod(Lambda,FHat,LambdaFHat);
  end ReadFHat;

begin

   MOmega2 := -Interval(T2C,T2W,1);
   M34Omega2 := (Three*Quarter)*MOmega2;

end Taylors2.Cheb.Fix;
