#!/usr/bin/env ruby

# Safety Checks
#
# Startup the virtual quad and make sure it doesn't allow combinations of things
# that could hurt people.

script_dir = File.expand_path(File.dirname(__FILE__))
require script_dir + "/testing_library"

bin_dir = script_dir + "/../../bin/"
Dir.chdir(bin_dir)


Timeout::timeout(30) {

  puts("Setting up...")

  # Start virtual quad
  quad_pid = Process.spawn("./virt-quad -q",
                           { :rlimit_as => 536870912, # 512 MiB total RAM
                             :rlimit_stack => 1048576}) # 1 MiB stack

  delay_spin_cursor(1)

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

  begin

    puts("Beginning tests...")

    # Set gravity and gear
    set_gear GEAR_OFF
    set_flap FLAP_ON
    `./virt-quad set rc_roll 0.498`
    `./virt-quad set rc_pitch 0.497`
    `./virt-quad set rc_yaw 0.498`

    puts("Check that motors are off at startup")
    check_motors_are_off "Motors weren't off at startup! How dangerous!"

    puts("Check that LED is off at startup")
    check_led(0, "LED was on at startup! It should be off so that we can verify when the quad is ready.")

    puts("Check that increasing the throttle does nothing to motors")
    # (because gear is still off)
    for i in (THROTTLE_MIN..THROTTLE_MAX).step(0.01)
      set_throttle(i)
      check_motors_are_off "Was able to turn on motors with GEAR off! Yikes!"
      check_led(0, "LED turned on while GEAR was 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)
    sleep 0.01
    check_motors_are_off "Motors turned on by gear while rc throttle was high! How dangerous!"
    i = THROTTLE_MAX
    while i > THROTTLE_MID
      i -= 0.01
      set_throttle(i)
      check_motors_are_off "Motors turned on while GEAR was off. Yikes!"
      check_led(0, "LED turned on while GEAR was off!")
      sleep 0.005
    end

    # (swtich GEAR back to off and bring throttle off)
    set_gear GEAR_OFF
    set_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)
    set_gear GEAR_ON
    sleep 0.01
    check_motors_are_off "Motors turned on while throttle was low! Yikes!"
    check_led(1, "LED didn't turn on when GEAR was switched on!")

    puts("Check that motors turn on")
    set_throttle THROTTLE_MID
    spinner = Thread.new { delay_spin_cursor(5) }
    motors = get_motor_averages(100, 5)
    spinner.exit
    p motors
    # motors.each { |value| assert_operator(value, :>, THROTTLE_EIGHTH) }
    motors_average = motors.reduce(:+).to_f / 4
    assert_operator(motors_average, :>, THROTTLE_EIGHTH)

    puts("Check that gear switch kills the motors")
    set_gear GEAR_OFF
    sleep 0.01
    check_motors_are_off "Motors didn't turn off when GEAR was switched off! How dangerous!"

    # (Bring the RC throttle back down)
    set_throttle THROTTLE_MIN

    puts "All safety checks passed."

  ensure

    Process.kill(9, quad_pid)
    Process.wait(quad_pid)

  end
}