function visu(t, y, fps, slowDownFactor, fileName)

if nargin < 3
    fps = 25;
end

if nargin < 4
    slowDownFactor = 1;
end

if nargin < 5
    fileName = [];
end

% Robot parameters
prms.r = 0.075;                  % Radius of the wheel [m]
prms.m0 = 1;                     % Mass of the wheel [kg]
prms.I0 = 1/2*prms.m0*prms.r^2;  % Moment of inertia of the wheel [kg m^2]

prms.l1 = 0.2;                   % Distance from the wheel to the CoM of the first link
prms.l1bar = 0.4;                % Length of the first link
prms.m1 = 1;                     % Mass of the first link [kg]
prms.I1 = 1/12*prms.m1*(prms.l1+prms.l1bar)^2;  % Moment of inertia of the first link with respect to CoM [kg m^2]
prms.m2 = 5;                     % Mass of the second link [kg]
prms.I2 = 0;                     % Moment of inertia of the second link with respect to CoM [kg m^2]
prms.l2bar = 0.4;                % Length of the second link [m] (all mass is concentrated at the end of the link)

% Interpolate the simulated trajectory
t_sim = (0:(1/fps):t(end))';

th0_interp = interp1(t, y(:,1), t_sim);
th1_interp = interp1(t, y(:,3), t_sim);
th2_interp = interp1(t, y(:,5), t_sim);

% Calculate the x-y position of the robot
x_interp = th0_interp*prms.r;
y_interp = prms.r*ones(size(x_interp));

% Calculate position of the end of the first link
l1_pos = [x_interp - prms.l1bar*sin(th1_interp),    y_interp + prms.l1bar*cos(th1_interp)];
l2_pos = l1_pos + [-prms.l2bar*sin(th1_interp+th2_interp),  prms.l2bar*cos(th1_interp+th2_interp)];
    
% Prepare the plots
h = figure(25);
clf

% Floor
plot([-10; 10], [0;0], 'k')

hold on
th = linspace(0, 2*pi, 50);
x_circ = prms.r*sin(th);
y_circ = prms.r*cos(th);

% Wheel init
f_wheel_point = plot([0; 0], [0; 2*prms.r], 'b-');
f_wheel_circ = plot(x_circ, y_circ + prms.r, 'b');

% Link-1 init
f_link1 = plot([0;0], prms.r+[0; prms.l1bar+prms.l1bar], 'bo-');
% Link-2 init
f_link2 = plot([0;0], prms.r+prms.l1bar+[0; prms.l2bar], 'bo-');

hold off
axis equal
axis tight
xlim([min([x_interp; l1_pos(:,1); l2_pos(:,1)])-0.1, max([y_interp; l1_pos(:,1); l2_pos(:,1)])+0.1])
ylim([min([l2_pos(:,2); 0])-0.1, max([l2_pos(:,2);0])+0.1])

f_title = title('Time: ');

xlabel('x [m]')
ylabel('y [m]')

% Simulation
for i = 1:numel(t_sim)
    tic
    
    % Wheel
    f_wheel_circ.XData = x_circ + x_interp(i);
    f_wheel_circ.YData = y_circ + y_interp(i);
    f_wheel_point.XData = [0 prms.r*sin(th0_interp(i))] + x_interp(i);
    f_wheel_point.YData = [0 prms.r*cos(th0_interp(i))] + y_interp(i);
    
    % Link1
    f_link1.XData = [x_interp(i) l1_pos(i, 1)];
    f_link1.YData = [y_interp(i) l1_pos(i, 2)];
    
    % Link2
    f_link2.XData = [l1_pos(i, 1) l2_pos(i, 1)];
    f_link2.YData = [l1_pos(i, 2) l2_pos(i, 2)];
 
    % Time
    f_title.String = sprintf('Time %6.2f s', t_sim(i));
    
    % Write to the GIF File 
    if ~isempty(fileName)
        % Capture the plot as an image
        frame = getframe(h);
        im = frame2im(frame);
        [imind,cm] = rgb2ind(im,256);
        
        if i == 1
            imwrite(imind, cm, fileName,'gif', 'LoopCount', Inf, 'DelayTime',slowDownFactor/fps);
        else
            imwrite(imind, cm, fileName,'gif','WriteMode','append', 'DelayTime',slowDownFactor/fps);
        end
    end
      
    t_elapsed = toc;
    pause((slowDownFactor/fps) - t_elapsed)
end