% graph search (aka algorithm A)

% works in SWI-Prolog.  
% Written by Tim Finin, http://umbc.edu/~finin/, finin@Umbc.edu


:- ensure_loaded(showPath).
:- ensure_loaded(dbug).
	
:- dynamic node/5.

/* **************************************************************

The graph is represented by facts of the form:
 node(S,Parent,Arcs,G,H) where

   S is a term representing this state.

   Parent is a term representing the state which is the immediate
    parent to s on the best known path from an intiial state to S.

   Arcs is either nil (no arcs recorded, i.e. S is in the set open) or
   a list of terms C-S2 which represents an arc from S to S2 of cost C.
 
   G is the cost of the best known path from the state state to S.

   H is the heuristic estimate of the cost of the best path
   from S to the nearest goal state.

In order to use gs, you must define the following predicates:

  - goal(S) true if S is a term which represents the goal state.
  - arc(S1,S2,C) true iff there is an arc from state S1 to S2 with cost C.
  - h(S,H) is the heuristic function as defined above.
  - f(G,H,F) F is the meteric used to select which nodes to expand next.
  - G and H are as defined above.  Default is "f(G,H,F) :- F is G+H.".
  - start(S) (optional) S is the state to start searching from.

************************************************************** */

gs :- 
  gs(Path), 
  showPath(Path).

gs(Solution) :-
  % if we aren't given a start state, then just pick one.
  start(S),
  gs(S,Solution).

gs(Start,Solution) :-
  % cleanup the database, add the start node, do the search and report.
  retractall(node(_,_,_,_,_)),
  h(Start,H),  
  assert(node(Start,Start,nil,0,H)),
  gSearch(Path),
  reverse(Path,Solution),
  gsReport(Solution).

gsReport(Path) :-
  % make a simple report on the solution.
  count(open(_),Open),
  count(closed(_),Closed),
  Total is Open+Closed,
  length(Path,PathLength),
  Path = [Start|_],
  last(Path,Goal),
  node(Goal,_,_,Cost,_),
  format("~p nodes in the graph with ~p expanded.~n",[Total,Closed]),
  format("Found a solution from ~p to ~p.~n",[Start,Goal]),
  format("  with length ~p nodes and cost ~p.~n",[PathLength,Cost]).

gSearch(Solution) :-
  select(State),
  (goal(State)
    ->  collect_path(State,Solution)
        | (expand(State), gSearch(Solution))).

select(State) :-
  % find an open state with minimal F value.
  findall(F-S, (node(S,_,nil,G,H),f(G,H,F)), OpenList),
  keysort(OpenList,[_-State|_]).

%% expand(S) expands state S by adding the children of S to 
%% the database and moving S from open to closed.

expand(State) :-
  dbug("Expanding state ~p.~n",[State]),
  retract(node(State,Parent,nil,G,H)),
  findall(ArcCost-Kid,
	  (arc(State,Kid,ArcCost),
	   NewG is G+ArcCost,   
           add_arc(State,Kid,NewG)),
	  Arcs),
  assert(node(State,Parent,Arcs,G,H)).

add_arc(Parent,Child,G) :-
  % Child is a new state so add it to the graph.
  (\+node(Child,_,_,_,_)),
  h(Child,H),
  dbug("Adding state ~p with parent ~p and cost ~p.~n",[Parent,Child,G]),
  assert(node(Child,Parent,nil,G,H)),
  !.

add_arc(Parent,Child,NewG) :-
  % Child state is already in the graph.
  % update the path costs to Child if the newly found
  % path thru Parent is better.
  node(Child,_CurrentParent,Arcs,CurrentG,H),
  CurrentG>NewG,
  !,
  % yes, this new path tru Parent is better.
  dbug("Updating ~p 's cost thru ~p to ~p.~n",[Child,Parent,NewG]),
  retract(node(Child,_,_,_,_)),
  assert(node(Child,Parent,Arcs,NewG,H)),
  % Check the Child's Children to see if we now have a better way to get to any of them.
  foreach(member(ArcCost-GrandChild,Arcs),
	  (GrandChildG is NewG+ArcCost,
          add_arc(Child,GrandChild,GrandChildG))).

add_arc(_,_,_,_).

collect_path(Start,[Start]) :- 
  % this is the start state.
  node(Start,Start,_Arcs,0,_H).

collect_path(S,[S|Path]) :-
  % add S to the evolving path back to the start state.
  node(S,Parent,_,_,_),
  collect_path(Parent,Path).


count(Goal,N) :-
  % N is the number of solutions found for Goal.
  findall(Goal,call(Goal),List),
  length(List,N).


open(S) :-  node(S,_,nil,_G,_H).
closed(S) :- node(S,_,Arcs,_G,_H), \+(Arcs=nil).

foreach(Binder,Body) :- call(Binder),once(Body),fail.
foreach(_,_).

% defaults 
arc(N1,N2,1) :- arc(N1,N2).

h(_,0).

f(G,H,F) :- F is G+H.