function [dist,path] = dijkstra(nodes,segments,sid,fid)
%DIJKSTRA Calculates the shortest distance and path between points on a map
%   using Dijkstra's Shortest Path Algorithm
% 
%   [DIST, PATH] = DIJKSTRA(NODES, SEGMENTS, SID, FID)
%   Calculates the shortest distance and path between start and finish 
%   nodes SID and FID, where NODES an Nx2 matrix with the format [X Y],
%   where X, Y are cartesian position coordinates, and SEGMENTS should 
%   be an Mx2 matrix with the format [N1 N2], where N1, N2 correspond to 
%   node IDs from the NODES list such that there is an [undirected] edge / 
%   segment between node N1 and node N2. SID should be an integer 
%   corresponding with the starting node, FID should be an integer 
%   corresponding with the finishing node. DIST is the shortest Euclidean 
%   distance between SID and FID along the map segments. DIST will have a 
%   value of INF if there are no segments connecting SID and FID. PATH is a
%   list of nodes containing the shortest route from SID to FID. NAN will 
%   be returned if there are no segments connecting SID to FID.

%% init
%general
n_nodes = size(nodes,1);
table = zeros(n_nodes,2);
shortest_distance = Inf(n_nodes,1);
settled = zeros(n_nodes,1);
path = num2cell(NaN(n_nodes,1));

%start node (sid)
shortest_distance(sid) = 0;
settled(sid) = 1;
path(sid) = {sid};

%determine ids
node_ids = 1:n_nodes;
segment_ids = 1:size(segments,1);
% segments = [segment_ids'; segments];

%% Dijkstra's algorithm
while settled(fid)==0,
    %update table
    table(:,1) = table(:,2);
    table(sid,2) = 0;
    
    %find neighboring nodes in the segments list
    n_ids = [segments(sid == segments(:,1),2);
        segments(sid == segments(:,2),1)];
    
    %calculate the distances to the neighboring nodes and keep track of the
    %paths
    for k = 1:length(n_ids),
        if ~settled(n_ids(k)),
            d = sqrt(sum((nodes(sid,1:2) - nodes(n_ids(k),1:2)).^2));
            if (table(n_ids(k),1) == 0) || ...
                    (table(n_ids(k),1) > (table(sid,1) + d)),
                table(n_ids(k),2) = table(sid,1) + d;
                tmp_path = path(sid);
                path(n_ids(k)) = {[tmp_path{1} n_ids(k)]};
            else
                table(n_ids(k),2) = table(n_ids(k),1);                
            end
        end
    end
    %find the minimum non-zero value in the table and save it
    nidx = find(table(:,2));
    ndx = find(table(nidx,2) == min(table(nidx,2)));
    
    if isempty(ndx),
        break
    else
        sid = nidx(ndx(1));
        shortest_distance(sid) = table(sid,2);
        settled(sid) = 1;
    end
end

%output
dist = shortest_distance(fid);
path = path(fid);
path = path{1};