%Program 6.4 Plotting program for one-body problem 
% Inputs: int=[a b] time interval, initial conditions
% ic = [x0 vx0 y0 vy0], x position, x velocity, y pos, y vel,
% h = stepsize, p = steps per point plotted
% Calls a one-step method such as trapstep.m
% Example usage: orbit([0 100],[0 1 2 0],0.01,5)
function z=orbit_three_body(int,ic,h,p)
n=round((int(2)-int(1))/(p*h));        % plot n points 
x0=ic(1);vx0=ic(2);y0=ic(3);vy0=ic(4); % grab initial conds
y(1,:)=[x0 vx0 y0 vy0 ic(5) ic(6) ic(7) ic(8) ic(9) ic(10) ic(11) ic(12)];t(1)=int(1);    % build y vector
set(gca,'XLim',[-5 5],'YLim',[-5 5],'XTick',[-5 0 5],'YTick',...
  [-5 0 5],'Drawmode','fast','Visible','on','NextPlot','add');
cla;
head=line('color','r','Marker','.','markersize',25,...
  'erase','xor','xdata',[],'ydata',[]);
tail=line('color','b','LineStyle','-','erase','none',...
  'xdata',[],'ydata',[]);
head2=line('color','r','Marker','.','markersize',25,...
  'erase','xor','xdata',[],'ydata',[]);
tail2=line('color','b','LineStyle','-','erase','none',...
  'xdata',[],'ydata',[]);
head3=line('color','r','Marker','.','markersize',25,...
  'erase','xor','xdata',[],'ydata',[]);
tail3=line('color','b','LineStyle','-','erase','none',...
  'xdata',[],'ydata',[]);
%[px,py,button]=ginput(1);         % include these three lines
%[px1,py1,button]=ginput(1);       % to enable mouse support
%y(1,:)=[px px1-px py py1-py];     % 2 clicks set direction
for k=1:n
  for i=1:p
    t(i+1)=t(i)+h;
    %y(i+1,:)=eulerstep(t(i),y(i,:),h);
    y(i+1,:)=trapstep(t(i),y(i,:),h);
  end
  y(1,:)=y(p+1,:);t(1)=t(p+1);
  
  % Body 1
  set(head,'xdata',y(1,1),'ydata',y(1,3))
  set(tail,'xdata',y(2:p,1),'ydata',y(2:p,3))
  
  % Body 2
  set(head2,'xdata',y(1,5),'ydata',y(1,7))
  set(tail2,'xdata',y(2:p,5),'ydata',y(2:p,7))
  
  % Body 3
  set(head3,'xdata',y(1,9),'ydata',y(1,11))
  set(tail3,'xdata',y(2:p,9),'ydata',y(2:p,11))
  drawnow;
end

function y=eulerstep(t,x,h)
%one step of the Euler method
y=x+h*ydot(t,x);

function y=trapstep(t, x, h)
z1=ydot(t, x);
g=x+h*z1;
z2=ydot(t+h, g);
y=x+h*(z1+z2)/2;

function z = ydot(t,x)

g=1;      % Strength of Gravity
m1=0.03;  % Mass 1 (Kg)
m2=0.3;   % Mass 2 (Kg)
m3=0.03;  % Mass 3 (Kg)
mg1=m1*g;
mg2=m2*g;
mg3=m3*g;

px1=x(1);py1=x(3);
px2=x(5);py2=x(7);
px3=x(9);py3=x(11);

vx1=x(2);vy1=x(4);
vx2=x(6);vy2=x(8);
vx3=x(10);vy3=x(12);

% Distances between masses
dist_2to1=sqrt((px2-px1)^2+(py2-py1)^2);
dist_3to1=sqrt((px3-px1)^2+(py3-py1)^2);
dist_3to2=sqrt((px3-px2)^2+(py3-py2)^2);

z=zeros(1,12);

% Update Mass 1
z(1)=vx1;
z(2)=(mg2*(px2-px1))/(dist_2to1^3) + (mg3*(px3-px1))/(dist_3to1^3);
z(3)=vy1;
z(4)=(mg2*(py2-py1))/(dist_2to1^3) + (mg3*(py3-py1))/(dist_3to1^3);

% Update Mass 2
z(5)=vx2;
z(6)=(mg1*(px1-px2))/(dist_2to1^3) + (mg3*(px3-px2))/(dist_3to2^3);
z(7)=vy2;
z(8)=(mg1*(py1-py2))/(dist_2to1^3) + (mg3*(py3-py2))/(dist_3to2^3);

%Update Mass 3
z(9)=vx3;
z(10)=(mg1*(px1-px3))/(dist_3to1^3) + (mg2*(px2-px3))/(dist_3to2^3);
z(11)=vy3;
z(12)=(mg1*(py1-py3))/(dist_3to1^3) + (mg2*(py2-py3))/(dist_3to2^3);


function z = ydot_one(t,x)
m2=3;g=1;mg2=m2*g;px2=0;py2=0;
px1=x(1);py1=x(3);vx1=x(2);vy1=x(4);
dist=sqrt((px2-px1)^2+(py2-py1)^2);
z=zeros(1,4);
z(1)=vx1;
z(2)=(mg2*(px2-px1))/(dist^3);
z(3)=vy1;
z(4)=(mg2*(py2-py1))/(dist^3);

% orbit_three_body([0 100000], [2 0.2 2 -0.2 0 0 0 0 -2 -0.2 -2 0.2], 0.01, 5)
% mod: orbit_three_body([0 100000], [2 0.20001 2 -0.2 0 0 0 0 -2 -0.2 -2 0.2], 0.01, 5)