%%      Implementação da matriz Jacobiano      %%

hold off %close all
clear all
clc
disp('   Robo - Implementação da Matriz jacobiano');

% inicialização de variaveis e constantes

    l1 = 1;                 % comprimento da tíbia
    l2 = 1;                 % comprimento do fémur
    l3 = 1;                 % comprimento do tronco
    t = 3;                  % tempo necessário para a execução do movimento
    f = 25;                 % numero de frames por segundo para animação do moviumento
    c = -1;                 % direcção do joelho (-1 -> lado direito; +1 -> lado esquerdo)
    angulo_tibia = pi/2;    % angulo que a tíbia faz com a horizontal
    angulo_femur = pi/2;    % angulo que o fémur faz com a horizontal
    angulo_tronco = pi/2;   % angulo 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)*(2*l2/7);  % junta do pé

    
% criação e animação do robo

    teta1 = angulo_tibia;                   % posição angular inicial da primeira junta
    teta2 = angulo_femur - teta1;           % posição angular inicial da segunda junta
    teta3 = angulo_tronco - (teta1 + teta2);% posição angular inicial da terceira junta
    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);

    while(1),
        
        clc
        disp('   Robo - Implementação da Matriz jacobiano');
        disp(strcat('L1 -> ', num2str(l1)));
        disp(strcat('L2 -> ', num2str(l2)));
        disp(strcat('Tempo de Execução -> ', num2str(t), 'seg'));

        
        [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
        if(isempty([a,b]))
            break;
        end
         hold on                %
         plot(a, b, 'x');       %
         hold off               %% indica no gráfico a posição final a atingir   
         
                 
        [x, vx, ax] = traject(robo(1,2,1), a, t, f);              % calcula o trajecto segundo o eixo dos xx
        [y, vy, ay] = traject(robo(2,2,1), b, t, f);              % calcula o trajecto segundo o eixo dos yy
        [teta1, teta2] = inv_cine(x-pos_ref(1,:), y-pos_ref(2,:), l1, l2, c); %inv_cine(x(1)-pos_ref(1,1), y(1)-pos_ref(2,1), l1, l2, c); %robo(1,2,1),robo(2,2,1), l1, l2, c);       % calcula o ângulo nas juntas necessário para atingir a posição final

        
        for k=1:t*f-1,
            jacobiano = gera_jacobiano(teta1(k), teta2(k), l1, l2);
            
            if(det(jacobiano)==0)
                disp('Erro -> Matriz jacobiano singular');
            else
                omega = inv(jacobiano)*[vx(k); vy(k)];
                omega1(k) = omega(1);
                omega2(k) = omega(2);
                teta1(k+1) = teta1(k)+omega1(k)*1/f;
                teta2(k+1) = teta2(k)+omega2(k)*1/f;
            end
            
        end
        
        teta3 = angulo_tronco*ones(1,length(teta1))-(teta1+teta2);
        [mov3, mov2, mov1] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);

        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),
            subplot(2,1,1);
            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                                       %% desenha braço, rasto e posição final a atingir
             
            
            subplot(2,1,2);                                                         %
            title('Erro sem compensação');                                          %% Faz o plot erro: distancia euclidiana entre a posição desejada e a posição real
            plot(0:1:i-1, sqrt((x(1:i)-mov2(1,1:i)).^2+(y(1:i)-mov2(2,1:i)).^2), 'k');  % (x,y) -> movimento exacto; 

            i = i+1;
            while(toc<1/f),         %
                pause(1/1000)       %
            end                     % responsavel pela animação com f frames por segundo
            tic
        end
 
        robo(:,:,1) = robo(:,:,end);    % actualiza a nova posição inicial do robô
        
    end     % fim do ciclo infinito
    
    clear all
    clc