% Изкуствен интелект, Факултет по математика и информатика, СУ "св. Кл. Охридски"
%--------------------------------------------------------------%

%   Informed Graph Searching Algorithms (Sicstus Prolog)       %

%   The graph should be represented as a set of facts          %

%   arc(Node1,Node2,Cost) loaded in the database               %

%--------------------------------------------------------------%



%--------------------------------------------------------------%

%   Best-First Search                                          %

%   call: best_first(+[[Start]],+Goal,-Path).                  %

%--------------------------------------------------------------%

best_first([[Goal|Path]|_],Goal,FinalPath):-
		reverse([Goal|Path],FinalPath).

best_first([Path|List],Goal,FinalPath):-
    extend(Path,NewPaths), 
    append(List,NewPaths,NewList),
    sort(NewList,Goal,SortedList),
    best_first(SortedList,Goal,FinalPath).


extend([Node|Path],NewPaths) :-

    findall([NewNode,Node|Path],

            (arc(Node,NewNode,_), 

            \+ member(NewNode,[Node|Path])), % for avoiding loops

            NewPaths).



sort([],Goal,[]).
sort(List,Goal,[MinPath|SortedList]):-
		min(List,Goal,MinPath,NewList),
		sort(NewList,Goal, SortedList).

min([Path1|List],Goal,Path2,[Path1|NewList]):-
	   min(List,Goal,Path2,NewList),
		h(Path1,Goal,H1),
		h(Path2,Goal,H2),
		H2<H1,!.
min([Path|List],Goal,Path,List).


%--------------------------------------------------------------%

%   Beam Search                                                %

%   call:beam(+[[Start]],+Goal,+BeamSize,-Path).               %

%--------------------------------------------------------------%

beam_search(Beam,[[Goal|Path]|_],Goal,FinalPath):-
		reverse([Goal|Path],FinalPath).

beam_search(Beam,[Path|List],Goal,FinalPath):-
    extend(Path,NewPaths), 
    append(List,NewPaths,NewList),
    sort(NewList,Goal,SortedList),
	  trim(Beam,SortedList,TrimedList),
    beam_search(Beam,TrimedList,Goal,FinalPath).




trim(N,[X|T],[X|V]) :- 

    N>0, !,

    M is N-1,

    trim(M,T,V).

trim(_,_,[]).

%--------------------------------------------------------------%

%   Hill Climbing 							   %

%   call:beam(+[[Start]],+Goal,+BeamSize,-Path).               %

%--------------------------------------------------------------%

% Version 1
hill_climbing1(List,Goal,FinalPath):-
    beam_search(1,List,Goal,FinalPath).

% Version2
hill_climbing([Goal|Path],Goal,FinalPath):-
		reverse([Goal|Path],FinalPath).

hill_climbing(Path,Goal,FinalPath):-
    extend(Path,NewPaths), 
   best_path(NewPaths,Goal,BestPath),
    hill_climbing(BestPath,Goal,FinalPath).


best_path([Path1|List],Goal,Path2):-
	    best_path(List,Goal,Path2),
		h(Path1,Goal,H1),
		h(Path2,Goal,H2),
		H2<H1,!.
best_path([Path|List],Goal,Path).



%--------------------------------------------------------------%

%   A*                                                         %

%   call: a_star(+[[Start]],+Goal,-Path).			         %

%--------------------------------------------------------------%

a_star([[Goal|Path]|_],Goal,FinalPath):-
	 reverse([Goal|Path], FinalPath).

a_star([Path|List],Goal,FinalPath) :-
    extend(Path,NewPaths),
    append(List,NewPaths,NewList), 
    sort1(NewList,Goal,SortedList),
    a_star(SortedList,Goal,FinalPath).


sort1([],Goal,[]).
sort1(List,Goal,[MinPath|SortedList]):-
		min1(List,Goal,MinPath,NewList),
		sort1(NewList,Goal, SortedList).

min1([Path1|List],Goal,Path2,[Path1|NewList]):-
	   min1(List,Goal,Path2,NewList),
		f(Path1,Goal,F1),
		f(Path2,Goal,F2),
		F2<F1,!.
min1([Path|List],Goal,Path,List).

%--------------------------------------------------------------%

%   Heuristic functions                                        %

%--------------------------------------------------------------%

h([Node|Path],Goal,H) :- stright_line_distance(Node,Goal,H). 



f(Path,Goal,F) :-                   % for A* search

    reverse_path_cost(Path,G), % calculate G

    h(Path,Goal,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).

reverse([],[]).
reverse([X|Rest],List):-
		reverse(Rest,NewRest),
		append(NewRest,[X],List).