#!/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(60) {

  puts("Setting up...")

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

  sleep 0.5

  # Set RC switches
  set_gear GEAR_OFF
  set_flap FLAP_ON

  # Set initial quad orientation (flat on the ground, facing forward)
  `./virt-quad set i2c_imu_x 0`
  `./virt-quad set i2c_imu_y 0`
  `./virt-quad set i2c_imu_z -1`
  `./virt-quad set rc_roll 0.498`
  `./virt-quad set rc_pitch 0.497`
  `./virt-quad set rc_yaw 0.498`

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

  begin
    puts "------------------------------------------"
    puts "-- Beginning safety checks test..."
    puts "------------------------------------------"

    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")
    # Motors should not engage when 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
    set_gear GEAR_ON
    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

    # Switch GEAR 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
    motors = get_motor_averages(100, 2)
    p motors
    motors.each { |value| assert_operator(value, :>, THROTTLE_QUAR) }

    puts("Check that when quad is tilted, motors respond correctly")
    puts("Tilting forwards...")
    `./virt-quad set i2c_imu_x 0.25`
    motors = get_motor_averages(100, 2)
    p motors
    assert_operator(motors[0], :>, motors[1])
    assert_operator(motors[0], :>, motors[3])
    assert_operator(motors[2], :>, motors[1])
    assert_operator(motors[2], :>, motors[3])
    puts("Tilting backwards...")
    `./virt-quad set i2c_imu_x -0.25`
    motors = get_motor_averages(100, 2)
    p motors
    assert_operator(motors[0], :<, motors[1])
    assert_operator(motors[0], :<, motors[3])
    assert_operator(motors[2], :<, motors[1])
    assert_operator(motors[2], :<, motors[3])
    puts("Tilting right...")
    `./virt-quad set i2c_imu_x 0`
    `./virt-quad set i2c_imu_y 0.25`
    motors = get_motor_averages(100, 2)
    p motors
    assert_operator(motors[0], :<, motors[2])
    assert_operator(motors[0], :<, motors[3])
    assert_operator(motors[1], :<, motors[2])
    assert_operator(motors[1], :<, motors[3])
    puts("Tilting left...")
    `./virt-quad set i2c_imu_y -0.25`
    motors = get_motor_averages(100, 2)
    p motors
    assert_operator(motors[0], :>, motors[2])
    assert_operator(motors[0], :>, motors[3])
    assert_operator(motors[1], :>, motors[2])
    assert_operator(motors[1], :>, motors[3])

    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 to minimum
    set_throttle THROTTLE_MIN

    puts "All safety checks passed."
    puts "------------------------------------------"

  ensure

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

  end
}