#!/usr/bin/env ruby

# Safety Checks
#
# Startup the virtual quad and make sure it doesn't allow combinations of things
# that could hurt people.
THROTTLE_MIN = 110200
THROTTLE_MAX = 191900
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 = 100000
MOTOR_MAX = 200000
GEAR_ON = 170800
GEAR_OFF = 118300
GRAVITY = 4096

MOTOR1 = "virt-quad-fifos/pwm-output-motor1"
MOTOR2 = "virt-quad-fifos/pwm-output-motor2"
MOTOR3 = "virt-quad-fifos/pwm-output-motor3"
MOTOR4 = "virt-quad-fifos/pwm-output-motor4"

GEAR = "virt-quad-fifos/pwm-input-gear"
THROTTLE = "virt-quad-fifos/pwm-input-throttle"

LED = "virt-quad-fifos/mio7-led"

I2C_MPU_ACCEL_X = "virt-quad-fifos/i2c-mpu-accel-x"
I2C_MPU_ACCEL_Y = "virt-quad-fifos/i2c-mpu-accel-y"
I2C_MPU_ACCEL_Z = "virt-quad-fifos/i2c-mpu-accel-z"

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

$fifos = Hash.new

def read_fifo_num(f)
  if not $fifos.key?(f)
    $fifos[f] = File.open(f)
  end
  $fifos[f].read().chomp.split("\n").last.to_i
end

# Utility functions
def check_motors_are_off
  motor1 = read_fifo_num(MOTOR1)
  motor2 = read_fifo_num(MOTOR2)
  motor3 = read_fifo_num(MOTOR3)
  motor4 = read_fifo_num(MOTOR4)
  assert_operator motor1, :<=, THROTTLE_MIN
  assert_operator motor2, :<=, THROTTLE_MIN
  assert_operator motor3, :<=, THROTTLE_MIN
  assert_operator motor4, :<=, THROTTLE_MIN
end

def get_motor_averages
  motors = [[], [], [], []]
  for i in 0..100
    motors[0].push(read_fifo_num(MOTOR1))
    motors[1].push(read_fifo_num(MOTOR2))
    motors[2].push(read_fifo_num(MOTOR3))
    motors[3].push(read_fifo_num(MOTOR4))
    sleep 0.010
  end
  average = []
  average[0] = motors[0].inject(:+).to_f / motors[0].size
  average[1] = motors[1].inject(:+).to_f / motors[1].size
  average[2] = motors[2].inject(:+).to_f / motors[2].size
  average[3] = motors[3].inject(:+).to_f / motors[3].size
  average
end

def check_led(on)
  led = read_fifo_num(LED)
  assert_equal(led, on)
end

script_dir = File.expand_path(File.dirname(__FILE__))
bin_dir = script_dir + "/../../bin/"
Dir.chdir(bin_dir)

# Start virtual quad
quad = Process.spawn("./virt-quad")

sleep 1

#################
#  Begin Tests
#################

begin
  puts("beginning tests")
  
  # Set gravity
  File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)

  puts("Check that motors are off at startup")
  check_motors_are_off

  puts("Check that LED is off at startup")
  check_led(0)

  puts("Check that increasing the throttle does nothing to motors")
  # (because gear is still off)
  for i in (THROTTLE_MIN..THROTTLE_MAX).step(1000)
    File.write(THROTTLE, i)
    check_motors_are_off
    sleep 0.005
  end

  puts("Check that flipping gear to 1 while throttle is high does nothing")
  # (motors should still be off, LED should still be off)
  File.write(GEAR, GEAR_ON)
  sleep 0.015
  check_motors_are_off
  i = THROTTLE_MAX
  while i > THROTTLE_MID
    i -= 1000
    File.write(THROTTLE, i)
    check_motors_are_off
    check_led 0
    sleep 0.005
  end

  # (swtich GEAR back to off and bring throttle off)
  File.write(GEAR, GEAR_OFF)
  File.write(THROTTLE, THROTTLE_MIN)

  puts("Check that the LED turns on when gear is flipped on")
  # (motors should still be off because our throttle is low)
  File.write(GEAR, GEAR_ON)
  sleep 0.020
  check_led 1
  check_motors_are_off

  puts("Check that motors turn on")
  File.write(THROTTLE, THROTTLE_MID)
  averages = get_motor_averages
  average = (averages[0] + averages[1] + averages[2] + averages[3])/4
  puts averages, "(#{average})"
  assert average.between?(THROTTLE_EIGHTH, MOTOR_MAX)

  # Check that gear switch kills the motors
  # (and that light goes off)
  File.write(GEAR, GEAR_OFF)
  sleep 0.040
  check_motors_are_off
  check_led 0

  # (Bring the RC throttle back down)
  File.write(THROTTLE, THROTTLE_MIN)

  # Check that we can resume flight
  File.write(GEAR, GEAR_ON)
  sleep 0.040
  check_led 1

  sleep 1
  puts "All safety checks passed."

ensure

  Process.kill(9, quad)

end