## Copyright (C) 2004-2016 David Legland <david.legland@grignon.inra.fr>
## Copyright (C) 2004-2016 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas)
## Copyright (C) 2016 Adapted to Octave by Juan Pablo Carbajal <ajuanpi+dev@gmail.com>
## All rights reserved.
##
## Redistribution and use in source and binary forms, with or without
## modification, are permitted provided that the following conditions are met:
##
##     1 Redistributions of source code must retain the above copyright notice,
##       this list of conditions and the following disclaimer.
##     2 Redistributions in binary form must reproduce the above copyright
##       notice, this list of conditions and the following disclaimer in the
##       documentation and/or other materials provided with the distribution.
##
## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ''AS IS''
## AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
## IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
## ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
## ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
## DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
## SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
## OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
## OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

%GRSHORTESTPATH Find a shortest path between two nodes in the graph
%
%   PATH = grShortestPath(NODES, EDGES, NODE1, NODE2, WEIGHTS)
%   NODES and EDGES defines the graph, NODE1 and NODE2 are indices of the
%   node extremities, and WEIGHTS is the set of weights associated to each
%   edge.
%   The function returns a set of node indices.
%
%   PATH = grShortestPath(NODES, EDGES, NODEINDS, WEIGHTS)
%   Specifies two or more points that must be traversed by the path, in the
%   specified order.
%
%   % Create a simple tree graph, and compute shortest path
%     [x y] = meshgrid([10 20 30], [10 20 30]);
%     nodes = [x(:) y(:)];
%     edges = [1 2;2 3;2 5;3 6; 4 5;4 7;5 8; 8 9];
%     drawGraph(nodes, edges)
%     axis equal; axis([0 40 0 40]);
%     drawNodeLabels(nodes, 1:9)
%     path = grShortestPath(nodes, edges, 1, 9);
%     % same as:
%     path = grShortestPath(nodes, edges, [1 9]);
%
%   See also
%     graphs, grFindMaximalLengthPath
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-05-22,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

function [nodePath, edgePath] = grShortestPath(nodes, edges, ind0, ind1, edgeWeights)
  % process the case where several node indices are specified in first arg.
  if length(ind0) > 1
      % following optional argument is edge values
      if exist('ind1', 'var')
          edgeWeights = ind1;
      else
          edgeWeights = ones(size(edges, 1), 1);
      end
      
      % concatenate path pieces
      nodePath = ind0(1);
      edgePath = [];
      for i = 2:length(ind0)
          [node0, edge0] = grShortestPath(nodes, edges, ind0(i-1), ind0(i), edgeWeights);
          nodePath = [nodePath ; node0(2:end)]; %#ok<AGROW>
          edgePath = [edgePath ; edge0]; %#ok<AGROW>
      end
      
      return;
  end

  % ensure weights are defined
  if ~exist('edgeWeights', 'var')
      edgeWeights = ones(size(edges, 1), 1);
  end

  % check indices limits
  nNodes = size(nodes, 1);
  if max(ind0) > nNodes
      error('Start index exceed number of nodes in the graph');
  end
  if max(ind1) > nNodes
      error('End index exceed number of nodes in the graph');
  end

  %% Initialisations
  % consider infinite distance for all nodes
  dists = inf * ones(nNodes, 1);

  % initial node is at distance 0 by definition
  dists(ind0) = 0;

  % create an array of indices for the predessor of each node
  preds = zeros(nNodes, 1);
  preds(ind0) = 0;

  % allocate memory to store the subgraph, which is a tree
  edges2 = zeros(nNodes-1, 2);

  % create an array of node flags
  unprocessedNodeInds = 1:nNodes;

  edgeCount = 0;

  %% Iterate until all nodes are flagged to 1
  while ~isempty(unprocessedNodeInds)
      % choose unprocessed node with lowest distance
      [tmp, ind] = min(dists(unprocessedNodeInds)); %#ok<ASGLU>
      ind = unprocessedNodeInds(ind);
      ind = ind(1);

      % mark current node as processed
      unprocessedNodeInds(unprocessedNodeInds == ind) = [];
      
      % if the current node is the target, it is not necessary to continue...
      if ind == ind1
          break;
      end
      
      % find the index list of edges incident to current node 
      adjEdgeInds = grAdjacentEdges(edges, ind);
      
      % select edges whose opposite node has not yet been processed
      inds2 = sum(ismember(edges(adjEdgeInds, :), unprocessedNodeInds), 2) > 0;
      adjEdgeInds = adjEdgeInds(inds2);
      
      % iterate over incident edges to update adjacent nodes
      for iNeigh = 1:length(adjEdgeInds)
          % extract index of current adjacent node
          edge = edges(adjEdgeInds(iNeigh), :);
          adjNodeInd = edge(~ismember(edge, ind));
          
          newDist = dists(ind) + edgeWeights(adjEdgeInds(iNeigh));
  %         dists(adjNodeInd) = min(dists(adjNodeInd), newDist);
          if newDist < dists(adjNodeInd)
              dists(adjNodeInd) = newDist;
              preds(adjNodeInd) = ind;
          end
      end

      % find the list of adjacent nodes
      adjNodeInds = unique(edges(adjEdgeInds, :));
      adjNodeInds(adjNodeInds == ind) = [];
      
      % choose the adjacent nodes with lowest distance, and add the
      % corresponding edges to the subgraph 
      if ~isempty(adjNodeInds)
          minDist = min(dists(adjNodeInds));
          closestNodeInds = adjNodeInds(dists(adjNodeInds) <= minDist);
          for iCloseNode = 1:length(closestNodeInds)
              edgeCount = edgeCount + 1;
              edges2(edgeCount, :) = [ind closestNodeInds(iCloseNode)];
          end
      end
  end

  %% Path creation

  % create the path: start from end index, and identify successive set of
  % neighbor edges and nodes

  nodeInd = ind1;
  nodePath = nodeInd;
  edgePath = [];

  % find predecessors until we reach ind0 node
  while nodeInd ~= ind0
      newNodeInd = preds(nodeInd);
      nodePath = [nodePath ; newNodeInd]; %#ok<AGROW>

      % search the edge (both directions) in the list of edges
      e_tmp                 = [nodeInd newNodeInd];
      [~,edgeInd]           = ismember ([e_tmp; e_tmp(end:-1:1)], edges, 'rows');
      edgeInd(edgeInd == 0) = []; % erase the one that isn't there
      edgePath = [edgePath ; edgeInd]; %#ok<AGROW>

      nodeInd = newNodeInd;
  end
      
  % reverse the path
  nodePath = nodePath(end:-1:1);
  edgePath = edgePath(end:-1:1);
endfunction

%!demo
%! % Create a simple tree graph, and compute shortest path
%! [x y] = meshgrid ([10 20 30], [10 20 30]);
%! nodes = [x(:) y(:)];
%! edges = [1 2;2 3;2 5;3 6; 4 5;4 7;5 8; 8 9];
%! drawGraph (nodes, edges)
%! drawNodeLabels (nodes, 1:9)
%! [nid eid] = grShortestPath (nodes, edges, 1, 9);
%! n_path = nodes(nid,:);
%! [~,e_path] = ismember (edges(eid,:), nid);
%! drawGraph (n_path, e_path,'none', {'color','red'});
%! hold on;
%! h = plot (n_path(1,1),n_path(1,2),'o',n_path(end,1),n_path(end,2),'o');
%! set(h(1),'markeredgecolor','none', 'markerfacecolor', 'g','markersize', 10);
%! set(h(2),'markeredgecolor','none', 'markerfacecolor', 'k','markersize', 10);
%! hold off
%! axis image
