
--** Robot exercise (your task is to re-program brain.1)  :)
--
--   Based on a original design by David Wood <D.C.Wood@kent.ac.uk>, 2007.
--   Hacked around a bit by Peter Welch <P.H.Welch@kent.ac.uk>, 2008.
--
--  [@text
--      +-------------------------------------------------<-----------------------------+
--      |                                                                               |
--      |                                +----------------<------------------+          |
--      |                                |                                   |          |
--      v                                v                                   |          |
--  +--------+            +-------+  +-------+  +-------+     +-------+  +-------+      |
--  |        |            |       |  |       |  |       |     |       |  |       |      |
--  | mini.  |----------->| scene |->| front |->| drone |..>..| drone |->| back  |------+
--  | raster |            |       |  |       |  |       |     |       |  |       |
--  +--------+            +-------+  +-------+  +-------+     +-------+  +-------+
--    |   ^                              ^        ^   |         ^   |
--    |   |                              |        |   |         |   |
--    |   |                              |        |   v         |   v
--    |   |    +---------------+         |      +-------+     +-------+
--    |   +----|               |         |      |       |     |       |
--    |        | event.handler |--->-----+      | brain | ... | brain |
--    +------>-|               |                |       |     |       |
--             +---------------+                +-------+     +-------+
--  ]


#INCLUDE "course.module"
#INCLUDE "miniraster.module"
#INCLUDE "rastergraphics.module"
#INCLUDE "rasterio.module"


VAL INT N.ROBOTS IS 8:


VAL INT BACKGROUND     IS COLOUR.BLACK:
VAL INT FOREGROUND     IS COLOUR.WHITE:


VAL INT MAX.WIDTH      IS 1000:                    --* Max width of raster
VAL INT MIN.WIDTH      IS  400:                    --* Min width of raster
VAL INT DEFAULT.WIDTH  IS  600:                    --* Default width of raster


VAL INT MAX.HEIGHT     IS  667:                    --* Max height of raster
VAL INT MIN.HEIGHT     IS  300:                    --* Min height of raster
VAL INT DEFAULT.HEIGHT IS  400:                    --* Default height 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


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


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


--* Information returned from drone
--
PROTOCOL LASER IS RANGES:               --* add did-we-find-something?


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


--* This should be in the rastergraphics module ...
--
--  @param angle The angle
--  @return The absolute value of the angle
--
ANGLE INLINE FUNCTION ABS (VAL ANGLE angle)
  ANGLE result:
  VALOF
    IF
      angle < 0
        result := -angle
      TRUE
        result := angle
    RESULT result
:


--* 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)

    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)

:


--* Set scene (in a stream of rasters)
--
--  @param in Receive 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 one built earlier
        out ! CLONE master.scene         -- output a copy (keeping original)
:


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


--* 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 move Receive movement instructions
--  @param laser Send what we see
--  @param in Receive raster
--  @param out Send raster
--
PROC drone (VAL STATE initial.state, VAL INT laser.range, robot.radius,
            CHAN MOVE move?, CHAN LASER laser!, CHAN RASTER in?, out!)
  
  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
          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

:


--* compute.speed as a function of range and robot footprint
--
--  Proportional to [@code log (speed)]
--
--  @param range Distance to obstacle
--  @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
:


--* Compute minimum range
--
--  @param ranges Array of ranges (there must be at least one element)
--  @return Minimum range
--
INT FUNCTION min (VAL []INT ranges)

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


--* Drive robot automatically.
--
--  Go in circles.
--  Slow down as minimum range ([@code robot.radius]) approaches.
--  Reverse direction on contact.
--
--  @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 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:
  WHILE TRUE
    RANGES ranges:
    SEQ
      laser ? ranges
      VAL INT velocity IS compute.speed (min (ranges), robot.radius):
      IF
        velocity = 0
          linear.velocity := -linear.velocity
        TRUE
          linear.velocity := velocity
      move ! linear.velocity; angular.velocity
