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

reduce(State) :-
	revise(State, RevisedState), 
	format("Checking goals for state changes ... ~n", []), 
	reduce_goals(RevisedState, 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),
        deliberate(NewState, Result).

deliberate(NewState, finished) :-
	format("Finished with the agent in this state~n", []),
	output_state(NewState). 

deliberate(NewState, continue) :-	
	reduce(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", []), 
	Remainder = [GoalContext|Rest],
        % format("Reduce goal state 1~n", []), 
	GoalContext = g(Name, Goal, Rules, Status, Plan),
	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", []), 
	Remainder = [GoalContext|Rest],
        % format("Reduce goal state 3~n", []), 
	% format("Goal is ", []), output_single_goal(GoalContext), nl, 
        % format("Checking goal context~n", []), 
	GoalContext = g(Name, Goal, Rules, pending, Plan),
        % format("Reduce goal state 3~n", []), 
        % format("Checking goal type~n", []), 
        goal_type(Goal, GoalType), member(GoalType, [maintain]), 
	goal_action(Name, GoalType, pending, B, NewB, Rules, Action),
        % format("Activating maintain goal~n", []), 
	member(Action, [activate]),
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, pending, waiting]), 
	NewGoalContext = g(Name, Goal, Rules, waiting, 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, waiting, Plan),
        goal_type(Goal, GoalType), member(GoalType, [maintain]), 
        % format("Reduce goal state 4~n", []), 
	goal_action(Name, GoalType, waiting, B, NewB, Rules, Action),
	member(Action, [respond]),
	Goal = maintain(MC, Predict, Recover, Prevent, Succ, Fail), 
        choose_goal(MC, Recover, Prevent, Chosen), 
	format("Goal ~k ~k: ~k -> ~k~n", [Name, Action, waiting, active]), 
	NewGoalContext = g(Name, Goal, Rules, active, Plan),
        % format("Chosen goal is ", []), display(Chosen), nl, 
	goal(g(Chosen, Goal2, Rules2)), 
        ExtraGoalContext = g(Chosen, Goal2, Rules2, pending, empty),
	append(SoFar, [NewGoalContext,ExtraGoalContext], Temp), 
        append(Temp, 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, waiting]), 
	NewGoalContext = g(Name, Goal, Rules, waiting, Plan), % rule ?
	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, waiting]), 
	NewGoalContext = g(Name, Goal, Rules, waiting, 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, Rules2, 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, 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, waiting, suspend). 
applicable_action(maintain, waiting, respond). 
applicable_action(maintain, active, reactivate). 
applicable_action(_, suspended, reactivate). 
applicable_action(_, suspended, reconsider). 
% See four-state diagram

update_action_beliefs(Name, B, Action, NewB) :-
       member(Name, [save,help,charge]), 
       member(Action, [drop, abort, suspend]),
       Bel =.. [Action,Name], 
       % format("Deleting ~k(~k)~n", [Action,Name]), 
       update_beliefs(B, [del(Bel), add(reactivate(search))], NewB).

update_action_beliefs(Name, B, Action, NewB) :- 
       \+ (member(Name, [save,help,charge]), member(Action, [drop, abort, suspend])), 
       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_beliefs(Name, B, Action, B) :-
       \+ member(Action, [drop, abort, suspend, activate, 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).

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 ~k ~k || ", [Name, GC, Status, Plan]).

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

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

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) :-
       execute1([], state(B,G), NewState, Status).

execute1(SoFar, state(B,[]), state(B, SoFar), finished). 
execute1(SoFar, state(B,G), NewState, Status) :-
	G = [GoalContext|Others], 
       Rest = [GoalContext|Others], 
       GoalContext = g(Name, Goal, Rules, Status, Plan), Status \= active, % Not active, so do not execute it. 
       % output_goals(Rest), 
       format("~k: not executing ", [Name]), 
       append(SoFar, [GoalContext], NewSoFar),
       execute1(NewSoFar, state(B,Others), NewState, Status).
