%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
% Author: Pawel Gutowski pawel.gutowski@gmail.com                        %
% University of Science and Technology AGH in Cracow                     %
% 2008                                                                   %
%                                                                        %
%                                                                        %
% PortName  = 'S1' or                                                    % 
%             'S2' or                                                    %
%             'S3' or                                                    %
%             'S4'                                                       %
%                                                                        %
% MotorName = 'A'  or                                                    %
%             'B'  or                                                    %
%             'C'                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
:- module(nxt_iCmd,[
			nxt_iCmd_open/0,
			nxt_iCmd_close/0,
			nxt_iCmd_distance/2,
			nxt_iCmd_pressed/2,
			nxt_iCmd_lightPercentage/2,
			nxt_iCmd_LED/2,
			nxt_iCmd_loudness/2,
			nxt_iCmd_batteryMilliVolt/1,
			nxt_iCmd_playSoundFile/2,
			nxt_iCmd_stopPlayingSound/0,
			nxt_iCmd_playTone/2,
		% motors
			nxt_iCmd_stopMotor/1,
			nxt_iCmd_setMotorSpeed/2,
			nxt_iCmd_getMotorSpeed/2,
			nxt_iCmd_rotateMotorToAngle/3,
			nxt_iCmd_getMotorAngle/2,
			nxt_iCmd_resetMotorAngle/1
		   ]).

nxt_iCmd_open:-
	writeln('Opening connection...'),
	jpl_call( 'icommand.nxt.comm.NXTCommand' , 'open', [], _),
	writeln('Configuring connection...'),
	jpl_call( 'icommand.nxt.comm.NXTCommand' , 'setVerify', [@(true)], _),
	writeln('Connection established!').

nxt_iCmd_close:-
	writeln('Closing connection...'),
	jpl_call( 'icommand.nxt.comm.NXTCommand' , 'close', [], _),
	writeln('Connection is closed.').


nxt_iCmd_distance(PortName, Dist):-
	writeln('Getting port handle...'),
	jpl_get('icommand.nxt.SensorPort', PortName, PortHandle),
	writeln('Creating ultrasonic sensor object...'),
	jpl_new('icommand.nxt.UltrasonicSensor', [PortHandle], UltraSonic),
	writeln('Reading distance from ultrasonic sensor...'),
	jpl_call( UltraSonic, 'getDistance', [], Dist),
	writeln('Reading finished.').

nxt_iCmd_pressed(PortName, Pressed):-
	writeln('Getting port handle...'),
	jpl_get('icommand.nxt.SensorPort', PortName, PortHandle),
	writeln('Creating touch sensor object...'),
	jpl_new('icommand.nxt.TouchSensor', [PortHandle], TouchSensor),
	writeln('Reading touch sensor state...'),
	jpl_call( TouchSensor, 'isPressed', [], Pressed),
	writeln('Reading finished.').

nxt_iCmd_lightPercentage(PortName, LightPercentage):-
	writeln('Getting port handle...'),
	jpl_get('icommand.nxt.SensorPort', PortName, PortHandle),
	writeln('Creating light sensor object...'),
	jpl_new('icommand.nxt.LightSensor', [PortHandle], LightSensor),
	writeln('Reading light percentage...'),
	jpl_call( LightSensor, 'getLightPercent', [], LightPercentage),
	writeln('Reading finished.').

nxt_iCmd_LED(PortName, IsON):-
	writeln('Getting port handle...'),
	jpl_get('icommand.nxt.SensorPort', PortName, PortHandle),
	writeln('Creating light sensor object...'),
	jpl_new('icommand.nxt.LightSensor', [PortHandle], LightSensor),
	writeln('Setting LED state...'),
	nxt_iCmdHelper_setLEDState(LightSensor, IsON),
	writeln('Setting finished.').

nxt_iCmd_loudness(PortName, Loudness):-
	writeln('Getting port handle...'),
	jpl_get('icommand.nxt.SensorPort', PortName, PortHandle),
	writeln('Creating Sound sensor object...'),
	jpl_new('icommand.nxt.SoundSensor', [PortHandle], SoundSensor),
	writeln('Reading loudness...'),
	jpl_call( SoundSensor, 'getdBA', [], Loudness),
	writeln('Reading finished.').

%
% Funkcje "dodatkowe"
%
%
nxt_iCmd_batteryMilliVolt(MilliVolt):-
	writeln('Checking battery millivoltage...'),
	jpl_call( 'icommand.nxt.Battery' , 'getVoltageMilliVolt', [], MilliVolt).


%
% Np: iCmd_playSoundFile('Good Job.rso', @(true)).	
%

