/* SYS$USER2:[PROLOG.PRO]ASTAR.DAT;33 */

/*  A* algorithm for searching trees, or general OR graphs*/
/*   for a heuristic function h which is a lower bound on */
/*   the cost remaining until reaching the goal.          */

search(Node,Answer) :-
   searchsub([leaf(0,Node,[])], [], leaf(1000000000,Node,[]),Answer).

/* searchsub(priority_queue_of_leaves, internal_nodes,
         best_goal_so_far, Answer)
   where the search keeps going until the queue is empty or
   the cost of the leaf node on the front of the priority queue
   has a cost greater than the cost of the best_goal_so_far.   */

searchsub([],Parents,leaf(Cutoff,Goal,Bestfather),Answer)
   :- goal(Goal),
      findanswer(Goal,Parents,Answer).
searchsub([leaf(Cost,_,_)|Rest],Parents,leaf(Cutoff,Goal,Bestfather),Answer)
   :- Cost > Cutoff, !, goal(Goal), findanswer(Goal,Parents,Answer).
searchsub(Queue, Parents, Oldbest,Answer)
   :- Queue=[leaf(_,Node,Father)|_],
       /* Either change the Bestsofar if Node is a goal node, or else
       put Node's sons in order into the priority Queue to get Newsorted */
      goal_or_expand(Queue,Parents,Newparents,Node,Father,
            Oldbest,Bestsofar,Newsorted),
      searchsub(Newsorted, Newparents, Bestsofar, Answer).

goal_or_expand([First|Rest],Parents,Parents,Node,Father,Oldbest,Newbest,Rest)
   :- goal(Node), !,
      checkbest(First,Oldbest,Newbest) .
goal_or_expand([leaf(Cost,Node,Father) | Rest],
   Parents, Newparents, _, _, Oldbest,Oldbest,Newsorted)
   :- Oldbest=leaf(Cutoff,_,_),
      findall(leaf(Evalfunc,Y,Node),
                     ok(Node,Y,Cost,Cutoff,Evalfunc),
                     List),
      insertlist(List,Rest,Newsorted,Parents,Newparents).

checkbest(leaf(Cost,Node,Father),leaf(Cutoff,_,_),leaf(Cost,Node,Father)) :-
      Cost =< Cutoff, !.
checkbest(_,X,X).

insertlist([],L,L,Parents,Parents) :- !.
insertlist([X|Y],L,Z,Parents,Newparents) :-
     replace(X,Parents,Updatedparents),
     !,
     insert1(X,L,NewL),
     insertlist(Y,NewL,Z,Updatedparents,Newparents).
insertlist([X|Y],L,Z,Parents,Newparents) :-
     insert1(X,L,NewL), insertlist(Y,NewL,Z,Parents,Newparents).
insert1(X,[],[X]).
insert1(leaf(Cost,Node,W), Z, [leaf(Cost,Node,W)|Z]) :-
Š     Z=[leaf(C,_,_)| _], Cost=<C, ! .
insert1(X,[Y|Z],[Y|W]) :- insert1(X,Z,W).



/* Assume that h is a lower bound, so can cut off nodes whose
   evaluation function > cost of best partial solution  */
ok(Node,Son,Cost,Cutoff,T) :-
                      edge(Node,Son,C),
                      estimate(Son,C,Est),
                      T is C + Cost + Est,
                      T =< Cutoff.

replace(X,[],[X]) :- !.
replace(leaf(T,Son,Father),[leaf(Oldcost,Son,_)|Rest],
           [leaf(T,Son,Father)|Rest]) :- !, T<Oldcost.
replace(X,[Y|Z],[Y|W]) :- replace(X,Z,W).

findanswer(Goal,Parents,[T,Goal|Path]) :-
   member(leaf(T,Goal,Father), Parents),
   Father\==[],
   !,
   findanswer(Father,Parents,Path).
findanswer(X,_,[0,X]).

member(X,[X|Y]) :- !.
member(X,[Y|Z]) :- member(X,Z), !.

findall(X,G,_) :-
   asserta(found(mark)),
   call(G),
   asserta(found(X)),
   fail.
findall(_,_,L) :- collect_found([],M), !, L = M.

collect_found(S,L) :- getnext(X), !, collect_found([X|S],L).
collect_found(L,L).

getnext(X) :- retract(found(X)), !, X \== mark.

/* The following heuristic function of the remaining cost should be
   changed so that the 3rd argument is an underestimate (lower
   bound) of the remaining cost to a goal. The first argument is
   the new leaf node, and the second argument is the cost of the
   edge which goes to this leaf node.                           */

estimate(_,_,0).

edge(a,b,10). edge(b,c,4). edge(a,c,5). edge(c,d,7). edge(b,d,4).
edge(d,e,5). edge(e,f,8). edge(b,d,3). edge(d,f,10). edge(e,g,4).
edge(b,g,15). goal(g).