close all
clear all

global mS mJ mE mA G mM mV mMe
mS = (2e+30);
mJ = (2e+27);
mA = (2.2e+14); %halley's comet
mE = (6e+24);
mM = (6.39e+23);
mV = (4.867e+24);
mMe= (3.285e+23);
G  = 6.7e-11;
% Masses and Big G
phi=5;
theta=45;
f=0;

R1 =1008330000000;%comet
R2=57910000000;  %mercury
R3=108200000000; %venus
R4=149600000000; %earth
R5=227940000000; %mars
R6=778330000000; %jupiter
theta1=6.34; %mercury inclination to invariable plane
theta2=2.19; %venus
theta3=1.57; %earth
theta4=1.67; %mars
theta5=.32;  %jupiter

rMex=R2*cosd(theta1);
rMez=R2*sind(theta1);
rVx=R3*cosd(theta2);
rVz=R3*sind(theta2);
rEx=R4*cosd(theta3);
rEz=R4*sind(theta3);
rMx=R5*cosd(theta4);
rMz=R5*sind(theta4);
rJx=R6*cosd(theta5);
rJz=R6*sind(theta5);
rAz=-R1*sind(phi);
xyR=-R1*cosd(phi);
rAx=xyR*sind(theta);
rAy=xyR*cosd(theta);

cmX=(0 + mA*rAx + mMe*rMex + mV*rVx + mE*rEx + mM*rMx + mJ*rJx)/(mS+mA+mMe+mV+mE+mM+mJ);
cmY=rAy/(mS+mA+mMe+mV+mE+mM+mJ);
cmZ=(0 + mA*rAz + mMe*rMez + mV*rVz + mE*rEz + mM*rMz + mJ*rJz)/(mS+mA+mMe+mV+mE+mM+mJ);
rCM=[cmX cmY cmZ]

rE  =[rEx, 0,rEz]-rCM;
rS  =[0 ,0 ,0]-rCM;
rJ  =[rJx, 0, rJz]-rCM;
rM  =[rMx, 0, rMz]-rCM;
rV  =[rVx, 0 ,rVz]-rCM;
rMe =[rMex, 0 ,rMez]-rCM;
rA  =[-343621826255.355 ,-224561800739.416, 13364193684.9905];%[-343621826255.355 ,-224561800739.416, 13364193684.9905];%subbed coords at time 734*dt [rAx rAy rAz]-rCM original
R   = norm(rS-rE);
vE  =[0;30000;0];
vJ  =[0;13000;0];
vA  =[9000;11000;2000];
vS  =[0;0;0];
vMe =[0;47000;0];
vV  =[0;35000;0];
vM  =[0;24000;0];

