:- dynamic(hcache/2).


gs(Start,Solution) :-
  % cleanup the database.
  retractall(open(_)),
  retractall(closed(_)),
  retractall(g(_,_)),
  retractall(parent(_,_)),
  retractall(hcache(_,_)),
  % add the state state as an open state.
  assert(open(Start)),
  assert(g(Start,0)),
  assert(parent(Start,Start)),
  % begin search
  gs1(Solution).


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

 


select(State) :-
  % find an open state with minimal F value.
  findall(F-S, (open(S),f(S,F)), OpenList),
  minList(OpenList,State).



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



%% gets the h value for state S either by finding it as a cached
%% value or computing it.

get_h(S,H) :- hcache(S,H), !.
get_h(S,H) :- 
  h(S,H),
  assert(hcache(S,H)).



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

expand(State) :-
  g(State,StateCost),
  % add children of S to graph.
  foreach(
    arc(State,S,ArcCost),
    (Cost is StateCost+ArcCost,
     add_state(State,S,Cost))),
  % move state from open to closed.
  retract(open(State)),
  assert(closed(State)),
  dbug(expand(State)).
 


add_state(Parent,Child,NewCost) :-
  % this state is already in the graph.
  % update the path costs if the newly found path is better.
  g(Child,OldCost)
    -> update(Child,Parent,OldCost,NewCost)
     | (assert(open(Child)),
        assert(parent(Child,Parent)),
        assert(g(Child,NewCost))).


update(State,Parent,OldCostToState,NewCostToState) :-
  % we just found a way to get to State through Parent
  % with a new cheaper cost og Cost.  Check the Children of
  % State to see if we now have a better way to get to any of them.
  %
  % Update information on S
  OldCostToState>NewCostToState,
  !,
  retract(parent(State,_)),
  assert(parent(State,Parent)),
  retract(g(State,_)),
  assert(g(State,NewCostToState)),
  %check the children of S
  foreach(arc(State,Child,ArcCost),
          (g(Child,OldCostToChild),
           NewCostToChild is NewCostToState+ArcCost,
           update(Child,State,OldCostToChild,NewCostToChild))).

update(_,_,_,_).

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

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


%% ----- miscellaneous utilities ------------------------------------------

%% minlist(+List,?Term) is true if List is a list of terms of the form
%% N-X where N is a number and X is an arbitrary term and if ?Term is the
%% X associated with the smalles N in that list.
%%
%% e.g., minlist([3-foo,5-bar,1-mumble,6-qux],X) will bind x to qux.
%%


minList([Head|Tail],Best) :- minList1(Head,Tail,Best).

minList1(_-S1,[],S1).
minList1(F1-S1,[F2-S2|Rest],Best) :-
  F1=<F2
    -> minList1(F1-S1,Rest,Best)
     | minList1(F2-S2,Rest,Best).


forall(Binder,Body) :- \+((Binder,\+(Body))).

foreach(Binder,Body) :- Binder,do(Body),fail.
foreach(_,_).

% do(X) executes X only once.
do(X) :- X,!.

dbug(T) :-
 dbugging 
    -> (nl,write(T))
     | true.


dbugging.