%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
%            BRACCOBALDO DELIVERY ROBOT        VERSION 2
%    
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  DATA DEFINITION
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Action/message mappings - numbers must correspond to NQC code

actionNum(turnaround           ,0).
actionNum(pickup,               1).
actionNum(putdown,              7).

actionNum(start_to_next_station,2).
actionNum(arrive_at_station    ,3).
actionNum(stop_abnormally      ,4).
actionNum(recover_position     ,6).
actionNum(push_sense_button    ,8).                                          % version 2
%actionNum(rel_sense_button    ,9).                                          % version 2
actionNum(mypanic              ,10).                                         % version 2
actionNum(check                ,11).                                         % version 2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Fluents

way_station(N) :- N = 1; N = 2; N = 3; N = 4; N = 5; N = 6.

prim_fluent(location).              % The last waystation the robot occupied
prim_fluent(direction).             % Aiming 1 (up) or -1 (down)
prim_fluent(motion).                % moving, stopped, or suspended (lost)

prim_fluent(delivery_requested(From)) :- way_station(From). % Delivery request is pending (From is 0 or way station number)
prim_fluent(holding_delivery_for(Loc)) :- way_station(Loc).
prim_fluent(in_hand).
prim_fluent(in_front_of_object).                                             % version 2
prim_fluent(object_on_the_way).                                              % version 2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Initial state

initially(location, 3).
initially(direction, 1). 
initially(motion, stopped).
initially(delivery_requested(_), 0).
initially(in_hand, 0).
initially(in_front_of_object, 0).                                            % version 2
initially(object_on_the_way, 0).                                             % version 2
initially(holding_delivery_for(X), 0) :- X = 1; X = 2; X = 3; X = 4; X = 5; X = 6.




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  ACTIONS
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

prim_action(turnaround).
prim_action(pickup).
prim_action(putdown).
prim_action(check).                                                            % version 2
prim_action(mypanic).                                                          % version 2
prim_action(start_to_next_station).

exog_action(arrive_at_station).
exog_action(stop_abnormally).  
exog_action(push_sense_button).                                              % version 2
%exog_action(rel_sense_button).                                               % version 2
exog_action(request_delivery(_, _)).
exog_action(cancel_request(_, _)).  

senses(_, _) :- fail.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Preconditions of prim actions

poss(pickup                 , in_hand = 0).
poss(putdown                , in_hand = 1).

poss(request_delivery(_, _), true).

poss(arrive_at_station, motion = moving).
poss(stop_abnormally  , motion = moving).
poss(turnaround       , motion = stopped).

%poss(rel_sense_button , in_front_of_object = 1).                             % version 2
poss(push_sense_button , in_front_of_object = 0).                            % version 2


poss(cancel_request(N, M), delivery_requested(N) = M).

poss(start_to_next_station, and(
                                motion=stopped, 
                                or(
                                   and(
                                       direction = 1,
                                       location < 6
                                      ), 
                                   and(
                                       direction = (-1),
                                       location > 1
                                      )
                                  )
                                )
).

poss(check                 , in_front_or_object = 0).
poss(mypanic               , true).            

    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Causal laws

% causes_val(Action,Fluent,Value,Cond): if (Cond) Action --> Fluent = Value

causes_val(turnaround            , direction              , D        , D is (-1) * direction).
causes_val(arrive_at_station     , location               , N        , N is location + direction).
causes_val(arrive_at_station     , motion                 , stopped  , true).
causes_val(start_to_next_station , motion                 , moving   , true).
causes_val(putdown               , in_front_of_object     , 0        , in_front_of_object = 1).   % version 2

causes_val(stop_abnormally       , motion                 , suspended, true).

causes_val(request_delivery(N, M), delivery_requested(N)  , M        , true).
causes_val(cancel_request(N, _)  , delivery_requested(N)  , 0        , true).

causes_val(pickup                , holding_delivery_for(M), 1        , delivery_requested(location) = M).
causes_val(pickup                , in_hand, 1, true).

causes_val(putdown               , delivery_requested(N)  , 0        , location = delivery_requested(N)).
causes_val(putdown               , holding_delivery_for(M), 0        , location = M).
causes_val(putdown               , in_hand, 0, true).

causes_val(push_sense_button     , in_front_of_object     , 1        , in_front_of_object = 0).   % version 2
causes_val(push_sense_button     , motion                 , stopped  , true).                     % version 2
%causes_val(rel_sense_button      , in_front_of_object     , 0        , in_front_of_object = 1).   % version 2
%causes_val(push_sense_button     , object_on_the_way      , 1        , unespected ).              % version 2

