#!/usr/bin/env ruby

# Test Flight
#
# A simple virtual test flight (take off, hover, and set back down)
#

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"

UART_TX = "virt-quad-fifos/uart-tx"

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

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

begin
  puts("Starting flight")

  # Set gravity
  File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)

  puts("Turning on GEAR...")
  File.write(GEAR, GEAR_ON)
  sleep 0.015

  puts("Increasing Thrust to half maximum...")
  for i in (THROTTLE_MIN..THROTTLE_MID).step(1000)
    File.write(THROTTLE, i)
    sleep 0.005
  end

  puts("Hovering for 10 seconds")
  sleep 10

  puts("Relaxing thrust to zero")
  i = THROTTLE_MID
  while i > THROTTLE_MIN
    i -= 1000
    File.write(THROTTLE, i)
    sleep 0.005
  end

  puts("Swiching off GEAR...")
  File.write(GEAR, GEAR_OFF)

  # puts("Collecting logs...")
  # msg = ""
  # misses = 0
  # fifo = File::open(UART_TX)
  # while misses < 10
  #   puts "trying..."
  #   if fifo.eof?
  #     misses += 1
  #     next
  #   end
  #   msg += fifo.read()
  # end
  
  # fifo.close()

  # puts msg

ensure

  Process.kill(9, quad)

end