%---------------------------------
%-- Visual tracking
%---------------------------------
 
clear all
close all


load erro_mov_horiz_close_cos.txt
load erro_mov_horiz_close_nocos.txt
load erro_mov_horiz_far_cos.txt
load erro_mov_horiz_far_nocos.txt

load time_mov_horiz_close_cos.txt
load time_mov_horiz_close_nocos.txt
load time_mov_horiz_far_cos.txt
load time_mov_horiz_far_nocos.txt

%---------------------------------
% Movimento horizontal: close_cos
%---------------------------------
Eh_cc = erro_mov_horiz_close_cos;
Texe = time_mov_horiz_close_cos(end,end)-time_mov_horiz_close_cos(1,2);
Ts = (Texe/length(Eh_cc));
t = 0:10e3*Ts:10e3*(Texe-Ts);

norm_Eh_cc = sqrt( (Eh_cc(:,2)-Eh_cc(:,3)).^2 + (Eh_cc(:,4)-Eh_cc(:,5)).^2 );
figure(1)
subplot(2,1,1), plot(t,norm_Eh_cc), title('close_cos')
subplot(2,1,2), plot(Eh_cc(:,6:8)), legend('pan','tilt','trunk')

%---------------------------------
% Movimento horizontal: close_nocos
%---------------------------------
Eh_cc = erro_mov_horiz_close_nocos;
Texe = time_mov_horiz_close_nocos(end,end)-time_mov_horiz_close_nocos(1,2);
Ts = (Texe/length(Eh_cc));
t = 0:10e3*Ts:10e3*(Texe-Ts);

norm_Eh_cc = sqrt( (Eh_cc(:,2)-Eh_cc(:,3)).^2 + (Eh_cc(:,4)-Eh_cc(:,5)).^2 );
figure(2)
subplot(2,1,1), plot(t,norm_Eh_cc), title('close_nocos')
subplot(2,1,2), plot(Eh_cc(:,6:8)), legend('pan','tilt','trunk')

%---------------------------------
% Movimento horizontal: far_cos
%---------------------------------
Eh_cc = erro_mov_horiz_far_cos;
Texe = time_mov_horiz_far_cos(end,end)-time_mov_horiz_far_cos(1,2);
Ts = (Texe/length(Eh_cc));
t = 0:10e3*Ts:10e3*(Texe-Ts);

norm_Eh_cc = sqrt( (Eh_cc(:,2)-Eh_cc(:,3)).^2 + (Eh_cc(:,4)-Eh_cc(:,5)).^2 );
figure(3)
subplot(2,1,1), plot(t,norm_Eh_cc), title('far_cos')
subplot(2,1,2), plot(Eh_cc(:,6:8)), legend('pan','tilt','trunk')

%---------------------------------
% Movimento horizontal: far_nocos
%---------------------------------
Eh_cc = erro_mov_horiz_far_nocos;
Texe = time_mov_horiz_far_nocos(end,end)-time_mov_horiz_far_nocos(1,2);
Ts = (Texe/length(Eh_cc));
t = 0:10e3*Ts:10e3*(Texe-Ts);

norm_Eh_cc = sqrt( (Eh_cc(:,2)-Eh_cc(:,3)).^2 + (Eh_cc(:,4)-Eh_cc(:,5)).^2 );
figure(4)
subplot(2,1,1), plot(t,norm_Eh_cc), title('far_nocos')
subplot(2,1,2), plot(Eh_cc(:,6:8)), legend('pan','tilt','trunk')