//dynamique
function qdot=Cart(t,q)
// q(1) q(2) le vecteur position
// q(3) q(4) le vecteur vitesse
global  alpha ;
// la constante de gravité redimensionnée
   
    qdot(1)=q(3);
    qdot(2)=q(4);
    mod=q(1)**2+q(2)**2
    if (mod>0) then
        qdot(3)=-alpha**2 *q(1)/  (mod**1.5);
        qdot(4)=-alpha**2 *q(2)/  (mod**1.5);
     else
         disp("Pb")
     end        
endfunction
// dynamique etat direct et adjoint
function qdot=Cartp2(t,q)
// q(1) q(2) le vecteur position etat direct
// q(3) q(4) le vecteur vitesse etat direct
// q(5) q(6) le vecteur position etat adjoint
// q(7) q(8) le vecteur vitesse etat adjoint
global  alpha beta0 ;
// la constante de gravité redimensionnée et le max de poussée redimensionnée
    qdot(1)=q(3);
    qdot(2)=q(4);
    qdot(7)=-q(5);
    qdot(8)=-q(6);
    mod=q(1)**2+q(2)**2;
    mod2=q(7)**2+q(8)**2;
    if (mod>0) then
        qdot(3)=-alpha**2 *q(1)/  (mod**1.5) -beta0*q(7)/sqrt(mod2);
        qdot(4)=-alpha**2 *q(2)/  (mod**1.5) -beta0*q(8)/sqrt(mod2);
        qdot(5)=-alpha**2*((2*q(1)**2-q(2)**2)*q(7)+ 3*q(1)*q(2)*q(8))/(mod**2.5);
        qdot(6)=-alpha**2*((2*q(2)**2-q(1)**2)*q(8)+ 3*q(1)*q(2)*q(7))/(mod**2.5);
     else
         disp("Pb")
     end        
endfunction
function [XZero]=Fp(X)
    global  alpha q1_0 q2_0 q1dot_0 q2dot_0;

// X:  p1 p2 p3 p4 tf
//XZero : sur le cercle - vecteur vitesse défini par la position - relartion (p3 p4(t)) ortho vecteur position - Hamiltonien+1


// Affiche un avertissement pour une exception en virgule flottante
ieee(1);   
 // On intègre et on renvoit ce que l'on veut nul
t1=0:X(5)/10:X(5);
z=ode("rk",[q1_0;q2_0;q1dot_0;q2dot_0;X(1);X(2);X(3);X(4)],0,t1,Cartp2);
z1 = z(1:8,11);
disp(X)
// distance au cercle
XZero(1)=(z1(1)**2+z1(2)**2-1);
XZero(2)=1*(-z1(3)-alpha*z1(2));
XZero(3)=1*(-z1(4)+alpha*z1(1));
XZero(4)=(-alpha*z1(8)-z1(5))*z1(2)+(-alpha*z1(7)+z1(6))*z1(1);
l = z1(5:8);
xldot = Cartp2(X(5),z1);

// on calcule l'hamiltonien au temps final
    HamFin=l'*(xldot(1:4));
    XZero(5)=HamFin+1;
//La fonction Cout a minimiser
J=X(5);
disp(XZero)
disp(J,"J")
// visualisation
// trajectoire
t1=0:X(5)/20:X(5);
z1=ode("rk",[q1_0;q2_0;q1dot_0;q2dot_0;X(1);X(2);X(3);X(4)],0,t1,Cartp2);


// vecteur controle : - (p3,p4)/(module(p3,p4)
for i=1:1:21
    x1(i)=z1(1,i);
    y1(i)=z1(2,i);

    x1p(i,i)=-z1(7,i)/sqrt(z1(7,i)**2+z1(8,i)**2);
    y1p(i,i)=-z1(8,i)/sqrt(z1(7,i)**2+z1(8,i)**2);
end

plot(z1(1,1:size(t1,2)),z1(2,1:size(t1,2)),"y");
champ1(x1,y1,x1p,y1p,rect=[-1.2,-1.2,1.2,1.2],arfact=1);

//champ(q1_0,q2_0,q1dot_0,q2dot_0,rect=[-2,-2,2,2],arfact=1)
//quiver(z,q2_0,q1dot_0,q2dot_0,'g')

//disp("t1 Xzero",XZero(1),X(1))
endfunction

//unités
// temps : h
// masse : kg
// distance : Mm

// script principal
//clear all; close all; format long;


global  alpha beta0 q1_0 q2_0 q1dot_0 q2dot_0;



pf=42.165;
p0=11.625;


