%clear all

close all
clc
disp('       Robo - trajecto no PC ou PJ');

% inicialização de variaveis e constantes

    footH = 1.75;   % altura do pé

    l1 = 9.5;                 % comprimento da tíbia
    l2 = 9.5;                 % comprimento do fémur
    l3 = 5;                 % comprimento do tronco
    t = 3;                  % tempo necessário para a execução do movimento
    f = 5;                 % numero de frames por segundo para animação do moviumento
    c = 1;                 % direcção do joelho (-1 -> lado direito; +1 -> lado esquerdo)
    tipo_plano = 'PJ';      % plano cartesiano('PC') ou plano das juntas('PJ') para calculo da trajectória
    angulo_tibia = pi/2;    % angulo inicial que a tíbia faz com a horizontal
    angulo_femur = pi/2;    % angulo inicial que o fémur faz com a horizontal
    angulo_tronco = pi/2;   % angulo constante que o tronco faz com a horizontal
    
    robo = zeros([2 4 t*f]);                % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
    robo(2,4,:) = ones(1,t*f)*(footH);     % junta do pé

    pos_inicial = [-30 0 0];     % lê o angulo inicial de cada servo (offset)
    applyjoint(H,2,0,pos_inicial)
    
    clc
    
% criação e animação do robo

    teta1 = pi/2-angulo_tibia;                   % posição angular inicial da primeira junta com a vertical
    teta2 = angulo_tibia-angulo_femur;           % posição angular inicial da segunda junta com a tibia
    teta3 = angulo_femur-angulo_tronco;% posição angular inicial da terceira junta com o fémur
    pos_ref = [ones(1,t*f)*robo(1,4); ones(1,t*f)*robo(2,4)];

    [robo(:,3,1), robo(:,2,1), robo(:,1,1)] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);     % calcula no espaço cartesiano a posição das juntas de acordo com o ângulo necessário
    robo(:,1:3,1) = robo(:,1:3,1)+pos_ref(:,1:3);            % normaliza a posição das juntas em relação à referencia -> pé
    desenhar_3R(robo(:,:,1), l1, l2, l3);

%     
%     disp('       Robo - trajecto no PC ou PJ');
%     disp(strcat('L1 -> ', num2str(l1)));
%     disp(strcat('L2 -> ', num2str(l2)));
%     disp(strcat('Tempo de Execução -> ', num2str(t), 'seg'));

    
     while(1),
        
        [a,b] = ginput(1);                % fica a aguardar que se indique a posição final a alcançar
        while(sqrt((a-pos_ref(1,1))^2+(b-pos_ref(2,1))^2)>(l1+l2)),         % teste ao alcance do braço            
                disp('Posição fora da gama de alcance do braço');
                [a,b] = ginput(1);                
        end

         hold on                %
         plot(a, b, 'x');       %
         hold off               %% indica no gráfico a posição final a atingir   
         

         
         [teta1i, teta2i] = inv_cine(robo(1,2)-robo(1,4), robo(2,2)-robo(2,4), l1, l2, c);
         [teta1f, teta2f] = inv_cine(a-robo(1,4), b-robo(2,4), l1, l2, c);

         [teta1, omega1, alfa1] = traject(teta1i, teta1f, t, f);              % calcula o trajecto para a 1ª junta
         [teta2, omega2, alfa2] = traject(teta2i, teta2f, t, f);              % calcula o trajecto para a 2ª junta
         teta3 = angulo_tronco*ones(1,length(teta1))-pi/2+(teta1-teta2);

         [mov3, mov2, mov1] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);
     
        pos_apply(1) = pos_inicial(1);
        pos_apply(2) = pos_inicial(2) - rad2deg(teta1f);
        pos_apply(3) = rad2deg(teta2f)- pos_inicial(3);
        pos_apply
        pause
        applyjoint(H, 2, 0, pos_apply);
        
        mov1 = mov1 + pos_ref;      %
        mov2 = mov2 + pos_ref;      %
        mov3 = mov3 + pos_ref;      %% normalização dos vectores movimento em relação à referencia - pé
        robo(:,1,:) = mov1;         %
        robo(:,2,:) = mov2;         %
        robo(:,3,:) = mov3;         %% actualização da matriz robo para a posterior execução do movimento
        
        % construcção da matriz com as posições do rasto em cada instante
        rasto = ones([2, t*f t*f]);     
        i = 1;
        while(i<t*f),
            rasto(:,1:i,i) = robo(:,2,1:i);
            i = i+1;
        end
        
        i = 1;
        tic
        while(i<t*f),   
            desenhar_3R(robo(:,:,i), l1, l2, l3)            %
             hold on                                        %
             plot(rasto(1,1:i,i), rasto(2,1:i,i), 'k-.');   %
             plot(a, b, 'x');                               %
             hold off                                       %
            i = i+1;                                        %% desenha braço, rasto e posição final a atingir
            while(toc<1/f),        %
                pause(1/1000)      %
            end                    % responsavel pela animação com f frames por segundo
            tic
        end
        
        robo(:,:,1) = robo(:,:,end);
        
     end     % fim do ciclo infinito