%%% EPS 109
%%% Computer Lab/Homework 13
%%% Runge Kutta Integration to Study Horseshoe
%%% and Tadpole Orbits of Asteroids

%%%Author: Paul Gierz
%%%18.11.09

%%% Main Code

function orbit()
clear
clc
clf
%Declare as Global

global ms mp ma G

%Set Constants

ms = 2*10^30;
mp = 6*10^24;
mm = 7*10^22;
G = 7*10^-11;

%%% Sun-Planet Initials

%Initialize Positions
R_s = 140000000000;     %Initial Radius
rs_s = [0,0];           %Initial Sun
rp_s = [R_s,0];           %Initial Planet

%Compute Center of Mass and shift system
rcm_s = (rs_s .* ms + rp_s .* mp) ./ (ms+mp);
rs_s = rs_s-rcm_s;
rp_s = rp_s-rcm_s;

%Initialize Velocities
mu_s = ms*mp/(mp+ms); %Reduced Mass

w2_s = G*ms*mp/R_s^3/mu_s;
w_s = sqrt(w2_s);   %Omega
T_s = 2*pi/w_s;     %Period

%Set Initial Velocities
vs_s = w_s .* [0,rs_s(1)];
vp_s = w_s .* [0,rp_s(1)];

%Create Y Vector
y = [rs_s(1);rs_s(2);vs_s(1);vs_s(2);   %Sun R&V
    rp_s(1);rp_s(2);vp_s(1);vp_s(2)];   %Planet R&V

%%% Planet-Moon Initials

%Initial Positions
R_m = 400000000;
rp_m = rp_s;
rm_m = rp_m - [R_m,0];

%Compute Center Of Mass and Shift
rcm_m = (rp_m .* mp + rm_m .* mm) ./ (mp+mm);
rp_m = rp_m-rcm_m;
rm_m = rm_m - rcm_m;

%Initialize Velociites
mu_m = mp*mm/(mp+mm);
w2_m = G*mp*mm/R_m^3/mu_m;
w_m = sqrt(w2_m);
T_m = 2*pi/w_m;

%Set Initial Velocities
vp_m = w_m .* [0,rp_m(1)];
vm_m = w_m .* [0,rm_m(1)];

%Create Z Vector
z = [rp_m(1);rp_m(2);vp_m(1);vp_m(2);
    rm_m(1);rm_m(2);vm_m(1);vm_m(2)];
%%% Subroutine 1: Kepler ODE for 3 Objects

function dydt = kepler_ode1(t,y)

%Extract R&V from Y vector
rs_s = y(1:2);
vs_s = y(3:4);
rp_s = y(5:6);
vp_s = y(7:8);

%Distances between objects
dsp = rs_s-rp_s;

%Set Forces
Fsp = -G*ms*mp/norm(dsp)^3 .* dsp;
%Accelerations
as_s = (+Fsp) ./ ms;
ap_s = (-Fsp) ./ mp;

%Create New Vector dydt
dydt = [vs_s(1);vs_s(2);as_s(1);as_s(2);
        vp_s(1);vp_s(2);ap_s(1);ap_s(2)];
 
end

function dzdt = kepler_ode2(t,z)

%Extract R&V from Z vector
rp_m = z(1:2);
vp_m = z(3:4);
rm_m = z(5:6);
vm_m = z(7:8);

%Distances between objects
dpm = rp_m-rm_m;

%Set Forces
Fpm = -G*mm*mp/norm(dpm)^3 .* dpm;
%Accelerations
ap_m = (+Fpm) ./ mp;
am_m = (-Fpm) ./ mm;

%Create New Vector dydt
dzdt = [vp_m(1);vp_m(2);ap_m(1);ap_m(2);
        vm_m(1);vm_m(2);am_m(1);am_m(2)];
 
end

%Time Initials
t = 0.0;
tmax = T_s;
dt = T_s/1000;

%Loop to Update Positions
i=0;
f=0;

while(t<tmax)
%h1 = figure(1)
%for j = 1:1000
    
    F1 = kepler_ode1(t,y);
    F2 = kepler_ode1(t+dt/2,y+(dt/2)*F1);
    F3 = kepler_ode1(t+dt/2,y+(dt/2)*F2);
    F4 = kepler_ode1(t+dt,y+dt*F3);
    
    y = y + (F1 + F2.*2 + F3.*2 + F4) .* (dt/6);
    
    
    
    G1 = kepler_ode2(t,z);
    G2 = kepler_ode2(t+dt/2,z+(dt/2)*G1);
    G3 = kepler_ode2(t+dt/2,z+(dt/2)*G2);
    G4 = kepler_ode2(t+dt,z+dt*G3);
    
    z = z + (G1 + G2.*2 + G3.*2 + G4) .* (dt/6);
    
    %Update i, t
    i = i + 1;
    t = t + dt;

    %Store Positions
    bsx(i)=y(1);
    bsy(i)=y(2);
    bpx(i)=y(5);
    bpy(i)=y(6);
    bmx(i)=y(5)+50*z(5);
    bmy(i)=y(6)+50*z(6);
    
    subplot (2,1,1)
    plot(bsx,bsy,'r*-',bpx,bpy,'bx',bmx,bmy,'go');
    axis([-2*10^11 2*10^11 -2*10^11 2*10^11])
    axis equal
    pause (0.000001)
   %pause   
    
        %Check Direction
        s_p = [y(1) y(2)];
        p_p = [y(5) y(6)];
        m_p = [y(5)+z(5) y(6)+z(6)];
        v_sp = p_p-s_p;
        v_pm = m_p-p_p;
        direction_test = v_sp(1)*v_pm(2) - v_sp(2)*v_pm(1);
        

        cos_theta = (v_sp(1) .* v_pm(1) + v_sp(2) .* v_pm(2)) ./ (norm(v_sp) .* norm(v_pm));
        alpha = [0:2:180];
        %hold off
            subplot (2,1,2)
            if direction_test >= 0
                plot(cos_theta*sind(alpha),cosd(alpha),'ro-',-sind(alpha),cosd(alpha),'ro-')
                axis([-3, 3, -1, 1])
                %axis equal
            else
                plot(-cos_theta*sind(alpha),cosd(alpha),'-bo',sind(alpha),cosd(alpha),'bo-')
                axis([-3, 3, -1, 1])
                %axis equal
            end
            

    %saveas(h1,strcat('Moon_Phases_',num2str(999+j),'.jpg'));
    %fprintf('Frame %4d of %4d\n',j,1000);
%end

end



end