function [M,Vr,Vc] = meanShiftIm( X,sigSpt,sigRng,softFlag,maxIter,minDel )
% Applies the meanShift algorithm to a joint spatial/range image.
%
% See "Mean Shift Analysis and Applications" by Comaniciu & Meer for info.
%
% Assumes X is an MxNxP array, where an X(i,j,:) represents the range data
% at locations (i,j).  This function runs meanShift on each of the MxN data
% points.  It takes advantage of the lattice structure of an image for
% efficiency (it only needs to calculate full distance between two points
% if they are near each other spatially).
%
% In the original formulation of the algorithm, after normalization of the
% data, the search window around each point x has radius 1 (ie
% corresponding to 1 std of the data). That is the search window only
% encloses 2*s+1 pixels, and of those, all which fall within 1 unit from x
% are used to calcluate the new mean.  If softFlag==0 the original
% formulation is used.  If softFlag==1, instead of using a fixed radius,
% each point p is used in the calulation of the mean with points close to x
% given significantly more weight.  Specifically, each point p is given
% weight exp(-dist(x,p)).  So instead of having a fixed cutoff at r, the
% cutoff is 'soft' (same idea as in softmax), and occurs at approximately
% r.  The implementation remains efficient by actually using a hard cutoff
% at points further then 2r spatially from x.
%
% The resulting matrix M is of size MxNx(P+2).  M(i,j,1) represents the
% convergent row location of X(i,j,:) - (which had initial row location i)
% and M(i,j,2) represents the final column location.  M(i,j,p+2) represents
% the convergent value for X(i,j,p).  The optionaly outputs Vr and Vc are
% 2D arrays where Vr(i,j)=M(i,j,1)-i and Vc(i,j)=M(i,j,2)-j.  That is they
% represent the spatial offset between the original location of a point and
% its convergent location.  Display using quiver(Vc,Vr,0).
%
% USAGE
%  [M,Vr,Vc] = meanShiftIm( X,sigSpt,sigRng,[softFlag],[maxIter],[minDel] )
%
% INPUTS
%  X        - MxNxP data array, P may be 1
%  sigSpt   - integer specifying spatial standard deviation
%  sigRng   - value specifying the standard deviation of the range data
%  softFlag - [0]- see above
%  maxIter  - [100] maximum number of iterations per data point
%  minDel   - [.001] minimum amount of spatial change defining convergence
%
% OUTPUTS
%  M        - array of convergent locations [see above]
%  Vr       - spatial motion in row direction
%  Vc       - spatial motion in col direction
%
% EXAMPLE
%  I=double(imread('cameraman.tif'))/255;
%  [M,Vr,Vc] = meanShiftIm( I,5,.2 );
%  figure(1); im(I); figure(2); im( M(:,:,3) );
%  % color image:
%  I=double(imread('hestain.png'))/255;
%  [M,Vr,Vc] = meanShiftIm( I,5,.2 );
%  figure(1); im(I); figure(2); im( M(:,:,3:end) );
%
% See also MEANSHIFT, MEANSHIFTIMEXPLORE
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

[sigSpt,er] = checkNumArgs( sigSpt, 1, 0, 1 ); error(er);
if( nargin<4 || isempty(softFlag)); softFlag = 0; end
if( nargin<5 || isempty(maxIter) ); maxIter = 100; end
if( nargin<6 || isempty(minDel)); minDel = .001; end

[mrows, ncols, p] = size(X); p = p+2;
[gridRs, gridCs] = ndgrid( 1:mrows, 1:ncols );
data = cat( 3, cat( 3, gridRs/sigSpt, gridCs/sigSpt), X/sigRng );

%%% MAIN LOOP
M = data;
ticId = ticStatus('meanShiftIm');  %t0 = clock;  tlast = t0;
if( softFlag ); radius = sigSpt*2; else radius = sigSpt; end
for i=1:mrows; for j=1:ncols; %#ok<ALIGN>
    Mij = data(i,j,:); Mij = Mij(:)';
    itercount = 0; diff = 1;
    while( itercount < maxIter && diff>minDel )

      % get data which is possibly relevant (within spatial range)
      r = round( Mij(1)*sigSpt );  c = round( Mij(2)*sigSpt );
      boundsr = max(1,r-radius):min(mrows,r+radius);
      boundsc = max(1,c-radius):min(ncols,c+radius);
      dataWin = data( boundsr, boundsc, : );
      dataWinF = reshape( dataWin, [], p );

      % get next mean
      MijOld = Mij;
      n = size( dataWinF, 1);
      D = sum( (dataWinF - ones(n,1)*Mij).^2, 2 );
      if( softFlag )
        S = exp( -D ); sumS = sum(S); Srep = S(:,ones(1,p));
        Mij = sum( dataWinF .* Srep, 1 ) / sumS;
      else
        dataWinF = dataWinF( D < 1, : );
        Mij = sum( dataWinF, 1 ) / size( dataWinF,1 );
      end

      % check if Mij changed [only on basis of x,y location]
      diff = sum( (MijOld(1:2)-Mij(1:2)).^2 );
      itercount = itercount+1;

    end
    M(i,j,:) = Mij(:);
    fracdone = ((i-1)*ncols+j) / (mrows*ncols);
    tocStatus( ticId, fracdone );
  end
end
M = cat(3, M(:,:,1:2)*sigSpt, M(:,:,3:end)*sigRng );

%%% Output spatial difference
if( nargout>1 )
  Vr = M(:,:,1)-gridRs;  Vc = M(:,:,2)-gridCs;
end
