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

sleep 1.5

set_gear GEAR_OFF
set_flap FLAP_ON

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

begin
  puts "------------------------------------------"
  puts "-- Beginning memory integrity test..."
  puts "------------------------------------------"

  # 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 3 seconds")
  sleep 3

  puts("Switching to autonomous and hovering for 3 seconds")
  set_flap FLAP_ON
  sleep 3

  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("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")
  puts everything
  if errors[1] > 0
    puts "Memory Integrity Check Failed."
    puts "------------------------------------------"
    Kernel.exit(1)
  end

  puts "Memory Integrity Check Passed."
    puts "------------------------------------------"
end