% Animaçao do duplo pendulo
%
% pendulo2_animate(traj_exp,traj_real,L1,L2)
%
% traj_exp : trajectoria prevista [t; theta1; theta2]
% traj_real: trajectoria efectiva [t; theta1; theta2]
% L1 e L2: Dimensoes dos elos

function pendulo2_animate(traj_exp,traj_real,L1,L2)

% Trajectoria efectiva
t_real=traj_real(1,:);
pos1_real=traj_real(2,:);
pos2_real=traj_real(3,:);

% Coordenadas cartesianas da extremidade de cada elo
[x1,x2,y1,y2]=cinematDir2(pos1_real,pos2_real,L1,L2);

% Trajectoria esperada (coordenadas cartesianas)
[garb,xp,garb,yp]=cinematDir2(traj_exp(2,:),traj_exp(3,:),L1,L2);

for i=1:length(t_real)
    % Desenho da trajectoria prevista
    plot(xp,yp,'g-.', xp(1),yp(1),'go', xp(end),yp(end),'gx');
    hold on
    % Desenho dos elos
    pendulo2_draw(x1(i),y1(i),x2(i),y2(i),1.1*(L1+L2));
    hold off
    
    drawnow;

    % Apresentaçao do instante de simulaçao
    t_now=t_real(i);
    if (rem(i-1,100)==0)
        fprintf('time=%g \r',t_now);
    end
end

fprintf('Done!\n');
