--** Test the blobfinder by having robots chase other robots.
-- This is intended for use with [@text bc6.cfg].

#INCLUDE "occplayer.module"
#INCLUDE "useful.module"
#INCLUDE "random.module"

PROC robot (VAL INT id)
  playerc.client.t client:
  playerc.blobfinder.t blobfinder:
  playerc.position2d.t position2d:
  INT result:
  SEQ
    --{{{  connect to Stage
    playerc.client.create (0, "localhost*#00", 6660 + id, client)
    playerc.client.connect (client, result)
    ASSERT (result = 0)
    --}}}
    --{{{  get a blobfinder device
    playerc.blobfinder.create (client, 0, blobfinder)
    playerc.blobfinder.subscribe (blobfinder, PLAYER.OPEN.MODE, result)
    ASSERT (result = 0)
    --}}}
    --{{{  get a position2d device
    playerc.position2d.create (client, 0, position2d)
    playerc.position2d.subscribe (position2d, PLAYER.OPEN.MODE, result)
    ASSERT (result = 0)
    --}}}

    INITIAL INT cycle IS 0:
    INITIAL RANDOM.STATE rs IS random.init (INT32 id):
    INITIAL REAL64 speed IS 0.0:
    INITIAL REAL64 angle IS 0.0:
    WHILE TRUE
      INITIAL BOOL chasing IS FALSE:
      INITIAL INT chasing.area IS 0:
      SEQ
        --{{{  wait for new data
        playerc.client.read (client, result)
        --}}}
        --{{{  examine the blobfinder
        INT width, count:
        playerc.blobfinder.blob.t blobs:
        SEQ
          playerc.blobfinder.t.width.get (blobfinder, width)
          playerc.blobfinder.t.blobs.count.get (blobfinder, count)
          playerc.blobfinder.t.blobs.get (blobfinder, blobs)

          -- Slight oddity: ...blobs.get is defined using the playerc... type
          -- rather than the player... one, so we need to cast the pointer it
          -- returns to the player... type.
          VAL player.blobfinder.blob.t blobs IS player.blobfinder.blob.t blobs:

          SEQ i = 0 FOR count
            -- Index into the array of blobs.
            VAL player.blobfinder.blob.t blob IS blobs @ i:

            INT x, y, area:
            SEQ
              player.blobfinder.blob.t.x.get (blob, x)
              player.blobfinder.blob.t.y.get (blob, y)
              player.blobfinder.blob.t.area.get (blob, area)

              trace.sisin ("robot ", id, " sees  blob ", i)
              trace.sisis ("  at (", x, ", ", y, ")*n")
              trace.sin ("  area ", area)

              IF
                area > 1000
                  --{{{  too close
                  SKIP
                  --}}}
                area > chasing.area
                  --{{{  chase it!
                  SEQ
                    trace.sn ("  chasing")
                    chasing, chasing.area := TRUE, area
                    --{{{  move a bit faster than normal
                    speed := 3.0
                    --}}}
                    --{{{  try to centre it in the blobfinder
                    IF
                      x < (width / 2)
                        angle := 1.0
                      TRUE
                        angle := -1.0
                    --}}}
                  --}}}
                TRUE
                  SKIP
        --}}}
        IF
          chasing
            --{{{  chasing something already
            SKIP
            --}}}
          (cycle \ 10) = 0
            --{{{  set random speed and direction
            REAL32 r:
            SEQ
              r, rs := random.real32 (3.0, rs)
              speed := REAL64 ROUND (r - 1.0)
              r, rs := random.real32 (2.0, rs)
              angle := REAL64 ROUND (r - 1.0)
            --}}}
          TRUE
            SKIP
        --{{{  update velocity
        playerc.position2d.set.cmd.vel (position2d, speed, 0.0, angle, PLAYER.POSITION2D.CMD.VEL, result)
        ASSERT (result = 0)
        --}}}
        cycle := cycle PLUS 1
:

PROC main ()
  PAR i = 0 FOR 6
    robot (i)
:
