%---------------------------------
%-- Visual tracking
%---------------------------------
clc
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

load erro_alinhamento.txt
load erro_mov_vertical.txt

%---------------------------------
% Alinhamento
%---------------------------------
Eh_cc = erro_alinhamento;
%Texe = time_mov_horiz_far_nocos(end,end)-time_mov_horiz_far_nocos(1,2);
%Ts = (Texe/length(Eh_cc));
t = linspace(0,1.83,32);

norm_Eh_cc = sqrt( (Eh_cc(:,1)-Eh_cc(:,2)).^2 + (Eh_cc(:,3)-Eh_cc(:,4)).^2 );
figure(11)
plot(t,norm_Eh_cc)
xlabel('Tempo(s)')
ylabel('Norma do erro(pixels)')
grid on
axis([0 2 0 80])
figure(12)
plot(t,Eh_cc(:,5:7))
grid on
legend('pan','tilt','tronco')
xlabel('Tempo(s)')
ylabel('graus (º)')

%---------------------------------
% 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:10e-7*Ts:10e-7*(Texe-Ts);

norm_Eh_cc = sqrt( (Eh_cc(:,2)-Eh_cc(:,3)).^2 + (Eh_cc(:,4)-Eh_cc(:,5)).^2 );
figure(1)
plot(t,norm_Eh_cc)
xlabel('Tempo(s)')
ylabel('Norma do erro(pixels)')
grid on
figure(2)
plot(t,Eh_cc(:,6:8))
grid on
legend('pan','tilt','tronco')
xlabel('Tempo(s)')
ylabel('graus (º)')
%axis([0 180 0 80])
%---------------------------------
% 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:10e-7*Ts:10e-7*(Texe-Ts);

norm_Eh_cc = sqrt( (Eh_cc(:,2)-Eh_cc(:,3)).^2 + (Eh_cc(:,4)-Eh_cc(:,5)).^2 );
figure(3)
plot(t,norm_Eh_cc)
xlabel('Tempo(s)')
ylabel('Norma do erro(pixels)')
grid on
figure(4)
plot(t,Eh_cc(:,6:8))
grid on
legend('pan','tilt','tronco')
xlabel('Tempo(s)')
ylabel('graus (º)')
%axis([0 180 0 80])
%---------------------------------
% 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:10e-7*Ts:10e-7*(Texe-Ts);

norm_Eh_cc = sqrt( (Eh_cc(:,2)-Eh_cc(:,3)).^2 + (Eh_cc(:,4)-Eh_cc(:,5)).^2 );
figure(5)
plot(t,norm_Eh_cc)
xlabel('Tempo(s)')
ylabel('Norma do erro(pixels)')
grid on
figure(6)
plot(t,Eh_cc(:,6:8))
grid on
legend('pan','tilt','tronco')
xlabel('Tempo(s)')
ylabel('graus (º)')
%axis([0 180 0 80])
%---------------------------------
% 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:10e-7*Ts:10e-7*(Texe-Ts);

norm_Eh_cc = sqrt( (Eh_cc(:,2)-Eh_cc(:,3)).^2 + (Eh_cc(:,4)-Eh_cc(:,5)).^2 );
figure(7)
plot(t,norm_Eh_cc)
xlabel('Tempo(s)')
ylabel('Norma do erro(pixels)')
grid on
figure(8)
plot(t,Eh_cc(:,6:8))
grid on
legend('pan','tilt','tronco')
xlabel('Tempo(s)')
ylabel('graus (º)')
%axis([0 180 0 80])
%---------------------------------
% Movimento vertical
%---------------------------------
Eh_cc = erro_mov_vertical;
%Texe = time_mov_horiz_far_nocos(end,end)-time_mov_horiz_far_nocos(1,2);
%Ts = (Texe/length(Eh_cc));
t = linspace(0,40e-3*500,499);

norm_Eh_cc = sqrt( (Eh_cc(:,1)-Eh_cc(:,2)).^2 + (Eh_cc(:,3)-Eh_cc(:,4)).^2 );
figure(9)
plot(t,norm_Eh_cc)
xlabel('Tempo(s)')
ylabel('Norma do erro(pixels)')
grid on
figure(10)
plot(t,Eh_cc(:,5:7))
grid on
legend('pan','tilt','tronco')
xlabel('Tempo(s)')
ylabel('graus (º)')
