%______________________________________________________________
% File: Sim_ArrowTracking_PTU
% Date: 20-Oct-2014
% Purpose: Simulate the tracking of the "arrow" cue with
%                   the pan-tilt unit (PTU) mounted on the FANUC
%______________________________________________________________
clear all
close all
clc
%______________________________________________________________
% Parameters and initializations
%______________________________________________________________
%L = [36.4 35.5 60.0 1]*1e-3;                         % Link lengths
L = [60 35.5 30 1]*1e-3;                              % Link lengths
FANUC_flag = 1; % Vai buscar dados ao fanuc?
Animate_motion_Flag = 0; % É para fazer animação do movimento?
UsePTUanglesData = 1; % Usar os dados reais da ptu?
XLimPlot = [13 26]; % Especificar o limite XX dos graficos
IsCubeDataExperiment = 1;% É a experiencia do cubo (controlo velocidade)
PauseEach50Its = 0; % fazer uma pausa a cada 50 iterações?
% Atualizar a visualização dos graficos a cada X iteração
PlotTime2Time = 1; %
VelAtualizePlot = 400; % 1 - 400  Especificar velocidade de atualização
%Analise se os dados são columativos ou não! (Já cheguei à conclusão que é
%indiferente! o que influencia é o erro nas medições, tanto nos angulos
%como na distância PTU - target
VerificaSeEComulativo = 0;
AnalisaDadosComulativos = 1;
FontSize = 16;
firstStop = 1;% Indica que é a primeira passagem que passa por determinado local

if IsCubeDataExperiment ==1;
    XLimPlot=[72.37 87.46];%[0 150];
end
if FANUC_flag == 1,
    % Read joint angles and FANUC's end-effector coordinates
    JOINT_data = csvread('data/servos_position.txt');
    FANUC_data = csvread('data/positions.txt');
    % Synchronized time vector
    if JOINT_data(1,1) > FANUC_data(1,1),  dtime  = JOINT_data(1,1);
    else dtime  = FANUC_data(1,1); end
    % PTU joint angles
    tr = JOINT_data(:,1)-dtime;
    Qr = JOINT_data(:,2:3)';
    ILog = Qr == 0;
    Qr(ILog) = NaN;
    Qr = Qr([2 1],:);
    
    % FANUC's end-effector coordinates
    FANUC_data = FANUC_data(1:2:end,:);
    fPptu = 1e-3*FANUC_data(:,2:4)';
    fPptu(2,:) = fPptu(2,:)-180*1e-3;           % Gripper's offset (YY-axis)
    t = FANUC_data(:,1)-dtime;
    Texe = t(end);
    FANUC_data(:,2:4) =  FANUC_data(:,2:4)/1000;% Convert mm to meters
    QrInterpol = [interp1(tr,Qr(1,:),t) interp1(tr,Qr(2,:),t)]'; % Interpolação para a var 't'
    %Servomotores: 600- 2400 || Avg= 1500
    ServosScalefactor = 1/1800*pi*0.6833/0.5812;
    QrInterpol = [(QrInterpol(1,:)-1500); (QrInterpol(2,:)-1500)]*ServosScalefactor;
    %-Qr(1,:)+1500)/10+5.83   -- -Qr(2,:)+1500)/10+33.675
    
    if IsCubeDataExperiment ==1
        
        H3 = figure(13);set(H3,'Color',[1 1 1])
        plot(t,FANUC_data(:,2:4)*1000,t,FANUC_data(:,5:7));
        hold on;
        draw_cube_movementes_separator_cont_VEL
        title('Posiçao Fanuc');
        hold off;
    end
else
    Ts = 2e-2;                                   % Sampling rate
    Texe = 2;                                    % Simulation time
    t = 0 :Ts:Texe;
    XLimPlot = [0 2];
end

DIM = length(t);
%______________________________________________________________
% Static arrow / Moving PTU
%______________________________________________________________
rP_arrow = [1 0 0 1]';        % Arrow position in Sr

if FANUC_flag == 1,
    fPptu = [fPptu ; ones(1,DIM)];
    rTf = [ 1  0  0  -614.086*1e-3
        0  1  0  -5.95e-3
        0  0  1   5.07e-3
        0  0  0    1 ];
    rPo = rTf*fPptu;
    rPo = rPo(1:3,:);
