function [nodesxnew, nodesynew, estmovex, estmovey] = fillNodes(nodesx, nodesy, distthresh)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Apply the registration coordinates to a group of videos within a folder
% Inputs:
%     - nodesx: Mx#frame matrix of the column positions of the peaks
%     identified in the correlation image
%     - nodesy: Mx#frame matrix of the row positions of the peaks
%     identified in the correlation image
%     - distthresh: The threshold to use to fill in nodes with an estimated
%     position
% Output:
%     - nodesxnew: Mx#frame matrix of the column positions of the peaks
%     identified in the correlation image with the filled in peaks
%     - nodesynew: Mx#frame matrix of the row positions of the peaks
%     identified in the correlation image with the filled in peaks
%     - estmovex: The estimated column movement
%     - estmovey: The estimated row movement
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

maxshift = 80;
[d1 d2] = size(nodesx);
nodesxnew = nodesx;
nodesynew = nodesy;
track = zeros(size(1, d2));
estmovex = diff(nodesx(:, 1));
estmovex(abs(estmovex) > maxshift) = NaN;
estmovex = fillmissing(estmovex, 'makima');
estmovey = diff(nodesy(:, 1));
estmovey(abs(estmovey) > maxshift) = NaN;
estmovey = fillmissing(estmovey, 'makima');
for fn = 2:d1
    x1 = nodesx(fn-1, :);
    x2 = nodesx(fn, :);
    y1 = nodesy(fn-1, :);
    y2 = nodesy(fn, :);
    if sum(~isnan(x2)) > 1
        [estmovex(fn) estmovey(fn)] = distMatVec(x1, x2, y1, y2, maxshift); 
    end
end

for fn = 2:d1
    x1 = nodesxnew(fn-1, :);
    x2 = nodesxnew(fn, :);
    y1 = nodesynew(fn-1, :);
    y2 = nodesynew(fn, :);
    if sum(~isnan(x2)) > 1
%         [estmovex(fn) estmovey(fn)] = distMatVec(x1, x2, y1, y2, 40);
        dist = sqrt((x2-(x1)').^2+(y2-(y1)').^2);
        lind = find(isnan(x2), 1);
        if isempty(lind)
            lind = length(x2)+1;
        end
        fillind = find(min(dist') > distthresh);
        nodesxnew(fn, lind:(lind+length(fillind)-1)) = x1(fillind)+estmovex(fn);
        nodesynew(fn, lind:(lind+length(fillind)-1)) = y1(fillind)+estmovey(fn);
        track(fn) = 1;
        
        [nd1 nd2] = size(nodesxnew);
        if nd2 ~= d2
            repnodex = nodesxnew(:, d2:nd2);
            repnodey = nodesynew(:, d2:nd2);
            nind = find(repnodex == 0 & repnodey == 0);
            repnodex(nind) = NaN;
            repnodey(nind) = NaN;
            nodesxnew(:, d2:nd2) = repnodex;
            nodesynew(:, d2:nd2) = repnodey;
        end
        [d1 d2] = size(nodesxnew);
        
    end
end

[nd1 nd2] = size(nodesxnew);
if nd2 ~= d2
    repnodex = nodesxnew(:, d2:nd2);
    repnodey = nodesynew(:, d2:nd2);
    nind = find(repnodex == 0 & repnodey == 0);
    repnodex(nind) = NaN;
    repnodey(nind) = NaN;
    nodesxnew(:, d2:nd2) = repnodex;
    nodesynew(:, d2:nd2) = repnodey;
end