%
% 2D path planning example on 50x50 grid with obstacles
%
% Yuri Malitsky (ynm@cs.brown.edu), Chad Jenkins (cjenkins@cs.brown.edu)  
% Modified: September 27, 2010
%
% based on Wikipedia A* entry
%
% call as a function with 1 argument for the search algorithm
%
% select search algorithm based on first input argument
% 1 - Depth first search (no backtracking)
% 2 - Breadth first search
% 3 - Dijkstra's algorithm
% 4 - greedy best-first search
% 5 - A* search: heuristic: euclidean distance to goal (default)
%
% second and third input arguments specify 2D goal location
% default: (12, 22)
%
% fourth and fifth input arguments specify 2D start location
% default: (25, 25)
%
% try:
% > pathplan(1)
% > pathplan(2)
% > pathplan(3)
% > pathplan(4)
% > pathplan(5)
%
% > pathplan(1,25,25,12,22)
% > pathplan(2,25,25,12,22)
% > pathplan(3,25,25,12,22)
% > pathplan(4,25,25,12,22)
% > pathplan(5,25,25,12,22)
%
% > pathplan(1,25,25,40,25)
% > pathplan(2,25,25,40,25)
% > pathplan(3,25,25,40,25)
% > pathplan(4,25,25,40,25)
% > pathplan(5,25,25,40,25)
%
% > pathplan(1,40,25,25,25)
% > pathplan(2,40,25,25,25)
% > pathplan(3,40,25,25,25)
% > pathplan(4,40,25,25,25)
% > pathplan(5,40,25,25,25)
%

function [output_args] = pathplan(algorithm,goal_x,goal_y,start_x,start_y)

hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GLOBALS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% size of map
size_x = 50;            % size of the map along the x-axis
size_y = 50;            % size of the map along the y-axis
size_t = size_x*size_y; % number of nodes in the map

axis([1 size_x 1 size_y])

% keeps track of information about the search 
closedset = zeros(size_t,1); % set of nodes already evaluated
came_from = [ ];        % the map of navigated nodes

openset = [ ];          % set of tentative nodes
                        %   format: [node index, g_score, h_score, f_score]
                        %   node index: index of the current node
                        %   g_score: distance from start along optimal path
                        %   h_score: estimate of distance to goal node
                        %   f_score: estimated distance from start to goal

% toggle display of intermediate information during search
display_visited_nodes = 1;
display_intermediate_set_parents = 0;
display_live = 0;

found = 0;              % flag for visiting goal node

% output variables for optimal path
stat_distance = 0;      % optimal distance from start to goal
stat_memory = 0;        % maximum amount of memory used to find the solution
stat_steps = 0;         % number of steps needed to find solution


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HELPER FUNCTIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% convert the (x,y) coordinate to a single index
function [ out_index ] = coord_to_index( x, y )
    out_index = ((y-1)*size_x + (x-1))+1;
end

% convert the position index to an (x,y) coordinate
function [ out_x, out_y ] = index_to_coord( index )
    index = index-1;
    out_x = mod( index, size_x )+1;
    out_y = floor( index / size_x )+1;
end

function [dist] = heuristic_distance( s, e )
   [s_x, s_y] = index_to_coord(s);
   [e_x, e_y] = index_to_coord(e);
    
   delta_x = abs(s_x - e_x);
   delta_y = abs(s_y - e_y);
   dist = sqrt(delta_x*delta_x + delta_y*delta_y);
end

function [index] = shift_node( current_index, step_x, step_y )
    [x,y] = index_to_coord( current_index );
    index = coord_to_index( x+step_x, y+step_y );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HANDLE FUNCTION PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% set default algorithm
if (~exist('algorithm'))
    algorithm = 5;
end

% set goal location
if (~exist('goal_x'))
    G(1) = 12;
else
    G(1) = goal_x;
end

if (~exist('goal_y'))
    G(2) = 22;
else
    G(2) = goal_y;
end

