[chore] projet structure

src/buffer.c

@@ -0,0 +1,61 @@
+#include "buffer.h"
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define max(a, b) ((a) <= (b) ? (a) : (b))
+
+void *malloc_checked(size_t size) {
+  void *result = malloc(size);
+  if (!result) {
+    perror("malloc()");
+    exit(EXIT_FAILURE);
+  }
+  return result;
+}
+
+char *strdup_checked(const char *s) {
+  char *result = strdup(s);
+  if (!result) {
+    perror("strdup()");
+    exit(EXIT_FAILURE);
+  }
+  return result;
+}
+
+buffer_t *buffer_alloc(size_t initial_size) {
+  buffer_t *buff = malloc_checked(sizeof *buff);
+  buff->data = malloc_checked(initial_size * sizeof *buff->data);
+  buff->size = initial_size;
+  buff->occupancy = 0;
+  return buff;
+}
+
+void buffer_free(buffer_t *buffer) {
+  assert (buffer);
+
+  free(buffer->data);
+  free(buffer);
+}
+
+void buffer_resize(buffer_t *buff, size_t new_size) {
+  assert (buff);
+  assert (new_size >= buff->size);
+
+  unsigned char *new_data = malloc_checked(new_size);
+  memcpy(new_data, buff->data, buff->occupancy);
+  free(buff->data);
+  buff->data = new_data;
+  buff->size = new_size;
+}
+
+void buffer_write(buffer_t *buff, void *data, size_t data_size) {
+  assert (buff);
+  if (buff->occupancy + data_size > buff->size)
+    buffer_resize(buff, max(buff->size + data_size, 2 * buff->size));
+  memcpy(buff->data + buff->occupancy, data, data_size);
+  buff->occupancy += data_size;
+}

src/buffer.h

@@ -0,0 +1,27 @@
+#ifndef BUFFER_H
+#define BUFFER_H
+
+#include <sys/types.h>
+
+/* A simple type of append-only buffers. */
+
+typedef struct buffer {
+  unsigned char *data;
+  size_t size;
+  size_t occupancy;
+} buffer_t;
+
+void *malloc_checked(size_t size);
+char *strdup_checked(const char *);
+
+buffer_t *buffer_alloc(size_t initial_size);
+void buffer_free(buffer_t *buff);
+
+void buffer_write(buffer_t *buff, void *data, size_t data_size);
+
+#define buffer_foreach(ty, var, buffer)                          \
+  for (ty *var = (ty *)buffer->data;                             \
+       var < (ty *)(buffer->data + buffer->occupancy);           \
+       var++)
+
+#endif  /* BUFFER_H */

src/challenge.ept

@@ -0,0 +1,21 @@
+open Globals
+
+node the_challenge(initial_ph : phase; top : bool)
+          returns (ph : phase; sta : status; sign : sign; evt : event;
+                   scoreA, scoreB : int; time : float)
+var itr : interrupt; ini_sens, sens : sensors;
+let
+  ini_sens = { s_road = { red = 128; green = 128; blue = 128 };
+               s_front = { red = 128; green = 128; blue = 128 };
+               s_sonar = cSONARFAR };
+  (ph, sta) = Vehicle.simulate(Map.read_itinerary(),
+                               ini_sens fby sens,
+                               Ok fby itr,
+                               initial_ph,
+                               top);
+  (sign, itr, sens, evt) = City.simulate(ph, time);
+  () = Map.soundEffects(Utilities.event_edge(evt), sta);
+  scoreA = City.scoringA(evt, sta);
+  time = City.wallclock(sta);
+  scoreB = City.scoringB(ph);
+tel

src/city.ept

@@ -0,0 +1,212 @@
+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
+
+node wrong_dir(ph : phase) returns (wrong : bool)
+var data : map_data; error : float; ang : float;
+let
+  data = lookup_phase(ph);
+  ang = ph.ph_head *. (pi /. 180.0);
+  error = data.dir_x *. Mathext.cos(ang) +. data.dir_y *. Mathext.sin(ang);
+  wrong = error <. -. 0.5;
+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; collision_count : 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 collision_count = 0 then -500 else 0)
+            + (if collision_count < 0 then 10 else 0);
+  collision_count = Utilities.countdown(not e.collisionEvent, 20);
+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

src/control.ept

@@ -0,0 +1,9 @@
+open Globals
+
+
+node controller(sens : sensors; iti : itielts)
+       returns (rspeed : wheels; arriving : bool)
+let
+  rspeed = { left = 00.0; right = 100.0 };
+  arriving = false;
+tel

src/cutils.c

@@ -0,0 +1,88 @@
+/* This file is part of SyncContest.
+   Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */
+
+#include "cutils.h"
+
+#include <assert.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+log_verbosity_level level = LOG_INFO;
+FILE *f = NULL;
+char *filename = NULL;
+
+void log_set_verbosity_level(log_verbosity_level l) {
+  level = l;
+}
+
+void log_message_v(log_verbosity_level msg_level, const char *fmt, va_list va) {
+  va_list vb;
+  FILE *out = msg_level == LOG_INFO ? stdout : stderr;
+
+  if (msg_level > level)
+    return;
+
+  va_copy(vb, va);
+  if (f != NULL) {
+    vfprintf(f, fmt, va);
+  }
+
+  vfprintf(out, fmt, vb);
+  fflush(out);
+}
+
+void log_message(log_verbosity_level msg_level, const char *fmt, ...) {
+  va_list va;
+  va_start(va, fmt);
+  log_message_v(msg_level, fmt, va);
+  va_end(va);
+}
+
+void log_fatal(const char *fmt, ...) {
+  va_list va;
+  va_start(va, fmt);
+  log_message_v(LOG_FATAL, fmt, va);
+  va_end(va);
+  exit(EXIT_FAILURE);
+}
+
+void log_info(const char *fmt, ...) {
+  va_list va;
+  va_start(va, fmt);
+  log_message_v(LOG_INFO, fmt, va);
+  va_end(va);
+}
+
+void log_debug(const char *fmt, ...) {
+  va_list va;
+  va_start(va, fmt);
+  log_message_v(LOG_DEBUG, fmt, va);
+  va_end(va);
+}
+
+void log_init(const char *fn) {
+  if (!fn) {
+    log_info("[log] initializing, not saving to file\n");
+    return;
+  }
+
+  filename = strdup(fn);
+  assert (filename);
+  f = fopen(filename, "w");
+  if (!f)
+    log_fatal("[log] could not open log file %s (fopen)", filename);
+
+  log_info("[log] logging to %s\n", filename);
+}
+
+void log_shutdown() {
+  if (f) {
+    log_info("[log] shutting down, closing %s\n", filename);
+    fclose(f);
+    free(filename);
+  } else {
+    log_info("[log] shutting down\n");
+  }
+}

src/cutils.h

@@ -0,0 +1,30 @@
+/* This file is part of SyncContest.
+   Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */
+
+#ifndef CUTILS_H
+#define CUTILS_H
+
+#include <stdarg.h>
+
+typedef enum {
+  LOG_FATAL = 0,
+  LOG_INFO  = 1,
+  LOG_DEBUG = 2,
+} log_verbosity_level;
+
+/* Calling `log_init(fn)` initializes the logging subsystem, asking it to save
+   log messages to the file `fn`. This pointer may be NULL, in which case the
+   messages are not saved. */
+void log_init(const char *filename);
+void log_shutdown();
+
+void log_set_verbosity_level(log_verbosity_level level);
+
+void log_message_v(log_verbosity_level level, const char *fmt, va_list);
+void log_message(log_verbosity_level level, const char *fmt, ...);
+
+void log_fatal(const char *fmt, ...);
+void log_info(const char *fmt, ...);
+void log_debug(const char *fmt, ...);
+
+#endif  /* CUTILS_H */

src/debug.c

@@ -0,0 +1,39 @@
+#include "debug.h"
+
+#include "cutils.h"
+
+void Debug__dbg_step(char *msg, Debug__dbg_out *o) {
+  log_info("%s\n", msg);
+}
+
+void Debug__dbg_bool_step(char *msg, bool x, Debug__dbg_bool_out *o) {
+  log_info("%s %d\n", msg, x);
+}
+
+void Debug__dbg_int_step(char *msg, int x, Debug__dbg_int_out *o) {
+  log_info("%s %d\n", msg, x);
+}
+
+void Debug__dbg_float_step(char *msg, float x, Debug__dbg_float_out *o) {
+  log_info("%s %f\n", msg, x);
+}
+
+void Debug__d_init_step(Debug__d_init_out *o) {
+  /* Empty by design */
+}
+
+void Debug__d_string_step(Debug__world _w, char *s, Debug__d_string_out *o) {
+  log_info("%s", s);
+}
+
+void Debug__d_bool_step(Debug__world _w, bool b, Debug__d_bool_out *o) {
+  log_info("%d", b);
+}
+
+void Debug__d_int_step(Debug__world _w, int i, Debug__d_int_out *o) {
+  log_info("%d", i);
+}
+
+void Debug__d_float_step(Debug__world _w, float f, Debug__d_float_out *o) {
+  log_info("%f", f);
+}

src/debug.epi

@@ -0,0 +1,11 @@
+external fun dbg(msg : string) returns ()
+external fun dbg_bool(msg : string; x : bool) returns ()
+external fun dbg_int(msg : string; x : int) returns ()
+external fun dbg_float(msg : string; x : float) returns ()
+
+type world
+external fun   d_init()                  returns (n : world)
+external fun d_string(w : world; string) returns (n : world)
+external fun   d_bool(w : world;   bool) returns (n : world)
+external fun    d_int(w : world;    int) returns (n : world)
+external fun  d_float(w : world;  float) returns (n : world)

src/debug.h

@@ -0,0 +1,23 @@
+#ifndef DEBUG_H
+#define DEBUG_H
+
+#include "stdbool.h"
+#include "assert.h"
+#include "pervasives.h"
+
+#include "hept_ffi.h"
+
+DECLARE_HEPT_FUN(Debug, dbg, (char *),);
+DECLARE_HEPT_FUN(Debug, dbg_bool, (char *, bool),);
+DECLARE_HEPT_FUN(Debug, dbg_int, (char *, int),);
+DECLARE_HEPT_FUN(Debug, dbg_float, (char *, float),);
+
+typedef struct { } Debug__world;
+
+DECLARE_HEPT_FUN_NULLARY(Debug, d_init, Debug__world n);
+DECLARE_HEPT_FUN(Debug, d_string, (Debug__world, char *), Debug__world n);
+DECLARE_HEPT_FUN(Debug, d_bool, (Debug__world, bool), Debug__world n);
+DECLARE_HEPT_FUN(Debug, d_int, (Debug__world, int), Debug__world n);
+DECLARE_HEPT_FUN(Debug, d_float, (Debug__world, float), Debug__world n);
+
+#endif  /* DEBUG_H */

src/debug_types.h

@@ -0,0 +1,4 @@
+#ifndef DEBUG_TYPES_H
+#define DEBUG_TYPES_H
+
+#endif  /* DEBUG_TYPES_H */

src/globals.ept

