% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% Function that reads, clean and adjust the inertial data. Cart data is the%
% data obtained from the FANUC whilst the filt data is the data obtaind    %
% from complementary_filter (comes from the sensors)                       %
%                                                                          %
%                                                                          %
% [cart, filt]=function_inertialData(initCart, endCart, initFilt, endFilt) %
%  INPUTS:   Name           | Type    | Dimensions | Description           %
%            initCart       | int     | 1x1        | beginning of cart data%
%            endCart        | int     | 1x1        | end of cart data      %
%            initFilt       | int     | 1x1        | beginning of filt data%
%            endFilt        | int     | 1x1        | end of filt data      %
%                                                                          %
% OTUPUTS:   Name           | Type    | Dimensions | Description           %
%            cart           | measure | nx13       | FANUC measures        %
%            filt           | measure | nx11       | Sensor measures       %
%                                                                          %
%                                                                          %
%           1 | 2 | 3 | 4 | 5 | 6 | 7 |  8 |  9 | 10 | 11 | 12 | 13 |      %
% cart = time | x | y | z | w | p | r | d1 | d2 | d3 | d4 | d4 | d6 |      %
% filt = time |:x |:y |:z |<x |<y |<z |.<x |.<y |.<z |  ? |                %
%                                                                          %
%   Where: :x is the linear acceleration in x                              %
%          <x is the angular position of x                                 %
%         .<x is the angular velocity of x                                 %
%          d1 to d6 are FANUC definitions                                  %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %
% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %

function [cart, filt]=function_inertialData(config, nbr_exp)
initCart = config(1,1);
endCart  = config(2,1);
initFilt = config(3,1);
endFilt  = config(4,1);
% Read csv files
expCart = csvread(sprintf('data/rosbag_Article%d/fanuc_cart.csv',nbr_exp),1,0);
expFilt = csvread(sprintf('data/rosbag_Article%d/fanuc_filt.csv',nbr_exp),1,0);
% Converts the nanoseconds into seconds. (exp_filt as a delay of 0.75
% seconds regardind dimensions 8:10...
expCart(:,1) = expCart(:,1)/1e9;
expFilt(:,1) = expFilt(:,1)/1e9;
% ... therefore, we use this cycle in order to rebalance that delay. This
% delay is due the time that the complementary_filter node (which is poorly
% made) takes to calculate the orientation. (It waits for around 25 values
% before outputting the first value.
delay_time = 0.75;
for i = 1:size(expFilt,1)
    for j = i:size(expFilt,1)-1
        if(sign(expFilt(i,1) - (expFilt(j,1) - delay_time)) ~= sign(expFilt(i,1) - (expFilt(j+1,1) - delay_time)))
            % There is sign variation, therefore the correct value is in
            % between j and j+1
            if(abs(expFilt(i,1) - expFilt(j,1)) > abs(expFilt(i,1) - expFilt(j+1,1)))
                % Closer to j+1
                expFilt(i,8:10) = expFilt(j+1,8:10);
            else
                expFilt(i,8:10) = expFilt(  j,8:10);
            end
        end
    end
end
% This operation allows that the fanuc and sensors referential are the same
% in order to be able to compare the results and converts the angular
% velocity measured from the sensor into SI
expFilt(:,8:10) = expFilt(:,8:10) * 180/pi;
expFilt(:,   9) = expFilt(:,   9) * -1;
expFilt(:,  10) = expFilt(:,  10) + 415.4883;
expFilt(:,5: 7) = expFilt(:,5: 7) * 0.07; 
% The Fanuc Robot uses a set of angles that have meaning when looked
% together. For intance, if we have a rotation of 90º in y axis the values
% that appear are {-90;0;-90}. However if we have a rotation of -90º then
% the values represented are {90;0;90}. If the rotation in y is 0º then the
% values are {-180;-90;0}. This suggests that in order to obtain a
% comparable reading of the angle in the y axis we have to add 90º and then
% multiply by 1 (if s >= 0) or by -1 (if s < 0)
if(nbr_exp == 1); expCart(:,6) = (expCart(:,6) + 90) .* (-sign(expCart(:,7)));
else              expCart(:,6) = (expCart(:,6) - 90) .* (-sign(expCart(:,7)));
end
% In this case, since the p angle has been corrected (exp_cart(:,6)), then
% the values of w and s (exp_cart(:,5) and exp_cart(:,7)) no longer obey
% the Fanuc angle convention, therefore their value is zero. (It is zero
% because there is no rotation in either x or z axis. This is known due to
% the nature of the experiment made)
expCart(:,5) = 0;
expCart(:,7) = 0;
% Before finishing, we need to rearrange the expFilt in order to swap the
% order of its columns of a) into b):
%          a) LinAcellX LinAcellY LinAcellZ
%             AngVelX   AngVelY   AngVelZ
%             AngPosX   AngPosY   AngPosZ
%          b) LinAcellX LinAcellY LinAcellZ
%             AngPosX   AngPosY   AngPosZ
%             AngVelX   AngVelY   AngVelZ
for i = 1:6
    tmp(:,i) = expFilt(:,i+4); 
end
expFilt(:, 5) = tmp(:,4);
expFilt(:, 6) = tmp(:,5);
expFilt(:, 7) = tmp(:,6);
expFilt(:, 8) = tmp(:,1);
expFilt(:, 9) = tmp(:,2);
expFilt(:,10) = tmp(:,3);
% Finally, we can create the variables that will be studied and analysed.
% Use the data that is of interest (This is because the recording of the
% experiment was not synchronized. In order to solve this I started to
% record the experiment at different times and then use the data that
% corresponds to a certain number of cycles.)
cart = expCart(initCart:endCart,:);
filt = expFilt(initFilt:endFilt,:);