%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                        %
% 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'                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


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!').

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


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.').

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.').

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.').

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...'),
	iCmdHelper_setLEDState(LightSensor, IsON),
	writeln('Setting finished.').

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.').

%
% 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?
%

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

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

iCmd_motorSpeed(MotorName, Speed):-
	nonvar(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.').

iCmd_motorSpeed(MotorName, Speed):-
	nonvar(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.').

%
% Przy odczycie nie bedzie znany kierunek, lecz tylko predkosc silnika
% Prawdopodobnie jest mozliwosc pobrania predkosci wraz z kierunkiem (predkosc ujemna) 
% W tym celu chyba trzeba skorzystac z klas nizszej warstwy: NXTCommand, OutputState.
%

iCmd_motorSpeed(MotorName, Speed):-
	var(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).

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


%
% HELPERS
%

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.').

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.').