#INCLUDE "occplayer.module"

#INCLUDE "player.inc"
#USE "control"
#USE "sensors"

VAL INT max.robo.speed IS 300:
VAL INT rotation.divisor IS 6:
VAL INT min.robo.speed IS 60:

INT FUNCTION min.int(VAL INT a, b)
  INT result:
  VALOF
    IF
      a > b
        result := b
      TRUE
        result := a
    RESULT result
:

PROC one.motor(CHAN INT speed?, CHAN MOTORS out!)
  WHILE TRUE
    INT spd:
    SEQ
      speed ? spd
      out ! spd ; 0 ; 0
:

PROC two.motors(CHAN INT left?, right?, CHAN MOTORS out!)
  INITIAL INT leftspeed IS 0:
  INITIAL INT rightspeed IS 0:
  WHILE TRUE
    SEQ
      ALT
        left ? leftspeed
          IF
            leftspeed > max.robo.speed
              leftspeed := max.robo.speed
            leftspeed < (-(max.robo.speed))
              leftspeed := (-(max.robo.speed))
            TRUE
              SKIP
        right ? rightspeed
          IF
            rightspeed > max.robo.speed
              rightspeed := max.robo.speed
            rightspeed < (-(max.robo.speed))
              rightspeed := (-(max.robo.speed))
            TRUE
              SKIP
      out ! (leftspeed + rightspeed) / 2; 0; (rightspeed - leftspeed) / rotation.divisor 
:

INT FUNCTION laser.scaler(VAL INT spd)
  INT ret:
  VALOF
    ret := ((spd / 8) * 3)
    RESULT ret
:

PROC positive.slice(CHAN LASER sick?, CHAN INT motor.speed, VAL INT start, length)
  CHAN INT min.out:
  PAR
    laser.min.slice(sick?, min.out!, start, length)
    WHILE TRUE
      INT speed:
      SEQ
        min.out ? speed
        motor.speed ! (min.robo.speed + max.robo.speed) - laser.scaler(speed)
:

PROC negative.slice(CHAN LASER sick?, CHAN INT motor.speed, VAL INT start, length)
  CHAN INT min.out:
  PAR
    laser.min.slice(sick?, min.out!, start, length)
    WHILE TRUE
      INT speed:
      SEQ
        min.out ? speed
        motor.speed ! min.robo.speed + laser.scaler(speed)
:
