function [ data_ind, truth_ind ] = intersect_sorted( data, truth )
% [ data_ind, truth_ind ] = intersect_sorted( data, truth )
%
% Description:
%
% This function intersects the sorted data and truth vectors and output the
% intersected indices. This function only works when both input vectors are
% sorted. 
%
% The default "intersect" function in Matlab does not make the sorted 
% assumption. As a result, it uses binary search to find the common
% elements from one vector to the other. And the time complexity is O(nlogn).
% With "intersect_sorted", the common elements can be found with linear
% time complexity O(n). It is much faster than "intersect" particularly when
% the size of input vectors are large.
%
% Input:
% data:  Sorted time vector of the data file
%        (e.g., InputData(:,InputDataName{k}{5}) in processDist() )
% truth: Sorted time vector of the truth file
%        (e.g., POSPac(:,dTime) in processDist() )
%
% Output:
% data_ind: The indices of the common elements from the data vector
% truth_ind: The indices of the common elements from the truth vector

 % Find the length of the data and truth time vectors
 data_length = length(data);
 truth_length = length(truth);
 
 % It is faster to initialize the output vectors first than change the size
 % in the loop below
 data_ind = NaN(data_length,1);
 truth_ind = NaN(data_length,1);
 
 % Find the first apperance of the data in the truth vector
 start_ind = find(data(1) == truth);
 % If not, start with the first index
 if(isempty(start_ind))
     start_ind = 1;
 end
 
 % Index for data
 ii = 1;
 % Index for truth
 jj = start_ind;
 % Counter for the output vector index
 cnt = 1;
 
while( ii <= data_length && jj <= truth_length )
    % If the data time is smaller than the truth time
    % move the data time forward
    if( data(ii) < truth(jj))
        ii = ii + 1;
    % If the data time is bigger than the truth time
    % move the truth time forward
    elseif ( data(ii) > truth(jj))
        jj = jj + 1;
    % If they are equal, move both forward and write
    % the indices to the output vectors
    else
        data_ind(cnt) = ii;
        truth_ind(cnt) = jj;
        ii = ii + 1;
        jj = jj + 1;
        cnt = cnt + 1;
    end
end

% Get rid of the "not used" space in the vectors
data_ind = data_ind(~isnan(data_ind));
truth_ind = truth_ind(~isnan(truth_ind));

end

