% Plotting program for two-body problem 
% Inputs: time interval inter, initial conditions
% ic = [x1 vx1 y1 vy1 x2 vx2 y2 vy2], x position, x velocity, y pos, y vel,
% number of steps n, steps per point plotted p
% Calls a one-step method such as trapstep.m
% Example usage: orbit_3body([0 100],[2 0.2 2 -0.2 0 0 0 0 -2 -0.2 -2 0.2],10000,5)

function z=orbit_3body(inter,ic,n,p)
inter = [0 100];
n = 10000;
p = 5;
h=(inter(2)-inter(1))/n;        % plot n points 

%ic = [2,2,.2,-.2,0,0,0,0,-2,-2,-.2,.2]; %init cond for part 10
ic = [-.97,.243,-.466,-.433,.97,-.243,-.466,-.433,0,0,2*.466,2*.433];


x1=ic(1);y1=ic(2);vx1=ic(3);vy1=ic(4);   % initial conds for m1
x2=ic(5);y2=ic(6);vx2=ic(7);vy2=ic(8);   % initial conds for m2
x3=ic(9);y3=ic(10);vx3=ic(11);vy3=ic(12);   % initial conds for m3

y(1,:)=[x1 vx1 y1 vy1 x2 vx2 y2 vy2 x3 vx3 y3 vy3];t(1)=inter(1);    % build y vector

%for z = 1:5
%    figure(z)
    set(gca,'XLim',[-5 5],'YLim',[-5 5],...
    'XTick',[-5 0 5],'YTick',[-5 0 5]);

    head1=animatedline('color','r','Marker','.','markersize',35);
    tail1=animatedline('color','b','LineStyle','-');
    head2=animatedline('color','g','Marker','.','markersize',35);
    tail2=animatedline('color','b','LineStyle','-');
    head3=animatedline('color','g','Marker','.','markersize',35);
    tail3=animatedline('color','b','LineStyle','-');
    %y(1,10) = vx3+10^(-5);
    for k=1:n/p
        for i=1:p
            t(i+1)=t(i)+h;
            y(i+1,:)=rk4(t(i),y(i,:),h);
        end
    y(1,:)=y(p+1,:);t(1)=t(p+1);
    clearpoints(head1);addpoints(head1,y(1,1),y(1,3))
    addpoints(tail1,y(1,1),y(1,3))
    clearpoints(head2);addpoints(head2,y(1,5),y(1,7))
    addpoints(tail2,y(1,5),y(1,7))
    clearpoints(head3);addpoints(head3,y(1,9),y(1,11))
    addpoints(tail3,y(1,9),y(1,11))
    drawnow;
    end;
%end;

function y=rk4(t,x,h)
s1=ydot(t,x);
s2=ydot(t+h/2,x+h*s1/2);
s3=ydot(t+h/2,x+h*s2/2);
s4=ydot(t+h,x+h*s3);
y=x+h*(s1+2*s2+2*s3+s4)/6;

function z = ydot(t,x)
m1=1;m2=1;m3=1;g=1;
mg1=m1*g;mg2=m2*g;mg3=m3*g;
px1=x(1);vx1=x(2);py1=x(3);vy1=x(4);
px2=x(5);vx2=x(6);py2=x(7);vy2=x(8);
px3=x(9);vx3=x(10);py3=x(11);vy3=x(12);

dist=sqrt((px2-px1)^2+(py2-py1)^2);
dist2=sqrt((px3-px1)^2+(py3-py1)^2);
dist3=sqrt((px3-px2)^2+(py3-py2)^2);

z=zeros(1,12);

z(1)=vx1;
z(2)=(mg2*(px2-px1))/(dist^3)+(mg3*(px3-px1))/(dist2^3);
z(3)=vy1;
z(4)=(mg2*(py2-py1))/(dist^3)+(mg3*(py3-py1))/(dist2^3);

z(5)=vx2;
z(6)=(mg1*(px1-px2))/(dist^3)+(mg3*(px3-px2))/(dist3^3);
z(7)=vy2;
z(8)=(mg1*(py1-py2))/(dist^3)+(mg3*(py3-py2))/(dist3^3);

z(9)=vx3;
z(10)=(mg1*(px1-px3))/(dist2^3)+(mg2*(px2-px3))/(dist3^3);
z(11)=vy3;
z(12)=(mg1*(py1-py3))/(dist2^3)+(mg2*(py2-py3))/(dist3^3);
