--** Robot simulation
--
--   @module robot3
--   @maintainer David Wood <D.C.Wood@kent.ac.uk>
--
--  [@text
--      +-------------------------------------------------<----------------------------+
--      |                                                                              |
--      |                                +----------------<-----------------+          |
--      |                                |                                  |          |
--      v                                v                                  |          |
--  +-------+             +-------+  +-------+  +-------+    +-------+  +-------+      |
--  |       |             |       |  |       |  |       |    |       |  |       |      |
--  | SDL   |------------>| scene |->| front |->| drone |..>.| drone |->| back  |------+
--  | Raster|             |       |  |       |  |       |    |       |  |       |
--  +-------+             +-------+  +-------+  +-------+    +-------+  +-------+
--    |   ^                              ^        ^   |        ^   |
--    |   |                              |        |   |        |   |
--    |   |                              |        |   v        |   v
--    |   |    +-------+                 |      +-------+    +-------+
--    |   +----|       |------->---------+      |       |    |       |
--    |        | event |----------->----------->|manual |    | auto  |
--    +------>-|       |                        |       |    |       |
--             +-------+                        +-------+    +-------+
--  ]

--  Tweaked by Fred Barnes 2008, Joystick support.
--  Incorporated various updates from Peter Welch (2008), 2009.


#INCLUDE "course.module"
#INCLUDE "sdlraster.module"
#INCLUDE "occSDL.module"
#INCLUDE "rastergraphics.module"
#INCLUDE "rasterio.module"
#INCLUDE "maths.module"
#INCLUDE "file.module"                            -- for file.get.options


--*{{{  BUTTON Mouse buttons.

VAL INT BUTTON.LEFT     IS 1:
VAL INT BUTTON.MIDDLE   IS 2:
VAL INT BUTTON.RIGHT    IS 3:
VAL INT BUTTON.FOREWARD IS 4:
VAL INT BUTTON.BACKWARD IS 5:

--*}}}

VAL INT N.ROBOTS          IS   128:               --* Default number of robots.

VAL INT BACKGROUND        IS COLOUR.BLACK:        --* Default background colour.
VAL INT FOREGROUND        IS COLOUR.WHITE:        --* Default foreground colour.

VAL INT WIDTH             IS  1012:               --* Width of raster.
VAL INT HEIGHT            IS  703:                --* Depth of raster.
VAL INT N.RASTERS         IS    3:                --* Number of rasters.

VAL INT MID.RAY           IS   16:                --* index of middle ray.
VAL INT N.RAYS            IS (MID.RAY * 2) + 1:   --* number of rays.
VAL ANGLE SCAN            IS 120 * ANGLE.DEGREE:  --* laser scan sweep.

DATA TYPE RANGES IS MOBILE []INT:                 --* Array of ranges.

--* Information returned from drone
PROTOCOL LASER IS RANGES:               --* Range measurements.

--* State of robot
DATA TYPE STATE
  RECORD
    INT X, Y:                           --* Position.
    ANGLE theta:                        --* Direction.
    INT linear.velocity:                --* Linear velocity.
    ANGLE angular.velocity:             --* Angular velocity.
    INT colour:                         --* Colour.
:

--* Movement instructions (from brain to wheels).
--
-- @item Linear velocity.
-- @item Angular velocity.
PROTOCOL MOVE IS INT; ANGLE:

--* Update commands for manual control.
PROTOCOL UPDATE
  CASE
    State; STATE			--* Set state
    Position; INT; INT                  --* Change position
    Motion; INT; ANGLE                  --* Change motion
:

--* Commands to drone
PROTOCOL COMMANDS
  CASE
    position; INT; INT
    faster
    slower
    clockwise
    anti.clockwise
    set.motion; INT; INT
    halt
    freeze
:

--* User control (of simulation).
PROTOCOL USER.CONTROL
  CASE
    freeze
    no.rays
    centre.ray
    rays
:

--* Simulation parameters.
DATA TYPE OPTIONS
  RECORD
    BOOL mixed:                   --* True if we should use mixed version 1 and 2 robots.
    BOOL rcolour:                 --* True if robots should be randomly coloured regardless.
    INT version:                  --* Version of robot to use (1 or 2).
    INT ran.scale:                --* Laser range scale (default 128).
    INT rad.scale:                --* Robot radius scale (default 4).
    INT fps:                      --* Frames-per-second limiting.
    BOOL show.fps:                --* True if frames-per-second should be calculated and shown.
    BOOL rplace:                  --* Random initial placement.
    BOOL ps3:                     --* True if using PS3 controllers (joystick) to drive.
:

--* Default simulation parameters.
VAL OPTIONS DEFAULT.OPTIONS IS [TRUE, FALSE, 0, 128, 4, 0, FALSE, FALSE, FALSE]:


