open Globals
open Utilities

(* Utilities *)

node wallclock(rstatus : status) returns (time : float)
var cpt : int;
let
  cpt = (if rstatus = Running then 1 else 0) + (0 fby cpt);
  time = timestep *. Mathext.float(cpt);
tel

fun lookup_phase(ph : phase) returns (data : map_data)
let
  data = Map.lookup_pos(ph.ph_pos);
tel

(* Event detection *)

fun light(lights : traflights; ph : phase)
 returns (light_run : bool)
var data : map_data;
let
  data = lookup_phase(ph);
  light_run = ph.ph_vel >. 0.01 and data.tl_required
              and (lights[> data.tl_number <]).tl_color = Red;
tel

fun speed(ph : phase) returns (speed_excess : bool)
let
  speed_excess = ph.ph_vel >. Mathext.float((lookup_phase(ph)).max_speed);
tel

fun exited_aux(pos : position; acc : bool) returns (accnew : bool)
let
  accnew = acc and (Map.lookup_pos(pos)).on_road;
tel

fun exited(ph : phase) returns (exit_road : bool)
var positions : position^2; dummy : phase^2;
let
  (positions, dummy) =
    map<<2>> Vehicle.car_geometry(ph^2, [[-. cDELTA, cB /. 2.0],
                                        [-. cDELTA, -. cB /. 2.0]]);
  exit_road = not fold<<2>> exited_aux(positions, true);
tel

node collision_aux(ph : phase; obst : obstacle; acc : bool)
         returns (accnew : bool)
var ang, angle, dist, distobst : float; close : bool;
let
  (ang, dist) = angle_dist(ph.ph_pos, obst.o_pos);
  angle = (pi /. 180.0) *. abs(normalize(ph.ph_head -. ang));
  accnew = acc or (obst.o_pres and (dist <=. cROBST or close));
  distobst = dist -. cROBST;
  close = Mathext.sin(angle) *. distobst <=. cSB
          and Mathext.cos(angle) *. distobst <=. cSA
          and Mathext.cos(angle) *. distobst >=. -. cSC;
tel

node collision(ph : phase; obstacles : obstacles)
     returns (collision_event : bool)
let
  collision_event = fold<<obstnum>> collision_aux(ph^obstnum,
                                                  obstacles,
                                                  false);
tel

(* The car angle with the road should be in the range -120..120 degree *)
node wrong_dir(ph : phase) returns (wrong : bool)
var data : map_data; norm, error : float; ang : float;
let
  data = lookup_phase(ph);
  ang = ph.ph_head *. (pi /. 180.0);
  norm = Mathext.sqrt(data.dir_x *. data.dir_x +. data.dir_y *. data.dir_y);
  error = data.dir_x *. Mathext.cos(ang) +. data.dir_y *. Mathext.sin(ang);
  wrong = error <. -. 0.5 *. norm;
tel

fun aggregate_events(lightRun, speedExcess, exitRoad, collisionEvent,
                     wrong : bool)
            returns (o : event; itr : interrupt)
let
  o = { lightRun = lightRun;
        speedExcess = speedExcess;
        exitRoad = exitRoad;
        collisionEvent = collisionEvent;
        dirEvent = wrong };
  itr = if exitRoad then Halt else Ok;
tel

node event_detection(sign : sign; ph : phase)
            returns (itr : interrupt; evts : event)
let
  (evts, itr) = aggregate_events (light(sign.si_tlights, ph),
                                  speed(ph),
                                  exited(ph),
                                  collision(ph, sign.si_obstacles),
                                  wrong_dir(ph));
tel

(* Sensor aggregation *)

fun ground_color_detection(ph : phase) returns (road_color : color)
let
  road_color = (lookup_phase(ph)).color;
tel

fun traffic_light_detection(ph : phase; traflights : traflights)
                   returns (tlight_color : color)