execute1(SoFar, state(B,G), state(NewB, NewGoals), continue) :-
       G = [GoalContext|Others], 
       GoalContext = g(Name, Goal, Rules, active, Plan), Plan \= empty, 
       % format("Checking maintenance activity ~n", []), 	
       append(SoFar, G, Goals), inactive_maintenance(B,Goals), % Active, and no maintenance goal activity, so execute plan. 
       format("~k executing ", [Name]), 
       append(G,SoFar,NewGs), 
       execute_step(state(B, NewGs), state(NewB, _NewGoals), PlanStatus, NewPlan), 
       format("~k ", [PlanStatus]), 
       nextplan(PlanStatus, NewPlan, Next), 
       NewGoalContext = g(Name, Goal, Rules, active, Next), 
       append(SoFar, [NewGoalContext], NewSoFar), append(NewSoFar, Others, NewGoals).

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

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].

% Ask and succeed
execute_step(state(B, G), state(B, NewGoals), success, []) :-
	G = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        Plan = [nil],
        NewGoalContext = g(Name, Goal, Rules, active, []),
        NewGoals = [NewGoalContext|Others].

execute_step(state(B, G), state(B, NewGoals), success, NewPlan) :-
	G = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        Plan = [ask(Phi)|NewPlan], 
        implies(B, [Phi]), 
        NewGoalContext = g(Name, Goal, Rules, active, NewPlan),
        NewGoals = [NewGoalContext|Others].

% Ask but fail
execute_step(state(B, G), state(B, NewGoals), failed, NewPlan) :-
	G = [GoalContext|Others], 
        GoalContext = g(Name, Goal, Rules, active, Plan), 
        Plan = [ask(Phi)|NewPlan], 
        \+ implies(B, [Phi]), 
        NewGoalContext = g(Name, Goal, Rules, active, empty),
        NewGoals = [NewGoalContext|Others].

