%Program 6.4 Plotting program for three-body problem
% Inputs: time interval inter, initial conditions
% ic1,ic2,ic3 = [x0 vx0 y0 vy0 z0 vz0]
%mass= [M1,M2,M3]
% number of steps n, steps per point plotted p
% Calls a one-step method (rk4step.m)
%function z=orbit3d(inter,ic1,ic2,ic3,mass,n,p)%Uncomment to access as func.
%Example use: orbit3([0 1],[-.970, -.466, .243, -.433,0,0],[.970, -.466,...
%-.243, -.433,0,0],[0, 2*(.466)+10^(-2), 0, 2*(.433),0,0],[1 1 1],1000,1)
cla;
%MakeQTMovie ('start','orbit1.mov');

%Figure eight orbit with rotation
inter = [0 .6];
ic1=[-.970, -.466, .243, -.433,0,0];
ic2=[.970, -.466, -.243, -.433,0,0.1];
ic3=[0, 2*(.466)+10^(-2), 0, 2*(.433),0,0];
mass = [1 1 1];

n=500;
h=((inter(2)-inter(1))/n);
p=1;

x1_0=ic1(1);vx1_0=ic1(2);y1_0=ic1(3);vy1_0=ic1(4); z1_0=ic1(5);vz1_0=ic1(6);  % grab initial conds for M1
r1(1,:)=[x1_0 vx1_0 y1_0 vy1_0 z1_0 vz1_0];   % build vector for M1 (row 1)

x2_0=ic2(1);vx2_0=ic2(2);y2_0=ic2(3);vy2_0=ic2(4); z2_0=ic2(5);vz2_0=ic2(6);   % grab initial conds for M2
r2(1,:)=[x2_0 vx2_0 y2_0 vy2_0 z2_0 vz2_0];    % build vector for M2 (row 1)

x3_0=ic3(1);vx3_0=ic3(2);y3_0=ic3(3);vy3_0=ic3(4); z3_0=ic3(5);vz3_0=ic3(6);  % grab initial conds for M3
r3(1,:)=[x3_0 vx3_0 y3_0 vy3_0 z3_0 vz3_0];   % build vector for M3 (row 1)

t = zeros(1,n);
t(1) = inter(1); %Set starting time
vector = horzcat(r1(1,:),r2(1,:),r3(1,:));  %Combine all 3 ICs into an 18-vector

figure(1)
title('Rotating Figure-eight Orbit','FontSize',16)
view(45,32);
set(gca,'Xlim',[-2,3],'Ylim',[-2,2],'Zlim',[0,10],...
    'Drawmode','fast','Visible','on','NextPlot','add');
grid on;
xlabel('x position (units)','FontSize',14)  %print x label
ylabel('y position (units)','FontSize',14)  %print y label
zlabel('z position (units)','FontSize',14)  %print z label


mass1head=line('color','r','Marker','.','markersize',25,...
    'erase','xor','xdata',[],'ydata',[],'zdata',[]);
mass2head=line('color','g','Marker','.','markersize',25,...
    'erase','xor','xdata',[],'ydata',[],'zdata',[]);
mass3head=line('color','b','Marker','.','markersize',25,...
    'erase','xor','xdata',[],'ydata',[],'zdata',[]);
track = zeros(18,n/p);  %Initialize matrix that stores data for graph
for k=1:n/p
    for i=1:p
        t(i+1)=t(i)+h;
        vector = rk4step(t(i),vector,mass,h);
        track(:,k) = vector;
        r1(i+1,:)=vector(1:6);  %Regain position and velocity for M1
        r2(i+1,:)=vector(7:12);  %Regain position and velocity for M2
        r3(i+1,:)=vector(13:18); %Regain position and velocity for M3
    end
    r1(1,:)=r1(p+1,:);
    r2(1,:)=r2(p+1,:);
    r3(1,:)=r3(p+1,:);
    t(1)=t(p+1);
    
    
    set(mass1head,'xdata',r1(1,1),'ydata',r1(1,3),'zdata',r1(1,5))  %plot ball for Mass 1
    set(mass2head,'xdata',r2(1,1),'ydata',r2(1,3),'zdata',r2(1,5))  %plot ball for Mass 2
    set(mass3head,'xdata',r3(1,1),'ydata',r3(1,3),'zdata',r3(1,5))  %plot ball for Mass 3
 
    drawnow;
    % MakeQTMovie ('addfigure');
end

%Plot path at end
plot3(track(1,:),track(3,:),track(5,:),'r-','LineWidth',1);
plot3(track(7,:),track(9,:),track(11,:),'g-','LineWidth',1);
plot3(track(13,:),track(15,:),track(17,:),'b-','LineWidth',1);
hold off;
%MakeQTMovie ('finish');

%----------------------------------------------
%Export to .xyz (Uncomment to export)

% fileID = fopen('mydata.xyz','w'); % Create and open a file.
% for i = 1:length(track)
%     fprintf(fileID,' 3\n');
%     fprintf(fileID,'Something\n');
%     fprintf(fileID,'Au    %.8f    %.8f    %.8f\n',track(1,i),track(3,i),track(5,i));
%     fprintf(fileID,'Au    %.8f    %.8f    %.8f\n',track(7,i),track(9,i),track(11,i));
%     fprintf(fileID,'Au    %.8f    %.8f    %.8f\n',track(13,i),track(15,i),track(17,i));
% end
% fclose(fileID); % Close the file.