---

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import math
from clover import Clover
from clover.srv import SetLEDs, Navigate
from std_srvs.srv import SetBool

rospy.init_node('flight_task')
clover = Clover()
clover.wait_for_connection()

set_leds = rospy.ServiceProxy('set_leds', SetLEDs)
navigate = rospy.ServiceProxy('navigate', Navigate)
arm = rospy.ServiceProxy('arm', SetBool)
land = rospy.ServiceProxy('land', SetBool)

NUM_LEDS = 30
HEIGHT = 1.0
SPEED = 0.5
TOL = 0.2


def set_color(r, g, b):
    for i in range(NUM_LEDS):
        set_leds(led=i, r=r, g=g, b=b)


def fly(x, y, z):
    navigate(x=x, y=y, z=z, frame_id='body', speed=SPEED, tolerance=TOL, auto_arm=False)
    while not rospy.is_shutdown():
        try:
            pos = clover.get_position()
            dist = math.sqrt((x - pos.x)**2 + (y - pos.y)**2 + (z - pos.z)**2)
            if dist < TOL:
                break
        except:
            pass
        rospy.sleep(0.5)


try:
    arm(True)
    rospy.sleep(1)
    set_color(255, 255, 255)
    fly(0, 0, HEIGHT)
    rospy.sleep(2)

    fly(2, 0, HEIGHT)
    fly(3, 1, HEIGHT)
    set_color(255, 0, 255)
    rospy.sleep(2)

    fly(4, 2, HEIGHT)
    set_color(255, 165, 0)
    rospy.sleep(1)

    fly(5, 3, HEIGHT)
    set_color(0, 255, 255)
    rospy.sleep(1)

    fly(5, 5, HEIGHT)
    set_color(128, 0, 128)
    rospy.sleep(2)

    fly(4, 5, HEIGHT)
    fly(3, 4, HEIGHT)
    set_color(255, 0, 0)
    rospy.sleep(2)

    fly(2, 5, HEIGHT)
    fly(1, 5, HEIGHT)
    set_color(255, 255, 0)
    rospy.sleep(2)

    fly(1, 4, HEIGHT)
    set_color(0, 255, 0)
    rospy.sleep(1)

    fly(2, 3, HEIGHT)
    set_color(0, 0, 255)
    rospy.sleep(1)

    fly(3, 1, HEIGHT)
    set_color(255, 0, 255)
    rospy.sleep(2)

    fly(0, 0, HEIGHT)
    rospy.sleep(1)
    fly(0, 0, 0.0)
    rospy.sleep(3)

    set_color(0, 0, 0)

except Exception as e:
    set_color(0, 0, 0)