# TODO put description here.
# Time is synchronized to simulation clock through the "simtime" library.

import sys

import helper
import math
import simtime
import time
import threading

# pip install --user dronekit
from dronekit import VehicleMode, LocationGlobalRelative

# Synchronize time.time() and time.sleep(n) with simulation clock
simtime_port = int(sys.argv[1])
simtime.connect(simtime_port)

# Connect to the Vehicles
vehicles = helper.connect_multiple(sys.argv[2:])

# ArduCopter initialisation can take a really long time
for vehicle in vehicles:
    vehicle.wait_ready('gps_0', 'armed', 'mode', 'attitude', timeout=100)

# Don't try to arm until autopilot is ready
for i, vehicle in enumerate(vehicles):
    while not vehicle.is_armable:
        print(" Waiting for vehicle {} to initialise...".format(i))
        time.sleep(5)

for i, vehicle in enumerate(vehicles):
    print("Arming motors in vehicle {}".format(i))
    # Copter should arm in GUIDED mode
    vehicle.mode = VehicleMode("GUIDED")
    vehicle.armed = True

# Confirm vehicle armed before attempting to take off
for i, vehicle in enumerate(vehicles):
    while not vehicle.armed:
        print(" Waiting for arming vehicle {}".format(i))
        time.sleep(1)

print("Taking off!")
target_altitude = 10
for vehicle in vehicles:
    vehicle.simple_takeoff(target_altitude)

# Wait until the vehicles reach a safe height before processing the goto
#  (otherwise the command after Vehicle.simple_takeoff will execute
#   immediately).
for i, vehicle in enumerate(vehicles):
    while True:
        print("Vehicle {} current altitude: {}".format(i, vehicle.location.global_relative_frame.alt))
        # Break and return from function just below target altitude.
        if vehicle.location.global_relative_frame.alt >= target_altitude * 0.95:
            print("Vehicle {} reached target altitude".format(i))
            break
        time.sleep(1)

# Move in circle
center = LocationGlobalRelative(-35.363261, 149.165230, target_altitude)
for base_angle in range(0, 360, 20):
    for i, vehicle in enumerate(vehicles):
        vehicle_angle = base_angle + (i * 360 / len(vehicles))
        vehicle.simple_goto(helper.get_location_metres(
            center,
            10 * math.sin(vehicle_angle * math.pi / 180),
            10 * math.cos(vehicle_angle * math.pi / 180)))

    time.sleep(1)

print("Landing!")
# Return To Launch (land)
for vehicle in vehicles:
    vehicle.mode = VehicleMode("RTL")
time.sleep(20)
for i, vehicle in enumerate(vehicles):
    while vehicle.armed:
        print(" Waiting for vehicle {} disarming...".format(i))
        vehicle.mode = VehicleMode("RTL")
        time.sleep(5)

    vehicle.close()
