% this file:
%   https://dataswamp.org/~incal/terror-2/t2.pl
%
% ev3-ports: in1    lego-ev3-touch                      sensor0
%            in2
%            in3    lego-ev3-color                      sensor2
%            in4    lego-ev3-ir                         sensor1
%           outA
%           outB    lego-ev3-l-motor   tacho    right   motor1
%           outC    lego-ev3-l-motor   tacho    left    motor0
%           outD    lego-ev3-m-motor   tacho            motor2
%
% DNC: lego-ev3-color  COL-CAL    factory calibration?
%      lego-ev3-ir     IR-CAL     always 0, also factory calibration?
%      lego-ev3-ir     IR-S-ALT   broke?
% http://docs.ev3dev.org/projects/lego-linux-drivers/en/ev3dev-stretch/sensor_data.html

% remote button   IR-REMOTE  IR-REM-ALT   @channel1
% -------------------------------------------------
% (none pressed)      0         384
%    (button up)                270
%        big/top      9         262
%      left up        1         279
%           down      2         292
%     right up        3         322
%           down      4         398
%          1 & 2     10         317
%          1 & 3      5         347
%          1 & 4      6         407
%          2 & 3      7         360
%          2 & 4      8         420
%          3 & 4     11         450
% ------------------------------------------------

% ev3-lego-color:
%   black 1, blue 2, green 3, yellow 4, red 5, white 6, brown 7

:- ['ev3dev-prolog/ev3.pl'].

%% quit SWI-Prolog

e :- halt.
exit :- halt.
h :- halt.

%% robot modes

stop_on_touch :-
    touch(_,T),
    ( T > 0 -> stop_all_motors, true ; false ).

%% drive mode

fall :-
    col_color(_,C),
    ( C == 0 -> rotate_left_90(20) ).

bump :-
    ir_prox(_,V),
    ( V < 50 -> turn_left(20) ).

random_drive(Min,Max) :-
    random_between(Min,Max,L),
    random_between(Min,Max,R),
    set_motors(L,R).

drive :-
    ( not(stop_on_touch) ->
      ignore(fall),
      ignore(bump),
      random_drive(10, 40),
      !,
      drive ).
d :- drive.

%% monitor modes

monitor_color :-
    writeln(""),
    color_write,
    flush_output,
    monitor_color.
mc :- monitor_color.

monitor_ir :-
    writeln(""),
    ir_write,
    flush_output,
    monitor_ir.
mi :- monitor_ir.

monitor_light :-
    writeln(""),
    light_write,
    flush_output,
    monitor_light.
ml :- monitor_light.

monitor_touch :-
    writeln(""),
    touch_write,
    flush_output,
    monitor_touch.
mt :- monitor_touch.

monitor :-
    writeln(""),
    sensors_write,
    flush_output,
    monitor.
m :- monitor.

%% motors

set_motors(L,R) :-
    motor_run('ev3-ports:outC', L),
    motor_run('ev3-ports:outB', R).

%% rotate

sleep_and_stop(T) :-
    sleep(T),
    stop_forward.

rotate_left_time(S,T) :-
    rotate_left(S),
    sleep_and_stop(T).

rotate_right_time(S,T) :-
    rotate_right(S),
    sleep_and_stop(T).

rotate_left_90(S) :-
    rotate_left_time(S, 2.2).  % approximation how long it takes with S 20

rotate_right_90(S) :-
    rotate_right_time(S, 2.2).

rotate_left(S) :-
    set_motors(-S, S).

rotate_right(S) :-
    set_motors(S, -S).

%% turn

turn_left(S) :-
    set_motors(0, S).

turn_right(S) :-
    set_motors(S, 0).

%% left and right

motors_run(S) :-
    set_motors(S, S).

stop_forward :-
    motors_run(0).

%% front

motor_front_run(S) :-
    motor_run('ev3-ports:outD', S).

%% stop all

motors_stop :- stop_all_motors.
st :- motors_stop.
s :- motors_stop.

%% all sensors

sensors_write :-
    touch_write,
    write(" "),
    light_write,
    write(" "),
    ir_write.
write_sensors :- sensors_write.

%% light sensor / lego-ev3-col support

col_ref_raw(P, V) :-
    light_sensor(P),
    mode(P, 'REF-RAW'),
    value(P, 0, V).

%% lego-ev3-ir support

ir_sensor(P) :-
    lego_sensor(P, 'lego-ev3-ir').

ir_prox(P, V) :-
  ir_sensor(P),
  mode(P, 'IR-PROX'),
  value(P, 0, V).

ir_rem_a(P, V) :-
  ir_sensor(P),
  mode(P, 'IR-REM-A'),
  value(P, 0, V).

ir_remote(P, V) :-
  ir_sensor(P),
  mode(P, 'IR-REMOTE'),
  value(P, 0, V).

ir_seek(P, V) :-
  ir_sensor(P),
  mode(P, 'IR-SEEK'),
  value(P, 0, V).

%% lego-ev3-ir

prox_write :-
    ir_prox(_,V), write("{p "), write(V), write("}").
write_prox :- prox_write.

rem_a_write :-
    ir_rem_a(_,V), write("{r-a "), write(V), write("}").
write_rem_a :- rem_a_write.

remote_write :-
    ir_remote(_,V), write("{r "), write(V), write("}").
write_remote :- remote_write.

seek_write :-
    ir_seek(_,V), write("{s "), write(V), write("}").
write_seek :- seek_write.

ir_write :-
    prox_write, write(" "),
    rem_a_write, write(" "),
    remote_write, write(" "),
    seek_write, write(" ").
write_ir :- ir_write.

%% lego-ev3-touch

touch_write :-
    touch(_,V),
    ( V > 0 -> write("T") ; write("t") ).
write_touch :- touch_write.

%% light sensor / lego-ev3-color

ambient_write :-
    col_ambient(_,V), write("a:"), write(V).
write_ambient :- ambient_write.

color_write :-
    col_color(_,V), write("c:"), write(V).
write_color :- color_write.

ref_raw_write :-
    col_ref_raw(_,V), write("rr:"), write(V).
write_ref_raw :- ref_raw_write.

reflect_write :-
    col_reflect(_,V), write("r:"), write(V).
write_reflect :- reflect_write.

rgb_write :-
    col_rgb_raw(_,R,G,B),
    write("("),
    write(R), write(" "),
    write(G), write(" "),
    write(B), write(")").
write_rgb :- rgb_write.

light_write :-
    ambient_write, write(" "),
    color_write, write(" "),
    ref_raw_write, write(" "),
    reflect_write, write(" "),
    rgb_write.
write_light :- light_write.
