chap2/chap2_matlab/code/A_star_search.m
function path = A_star_search(map,MAX_X,MAX_Y)
%%  This part is about map/obstacle/and other settings
%pre-process the grid map, add offset
size_map = size(map,1);
Y_offset = 0;
X_offset = 0;

%Define the 2D grid map array.
%Obstacle = -1, Target = 0, Start = 1
MAP = 2 * (ones(MAX_X, MAX_Y));

%Initialize MAP with location of the target
xval = floor(map(size_map, 1)) + X_offset;
yval = floor(map(size_map, 2)) + Y_offset;
xTarget = xval;
yTarget = yval;
MAP(xval,yval) = 0;

%Initialize MAP with location of the obstacle
for i = 2: size_map - 1
    xval = floor(map(i, 1)) + X_offset;
    yval = floor(map(i, 2)) + Y_offset;
    MAP(xval,yval) = -1;
end

%Initialize MAP with location of the start point
xval = floor(map(1, 1)) + X_offset;
yval = floor(map(1, 2)) + Y_offset;
xStart = xval;
yStart = yval;
MAP(xval,yval) = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LISTS USED FOR ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%OPEN LIST STRUCTURE
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
OPEN = [];

%CLOSED LIST STRUCTURE
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2);
CLOSED = [];

%Put all obstacles on the Closed list
k = 1; %Dummy counter
for i=1:MAX_X
    for j=1:MAX_Y
        if(MAP(i,j) == -1)
            CLOSED(k,1) = i;
            CLOSED(k,2) = j;
            k = k+1;
        end
    end
end
CLOSED_COUNT = size(CLOSED,1);

%set the starting node as the first node
xNode = xStart; % xval = xStart
yNode = yStart;
OPEN_COUNT = 1;
goal_distance = distance(xNode,yNode,xTarget,yTarget);
path_cost = 0;
OPEN(OPEN_COUNT,:) = insert_open(xNode,yNode,xNode,yNode,goal_distance,path_cost,goal_distance);
OPEN(OPEN_COUNT,1) = 0;
CLOSED_COUNT = CLOSED_COUNT + 1;
CLOSED(CLOSED_COUNT,1) = xNode;
CLOSED(CLOSED_COUNT,2) = yNode;
NoPath = 1;

%% This part is your homework
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while(NoPath == 1 && (xNode ~= xTarget || yNode ~= yTarget))
    % no path or arrive target
    
    % for unexpanded neighbors m of n
    exp_n = expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y);
    
    for i = 1:size(exp_n,1)
        flag = 0;
        for j = 1:OPEN_COUNT
            if (exp_n(i,1) == OPEN(j,2) && exp_n(i,2) == OPEN(j,3))
                if (exp_n(i,4)<OPEN(j,7))
                    % update parents
                    OPEN(j,4)=xNode;
                    OPEN(j,5)=yNode;
                    OPEN(j,7)=exp_n(i,4);
                    OPEN(j,8)=OPEN(j,7) + OPEN(j,6);
                end
                flag = 1; %do not need to insert to open list
            end
        end
        if flag == 0 %put new exp_n back to OPTN
            OPEN_COUNT = OPEN_COUNT + 1;
            %--------------------------------------------------------------------------
            %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
            %-------------------------------------------------------------------------
            OPEN(OPEN_COUNT,:) = insert_open(exp_n(i,1),exp_n(i,2),...
                xNode,yNode,exp_n(i,3),exp_n(i,4),exp_n(i,5));
        end
    end
    
    % Remove the node n with lowert f(n)
    fmin_index = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget);
    
    % No path can be found
    if fmin_index == -1
        NoPath = 0;
        break;
    end
    
    xNode = OPEN(fmin_index,2);
    yNode = OPEN(fmin_index,3);
    path_cost = OPEN(fmin_index,7);
    
    % Mark n as expanded
    % put pump_index to close
    OPEN (fmin_index, 1) = 0;
    
    % Put the visited open to close
    CLOSED_COUNT = CLOSED_COUNT + 1;
    CLOSED(CLOSED_COUNT,1) = xNode;
    CLOSED(CLOSED_COUNT,2) = yNode;
    
end %End of While Loop

%Once algorithm has run The optimal path is generated by starting of at the
%last node(if it is the target node) and then identifying its parent node
%until it reaches the start node. This is the optimal path

path = [];
if NoPath == 1 % path exist
    xval = xTarget;
    yval = yTarget;
    while ~(xval==xStart && yval==yStart)
        path = [path; xval-0.5, yval-0.5];
        current_index = node_index(OPEN, xval, yval);
        xval = OPEN(current_index,4);
        yval = OPEN(current_index,5);
    end
    path=[path; xStart-0.5, yStart-0.5]; % compensent plot number
end
end
