/** <module> NXT Mindstroms - operation of components.
@author Piotr Hołownia
*/

:- module(nxt_icommand,[
		  nxt_connection_open/0,
		  nxt_connection_close/0,
		  nxt_motor/2, nxt_motor/3,
		  nxt_connect_sensor/1,
		  nxt_touch_sensor/1,
		  nxt_sound_sensor/1,
		  nxt_light_sensor/1,
		  nxt_light_sensor_LED/1,
		  nxt_ultrasonic_sensor/1
		 ]).

:- dynamic nxt_touch_sensor_connected/1,
	nxt_sound_sensor_connected/1,
	nxt_light_sensor_connected/1,
	nxt_ultrasonic_sensor_connected/1.

:- use_module(threads).

%%	nxt_open_connection
%
%	Opens connection via Bluetooth using iCommand.

nxt_connection_open :-
	jpl_call('icommand.nxt.comm.NXTCommand','open',[],_),
	writeln('Connected via Bluetooth using iCommand!').

%%	nxt_close_connection
%
%	Closes connection via Bluetooth using iCommand.

nxt_connection_close :-
	jpl_call('icommand.nxt.comm.NXTCommand','close',[],_),
	writeln('Disconnected!').

%%	motor(+Motor,+Speed).
%%	motor(+Motor,-Speed).
%
%	Reads speed of the specified motor
%	or rotates it at specified speed.

nxt_motor(_,Speed) :-
	nonvar(Speed),
	Speed > 900,
	writeln('Rotational speed is to high!').

nxt_motor(_,Speed) :-
	nonvar(Speed),
	Speed < -900,
	writeln('Rotational speed is to high!').

nxt_motor(_,Speed,_) :-
	nonvar(Speed),
	Speed > 900,
	writeln('Rotational speed is to high!').

nxt_motor(_,Speed,_) :-
	nonvar(Speed),
	Speed < -900,
	writeln('Rotational speed is to high!').

nxt_motor(Motor,Speed) :-
	nonvar(Motor),nonvar(Speed),
	Speed > 0,
	jpl_get('icommand.nxt.Motor',Motor,MotorHandle),
	jpl_call(MotorHandle,'setSpeed',[Speed],_),
	jpl_call(MotorHandle,'forward',[],_).

nxt_motor(Motor,Speed) :-
	nonvar(Motor),nonvar(Speed),
	Speed < 0,
	BackwardSpeed is -Speed,
	jpl_get('icommand.nxt.Motor',Motor,MotorHandle),
	jpl_call(MotorHandle,'setSpeed',[BackwardSpeed],_),
	jpl_call(MotorHandle,'backward',[],_).

nxt_motor(Motor,Speed) :-
	% Returns absolute value of speed.
	nonvar(Motor),var(Speed),
	jpl_get('icommand.nxt.Motor', Motor, MotorHandle),
	jpl_call(MotorHandle, 'getSpeed', [], Speed).

nxt_motor(Motor,0) :-
	nonvar(Motor),
	jpl_get('icommand.nxt.Motor',Motor,MotorHandle),
       	jpl_call(MotorHandle,'stop',[],_),
	jpl_call(MotorHandle,'setSpeed',[0],_).

%%	nxt_motor(+Motor,+Speed,time(+Time)).
%
%	Changes motor speed for a specified time.

nxt_motor(Motor,Speed,time(Time)) :-
	nonvar(Motor),nonvar(Speed),nonvar(Time),
	nxt_motor(Motor,OldSpeed),
	nxt_motor(Motor,Speed),
	timer_create(_,Time,nxt_motor(Motor,OldSpeed)).

