---

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

import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from clover import srv
from std_srvs.srv import Trigger

rospy.init_node('clover_cv_mission')

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

current_color = "white"

def image_callback(data):
    global current_color
    try:
        # Конвертируем ROS-картинку с камеры в формат OpenCV
        cv_image = bridge.imgmsg_to_cv2(data, "bgr8")
        h, w, _ = cv_image.shape
        
        # Вырезаем только центральный квадрат кадра, куда смотрит дрон
        roi = cv_image[int(h/3):int(2*h/3), int(w/3):int(2*w/3)]
        hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
        
        # Рассчитываем средний цвет в центре кадра
        avg_hsv = np.mean(roi, axis=(0, 1))
        hue = avg_hsv[0]
        sat = avg_hsv[1]
        val = avg_hsv[2]
        
        # Детектор цветов по диапазонам HSV в Gazebo
        if sat > 50 and val > 50:
            if hue < 10 or hue > 170:
                current_color = "red"
                set_effect(effect='fill', r=255, g=0, b=0)
            elif 10 <= hue < 25:
                current_color = "orange"
                set_effect(effect='fill', r=255, g=165, b=0)
            elif 25 <= hue < 35:
                current_color = "yellow"
                set_effect(effect='fill', r=255, g=255, b=0)
            elif 35 <= hue < 85:
                current_color = "green"
                set_effect(effect='fill', r=0, g=255, b=0)
            elif 85 <= hue < 125:
                current_color = "blue"
                set_effect(effect='fill', r=0, g=0, b=255)
            elif 125 <= hue < 145:
                current_color = "purple"
                set_effect(effect='fill', r=128, g=0, b=128)
            elif 145 <= hue < 170:
                current_color = "pink"
                set_effect(effect='fill', r=255, g=0, b=255)
    except Exception as e:
        pass

# Подписываемся на топик нижней камеры Клевера
image_sub = rospy.Subscriber('/main_camera/image_raw', Image, image_callback)

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

try:
    # 1. Взлет (Белая индикация на старте)
    set_effect(effect='fill', r=255, g=255, b=255)
    navigate(x=-3.0, y=-3.0, z=1.0, speed=0.4, frame_id='map', auto_arm=True)
    rospy.sleep(6.0)

    # 2. К блоку 38 через маркер 44
    fly_absolute(x=-3.0, y=-1.0, delay=5.0)  
    fly_absolute(x=-2.0, y=-1.0, delay=5.0)  

    # 3. Полет по правой стороне к блоку 12
    fly_absolute(x=-3.0, y=3.0, delay=6.0)   
    fly_absolute(x=-2.0, y=3.0, delay=5.0)   
    fly_absolute(x=0.0, y=3.0, delay=5.0)    
    fly_absolute(x=2.0, y=3.0, delay=5.0)    
    fly_absolute(x=2.0, y=2.0, delay=5.0)    

    # 4. К блоку 17 через точку 11
    fly_absolute(x=2.0, y=1.0, delay=5.0)    
    fly_absolute(x=1.0, y=1.0, delay=5.0)    

    # 5. К блоку 8 через точку 9
    fly_absolute(x=1.0, y=-1.0, delay=5.0)   
    fly_absolute(x=2.0, y=-1.0, delay=5.0)   
    fly_absolute(x=2.0, y=-2.0, delay=5.0)   

    # 6. Полет по левой стороне обратно к блоку 38
    fly_absolute(x=3.0, y=-3.0, delay=6.0)   
    fly_absolute(x=2.0, y=-3.0, delay=5.0)   
    fly_absolute(x=0.0, y=-3.0, delay=5.0)   
    fly_absolute(x=-1.0, y=-3.0, delay=5.0)  
    fly_absolute(x=-2.0, y=-1.0, delay=5.0)  

    # 7. Возврат на маркер 42 и посадка
    fly_absolute(x=-3.0, y=-3.0, delay=6.0)

    land()
    rospy.sleep(5.0)
    set_effect(effect='fill', r=0, g=0, b=0)

except rospy.ROSInterruptException:
    pass