/** <module> NXT Mindstroms - simple movement using mid-level iCommand methods.
@author Piotr Hołownia
*/

:- module(nxt_java_movement,[
		nxt_set_robot/5,
		nxt_stop/0,
		nxt_go/1, nxt_go/2,
		nxt_go_cm/2, nxt_go_cm/3,
		nxt_go_in/2, nxt_go_in/3,
		nxt_turn_degrees/2
	       ]).

:- dynamic nxt_robot/1.
	
:- use_module(nxt_components).
:- use_module(threads).

% %	nxt_set_robot(+WheelCircumference,+AxleLenght,+LeftMotor,+RightMotor,+Reverse).
%
%	Changes robot settings.

nxt_set_robot(WheelCircumference,AxleLenght,LeftMotor,RightMotor,Reverse) :-
	Reverse is 1,
	nonvar(WheelCircumference),nonvar(AxleLenght),
	nonvar(LeftMotor),nonvar(RightMotor),nonvar(Reverse),
	WheelDiameter is WheelCircumference/pi,
	jpl_new('icommand.navigation.Pilot',[WheelDiameter,AxleLenght,LeftMotor,RightMotor,@(true)],PilotHandle),
	retractall(nxt_robot(_)),
	assert(nxt_robot(PilotHandle)).

:- nxt_set_robot(17.5,11,'C','B',0).

nxt_set_robot(WheelCircumference,AxleLenght,LeftMotor,RightMotor,Reverse) :-
	Reverse is 0,
	nonvar(WheelCircumference),nonvar(AxleLenght),
	nonvar(LeftMotor),nonvar(RightMotor),nonvar(Reverse),
	WheelDiameter is WheelCircumference/pi,
	jpl_new('icommand.navigation.Pilot',[WheelDiameter,AxleLenght,LeftMotor,RightMotor,@(false)],PilotHandle),
	retractall(nxt_robot(_)),
	assert(nxt_robot(PilotHandle)).
	
%%	nxt_stop.
%
%	Stops the robot.

nxt_stop :-
	nxt_robot(PilotHandle),
	jpl_call(PilotHandle,'stop',[],_).

%%	nxt_go(+Speed).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is rotational speed of the wheel in degrees per second.
%	Wheels stop when nxt_stop predicate is called.
%	If the robot senses an obstacle, it will stop.

nxt_go(Speed) :-
	nxt_go(Speed,force),
	trigger_create(_,nxt_touch_sensor(1),nxt_stop).

%%	nxt_go(+Speed,force).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is rotational speed of the wheel in degrees per second.
%	Wheels stop when nxt_stop predicate is called.
%	If the robot hits an obstacle, it will try to push it.

nxt_go(Speed,force) :-
	Speed > 0,
	nxt_robot(PilotHandle),
	jpl_call(PilotHandle,'setSpeed',[Speed],_),
	jpl_call(PilotHandle,'forward',[],_).

nxt_go(Speed,force) :-
	Speed < 0,
	nxt_robot(PilotHandle),
	jpl_call(PilotHandle,'setSpeed',[-Speed],_),
	jpl_call(PilotHandle,'backward',[],_).
	
%%	nxt_go_cm(+Speed,+Distance).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is rotational speed of the wheel in degrees per second.
%	Wheels will stop, if the Distance (in cm) is reached.
%	If the robot senses an obstacle, it will stop.

nxt_go_cm(Speed,Distance) :-
	nxt_go_cm(Speed,Distance,force),
	trigger_create(_,nxt_touch_sensor(1),nxt_stop).

%%	nxt_go_cm(+Speed,+Distance,force).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is rotational speed of the wheel in degrees per second.
%	Wheels will stop, if the Distance (in cm) is reached.
%	If the robot hits an obstacle, it will try to push it.

nxt_go_cm(Speed,Distance,force) :-
	Speed > 0,
	nxt_robot(PilotHandle),
	jpl_call(PilotHandle,'setSpeed',[Speed],_),
	jpl_call(PilotHandle,'travel',[Distance,@(true)],_).

nxt_go_cm(Speed,Distance,force) :-
	Speed < 0,
	nxt_robot(PilotHandle),
	jpl_call(PilotHandle,'setSpeed',[-Speed],_),
	jpl_call(PilotHandle,'travel',[-Distance,@(true)],_).

%%	nxt_go_in(+Speed,+Distance).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is rotational speed of the wheel in degrees per second.
%	Wheels will stop, if the Distance (in inches) is reached.
%	If the robot senses an obstacle, it will stop.

nxt_go_in(Speed,Distance) :-
	nxt_go_cm(Speed,2.54*Distance).

%%	nxt_go_in(+Speed,+Distance,force).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is rotational speed of the wheel in degrees per second.
%	Wheels will stop, if the Distance (in inches) is reached.
%	If the robot hits an obstacle, it will try to push it.

nxt_go_in(Speed,Distance,force) :-
	nxt_go_cm(Speed,2.54*Distance,force).

%%	nxt_turn_degrees(+Speed,+Degrees).
%
%	Rotates the robot in place to its left (if Degrees is greater
%	than 0) or right (if Degrees is smaller than 0). Speed is
%	rotational speed of the wheel in degrees per second. Wheels will
%	stop, when specified revolution (Degrees) of the robot is
%	reached.

nxt_turn_degrees(Speed,Degrees) :-
	nxt_robot(PilotHandle),
	jpl_call(PilotHandle,'setSpeed',[Speed],_),
	jpl_call(PilotHandle,'rotate',[Degrees,@(true)],_).