#!/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...")


  sleep 1

  # 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(5)

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

  begin

    puts("Beginning tests...")

    # Set gravity and gear
    File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
    File.write(GEAR, GEAR_OFF)

    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)
      File.write(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)
    File.write(GEAR, GEAR_ON)
    sleep 0.5
    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
      File.write(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)
    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.5
    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")
    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)

    puts("Check that gear switch kills the motors")
    # (and that light goes off)
    File.write(GEAR, GEAR_OFF)
    sleep 0.5
    check_motors_are_off "Motors didn't turn off when GEAR was switched off! How dangerous!"
    #check_led 0

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

    sleep 1
    puts "All safety checks passed."

  ensure

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

  end
}