---

#!/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)

try:
    # Шаг 1: Взлет на 1 метр с белой индикацией
    set_effect(effect='solid', r=255, g=255, b=255)
    navigate(x=0, y=0, z=1.0, speed=0.5, frame_id='body', auto_arm=True)
    rospy.sleep(5.0)

    # Шаг 2: Вперед на 1 метр к маркеру 44, затем на 1 метр к блоку 38
    navigate(x=1.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    navigate(x=1.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    set_effect(effect='solid', r=128, g=0, b=128)
    rospy.sleep(1.0)

    # Шаг 3: Полет по правой стороне к блоку 12
    # Смещаемся вправо на 5 метров к маркеру 48 (в ROS Клевера право — это минус Y)
    navigate(x=0, y=-5.0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(6.0)
    # Вперед на 1 метр над голубым блоком (маркер 41)
    navigate(x=1.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    set_effect(effect='solid', r=0, g=191, b=255)
    rospy.sleep(1.0)
    # Вперед на 2 метра над фиолетовым блоком (маркер 27)
    navigate(x=2.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(5.0)
    set_effect(effect='solid', r=128, g=0, b=128)
    rospy.sleep(1.0)
    # Вперед на 2 метра к маркеру 13
    navigate(x=2.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(5.0)
    # Влево на 1 метр к финишному блоку 12
    navigate(x=0, y=1.0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    set_effect(effect='solid', r=255, g=255, b=255)
    rospy.sleep(1.0)

    # Шаг 4: К блоку 17 через точку 11
    # Назад на 1 метр к маркеру 11
    navigate(x=-1.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    # Назад на 1 метр и вправо на 1 метр к блоку 17
    navigate(x=-1.0, y=-1.0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    set_effect(effect='solid', r=255, g=0, b=0)
    rospy.sleep(1.0)

    # Шаг 5: К блоку 8 через точку 9
    # Назад на 2 метра к маркеру 9
    navigate(x=-2.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(5.0)
    # Вправо на 1 метр к блоку 8
    navigate(x=0, y=-1.0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    set_effect(effect='solid', r=0, g=255, b=0)
    rospy.sleep(1.0)

    # Шаг 6: Полет по левой стороне обратно к блоку 38
    # Влево на 7 метров в самый левый ряд (маркер 0)
    navigate(x=0, y=7.0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(8.0)
    # Вперед на 1 метр над желтым блоком (маркер 7)
    navigate(x=1.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    set_effect(effect='solid', r=255, g=255, b=0)
    rospy.sleep(1.0)
    # Вперед на 2 метра над синим блоком (маркер 21)
    navigate(x=2.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(5.0)
    set_effect(effect='solid', r=0, g=0, b=255)
    rospy.sleep(1.0)
    # Вперед на 2 метра к маркеру 35
    navigate(x=2.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(5.0)
    # Вправо на 1 метр к блоку 38
    navigate(x=0, y=-1.0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    set_effect(effect='solid', r=128, g=0, b=128)
    rospy.sleep(1.0)

    # Шаг 7: Возврат на маркер 42 и посадка
    # Влево на 1 метр к маркеру 35
    navigate(x=0, y=1.0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(4.0)
    # Назад на 5 метров строго в точку старта (маркер 42)
    navigate(x=-5.0, y=0, z=0, speed=0.5, frame_id='body')
    rospy.sleep(7.0)

    land()
    rospy.sleep(4.0)
    set_effect(effect='solid', r=0, g=0, b=0)

except rospy.ROSInterruptException:
    pass