@@ -0,0 +1,238 @@
+(* Types *)
+
+const obstnum : int = 10
+
+const trafnum : int = 6
+
+const itinum : int = 50
+
+type color = { red : int; green : int; blue : int }
+
+type colorQ = Red | Green | Amber | Other
+
+type position = { x : float; y : float }
+
+type interrupt = Wait | Halt | Ok
+
+type status = Preparing | Running | Arrived | Stopped
+
+type event = {
+  lightRun : bool;
+  speedExcess : bool;
+  exitRoad : bool;
+  collisionEvent : bool;
+  dirEvent : bool
+}
+
+type obstacle = { o_pos : position; o_pres : bool }
+
+type obstacles = obstacle^obstnum
+
+type traflight = { tl_pos : position; tl_color : colorQ }
+
+type traflights = traflight^trafnum
+
+type sign = { si_tlights : traflights; si_obstacles : obstacles }
+
+type phase = { ph_pos : position; ph_vel : float; ph_head : float }
+
+type sensors = { s_road : color; s_front : color; s_sonar : int }
+
+type action = Go | Turn | Stop
+
+type itielt = { act : action; param : float }
+
+type itielts = itielt^itinum
+
+type wheels = { left : float; right : float }
+
+(* information on one point on the map *)
+type map_data = { on_road : bool; (* is it on the road? *)
+                  color : color; (* road color *)
+                  max_speed : int; (* maximum allowed speed *)
+                  tl_number : int; (* which traffic light is visible? *)
+                  tl_required : bool; (* should we stop on red traffic light? *)
+                  dir_x : float; (* directing vector, abscissa *)
+                  dir_y : float  (* directing vector, ordinate *) }
+
+(* information on one obstacle *)
+type param_obst = { pot_pos : position; (* position of the obstacle *)
+                    pot_since : float;  (* appearance time *)
+                    pot_till : float    (* disappearance time *) }
+
+type param_obsts = param_obst^obstnum
+
+type param_tlight = { ptl_pos : position; (* traffic light position *)
+                      ptl_green : int;    (* time on green (s) *)
+                      ptl_amber : int;    (* time on amber (s) *)
+                      ptl_red : int;      (* time on red (s) *)
+                      ptl_phase : int     (* initial phase (s) *) }
+
+type param_tlights = param_tlight^trafnum
+
+(* Constants *)
+
+const pi : float = 3.1415926
+
+const red : color = { red = 255; green = 0; blue = 0 }
+
+const green : color = { red = 0; green = 255; blue = 0 }
+
+const blue : color = { red = 0; green = 0; blue = 255 }
+
+const amber : color = { red = 255; green = 191; blue = 0 }
+
+const cyan : color = { red = 0; green = 255; blue = 255 }
+
+const gray : color = { red = 128; green = 128; blue = 128 }
+
+const magenta : color = { red = 255; green = 0; blue = 255 }
+
+const cB : float = 6.0
+
+const cD : float = 5.5
+
+const cDELTA : float = 0.0
+
+const cMAXWHEEL : float = 500.0
+
+const cMAXSPEED : float = 25.0
+
+const cSONARFAR : int = 1000
+
+const cROBST : float = 1.5
+
+const cSA : float = 3.0
+
+const cSB : float = 3.0
+
+const cSC : float = 7.0
+
+const idlew : wheels = { left = 0.0; right = 0.0 }
+
+const timestep : float = 0.01 (* in seconds *)
+
+(* Debugging *)
+
+(* TODO: define d_position *)
+
+fun dbg_position(msg : string; p : position) returns ()
+var w0, w1, w2, w3, w4, w5 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  w1 = Debug.d_string(w0, "{ x = ");
+  w2 = Debug.d_float(w1, p.x);
+  w3 = Debug.d_string(w2, "; y = ");
+  w4 = Debug.d_float(w3, p.y);
+  w5 = Debug.d_string(w4, " }\n");
+tel
+
+fun dbg_phase(msg : string; ph : phase) returns ()
+var w0, w1, w2, w3, w4, w5, w6, w7, w8, w9 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  w1 = Debug.d_string(w0, "{ x = ");
+  w2 = Debug.d_float(w1, ph.ph_pos.x);
+  w3 = Debug.d_string(w2, "; y = ");
+  w4 = Debug.d_float(w3, ph.ph_pos.y);
+  w5 = Debug.d_string(w4, "; head = ");
+  w6 = Debug.d_float(w5, ph.ph_head);
+  w7 = Debug.d_string(w6, "; vel = ");
+  w8 = Debug.d_float(w7, ph.ph_vel);
+  w9 = Debug.d_string(w8, " }\n");
+tel
+
+fun dbg_interrupt(msg : string; itr : interrupt) returns ()
+var w0, w1, w2 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  switch itr
+  | Wait do w1 = Debug.d_string(w0, "Wait");
+  | Halt do w1 = Debug.d_string(w0, "Halt");
+  | Ok do w1 = Debug.d_string(w0, "Ok");
+  end;
+  w2 = Debug.d_string(w1, "\n");
+tel
+
+fun dbg_status(msg : string; sta : status) returns ()
+var w0, w1, w2 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  switch sta
+  | Preparing do w1 = Debug.d_string(w0, "Preparing");
+  | Running do w1 = Debug.d_string(w0, "Running");
+  | Arrived do w1 = Debug.d_string(w0, "Arrived");
+  | Stopped do w1 = Debug.d_string(w0, "Stopped");
+  end;
+  w2 = Debug.d_string(w1, "\n");
+tel
+
+fun dbg_wheels(msg : string; wh : wheels) returns ()
+var w0, w1, w2, w3, w4, w5 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  w1 = Debug.d_string(w0, "{ left = ");
+  w2 = Debug.d_float(w1, wh.left);
+  w3 = Debug.d_string(w2, "; right = ");
+  w4 = Debug.d_float(w3, wh.right);
+  w5 = Debug.d_string(w4, " }\n");
+tel
+
+fun dbg_action(msg : string; act : action) returns ()
+var w0, w1, w2 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  switch act
+  | Go do w1 = Debug.d_string(w0, "Go");
+  | Turn do w1 = Debug.d_string(w0, "Turn");
+  | Stop do w1 = Debug.d_string(w0, "Stop");
+  end;
+  w2 = Debug.d_string(w1, "\n");
+tel
+
+fun d_color(w0 : Debug.world; c : color)
+   returns (w7 : Debug.world)
+var w1, w2, w3, w4, w5, w6 : Debug.world;
+let
+  w1 = Debug.d_string(w0, "{ red = ");
+  w2 = Debug.d_int(w1, c.red);
+  w3 = Debug.d_string(w2, "; green = ");
+  w4 = Debug.d_int(w3, c.green);
+  w5 = Debug.d_string(w4, "; blue = ");
+  w6 = Debug.d_int(w5, c.blue);
+  w7 = Debug.d_string(w6, " }");
+tel
+
+fun dbg_color(msg : string; c : color) returns ()
+var w0, w1, w2 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  w1 = d_color(w0, c);
+  w2 = Debug.d_string(w1, "\n");
+tel
+
+fun dbg_colorq(msg : string; c : colorQ) returns ()
+var w0, w1, w2 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  switch c
+  | Red do w1 = Debug.d_string(w0, "Red");
+  | Green do w1 = Debug.d_string(w0, "Green");
+  | Amber do w1 = Debug.d_string(w0, "Amber");
+  | Other do w1 = Debug.d_string(w0, "Other");
+  end;
+  w2 = Debug.d_string(w1, "\n");
+tel
+
+fun dbg_sensors(msg : string; s : sensors) returns ()
+var w0, w1, w2, w3, w4, w5, w6, w7 : Debug.world;
+let
+  w0 = Debug.d_string(Debug.d_init(), msg);
+  w1 = Debug.d_string(w0, "{ s_road = ");
+  w2 = d_color(w1, s.s_road);
+  w3 = Debug.d_string(w2, "; s_front = ");
+  w4 = d_color(w3, s.s_front);
+  w5 = Debug.d_string(w4, "; s_sonar = ");
+  w6 = Debug.d_int(w5, s.s_sonar);
+  w7 = Debug.d_string(w6, " }\n");
+tel

src/hept_ffi.h

@@ -0,0 +1,52 @@
+#ifndef HEPT_FFI_H
+#define HEPT_FFI_H
+
+#define UNPAREN(...) __VA_ARGS__
+
+#define DECLARE_HEPT_FUN(module, name, inputs, outputs)                 \
+  typedef struct { outputs; } module ## __ ## name ## _out;             \
+  void module ## __ ## name ##_step(UNPAREN inputs,                     \
+                                    module ## __ ## name ## _out *)
+
+#define DECLARE_HEPT_FUN_NULLARY(module, name, outputs)                 \
+  typedef struct { outputs; } module ## __ ## name ## _out;             \
+  void module ## __ ## name ##_step(module ## __ ## name ## _out *)
+
+#define DEFINE_HEPT_FUN(module, name, inputs)                          \
+  void module ## __ ## name ##_step(UNPAREN inputs,                    \
+                                    module ## __ ## name ## _out *out)
+
+#define DEFINE_HEPT_FUN_NULLARY(module, name, inputs)                   \
+  void module ## __ ## name ##_step(module ## __ ## name ## _out *out)
+
+#define DECLARE_HEPT_NODE(module, name, inputs, outputs, state)         \
+  typedef struct { outputs; } module ## __ ## name ## _out;             \
+  typedef struct { state; } module ## __ ## name ## _mem;               \
+  void module ## __ ## name ##_step(UNPAREN inputs,                     \
+                                    module ## __ ## name ## _out *,     \
+                                    module ## __ ## name ## _mem *);    \
+  void module ## __ ## name ##_reset(module ## __ ## name ## _mem *)
+
+#define DECLARE_HEPT_NODE_NULLARY(module, name, outputs, state)         \
+  typedef struct { outputs; } module ## __ ## name ## _out;             \
+  typedef struct { state; } module ## __ ## name ## _mem;               \
+  void module ## __ ## name ##_step(module ## __ ## name ## _out *,     \
+                                    module ## __ ## name ## _mem *);    \
+  void module ## __ ## name ##_reset(module ## __ ## name ## _mem *)
+
+#define DEFINE_HEPT_NODE_RESET(module, name)                            \
+  void module ## __ ## name ##_reset(module ## __ ## name ## _mem *mem)
+
+#define DEFINE_HEPT_NODE_STEP(module, name, inputs)                     \
+  void module ## __ ## name ##_step(UNPAREN inputs,                     \
+                                    module ## __ ## name ## _out *out,  \
+                                    module ## __ ## name ## _mem *mem)
+
+#define DEFINE_HEPT_NODE_NULLARY_STEP(module, name, inputs)             \
+  void module ## __ ## name ##_step(module ## __ ## name ## _out *out,  \
+                                    module ## __ ## name ## _mem *mem)
+
+/* FIXME remove when Heptagon's pervasives.h has been fixed. */
+typedef char * string;
+
+#endif  /* HEPT_FFI */

src/main.c

@@ -0,0 +1,133 @@
+/* This file is part of SyncContest.
+   Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include "trace.h"
+#include "cutils.h"
+#include "map.h"
+#include "simulation_loop.h"
+
+void usage() {
+  fprintf(stderr, "Usage: scontest [OPTIONS] file.map\n");
+  fprintf(stderr, "Options:\n");
+  fprintf(stderr, "  -v              Be verbose\n");
+  fprintf(stderr, "  -g              Show graphics\n");
+  fprintf(stderr, "  -t              Start racing immediately\n");
+  fprintf(stderr, "  -s <fps>        Simulation step/s (default: 60)\n");
+  fprintf(stderr, "  -o <file>       Save log messages to <file>\n");
+  fprintf(stderr, "  -w              Run in headless mode\n");
+  fprintf(stderr, "  -m <ticks>      Run at most <ticks> synchronous steps\n");
+  fprintf(stderr, "  -a              Enable audio\n");
+  fprintf(stderr, "  -e              Enable obstacles\n");
+  fprintf(stderr, "  -h              Display this message\n");
+}
+
+int main(int argc, char **argv) {
+  bool show_guide = true, headless = false, audio = false, obstacles = false;
+  int initial_top = false, opt;
+  char *log_filename = NULL;
+  size_t max_synchronous_steps = 0;
+  float sps = 60.f;
+
+  hept_trace_init();
+
+  /* Parse command line. */
+  while ((opt = getopt(argc, argv, "vgtf:o:wm:hae")) != -1) {
+    switch (opt) {
+    case 'v':
+      log_set_verbosity_level(LOG_DEBUG);
+      break;
+
+    case 'g':
+      show_guide = false;
+      break;
+
+    case 't':
+      initial_top = true;
+      break;
+
+    case 's':
+      sps = atof(optarg);
+      break;
+
+    case 'h':
+      usage();
+      return EXIT_SUCCESS;
+
+    case 'o':
+      log_filename = optarg;
+      break;
+
+    case 'w':
+      headless = true;
+      break;
+
+    case 'm':
+      max_synchronous_steps = atoi(optarg);
+      break;
+
+    case 'a':
+      audio = true;
+      break;
+
+    case 'e':
+      obstacles = true;
+      break;
+
+    default:
+      usage();
+      return EXIT_FAILURE;
+    }
+  }
+
+  if (optind >= argc) {
+    usage();
+    return EXIT_FAILURE;
+  }
+
+  /* Initialize logging system. */
+  log_init(log_filename);
+
+  /* Load the map. */
+  const char *filename = argv[optind];
+  map_load(filename);
+
+  /* Mark all obstacles as absent if they have been disabled. */
+  if (!obstacles) {
+    for (size_t i = 0; i < map->obst_sz; i++)
+      map->obst_arr[i].pot_since = map->obst_arr[i].pot_till = -1.f;
+  }
+
+  /* Run the simulation loop. */
+  race_result_t r =
+    simulation_loop(show_guide,
+                    initial_top,
+                    sps,
+                    headless,
+                    audio,
+                    max_synchronous_steps);
+
+  switch (r) {
+  case RACE_SUCCESS:
+    log_info("[simulation] race succeeded\n");
+    break;
+  case RACE_TIMEOUT:
+    log_info("[simulation] race quit before finishing\n");
+    break;
+  case RACE_CRASH:
+    log_info("[simulation] car crashed\n");
+    break;
+  }
+
+  /* Free the map, shutdown logs, and return. */
+  map_destroy();
+  log_shutdown();
+  hept_trace_quit();
+
+  /* Return the race result as an exit code. */
+  return r;
+}

src/map.c

