function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [distanceToNode, parentOfNode, nodeTrajectory] = dijkstra(nbNodes, visibilityGraph)
%
% Task: Perform the Dijkstra algorithm on a given visibility graph
%
% Inputs:
%	-nbNodes: number of nodes of the graph excluding the starting and goal points
%	-visibilityGraph: a matrix containing the distance between connected nodes
%		(NaN refers to not connected nodes)
%		The matrix has a size of (nbNodes+2)x(nbNodes+2)
%		The first row/col corresponds to the Starting point, the last row/col to the Goal point.
%
% Outputs:
%	- distanceToNode: distance between the current node and its parent
%	- parentOfNode: index of the parent node for each node
%	- nodeTrajectory: best trajectory
%
% Guillaume Gibert (guillaume.gibert@ecam.fr)
% 17/03/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

constantLargeDitance=10000;

visitedNodes = zeros(1, nbNodes+2);
distanceToNode = constantLargeDitance*ones(1, nbNodes+2);
distanceToNode(1) = 0;
parentOfNode = zeros(1, nbNodes+2);

fprintf('##Starting Dijkstra''s algorithm...\n')

while (sum(visitedNodes(:)==0))
	thresholdDistance = constantLargeDitance+1;
	for l_node=1:nbNodes+2
		%l_node
		if (visitedNodes(l_node)==0 &&  distanceToNode(l_node) < thresholdDistance)
			minIndex = l_node;
			thresholdDistance = distanceToNode(l_node);
		end
	end

	fprintf('-->Visiting N%d\n', minIndex-1)

	visitedNodes(minIndex) = 1;
	for l_node=1:nbNodes+2
		%l_node
    if (visibilityGraph{minIndex,l_node}!="NaN")
      if (l_node~=minIndex && ~isnan(visibilityGraph{minIndex, l_node}))
        distance = distanceToNode(minIndex) + visibilityGraph{minIndex,l_node};
        if (distance < distanceToNode(l_node))
          distanceToNode(l_node) = distance;
          parentOfNode(l_node) = minIndex;
        end
      end
    endif
	end
end
fprintf('##Dijkstra''s algorithm is done!\n')
fprintf('##Results\n')
fprintf('Minimal distance to target: %d\n', distanceToNode(nbNodes+2))
nodeIndex = nbNodes+2;
nodeTrajectory = [];
while(nodeIndex~=1)
  if (nodeIndex!=0)
	  nodeIndex = parentOfNode(nodeIndex);
	  nodeTrajectory = [nodeTrajectory nodeIndex];
  endif
end
fprintf('S-->');
for l_node=2:length(nodeTrajectory)
	fprintf('N%d-->', nodeTrajectory(length(nodeTrajectory)-(l_node-1))-1);
end
fprintf('G\n');
fprintf('########\n');