:- reconsult(akcje).

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

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

% Ruch do przodu o zadany kąt z zadaną prędkością obrotową.
goForward(Angle,Velocity) :-
	aSpeedForward(Velocity),
	bSpeedForward(Velocity),
	aRotate(Angle),
	bRotate(Angle).

% Ruch do przodu przez zadaną liczbę sekund z zadaną prędkością
% obrotową.
goForwardSec(Time,Velocity) :-
	Angle is Velocity*Time,
	goForward(Angle,Velocity).

% Ruch do przodu o zadaną odległość w cm z zadaną prędkością
% obrotową.
goForwardCm(Distance,Velocity) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	goForward(Angle,Velocity).

% Ruch do przodu o zadaną odległość w calach z zadaną prędkością
% obrotową.
goForwardIn(Distance,Velocity) :-
	goForwardCm(2.54*Distance,Velocity).

% Ruch do przodu o zadaną odległość w cm przez zadaną liczbę sekund.
goForwardCmSec(Distance,Time) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	Velocity is Angle/Time,
	goForward(Angle,Velocity).

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

% Obrót w lewo w miejscu z zadanym kątem obrotu kół i zadana ich
% prędkością obrotową.
turnLeft(Angle,Velocity) :-
	aSpeedBackward(Velocity),
	bSpeedForward(Velocity),
	aRotate(Angle),
	bRotate(Angle).

% Obrót w lewo w miejscu o zadany kąt z zadaną prędkością obrotową kół.
turnLeftDegrees(Degrees,Velocity) :-
	axleLength(AL),
	wheelCircumference(WC),
	%Distance is pi*aL*Degrees/360,
	%Angle is Distance/WC*360,
	Angle is pi*AL*Degrees/WC,
	turnLeft(Angle,Velocity).
	
% Wykonanie skrętu o zadanym promieniu w lewo
% o zadany kąt w zadanej liczbie sekund.
turnLeftForward(Radius,Degrees,Time) :-
	%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,
	Velocity1 is Angle1/Time,
	aSpeedForward(Velocity1),
	Velocity2 is Angle2/Time,
	bSpeedForward(Velocity2),
	aRotate(Angle1),
	bRotate(Angle2).