# DroneKit-Python program based on the "Simple Go To (Copter)" example.
# Time is synchronized to simulation clock through the "simtime" library.

import sys

import simtime
import time

# pip install --user dronekit
from dronekit import connect, 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 Vehicle
uav_name, mavlink_sysid, mavlink_port = sys.argv[2].split(':')
mavlink_sysid = int(mavlink_sysid)
mavlink_port = int(mavlink_port)
vehicle = connect(
    'tcp:127.0.0.1:{}'.format(mavlink_port),
    source_system=mavlink_sysid + 100)

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

# Don't try to arm until autopilot is ready
while not vehicle.is_armable:
    print(" Waiting for vehicle to initialise...")
    time.sleep(5)

print("Arming motors")
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True

# Confirm vehicle armed before attempting to take off
while not vehicle.armed:
    print(" Waiting for arming...")
    time.sleep(1)

print("Taking off!")
target_altitude = 5 + mavlink_sysid * 2
vehicle.simple_takeoff(target_altitude)  # Take off to target altitude

# Wait until the vehicle reaches a safe height before processing the goto
#  (otherwise the command after Vehicle.simple_takeoff will execute
#   immediately).
while True:
    print(" Current altitude: {}".format(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("Reached target altitude")
        break
    time.sleep(1)

# Save current position so we can land safely later
landing_pos = vehicle.location.global_relative_frame
landing_pos.alt = target_altitude

pointA = LocationGlobalRelative(-35.362876, 149.165046, target_altitude)
pointB = LocationGlobalRelative(-35.363040, 149.165075, target_altitude)

# Go to A, B, A, B
for i in range(2):
    print("Goint to point A")
    vehicle.simple_goto(pointA, groundspeed=10)
    time.sleep(16)

    print("Goint to point B")
    vehicle.simple_goto(pointB, groundspeed=10)
    time.sleep(16)

# Land
print("Landing!")
vehicle.simple_goto(landing_pos, groundspeed=10)
time.sleep(20)

# Return To Launch (land)
while vehicle.armed:
    print(" Waiting for disarming...")
    vehicle.mode = VehicleMode("RTL")
    time.sleep(5)

vehicle.close()
