% Program 6.4 Plotting program for two-body problem
% Inputs: time interval inter, initial conditions
% ic = [x1 vx1 y1 vy1 x2 vx2 y2 vy2], x1-x2 pos, x1-x2 vel, y1-y2 pos, y1-y2 vel
% m = [m1 m2] (masses of the three bodies)
% number of steps n, steps per point plotted p
% Outputs animated gif to fname (unless fname is ‘’)
% Case 1: orbit2b([0 100],[2 0.2 2 -0.02 0 -0.02 0 .02],[0.03 0.3], 10000, 100, 'twobodyfig.gif')
function z=orbit2b(inter,ic,m,n,p,fname)
h=(inter(2)-inter(1))/n;    % plot n points
x1=ic(1);vx1=ic(2);y1=ic(3);vy1=ic(4);  % grab initial conds for body 1
x2=ic(5);vx2=ic(6);y2=ic(7);vy2=ic(8);  % grab initial conds for body 2
y(1,:)=[x1 vx1 y1 vy1 x2 vx2 y2 vy2];t(1)=inter(1);  % build y vector    
set(gca,'XLim',[-5 5],'YLim',[-5 5],'XTick',[-5 0 5],'YTick',...
  [-5 0 5],'Drawmode','fast','Visible','on');
cla;
%sun=line('color','y','Marker','.','markersize',25,...
%  'xdata',0,'ydata',0);
drawnow;
tail1=line('color',[1,0,0],'LineStyle','-','erase','normal',...
  'xdata',[],'ydata',[],'LineWidth',2);
hist1=line('color',[1,0,0],'LineStyle',':','erase','normal',...
  'xdata',[],'ydata',[]);
tail2=line('color',[0,1,0],'LineStyle','-','erase','normal',...
  'xdata',[],'ydata',[],'LineWidth',2);
hist2=line('color',[0,1,0],'LineStyle',':','erase','normal',...
  'xdata',[],'ydata',[]);
head1=line('color',[1,0,0],'Marker','.','markersize',4 * log(m(1)) + 35,...
  'erase','normal','xdata',[],'ydata',[]);
head2=line('color',[0,1,0],'Marker','.','markersize',4 * log(m(2)) + 35,...
  'erase','normal','xdata',[],'ydata',[]);


% initialize recency line and full history data
line_len = round(50/h/p);
linedata = zeros(line_len,8);
fullhist = zeros(1,8);
for k=1:line_len
    linedata(k,:) = y(1,:);
end
fullhist(1,:) = y(1,:);

% main loop - simulate for p steps, then sa
for k=1:n/p
  for iter=1:5
    for inc=1:p/5
      i = (iter-1) * p/5 + inc;
      t(i+1) = t(i)+h;
      y(i+1,:) = rungekutta4step(t(i),y(i,:),h,m);
    end
    for i=1:(line_len-1)
      linedata(i, :) = linedata(i+1, :);
    end
    y(1,:)=y(iter*p/5+1,:);t(1)=t(iter*p/5+1);
    linedata(line_len,:) = y(1,:);
    fullhist(size(fullhist,1)+1,:)=linedata(1,:);
  end
  
  set(head1,'xdata',y(1,1),'ydata',y(1,3))
  set(head2,'xdata',y(1,5),'ydata',y(1,7))
  set(tail1,'xdata',linedata(:,1),'ydata',linedata(:,3))
  set(tail2,'xdata',linedata(:,5),'ydata',linedata(:,7))
  if k == n/p
    set(hist1,'xdata',fullhist(:,1),'ydata',fullhist(:,3))
    set(hist2,'xdata',fullhist(:,5),'ydata',fullhist(:,7))
  end
  drawnow;
 
  %gif code
  if strcmp(fname, '') == 0
    frm = getframe(1);
    img = frame2im(frm);
    [A,map] = rgb2ind(img,256);
    if k == 1 %first frame
        imwrite(A,map,fname,'gif','LoopCount',Inf,'DelayTime',1);
    elseif k == n/p %final frame
        imwrite(A,map,fname,'gif','WriteMode','append','DelayTime',1);
    else
        imwrite(A,map,fname,'gif','WriteMode','append','DelayTime', h * p / 20);
    end
  end
end

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

% function y=trapezoidstep(t,x,h)
% %one step of trapzeoid method
% y=x+h/2*(ydot(t,x) + ydot(t+h, x+h*ydot(t,x)));

function y=rungekutta4step(t,x,h,m)
% one step of RK4 method
k1 = ydot(t, x, m);
k2 = ydot(t + h/2, x + h/2 * k1, m);
k3 = ydot(t + h/2, x + h/2 * k2, m);
k4 = ydot(t + h, x + h * k3, m);
y = x + h/6 * (k1 + 2*k2 + 2*k3 + k4);

function z = ydot(t,x,m)
m1=m(1);m2=m(2);
g=1;mg1=m1*g;mg2=m2*g;
px1=x(1);py1=x(3);vx1=x(2);vy1=x(4);
px2=x(5);py2=x(7);vx2=x(6);vy2=x(8);
dist=sqrt((px2-px1)^2+(py2-py1)^2);

z=zeros(1,8);
z(1)=vx1;
z(2)=((mg2*(px2-px1))/(dist^3));
z(3)=vy1;
z(4)=((mg2*(py2-py1))/(dist^3));
z(5)=vx2;
z(6)=((mg1*(px1-px2))/(dist^3));
z(7)=vy2;
z(8)=((mg1*(py1-py2))/(dist^3));