%--------------------------------------------------------------------------
% Test of ASTAR Path Planning Implementation
%--------------------------------------------------------------------------

% Importing Original Image File
Image=imread('imobstacle.png');

% Generating a Binary Image Map
    % BW=FindObjects(Image);
    % Returns a binary image map when the black boxes represent the areas
    % where the path is not allowed (Matrix locations where the value is
    % equal to 0) [0=Black 1=White]
% Temporary file import
load 'imdata.mat';
BW=FindObjects(im);
%BW(150:200,250:300)=0;
%MAP=imcomplement(BW);
MAP(200:350,150:200)=0;
MAP(100:200,1:200)=0;
MAP=imcomplement(MAP);
% Take the Start and End Positions
Start=Select_im_pts(Image,1); % Y loc first, X loc second
End=Select_im_pts(Image,1); % Y loc first, X loc second

% Create the Goal Node for the algorithm to process
Goal=int8(zeros(480,640));
Goal(End(1),End(2))=1;

% Create an arbitrary distance between nodes
Distance=8;

% Running Path Planning
Path=ASTAR_Algorithm(Start(2),Start(1),MAP,Goal,Distance);
% End

if size(Path,2)>1
    figure(10)
    imagesc((MAP))
    colormap(flipud(gray));

    hold on
    plot(Path(1,2),Path(1,1),'o','color','k')
    plot(Path(end,2),Path(end,1),'o','color','b')
    plot(Path(:,2),Path(:,1),'r')
    legend('Goal','Start','Path')
else
     pause(1);
 h=msgbox('Sorry, No path exists to the Target!','warn');
 uiwait(h,5);
end