#INCLUDE "srv1.inc"
#INCLUDE "delay.inc"
#INCLUDE "out.occ"

PROC drain.uart (CHAN BYTE in?)
  INITIAL INT clear IS 0:
  WHILE clear < 10
    PRI ALT
      BYTE b:
      in ? b
        SKIP
      TRUE & SKIP
        clear := clear + 1
:

PROC protocol.error (VAL BYTE prefix, ch, RESULT BOOL custom.response, CHAN BYTE out!)
  SEQ
    custom.response := TRUE
    out.string("Invalid command ", 0, out!)
    IF
      prefix = 0
        SKIP
      TRUE
        out ! prefix
    out ! ch
    out.string(" found in protocol*n", 0, out!)
:

PROC srv1 (CAMERA! camera, CONSOLE! console, LASER! lasers, MOTOR! motors, SYSTEM! system)
  INITIAL [3]BYTE response IS ['#', 0, '*n']:
  INITIAL INT frame.count IS 0:
  INITIAL INT width IS 320:
  INITIAL INT height IS 256:
  INITIAL BOOL overlay IS TRUE:
  INITIAL INT quality IS 3:
  SEQ
    camera[req] ! set.stream.mode; 320; 256
    camera[rsp] ? CASE ok

    WHILE TRUE
      INITIAL BOOL custom.response IS FALSE:
      BYTE ch:
      SEQ
        console[in] ? ch
        response[1] := ch
        CASE ch
          -- Direct motor control
          'M'
            SKIP
          -- Drift left
          '7'
            motors[req] ! turn; 15
          -- Drive forward
          '8'
            motors[req] ! all; 50; 50
          -- Drift right
          '9'
            motors[req] ! turn; -15
          -- Drive left
          '4'
            motors[req] ! turn; 30
          -- Stop
          '5'
            motors[req] ! all; 50; 50
          -- Drive right
          '6'
            motors[req] ! turn; -30
          -- Back left
          '1'
            motors[req] ! back.turn; 30
          -- Reverse
          '2'
            motors[req] ! delta; -1; -1
          -- Back right
          '3'
            motors[req] ! back.turn; -30
          -- Clockwise turn
          '.'
            SEQ
              motors[req] ! turn; 75
              delay.ms(200)
              motors[req] ! stop
          -- High motor speed range
          '+'
            SKIP
          -- Low motor speed range
          '-'
            SKIP
          -- Set camera resolution to 160x128
          'a'
            SEQ
              width, height := 160, 128
              camera[req] ! set.stream.mode; width; height
              camera[rsp] ? CASE ok
          -- Set camera resolution to 320x256
          'b'
            SEQ
              width, height := 320, 256
              camera[req] ! set.stream.mode; width; height
              camera[rsp] ? CASE ok
          -- Set camera resolution to 640x512
          'c'
            SEQ
              width, height := 640, 512
              camera[req] ! set.stream.mode; width; height
              camera[rsp] ? CASE ok
          -- Set camera resolution to 1280x1024
          'A'
            SEQ
              width, height := 1280, 1024
              camera[req] ! set.capture.mode; width; height
              camera[rsp] ? CASE ok
          -- Grab JPEG compressed video frame
          'I'
            MOBILE []BYTE frame, jpeg.data:
            INITIAL [10]BYTE header IS "##IMJ00000":
            INT jpeg.bytes:
            SEQ
              camera[req] ! get.frame
              camera[rsp] ? CASE frame; frame
              IF
                overlay
                  INITIAL [9]BYTE caption IS "frame 000":
                  SEQ
                    caption[6] := BYTE ('0' + (((frame.count / 100) \ 10)))
                    caption[7] := BYTE ('0' + (((frame.count / 10) \ 10)))
                    caption[8] := BYTE ('0' +  (frame.count \ 10))
                    draw.caption.on.frame (width, caption, frame)
                TRUE
                  SKIP
              jpeg.data := MOBILE [(SIZE frame) >> 2]BYTE
              jpeg.encode.frame (width, height, quality, frame, jpeg.data, jpeg.bytes)
              CASE width
                80
                  header[5] := '1'
                160
                  header[5] := '3'
                320
                  header[5] := '5'
                640
                  header[5] := '7'
                1280
                  header[5] := '9'
              
              [4]BYTE size RETYPES jpeg.bytes:
              SEQ i = 0 FOR 4
                header[6 + i] := size[i]
              SEQ i = 0 FOR SIZE header
                console[out] ! header[i]

              console[cmd.req] ! bulk.out; FALSE; jpeg.bytes; jpeg.data

              drain.uart (console[in]?)

              frame.count := frame.count + 1
              custom.response := TRUE
          -- Camera overlay
          'o'
            overlay := TRUE
          'O'
            overlay := FALSE
          -- Lasers
          'l'
            lasers[req] ! all; TRUE  -- On
          'L'
            lasers[req] ! all; FALSE -- Off
          -- Time in MS since reset
          't'
            SKIP
          -- JPEG Quality
          'q'
            SEQ
              console[in] ? ch
              quality := INT (ch - '0')
              IF
                quality < 1
                  quality := 1
                quality > 8
                  quality := 8
                TRUE
                  SKIP
              out.string ("##quality ", 0, console[out]!)
              out.int (quality, 0, console[out]!)
              console[out] ! '*n'
              custom.response := TRUE
          -- Range in centimeters to nearest object
          'R'
            SKIP
          -- Version
          'V'
            SKIP
          -- IO pins
          'd'
            SEQ
              console[in] ? ch
              CASE ch
                'd'
                  SKIP -- Direction
                'r'
                  SKIP -- Read
                'w'
                  SKIP -- Write
                ELSE
                  protocol.error('d', ch, custom.response, console[out]!)
          -- Xmodem recieve file
          'X'
            SKIP
          -- Flash Memory
          'z'
            SEQ
              console[in] ? ch
              CASE ch
                'w'
                  SKIP -- Write
                'r'
                  SKIP -- Read
                'd'
                  SKIP -- Dump
                'Z'
                  SKIP -- Update boot sector
                ELSE
                  protocol.error('z', ch, custom.response, console[out]!)
          -- Vision commands
          'v'
            SEQ
              console[in] ? ch
              CASE ch
                'g'
                  SKIP -- Grab and sample region
                'r'
                  SKIP -- Retrieve stored color info from color bin
                'c'
                  SKIP -- Set the contents of a color bin
                's'
                  SKIP -- View pixel column vector data from color bin
                'f'
                  SKIP -- Distance to first pixel matching target color
                'b'
                  SKIP -- Search for blobs
                'n'
                  SKIP -- Blobs, returns the number of pixels matching target color
                ELSE
                  protocol.error('v', ch, custom.response, console[out]!)
          'Q'
            SKIP -- Execute C program stored in flash memory.
          -- Carriage Return
          '*n'
            custom.response := TRUE -- Ignore, makes netcat/telnet happy.
          ELSE
            protocol.error(0, ch, custom.response, console[out]!)
        -- Respond to commands, if appropriate.
        IF
          NOT custom.response
            out.string(response, 0, console[out]!)
          TRUE
            SKIP
:
