%
% 2D path planning example
%
% Yuri Malitsky (ynm@cs.brown.edu), Chad Jenkins (cjenkins@cs.brown.edu)
% Modified: February 1, 2009
%
% based on Wikipedia A* entry
%
% call as a function with 1 argument for the search algorithm 
%

function [ output_args ] = pathfind( algorithm )

if (~exist('algorithm'))
    algorithm = 1;
end

MapSize = 50;                     
visited = zeros(MapSize,MapSize); 
costs   = zeros(MapSize,MapSize); 


rectangle('Position',[0,0,MapSize,MapSize],'FaceColor','g');
hold on;

oxs = 10;   
oys = 30;   
oxw = 20;   
oyw = 10;   
rectangle('Position',[oxs,oys,oxw,oyw],'FaceColor','b');
for x = oxs:(oxs+oxw)
   for y = oys:(oys+oyw)
       visited(x,y) = 1;
   end
end

oxs = 30;  
oys = 20;   
oxw = 10;   
oyw = 20;   
rectangle('Position',[oxs,oys,oxw,oyw],'FaceColor','b');
for x = oxs:(oxs+oxw)
   for y = oys:(oys+oyw)
       visited(x,y) = 1;
   end
end

oxs = 30;   
oys = 10;   
oxw = 20;   
oyw = 10;   
rectangle('Position',[oxs,oys,oxw,oyw],'FaceColor','b');
for x = oxs:(oxs+oxw)
   for y = oys:(oys+oyw)
       visited(x,y) = 1;
   end
end



S = [10;10];                        
G = [45;30];                       


visited(S(1,1),S(2,1)) = 1;       

search_stack(:,1) = [0;0;S];
search_point = 1;

found = 0; 
currentDist = 0;

stat_distance = 0;
stat_memory = 0;
stat_steps = 0;

while ((search_point > 0) & (~found))   
    stat_memory = max(stat_memory, size(search_stack,2) );
    stat_steps = stat_steps + 1;

    if algorithm == 2 || algorithm == 3
        search_stack = sortrows( search_stack', 1)';
    end
    
    data = search_stack(:,search_point);
    currentDist = data(2);
    current = data(3:4);

    search_point = search_point - 1;

    for i = -1:1
        for j = -1:1
            if (~((i == 0) & (j == 0)))
                if ((current(1,1)+i < MapSize) & (current(2,1)+j < MapSize) & (current(1,1)+i > 0) & (current(2,1)+j > 0))
                    if (~visited(current(1,1)+i,current(2,1)+j))
                        visited(current(1,1)+i,current(2,1)+j) = 1;

                        parent(current(1,1)+i,current(2,1)+j,1) = current(1,1);
                        parent(current(1,1)+i,current(2,1)+j,2) = current(2,1);
 
                        if ((current(1,1)+i == G(1,1)) & (current(2,1)+j == G(2,1)))
                            found = 1;
                            stat_distance = currentDist;
                            fprintf(1,'found it!\n');
                        end
                        
                        step = sqrt(i*i + j*j);
                        dist = currentDist + step;
                        
                        if algorithm == 3
                            deltax = abs((current(1,1)+i)-G(1,1));
                            deltay = abs((current(2,1)+j)-G(2,1));
                            heuristicDist = sqrt(deltax*deltax + deltay*deltay); 

                            dist = dist + heuristicDist;
                        end

                        search_point = search_point + 1;
                        search_stack(:,search_point) = [-dist;currentDist+step;[current(1,1)+i;current(2,1)+j]];
                    end
                end
            end
        end
    end
end

clear current;
current = G;
planpath(1:2,1) = current; 

while ((S(1,1) ~= current(1,1)) | (S(2,1) ~= current(2,1)))
    tempcurrent(1,1) = parent(current(1,1),current(2,1),1);
    tempcurrent(2,1) = parent(current(1,1),current(2,1),2);
    planpath(1:2,size(planpath,2)+1) = tempcurrent;
    current = tempcurrent;
end

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

stat_distance
stat_memory
stat_steps

end
 


