%  Daniel Kawano, Rose-Hulman Institute of Technology
%  Last modified:  Feb 10, 2016

function animate_poisson_top(L3, psi, theta, phi, x1, x2, x3, dt, tpause)

%  Convert dimensions and translational motion to centimeters to better 
%  display the top:

L3 = L3*100;        %  cm
x1 = x1*100;        %  cm
x2 = x2*100;        %  cm
x3 = x3*100;        %  cm

%  Create a circle for the disk representing the main body of the top:

r = L3;                                 %  cm
angle = linspace(0, 2*pi, 40)';         %  rad
circ1 = r*cos(angle);                   %  cm
circ2 = r*sin(angle);                   %  cm

%  The plane of the disk and the orientation of the top's axle vary over 
%  time according to the evolution of the corotational basis {e1,e2,e3}. 
%  Solve for how {e1,e2,e3} vary over time.  Also, keep track of the axle's
%  point of contact with the ground (point P), as well as material points 
%  on the disk's periphery (point A) and at the tip of the axle:

for k = 1:length(psi)
    R1 = [cos(psi(k)), sin(psi(k)), 0;              %  3-1-3 set of               
          -sin(psi(k)), cos(psi(k)), 0;             %  Euler angles
          0, 0, 1];
    R2 = [1, 0, 0;                               
          0, cos(theta(k)), sin(theta(k));       
          0, -sin(theta(k)), cos(theta(k))];    
    R3 = [cos(phi(k)), sin(phi(k)), 0;           
          -sin(phi(k)), cos(phi(k)), 0;          
          0, 0, 1];
    e1(:,k) = ([1, 0, 0]*(R3*R2*R1))';                      %  e1
    e2(:,k) = ([0, 1, 0]*(R3*R2*R1))';                      %  e2        
    e3(:,k) = ([0, 0, 1]*(R3*R2*R1))';                      %  e3
    xcirc(:,k) = x1(k) + circ1*e1(1,k) + circ2*e2(1,k);     %  cm
    ycirc(:,k) = x2(k) + circ1*e1(2,k) + circ2*e2(2,k);     %  cm
    zcirc(:,k) = x3(k) + circ1*e1(3,k) + circ2*e2(3,k);     %  cm
    xP(k,1) = x1(k) - L3*e3(1,k);                           %  cm
    yP(k,1) = x2(k) - L3*e3(2,k);                           %  cm
    zP(k,1) = x3(k) - L3*e3(3,k);                           %  cm
    xA(k,1) = x1(k) + r*e2(1,k);                            %  cm
    yA(k,1) = x2(k) + r*e2(2,k);                            %  cm
    zA(k,1) = x3(k) + r*e2(3,k);                            %  cm
    xT(k,1) = xP(k) + (3*L3)*e3(1,k);                       %  cm
    yT(k,1) = yP(k) + (3*L3)*e3(2,k);                       %  cm
    zT(k,1) = zP(k) + (3*L3)*e3(3,k);                       %  cm
end

%  Set up the figure window:

figure
set(gcf, 'color', 'w')
plot3(x1(1), x2(1), x3(1));
xlabel('\itx\rm_{1} (cm)')
set(gca, 'xdir', 'reverse')
ylabel('\itx\rm_{2} (cm)')
set(gca, 'ydir', 'reverse')
zlabel('\itx\rm_{3} (cm)            ', 'rotation', 0)
axis equal
xlim([1.2*min(min(min(xcirc)), min(xT)), 1.2*max(max(max(xcirc)), max(xT))])
ylim([1.2*min(min(min(ycirc)), min(yT)), 1.2*max(max(max(ycirc)), max(yT))])
zlim([min(1.2*min(zT), 0), 1.2*max(zT)])
grid on

%  Trace out the path of the axle's contact point P.  Also, orient the 
%  top's disk and axle appropriately, and highlight a material point A on 
%  the disk's periphery: 

path = line('xdata', xP(1:1), 'ydata', yP(1:1), 'zdata', zP(1:1), 'color', 'b', 'linewidth', 2);
axle = line('xdata', [xP(1), xT(1)], 'ydata', [yP(1), yT(1)], 'zdata', [zP(1), zT(1)], 'color', 'k', 'linewidth', 2);
disk = patch('xdata', xcirc(:,1), 'ydata', ycirc(:,1), 'zdata', zcirc(:,1), 'facecolor', [1, 0.8, 0.6], 'linewidth', 2);
pointA = line('xdata', xA(1), 'ydata', yA(1), 'zdata', zA(1), 'marker', 'o', 'color', 'r', 'markerfacecolor', 'r', 'linewidth', 3);

%  Animate the top's motion by updating the figure with its current 
%  location and orientation:

pause

% animation = VideoWriter('poisson-top.avi');
% animation.FrameRate = 1/dt;
% open(animation);

for k = 1:length(xT)
    set(path, 'xdata', xP(1:k), 'ydata', yP(1:k), 'zdata', zP(1:k));
    set(axle, 'xdata', [xP(k), xT(k)], 'ydata', [yP(k), yT(k)], 'zdata', [zP(k), zT(k)]);
    set(disk, 'xdata', xcirc(:,k), 'ydata', ycirc(:,k), 'zdata', zcirc(:,k));
    set(pointA, 'xdata', xA(k), 'ydata', yA(k), 'zdata', zA(k));
    drawnow
    % writeVideo(animation, getframe(gcf));
    pause(tpause)
end 

% close(animation);