nxt_iCmd_playSoundFile(SoundName, Repeat):-
	writeln('Starting sound playing...'),
	jpl_call( 'icommand.nxt.Sound' , 'playSoundFile', [SoundName, Repeat], _).

nxt_iCmd_stopPlayingSound:-
	writeln('Stopping sound playing...'),
	jpl_call( 'icommand.nxt.Sound' , 'stopSoundPlayback', [], _).

%
% Frequency [Hz]
% Duration  [ms]
%

nxt_iCmd_playTone(Frequency, Duration):-
	writeln('Starting tone playing...'),
	jpl_call( 'icommand.nxt.Sound' , 'playTone', [Frequency, Duration], _).



%
% Funkcja "stop()" działa - z kodu źródłowego iCommand wynika, że jest to nic innego
% jak ustawienie prędkości (mocy?) silnika na 0.
% Czy nie istenieje ryzyko spalenia silnika?
%

nxt_iCmd_stopMotor(MotorName):-
	jpl_get('icommand.nxt.Motor', MotorName, MotorHandle),
	jpl_call(MotorHandle, 'stop', [], _).

%
% Speed [degrees/sec].
% Kierunek obrotu okresla znak +/- predkosci.
%

nxt_iCmd_setMotorSpeed(MotorName, Speed):-
	writeln('Setting motor speed...'),
	Speed >= 0,
	writeln('Setting forward motor speed...'),
	jpl_get('icommand.nxt.Motor', MotorName, MotorHandle),
	jpl_call( MotorHandle, 'setSpeed', [Speed], _),
	jpl_call( MotorHandle, 'forward',  [], _),
	writeln('Motor is now moving.').

nxt_iCmd_setMotorSpeed(MotorName, Speed):-
	writeln('Setting motor speed...'), 
	Speed < 0,
	writeln('Setting backward motor speed...'),
	jpl_get('icommand.nxt.Motor', MotorName, MotorHandle),
	writeln('Counting module of Speed...'),
	NewSpeed is -Speed,
	write('Module of speed is: '),
	writeln(NewSpeed),
	writeln('Setting motor speed...'),
	jpl_call( MotorHandle, 'setSpeed', [NewSpeed], _),
	writeln('Starting the backward movement of motor...'),
	jpl_call( MotorHandle, 'backward',  [], _),
	writeln('Motor is now moving.').

%
% UWAGA: 
% Przy odczycie nie bedzie znany kierunek, lecz tylko predkosc silnika!
% iCommand nie daje możlwiości odczytania kierunku obrotu
%

nxt_iCmd_getMotorSpeed(MotorName, Speed):-
	writeln('Reading actual motor speed...'),
	jpl_get('icommand.nxt.Motor', MotorName, MotorHandle),
	jpl_call(MotorHandle, 'isMoving', [], IsMoving),
	IsMoving == @(true),
	jpl_call(MotorHandle, 'getSpeed', [], Speed).

nxt_iCmd_getMotorSpeed(MotorName, Speed):-
	jpl_get('icommand.nxt.Motor', MotorName, MotorHandle),
	jpl_call(MotorHandle, 'isMoving', [], IsMoving),
	IsMoving == @(false),
	Speed = 0.

nxt_iCmd_rotateMotorToAngle(MotorName, Angle, Speed):-
	writeln('Rotating motor to...'),
	jpl_get('icommand.nxt.Motor', MotorName, MotorHandle),
	jpl_call( MotorHandle, 'setSpeed', [Speed], _),
	jpl_call( MotorHandle, 'rotateTo',  [Angle, @(true)], _),
	writeln('Motor is now moving.').

nxt_iCmd_getMotorAngle(MotorName, Angle):-
	writeln('Reading actual tacho count...'),
	jpl_get('icommand.nxt.Motor', MotorName, MotorHandle),
	jpl_call(MotorHandle, 'getTachoCount', [], Angle).

nxt_iCmd_resetMotorAngle(MotorName):-
	writeln('Resetting tacho count...'),
	jpl_get('icommand.nxt.Motor', MotorName, MotorHandle),
	jpl_call(MotorHandle, 'resetTachoCount', [], _).


%
% HELPERS
%

nxt_iCmdHelper_setLEDState(LightSensor, IsON):-
	writeln('Checking if LED should be turned ON...'),
	IsON == @(true),
	writeln('Yes, LED should be turned ON - turning ON...'),
	jpl_call( LightSensor, 'activate', [], _),
	writeln('LED has been turned ON.').

nxt_iCmdHelper_setLEDState(LightSensor, IsON):-
	writeln('Checking if LED should be turned ON...'),
	IsON == @(false),
	writeln('No, LED should be turned OFF - turning OFF...'),
	jpl_call( LightSensor, 'passivate', [], _),
	writeln('LED has been turned OFF.').