%%%%%%%%%%%%%%%%%%
%% BLOCKS WORLD %%
%%%%%%%%%%%%%%%%%%

% Change to true if you want more detailed logs
logging_enabled(false).

% ACTION DESCRIPTION SCHEMA
% strips_rule(action(args), preconds, add-list, delete-list)
    

% ACTION I 
strips_rule(stack(X,Y), % name and arguments
	[holding(X), clear(Y)], % list of conditions
	[on(X,Y), clear(X), handempty], % list of added facts
	[clear(Y), holding(X)]). % list of removed

%TODO: ACTION II 

%TODO: ACTION III 

%TODO: ACTION IV

%%%%%%%%%%%%%%%%%%%%%%%
%% EXAMPLES OF USAGE %%
%%%%%%%%%%%%%%%%%%%%%%%

%% EXAMPLE I
problem1 :-
  plan(
  [ontable(a),ontable(b),ontable(c),clear(a),clear(b),clear(c),handempty],
  [on(b,c), on(a,b)]).

%% EXAMPLE II
problem2 :-
  plan(
  [on(a,c),ontable(b),ontable(c),clear(a),clear(b),handempty],
  [on(b,c), on(a,b)]).

%% EXAMPLE III
problem3 :-
  plan(
    [on(c,a),ontable(a),ontable(b),clear(b),clear(c),handempty],
    [on(b,c), on(a,b)]).

%TODO: EXAMPLE IV
problem4 :- write('TODO').

%%%%%%%%%%%%%%%%%%%%
%% STRIPS PLANNER %%
%%%%%%%%%%%%%%%%%%%%

% Strips as a regression planner.
% Top-level interface.
strips(InitState, GoalList, Plan) :-
	strips1(InitState, GoalList, [], [], _, RevPlan),
	reverse(RevPlan, Plan).

% Base case: All goals satisfied in State.
strips1(State, GoalList, Plan, _, State, Plan) :-
 subset(GoalList, State).

% Plan
strips1(State, GoalList, Plan, BadActions, NewState, NewPlan) :-
    possible_goals_groundings(GoalList, State, GoalsGroundings),
    member(GroundedGoalList, GoalsGroundings),
    strips_log(''),
    strips_log('-- state: '),
    strips_log(State),
    strips_log('-- goals: '),
    strips_log(GroundedGoalList),
    strips_log('-- bad actions: '),
    strips_log(BadActions),
    member(Goal, GroundedGoalList),
	not(member(Goal, State)),    % Find unsatisfied goal.
	strips_log('Attempting goal:  '),
    strips_log(Goal),
    strips_log(' Choosing Action:  '), 
	adds(Ac, Goal),              % Find action that achieves it.
    strips_log(Ac),
	not(member(Ac, BadActions)), % Do not repeat actions (dangerous).
	strips_log(' -- not a bad action. --'),
	preclist(Ac, PrecList),      % Find preconds and achieve them.
    remember_action(Ac, BadActions, ExtBadActions),
	strips1(State, PrecList, Plan, ExtBadActions, TmpState1, TmpPlan1),
    apply_rule(Ac, TmpState1, TmpState2),  % Recurse w/new state and goals.
    sort(TmpState1, HistoryState),
    strips1(TmpState2,GroundedGoalList, [HistoryState-Ac|TmpPlan1],BadActions,NewState,NewPlan).
	
% Apply Rule
apply_rule(Ac, State, NewState) :-
    strips_log(''),
    strips_log('Simulating move: '),
	strips_log(Ac),
    strips_log(State),
	strips_log('\t====>'),
	dellist(Ac, DelList),                     % find deleted props
	subtract(State, DelList, TmpState),       % remove deleted props
	addlist(Ac, AddList),                     % find added props
	union(AddList, TmpState, NewState),       % add props to old state
	strips_log(NewState).

% Utilities
remember_action(Action, OldList, [ConstantAction|OldList]) :-
    remove_variables(Action, ConstantAction).
remove_variables(Action, ConstantAction) :-
    Action =.. ActionList,
    maplist(remove_variable, ActionList, ConstantActionList),
    ConstantAction =.. ConstantActionList.
remove_variable(Constant, Constant) :-
    \+ var(Constant).
remove_variable(Variable, Constant) :-
    var(Variable),
    gensym(strips_constant, Constant).     

possible_goals_groundings(Goals, State, PossibleGoals) :-
    findall(PG, ground_goals(Goals, State, PG), AllPossibleGoals),
    prune_goals(AllPossibleGoals, PossibleGoals).

prune_goals([OnlyOneGoal], [OnlyOneGoal]) :- !.
prune_goals([FirstGoal|Rest], [FirstGoal|RestMinusOne]) :-
    remove_last_element(Rest, RestMinusOne).

remove_last_element([_], []).
remove_last_element([H1|[HT|TT]],[H1|RT]) :-
    remove_last_element([HT|TT], RT).

ground_goals(Goals, State, GroundedGoals) :-
    copy_term(Goals, GroundedGoals),
    ground_goals(GroundedGoals, State).
ground_goals([], _).
ground_goals([H|T], State) :-
    (member(H, State) ; true),
    ground_goals(T, State).

prune_plan([],[]).
prune_plan([H-_|T], NT) :-
    plan_member(H,T), !,
    prune_plan(H,T,TT),
    prune_plan(TT,NT).
prune_plan([H|T],[H|NT]) :-
    prune_plan(T,NT).
prune_plan(H,[H-A|T],[H-A|T]).
prune_plan(H,[G-_|T],NT) :-
    H \= G,
    prune_plan(H,T,NT).
plan_member(H, [H-_|_]) :- !.
plan_member(H, [_|T]) :- plan_member(H,T).
    
preclist(Action, Plist) :- strips_rule(Action, Plist, _, _).
prec(Action, Cond) :- preclist(Action, Plist), member(Cond, Plist).
addlist(Action, Alist) :- strips_rule(Action, _, Alist, _).
adds(Action, Cond) :- addlist(Action, Alist), member(Cond, Alist).
dellist(Action, Dlist) :- strips_rule(Action, _, _, Dlist).
dels(Action, Cond) :- dellist(Action, Dlist), member(Cond, Dlist).

% Pretty-print Init, Goal, and Plan.
plan(InitState, Goal) :-
	strips(InitState,Goal,Plan),
	write('\n\nInit: '), write(InitState),nl,
	write('Goal: '),write(Goal),nl,
	write('Plan:\n'),
    prune_plan(Plan, PrunnedPlan),
    writeplan(PrunnedPlan),nl.

% Pretty-print the plan.
writeplan([]).
writeplan([_-A|B]):-
  write('       '),write(A),nl,
  writeplan(B).

strips_log(X) :-
    logging_enabled(true), !,
    write(X), nl.
strips_log(_).