clear all;close all;clc
global GEO mu wt
%AsteroideEROS
GEO=[34.4e3 11.2e3 11.2e3];%metros
% GEO=[34.4e3 34.4e3 34.4e3];
% mu=4.463e-4;
rho=2.67/1000*100^3;%kilogramos/m^3
mu=rho*4/3*pi*(GEO(1)*GEO(2)*GEO(3))*(6.67384e-11)%metros^3/s^2
Ta=5.27;%periodo de rotacion en horas
wt=2*pi/(Ta*3600)%rad/s
aorb=2.8611;
eorb=0.4118657;


% %AsteroideIDA
% GEO=[59.8e3 25.4e3 18.6e3];%metros
% rho=2.6/1000*100^3;%kilogramos/m^3
% mu=rho*4/3*pi*(GEO(1)*GEO(2)*GEO(3))*(6.67384e-11)%metros^3/s^2
% Ta=4.634;%periodo de rotacion en horas
% wt=2*pi/(Ta*3600)%rad/s
% aorb=2.8611;
% eorb=0.4118657;

% %AsteroideLUTETIA
% GEO=[121e3 101e3 75e3];%metros
% rho=3.4/1000*100^3;%kilogramos/m^3
% mu=rho*4/3*pi*(GEO(1)*GEO(2)*GEO(3))*(6.67384e-11)%metros^3/s^2
% Ta=8.165;%periodo de rotacion en horas
% wt=2*pi/(Ta*3600)%rad/s
% aorb=2.8611;
% eorb=0.4118657;

% %AsteroideMATILDE
% GEO=[66e3 48e3 75e3];%metros
% rho=1.3/1000*100^3;%kilogramos/m^3
% mu=rho*4/3*pi*(GEO(1)*GEO(2)*GEO(3))*(6.67384e-11)%metros^3/s^2
% Ta=417.7;%periodo de rotacion en horas
% wt=2*pi/(Ta*3600)%rad/s
% aorb=2.8611;
% eorb=0.4118657;

 
% %AsteroideITOKAWA
% GEO=[0.535e3 0.294e3 0.209e3];%metros
% % GEO=[34.4e3 34.4e3 34.4e3];
% % mu=4.463e-4;
% rho=1.9/1000*100^3;%kilogramos/m^3
% mu=rho*4/3*pi*(GEO(1)*GEO(2)*GEO(3))*(6.67384e-11)%metros^3/s^2
% Ta=12.132;%periodo de rotacion en horas
% wt=2*pi/(Ta*3600)%rad/s
% delta=4*pi*(6.67384e-11)/3*rho*GEO(2)*GEO(3)/GEO(1)^2/wt^2


% GEO=[6378.14e3 6378.14e3 6378.14e3];
% % mu=4.463e-4;
% rho=2.67/1000*100^3;%kilogramos/m^3
% % mu=rho*4/3*pi*(GEO(1)*GEO(2)*GEO(3))*(6.67384e-11)%metros^3/s^2
% mu=398600.4*1000^3;
% wt=2*pi/(24*3600)%rad/s
% Ta=5.27;
% % wt=0;
%

muS=132712439935.5;%%SOL Km^3/s^

%%Simulacion:


% a=((Ta*3600/2/pi)^2*mu)^(1/3)

%Comp de eq en X
[GU U]=Gpot(GEO(1),0,0,GEO,mu);
psepotX=GEO(1)*wt^2+GU(1)

%Comp de eq en Y
[GU U]=Gpot(0,GEO(2),0,GEO,mu);
psepotY=GEO(2)*wt^2+GU(2)

    if psepotX<0
        Xeq = fzero(@(x) eqX(x),2*GEO(1));
        
        disp('Saddlepoint')

[GU U]=Gpot(Xeq,0,0,GEO,mu)
Ux=GU(1)
Uy=GU(2)
Uz=GU(3)

[GGU U]=GGpot(Xeq,0,0,GEO,mu);

Uxx=GGU(1)
Uyy=GGU(2)
Uzz=GGU(3)

cond1=(wt^2+Uxx)*(wt^2+Uyy)


y0=[Xeq 0 0 0 0 0]

Cs=-1/2*(y0(4)^2+y0(5)^2+y0(6)^2)+1/2*wt^2*(y0(1)^2+y0(2)^2)+pot(y0(1),y0(2),y0(3),GEO,mu)
    else
        disp('No hay punto de equilibrio en el ejeX')
    end
    
    if psepotX<0
        Yeq = fzero(@(yi) eqY(yi),GEO(2))
        
        disp('Centpoint')

[GU U]=Gpot(0,Yeq,0,GEO,mu)
Ux=GU(1)
Uy=GU(2)
Uz=GU(3)

[GGU U]=GGpot(0,Yeq,0,GEO,mu);