% set start location
if (~exist('start_x'))
    S(1) = 25;
else
    S(1) = start_x;
end

if (~exist('start_y'))
    S(2) = 25;
else
    S(2) = start_y;
end



        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SET THE MAP
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% draw grid environment
rectangle('Position',[1,1,size_x,size_y],'FaceColor',[0 .4 0]);
hold on;

% "water" obstacles
oxs = 7;
oys = 25;
oxw = 10;
oyw = 5;
rectangle('Position',[oxs,oys,oxw,oyw],'FaceColor','b');
for x = oxs:(oxs+oxw)
    for y = oys:(oys+oyw)
        closedset( coord_to_index(x,y) ) = 1;
    end
end

oxs = 15;
oys = 20;
oxw = 2;
oyw = 5;
rectangle('Position',[oxs,oys,oxw,oyw],'FaceColor','b');
for x = oxs:(oxs+oxw)
    for y = oys:(oys+oyw)
        closedset( coord_to_index(x,y) ) = 1;
    end
end

oxs = 7;
oys = 15;
oxw = 10;
oyw = 5;
rectangle('Position',[oxs,oys,oxw,oyw],'FaceColor','b');
for x = oxs:(oxs+oxw)
    for y = oys:(oys+oyw)
        closedset( coord_to_index(x,y) ) = 1;
    end
end

oxs = 7;
oys = 30;
oxw = 3;
oyw = 100;
rectangle('Position',[oxs,oys,oxw,oyw],'FaceColor','b');
for x = oxs:(oxs+oxw)
    for y = oys:(oys+oyw)
        closedset( coord_to_index(x,y) ) = 1;
    end
end

% start and goal locations
plot(S(1),S(2),'m.','MarkerSize',25);
plot(G(1),G(2),'r.','MarkerSize',25);
drawnow;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PERFORM SEARCH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


node_start = coord_to_index( S(1), S(2) );
node_goal  = coord_to_index( G(1), G(2) );

% add the start node into the list of nodes to explore
openset = [ node_start, ...
            0, ...
            heuristic_distance( node_start, node_goal ), ...
            0 + heuristic_distance( node_start, node_goal ) ];