@@ -0,0 +1,987 @@
+#include "map.h"
+
+#include <string.h>
+
+#include "mymath.h"
+#include "cutils.h"
+
+map_t *map;
+
+/*
+ * =========================================================================
+ * Geometry
+ * =========================================================================
+ */
+
+/** Euclidian distance betwen two points (xa,ya) and (xb,yb) */
+float distance(float xa, float ya, float xb, float yb)
+{
+  /* use of math.h function */
+  return hypotf(xa - xb, ya - yb);
+}
+
+/** Convert angle from radian to degree */
+float todegree(float a)
+{
+  return (a * 180.0) / M_PI;
+}
+
+/** Convert angle from degree to radian */
+float toradian(float a)
+{
+  return (a * M_PI) / 180.0;
+}
+
+/** Get the cadran of the angle (in degree) */
+int tocadran(float a) {
+  if ((0 <= a && a < 90) ||
+      (-360 <= a && a < -270))
+    return 1;
+  if ((90 <= a && a < 180) ||
+      (-270 <= a && a < -180))
+    return 2;
+  if ((180 <= a && a < 270) ||
+      (-180 <= a && a < -90))
+    return 3;
+  return 4;
+}
+
+/** Angle (in degree) of a segment */
+float lineAngle(float x1, float y1, float x2, float y2)
+{
+  /* use of math.h function for arctan */
+  return todegree(atan2(y2-y1,x2-x1));
+}
+
+/** Angle inside an arc delimited by (from,to) */
+bool isInArc(float a, float from, float to)
+{
+  if (from < to) {
+    // trigo direction
+    return (from <= a && a <= to) || (from <= (a+360) && (a+360) <= to);
+  }
+  else {
+    // clock direction
+    return (to <= a && a <= from) || (to <= (a+360) && (a+360) <= from);
+  }
+}
+
+/** Angles in ordred inside an arc delimited by (from,to) */
+bool fourAnglesInOrder(float from,float x,float y, float to)
+{
+  if (from < to) {
+    // trigo direction
+    return
+      (from<=x && x<=y && y<=to) ||
+      (from<=x+360 && x+360<=y+360 && y+360<=to) ||
+      (from<=x && x<=y+360 && y+360<=to) ||
+      (from<=x+360 && x+360<=y && y<=to);
+  } else  {
+    // clock direction
+    return (to <= y && y<=x && x <= from) ||
+      (to <= y+360 && y+360<=x+360 && x+360 <= from) ||
+      (to <= y+360 && y+360<=x && x <= from) ||
+      (to <= y && y<=x+360 && x+360 <= from);
+  }
+}
+
+/** Product of two vectors for directions */
+float dirProd(float dir1x, float dir1y, float dir2x, float dir2y)
+{
+  return dir1x * dir2x + dir1y * dir2y;
+}
+
+float dirVProd(float dir1x, float dir1y, float dir2x, float dir2y)
+{
+  return dir1x * dir2y - dir1y * dir2x;
+}
+
+/** Size of a direction vector */
+float dirNorm(float dir1x, float dir1y)
+{
+  /* use of math.h function */
+  return hypot(dir1x, dir1y);
+}
+
+/** Cos of directions */
+float dirCos(float dir1x, float dir1y, float dir2x, float dir2y)
+{
+  return dirProd(dir1x, dir1y, dir2x, dir2y)
+    / (dirNorm(dir1x, dir1y) * dirNorm(dir2x, dir2y));
+}
+
+/** Project point (x,y) on line d(p1,p2) and
+ *  return result on (px, py)
+ */
+void
+dirProjPoint( /* IN  */ float x, float y, position_t * p1, position_t * p2,
+              /* OUT */ float *px, float *py)
+{
+  //compute direction vector for d(p1, p2)
+  float           dir_x = p2->x - p1->x;
+  float           dir_y = p2->y - p1->y;
+  float           dirn = dirNorm(dir_x, dir_y);
+
+  //compute P1 - H
+  float           p1h = ((x - p1->x) * dir_x + (y - p1->y) * dir_y) / dirn;
+
+  //compute h
+  (*px) = p1->x + (p1h * dir_x) / dirn;
+  (*py) = p1->y + (p1h * dir_y) / dirn;
+
+  return;
+}
+
+/*
+ * =========================================================================
+ * Parsing
+ * =========================================================================
+ */
+
+typedef void (*map_segment_line_loader)(FILE *,       /* file to read */
+                                        const char *, /* file name */
+                                        void *,       /* pointer to the element
+                                                         to be filled */
+                                        size_t);    /* line number */
+
+void map_load_segment(FILE *f,
+                      const char *filename,
+                      map_segment_line_loader loader,
+                      const char *segment_name,
+                      void *psegment,
+                      int *segment_size,
+                      size_t element_size,
+                      size_t segment_max_size,
+                      size_t *pline) {
+  assert (f);
+  assert (filename);
+  assert (segment_name);
+  assert (psegment);
+  assert (segment_size);
+  assert (pline);
+
+  char kword[15], **segment = (char **)psegment;
+  int rcode;
+
+  /* read until "segment_name" encountered */
+  while (true) {
+    rcode = fscanf(f, "%14s", kword);
+    (*pline)++;
+
+    if ((rcode == EOF) || (strcmp(kword, "end") == 0))
+      log_fatal("[map %s] could not find segment %s\n", filename, segment_name);
+
+    if (strcmp(kword, segment_name) == 0)
+      break;
+  }
+
+  /* "segment_name" has been read, now read item number */
+  rcode = fscanf(f, "%d", segment_size);
+  if (rcode == EOF)
+    log_fatal("[map %s] reached end of file while looking"
+              " for element count in segment %s (line %zu)\n",
+              filename, segment_name, *pline);
+  if ((*segment_size < 0) || (*segment_size > segment_max_size))
+    log_fatal("[map %s] too many (%d) elements in "
+              "segment %s (should be <= %d) (line %zu)\n",
+              filename, *segment_size, segment_name, segment_max_size, *pline);
+  (*pline)++;
+
+  /* allocate enough elements */
+  *segment = calloc(*segment_size, element_size);
+  assert(*segment);
+
+  log_info("[map %s] starting to read segment %s of size %zu\n",
+      filename, segment_name, *segment_size);
+  for (size_t i = 0; i < *segment_size; i++, (*pline)++)
+    loader(f, filename, &(*segment)[i * element_size], *pline);
+  log_info("[map %s] finished reading segment %s\n", filename, segment_name);
+}
+
+void rd_segment_line_loader(FILE *f,
+                            const char *filename,
+                            void *p,
+                            size_t line) {
+  assert (f);
+  assert (filename);
+  assert (p);
+
+  char kword[15];
+  road_t *rd = (road_t *)p;
+  int rcode = fscanf(f, "%14s", kword);
+
+  if (rcode == EOF)
+    log_fatal("[map %s] 'line|arc' expected (line %zu)\n", filename, line);
+
+  if (strcmp(kword, "line") == 0) {
+    int nbdir = 0;
+    /* read "line <dir> <max_speed> <startX> <startY> <endX> <endY>" */
+    rcode = fscanf(f, "%d %d %f %f %f %f",
+                   &nbdir, &rd->max_speed,
+                   &rd->u.line.startp.x, &rd->u.line.startp.y,
+                   &rd->u.line.endp.x, &rd->u.line.endp.y);
+    if ((rcode == EOF) ||
+        (nbdir < 1) ||
+        (nbdir > 2) ||
+        (rd->max_speed <= 0) ||
+        (rd->max_speed >= SPEED_MAX) ||
+        (rd->u.line.startp.x <= MIN_X) ||
+        (rd->u.line.startp.x >= MAX_X) ||
+        (rd->u.line.endp.x <= MIN_X) ||
+        (rd->u.line.endp.x >= MAX_X) ||
+        (rd->u.line.startp.y <= MIN_Y) ||
+        (rd->u.line.startp.y >= MAX_Y) ||
+        (rd->u.line.endp.y <= MIN_Y) ||
+        (rd->u.line.endp.y >= MAX_Y))
+      log_fatal("[map_read %s]: 'line' format error (line %zu)\n",
+                filename, line);
+
+    rd->color = COL_ROADLINE;
+    rd->kind = (nbdir == 1) ? RD_LINE_1 : RD_LINE_2;
+    rd->dir_x = (rd->u.line.endp.x - rd->u.line.startp.x);
+    rd->dir_y = (rd->u.line.endp.y - rd->u.line.startp.y);
+  } else if (strcmp(kword, "arc") == 0) {
+    int nbdir = 0;
+    /* read "arc <dir> <max_speed> <centerX> <centerY>
+       <radius> <start_angle> <end_angle>" */
+    rcode = fscanf(f, "%d %d %f %f %f %f %f",
+                   &nbdir, &rd->max_speed,
+                   &rd->u.arc.center.x, &rd->u.arc.center.y,
+                   &rd->u.arc.radius,
+                   &rd->u.arc.start_angle,
+                   &rd->u.arc.end_angle);
+    if ((rcode == EOF) ||
+        (nbdir != 1) ||
+        (rd->max_speed <= 0) ||
+        (rd->max_speed >= SPEED_MAX) ||
+        (rd->u.arc.center.x <= MIN_X) ||
+        (rd->u.arc.center.x >= MAX_X) ||
+        (rd->u.arc.center.y <= MIN_Y) ||
+        (rd->u.arc.center.y >= MAX_Y) ||
+        (rd->u.arc.radius <= 0) ||
+        (rd->u.arc.start_angle < -180.0) ||
+        (rd->u.arc.start_angle >= 360.0) ||
+        (rd->u.arc.end_angle < -180.0) ||
+        (rd->u.arc.end_angle >= 360.0))
+      log_fatal("[map %s]: 'arc' format error (line %zu)\n",
+                filename, line);
+    rd->color = COL_ROADLINE;
+    rd->kind = RD_ARC;
+    rd->dir_x = 0.0;
+    rd->dir_y = (rd->u.arc.start_angle < rd->u.arc.end_angle) ? -1.0 : 1.0;
+  } else {
+    log_fatal("[map %s] unknown road type %s (line %zu)\n",
+              filename, kword, line);
+  }
+}
+
+void wayp_segment_line_loader(FILE *f,
+                              const char *filename,
+                              void *p,
+                              size_t line) {
+  assert (f);
+  assert (filename);
+  assert (p);
+
+  int rcode;
+  waypoint_t *wp = (waypoint_t *)p;
+
+  /* read <road> <X> <Y> */
+  rcode = fscanf(f, "%d %f %f", &wp->road, &wp->position.x, &wp->position.y);
+  if ((rcode == EOF) ||
+      (wp->road < 0) ||
+      (wp->road >= map->road_sz) ||
+      (wp->position.x <= MIN_X) ||
+      (wp->position.x >= MAX_X) ||
+      (wp->position.y <= MIN_Y) ||
+      (wp->position.y >= MAX_Y)) {
+    log_fatal("[map %s] waypoint format error (line %zu)\n", filename, line);
+  }
+}
+
+void tl_segment_line_loader(FILE *f,
+                            const char *filename,
+                            void *p,
+                            size_t line) {
+  assert (f);
+  assert (filename);
+  assert (p);
+
+  int rcode;
+  tlight_t *tl = (tlight_t *)p;
+
+  /* read <road> <X> <Y> <delayR> <delayA> <delayG> <dephase> */
+  rcode = fscanf(f, "%d %f %f %d %d %d %d",
+                 &tl->road,
+                 &tl->tl.ptl_pos.x, &tl->tl.ptl_pos.y,
+                 &tl->tl.ptl_red, &tl->tl.ptl_amber, &tl->tl.ptl_green,
+                 &tl->tl.ptl_phase);
+  if ((rcode == EOF) ||
+      (tl->road < 0) ||
+      (tl->road >= map->road_sz) ||
+      (tl->tl.ptl_pos.x <= MIN_X) ||
+      (tl->tl.ptl_pos.x >= MAX_X) ||
+      (tl->tl.ptl_pos.y <= MIN_Y) ||
+      (tl->tl.ptl_pos.y >= MAX_Y))
+    log_fatal("[map %s] traffic light format error (line %zu)\n",
+              filename, line);
+}
+
+void st_segment_line_loader(FILE *f,
+                            const char *filename,
+                            void *p,
+                            size_t line) {
+  assert (f);
+  assert (filename);
+  assert (p);
+
+  int rcode;
+  stop_t *st = (stop_t *)p;
+
+  /* read <road> <tlight> <X> <Y> */
+  rcode = fscanf(f, "%d %d %f %f",
+                 &st->road, &st->sema,
+                 &st->position.x, &st->position.y);
+  if ((rcode == EOF) ||
+      (st->road < 0) ||
+      (st->road >= map->road_sz) ||
+      (st->sema < 0) ||
+      (st->sema >= map->tlight_sz) ||
+      (st->position.x <= MIN_X) ||
+      (st->position.x >= MAX_X) ||
+      (st->position.y <= MIN_Y) ||
+      (st->position.y >= MAX_Y))
+    log_fatal("[map %s] stop format error (line %zu)\n", filename, line);
+}
+
+void obst_segment_line_loader(FILE *f,
+                              const char *filename,
+                              void *p,
+                              size_t line) {
+  assert (f);
+  assert (filename);
+  assert (p);
+
+  int rcode;
+  obst_t *obst = (obst_t *)p;
+
+  rcode = fscanf(f, "%f %f %f %f", &obst->pot_pos.x, &obst->pot_pos.y,
+                 &obst->pot_since, &obst->pot_till);
+  if ((rcode == EOF) ||
+      (obst->pot_pos.x <= MIN_X) ||
+      (obst->pot_pos.x >= MAX_X) ||
+      (obst->pot_pos.y <= MIN_Y) ||
+      (obst->pot_pos.y >= MAX_Y))
+    log_fatal("[map %s] obstacle format error (line %zu)\n", filename, line);
+}
+
+void iti_segment_line_loader(FILE *f,
+                             const char *filename,
+                             void *p,
+                             size_t line) {
+  assert (f);
+  assert (filename);
+  assert (p);
+
+  char kword[15];
+  iti_t *iti = (iti_t *)p;
+  int rcode = fscanf(f, "%14s %f", kword, &iti->param);
+
+  if (strcmp(kword, "go") == 0) {
+    iti->act = Globals__Go;
+    if ((rcode == EOF) || (iti->param <= 0.f) || (iti->param > SPEED_MAX))
+      log_fatal("[map %s] itinerary format error (action Go, line %zu)\n",
+                filename, line);
+  } else if (strcmp(kword, "turn") == 0) {
+    iti->act = Globals__Turn;
+    if ((rcode == EOF) || (iti->param < -180.0) || (iti->param >= 360.0))
+      log_fatal("[map %s] itinerary format error (action Turn, line %zu)\n",
+                filename, line);
+  } else if (strcmp(kword, "stop") == 0) {
+    iti->act = Globals__Stop;
+    iti->param = 0.f;           /* Dummy */
+  } else {
+    log_fatal("[map %s] itinerary format error (unknown action %s, line %zu)\n",
+              filename, kword, line);
+  }
+}
+
+void map_read_string_line(FILE *f, const char *filename,
+                          const char *expected_kw, char *buff, size_t *pline) {
+  char kword[15];
+  int rcode = fscanf(f, "%14s %249s", kword, buff);
+  if ((rcode == EOF) || (strcmp(kword, expected_kw) != 0))
+    log_fatal("[map %s] could not read %s (line %zu)\n",
+              filename, expected_kw, *pline);
+  if (buff[0] != '"' || buff[strlen(buff) - 1] != '"')
+    log_fatal("[map %s] string after %s should be between double quotes"
+              " (line %zu)\n",
+              filename, expected_kw, *pline);
+  (*pline)++;
+
+  /* Remove surrounding double quotes. */
+  memmove(buff, buff + 1, strlen(buff) - 2);
+  buff[strlen(buff) - 2] = 0;
+
+  log_info("[map %s] read %s: %s\n", filename, expected_kw, buff);
+}
+
+void map_load(const char *filename) {
+  map = malloc(sizeof *map);
+  assert(map);
+  bzero(map, sizeof *map);
+
+  size_t line = 1;
+  FILE *f = fopen(filename, "r");
+  if (!f)
+    log_fatal("[map %s] could not open file\n", filename);
+
+  map_read_string_line(f, filename, "map", map->name, &line);
+  map_read_string_line(f, filename, "graphics", map->graphics, &line);
+  map_read_string_line(f, filename, "guide", map->guide, &line);
+
+  char kword[15];
+  int rcode = fscanf(f, "%14s %f %f %f", kword,
+                     &map->init_phase.ph_pos.x,
+                     &map->init_phase.ph_pos.y,
+                     &map->init_phase.ph_head);
+  if ((rcode == EOF) || (strcmp(kword, "init") != 0))
+    log_fatal("[map %s] could not read init (line %zu)\n", filename, line);
+  line++;
+  log_info("[map %s] read init: x = %f, y = %f, head = %f\n",
+      filename,
+      map->init_phase.ph_pos.x,
+      map->init_phase.ph_pos.y,
+      map->init_phase.ph_head);
+
+  /* Read the roads. */
+  map_load_segment(f, filename, rd_segment_line_loader, "rd",
+                   &map->road_arr, &map->road_sz, sizeof *map->road_arr,
+                   MAX_ROAD_COUNT, &line);
+
+  /* Read the waypoints. */
+  map_load_segment(f, filename, wayp_segment_line_loader, "wp",
+                   &map->wayp_arr, &map->wayp_sz, sizeof *map->wayp_arr,
+                   MAX_WAYP_COUNT, &line);
+
+  /* Read the traffic lights. */
+  map_load_segment(f, filename, tl_segment_line_loader, "tl",
+                   &map->tlight_arr, &map->tlight_sz, sizeof *map->tlight_arr,
+                   MAX_TL_COUNT, &line);
+
+  /* Read the stops. */
+  map_load_segment(f, filename, st_segment_line_loader, "st",
+                   &map->stop_arr, &map->stop_sz, sizeof *map->stop_arr,
+                   MAX_STOP_COUNT, &line);
+
+  /* Read the obstacles. */
+  map_load_segment(f, filename, obst_segment_line_loader, "obst",
+                   &map->obst_arr, &map->obst_sz, sizeof *map->obst_arr,
+                   MAX_OBST_COUNT, &line);
+
+  /* Read the obstacles. */
+  map_load_segment(f, filename, iti_segment_line_loader, "iti",
+                   &map->iti_arr, &map->iti_sz, sizeof *map->iti_arr,
+                   MAX_ITI_COUNT, &line);
+
+  /* Close file and return. */
+  fclose(f);
+}
+
+void map_destroy() {
+  if (!map)
+    return;
+
+  /* Fields have been initialized to NULL, it is safe to call free() on them. */
+  free(map->road_arr);
+  free(map->wayp_arr);
+  free(map->tlight_arr);
+  free(map->stop_arr);
+  free(map->obst_arr);
+  free(map);
+  map = NULL;
+}
+
+void Map__read_obstacles_step(Map__read_obstacles_out *o) {
+  /* TODO very inefficient */
+  memcpy(o->obst, map->obst_arr, map->obst_sz * sizeof *o->obst);
+
+  /* The map file might contain fewer obstacles than Globals__obstnum. */
+  for (size_t i = map->obst_sz; i < MAX_OBST_COUNT; i++) {
+    o->obst[i].pot_pos.x = 0.f;
+    o->obst[i].pot_pos.y = 0.f;
+    o->obst[i].pot_since = -1.f;
+    o->obst[i].pot_till = -1.f;
+  }
+}
+
+void Map__read_itinerary_step(Map__read_itinerary_out *o) {
+  /* TODO very inefficient */
+  memcpy(o->iti, map->iti_arr, map->iti_sz * sizeof *o->iti);
+
+  /* The map file might contain fewer itinerary steps than Globals__itinum. */
+  for (size_t i = map->iti_sz; i < MAX_ITI_COUNT; i++) {
+    o->iti[i].act = Globals__Stop;
+    o->iti[i].param = 0.f;
+  }
+}
+
+void Map__read_traffic_lights_step(Map__read_traffic_lights_out *o) {
+  assert (map->tlight_sz <= Globals__trafnum);
+  for (size_t i = 0; i < map->tlight_sz; i++)
+    o->tlights[i] = map->tlight_arr[i].tl;
+  for (size_t i = map->tlight_sz; i < MAX_TL_COUNT; i++)
+    o->tlights[i] = (Globals__param_tlight){ {-100, -100}, 0, 0, 0, 0 };
+}
+
+bool colors_equal(const Globals__color *a, const Globals__color *b) {
+  return a->red == b->red && a->green == b->green && a->blue == b->blue;
+}
+
+bool isOnRoadLine1(road_t *rd, float x, float y,
+                   Globals__color *col, double *d, float *dir_x, float *dir_y) {
+  //test that may be inside road area
+  // -compute projection on line
+  float                   px = 0.0;
+  float                   py = 0.0;
+  dirProjPoint(x, y, &(rd->u.line.startp), &(rd->u.line.endp), &px, &py);
+  //-compare with ends of the segment
+  if ((px <= fmax(rd->u.line.startp.x, rd->u.line.endp.x)+EPS) &&
+      (px >= fmin(rd->u.line.startp.x, rd->u.line.endp.x)-EPS) &&
+      (py <= fmax(rd->u.line.startp.y, rd->u.line.endp.y)+EPS) &&
+      (py >= fmin(rd->u.line.startp.y, rd->u.line.endp.y)-EPS))
+    //the projection is inside, compute distance to rd
+    (*d) = distance(x, y, px, py);
+  else
+    //the projection is outside the segment
+    // let us test the extremities:
+    (*d) = fmin(distance(x,y,rd->u.line.startp.x,rd->u.line.startp.y),
+                distance(x,y,rd->u.line.endp.x,rd->u.line.endp.y));
+
+  //-compare with the width of the road
+  if ((*d) >= RD_SIZE_HALF_WIDTH) {
+    log_debug("[geometry] (%.2f, %.2f) is too far from road %p (dist. %.2f)!\n",
+              x, y, rd, *d);
+    return false;
+  }
+
+  (*col) = rd->color;
+  (*dir_x) = rd->u.line.endp.x - rd->u.line.startp.x;
+  (*dir_y) = rd->u.line.endp.y - rd->u.line.startp.y;
+
+  if ((*d) < RD_SIZE_LINE) {
+    //distance less than width of the road line,
+    //then on road with one color
+    //log_debug("Position on road line: %f!\n", *d);
+  }
+  else {
+    //otherwise, compute the side of the road
+    // left or right using direction(x, y)->(px, py)
+    double          dirPX = px - x;     /* -b where b = x1 - x2 */
+    double          dirPY = py - y;     /* a  where a = y2 - y1 */
+    double          sinDir = dirVProd(*dir_x, *dir_y, dirPX, dirPY);
+    if (sinDir < 0) {
+      //on left
+      (*col) = COL_ROADLEFT;
+#ifndef MAP_BMP
+      if ((*d) <= RD_SIZE_LINE2)
+        col->green = (unsigned int)(255.*((*d)-1.0));
+#endif
+    } else {
+      //on right
+      (*col) = COL_ROADRIGHT;
+#ifndef MAP_BMP
+      if ((*d) <= RD_SIZE_LINE2)
+        col->red = (unsigned int)(255.*((*d)-1.0));
+#endif
+    }
+    //log_debug("Position on road: %f!\n", *d);
+  }
+  return true;
+}
+
+bool isOnRoadLine2(road_t *rd, float x, float y,
+                   Globals__color *col, double *d, float *dir_x, float *dir_y) {
+  /* TODO missing from original project */
+  return false;
+}
+
+bool isOnRoadArc(/* IN  */
+                 road_t *rd, float x, float y,
+                 /* OUT */
+                 Globals__color *col, double *d,
+                 float *dir_x, float *dir_y)
+{
+  //center
+  double          cx = rd->u.arc.center.x;
+  double          cy = rd->u.arc.center.y;
+
+  //compute angle(in degrees) from center
+  double          a = lineAngle(cx, cy, x, y);
+
+  //compute signed distance to the circle c
+  double     dist_c = distance(cx, cy, x, y) - rd->u.arc.radius;
+
+  if (isInArc(a, rd->u.arc.start_angle,rd->u.arc.end_angle)) {
+    //within the angle
+    (*d) = fabs(dist_c);
+  }
+  else {
+    //Point outside road angles,
+    //  consider the distance to extremities
+    (*d) = fmin(distance(x,y,
+                         cx+rd->u.arc.radius*cos(toradian(rd->u.arc.start_angle)),
+                         cy+rd->u.arc.radius*sin(toradian(rd->u.arc.start_angle))),
+                distance(x,y,
+                         cx+rd->u.arc.radius*cos(toradian(rd->u.arc.end_angle)),
+                         cy+rd->u.arc.radius*sin(toradian(rd->u.arc.end_angle))));
+  }
+
+  if ((*d) >= RD_SIZE_HALF_WIDTH) {
+    //log_debug("Position too far from road: %f!\n", (*d));
+    return false;
+  }
+
+  (*col) = rd->color;
+
+  //direction is normal to d(c, (x, y))
+  //its sign depends on rotation
+  if (rd->u.arc.start_angle < rd->u.arc.end_angle){//counterclockwise
+    (*dir_x) = cy - y;
+    (*dir_y) = x - cx;
+  }
+  else {//clockwise
+    (*dir_x) = y - cy;           /* a where a = y2 - y1 */
+    (*dir_y) = cx - x;           /* b where b = x1 - x2 */
+  }
+
+  if ((*d) < RD_SIZE_LINE) {
+    //distance less than width of the road line,
+    //then on road with one color
+    //log_debug("Point on road line: %f!\n", *d);
+  } else {
+    //otherwise, compute the side of the road
+    // left or right using the sense(trigo or clock) of the road
+    if (((rd->u.arc.start_angle < rd->u.arc.end_angle) //trigo dir
+         && dist_c < 0) ||
+        ((rd->u.arc.start_angle > rd->u.arc.end_angle) // clock dir
+         && dist_c > 0)) {
+      (*col) = COL_ROADLEFT;
+#ifndef MAP_BMP
+      if ((*d) <= RD_SIZE_LINE2)
+        col->green = (unsigned int)(255.*((*d)-1.0));
+#endif
+    } else {
+      (*col) = COL_ROADRIGHT;
+#ifndef MAP_BMP
+      if ((*d) <= RD_SIZE_LINE2)
+        col->red = (unsigned int)(255.*((*d)-1.0));
+#endif
+    }
+  }
+  //log_debug("Point on road: %f!\n", *d);
+  return true;
+}
+
+int isOnTLight(int x, int y, int rd, float dir_x, float dir_y)
+{
+  if (map->tlight_arr == NULL ||
+      map->tlight_sz <= 0)
+    return -1;
+  //no traffic light
+
+  for (int i = 0; i < map->tlight_sz; i++) {
+    tlight_t *tl = &map->tlight_arr[i];
+    if (tl->road != rd)
+      continue;
+    double          d = distance(x, y, tl->tl.ptl_pos.x, tl->tl.ptl_pos.y);
+
+   /* double        cosdir = dirCos(tl->tl.ptl_pos.y - y, x - tl->tl.ptl_pos.x,
+                                    dir_x, dir_y); */    //MS
+   double           cosdir = dirCos(tl->tl.ptl_pos.x-x, tl->tl.ptl_pos.y-y,
+                                    dir_x, dir_y);     //EA
+    if (d < TL_VIEW && cosdir > TL_COSDIR)
+      return i;
+  }
+  return -1;
+}
+
+bool isAfterStop(int x, int y, int rid, float dir_x, float dir_y, int* tl)
+{
+  (*tl) = -1;
+
+  if (map->stop_arr == NULL ||
+      map->stop_sz <= 0)
+    return false;
+  //no stop points
+
+  for (int i = 0; i < map->stop_sz; i++) {
+    stop_t *sp = &map->stop_arr[i];
+    if (sp->road != rid)
+      continue;
+    // check the distance to the point
+    road_t *rd = &map->road_arr[sp->road];
+    if (rd->kind == RD_LINE_1 ||
+        rd->kind == RD_LINE_2) {
+      // line
+      // - compute projection on roadline
+      float px = 0.0;
+      float py = 0.0;
+      dirProjPoint(x, y, &(rd->u.line.startp), &(rd->u.line.endp), &px, &py);
+      // - compare with the bounds of the stop point
+      double dsp = distance(px, py, sp->position.x, sp->position.y);
+      double dir_x = px - sp->position.x;
+      double dir_y = py - sp->position.y;
+      if (dsp >= (RD_SIZE_STOP-EPS) &&
+          dsp <= (STP_AFTER+RD_SIZE_STOP+EPS) &&
+          (dirProd(rd->dir_x, rd->dir_y, dir_x, dir_y) >= 0))
+        {
+          log_debug("[geometry] (%d, %d) is after stop %d (dist %.2f)\n",
+                    x, y, i, dsp);
+          (*tl) = sp->sema;
+          return true;
+        }
+      else
+        {
+          log_debug("Position too far/not AFTER point on line: %lf!\n", dsp);
+          log_debug("rd->dir(%lf,%lf) <> dir(%lf,%lf)\n", rd->dir_x, rd->dir_y,
+                    dir_x, dir_y);
+        }
+    }
+    else {
+      // arc
+      // -compute angle of point in degrees
+      double apoint = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y,
+                                sp->position.x, sp->position.y);
+      double apos = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, x, y);
+      double dsp = toradian(fabs(apoint - apos))*rd->u.arc.radius;
+      if (dsp >= (RD_SIZE_STOP-EPS) &&
+          dsp <= (STP_AFTER+RD_SIZE_STOP+EPS)) {
+        // - check that the direction is AFTER
+        if (fourAnglesInOrder(rd->u.arc.start_angle,
+                              apoint, apos,
+                              rd->u.arc.end_angle))
+            {
+              log_debug("Position AFTER point on arc: %lf!\n", apos);
+              (*tl) = sp->sema;
+              return true;
+            }
+          else
+            {
+              log_debug("Position too far/not AFTER point on arc: %lf!\n", apos);
+              return false;
+            }
+      }
+      else {
+        log_debug("Position too far/not AFTER point on arc: %lf degrees!\n", apos);
+      }
+    }
+  }
+  return false;
+}
+
+bool
+isPositionOnPoint(float x, float y, road_t* rd, position_t* p, double pWidth)
+{
+  if (rd->kind == RD_LINE_1 ||
+      rd->kind == RD_LINE_2) { // line
+    // - compute projection on road line
+    float           px = 0.0;
+    float           py = 0.0;
+    position_t      startp = {0, 0};
+    position_t      endp = {0, 0};
+
+    startp = (rd->u.line.startp);
+    endp = (rd->u.line.endp);
+    dirProjPoint(x, y, &(startp), &(endp), &px, &py);
+    // - compare with the bounds of the waypoint
+    double          dwp = distance(px, py, p->x, p->y);
+    if (dwp <= pWidth) {
+      log_debug("Position near point on line: %lf!\n", dwp);
+      return true;
+    }
+    else {
+      //log_debug("Position too far from point on line: %lf!\n", dwp);
+      return false;
+    }
+  }
+  else {
+    // arc
+    // - compute angle of point in degrees
+    double apoint = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, p->x, p->y);
+    double apos = lineAngle(rd->u.arc.center.x, rd->u.arc.center.y, x, y);
+    double dwp = toradian(fabs(apoint - apos))*rd->u.arc.radius;
+    if (dwp <= pWidth) {
+      log_debug("Position near point on arc: %lf!\n", dwp);
+      return true;
+    }
+    else {
+      log_debug("Position too far from point on arc: %lf!\n", dwp);
+      return false;
+    }
+  }
+}
+
+Globals__color getColorPoint(int rid, float x, float y) {
+  // first go through waypoints
+  log_debug("[geometry] looking for waypoints at (%.2f, %.2f) on road %d\n",
+            x, y, rid);
+  for (int i = 0; i < map->wayp_sz; i++) {
+    waypoint_t *wp = &map->wayp_arr[i];
+    if (wp->road != rid)
+      {
+        log_debug("Waypoint not on road %d!\n", rid);
+        continue;
+      }
+    road_t *rd = &map->road_arr[wp->road];
+    // waitpoints are ruban
+    if (isPositionOnPoint(x, y, rd, &(wp->position), RD_SIZE_WAYPOINT)) {
+      return COL_WAYPOINT;
+      //one waypoint by position
+    }
+  }
+  log_debug("[geometry] finished walking waypoints\n");
+
+  // then go to stop points
+  log_debug("[geometry] looking for stops at (%.2f, %.2f) on road %d\n",
+            x, y, rid);
+  for (int i = 0; i < map->stop_sz; i++) {
+    stop_t *sp = &map->stop_arr[i];
+    if (sp->road != rid) {
+      log_debug("Stop not on road %d!\n", rid);
+      continue;
+    }
+    road_t *rd = &map->road_arr[sp->road];
+    // stop points are ruban
+    if (isPositionOnPoint(x, y, rd, &sp->position, RD_SIZE_STOP)) {
+      log_debug("[geometry] (%.2f, %.2f) at stop %d\n", x, y, i);
+      return COL_STOP;
+      //one stop by position
+    }
+  }
+  log_debug("[geometry] finished walking stops\n");
+
+  return COL_OUT;
+}
+
+DEFINE_HEPT_FUN(Map, lookup_pos, (Globals__position pos)) {
+  float x = pos.x, y = pos.y;
+
+  out->data.on_road = false;
+  out->data.color = COL_OUT;
+  out->data.max_speed = SPEED_MIN;
+  out->data.tl_number = -1;
+  out->data.tl_required = false;
+  out->data.dir_x = -1.0;
+  out->data.dir_y = 0.0;
+
+  log_debug("[geometry] querying pos (%.2f, %.2f)\n", x, y);
+  if (map == NULL)
+    log_fatal("[geometry] map has not been initialized\n");
+
+  int min_rd = -1;
+  double min_d = 700.;
+  for (int rid = 0; rid < map->road_sz; rid++) {
+    road_t *rd = &map->road_arr[rid];
+    double d = 0.;
+    Globals__color col = COL_OUT;
+    bool onRoad = false;
+    float dir_X = 0.0;
+    float dir_Y = 0.0;
+    switch (rd->kind) {
+    case RD_LINE_1:
+      onRoad = isOnRoadLine1(rd, x, y, &col, &d, &dir_X, &dir_Y);
+      break;
+    case RD_LINE_2:
+      onRoad = isOnRoadLine2(rd, x, y, &col, &d, &dir_X, &dir_Y);
+      break;
+    case RD_ARC:
+      onRoad = isOnRoadArc(rd, x, y, &col, &d, &dir_X, &dir_Y);
+      break;
+    case RD_OTHER:
+      break;
+    }
+
+    if (onRoad && (d < min_d)) {
+      min_d = d;
+      min_rd = rid;
+      out->data.color = col;
+      out->data.dir_x = dir_X;
+      out->data.dir_y = dir_Y;
+      out->data.max_speed = map->road_arr[min_rd].max_speed;
+      /* Update color when a waypoint or stop. */
+      col = getColorPoint(rid, x, y);
+      if (colors_equal(&col, &COL_OUT))
+        out->data.color = out->data.color;
+      else if (colors_equal(&col, &COL_STOP)) {
+        /* TODO: update red color */
+        out->data.color = col;
+      }
+      else {
+        /* TODO: update green color */
+        out->data.color = col;
+      }
+    }
+  }
+
+  /* Compute the return type. */
+  if (min_rd >= 0) {
+    log_debug("[geometry] (%.2f, %.2f) is on road %d\n", x, y, min_rd);
+    out->data.on_road = true;
+    int tl = -1;
+    out->data.tl_number =
+      isOnTLight(x, y, min_rd, out->data.dir_x, out->data.dir_y);
+    out->data.tl_required = isAfterStop(x, y, min_rd,
+                                      out->data.dir_x, out->data.dir_y, &tl);
+    if(out->data.tl_required) {
+      if (tl != out->data.tl_number) {
+        log_debug("Warning: on TL %ld != ", out->data.tl_number);
+        log_debug("after TL %d!\n", tl);
+      }
+      out->data.tl_number = tl;
+    }
+  }
+
+  /* Log the result. */
+  log_debug("[geometry] { on_road = %d; color = (%d, %d, %d);"
+            " dir = (%2.2f, %2.2f); tl = (%d, %d); }\n",
+            out->data.on_road,
+            out->data.color.red, out->data.color.green, out->data.color.blue,
+            out->data.dir_x, out->data.dir_y,
+            out->data.tl_number, out->data.tl_required);
+}
+
+
+void play_asset_wav(SDL_AudioDeviceID audio_device, asset_wav_t *wav) {
+  if (!audio_device) {
+    log_info("[sdl] no audio device\n");
+    return;
+  }
+
+  if (SDL_QueueAudio(audio_device, wav->buffer, wav->size))
+    log_fatal("[sdl] could not queue audio\n");
+}
+
+asset_wav_t collision, wrong_dir, exit_road, light_run, speed_excess;
+SDL_AudioDeviceID audio_device = 0;
+
+DEFINE_HEPT_FUN(Map, soundEffects, (Globals__event evt, Globals__status sta)) {
+  if (sta == Globals__Preparing)
+    return;
+  if (evt.exitRoad) {
+    play_asset_wav(audio_device, &exit_road);
+    log_info("[audio] car left the road\n");
+  } else if (evt.collisionEvent) {
+    play_asset_wav(audio_device, &collision);
+    log_info("[audio] car has collided with an obstacle\n");
+  } else if (evt.dirEvent) {
+    play_asset_wav(audio_device, &wrong_dir);
+    log_info("[audio] car goes in the wrong direction\n");
+  } else if (evt.lightRun) {
+    play_asset_wav(audio_device, &light_run);
+    log_info("[audio] car has run a red light\n");
+  } else if (evt.speedExcess) {
+    play_asset_wav(audio_device, &speed_excess);
+    log_info("[audio] car is going too fast\n");
+  }
+}

