import math
import threading
import time

from dronekit import connect, LocationGlobal, LocationGlobalRelative

# Connect to a list of Vehicles.
# Note: connect() blocks until ArduCopter is actually ready to respond, but this
# can only happen after all connections have been enstablished. In order to
# avoid this circular dependency, we run all connect() calls in parallel.
def connect_multiple(argv):
    vehicles = [ None ] * len(argv)

    print("Connecting to all vechicles...")

    def do_connect(i, mavlink_port, mavlink_sysid):
        print(" Connecting to vehicle {}...".format(i))
        vehicles[i] = connect(
            'tcp:127.0.0.1:{}'.format(mavlink_port),
            source_system=mavlink_sysid + 100)
        print(" Connected to vehicle {}".format(i))

    # Prepare our threads
    connection_threads = []
    for i, arg in enumerate(argv):
        uav_name, mavlink_sysid, mavlink_port = arg.split(':')
        mavlink_sysid = int(mavlink_sysid)
        mavlink_port = int(mavlink_port)
        thread = threading.Thread(target=do_connect, args=(i, mavlink_port, mavlink_sysid))
        connection_threads.append(thread)

    # Start them
    for thread in connection_threads:
        thread.start()

    # Wait for them to complete the connect() call
    for thread in connection_threads:
        if thread.is_alive():
            time.sleep(1) # let simulation time advance while we wait
        else:
            thread.join()

    return vehicles

# source: http://python.dronekit.io/examples/guided-set-speed-yaw-demo.html
def get_location_metres(original_location, dNorth, dEast):
    """
    Returns a LocationGlobal object containing the latitude/longitude `dNorth` and `dEast` metres from the 
    specified `original_location`. The returned LocationGlobal has the same `alt` value
    as `original_location`.

    The function is useful when you want to move the vehicle around specifying locations relative to 
    the current vehicle position.

    The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles.

    For more information see:
    http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters
    """
    earth_radius = 6378137.0 # Radius of "spherical" earth
    # Coordinate offsets in radians
    dLat = dNorth/earth_radius
    dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180))

    # New position in decimal degrees
    newlat = original_location.lat + (dLat * 180/math.pi)
    newlon = original_location.lon + (dLon * 180/math.pi)
    if type(original_location) is LocationGlobal:
        targetlocation=LocationGlobal(newlat, newlon,original_location.alt)
    elif type(original_location) is LocationGlobalRelative:
        targetlocation=LocationGlobalRelative(newlat, newlon,original_location.alt)
    else:
        raise Exception("Invalid Location object passed")

    return targetlocation
