---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Автономный полёт по трассе на полигоне ArUco-меток (7×7, маркеры 0–48).
Платформа: Clover + Gazebo + ROS (simple_offboard).

Маршрут:
  42 (взлёт, белый LED) → 44 → 38 (розовый) →
  правый борт (оранжевый, голубой) → 12 (фиолетовый) →
  11 → 17 (красный) → 9 → 8 (жёлтый) →
  левый борт (зелёный, тёмно-синий) → 38 (розовый) →
  42 (посадка, LED выкл.)
"""

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

# --- Параметры полёта ---
FLIGHT_Z = 1.0          # высота полёта, м
SPEED = 0.8             # скорость, м/с
TOLERANCE = 0.25        # допуск достижения точки, м
ARUCO_MAP_WAIT = 5.0    # ожидание появления фрейма aruco_map после взлёта, с

# --- ROS-сервисы ---
rospy.init_node('autonomous_flight')

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', SetLEDEffect)


def set_led(r, g, b, effect='fill'):
    """Включить светодиодную ленту заданным цветом (RGB 0–255)."""
    set_effect(effect=effect, r=r, g=g, b=b)


def led_off():
    set_led(0, 0, 0)


# Цвета блоков
WHITE   = (255, 255, 255)
PINK    = (255,   0, 255)
ORANGE  = (255, 128,   0)
CYAN    = (  0, 255, 255)
PURPLE  = (128,   0, 255)
RED     = (255,   0,   0)
YELLOW  = (255, 255,   0)
GREEN   = (  0, 255,   0)
DARK_BLUE = (  0,   0, 139)


def marker_xy(marker_id):
    """
    Координаты центра маркера в фрейме aruco_map.
    Начало координат — маркер №42 (левый нижний угол).
    Ось X — вправо, ось Y — вверх (к маркеру №0).
    Шаг между центрами меток — 1 м.
    """
    row = marker_id // 7   # 0 — верхний ряд (маркеры 0–6)
    col = marker_id % 7
    return col * 1.0, (6 - row) * 1.0


def midpoint(marker_a, marker_b):
    """Середина между центрами двух маркеров."""
    xa, ya = marker_xy(marker_a)
    xb, yb = marker_xy(marker_b)
    return (xa + xb) / 2.0, (ya + yb) / 2.0


# Координаты блоков, расположенных между метками
GREEN_BLOCK     = midpoint(14, 21)   # зелёный: между 14 и 21
DARK_BLUE_BLOCK = midpoint(22, 23)     # тёмно-синий: между 22 и 23
CYAN_BLOCK      = midpoint(19, 20)   # голубой: между 19 и 20
ORANGE_BLOCK    = midpoint(26, 33)   # оранжевый: между 26 и 33


def wait_arrival(tolerance=TOLERANCE):
    """Ожидание достижения целевой точки navigate."""
    while not rospy.is_shutdown():
        telem = get_telemetry(frame_id='navigate_target')
        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
            break
        rospy.sleep(0.2)


def fly_to(x, y, z=FLIGHT_Z, frame_id='aruco_map', speed=SPEED):
    """Полёт в точку и ожидание прибытия."""
    navigate(x=x, y=y, z=z, speed=speed, frame_id=frame_id)
    wait_arrival()


def fly_to_marker(marker_id, z=FLIGHT_Z, speed=SPEED):
    """Полёт над центром маркера."""
    fly_to(0, 0, z=z, frame_id='aruco_{}'.format(marker_id), speed=speed)


def fly_to_block(xy, color, z=FLIGHT_Z, speed=SPEED):
    """Полёт над блоком между метками с включением LED."""
    set_led(*color)
    fly_to(xy[0], xy[1], z=z, speed=speed)


def takeoff():
    """Взлёт с маркера №42 на высоту 1 м."""
    set_led(*WHITE)
    navigate(x=0, y=0, z=FLIGHT_Z, speed=0.5, frame_id='body', auto_arm=True)
    wait_arrival()
    rospy.sleep(ARUCO_MAP_WAIT)


def land_at_home():
    """Посадка на маркер №42."""
    fly_to_marker(42, z=FLIGHT_Z)
    land()
    while get_telemetry().armed:
        rospy.sleep(0.2)
    led_off()


def run_mission():
    takeoff()

    # Пролёт над №44, затем над розовым блоком на №38
    fly_to_marker(44)
    set_led(*PINK)
    fly_to_marker(38)

    # Правый борт: оранжевый → голубой → фиолетовый (№12)
    fly_to_block(ORANGE_BLOCK, ORANGE)
    fly_to_block(CYAN_BLOCK, CYAN)
    set_led(*PURPLE)
    fly_to_marker(12)

    # Над №11, затем над красным блоком на №17
    fly_to_marker(11)
    set_led(*RED)
    fly_to_marker(17)

    # Над №9, затем над жёлтым блоком на №8
    fly_to_marker(9)
    set_led(*YELLOW)
    fly_to_marker(8)

    # Левый борт: зелёный → тёмно-синий → розовый (№38)
    fly_to_block(GREEN_BLOCK, GREEN)
    fly_to_block(DARK_BLUE_BLOCK, DARK_BLUE)
    set_led(*PINK)
    fly_to_marker(38)

    land_at_home()


if __name__ == '__main__':
    try:
        run_mission()
    except rospy.ROSInterruptException:
        pass
    except KeyboardInterrupt:
        rospy.loginfo('Прерывание — посадка')
        try:
            land()
            led_off()
        except rospy.ServiceException:
            pass