--{{{  ANGLE INLINE FUNCTION ABS (VAL ANGLE angle)
ANGLE INLINE FUNCTION ABS (VAL ANGLE angle)

  ANGLE result:
  VALOF
    IF
      angle < 0
        result := -angle
      TRUE
        result := angle
    RESULT result
:

--}}}
--{{{  PROC set.scene (RASTER raster)
--* Set scene (in an individual raster).
--
--  @param raster Draws the scene here
--
PROC set.scene (RASTER raster)

  VAL INT d IS SIZE raster:
  VAL INT w IS SIZE raster[0]:

  SEQ

    clear.raster (raster, COLOUR.BLACK)

    #IF FALSE

    fill.circle ((200*w) >> 10, (300*d) >> 10, (225*d) >> 10, FOREGROUND, raster)
    fill.circle ((200*w) >> 10, (300*d) >> 10, (150*d) >> 10, BACKGROUND, raster)
    fill.rectangle ((175*w) >> 10, (300*d) >> 10, (50*w) >> 10, (230*d) >> 10, BACKGROUND, raster)

    fill.rectangle ((500*w) >> 10, (300*d) >> 10, (400*w) >> 10, (450*d) >> 10, FOREGROUND, raster)
    fill.rectangle ((550*w) >> 10, (300*d) >> 10, (100*w) >> 10, (375*d) >> 10, BACKGROUND, raster)
    fill.rectangle ((640*w) >> 10, (525*d) >> 10, (208 *w) >> 10, (150*d) >> 10, BACKGROUND, raster)
    fill.rectangle ((800*w) >> 10, (375*d) >> 10,  (50*w) >> 10, (300*d) >> 10, BACKGROUND, raster)
    fill.rectangle ((700*w) >> 10, (375*d) >> 10, (150*w) >> 10,  (75*d) >> 10, BACKGROUND, raster)
    fill.rectangle ((700*w) >> 10, (300*d) >> 10,  (50*w) >> 10, (150*d) >> 10, BACKGROUND, raster)

    fill.rectangle (0, (700*d) >> 10, (350*w) >> 10,  (75*d) >> 10, FOREGROUND, raster)
    fill.circle ((350*w) >> 10, (738*d) >> 10, (75*d) >> 10, FOREGROUND, raster)
                                                
    fill.circle (w, 0, (225*d) >> 10, FOREGROUND, raster)
    fill.circle (w, 0, (150*d) >> 10, BACKGROUND, raster)

    #ELSE

    read.raster ("robot-track-2.png", raster)

    #ENDIF

:


--}}}
--{{{  PROC scene (CHAN RASTER in?, out!)
--* Set scene (in a stream of rasters).
--
--  @param in Receive blank raster
--  @param out Send raster with scene
--
PROC scene (CHAN RASTER in?, out!)
  RASTER master.scene, raster:
  SEQ
    in ? raster                       -- needed only for its dimensions
    master.scene := CLONE raster      -- construct master with same size
    set.scene (master.scene)          -- build the scene just once
    out ! CLONE master.scene          -- output a copy (keeping original)

    WHILE TRUE
      SEQ
        in ? raster                   -- replace with the ony built earlier
        out ! CLONE master.scene      -- output a copy (keeping original)
:

--}}}
--{{{  PROC fan (...)
--* Draw a fan of lines radiating from a point.
--
--  Stop when they reach a distance of [@code r.max],
--  or hit something of colour [@code c.0]
--  or the edge of the raster.
--
--  Number of rays = [@code SIZE ranges]
--
--  @param x.0, y.0 Centre
--  @param r.max Maximum range
--  @param theta Central direction
--  @param phi Half-angle
--  @param c.0 Stop when it hits this colour
--  @param c.1 Colour of fan
--  @param ranges Return ranges of rays
--  @param raster Reaster
--
PROC fan (VAL INT x.0, y.0, r.max, VAL ANGLE theta, phi, VAL INT c.0, c.1, RESULT RANGES ranges, RASTER raster)

  VAL INT n IS SIZE ranges:
  VAL INT n1 IS n MINUS 1:
  VAL ANGLE delta IS 2 * (phi / n1):

  INT x, y, d.x, d.y:
  SEQ
    INITIAL ANGLE psi IS theta - phi:
    SEQ k = 0 FOR n
      SEQ
        x, y := x.0, y.0
        d.x, d.y := CORDIC (r.max, 0, psi)
        draw.line (x, y, d.x, d.y, c.0, c.1, raster)
        psi := psi + delta
        #PRAGMA DEFINED ranges                            -- avoid kroc defined analysis bug
        ANGLE junk:
        ranges[k], junk := CIDROC (x MINUS x.0, y MINUS y.0)
    #PRAGMA DEFINED ranges                                -- avoid kroc defined analysis bug
    x, y := x.0, y.0
    d.x, d.y := CORDIC (r.max, 0, theta)
    draw.line (x, y, d.x, d.y, c.0, COLOUR.RED, raster)   -- show which way we're going
:
--}}}

--{{{  PROC drone (...)
--* This is the eyes and wheels of a robot.
--
--  In each cycle, it (attempts to) move, then uses its scanner to observe
--  neighbourhood, passes range information back to its brain and receives
--  revised instructions for linear and angular velocities.
--
--  It scans the raster image of its environment by drawing itself (so that
--  other robots can see it) and generating rays angled out forwards.
--  Each raster image passes three times through all these drones.
--
--  Pass 1: draw robot in [@code FOREGROUND] colour.
--  Pass 2: erase robot (so it won't see itself), draw the rays (stopping if
--          they hit the [@code FOREGROUND] colour) and, then,
--          re-draw robot - again in [@code FOREGROUND] colour.
--  Pass 3: re-draw in own colour.
--
--  @param initial.state Initial state of this robot.
--  @param laser.range Range of the laser range-finder.
--  @param robot.radius Radius of the robot.
--  @param move Receive movement instructions.
--  @param laser Send what we see.
--  @param in Receive raster.
--  @param out Send raster.
--  @param show.centre.ray Whether or not to show central ray.
--
PROC drone (VAL STATE initial.state, VAL INT laser.range, robot.radius,
            CHAN MOVE move?, CHAN LASER laser!, CHAN RASTER in?, out!,
            BOOL show.centre.ray)
  
  INITIAL STATE state IS initial.state:

  INT x IS state[X]:                    -- unpack state (for speed)
  INT y IS state[Y]:
  ANGLE theta IS state[theta]:
  INT linear.velocity IS state[linear.velocity]:
  ANGLE angular.velocity IS state[angular.velocity]:
  INT colour IS state[colour]:

  WHILE TRUE

    RASTER raster:                      -- mobile
    RANGES ranges:                      -- mobile

    SEQ

      ranges := MOBILE [N.RAYS]INT      -- new array constructed each cycle!

      INT d.x, d.y:
      BOOL hit:
      SEQ

        IF                              -- attempt to move
          linear.velocity < 0
            linear.velocity, theta := -linear.velocity, ~theta
          TRUE
            SKIP
        theta := theta PLUS angular.velocity
        d.x, d.y := CORDIC (linear.velocity, 0, theta)
        x, y := x + d.x, y + d.y

        -- Pass 1
        in ? raster
        fill.circle (x, y, robot.radius, FOREGROUND, raster)        -- mark as target
        out ! raster

        -- Pass 2
        in ? raster
        fill.circle (x, y, robot.radius, BACKGROUND, raster)        -- unmark self
        fan (x, y, laser.range, theta, SCAN/2,                      -- generate rays
             FOREGROUND, COLOUR.YELLOW, ranges, raster)
        laser ! ranges                                              -- send to brain
        hit := circle.hits (x, y, robot.radius, FOREGROUND, raster)
        fill.circle (x, y, robot.radius, FOREGROUND, raster)        -- mark again
        out ! raster

        -- Pass 3
        in ? raster
        fill.circle (x, y, robot.radius, colour, raster)
        IF
          show.centre.ray
            SEQ
              d.x, d.y := CORDIC (laser.range, 0, theta)
              INITIAL INT x IS x:
              INITIAL INT y IS y:
              draw.line (x, y, d.x, d.y, FOREGROUND, COLOUR.YELLOW, raster)   -- show which way we're going
          TRUE
            SKIP
        IF
          hit          -- indicate we hit something
            SEQ
              fill.circle (x, y, robot.radius - 3, COLOUR.BLACK, raster)
              fill.circle (x, y, robot.radius - 6, COLOUR.WHITE, raster)
          TRUE
            SKIP
        out ! raster

      move ? linear.velocity; angular.velocity                      -- receive from brain

:
--}}}

--{{{  INT FUNCTION compute.speed (VAL INT range, robot.radius)
--* Speed as a function of distance and robot footprint.
--
--  Proportional to [@code log (distance)].
--
--  @param range Distance.
--  @param robot.radius Robot radius.
--  @return Speed.
--
INT FUNCTION compute.speed (VAL INT range, robot.radius)
  INT speed:
  VALOF
    IF
      IF i = 0 FOR 7
        range <= (robot.radius << i)
          speed := i
      TRUE
        speed := 7
    RESULT speed
:

--}}}
--{{{  INT FUNCTION min (VAL []INT ranges)
--* Minimum range
--
--  @param ranges Array of ranges
--  @return Minimum range
--
INT FUNCTION min (VAL []INT ranges)

  INT result:
  VALOF
    SEQ
      result := ranges[0]
      SEQ i = 0 FOR SIZE ranges
        IF
          ranges[i] < result
            result := ranges[i]
          TRUE
            SKIP
    RESULT result
:
--}}}

--{{{  PROC brain.0 (...)
--* Drive robot automatically.
--
--  Go in circles.
--  Slow down as minimum range ([@code robot.radius]) approaches.
--  Reverse direction on contact.
--
--  @param id Unique identifier for this robot.
--  @param initial.linear.velocity Initial linear velocity.
--  @param initial.angular.velocity Initial angular velocity.
--  @param robot.radius Robot radius.
--  @param move Send movement instructions to drone.
--  @param laser Receive ranges to any obstacles in our path.
--
PROC brain.0 (VAL INT id, initial.linear.velocity, VAL ANGLE initial.angular.velocity,
              VAL INT robot.radius, CHAN MOVE move!, CHAN LASER laser?)

  INITIAL INT linear.velocity IS initial.linear.velocity:
  INITIAL ANGLE angular.velocity IS initial.angular.velocity:
  VAL ANGLE increment IS ANGLE.DEGREE/10:
  VAL ANGLE min.av IS - (10 * ANGLE.DEGREE):
  VAL ANGLE max.av IS 10 * ANGLE.DEGREE:
  INITIAL BOOL decreasing IS TRUE:
  WHILE TRUE
    RANGES ranges:
    SEQ
      laser ? ranges
      VAL INT velocity IS compute.speed (min (ranges), robot.radius):
      IF
        velocity = 0
          SEQ
            IF
              linear.velocity < 0
                linear.velocity := 1
              TRUE
                linear.velocity := -1
            -- angular.velocity := -angular.velocity
            -- decreasing := NOT decreasing
            --linear.velocity := -linear.velocity
        TRUE
          linear.velocity := velocity
      IF
        decreasing
          SEQ
            angular.velocity := angular.velocity MINUS increment
            IF
              angular.velocity < min.av
                angular.velocity, decreasing := min.av, FALSE
              TRUE
                SKIP
        TRUE
          SEQ
            angular.velocity := angular.velocity PLUS increment
            IF
              angular.velocity > max.av
                angular.velocity, decreasing := max.av, TRUE
              TRUE
                SKIP
      move ! linear.velocity; angular.velocity
:


--}}}
--{{{  PROC brain.1 (...)
--* Drive robot automatically.
--
--  You decide ... :)
--  But you must avoid obstacles and keep moving ...
--
--  @param id Unique identifier for this robot.
--  @param initial.linear.velocity Initial linear velocity
--  @param initial.angular.velocity Initial angular velocity
--  @param robot.radius Robot radius
--  @param move Send movement instructions to drone
--  @param laser Receive ranges to any obstacles in our path
--
PROC brain.1 (VAL INT id, VAL INT initial.linear.velocity, VAL ANGLE initial.angular.velocity,
              VAL INT robot.radius, CHAN MOVE move!, CHAN LASER laser?)

  brain.0 (id, initial.linear.velocity, initial.angular.velocity,      -- you can do better than this!
           robot.radius, move!, laser?)

:
--}}}

