function[p] =AstarPathPlanner(StartNode,stateSpace,EndNode,SizeofStateSpace)%To put gui back in un comment out lines 44,87,88,102,103,225-end
%Based on the ASTARDEMO Demonstration of A*fieldpointers 
%   By Bob L. Sturm, Ph. D., Assistant Professor
%   Department of Architecture, Design and Media Technology
%     formerly Medialogy
%   Aalborg University i Ballerup
%     formerly Aalborg University Copenhagen
%   $Revision: 0.1 $  $Date: 2010 Feb. 17 18h24:24$
%   Modified by Ian Graven
%   Department of Computer Science, Dalhousie University

%This function returns a list of x,y coordinates along the path

%1. StartNode: holds the x,y coordinates of the start node in the stateSpace
%matrix
%2. stateSpace: is a square matrix with ones for unreachable positions and
%zeros for passable positions
%3. EndNode: holds the x,y coordinates of the goal node in the stateSpace
%matrix
%4. SizeofStateSpace: is the length of one of the sides of the Square
%matirx stateSpace
%Example:
%states=[0,0,1;0,1,0;0,0,0;]
%The Starting node is [1,2]
%The Goal Node =[2,3]
%AstarPathPlanner([1,2],states,[2,3],3)
%will return
%      2     3
%      3     3
%      3     2
%      3     1
%      2     1
%      1     1
%      1     2
%These are the coordinates of the path from the goal position to the
%Starting Position. (Notice the path goes around the 1's in the Statespace as those are unaccessable)
%Note: StartNode~=EndNode (The code will not work if this is false)
%If stateSpace(EndNode(1),EndNode(2))==1 there is no solution
%If there is no solution, No Solution! will be printed
%This program assumes that starting node is accessable (That is
%stateSpace(StartNode(1),StartNode(2))==0)
n = SizeofStateSpace;   % field size n x n tiles
wallpercent = 0.0;  % this percent of field is walls

% create the n x n FIELD with wallpercent walls containing movement costs, 
% a starting position STARTPOSIND, a goal position GOALPOSIND, the costs 
% A* will computer (f = g + h) for each tile COSTCHART, and a matrix in 
% which to store the pointers FIELDPOINTERS
[field, startposind, goalposind, costchart, fieldpointers] = ...
  initializeField(n,wallpercent,StartNode,EndNode);
stateSpace1=zeros(SizeofStateSpace);
g=zeros(SizeofStateSpace);
for i=1:SizeofStateSpace
    for j=1:SizeofStateSpace
        if stateSpace(i,j)==1
            stateSpace1(i,j)=inf;
        end
    end
end
field=stateSpace1;
% initialize the OPEN and CLOSED sets and keep track of the f scores
setOpen = startposind; setOpenCosts = Inf;
setClosed = [setOpen]; setClosedCosts = [setOpenCosts];

% keep track of the number of iterations to exit gracefully if no solution
counterIterations = 1;

% create figure so we can witness the magic
% axishandle = createFigure(field,costchart,startposind,goalposind);%%

% as long as we have not found the goal and exceeded the number of tiles
while ~max(setOpen == goalposind) && counterIterations < n^2
  % for each element in OPEN
  for ii=1:length(setOpen)
    % find f in neighboring spaces that are not members of CLOSED
    [costs,posinds,pointers,g] = findFValue(setOpen(ii),setClosed,field,goalposind,g);
    % for each of these spaces
    for jj=1:length(posinds)
      % if there is a number this tile has not been looked at before
      if ~isnan(costs(jj))
        % if there is no pointer or it is not the "goal"
        if isempty(fieldpointers{posinds(jj)}) || strcmp(fieldpointers{posinds(jj)},'G')
          % record pointer, cost, and add tile position and cost to CLOSED
          fieldpointers(posinds(jj)) = pointers(jj);
          costchart(posinds(jj)) = costs(jj);
          setClosed = [setClosed; posinds(jj)];
          setClosedCosts = [setClosedCosts; costs(jj)];
        end
      end
    end
    % if we have looked at all neighboring spaces, move tile to CLOSED
    % permanently with an infinite f value
    if sum(isnan(costs) + isinf(costs)) == 4
      setClosed = [setClosed; setOpen(ii)];
      setClosedCosts = [setClosedCosts; Inf];
    end
  end
  
  % find tiles in CLOSED with smallest costs, and transfer these to OPEN
  IOpen = find(setClosedCosts == min(setClosedCosts));
  setOpen = setClosed(IOpen);
  setOpenCosts = setClosedCosts(IOpen);
  
  % keep all other tiles in CLOSED
  IClosed = find(setClosedCosts > min(setClosedCosts));
  setClosed = setClosed(IClosed);
  setClosedCosts = setClosedCosts(IClosed);
  
  % make sure those positions with walls have infinite cost
  costchart(field == Inf) = Inf;
  % replace data in figure with new costs, and draw now, then pause
%   set(axishandle,'CData',[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);%%
%   drawnow; pause(0.1);%%
  % if there are no more possibilities, give up à la Pee-Wee
  if isempty(setOpen) || isempty(setClosed)
    disp('No Solution!');% [y,Fs] = wavread('pewee-ahh.wav'); sound(y,Fs);
    return;
  end
  % hack to make image look right
  % set(gca,'CLim',[0 3*max(costchart(find(costchart < Inf)))]);
  % iterate counter
  counterIterations = counterIterations + 1;
end
% now find the way back using FIELDPOINTERS, starting from goal position
p = findWayBack(goalposind,fieldpointers);
% plot final path
% plot(p(:,2)+0.5,p(:,1)+0.5,'k--','LineWidth',8);%%
% drawnow;%%
% celebrate
% [y,Fs] = wavread('wee.wav'); sound(y,Fs);
fieldpointer=[p(end,1)-p(end-1,1),p(end,2)-p(end-1,2)];
% fieldpointer1=[];
if fieldpointer(1)==0
    fieldpointer1 = fieldpointer(2)+3;
else
    fieldpointer1 = fieldpointer(1)+2;
end


% end of the main function

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function p = findWayBack(goalposind,fieldpointers)
% This function will follow the pointers from the goal position to the
% starting position
    n = length(fieldpointers);  % length of the field
    posind = goalposind;
    % convert linear index into [row column]
    [py,px] = ind2sub([n,n],posind);
    % store initial position
    p = [py px];
    % until we are at the starting position
    while ~strcmp(fieldpointers{posind},'S')
      switch fieldpointers{posind}
        case 'L' % move left
          px = px - 1;
        case 'R' % move right
          px = px + 1;
        case 'U' % move up
          py = py - 1;
        case 'D' % move down
          py = py + 1;
      end
      p = [p; py px];
      % convert [row column] to linear index
      posind = sub2ind([n n],py,px);
    end
% end of this function

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [cost,posinds,pointer,g] = findFValue(posind,setClosed,field,goalposind,g)
% This function finds the f = g + h value for each tile around that at
% POSIND and that is not included in CLOSED. Heuristic is computed using
% the Manhattan distance, also know as the city block or taxi cab distance
    n = length(field);  % length of the field
    % convert linear index into [row column]
    [currentpos(1) currentpos(2)] = ind2sub([n n],posind);
    [goalpos(1) goalpos(2)] = ind2sub([n n],goalposind);
    % places to store f value, position, and pointer
    cost = NaN*ones(4,1); pos = ones(4,2); pointer = cell(4,1);
    
    % if we can look left
    newx = currentpos(2) - 1; newy = currentpos(1);
    if newx > 0 && ~max(setClosed == sub2ind([n n], newy, newx))
      pos(1,:) = [newy newx];
      pointer{1} = 'R';
      
      if g(newx,newy)+1<g(currentpos(2),currentpos(1))|| g(newx,newy)==0
          g(newx,newy)=g(currentpos(2),currentpos(1))+1;
      end
      cost(1) = field(newy,newx) + abs(goalpos(2)-newx) + abs(goalpos(1)-newy)+ g(newx,newy);
    end

    % if we can look right
    newx = currentpos(2) + 1; newy = currentpos(1);
    if newx <= n && ~max(setClosed == sub2ind([n n], newy, newx))
      pos(2,:) = [newy newx];
      pointer{2} = 'L';
      if g(newx,newy)+1<g(currentpos(2),currentpos(1))|| g(newx,newy)==0
          g(newx,newy)=g(currentpos(2),currentpos(1))+1;
      end
      cost(2) = field(newy,newx) + abs(goalpos(2)-newx) + abs(goalpos(1)-newy)+ g(newx,newy);
    end

    % if we can look up
    
    newx = currentpos(2); newy = currentpos(1)-1;
    if newy > 0 && ~max(setClosed == sub2ind([n n], newy, newx))
      pos(3,:) = [newy newx];
      pointer{3} = 'D';
      if g(newx,newy)+1<g(currentpos(2),currentpos(1))|| g(newx,newy)==0
          g(newx,newy)=g(currentpos(2),currentpos(1))+1;
      end
      cost(3) = field(newy,newx) + abs(goalpos(2)-newx) + abs(goalpos(1)-newy)+ g(newx,newy);
    end
    % if we can look down
    newx = currentpos(2); newy = currentpos(1)+1;
    if newy <= n && ~max(setClosed == sub2ind([n n], newy, newx))
      pos(4,:) = [newy newx];
      pointer{4} = 'U';
      if g(newx,newy)+1<g(currentpos(2),currentpos(1))|| g(newx,newy)==0
          g(newx,newy)=g(currentpos(2),currentpos(1))+1;
      end
      cost(4) = field(newy,newx) + abs(goalpos(2)-newx) + abs(goalpos(1)-newy)+ g(newx,newy);
    end
    % return [row column] to linear index
    posinds = sub2ind([n n],pos(:,1),pos(:,2));
% end of this function

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [field, startposind, goalposind, costchart, fieldpointers] = ...
  initializeField(n,wallpercent,StartNode,EndNode)
% This function will create a field with movement costs and walls, a start
% and goal position at random, a matrix in which A* will store f values,
% and a cell matrix in which A* will store pointers
    % create the field and place walls with infinite cost
    field = ones(n,n);  % replace with 0.1*rand(n,n) to have fun!
    field(ind2sub([n n],ceil(n^2.*rand(floor(n*n*wallpercent),1)))) = Inf;
    % create random start position and goal position
    startposind = sub2ind([n,n],StartNode(1),StartNode(2));%,ceil(n.*rand),ceil(n.*rand));
    goalposind = sub2ind([n,n],EndNode(1),EndNode(2));%,ceil(n.*rand),ceil(n.*rand));
    % force movement cost at start and goal positions to not be walls
    field(startposind) = 1; field(goalposind) = 1;
    % put not a numbers (NaN) in cost chart so A* knows where to look 
    costchart = NaN*ones(n,n);
    % set the cost at the starting position to be 0
    costchart(startposind) = 0;
    % make fieldpointers as a cell array
    fieldpointers = cell(n,n);
    % set the start pointer to be "S" for start, "G" for goal
    fieldpointers{startposind} = 'S'; fieldpointers{goalposind} = 'G';
    % everywhere there is a wall, put a 0 so A* does not consider it
    fieldpointers(field == Inf) = {0};
