function [P cost] = shortest_path(C)
% We will find the shortest path from a start node to an end node in a
% graph.  C is a matrix that encodes the edge weights in the graph.
% We're going to use an encoding of the edges that is suitable when the
% edges are very sparse (most nodes in the graph are not connected to most
% other nodes) and when the maximum number of edges leaving any node is
% small.  Every row of C will encode the edges leaving one node of the
% graph.  There must be a row for every node in the graph, so the graph has
% nodes numbered 1, 2, ... size(C,1).  
% C(i,:) will encode all edges leaving node i in the graph. The 
% row will contain an alternating list of nodes that there is an edge to, and the 
% weights of these edges. 
% For example, suppose C(i,:) = [4 3.2 7 9].  This means that there are
% edges going from node i to node 4, with a weight of 3.2, and from node i
% to node 7, with a weight of 9.
% This format assumes that every node has the same number of neighbors.
% When this isn't true, you can just add false neighbors that are 0.  So,
% if C(i,:) = [4 3 0 0] this means node i has just one neighbor, 4, with an
% edge weight from i to 4 of 3.
% We will assume that node 1 is the start node of the graph, and node
% size(C,1) is the end node of the graph.
  
n = size(C,1);
active = [2:n; inf*ones(1,n-1); -1*ones(1,n-1)];
% Every column of active contains the index of a node, the current minimum
% cost of getting to that node, and the prior node that produced that cost.
current = [1; 0; -1];
% The currently expanded node.  
inactive = current;
% This will contain a list of all nodes that we've found the shortest path
% to.  It has the same format as active.
while current(1) ~= n
    active = update_neighbors(C, active, current);
    [current, active] = pop_best(active);
    inactive = [inactive, current];
end
[P cost] = trace_path(inactive);

function active = update_neighbors(C,active,current)
node = current(1);
cost = current(2);
neighbors_vector = C(node, :);
k = length(neighbors_vector);
neighbor_nodes = neighbors_vector(1:2:k);
edge_weights = neighbors_vector(2:2:k);
for i = 1:length(neighbor_nodes)
    n = neighbor_nodes(i);
    w = edge_weights(i);
    if n > 0
        index = find(n == (active(1,:)));
    else
        index = [];
    end
    % This allows n to be 0 when there is no neighbor.  
    if length(index) == 1
        if active(2,index) > (cost + w)
           active(2,index) = cost+w;
           active(3,index) = node;
        end
    end
end

function [current, active] = pop_best(active)
best_nodes = find(active(2,:) == min(active(2,:)));
best_node = best_nodes(1);
% In case there are several with the same cost.
current = active(:, best_node);
if current(2) == inf
    error('Seems like there is no path to the end node.');
end
active(:, best_node) = [-1; inf; -1];

function [P, cost] = trace_path(inactive)
n = max(inactive(1,:));
end_col = find(n == inactive(1,:));
current = inactive(:,end_col);
cost = current(2);
P = [n];
while current(1) ~= 1
    if current(3) == -1
        error('it seems that there was no path from start to end.');
    end
    col = find(current(3) == inactive(1,:));
    current = inactive(:,col);
    P = [current(1), P];
end