Uxx=GGU(1)
Uyy=GGU(2)
Uzz=GGU(3)

cond2=(wt^2+Uxx)*(wt^2+Uyy)

y0=[0 Yeq 0 0 0 0]

Cc=-1/2*(y0(4)^2+y0(5)^2+y0(6)^2)+1/2*wt^2*(y0(1)^2+y0(2)^2)+pot(y0(1),y0(2),y0(3),GEO,mu)
    else
        disp('No hay punto de equilibrio en el ejeY')
    end










% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Condiciones iniciales[x y z xp yp zp]

% % y0=[-100e3 0 0 5 -7 0]
% Csuf=Cs
% Yh = fzero(@(yi) HillRad(yi,Csuf),70e3)
% 
% % % XP = fzero(@(Xp) orbYxp(Xp,Csuf,Y-1e3),1)
% [X,Y,TZ]=Surff(Csuf,GEO,mu,wt);
% % figure(20)
% % mesh(X,Y,TZ)
% % % figure(10)
% % hold on
% % t = linspace(0,2*pi);plot(GEO(1)*cos(t),GEO(2)*sin(t),'LineWidth',2.5,'Color',[0.1,0.1,1])
% % hold on
% % contour(X,Y,TZ,[0 0],'LineWidth',3,'Color',[1,0.2,0.1]);
% % colorbar;
% % axis([-3*GEO(1) 3*GEO(1) -4*GEO(2) 4*GEO(2)])
% % % axis equal
% % grid on
% 
% % stop
% %Orbita Per retrograda
% % Xi=50e3
% % VY = fzero(@(Ypi) OrbPer(Ypi,Xi),[-30 -20],optimset('TolX',1e-5,'TolFun',1e-5))
% %Ypi=-24.882245153220357
% % Xi=50e3
% %Ypi=-10.877266116216234
% in=20*pi/180;
% YhINC = fzero(@(yi) HillRadINC(yi,Csuf,in),100e3)
% 
% Xo=0;
% Yo=YhINC+9.5e3;
% Zo=0;
% % 
% 
% Vi=abs(cos(in)*sqrt(mu/Yo)-wt*Yo);
% vx=Vi
% vz=sqrt(mu/Yo)*sin(in)
% % % Vf=sqrt(mu/Yh)
% % % Vpm=wt*Yh
% % 
% % T=EnCin(Xo,Yo,Csuf,GEO,mu,wt);
% % Vi=sqrt(T)
% % Vi=Vf+Vpm
% y0=[0 Yo 0 vx 0 vz]
% C=-1/2*(y0(4)^2+y0(5)^2+y0(6)^2)+1/2*wt^2*(y0(1)^2+y0(2)^2)+pot(y0(1),y0(2),y0(3),GEO,mu)
% Csuf=C;
% 
% [X,Y,TZ]=Surff(Csuf,GEO,mu,wt);
% figure(20)
% mesh(X,Y,TZ)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%Condiciones iniciales en elementos orbitales(ejes fijos)[e a(metros) inc OM w TA] 
%%angulos en grados

eo=[0 60.65e3 45 0 0 0];%Orbita periodica EROS
% eo=[0.05 100e3 30 180 50 30]

[coord]=rvSF(eo);
y0=coord'
C=-1/2*(y0(4)^2+y0(5)^2+y0(6)^2)+1/2*wt^2*(y0(1)^2+y0(2)^2)+pot(y0(1),y0(2),y0(3),GEO,mu)
Csuf=C;

[X,Y,TZ]=Surff(Csuf,GEO,mu,wt);
figure(20)
mesh(X,Y,TZ)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Comparación fuerza del sol con la del asteroide:
Comp=(muS/((aorb*(1-eorb^2))*149.6E6)^2/(mu/(norm(y0(1:3)))^2))^-1

%Tiempo de simulación
Tint=8*86400; 
% Tint=Tast;
min=0.5;

TIEMPO=[0:60*min:Tint];
L=length(TIEMPO);
TIEMPO2=[0 Tint];
TOL=1e-8;

misopciones=odeset('AbsTol',TOL/1000,'RelTol',TOL/10,'Stats','on');