Y=[rA(1);rA(2);rA(3);vA(1);vA(2);vA(3);rS(1);rS(2);rS(3);vS(1);vS(2);vS(3);rMe(1);rMe(2);rMe(3);vMe(1);vMe(2);vMe(3);rV(1);rV(2);rV(3);vV(1);vV(2);vV(3);rE(1);rE(2);rE(3);vE(1);vE(2);vE(3);rM(1);rM(2);rM(3);vM(1);vM(2);vM(3);rJ(1);rJ(2);rJ(3);vJ(1);vJ(2);vJ(3)];
f =  0;
z =  0;
t =  0.0;
T = sqrt((4*pi^2/(G*(mS+mE)))*((norm(rS-rE))^3));
tmax =  20*T;
dt= 31622400/(365*2);
n = int64(tmax/dt); % number of steps
tt = zeros(n+1,1);
Yt = zeros(n+1,size(Y,1));
tt(1)   = t;
Yt(1,:) = Y';
w=1;
for i=0:n
    f1 = kepler_ode(t     , Y );
    f2 = kepler_ode(t+dt/2, Y+f1 .*  (dt/2));
    f3 = kepler_ode(t+dt/2, Y+f2 .*  (dt/2));
    f4 = kepler_ode(t+dt  , Y+f3 .*   dt);
    
    
    Y = Y + (f1 + f2.*2 + f3.*2 + f4) .* (dt/6);
    
    t = t+dt;
    tt(i+1)   = t;  % this records the time at each step
    Yt(i+1,:) = Y'; % this records the vector Y at each step --> Yt(:,1) has all x coordinates
    % plot3(Yt(i+1,1),Yt(i+1,2),Yt(i+1,3),'c*');
    %hold on
    plot3(Yt(i+1,7),Yt(i+1,8),Yt(i+1,9),'y*','markersize',15);
    hold on
    plot3(Yt(i+1,13),Yt(i+1,14),Yt(i+1,15),'g.')
    hold on
    plot3(Yt(i+1,19),Yt(i+1,20),Yt(i+1,21),'w.',Yt(i+1,25),Yt(i+1,26),Yt(i+1,27),'b.',Yt(i+1,31),Yt(i+1,32),Yt(i+1,33),'r.','markersize',7);
    hold on
    %plot3(Yt(i+1,37),Yt(i+1,38),Yt(i+1,39),'m.','markersize',10);
    %hold on
    plot3(Yt(:,13),Yt(:,14),Yt(:,15),'g.',Yt(:,19),Yt(:,20),Yt(:,21),'w.',Yt(:,25),Yt(:,26),Yt(:,27),'b.',Yt(:,31),Yt(:,32),Yt(:,33),'r.','markersize', 2);%,Yt(:,37),Yt(:,38),Yt(:,39),'m.'
    hold on
    %plot3(Yt(:,1),Yt(:,2),Yt(:,3),'c.','markersize',5);
    hold off
    whitebg(gcf,'k')
    axis equal
    %axis(1.0e+11 *[   -2.0000    8.0000   -1.5000    2.5000   -0.0600    0.0800]);
    %campos(1.0e+12 * [ -0.9847   -3.7885    0.0541]);
    % if i>330
    %    campos(1.0e+12 * [2.8725   -2.5414    0.6245]);
    %end
    %if i == 330
    %   for w=1:73
    %       camorbit(1,0,'data',[0 0 1]);
    %      pause(.01)
    %         z=z+1;
    %set(gcf, 'InvertHardCopy', 'off');
    %saveas(gcf,['CometSSSim_',sprintf('%03d',z),'.png'])
    %z+1;
    %z
    %  end
    % end
    %z=z+1;
    %if i>1800
     %       set(gcf, 'InvertHardCopy', 'off');
      %      saveas(gcf,['CometSSSim_',sprintf('%03d',z),'.png'])
       %     z=z+1;
    %end
    %z
    pause(.01)
end


%plot3(Yt(i+1,1),Yt(i+1,2),Yt(i+1,3),'c*');
%hold on
%plot3(Yt(i+1,7),Yt(i+1,8),Yt(i+1,9),'y*','markersize',15);
%hold on
%plot3(Yt(i+1,13),Yt(i+1,14),Yt(i+1,15),'g.')
%hold on
%plot3(Yt(i+1,19),Yt(i+1,20),Yt(i+1,21),'w.',Yt(i+1,25),Yt(i+1,26),Yt(i+1,27),'b.',Yt(i+1,31),Yt(i+1,32),Yt(i+1,33),'r.','markersize',7);
%hold on
%plot3(Yt(i+1,37),Yt(i+1,38),Yt(i+1,39),'m.','markersize',10);
%hold on
%plot3(Yt(:,7),Yt(:,8),Yt(:,9),'y--',Yt(:,13),Yt(:,14),Yt(:,15),'g.',Yt(:,19),Yt(:,20),Yt(:,21),'w.',Yt(:,25),Yt(:,26),Yt(:,27),'b.',Yt(:,31),Yt(:,32),Yt(:,33),'r.',Yt(:,37),Yt(:,38),Yt(:,39),'m.','markersize', 2);
%hold on
%plot3(Yt(:,1),Yt(:,2),Yt(:,3),'c.','markersize',5);
%hold off
%whitebg(gcf,'k')
%axis([-3.5e+11 2.5e+11 -2e+11 2.5e+11 -10e+10 5e+10]);
%campos(1.0e+12 * [2.8725   -2.5414    0.6245]);
%for w=1:120
%   camorbit(1,0,'data',[0 0 1]);
%  pause(.1)
%     z=z+1;
%    set(gcf, 'InvertHardCopy', 'off');
%   saveas(gcf,['CometSSSim_',sprintf('%03d',z),'.png'])
%  z
%convert -delay 3 CometSSSim_???.png movie3.gif

