function [xEst, yEst, N_iter, VWout, cov ] = VWCM(S, N_max, term_cond)
%VWCM Virtual Window Center of Mass position estimator
%   [xEst, yEst, N_iter, VWout, cov ] = VWCM(S,N_max,term_cond) Virtual
%   Window Center of Mass estimator for quickly calculating the centroid of
%   a spot in an image.  The algorithm estimates the underlying centroid
%   using adaptive subpixel recentering of the window.  
%
%   Returns estimate of centroid in pixels relative to image center.  For
%   N_max = 1, the algorithm corresponds to a straight (non-VW) centroid.
%
%   If you use this code for your work, please reference the appropriate
%   literature [1] Andrew Berglund and Matthew McMahon, NIST, Spring 2008.
% 
%INPUTS:
%   REQUIRED:
%       S: square image matrix over which to compute the centroid
%   OPTIONAL:
%       N_max: maximum number of iterations to use (default 200)
%       term_cond: termination condition: iteration terminates when
%       successive estimates are separated by less than term_cond*pixel
%       (default 0.001)
%
%OUTPUTS:
%   REQUIRED:
%       x_est: estimated x-position IN PIXELS RELATIVE TO CENTER OF IMAGE S
%       y_est: estimated y-position IN PIXELS RELATIVE TO CENTER OF IMAGE S
%   OPTIONAL:
%       N_iter: actual number of iterations for convergence; If algorithm
%               fails to converge before N_max iterations, N_iter = -1.
%       VWout: structure containing the following fields;
%               VWout.S = final windowed image
%               VWout.X = x-coordinates corresponding to VWout.S
%               VWout.Y = y-coordinates corresponding to VWout.S
%       cov: structure containing the covariance of x_est & y_est
%               cov.XX = variance of x (corresponding to <x^2> - <x>^2)
%               cov.YY = variance of y
%               cov.XY = covariance of x and y (corresponding to <xy> - <x><y>)
% 
%Version History:
%   Created: Spring 2008, Andrew Berglund and Matthew McMahon, NIST
%   Revised AJB 3/2010 to calculate x and y CMs with separate virtual windows.
%Latest Update:
%   AJB 7/27/2011
%
%REFERENCES
%   [1] "Fast, bias-free algorithm for tracking single particles with
%   variable size and shape," A. J. Berglund, M. D. McMahon, J. J.
%   McClelland, and J. A. Liddle, Optics Express 16, 14064 (2008)
%
%DISCLAIMERS: This software was developed at the National Institute of
%Standards and Technology by employees of the Federal Government in the
%course of their official duties. Pursuant to title 17 Section 105 of the
%United States Code this software is not subject to copyright protection
%and is in the public domain. DSigma2_Est is an experimental system. NIST
%assumes no responsibility whatsoever for its use by other parties, and
%makes no guarantees, expressed or implied, about its quality, reliability,
%or any other characteristic. We would appreciate acknowledgement if the
%software is used. Certain commercial software is identified in this system
%in order to specify the procedure adequately. Such identification is not
%intended to imply recommendation or endorsement by the National Institute
%of Standards and Technology, nor is it intended to imply that the
%materials or equipment identified are necessarily the best available for
%the purpose.

debug = 0; % setting debug=1 draws figures to monitor progress of algorithm

% Set variable defaults
if nargin < 2;
    N_max = 200;
    term_cond = 1e-3;
elseif nargin < 3;
    term_cond = 1e-3;
end;

calcCov = 0;
if nargout > 4; 
    calcCov = 1; % Calculate covariance matrix if requested
end;

% Precondition data
% Ensure no negative values
S = S-min(S(:));

% Construct pixel coordinates grid with origin at image center
N = size(S,2);
dx = 1; % pixel width = 1 by definition
dy = 1;

% Construct vector of positions
pxl = (1:N) - (N+1)/2;
% Construct grid of NxN pixels centered at 0 by definition
[xPxl,yPxl] = meshgrid(pxl); 

% Initialize loop variables
N_iter = 0;
done = 0;
dxShift = 0;
dyShift = 0;
% Initial position estimate guess: center of window
xEst = 0;  
yEst = 0;

if debug;hf=figure;end;
% VWCM Algorithm
while ~done;
    N_iter = N_iter + 1;        % Increment counter variable
    xEst_old = xEst;            % Save previous position estimate
    yEst_old = yEst;
    
    % Start from the original image and coordinates at each iteration step  
    STmp = S;  
    xPxlTmp = xPxl;             
    yPxlTmp = yPxl;                 
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % ("Super-pixel" shifts)
    % If more than 0.5 pixel shift is required, truncate full rows and
    % columns before moving on to sub-pixel (virtual) shifts.
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    while (dxShift > dx/2) & size(STmp,2)>2;%all(size(STmp)>[2 2]); 
        % 'dxShift' keeps track of how much *more* of the image needs to be
        % truncated.  If this amount is larger than half a pixel, start by
        % truncating a full row/column and decreasing dxShift by half a
        % pixel.  Then dxShift represents the remaining distance to be
        % passed to the 'sub-pixel shifts' routine.
        dxShift = dxShift - dx/2;   
        xPxlTmp = xPxlTmp(:,2:end);
        yPxlTmp = yPxlTmp(:,2:end);
        STmp = STmp(:,2:end);
    end;

    while (dxShift < -dx/2) & size(STmp,2)>2;%all(size(STmp)>[2 2]);
        % Same as previous but from opposite direction.
        dxShift = dxShift + dx/2;
        xPxlTmp = xPxlTmp(:,1:end-1);
        yPxlTmp = yPxlTmp(:,1:end-1);
        STmp = STmp(:,1:end-1);
    end;

    while (dyShift > dy/2) & size(STmp,1)>2;%all(size(STmp)>[2 2]);
        dyShift = dyShift - dy/2;
        xPxlTmp = xPxlTmp(2:end,:);
        yPxlTmp = yPxlTmp(2:end,:);
        STmp = STmp(2:end,:);
    end;

    while (dyShift < -dy/2) & size(STmp,1)>2;%all(size(STmp)>[2 2]);
        dyShift = dyShift + dy/2;
        xPxlTmp = xPxlTmp(1:end-1,:);
        yPxlTmp = yPxlTmp(1:end-1,:);
        STmp = STmp(1:end-1,:);
    end;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Sub-pixel shifts          
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % Mask for scaling image values corresponding to the Virtual Window
    xMask = ones(size(STmp));  
    yMask = ones(size(STmp));
    
    if dxShift > 0;
        % Redefine edge pixel coordinates by subpixel amounts
        xPxlTmp(:,1) = xPxlTmp(:,1) + dxShift;
        % Scale edge pixel values according to the amount of shift
        xMask(:,1) = 1-2*dxShift/dx;
    else
        xPxlTmp(:,end) = xPxlTmp(:,end) + dxShift;
        xMask(:,end) = 1 + 2*dxShift/dx;
    end;

    if dyShift > 0
        yPxlTmp(1,:) = yPxlTmp(1,:) + dyShift;
        yMask(1,:) = 1-2*dyShift/dy;
    else
        yPxlTmp(end,:) = yPxlTmp(end,:) + dyShift;
        yMask(end,:) = 1 + 2*dyShift/dy;
    end;

    % Apply the Virtual Window mask
      xEst = sum(STmp(:).*xMask(:).*xPxlTmp(:))/sum(STmp(:).*xMask(:));
      yEst = sum(STmp(:).*yMask(:).*yPxlTmp(:))/sum(STmp(:).*yMask(:));
    if debug;
        figure(hf)
        pcolor(xPxlTmp,yPxlTmp,STmp);
%         colormap gray
        hold on;
        plot(xEst,yEst,'wo');
        hold off
        axis([min(xPxl(:)) max(xPxl(:)) min(yPxl(:)) max(yPxl(:))])
        caxis([min(S(:)) max(S(:))])
        drawnow;
        pause(.1);
    end;
    % Shift by current estimate next time through the loop
    dxShift = xEst;
    dyShift = yEst;
    
    % Exit if maximum number of iterations is reached
    if N_iter == N_max;
        if N_max > 1; % don't display convergence error if user requested one iteration only
            disp(strcat(['VWC algorithm (final) did not converge. Terminated at N = ' num2str(N_max)]));
        end;
        N_iter = -1;
        done = 1;
    end;
    
    % Check convergence
    if ((xEst-xEst_old)^2+(yEst-yEst_old)^2) < term_cond^2;
        done = 1;   
    end;
end;

if debug;
    figure;
    pcolor(xPxl,yPxl,S);
    hold on;
    plot(xEst,yEst,'wo');
    hold off
end;

% Calculate covariance matrix if requested
% Useful for rotation-angle estimation.
if calcCov;
        norm = sum(STmp(:));

    % Estimate of variance (<x^2> - <x>^2)
    cov.XX = sum(STmp(:).*xPxlTmp(:).^2)/norm - xEst^2;
    % Estimate of variance (<y^2> - <y>^2)
    cov.YY = sum(STmp(:).*yPxlTmp(:).^2)/norm - yEst^2; 
    % Estimate of variance (<xy> - <x><y>)
    cov.XY = sum(STmp(:).*xPxlTmp(:).*yPxlTmp(:))/norm - xEst*yEst;  
end;
% Pack final truncated image and pixel coordinate grid into a structure
VWout.S = STmp;
VWout.X = xPxlTmp;
VWout.Y = yPxlTmp;