:- module(nxt_movement,[
		go/2, go/3,
		go_sec/2,
		go_cm/2,
		go_in/2,
		go_cm_sec/2,
		go_in_sec/2,
		turn/2, turn/4,
		turn_degrees/2
	       ]).
		
	
:- use_module(nxt_actions).
:- use_module(nxt_components).

% Obwod kola w cm.
wheelCircumference(17.5).

% Rozstaw kol w cm.
axleLength(15).

% Ruch o zadany kat z zadana predkoscia obrotowa.
% Jesli Speed dodatnie - ruch w przod, ujemne - w tyl.
% Ruch odbywa sie do napotkania przeszkody.
go(Angle,Speed) :-
	go(Angle,Speed,force),
	create_trigger(T),
	trigger_fired(T,touch_sensor(pressed),[
					       nxt_motor(c,0),
					       nxt_motor(b,0)
					      ]).
% Ruch odbywa się bez względu na przeszkody.
go(Angle,Speed,force) :-
	motor(c,Speed,Angle),
	motor(b,Speed,Angle).

% Ruch przez zadana liczbe sekund z zadana predkoscia
% obrotowa.
% Jesli Speed dodatnie - ruch w przod, ujemne - w tyl.
go_sec(Time,Speed) :-
	motor(c,Speed,time(Time)),
	motor(b,Speed,time(Time)).

% Ruch o zadana odleglosc w cm z zadana predkoscia
% obrotowa.
% Jesli Speed dodatnie - ruch w przod, ujemne - w tyl.
go_cm(Distance,Speed) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	go(Angle,Speed).

% Ruch o zadana odleglosc w calach z zadana predkoscia
% obrotowa.
% Jesli Speed dodatnie - ruch w przod, ujemne - w tyl.
go_in(Distance,Speed) :-
	go_cm(2.54*Distance,Speed).

% Ruch do przodu o zadana odleglosc w cm przez zadana liczbe sekund.
% Jesli Distance dodatnie - ruch w przod, ujemne - w tyl.
go_cm_sec(Distance,Time) :-
	wheelCircumference(WC),
	Angle is Distance/WC*360,
	Speed is Angle/Time,
	go(-Angle,Speed).

% Ruch do przodu o zadana odleglosc w calach przez zadana liczbe sekund.
go_in_sec(Distance,Time) :-
	go_cm_sec(2.54*Distance,Time).

% Obrot w miejscu z zadanym katem obrotu kol i zadana ich
% predkoscia obrotowa. Jesli Angle dodatnie - obrot w prawo, ujemne - w lewo.
turn(Angle,Speed) :-
	Angle > 0,
	motor(c,Speed,Angle),
	motor(b,-Speed,Angle).
turn(Angle,Speed) :-
	Angle < 0,
	motor(c,-Speed,Angle),
	motor(b,Speed,Angle).

% Obrot w miejscu o zadany kat z zadana predkoscia obrotowa kol.
% Jesli Degrees dodatnie - obrot w prawo, ujemne - w lewo.
turn_degrees(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 skretu o zadanym promieniu
% o zadany kat w zadanej liczbie sekund.
% Jesli Degrees dodatnie - obrot w prawo, ujemne - w lewo.
turn(forward,Radius,Degrees,Time) :-
	Degrees > 0,
	turn_parameters(Radius,Degrees,Time,Angle1,Angle2,Speed1,Speed2),
	motor(c,Speed2,Angle2),
	motor(b,Speed1,Angle1).

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

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

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

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