else
    Pi = [-2 2 0.5]'; Pf = [-2 -2 0.6]'; % Initial/final camera position S0
    rPo = zeros(3,DIM);
    [ rPo(1,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(1),Pf(1),5 );
    [ rPo(2,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(2),Pf(2),5 );
    [ rPo(3,:),dP,ddP,counter ] = PolyTrajGenerator( t,Pi(3),Pf(3),5 );
end
%______________________________________________________________
% Desired PTU's tracking: reference joint angles ii
%______________________________________________________________
Qd = zeros(2,DIM);
for ii = 1:DIM, % Inverse kinematics _________________________
    rTo = [eye(3) rPo(:,ii); 0 0 0 1];
    SoP_arrow = rTo\rP_arrow;
    Qd(:,ii) = InverseKinematics( SoP_arrow,L);
end

%plot para comparar os angulos reais e calculados ('ideais')
H4 = figure(12);set(H4,'Color',[1 1 1])
plot(t,Qd,t,[QrInterpol(1,:); -QrInterpol(2,:)]);
ylabel('Position (m)','FontSize',FontSize);
title('Comparação angulos reais e ideais');
if IsCubeDataExperiment == 1
    draw_cube_movementes_separator_cont_VEL;
end
hold off;
%______________________________________________________________
% Animate motion using a PTU's proportional controller
%______________________________________________________________
if Animate_motion_Flag == 1
    H=figure(1); set(H,'Color',[1 1 1]), EL = 0; AZ = 90;
    figure(2)
end
Q = zeros(2,DIM);

tComulat = zeros(1,DIM);
Vel_PTU_egomotion = zeros(2,DIM); % primeira linha espaço XY, e a segunda é em Z
Vel_PTU_egomotionPTUangle = zeros(2,DIM);
Pos_PTU_egomotion = zeros(1,DIM);
% Gurada a posição com erro na dist ao target no egomotion 'ideal'
Pos_PTU_egoIdealDistErr = zeros(4,DIM);
% Gurada a Velocidade com erro na dist ao target no egomotion 'ideal'
Vel_PTU_egoIdealDistErr = zeros(4,DIM);
% Gurada a posição com erro na dist ao target no egomotion 'ideal'

% comulativo
Pos_PTU_egoIdealDistErrComulativo = zeros(4,DIM);
Pos_PTU_egomotionPTUangle = zeros(1,DIM);
Pos_PTU_egoPTU_Real_angDistErr = zeros(4,DIM);
Vel_PTU_egoPTU_Real_angDistErr = zeros(4,DIM);
ErrorPercentages = [0.5 2 10 20];
%ErrorPercentages = [5 5 5 5];
Altura_PTU_egomotion = zeros(1,DIM);
Altura_PTU_egomotionIdealDistErr = zeros(4,DIM);
Altura_PTU_egomotionRealDistErr = zeros(4,DIM);
Altura_PTU_egomotionPTUangle = zeros(1,DIM);
Dist_PTU_arrow3D = zeros(1,DIM);
Dist_PTU_arrow2D = zeros(1,DIM);
Dist_PTU_arrow2DErr = zeros(4,DIM);
Vel_PTU_simulator = zeros(3,DIM);
Pos_PTU_simulator = zeros(3,DIM);
DeltaDistErr = zeros(1,4);
DeltaDistPTUangRealDistErr = zeros(1,4);
Vel_Fanuc = zeros(6,DIM); % Velocidade de cada componente do fanuc
ERR_img = zeros(2,DIM);
sumDeltaDist = 0; % Inicialização para verificação dos erros acomulativos
P_ptu = zeros(3,4); P_cam = zeros(3,4);
for ii = 1:DIM,
    if ii == 1, % Initial alignment of the camera with the arrow
        % Inverse kinematics _____________________________________
        rTo = [eye(3) rPo(:,1); 0 0 0 1];
        SoP_arrow = rTo\rP_arrow;
        Q(:,ii) = InverseKinematics( SoP_arrow,L );
    else
        % Proportional control based on the image error
        La = P_ptu(:,4); Lb = rP_arrow(1:3);
        P0 = P_cam(:,1); P1 = P_cam(:,2); P2 = P_cam(:,3);
        A = [La-Lb P1-P0 P2-P0];
        b = La-P0;
        S = A\b;
        P_img = La+(Lb-La)*S(1);
        q1 = Q(1,ii-1); q2 = Q(2,ii-1);
        s1 = sin(q1); c1 = cos(q1); s2 = sin(q2); c2 = cos(q2);
        l1 = L(1); l2 = L(2); l3 = L(3)-0.01*L(3);
        T = [ c1*c2 -c1*s2 s1 (l3*c1*c2-l2*c1*s2)+rPo(1,ii-1)
            s1*c2 -s1*s2 -c1 (l3*s1*c2-l2*s1*s2)+rPo(2,ii-1)
            s2        c2         0   (l3*s2+l2*c2+l1)+rPo(3,ii-1)
            0          0           0  1                                ];
        P_img = T\[P_img; 1];
        ERR_img(:,ii) = [P_img(3) P_img(2)]';
        Q(1,ii) = 400*P_img(3)+Q(1,ii-1);
        Q(2,ii) = -400*P_img(2)+Q(2,ii-1);
        Q_ptu = Q(:,1:ii)*180/pi;
        %pause
    end
    % Direct kinematics ______________________________________
    [P_ptu P_cam] = DirectKinematics( Q(:,ii),L );
    P_ptu = P_ptu + repmat(rPo(:,ii),1,4);
    P_cam = P_cam + repmat(rPo(:,ii),1,4);
    % Animate motion
    if Animate_motion_Flag == 1
        figure(1), clf
        hold on
        H = plot3(P_ptu(1,:),P_ptu(2,:),P_ptu(3,:),'ro-', rP_arrow(1),rP_arrow(2),rP_arrow(3),'bx',...
            [P_ptu(1,end) rP_arrow(1)],[P_ptu(2,end) rP_arrow(2)],[P_ptu(3,end) rP_arrow(3)],'g:',...
            rPo(1,1:ii),rPo(2,1:ii),rPo(3,1:ii),'r.');
        fill3([P_cam(1,:) P_cam(1,1)],[P_cam(2,:) P_cam(2,1)],[P_cam(3,:) P_cam(3,1)],'k')
        xlabel('XX'), ylabel('YY'), zlabel('ZZ')
        %axis equal, axis([-0.5 1.5 -0.5 0.5 -1 2]), view(EL,AZ), %grid on
        % Pan-tilt joint angles
        figure(2)
        subplot(2,1,1), plot(t(1:ii),Q(1,1:ii)*180/pi,'r.'), xlim([0 Texe])
        xlabel('Time (s)'), ylabel('PAN: Q1 (deg)')
        subplot(2,1,2), plot(t(1:ii),Q(2,1:ii)*180/pi,'r.'), xlim([0 Texe])
        xlabel('Time (s)'), ylabel('TILT: Q2 (deg)')
        
        if ii == 1, pause, [EL,AZ] = view;  end
    end
    pause(0.0001)
    
    %
    % EGOMOTION
    %
    if rem(ii,VelAtualizePlot) == 0 && PauseEach50Its == 1, pause();end %Pausa de 50 em 50 iteracoes
    %  ---- Pan ---
    Pos_PTU_and_arrow = [P_ptu(1,end) P_ptu(2,end) P_ptu(3,end); rP_arrow(1) rP_arrow(2) rP_arrow(3)];
    
    %Calculo da distancia PTU - Target
    Dist_PTU_arrow3D(ii) = pdist(Pos_PTU_and_arrow,'euclidean');
    Dist_PTU_arrow2D(ii) = pdist(Pos_PTU_and_arrow(1:2,1:2),'euclidean');%Visto de 'cima' (planoXY)
    
    % Ao cálculo da distancia, acrescenta um erro gauciano ao valor
    % conforme a variavel ErrorPercentages
    for jj=1:4
        Dist_PTU_arrow2DErr(jj,ii) = getNormGausVar(Dist_PTU_arrow2D(ii),ErrorPercentages(jj));
    end
    
    if ii == 1, %Initialization
        Vel_Fanuc(2,1) = 0;
        Prev_P_ptu = [P_ptu(1,end) P_ptu(2,end) P_ptu(3,end)];
    else
        % Calculations
        Delta_t = t(ii) - t(ii-1);
        
        if UsePTUanglesData == 1 &&  ~isnan(QrInterpol(1,ii)) &&  ~isnan(QrInterpol(1,ii-1))%Angulo real da PTU
            DeltaDistPTUangReal = getEgomotionDist(Dist_PTU_arrow2D(ii-1),...
                QrInterpol(1,ii-1),Dist_PTU_arrow2D(ii),QrInterpol(1,ii));
            % calculo da distacia Egomotion com erro gaussiano no calculo da distancia ao
            % target.
            for jj=1:4
                DeltaDistPTUangRealDistErr(jj) = getEgomotionDist(Dist_PTU_arrow2DErr(jj,ii-1),QrInterpol(1,ii-1),...
                    Dist_PTU_arrow2DErr(jj,ii),QrInterpol(1,ii));
            end
        end
        DeltaDist = getEgomotionDist(Dist_PTU_arrow2D(ii-1),Qd(1,ii-1),Dist_PTU_arrow2D(ii),Qd(1,ii));
        % calculo da distacia Egomotion com o erro gaussiano no calculo da distancia ao
        % target.
        for jj=1:4
            DeltaDistErr(jj) = getEgomotionDist(Dist_PTU_arrow2DErr(jj,ii-1),...
                Qd(1,ii-1),Dist_PTU_arrow2DErr(jj,ii),Qd(1,ii));
        end
        % Para calculo da velocidade
        DeltaDistP_ptu = [P_ptu(1,end) P_ptu(2,end) P_ptu(3,end)] - Prev_P_ptu;
        
        Pos_PTU_egomotion(1,ii) = Pos_PTU_egomotion(1,ii-1) - DeltaDist;
        % Adiciono erro na distancia ao target para verificar o
        % que acontece na estimativa da egomotion
        for jj=1:4
            Pos_PTU_egoIdealDistErr(jj,ii) = Pos_PTU_egoIdealDistErr(jj,ii-1) - DeltaDistErr(jj);
            Vel_PTU_egoIdealDistErr(jj,ii) = -DeltaDistErr(jj)/Delta_t;
        end
        
        if UsePTUanglesData == 1 && exist('DeltaDistPTUangReal','var')%Angulo real da PTU
            Pos_PTU_egomotionPTUangle(1,ii) = Pos_PTU_egomotionPTUangle(1,ii-1) + DeltaDistPTUangReal;
            % faz o mesmo calculo anterior mas para
            for jj=1:4
                Pos_PTU_egoPTU_Real_angDistErr(jj,ii) = ...
                    Pos_PTU_egoPTU_Real_angDistErr(jj,ii-1) + DeltaDistPTUangRealDistErr(jj);
                Vel_PTU_egoPTU_Real_angDistErr(jj,ii) = ...
                    DeltaDistPTUangRealDistErr(jj)/Delta_t;
            end
        end
        
        Vel_PTU_egomotion(1,ii) = -DeltaDist/Delta_t;
        if UsePTUanglesData == 1 && exist('DeltaDistPTUangReal','var')%Angulo real da PTU
            Vel_PTU_egomotionPTUangle(1,ii) = DeltaDistPTUangReal/Delta_t;
        end
        
        if FANUC_flag == 1,
            Vel_Fanuc(2,ii) = (FANUC_data(ii,3) - FANUC_data(ii-1,3))/Delta_t;
        else
            Vel_PTU_simulator(:,ii) = DeltaDistP_ptu/Delta_t;
        end
    end
    %Verificação se a distancia comulativa é igual - Para os dados
    %"reais"
    if VerificaSeEComulativo == 1
        Startii = 117; Endii = 345;
        if ii > Startii && ii <= Endii
            disp(DeltaDistPTUangReal);
            sumDeltaDist = sumDeltaDist + DeltaDistPTUangReal;
            if ii == Endii
                disp('soma direta antes do gráfico');disp(sumDeltaDist);
                desltadist = getEgomotionDist(Dist_PTU_arrow2D(Startii),QrInterpol(1,Startii),...
                    Dist_PTU_arrow2D(Endii),QrInterpol(1,Endii));
                disp('result:');disp(desltadist);
                disp('no gráfico:');disp(Pos_PTU_egomotionPTUangle(1,Endii) - Pos_PTU_egomotionPTUangle(1,Startii));
            end
        end
    end
    
    if AnalisaDadosComulativos == 1
        if ii == 1
            iiComulativo = [2 2 2 2];% contador
            jumpComulativo = [5 10 20 50];% salto conforme a experiencia
        else
            H1 = figure(14);set(H1,'Color',[1 1 1])
            %figure('name','DadosComulativos_Erro_Dist','Color',[1 1 1]);
            %Determina qual dos erros aplica
            errorIntensity = 1;
            if rem(ii,jumpComulativo(errorIntensity)) == 1
                %Iteracao anterior global que entrou aqui
                itAnterior = ii - jumpComulativo(errorIntensity);
                % Calcula distancia egomotion percorrida desde a ultima
                % iteração com erro gaussiano acrescentado na distancia da
                % ptu ao target
                desltadist = getEgomotionDist(Dist_PTU_arrow2DErr(errorIntensity,itAnterior),Qd(1,itAnterior),...
                    Dist_PTU_arrow2DErr(errorIntensity,ii),Qd(1,ii));
                % Guarda o aculutativo
                Pos_PTU_egoIdealDistErrComulativo(errorIntensity,iiComulativo(errorIntensity)) = ...
                    Pos_PTU_egoIdealDistErrComulativo(errorIntensity,iiComulativo(errorIntensity)-1) - desltadist;
                % incrementa o contador
                iiComulativo(errorIntensity) = iiComulativo(errorIntensity) + 1;
            end
            %Determina qual dos erros aplica
            errorIntensity = 2;
            if rem(ii,jumpComulativo(errorIntensity)) == 1
                %Iteracao anterior global que entrou aqui
                itAnterior = ii - jumpComulativo(errorIntensity);
                % Calcula distancia egomotion percorrida desde a ultima
                % iteração com erro gaussiano acrescentado na distancia da
                % ptu ao target
                desltadist = getEgomotionDist(Dist_PTU_arrow2DErr(errorIntensity,itAnterior),Qd(1,itAnterior),...
                    Dist_PTU_arrow2DErr(errorIntensity,ii),Qd(1,ii));
                % Guarda o aculutativo
                Pos_PTU_egoIdealDistErrComulativo(errorIntensity,iiComulativo(errorIntensity)) = ...
                    Pos_PTU_egoIdealDistErrComulativo(errorIntensity,iiComulativo(errorIntensity)-1) - desltadist;
                % incrementa o contador
                iiComulativo(errorIntensity) = iiComulativo(errorIntensity) + 1;
            end
            %Determina qual dos erros aplica
            errorIntensity = 3;
            if rem(ii,jumpComulativo(errorIntensity)) == 1
                %Iteracao anterior global que entrou aqui
                itAnterior = ii - jumpComulativo(errorIntensity);
                % Calcula distancia egomotion percorrida desde a ultima
                % iteração com erro gaussiano acrescentado na distancia da
                % ptu ao target
                desltadist = getEgomotionDist(Dist_PTU_arrow2DErr(errorIntensity,itAnterior),Qd(1,itAnterior),...
                    Dist_PTU_arrow2DErr(errorIntensity,ii),Qd(1,ii));
                % Guarda o aculutativo
                Pos_PTU_egoIdealDistErrComulativo(errorIntensity,iiComulativo(errorIntensity)) = ...
                    Pos_PTU_egoIdealDistErrComulativo(errorIntensity,iiComulativo(errorIntensity)-1) - desltadist;
                % incrementa o contador
                iiComulativo(errorIntensity) = iiComulativo(errorIntensity) + 1;
            end
            %Determina qual dos erros aplica
            errorIntensity = 4;
            if rem(ii,jumpComulativo(errorIntensity)) == 1
                %Iteracao anterior global que entrou aqui
                itAnterior = ii - jumpComulativo(errorIntensity);
                % Calcula distancia egomotion percorrida desde a ultima
                % iteração com erro gaussiano acrescentado na distancia da
                % ptu ao target
                desltadist = getEgomotionDist(Dist_PTU_arrow2DErr(errorIntensity,itAnterior),Qd(1,itAnterior),...
                    Dist_PTU_arrow2DErr(errorIntensity,ii),Qd(1,ii));
                % Guarda o aculutativo
                Pos_PTU_egoIdealDistErrComulativo(errorIntensity,iiComulativo(errorIntensity)) = ...
                    Pos_PTU_egoIdealDistErrComulativo(errorIntensity,iiComulativo(errorIntensity)-1) - desltadist;
                iiComulativo(errorIntensity) = iiComulativo(errorIntensity) + 1;
            end
            if PlotTime2Time == 0 || rem(ii,VelAtualizePlot) == 0 || ii >= DIM - 1
                %Plot do grafico
                plot(t(1:jumpComulativo(1):ii),Pos_PTU_egoIdealDistErrComulativo(1,1:(iiComulativo(1)-1)),...
                    t(1:jumpComulativo(2):ii),Pos_PTU_egoIdealDistErrComulativo(2,1:(iiComulativo(2)-1)),...
                    t(1:jumpComulativo(3):ii),Pos_PTU_egoIdealDistErrComulativo(3,1:(iiComulativo(3)-1)),...
                    t(1:jumpComulativo(4):ii),Pos_PTU_egoIdealDistErrComulativo(4,1:(iiComulativo(4)-1)))
                xlim(XLimPlot);
                set(gca,'FontSize',FontSize),...
                xlabel('Time (s)','FontSize',FontSize),... 
                hPlot=ylabel('Position (m)','FontSize',FontSize);
                set(hPlot, 'FontSize', FontSize)
                title('Error in distance to target and comulative error');
                legend(strcat(num2str(ErrorPercentages(1)),' % error and jump ',...
                    num2str(jumpComulativo(1)),' iterations'),...
                    strcat(num2str(ErrorPercentages(2)),' % error and jump ',...
                    num2str(jumpComulativo(2)),' iterations'),...
                    strcat(num2str(ErrorPercentages(3)),' % error and jump ',...
                    num2str(jumpComulativo(3)),' iterations'),...
                    strcat(num2str(ErrorPercentages(4)),' % error and jump ',...
                    num2str(jumpComulativo(4)),' iterations'))
            end
        end
        hold off;
    end
    % Plot
    if PlotTime2Time == 0 || rem(ii,VelAtualizePlot) == 0 || ii >= DIM - 1
        Pos_PTU_simulator(:,ii) = [P_ptu(1,end) P_ptu(2,end) P_ptu(3,end)];
        
        % Plot do grafico da distancia do target à PTU
        H6 = figure(16);set(H6,'Color',[1 1 1])
        plot(t(1:ii)-XLimPlot(1),Dist_PTU_arrow2D(1:ii));
        if ii == DIM % Lasts iteration
            addaxis(t(1:ii)-XLimPlot(1),Pos_PTU_egoPTU_Real_angDistErr(1:4,1:ii));
            addaxislabel(2,'Position of PTU with Egomotion and real angles of the PTU(m)');
            addaxis(t(1:ii)-XLimPlot(1),Pos_PTU_egomotionPTUangle(1,1:ii));
            addaxislabel(3,'Position of PTU with Egomotion(m)');
            addaxisset([-0.4 1],2);
            addaxisset([-0.4 1],3);
        end
        xlim(XLimPlot-XLimPlot(1));
        set(gca,'FontSize',FontSize),...
        xlabel('Time (s)','FontSize',FontSize),... 
        hPlot=ylabel('Distance of the PTU to de target (m)','FontSize',FontSize);
        set(hPlot, 'FontSize', FontSize)
        legend('Distance of the PTU to de target','Egomotiion with real angles',...
            '0.5% error','2% error',...
            '10% error','20% error')
        
        
        % VELOCIDADE PTU
        H6 = figure(17);set(H6,'Color',[1 1 1])
        plot(t(1:ii)-XLimPlot(1),Vel_Fanuc(2,1:ii));
        if ii == DIM % Last iteration?
            addaxis(t(1:ii)-XLimPlot(1),Vel_PTU_egomotion(1,1:ii));
            addaxis(t(1:ii)-XLimPlot(1),Vel_PTU_egomotionPTUangle(1,1:ii));
            addaxislabel(2,'Velocity of PTU with Egomotion(m)');
            addaxislabel(3,'Velocity of PTU with Egomotion and real angles of the PTU(m)');
            %addaxisset([-0.4 1],2);
            %addaxisset([-0.4 1],3);
        end
        xlim(XLimPlot-XLimPlot(1));
        set(gca,'FontSize',FontSize),...
        xlabel('Time (s)','FontSize',FontSize),... 
        hPlot=ylabel('Real velocity of PTU mesured by the Fanuc(m)','FontSize',FontSize);
        set(hPlot, 'FontSize', FontSize)
        
        % Posição PTU c/ Egomotion
        H2 = figure(15);set(H2,'Color',[1 1 1])
        subplot(3,2,3),
        plot(t(1:ii),Pos_PTU_egomotion(1,1:ii),t(1:ii),...
            Pos_PTU_egoIdealDistErr(1,1:ii),t(1:ii),...
            Pos_PTU_egoIdealDistErr(2,1:ii),t(1:ii),...
            Pos_PTU_egoIdealDistErr(3,1:ii),t(1:ii),...
            Pos_PTU_egoIdealDistErr(4,1:ii))%,t(1:ii),FANUC_data(1:ii,3)-FANUC_data(1,3),'r:');
        xlim(XLimPlot);
        set(gca,'FontSize',FontSize),...
        xlabel('Time (s)','FontSize',FontSize),... 
        hPlot=ylabel('Position (m)','FontSize',FontSize);
        set(hPlot, 'FontSize', FontSize)
        title('Posição PTU c/ Egomotion');
        
        % Velocidade PTU c/Egomotion
        subplot(3,2,4),
        plot(t(1:ii),Vel_PTU_egomotion(1,1:ii))%,t(1:ii),...
%             Vel_PTU_egoIdealDistErr(1,1:ii),t(1ii),...
%             Vel_PTU_egoIdealDistErr(2,1:ii),t(1:ii),...
%             Vel_PTU_egoIdealDistErr(3,1:ii),t(1:ii),...
%             Vel_PTU_egoIdealDistErr(4,1:ii))
        xlim(XLimPlot);
        set(gca,'FontSize',FontSize),...
        xlabel('Time (s)','FontSize',FontSize),... 
        hPlot=ylabel('Velocity (m/s)','FontSize',FontSize);
        set(hPlot, 'FontSize', FontSize)
        title('Velocidade PTU c/Egomotion');
        
        if UsePTUanglesData == 1%Angulo real da PTU
            % Posição PTU c/ Egomotion c/anglos Reais
            subplot(3,2,5),plot(t(1:ii),Pos_PTU_egomotionPTUangle(1,1:ii),...
                t(1:ii),Pos_PTU_egoPTU_Real_angDistErr(1,1:ii),...
                t(1:ii),Pos_PTU_egoPTU_Real_angDistErr(2,1:ii),...
                t(1:ii),Pos_PTU_egoPTU_Real_angDistErr(3,1:ii),...
                t(1:ii),Pos_PTU_egoPTU_Real_angDistErr(4,1:ii))
            xlim(XLimPlot);
            set(gca,'FontSize',FontSize),...
            xlabel('Time (s)','FontSize',FontSize),... 
            hPlot=ylabel('Position (m)','FontSize',FontSize);
            set(hPlot, 'FontSize', FontSize)
            title('Posição PTU c/ Egomotion c/anglos Reais');
            
            % Velocidade PTU c/Egomotion c/anglos Reais
            subplot(3,2,6),plot(t(1:ii),Vel_PTU_egomotionPTUangle(1,1:ii))%,...
