plan(State, Goal, _, Moves) :-
  equal_set(State, Goal),
  write('moves are'), nl,
  reverse_print_stack(Moves).
plan(State, Goal, Been_list, Moves) :-
  move(Name, Preconditions, Actions),
  conditions_met(Preconditions, State),
  change_state(State, Actions, Child_state),
  not(member_state(Child_state, Been_list)),
  stack(Child_state, Been_list, New_been_list),
  stack(Name, Moves, New_moves),
  write('Trying '), write(Name), nl, 
  plan(Child_state, Goal, New_been_list, New_moves),!.

change_state(S, [], S).
change_state(S, [add(P)|T], S_new) :-
  change_state(S, T, S2), add_if_not_in_set(P, S2, S_new), !.
change_state(S, [del(P)|T], S_new) :-
  change_state(S, T, S2),
  delete_if_in_set(P, S2, S_new), !.
  conditions_met(P, S) :- subset(P, S).

member_state(S, [H|_]) :- equal_set(S, H)   
/* , write('FOUND STATE = '), write(S), nl */
.
member_state(S, [_|T]) :- member_state(S, T).

reverse_print_stack(S) :- empty_stack(S).
reverse_print_stack(S) :- 
  stack(E, Rest, S), 
  reverse_print_stack(Rest),
  write(E), nl.

/* sample moves: form is move(Name,Precond,Actions) */

move(unstack(X,Y), [handempty, clear(X), on(X, Y)],
  [del(handempty),del(clear(X)),del(on(X, Y)),add(clear(Y)),add(holding(X))]).
move(pickup(X), [handempty, clear(X), ontable(X)],
  [del(handempty), del(clear(X)), del(ontable(X)), add(holding(X))]).
move(putdown(X), [holding(X)], 
  [del(holding(X)), add(ontable(X)), add(clear(X)), add(handempty)]).
move(stack(X, Y), [holding(X), clear(Y)],
  [del(holding(X)),del(clear(Y)),add(handempty),add(on(X, Y)),add(clear(X))]).

go(S, G) :- plan(S, G, [S], []).

test1 :-
 go([handempty, ontable(b), ontable(c), on(a, b), clear(c), clear(a)],
    [handempty, ontable(c), on(a,b), on(b, c), clear(a)]).
test2 :-
 go([handempty, ontable(a), ontable(c), ontable(d), clear(c), clear(b),
     clear(e), on(b,a), on(e,d)],
    [handempty, ontable(c), ontable(d), clear(b), clear(e), on(b,a),
     on(a,c), on(e,d)]). 

/* Stack ADT */

empty_stack([]).
stack(Top,Stack,[Top|Stack]).
stack(T,_,_) :- write('UNSTACKING '), write(T), nl, !, fail.
member_stack(Element,Stack) :- member(Element,Stack).
add_list_to_stack(List,Stack,Result) :- append(List,Stack,Result).

/* Set ADT */

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_diff([],_,[]).
set_diff([H|T],S,T_new) :- member_set(H,S), set_diff(T,S,T_new), !.
set_diff([H|T],S,[H|T_new]) :- set_diff(T,S,T_new), !.

equal_set(S1,S2) :- subset(S1,S2), subset(S2,S1).

/* Sample Trace */
/*
 ?- test.
Trying unstack(a, b)
Trying putdown(a)
Trying pickup(b)
Trying stack(b, a)
Trying pickup(c)
Trying stack(c, b)
UNSTACKING stack(c, b)
UNSTACKING [handempty, on(c, b), clear(c), on(b, a), ontable(a)]
UNSTACKING pickup(c)
UNSTACKING [holding(c), on(b, a), clear(b), ontable(a)]
UNSTACKING stack(b, a)
UNSTACKING [handempty, on(b, a), clear(b), ontable(a), ontable(c), clear(c)]
Trying stack(b, c)
Trying pickup(a)
Trying stack(a, b)
moves are
unstack(a, b)
putdown(a)
pickup(b)
stack(b, c)
pickup(a)
stack(a, b)

Yes
*/