---

#!/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_body(x, y, z=0.0, speed=0.5, delay=5.0):
    """Функция перемещения относительно текущего положения дрона"""
    navigate(x=x, y=y, z=z, speed=speed, frame_id='body', 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(0.5)

try:
    # === ШАГ 1: СТАРТ И ВЗЛЕТ ===
    # Включаем белую индикацию по заданию
    set_led_color(255, 255, 255)
    # Взлетаем вертикально вверх на высоту 1.0 метр
    navigate(x=0.0, y=0.0, z=1.0, speed=0.5, frame_id='body', auto_arm=True)
    rospy.sleep(5.0)

    # === ШАГ 2: К БЛОКУ №38 ЧЕРЕЗ ТОЧКУ №44 ===
    # Летим вперед на 1 метр (оказываемся над точкой 44)
    fly_body(x=1.0, y=0.0)
    # Летим вперед еще на 1 метр (оказываемся над блоком 38)
    fly_body(x=1.0, y=0.0)
    # Включаем фиолетовый цвет блока 38
    set_led_color(128, 0, 128)

    # === ШАГ 3: ПОЛЕТ ПО ПРАВОЙ СТОРОНЕ К БЛОКУ №12 ===
    # Смещаемся вбок на 5 метров на правую сторону карты к ряду маркеров
    fly_body(x=0.0, y=5.0, delay=6.0)
    # Летим вперед на 1 метр (оказываемся над голубым блоком, маркер 41)
    fly_body(x=1.0, y=0.0)
    # Включаем голубой цвет
    set_led_color(0, 191, 255)
    
    # Летим вперед на 2 метра (оказываемся над фиолетовым блоком, маркер 27)
    fly_body(x=2.0, y=0.0, delay=5.0)
    # Включаем фиолетовый цвет
    set_led_color(128, 0, 128)
    
    # Летим вперед еще на 2 метра (долетаем до маркера 13)
    fly_body(x=2.0, y=0.0, delay=5.0)
    # Смещаемся вбок на 1 метр (оказываемся точно над финишным блоком 12)
    fly_body(x=0.0, y=1.0)
    # Включаем белый/серый цвет блока 12
    set_led_color(255, 255, 255)

    # === ШАГ 4: К БЛОКУ №17 ЧЕРЕЗ ТОЧКУ №11 ===
    # Летим назад на 1 метр (возвращаемся на маркер 11)
    fly_body(x=-1.0, y=0.0)
    # Летим назад на 1 метр и смещаемся вбок на 1 метр к блоку 17
    fly_body(x=-1.0, y=-1.0)
    # Включаем красный цвет блока 17
    set_led_color(255, 0, 0)

    # === ШАГ 5: К БЛОКУ №8 ЧЕРЕЗ ТОЧКУ №9 ===
    # Летим назад на 2 метра (оказываемся на маркере 9)
    fly_body(x=-2.0, y=0.0, delay=5.0)
    # Смещаемся вбок на 1 метр (оказываемся над блоком 8)
    fly_body(x=0.0, y=-1.0)
    # Включаем зеленый цвет блока 8
    set_led_color(0, 255, 0)

    # === ШАГ 6: ПОЛЕТ ПО ЛЕВОЙ СТОРОНЕ ОБРАТНО К БЛОКУ №38 ===
    # Смещаемся вбок на 7 метров в самый левый ряд карты (маркер 0)
    fly_body(x=0.0, y=7.0, delay=8.0)
    # Летим вперед на 1 метр (оказываемся над желтым блоком, маркер 7)
    fly_body(x=1.0, y=0.0)
    # Включаем желтый цвет
    set_led_color(255, 255, 0)
    
    # Летим вперед на 2 метра (оказываемся над синим блоком, маркер 21)
    fly_body(x=2.0, y=0.0, delay=5.0)
    # Включаем синий цвет
    set_led_color(0, 0, 255)
    
    # Летим вперед еще на 2 метра (долетаем до маркера 35)
    fly_body(x=2.0, y=0.0, delay=5.0)
    # Смещаемся вбок на 1 метр (возвращаемся на блок 38)
    fly_body(x=0.0, y=1.0)
    # Включаем фиолетовый цвет блока 38
    set_led_color(128, 0, 128)

    # === ШАГ 7: ВОЗВРАТ И ПОСАДКА НА МАРКЕР №42 ===
    # Смещаемся обратно в левый ряд на маркер 35
    fly_body(x=0.0, y=-1.0)
    # Летим назад на 5 метров строго в точку старта (маркер 42)
    fly_body(x=-5.0, y=0.0, delay=6.0)

    # Выполняем посадку
    land()
    rospy.sleep(4.0)
    # Полностью выключаем светодиодную ленту (черный цвет)
    set_led_color(0, 0, 0)

except rospy.ROSInterruptException:
    pass