tic 
[t,y]=ode113(@ecdifmov, TIEMPO2, y0',misopciones);
toc

ysim=y;
clear y
y=zeros(L,6);
y(:,1) = interp1(t,ysim(:,1),TIEMPO,'spline');
y(:,2) = interp1(t,ysim(:,2),TIEMPO,'spline');
y(:,3) = interp1(t,ysim(:,3),TIEMPO,'spline');
y(:,4) = interp1(t,ysim(:,4),TIEMPO,'spline');
y(:,5) = interp1(t,ysim(:,5),TIEMPO,'spline');
y(:,6) = interp1(t,ysim(:,6),TIEMPO,'spline');
t=TIEMPO;
LAST=length(y(:,1:6))

DISTEJES=max(max(y(:,1:3)));

%%Representación del asteroide
[xE, yE, zE] = ellipsoid(0,0,0,GEO(1),GEO(2),GEO(3),30);
figure(1)
surf(xE, yE, zE)
% axis equal

%Representación de la órbita
% hold on
% plot3(y(:,1),y(:,2),y(:,3))

axis equal
hold on
plot3(y(:,1),y(:,2),y(:,3),'LineWidth',1)
axis(1.3*[-DISTEJES DISTEJES -DISTEJES DISTEJES -DISTEJES DISTEJES])
% axis([-3*GEO(1) 3*GEO(1) -3*GEO(2) 3*GEO(2) -3*GEO(3) 3*GEO(3)])
% axis([-10000e3 10000e3 -10000e3 10000e3 -10000e3 10000e3])
grid on
hold on
title('Órbita relativa al asteroide')
xlabel('x (Km)')
ylabel('y (Km)')
zlabel('z (Km)')
% contour(X,Y,TZ,[0 0],'LineWidth',3,'Color',[1,0.2,0.1]);


% %%%%%%%%%%%%Hacer animación
% %%
% figure (2)
% [xE, yE, zE] = ellipsoid(0,0,0,GEO(1),GEO(2),GEO(3),30);
% surf(xE, yE, zE)
% axis([-DISTEJES DISTEJES -DISTEJES DISTEJES -DISTEJES DISTEJES])
% % axis([-10000e4 10000e4 -10000e4 10000e4 -10000e4 10000e4])
%  title('Animación de la órbita simulada')
% xlabel('Eje x')
% ylabel('Eje y')
% zlabel('Eje z')
%  
%  hold on
%  plot3(y(1,1),y(1,2),y(1,3))
%  for i=2:LAST
%  %%Representación de la órbita
%  hold on
%  plot3(y(i,1),y(i,2),y(i,3),'*','MarkerSize',2)
%  pause(0.000001)
%  end

 %%Representar evolucion de los elementos orbitales
 EO=zeros(LAST,6);
 for i=1:LAST
     EO(i,:)=eloSF(y(i,:),t(i));
 end
 
 %Representar la evolucion del semieje mayor
 figure(3)
 plot(t,EO(:,2))
 title('Semieje Mayor')
 ylabel('Km')
 xlabel('t(s)')
 grid on
 
 Z = trapz(t,EO(:,2));
 valMed=Z/Tint

  %Representar la evolucion de la inclinacion
 figure(4)
 plot(t,EO(:,3))
 title('Inclinacion')
 ylabel('i(º)')
 xlabel('t(s)')
 grid on
   %Representar la evolucion de OM
% %    OM=EO(:,4)+wt*t'*180/pi;
% %    OM=OM/360;
% %    OM=OM-floor(OM);
% %    OM=360*OM;
% %    EO(:,4)=OM;
 figure(5)
 plot(t,EO(:,4))
 title('Ascension recta del nodo ascendente') 
 ylabel('\Omega(º)')
 xlabel('t(s)')
 grid on
 %Representar la evolucion de w
 figure(6)
 plot(t,EO(:,5))
 title('Argumento del perigeo') 
 ylabel('w(º)')
 xlabel('t(s)')
 grid on
 %Representar la evolucion de TA
 figure(7)
 plot(t,EO(:,6))
 title('Anomalía verdadera')
 ylabel('\theta(º)')
 xlabel('t(s)')
 grid on
 %Representar la evolucion de la excentricidad
 figure(8)
 plot(t,EO(:,1))
 title('Excentricidad')
 ylabel('')
 xlabel('t(s)')
 grid on
 
 O=zeros(LAST,6);
 for i=1:LAST
     O(i,:)=rv(EO(i,:));
 end
 
 figure(9)
 title('Órbita')
 plot3(O(:,1),O(:,2),O(:,3),'LineWidth',2)
 axis(1.3*[-DISTEJES DISTEJES -DISTEJES DISTEJES -DISTEJES DISTEJES])
 axis equal
 title('Órbita relativa a los ejes fijos')
 xlabel('x (Km)')
 ylabel('y (Km)')
 zlabel('z (Km)')
 grid on
 
 

%  %Animacion de orbita en ejes fijos
%   figure(10)
%    title('Órbita')
%    axis(1.5*[-DISTEJES DISTEJES -DISTEJES DISTEJES -DISTEJES DISTEJES])
%     grid on
%  for i=1:length(t)
%      
% hold on
%  plot3(O(i,1),O(i,2),O(i,3),'*b','MarkerSize',3)
%  
%  pause(0.00001)
%  end
 