% 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')
start_pos = 1;
end_pos = size(Positions,1);

figure;
plot(Fanuc(:,1),Fanuc(:,2)-Fanuc(1,2),Fanuc(:,1),Fanuc(:,3)-Fanuc(1,3),Fanuc(:,1),Fanuc(:,4)-Fanuc(1,4),Positions(start_pos:end_pos,1),Positions(start_pos:end_pos,[2 3]),Positions(start_pos:end_pos,1),304,'-b',Positions(start_pos:end_pos,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: x','arrows position in image: y','set point:x','set point:y');

% 
% 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')

