with Strings, Globals;
use Strings, Globals;

pragma Elaborate_All (Strings,Globals);

package body Taylors1.Cheb.Fix is

   use Lin,Lin_Contr;

   function Energy(F: Traj) return TScalar is
      Theta,Yp,Thetap,SinTheta,SqrSinTheta: TScalar;
   begin
      Val(F(2),TOne,Theta);
      DerAtOne(F(1),Yp);
      DerAtOne(F(2),Thetap);
      SinTheta := Sin(Theta);
      SqrSinTheta := Sqr(SinTheta);
      return -(Half*Sqr(Yp)+Sqr(Thetap)/Flt(6))/MOmega2+(SqrSinTheta+Half*Sqr(SqrSinTheta));
   end Energy;

   procedure DerP(F: in Traj; M: in out SMT.Matrix) is
      Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff: TayCheb;
      Y:       TayCheb renames F(1);
      Theta:   TayCheb renames F(2);
      DTheta1: TayCheb renames F(3);
      DTheta2: TayCheb renames F(4);
      DGTheta1,DGTheta2: TayCheb;
   begin
      Val(DTheta1,TOne,M(1,1));
      Val(DTheta2,TOne,M(2,1));
      Aux(Y,Theta,Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff);
      Prod(DThCoeff,DTheta1,DGTheta1);
      Prod(DThCoeff,DTheta2,DGTheta2);
      AntiDer(DGTheta1);
      SetIV(DGTheta1,TZero);
      Val(DGTheta1,TOne,M(1,2));
      AntiDer(DGTheta2);
      SetIV(DGTheta2,IV);
      Val(DGTheta2,TOne,M(2,2));
      SMT.Mult(MFac,M);
   end DerP;

   procedure Aux(Y,Theta: in TayCheb; Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,
                                        Cos4Theta,SixYplus2,EightYplus6,DThCoeff: in out TayCheb) is
   begin
      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;
      Add(Scal(Two),SixYplus2.C(0).C(0));     -- 6y+2
      EightYplus6:= Flt(8)*Y;
      Add(Scal(6),EightYplus6.C(0).C(0));     -- 8y+6
      DThCoeff := Two*(EightYplus6+Flt(12)*Y2)*Cos2Theta;
      Sub(Flt(4)*Cos4Theta,DThCoeff);
      Mult(M34Omega2,DThCoeff);
   end Aux;

   procedure GVar(DThCoeff,DTheta1,DTheta2: in TayCheb; DGTheta1,DGTheta2: in out TayCheb) is
   begin
      Prod(DThCoeff,DTheta1,DGTheta1);
      AntiDer(DGTheta1);
      SetIV(DGTheta1,TZero);
      AntiDer(DGTheta1);
      SetIV(DGTheta1,IV);
      Prod(DThCoeff,DTheta2,DGTheta2);
      AntiDer(DGTheta2);
      SetIV(DGTheta2,IV);
      AntiDer(DGTheta2);
      SetIV(DGTheta2,TZero);
   end GVar;

   procedure GMap(F: in Traj; GF: in out Traj) is
      Y2,SinTheta,SqrSinTheta,Sin2Theta,Sin4Theta,Cos2Theta,Cos4Theta,SixYplus2,EightYplus6,DThCoeff: TayCheb;
      Y:        TayCheb renames F(1);
      Theta:    TayCheb renames F(2);
      DTheta1:  TayCheb renames F(3);
      DTheta2:  TayCheb renames F(4);
      GY:       TayCheb renames GF(1);
      GTheta:   TayCheb renames GF(2);
      DGTheta1: TayCheb renames GF(3);
      DGTheta2: TayCheb renames GF(4);
      S: TScalar;
   begin
      Aux(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);
      Copy(EightYplus6,GTheta);
      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));
      GVar(DThCoeff,DTheta1,DTheta2,DGTheta1,DGTheta2);
   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);
      DTheta1:  TayCheb renames H(3);
      DTheta2:  TayCheb renames H(4);
      DGY:      TayCheb renames DGH(1);
      DGTheta:  TayCheb renames DGH(2);
      DGTheta1: TayCheb renames DGH(3);
      DGTheta2: TayCheb renames DGH(4);
      S: TScalar;
   begin
      DGY := Four*Y;
      Add(Scal(Two),DGY.C(0).C(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); -- (24y+8)Sin(2Theta)dY+((2(6+8Y+12Y^2))Cos(2Theta)-4Cos(4Theta))dTheta
      Mult(M34Omega2,DGTheta);
      AntiDer(DGTheta);
      SumOdd(DGTheta,S);
      Add(DGTheta.C(1),S);
      AntiDerP(DGTheta);
      Sum(S,DTheta.C(0),DGTheta.C(0));
      GVar(DThCoeff,DTheta1,DTheta2,DGTheta1,DGTheta2);
   end DGMap;

   procedure DContrNorm(M: in SMatrix; F: in Traj; PackSize: in Positive; R: out Radius) is
      Y:        TayCheb renames F(1);
      Theta:    TayCheb renames F(2);
      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(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:        TayCheb renames F(1);
      Theta:    TayCheb renames F(2);
      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(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;

end Taylors1.Cheb.Fix;
