#INCLUDE "srv1.inc"
#INCLUDE "delay.inc"
#INCLUDE "ov9655.inc"
#INCLUDE "ppi_dma_const.inc"

DATA TYPE PPI.DMA.CONFIG
  PACKED RECORD
    INT mode:
    INT width:
    INT height:
    INT bpp:
:

VAL BYTE CAMERA.ADDRESS IS #30:

PROC upload.config (TWI! twi, VAL [][2]BYTE cfg)
  MOBILE []BYTE reg.set:
  SEQ
    reg.set := MOBILE [2]BYTE
    #PRAGMA DEFINED reg.set
    SEQ i = 0 FOR SIZE cfg
      VAL [2]BYTE cfg IS cfg[i]:
      INT ret:
      SEQ
        reg.set[0] := cfg[0]
        reg.set[1] := cfg[1]
        twi[req] ! send; CAMERA.ADDRESS; reg.set
        twi[rsp] ? CASE ret; ret; reg.set
:

PROC program.camera (CHAN BOOL twi.req!, TWI! twi, VAL INT w, h, VAL BYTE com8)
  SEQ
    -- Claim TWI and configure for OV9655 access
    twi.req ! TRUE
    twi[req] ! set.clock.div; 67; 33
    twi[rsp] ? CASE ok
    twi[req] ! set.sccb; TRUE
    IF
      (w = 0) AND (h = 0)
        SEQ
          upload.config (twi, OV9655.SETUP)
          delay.ms (500)
          upload.config (twi, OV9655.SETUP)
      TRUE
        SEQ
          -- Set COM8
          upload.config (twi, [ [ OV9655.COM8, com8 ] ])
          -- Upload configuration
          CASE w
            160
              upload.config (twi, OV9655.QQVGA)
            320
              upload.config (twi, OV9655.QVGA)
            640
              upload.config (twi, OV9655.VGA)
            1280
              upload.config (twi, OV9655.SXGA)

    -- Release TWI, driver will reset to user config
    twi.req ! FALSE
:

PROC camera (CHAN BOOL twi.req!, TWI! twi, CAMERA? svr)
  
  BOOL FUNCTION valid.mode (VAL INT w, h)
    BOOL valid:
    VALOF
      IF
        (w = 160) AND (h = 128)
          valid := TRUE
        (w = 320) AND (h = 256)
          valid := TRUE
        (w = 640) AND (h = 512)
          valid := TRUE
        (w = 1280) AND (h = 1024)
          valid := TRUE
        TRUE
          valid := FALSE
      RESULT valid
  :

  CHAN PPI.DMA.CONFIG ppi.cfg:
  PLACE ppi.cfg AT #2:

  CHAN INT ppi.error:
  PLACE ppi.error AT #2:

  CHAN MOBILE []BYTE ppi.data:
  PLACE ppi.data AT #2:

  VAL BYTE COM8.AUTO.ADJUST IS #CF:
  VAL BYTE COM8.ONLY.AWB    IS #02:

  INITIAL PPI.DMA.CONFIG config IS [ PPI.DMA.STOP, 0, 0, 2 ]:
  INITIAL BYTE com8 IS COM8.AUTO.ADJUST:

  SEQ
    -- Initialise camera
    program.camera (twi.req!, twi!, 0, 0, 0)

    INITIAL BOOL done IS FALSE:
    WHILE NOT done
      svr[req] ? CASE
        disconnected
          done := TRUE

        get.auto.adjust
          svr[rsp] ! auto.adjust; (com8 = COM8.AUTO.ADJUST)

        get.frame
          IF
            config[mode] <> PPI.DMA.STOP
              MOBILE []BYTE data:
              SEQ
                ppi.data ? data
                IF
                  DEFINED data
                    -- No error; pass frame
                    svr[rsp] ! frame; data
                  TRUE
                    -- Frame empty; error
                    INT error:
                    SEQ
                      ppi.error ? error
                      IF
                        error = 0
                          svr[rsp] ! error; ERROR.NO.MEMORY
                        TRUE
                          SEQ
                            svr[rsp] ! error; ERROR.HARDWARE
                      
                      config[mode] := PPI.DMA.STOP
                      ppi.cfg ! config
            TRUE
              svr[rsp] ! error; ERROR.INV.STATE

        get.mode
          IF
            config[mode] = PPI.DMA.STREAM
              svr[rsp] ! stream.mode; config[width]; config[height]
            config[mode] = PPI.DMA.ONE.SHOT
              svr[rsp] ! capture.mode; config[width]; config[height]
            TRUE
              svr[rsp] ! error; ERROR.INV.STATE

        init
          SEQ
            config[mode] := PPI.DMA.STOP
            ppi.cfg ! config
            program.camera (twi.req!, twi!, 0, 0, com8)

        BOOL on:
        set.auto.adjust; on
          SEQ
            IF
              on
                com8 := COM8.AUTO.ADJUST
              TRUE
                com8 := COM8.ONLY.AWB
            IF
              config[mode] <> PPI.DMA.STOP
                program.camera (twi.req!, twi!, config[width], config[height], com8)
              TRUE
                SKIP
            svr[rsp] ! ok

        INT w, h:
        set.capture.mode; w; h
          IF
            valid.mode (w, h)
              SEQ
                config[mode] := PPI.DMA.STOP
                ppi.cfg ! config
                program.camera (twi.req!, twi!, w, h, com8)
                config[mode], config[width], config[height] := PPI.DMA.ONE.SHOT, w, h
                ppi.cfg ! config
                svr[rsp] ! ok
            TRUE
              svr[rsp] ! error; ERROR.INV.PARAM

        INT w, h:
        set.stream.mode; w; h
          IF
            valid.mode (w, h)
              SEQ
                config[mode] := PPI.DMA.STOP
                ppi.cfg ! config
                program.camera (twi.req!, twi!, w, h, com8)
                config[mode], config[width], config[height] := PPI.DMA.STREAM, w, h
                ppi.cfg ! config
                svr[rsp] ! ok
            TRUE
              svr[rsp] ! error; ERROR.INV.PARAM

        stop
          SEQ
            config[mode] := PPI.DMA.STOP
            ppi.cfg ! config
    
    -- Stop any active DMA
    IF
      config[mode] <> PPI.DMA.STOP
        SEQ
          config[mode] := PPI.DMA.STOP
          ppi.cfg ! config
      TRUE
        SKIP

    -- Shutdown internal TWI
    twi.req ! TRUE
    twi[req] ! disconnected
:

