function anim_loop2(t_x,X,v_walk,T_dur,bsymm)
% This function creates a stick animation of the 7-link model used by
% legs.m  for a whole gait cycle for "looping" animations
% Note1: the data do not include the nodal value at half or at the end of 
%        the gait cycle.
% note2: this function is still special for the 2D model in legs2d
% Input:    t_x - 1 x n_x vector of time instants for the states
%           X - n_x x 50 matrix of states
%           v_walk - walking speed
%           T_dur - duration of half a gait cycle
%           bsymm - bilateral symmetry flag (0-no symmetry; 1-symmetry)
% Last modification: 04/18/2008 (by Marko Ackermann)

% Generate movie?
gm = 0; % 0 - no
        % 1 - yes
        
% If gm = 1 specify slow motion factor
smf = 4; % choose "1" for real time
        
% Specify number of snap shots
ns = 101;


% Vector of transformation to mirror states at the end to match states at
% the beginning
if bsymm == 1 % bilateral symmetry activated
    vm = [1:3 7:9 4:6 ... % gen coord.
          10:12 16:18 13:15 ... % gen. vels.
          27:34 19:26 ... % CE muscle lengths
          43:50 35:42]'; % muscle activations
else % bsymm = 0 - no bilateral symmetry
    vm = (1:2*16+2*9)';
end


% Step length
if bsymm == 1 % data fot half a gait cycle
    Ls = v_walk*T_dur;
else % bsymm = 0 - data for whole gait cycle
    Ls = v_walk*T_dur/2;
end


% Use legs2d.dll now to generate outputs at ns
% equidistant time points along a whole gait cycle
if bsymm == 1
    Tw = 2*T_dur; % duration of whole gait cycle
    tout = 0:2*T_dur/(ns-1):t_x(1)+2*T_dur-2*T_dur/(ns-1);			% 1 x (ns-1) matrix
    xout = interp1([t_x T_dur+t_x 2*T_dur], [[X(:,1)-v_walk*t_x' X(:,2:50)]; [X(:,1)-v_walk*t_x' X(:,vm(2:50))]; [X(1,1) X(1,2:50)]], tout)';	% 50 x ns matrix
else % bsymm = 0 
    Tw = T_dur; % duration of whole gait cycle
    tout = 0:T_dur/(ns-1):T_dur-T_dur/(ns-1);			% 1 x (ns-1) matrix
    xout = interp1([t_x T_dur], [[X(:,1)-v_walk*t_x' X(:,2:50)]; [X(1,1) X(1,2:50)]], tout)';    
end

% Note: since we are only interested on the ground reaction forces and 
%       kinematics it is OK to use muscle activations intead of neural 
%       excitations as input to legs2d
[xoutdot, cm, grf, mforces, stick, fce] = legs2d(tout, xout, xout(35:50,:));

figure(5);
%list1 = [1 2 3 4 9 10 4];		% points on trunk and left leg
list1 = [2 3 4 9 10 4];		    % points on left leg
list2 = [2 5 6 7 8 6];			% points on right leg
list3 = [1 2];                  % points on trunk
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
x3 = 2*list3-1;		% indices of X coordinates in point list 3
y3 = 2*list3;		% indices of Y coordinates in point list 3

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

for i=1:ns-1
	clf; hold on;
	%axis([-1.5 1.5 -0.5 1.5]);
    axis([xminimum-1 xmaximum+0.1 -0.5 1.5]);
	axis('equal');
	axis('manual');
    
    % Drawing the floor
	plot([xminimum xmaximum],[0 0],'k','LineWidth',1);				% draw the floor
    
    % Select separation between markers specifying a number of markers per
    % divisions of the step length
    n_mkrs = 2;
    for x_orig=-2*Ls:Ls/2:4*Ls
        x_mkrs = x_orig - (tout(i)-tout(1))*v_walk;
        if (x_mkrs > xminimum) & (x_mkrs < xmaximum)
            plot([x_mkrs x_mkrs],[0 -0.02],'k','LineWidth',1);
        end
        
    end
    
	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
    plot(stick(x3,i),stick(y3,i),'b');		% draw trunk
    
    %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=xout(35:50,:);
    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
    
    box on
    
    if gm == 1
        F(i) = getframe;
    end
    
	%disp('Hit any key to continue...');
    pause;
end

if gm == 1   
    movie2avi(F,'test10.avi','compression','None','fps',(ns/Tw)/smf) % set smf to "1" for real time
    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