% Add belief
execute_step(state(B, G), state(NewB, NewGoals), success, NewPlan) :-
	G = [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(state(B, G), state(B, NewGoals), success, NewPlan) :-
	G = [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(state(B, G), state(NewB, NewGoals), success, NewPlan) :-
	G = [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(state(B, G), state(NewB, NewGoals), success, NewPlan) :-
	G = [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", []), 
	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(state(B, G), state(B, NewGoals), failed, empty) :-
	G = [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].

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

        execute_step(state(B, G), state(NB, NGoals), Status, NewPlan1), 
        append(NewPlan1, NewP, NewPlan),

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


% 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). 

% Halt cases
execute_plan(B,[], B,[], success) :-
	format("Done~n", []),!. 

% Ongoing simple cases
execute_plan(B,P, BF, PF, Status):-
	% format("Simple~n", []), 
	P \= [_ '|>' _|_], P \= [_ '}}' _|_], 
	(P \= goal(_,_,_); (P = goal(S,PP,F), PP \= _ '|>' _)), 
	format("Current plan is ", []), display(P), nl, 
	next(B,P, NewB, NewP),
	format("Next Plan is ", []), display(NewP), nl, 
	revise(NewB, NewP, NB, NP),
	prioritise(NP, NPs),
	execute_plan(NB,NPs,BF,PF,Status),!.
	
% Ongoing complex cases
% fail
execute_plan(B,P,BF,PF, Status) :-
	% format("complex 1~n", []), 
	P = [P1 '|>' P2|Rest], 
	P1 \== nil, 
	format("Fail case 1~n", []), 
        append([P1], Rest, NewP), 
	revise(B, NewP, NB,NP),
	prioritise(NP, NPs),
	execute_plan(NB,NPs,BF,PF,Status),!. 

% fail
execute_plan(B,P,BF,PF,Status) :-
	% format("complex 2~n", []), 
	P = [P1 '|>' P2|Rest], 
	P1 \== nil, 
	format("Fail case 2~n", []), 
	append([P2], Rest, New), 
	revise(B, New, NB, NP),
	prioritise(NP, NPs),
	execute_plan(NB,NPs,BF,PF, Status), !. 

% Sequence
execute_plan(B,P,BF,PF, Status):-
	format("complex 3~n", []), 
	P = [P1 '}}' P2|Rest], 
	P1 \== nil, 	format("Sequence~n", []), 
	append([P1], Rest, New), 
	revise(B, New, NB,NP),
	prioritise(NP, NPs),
	execute_plan(NB,NPs,BF1,PF1, Status1), append([P2], PF1, New2), 
	Status1 = success, 
	revise(BF1,New2,NB2,NP2), 
	prioritise(NP2, NP2s),
	execute_plan(NB2,NP2s,BF,PF, Status),!.

execute_plan(B,P,B,P, failed).

%% For now, do not consider plans with subgoals
% % goal with fail
% execute_plan(state(B, G, [goal(S,P,F)|Rest]), state(NewB, NewG, NewP)) :- 
% 	% format("complex 4~n", []), 
% 	P = (P1 '|>' P2), \+ implies(B, [S]), \+ implies(B, [F]),
% 	\+ member(P1, [true,fail]), 
% 	% format("Goal fail ~n", []), 
% 	revise(state(B, G, [P1]), state(NB,NG,NP)),
% 	prioritise(NP, NPs),
% 	execute_plan(state(NB,NG,NPs),state(BF1,GF1,PF1)), planify(PF1, Plan), 
% 	revise(state(BF1,GF1,[goal(S,Plan '|>' P2,F)|Rest]), state(NB2,NG2,NP2)), 
% 	prioritise(NP2, NP2s),
% 	execute_plan(state(NB2,NG2,NP2s),state(BF,GF,PF)).
% 
% execute_plan(state(B, G, [goal(S,P,F)|Rest]), state(BF,GF,PF)) :-
% 	% format("complex 5~n", []), 
% 	P = (P1 '|>' P2), \+ implies(B, [S]), \+ implies(B, [F]), 
% 	member(P1, [true,fail]),
% 	% format("Goal repeat~n", []), 
% 	revise(state(B,G,[goal(S, P2 '|>' P2, F)|Rest]), state(NB,NG,NP)),
% 	prioritise(NP, NPs),
% 	execute_plan(state(NB,NG,NPs),state(BF,GF,PF)).

revise(B,P, B, NewP) :-
	% write(state(B,G,P)), 
	format("~nEvent? ([]. for no change): ", []), ttyflush, 
	read(Term), process(Term, List), append(List, P, NewP).

revise(state(B,G), state(NewB, NewG)) :-
        format("~nEvent? ([]. for no change): ", []), ttyflush, 
       	read(Term), process(Term, List), 
        process_events(List, state(B,G), state(NewB, NewG)).

process_events([], State, State).
process_events([ev(Event)|Rest], state(B,G), state(NewB, NewG)) :-
        process_event(Event, state(B,G), state(NB,NG)),
        process_events(Rest, state(NB,NG), state(NewB, NewG)).

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
% Initial goals for Alpha, Bravo, Charlie example:

% g(search, perform(search,searched,false), R1, pending, search)
% g(recharge, maintain(Current_charge > Return_time, predict,Charge,Charge,true,true), R2, pending, empty)
% MC is current_charge > return_time,
% Charge is achieve(recharged,false),
% R1 is standard(search, {S,F}, false)
% R2 is standard(recharge, {drop(charge),abort(charge)}, false) U <recharged, reactivate>
% R3 is R2 U <\neg MC, MC \land predict(\neg MC), respond>
% R4 is standard(save, {at(survivor, base)}, false)
% R5 is R1 U < at(survivor, base), reconsider>
% R6 is standard(help, satisfied(Other),false)
% R7 is standard(charge, recharged, \neg C \lor (C \land predict(\neg C))) U <drop(recharge),suspend(recharge),abort(recharge), abort>
% standard(Id,Succ,Cond) is
%   <{Succ, drop(Id)},drop>, <abort(Id),abort>, <suspend(Id),suspend>,<Cond \land activate(Id),activate>
% R1 is [act([S,drop(search)],drop), act([abort(search)],abort), act([suspend(search)],suspend), act([activate(search)],activate)]
% R2 is [act([drop(charge),abort(charge),drop(recharge)],drop), act([abort(recharge)],abort), act([suspend(recharge)],suspend), act([activate(recharge)], activate), act([recharged],reactivate)]
% R3 is [act([drop(charge),abort(charge),drop(recharge)],drop), act([abort(recharge)],abort), act([suspend(recharge)],suspend), act([activate(recharge)], activate), act([recharged],reactivate), act([current_charge =< return_time, (current_charge > return_time, predict(current_charge =< return_time))], respond)]
% R4 is [act([at(survivor,base),drop(save)],drop), act([abort(save)],abort), act([suspend(save)],suspend), act([activate(save)],activate)]
% R5 is [act([S,drop(search)],drop), act([abort(search)],abort), act([suspend(search)],suspend), act([activate(search)],activate), act([at(survivor, base)], reconsider)]
% R6 is [act([satisfied(Other),drop(help)],drop), act([abort(help)],abort), act([suspend(help)],suspend), act([activate(help)],activate)]
% R7 is [act([recharged,drop(charge)],drop), act([drop(recharge),suspend(recharge),abort(recharge),abort(charge)],abort), act([suspend(charge)],suspend), act([(current_charge > return_time, predict(current_charge =< return_time),activate(charge))],activate)]

rules(1, [act([searched,drop(search)],drop), act([abort(search)],abort), act([suspend(search)],suspend), act([activate(search)],activate)]).
rules(2, [act([drop(charge),abort(charge),drop(recharge)],drop), act([abort(recharge)],abort), act([suspend(recharge)],suspend), act([activate(recharge)], activate), act([recharged],reactivate)]).
rules(3, [act([drop(charge),abort(charge),drop(recharge)],drop), act([abort(recharge)],abort), act([suspend(recharge)],suspend), act([activate(recharge)], activate), act([recharged],reactivate), act([[current_charge(X), return_time(Y), X =< Y], predict_violation], respond)]).
rules(4, [act([at(survivor,base),drop(save)],drop), act([abort(save)],abort), act([suspend(save)],suspend), act([activate(save)],activate)]).
rules(5, [act([S,drop(search)],drop), act([abort(search)],abort), act([suspend(search)],suspend), act([activate(search)],activate), act([at(survivor, base)], reconsider)]).
rules(6, [act([satisfied(Other),drop(help)],drop), act([abort(help)],abort), act([suspend(help)],suspend), act([activate(help)],activate)]).
rules(7, [act([recharged,drop(charge)],drop), act([drop(recharge),suspend(recharge),abort(recharge),abort(charge)],abort), act([suspend(charge)],suspend), act([[current_charge(X), return_time(Y), X =< Y, activate(charge)], [predict_violation, activate(charge)]], activate)]). 

initial_state(state(B,G)) :- 
     initial_beliefs(B),
     initial_goals(G). 
    
initial_goals([G1,G2]) :-
     search_initial(G1),
     maintain_initial(G2).

initial_beliefs([activate(search),activate(recharge),current_charge(240), return_time(0)]).

search_initial(g(search,perform(search,searched,false), R1, pending,  S)) :-  rules(1, R1), search_plan(S). 

% search_plan( [ s1 '}}' (s2 '}}' (s3 '}}' (s4 '}}' (s5 '}}' (s6 '}}' (s7 '}}' (s8 '}}' (s9 '}}' s10)))))))) ] ). 
search_plan( [r1,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10 ] ). 
maintain_initial(g(recharge, maintain([mc1], predict,charge,charge,false, false), R2, pending, empty)) :-     rules(3, R2). 
% Need to work out exactly how to state the maintenance condition. Presumably query to beliefs, eg current_charge(X), return_time(Y), X > Y. 

% Other goals
% Name, goal, rules only are needed. Status will commence in pending with the empty plan. 
goal(g(save,achieve(at(survivor,base),false), Rules)) :-  rules(4, Rules). 
goal(g(help,achieve(satisfied(Other),false), Rules)) :-   rules(6, Rules). 
goal(g(charge,achieve(recharged,false), Rules)) :-   rules(7, Rules). 

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(charge, _, _, [charge]).
find_plan(help, _, _, [assist]).
find_plan(help, _, _, [assist]). % Very dirty hack! Idea is to have assist chosen 3 times as the plan for help. 
find_plan(help, _, _, [assist]).
find_plan(save, _, _, [taketobase]).

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

implies_single(B, G) :- G \= mc1, member(G, B). 
implies_single(B, G) :- G \= mc1, G. % For built-ins.
implies_single(B, G) :- G \= mc1, member(rule(Head, Body), B), Head = G, implies(B, Body).
implies_single(B,mc1) :- 
      implies(B, current_charge(X)),
      implies(B, return_time(Y)),
      X > Y.      		 
     
robots :- initial_state(State), format("Initial robots state is:~n",[]), output_state(State), reduce(State).

event(survivor, [del(activate(search)),add(suspend(search)),add(activate(save))], [g(save,Goal,Rules,pending,empty)]) :- goal(g(save,Goal,Rules)).
event(request, [del(activate(search)),add(suspend(search)), add(activate(help))], [g(help,Goal,Rules,pending,empty)]) :- goal(g(help,Goal,Rules)). 
event(abort(help), [del(activate(help)),add(abort(help))], []). 
event(satisfied(bravo), [add(satisfied(bravo))], []). 
event(predict_violation, [add(predict_violation),add(suspend(search)),add(activate(charge))], []). 

% action(Name, PreCondition, Test, Calculate, Change)
action(s1, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(20))]). 
action(s2, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(21))]). 
action(s3, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(22))]). 
action(s4, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(23))]). 
action(s5, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(24))]). 
action(s6, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(25))]). 
action(s7, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(26))]). 
action(s8, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(27))]). 
action(s9, [current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(28))]). 
action(s10,[current_charge(X), X >= 15], [current_charge(X)], X1 is X - 15, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(29)), searched]). 
action(charge, [return_time(0)], [current_charge(X)], true, [del(current_charge(X)), add(current_charge(240))]).

