function anim_stick(t_x,X,t_u,M_U)
% This function creates a stick animation of the 7-link model used by
% legs2d.m for "looping" animations
% Input:    t_x - 1 x n_x vector of time instants for the states
%           X - n_x x 50 matrix of states
% Last modification: 04.23.2008 (by Marko Ackermann)

% Generate movie?
gm = 0; % 0 - no
        % 1 - yes

% Specify number of snapshots
ns = 25;

% use legs2d.dll now to generate outputs at ns equidistant time points
tout = t_x(end)*(0:ns-1)/(ns-1);			% 1 x ns matrix
xout = interp1(t_x, [X(:,1)-X(1,1) X(:,2:50)], tout)';			% 50 x 101 matrix
uout = interp1(t_u, M_U, tout)';	% 16 x 101 matrix

[xoutdot, cm, grf, mforces, stick, fce] = legs2d(tout, xout, uout);
figure(5);
list1 = [1 2 3 4 9 10 4];		% points on trunk and left leg
list2 = [2 5 6 7 8 6];			% points on right leg
x1 = 2*list1-1;		% indices of X coordinates in point list 1
y1 = 2*list1;		% indices of Y coordinates in point list 1
x2 = 2*list2-1;		% indices of X coordinates in point list 2
y2 = 2*list2;		% indices of Y coordinates in point list 2

xminimum = min([stick(13,:) stick(17,:)])-0.1;
xmaximum = max([stick(15,:) stick(19,:)])+0.1;

for i=1:ns   
	clf; hold on;
    axis([xminimum-1 xmaximum -0.5 1.5]);
	axis('equal');
	axis('manual');
	plot([xminimum xmaximum],[0 0],'k');				% draw the floor
	plot(stick(x1,i),stick(y1,i),'g');		% draw left side of body in green
	plot(stick(x2,i),stick(y2,i),'r');		% draw right side of body in red
    %grf(:,i)
    draw_arrow([grf(3,i) 0], [grf(1,i) grf(2,i)]/1000, 'g', 0.1); % draw left ground reaction force
    draw_arrow([grf(6,i) 0], [grf(4,i) grf(5,i)]/1000, 'r', 0.1); % draw right ground reaction force
    
    % Plot the time history of the neural excitations
    T=tout;
    A=uout;
    dz = 0.1;
    xend = xminimum-0.1;
    xbegin= xend-0.6;
    %zbegin = 0;
    %zend = 0.8;
    zbegin = -0.3;
    zend = 1.3;    
    points = xbegin:(xend-xbegin)/(length(T)-1):(xbegin+(i-1)*(xend-xbegin)/(length(T)-1));
    plot([xend; xbegin],[zend; zend],'k');
    plot([xend; xend],[zbegin; zend],'k'); 
    
    m_names = ['ILIO';'GLU ';'HAM ';'RF  ';'VAS ';'GAS ';'SOL ';'TA  ';'ILIO';'GLU ';'HAM ';'RF  ';'VAS ';'GAS ';'SOL ';'TA  '];
    musc_names = cellstr(m_names);  
    for j = 1:16
        % Muscle Activation:
        plot([xbegin; xbegin],[zbegin+(j-1)*dz;  zbegin+j*dz],'k');
        plot([xbegin; xend],[zbegin+(j-1)*dz; zbegin+(j-1)*dz],'k');
        if j <= 8
            color_p = 'r';
        else
            color_p = 'g';
        end
        plot(points, dz*A(j,1:i)+zbegin+(j-1)*dz,color_p);
        %plot(points, dz*A(j+8,1:i)+zbegin+(j-1)*dz,'g');
        text(xbegin-0.2,zbegin+(j-1)*dz+dz/2,musc_names(j),'FontSize',10);
    end
    
    if gm == 1
        F(i) = getframe;
        %aviobj = addframe(aviobj,F(i));
    end
    
	%disp('Hit any key to continue...');
    pause;
end

if gm == 1
    movie2avi(F,'test4.avi','compression','None','fps',20)
    return
end

    function draw_arrow(p_head, delta_tail, color, asp)
    % this function draws an arrow in the active 2_D plot
    % where:
    % p_head - 1x2 vector of (X,Y) coordinates of the head of the arrow
    % delta_tail - 1x2 vector of (X,Y) differential coord. of the tip of the head
    % in relation to the tail of the arrow.
    % color - color of the arrow
    % asp - aspect of the arrow - ("length" of the head)/(total length of the arrow)

    % Length of the arrow:
    L = norm(delta_tail);

    % angle of the arrow head:
    alpha = pi/9;

    % Position of the base points of the head of the arrow:
    p_1 = p_head - asp*delta_tail + [delta_tail(2) -delta_tail(1)]*asp*tan(alpha);
    p_2 = p_head - asp*delta_tail + [-delta_tail(2) delta_tail(1)]*asp*tan(alpha);

    hold on
    % Plot body of the arrow:
    if norm(delta_tail) ~= 0
        plot([p_head(1) p_head(1)-delta_tail(1)], [p_head(2) p_head(2)-delta_tail(2)],color,'LineWidth',1);
    end

    % Plot head:
    if norm(delta_tail) ~= 0
        plot([p_1(1) p_head(1) p_2(1)],[p_1(2) p_head(2) p_2(2)],color,'LineWidth',1);
    end
    end
end