% while there are nodes still in the tentative node list
while ( size(openset,1) > 0 & found == 0 )
    % keep track of statistics about the search
    stat_memory = max(stat_memory, size(openset,1) );
    stat_steps = stat_steps + 1;

    % dequeue node current being visited, the node differs based on the
    % search algorithm being used:
    %   DFS: node most recently added to the list of tentative nodes
    %   BFS: node placed earliest in the list of tentative nodes
    %   Dijkstra: node with lowest path distance to start node
    %   greedy best-first: node with lowest straight line distance to goal 
    %   A*: node with lowest (path distance to start + straight line
    %       distance to goal)
    current_index = 1;
    if( algorithm == 1 )
       current_index = size(openset,1); 
    end

    % sort nodes based on exploration priority, this is based on f_score
    %   for A*, h_score for greedy best-first, and g_score for Dijkstra
    if algorithm == 4 || algorithm == 5 
        openset = sortrows( openset, 4 );
    elseif algorithm == 3
        openset = sortrows( openset, 2 );
    end

    node_current = openset(current_index, 1);   % current node index
    dist_current = openset(current_index, 2);   % distance from the start
    
    % remove current node from list of tentative nodes
    if algorithm == 1 
        openset = openset(1:size(openset,1)-1,:);
    else
        openset = openset(2:size(openset,1),:);
    end
    
    % mark that the current node has been visited
    closedset(node_current) = 1;
    
    
    % if goal is found, our search is over
    if ( node_current == node_goal)
        found = 1;
        stat_distance = dist_current;
        break;
    end
    
    if (display_visited_nodes)
        [disp_x, disp_y] = index_to_coord(node_current);
        plot(disp_x, disp_y,'.','Color',[0.1 0.9 0.1]);
        if( display_live == 1 )
            drawnow;
        end
    end

    % scan all the neighbors of the current node and add them to the list
    % of tentative nodes that need to be visited in the future
    for i = -1:1
        for j = -1:1 
            
            node_neighbor = shift_node( node_current, i, j );
            
            [c_x, c_y] = index_to_coord(node_current);
            
            % make sure neighbores are among the 4-connected nodes and
            % within the map
            if ( ~((i == 0) & (j == 0)) ...  % enforce not current          
                    & (abs(i+j)==1) ...  % enforce 4-connected
                    & (c_x+i > 0) ...  % boundary check
                    & (c_y+j > 0) ...
                    & (c_x+i <= size_x) ...
                    & (c_y+j <= size_y) )
                    
                % skip this neighbor if it has been visited previously
                if ( closedset(node_neighbor) == 0 )
                    
                    % distance along path through current visit node
                    tentative_g_score = dist_current + sqrt(i*i + j*j);


                    % check if this neighbor is already in the tentative set
                    node_index = find( openset(:,1) == node_current );
                   
                    % if this is the first time we are seeing this node,
                    % add it to the tentative list. Otherwise if have found
                    % a shorter path to the node, update the nodes ancestry
                    if ( isempty(node_index) )
                        openset = [openset; [node_neighbor, tentative_g_score, 0, 0]];
                        node_index = size(openset,1);
                        update_distance = 1;                      
                    elseif ( tentative_g_score < openset(node_index,2) )
                        if( algorithm == 3 || algorithm == 4 || algorithm == 5 )
                            update_distance = 1;
                        end
                    else
                        update_distance = 0;
                    end
                        
                 
                    % update the neighbor's path routing, path
                    % distance, and exploration priority
                    if (update_distance)
                        if (display_visited_nodes)
                            [disp_x_n, disp_y_n] = index_to_coord(node_neighbor);
                            plot(disp_x_n, disp_y_n,'.','Color',[0.7 0.5 0]);
                        end;
                        
                        if (display_intermediate_set_parents)
                            plot(disp_x, disp_y,'.','Color',[.6 0 .6]);
                            plot([c_x ;c_x+i],[c_y; c_y+j],'-');
                         end

                         % update routing, meaning the best parent that 
                         %leads to this node
                         came_from(node_neighbor) = node_current;
                         
                         if( algorithm == 5 )
                             openset(node_index,3) = heuristic_distance( node_neighbor, node_goal );
                             openset(node_index,4) = tentative_g_score + heuristic_distance( node_neighbor, node_goal );
                         elseif( algorithm == 4 || algorithm == 3 )
                             openset(node_index,3) = 0;
                             openset(node_index,4) = heuristic_distance( node_neighbor, node_goal );
                         end
                    end
                    
                    % if goal is found, our search is over
                    if ( node_neighbor == node_goal & (algorithm == 1))
                        came_from(node_neighbor) = node_current;
                        found = 1;
                        stat_distance = tentative_g_score;
                        break;
                    end
                end
            end
        end
    end
    
    %if (display_visited_nodes)
    %    [disp_x, disp_y] = index_to_coord(node_current);
    %    plot(disp_x, disp_y,'.','Color',[0 0.9 0]);
    %    if( display_live )
    %        drawnow;
    %    end
    %end
    
end

% trace path from goal back to start
current = node_goal;
planpath=[];
stat_distance = 0;
while (current ~= node_start)
    [c_x,c_y] = index_to_coord( current );
    planpath = [planpath; [c_x, c_y]];
    current = came_from( current );
    stat_distance = stat_distance + 1;
end
[c_x,c_y] = index_to_coord( current );
planpath = [planpath; [c_x, c_y]];

% display path
figure(1);
plot(planpath(:,1),planpath(:,2),'y-','LineWidth',4);
plot(S(1),S(2),'m.','MarkerSize',25);
plot(G(1),G(2),'r.','MarkerSize',25);

axis([1 size_x 1 size_y])

stat_distance
stat_memory
stat_steps

%planpath


end