?- op(500, xfx, '||'). % For parallel 
?- op(500, xfx, '|>'). % For triangle 
?- op(500, xfx, '}}' ). % For sequence
?- op(500, xfx, '::' ). % For wait

reduce(_, state(B, [])) :-
	format("No more goals to execute for the agent in this state~n", []),
	output_state(state(B,[])), nl.
reduce(N, state(B,G)) :- 
	G \= [],
	output_state(state(B,G)), 
	revise(N, state(B,G), RevisedState, Status),
	N1 is N+1, 
	proceed_or_halt(N1, RevisedState, Status).

proceed_or_halt(_, State, halt) :-
        format("Halt event detected~n", []), 
	output_state(State), nl.
proceed_or_halt(N, State, Status):-
	Status \= halt, 
	format("Checking goals for state changes ... ~n", []), 
	output_state(State), 
	reduce_goals(State, TempState1),
        % format("State is ", []), display(TempState1), nl, 
	% TempState1 = state(_, Goals1), format("Goals: ", []), output_goals(Goals1), 
	% format("Managing active goals~n", []), 
        plan_goals(TempState1, TempState2), 
        % format("State is ", []), display(TempState2), nl, 
	% TempState2 = state(_, Goals2), format("Goals: ", []), output_goals(Goals2), 
	% format("Executing~n", []), 
	% output_state(TempState2), 
	execute_state(TempState2, NewState, Result), nl, 
        reduce(N, NewState). 

reduce_goals(state(B,G), NewState) :-
	reduce_goal(B, [], G, state(NewB,NewG), Status), 
	reduce_or_halt(NewB, NewG, Status, NewState).

reduce_or_halt(B, G, nochange, state(B,G)) :-
	% format("No more goal changes~n", []). 
	true. 
reduce_or_halt(B, G, update, NewState) :-
	reduce_goals(state(B,G), NewState). 

reduce_goal(B, SoFar, Remainder, state(NewB,Result), update) :- 
        % format("Reduce goal state 1~n", []), ttyflush, 
	Remainder = [GoalContext|Rest],
        % format("Reduce goal state 1~n", []),  ttyflush, 
	GoalContext = g(Name, Goal, Rules, Status, Plan),
        % format("Calling goal_action ...~n", []),  ttyflush, 
	goal_type(Goal, GoalType), 
	goal_action(Name, GoalType, Status, B, NewB, Rules, Action),
	member(Action, [drop,abort]), % rule 7
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, Status, dropped]), 
	append(SoFar, Rest, Result).

reduce_goal(B, SoFar, Remainder, state(NewB, Result), update) :- 
        % format("Reduce goal state 2~n", []), ttyflush,  
	Remainder = [GoalContext|Rest],
	GoalContext = g(Name, Goal, Rules, pending, Plan),
        goal_type(Goal, GoalType), member(GoalType, [perform, achieve]), 
        % format("Reduce goal state 2~n", []), ttyflush,  
	goal_action(Name, GoalType, pending, B, NewB, Rules, Action),
	% format("Action is activate~n", []), ttyflush, 
	member(Action, [activate]),
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, pending, active]), ttyflush, 
	NewGoalContext = g(Name, Goal, Rules, active, Plan),  % rule 1
	append(SoFar, [NewGoalContext|Rest], Result). 

reduce_goal(B, SoFar, Remainder, state(NewB, Result), update) :- 
        % format("Reduce goal state 3~n", []),  ttyflush, 
	Remainder = [GoalContext|Rest],
        % format("Reduce goal state 3~n", []),  ttyflush, 
	% format("Goal is ", []), output_single_goal(GoalContext), nl, 
        % format("Checking goal context~n", []),  ttyflush, 
	GoalContext = g(Name, Goal, Rules, pending, Plan),
        % format("Reduce goal state 3~n", []),  ttyflush, 
        % format("Checking goal type~n", []),  ttyflush, 
        goal_type(Goal, GoalType), member(GoalType, [maintain]), 
	goal_action(Name, GoalType, pending, B, NewB, Rules, Action),
        % format("Activating maintain goal~n", []),  ttyflush, 
	member(Action, [activate]),
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, pending, monitoring]), 
	NewGoalContext = g(Name, Goal, Rules, monitoring, Plan), % rule ?
	append(SoFar, [NewGoalContext|Rest], Result). 

reduce_goal(B, SoFar, Remainder, state(NewB, Result), update) :- 
	Remainder = [GoalContext|Rest],
        % format("Reduce goal state 4~n", []), 
	GoalContext = g(Name, Goal, Rules, monitoring, Plan),
        goal_type(Goal, GoalType), member(GoalType, [maintain]), 
        % format("Reduce goal state 4~n", []), 
	goal_action(Name, GoalType, monitoring, B, NewB, Rules, Action),
	member(Action, [respond]),
        % format("Responding~n", []), 
	Goal = maintain(MC, Predict, Recover, Prevent, Succ, Fail), 
        choose_goal(MC, Recover, Prevent, Chosen), 
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, monitoring, active]), 
%%% New stuff is below ... 
        format("Chosen goal is ", []), display(Chosen),nl, 
	NewGoalContext = g(Name, Goal, Rules, active, subgoal(Chosen)),
	append(SoFar, [NewGoalContext|Rest], Result).

