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

import helper
import math
import ns3interface
import simtime
import struct
import sys
import time

# pip install --user dronekit
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil

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

# Parse other commandline arguments
uav_name, mavlink_sysid, mavlink_port = sys.argv[2].split(':')
mavlink_sysid = int(mavlink_sysid)
mavlink_port = int(mavlink_port)

# Connect to the ns3 network simulator
ns3interface.connect('127.0.0.1', mavlink_sysid - 1)

# Connect to the Vehicle
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
vehicle.simple_takeoff(target_altitude)  # Take off to target altitude
time.sleep(10)

if mavlink_sysid == 1:
    # We are the "reference" vehicle; we just hover a fixed point and
    # continuosly broadcast our position and a sequence number
    seqnum = 1
    while True:
        lat = vehicle.location.global_relative_frame.lat
        lon = vehicle.location.global_relative_frame.lon
        vehicle.simple_goto(LocationGlobalRelative(lat, lon, target_altitude))
        ns3interface.sendto(struct.pack("<Idd", seqnum, lat, lon), 1)
        seqnum = seqnum + 1
        time.sleep(.5)
elif mavlink_sysid == 2:
    # Set target position: a point that is more than 1 km away
    vehicle.simple_goto(LocationGlobalRelative(-35.3533385, 149.1635790, target_altitude))

    # We are the "probe" vehicle; we keep going further and print the distance
    # from the reference vehicle everytime we receive a message
    while True:
        # Process incoming messages
        while ns3interface.message_available():
            payload, sender = ns3interface.recvfrom()
            seqnum, lat, lon = struct.unpack("<Idd", payload)
            distance = helper.get_distance_metres(
                LocationGlobalRelative(lat, lon),
                vehicle.location.global_relative_frame
            )
            print('The probe vehicle received a message: seqnum={} distance={} meters'.format(seqnum, distance))

        time.sleep(.1)

# This line cannot be reached in this example
vehicle.close()
