clear
clf

global mEarth mMoon mSatellite mRocket G
mEarth    = 6e24;
mMoon = .012*mEarth;
mSatellite = 15000;
mRocket = 15000;
G       = 6.63e-11;

% initial positions of moon, earth, apollo, rocket
rxm0 = 4e8;
rym0 = 0;
rzm0 = 0;
rxe0 = 0;
rye0 = 0;
rze0 = 0;
rxs0 = -6400e3;
rys0 = 0;
rzs0 = 0;
rxr0 = -6400e3;
ryr0 = 0;
rzr0 = 0;

%magnitude of moons distance from earth
R=sqrt((rxm0-rxe0)^2 + (rym0-rye0)^2 + (rzm0-rze0)^2);

%center of mass
rcm = (mMoon*[rxm0 rym0 rzm0] + mEarth*[rxe0 rye0 rze0])/(mMoon + mEarth);

%new positions based off center of mass
rxmn0 = rxm0 - rcm(1);
rymn0 = rym0 - rcm(2);
rzmn0 = rzm0 - rcm(3);
rxen0 = rxe0 - rcm(1);
ryen0 = rye0 - rcm(2);
rzen0 = rze0 - rcm(3);
rxsn0 = rxs0 - rcm(1);
rysn0 = rys0 - rcm(2);
rzsn0 = rzs0 - rcm(3);
rxrn0 = rxr0 - rcm(1);
ryrn0 = ryr0 - rcm(2);
rzrn0 = rzr0 - rcm(3);

% angular velocity
w = sqrt((G*(mMoon + mEarth))/R^3);

% v= w*r, where r is the distance from the center of mass to the object
rmd = sqrt(rxmn0^2 + rymn0^2 + rzmn0^2);
red = sqrt(rxen0^2 + ryen0^2 + rzen0^2);


% initial velocities
vxm0 = 0;
vym0 = w*rmd;
vzm0 = 0;
vxe0 = 0;
vye0 = 0;
vze0 = 0;
vxs0 = -9000;
vys0 = 0;
vzs0 = 0;
vxr0 = -9000;
vyr0 = 0;
vzr0 = 0;


Y = [rxmn0;rymn0;rzmn0;vxm0;vym0;vzm0;rxen0;ryen0;rzen0;vxe0;vye0;vze0;rxsn0;rysn0;rzsn0;vxs0;vys0;vys0;rxrn0; ryrn0; rzrn0; vxr0; vyr0; vzr0];

% position vectors of moon, earth, and apollo
rsat=[Y(13) Y(14) Y(15)];
rmoon=[Y(1) Y(2) Y(3)];
rearth=[Y(7) Y(8) Y(9)];

t    =  0.0;
Period= sqrt(4*pi*pi*R^3/(G*(mMoon+mEarth)));
tmax =  Period;
dt   =  300;
n    = 1970;

satellitedetach=dt*12; % Time when apollo detaches from service module
translunar=dt*100; % Time when Apollo thrusts toward moon
lunarorbit=dt*896; % Time when Apollo brakes into moon orbit
transearth=dt*1075; % Time when Apollo thrusts toward Earth
earthorbit= dt*1819; % Time when Apollo brakes into Earth orbit

tt = zeros(n+1,1);
Yt = zeros(n+1,size(Y,1));
tt(1)   = t;
Yt(1,:) = Y';
f=0;

for i=1:n
    
    if dt*i == satellitedetach
        Y(16)=0;
        Y(17)= -4500;
    end
    if dt*i == translunar
        Y(16)= Y(16) + 1450;
        Y(17)= Y(17) - 1750;
    end
    if dt*i == lunarorbit
        dsm=norm(rsat-rmoon);
        wsm = sqrt((G*(mSatellite + mMoon))/dsm^3);
        
        Y(16)= wsm*dsm;
        Y(17)= wsm*dsm;
    end
    if dt*i == transearth
        Y(16)= Y(16)+600;
        Y(17)= Y(17)-550;
    end
    if dt*i == earthorbit
        dse= norm(rearth-rsat);
        wse= sqrt((G*(mSatellite + mEarth))/dse^3);
        Y(16)= 0;
        Y(17)= -.4*wse*dse;
    end
    
    
    f1 = Apollofunc(t     , Y                );
    f2 = Apollofunc(t+dt/2, Y+f1 .*  (dt/2)  );
    f3 = Apollofunc(t+dt/2, Y+f2 .*  (dt/2)  );
    f4 = Apollofunc(t+dt  , Y+f3 .*   dt     );
    
    Y = Y + (f1 + f2.*2 + f3.*2 + f4) .* (dt/6);
    
    
    if mod(i,15)==0
        
        if i < 177
            whitebg(gcf,'k')
            plot3(Y(1), Y(2), Y(3),'w.','MarkerSize',10)
            hold on
            plot3(Y(7), Y(8),Y(9), 'b.','MarkerSize',180)
            hold on
            if i < 28
                plot3(Y(19), Y(20), Y(21), 'r<','Markersize',7.5)
            end
            hold on
            plot3(Y(13), Y(14),Y(15),  'g*','Markersize',5)
            hold off
            
            axis([-.4e8 .4e8 -.4e8 .4e8 -1e6 1e6])
        else
            
            plot3(Y(1), Y(2), Y(3),'w.','MarkerSize',10)
            hold on
            plot3(Y(7), Y(8),Y(9), 'b.','MarkerSize',60)
            hold on
            if i < 28
                plot3(Y(19), Y(20), Y(21), 'r<','Markersize',10)
            end
            hold on
            plot3(Y(13), Y(14),Y(15),  'g*','Markersize',5)
            
            whitebg(gcf,'k')
            axis([-.1e9 .4e9 -.1e9 .35e9 -1e7 1e7])
        end
        
        f=f+1;
        set(gcf, 'InvertHardCopy', 'off');
        saveas(gcf, ['Apollo_',sprintf('%03d',f),'.png'])
        pause(0.01)
        
        hold off
    end
    
    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
end

