%% Function A7AStar()
% map- matrix full of zeros denoting free space and ones denoting
%  obstacles.
% start- start point
% qgoal- end point
% path- path from start to goal
% Takes in a map, start point, and goal point and returns a path from the
% start to the goal via the Wavefront planner algorithm.
function path = Key_AStar(start, qgoal,map)

% The hueristic function will be a function for ease of calculation.
heur = @(q) max(abs(qgoal - q));

% List of neighbor offsets. 
nei = [-1 -1;-1 0;-1 1;0 -1;0 1;1 -1;1 0;1 1]; %8-point
% nei = [-1 0;0 -1;0 1;1 0];	%4-point

%Open list stores [pointX pointY (distance from start) 
%	(hueristic to goal + distance from start)]
open = [start 0 heur(start)];

%Negate the map
map = -map;

%size of the map for reference in back function
[mapn mapm] = size(map);    

% The back function will be a cell. Each entry in the cell corresponds to
% an entry in the map. Each entry will store either nothing (ie the point
% is not in the closed list) or the previous point with the distance from 
% the start.
% Closed list isn't actually used- it's easier to test the back function
% for existence.
back = cell(mapn,mapm);
back{start(1),start(2)} = -1; %flag the path start

while(~isempty(open))   %main loop
	
    %sort the open list in accordance with the actual distance to point
    %plus the heuristic of the point (column 4 of the list)
    open = sortrows(open,4);
	
	%There is a possibilty of repeats in the open list. Since the list is
	%sorted, we can remove the lesser-performing repeats with these lines.
	[~, I, ~] = unique(open(:,1:2),'rows','first');
	I = sort(I);
	open = open(I,:);
	
	% Pop the first point off the open list
	x = open(1,:);
	open = open(2:end,:);
    
    %if the best in the open list happens to be the goal
    if(x(1:2) == qgoal), break; end;
	
	% Assign this point in the map its distance from the start.
	map(x(1),x(2)) = x(3);
	
% 	imagesc(map)
% 	pause(.001);

	% Expand current poin x by adding it's valid neighbors to the open
	% list.
	for k=1:length(nei)
		% Try to create a new x value. If we err doing this, we don't want
		% that point.
		try
			% Make sure this point is not an obstacle. If it is, go to the
			% next neighbor.
			if(map(x(1)+nei(k,1),x(2)+nei(k,2)) == -1),continue;end;
			
			%newly expanded value
			newx = [x(1)+nei(k,1),...	%x index
				    x(2)+nei(k,2),...	%y index
					x(3)+1,...			%distance from goal
					%distance plus hueristic
					x(3)+1+heur([x(1)+nei(k,1),x(2)+nei(k,2)])]; 
			
		% Go to next point on error
		catch e,continue;end
		
		% Check to see if this neighbor has been added to the closed list.
		% (Does this point have an entry in the back path?)
		if(~isempty(back{newx(1),newx(2)},2))
			%this has to be nested, or an out of bound error may occur
			%with the back cell
			
			% If the back path does exist, add it to the open list if the
			% previous distance from start is greater than this one.
			if(back{newx(1),newx(2)}(3) > newx(3))
				%we add it again to the open list to see if points
				%adjacent to that point can be reached more quickly
				open = [newx;open];
				
				% Update the back path with new info.
				back{newx(1),newx(2)} = x([1,2,3]);
			end
		else
			% This neighbor is not part of the back path, so add it to open
			% list and back path.
			open = [newx;open];
			back{newx(1),newx(2)} = x([1,2,3]);
		end
		
	end
end

%% We're done the algorithm. Now we back-search for the start from the
%  goal.
if(isempty(back{qgoal(1),qgoal(2)})) %the goal has no back path; FAIL
    disp('Unreachable');
    path = -1;
else
    %The goal has a backpath; therefore, we can get to the start from the
    %goal.
	
	% Check the goal and add it's back path to a temporary variable. Repeat
	% process for that point and consecutive points until the start is
	% found.
    tmp = qgoal;
    while(back{tmp(end,1),tmp(end,2)} ~= -1)
        tmp(end+1,:) = back{tmp(end,1),tmp(end,2)}([1,2]);
	end
	
	%path is the tmp variable backwards
	path = tmp(end:-1:1,:);
	
	%Plot the path.
	imagesc(map); colormap gray;
	hold on;
	plot(path(:,2),path(:,1),'k','LineWidth',3);
	hold off;
end


end