% end of this function
% 
% %%%%%%%%%%%%%%%%%%%%  
% function axishandle = createFigure(field,costchart,startposind,goalposind)%%
% % This function creates a pretty figure
%     % If there is no figure open, then create one
%     if isempty(gcbf)
%       f1 = figure('Position',[600 300 500 500],'Units','Normalized', ...
%         'MenuBar','none');
%       Caxes2 = axes('position', [0.01 0.01 0.98 0.98],'FontSize',12, ...
%         'FontName','Helvetica');
%     else
%       % get the current figure, and clear it
%       gcf; cla;
%     end
%     n = length(field);
%     % plot field where walls are black, and everything else is white
%     field(field < Inf) = 0;
%     pcolor([1:n+1],[1:n+1],[field field(:,end); field(end,:) field(end,end)]);
%     % set the colormap for the ploting the cost and looking really nice
%     cmap = colormap('jet');
%     % make first entry be white, and last be black
%     cmap(1,:) = zeros(3,1); cmap(end,:) = ones(3,1);
%     % apply the colormap, but make red be closer to goal
%     colormap(flipud(cmap));
%     % keep the plot so we can plot over it
%     hold on;
%     % now plot the f values for all tiles evaluated
%     axishandle = pcolor([1:n+1],[1:n+1],[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);
%     % plot goal as a yellow square, and start as a green circle
%     [goalposy,goalposx] = ind2sub([n,n],goalposind);
%     [startposy,startposx] = ind2sub([n,n],startposind);
%     plot(goalposx+0.5,goalposy+0.5,'ys','MarkerSize',10,'LineWidth',6);
%     plot(startposx+0.5,startposy+0.5,'go','MarkerSize',10,'LineWidth',6);
%     % add a button so that can re-do the demonstration
%     uicontrol('Style','pushbutton','String','RE-DO', 'FontSize',12, ...
%       'Position', [1 1 60 40], 'Callback','astardemo');
%  % end of this function 