src/map.epi

@@ -0,0 +1,7 @@
+open Globals
+
+external fun read_obstacles() returns (obst : param_obsts)
+external fun read_itinerary() returns (iti : itielts)
+external fun read_traffic_lights() returns (tlights : param_tlights)
+external fun lookup_pos(pos : position) returns (data : map_data)
+external fun soundEffects(evt : event; sta : status) returns ()

src/map.h

@@ -0,0 +1,248 @@
+#ifndef MAP_H
+#define MAP_H
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <SDL.h>
+
+#include "pervasives.h"
+#include "hept_ffi.h"
+#include "globals_types.h"
+
+#define MAX_ROAD_COUNT  50               /* maximum number of roads */
+#define MAX_WAYP_COUNT  50               /* maximum number of waypoints */
+#define MAX_TL_COUNT    Globals__trafnum /* maximum number of traffic lights */
+#define MAX_STOP_COUNT  50               /* maximum number of stops */
+#define MAX_OBST_COUNT  Globals__obstnum /* maximum number of obstacles */
+#define MAX_ITI_COUNT   Globals__itinum  /* maximum number of itinerary steps */
+
+/*
+ * =========================================================================
+ * Reference system
+ * =========================================================================
+ */
+
+/** Size of the Carthesian space (in cm) */
+#define MIN_X 0
+#define MIN_Y 0
+#define MAX_X 600
+#define MAX_Y 300
+
+/** Precision of comparison between points */
+#define EPS 0.001
+
+/** Type of points used, in a Carthesian space */
+/** see @code{positionTy} in kcg_types.h */
+
+float toradian(float);
+
+/*
+ * =========================================================================
+ * Car size
+ * =========================================================================
+ */
+
+/**
+ * Car plan with o-point is the position of camera
+ *
+ * +---------------+
+ * |  SC      |SB  |
+ * |----------o----| SA
+ * |          |SD  |
+ * +---------------+
+ *
+ * Dimensions defined as constants in kcg_const.h
+ * #define SA (kcg_lit_float64(3.0))
+ * #define SB (kcg_lit_float64(3.0))
+ * #define SD (kcg_lit_float64(3.0))
+ * #define SC (kcg_lit_float64(7.0))
+ */
+#define CAR_WIDTH       (SB+SD)
+#define CAR_HALF_WIDTH   SB
+
+/*
+ * =========================================================================
+ * Colors
+ * =========================================================================
+ */
+
+/** Color type @code{colorTy} defined in @see{kcg_types.h} */
+
+/** True colors used
+ *  defined in @see{kcg_const.h} as constants of SCADE
+ *
+ * extern const colorTy AMBER;
+ * extern const colorTy GRAY;
+ * extern const colorTy CYAN;
+ * extern const colorTy MAGENTA;
+ * extern const colorTy YELLOW;
+ * extern const colorTy BLUE;
+ * extern const colorTy GREEN;
+ * extern const colorTy RED;
+ */
+
+/** Symbolic colors used for road, points, etc */
+#define COL_ROADLINE   Globals__blue
+#define COL_ROADLEFT   Globals__cyan
+#define COL_ROADRIGHT  Globals__magenta
+#define COL_WAYPOINT   Globals__green
+#define COL_STOP       Globals__red
+#define COL_OUT        Globals__gray
+
+/** String colors used to print */
+#define STR_ROADLINE   "blue"
+#define STR_ROADLEFT   "cyan"
+#define STR_ROADRIGHT  "magenta"
+#define STR_WAYPOINT   "yellow"
+#define STR_STOP       "red"
+#define STR_TLIGHT     "orange"
+#define STR_OUT        "gray"
+
+/*
+ * =========================================================================
+ * Road segments and points
+ * =========================================================================
+ */
+
+/** Kind of road segments */
+typedef enum {
+  RD_LINE_1 = 0,                /* linear road, one direction */
+  RD_LINE_2,                    /* linear road, two directions */
+  RD_ARC,                       /* arc road, one direction */
+  RD_OTHER                      /* INTERNAL USE ONLY */
+} road_kind_t;
+
+/** Dimensions used for road (in cm) */
+#define RD_SIZE_LINE        1.0
+#define RD_SIZE_LINE2       2.0
+#define RD_SIZE_HALF_WIDTH  10.0 /* 10.0 in simulator version */
+#define RD_SIZE_WAYPOINT    1.0  /* the size of the half of the ruban */
+#define RD_SIZE_STOP        1.0
+
+/** Dimensions for traffic lights (in cm) */
+#define TL_NUMBER   5
+#define TL_VIEW     50.0
+#define TL_COSDIR   0.5
+#define STP_AFTER   3.0
+
+/** Speeds */
+#define SPEED_MIN 20
+#define SPEED_MAX 40
+
+typedef Globals__phase phase_t;
+
+/** Road segment */
+typedef struct {
+  road_kind_t  kind;           /* kind to select informations */
+
+  Globals__color color;          /* color used on the middle line */
+  float          dir_x, dir_y;   /* direction vector/sense for line/arc (x) */
+  int            max_speed;      /* maximal speed */
+
+  union {
+    struct {                    /* parameters for a linear road */
+      Globals__position startp; /* on the middle line */
+      Globals__position endp;   /* on the middle line */
+    } line;
+    struct {                         /* parameters for a arc road */
+      Globals__position center;      /* the circle center */
+      float             radius;      /* the circle radius (in cm) */
+      float             start_angle; /* start and stop angles */
+      float             end_angle;
+    } arc;
+  } u;
+} road_t;
+
+/** Waypoint used to consult the map */
+typedef struct {
+  int                   road;     /* road identifier */
+  Globals__position     position; /* on the middle line of the road */
+} waypoint_t;
+
+/** Stop point used to signal a traffic light */
+typedef struct {
+  int                   road;     /* road identifier */
+  int                   sema;     /* traffic light identifier */
+  Globals__position     position; /* on the middle line of a road */
+} stop_t;
+
+/** Traffic lights: added road reference to type
+ * @code{paramTLTy_City} defined in @see{kcg_tpes.h} */
+typedef struct {
+  Globals__param_tlight tl;
+  int                   road;   /* road identifier that the tlight controls */
+} tlight_t;
+
+typedef Globals__param_obst obst_t;
+
+typedef Globals__itielt iti_t;
+
+typedef Globals__position position_t;
+
+/*
+ * =========================================================================
+ * Maps
+ * =========================================================================
+ */
+
+/** One map contains roads, waypoints, traffic lights and stop points */
+typedef struct {
+  char                 name[255];     /* Name of the map */
+  char                 graphics[255]; /* Path to graphics file (bmp) */
+  char                 guide[255];    /* Path to guide file (bmp) */
+  phase_t              init_phase;    /* Initial phase of the robot */
+  road_t               *road_arr;     /* Roads */
+  int                  tlight_sz;     /* Road count */
+  waypoint_t           *wayp_arr;     /* Waypoints */
+  int                  road_sz;       /* Waypoint count */
+  tlight_t             *tlight_arr;   /* Traffic lights */
+  int                  wayp_sz;       /* Traffic light count */
+  stop_t               *stop_arr;     /* Stops */
+  int                  stop_sz;       /* Stop count */
+  obst_t               *obst_arr;     /* Obstacles */
+  int                  obst_sz;       /* Obstacle count */
+  iti_t                *iti_arr;      /* Itinerary */
+  int                  iti_sz;        /* Itinerary step count */
+} map_t;
+
+extern map_t *map;
+
+void map_load(const char *);  /* parse and load global map file */
+void map_destroy();           /* free the map loaded via load_map() */
+
+/*
+ * =========================================================================
+ * Functions exported to the Heptagon side
+ * =========================================================================
+ */
+
+typedef struct asset_wav {
+  SDL_AudioSpec spec;
+  Uint8 *buffer;
+  Uint32 size;
+} asset_wav_t;
+
+extern asset_wav_t collision, wrong_dir, exit_road, light_run, speed_excess;
+extern SDL_AudioDeviceID audio_device;
+
+DECLARE_HEPT_FUN_NULLARY(Map,
+                         read_obstacles,
+                         Globals__param_obsts obst);
+DECLARE_HEPT_FUN_NULLARY(Map,
+                         read_traffic_lights,
+                         Globals__param_tlights tlights);
+DECLARE_HEPT_FUN_NULLARY(Map,
+                         read_itinerary,
+                         Globals__itielts iti);
+DECLARE_HEPT_FUN(Map,
+                 lookup_pos,
+                 (Globals__position),
+                 Globals__map_data data);
+DECLARE_HEPT_FUN(Map,
+                 soundEffects,
+                 (Globals__event, Globals__status),);
+
+#endif  /* MAP_H */

