%
%     Implementation by Piotr Hołownia <holownia@student.agh.edu.pl>
%
%     Copyright (C) 2006-9 by the HeKatE Project
%
%     PlNXT has been develped by the HeKatE Project, 
%     see http://hekate.ia.agh.edu.pl
%
%     This file is part of PlNXT.
%
%     PlNXT is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     PlNXT is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with PlNXT.  If not, see <http://www.gnu.org/licenses/>.
%

:- module(nxt_movement,[
		nxt_set_robot/9,
		nxt_stop/0,
		nxt_go/1, nxt_go/2, nxt_go/3,
		nxt_go_sec/2, nxt_go_sec/3,
		nxt_go_cm/2, nxt_go_cm/3,
		nxt_go_in/2, nxt_go_in/3,
		nxt_go_cm_sec/2, nxt_go_cm_sec/3,
		nxt_go_in_sec/2, nxt_go_in_sec/3,
		nxt_turn/2, nxt_turn/3, nxt_turn/4,
		nxt_turn_degrees/2
	       ]).

/** <module> NXT Mindstroms - simple movement.

@author Piotr Hołownia
@license GNU General Public License
*/

:- dynamic nxt_robot/9.
	
:- use_module(nxt_sensomoto).
:- use_module(threads).

%%	nxt_set_robot(+WheelCircumference,+AxleLenght,+LeftMotor,+RightMotor,+Reverse,+TouchPort,+SoundPort,+LightPort,+UltrasonicPort).
%
%	Changes the robot's settings. !! Reverse not implemented!

nxt_set_robot(WheelCircumference,AxleLenght,LeftMotor,RightMotor,
	      Reverse,TouchPort,SoundPort,LightPort,UltrasonicPort) :-
	nonvar(WheelCircumference),nonvar(AxleLenght),
	nonvar(LeftMotor),nonvar(RightMotor),nonvar(Reverse),
	retractall(nxt_robot(_,_,_,_,_,_,_,_,_)),
	assert(nxt_robot(WheelCircumference,AxleLenght,LeftMotor,RightMotor,
			 Reverse,TouchPort,SoundPort,LightPort,UltrasonicPort)).

:- nxt_set_robot(17.5,11,'C','B',false,'S1','S2','S3','S4').

%%	nxt_stop.
%
%	Stops the robot.

nxt_stop :-
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_motor(LM,0),
	nxt_motor(RM,0).

%%	nxt_go(+Speed).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is the 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_robot(_,_,_,_,_,TouchP,_,_,_),
	nxt_go(Speed,force),
	trigger_create(_,nxt_sensomoto:nxt_touch_sensor(TouchP,1),nxt_stop).

%%	nxt_go(+Speed,+Option).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is the 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.
%	Option is:
%
%		* force

nxt_go(Speed,force) :-
	Speed \= 0,
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_motor(LM,Speed),
	nxt_motor(RM,Speed).

%%	nxt_go(+Speed,+Angle).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is the 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.

nxt_go(Speed,Angle) :-
	nxt_robot(_,_,_,_,_,TouchP,_,_,_),
	nxt_go(Speed,Angle,force),
	trigger_create(_,nxt_sensomoto:nxt_touch_sensor(TouchP,1),nxt_stop).

%%	nxt_go(+Speed,+Angle,+Option).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is the 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.
%	Option is:
%
%		* force

nxt_go(Speed,Angle,force) :-
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_motor(LM,Speed,Angle),
	nxt_motor(RM,Speed,Angle).

%%	nxt_go_sec(+Speed,+Time).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is the 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.

nxt_go_sec(Speed,Time) :-
	nxt_robot(_,_,_,_,_,TouchP,_,_,_),
	nxt_go_sec(Speed,Time,force),
	trigger_create(_,nxt_sensomoto:nxt_touch_sensor(TouchP,1),nxt_stop).

%%	nxt_go_sec(+Speed,+Time,+Option).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is the 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.
%	Option is:
%
%		* force

nxt_go_sec(Speed,Time,force) :-
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_motor(LM,Speed,time(Time)),
	nxt_motor(RM,Speed,time(Time)).

%%	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 the 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_robot(WC,_,_,_,_,_,_,_,_),
	Angle is round(Distance/WC*360),
	nxt_go(Speed,Angle).

%%	nxt_go_cm(+Speed,+Distance,+Option).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is the 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.
%	Option is:
%
%		* force

nxt_go_cm(Speed,Distance,force) :-
	nxt_robot(WC,_,_,_,_,_,_,_,_),
	Angle is round(Distance/WC*360),
	nxt_go(Speed,Angle,force).

%%	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 the 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,+Option).
%
%	Moves the robot forward (if Speed is greater than 0)
%	or backward (if Speed is smaller than 0).
%	Speed is the 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.
%	Option is:
%
%		* force

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

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

nxt_go_cm_sec(Distance,Time) :-
	Distance > 0,
	nxt_robot(WC,_,_,_,_,_,_,_,_),
	Angle is round(Distance/WC*360),
	Speed is round(Angle/Time),
	nxt_go(Speed,Angle).

nxt_go_cm_sec(Distance,Time) :-
	Distance < 0,
	nxt_robot(WC,_,_,_,_,_,_,_,_),
	Angle is -round(Distance/WC*360),
	Speed is -round(Angle/Time),
	nxt_go(Speed,Angle).

