%
%  CIS 8603
%  A* Project
%
%  Team Members:
%                Semir Elezovikj
%                Mo'taz Abdul Aziz Ali Al-Hami
%                Howard Liu
%


function [path_rows path_cols] = astar(map,start,goal)

% initialization
start_row = start(1);
start_col = start(2);

goal_row = goal(1);
goal_col = goal(2);

neighbor_list = [-1 -1; 0 -1; 1 -1; -1 0; 1 0; -1 1; 0 1; 1 1];
% this is used to create neighbor nodes (successors) of 8 different directions

% [map_r, map_c] = size(map);
% max_size = map_r * map_c;
map_r = 100;    % since we know the map size is fixed to 100x100
map_c = 100;
max_size = 10000;

came_from_x = zeros(map_r, map_c);  % this matrix records the parent nodes' x coords
came_from_y = zeros(map_r, map_c);  % this matrix records the parent nodes' y coords

g_score = zeros(map_r, map_c);      % matrix for g(n)
g_score(start_col, start_row) = map(start_col, start_row);  % g(start)

f_score = zeros(map_r, map_c);  % matrix for f(n)

% f(n) = g(n) + h(n)
% where h(n) = Diagonal Distance = max(abs(x1-x2),abs(y1-y2))
f_score(start_col, start_row) = g_score(start_col, start_row) + max(abs(goal_row - start_row), abs(goal_col - start_col));  % f(start)

isInClosedSet = zeros(map_r, map_c);    % binary matrix that indicates whether a node is in Closed set or not

openset = zeros(max_size,2);            % Open set
openset(1,:) = [start_row start_col];
openset_size = 1;

isInOpenSet = zeros(map_r, map_c);      % binary matrix that indicates whether a node is in Open set or not
isInOpenSet(start_col,start_row) = 1;

path = zeros(2,max_size);               % used for storing the final resulting path

% A* algorithm starts

while (openset_size > 0)
    
    % current node: select the node in Open set with the lowest f(n) score
    
    min_f = f_score(openset(1,2), openset(1,1));
    min_f_index = 1;
    for i = 2:1:openset_size
        if min_f > f_score(openset(i,2), openset(i,1));
            min_f = f_score(openset(i,2), openset(i,1));
            min_f_index = i;
        end
    end
    current = openset(min_f_index,:);
    
    % if current node is the goal node, we're done
    
    if (current(1) == goal(1)) && (current(2) == goal(2));
        path(1,1) = current(1);
        path(2,1) = current(2);
        k = 1;
        while(path(1,k)>0 && path(2,k)>0)   % backtracking the path from goal to start
            path(1,k+1) = came_from_x(path(2,k), path(1,k));
            path(2,k+1) = came_from_y(path(2,k), path(1,k));
            k=k+1;
        end
        path_rows = path(1,k-1:-1:1);
        path_cols = path(2,k-1:-1:1);
        return
        
    end
    
    % remove current node from Open set
    openset(min_f_index,1) = openset(openset_size,1);   % copy the last row to replace the current row
    openset(min_f_index,2) = openset(openset_size,2);
    openset_size = openset_size - 1;                    % decrement size
    isInOpenSet(current(2),current(1)) = 0;
    
    
    % add current node to Closed set
    isInClosedSet(current(2), current(1)) = 1;
    
    
    % loop for all neighbors (successors)
    
    for n = 1:8
        neighbor = neighbor_list(n,:) + current;
        
        if(neighbor(1)<=0 || neighbor(1)>map_c || neighbor(2)<=0 || neighbor(2)>map_r)
            continue                 % neighbor node is not inside the map
        end
        
        
        if (isInClosedSet(neighbor(2),neighbor(1)) == 1)
            continue                 % neighbor node already in Closed set
        end
        
        % tentative_g_score = g(current) + cost_from_current_to_neighbor
        tentative_g_score = g_score(current(2), current(1)) + map(neighbor(2), neighbor(1));
        
        
        % if neighbor is not in Open set or tentative_g_score < g(neighbor)
        if isInOpenSet(neighbor(2),neighbor(1)) == 0 || tentative_g_score < g_score(neighbor(2),neighbor(1))
            
            % if neighbor is not in Open set, add it to Open set
            if isInOpenSet(neighbor(2),neighbor(1)) == 0
                openset_size = openset_size + 1;        % increment size
                openset(openset_size,1) = neighbor(1);  % copy to the end of Open set
                openset(openset_size,2) = neighbor(2);
                isInOpenSet(neighbor(2),neighbor(1)) = 1;
            end
            
            % records the parent node, so we can backtrack the path
            came_from_x(neighbor(2),neighbor(1)) = current(1);
            came_from_y(neighbor(2),neighbor(1)) = current(2);
            
            % set g(neighbor) = tentative_g_score
            g_score(neighbor(2),neighbor(1))= tentative_g_score;
            
            % compute f(neighbor) = tentative_g_score + h(neighbor)
            % where h(n) = Diagonal Distance = max(abs(x1-x2),abs(y1-y2))
            f_score(neighbor(2),neighbor(1)) = tentative_g_score + max(abs(goal_row - neighbor(1)), abs(goal_col - neighbor(2)));
            
        end
    end
    
end

% return failure (no path found)
path_rows = [start_row];
path_cols = [start_col];
end