%                  t(1:ii),Vel_PTU_egoPTU_Real_angDistErr(1,1:ii),...
%                  t(1:ii),Vel_PTU_egoPTU_Real_angDistErr(2,1:ii),...
%                  t(1:ii),Vel_PTU_egoPTU_Real_angDistErr(3,1:ii),...
%                  t(1:ii),Vel_PTU_egoPTU_Real_angDistErr(4,1:ii))
            xlim(XLimPlot);
            set(gca,'FontSize',FontSize),...
            xlabel('Time (s)','FontSize',FontSize),... 
            hPlot=ylabel('Velocity (m/s)','FontSize',FontSize);
            set(hPlot, 'FontSize', FontSize)
            title('Velocidade PTU c/Egomotion c/anglos Reais');
        end
        
        if FANUC_flag == 1,
            % PTU''s YY Position with Fanuc Data
            subplot(3,2,1),plot(t(1:ii),FANUC_data(1:ii,3)-FANUC_data(1,3))
            xlim(XLimPlot);
            title('PTU''s YY Position with Fanuc Data');
            set(gca,'FontSize',FontSize),...
            xlabel('Time (s)','FontSize',FontSize),... 
            hPlot=ylabel('Position (m)','FontSize',FontSize);
            set(hPlot, 'FontSize', FontSize)
            
            % PTU''s YY Velocity with Fanuc Data
            subplot(3,2,2),plot(t(1:ii),Vel_Fanuc(2,1:ii))
            xlim(XLimPlot);
            set(gca,'FontSize',FontSize),...
            xlabel('Time (s)','FontSize',FontSize),... 
            hPlot=ylabel('Velocity (m/s)','FontSize',FontSize);
            set(hPlot, 'FontSize', FontSize)
            title('PTU''s YY Velocity with Fanuc Data');
            
        else
            subplot(3,2,1),plot(t(1:ii),Pos_PTU_simulator(2,1:ii))
            xlim(XLimPlot);
            set(gca,'FontSize',FontSize),...
            xlabel('Time (s)','FontSize',FontSize),... 
            hPlot=ylabel('Position (m)','FontSize',FontSize);
            set(hPlot, 'FontSize', FontSize)
            title('Posição YY da PTU no simulador');
            subplot(3,2,2),plot(t(1:ii),-Vel_PTU_simulator(2,1:ii))
            xlim(XLimPlot);
            title('Velocidade PTU no simulador');
        end
    end
    Prev_P_ptu = [P_ptu(1,end) P_ptu(2,end) P_ptu(3,end)];
    
    %
    % -- - TILT - --
    %
    Altura_PTU_egomotion(ii) = -Dist_PTU_arrow3D(ii)*sin(Qd(2,ii));
    % Calcula a altura com o egomotion para os dados 'ideais' e aplica erro
    % gaussiano à distância 3D
    for jj=1:4
        Altura_PTU_egomotionIdealDistErr(jj,ii) = -getNormGausVar(Dist_PTU_arrow3D(ii),ErrorPercentages(jj))*sin(Qd(2,ii));
    end
    if FANUC_flag == 1 && UsePTUanglesData == 1% Angulo real da PTU
        Altura_PTU_egomotionPTUangle(ii) = Dist_PTU_arrow3D(ii)*sin(QrInterpol(2,ii))...
            -Altura_PTU_egomotionPTUangle(2);
        % aplica erro com dados 'reais'
        for jj=1:4
            Altura_PTU_egomotionRealDistErr(jj,ii) = getNormGausVar(Dist_PTU_arrow3D(ii),...
                ErrorPercentages(jj))*sin(QrInterpol(2,ii))-Altura_PTU_egomotionRealDistErr(2);
        end
    end
    % Plot dos gráficos
    if PlotTime2Time == 0 || rem(ii,VelAtualizePlot) == 0 || ii >= DIM - 1
        
        % Plot do grafico da distancia do target à PTU
        H6 = figure(18);set(H6,'Color',[1 1 1])
        plot(t(1:ii)-XLimPlot(1),Dist_PTU_arrow3D(1:ii));
        if ii == DIM % Lasts iteration
            addaxis(t(1:ii)-XLimPlot(1),-Altura_PTU_egomotionPTUangle(1:ii));
            addaxis(t(1:ii)-XLimPlot(1),Altura_PTU_egomotion(1:ii));
            addaxislabel(2,'Position of PTU with Egomotion and real angles(m)');
            addaxislabel(3,'Position of PTU with Egomotion (m)');
            addaxisset([-0.5 1],2);
            addaxisset([-0.5 1],3);
        end
        xlim(XLimPlot-XLimPlot(1));
        set(gca,'FontSize',FontSize),...
        xlabel('Time (s)','FontSize',FontSize),... 
        hPlot=ylabel('Distance of the PTU to de target (m)','FontSize',FontSize);
        set(hPlot, 'FontSize', FontSize)
        
        
        H5 = figure(4);set(H5,'Color',[1 1 1])
        if FANUC_flag == 1
            subplot(3,1,1),plot(t(1:ii),FANUC_data(1:ii,4)-FANUC_data(1,4))
            xlim(XLimPlot);
            title('Posiçao ZZ da PTU c/Fanuc');
        else
            subplot(3,1,1),plot(t(1:ii),Pos_PTU_simulator(3,1:ii))
            xlim(XLimPlot);
            title('Posição ZZ da PTU no simulador');
        end
        subplot(3,1,2),
        plot(...%t(1:ii),Altura_PTU_egomotion(1:ii),...
            t(1:ii),Altura_PTU_egomotionIdealDistErr(1,1:ii),...
            t(1:ii),Altura_PTU_egomotionIdealDistErr(2,1:ii),...
            t(1:ii),Altura_PTU_egomotionIdealDistErr(3,1:ii),...
            t(1:ii),Altura_PTU_egomotionIdealDistErr(4,1:ii))
        xlim(XLimPlot);
            set(gca,'FontSize',FontSize),...
            xlabel('Time (s)','FontSize',FontSize),... 
            hPlot=ylabel('Position (m)','FontSize',FontSize);
            set(hPlot, 'FontSize', FontSize)
        if IsCubeDataExperiment == 1
            draw_cube_movementes_separator_cont_VEL;
        end
        title('Posição em ZZ da PTU c/egomotion');
        if UsePTUanglesData == 1%Angulo real da PTU
            subplot(3,1,3),
            plot(...%t(1:ii),Altura_PTU_egomotionPTUangle(1:ii),...
                t(1:ii),Altura_PTU_egomotionRealDistErr(1,1:ii),...
                t(1:ii),Altura_PTU_egomotionRealDistErr(2,1:ii),...
                t(1:ii),Altura_PTU_egomotionRealDistErr(3,1:ii),...
                t(1:ii),Altura_PTU_egomotionRealDistErr(4,1:ii));
            hold on;
            xlim(XLimPlot);
            title('Posição em ZZ da PTU c/egomotion c/PTU Real angles');
            if IsCubeDataExperiment == 1
                draw_cube_movementes_separator_cont_VEL;
            end
            hold off;
        end
    end