src/map_types.h

@@ -0,0 +1,4 @@
+#ifndef MAP_TYPES_H
+#define MAP_TYPES_H
+
+#endif  /* MAP_TYPES_H */

src/mathext.c

@@ -0,0 +1,42 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "mymath.h"
+#include "mathext.h"
+
+void Mathext__float_step(int x, Mathext__float_out *o) {
+  o->o = (float)x;
+}
+
+void Mathext__int_step(float x, Mathext__int_out *o) {
+  o->o = (int)x;
+}
+
+void Mathext__floor_step(float x, Mathext__floor_out *o) {
+  o->o = floorf(x);
+}
+
+void Mathext__sin_step(float x, Mathext__sin_out *o) {
+  o->o = sinf(x);
+}
+
+void Mathext__cos_step(float x, Mathext__cos_out *o) {
+  o->o = cosf(x);
+}
+
+void Mathext__atan2_step(float y, float x, Mathext__atan2_out *o) {
+  o->o = atan2f(y, x);
+}
+
+void Mathext__hypot_step(float x, float y, Mathext__hypot_out *o) {
+  o->o = hypotf(x, y);
+}
+
+void Mathext__sqrt_step(float x2, Mathext__sqrt_out *o) {
+  o->o = sqrtf(x2);
+}
+
+void Mathext__modulo_step(int x, int y, Mathext__modulo_out *o) {
+  o->o = x % y;
+}

src/mathext.epi

@@ -0,0 +1,11 @@
+external fun float(x : int) returns (o : float)
+external fun int(x : float) returns (o : int)
+external fun floor(x : float) returns (o : float)
+
+external fun sin(x : float) returns (o : float)
+external fun cos(x : float) returns (o : float)
+external fun atan2(y : float; x : float) returns (o : float)
+external fun hypot(x : float; y : float) returns (o : float)
+external fun sqrt(x2 : float) returns (o : float)
+
+external fun modulo(x : int; y : int) returns (o : int)

src/mathext.h