// les constantes
delta=0.051112*10^3*1/3600;
mu=398600.47*10^(-9)*(3600^2);
alpha=sqrt(mu/(pf^3));
// poussée faible 60N
Fmax=60*10^(-6)*(3600^2);
// poussée forte 3000N
Fmax=50*60*10^(-6)*(3600^2);
epsilon=Fmax*sqrt(pf/mu);

m0=1500;
beta0=alpha*epsilon/m0;

//integration directe en coordonnées q1 q2 q1dot q2dot
// cas geostationnaire orbite haute
q1_0=0;
q2_0=1;
q1dot_0=-alpha;
q2dot_0=0;
t=0:0.1:24;
z=ode("rk",[q1_0;q2_0;q1dot_0;q2dot_0],0,t,Cart);
isoview(-1.2,1.2,-1.2,1.2)
plot(z(1,1:size(t,2)),z(2,1:size(t,2)),'b.')
// energie
for i=1:1:size(t,2)
 E(1,i)=0.5*(z(3,i)**2+z(4,i)**2);
 E(2,i)=-alpha**2/ sqrt(z(1,i)**2+z(2,i)**2); 
 E(3,i)=E(1,i)+E(2,i);
end


// cas geostationnaire orbite basse p0=11.625km
q1_0=p0/pf;
q2_0=0;
q1dot_0=0;
q2dot_0=alpha*sqrt(pf/p0);

t2=0:0.05:24;
z2=ode("rk",[q1_0;q2_0;q1dot_0;q2dot_0],0,t2,Cart);
plot(z2(1,1:size(t2,2)),z2(2,1:size(t2,2)),'g')
// energie
for i=1:1:size(t2,2)
 E2(1,i)=0.5*(z2(3,i)**2+z2(4,i)**2);
 E2(2,i)=-alpha**2/ sqrt(z2(1,i)**2+z2(2,i)**2); 
 E2(3,i)=E2(1,i)+E2(2,i);
end

// cercle p0

// solution 0 tour -2.3 -1.2 -0.7 -1.48 2.07 pour 3000N
//initialisation grossiere
a0 = [-10; 0; 0; -8];
tf =8;
// initialisation fine
a0 = [-2; -1; -1; -1.5];
tf =2;

// lancement de la méthode de tir
//options=optimset('Display','iter');
a0tf= fsolve([a0;tf],Fp);

// recalcul de la solution pour visualisation et calcul d'énergie
t1=0:a0tf(5)/50:a0tf(5);
z1=ode("rk",[q1_0;q2_0;q1dot_0;q2dot_0;a0tf(1);a0tf(2);a0tf(3);a0tf(4)],0,t1,Cartp2);

// energie
for i=1:1:size(t1,2)
 E1(1,i)=0.5*(z1(3,i)**2+z1(4,i)**2);
 E1(2,i)=-alpha**2/ sqrt(z1(1,i)**2+z1(2,i)**2); 
 E1(3,i)=E1(1,i)+E1(2,i);
end
// visualisation trajectoire
plot(z1(1,1:size(t1,2)),z1(2,1:size(t1,2)),"r.")
// visualisation vecteur poussée
for i=1:1:size(t1,2)
    x1(i)=z1(1,i)-0.07*z1(7,i)/sqrt(z1(7,i)**2+z1(8,i)**2);
    y1(i)=z1(2,i)-0.07*z1(8,i)/sqrt(z1(7,i)**2+z1(8,i)**2);

    x1p(i,i)=-z1(7,i)/sqrt(z1(7,i)**2+z1(8,i)**2);
    y1p(i,i)=-z1(8,i)/sqrt(z1(7,i)**2+z1(8,i)**2);
end

//a=get("current_axes");//get the handle of the newly created axes

//a.data_bounds=[-1.2,-1.2;1.2,1.2];
champ(x1,y1,x1p,y1p,arfact=0.7);
//champ(-5:5,-5:5,rand(11,11),rand(11,11))

// *** decommenter si on veut regarder les energies ***

// $$$ scf
// $$$ // visualisation energie
// $$$ plot( t(1,1:size(t,2)),  E(1,1:size(t,2)),'*b',t(1,1:size(t,2)),E(2,1:size(t,2)),'.b',t(1,1:size(t,2)),E(3,1:size(t,2)),'b')
// $$$ plot(t1(1,1:size(t1,2)),E1(1,1:size(t1,2)),'*r',t1(1,1:size(t1,2)),E1(2,1:size(t1,2)),'.r',t1(1,1:size(t1,2)),E1(3,1:size(t1,2)),'r')
// $$$ plot(t2(1,1:size(t2,2)),E2(1,1:size(t2,2)),'*g',t2(1,1:size(t2,2)),E2(2,1:size(t2,2)),'.g',t2(1,1:size(t2,2)),E2(3,1:size(t2,2)),'g')
// $$$