end

if IsCubeDataExperiment == 1
    figure(9);
    plot(t,FANUC_data(:,2) + FANUC_data(:,3))
end


break;

%______________________________________________________________
% Performance measure
%______________________________________________________________
if FANUC_flag == 1
    figure(6)
    subplot(2,1,1), plot(t,Qd(1,:)*180/pi,'r:', t,Q(1,:)*180/pi,'k', tr,(-Qr(1,:)+1500)/10+5.83,'b')
    xlabel('Time (s)'), ylabel('PAN: Q1 (deg)'), legend('q1d','q1','q1r')
    xlim([13 21])
    subplot(2,1,2), plot(t,Qd(2,:)*180/pi,'r:', t,Q(2,:)*180/pi,'k', tr,(-Qr(2,:)+1500)/10+33.675,'b')
    xlabel('Time (s)'), ylabel('TILT: Q2 (deg)'), legend('q2d','q2','q2r')
    xlim([13 21])
end
figure(7)
subplot(2,1,1), plot(t,(Qd(1,:)-Q(1,:))*180/pi,'r')
xlabel('Time (s)'), ylabel('PAN err (deg)')
subplot(2,1,2), plot(t,(Qd(2,:)-Q(2,:))*180/pi,'r')
xlabel('Time (s)'), ylabel('TILT err (deg)')
figure(8)
subplot(2,1,1), plot(t,ERR_img(1,:),'r')
xlabel('Time (s)'), ylabel('XX-err (m)'), title('Arrow Centroid Projected on the Image Plane')
subplot(2,1,2), plot(t,ERR_img(2,:),'r')
xlabel('Time (s)'), ylabel('YY-err (m)')
%______________________________________________________________