##########DEPENDENCIES#############
from dronekit import connect, Vehicle, VehicleMode, LocationGlobalRelative, APIException, Parameters
from pymavlink import mavutil
import time
import socket
#import exceptions
import math
import argparse

#########FUNCTIONS#################
def connectMyCopter():

	parser = argparse.ArgumentParser(description='commands')
	parser.add_argument('--connect')
	args = parser.parse_args()

	connection_string = args.connect

	if not connection_string:
		import dronekit_sitl
		sitl = dronekit_sitl.start_default()
		connection_string = sitl.connection_string()

	vehicle = connect(connection_string,wait_ready=True)
	time.sleep(10)

	return vehicle

def arm_and_takeoff(targetHeight):
	while vehicle.is_armable!=True:
		print("Waiting for vehicle to become armable.")
		time.sleep(2)
	print("Vehicle is now armable")
	time.sleep(5)
	
	Vehicle.mode = VehicleMode("GUIDED")

	while vehicle.mode.name != 'GUIDED':
		print("Waiting for drone to enter GUIDED flight mode")
		time.sleep(2)
	print("Vehicle now in GUIDED MODE. Have fun!!: %s" % vehicle.mode.name)
	time.sleep(5)

	vehicle.armed = True
	while vehicle.armed==False:
		print("Waiting for vehicle to become armed!: %s" % vehicle.armed)
		time.sleep(1)
	print("Look out! Virtual props are spinning!!: %s" % vehicle.armed)
	time.sleep(2)

	vehicle.simple_takeoff(targetHeight) ##meters

	while True:
		print("Current Altitude: %d"%vehicle.location.global_relative_frame.alt)
		if vehicle.location.global_relative_frame.alt>=.95*targetHeight:
			break
		time.sleep(1)
	print("Target altitude reached!!")

	return None

##########MAIN EXECUTABLE###########

vehicle = connectMyCopter()
arm_and_takeoff(10)