clear all; close all; clc
%Programa que propaga la posición del asteroide en el sistema solar
%integrando la ecuación del movimiento usando ode45/113. Tiene en cuenta tanto
%la fuerza central del sol, como las perturbaciones debidas a la TIERRA y a
%JUPITER.

%%Se utilizan varias funciones:
    %-- ecdifmov.m Ecuación que devuelve los terminos derivados tanto de la
    %posición como de la velocidad, para que el ode45 determine la posición
    %en instantes sucesivos.
    
    %-- elo.m Función que convierte las coordenadas cartesianas en
    %elementos orbitales Keplerianos (r,v)--->[EC,A(UA),INC,OM,W,TA]
    
    %-- rv.m Función que transforma los elementos orbitales Keplerianos en
    %coordenadas cartesianas [EC,A(Km),INC,OM,W,TA]--->(r,v).
    
    %-- pplaneta.m Función que propaga la Anomalía verdadera de un planeta
    %un tiempo Deltat desde su estado en la época introducida hasta la época del asteroide 
    %(se introducen los elementos orbitales en la época y se propagan el Deltat). Para ello
    %utiliza las leyes horarias.

%%Constantes necesarias
global epoch EPOCH AU mus EOTi EOJu mut muj
muj=126686511;
mut=398600.4+4902.8;
mus=132712439935.5;
AU=149.598E6; %Para convertir UA en Km.


% %%Elementos orbitales [EC,A(Km),INC,OM,W,TA] (angulos en rad) de EROS en
% %la época
% epoch=2457388.5;
% [eoAST]=[2.226813330714129E-01, 1.457877383771478E+00*AU, 1.082860115378293E+01*pi/180, 3.043298340769508E+02*pi/180, 1.787979768290810E+02*pi/180, 1.135171214708696E+02*pi/180]
% [coord]=rv(eoAST); %Calculo de coordenadas del asteroide en el sistema de ejes cartesiano (plano xy la ecliptica)
% y0=[coord(1:3)',coord(4:6)'];
% %%

% %%Elementos orbitales [EC,A(Km),INC,OM,W,TA] (angulos en rad) de EROS en
% %la época
% epoch=2458849.500000000;
% [eoAST]=[2.228488938345887E-01, 1.458180507589480E+00*AU, 1.083017848332440E+01*pi/180, 3.042990867335021E+02*pi/180, 1.788691040922637E+02*pi/180, 1.842764994868708E+02*pi/180]
% [coord]=rv(eoAST); %Calculo de coordenadas del asteroide en el sistema de ejes cartesiano (plano xy la ecliptica)
% y0=[coord(1:3)',coord(4:6)'];
%%


% %%Elementos orbitales [EC,A(Km),INC,OM,W,TA] (angulos en rad) de BENNU en
% %%la época
% epoch=2457388.5; %Epoca donde se dan los elementos orbitales del Asteroide
% [eoAST]=[2.036937581117480E-01, 1.125974114072213E+00*AU, 6.035034188605186E+00*pi/180, 2.032966840666327E+00*pi/180, 6.628640155709100E+01*pi/180, 1.717456089365612E+02*pi/180]
% [coord]=rv(eoAST); %Calculo de coordenadas del asteroide en el sistema de ejes cartesiano (plano xy la ecliptica) 
% y0=[coord(1:3)',coord(4:6)'];

% %%Elementos orbitales [EC,A(Km),INC,OM,W,TA] (angulos en rad) de BENNU en
% %%la época
% epoch=2457296.5000000005; %Epoca donde se dan los elementos orbitales del Asteroide
% [eoAST]=[2.036778691089617E-01, 1.125990941576946E+00*AU, 6.035052915201147E+00*pi/180, 2.033125057120579E+00*pi/180, 6.629035363618713E+01*pi/180, 1.144466255616703E+02*pi/180]
% [coord]=rv(eoAST); %Calculo de coordenadas del asteroide en el sistema de ejes cartesiano (plano xy la ecliptica) 
% y0=[coord(1:3)',coord(4:6)'];

% %%Elementos orbitales [EC,A(Km),INC,OM,W,TA] (angulos en rad) de 2007 UN12 en
% %%la época
% epoch=2457388.5; %Epoca donde se dan los elementos orbitales del Asteroide
% [eoAST]=[6.048471053892168E-02, 1.053775829820429E+00*AU, 2.353880652942084E-01*pi/180, 2.160483787140163E+02*pi/180, 1.344338077930306E+02*pi/180, 2.348229983326196E+02*pi/180]
% [coord]=rv(eoAST); %Calculo de coordenadas del asteroide en el sistema de ejes cartesiano (plano xy la ecliptica) 
% y0=[coord(1:3)',coord(4:6)'];

%%Elementos orbitales [EC,A(Km),INC,OM,W,TA] (angulos en rad) de 2015 TB145 en
%%la época
epoch=2457296.500000000; %Epoca donde se dan los elementos orbitales del Asteroide
[eoAST]=[8.604349428346272E-01, 2.111611234964862E+00*AU, 3.968598127764300E+01*pi/180, 3.773302760716624E+01*pi/180, 1.215374234897329E+02*pi/180, 2.234201028780358E+02*pi/180]
[coord]=rv(eoAST); %Calculo de coordenadas del asteroide en el sistema de ejes cartesiano (plano xy la ecliptica) 
y0=[coord(1:3)',coord(4:6)'];

% Tast=2*pi*sqrt(eoAST(2)^3/mus);%Periodo orbital del asteroide en la época

format long
%
%Calcular la pos de un planeta en epoch: TIERRA
EPOCH=2451545; %Epoca donde se dan los elementos orbitales del planeta (DEBE SER ANTERIOR A EPOCH)
EOTi=[0.01671123,1.00000261*AU,-0.00001531*pi/180,0*pi/180,102.93768192*pi/180,100.46457166*pi/180,-0.00004392,0.00000562*AU,-0.01294668*pi/180,0*pi/180,0.32327364*pi/180,35999.37244981*pi/180];
T=2*pi*sqrt(EOTi(2)^3/(mus));
n=2*pi/T;
DeltaT=(epoch-EPOCH);
EOt=pplanetaLP(DeltaT,EOTi);   
clear EOTi
    M=EOt(6)-EOt(5);
    M=2*pi*(M/2/pi-floor(M/2/pi));
    EOt(5)=EOt(5)-EOt(4);

    myfun = @(E,M) M-E+EOt(1)*sin(E);  
    X = fzero(@(E) myfun(E,M),0.1);
    TAepoch=2*atan(sqrt((1+EOt(1))/(1-EOt(1)))*tan(X/2));

    EOt(6)=TAepoch;    
EOTi=EOt(1:6);
%%

% %Calcular la pos de un planeta en epoch: TIERRA
% EPOCH=2457388.500000000; %Epoca donde se dan los elementos orbitales del planeta (DEBE SER ANTERIOR A EPOCH)
% EOTi=[1.670414806811060E-02,1.000000012269127*AU,2.082301851418862E-03*pi/180,1.727635015818825E+02*pi/180,2.902089575039330E+02*pi/180,3.567934146133952E+02*pi/180];
% T=2*pi*sqrt(EOTi(2)^3/(mus));
% n=2*pi/T;
% DeltaT=86400*(epoch-EPOCH);
% if DeltaT==0
% else
% EOt=pplanetaCP(DeltaT,EOTi);   
% EOTi(6)=EOt(6);
% end


%Calcular la pos de un planeta en epoch: JUPITER
EPOCH=2451545; %Epoca donde se dan los elementos orbitales del planeta (DEBE SER ANTERIOR A EPOCH)
EOJu=[0.04838624,5.20288700*AU,1.30439695*pi/180,100.47390909*pi/180,14.72847983*pi/180,34.39644051*pi/180,-0.00013253,-0.00011607*AU,-0.00183714*pi/180,0.20469106*pi/180,0.21252668*pi/180,3034.74612775*pi/180];
T=2*pi*sqrt(EOJu(2)^3/(mus));
n=2*pi/T;
DeltaT=(epoch-EPOCH);
EOt=pplanetaLP(DeltaT,EOJu); 
clear EOJu
    M=EOt(6)-EOt(5);
    M=2*pi*(M/2/pi-floor(M/2/pi));
    EOt(5)=EOt(5)-EOt(4);

    myfun = @(E,M) M-E+EOt(1)*sin(E);  
    X = fzero(@(E) myfun(E,M),0.1);
    TAepoch=2*atan(sqrt((1+EOt(1))/(1-EOt(1)))*tan(X/2));

    EOt(6)=TAepoch;    
EOJu=EOt(1:6);
%%%%

%Tiempo del paso de integración (horas)
ho=10;
%%
%Tiempo de simulación
Tint=5*365.25*86400; 
% Tint=Tast;


TIEMPO=[0:3600*ho:Tint];
L=length(TIEMPO);
TIEMPO2=[0 Tint];
TOL=1e-13;

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))

yf=y(LAST,1:6);

rf=yf(1:3)';
vf=yf(4:6)';

%Elementos orbitales del asteroide al final de la simulación.
% e=elo(rf,vf,mus)

% % % %%Representación de la órbita
% % figure (1)
% % plot3(y(:,1),y(:,2),y(:,3))
% % grid on
% % axis([-10E8 10E8 -10E8 10E8 -100E6 100E6])
% % %Representar la TIERRA
% % for i=1:LAST
% %     EOt=pplaneta(t(i),EOTi);
% %     [coord]=rv(EOt);
% %     rp(i,1:3)=coord(1:3)';
% % end
% % hold on
% % plot3(rp(:,1),rp(:,2),rp(:,3),'r')
% % %Representar JUPITER
% % for i=1:LAST
% %     EOt=pplaneta(t(i),EOJu);
% %     [coord]=rv(EOt);
% %     rp(i,1:3)=coord(1:3)';
% % end
% % hold on
% % plot3(rp(:,1),rp(:,2),rp(:,3),'g')
% % 
% % hold on
% % plot3(0,0,0,'y*')


%%POSICIÓN DE LOS PLANETAS EN CADA INSTANTE DE LA SIMULACIÓN
%TIERRA
% EOTi(3)=EOTi(3)*180/pi;
% EOTi(4)=EOTi(4)*180/pi;
% EOTi(5)=EOTi(5)*180/pi;
% EOTi(6)=EOTi(6)*180/pi;
% format shorte
% EOTi
% EOJu(3)=EOJu(3)*180/pi;
% EOJu(4)=EOJu(4)*180/pi;
% EOJu(5)=EOJu(5)*180/pi;
% EOJu(6)=EOJu(6)*180/pi;
% EOJu
% pause
 for i=1:LAST
     EOt=pplanetaCP(t(i),EOTi);
     [coord]=rv(EOt);
     rpT(i,1:3)=coord(1:3)';
 end

%JUPITER
 for i=1:LAST
     EOt=pplanetaCP(t(i),EOJu);
     [coord]=rv(EOt);
     rpJ(i,1:3)=coord(1:3)';
 end
%%
 
%Representación de la órbita completa 
figure (1)

title('Representación de la órbita simulada')
xlabel('Eje x')
ylabel('Eje y')
zlabel('Eje z')

hold on
plot3(0,0,0,'y*','MarkerSize',10) 
hold on
plot3(y(:,1),y(:,2),y(:,3),'LineWidth',1.7,'Color',[0,1,0.9])
grid on
axis([-10E8 10E8 -10E8 10E8 -10E8 10E8])
%Representar la TIERRA
hold on
plot3(rpT(:,1),rpT(:,2),rpT(:,3),'LineWidth',1.7,'Color',[0,0.1,1])
%Representar JUPITER
hold on
plot3(rpJ(:,1),rpJ(:,2),rpJ(:,3),'LineWidth',1.7,'Color',[1,0.1,0.1])
legend('Sol','Asteroide','Tierra','Jupiter')
    %Representar el nodo ascendente/descendente
    zant=y(1,3); %ASTEROIDE
    for j=2:LAST
        if y(j,3)*zant<0
            hold on
            plot3(y(j,1),y(j,2),y(j,3),'*','Color',[0,0.8,0.6],'MarkerSize',4)
        end
        zant=y(j,3);
    end
    
    zant=rpT(1,3); %TIERRA
    for j=2:LAST
        if rpT(j,3)*zant<0
            hold on
            plot3(rpT(j,1),rpT(j,2),rpT(j,3),'*','Color',[0,0,1],'MarkerSize',4)
        end
        zant=rpT(j,3);
    end
    %%%
 
 
 
%%
% %%%%%%%%%%%%Hacer animación
% %%
% figure (2)
% plot3(0,0,0,'y*')
% grid on
%  axis([-10E8 10E8 -10E8 10E8 -10E8 10E8])
%  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))
%  hold on
%  plot3(rpT(1,1),rpT(1,2),rpT(1,3),'r')
%  hold on
%  plot3(rpJ(1,1),rpJ(1,2),rpJ(1,3),'g')
%  legend('Sol','Asteroide','Tierra','Jupiter')
%  for i=2:LAST
%  %%Representación de la órbita
%  hold on
%  plot3(y(i,1),y(i,2),y(i,3))
%  %Representar la TIERRA
%  hold on
%  plot3(rpT(i,1),rpT(i,2),rpT(i,3),'r')
%  %Representar JUPITER
%  hold on
%  plot3(rpJ(i,1),rpJ(i,2),rpJ(i,3),'g')
%  pause(0.00001)
%  end
% load('elreal.mat');
% Ereal=[ereal areal*AU increal*pi/180 omreal*pi/180 wreal*pi/180 tareal*pi/180];
% clear ereal areal increal omreal wreal tareal
% load('elrealTi.mat');
% ErealTi=[ereal areal*AU increal*pi/180 omreal*pi/180 wreal*pi/180 tareal*pi/180];
% 
% rsim=y(:,1:3); %posición del asteroide segun la simulación
% 
% rreal=[];
% for i=1:LAST
%     [coord]=rv(Ereal(i,:));
%      rreal(i,1:3)=coord(1:3)';
% end
% 
% rpTreal=[];
% for i=1:LAST
%     [coord]=rv(ErealTi(i,:));
%      rpTreal(i,1:3)=coord(1:3)';
% end
% 
% 
% % rreal(1:3,:)
% % rsim(1:3,:)
% 
% instT=[1:1:length(t)];
% 
% % figure(3) %error absoluto en z
% % plot(instT,abs(rreal(:,3)-rsim(:,3)))
% % 
% % figure(4) %error realtivo en z
% % plot(instT,abs(rreal(:,3)-rsim(:,3))/abs(rreal(:,3))*100)
% % 
% % figure(5) %error absoluto en y
% % plot(instT,abs(rreal(:,2)-rsim(:,2)))
% % 
% % figure(6) %error realtivo en y
% % plot(instT,abs(rreal(:,2)-rsim(:,2))/abs(rreal(:,2))*100)
% % 
% % figure(7) %error absoluto en x
% % plot(instT,abs(rreal(:,1)-rsim(:,1)))
% % 
% % figure(8) %error realtivo en x
% % plot(instT,abs(rreal(:,1)-rsim(:,1))/abs(rreal(:,1))*100)
% 
% Rreal=[]
% dif=[]
% DisT=[]
% DT=[]
% 
% 
% for i=1:length(t)
% Rreal(i)=norm(rreal(i,1:3));
% end
% 
% for i=1:length(t)
% dif(i)=norm(rreal(i,1:3)-rsim(i,1:3));
% end
% 
% for i=1:length(t)
% DisT(i)=norm(rsim(i,1:3)-rpT(i,1:3));
% end
% 
% for i=1:length(t)
% DT(i)=norm(rpT(i,1:3)-rpTreal(i,1:3));
% end
% 
% 
% 
% 
% figure(9) %error absoluto en r
% plot(instT,dif,'LineWidth',2,'Color',[0,0.7,0.9])
% grid on
% title('Distancia entre el asteroide simulado y el real')
% 
% figure(10) %error realtivo en r
% plot(instT,dif./Rreal*100,'LineWidth',2,'Color',[0,0.7,0.9])
% grid on
% title('Distancia relativa a la distancia al Sol del asteroide real (%)')
% 
% figure(11) %error realtivo en r
% plot(instT,DisT,'LineWidth',2,'Color',[0,0.4,1])
% grid on
% title('Distancia entre el asteroide simulado y la Tierra')
% %%%%%%%%%%%Representación de la órbita completa 
% figure (12)
% 
% title('Representación de la órbita simulada')
% xlabel('Eje x')
% ylabel('Eje y')
% zlabel('Eje z')
% 
% hold on
% plot3(0,0,0,'y*') 
% hold on
% plot3(y(:,1),y(:,2),y(:,3))
% hold on
% plot3(rreal(:,1),rreal(:,2),rreal(:,3),'m')
% grid on
% axis([-10E8 10E8 -10E8 10E8 -10E8 10E8])
% %Representar la TIERRA
% hold on
% plot3(rpT(:,1),rpT(:,2),rpT(:,3),'r')
% %Representar JUPITER
% hold on
% plot3(rpJ(:,1),rpJ(:,2),rpJ(:,3),'g')
% legend('Sol','AsteroideSIM','AsteroideREAL','Tierra','Jupiter')
%     %Representar el nodo ascendente/descendente
%     zant=y(1,3); %ASTEROIDE
%     for j=2:LAST
%         if y(j,3)*zant<0
%             hold on
%             plot3(y(j,1),y(j,2),y(j,3),'b*')
%         end
%         zant=y(j,3);
%     end
%     zant=rreal(1,3); %ASTEROIDE
%     for j=2:LAST
%         if rreal(j,3)*zant<0
%             hold on
%             plot3(rreal(j,1),rreal(j,2),rreal(j,3),'m*')
%         end
%         zant=rreal(j,3);
%     end
%     
%     zant=rpT(1,3); %TIERRA
%     for j=2:LAST
%         if rpT(j,3)*zant<0
%             hold on
%             plot3(rpT(j,1),rpT(j,2),rpT(j,3),'r*')
%         end
%         zant=rpT(j,3);
%     end
%     %%%
%     
%     %
% % %%%%%%%%%%%%Hacer animación
% % %%
% % figure (2)
% % plot3(0,0,0,'y*')
% % grid on
% %  axis([-10E8 10E8 -10E8 10E8 -10E8 10E8])
% %  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))
% %  hold on
% %  plot3(rreal(1,1),rreal(1,2),rreal(1,3),'m')
% %  hold on
% %  plot3(rpT(1,1),rpT(1,2),rpT(1,3),'r')
% %  hold on
% %  plot3(rpJ(1,1),rpJ(1,2),rpJ(1,3),'g')
% %  legend('Sol','AsteroideSIM','AsteroideREAL','Tierra','Jupiter')
% %  for i=2:LAST
% %  %%Representación de la órbita
% %  hold on
% %  plot3(y(i,1),y(i,2),y(i,3))
% %  hold on
% %  plot3(rreal(i,1),rreal(i,2),rreal(i,3),'m')
% %  %Representar la TIERRA
% %  hold on
% %  plot3(rpT(i,1),rpT(i,2),rpT(i,3),'r')
% %  %Representar JUPITER
% %  hold on
% %  plot3(rpJ(i,1),rpJ(i,2),rpJ(i,3),'g')
% %  pause(0.000001)
% %  end
% 
% %%Comparación de las fuerzas
% for i=1:LAST
%     FT(i)=norm(mut*(rpT(i,1:3)-rreal(i,1:3))/norm(rpT(i,1:3)-rreal(i,1:3))^3-mut*rpT(i,1:3)/norm(rpT(i,1:3))^3);
% end
% 
% for i=1:LAST
%     FS(i)=norm(mus*rreal(i,1:3)/norm(rreal(i,1:3))^3);
% end
% 
% for i=1:LAST
%     FJ(i)=norm(muj*(rpJ(i,1:3)-rreal(i,1:3))/norm(rpJ(i,1:3)-rreal(i,1:3))^3-muj*rpJ(i,1:3)/norm(rpJ(i,1:3))^3);
% 
% end
% 
% figure(13)
% plot(instT,FT,'b',instT,FS,'y',instT,FJ,'g')
% title('Fuerza del sol(AMARILLA) y de la tierra(azul) y Jup (ROJA)')
% 
% figure(14)
% plot(instT,FT./FS,'LineWidth',2,'Color',[1,0.2,0.2],instT,FJ./FS,'LineWidth',2,'Color',[0.2,0.2,0.1])
% grid on
% legend('Tierra','Tierra')
% title('Fuerza de Jupiter y de la Tierra realtivas a la fuerza del Sol')
% 
% figure(15)
% plot(instT,DT,'LineWidth',2,'Color',[0,0.7,0.9])
% grid on
% title('Distancia entre la Tierra real y la simulada')
% 
% figure(16)
% plot(instT,DT./norm(rpTreal)*100,'LineWidth',2,'Color',[0,0.4,0.9])
% grid on
% title('Distancia realtiva a la distancia de la 	Tierra real al Sol (%)')

