// dynamique etat direct
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é redimensionée

    qdot(1)=q(3);
    qdot(2)=q(4);
    mod=sqrt(q(1)**2+q(2)**2)**3;
    if (mod>0) then
        qdot(3)=-alpha**2 *q(1)/  mod;
        qdot(4)=-alpha**2 *q(2)/  mod;
     else
         disp("Pb")
     end    
    
endfunction
// dynamique etat direct et adjoint
function qdot=Cartp(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 ;
// la constante de gravité 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;
    if (mod>0) then
        qdot(3)=-alpha**2 *q(1)/  (mod**1.5);
        qdot(4)=-alpha**2 *q(2)/  (mod**1.5);
        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_init q2dot_0_init;
 // Mode d'affichage
// X: u1 u2 p1 p2 tf
//XZero : sur le cercle - p3(t) - p4(t) ortho p1(t)P2(t) _-Hamiltonien
//mode(0);

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

// on calcule le Hamiltonien au temps final
    HamFin=l'*(xldot(1:4));
    XZero(5)=HamFin;
//La fonction Cout a minimiser
J=(-X(1)+q2dot_0_init)**2+(-X(2)+q2dot_0_init)**2+(-z1(3)-alpha*z1(2))**2+(-z1(4)+alpha*z1(1))**2;
disp(J,X(2),"J")

//visualisation au cours de la recherche des zeros des trajectoires essayees
t1=0:X(5)/20:X(5)
q1dot_0=X(1)
q2dot_0=X(2)

z1=ode("rk",[q1_0;q2_0;q1dot_0;q2dot_0],0,t1,Cart);
//disp(size(z1)) 
//la trajectoire
plot(z1(1,1:size(t1,2)),z1(2,1:size(t1,2)),"y")
//le controle
// construction de la grille de visualisation
    x1(1)=z1(1,1)
    y1(1)=z1(2,1)
    x1p(1,1)=z1(3,1)
    y1p(1,1)=z1(4,1)
 
    x1(2)=z1(1,21)
    y1(2)=z1(2,21)
    x1p(2,2)=z1(3,21)
    y1p(2,2)=z1(4,21)
champ1(x1,y1,x1p,y1p,rect=[-1.2,-1.2,1.2,1.2])

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

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

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


global  alpha q1_0 q2_0 q1dot_0_init q2dot_0_init;

// constante tf=C/Tmax
C=1.145585322127123e+04;


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));
Tmax=60*10^(-6)*(3600^2);
epsilon=Tmax*sqrt(pf/mu);
m0=1500;


//integration directe en coordonnées q1 q2 q1dot q2dot
// cas ellipse
//q1_0=-4*p0/pf
//q2_0=0
//q1dot_0=0
//q2dot_0=-0.25*alpha*sqrt(pf/p0)
// cas geostationnaire orbite haute
q1_0=0
q2_0=1
q1dot_0=-alpha
q2dot_0=0
t=0:0.5: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
q1_0=p0/pf/(1+0.75)
q2_0=0
q1dot_0=0
q2dot_0=alpha*sqrt(pf*1.75/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


// cas geostationnaire orbite basse
// recherche de la poussee minimale 
q1_0=p0/pf/(1+0.75)
q2_0=0
q1dot_0_init=0
q2dot_0_init=alpha*sqrt(1.75*pf/p0)
// on cherche V1 V2 P1 P2 et t  
// on cherche le t qui intercepte  l orbite haute
// conditions de transversalité + Hamiltonien nul
//tf0=1.
//tf= fsolve(tf0,F);

// initialisation sauvage 
//a0 = [-0.7; V1; 1; 1]
//
a0 = [-0.6; 0.6; 1; 1];
tf = 1.;
    
// lancement de la méthode de tir
options=optimset('Display','iter');
a0tf= fsolve([a0;tf],Fp);

t1=0:a0tf(5)/50:a0tf(5);
q1dot_0=a0tf(1)
q2dot_0=a0tf(2)

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

z1=ode("rk",[q1_0;q2_0;q1dot_0;q2dot_0],0,t1,Cart);
// 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
plot(z1(1,1:size(t1,2)),z1(2,1:size(t1,2)),"r")
// on remet du boost
//DeltaV2=sqrt(2*0.0066)
//q1_0=z1(1,size(t1,2))
//q2_0=z1(2,size(t1,2))
//disp(q1_0,q2_0)
//q1dot_0=z1(3,size(t1,2))* (1+DeltaV2/sqrt(z1(3,size(t1,2))**2+z1(4,size(t1,2))**2) )
//q2dot_0=z1(4,size(t1,2))* (1+DeltaV2/sqrt(z1(3,size(t1,2))**2+z1(4,size(t1,2))**2) )
//disp(q1dot_0,q2dot_0)
//t3=5:0.5:12
//z3=ode("rk",[q1_0;q2_0;q1dot_0;q2dot_0],5,t3,Cart)

//// energie
//for i=1:1:size(t3,2)
// E3(1,i)=0.5*(z3(3,i)**2+z3(4,i)**2)
// E3(2,i)=-alpha**2/ sqrt(z3(1,i)**2+z3(2,i)**2) 
// E3(3,i)=E3(1,i)+E3(2,i)
//end


//disp(t1);
//plot(z3(1,1:size(t3,2)),z3(2,1:size(t3,2)),"m+");
// $$$ scf
// $$$ 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')
// $$$ //plot(t3(1,1:size(t3,2)),E3(1,1:size(t3,2)),'*m',t3(1,1:size(t3,2)),E3(2,1:size(t3,2)),'.m',t3(1,1:size(t3,2)),E3(3,1:size(t3,2)),'m')