reduce_goal(B, SoFar, Remainder, state(NewB, Result), update) :- 
	Remainder = [GoalContext|Rest],
	% format("Reduce goal state 5~n", []), 
	GoalContext = g(Name, Goal, Rules, active, Plan),
        goal_type(Goal, GoalType), member(GoalType, [maintain]), 
	goal_action(Name, GoalType, active, B, NewB, Rules, Action),
	member(Action, [reactivate]),
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, active, monitoring]), 
	NewGoalContext = g(Name, Goal, Rules, monitoring, empty),
	append(SoFar, [NewGoalContext|Rest], Result). 

reduce_goal(B, SoFar, Remainder, state(NewB, Result), update) :- 
        Remainder = [GoalContext|Rest],
	% format("Reduce goal state 6~n", []), 
	GoalContext = g(Name, Goal, Rules, suspended, Plan),
        goal_type(Goal, GoalType), member(GoalType, [maintain]), 
	goal_action(Name, GoalType, suspended, B, NewB, Rules, Action),
	member(Action, [reactivate]),
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, suspended, monitoring]), 
	NewGoalContext = g(Name, Goal, Rules, monitoring, Plan), % rule ?
	append(SoFar, [NewGoalContext|Rest], Result). 

reduce_goal(B, SoFar, Remainder, state(NewB, Result), update) :- 
        Remainder = [GoalContext|Rest],
        % format("Reduce goal state 7~n", []), 
	GoalContext = g(Name, Goal, Rules, suspended, Plan),
        goal_type(Goal, GoalType), member(GoalType, [perform, achieve]), 
	goal_action(Name, GoalType, suspended, B, NewB, Rules, Action),
	member(Action, [reactivate]),
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, suspended, active]), 
	NewGoalContext = g(Name, Goal, Rules, active, Plan), % rule ?
	append(SoFar, [NewGoalContext|Rest], Result). 

reduce_goal(B, SoFar, Remainder, state(NewB, Result), update) :- 
	Remainder = [GoalContext|Rest],
        % format("Reduce goal state 8~n", []), 
	GoalContext = g(Name, Goal, Rules, suspended, Plan),
        goal_type(Goal, GoalType), member(GoalType, [perform, achieve]), 
	goal_action(Name, GoalType, suspended, B, NewB, Rules, Action),
	member(Action, [reconsider]),
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, suspended, pending]), 
	NewGoalContext = g(Name, Goal, Rules, pending, Plan), % rule ?
	append(SoFar, [NewGoalContext|Rest], Result). 

reduce_goal(B, SoFar, Remainder, state(NewB, Result), update) :- 
	Remainder = [GoalContext|Rest],
        % format("Reduce goal state 9~n", []), 
	GoalContext = g(Name, Goal, Rules, Status, Plan), member(Status, [pending,active]), 
        goal_type(Goal, GoalType), member(GoalType, [perform, achieve]), 
	goal_action(Name, GoalType, Status, B, NewB, Rules, Action),
	member(Action, [suspend]),
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, Status, suspended]), 
        % rec_rules(Name, Status, Rules, Rules2), 
	NewGoalContext = g(Name, Goal, Rules, suspended, Plan), % rule ?
	append(SoFar, [NewGoalContext|Rest], Result). 

reduce_goal(B, SoFar, Remainder, NewState, Status) :- 
	Remainder = [GoalContext|Rest],
        % format("Reduce goal state 10~n", []), 
	GoalContext = g(Name, Goal, Rules, State, _Plan),
	goal_type(Goal, GoalType), 
	% format("Goal is ", []), output_single_goal(GoalContext), nl, 
	goal_action(Name, GoalType, State, B, NewB, Rules, Action), 
	member(Action, [no_action]),
        % format("~k stable, ", [Name]), 
	append(SoFar, [GoalContext], NewSoFar), 
	% format("~nCalling reduce_goal with ~nSoFar:", []), display(NewSoFar), nl, format("Rest: ", []), display(Rest), nl, 
	reduce_goal(NewB, NewSoFar, Rest, NewState, Status).

reduce_goal(B, SoFar, [], State, nochange) :-
	format("All goals stable~n", []),
	State = state(B,SoFar). 

%% Need to add rules for stopping plans executing when there is a goal change. (8a,8b,9,10). 

choose_goal(MC, Recover, Prevent, Prevent) :-	implies(B, MC).
choose_goal(MC, Recover, Prevent, Recover) :-	\+ implies(B, MC).

goal_action(Name, GoalType, Status, B, NewB, Rules, Action) :-
	member(act(Cond,Action), Rules),
	member(C, Cond), convert(C, Goal), 
	implies(B, Goal),
	applicable_action(GoalType, Status, Action),
        % format("Applying action ~k to ~k because of ", [Action,Name]), display(Goal), nl, 
        update_action_beliefs(Name, GoalType, B, Action, NewB).         

goal_action(Name, GoalType, Status, B, B, Rules, no_action) :-
 	\+ (member(act(Cond,Action), Rules), member(C, Cond), implies(B, C), applicable_action(GoalType, Status, Action)). 

convert(C, C) :- C = [_|_]. % Already a list 
convert(C, [C]) :- C \= [_|_]. % Already a list 

% applicable(GoalType, Status, Action)
applicable_action(_, _, drop). 
applicable_action(_, _, abort). 
applicable_action(_, pending, activate). 
applicable_action(_, pending, suspend). 
applicable_action(_, active, suspend). 
applicable_action(maintain, monitoring, suspend). 
applicable_action(maintain, monitoring, respond). 
applicable_action(maintain, suspended, rerespond). 
applicable_action(maintain, active, reactivate). 
applicable_action(_, suspended, reactivate). 
applicable_action(_, suspended, reconsider). 
% See four-state diagram