:


--* Drive robot automatically.
--
--  You decide ... :)
--  But you must avoid obstacles and keep moving ...
--
--  @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 initial.linear.velocity, VAL ANGLE initial.angular.velocity,
              VAL INT robot.radius, CHAN MOVE move!, CHAN LASER laser?)

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

:


--* Handle raster events
--
--  @param events Receive SDL events
--  @param freeze Freeze signal
--
--  Just demonstrates receiving ordinary characters and
--  the raster window close event.  Contol characters and
--  mouse events are also possible - see the miniraster
--  documentation.
--
PROC raster.events.handler (CHAN RASTER.EVENT events?, CHAN INT freeze!)

  WHILE TRUE

    RASTER.EVENT event:
    SEQ
      events ? event
      CASE event[type]

        RASTER.ET.KEYDOWN                  -- normal keys
          CASE event[id]
            'q'
              STOP                         -- crude crash :(
            ' '
              SEQ
                freeze ! 0
                RASTER.EVENT any:
                SEQ
                  events ? any
                  WHILE any[type] <> RASTER.ET.KEYDOWN
                    events ? any
                freeze ! 0
            ELSE
              SKIP

        RASTER.ET.QUIT                     -- click on [X]
          STOP                             -- crude crash :(

        ELSE                               -- anything else?
          SKIP

:


--* front of pipeline
--
--  @param in.1 Receive new raster
--  @param in.2 Receive raster for next phase
--  @param out Send raster
--  @param freeze Frezze control
--
PROC front (CHAN RASTER in.1?, in.2?, out!, CHAN INT freeze?)

  WHILE TRUE

    PRI ALT

      INT any:
      freeze ? any
        freeze ? any

      RASTER raster:
      in.1 ? raster
        SEQ
          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 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
:


--* 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]:
                   

--* 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
--
PROC place (VAL INT robot.radius, INT x, y, [][]INT raster, INT seed)
  SEQ
    VAL INT width IS SIZE raster[0]:
    VAL INT height IS SIZE raster:
    INITIAL BOOL looking IS TRUE:
    #PRAGMA DEFINED raster, looking            -- avoid kroc defined analysis warning
    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)
:
                   

--* 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 mixed Whether there are mixed brain types
--
PROC place.robots ([N.ROBOTS]STATE s, VAL INT raster.width, raster.height, robot.radius, VAL BOOL mixed)

  INT seed:                                  -- random number seed
  RASTER raster:                             -- scratch pad
  
  SEQ
    
    raster := MOBILE [raster.height][raster.width]INT
    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)

        s.i[linear.velocity] := 1
        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[angular.velocity] := rand * ANGLE.DEGREE

        IF
          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]

:


--* draw welcome screen
--
--  @param screen Standard output (probably)
--
PROC welcome (CHAN BYTE screen!)
  SEQ
    erase.screen (screen!)                  -- clear the screen
    cursor.x.y (1, 1, screen!)              -- place cursor at top-left
    out.string ("Robot (v3), copyleft (2007) D.C.Wood and P.H.Welch*c*n", 0, screen!)
    out.string ("==================================================*c*n*n", 0, screen!)
:
                   

--* obtain user chosen system parameters
--
--  @param prompt User prompt string
--  @param answer min <= answer <= max
--  @param min smallest answer allowed
--  @param max largest answer allowed
--  @param keyboard Standard input (probably)
--  @param screen Standard output (probably)
--
PROC ask.min.max.int (VAL []BYTE prompt, INT answer, VAL INT min, max, CHAN BYTE keyboard?, screen!)
  SEQ
    -- note: the argument "4" below is the max number of characters allowed in reply.
    -- note: this value should be computed from min and max (fix me)
    ask.int (prompt, answer, 4, keyboard?, screen!)
    -- deduce: -999 <= answer <= 9999
    WHILE (answer < min) OR (answer > max)
      SEQ
        out.string ("  ****** Please answer between ", 0, screen!)
        out.int (min, 0, screen!)
        out.string (" and ", 0, screen!)
        out.int (max, 0, screen!)
        out.string (" inclusive*c*n", 0, screen!)
        ask.int (prompt, answer, 4, keyboard?, screen!)
    -- deduce: min <= answer <= max
