#!/usr/bin/python
import rospy
import thread
import sys
import math
from mavros_msgs.msg import RCIn
from clever import srv
from time import sleep
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool

states = ('start','stop','unknown')
state = states[2]

rospy.init_node('Clever3_RC_Script')

navigate = rospy.ServiceProxy('/navigate', srv.Navigate)
set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
get_telemetry = rospy.ServiceProxy('/get_telemetry', srv.GetTelemetry)
arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)

def get_distance(x1, y1, z1, x2, y2, z2):
	return math.sqrt((x1-x2)**2 + (y1-y2)**2 +(z1-z2)**2)

def takeoff (zp, sp = 1, tolerance = 0.2):
	start = get_telemetry()
	print navigate(z=zp, speed=sp, frame_id='fcu_horiz', auto_arm=True)
	while True:
		telem = get_telemetry()
		delta = abs(abs(telem.z - start.z)-zp)
		if delta < tolerance:
			break
		rospy.sleep(0.2)

def land(sp = 1, land_height = -1, tolerance = 0.25):
	print 'land!'
	z0 = get_telemetry(frame_id='local_origin').z
	print z0
	h = get_telemetry(frame_id='aruco_map').z
	print h
	print navigate(z=-h+land_height, speed=sp, frame_id='fcu_horiz')
	while True:
		z = get_telemetry(frame_id='local_origin').z
		delta = z0-z-h
		print delta
		if (abs(delta) < tolerance):
			print get_telemetry(frame_id='local_origin')
			arming(False)
			break
		rospy.sleep(0.2)

def flight_to_point(xp, yp, zp, sp = 1, breakable = True, tolerance = 0.2, constant_yaw = True):
	frame_id = 'aruco_map'
	if constant_yaw:
		current_yaw = get_telemetry(frame_id = 'aruco_map').yaw
		print navigate(frame_id=frame_id, x=xp, y=yp, z=zp, speed=sp, yaw = current_yaw)
	else:
		print navigate(frame_id=frame_id, x=xp, y=yp, z=zp, speed=sp)
	while True:
		if breakable and state == 'stop':
			return
		telem = get_telemetry(frame_id=frame_id)
		if get_distance(xp, yp, zp, telem.x, telem.y, telem.z) < tolerance:
			break
		rospy.sleep(0.2)

# copter parameters

speed = 1
z = 1

# rectangle parameters

width = 1
height = 1
x0 = 0.1
y0 = 0.6

# flight program

def flight_program (param):
	while True:

		global state
		print 'waiting for stop!'
		while state != 'stop':
		    rospy.sleep(0.1)
		print 'waiting for start...'
		while state == 'stop':
		    rospy.sleep(0.1)
		print 'start!'

		takeoff(z) #takeoff
		flight_to_point(x0, y0, z, speed)
		while True:
			flight_to_point(x0, y0 + height, z, speed)
			flight_to_point(x0 + width, y0 + height, z, speed)
			flight_to_point(x0 + width, y0, z, speed)
			flight_to_point(x0, y0, z, speed, breakable = False)
			if state == 'stop':
				break
		land()

# Вызывается при обновлении данных из топика
def callback(data):
	global state
	# Обрабатываем данные с 6 канала пульта
	if data.channels[5] < 1100:
		state = states[1]
	elif data.channels[5] > 1900:
		state = states[0]
	else:
		state = states[2]

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.

	rospy.Subscriber('mavros/rc/in', RCIn, callback)

    # spin() simply keeps python from exiting until this node is stopped

	rospy.spin()

param = []
thread.start_new_thread(flight_program, (param,))
listener()