let
  tlight_color = Utilities.encode_color(
                   (traflights.[(lookup_phase(ph)).tl_number]
                    default { tl_pos = { x = 0.0; y = 0.0 }; tl_color = Other })
                   .tl_color
                 );
tel

fun obstacles_detection_aux(ph : phase; obst : obstacle; acc : int)
                   returns (accnew : int)
var a, d, d1 : float; sonar : int;
let
  (a, d) = Utilities.angle_dist(ph.ph_pos, obst.o_pos);
  d1 = d -. cROBST;
  sonar = if Utilities.abs(Utilities.normalize(ph.ph_head -. a)) <=. 30.0
              and d1 <=. 100.0 and obst.o_pres
          then Mathext.int(d1)
          else cSONARFAR;
  accnew = Utilities.min_int(sonar, acc);
tel

node obstacle_detection(ph : phase; obstacles : obstacles)
              returns (sonar : int)
let
  sonar = fold<<obstnum>> obstacles_detection_aux(ph^obstnum,
                                                  obstacles,
                                                  cSONARFAR);
tel

node robot_sensors(ph : phase; sign : sign) returns (sens : sensors)
let
  sens = { s_road = ground_color_detection(ph);
           s_front = traffic_light_detection(ph, sign.si_tlights);
           s_sonar = obstacle_detection(ph, sign.si_obstacles) }
tel

(* The city *)

fun traffic_lights_aux(p : param_tlight; time : float) returns (tl : traflight)
var cpt, period : int; light : colorQ;
let
  period = Utilities.max_int(1, p.ptl_amber + p.ptl_green + p.ptl_red);
  cpt = Mathext.modulo(Mathext.int(time) + p.ptl_phase, period);
  if cpt < p.ptl_green then light = Green
  else if cpt < p.ptl_amber + p.ptl_green then light = Amber
  else light = Red
  end
  end;
  tl = { tl_pos = p.ptl_pos; tl_color = light };
tel

fun traffic_lights(time : float) returns (all_lights : traflights)
var lights : param_tlights;
let
  lights = Map.read_traffic_lights();
  all_lights = map<<trafnum>> traffic_lights_aux(lights, time^trafnum);
tel

fun all_obstacles_aux(po : param_obst; time : float) returns (o : obstacle)
let
  o = { o_pos = po.pot_pos;
        o_pres = po.pot_since <=. time and time <=. po.pot_till };
tel

fun all_obstacles(time : float) returns (obstacles : obstacles)
let
  obstacles = map<<obstnum>> all_obstacles_aux(Map.read_obstacles(),
                                               time^obstnum);
tel

node simulate(ph : phase; time : float)
     returns (sign : sign; itr : interrupt; sens : sensors; evt : event)
let
  sign = { si_tlights = traffic_lights(time);
           si_obstacles = all_obstacles(time) };
  (itr, evt) = event_detection(sign, ph);
  sens = robot_sensors(ph, sign);
tel

(* Scoring *)

node scoringA(e : event; rstatus : status) returns (score : int)
var penalty : int;
let
  score = (10000 fby score)
          + if Utilities.rising_edge(rstatus = Arrived) then 1000 else 0
          + if rstatus = Running then penalty else 0;
  penalty = (if Utilities.rising_edge(e.lightRun) then -500 else 0)
            + (if Utilities.rising_edge(e.speedExcess) then -100 else 0)
            + (if e.speedExcess then -2 else 0)
            + (if Utilities.rising_edge(e.exitRoad) then -5000 else 0)
            + (if Utilities.rising_edge(e.dirEvent) then -2000 else 0)
            + (if Utilities.rising_edge(e.collisionEvent) then -500 else 0);
tel

node scoringB(ph : phase) returns (score : int)
var v : float;
let
  v = Utilities.variation(ph.ph_vel >=. 1.0, ph.ph_head, timestep)
      /. (1.0 +. Utilities.uptime(ph.ph_vel, timestep));
  score = Mathext.int(1000.0 -. v);
tel