update_action_beliefs(Name, GoalType, B, Action, NewB) :- 
	update_action_beliefs1(Name, B, Action, NB1), % deal with events
	update_goal_beliefs(Name, GoalType, NB1, Action, NewB), % keep track of goal state changes,
	true. 

update_action_beliefs1(Name, B, Action, NewB) :-
       member(Action, [drop, abort, suspend, activate, reactivate, reconsider]),
       Bel =.. [Action,Name], 
       % format("Deleting ~k(~k)~n", [Action,Name]), 
       update_beliefs(B, [del(Bel)], NewB).

update_action_beliefs1(Name, B, Action, B) :-
       \+ member(Action, [drop, abort, suspend, activate, reactivate, reconsider]).

% Bit of a hack ... could be done more simply ... 
update_goal_beliefs(Name, achieve, B, activate, NewB) :-
	update_beliefs(B, [add(active(Name))], NewB).
update_goal_beliefs(Name, achieve, B, suspend, NewB) :-
	update_beliefs(B, [del(active(Name)), add(suspended(Name))], NewB).
update_goal_beliefs(Name, achieve, B, Action, NewB) :-
	member(Action, [drop, abort]), 
	update_beliefs(B, [del(active(Name)), del(suspended(Name))], NewB).
update_goal_beliefs(Name, achieve, B, reactivate, NewB) :-
	update_beliefs(B, [del(suspended(Name)), add(active(Name))], NewB).
update_goal_beliefs(Name, achieve, B, reconsider, NewB) :-
	update_beliefs(B, [del(suspended(Name))], NewB).
update_goal_beliefs(Name, achieve, B, Action, B) :-
	\+ member(Action, [drop, abort, suspend, activate, reactivate, reconsider]). 

update_goal_beliefs(Name, maintain, B, respond, NewB) :-
	update_beliefs(B, [add(active(Name))], NewB). 
update_goal_beliefs(Name, maintain, B, suspend, NewB) :-
	update_beliefs(B, [add(suspended(Name)), del(active(Name))], NewB). 
update_goal_beliefs(Name, maintain, B, reactivate, NewB) :-
	update_beliefs(B, [del(active(Name)), del(suspended(Name))], NewB). 
update_goal_beliefs(Name, maintain, B, reconsider, NewB) :-
	update_beliefs(B, [del(suspended(Name))], NewB). 
update_goal_beliefs(Name, maintain, B, Action, NewB) :-
	member(Action, [drop, abort]), 
	update_beliefs(B, [del(active(Name)), del(suspended(Name))], NewB).
update_goal_beliefs(Name, maintain, B, Action, B) :-
	\+ member(Action, [drop, abort, suspend, respond, reactivate, reconsider]). 

% Find more plans, when needed, or drop goals for which no (more) plans can be found
plan_goals(state(B,G), state(NewB, NewG)) :-
        plan_goals1(B, [], G, NewB, NewG).

plan_goals1(B, SoFar, [], B, SoFar). 
plan_goals1(B, SoFar, Rest, NewB, NewG) :-
        Rest = [GoalContext|Others],
        GoalContext = g(Name, Goal, Rules, Status, Plan), Status \= active, % Do nothing to goals not in the active state. 
        append(SoFar, [GoalContext], NewSoFar),
        plan_goals1(B, NewSoFar, Others, NewB, NewG).

plan_goals1(B, SoFar, Rest, NewB, NewG) :-
        Rest = [GoalContext|Others],
        GoalContext = g(Name, Goal, Rules, active, Plan), Plan \= empty, % Do nothing to goals which already have a plan 
        append(SoFar, [GoalContext], NewSoFar),
        plan_goals1(B, NewSoFar, Others, NewB, NewG).

%% Need to rethink this case. Why is it here exactly?
plan_goals1(B, SoFar, Rest, NewB, NewG) :-
        Rest = [GoalContext|Others],
        GoalContext = g(Name, Goal, Rules, active, empty), 
	goal_type(Goal, GoalType), member(GoalType, [maintain]), % Do nothing to active maintenance goals
        append(SoFar, [GoalContext], NewSoFar),
        plan_goals1(B, NewSoFar, Others, NewB, NewG).

plan_goals1(B, SoFar, Rest, NewB, NewG) :-
        Rest = [GoalContext|Others],
        GoalContext = g(Name, Goal, Rules, active, empty), % Perform or achieve goal active without a plan, so if the status is not about to change, find a new plan. 
	goal_type(Goal, GoalType), member(GoalType, [perform,achieve]),
        remains_active(GoalType, active, B, Rules),
        find_plan(Name, Goal, B, Plan), % May need more input than this
        Plan \= empty,
        NewGoalContext = g(Name, Goal, Rules, active, Plan), 
        append(SoFar, [NewGoalContext], NewSoFar),
        plan_goals1(B, NewSoFar, Others, NewB, NewG).
      
plan_goals1(B, SoFar, Rest, NewB, NewG) :-
        Rest = [GoalContext|Others],
        GoalContext = g(Name, Goal, Rules, active, empty), % Active without a plan, so if the status is not about to change, find a new plan. 
	goal_type(Goal, GoalType), 
        remains_active(GoalType, active, B, Rules),
        find_plan(Name, Goal, B, Plan), % May need more input than this
        Plan = empty,            % Cannot find a plan, so bugger it!
        plan_goals1(B, SoFar, Others, NewB, NewG).
      
remains_active(GoalType, Status, B, Rules) :-
        findall(Action, goal_action(_, GoalType, Status, B,_, Rules,Action), List),
        subset(List, [no_action, activate]).