action(r1,  [true], [current_charge(X)], X1 is X - 20, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(20))]). 
action(r2,  [true], [current_charge(X)], X1 is X - 21, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(21))]). 
action(r3,  [true], [current_charge(X)], X1 is X - 22, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(22))]). 
action(r4,  [true], [current_charge(X)], X1 is X - 23, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(23))]). 
action(r5,  [true], [current_charge(X)], X1 is X - 24, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(24))]). 
action(r6,  [true], [current_charge(X)], X1 is X - 25, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(25))]). 
action(r7,  [true], [current_charge(X)], X1 is X - 26, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(26))]). 
action(r8,  [true], [current_charge(X)], X1 is X - 27, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(27))]). 
action(r9,  [true], [current_charge(X)], X1 is X - 28, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(28))]). 
action(r10, [true], [current_charge(X)], X1 is X - 29, [del(current_charge(X)), add(current_charge(X1)), del(return_time(Z)), add(return_time(29))]). 
action(taketobase, [current_charge(X), return_time(Y), X >= Y], [current_charge(X),return_time(Y)], X1 is X - Y, [del(current_charge(X)), add(current_charge(X1)),del(return_time(Y)), add(return_time(0)), add(at(survivor,base))]).
action(assist, [true], [true], true, []). 

% 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.
recharged :- fail.
current_charge(_) :- fail.
return_time(_) :- fail.
at(_,_) :- fail.
satisfied(_) :- fail.
predict_violation :- fail.
mc1 :- fail.

