function [pathsx, nodesx, nodesy] = Astar2D_estimate(nodesx, nodesy, estpathx, estpathy, alpha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Performs Dijkstra's path finding algorithm to identify the shortest path
% through the possible nodes. Uses the euclidean distance and an estimated path
% to act as a heuristic. 
% Inputs:
%     - nodesx: optionsxTime array of the nodes for the column shifts
%     - nodesy: optionsxTime array of the nodes for the row shifts
%     - estpathx: Vector of the estimated column shifts using the estimated movement
%     vector for each frame
%     - estpathy:Vector of the estimated row shifts using the estimated movement
%     vector for each frame
%     - alpha: A weighting factor that determined how much the estimated
%     paths will influence the output of the path finding algorithm
% Output:
%     - pathsx: indices of the final path selected 
%     - nodesx: repeat of the input
%     - nodesy: repeat of the input
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[d1 d2] = size(nodesx);
nodeindx = find(~isnan(nodesx));
[colx rowx] = ind2sub([d1 d2], nodeindx);

%% Set up for the algorithm
nodeid = 1:length(nodeindx);

unknownlist = nodeid(1:end);
distances = Inf(size(nodeid));
distances(1) = 0;
previous = ones(size(nodeid));



%% Perform Dijktra's algorithm
% by searching through the options of the next possible timestep 
while ~isempty(unknownlist)
    [minmove mind] = min(distances(unknownlist));
    nodevalx = nodesx(nodeindx(unknownlist(mind)));
    nodevaly = nodesy(nodeindx(unknownlist(mind)));
    choicecol = colx(unknownlist(mind));
    neighbors = find(colx == choicecol+1);
    neighborvalx = nodesx(nodeindx(neighbors));
    neighborvaly = nodesy(nodeindx(neighbors));
    neighbordist = (nodevalx-neighborvalx).^2+(nodevaly-neighborvaly).^2;
    %Calculate the cost of the heuristic
    if ~isempty(neighbors)
        hx = estpathx(choicecol+1)-estpathx(choicecol);
        hy = estpathy(choicecol+1)-estpathx(choicecol);
        heuristic = sqrt((estpathx(choicecol+1)-neighborvalx).^2+(estpathy(choicecol+1)-neighborvaly).^2);
    end

    for fn = 1:length(neighbordist)
        olddist = distances(neighbors(fn));
        if minmove+neighbordist(fn) < olddist
            previous(neighbors(fn)) = unknownlist(mind);
        end
        distances(neighbors(fn)) = min([olddist minmove+neighbordist(fn)+alpha*heuristic(fn)]);
    end
    unknownlist(mind) = [];
end

%% Back out the indices of the final path
paths = ones(d1, 1);
maxcol = max(colx);
endcol = find(colx == maxcol);
[mindist mindistind] = min(distances(endcol));
paths(end) = endcol(mindistind);


for fn = fliplr(2:max(colx))
    paths(fn-1) = previous(paths(fn));
    
end

% Convert for use in the input nodesx reference frame
% (which is the same as nodesy)
pathsx = nodeindx(paths);