@@ -0,0 +1,22 @@
+#ifndef MATHEXT_H
+#define MATHEXT_H
+
+#include "stdbool.h"
+#include "assert.h"
+#include "pervasives.h"
+
+#include "hept_ffi.h"
+
+DECLARE_HEPT_FUN(Mathext, float, (int), float o);
+DECLARE_HEPT_FUN(Mathext, int, (float), int o);
+DECLARE_HEPT_FUN(Mathext, floor, (float), float o);
+
+DECLARE_HEPT_FUN(Mathext, sin, (float), float o);
+DECLARE_HEPT_FUN(Mathext, cos, (float), float o);
+DECLARE_HEPT_FUN(Mathext, atan2, (float, float), float o);
+DECLARE_HEPT_FUN(Mathext, hypot, (float, float), float o);
+DECLARE_HEPT_FUN(Mathext, sqrt, (float), float o);
+
+DECLARE_HEPT_FUN(Mathext, modulo, (int, int), int o);
+
+#endif  /* MATHEXT_H */

src/mathext_types.h

@@ -0,0 +1,4 @@
+#ifndef MATHEXT_TYPES_H
+#define MATHEXT_TYPES_H
+
+#endif  /* MATHEXT_TYPES_H */

src/mymath.h

@@ -0,0 +1,13 @@
+#ifndef MYMATH_H
+#define MYMATH_H
+
+/* Avoid Heptagon's math.h. */
+#ifndef __APPLE__
+#include </usr/include/math.h>
+#endif
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#endif  /* MYMATH_H */

src/simulation_loop.c

@@ -0,0 +1,414 @@
+/* This file is part of SyncContest.
+   Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */
+
+#include "simulation_loop.h"
+
+#include <SDL.h>
+
+#include "mymath.h"
+#include "challenge.h"
+#include "cutils.h"
+#include "map.h"
+
+#ifndef ASSET_DIR_PATH
+#define ASSET_DIR_PATH "./assets"
+#endif
+
+void find_absolute_asset_path(char *path, size_t path_size,
+                              const char *filename) {
+  snprintf(path, path_size, "%s/%s", ASSET_DIR_PATH, filename);
+}
+
+void load_asset_wav(const char *filename, asset_wav_t *wav) {
+  assert (filename);
+  assert (wav);
+
+  char filepath[512];
+
+  find_absolute_asset_path(filepath, sizeof filepath, filename);
+  if (!SDL_LoadWAV(filepath, &wav->spec, &wav->buffer, &wav->size))
+    log_fatal("[sdl] could not open WAV file %s (%s)\n",
+              filepath, SDL_GetError());
+}
+
+void free_asset_wav(asset_wav_t *wav) {
+  assert (wav);
+  SDL_FreeWAV(wav->buffer);
+  wav->buffer = NULL;
+}
+
+SDL_Surface *load_asset_bmp_surface(const char *filename) {
+  SDL_Surface *r;
+  char filepath[512];
+
+  find_absolute_asset_path(filepath, sizeof filepath, filename);
+
+  if ((r = SDL_LoadBMP(filepath)) == NULL)
+    log_fatal("[sdl] could not load %s\n", filepath);
+
+  return r;
+}
+
+SDL_Texture *load_asset_bmp_texture(const char *filename, SDL_Renderer *r,
+                                    int *texture_width, int *texture_height) {
+  SDL_Surface *s = load_asset_bmp_surface(filename);
+  SDL_Texture *t = SDL_CreateTextureFromSurface(r, s);
+
+  if (!t)
+    log_fatal("[sdl] could not load texture from surface %s (%s)\n",
+              filename, SDL_GetError());
+  SDL_FreeSurface(s);
+
+  if (texture_width && texture_height
+      && SDL_QueryTexture(t, NULL, NULL, texture_width, texture_height))
+    log_fatal("[sdl] could not query texture %s (%s)\n",
+              filename, SDL_GetError());
+
+  log_info("[sdl] loaded texture %s\n", filename);
+  return t;
+}
+
+/* Our simulation assumes the origin is on the bottom-left corner of the window,
+   unlike SDL which assumes it is on the top-left corner. The function below
+   transforms a point from geometric space into SDL space. */
+void sdl_space_of_position(Globals__position *position, int *x, int *y) {
+  *x = position->x;
+  *y = MAX_Y - position->y;
+}
+
+void sdl_point_of_position(Globals__position *position, SDL_Point *point) {
+  sdl_space_of_position(position, &point->x, &point->y);
+}
+
+int draw_point(SDL_Renderer *rd, Globals__position *p) {
+  int x, y;
+  sdl_space_of_position(p, &x, &y);
+  return SDL_RenderDrawPoint(rd, x, y);
+}
+
+int draw_line(SDL_Renderer *rd,
+              Globals__position *startp, Globals__position *endp) {
+  int x1, y1, x2, y2;
+  sdl_space_of_position(startp, &x1, &y1);
+  sdl_space_of_position(endp, &x2, &y2);
+  return SDL_RenderDrawLine(rd, x1, y1, x2, y2);
+}
+
+void draw_rectangle(SDL_Renderer *rd, Globals__position *center, size_t l,
+                    uint32_t r, uint32_t g, uint32_t b) {
+  SDL_Rect rect = (SDL_Rect){ 0, 0, l, l };
+  /* Compute coordinates. */
+  sdl_space_of_position(center, &rect.x, &rect.y);
+  rect.x -= l / 2;
+  rect.y -= l / 2;
+  /* Render rectangle. */
+  SDL_SetRenderDrawColor(rd, r, g, b, 0x00);
+  if (SDL_RenderFillRect(rd, &rect) < 0)
+    log_fatal("[sdl] could not draw rectangle (%s)\n", SDL_GetError());
+  SDL_SetRenderDrawColor(rd, 0xFF, 0xFF, 0xFF, 0);
+  if (draw_point(rd, center) < 0)
+    log_fatal("[sdl] could not draw point (%s)\n", SDL_GetError());
+}
+
+void draw_tile(SDL_Renderer *rd,
+               SDL_Texture *texture,
+               Globals__position *p,
+               double angle,
+               int w,
+               int h) {
+  SDL_Rect dst_rect = { 0, 0, w, h };
+  Globals__position center = { p->x - w / 2, p->y + h / 2 };
+  sdl_space_of_position(&center, &dst_rect.x, &dst_rect.y);
+
+  SDL_RenderCopyEx(rd,             /* renderer */
+                   texture,        /* texture */
+                   NULL,           /* entire texture */
+                   &dst_rect,      /* destination */
+                   angle,          /* angle (cw) */
+                   NULL,           /* center dst_rec */
+                   SDL_FLIP_NONE); /* no flipping */
+}
+
+race_result_t simulation_loop(bool show_guide,
+                              int initial_top,
+                              float sps,
+                              bool headless,
+                              bool audio,
+                              size_t max_synchronous_steps) {
+  SDL_Window *w;
+  bool quit = false;                /* Shall we quit? */
+  race_result_t res = RACE_TIMEOUT; /* Did we complete the race? */
+  int top = initial_top;            /* Has the race started? */
+  bool verbose = false, debug = false;
+  int car_w, car_h, obs_w, obs_h;
+  SDL_Texture *bg, *car, *obs;
+  SDL_Renderer *r;
+  size_t current_tick = 0;
+
+  /* Initialize SDL and acquire resources. */
+
+  if (!headless) {
+    if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_AUDIO) < 0)
+      log_fatal("[sdl] could not initialize SDL library (%s)\n",
+                SDL_GetError());
+
+    if ((w = SDL_CreateWindow("Synchronous Contest " YEAR " v" VERSION,
+                              SDL_WINDOWPOS_UNDEFINED,
+                              SDL_WINDOWPOS_UNDEFINED,
+                              MAX_X,
+                              MAX_Y,
+                              SDL_WINDOW_SHOWN)) == NULL)
+      log_fatal("[sdl] could not open window (%s)\n", SDL_GetError());
+
+    r = SDL_CreateRenderer(w, -1, SDL_RENDERER_ACCELERATED);
+    if (!r)
+      log_fatal("[sdl] could not create renderer (%s)\n", SDL_GetError());
+
+    if (!r)
+      log_fatal("[sdl] could not create renderer (%s)\n", SDL_GetError());
+
+    bg = load_asset_bmp_texture(show_guide ? map->guide : map->graphics,
+                                r,
+                                NULL,
+                                NULL);
+    car = load_asset_bmp_texture("orange.bmp",
+                                 r,
+                                 &car_w,
+                                 &car_h);
+    obs = load_asset_bmp_texture("obst.bmp",
+                                 r,
+                                 &obs_w,
+                                 &obs_h);
+
+    /* Load sounds and setup audio device. */
+
+    load_asset_wav("collision.wav", &collision);
+    load_asset_wav("direction.wav", &wrong_dir);
+    load_asset_wav("exit.wav", &exit_road);
+    load_asset_wav("light.wav", &light_run);
+    load_asset_wav("speed.wav", &speed_excess);
+
+    if (audio) {
+      if (!(audio_device =
+            SDL_OpenAudioDevice(NULL, 0, &collision.spec, NULL, 0))) {
+        log_info("[sdl] could not open audio device\n");
+      } else
+        SDL_PauseAudioDevice(audio_device, 0);
+    }
+  }
+
+  /* Initialize synchronous state. */
+
+  Challenge__the_challenge_out out;
+  Challenge__the_challenge_mem mem;
+  Challenge__the_challenge_reset(&mem);
+
+  /* Setup time counters. */
+
+  const uint32_t sync_dt_ms = 1000.f * Globals__timestep;
+  const uint32_t simulation_dt_ms = (1. / (double)sps) * 1000.;
+  uint32_t time_budget_ms = sync_dt_ms; /* Enough to do one initial step. */
+
+  log_info("[simulation] starting (%zu ms/cycle)\n", simulation_dt_ms);
+
+  while (!quit
+         && (!max_synchronous_steps || current_tick < max_synchronous_steps)) {
+    SDL_Event e;
+    uint32_t start_time_ms = SDL_GetTicks();
+
+    /* Perform as many synchronous steps as possible within our budget. */
+    while (time_budget_ms >= sync_dt_ms
+           && (!max_synchronous_steps
+               || current_tick < max_synchronous_steps)) {
+      time_budget_ms -= sync_dt_ms;
+      Challenge__the_challenge_step(map->init_phase, top, &out, &mem);
+      current_tick++;
+      /* Check robot status once simulation has started. */
+      if (top) {
+        switch (out.sta) {
+        case Globals__Preparing:
+        case Globals__Running:
+          break;
+        case Globals__Arrived:
+          log_info("[simulation %08zu] race finished\n", current_tick);
+          res = RACE_SUCCESS;
+          quit = true;
+          break;
+        case Globals__Stopped:
+          log_info("[simulation %08zu] car stopped\n", current_tick);
+          res = RACE_CRASH;
+          quit = true;
+          break;
+        }
+      }
+      if (!debug && !verbose && !quit) {
+        printf("\e[?25l");  /* Disable cursor */
+        printf("H %06.2f\tV %06.2f\tT %06.2f\tS %09d\r",
+               out.ph.ph_head, out.ph.ph_vel, out.time, out.scoreA);
+        printf("\e[?25h");  /* Re-enable cursor */
+      }
+    }
+
+    if (!headless) {
+      /* Process events, including key presses. */
+      while (SDL_PollEvent(&e) != 0) {
+        switch (e.type) {
+        case SDL_QUIT:
+          quit = true;
+          break;
+        case SDL_KEYDOWN:
+          switch (e.key.keysym.sym) {
+          case SDLK_q:
+            quit = true;
+            break;
+          case SDLK_t:
+            top = true;
+            break;
+          case SDLK_d:
+            debug = !debug;
+            break;
+          case SDLK_v:
+            if (verbose)
+              log_set_verbosity_level(LOG_INFO);
+            else
+              log_set_verbosity_level(LOG_DEBUG);
+            verbose = !verbose;
+            break;
+          case SDLK_UP:
+            map->init_phase.ph_head += 2;
+            break;
+          case SDLK_DOWN:
+            map->init_phase.ph_head -= 2;
+            break;
+          }
+          break;
+        }
+      }
+
+      /* Render the scene, which includes the background as well as the car. */
+      SDL_SetRenderDrawColor(r, 0xFF, 0xFF, 0xFF, 0xFF);
+
+      SDL_RenderClear(r);
+      SDL_RenderCopy(r, bg, NULL, NULL);
+
+      if (!debug)
+        draw_tile(r, car, &out.ph.ph_pos, 360.f - out.ph.ph_head, car_w, car_h);
+      else {
+        /* In debug mode, render the car as a plain square. */
+        Globals__phase *ph; Globals__position endp; float f = 20.0;
+        if (top) {
+          ph = &out.ph;
+          f *= ph->ph_vel / SPEED_MAX;
+        } else
+          ph = &map->init_phase;
+        draw_rectangle(r, &ph->ph_pos, 5, 0x00, 0x00, 0x00);
+        /* Draw direction vector. */
+        endp.x = ph->ph_pos.x + f * cos(ph->ph_head / 360. * 2. * M_PI);
+        endp.y = ph->ph_pos.y + f * sin(ph->ph_head / 360. * 2. * M_PI);
+        SDL_SetRenderDrawColor(r, 0x00, 0x00, 0x00, 0x00);
+        draw_line(r, &ph->ph_pos, &endp);
+        /* Draw detected map vector. */
+        Map__lookup_pos_out out;
+        Map__lookup_pos_step(ph->ph_pos, &out);
+        endp.x = ph->ph_pos.x + out.data.dir_x;
+        endp.y = ph->ph_pos.y + out.data.dir_y;
+        SDL_SetRenderDrawColor(r, 0xAE, 0xB4, 0xC0, 0x00);
+        draw_line(r, &ph->ph_pos, &endp);
+        /* Reset color. */
+        SDL_SetRenderDrawColor(r, 0xFF, 0xFF, 0xFF, 0);
+      }
+
+      /* We draw the signalization info, when relevant. */
+      if (!debug) {
+        for (size_t i = 0; i < MAX_OBST_COUNT; i++) {
+          Globals__obstacle *o = &out.sign.si_obstacles[i];
+          if (o->o_pres)
+            draw_tile(r, obs, &o->o_pos, 0.f, obs_w, obs_h);
+        }
+
+        for (size_t i = 0; i < MAX_TL_COUNT; i++) {
+          Utilities__encode_color_out enc;
+          Globals__traflight *t = &out.sign.si_tlights[i];
+          Utilities__encode_color_step(t->tl_color, &enc);
+          draw_rectangle(r, &t->tl_pos, 10, enc.a.red, enc.a.green, enc.a.blue);
+        }
+      }
+
+      /* In debug mode, we also render the raw information coming from the
+         map. This is useful to understand the map file contents. */
+      if (debug) {
+        SDL_SetRenderDrawColor(r, 0x00, 0x00, 0xFF, 0xFF);
+        for (size_t i = 0; i < map->road_sz; i++) {
+          switch (map->road_arr[i].kind) {
+          case RD_LINE_1:
+            draw_line(r,
+                      &map->road_arr[i].u.line.startp,
+                      &map->road_arr[i].u.line.endp);
+            break;
+          default:
+            /* TODO draw curved roads. */
+            break;
+          }
+        }
+
+        /* Draw waypoints. */
+        for (size_t i = 0; i < map->wayp_sz; i++)
+          draw_rectangle(r, &map->wayp_arr[i].position,
+                         10, 0xFF, 0x00, 0x00);
+
+        /* Draw traffic lights. */
+        for (size_t i = 0; i < map->tlight_sz; i++)
+          draw_rectangle(r, &map->tlight_arr[i].tl.ptl_pos,
+                         10, 0x00, 0x00, 0xFF);
+
+        /* Draw stops. */
+        for (size_t i = 0; i < map->stop_sz; i++)
+          draw_rectangle(r, &map->stop_arr[i].position,
+                         10, 0x84, 0x21, 0xFF);
+
+        /* Draw obstacles. */
+        for (size_t i = 0; i < map->obst_sz; i++)
+          draw_rectangle(r, &map->obst_arr[i].pot_pos,
+                         10, 0x12, 0xAE, 0x00);
+      }
+
+      SDL_RenderPresent(r);
+    }
+
+    /* Sleep for our remaining per-simulation time. */
+    uint32_t stop_time_ms = SDL_GetTicks();
+    uint32_t frame_time_ms = stop_time_ms - start_time_ms;
+    if (frame_time_ms < simulation_dt_ms) {
+      log_debug("[simulation %08zu] %zu elapsed, sleeping for %zu ms\n",
+                current_tick, frame_time_ms, simulation_dt_ms - frame_time_ms);
+      SDL_Delay(simulation_dt_ms - frame_time_ms);
+    }
+
+    /* Accumulate time for the synchronous step. */
+    time_budget_ms += fmax(simulation_dt_ms, frame_time_ms);
+  }
+  log_info("[simulation %08zu] shutting down, score = %zu, time = %f\n",
+           current_tick, out.scoreA, out.time);
+
+  /* Wait for audio queue to be empty. */
+  if (audio_device) {
+    Uint32 audio_buffered;
+    while ((audio_buffered = SDL_GetQueuedAudioSize(audio_device)) != 0)
+      SDL_Delay(50);
+  }
+
+  free_asset_wav(&collision);
+  free_asset_wav(&wrong_dir);
+  free_asset_wav(&exit_road);
+  free_asset_wav(&light_run);
+  free_asset_wav(&speed_excess);
+
+  SDL_DestroyTexture(obs);
+  SDL_DestroyTexture(car);
+  SDL_DestroyTexture(bg);
+  SDL_DestroyRenderer(r);
+  SDL_DestroyWindow(w);
+  SDL_Quit();
+
+  return res;
+}

