input_map = false(10);              % Create an Input Map
input_map (3:9, 5:7) = 1;           % Add an obstacle
start_coords = [6, 1];              % Save the location of start coordinate
dest_coords = [8, 9];               % Save location of destination coordinate
cmap = [1   1   1;                  % Create a color map
    0   0   0;
    1   0   0;
    0   0   1;
    0   1   0;
    1   1   0;
    0.5 0.5 0.5];
colormap(cmap);                     % Sets the colormap for the current figure
[nrows, ncols] = size(input_map);   % Save the size of the input_map

map = zeros(nrows,ncols);           % Create map to save the states of each grid cell
map(~input_map) = 1;                % Mark free cells on map
map(input_map) = 2;                 % Mark obstacle cells on map
start_node = sub2ind(size(map), start_coords(1), start_coords(2));      % Generate linear indices of start node
dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));         % Generate linear indices of dest node
map(start_node) = 5;                % Mark start node on map
map(dest_node) = 6;                 % Mark destination node on map

% Initialize distance from start array to inifinity
distanceFromStart = Inf(nrows,ncols);

% Create a map that holds the index of its parent for each grid cell
parent = zeros(nrows, ncols);

distanceFromStart(start_node) = 0;  % distance of start node is zero

image(1.5, 1.5, map);
grid on;                        % Display grid lines
axis image;                     % Set axis limits
drawnow;                        % Update figure

drawMapEveryTime = true;            % To see how nodes expand on the grid

while true                              % Create an infinite loop
    map(start_node) = 5;                % Mark start node on map
    map(dest_node) = 6;                 % Mark destination node on map

    if (drawMapEveryTime)
        image(1.5, 1.5, map);
        grid on;                        % Display grid lines
        axis image;                     % Set axis limits
        drawnow;                        % Update figure
    end

    % Find the node with the minimum distance
    [min_dist, current] = min(distanceFromStart(:));
    % Compute row, column coordinates of current node from linear index
    [i, j] = ind2sub(size(distanceFromStart), current);

    % Create an exit condition for the infinite loop to end
    if ((current == dest_node) || isinf(min_dist)) break
    end

    % Update distance value of element right of current element
    if (i+1 <= nrows && distanceFromStart(i+1, j) > min_dist + 1)
        if (parent(i+1, j) == 0 && input_map(i+1,j)~=1 && parent(current)~= sub2ind(size(map), i+1, j))
            distanceFromStart(i+1, j) = min_dist + 1;
            map(sub2ind(size(map), i+1, j)) = 4;    % Mark the neighbour of current as processing
            parent(i+1, j)= current;
        end
    end

    % Update distance value of element left of current element
    if (i-1 >= 1 && distanceFromStart(i-1, j) > min_dist + 1)
        if (parent(i-1, j) == 0 && input_map(i-1,j)~=1 && parent(current)~= sub2ind(size(map), i-1, j))
            distanceFromStart(i-1, j) = min_dist + 1;
            map(sub2ind(size(map), i-1, j)) = 4;    % Mark the neighbour of current as processing
            parent(i-1, j)= current;
        end
    end

    % Update distance value of element top of current element
    if (j-1 >= 1 && distanceFromStart(i, j-1) > min_dist + 1)
        if (parent(i, j-1) == 0 && input_map(i,j-1)~=1 && parent(current)~= sub2ind(size(map), i, j-1))
            distanceFromStart(i, j-1) = min_dist + 1;
            map(sub2ind(size(map), i, j-1)) = 4;    % Mark the neighbour of current as processing
            parent(i, j-1)= current;
        end
    end

    % Update distance value of element bottom of current element
    if (j+1 <= ncols && distanceFromStart(i, j+1) > min_dist + 1)
        if (parent(i, j+1) == 0 && input_map(i,j+1)~=1 && parent(current)~= sub2ind(size(map), i, j+1))
            distanceFromStart(i, j+1) = min_dist + 1;
            map(sub2ind(size(map), i, j+1)) = 4;    % Mark the neighbour of current as processing
            parent(i, j+1)= current;
        end
    end

    distanceFromStart(current) = -log(0);   % change the distance of current from start as infinity
    map(current) = 3;                       % mark the current point as processed
end

if (isinf(distanceFromStart(dest_node))) route = [];    % if distance to destination node is infinity
else route = [dest_node];                               % else backtrace the route from destination node
    while (parent(route(1)) ~= 0)                       % check front of route for start node
        route = [parent(route(1)), route];              % add parent of current node to front of route
    end

    for k = 2:length(route) - 1         % To visualize the map and the path
        map(route(k)) = 7;
        pause(0.001);                   % Pause the code for a while
        image(1.5, 1.5, map);
        grid on;                        % Display grid lines
        axis image;                     % Set axis lengths
    end
end

% Add legends to the colormapped image
hold on;
for K = 1:7
    hidden_h(K) = surf(zeros(2, 2), 'edgecolor', 'none', ...
        'facecolor', cmap(K, :));
end
hold off

uistack(hidden_h, 'bottom');
legend(hidden_h, {'free space', 'obstacles', 'closed nodes', ...
    'open nodes', 'start node', 'goal node', 'shortest path'} )