:- ["../../interfaces/gti/gti.ecl"].


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% Execution of Primitive Actions
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

doReally(queryHeadJoints(Pan, Nod, Tilt, Time), _):-
	doReally(querySensors([headPan, headTilt2, headTilt1], [Pan, Nod, Tilt], Time)),
	writeln(queryHeadJoints(Pan, Nod, Tilt, Time)).

doReally(queryNearDistance(Dist, Time), _):-
	doReally(querySensors([nearIRDist], [Dist], Time)),
	writeln(queryNearDistance(Dist, Time)).

doReally(queryBall(Ball, Visible, Xcoord, Ycoord, Area, Time), _):-
	doReally(queryBall(Ball, Visible, Xcoord, Ycoord, Area, Time)),
	writeln(queryBall(Ball, Visible, Xcoord, Ycoord, Area, Time)).

doReally(searchBall(Ball, Found, Time), _):-
	doReally(searchBall(Ball, Found, Time)),
	writeln(searchBall(Ball, Found, Time)).
	
doReally(pan(Ang, T), _):-
	doReally(moveJoints([[headPan, Ang]], T)),
	writeln(pan(Ang, T)).

doReally(nod(Ang, T), _):-
	doReally(moveJoints([[headTilt2, Ang]], T)),
	writeln(nod(Ang, T)).
	
doReally(tilt(Ang, T), _):-
	doReally(moveJoints([[headTilt1, Ang]], T)),
	writeln(tilt(Ang, T)).

doReally(walk(Dist, T), _):-
	doReally(walk(Dist, 0, T)),
  	writeln(walked(Dist, T)).
	
doReally(turn(Ang, T), _):-
	doReally(turn(Ang, T)),
  	writeln(turned(A, T)).

doReally(getReady(T), _):-
	doReally(motion("getrdy.mot", T)),
	writeln(getReady(T)).
	
doReally(noOp(T), _) :-
	writeln(noOp(T)).
	
doReally(wait(Duration, T), _):-
	sleep(Duration),
	writeln(wait(T)).
	
doReally(playSound(File, T), _):-
	doReally(playSound(File, T)),
	writeln(playSound(File, T)).

	
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Execution of Macro Actions
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

doReally(north(T), _):-
	macroAction(north(T), Proc),
	writeln("\n\n Performing macro action north(T) = "),
	writeln(Proc),
	online(Proc : nil, s0, inf, Pol, V),
	writeln("\n\n Done macro action NORTH \n\n").

doReally(east(T), _):-
	macroAction(east(T), Proc),
	writeln("\n\n Performing macro action east(T) = "),
	writeln(Proc),
	online(Proc : nil, s0, inf, Pol, V),
	writeln("\n\n Done macro action EAST \n\n").

doReally(west(T), _):-
	macroAction(west(T), Proc),
	writeln("\n\n Performing macro action west(T) = "),
	writeln(Proc),
	online(Proc : nil, s0, inf, Pol, V),
	writeln("\n\n Done macro action WEST \n\n").
	
doReally(south(T), _):-
	macroAction(south(T), Proc),
	writeln("\n\n Performing macro action south(T) = "),
	writeln(Proc),
	online(Proc : nil, s0, inf, Pol, V),
	writeln("\n\n Done macro action SOUTH \n\n").	


	/* ----------------------------------------------- */
	
%
% No exogenous action
% 
senseExo(S, S).


% 
% Some convinient aliases and hints
% 
approachDot(Ball):- 
	online(approachDot(Ball) : nil, s0, inf, Pol, V).

traverseMaze:- 
	online(traverseMaze : nil, s0, inf, Pol, V).

	
:- nl, nl.	
:- writeln("doReally(motion(\"getrdy.mot\", 1)).").
:- writeln("approachDot(pink).").
:- writeln("traverseMaze.").
:- nl, nl.



% :- spy(doReally).