src/simulation_loop.h

@@ -0,0 +1,23 @@
+/* This file is part of SyncContest.
+   Copyright (C) 2017-2020 Eugene Asarin, Mihaela Sighireanu, Adrien Guatto. */
+
+#ifndef SIMULATION_LOOP_H
+#define SIMULATION_LOOP_H
+
+#include <stdbool.h>
+#include <stddef.h>
+
+typedef enum {
+  RACE_SUCCESS,
+  RACE_CRASH,
+  RACE_TIMEOUT
+} race_result_t;
+
+race_result_t simulation_loop(bool show_guide,
+                              int initial_top,
+                              float sps,
+                              bool headless,
+                              bool audio,
+                              size_t max_synchronous_steps);
+
+#endif  /* SIMULATION_LOOP_H */

src/trace.c

@@ -0,0 +1,70 @@
+#include "trace.h"
+
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "trace_lib.h"
+
+trace_file_t *trace = NULL;
+
+#define HEPT_TRACE_ENV_VAR "HEPT_TRACE"
+
+void hept_trace_init() {
+  if (trace || !getenv(HEPT_TRACE_ENV_VAR))
+    return;
+  trace = trace_file_alloc(TRACE_TIME_UNIT_S, 1);
+}
+
+void hept_trace_quit() {
+  if (trace) {
+    trace_file_write(trace, getenv(HEPT_TRACE_ENV_VAR));
+    trace_file_free(trace);
+    trace = NULL;
+  }
+}
+
+static inline void trace_samples(trace_signal_t **signal,
+                          const char *name, trace_signal_type_t type,
+                          void *samples, size_t count) {
+  if (!trace)
+    return;
+
+  if (!*signal) {
+    *signal = trace_file_lookup_signal(trace, name);
+    if (!*signal) {
+      *signal = trace_signal_alloc(name, type, 1 << 17);
+      if (!trace_file_add_signal(trace, *signal)) {
+        perror("trace_file_add_signal()\n");
+        exit(EXIT_FAILURE);
+      }
+    }
+  }
+  assert (*signal);
+  trace_add_samples(*signal, samples, count);
+}
+
+DEFINE_HEPT_NODE_RESET(Trace, trace_bool) {
+  mem->signal = NULL;
+}
+
+DEFINE_HEPT_NODE_STEP(Trace, trace_bool, (string name, int v)) {
+  trace_samples(&mem->signal, name, TRACE_SIGNAL_TYPE_BOOL, &v, 1);
+}
+
+DEFINE_HEPT_NODE_RESET(Trace, trace_int) {
+  mem->signal = NULL;
+}
+
+DEFINE_HEPT_NODE_STEP(Trace, trace_int, (string name, int v)) {
+  trace_samples(&mem->signal, name, TRACE_SIGNAL_TYPE_INT, &v, 1);
+}
+
+DEFINE_HEPT_NODE_RESET(Trace, trace_float) {
+  mem->signal = NULL;
+}
+
+DEFINE_HEPT_NODE_STEP(Trace, trace_float, (string name, float v)) {
+  trace_samples(&mem->signal, name, TRACE_SIGNAL_TYPE_FLOAT, &v, 1);
+}

src/trace.epi

@@ -0,0 +1,6 @@
+(* A basic library for producing VCD files from Heptagon code, typically used
+   for debugging purposes. *)
+
+external node trace_bool(name : string; v : bool) returns ()
+external node trace_int(name : string; v : int) returns ()
+external node trace_float(name : string; v : float) returns ()

src/trace.h

@@ -0,0 +1,17 @@
+#ifndef TRACE_H
+#define TRACE_H
+
+#include <stdbool.h>
+#include <stddef.h>
+
+#include "hept_ffi.h"
+#include "trace_lib.h"
+
+void hept_trace_init();
+void hept_trace_quit();
+
+DECLARE_HEPT_NODE(Trace, trace_bool, (string, int),, trace_signal_t *signal);
+DECLARE_HEPT_NODE(Trace, trace_int, (string, int),, trace_signal_t *signal);
+DECLARE_HEPT_NODE(Trace, trace_float, (string, float),, trace_signal_t *signal);
+
+#endif  /* TRACE */

src/trace_lib.c

@@ -0,0 +1,328 @@
+#include "trace_lib.h"
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+#include "buffer.h"
+
+size_t trace_sizeof_signal_type(trace_signal_type_t type) {
+  switch (type) {
+  case TRACE_SIGNAL_TYPE_BOOL:
+    return sizeof(int);
+  case TRACE_SIGNAL_TYPE_INT:
+    return sizeof(int);
+  case TRACE_SIGNAL_TYPE_FLOAT:
+    return sizeof(float);
+  }
+  /* UNREACHABLE */
+  assert (false);
+  return 0;
+}
+
+typedef struct trace_signal {
+  char *name;
+  trace_signal_type_t type;
+  buffer_t *samples;
+} trace_signal_t;
+
+trace_signal_t *trace_signal_alloc(const char *name,
+                               trace_signal_type_t type,
+                               size_t initial_buffer_size) {
+  trace_signal_t *res = malloc_checked(sizeof *res);
+  res->name = strdup_checked(name);
+  res->type = type;
+  res->samples =
+    buffer_alloc(initial_buffer_size * trace_sizeof_signal_type(type));
+  return res;
+}
+
+void trace_signal_free(trace_signal_t *signal) {
+  assert (signal);
+
+  buffer_free(signal->samples);
+  free(signal->name);
+  free(signal);
+}
+
+void trace_add_samples(trace_signal_t *signal, void *samples, size_t count) {
+  assert (signal);
+  assert (samples);
+  buffer_write(signal->samples,
+               samples,
+               count * trace_sizeof_signal_type(signal->type));
+}
+
+const char *trace_time_unit_repr(trace_time_unit_t u) {
+  const char *table[] = { "s", "ms", "us", "ns", "ps", "fs" };
+  assert (TRACE_TIME_UNIT_S <= u && u <= TRACE_TIME_UNIT_FS);
+  return table[u];
+}
+
+typedef struct trace_file {
+  buffer_t *signals;
+  trace_time_unit_t time_unit;
+  size_t time_unit_factor;
+} trace_file_t;
+
+trace_file_t *trace_file_alloc(trace_time_unit_t time_unit,
+                               size_t time_unit_factor) {
+  trace_file_t *trace = malloc_checked(sizeof *trace);
+  trace->time_unit = time_unit;
+  trace->time_unit_factor = time_unit_factor;
+  trace->signals = buffer_alloc(10 * sizeof(trace_signal_t));
+
+  return trace;
+}
+
+void trace_file_free(trace_file_t *trace) {
+  assert (trace);
+
+  /* Free buffers. */
+  buffer_foreach (trace_signal_t *, psig, trace->signals)
+    trace_signal_free(*psig);
+  buffer_free(trace->signals);
+
+  free(trace);
+}
+
+trace_signal_t *trace_file_lookup_signal(const trace_file_t *trace,
+                                         const char *signal_name) {
+  assert (trace);
+  assert (signal_name);
+
+  trace_signal_t *res = NULL;
+
+  buffer_foreach (trace_signal_t *, psig, trace->signals) {
+    if (strcmp((*psig)->name, signal_name) == 0) {
+      res = *psig;
+      break;
+    }
+  }
+
+  return res;
+}
+
+bool trace_file_add_signal(const trace_file_t *trace, trace_signal_t *signal) {
+  assert (trace);
+  assert (signal);
+
+  if (trace_file_lookup_signal(trace, signal->name))
+    return false;
+
+  buffer_write(trace->signals, &signal, sizeof signal);
+  return true;
+}
+
+typedef void (trace_backend_write_header_f)(FILE *, trace_file_t *);
+typedef void (trace_backend_write_cycle_beg_f)(FILE *, size_t);
+typedef void (trace_backend_write_cycle_end_f)(FILE *, size_t);
+typedef void (trace_backend_write_sample_f)(FILE *, trace_signal_t *, void *);
+typedef void (trace_backend_write_sample_missing_f)(FILE *, trace_signal_t *);
+
+typedef struct trace_backend {
+  char *file_extension;
+  trace_backend_write_header_f *write_header;
+  trace_backend_write_cycle_beg_f *write_cycle_beg;
+  trace_backend_write_cycle_end_f *write_cycle_end;
+  trace_backend_write_sample_f *write_sample;
+  trace_backend_write_sample_missing_f *write_sample_missing;
+} trace_backend_t;
+
+void trace_file_write_vcd_header(FILE *f, trace_file_t *trace) {
+  time_t current_time;
+  time(&current_time);
+
+  fprintf(f, "$version Generated by trace.c $end\n");
+  fprintf(f, "$date %s $end\n", ctime(&current_time));
+  fprintf(f, "$timescale %zu %s $end\n",
+          trace->time_unit_factor,
+          trace_time_unit_repr(trace->time_unit));
+
+  /* Dump signal declarations. */
+  fprintf(f, "$scope module Top $end\n");
+  buffer_foreach (trace_signal_t *, psig, trace->signals) {
+    fprintf(f, "$var ");
+    switch ((*psig)->type) {
+    case TRACE_SIGNAL_TYPE_BOOL:
+      fprintf(f, "wire 1");
+      break;
+    case TRACE_SIGNAL_TYPE_INT:
+      fprintf(f, "integer %zu", 8 * sizeof(int));
+      break;
+    case TRACE_SIGNAL_TYPE_FLOAT:
+      fprintf(f, "real 32");
+      break;
+    }
+    fprintf(f, " %p %s $end\n", (*psig), (*psig)->name);
+  }
+  fprintf(f, "$upscope $end\n");
+  fprintf(f, "$enddefinitions\n");
+
+  /* Dump samples. */
+  fprintf(f, "$dumpvars\n");
+}
+
+void trace_file_write_vcd_cycle_beg(FILE *f, size_t cycle) {
+  fprintf(f, "#%zu\n", cycle);
+}
+
+void trace_file_write_vcd_cycle_end(FILE *f, size_t cycle) {
+}
+
+void trace_file_write_vcd_sample(FILE *f, trace_signal_t *sig, void *sample) {
+  switch (sig->type) {
+  case TRACE_SIGNAL_TYPE_BOOL:
+    fprintf(f, "%d%p\n", (*(int *)sample ? 1 : 0), sig);
+    break;
+  case TRACE_SIGNAL_TYPE_INT:
+    fprintf(f, "r%d %p\n", *(int *)sample, sig);
+    break;
+  case TRACE_SIGNAL_TYPE_FLOAT:
+    fprintf(f, "r%.16g %p\n", *(float *)sample, sig);
+    break;
+  }
+}
+
+void trace_file_write_vcd_sample_missing(FILE *f, trace_signal_t *sig) {
+}
+
+void trace_file_write_csv_header(FILE *f, trace_file_t *trace) {
+  buffer_foreach (trace_signal_t *, psig, trace->signals) {
+    fprintf(f, "%s,", (*psig)->name);
+  }
+  fprintf(f, "\n");
+}
+
+void trace_file_write_csv_cycle_beg(FILE *f, size_t cycle) {
+}
+
+void trace_file_write_csv_cycle_end(FILE *f, size_t cycle) {
+  fprintf(f, "\n");
+}
+
+void trace_file_write_csv_sample(FILE *f, trace_signal_t *sig, void *sample) {
+  switch (sig->type) {
+  case TRACE_SIGNAL_TYPE_BOOL:
+    fprintf(f, "%d,", (*(int *)sample ? 1 : 0));
+    break;
+  case TRACE_SIGNAL_TYPE_INT:
+    fprintf(f, "%d,", *(int *)sample);
+    break;
+  case TRACE_SIGNAL_TYPE_FLOAT:
+    fprintf(f, "%f,", *(float *)sample);
+    break;
+  }
+}
+
+void trace_file_write_csv_sample_missing(FILE *f, trace_signal_t *sig) {
+  fprintf(f, "XXX,");
+}
+
+trace_backend_t backends[] = {
+  {
+    ".vcd",
+    trace_file_write_vcd_header,
+    trace_file_write_vcd_cycle_beg,
+    trace_file_write_vcd_cycle_end,
+    trace_file_write_vcd_sample,
+    trace_file_write_vcd_sample_missing,
+  },
+  {
+    ".csv",
+    trace_file_write_csv_header,
+    trace_file_write_csv_cycle_beg,
+    trace_file_write_csv_cycle_end,
+    trace_file_write_csv_sample,
+    trace_file_write_csv_sample_missing,
+  },
+};
+
+bool trace_file_write(trace_file_t *trace, const char *file_name) {
+  assert (trace);
+  assert (file_name);
+
+  FILE *f = fopen(file_name, "w");
+
+  if (!f)
+    return false;
+
+  trace_backend_t *backend = NULL;
+  char *file_ext;
+
+  /* Search for the backend. */
+
+  if (!(file_ext = strrchr(file_name, '.'))) {
+    fprintf(stderr, "[trace] could not determine file extension of %s\n",
+            file_name);
+    return false;
+  }
+
+  for (size_t i = 0; i < sizeof(backends) / sizeof(trace_backend_t); i++) {
+    if (!strcmp(backends[i].file_extension, file_ext)) {
+      backend = &backends[i];
+      break;
+    }
+  }
+
+  if (!backend) {
+    fprintf(stderr, "[trace] unknown file extension \"%s\"\n", file_ext);
+    return false;
+  }
+
+  /* Write header. */
+  backend->write_header(f, trace);
+
+  /* Write samples. */
+
+  /* We maintain an array of pointers to the current samples in each signal, and
+     walk through them as long as none of them is finished. */
+  size_t signal_count = trace->signals->occupancy / sizeof(trace_signal_t *);
+  unsigned char **psamples = calloc(signal_count, sizeof(unsigned char *));
+  assert (psamples);
+  for (size_t i = 0; i < signal_count; i++)
+    psamples[i] = ((trace_signal_t **)trace->signals->data)[i]->samples->data;
+
+  /* Loop until all signals have been depleted, consuming one sample from each
+     non-depleted signal at each cycle. */
+  for (size_t cycle = 0;; cycle++) {
+    /* Check if there is at least one active sample. */
+    bool active = false;
+    for (size_t i = 0; i < signal_count; i++) {
+      trace_signal_t *sig = ((trace_signal_t **)trace->signals->data)[i];
+      if (psamples[i] < sig->samples->data + sig->samples->occupancy) {
+        active = true;
+        break;
+      }
+    }
+
+    if (!active)
+      break;
+
+    /* Write cycle-start marker. */
+    backend->write_cycle_beg(f, cycle);
+
+    /* Write each sample, including missing ones. */
+    for (size_t i = 0; i < signal_count; i++) {
+      trace_signal_t *sig = ((trace_signal_t **)trace->signals->data)[i];
+
+      if (psamples[i] < sig->samples->data + sig->samples->occupancy) {
+        active = true;
+        backend->write_sample(f, sig, psamples[i]);
+        psamples[i] += trace_sizeof_signal_type(sig->type);
+      } else
+        backend->write_sample_missing(f, sig);
+    }
+
+    /* Write cycle-stop marker. */
+    backend->write_cycle_end(f, cycle);
+  }
+
+  free(psamples);
+
+  fclose(f);
+  return true;
+}

