#!/usr/bin/env ruby

# Test Flight
#
# A simple virtual test flight (take off, hover, and set back down)
# with valgrind, in order to detect memory leaks

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

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

puts("Firing up the quad...")

# Start virtual quad
quad = Process.spawn("valgrind --leak-check=full --log-file=./valgrind.out ./virt-quad")

delay_spin_cursor(3)

set_gear GEAR_OFF
set_flap FLAP_ON

##################
#  Begin Flight!
##################

begin
  puts("Starting flight...")

  # Set initial quad orientation (flat on 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`

  puts("Turning on GEAR...")
  set_gear GEAR_ON
  sleep 0.015

  puts("Increasing Thrust to half maximum...")
  for i in (THROTTLE_MIN..THROTTLE_MID).step(0.01)
    set_throttle(i)
    sleep 0.005
  end

  puts("Hovering for 5 seconds")
  delay_spin_cursor(5)

  puts("Switching to autonomous and hovering for 5 seconds")
  set_flap FLAP_ON
  delay_spin_cursor(5)

  puts("Switch back to manual, relaxing thrust to zero")
  set_flap FLAP_OFF
  i = THROTTLE_MID
  while i > THROTTLE_MIN
    i -= 0.01
    set_throttle(i)
    sleep 0.005
  end

  puts("Swiching off GEAR...")
  set_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

  puts("Flight ended successfully.");
ensure

  Process.kill(2, quad)

  sleep 1 # wait for valgrind to write to file

  # Process the valgrind file
  file = File.open("./valgrind.out")
  everything = file.readlines
  last_line = everything.last
  errors = last_line.scanf("%s ERROR SUMMARY: %d errors %s")
  if errors[1] > 0
    puts everything
    puts "Memory Integrity Check Failed."
    Kernel.exit(1)
  end

  puts "Memory Integrity Check Passed."
end