%%	nxt_go_cm_sec(+Distance,+Time,+Option).
%
%	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.
%	Option is:
%
%		* force

nxt_go_cm_sec(Distance,Time,force) :-
	Distance > 0,
	nxt_robot(WC,_,_,_,_,_,_,_,_),
	Angle is round(Distance/WC*360),
	writeln(Angle),
	Speed is round(Angle/Time),
	writeln(Speed),
	nxt_go(Speed,Angle,force).

nxt_go_cm_sec(Distance,Time,force) :-
	Distance < 0,
	nxt_robot(WC,_,_,_,_,_,_,_,_),
	Angle is -round(Distance/WC*360),
	writeln(Angle),
	Speed is -round(Angle/Time),
	writeln(Speed),
	nxt_go(Speed,Angle,force).

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

nxt_go_in_sec(Distance,Time) :-
	nxt_go_cm_sec(2.54*Distance,Time).

%%	nxt_go_in_sec(+Distance,+Time,+Option).
%
%	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.
%	Option is:
%
%		* force

nxt_go_in_sec(Distance,Time,force) :-
	nxt_go_cm_sec(2.54*Distance,Time,force).

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

nxt_turn(Speed,Angle) :-
	Angle > 0,
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_motor(LM,-Speed,Angle),
	nxt_motor(RM,Speed,Angle).

nxt_turn(Speed,Angle) :-
	Angle < 0,
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	MinusAngle is -Angle,
	nxt_motor(LM,Speed,MinusAngle),
	nxt_motor(RM,-Speed,MinusAngle).

%%	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 the 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(WC,AL,_,_,_,_,_,_,_),
	%Distance is pi*AL*Degrees/360,
	%Angle is Distance/WC*360,
	Angle is round(pi*AL*Degrees/WC),
	nxt_turn(Speed,Angle).

%%	nxt_turn(+Radius,+Degrees,+Time).
%
%	Makes robot turn with specified turning radius (Radius)
%	moving forward (if Degrees is positive) or backward (if negative).
%	Robot turns left (if Radius is positive) or right (if negative).
%	Robot reaches the specified revolution (Degrees) in Time (in seconds).
%	If the robot senses an obstacle, it will stop.

nxt_turn(Radius,Degrees,Time) :-
	nxt_robot(_,_,_,_,_,TouchP,_,_,_),
	nxt_turn(Radius,Degrees,Time,force),
	trigger_create(_,nxt_sensomoto:nxt_touch_sensor(TouchP,1),nxt_stop).

%%	nxt_turn(+Radius,+Degrees,+Time,+Option).
%
%	Makes robot turn with specified turning radius (Radius)
%	moving forward (if Degrees is positive) or backward (if negative).
%	Robot turns left (if Radius is positive) or right (if negative).
%	Robot reaches the specified revolution (Degrees) in Time (in seconds).
%	If the robot hits an obstacle, it will try to push it.
%	Option is:
%
%		* force

nxt_turn(Radius,Degrees,Time,force) :-
	Degrees > 0,
	Radius > 0,
	%LF
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_turn_parameters(Radius,Degrees,Time,InAngle,OutAngle,InSpeed,OutSpeed),
	nxt_motor(LM,InSpeed,InAngle),
	nxt_motor(RM,OutSpeed,OutAngle).

nxt_turn(Radius,Degrees,Time,force) :-
	Degrees > 0,
	Radius < 0,
	%RF
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_turn_parameters(-Radius,Degrees,Time,InAngle,OutAngle,InSpeed,OutSpeed),
	nxt_motor(LM,OutSpeed,OutAngle),
	nxt_motor(RM,InSpeed,InAngle).

nxt_turn(Radius,Degrees,Time,force) :-
	Degrees < 0,
	Radius > 0,
	%LB
	nxt_turn_parameters(Radius,-Degrees,Time,InAngle,OutAngle,InSpeed,OutSpeed),
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_motor(LM,-InSpeed,InAngle),
	nxt_motor(RM,-OutSpeed,OutAngle).

nxt_turn(Radius,Degrees,Time,force) :-
	Degrees < 0,
	Radius < 0,
	%RB
	nxt_turn_parameters(-Radius,-Degrees,Time,InAngle,OutAngle,InSpeed,OutSpeed),
	nxt_robot(_,_,LM,RM,_,_,_,_,_),
	nxt_motor(LM,-OutSpeed,OutAngle),
	nxt_motor(RM,-InSpeed,InAngle).

nxt_turn_parameters(Radius,Degrees,Time,InAngle,OutAngle,InSpeed,OutSpeed) :-
	nxt_robot(WC,AL,_,_,_,_,_,_,_),
	%InDistance is 2*pi*Radius*Degrees/360,
	%OutDistance is 2*pi*(Radius+AL)*Degrees/360,
	%InAngle is InDistance/WC*360,
	%OutAngle is OutDistance/WC*360,
	InAngle is round(2*pi*Radius*Degrees/WC),
	OutAngle is round(2*pi*(Radius+AL)*Degrees/WC),
	InSpeed is round(InAngle/Time),
	OutSpeed is round(OutAngle/Time).