%% teste do controlador de posição proporcional %%

close all
clear teta1 teta2 teta3 j1 j2 j3 robo robo_real
clc
fprintf('\t\tTeste\n');

applyjoint(H,2,0,[-20 20 0]);

% Parâmetros de controlo variáveis

    carga = 1167;

    angulo_flexao = 50;  % angulo da junta 1 em graus em relação à superficie
    
    j1_ki = 7;
    j1_kp = 0;
    j1_kd = 0;

    j2_ki = 5;
    j2_kp = 0;
    j2_kd = 0;

    j3_ki = 3;
    j3_kp = 0;
    j3_kd = 0;
    
    
    error = 1;
    while(error),
        [rx,error,errorstr,tries] = applycontrol(H,2,0,[j1_ki j2_ki j3_ki]);
        [rx,error,errorstr,tries] = applycontrol(H,2,1,[j1_kp j2_kp j3_kp]);
        [rx,error,errorstr,tries] = applycontrol(H,2,2,[j1_kd j2_kd j3_kd]);
    end
    
% inicialização de variaveis e constantes


    l1 = 9.5;               % comprimento da tíbia
    l2 = 9.5;               % comprimento do fémur
    l3 = 6.8;               % comprimento do tronco
    h_base = 6.2;           % distancia da base do pé à primeira junta
    t = 50*20/1000;         % duração do movimento
    f = 10;                 % 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 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
    
    flecte_t1 = deg2rad(angulo_flexao);                 % angulo da junta 1 para flectir a perna
    flecte_t2 = pi-2*flecte_t1;                         % angulo da junta 2 para flectir a perna
    flecte_t3 = angulo_tronco-flecte_t2-flecte_t1;      % angulo da junta 3 para flectir a perna com o tronco sempre direito

    robo = zeros([2 4 1]);    % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
    robo(2,4,1) = h_base;     % junta do pé

    robo_real = zeros([2 4 1]);    % Criação de vector de posição inicial das juntas (tronco, anca, joelho, pé)   
    robo_real(2,4,1) = h_base;     % junta do pé

    
% Criação da zona de construcção dos plots

    f_j1 = figure('Units', 'normalized', 'Position', [0 0.5, 0.5 0.5], 'MenuBar', 'none', 'Name', 'Posição da junta1', 'DoubleBuffer', 'on');
    f_j2 = figure('Units', 'normalized', 'Position', [0.5 0.5, 0.5 0.5], 'MenuBar', 'none', 'Name', 'Posição da junta2', 'DoubleBuffer', 'on');
    f_j3 = figure('Units', 'normalized', 'Position', [0 0 0.5 0.5], 'MenuBar', 'none', 'Name', 'Posição da junta3', 'DoubleBuffer', 'on');
    f_sim = figure('Units', 'normalized', 'Position', [0 0 1 1], 'MenuBar', 'none', 'Name', 'Simulação', 'DoubleBuffer', 'on');
    
% criação e animação do robo

    % robo virtual
        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,4)*robo(1,4,1); ones(1,4)*robo(2,4,1)];

        [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_2D(robo(:,:,1), l1, l2, l3, f_sim, 'r');
       
    % robo real
        error=1;
        while(error)
            [rx,error,errorstr,tries] = applycontrol(H,2,3,[1 1 1]);
            [off_set_juntas,state,rx,error,errorstr,tries] = readjoint(H,2,0);            
%             while(sum(state(6:8))), 
%             end;
        end
        [robo_real(:,3,1), robo_real(:,2,1), robo_real(:,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_real(:,1:3,1) = robo_real(:,1:3,1)+pos_ref(:,1:3);            % normaliza a posição das juntas em relação à referencia -> pé
        desenhar_3R_2D(robo_real(:,:,1), l1, l2, l3, f_sim, 'g');
        legend('Robo Virtual', 'Robo Real');
%         flecte(1) = -(90-angulo_flexao);
%         flecte(2) = -(180+2*flecte(1));
%         flecte(3) = angulo_tronco-flecte(2)+flecte(1);
%         flecte = [flecte(1), flecte(2), flecte(3)] - off_set_juntas;
        
        
% inicio
        
        %         [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]))     %
        %              close all;         %
        %              break;             %
        %          end                    %% se for pressionado "enter" sai da simulação
        %         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 = flecte_t1;
        teta2f = flecte_t2;

            [a,b,c] = direct_cine_3R(teta1f, teta2f, angulo_tronco-(teta1f+teta2f), l1, l2, l3);
            a=b(1)+pos_ref(1,1);
            b=b(2)+pos_ref(2,1);
            hold on
            plot(a, b, 'x');
            hold off
        
        [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))-(teta1+teta2);

        [mov3, mov2, mov1] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3);

        pos_ref = [ones(1,t*f).*pos_ref(1,1); ones(1,t*f).*pos_ref(2,1)];
        mov1 = mov1 + pos_ref;      %
        mov2 = mov2 + pos_ref;      %
        mov3 = mov3 + pos_ref;      %% normalização dos vectores movimento em relação à referencia - pé
        for i=1:t*f,
            robo(:,1,i) = mov1(:,i);         %
            robo(:,2,i) = mov2(:,i);         %
            robo(:,3,i) = mov3(:,i);         %% actualização da matriz robo para a posterior execução do movimento
            robo(:,4,i) = robo(:,4,1);
        end
        
        
        % 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
        flecte = rad2deg([-(pi/2-teta1f) -teta2f -teta3(end)])+off_set_juntas;

