% César Sousa 9-10-2014

clc; clear all; close all;

format long % to print all digits of the number in the command window

%load the info from the files
Positions = csvread('find_arrow_position.csv',1,0);
State = csvread('find_arrow_state.csv',1,0);
Fanuc = csvread('positions.txt');
Servos = csvread('head_comands.txt');

Fanuc = Fanuc(1:2:end,:); % delete fanuc configuration values
% eliminar coluna com o commando da direcao e ultima coluna que não existe
% no ficheiro mas que aparece aqui não sei porque!
Servos = Servos(:,[1 3 4]);

% configure the time lime to be equal to the othes
Fanuc(:,1) = Fanuc(:,1) * 1000000000; 
Servos(:,1) = Servos(:,1) * 1000000000; 

%get the starting time of the experience
start_time_Positions = Positions(1,1)
start_time_State = State(1,1)
start_time_Fanuc = Fanuc(1,1)
start_time = max([start_time_Positions start_time_State start_time_Fanuc])

%get the ending time of the experience
end_time_Positions = Positions(end,1)
end_time_State = State(end,1)
end_time_Fanuc = Fanuc(end,1)
end_time = min([end_time_Positions end_time_State end_time_Fanuc])

if end_time < start_time
    warning('qqer coisa esta mal nos tempos!!');
end

%delete the first rows that dont have match with the other data.
Positions = del_rows_until(Positions,start_time);
State = del_rows_until(State,start_time);
Fanuc = del_rows_until(Fanuc,start_time);

%delete the last rows that dont have match with the other data.
Positions = del_rows_from(Positions,end_time);
State = del_rows_from(State,end_time);
Fanuc = del_rows_from(Fanuc,end_time);

% Positions = set_time_line(Positions);
% State = set_time_line(State);
% Fanuc = set_time_line(Fanuc);

% figure;
% plot(State(:,1),State(:,3:end));
% title('set point for servos came from Visp')
% xlabel('time')
% ylabel('setpoints')
% 
% figure;
% plot(Fanuc(:,1),Fanuc(:,[2 3 4]));
% title('Fanuc')
% xlabel('time')
% ylabel('setpoints')
% 
% figure;
% plot(Servos(:,1),Servos(:,2:end));
% title('Servos')
% xlabel('time')
% ylabel('setpoints')

figure;
plot(Fanuc(:,1),Fanuc(:,[2 3 4]),Positions(:,1),Positions(:,[2 3]),Positions(:,1),304,'-b',Positions(:,1),263.4,'-g');
title('Positions of the find_arrow algorithm')
xlabel('time')
ylabel('setpoints')
legend('Fanuc: x','Fanuc: y','Fanuc: z','arrows position in image:');

% 
% figure;
% plot(State(:,1),State(:,3:end)*25+290,Positions(:,1),Positions(:,2:end));
% title('set point for servos came from Visp + center position of the find_arrow algorithm')
% xlabel('time')
% ylabel('setpoints')