% A* in Prolog as described in ch.4 of:
% George F. Luger and William A. Stubberfield.
% _AI Algorithms, Data Structures, and Idioms in Prolog, Lisp, and
% Java_. Addison Wesley, 2009
% (http://wps.aw.com/wps/media/objects/5771/5909832/PDF/Luger_0136070477_1.pdf),
% cited as [LS]
%

% The ADT Priority Queue (ch.3 [LS])
empty_pq([]).
insert_pq(State, [], [State]) :- !.
insert_pq(State, [H|Tail], [State,H|Tail]) :- precedes(State, H).
insert_pq(State, [H|T], [H|Tnew]) :-
	insert_pq(State, T, Tnew).
precedes([_,_,_,_,F1],[_,_,_,_,F2]) :- F1 < F2. % states are lists of 5 elts
%precedes(X, Y) :- X < Y. % depends on the problem
dequeue_pq(E, [E|T], T).
% dequeue_pq(E, [E|T], _). This clause for dequeue is an error.
member_pq(Element, Queue) :- member(Element, Queue).
insert_list_pq([], L, L).
insert_list_pq([State|Tail], L, New_L) :-
	insert_pq(State, L, L2),
	insert_list_pq(Tail, L2, New_L).

%The ADT Set (ch.3 [LS])
empty_set([]).
member_set(E,S) :- member(E,S).
delete_if_in_set(_, [], []).
delete_if_in_set(E, [E|T], T) :- !.
delete_if_in_set(E, [H|T], [H|T_new]) :-
	delete_if_in_set(E, T, T_new), !.
add_if_not_in_set(X, S, S) :- member(X, S), !.
add_if_not_in_set(X, S, [X|S]).
union([], S, S).
union([H|T], S, S_new) :-
	union(T, S, S2),
	add_if_not_in_set(H, S2, S_new), !.
subset([], _).
subset([H|T], S) :- member_set(H, S), subset(T, S).
intersection([],_,[]).
intersection([H|T], S, [H|S_new]) :-
	member_set(H, S),
	intersection(T, S, S_new), !.
intersection([_|T], S, S_new) :-
	intersection(T, S, S_new).
set_difference([], _, []).
set_difference([H|T], S, T_new) :-
	member_set(H, S),
	set_difference(T, S, T_new), !.
set_difference([H|T], S, [H|T_new]) :-
	set_difference(T, S, T_new), !.
equal_set(S1, S2) :-
	subset(S1, S2), subset(S2, S1).

% A* with monotone heuristics
% heuristic/3, unsafe/1, and move/2 are application_specific
% member_set/2 needs to work with lists containing five-element lists
%%% Each move has cost one. DOES NOT check whether g'<g---VERY
%%% SPECIAL CASE!!!%%%

go(Start, Goal) :-
	empty_set(Closed_set), empty_pq(Open),
	heuristic(Start, Goal, H),
	insert_pq([Start, nil, 0, H, H], Open, Open_pq),
	path(Open_pq, Closed_set, Goal).
path(Open_pq, _, _) :-
	empty_pq(Open_pq),
	write('Graph searched, no solution found.').
path(Open_pq, Closed_set, Goal) :-
	dequeue_pq([State, Parent, _, _, _], Open_pq, _),
	State = Goal,
	write('The solution path is: '), nl,
	printsolution([State, Parent, _, _, _], Closed_set).
path(Open_pq, Closed_set, Goal) :-
	dequeue_pq([State, Parent, D, H, S], Open_pq, Rest_open_pq),
	writelist([State, 'is taken off the OPEN list']),
	get_children([State, Parent, D, H, S],
		     Rest_open_pq, Closed_set, Children, Goal),
	insert_list_pq(Children, Rest_open_pq, New_open_pq),
	writelist(['after entering the successors of', State, 'the OPEN list is', New_open_pq]),
	union([[State, Parent, D, H, S]], Closed_set, New_closed_set),
	writelist(['after closing', State, 'the CLOSED list is', New_closed_set]),
	path(New_open_pq, New_closed_set, Goal), !.  % useless (green) cut?
get_children([State,_,D,_,_],Rest_open_pq, Closed_set, Children, Goal) :-
	(   bagof(Child, moves([State, _, D, _, _], Rest_open_pq,
			   Closed_set, Child, Goal),
	      Children); Children = []). % in SWI, bagof fails if no matches for 2nd argument
	% writelist(['The children of', State, 'are', Children]).
moves([State, _, Depth, _, _], Rest_open_pq, Closed_set,
      [Next, State, New_D, H, S], Goal) :-
	move(State, Next),
	not(unsafe(Next)),	% application specific
	not(member_pq([Next, _, _, _, _], Rest_open_pq)), % WORKS B/C ALL ARC COSTS ARE 1
	not(member_set([Next, _, _, _, _], Closed_set)), % assumes monotone h
	New_D is Depth + 1,
	heuristic(Next, Goal, H),  %application specific
	S is New_D + H.
printsolution([State, nil, _, _, _], _) :- write(State), nl.
printsolution([State, Parent, _, _, _], Closed_set) :-
	member_set([Parent, Grandparent, _, _, _], Closed_set),
	printsolution([Parent, Grandparent, _, _, _], Closed_set),
	write(State), nl.

% move/2 for FWGC problem
% state(Farmer, Wolf, Goat, Cabbage)
move(state(X,X,G,C), state(Y,Y,G,C)) :-
	opp(X,Y), not(unsafe(state(Y,Y,G,C))).
	% writelist(['try farmer-wolf', Y,Y,G,C]).
move(state(X,W,X,C), state(Y,W,Y,C)) :-
	opp(X,Y), not(unsafe(state(Y,W,Y,C))).
	% writelist(['try farmer-goat', Y,W,Y,C]).
move(state(X,W,G,X), state(Y,W,G,Y)) :-
	opp(X,Y), not(unsafe(state(Y,W,G,Y))).
	% writelist(['try farmer-cabbage', Y,W,G,Y]).
move(state(X,W,G,C), state(Y,W,G,C)) :-
	opp(X,Y), not(unsafe(state(Y,W,G,C))).
	% writelist(['try farmer by itself', Y,W,G,C]).
% move(state(F,W,G,C), state(F,W,G,C)) :-
	% writelist(['Backtrack at:', F,W,G,C]), fail.

% opp/2 for FWGC problem
opp(e,w). opp(w,e).

% unsafe/1 for FWGC problem
unsafe(state(X,Y,Y,_)) :- opp(X,Y). % wolf and goat together without farmer
unsafe(state(X,_,Y,Y)) :- opp(X,Y). % goat and cabbage together without farmer

% heuristic/3 for FWCG problem; start with all zeros.
heuristic(_,_,0).  % The uninformed heuristic

% writelist/1
writelist([]) :- nl.
writelist([E|T]) :- write(E), write(' '), writelist(T).