%         while(sum(state(6:8))~=3), 
%             disp('em movimento')
%         end
        i = 1;
        applyjoint(H,2,0,flecte);
        
        tic
        while(i<(t+5)*f),   
            
            error=1;
            while(error)
                [pos_real,state,rx,error,errorstr,tries] = readjoint(H,2,0);
            end
            aux = deg2rad(pos_real-off_set_juntas);
            aux(1) = pi/2+aux(1);
            j1(i) = aux(1);
            j2(i) = -aux(2);
            j3(i) = -aux(3);
            [t1, t2, t3] = direct_cine_3R(j1(i), j2(i), j3(i), l1, l2, l3);
            
            robo_real(:,4,i) = pos_ref(:,1);
            robo_real(:,3,i) = t1+pos_ref(:,1);
            robo_real(:,2,i) = t2+pos_ref(:,1);
            robo_real(:,1,i) = t3+pos_ref(:,1);
            
                        
            desenhar_3R_2D(robo(:,:,end), l1, l2, l3, f_sim, 'r')         %
            hold on                                                     %
            desenhar_3R_2D(robo_real(:,:,i), l1, l2, l3, f_sim, 'g')    %
            hold off                                                   %% desenha braço, rasto e posição final a atingir
            legend('Robo Virtual', 'Robo Real');
            
            i = i+1;                                                    
            while(toc<i/f),        %
            end                    % responsavel pela animação com f frames por segundo
        end
        
        error_plot2(rad2deg(teta1f), rad2deg(j1), f_j1);

        error_plot2(rad2deg(teta2f), rad2deg(j2), f_j2);

        error_plot2(rad2deg(teta3(end)), rad2deg(j3), f_j3);
        
        set(f_sim, 'Position', [0.5 0 0.5 0.5]);
        
        fprintf('Erro final:\n\n\tjunta 1 = %gº\n\tjunta 2 = %gº\n\tjunta 3 = %gº\n', ...
                                            abs(rad2deg(teta1(end)-j1(end))), abs(rad2deg(teta2(end)-j2(end))), abs(rad2deg(teta3(end)-j3(end))));
                                            
        
        robo(:,:,1) = robo(:,:,end);
        
        
        
        pause(3)
        
        applyjoint(H,2,0,off_set_juntas)
        
        resposta = ask
        if(resposta == 'Sim'),
            hgsave(f_sim, ['simulação_carga' num2str(carga) '_ki' num2str(j1_ki) '_kp' num2str(j2_ki) '_kd' num2str(j3_ki)])
            hgsave(f_j1, ['junta1_carga' num2str(carga) '_ki' num2str(j1_ki) '_kp' num2str(j2_ki) '_kd' num2str(j3_ki)])
            hgsave(f_j2, ['junta2_carga' num2str(carga) '_ki' num2str(j1_ki) '_kp' num2str(j2_ki) '_kd' num2str(j3_ki)])
            hgsave(f_j3, ['junta3_carga' num2str(carga) '_ki' num2str(j1_ki) '_kp' num2str(j2_ki) '_kd' num2str(j3_ki)])
        end