output_state(state(B,G)) :-
	format("Beliefs: ", []), output_beliefs(B),
	format("Goals: ", []), output_goals(G).

output_goals([]) :- nl. 
output_goals([Goal|Goals]) :-
	output_single_goal(Goal),
	output_goals(Goals).

output_single_goal(Goal) :-
	Goal = g(Name, _GC, Rules, Status, Plan),
	format("~k: ~k ", [Name, Status]),
        output_plan(Plan),
        format(" || ", []). 

output_beliefs([]) :- nl. 
output_beliefs([Goal|Goals]) :-
	output_single_belief(Goal),
	output_beliefs(Goals).

output_single_belief(Belief) :-
	format("~k ", [Belief]).

output_plan(Plan) :-
	Plan = subgoal(Name),
	write(g(Name)).
output_plan(Plan) :-
	Plan \= subgoal(_), 
	write(Plan). 

subset([], _).
subset([Element|Rest], List) :-
       member(Element, List),
       subset(Rest, List).

execute_state(state(B,[]), state(B,[]), finished).
execute_state(state(B,G), NewState, Status) :-
       G \= [], 
       % update_waits([], state(B,G), NewState1), %% This allows 'eager' updating of waits. To be lazy, just omit this step. 
       NewState1 = state(B,G), % omit wait processing for now ... 
       execute1([], NewState1, NewState, Status).

%%% Need to think how to incorporate wait features. 
execute1(SoFar, state(B,[]), state(B, SoFar), finished). 
execute1(SoFar, state(B,Goals), NewState, Search_Status) :-
       % format("Execute 1~n", []), ttyflush, 
       Goals = [GoalContext|Others], 
       % Rest = [GoalContext|Others], 
       GoalContext = g(Name, Goal, Rules, Status, Plan), 
       % format("Goal is ~k~n", [Name]), ttyflush, 
       Status \= active, % Not active, so do not execute it. 
       % output_goals(Rest), 
       format("~k: not executing ", [Name]), ttyflush, 
       append(SoFar, [GoalContext], NewSoFar),
       execute1(NewSoFar, state(B,Others), NewState, Search_Status).
execute1(SoFar, state(B,Goals), state(NewB, NewGoals), continue) :-
       % format("Execute 2~n", []), ttyflush, 
       Goals = [GoalContext|Others], 
       GoalContext = g(Name, Goal, Rules, active, Plan), 
       % format("Goal is ~k~n", [Name]), ttyflush, 
       Plan \= empty, 
       % format("Found goal to execute~n", []), ttyflush, 
       % format("Checking maintenance activity ~n", []), 	
       % append(SoFar, G, Goals), % inactive_maintenance(B,Goals), % Active, and no maintenance goal activity, so execute plan. %%% Why? Don't need this? 
       format("~k executing ", [Name]), write(Plan), ttyflush, 
       execute_step(B, NewB, Goals, NGoals), 
       append(SoFar, NGoals, NewGoals).

% nextplan(success, Plan, Plan).
% nextplan(failed, _, empty). 

update_waits(SoFar, state(B,[]), state(B, SoFar)). 
update_waits(SoFar, state(B,G), NewState) :-
       G = [GoalContext|Others], 
       GoalContext = g(Name, Goal, Rules, Status, Plan), Status \= active, % Not active, so do not execute it. 
       format("~k: not active ", [Name]), 
       append(SoFar, [GoalContext], NewSoFar),
       update_waits(NewSoFar, state(B,Others), NewState).
update_waits(SoFar, state(B,G), state(NewB, NewGoals)) :-
       G = [GoalContext|Others], 
       GoalContext = g(Name, Goal, Rules, active, Plan), update_plan(B, Plan, Plan, no_change), % Plan is not wait, or it is but condition is not true. Leave it alone. 
       format("~k: no change ", [Name]), 
       append(SoFar, [GoalContext], NewSoFar), 
       update_waits(NewSoFar, stat(B,Others), NewGoals).
update_waits(SoFar, state(B,G), state(NewB, NewGoals)) :-
       G = [GoalContext|Others], 
       GoalContext = g(Name, Goal, Rules, active, Plan), update_plan(B, Plan, NewPlan, change), % Plan is wait, and condition is true, so update it. 
       format("~k: changing ", [Name]), 
       NewGoalContext = g(Name, Goal, Rules, active, NewPlan), 
       append(SoFar, [NewGoalContext], NewSoFar), 
       update_waits(NewSoFar, stat(B,Others), NewGoals).

%% Might have to traverse entire plan to find 'wait frontier' due to }}, || and |> ... 	
update_plan(B, Plan, Plan, no_change) :- Plan \= Guard :: Body. 
update_plan(B, Plan, Plan, no_change) :- 
	Plan == Guard :: Body,
	\+ implies(B, Guard).
update_plan(B, Plan, NewPlan, change) :- 
	Plan == Guard :: Body, 
	implies(B, Guard),
	NewPlan = Body. 

inactive_maintenance(B, Goals) :-
      \+ ( member(GoalContext, Goals), 
           GoalContext = g(Name, Goal, Rules, Status, Plan),
           goal_type(Goal, GoalType), member(GoalType, [maintain]), 
           goal_action(_, GoalType, Status, B, NewB, Rules, activate)
         ). 

goal_type(Goal, GoalType) :- Goal =.. [GoalType|Args].


