% Leitura da trajectoria do duplo pendulo atraves do rato.
%
% [x,y]=getPositions(N,theta1_0,theta2_0,L1,L2)
%
% Entrada:  N - Numero de pontos a ler
%           theta1_0, theta2_0 - Angulos iniciais das juntas
%           L1, L2 - Dimensoes dos elos

function [t,x,y]=getPositions(N,theta1_0,theta2_0,L1,L2)

% Verificaçao dos argumentos
if (nargin<5)
    L2=1;
end
if (nargin<4)
    L1=1;
end
if (nargin<3)
    theta2_0=0;
end
if (nargin<2)
    theta1_0=0;
end
if (nargin<1)
    N=1;
end

% Obtençao das coordenadas associadas as condiçoes iniciais
[x1,x2,y1,y2]=cinematDir2(theta1_0,theta2_0,L1,L2);

% Desenho do duplo pendulo
pendulo2_draw(x1,y1,x2,y2,1.1*(L1+L2));

[x,y]=ginput(N);

dist_min=L1-L2;
dist_max=L1+L2;

% Correcçao das coordenadas introduzidas
for i=1:N    
    % Distancia (ao quadrado) da origem ao ponto (x,y)
    dist2=x(i)^2+y(i)^2;
    
    % Se fora da gama possivel, coloca-lo na fronteira
    if (dist2>dist_max^2 || dist2<dist_min^2)
        alpha=atan2(y(i),x(i));                 % Angulo que faz com o eixo xx
        if (dist2>dist_max^2)                   % Determinaçao da distancia a considerar (maxima ou minima)
            dist_lim=dist_max;
        else
            dist_lim=dist_min;
        end
        x(i)=dist_lim*cos(alpha);               % Corrigir o ponto para as coordenadas limite
        y(i)=dist_lim*sin(alpha);
    end
end

% Adiçao da primeira posiçao (condiçoes iniciais)
t=zeros(N+1,1);
x=[x2; x];
y=[y2; y];

% Instantes de tempo de execuçao
fprintf('Introduza os instantes de tempo para a execuçao de cada trajectroria:\n');
fprintf('p1=(%g,%g)\n\tt1=%g\n',x(1),y(1),t(1));
for i=2:length(t)
    while (1)
        fprintf('p%d=(%g,%g)\n\tt%d=',i,x(i),y(i),i);
        t(i)=input('');
        
        % O instante deve ser superior ao actual
        if (t(i)>t(i-1))
            break;
        end
        fprintf('ERRO: Instante de tempo invalido!\n');
    end
end
