% first srcipt to read the csv files of the experiments for master thesys
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('servos_position.txt');
%     Visp = csvread('track_arrow.txt');

% delete fanuc configuration values
    Fanuc = Fanuc(1:2:end,:); 
    plot(Fanuc(:,1),Fanuc(:,[2 3 4]))
   

% configure the time lime to be equal to the othes
    Positions(:,1) = Positions(:,1) / 1000000000; 
    State(:,1) = State(:,1) / 1000000000; 
%     Visp(:,1) = Visp(:,1) * 1000000000;

% get the starting time of the experience
    start_time = max([Servos(1,1) Fanuc(1,1) State(1,1) Positions(1,1)])
    
% get the ending time of the experience
    end_time = min([Fanuc(end,1) State(end,1) Positions(end,1)])

    if end_time < start_time
        warning('qqer coisa esta mal nos tempos!!')
    end
    
%     Positions(:,1) = set_time_line(Positions(:,1),start_time)
    State(:,1) = set_time_line(State(:,1),start_time);
    Fanuc(:,1) = set_time_line(Fanuc(:,1),start_time);
%     Servos(:,1) = set_time_line(Servos(:,1),start_time)
       A1=trapz(State(:,1),State(:,[4]))
    plotyy(Fanuc(:,1),Fanuc(:,[3]),State(:,1),State(:,[4]));
    hold on;
    legend('fanuc','command to servos');