:
                   

--* obtain user chosen system parameters
--
--  @param mixed Whether there are mixed brain types
--  @param version Brain type, if not mixed (in which case -1)
--  @param raster.width Desired width of raster
--  @param raster.height Desired height of raster
--  @param keyboard Standard input (probably)
--  @param screen Standard output (probably)
--
PROC obtain (BOOL mixed, INT version, raster.width, raster.height, CHAN BYTE keyboard?, screen!)
  BOOL default.dimensions:
  SEQ
    ask.bool ("Two types of robot? ", mixed, keyboard?, screen!)
    IF
      NOT mixed
        SEQ
          version := -1
          WHILE NOT ((version = 0) OR (version = 1))
            ask.int ("Version number (0 or 1)? ", version, 1, keyboard?, screen!)
      TRUE
        version := -1
    ask.bool ("Default raster dimensions (600 x 400)? ", default.dimensions, keyboard?, screen!)
    IF
      default.dimensions
        raster.width, raster.height := DEFAULT.WIDTH, DEFAULT.HEIGHT
      TRUE
        SEQ
          out.string ("*c*n", 0, screen!)
          ask.min.max.int ("raster width? ", raster.width, MIN.WIDTH, MAX.WIDTH, keyboard?, screen!)
          ask.min.max.int ("raster height? ", raster.height, MIN.HEIGHT, MAX.HEIGHT, keyboard?, screen!)
:


--* the main process
--
-- @param keyboard The standard input channel (stdin, in Unix-speak)
-- @param screen The standard output channel (stdout, in Unix-speak)
-- @param error The standard error channel (stderr, in Unix-speak)
--
PROC main (CHAN BYTE keyboard?, screen!, error!)

  [N.ROBOTS]STATE s:

  BOOL mixed:              -- two robot versions?
  INT version:             -- if not mixed, use this robot version

  INT raster.width, raster.height:
  
  INT laser.range, robot.radius:
  
  SEQ
    
    welcome (screen!)
    obtain (mixed, version, raster.width, raster.height, keyboard?, screen!)
    
    laser.range := (256*raster.width) >> 10
    robot.radius := (8*raster.width) >> 10
    
    place.robots (s, raster.width, raster.height, robot.radius, mixed)
    #PRAGMA DEFINED s      -- avoid kroc defined analysis warning ...

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

    CHAN RASTER source, return:
    CHAN RASTER.EVENT events:
    CHAN INT freeze:
    CHAN RASTER b, c:

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

    PAR

      miniraster ("Robots", raster.width, raster.height, N.RASTERS, return?, source!, events!)
      raster.events.handler (events?, freeze!)

      scene (source?, b!)

      front (b?, c?, d[0]!, freeze?)

      PAR i = 0 FOR N.ROBOTS
        PAR
          drone (s[i], laser.range, robot.radius, h[i]?, j[i]!, d[i]?, d[i + 1]!)
          VAL INT linear.velocity IS s[i][linear.velocity]:
          VAL ANGLE angular.velocity IS s[i][angular.velocity]:
          IF
            mixed
              IF
                (i /\ 1) = 0
                  brain.0 (linear.velocity, angular.velocity, robot.radius, h[i]!, j[i]?)
                TRUE
                  brain.1 (linear.velocity, angular.velocity, robot.radius, h[i]!, j[i]?)
            version = 0
              brain.0 (linear.velocity, angular.velocity, robot.radius, h[i]!, j[i]?)
            version = 1
              brain.1 (linear.velocity, angular.velocity, robot.radius, h[i]!, j[i]?)

      back (d[N.ROBOTS]?, c!, return!)

:
