/** <module> NXT Mindstroms - simple movement.
@author Piotr Hołownia
*/

:- module(nxt_movement,[
		go/2, go/3,
		go_sec/2, go_sec/3,
		go_cm/2, go_cm/3,
		go_in/2, go_in/3,
		go_cm_sec/2, go_cm_sec/3,
		go_in_sec/2, go_in_sec/3,
		turn/2, turn/4,
		turn_degrees/2
	       ]).
		
	
:- use_module(nxt_actions).
:- use_module(nxt_components).

% Circumference of wheels in cm.
wheelCircumference(17.5).

% Length of the axle in cm.
axleLength(15).

%%	go(+Angle,+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 after revolution of Angle (in degrees).
%	If the robot senses an obstacle, it will stop.

go(Angle,Speed) :-
	go(Angle,Speed,force),
	trigger_create(_,touch_sensor(pressed),[
					       motor(c,0),
					       motor(b,0)
					      ]).

%%	go(+Angle,+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 after revolution of Angle (in degrees).
%	If the robot hits an obstacle, it will try to push it.

go(Angle,Speed,force) :-
	motor(c,Speed,Angle),
	motor(b,Speed,Angle).

%%	go(+Time,+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 after specified time (in seconds).
%	If the robot senses an obstacle, it will stop.

go_sec(Time,Speed) :-
	go_sec(Time,Speed,force),
	trigger_create(_,touch_sensor(pressed),[
					       motor(c,0),
					       motor(b,0)
					      ]).

%%	go(+Time,+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 after specified time (in seconds).
%	If the robot hits an obstacle, it will try to push it.

go_sec(Time,Speed,force) :-
	motor(c,Speed,time(Time)),
	motor(b,Speed,time(Time)).

%%	go_cm(+Distance,+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 will stop, if the Distance (in cm) is reached.
%	If the robot senses an obstacle, it will stop.

go_cm(Distance,Speed) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	go(Angle,Speed).

%%	go_cm(+Distance,+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 will stop, if the Distance (in cm) is reached.
%	If the robot hits an obstacle, it will try to push it.

go_cm(Distance,Speed,force) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	go(Angle,Speed,force).

%%	go_in(+Distance,+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 will stop, if the Distance (in inches) is reached.
%	If the robot senses an obstacle, it will stop.

go_in(Distance,Speed) :-
	go_cm(2.54*Distance,Speed).

%%	go_in(+Distance,+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 will stop, if the Distance (in inches) is reached.
%	If the robot hits an obstacle, it will try to push it.

go_in(Distance,Speed,force) :-
	go_cm(2.54*Distance,Speed,force).

%%	go_cm_sec(+Distance,+Time).
%
%	Moves the robot forward (if Distance is greater than 0)
%	or backward (if Distance is smaller than 0).
%	Robot reaches the Distance (in cm) in Time (in seconds).
%	If the robot senses an obstacle, it will stop.

go_cm_sec(Distance,Time) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	Speed is Angle/Time,
	go(-Angle,Speed).

%%	go_cm_sec(+Distance,+Time,force).
%
%	Moves the robot forward (if Distance is greater than 0)
%	or backward (if Distance is smaller than 0).
%	Robot reaches the Distance (in cm) in Time (in seconds).
%	If the robot hits an obstacle, it will try to push it.

go_cm_sec(Distance,Time,force) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	Speed is Angle/Time,
	go(-Angle,Speed,force).

%%	go_in_sec(+Distance,+Time).
%
%	Moves the robot forward (if Distance is greater than 0)
%	or backward (if Distance is smaller than 0).
%	Robot reaches the Distance (in inches) in Time (in seconds).
%	If the robot senses an obstacle, it will stop.

go_in_sec(Distance,Time) :-
	go_cm_sec(2.54*Distance,Time).

%%	go_in_sec(+Distance,+Time,force).
%
%	Moves the robot forward (if Distance is greater than 0)
%	or backward (if Distance is smaller than 0).
%	Robot reaches the Distance (in inches) in Time (in seconds).
%	If the robot hits an obstacle, it will try to push it.

go_in_sec(Distance,Time,force) :-
	go_cm_sec(2.54*Distance,Time,force).

%%	turn(+Angle,+Speed).
%
%	Rotates the robot in place to its rigth (if Angle is greater than 0)
%	or left (if Angle is smaller than 0).
%	Speed is rotational speed of the wheel in degrees per second.
%	Wheels stop after revolution of Angle (in degrees).

turn(Angle,Speed) :-
	Angle > 0,
	motor(c,Speed,Angle),
	motor(b,-Speed,Angle).
turn(Angle,Speed) :-
	Angle < 0,
	motor(c,-Speed,Angle),
	motor(b,Speed,Angle).

%%	turn_degrees(+Degrees,+Speed).
%
%	Rotates the robot in place to its rigth (if Degrees is greater than 0)
%	or left (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.
	
turn_degrees(Degrees,Speed) :-
	axleLength(AL),
	wheelCircumference(WC),
	%Distance is pi*aL*Degrees/360,
	%Angle is Distance/WC*360,
	Angle is pi*AL*Degrees/WC,
	turn(Angle,Speed).

%%	turn(forward,+Radius,+Degrees,+Time).
%
%	Makes robot turn with specified turning radius (Radius) moving forward
%	to its right (if Degrees is greater than 0)
%	or left (if Degrees is smaller than 0).
%	Robot reaches the specified revolution (Degrees) in Time (in seconds).
%	If the robot senses an obstacle, it will stop.

turn(forward,Radius,Degrees,Time) :-
	turn(forward,Radius,Degrees,Time,force),
	trigger_create(_,touch_sensor(pressed),[
					       motor(c,0),
					       motor(b,0)
					      ]).
%%	turn(forward,+Radius,+Degrees,+Time,force).
%
%	Makes robot turn with specified turning radius (Radius) moving forward
%	to its right (if Degrees is greater than 0)
%	or left (if Degrees is smaller than 0).
%	Robot reaches the specified revolution (Degrees) in Time (in seconds).
%	If the robot hits an obstacle, it will try to push it.

turn(forward,Radius,Degrees,Time,force) :-
	Degrees > 0,
	turn_parameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	motor(c,Speed2,Angle2),
	motor(b,Speed1,Angle1).

turn(forward,Radius,Degrees,Time,force) :-
	Degrees < 0,
	turn_parameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	motor(b,Speed2,Angle2),
	motor(c,Speed1,Angle1).

%%	turn(backward,+Radius,+Degrees,+Time).
%
%	Makes robot turn with specified turning radius (Radius) moving backward
%	to its right (if Degrees is greater than 0)
%	or left (if Degrees is smaller than 0).
%	Robot reaches the specified revolution (Degrees) in Time (in seconds).
%	If the robot senses an obstacle, it will stop.

turn(backward,Radius,Degrees,Time) :-
	turn(backward,Radius,Degrees,Time,force),
	trigger_create(_,touch_sensor(pressed),[
					       motor(c,0),
					       motor(b,0)
					      ]).

%%	turn(backward,+Radius,+Degrees,+Time,force).
%
%	Makes robot turn with specified turning radius (Radius) moving backward
%	to its right (if Degrees is greater than 0)
%	or left (if Degrees is smaller than 0).
%	Robot reaches the specified revolution (Degrees) in Time (in seconds).
%	If the robot hits an obstacle, it will try to push it.

turn(backward,Radius,Degrees,Time,force) :-
	Degrees > 0,
	turn_parameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	motor(c,-Speed2,Angle2),
	motor(b,-Speed1,Angle1).

turn(backward,Radius,Degrees,Time,force) :-
	Degrees < 0,
	turn_parameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	motor(b,-Speed2,Angle2),
	motor(c,-Speed1,Angle1).

turn_parameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2) :-
	%Distance1 is 2*pi*Radius*Degrees/360,
	axleLength(AL),
	%Distance2 is 2*pi*(Radius+AL)*Degrees/360,
	wheelCircumference(WC),
	%Angle1 is Distance1/WC*360,
	%Angle2 is Distance2/WC*360,
	Angle1 is 2*pi*Radius*Degrees/WC,
	Angle2 is 2*pi*(Radius+AL)*Degrees/WC,
	Speed1 is Angle1/Time,
	Speed2 is Angle2/Time.