function [stateDistance] = tracknearestneighbors(data, nearestIndex, nMaxDt)
% TRACKNEARESTNEIGHBORS Track nearest neighbors separation forward in time.
%   TRACKNEARESTNEIGHBORS(DATA,NEARESTINDEX,NMAXDT) returns the distance between
%   nearest neigbors identified by NEARESTINDEX forward NMAXDT samples, where 
%   NMAXDT is an integer.
% 
%   Input
%       DATA         | m x n           | double | # of samples x # of states
%       NEARESTINDEX | (m-nMaxDt) x nn | double | [1]
%       NMAXDT       | 1 x 1           | double | # of samples to follow forward
%
%       [1] NEARESTINDEX is a matrix of the indicies of nearest neigbors in DATA
%           that correspond to sample I, where I is row index of NEARESTINDEX.
%           NN is the number of nearest neighbors to sample I.
%
%   Output
%       STATEDISTANCE | (m-nMaxDt) x (nMaxDt+1) x nn | double | [2]
%
%       [2] STATEDISTANCE is a 3-dimensional matrix of the distance of the
%       NN nearest neighbors from each row index tracked forward nMaxDt time
%       samples. For instance, the stateDistance(1,:,2) is the distance between
%       the state at time sample 1, and its second nearest neighbor, tracked
%       forward nMaxDt samples.
%   
%   Example       
%
%   Reference
%
%   See also COMPUTEDIVERGENCE.  

% Created by: Kevin P. Granata Musculoskeletal Biomechanics Laboratory
%             http://www.biomechanics.esm.vt.edu
% Last Revision: 19 June 2007

% Check that nMaxDt is an integer
  if (fix(nMaxDt) ~= nMaxDt)
      error('Input argument nMaxDt should be an integer.');
  end;

% Determine total number of data samples and number of nearest neighbors
% Note that divegence can not be tracked for samples that are within nMaxDt from
% the end of the entire data set.
  nSamples = size(data,1);
  nNearest = size(nearestIndex,2);

% Check that nearestIndex only contains indicies that may be tracked forward in
% time a minimum of nMaxDt samples
  if any(any(nearestIndex + nMaxDt > nSamples))
      error(['nearestIndex contains indicies that cannot be tracked ',...
             ' forward nMaxDt samples. It may be necessary to re-execute ',...
             'findnearestneighbors if nMaxDt has been changed.']);
  end;  

% Initialize stateDistance
  stateDistance = zeros(nSamples-nMaxDt,nMaxDt+1,nNearest);

% Valid sample time indicies that may be tracked forward nMaxDt
  t = 1:(nSamples-nMaxDt);

% Track state distance of nearest neighbors
  for dt = 0:nMaxDt       
      for i = 1:nNearest;        
          stateVector = data(t+dt,:)-data(nearestIndex(t,i)+dt,:);
          %stateDistance(t,dt+1,i) = sqrt(sum(stateVector.^2,2));
          stateDistance(t,dt+1,i) = sum(stateVector.^2,2);
      end % i
  end; % dt
  
% Convert to Euclid distance
  stateDistance = sqrt(stateDistance); 

end % END tracknearestneighbors