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

import sys
import random

import caminterface
import cv2
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, cambuffer_ip, cambuffer_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)

caminterface.connect(cambuffer_ip, int(cambuffer_port))

# ArduCopter initialisation can take a really long time
vehicle.wait_ready('gps_0', 'armed', 'mode', 'attitude', 'gimbal', 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

# 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

# Note: vehicle.gimbal.target_location only works if ArduCopter is already
# in position control. We have to issue a vehicle.simple_goto command first!
vehicle.simple_goto(LocationGlobalRelative(
        -35.363261, 149.165230, target_altitude
    ), groundspeed=10)

# Aim the gimbal at the top of the fountain
time.sleep(1)
vehicle.gimbal.target_location(LocationGlobalRelative(
    -35.363261, 149.165230, 2
))

# Move randomly for a while, while displaying the image in a OpenCV window
for i in range(10):
    vehicle.simple_goto(LocationGlobalRelative(
        -35.363261 + random.uniform(-.0001, +.0001),
        149.165230 + random.uniform(-.0001, +.0001),
        target_altitude + random.uniform(-3, +3)
    ), groundspeed=10)

    # Sleep for five seconds, while displaying incoming images
    for j in range(50):
        img, ts = caminterface.next_image()
        if img is not None:
            #print('received image with timestamp={}'.format(ts))
            cv2.imshow('{}: image'.format(uav_name), img)
            cv2.waitKey(1)
        time.sleep(.1)

cv2.destroyAllWindows()

# Land
print("Landing!")
vehicle.gimbal.release()
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()
