THROTTLE_MIN = 0.10200
THROTTLE_MAX = 0.91900
THROTTLE_MID = (THROTTLE_MAX + THROTTLE_MIN)/2
THROTTLE_3_4 = (THROTTLE_MAX + THROTTLE_MID)/2
THROTTLE_QUAR = (THROTTLE_MID + THROTTLE_MIN)/2
THROTTLE_EIGHTH = (THROTTLE_QUAR + THROTTLE_MIN)/2
THROTTLE_16 = (THROTTLE_EIGHTH + THROTTLE_MIN)/2
MOTOR_MIN = 0.0
MOTOR_MAX = 1.0
GEAR_ON = 0.70800
GEAR_OFF = 0.18300
FLAP_ON = 0.9
FLAP_OFF = 0.1
GRAVITY = 4096

UART_RX = "virt-quad-fifos/uart-rx"
UART_TX = "virt-quad-fifos/uart-tx"

require 'test/unit/assertions'
require 'timeout'
require 'thread'
require 'scanf'
include Test::Unit::Assertions

# Utility functions
def check_motors_are_off(msg)
  motors = [ ]
  motors.push(`./virt-quad get motor1`.to_f)
  motors.push(`./virt-quad get motor2`.to_f)
  motors.push(`./virt-quad get motor3`.to_f)
  motors.push(`./virt-quad get motor4`.to_f)
  motors.each { |val|
    assert_operator(val, :<=, THROTTLE_MIN, msg)
  }
end

def check_led(is_on, msg)
  led = `./virt-quad get led`.to_i
  assert_equal(is_on, led, msg)
end

def set_gear(gear)
  `./virt-quad set rc_gear #{gear}`
end

def set_flap(flap)
  `./virt-quad set rc_flap #{flap}`
end

def set_throttle(throttle)
  `./virt-quad set rc_throttle #{throttle}`
end

def get_motor_averages(times, total_time)
  motor_sums = [0.0, 0.0, 0.0, 0.0]
  for i in 0..times
    motors = [ ]
    motors.push(`./virt-quad get motor1`.to_f)
    motors.push(`./virt-quad get motor2`.to_f)
    motors.push(`./virt-quad get motor3`.to_f)
    motors.push(`./virt-quad get motor4`.to_f)
    for i in 0..3
      motor_sums[i] += motors[i]
    end
    sleep (total_time.to_f / times.to_f)
  end
  motor_sums.map {|val| val / times}
end

def send_packet(bytes)
  File.write(UART_RX, bytes.pack("C" * bytes.length))
end

def recv_packet
  fifo = File.open(UART_TX)

  # Receive the header
  msg = []
  for i in 1..7
    sleep 0.010
    c = fifo.read(1)
    msg.push(c)
  end

  # Receive the remaining data, according to the header specified length
  length = msg[5..7].join().unpack("S")[0]
  msg = []
  for i in 1..length
    sleep 0.010
    msg.push(fifo.read(1))
  end
  fifo.close

  msg.join()
end