% Trivial plan
execute_step(B, B, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        (Plan = [nil]; Plan = []),!, 
        NewGoalContext = g(Name, Goal, Rules, active, []),
	NewGoals = [NewGoalContext|Others]. 

% Ask and succeed
execute_step(B, B, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        Plan = [ask(Phi)|NewPlan], 
	format("Ask plan 1~n", []), 
        implies(B, [Phi]), !, 
        NewGoalContext = g(Name, Goal, Rules, active, NewPlan),
	NewGoals = [NewGoalContext|Others]. 

% Ask but fail
execute_step(B, B, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        Plan = [ask(Phi)|NewPlan],
	format("Ask plan 2~n", []),  
        \+ implies(B, [Phi]), !, 
        NewGoalContext = g(Name, Goal, Rules, active, empty),
	NewGoals = [NewGoalContext|Others]. 

% Add belief
execute_step(B, NewB, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        Plan = [add(A)|NewPlan], !, 
        append([A], B, NewB), 
        NewGoalContext = g(Name, Goal, Rules, active, NewPlan),
	NewGoals = [NewGoalContext|Others]. 

% Delete belief, but unnecessary
execute_step(B, B, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        Plan = [del(A)|NewPlan], 
        \+ implies(B, [A]), !, 
        NewGoalContext = g(Name, Goal, Rules, active, NewPlan),
	NewGoals = [NewGoalContext|Others].  

% Real delete belief
execute_step(B, NewB, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        Plan = [del(A)|NewPlan], 
        implies(B, [A]), !, 
        delete(B, A, NewB), 
        NewGoalContext = g(Name, Goal, Rules, active, NewPlan),
	NewGoals = [NewGoalContext|Others]. 

% Perform action and succeeds
execute_step(B, NewB, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
	Plan = [A|NewPlan], 
	% format("Trying action ~k~n", [A]), 
	action(A, PreCond, Test, Calculate, List),
	% format("Doing action ~k~n", [A]), 
	implies(B, PreCond), !, 
	% format("Precondition done~n", []), 
	implies(B, Test),
	Calculate, 
	% format("Test and calculation done~n", []), 
	% format("List is ", []), write(List), nl, 
	update_beliefs(B, List, NewB),
	% format("Updated beliefs: ", []), output_beliefs(NewB), 
	% format("Beliefs updated~n", []), 
        NewGoalContext = g(Name, Goal, Rules, active, NewPlan),
	NewGoals = [NewGoalContext|Others]. 

% Perform action and fails
execute_step(B, B, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
	Plan = [A|_NewPlan], 
	% format("Trying action ~k~n", [A]), 
	action(A, PreCond, Test, Calculate, List),
	format("Doing action ~k~n", [A]), 
	\+ implies(B, PreCond), !, 
	format("Precondition failed~n", []), 
        NewGoalContext = g(Name, Goal, Rules, active, empty),
	NewGoals = [NewGoalContext|Others]. 

% Wait with condition succeeding (so like action)
execute_step(B, B, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
	Plan = [(Guard :: Body)|RestPlan], 
	implies(B, [Guard]),
	append(Body, RestPlan, NewPlan), 
	NewGoalContext = g(Name, Goal, Rules, active, NewPlan),
	NewGoals = [NewGoalContext|Others]. 

% Wait with condition failing
execute_step(B, B, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
	Plan = [(Guard :: Body)|_], 
	\+ implies(B, [Guard]),
	append(Others, [GoalContext], NewGoals).  %% add to the end to ensure fairness ... 

% Sequence
execute_step(B, NewB, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
	Plan = [P1|NewP], 
	\+ action(P1, _PreCond, _Test, _Calculate, _List),

        execute_step(B, NewB, Goals, NGoals), 
	member(g(Name, Goal, Rules, active, NewPlan1), NGoals), 
	delete(NGoals, g(Name, Goal, Rules, active, NewPlan1), NGoals1), 
        append(NewPlan1, NewP, NewPlan),

        NewGoalContext = g(Name, Goal, Rules, active, NewPlan),
	NewGoals = [NewGoalContext|NGoals1]. 

% Subgoal
execute_step(B, B, Goals, NewGoals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
	Plan = subgoal(SubName), !, 
	% format("Trying subgoal ... ~n", []), 
	goal(g(SubName, SubGoal, SubRules)), 
	% format("Executing subgoal 1~n", []), 
        success_and_failure_conditions(SubGoal, Success, Failure), 
        %% Check correct action from paper ..
	% format("Executing subgoal 2~n", []), 
	% SubGoalGuard = (Success; Failure; drop(SubName); abort(SubName)), 
	% Hack to make examples work easily!!
	SubGoalGuard = mctest, 
	% format("Executing subgoal 3~n", []), 
	SubGoalPlan = [SubGoalGuard :: [ask(Success)] ], 
	% format("Executing subgoal 4~n", []), 
	NewGoalContext = g(Name, Goal, Rules, active, SubGoalPlan), 
	% format("Executing subgoal 5~n", []), 
	SubGoalContext = g(SubName, SubGoal, SubRules, pending, empty), 
	% format("New Goal Plan is ", []), write(SubGoalPlan), nl, 
	% 
format("SubGoalContext is ", []), write(SubGoalContext), nl, 
	append([NewGoalContext, SubGoalContext], Others, NewGoals). 

execute_step(B, B, Goals, Goals) :-
	Goals = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
	format("Unrecognised Plan:  ~k~n", [Plan]), 
	true. 

success_and_failure_conditions(SubGoal, Success, Failure) :-
	SubGoal = achieve(_, Success, Failure).
success_and_failure_conditions(SubGoal, Success, Failure) :-
	SubGoal = maintain(_, _, _, _, Success, Failure).

% next(B, [ask(Phi)|Rest], B, Rest) :-	implies(B, [Phi]).
% next(B, [add(A)|Rest], NB, Rest) :-	append(B, [A], NB).
% next(B, [del(A)|Rest], B, Rest) :-	\+ implies(B, [A]). 
% next(B, [del(A)|Rest], NB, Rest) :-	implies(B, [A]), delete(B, A, NB). %% Should be more general, but ... 
	
% Reduce_plan(state([b,d], [g,h], [goal(c, (add(e) '||' del(d)) '}}' add(f), x)]), S).


% P is a list of guarded plans
% A rule E: Ph <- Pl is represented as rule(E, Ph, Pl). 
% See also Event rule below. 
gather(E, plans(P)) :-
	findall(p(Ph,Pl), rule(E, Ph, Pl), P).

% rule(fred, b, c).
% rule(dip, d, e).
% rule(dip, f, g).
	
% next(state(Belief, Goal, Plan), state(NewBelief, NewGoal, NewPlan)). 
% The second state is one step on from the first. This is defined for "simple" cases only ... 
% Belief, Goal and Plan are lists of formulae. 

% nil rules
next(B, [nil|P], B, P).
next(B, [nil '|>' P|Rest], B, Rest).
next(B, [nil '}}' P2|Rest], B, New) :- append([P2], Rest, New).
next(B, [nil '||' P2|Rest], B, New) :- append([P2], Rest, New).
next(B, [P1 '||' nil|Rest], B, New) :- P1 \== nil, append([P1], Rest, New). 
% Event rule
next(B, [ev(E)|Rest], B, [plans(P)|Rest]) :-	gather(E, plans(P)). 
% ? rule 
next(B, [ask(Phi)|Rest], B, Rest) :-	implies(B, [Phi]).

% Belief change rules
next(B, [add(A)|Rest], NB, Rest) :-	append(B, [A], NB).
next(B, [del(A)|Rest], B, Rest) :-	\+ implies(B, [A]). 
next(B, [del(A)|Rest], NB, Rest) :-	implies(B, [A]), delete(B, A, NB). %% Should be more general, but ... 

% Select rule
next(B, [plans(P)|Rest], B, [Act '|>' plans(NP)|Rest]) :-
	write(P), nl, 
	member(p(Pre,Act), P), implies(B, [Pre]),
	delete(P, p(Pre,Act), NP).

% Parallel ...
next(B, [P1 '||' P2|Rest], B, NewP) :-
	P1 \== nil,  P2 \== nil, append([P1,P2], Rest, NewP). 
	
% rules when plan step is goal. 
%% Currently not implemented. 
% next(B, [goal(S,P,F)|Rest]), B, Rest) :-  implies(B, [S]).
% 
% next(B, [goal(S,P,F)|Rest]), B, [fail|Rest])) :- 
% 	implies(B, [F]). %% check this case 
% 
% next(B, [goal(S,P,F)|Rest]), B, NG, [goal(S, P '|>' P, F)|Rest]) :- 
% 	P \= (_ '|>' _),	\+ implies(B, [S]), \+ implies(B, [F]), 
% 	append(G, [g(S,F)], NG).

% action case
next(B, [A|Rest], NB, Rest) :- 
	format("Trying action ~k~n", [A]), 
	action(A, PreCond, Goal, Calculate, List),
	format("Doing action ~k~n", [A]), 
	implies(B, PreCond),
	format("Precondition done~n", []), 
	implies(B, Goal),
	Calculate, 
	format("Goal and calculation done~n", []), 
	update_beliefs(B, List, NB),
	format("Updated beliefs: ", []), output_beliefs(NB), 
	% format("Beliefs updated~n", []), 
	true. 

update_beliefs(B, [], B).
update_beliefs(B, [del(B1)|Rest], NewB) :-
	delete_belief(B, B1, NB),
	update_beliefs(NB, Rest, NewB). 
update_beliefs(B, [add(B1)|Rest], NewB) :-
	append([B1], B, NB), 
	update_beliefs(NB, Rest, NewB). 

delete_belief(B, B1, NB) :-
% 	implies(B, [B1]),
	delete(B, B1, NB).

delete_belief(B, B1, B) :-
	\+ implies(B, [B1]).

add_beliefs(B, Add, NewB) :-
	append(B, Add, NewB). 


revise(N, state(B,G), state(NewB, NewG), Status) :-
        format("-----------------------------------------------------~n", []), 
        format("Event (~d)? ([]. for no change, [halt]. to halt): ", [N]), ttyflush, 
       	read(Term), process(Term, List), 
        process_events(List, state(B,G), state(NewB, NewG), Status).

%% Include a 'finish' event here?
process_events([], State, State, continue).
process_events([ev(halt)|_Rest], state(B,G), state(B, G), halt).
process_events([ev(Event)|Rest], state(B,G), state(NewB, NewG), Status) :-
	Event \= halt, 
        process_event(Event, state(B,G), state(NB,NG)),
        process_events(Rest, state(NB,NG), state(NewB, NewG), Status).

process_event(Event, state(B,G), state(NewB,NewG)) :-
        event(Event, Belief_Updates, Goal_Additions),
        update_beliefs(B, Belief_Updates, NewB),
        append(Goal_Additions, G, NewG).

prioritise(P,P). % To allow for altering order in which these are done. 

process([],[]).  % To allow for more sophisticated processing. 
process([Event|Rest],[ev(Event)|Others]) :- 
	process(Rest, Others). 

% Converts list of concurrent plan steps into a single plan
planify([], nil).
planify([P], P) :- length([P], 1).
planify([P1|Rest], Plan) :- length([P1|Rest], N), N > 1, planify(Rest,P2), Plan = P1 '||' P2.

% goal1 :- reduce(state([b,d], [g,h], [(add(c) '||' del(d)) '}}' add(e)]), S).
% goal2 :- reduce(state([b,d], [g,h], [goal(b, add(c), f)]), S).
% goal3 :- reduce(state([b,d], [g,h], [goal(z, add(c), d)]), S).
% goal4 :- reduce(state([b,d], [g,h], [goal(z, add(c), x)]), S).



%% example specific stuff for Curiosity: 

% Other goals
% Name, goal, rules only are needed. Status will commence in pending with the empty plan. 
goal(g(exp,achieve(dawn, experiments_done,out_of_time), Rules)) :-  rules(exp, Rules). 
goal(g(sciopp,achieve(rock_seen, rock_analysed, analysis_failure), Rules)) :-   rules(sciopp, Rules). 
goal(g(battery,maintain(true, charge, charge, low_battery,  false, false), Rules)) :-   rules(battery, Rules). 
goal(g(return,achieve(nearly_dusk, at_base, movement_failure), Rules)) :-   rules(return, Rules). 
goal(g(charge,achieve(true, recharged,false), Rules)) :-   rules(charge, Rules). 
goal(g(nighttime,achieve(calm_dusk, dawn, securing_failure), Rules)) :-   rules(nighttime, Rules). 

% need beliefs for 
% out_of_time to reduce to dusk, \+ experiments_done
% battery_low to reduce to battery_level(L), L =< 10. 
% recharged to reduce to battery_level(L), L >= 100. 
% predict_violation (to magic!)
% calm_dusk to reduce to nearly_dusk, calm_weather
%% standard is [act([Succ, drop(I)], drop), act([abort(I)], abort), act([(Cond,activate(I))], activate), act([reactivate(I)],reactivate), act([reconsider(I)], reconsider)

rules(exp, [
act([experiments_done, out_of_time, drop(exp)], drop), 
act([abort(exp), active(return)], abort), 
act([[dawn, activate(exp)]], activate), 
act([suspend(exp), active(nighttime), active(sciopp), active(battery)], suspend), 
act([reactivate(exp)], reactivate), 
act([reconsider(exp)], reconsider) 
]).

rules(sciopp, [
act([rock_analysed, drop(sciopp)], drop), 
act([abort(sciopp)], abort), 
act([[rock_seen, activate(sciopp)]], activate),
act([suspend(sciopp), [active(sciopp), active(nighttime)], [active(sciopp),active(battery)]], suspend), 
act([reactivate(sciopp)], reactivate), 
act([reconsider(sciopp)], reconsider) 
]).
rules(battery, [
act([drop(battery)],drop), 
act([abort(battery)],abort), 
act([activate(battery)], activate), 
act([suspend(battery)],suspend),  
act([[current_charge(X), X =< 10], predict_violation], respond),  
act([reactivate(battery)], reactivate), 
act([reconsider(battery)], reconsider), 
act([rerespond(battery)], rerespond) 
]).
rules(return, [
act([at_base, drop(return)],drop), 
act([abort(return)],abort),
act([[normal_dusk, activate(return)]], activate), 
act([suspend(return),[active(sciopp),active(return)],[active(battery),active(return)]],suspend),  
act([reactivate(return)], reactivate), 
act([reconsider(return)], reconsider)
 ]).
rules(charge, [
act([recharged, drop(charge)],drop), 
act([abort(charge)],abort), 
act([activate(charge)],activate), 
act([suspend(charge)],suspend), 
act([reactivate(charge)], reactivate), 
act([reconsider(charge)], reconsider)
]).
rules(nighttime, [
act([done,drop(nighttime)],drop), 
act([abort(nighttime)],abort),
act([[calm_dusk, activate(nighttime)]], activate), 
act([suspend(nighttime),[active(nighttime),active(battery)]],suspend),  
act([reactivate(nighttime)], reactivate), 
act([reconsider(nighttime)], reconsider)
 ]).

initial_state(state(B,G)) :- initial_beliefs(B), initial_goals(G). 

initial_goals([Exp, SciOpp, Battery, Return, Nighttime]) :- 
	goal(g(exp, ExpGoal, ExpRules)), Exp = g(exp, ExpGoal, ExpRules, pending, empty),
	goal(g(sciopp, SciGoal, SciRules)), SciOpp = g(sciopp, SciGoal, SciRules, pending, empty),
	goal(g(battery, BatGoal, BatRules)), Battery = g(battery, BatGoal, BatRules, pending, empty),
	goal(g(return, RetGoal, RetRules)), Return = g(return, RetGoal, RetRules, pending, empty),
	goal(g(nighttime, NightGoal, NightRules)), Nighttime = g(nighttime, NightGoal, NightRules, pending, empty),
	true. 

initial_beliefs([dawn,calm_weather,activate(exp),activate(sciopp),activate(battery),activate(return),activate(nighttime),current_charge(100)]).

rec_rules(search, pending, R, R1) :- append([act([reconsider(search)],reconsider)], R, R1). 
rec_rules(search, active, R, R2) :- append([act([reconsider(search)],reconsider), act([reactivate(search)],reactivate)], R, R2). 
% rec_rules(save, pending, R, R1) :-
% rec_rules(save, active, R, R1) :-
% rec_rules(help, pending, R, R1) :-
% rec_rules(help, active, R, R1) :-
% rec_rules(charge, pending, R, R1) :-
% rec_rules(charge, active, R, R1) :-

find_plan(exp, _, _, [p1,p2,p3,p4]).
find_plan(sciopp, _, _, [s1,s2]).
find_plan(return, _, _, [return]).
find_plan(charge, _, _, [charge]).
find_plan(nighttime, _, _, [n1,n2,nearly_dawn::[n3]]).

% need beliefs for 
% out_of_time to reduce to dusk, \+ experiments_done
% battery_low to reduce to battery_level(L), L =< 10. 
% recharged to reduce to battery_level(L), L >= 100. 
% predict_violation (to magic!)
% calm_dusk to reduce to nearly_dusk, calm_weather
% normal_dusk to reduce to nearly_dusk, \+ calm_weather

implies(B, []). 
implies(B, [F|Rest]) :-
	implies_single(B, F),
	implies(B, Rest).

% implies_single(B, G) :- G \= mc1, G \= mctest, G \= recharged, member(G, B). 
implies_single(B, G) :- \+ member(G, [mc1, mctest, recharged, battery_low, out_of_time, calm_dusk, normal_dusk]), member(G, B). 
implies_single(B, G) :- \+ member(G, [mc1, mctest, recharged, battery_low, out_of_time, calm_dusk, normal_dusk]), G. % For built-ins.
implies_single(B, G) :- \+ member(G, [mc1, mctest, recharged, battery_low, out_of_time, calm_dusk, normal_dusk]), member(rule(Head, Body), B), Head = G, implies(B, Body).
implies_single(B,mc1) :- implies(B, [current_charge(X)]), X > 10.      	
implies_single(B,mctest) :- implies(B, [recharged]). 
implies_single(B,mctest) :- implies(B, [drop(charge)]). 
implies_single(B,mctest) :- implies(B, [abort(charge)]). 
implies_single(B,recharged) :- implies(B, [current_charge(X)]), X >= 100. 
implies_single(B,battery_low) :- implies(B, [current_charge(X)]), X =< 10. 
implies_single(B,out_of_time) :- implies(B, [dusk]), \+ implies(B, [experiments_done]). 
implies_single(B,calm_dusk) :- implies(B, [nearly_dusk]), implies(B, [calm_weather]). 
implies_single(B,normal_dusk) :- implies(B, [nearly_dusk]), \+ implies(B, [calm_weather]). 

event(predict_violation, [add(predict_violation),add(suspend(exp)),add(activate(charge))], []). 
event(drop(Name), [add(drop(Name))], []). 
event(abort(Name), [add(abort(Name))], []). 
event(suspend(Name), [add(suspend(Name))], []). 
event(activate(Name), [add(activate(Name))], []). 
event(reactivate(Name), [add(reactivate(Name))], []). 
event(reconsider(Name), [add(reconsider(Name))], []). 
event(rock_seen, [add(rock_seen)], []). 
event(dusk, [add(dusk)], []). 
event(nearly_dusk, [add(nearly_dusk)], []). 
event(nearly_dawn, [add(nearly_dawn)], []). 

% action(Name, PreCondition, Test, Calculate, Change)
action(p1, [current_charge(X), X >= 20], [current_charge(X)], X1 is X - 20, [del(current_charge(X)), add(current_charge(X1)), del(dawn)]). 
action(p2, [current_charge(X), X >= 50], [current_charge(X)], X1 is X - 50, [del(current_charge(X)), add(current_charge(X1))]). 
action(p3, [current_charge(X), X >= 22], [current_charge(X)], X1 is X - 22, [del(current_charge(X)), add(current_charge(X1))]). 
action(p4, [current_charge(X), X >= 20], [current_charge(X)], X1 is X - 20, [del(current_charge(X)), add(current_charge(X1)), add(experiments_done)]). 
action(s1, [true], [true], true, [add(at_rock)] ). 
action(s2, [at_rock], [true], true, [del(at_rock), add(rock_analysed) ]). 
action(charge, [true], [current_charge(X)], true, [del(current_charge(X)), add(current_charge(100)), del(predict_violation)] ).
action(return, [true], [true], true, [add(at_base)] ).
action(n1, [true], [true], true, [] ). 
action(n2, [true], [true], true, [add(stable_position)] ). 
action(n3, [true], [true], true, [del(nearly_dawn), add(done) ] ). 

% initial_beliefs(B), action(A, PreCond, Calculate, List), Calculate, update_beliefs(B, List, NewB).

% Ugly hacks until I can think of some better way to handle built-ins in the simplistic belief-specific meta-interpreter.
searched :- fail.
drop(_) :- fail.
abort(_) :- fail.
suspend(_) :- fail.
activate(_) :- fail.
reactivate(_) :- fail.
reconsider(_) :- fail.
respond(_) :- fail.
rerespond(_) :- fail.
active(_) :- fail.
suspended(_) :- fail.
recharged :- fail.
current_charge(_) :- fail.
return_time(_) :- fail.
at(_,_) :- fail.
satisfied(_) :- fail.
predict_violation :- fail.
mc1 :- fail.
experiments_done :- fail.
out_of_time :- fail.
at_rock :- fail.
at_base :- fail.
rock_analysed :- fail.
rock_seen :- fail.
active(_) :- fail.
dawn :- fail.
new_dawn :- fail.
nearly_dusk :- fail.
nearly_dawn :- fail.
calm_weather :- fail.
dusk :- fail.
done :- fail.

curiosity :- initial_state(State), format("Curiosity commences execution ... ~n",[]), reduce(0, State).


