#!/usr/bin/env ruby

# Logging test
#
# Let the quad fly for a bit, and then ensure we can
# get its logs

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...")

  # Start virtual quad
  quad_pid = Process.spawn("./virt-quad -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_OFF

  # 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 logging 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
    set_flap FLAP_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_OFF
    sleep 3

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

    # Get logs

    Thread.new {
      sleep 0.5
      puts("Swiching off GEAR...")
      set_gear GEAR_OFF
    }

    logs = []

    begin
      while true
        logs.push(recv_packet)
      end
    rescue Timeout::Error
      puts "No logs left"
    end

    if logs.length == 2
      log_data = logs[1].split("\n")
      for data in log_data
        puts data
      end
      p log_data.length
    end

    puts "------------------------------------------"

  ensure

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

  end
}