%GRAPHPATH Compute shortest paths in a graph
%
%	[PATH,D] = GRAPHPATH(N,L,E)
%
% INPUT
%   N   Index of start node
%   L   Nx2 array with indices of all connected nodes in the graph
%   E   Vector with N corresponding distances
%       Default: all distances equal to 1
%
% OUTPUT
%   PATH Cell array with paths from node N to all other nodes
%   D    Cell array with edge length of paths in PATH
%
% DESCRIPTION
% The shortest paths are found from node N to all other nodes 
% in the graph defined by {L,E}
%
% SEE ALSO
% DISTGRAPH, PLOTGRAPH, KMST

% Copyright: R.P.W. Duin, r.p.w.duin@prtools.org
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function [nodepath,pathdist] = graphpath(nstart,L,e);

L = [L; fliplr(L)];
n = size(L,1);
if nargin < 3
	e = ones(n,1);
else
	e = [e(:);e(:)];
end
k = max(L(:));

nodedist = repmat(inf,1,k);  % distance of node to source
nodeconn = cell(1,k);        % connections per node
nodepath = cell(1,k);        % path from source to each node
pathdist = cell(1,k);        % distances along path
edgelen  = cell(1,k);
for j=1:k
	J = find(L(:,1)==j);
	nodeconn{j} = L(J,2);
	edgelen{j} = e(J);
end

K = nstart;                  % active nodes
nodepath{nstart} = nstart;
nodedist(nstart) = 0;
pathdist{nstart} = 0;

while ~isempty(K)
	Knew = [];
	for j=1:length(K)
		C = nodeconn{K(j)};         % connecting nodes
		for i=1:length(C)
			ndist = nodedist(K(j)) + edgelen{K(j)}(i);
			if ndist < nodedist(C(i))
				nodedist(C(i)) = ndist;
				nodepath{C(i)} = [nodepath{K(j)} C(i)];
				pathdist{C(i)} = [pathdist{K(j)} edgelen{K(j)}(i)];
				Knew = [Knew C(i)];
			end
		end
	end
	K = Knew;
end