--** Robot simulation
--
--   @module Robot
--   @maintainer David Wood <D.C.Wood@kent.ac.uk>
--
--  [@text
--                 +--------------------------------------<----------------------------+
--                 |                                                                   |
--                 |                     +----------------<-----------------+          |
--                 |                     |                                  |          |
--                 v                     v                                  |          |
--  +-------+  +-------+  +-------+  +-------+  +-------+    +-------+  +-------+  +-------+
--  |       |  |       |  |       |  |       |  |       |    |       |  |       |  |       |
--  | SDL   |->|Source |->| Scene |->| Head  |->| Drone |..>.| Drone |->| Tail  |->|Display|
--  | Raster|  |       |  |       |  |       |  |       |    |       |  |       |  |       |
--  +-------+  +-------+  +-------+  +-------+  +-------+    +-------+  +-------+  +-------+
--    |   ^                                       ^   |        ^   |
--    |   |                                       |   |        |   |
--    |   |                                       |   v        |   v
--    |   |    +-------+                        +-------+    +-------+
--    |   +----|       |                        |       |    |       |
--    |        | Event |----------->----------->|Manual |    | Auto  |
--    +------>-|       |                        |       |    |       |
--             +-------+                        +-------+    +-------+
--  ]

--  To do:
--  * Scale angles etc. to reduce quantization errors
--  * More/different commands for Manual driver
--  * Different Auto algorithms

#INCLUDE "course.module"
#INCLUDE "sdlraster.module"

#INCLUDE "graphics.inc"
#INCLUDE "hit.inc"