%%	nxt_motor(+Motor,+Speed,angle(+Angle).
%%	nxt_motor(+Motor,+Speed,+Angle).
%
%	Rotates motor at specified speed.
%	Forward if Speed is positive, backward if Speed is negative.
%	Motor will stop, when specified revolution (Angle in degrees) is reached.

nxt_motor(Motor,Speed,angle(Angle)) :-
	Speed > 0,
	nonvar(Motor),nonvar(Speed),nonvar(Angle),
	jpl_get('icommand.nxt.Motor',Motor,MotorHandle),
       	jpl_call(MotorHandle,'setSpeed',[Speed],_),
	jpl_call(MotorHandle,'rotate',[Angle,@(true)],_),
	jpl_call(MotorHandle,'setSpeed',[0],_).

nxt_motor(Motor,Speed,angle(Angle)) :-
	Speed < 0,
	nonvar(Motor),nonvar(Speed),nonvar(Angle),
	MinusSpeed is -Speed,
	MinusAngle is -Angle,
	jpl_get('icommand.nxt.Motor',Motor,MotorHandle),
       	jpl_call(MotorHandle,'setSpeed',[MinusSpeed],_),
	jpl_call(MotorHandle,'rotate',[MinusAngle,@(true)],_),
	jpl_call(MotorHandle,'setSpeed',[0],_).

nxt_motor(Motor,Speed,Angle) :- 
	nxt_motor(Motor,Speed,angle(Angle)). 

%%	nxt_connect_sensor(<sensor_type>(+Port))
%
%	Creates java object for sensor connected to Port.
%	It has to be used once before getting sensor reading.
%	<sensor_type> is touch, sound, light, ultrasonic.
%	Keep on mind that you can use this predicate for ultrasonic
%	sensor only when it is really connected to your NXT brick and
%	connection via BT is established.
%	Others must be really connected, only when you are getting
%	readings.

nxt_connect_sensor(touch(Port)) :-
	nonvar(Port),
	retractall(nxt_touch_sensor_connected(_)),
	jpl_get('icommand.nxt.SensorPort',Port,PortHandle),
	jpl_new('icommand.nxt.TouchSensor',[PortHandle],SensorHandle),
	assert(nxt_touch_sensor_connected(SensorHandle)).

nxt_connect_sensor(sound(Port)) :-
	nonvar(Port),
	retractall(nxt_sound_sensor_connected(_)),
	jpl_get('icommand.nxt.SensorPort',Port,PortHandle),
	jpl_new('icommand.nxt.SoundSensor',[PortHandle],SensorHandle),
	assert(nxt_sound_sensor_connected(SensorHandle)).

nxt_connect_sensor(light(Port)) :-
	nonvar(Port),
	retractall(nxt_light_sensor_connected(_)),
	jpl_get('icommand.nxt.SensorPort',Port,PortHandle),
	jpl_new('icommand.nxt.LightSensor',[PortHandle],SensorHandle),
	assert(nxt_light_sensor_connected(SensorHandle)).

nxt_connect_sensor(ultrasonic(Port)) :-
	nonvar(Port),
	retractall(nxt_ultrasonic_sensor_connected(_)),
	jpl_get('icommand.nxt.SensorPort',Port,PortHandle),
	jpl_new('icommand.nxt.UltrasonicSensor',[PortHandle],SensorHandle),
	assert(nxt_ultrasonic_sensor_connected(SensorHandle)).


%%	nxt_touch_sensor(-Value).
%
%	Gets touch sensor reading.

nxt_touch_sensor(_) :-
	not(nxt_touch_sensor_connected(_)),
	writeln('Use nxt_connect_sensor/1 first!').

nxt_touch_sensor(Value) :-
	nxt_touch_sensor_connected(SensorHandle),
	jpl_call(SensorHandle,'isPressed',[],Pressed),
	nxt_touch_sensor_return(Pressed,Value).

nxt_touch_sensor_return(Pressed,Value) :-
	Pressed == @(false),
	Value is 0.

nxt_touch_sensor_return(Pressed,Value) :-
	Pressed == @(true),
	Value is 1.


%%	nxt_sound_sensor(-Value).
%
%	Gets sound sensor reading.

nxt_sound_sensor(_) :-
	not(nxt_sound_sensor_connected(_)),
	writeln('Use nxt_connect_sensor/1 first!').

nxt_sound_sensor(Value) :-
	nxt_sound_sensor_connected(SensorHandle),
	jpl_call(SensorHandle,'getdBA',[],DBA),
	Value is DBA.

%%	nxt_light_sensor(-Value).
%
%	Gets light sensor reading.

nxt_light_sensor(_) :-
	not(nxt_light_sensor_connected(_)),
	writeln('Use nxt_connect_sensor/1 first!').

nxt_light_sensor(Value) :-
	nxt_light_sensor_connected(SensorHandle),
	jpl_call(SensorHandle,'getLightPercent',[],Percent),
	Value is Percent.

%%	nxt_light_sensor_LED(<setting>).
%
%	Sets the LED on if <setting> is activate or off if passivate.

nxt_light_sensor_LED(activate) :-
	nxt_light_sensor_connected(SensorHandle),
	jpl_call(SensorHandle,'activate',[],_).

nxt_light_sensor_LED(passivate) :-
	nxt_light_sensor_connected(SensorHandle),
	jpl_call(SensorHandle,'passivate',[],_).


%%	nxt_ultrasonic_sensor(-Value).
%
%	Gets ultrasonic sensor reading.

nxt_ultrasonic_sensor(_) :-
	not(nxt_ultrasonic_sensor_connected(_)),
	writeln('Use nxt_connect_sensor/1 first!').

nxt_ultrasonic_sensor(Value) :-
	nxt_ultrasonic_sensor_connected(SensorHandle),
	jpl_call(SensorHandle,'getDistance',[],Distance),
	Value is Distance.