--{{{  PROC manual (...)
--* Drive robot manually.
--
--  @param initial.linear.velocity Initial linear velocity.
--  @param initial.angular.velocity Initial angular velocity.
--  @param robot.radius Radius of the robot.
--  @param commands Received commands (to drive the robot around).
--  @param move Commands to move robot.
--  @param laser Received laser ranges.
--  @param opts Simulation parameters.
--
PROC manual (VAL INT initial.linear.velocity, VAL ANGLE initial.angular.velocity,
              VAL INT robot.radius, CHAN COMMANDS commands?, CHAN MOVE move!,
              CHAN LASER laser?, VAL OPTIONS opts)

  INITIAL INT linear.velocity IS initial.linear.velocity:
  INITIAL ANGLE angular.velocity IS initial.angular.velocity:


  VAL ANGLE D.W IS ANGLE.DEGREE / 2:

  SEQ

    INITIAL BOOL seen.setm IS FALSE:
    WHILE TRUE
      RANGES ranges:
      INITIAL BOOL flag IS FALSE:
      INT x, y:
      SEQ
        laser ? ranges
        --{{{  poll for commands or skip and compute new things
        PRI ALT
          commands ? CASE
            position; x; y
              flag := TRUE
            faster
              angular.velocity := angular.velocity + D.W
            slower
              angular.velocity := angular.velocity - D.W
            clockwise
              angular.velocity :=  ABS (angular.velocity)
            anti.clockwise
              angular.velocity := -ABS (angular.velocity)
            halt
              linear.velocity, angular.velocity := 0, 0
            freeze
              commands ? CASE freeze

            INT ang, fwd:
            set.motion; ang; fwd
              SEQ
                IF
                  opts[ps3]
                    SEQ
                      -- use values raw
                      linear.velocity, angular.velocity := fwd, (ANGLE ang)
                  TRUE
                    SEQ
                      linear.velocity := fwd / 7
                      angular.velocity := (ANGLE.DEGREE / 15) * (ANGLE ang)

                -- make sure we don't run into scenary!
                VAL INT speed IS compute.speed (min (ranges), robot.radius):
                IF
                  speed < linear.velocity
                    linear.velocity := speed
                  TRUE
                    SKIP

                seen.setm := TRUE

          SKIP
            IF
              NOT seen.setm
                VAL INT speed IS compute.speed (min (ranges), robot.radius):
                IF
                  speed = 0
                    linear.velocity := 0
                  TRUE
                    linear.velocity := speed
              TRUE
                VAL INT speed IS compute.speed (min (ranges), robot.radius):
                IF
                  speed < linear.velocity
                    linear.velocity := speed
                  TRUE
                    SKIP
        --}}}
        IF
          flag
            -- update ! Position; x; y
            -- FIXME: this is missing!
            SKIP
          TRUE
            move ! linear.velocity; angular.velocity

:

--}}}
--{{{  PROC event.handler (CHAN RASTER.EVENT events?, SHARED CHAN COMMANDS commands!)
--* Handle SDL events.
--
--  @param events Receive SDL events.
--  @param commands Send commands to robot driver.
--  @param control User control signal.
--
PROC event.handler (CHAN RASTER.EVENT events?, SHARED CHAN COMMANDS commands!, CHAN USER.CONTROL control!)

  WHILE TRUE
    RASTER.EVENT event:
    SEQ
      events ? event
      CASE event[type]
        --{{{  RASTER.ET.KEYDOWN
        RASTER.ET.KEYDOWN			-- normal keys
          CASE event[id]
            'q', 'x'
              STOP
            'a'
              CLAIM commands!
                commands ! anti.clockwise
            'c'
              CLAIM commands!
                commands ! clockwise
            'f'
              CLAIM commands!
                commands ! faster
            's'
              CLAIM commands!
                commands ! slower
            'h'
              CLAIM commands!
                commands ! halt
            ' '
              SEQ
                control ! freeze
                RASTER.EVENT any:
                SEQ
                  events ? any
                  WHILE any[type] <> RASTER.ET.KEYDOWN
                    events ? any
                control ! freeze
            '0'
              control ! no.rays
            '1'
              control ! centre.ray
            'r'
              control ! rays
            INT SDLK.LEFT
              CLAIM commands!
                commands ! anti.clockwise
            INT SDLK.RIGHT
              CLAIM commands!
                commands ! clockwise
            INT SDLK.UP
              CLAIM commands!
                commands ! faster
            INT SDLK.DOWN
              CLAIM commands!
                commands ! slower
            ELSE
              SKIP

        --}}}
        --{{{  RASTER.ET.MOUSEBUTTONDOWN
        RASTER.ET.MOUSEBUTTONDOWN		-- mouse buttons
          SEQ
            CASE event[id]
              BUTTON.LEFT
                SKIP
              BUTTON.MIDDLE
                CLAIM commands!
                  commands ! position; INT event[x]; INT event[y]
              BUTTON.RIGHT
                SKIP
              BUTTON.FOREWARD
                CLAIM commands!
                  commands ! faster
              BUTTON.BACKWARD
                CLAIM commands!
                  commands ! slower
              ELSE
                SKIP

        --}}}
        --{{{  RASTER.ET.QUIT
        RASTER.ET.QUIT			-- click on [X]
          STOP

        --}}}
        ELSE				-- anything else?
          SKIP
:
--}}}
PROC frame.timer (CHAN RASTER in?, out!, VAL INT fps, VAL BOOL show.fps, SHARED CHAN BYTE screen!) --{{{

  VAL INT TIME.COUNT IS 100:
  INITIAL INT count IS 0:
  
  TIMER tim:
  INT t.0, t.1, t.step, t.t0:

  RASTER r:
  SEQ

    IF
      fps = 0
        t.step := 0
      TRUE
        t.step := 1000000 / fps

    tim ? t.0
    t.t0 := t.0

    WHILE TRUE
      SEQ
        in ? r
        IF
          t.step > 0
            SEQ
              t.t0 := t.t0 PLUS t.step
              tim ? AFTER t.t0
          TRUE
            SKIP
        out ! r

        count := count + 1
        IF
          NOT show.fps
            SKIP
          count = TIME.COUNT
            INT time.difference:
            SEQ
              tim ? t.1
              VAL INT time.difference IS t.1 MINUS t.0:
              VAL INT frame.time IS time.difference / (TIME.COUNT*1000):
              CLAIM screen!
                SEQ
                  out.string ("+++ Frame time = ", 0, screen!)
                  out.int (frame.time, 0, screen!)
                  out.string (" ms*c*n", 0, screen!)
              count := 0
              tim ? t.0
              t.t0 := t.0
          TRUE
            SKIP
              
:
--}}}


--{{{  PROC joystick.handler (CT.SDLJSEVENT! ev, SHARED CHAN COMMANDS commands!)
--* Joystick handler.
--
-- @param ev Received joystick events.
-- @param commands Generated commands.
--
PROC joystick.handler (CT.SDLJSEVENT! ev, SHARED CHAN COMMANDS commands!)
  INITIAL BOOL loop IS TRUE:
  WHILE loop
    SEQ
      ev[out] ? CASE
        INT ax.delta, bt.delta:
        MOBILE []INT16 ax.vals:
        MOBILE []BYTE bt.vals:
        change; ax.delta; bt.delta; ax.vals; bt.vals
          IF
            (SIZE ax.vals) >= 2
              -- update with normalised axis values
              SEQ
                IF
                  ax.vals[1] < 0
                    ax.vals[1] := 0                 -- can't go backwards!
                  TRUE
                    SKIP
                CLAIM commands!
                  commands ! set.motion; INT ax.vals[0]; INT ax.vals[1]
            TRUE
              SKIP
        quit
          loop := FALSE
            
:
--}}}
--{{{  PROC joystick.handler.ps3 (CT.SDLJSEVENT! ev, SHARED CHAN COMMANDS commands!)
--* Joystick handler for Sony PlayStation3 controllers.
-- This uses the first two axes to compute the direction we want to travel in,
-- and the right trigger to control speed.
--
-- @param ev Received joystick events.
-- @param commands Generated commands.
--
PROC joystick.handler.ps3 (CT.SDLJSEVENT! ev, SHARED CHAN COMMANDS commands!)
  INITIAL BOOL loop IS TRUE:
  INITIAL ANGLE a.last IS 0:
  WHILE loop
    SEQ
      ev[out] ? CASE
        INT ax.delta, bt.delta:
        MOBILE []INT16 ax.vals:
        MOBILE []BYTE bt.vals:
        change; ax.delta; bt.delta; ax.vals; bt.vals
          IF
            (SIZE ax.vals) >= 14
              -- update with normalised axis values
              INT vel, ang, rad:
              ANGLE a:
              SEQ
                vel := 0
                IF
                  (ax.vals[0] <> 0) OR (ax.vals[1] <> 0)
                    SEQ
                      rad, a := CIDROC ((INT ax.vals[0]), (INT ax.vals[1]))
                      IF
                        rad < 1024
                          a := a.last
                        TRUE
                          vel := rad / 6000
                  TRUE
                    rad, a := 0, a.last

                ang := INT (a MINUS a.last)
                a.last := a

                CLAIM commands!
                  commands ! set.motion; ang; vel
            TRUE
              SKIP
        quit
          loop := FALSE
            
:
--}}}
--{{{  PROC front (...)
--* front of pipeline
--
--  @param in.1 Receive new raster
--  @param in.2 Receive raster for next phase
--  @param out Send raster
--  @param control User control signal
--
PROC front (CHAN RASTER in.1?, in.2?, out!, CHAN USER.CONTROL control?,
            BOOL show.centre.ray)

  INITIAL BOOL show.rays IS TRUE:
  INITIAL BOOL my.show.centre.ray IS show.centre.ray:

  WHILE TRUE

    PRI ALT

      control ? CASE
        freeze
          control ? CASE freeze
        no.rays
          show.rays, my.show.centre.ray := FALSE, FALSE
        centre.ray
          show.rays, my.show.centre.ray := FALSE, TRUE
        rays
          show.rays, my.show.centre.ray := TRUE, FALSE

      RASTER raster:
      in.1 ? raster
        SEQ
          IF
            show.centre.ray <> my.show.centre.ray
              show.centre.ray := my.show.centre.ray
            TRUE
              SKIP
          IF
            show.rays
              SEQ
                out ! raster
                in.2 ? raster
                out ! raster
                in.2 ? raster
                out ! raster
            TRUE
              RASTER original:
              SEQ
                original := CLONE raster
                out ! raster
                in.2 ? raster
                out ! raster
                in.2 ? raster
                out ! original

:
--}}}
--{{{  PROC back (CHAN RASTER in?, out.1!, out.2!)
--* tail of pipeline
--
--  @param in Receive raster
--  @param out.1 Send raster for next phase
--  @param out.2 Send raster completed raster
--
PROC back (CHAN RASTER in?, out.1!, out.2!)

  WHILE TRUE
    RASTER raster:
    SEQ
      SEQ i = 0 FOR 2
        SEQ
          in ? raster
          out.1 ! raster
      in ? raster
      out.2 ! raster
:
--}}}
--{{{  PROC joystick.setup.and.handler (SHARED CHAN COMMANDS commands!, VAL OPTIONS opts, SHARED CHAN BYTE screen!)
--* Does joystick setup and handling.
--
-- @param commands Commands to control robot.
-- @param opts Simulation options.
-- @param screen Shared screen channel.
--
PROC joystick.setup.and.handler (SHARED CHAN COMMANDS commands!, VAL OPTIONS opts, SHARED CHAN BYTE screen!)
  CT.SDLJOY! j.cli:
  SEQ
    occ.SDL.joy.start (j.cli)

    --{{{  joystick setup and handler
    IF
      DEFINED j.cli
        SEQ
          j.cli[in] ! connect; 0
          j.cli[out] ? CASE
            SHARED CT.SDLJS! stick:
            CT.SDLJSEVENT! ev, ev2:
            CT.SDLJSCALIBRATE! cal:
            joystick; stick
              SEQ
                CLAIM stick!
                  SEQ
                    stick[in] ! enable
                    stick[out] ? CASE event.chan; ev

                IF
                  opts[ps3]
                    --{{{  PS3 controllers are solid and don't require calibration :)
                    joystick.handler.ps3 (ev, commands!)

                    --}}}
                  TRUE
                    --{{{  ordinary control wheels do need calibration
                    SEQ
                      occ.SDL.joy.calibrator (ev, ev2, cal)

                      CHAN BOOL skip:
                      PAR
                        SEQ
                          occ.SDL.joy.docalibrate (cal!, screen!)
                          skip ! FALSE
                        INITIAL BOOL l.loop IS TRUE:
                        WHILE l.loop
                          PRI ALT
                            skip ? l.loop
                              SKIP
                            ev2[out] ? CASE
                              INT axd, btd:
                              MOBILE []INT16 ax.vals:
                              MOBILE []BYTE bt.vals:
                              change; axd; btd; ax.vals; bt.vals
                                SKIP
                              quit
                                SKIP
                      -- now pass events through to manual controller
                      joystick.handler (ev2, commands!)
                    --}}}
            error
              CLAIM screen!
                out.string ("No joysticks found, keyboard control only!*n", 0, screen!)
      TRUE
        CLAIM screen!
          out.string ("Failed to initialise SDL for joystick handling!*n", 0, screen!)
    --}}}
:
--}}}

