%Program 6.3.9 Plotting program for two-body problem 
% Inputs: int=[a b] time interval, initial conditions
% ic1 = [x0 vx0 y0 vy0], x position, x velocity, y pos, y vel, Mass 1
% ic2 = [x0 vx0 y0 vy0], x position, x velocity, y pos, y vel, Mass 2
% h = stepsize (smaller=more accuracy), p=Euler IVP steps per point plotted
% Calls a one-step method such as trapstep.m (or in this case Euler Method)
% Example usage: project3Ca_orbit([0 1200],[2 .2 2 -.2],[0 -.01 0 .01], 0.001,5)
% Integer [0 100] = integer values for start / stop time interval
% xp [0 1 2 0] = x position x component of velocity
% yp                 y position y component of velocity
% h .01 = Euler Method step size h 
%  5 number of Euler IVP points calculated

function z=orbit(int,ic1,ic2,h,p)
% MakeQTMovie ('start','project3Ca_orbit.mov');
% MakeQTMovie ('framerate', 500);  %  frames per second
n=round((int(2)-int(1))/(p*h)); % plot n points
M1=.03; % Mass #1 in Kg
M2=.3; % Mass #2 in Kg

x1_0=ic1(1);vx1_0=ic1(2);y1_0=ic1(3);vy1_0=ic1(4); %Initial values for M1
y1(1,:)=[x1_0 vx1_0 y1_0 vy1_0];  %Making y1 a vector

x2_0=ic2(1);vx2_0=ic2(2);y2_0=ic2(3);vy2_0=ic2(4); %Initial values for M2
y2(1,:)=[x2_0 vx2_0 y2_0 vy2_0];  %Making y2 a vector

t(1)=int(1);                           % start time at t=time interval a
str1=('Euler''s Method applied to two-body orbital mechanics problem');
str2=('M1 (red in Kg)    M2 (yellow in Kg)');
str3=('g= 1 m^3/Kgs^2');
str4=('(15 billion times stronger than gravity on earth)');
str5=('Euler Method stepsize h =');
str=sprintf('%s\n %s\n %s\n %s\n %s %f',str1, str2, str3, str4, str5, h);
title(str);                                 %print title with line breaks

set(gca,'XLim',[-10 10],'YLim',[-10 10],'XTick',[-10:2:10],'YTick',...
  [-10:2:10],'Drawmode','fast','Visible','on','NextPlot','add');
grid on;
xlabel('x position in meters','FontSize',18)  %print x label
ylabel('y position in meters','FontSize',18)  %print y label
axis square;
cla;
mass2head=line('color','y','Marker','.','markersize',25,...
  'erase','xor','xdata',[],'ydata',[]);  %mass 2 location
mass1head=line('color','r','Marker','.','markersize',25,...
  'erase','xor','xdata',[],'ydata',[]);
mass2tail=line('color','k','LineStyle','-','erase','none',...
  'xdata',[],'ydata',[]);
mass1tail=line('color','y','LineStyle','-','erase','none',...
  'xdata',[],'ydata',[]);                % for mass1 red+yellow=cyan
drawnow;

for k=1:n
  for i=1:p
    t(i+1)=t(i)+h;                       % increment time by h seconds
    y1(i+1,:)=eulerstep(t(i),y1(i,:),y2(i,:),M2,h); %compute new data for Mass 1
    y2(i+1,:)=eulerstep(t(i),y2(i,:),y1(i,:),M1,h); %compute new data for Mass 2
   end
  y1(1,:)=y1(p+1,:);   %add Mass 1 data for p-th incremental change to y1
  y2(1,:)=y2(p+1,:);   %add Mass 2 data for p-th incremental change to y2
  t(1)=t(p+1);         %reset initial t to time for p+1 point
  set(mass1head,'xdata',y1(1,1),'ydata',y1(1,3))  %plot ball for Mass 1
  set(mass1tail,'xdata',y1(2:p,1),'ydata',y1(2:p,3)) %plot tail for Mass 1 
  set(mass2head,'xdata',y2(1,1),'ydata',y2(1,3))  %plot ball for Mass 2
  set(mass2tail,'xdata',y2(2:p,1),'ydata',y2(2:p,3)) %plot tail for Mass 2 
  drawnow;
%   MakeQTMovie ('addfigure');
%   pause(.001)
end

% MakeQTMovie ('finish');
% print -djpg project3C_a.jpg;

function y=eulerstep(t,x1,x2,mass,h)
%one step of the Euler method
y=x1+h*ydot1(t,x1,x2,mass);  %Find new V/A for M1 wrt h

%Actually turning it into a 2 object problem 
function z = ydot1(t,a,b,mass)
%New step p for V and A for M1 due to M2
%px2=0;py2=0;          
%M2 is stationary
g=1;                   % g = 1 Kgm^3/s^2
mg2=g*mass;           %g*M1*M2
px1=a(1);py1=a(3);     %x & y positions for M1 
vx1=a(2);vy1=a(4);     %x & y velocity for M1
%------------
% Comment out next two lines to for 1 object.
%------------
px2=b(1);py2=b(3);     %x & y position for M2
vx2=b(2);vy2=b(4);     %x & y velocity for M2

dist1=sqrt((px2-px1)^2+(py2-py1)^2);  %Distance between masses

z=zeros(1,4);         %init z vector for Mass a to all zeros
z(1)=vx1;             %x'=velocity of Mass a in x direction
z(2)=(mg2*(px2-px1))/(dist1^3); %vx'=accl of Mass a due to Mass b in x dir.
z(3)=vy1;             %y'=velocity of Mass a in y direction
z(4)=(mg2*(py2-py1))/(dist1^3); %vy'=accl of Mass a due to Mass b in y dir.
