function pendulumAnimation(t, x, theta, r)
%CARTANIMATION Shows animation of pendulum on a cart on a track for 
%   EE C128/ME C134. This function does not output anything, it only shows 
%   an animation of a pendulum on a cart on a rail.
%   Inputs:
%   t - time array (seconds)
%   x - position of cart (meters)
%   theta - angle of cart (radians)
%   r - reference position (only) of cart (meters) [optional]

% Check if correction number of inputs
if nargin == 3
    refFlg = false;
elseif nargin == 4
    refFlg = true;
else
    error("Error: Inputs should be (time, postion, angle, reference [optional])");
end

% Check if sizes of inputs are the same
if refFlg
    if size(t) ~= size(x)
        if size(t) ~= size(r)
            error("Error: Inputs should be the same size.");
        end
    end
else
    if size(t) ~= size(x)
        error("Error: Inputs should be the same size.");
    end
end

% Get number of frames and frame information that is needed
fps = 30;
endTime = t(end);
numFrames = floor(endTime*fps)+2;

frames(numFrames) = struct('cdata',[],'colormap',[]);

newT = [0:1/fps:endTime, endTime];
newX = interpn(t,x,newT);
newTheta = interpn(t,theta,newT);
newR = zeros(length(newT));
refCart = 0;
refPend = 0;

if refFlg
    newR = interpn(t,r,newT);
end

% Plot each frame
rLength = 1.5;
cLength = 0.1;
cHeight = 0.05;
pLength = 2*0.3302;

a = figure('Visible','off');
plot([-(rLength+cLength)/2, (rLength+cLength)/2],[0,0],'k','LineWidth',2);

if refFlg
    refCart = rectangle('Position',[-cLength/2+newR(1),0,cLength,cHeight], ...
    'FaceColor','None','EdgeColor','r');
    hold on;
    refPend = plot([newX(1),newX(1)],[cHeight/2,cHeight/2+pLength],'r', ...
        'Linewidth',1.5);
    hold off;
end

cart = rectangle('Position',[-cLength/2+newX(1),0,cLength,cHeight], ...
    'FaceColor','b','EdgeColor','None');
hold on;
pendulum = plot([newX(1), newX(1)+pLength*sin(newTheta(1))], ...
                [cHeight/2, cHeight/2+pLength*cos(newTheta(1))], ...
                'b','Linewidth',1.5);
hold off;
            
xlim([-1.25*rLength/2, 1.25*rLength/2]);
xlabel('Position (m)');
axis equal;
frames(1) = getframe(a);
fprintf("Rendering animation...\n");

parfor index = 2:numFrames
    
    if refFlg
        set(refCart,'Position',[-cLength/2+newR(index),0,cLength,cHeight]);
        set(refPend,'XData',[newX(index),newX(index)]);
    end
    
    set(cart,'Position',[-cLength/2+newX(index),0,cLength,cHeight]);
    set(pendulum,'XData',[newX(index),newX(index)+pLength*sin(newTheta(index))]);
    set(pendulum,'YData',[cHeight/2,cHeight/2+pLength*cos(newTheta(index))]);
    title(sprintf("Time: %.2f sec",newT(index)));
    frames(index) = getframe(a);
end

fprintf("Done!\n");

% Play movie
answer = 'y';
while answer == 'y'
    theFig = figure();
    movie(gcf,frames,1,fps);
    pause(0.5);
    close(theFig);
    answer = input("Play again? (y/n): ",'s');
    while (answer ~= 'n' && answer ~= 'y')
        answer = input("Invalid response, please enter 'y' or 'n': ",'s');
    end
end

end

% Author: Conrad Holda