(*
Idea:
- follow a (relatively) large line (~20mm)
- 4 sensor configs are meaninfull:
  * BB straight line 
  * BW left curve
  * WB right curve
  * BB out
- when obstacle distance is less than some bound (here 15cm)
  * stop running, wait a while
  * U-turn
  * start following the line on the other direction
*)
include "code/utils.lus"

(*
uturn task:
- sample odist,
- decides when to U-turn
*)
node uturn(
	odist: real;
	-- from liner
	running: bool;
	ww, wb, bw, bb: bool
) returns (
	buzzer: bool;
	do_turn: bool;
	mlt, mrt: int
);
var
	speed: int;
	obstacle: bool;
	ready, turning, done: bool;
	bb2bw, bw2ww, ww2wb, wb2bb : bool;
let
	obstacle = (odist <= 15.0);
	do_turn = switch(false, obstacle and running, pre done or not running);
	ready = for_at_least(do_turn, 10);
	turning = switch(false, edge(ready), done or not do_turn);

	-- suppose we start with bb
	bb2bw = switch(false, edge(turning), bw or not do_turn);
	bw2ww = switch(false, xedge(not bb2bw) and ready, ww or not do_turn);
	ww2wb = switch(false, xedge(not bw2ww) and ready, wb or not do_turn);
	wb2bb = switch(false, xedge(not ww2wb) and ready, bb or not do_turn);
	--done = switch(false, (false -> pre wb2bb) and bb, not do_turn);
	done = switch(false, (false -> pre ww2wb) and wb, not do_turn);
	speed = 80;
	(mlt, mrt) = if turning then (speed, -speed) else (0,0);
	buzzer = running and obstacle;
tel

(*
Samples line sensors,
Delegates the U-turn decision (and realization) to uturn
*)
node liner(
	k1: bool; sensor_left, sensor_right: bool;
	-- from uturn
	do_turn: bool;
	ml_turn, mr_turn: int
) returns (
	motor_left, motor_right: int;
	red_light, yellow_light, green_light: bool;
	-- to uturn
	running: bool;
	ww, wb, bw, bb: bool
);
var 
	onoff, ready: bool;
	straight, lcurve, rcurve: bool;
	reset: bool;
	ml, mr: int;
	Hc, Lc, Oc: int;
let
	-- decides when running
	onoff = for_exactly(k1,3);
	ready = (false -> pre ready) xor onoff;
	reset = true -> not ready;
	running = for_at_least(ready, 30);
	-- input sensor digest
   ww = not (sensor_left) and not (sensor_right);
   wb = not (sensor_left) and     (sensor_right);
   bw =     (sensor_left) and not (sensor_right);
   bb =     (sensor_left) and     (sensor_right);
	-- left/right curve detection
   lcurve = switch(false, bw and running, bb or reset);
   rcurve = switch(false, wb and running, bb or reset);
   straight = not (lcurve or rcurve);
	
	Hc = 90; Lc = 70; Oc = 0;
   (ml, mr) =
      if (not running) then (0,0)
		else if do_turn then (ml_turn, mr_turn)
      else if rcurve then (Hc, 0)
      else if lcurve then(0, Hc)
      else (Hc, Hc);

	--(motor_left, motor_right) = (mfilter(ml), mfilter(mr));
	(motor_left, motor_right) = (ml, mr);

	red_light = lcurve;
	yellow_light = running and not (lcurve or rcurve);
	green_light = rcurve;
tel

(*
Main controler, combines (and connect) liner and uturn
*)
node control(
	k1: bool; sensor_left, sensor_right: bool;
	odist: real
) returns (
	motor_left, motor_right: int;
	buzzer: bool;
	red_light, yellow_light, green_light: bool
);
var
	do_turn: bool;
	mlt, mrt: int;
	running, ww, wb, bw, bb: bool;
let
	(buzzer, do_turn, mlt, mrt) = uturn(odist, running, ww, wb, bw, bb);
	(motor_left, motor_right,
	 red_light, yellow_light, green_light,
	 running,  ww, wb, bw, bb) = liner(k1, sensor_left, sensor_right, do_turn, mlt, mrt);
tel