src/trace_lib.h

@@ -0,0 +1,47 @@
+#ifndef TRACE_LIB_H
+#define TRACE_LIB_H
+
+#include <stdbool.h>
+#include <stddef.h>
+
+typedef enum trace_signal_type {
+  TRACE_SIGNAL_TYPE_FLOAT,
+  TRACE_SIGNAL_TYPE_INT,
+  TRACE_SIGNAL_TYPE_BOOL
+} trace_signal_type_t;
+
+size_t trace_sizeof_signal_type(trace_signal_type_t);
+
+typedef struct trace_signal trace_signal_t;
+
+trace_signal_t *trace_signal_alloc(const char *name,
+                                   trace_signal_type_t type,
+                                   size_t initial_buffer_size);
+void trace_signal_free(trace_signal_t *signal);
+
+void trace_add_samples(trace_signal_t *signal, void *samples, size_t count);
+
+typedef enum trace_time_unit {
+  TRACE_TIME_UNIT_S,
+  TRACE_TIME_UNIT_MS,
+  TRACE_TIME_UNIT_US,
+  TRACE_TIME_UNIT_NS,
+  TRACE_TIME_UNIT_PS,
+  TRACE_TIME_UNIT_FS,
+} trace_time_unit_t;
+
+const char *trace_time_unit_repr(trace_time_unit_t);
+
+typedef struct trace_file trace_file_t;
+
+trace_file_t *trace_file_alloc(trace_time_unit_t time_unit,
+                               size_t time_unit_factor);
+void trace_file_free(trace_file_t *);
+
+trace_signal_t *trace_file_lookup_signal(const trace_file_t *trace,
+                                         const char *signal_name);
+bool trace_file_add_signal(const trace_file_t *trace, trace_signal_t *signal);
+
+bool trace_file_write(trace_file_t *, const char *file_name);
+
+#endif  /* TRACE_LIB_H */

src/utilities.ept

@@ -0,0 +1,216 @@
+open Globals
+
+(* Utility nodes *)
+
+fun min_float(a, b : float) returns (o : float)
+let
+  o = if a <. b then a else b;
+tel
+
+fun min_int(a, b : int) returns (o : int)
+let
+  o = if a < b then a else b;
+tel
+
+fun max_float(a, b : float) returns (o : float)
+let
+  o = if a <. b then b else a;
+tel
+
+fun max_int(a, b : int) returns (o : int)
+let
+  o = if a < b then b else a;
+tel
+
+node countdown(e : bool; ini : int) returns (o : int)
+var mem : int;
+let
+  mem = ini fby o;
+  o = mem - if e then 1 else 0;
+tel
+
+fun angle_dist(from, to : position) returns (angle, dist : float)
+let
+  dist = Mathext.hypot(to.x -. from.x, to.y -. from.y);
+  angle = Mathext.atan2(to.y -. from.y, to.x -. from.x) *. 180.0 /. pi;
+tel
+
+fun bound(x, ceiling : float) returns (o : float)
+let
+  o = if x <. -. ceiling then -. ceiling
+      else if x >. ceiling then ceiling
+      else x;
+tel
+
+fun norm_color(a : color) returns (norm : float)
+let
+  norm = Mathext.sqrt(Mathext.float(a.red) *. Mathext.float(a.red)
+                      +. Mathext.float(a.green) *. Mathext.float(a.green)
+                      +. Mathext.float(a.blue) *. Mathext.float(a.blue));
+tel
+
+fun compare_colors(a, b : color) returns (correlation : float)
+let
+  correlation = (Mathext.float(a.red) *. Mathext.float(b.red)
+                 +. Mathext.float(a.green) *. Mathext.float(b.green)
+                 +. Mathext.float(a.blue) *. Mathext.float(b.blue))
+                /. (norm_color(a) *. norm_color(b));
+tel
+
+(* Tries to interpret an RGB triple into a quantitative color. *)
+fun decode_color(a : color) returns (q : colorQ)
+var rr, gg, aa : float;
+let
+  rr = compare_colors(a, red);
+  gg = compare_colors(a, green);
+  aa = compare_colors(a, amber);
+  q = if rr >. 0.85 then Red
+      else if gg >. 0.85 then Green
+      else if aa >. 0.85 then Amber
+      else Other;
+tel
+
+(* Transforms a quantitative color into an RGB triple. *)
+fun encode_color(q : colorQ) returns (a : color)
+let
+  a = merge q (Red -> red) (Green -> green) (Amber -> amber) (Other -> gray);
+tel
+
+node rising_edge(b : bool) returns (e : bool)
+let
+  e = b and (false fby not b);
+tel
+
+node falling_edge(b : bool) returns (e : bool)
+let
+  e = false -> rising_edge(not b);
+tel
+
+node after(ini : int) returns (o : bool)
+var n : int;
+let
+  n = ini fby (if o then n else n - 1);
+  o = n = 0;
+tel
+
+node event_edge(e : event) returns (o : event)
+let
+  o = {
+        lightRun = rising_edge(e.lightRun);
+        speedExcess = rising_edge(e.speedExcess);
+        exitRoad = rising_edge(e.exitRoad);
+        collisionEvent = rising_edge(e.collisionEvent);
+        dirEvent = rising_edge(e.dirEvent)
+      };
+tel
+
+(* Integrates an input signal using trapezium formula. (1/s) *)
+node integrator(x, step, ini : float) returns (o : float)
+var s : float;
+let
+  s = ((ini -. (step *. x) /. 2.0) fby s) +. step *. x;
+  o = s -. (step *. x) /. 2.0;
+tel
+
+node integrator_en(x, step, ini : float; en : bool) returns (o : float)
+var oi : float :: . on en;
+let
+  oi = integrator(x when en, step when en, ini when en);
+  o = merge en oi ((ini whenot en) -> (pre o whenot en));
+tel
+
+node derivative(x, step : float) returns (y : float)
+let
+  y = (x -. 0.0 fby x) /. step;
+tel
+
+node lowpass(x, a : float) returns (y : float)
+let
+  y = a *. x +. (1.0 -. a) *. (0.0 fby y);
+tel
+
+(* Normalize an angle to the [-180, 180) interval. *)
+fun normalize(angle : float) returns (normal : float)
+let
+  normal = angle -. 360.0 *. Mathext.floor((angle +. 180.0) /. 360.0);
+tel
+
+fun pos2vec(pos : position) returns (vec : float^2)
+let
+  vec = [ pos.x, pos.y ];
+tel
+
+fun vec2pos(vec : float^2) returns (pos : position)
+let
+  pos = { x = vec[0]; y = vec[1] };
+tel
+
+fun mat_rot(alpha : float) returns (res : float^2^2)
+var si, co : float;
+let
+  si = Mathext.sin(alpha *. (pi /. 180.0));
+  co = Mathext.cos(alpha *. (pi /. 180.0));
+  res = [[co, -. si], [-. si, co]];
+tel
+
+fun vec_add<<n : int>>(v1, v2 : float^n) returns (o : float^n)
+let
+  o = map<<n>> ( +. )(v1, v2);
+tel
+
+fun vec_prod<<n : int>>(v1, v2 : float^n) returns (o : float^n)
+let
+  o = map<<n>> ( *. )(v1, v2);
+tel
+
+fun dotp<<n : int>>(v1, v2 : float^n) returns (o : float)
+let
+  o = fold<<n>> ( +. )(vec_prod<<n>>(v1, v2), 0.0);
+tel
+
+fun mat_vec_prod<<n : int>>(mat : float^n^n; vec : float^n)
+                   returns (o : float^n)
+let
+  o = map<<n>> (dotp<<n>>)(mat, vec^n);
+tel
+
+(* TODO The following two nodes are simple wrapper around the generic ones
+   defined below. They are needed since, as of October 24 2020, the Heptagon
+   compiler crashes when importing nodes with static parameters from another
+   module; see the GH issue below.
+
+   https://gitlab.inria.fr/synchrone/heptagon/-/issues/12
+
+   They should be removed once this issue has been fixed.
+*)
+
+fun vec_add2(v1, v2 : float^2) returns (o : float^2)
+let
+  o = vec_add<<2>>(v1, v2);
+tel
+
+fun mat_vec_prod2(mat : float^2^2; vec : float^2) returns (o : float^2)
+let
+  o = mat_vec_prod<<2>>(mat, vec);
+tel
+
+fun abs(x : float) returns (o : float)
+let
+  o = if x <. 0.0 then -. x else x;
+tel
+
+(* Time elapsed with |x| >= 1. *)
+node uptime(x, step : float) returns (t : float)
+let
+  t = integrator(if abs(x) >=. 1.0 then 1.0 else 0.0, step, 0.0);
+tel
+
+(* Variation of x (integral of |x'| over the time when enabled is true) adapted
+   for an angle normalized to [-180, 180). *)
+node variation(enabled : bool; x, step : float) returns (o : float)
+let
+  o = integrator_en(abs(normalize(x -. (0.0 fby x)) /. step),
+                    step,
+                    0.0,
+                    enabled);
+tel

src/vehicle.ept

@@ -0,0 +1,87 @@
+open Globals
+open Utilities
+
+(* Robot nodes *)
+
+fun car_geometry(phase : phase; vec : float^2)
+        returns (newpos : position; newphase : phase)
+let
+  newphase = { phase with .ph_pos = newpos };
+  newpos = vec2pos(vec_add2(pos2vec(phase.ph_pos),
+                                mat_vec_prod2(mat_rot(phase.ph_head),
+                                              vec)));
+tel
+
+node driver(top : bool; sens : sensors; itr : interrupt; iti : itielts)
+   returns (rspeed : wheels; sta : status)
+let
+  automaton
+    state Preparing
+      do rspeed = idlew;
+         sta = Preparing
+      until top then Running
+
+    state Running
+      var arriving : bool;
+      do (rspeed, arriving) = Control.controller(sens, iti);
+         sta = Running
+      until arriving then Arrived | itr = Halt then Stopped
+
+    state Arrived
+      do rspeed = idlew;
+         sta = Arrived
+
+    state Stopped
+      do rspeed = idlew;
+         sta = Stopped
+  end
+tel
+
+(* Kinematic model of the car. At the beginning the robot can be positionned
+   arbitrarily after "top" pressed, it runs according to physics. *)
+node physical_model(top : bool; rspeed : wheels; ini_ph : phase)
+           returns (ph : phase)
+var last alpha0 : float; last x0 : float; last y0 : float;
+let
+  automaton
+    state Positioning
+      var dummy : phase; pos : position;
+      do (pos, dummy) = car_geometry(ini_ph, [ -. cDELTA, 0.0]);
+         x0 = pos.x;
+         y0 = pos.y;
+         alpha0 = ini_ph.ph_head;
+         ph = { ini_ph with .ph_vel = 0.0 };
+      until top then On
+
+    state On
+      var si, co, alpha, vL, vR, v : float; dummy : position;
+      do si = Mathext.sin((alpha *. pi) /. 180.0);
+         co = Mathext.cos((alpha *. pi) /. 180.0);
+         vL = Utilities.bound(rspeed.left, cMAXWHEEL) *. ((pi *. cD) /. 360.0);
+         vR = Utilities.bound(rspeed.right, cMAXWHEEL) *. ((pi *. cD) /. 360.0);
+         v = (vL +. vR) /. 2.0;
+         alpha =
+           Utilities.normalize(Utilities.integrator(
+                (vR -. vL) *. 180.0 /. (pi *. cB),
+                timestep,
+                alpha0));
+         (dummy, ph) = car_geometry({
+                            ph_pos = {
+                              x = Utilities.integrator(v *. co, timestep, x0);
+                              y = Utilities.integrator(v *. si, timestep, y0)
+                            };
+                            ph_vel = v;
+                            ph_head = alpha
+                          }, [ cDELTA, 0.0 ]);
+  end
+tel
+
+node simulate(iti : itielts; sens : sensors;
+              itr : interrupt; ini_ph : phase;
+              top : bool)
+     returns (ph : phase; sta : status)
+var rspeed : wheels;
+let
+  (rspeed, sta) = driver(top, sens, itr, iti);
+  ph = physical_model(top, rspeed, ini_ph);
+tel