causes_val(mypanic               , delivery_requested(N), 0   , location = N).

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  GOLOG PROGRAM
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% proc(+Name, +Program): A user-defined procedure called Name with Program body


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% UNESPECTED() - c'e' un oggetto sulla pista
%
%proc(unespected,
%     and(not(stop_for_pickup(location)),
%         in_hand = 0) 
%).
         

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STOP_FOR_PICKUP(N) - siamo in N: devo caricare qualcosa ?
%
proc(stop_for_pickup(N),
     and(some(m,
          and(
              delivery_requested(N)=m,
              way_station(m)	      
             )
          ),
	in_hand=0)
).  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LOOK_FOR - cerca se l'oggetto si trova nelle vicinanze della station
%proc(look_for, 
%    and(in_front_of_object=0, 
%    check

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% PICKUP_FAILURE(N) - siamo in N, devo caricare un oggetto ma non trovo niente
%
proc(pickup_failure(N),					% version 2
     and(some(m,
         and(
	     delivery_requested(N)=m,
              way_station(m)	      
             )
          ),
	in_hand=0)
%    write('PANICOOOOOOO!!Non trovo nulla!!')
).

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STOP_FOR_PUTDOWN(N) - siamo in N: devo scaricare qualcosa ?
%
proc(stop_for_putdown(N),
     and(holding_delivery_for(N) = 1,
        in_hand = 1)
).

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% NEXT_LOCATION_TO_SERVE(N) - e' N una stazione in cui dobbiamo fermarci?
%
% es. perche' c'e' q.cosa da prendere.

%proc(next_location_to_serve(N), stop_requested(N)).  % Random choice

proc(next_location_to_serve(N), 
     or(
        stop_for_pickup(N),
        stop_for_putdown(N)
       )  % Random choice
).

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HEAD_TO_NEXT_STATION(D) - %D = 1 o -1

proc(head_to_next_station(D),
     if(direction = D, start_to_next_station, turnaround)).


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RECOVER_POSITION - chiede al'utente di sistemare le cose.

proc(recover_position, ?(reset(location, direction))). 

% reset(Location, Direction): auxiliary predicate

reset(Location,Direction) :-
    write('**** I got lost heading from waystation '),
    write(Location),
    (Direction = 1 -> write(' going up.') ; write(' going down.')), nl,
    write('**** Please position me between waystations in the correct '),
    write('direction, and type any key when ready: '),
    get(_).
    
    


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  MAIN ROUTINE
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

proc(control, prioritized_interrupts(
    [interrupt(motion = suspended, [recover_position, start_to_next_station]),
    interrupt(motion = moving         , wait),
    
    interrupt(stop_for_pickup(location),
      [      
       if(in_front_of_object = 0, mypanic,           % version 2
                                  pickup)
      ]),
    
    
  
%    interrupt(pickup_failure(location), [mypanic]),
    interrupt(stop_for_putdown(location), [putdown]),        
    interrupt(n, next_location_to_serve(n), % per ogni n
      [
       if(location < n, head_to_next_station( 1),
                       head_to_next_station(-1))
      ]),
    
    
    interrupt(location > 1            , head_to_next_station(-1)),
        
    interrupt(true                    , wait)
])).



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HOOKS TO legorcx.pl CODE
%  This section provides predicates for executing actions from the IndiGolog
%  interpreter on the RCX and returning exogenous actions detected by
%  the RCX to the interpreter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% initialize: Perform any application dependent initialization
initialize :-
    initializeRcx,
    initializeExog.

% finalize: Application dependent wrap-up
finalize :-
    finalizeRcx,
    finalizeExog.

% execute(+Action, +History, -SensingResult): Execute Action on RCX returning
%     SensingResult. Current action History is supplied for debugging
%     purposes in case something goes wrong
execute(Action, History, SensingResult) :-
    write('Executing action: '), write(Action), nl,
    actionNum(Action, N),
    sendRcxActionNumber(N, SensingNumber),
    translateSensorValue(Action, SensingNumber, SensingResult),
    write('    Sensing result: '), write(SensingResult), nl, nl,
    errorRecoveryData(History).

% If previous clause fails, call user-defined debug routine.
% The debug routine can reattempt the action returning SensingResult
execute(Action, History, SensingResult) :-
    debugRcx(Action, History, SensingResult).

% exog_occurs(-ExogList): Check for occurrence of exogenous actions and
%     return them in list ExogList. Exogenous actions can occur at
%     various sources: here we check the RCX and the keyboard
exog_occurs(ExogList) :-
    checkRcxExog(RcxExogList),
    (RcxExogList == [] -> true;
        (write('    Rcx exogenous action: '), write(RcxExogList), nl, nl)),
    checkOtherExog(OtherExogList),
    (OtherExogList == [] -> true;
        (write('    Other exogenous action: '), write(OtherExogList), nl, nl)),
    append(RcxExogList, OtherExogList, ExogList).

% checkRcxExog(-RcxExogList): Check for occurrence of exogenous actions
%     at RCX. At present RCX can only report one exogenous action at a time
checkRcxExog([Action]) :-
    receiveRcxActionNumber([N]),
    actionNum(Action, N),
    write('Exogenous action: *'), write(Action), write('* has occurred'), nl, !.

checkRcxExog([]).    % No exogenous action from RCX

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use the following for testing independent of legorcx.pl.
% Uncomment to run.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%execute(Action, _, SensingResult) :-
%%    write('Executing action: '), write(Action), nl,
%%    (senses(Action, _) ->
%%        (write('[Enter Sensing value, terminate with "."]: '),
%%             read(SensingResult));
%%        nl),
%%    write('    Sensing result: '), write(SensingResult), nl, nl.
%%
%%% If previous clause fails, call user-defined debug routine
%%execute(Action, History, SensingResult) :-
%%    debugRcx(Action, History, SensingResult).
%%
%%% It is probably better to do this by only checking for exogenous actions
%%%    from the keyboard but, if you want to go through the process slowly,
%%%    do it this way!
%%
%%exog_occurs(ExogList) :-
%%    write('Enter list (possibly empty) of exogenous actions: '),
%%    read(TempExogList),
%%    ((TempExogList = [_|_] ; TempExogList == []) ->
%%        (ExogList=TempExogList,
%%            write('    Exogenous actions: *'), write(ExogList),
%%            write('* have occurred'), nl, nl);
%%        (write('ERROR: You must enter a list (possibly empty) of actions'),nl,
%%            exog_occurs(ExogList))
%%    ).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Translation of sensor values from RCX
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% translateSensorValue(+Action, +SensorValue, SensingResult): Translate
%     the value SensorValue returned by the RCX sensor into a legal
%     SensingResult under Action

translateSensorValue(_, SensorValue, SensorValue).  % Just leave it for now

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DEBUG ROUTINES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% debugRcx(+Action, +History, -SensingResult): The sendRcxActionNumber/2
%     predicate failed (RCX panicked or there was a problem with the
%     communication). This predicate attempts to provide some basic debug
%     and error recovery.
%     If you can't be bothered, uncomment the following clause and
%     the program will abort on failure to execute an action
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%debugRcx(_, _, _) :-
%%    write('ERROR: A problem was encountered while trying to execute action'),
%%    nl,
%%    abort.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
debugRcx(Action, History, SensingResult) :-
    write('** There is a problem with the RCX. It may need to be reset.'), nl,
    errorRecoveryData(History),
    errorRecoveryProc,
    execute(Action, History, SensingResult). % Try action again

% errorRecoveryData(+History): Extract values of primitive fluents at
%     the point where Hist actions are performed.
errorRecoveryData(History) :-
    write('    Actions performed so far: '),
    write(History), nl,
    bagof(U, prim_fluent(U), FluentList),
    printFluentValues(FluentList, History).

% printFluentValues(+FluentList, +History): Print value of primitive fluents
%     at the point where History actions have been performed
printFluentValues([], _) :-
    write('-----------------------------------------------'),
    nl.

printFluentValues([Hf | FluentList], History) :-
    write('    Primitive fluent '),
    write(Hf),
    write(' has value '),
    has_val(Hf, Hv, History),
    write(Hv), nl,
    printFluentValues(FluentList, History).

% errorRecoveryProc: What to do in case of error. In this case, ask the user
%     to reposition the RCX so that last action can be re-attempted
errorRecoveryProc:-
    write('If you wish to abort, enter "a".'), nl,
    write('If you wish to continue execution, place RCX in a position'), nl,
    write('consistent with these values and hit any other key.'), nl,
    get0(Val),
    get0(_),                     % Clear carriage return
    (Val == 65; Val == 97) ->    % 65 is ASCII 'A', 97 is ASCII 'a'
         abort;
         true.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EOF: Delivery/delivery.pl
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
