#!/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

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

# Utility functions
def check_motors_are_off
  motor1 = File.read("virt-quad-fifos/pwm-output-motor1")
  motor2 = File.read("virt-quad-fifos/pwm-output-motor2")
  motor3 = File.read("virt-quad-fifos/pwm-output-motor3")
  motor4 = File.read("virt-quad-fifos/pwm-output-motor4")
  assert_operator motor1.to_i, :<=, THROTTLE_MIN
  assert_operator motor2.to_i, :<=, THROTTLE_MIN
  assert_operator motor3.to_i, :<=, THROTTLE_MIN
  assert_operator motor4.to_i, :<=, THROTTLE_MIN
end

def check_led(on)
  led = File.read("virt-quad-fifos/mio7-led")
  assert_equal(led.to_i, 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

  # Check that motors are off at startup
  check_motors_are_off

  # Check that LED is off at startup
  check_led(0)

  # 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("virt-quad-fifos/pwm-input-throttle", i)
    check_motors_are_off
    sleep 0.005
  end

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

  # (swtich GEAR back to off and bring throttle off)
  File.write("virt-quad-fifos/pwm-input-gear", GEAR_OFF)
  File.write("virt-quad-fifos/pwm-input-throttle", THROTTLE_MIN)

  # Check that the LED turns on when gear is flipped on
  # (motors should still be off because our throttle is low)
  File.write("virt-quad-fifos/pwm-input-gear", GEAR_ON)
  sleep 0.010
  check_led 1
  check_motors_are_off

  # Check that motors turn on
  File.write("virt-quad-fifos/pwm-input-throttle", THROTTLE_MID)
  motor1 = []
  motor2 = []
  motor3 = []
  motor4 = []
  for i in 0..100
    motor1.push(File.read("virt-quad-fifos/pwm-output-motor1").to_i)
    motor2.push(File.read("virt-quad-fifos/pwm-output-motor2").to_i)
    motor3.push(File.read("virt-quad-fifos/pwm-output-motor3").to_i)
    motor4.push(File.read("virt-quad-fifos/pwm-output-motor4").to_i)
    sleep 0.005
  end
  average1 = motor1.inject(:+).to_f / motor1.size
  average2 = motor2.inject(:+).to_f / motor2.size
  average3 = motor3.inject(:+).to_f / motor3.size
  average4 = motor4.inject(:+).to_f / motor4.size
  average = (average1 + average2 + average3 + average4)/4
  puts average1, average2, average3, average4, "(#{average})"
  assert average.between?(THROTTLE_EIGHTH, MOTOR_MAX)

  # Check that gear switch kills the motors
  # (and that light goes off)
  File.write("virt-quad-fifos/pwm-input-gear", GEAR_OFF)
  sleep 0.015
  check_motors_are_off
  check_led 0

  # (Bring the RC throttle back down)
  File.write("virt-quad-fifos/pwm-input-throttle", THROTTLE_MIN)

  # Check that we can resume flight
  File.write("virt-quad-fifos/pwm-input-gear", GEAR_ON)
  sleep 0.015
  check_led 1

  sleep 1
  puts "All safety checks passed."

ensure

  Process.kill(9, quad)

end