function [estmovex, estmovey] = distMatVec(x1, x2, y1, y2, distthresh)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Identifies nearby neighboring peaks/nodes between frames. This is then
%used to construct the estimated movement vector based on the median
%movement. 
% Inputs:
%     - x1: Vector of the column location of the peaks for frame 1
%     - x2: Vector of the column location of the peaks for frame 2
%     - y1: Vector of the row location of the peaks for frame 1
%     - y2: Vector of the row location of the peaks for frame 2
%     - distthresh: The furthest distance in pixels which will be
%     considered for identifying pairs of peaks within the 2 correlation
%     images
% Output:
%     - estmovex: Estimated column shift from the population of peaks
%     - estmovey:  Estimated row shift from the population of peaks
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


x1 = x1(~isnan(x1));
x2 = x2(~isnan(x2));
y1 = y1(~isnan(y1));
y2 = y2(~isnan(y2));
distmat = sqrt((x1-x2').^2+(y1-y2').^2);

M = matchpairs(distmat, 20);
Mind = sub2ind(size(distmat), M(:, 1), M(:, 2));
[minval, mindind] = min(distmat(Mind), [], 2);

usel = find(minval < distthresh);

if ~isempty(M)
    mvecx = x2(M(usel, 1))-x1(M(usel, 2));
    mvecy = y2(M(usel, 1))-y1(M(usel, 2));
else
    mvecx = 0;
    mvecy = 0;
end
estmovex = median(mvecx);
estmovey = median(mvecy);

end