--{{{  VAL []INT CLUT
--* Colour lookup table (used by [@code place.robots] below)
VAL []INT CLUT IS [COLOUR.RED, COLOUR.GREEN, COLOUR.BLUE,
                   COLOUR.MAGENTA, COLOUR.CYAN, COLOUR.ORANGE, COLOUR.PURPLE,
                   COLOUR.PINK, COLOUR.SKY, COLOUR.NAVY, COLOUR.MAROON,
                   COLOUR.VIOLET, COLOUR.BROWN, COLOUR.GREY]:
--}}}
--{{{  PROC place (VAL INT robot.radius, INT x, y, [][]INT raster, INT seed, VAL OPTIONS opts)
--* place a robot safely on the raster
--
--  @param robot.radius The radius of the robot footprint.
--  @param x Where the robot gets placed (x-coord).
--  @param y Where the robot gets placed (y-coord).
--  @param raster This must have the background in place.
--  @param seed Random number seed.
--  @param opts Simulation options.
--
PROC place (VAL INT robot.radius, INT x, y, [][]INT raster, INT seed, VAL OPTIONS opts)
  SEQ
    VAL INT width IS SIZE raster[0]:
    VAL INT height IS SIZE raster:
    INITIAL BOOL looking IS TRUE:
    IF
      opts[rplace]
        SEQ
          WHILE looking
            SEQ
              x, seed := random (width, seed)
              y, seed := random (height, seed)
              looking := circle.hits (x, y, robot.radius, FOREGROUND, raster)
          #PRAGMA DEFINED x, y                       -- avoid kroc defined analysis warning
          fill.circle (x, y, robot.radius, FOREGROUND, raster)
      TRUE
        INT x1, x2, y1, y2:
        SEQ
          x1, x2 := 457+robot.radius, 556-robot.radius
          y1, y2 := 272+robot.radius, 370-robot.radius

          WHILE looking
            INITIAL INT l.count IS 100:             -- try this many times to get a convenient placement, then try anywhere
            SEQ
              WHILE looking AND (l.count > 0)
                SEQ
                  x, seed := random ((x2 - x1), seed)
                  y, seed := random ((y2 - y1), seed)
                  looking := circle.hits (x1+x, y1+y, robot.radius, FOREGROUND, raster)
                  l.count := l.count - 1
              IF
                l.count = 0
                  SEQ
                    -- failed to place in designated area, expand range
                    x1 := x1 - (robot.radius * 3)
                    x2 := x2 + (robot.radius * 3)
                    y1 := y1 - (robot.radius * 3)
                    y2 := y2 + (robot.radius * 3)
                    IF
                      x1 < 0
                        x1 := 0
                      TRUE
                        SKIP
                    IF
                      x2 >= width
                        x2 := width - 1
                      TRUE
                        SKIP
                    IF
                      y1 < 0
                        y1 := 0
                      TRUE
                        SKIP
                    IF
                      y2 >= height
                        y2 := height - 1
                      TRUE
                        SKIP
                TRUE
                  SKIP

          #PRAGMA DEFINED x, y                       -- avoid kroc defined analysis warning
          x := x + x1
          y := y + y1
          fill.circle (x, y, robot.radius, FOREGROUND, raster)
:
--}}}
--{{{  PROC place.robots ([N.ROBOTS]STATE s, VAL INT raster.width, raster.height, robot.radius, VAL OPTIONS opts)
--* randomly place all the robots in empty space on the raster
--
--  @param s The robot states (to be set by this procedure)
--  @param raster.width Width of raster
--  @param raster.height Hieght of raster
--  @param robot.radius Radius of the robot
--  @param opts Simulation options.
--
PROC place.robots ([N.ROBOTS]STATE s, VAL INT raster.width, raster.height, robot.radius, VAL OPTIONS opts)

  INT seed:
  RASTER raster:
  
  SEQ
    
    raster := MOBILE [raster.height][raster.width]INT
    clear.raster (raster, COLOUR.BLACK)
    set.scene (raster)
    
    TIMER tim:                               -- initialise random number seed
    tim ? seed
    seed := (seed >> 2) + 1

    SEQ i = 0 FOR N.ROBOTS                   -- placement of robots

      STATE s.i IS s[i]:
      INT rand:
      SEQ

        place (robot.radius, s.i[X], s.i[Y], raster, seed, opts)

        s.i[linear.velocity] := 1
        rand, seed := random (INT ANGLE.RIGHT, seed)
        IF
          i = 0
            s.i[theta] := 0                   -- manual drone must point the right way
          TRUE
            s.i[theta] := ANGLE (2 * rand)
        rand, seed := random (6, seed)
        rand := ((rand - 3) * 2) + 1         -- odd (zero is bad)
        s.i[angular.velocity] := rand * ANGLE.DEGREE

        IF
          opts[rcolour]
            SEQ
              rand, seed := random (SIZE CLUT, seed)
              s.i[colour] := CLUT[rand]
          opts[mixed]
            IF
              (i /\ 1) = 0
                s.i[colour] := COLOUR.RED
              TRUE
                s.i[colour] := COLOUR.GREEN
          TRUE
            SEQ
              rand, seed := random (SIZE CLUT, seed)
              s.i[colour] := CLUT[rand]

:
--}}}

