%--------------------------------------------------------------%
%   Informed Graph Searching Algorithms (Sicstus Prolog)       %
%   The graph should be represented as a set of facts          %
%   arc(Node1,Node2,Cost) loaded in the database               %
%   (C) 1998 Zdravko Markov                                    %
%--------------------------------------------------------------%

%--------------------------------------------------------------%
%   Best-First Search                                          %
%   call: best_first(+[[Start]],+Goal,-Path,-ExploredNodes).   %
%--------------------------------------------------------------%
best_first([[Goal|Path]|_],Goal,[Goal|Path],0).
best_first([Path|Queue],Goal,FinalPath,N) :-
    extend(Path,NewPaths), 
    append(Queue,NewPaths,Queue1),
    sort_queue1(Queue1,NewQueue),
    best_first(NewQueue,Goal,FinalPath,M),
    N is M+1.

extend([Node|Path],NewPaths) :-
    findall([NewNode,Node|Path],
            (arc(Node,NewNode,_), 
            \+ member(NewNode,Path)), % for avoiding loops
            NewPaths).

sort_queue1(L,L2) :-
    swap1(L,L1), !,
    sort_queue1(L1,L2).
sort_queue1(L,L).

swap1([[A1|B1],[A2|B2]|T],[[A2|B2],[A1|B1]|T]) :-
    h(A1,W1),
    h(A2,W2),
    W1>W2.
swap1([X|T],[X|V]) :-
    swap1(T,V).

%--------------------------------------------------------------%
%   Beam Search                                                %
%   call:beam(+[[Start]],+Goal,+BeamSize,-Path,-ExploredNodes).%
%--------------------------------------------------------------%
beam([[Goal|Path]|_],Goal,_,[Goal|Path],0).
beam([Path|Queue],Goal,Beam,FinalPath,N) :-
    extend(Path,NewPaths), 
    append(Queue,NewPaths,Queue1), 
    sort_queue1(Queue1,Queue2),
    trim(Beam,Queue2,NewQueue),
    beam(NewQueue,Goal,Beam,FinalPath,M),
    N is M+1.

trim(N,[X|T],[X|V]) :- 
    N>0, !,
    M is N-1,
    trim(M,T,V).
trim(_,_,[]).

%--------------------------------------------------------------%
%   A*                                                         %
%   call: a_star(+[[Start]],+Goal,-Path,-ExploredNodes).       %
%--------------------------------------------------------------%
a_star([[Goal|Path]|_],Goal,[Goal|Path],0).
a_star([Path|Queue],Goal,FinalPath,N) :-
    extend(Path,NewPaths),
    append(Queue,NewPaths,Queue1), 
    sort_queue2(Queue1,NewQueue),
    a_star(NewQueue,Goal,FinalPath,M),
    N is M+1.

sort_queue2(L,L2) :-
    swap2(L,L1), !,
    sort_queue2(L1,L2).
sort_queue2(L,L).

swap2([X,Y|T],[Y,X|T]) :-
    f(X,DX),
    f(Y,DY),
    DX>DY.
swap2([X|T],[X|V]) :-
    swap2(T,V).

%--------------------------------------------------------------%
%   Heuristic functions                                        %
%--------------------------------------------------------------%
h(X,H) :-                       % for best-first and beam search
    stright_line_distance(X,H). % the distance to Bucharest

f([X|T],F) :-                   % for A* search
    reverse_path_cost([X|T],G), % calculate G
    h(X,H),                     % calculate H
    F is G+H.                   % F = G + H

reverse_path_cost([A,B],Cost) :-
    arc(B,A,Cost).
reverse_path_cost([A,B|T],Cost) :-
    arc(B,A,Cost1),
    reverse_path_cost([B|T],Cost2),
    Cost is Cost1+Cost2.

%--------------------------------------------------------------%
% Auxilliary predicates                                

member(X,[X|_]).
member(X,[_|T]) :-
    member(X,T). 

append([],L,L).
append([H|T],L,[H|V]) :-
    append(T,L,V).

