---

#!/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_grid(x, y, z=1.0, speed=0.4, delay=7.0):
    navigate(x=x, y=y, z=z, speed=speed, frame_id='navigate_target', 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. ВЗЛЕТ НА 1 МЕТР
    set_led_color(255, 255, 255)
    navigate(x=0.0, y=0.0, z=1.0, speed=0.4, frame_id='body', auto_arm=True)
    rospy.sleep(6.0)

    # 2. К БЛОКУ № 38 ЧЕРЕЗ ТОЧКУ № 44 (Вперед на строку блока, затем вбок)
    fly_grid(x=1.0, y=0.0, delay=5.0)  
    fly_grid(x=1.0, y=2.0, delay=6.0) 
    set_led_color(128, 0, 128)          

    # 3. ПОЛЕТ ПО ПРАВОЙ СТОРОНЕ К БЛОКУ № 12 (По диагонали)
    fly_grid(x=1.0, y=5.0, delay=6.0) 
    set_led_color(0, 191, 255)          
    
    fly_grid(x=3.0, y=5.0, delay=6.0)  
    set_led_color(128, 0, 128)          
    
    fly_grid(x=5.0, y=4.0, delay=6.0)  
    set_led_color(255, 165, 0)        

    # 4. К БЛОКУ № 17 ЧЕРЕЗ ТОЧКУ № 11 (Сначала назад по X, потом по Y)
    fly_grid(x=4.0, y=4.0, delay=5.0) 
    fly_grid(x=3.0, y=4.0, delay=5.0) 
    fly_grid(x=3.0, y=5.0, delay=5.0) 
    set_led_color(255, 0, 0)            

    # 5. К БЛОКУ № 8 ЧЕРЕЗ ТОЧКУ № 9 (Сначала назад по X, потом по Y)
    fly_grid(x=2.0, y=5.0, delay=5.0) 
    fly_grid(x=2.0, y=4.0, delay=5.0)  
    fly_grid(x=3.0, y=4.0, delay=5.0)  
    set_led_color(0, 255, 0)            

    # 6. ПОЛЕТ ПО ЛЕВОЙ СТОРОНЕ К БЛОКУ № 38 (По диагонали)
    fly_grid(x=4.0, y=3.0, delay=6.0)  
    
    fly_grid(x=2.5, y=0.0, delay=6.0) 
    set_led_color(255, 0, 255)          
    
    fly_grid(x=0.5, y=0.0, delay=6.0) 
    set_led_color(0, 0, 255)            
    
    fly_grid(x=1.0, y=2.0, delay=7.0) 
    set_led_color(128, 0, 128)          

    # 7. ФИНИШ НА МАРКЕР 42
    fly_grid(x=0.0, y=0.0, delay=6.0) 

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

except rospy.ROSInterruptException:
    pass