--{{{  PROC welcome (CHAN BYTE out!)
--* Prints the welcome message.
--
-- @param out Screen output channel (probably).
--
PROC welcome (CHAN BYTE out!)
  SEQ
    erase.screen (out!)                 -- clear screen
    cursor.x.y (1, 1, out!)             -- cursor at top-left

    out.string ("Robot (v3), Copyright (C) 2007-2009  D.C.Wood and P.H.Welch*c*n", 0, out!)
    out.string ("Modifications Copyright (C) 2009 F.R.M.Barnes*c*n", 0, out!)
    out.string ("==========================================================*c*n*n", 0, out!)
:
--}}}

PROC process.options (OPTIONS opts, RESULT BOOL continue, CHAN BYTE screen!) --{{{

  PROC usage (CHAN BYTE out!) --{{{
    SEQ
      out.string ("usage: robot3 [options]*n", 0, out!)
      out.string ("where options are:*n", 0, out!)
      out.string ("  -m    mixed version 1 and 2 robots (default)*n", 0, out!)
      out.string ("  -1    version 1 robots only*n", 0, out!)
      out.string ("  -2    version 2 robots only*n", 0, out!)
      out.string ("  -h    show this help*n", 0, out!)
      out.string ("  -c    randomly coloured robots*n", 0, out!)
      out.string ("  -r N  set robot radius scale*n", 0, out!)
      out.string ("  -l N  set laser range scale*n", 0, out!)
      out.string ("  -f N  set limit on frames-per-second*n", 0, out!)
      out.string ("  -s    show frames-per-second*n", 0, out!)
      out.string ("  -p    random initial placement*n", 0, out!)
      out.string ("  -P    using playstation3 joystick*n", 0, out!)
  :
  --}}}

  CHAN GETOPT opt:
  PAR
    file.get.options ("mhc12l:r:f:spP", opt!)
    SEQ
      continue := TRUE

      INITIAL BOOL loop IS TRUE:
      WHILE loop
        opt ? CASE
          BYTE o:
          opt; o
            CASE o
              'm'
                opts[mixed] := TRUE
              'h'
                SEQ
                  usage (screen!)
                  continue := FALSE
              '1'
                opts[mixed], opts[version] := FALSE, 1
              '2'
                opts[mixed], opts[version] := FALSE, 2
              'c'
                opts[rcolour] := TRUE
              's'
                opts[show.fps] := TRUE
              'p'
                opts[rplace] := TRUE
              'P'
                opts[ps3] := TRUE
          BYTE o:
          MOBILE []BYTE val:
          opt.arg; o; val
            CASE o
              'r'
                BOOL p.ok:
                SEQ
                  p.ok, opts[rad.scale] := string.to.int (val)
                  IF
                    NOT p.ok
                      SEQ
                        out.string ("bad integer value *'", 0, screen!)
                        out.string (val, 0, screen!)
                        out.string ("*'*n", 0, screen!)
                        continue := FALSE
                    TRUE
                      SKIP
              'l'
                BOOL p.ok:
                SEQ
                  p.ok, opts[ran.scale] := string.to.int (val)
                  IF
                    NOT p.ok
                      SEQ
                        out.string ("bad integer value *'", 0, screen!)
                        out.string (val, 0, screen!)
                        out.string ("*'*n", 0, screen!)
                        continue := FALSE
                    TRUE
                      SKIP
              'f'
                BOOL p.ok:
                SEQ
                  p.ok, opts[fps] := string.to.int (val)
                  IF
                    NOT p.ok
                      SEQ
                        out.string ("bad integer value *'", 0, screen!)
                        out.string (val, 0, screen!)
                        out.string ("*'*n", 0, screen!)
                        continue := FALSE
                    TRUE
                      SKIP
          MOBILE []BYTE val:
          arg; val
            SEQ
              out.string ("unrecognised argument *'", 0, screen!)
              out.string (val, 0, screen!)
              out.string ("*'*n", 0, screen!)
              continue := FALSE
          BYTE o:
          bad; o
            SEQ
              out.string ("unrecognised option *'", 0, screen!)
              screen ! o
              out.string ("*'*n", 0, screen!)
              continue := FALSE
          done
            loop := FALSE
:
--}}}