--*{ 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 BACKGROUND IS COLOUR.BLACK:
VAL INT FOREGROUND IS COLOUR.WHITE:

VAL INT WIDTH   IS 1001:		--* Width of raster
VAL INT DEPTH   IS  651:		--* Depth of raster
VAL INT RASTERS IS    4:		--* Number of rasters
VAL INT RANGE   IS  256:		--* Range of laser/radar/sonar
VAL INT RADIUS  IS    8:		--* Size of robot
VAL INT MID     IS   16:		--* index of middle ray
VAL INT RAYS    IS (MID * 2) + 1:	--* number of rays

DATA TYPE RANGES IS [RAYS]INT:		--* array of ranges

#INCLUDE "fan.inc"

--* Information returned from drone

PROTOCOL INFO IS RANGES:		--* and/or Hit?

--* State of robot

DATA TYPE STATE
  RECORD
    INT X, Y:                           --* Position
    INT V:                              --* Linear velocity
    ANGLE Theta:                        --* Direction
    ANGLE W:                            --* Angular velocity
    INT Colour:
:

--* Update state or motion

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				--*
    Anticlockwise			--*
    Halt				--*
:

--* Source of rasters
--
--  @param delay Time between frames
--  @param in.1 Receive new raster
--  @param in.2 Receive recycled raster
--  @param out Send raster

PROC Source (VAL INT delay, CHAN RASTER in?, out!)

  TIMER tim:
  INT t:
  RASTER raster:
  SEQ
    SEQ i = 0 FOR RASTERS
      SEQ
        in ? raster
        out ! raster
    tim ? t
    INITIAL INT c IS COLOUR.RED:
    WHILE TRUE
      RASTER raster:
      SEQ
        t := t PLUS delay
        tim ? AFTER t
        in ? raster
        clear.raster (raster, #000000)
        Rectangle (5, 5, 10, 10, c, raster)	-- tell-tail
        c := (COLOUR.RED + COLOUR.GREEN) - c
        out ! raster
:

--* Set scene
--
--  @param in Receive blank raster
--  @param out Send raster with scene

PROC Scene (CHAN RASTER in?, out!)

  RASTER raster:
  WHILE TRUE
    SEQ
      in ? raster

      Disc (200, 200, 150, FOREGROUND, raster)
      Disc (200, 200, 100, BACKGROUND, raster)
      Rectangle (175, 200, 50, 200, BACKGROUND, raster)

      Rectangle (500, 200, 400, 300, FOREGROUND, raster)
      Rectangle (550, 200, 100, 250, BACKGROUND, raster)
      Rectangle (600, 350, 200, 100, BACKGROUND, raster)
      Rectangle (800, 250,  50, 200, BACKGROUND, raster)
      Rectangle (700, 250, 100,  50, BACKGROUND, raster)
      Rectangle (700, 200,  50,  50, BACKGROUND, raster)

      Rectangle (  0, 450, 350,  50, FOREGROUND, raster)
      Disc (350, 475, 50, FOREGROUND, raster)

      Disc (WIDTH, 0, 150, FOREGROUND, raster)
      Disc (WIDTH, 0, 100, BACKGROUND, raster)

      out ! raster
:

--* Minimum range

--  @param ranges Array of ranges
--  @return Minimum range

INT FUNCTION Min (VAL RANGES 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
:

--* Draw robot.
--
--  [@item] Pass 1: Draw robot in [@code FOREGROUND] colour;
--  [@item] Pass 2: Delete robot (so it won't see itself);
--          Detect ranges, including other robots;
--          Re-draw in [@code FOREGROUND] colour (so others can see it);
--  [@item] Pass 3: Re-draw in own colour.
--  [@item] Update state.
--
--  @param update Receive updated state
--  @param info Signal hit
--  @param in Receive raster
--  @param out Send raster

PROC Drone (CHAN UPDATE update?, CHAN INFO info!, CHAN RASTER in?, out!)

  STATE s:
  SEQ
    update ? CASE			-- initialize
      State; s
        SKIP
    WHILE TRUE
      RASTER raster:
      RANGES ranges:			-- should be mobile
      SEQ
        INT x IS s[X]:
        INT y IS s[Y]:
        INT v IS s[V]:
        ANGLE theta IS s[Theta]:
        INITIAL ANGLE w IS s[W]:
        INT colour IS s[Colour]:
        INT d.x, d.y:
        BOOL hit:
        SEQ

          in ? raster
          Disc (x, y, RADIUS, FOREGROUND, raster)	-- mark as target
          out ! raster

          in ? raster
          Disc (x, y, RADIUS, BACKGROUND, raster)	-- unmark self
          Fan (x, y, RANGE, theta, ANGLE.RIGHT / 2,
               FOREGROUND, COLOUR.YELLOW, ranges, raster)
          Disc (x, y, RADIUS, FOREGROUND, raster)	-- mark again
          out ! raster

          in ? raster
          Disc (x, y, RADIUS, colour, raster)
          hit := Hit (x, y, RADIUS, FOREGROUND, raster)
          IF
            hit
              SEQ
                Disc (x, y, RADIUS - 3, COLOUR.BLACK, raster)
                Disc (x, y, RADIUS - 6, COLOUR.WHITE, raster)
            TRUE
              SKIP
          out ! raster

          IF
            v < 0
              v, theta := -v, ~theta
            TRUE
              SKIP
          theta := theta PLUS w
          d.x, d.y := Cordic (v, 0, theta)
          x, y := x + d.x, y + d.y
          info ! ranges

        update ? CASE
          State; s
            SKIP
          Position; s[X]; s[Y]
            SKIP
          Motion; s[V]; s[W]
            SKIP
:

--* Speed as a function of distance.
--
--  Proportional to [@code log (distance)]
--
--  @param range Distance
--  @return Speed

INT FUNCTION Speed (VAL INT range)

  INT result:
  VALOF
    IF i = 0 FOR 7
      range <= (RADIUS << i)
        result := i
    RESULT result
:

--* Drive robot automatically.
--
--  Slow down as minimum range approaches [@code RADIUS];
--  Reverse direction on contact.
--
--  @param s Initial state
--  @param update Send update to state
--  @param info Receive info

PROC Auto (VAL STATE s, CHAN UPDATE update!, CHAN INFO info?)

  SEQ
    update ! State; s			-- initialize drone
    INITIAL INT   v IS s[V]:		-- only needs to know these
    INITIAL ANGLE w IS s[W]:
    WHILE TRUE
      RANGES ranges:
      SEQ
        info ? ranges
        VAL INT speed IS Speed (Min (ranges)):
        IF
          speed = 0
            v := -v
          TRUE
            v := speed
        update ! Motion; v; w
:

ANGLE INLINE FUNCTION ABS (VAL ANGLE w)

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

--* Drive robot manually.
--
--  @param s Initial state
--  @param commands Receive commands
--  @param update Send update to state
--  @param info Receive info

PROC Manual (VAL STATE s, CHAN COMMANDS commands?, CHAN UPDATE update!, CHAN INFO info?)

  SEQ
    update ! State; s			-- initialize drone
    INITIAL INT   v IS s[V]:		-- only needs to know these
    INITIAL ANGLE w IS s[W]:
    WHILE TRUE
      RANGES ranges:
      INITIAL BOOL flag IS FALSE:
      INT x, y:
      SEQ
        info ? ranges
        PRI ALT
          commands ? CASE
            Position; x; y
              flag := TRUE
            Faster
              w := w + ANGLE.DEGREE
            Slower
              w := w - ANGLE.DEGREE
            Clockwise
              w :=  ABS (w)
            Anticlockwise
              w := -ABS (w)
            Halt
              v, w := 0, 0
          SKIP
            VAL INT speed IS Speed (Min (ranges)):
            IF
              speed = 0
                v, w := 0, 0
              TRUE
                v := speed
        IF
          flag
            update ! Position; x; y
          TRUE
            update ! Motion; v; w
:

--* Handle SDL events
--
--  @param events Receive SDL events
--  @param commands Send commands to robot driver

--  Not realistic, but demonstrates mouse clicks, contol characters,
--  and ordinary characters.

PROC Events (CHAN RASTER.EVENT events?, CHAN COMMANDS commands!)

  RASTER.EVENT event:
  WHILE TRUE
    SEQ
      events ? event
      CASE event[type]
        RASTER.ET.KEYDOWN			-- normal keys
          VAL INT key IS (event[id] /\ #FF):
          VAL INT LOWER IS 'a' >< 'A':
          CASE key \/ LOWER
            'q', 'x'
              STOP
            'a'
              commands ! Anticlockwise
            'c'
              commands ! Clockwise
            'f'
              commands ! Faster
            's'
              commands ! Slower
            'h'
              commands ! Halt
            ELSE
              SKIP

        RASTER.ET.MOUSEBUTTONDOWN	-- mouse buttons
          INT x, y:
          SEQ
            x, y := INT event[x], INT event[y]
            CASE event[id]
              1
                commands ! Position; x; y
              2
                SKIP
              3
                SKIP
              4
                commands ! Faster
              5
                commands ! Slower
              ELSE
                SKIP

        RASTER.ET.SHUTDOWN		-- click on [X]
          STOP

        ELSE				-- anything else?
          SKIP
:

--* Head of pipeline

--  @param in.1 Receive new raster
--  @param in.2 Receive raster for next phase
--  @param out Send raster

PROC Head (CHAN RASTER in.1?, in.2?, out!)

  WHILE TRUE
    RASTER raster:
    SEQ
      in.1 ? raster
      out ! raster
      SEQ i = 0 FOR 2
        SEQ
          in.2 ? raster
          out ! raster
:

--* Tail of pipeline

--  @param in Receive raster
--  @param out.1 Send raster for next phase
--  @param out.2 Send raster completed raster

PROC Tail (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
:

VAL INT SECOND IS 1000000:		--*

--* Main program

PROC Main (SHARED CHAN BYTE screen!)

  --* Initial state of robots

  --                      X    Y   V Theta W  Colour
  VAL []STATE state IS [[950,  50, 0,  0, 20, COLOUR.WHITE  ],
                        [200, 250, 1,  0,  0, COLOUR.RED    ],
                        [900, 550, 1,  0,  0, COLOUR.GREEN  ],
                        [650,  50, 1,  0,  0, COLOUR.BLUE   ],
                        [750, 100, 1,  0,  0, COLOUR.MAGENTA],
                        [ 50,  50, 1,  0,  0, COLOUR.YELLOW ],
                        [775, 275, 1,  0,  0, COLOUR.CYAN   ],
                        [575, 425, 1,  0,  0, COLOUR.ORANGE ],
                        [100, 550, 1,  0, -0, COLOUR.PURPLE ],
                        [150, 550, 1,  0,  0, COLOUR.PINK   ],
                        [200, 550, 1,  0,  0, COLOUR.SKY    ],
                        [250, 550, 1,  0,  0, COLOUR.NAVY   ],
                        [300, 550, 1,  0,  0, COLOUR.MAROON ],
                        [350, 550, 1,  0,  0, COLOUR.VIOLET ],
                        [400, 550, 1,  0,  0, COLOUR.BROWN  ],
                        [450, 550, 1,  0,  0, COLOUR.GREY   ]]:
  VAL INT ROBOTS IS SIZE state:
  VAL INT DELAY IS SECOND / 25:
  TIMER tim:
  INT rand, seed:
  INITIAL [ROBOTS]STATE s IS state:
  SEQ
    tim ? seed
    seed := (seed >> 2) + 1
    SEQ i = 1 FOR ROBOTS - 1
      SEQ
        rand, seed := random (INT ANGLE.RIGHT, seed)
        s[i][Theta] := ANGLE (2 * rand)
        rand, seed := random (6, seed)
        rand := ((rand - 3) * 2) + 1	-- odd (zero is bad)
        s[i][W] := rand * ANGLE.DEGREE

    CHAN RASTER source, return:
    CHAN RASTER.EVENT events:
    CHAN COMMANDS commands:
    CHAN RASTER a, b, c, e:
    [ROBOTS]CHAN RASTER d:
    [ROBOTS]CHAN UPDATE h:
    [ROBOTS]CHAN INFO j:
    PAR
      raster.display ("Robots", WIDTH, DEPTH, RASTERS, return?, source!, events!)
      Source (DELAY, source?, a!)
      Scene (a?, b!)
      Head (b?, e?, c!)
      Drone (h[0]?, j[0]!, c?, d[0]!)
      -- Auto (s[0], h[0]!, j[0]?)
      Manual (s[0], commands?, h[0]!, j[0]?)
      Events (events?, commands!)
      PAR i = 1 FOR ROBOTS - 1
        PAR
          Drone (h[i]?, j[i]!, d[i - 1]?, d[i]!)
          Auto (s[i], h[i]!, j[i]?)
      Tail (d[ROBOTS - 1]?, e!, return!)
:
