%Program 6.4 Plotting program for one-body problem 
% Inputs: time interval inter, initial conditions
% ic = [x0 vx0 y0 vy0], x position, x velocity, y pos, y vel, and so on for
% more than one particle
%icM=[mass1 mass2 ... massN]
% number of steps n, steps per point plotted p
% Calls a one-step method such as trapstep.m
% Example usage: orbit2016([0 100],[0 1 2 0],10000,5)

%One Body Problem:
%Project3([0 100],[0 1 2 0],?,10000,5)

%Question 9: Use the following call:
%Project3([0 100],[2 .2 2 -.2 0 -.02 0 .02],[.03 .3],10000,5)

%Question 10A:
%Project3([0,100],[2 .2 2 -.2 0 0 0 0 -2 -.2 -2 .2],[.03 .3 .03] ,10000, 5)

%Question 10B:
%Project3([0,200],[2 .20001 2 -.2 0 0 0 0 -2 -.2 -2 .2],[.03 .3 .03], 10000, 5)

%Question 11A:
%Project3([0,100],[-.970 -.466 .243 -.433 .970 -.466 -.243 -.433 0 -2*-.466 0 -2*-.433],[1 1 1] ,10000, 5)

function z=Project3(inter,ic,icM,n,p)
len=length(ic);
switch len
    case 4
        x0=ic(1);vx0=ic(2);y0=ic(3);vy0=ic(4);
        y(1,:)=[x0 vx0 y0 vy0];
    case 8
        x0=ic(1);vx0=ic(2);y0=ic(3);vy0=ic(4);
        x1=ic(5);vx1=ic(6);y1=ic(7);vy1=ic(8);
        y(1,:)=[x0 vx0 y0 vy0 x1 vx1 y1 vy1];   
    case 12
        x0=ic(1);vx0=ic(2);y0=ic(3);vy0=ic(4); 
        x1=ic(5);vx1=ic(6);y1=ic(7);vy1=ic(8);
        x2=ic(9);vx2=ic(10);y2=ic(11);vy2=ic(12);
        y(1,:)=[x0 vx0 y0 vy0 x1 vx1 y1 vy1 x2 vx2 y2 vy2];
    otherwise
        z='Not enough arguments, please enter more';
        return
end
h=(inter(2)-inter(1))/n;        % plot n points 
t(1)=inter(1);    
set(gca,'XLim',[-5 5],'YLim',[-5 5],'XTick',[-5:1:5],'YTick',[-5:1:5]);
switch len
    case 12
        head=animatedline('color',[0 .5 0],'Marker','.','markersize',35);
        tail=animatedline('color',[0 .5 0],'LineStyle','-');
        head1=animatedline('color','r','Marker','.','markersize',35);
        tail1=animatedline('color','r','LineStyle','-');
        head2=animatedline('color','b','Marker','.','markersize',35);
        tail2=animatedline('color','b','LineStyle','-');
    otherwise
        head=animatedline('color',[0 .5 0],'Marker','.','markersize',35);
        tail=animatedline('color',[0 .5 0],'LineStyle','-');
        head1=animatedline('color','r','Marker','.','markersize',35);
        tail1=animatedline('color','r','LineStyle','-');
end
%[px,py]=ginput(1);                % include these three lines
%[px1,py1]=ginput(1);              % to enable mouse support
%y(1,:)=[px px1-px py py1-py];     % 2 clicks set direction
for k=1:n/p
  for i=1:p
    t(i+1)=t(i)+h;
    y(i+1,:)=trapstep(t(i),y(i,:),h);
  end
  y(1,:)=y(p+1,:);t(1)=t(p+1);
  switch len
      case 12
          clearpoints(head);addpoints(head,y(1,1),y(1,3))
          addpoints(tail,y(1,1),y(1,3))
          clearpoints(head1);addpoints(head1,y(1,5),y(1,7))
          addpoints(tail1,y(1,5),y(1,7))
          clearpoints(head2);addpoints(head2,y(1,9),y(1,11))
          addpoints(tail2,y(1,9),y(1,11))
      otherwise
          clearpoints(head);addpoints(head,y(1,1),y(1,3))
          addpoints(tail,y(1,1),y(1,3))
          clearpoints(head1);addpoints(head1,y(1,5),y(1,7))
          addpoints(tail1,y(1,5),y(1,7))
  end
  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)
%one step of the Trapezoid Method
switch len
    case 4
        z1=ydot(t,x);
        g=x+h*z1;
        z2=ydot(t+h,g);
        y=x+h*(z1+z2)/2;
    case 8
        z1=ydot(t,x);
        g=x+h*z1;
        z2=ydot(t+h,g);
        y=x+h*(z1+z2)/2;
    case 12
        z1=ydot(t,x);
        g=x+h*z1;
        z2=ydot(t+h,g);
        y=x+h*(z1+z2)/2;
    otherwise
        return
end
end

function z=ydot(t,x)
    switch len
        case 4
            m2=icM(1);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);
        case 8
            m1=icM(1);m2=icM(2);g=1;mg2=m2*g;mg1=m1*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);
            
            deltaX=px1-px2; deltaY=py1-py2;
            dist=sqrt((deltaX)^2+(deltaY)^2);
            z=zeros(1,8);
            
            z(1)=vx1;
            z(2)=(mg2*-1*deltaX)/(dist^3);
            z(3)=vy1;
            z(4)=(mg2*-1*deltaY)/(dist^3);
            
            z(5)=vx2;
            z(6)=mg1*deltaX/(dist^3);
            z(7)=vy2;
            z(8)=mg1*deltaY/(dist^3);
            
        case 12
            z=zeros(1,12);
            m1=icM(1);m2=icM(2);m3=icM(3);g=1;mg2=m2*g;mg1=m1*g;mg3=m3*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);
            px3=x(9);py3=x(11);vx3=x(10);vy3=x(12);

            deltaX12=px1-px2; deltaY12=py1-py2;
            deltaX13=px1-px3;deltaY13=py1-py3;
            deltaX23=px2-px3;deltaY23=py2-py3;

            dist12=sqrt((deltaX12)^2+(deltaY12)^2);
            dist13=sqrt((deltaX13)^2+(deltaY13)^2);
            dist23=sqrt((deltaX23)^2+(deltaY23)^2);

            z(1)=vx1;
            z(2)=(mg2*-1*deltaX12/(dist12^3))+(mg3*-1*deltaX13/(dist13^3));
            z(3)=vy1;
            z(4)=(mg2*-1*deltaY12/(dist12^3))+(mg3*-1*deltaY13/(dist13^3));

            z(5)=vx2;
            z(6)=(mg1*deltaX12/(dist12^3))+(mg3*-1*deltaX23/(dist23^3));
            z(7)=vy2;
            z(8)=(mg1*deltaY12/(dist12^3))+(mg3*-1*deltaY23/(dist23^3));

            z(9)=vx3;
            z(10)=(mg2*deltaX23/(dist23^3))+(mg1*deltaX13/(dist13^3));
            z(11)=vy3;
            z(12)=(mg2*deltaY23/(dist23^3))+(mg1*deltaY13/(dist13^3));
    end
end
end