:- reconsult(silnik).

% Obwód koła w cm.
wheelCircumference(17.5).

% Rozstaw kół w cm.
axleLength(15).

% Ruch o zadany kąt z zadaną prędkością obrotową.
% Jesli Speed dodatnie - ruch w przod, ujemne - w tyl.
go(Angle,Speed) :-
	nxtMotor(c,Speed,Angle),
	nxtMotor(b,Speed,Angle).

% Ruch przez zadaną liczbę sekund z zadaną prędkością
% obrotową.
% Jesli Speed dodatnie - ruch w przod, ujemne - w tyl.
goSec(Time,Speed) :-
	nxtMotor(c,Speed,[time,Time]),
	nxtMotor(b,Speed,[time,Time]).

% Ruch o zadaną odległość w cm z zadaną prędkością
% obrotową.
% Jesli Speed dodatnie - ruch w przod, ujemne - w tyl.
goCm(Distance,Speed) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	go(Angle,Speed).

% Ruch o zadaną odległość w calach z zadaną prędkością
% obrotową.
% Jesli Speed dodatnie - ruch w przod, ujemne - w tyl.
goIn(Distance,Speed) :-
	goCm(2.54*Distance,Speed).

% Ruch do przodu o zadaną odległość w cm przez zadaną liczbę sekund.
% Jesli Distance dodatnie - ruch w przod, ujemne - w tyl.
goCmSec(Distance,Time) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	Speed is Angle/Time,
	go(-Angle,Speed).

% Ruch do przodu o zadaną odległość w calach przez zadaną liczbę sekund.
goInSec(Distance,Time) :-
	goCmSec(2.54*Distance,Time).

% Obrót w miejscu z zadanym kątem obrotu kół i zadana ich
% prędkością obrotową. Jesli Angle dodatnie - obrot w prawo, ujemne - w lewo.
turn(Angle,Speed) :-
	Angle > 0,
	nxtMotor(c,Speed,Angle),
	nxtMotor(b,-Speed,Angle).
turn(Angle,Speed) :-
	Angle < 0,
	nxtMotor(c,-Speed,Angle),
	nxtMotor(b,Speed,Angle).

% Obrót w miejscu o zadany kąt z zadaną prędkością obrotową kół.
% Jesli Degrees dodatnie - obrot w prawo, ujemne - w lewo.
turnDegrees(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).
	
% Wykonanie skrętu o zadanym promieniu
% o zadany kąt w zadanej liczbie sekund.
% Jesli Degrees dodatnie - obrot w prawo, ujemne - w lewo.
turn(forward,Radius,Degrees,Time) :-
	Degrees > 0,
	turnParameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	nxtMotor(c,Speed2,Angle2),
	nxtMotor(b,Speed1,Angle1).

turn(forward,Radius,Degrees,Time) :-
	Degrees < 0,
	turnParameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	nxtMotor(b,Speed2,Angle2),
	nxtMotor(c,Speed1,Angle1).

turn(backward,Radius,Degrees,Time) :-
	Degrees > 0,
	turnParameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	nxtMotor(c,-Speed2,Angle2),
	nxtMotor(b,-Speed1,Angle1).

turn(backward,Radius,Degrees,Time) :-
	Degrees < 0,
	turnParameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	nxtMotor(b,-Speed2,Angle2),
	nxtMotor(c,-Speed1,Angle1).

turnParameters(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.