---

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

import rospy
from clover import srv
from std_srvs.srv import Trigger

rospy.init_node('clover_autonomous_mission')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
land = rospy.ServiceProxy('land', Trigger)
set_effect = rospy.ServiceProxy('led/set_effect', srv.SetLEDEffect)

def fly_map(x, y, z=1.0, speed=0.4, delay=6.5):
    navigate(x=x, y=y, z=z, speed=speed, frame_id='map', auto_arm=True)
    rospy.sleep(delay)

def set_led_color(r, g, b):
    set_effect(effect='fill', r=r, g=g, b=b)
    rospy.sleep(1.0)

try:
    # 1. Взлет строго на маркере 42
    set_led_color(255, 255, 255)
    navigate(x=5.0, y=5.5, z=1.0, speed=0.4, frame_id='map', auto_arm=True)
    rospy.sleep(6.0)

    # 2. К блоку 38 через маркер 44
    fly_map(x=5.0, y=5.0, delay=5.0)  # Точка 44
    fly_map(x=5.0, y=5.0, delay=5.0)  # Блок 38 (Фиолетовый)
    set_led_color(128, 0, 128)

    # 3. Полет по правой стороне к блоку 12
    fly_map(x=5.0, y=3.5, delay=5.5)  # Маркер 48
    fly_map(x=5.5, y=3.5, delay=5.0)  # Блок 41 (Голубой)
    set_led_color(0, 191, 255)
    fly_map(x=6.5, y=3.5, delay=5.0)  # Блок 27 (Фиолетовый)
    set_led_color(128, 0, 128)
    fly_map(x=7.5, y=3.5, delay=5.0)  # Маркер 13
    fly_map(x=4.5, y=2.5, delay=5.5)  # Блок 12 (Оранжевый)
    set_led_color(255, 165, 0)

    # 4. К блоку 17 через точку 11
    fly_map(x=4.5, y=3.0, delay=5.0)  # Точка 11
    fly_map(x=5.0, y=3.0, delay=5.0)  # Блок 17 (Красный)
    set_led_color(255, 0, 0)

    # 5. К блоку 8 через точку 9
    fly_map(x=5.0, y=4.0, delay=5.0)  # Точка 10
    fly_map(x=4.5, y=4.0, delay=5.0)  # Точка 9
    fly_map(x=4.5, y=4.5, delay=5.0)  # Блок 8 (Зеленый)
    set_led_color(0, 255, 0)

    # 6. Полет по левой стороне обратно к блоку 38
    fly_map(x=4.5, y=5.5, delay=5.5)  # Маркер 0
    fly_map(x=5.5, y=5.5, delay=5.0)  # Блок 7 (Розовый)
    set_led_color(255, 0, 255)
    fly_map(x=6.5, y=5.5, delay=5.0)  # Блок 21 (Синий)
    set_led_color(0, 0, 255)
    fly_map(x=7.5, y=5.5, delay=5.0)  # Маркер 35
    fly_map(x=5.0, y=5.0, delay=5.5)  # Возврат к блоку 38 (Фиолетовый)
    set_led_color(128, 0, 128)

    # 7. Возврат на маркер 42 и посадка
    fly_map(x=5.0, y=5.5, delay=6.0)  # Маркер 42

    land()
    rospy.sleep(5.0)
    set_led_color(0, 0, 0)

except rospy.ROSInterruptException:
    pass