--{{{  PROC main (CHAN BYTE keyboard?, SHARED CHAN BYTE screen!)
--* Top-level process
--
-- @param keyboard Console keyboard channel.
-- @param screen Shared console screen channel.
--
PROC main (CHAN BYTE keyboard?, SHARED CHAN BYTE screen!)

  [N.ROBOTS]STATE s:

  OPTIONS opts:

  INT raster.width, raster.height:
  INT laser.range, robot.radius:

  BOOL continue:
  SEQ

    CLAIM screen!
      welcome (screen!)

    -- this is fixed in this version (image file)
    raster.width := WIDTH
    raster.height := HEIGHT

    -- set defaults
    opts := DEFAULT.OPTIONS

    CLAIM screen!
      process.options (opts, continue, screen!)

    laser.range := (opts[ran.scale]*raster.width) >> 10
    robot.radius := (opts[rad.scale]*raster.width) >> 10


    IF
      continue
        SEQ
          place.robots (s, raster.width, raster.height, robot.radius, opts)
          #PRAGMA DEFINED s

          s[0][colour] := COLOUR.WHITE

          -- all set ... let's go!

          CHAN RASTER source, done, return:
          CHAN RASTER.EVENT events:
          CHAN USER.CONTROL control:
          CHAN RASTER b, c:
          SHARED! CHAN COMMANDS commands:

          [N.ROBOTS + 1]CHAN RASTER d:
          [N.ROBOTS]CHAN MOVE h:
          [N.ROBOTS]CHAN LASER j:

          BOOL show.centre.ray:
          #PRAGMA SHARED show.centre.ray

          PAR

            raster.display ("Robots (v3)", raster.width, raster.height, N.RASTERS, return?, source!, events!)
            event.handler (events?, commands!, control!)

            scene (source?, b!)

            front (b?, c?, d[0]!, control?, show.centre.ray)

            --{{{  automatic robots [1 .. (N.ROBOTS-1)]
            PAR i = 1 FOR N.ROBOTS - 1
              PAR
                drone (s[i], laser.range, robot.radius, h[i]?, j[i]!, d[i]?, d[i + 1]!, show.centre.ray)
                VAL INT linear.velocity IS s[i][linear.velocity]:
                VAL ANGLE angular.velocity IS s[i][angular.velocity]:
                IF
                  opts[mixed]
                    IF
                      (i /\ 1) = 0
                        brain.0 (i, linear.velocity, angular.velocity, robot.radius, h[i]!, j[i]?)
                      TRUE
                        brain.1 (i, linear.velocity, angular.velocity, robot.radius, h[i]!, j[i]?)
                  opts[version] = 1
                    brain.0 (i, linear.velocity, angular.velocity, robot.radius, h[i]!, j[i]?)
                  opts[version] = 2
                    brain.1 (i, linear.velocity, angular.velocity, robot.radius, h[i]!, j[i]?)
            --}}}
            --{{{  manual control [0]
            PAR
              drone (s[0], laser.range, robot.radius, h[0]?, j[0]!, d[0]?, d[1]!, show.centre.ray)
              manual (0, 0, robot.radius, commands?, h[0]!, j[0]?, opts)

            --}}}
            --{{{  joystick control
            joystick.setup.and.handler (commands!, opts, screen!)

            --}}}
            back (d[N.ROBOTS]?, c!, done!)
            frame.timer (done?, return!, opts[fps], opts[show.fps], screen!)
      TRUE
        SKIP

:
--}}}

