function ipend_simulate_0(t,x,u,r)
%% 
%
%  file:   gyak7_simulate_pendulum_0.m
%  author: Peter Polcz <ppolcz@gmail.com>
%
%  Created on 2017.03.27. Monday, 13:46:12
%

global PUBLISH
if PUBLISH
    try 
        persist = evalin('base','persist'); 
    catch
        persist = pcz_persist(mfilename('fullpath'), []);
    end
end

dim = min(size(x));

% Frames per second
fps = 20;
Ts = 1/fps;

% Resampling x(t) using the interp1 function (`_rs` stands for `resampled`)
t_rs = t(1):Ts:t(end);
x_rs = interp1(t,x,t_rs);

u_rs = [];
u_t = [];
if nargin >= 3
    if isa(u,'function_handle')
        u_rs = u(t_rs);
        u_t = u(t);
    else
        u_rs = interp1(t,u,t_rs);
        u_t = u;
    end
end

r_rs = [];
r_t = [];
if nargin >= 4
    if isa(r,'function_handle')
        r_rs = r(t_rs);
        r_t = r(t);
    else
        r_rs = interp1(t,r,t_rs);
        r_t = r;
    end
end

% Split the state vector
r_rs = x_rs(:,1);
if dim == 4
    phi_rs = x_rs(:,3);
    plot_1_data = x(:,1:2);
    plot_2_data = x(:,3:4);
elseif dim == 2
    phi_rs = x_rs(:,2);
    plot_1_data = x(:,1);
    plot_2_data = x(:,2);
else
    error 'The dimension of x must be 2 or 4'
end

% Figure dimensions
l = 1;
cl = 1;
cw = 0.1;

r_init = r_rs;
x1 = r_init + l*sin(phi_rs);
y1 = l*cos(phi_rs);

fig = figure(19);
set(fig,'Position', [ 521 254 1020 407 ], 'Color', [1 1 1])

if nargin == 2
    subplot(221), hold off;
    plot(t, plot_1_data), setxlim
    title('Chart''s position and velocity'), grid on;
    
    subplot(222), hold off;
    plot(t, plot_2_data), setxlim
    title('Angle and angular velocity'), grid on;
elseif nargin >= 3
    subplot(231), hold off;
    plot(t, [ plot_1_data ]), setxlim
    title('Chart''s position and velocity'), grid on;
    
    if nargin >= 4
        hold on
        plot(t,r_t)
    end
    
    subplot(232), hold off;
    plot(t, plot_2_data), setxlim
    title('Angle and angular velocity'), grid on;

    subplot(233), hold off;
    plot(t,u_t), setxlim
    title('Input'), grid on;
end

if nargin >= 3 && ~isempty(u_rs)
    plotter = @plot_pendulum_arrow_vec;
else
    plotter = @plot_pendulum;
end

subplot(2,2,[3 4]), hold off
if ~PUBLISH
    pause(0.1)
    plotter(1);
    pause(0.5)
end

tic;
for kk = 1:length(t_rs)
    plotter(kk)
    if PUBLISH
        frames(kk) = getframe(fig);
    else
        pause(Ts - toc)
        tic
    end
end

if PUBLISH
    delete(fig)
    persist.pub_vid_write(frames)
end

    function plot_pendulum(kk)
        plot([r_init(kk)-cl/2, r_init(kk)-cl/2, r_init(kk)+cl/2, r_init(kk)+cl/2, r_init(kk)-cl/2], [-cw/2, cw/2, cw/2, -cw/2, -cw/2])
        hold on;
        plot([r_init(kk); x1(kk)],[0; y1(kk)],'b-'),
        plot(x1(kk), y1(kk), 'o')
        axis([-13 13 -2 2]), grid on;
        text(6,-1.5,sprintf('t = %g [s]', round(t_rs(kk))));
        hold off;
    end

    function plot_pendulum_arrow_vec(kk)
        plot([r_init(kk)-cl/2, r_init(kk)-cl/2, r_init(kk)+cl/2, r_init(kk)+cl/2, r_init(kk)-cl/2], [-cw/2, cw/2, cw/2, -cw/2, -cw/2])
        hold on;
        plot([r_init(kk); x1(kk)],[0; y1(kk)],'b-'),
        plot(x1(kk), y1(kk), 'o')
        quiver(r_init(kk), -0.2, u_rs(kk), 0, 'Color', [161, 15, 7]/255, 'LineWidth', 2)
        axis([-13 13 -2 2]), grid on;
        text(6,-1.5,sprintf('t = %g [s]', round(t_rs(kk))));
        hold off;
    end

    function setxlim
        xlim(t([1,end]))
    end
end
