function [nearestIndex,initialDistance] = findnearestneighbors(data,nNearest,nSkip,nMaxDt)
% FINDNEARESTNEIGHBORS Find the nearest neighbors in a state-data set.
%   FINDNEARESTNEIGHBORS(DATA,NNEAREST,NSKIP,NMAXDT) returns the indicies of the 
%   nearest neighbors. DATA should be formated such that the rows correspond to 
%   samples of the state and the columns are the states at that sample. 
%
%   NSKIP is the number of data samples to skip both forward and backward in 
%   time to avoid sampling nearest neighbors on the same cycle. 
%
%   NMAXDT is the number of time samples that nearest neighbors will be tracked
%   forward in time.
%
%   [NEARESTINDEX,INITIALDISTANCE] = FINDNEARESTNEIGHBORS(...) 
%   returns the initial distance between nearest neighbors.
% 
%   Input
%       DATA     | m x n | double | m is # of samples and n is # of states
%       NNEAREST | 1 x 1 | double | integer, # of nearest neighbors
%       NMAXDT   | 1 x 1 | double | integer, # samples will be tracked forward 
%
%   Output
%       NEARESTINDEX    | (m-nMaxDt) x nNEAREST | double | [1]
%       INITIALDISTANCE | (m-nMaxDt) x nNEAREST | double | [2]
%   
%       [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.
%       [1] INITIALDISTANCE is a matrix of the initial distance between nearest 
%           neigbors in DATA.
%
%   Example 
%
%   Reference
%
%   See also COMPUTEDIVERGENCE.  

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

  nSamples = size(data,1); % Total # of time samples
  data     = data(1:(nSamples-nMaxDt),:); % Truncate data by nMaxDt
  nSamples = size(data,1); % Actual # of samples that may be tracked forward

  nearestIndex    = zeros(nSamples,nNearest); % Initialize nearestIndex
  initialDistance = zeros(nSamples,nNearest); % Initialize initialDistance

% Find nearest neighbor for every time sample    
  for t = 1:nSamples             
      statei   = ones(nSamples,1)*data(t,:); 
      distance = sum((data-statei).^2,2);    % Compute Euclid distance squared       
    
      % Skip values close to current time index
      skipStart = max(1,t-nSkip);
      skipEnd   = min(t+nSkip,nSamples);
      skip      = skipStart:skipEnd; % Indicies to skip
      distance(skip) = inf; % Replace with inf
    
      % Find nNearest nearest neighbors for each time sample
      for j = 1:nNearest        
          
          % Skip values around nearest neighbors that have already been found
          for k = 1:j-1
              skipStart = max(1,nearestIndex(t,k)-nSkip);
              skipEnd   = min(nearestIndex(t,k)+nSkip,nSamples);
              skip      = skipStart:skipEnd; % Indicies to skip
              distance(skip) = inf;  % Replace with inf
          end;        
          [initialDistance(t,j),nearestIndex(t,j)] = min(distance);   
      end;
  end;
  
% Convert to Euclid distance
  initialDistance = sqrt(initialDistance); 

end % END findnearestneighbors