---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Autonomous flight: 7x7 ArUco field (markers 0-48).
Clover + Gazebo, relative navigation via navigate_target.

LED turns ON before flying to each colored point (visible during overflight).
Altitude held via set_altitude (avoids invalid setpoints / climb).
"""

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.35
LED_PAUSE = 2.0       # pause after LED on (visible on video)
HOVER_PAUSE = 1.5     # pause over each point before next leg

rospy.init_node('clover_autonomous_mission')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
land = rospy.ServiceProxy('land', Trigger)
set_effect = rospy.ServiceProxy('led/set_effect', SetLEDEffect)


def set_led(r, g, b):
    try:
        set_effect(effect='fill', r=r, g=g, b=b)
    except rospy.ServiceException:
        pass
    rospy.sleep(LED_PAUSE)


def hold_altitude():
    set_altitude(z=FLIGHT_Z)


def fly_rel(dx, dy, speed=SPEED):
    """Move dx/dy in navigate_target frame, keep altitude fixed."""
    hold_altitude()
    navigate(x=dx, y=dy, speed=speed, frame_id='navigate_target')
    dist = math.hypot(dx, dy)
    rospy.sleep(max(dist / speed + 2.5, 4.0))
    rospy.sleep(HOVER_PAUSE)


def marker_xy(marker_id):
    row = marker_id // 7
    col = marker_id % 7
    return col * 1.0, (6 - row) * 1.0


def midpoint(a, b):
    xa, ya = marker_xy(a)
    xb, yb = marker_xy(b)
    return (xa + xb) / 2.0, (ya + yb) / 2.0


# (dx, dy, LED rgb or None) — LED before flight to this point
LEG = [
    (2.0,  0.0,  None),              # over marker 44
    (1.0,  1.0,  (255,   0, 255)),   # marker 38 pink
    (2.0,  1.5,  (255, 165,   0)),   # orange block
    (0.5,  1.5,  (  0, 255, 255)),   # cyan block
    (-0.5, 1.0,  (128,   0, 255)),   # marker 12 purple
    (-1.0, 0.0,  None),              # over marker 11
    (-1.0,-1.0,  (255,   0,   0)),   # marker 17 red
    (-1.0, 1.0,  None),              # over marker 9
    (-1.0, 0.0,  (255, 255,   0)),   # marker 8 yellow
    (-1.0,-1.5,  (  0, 255,   0)),   # green block
    ( 1.5,-0.5,  (  0,   0, 139)),   # dark blue block
    ( 1.5,-2.0,  (255,   0, 255)),   # marker 38 pink
    (-3.0,-1.0,  None),              # marker 42 home
]


try:
    # 1. Takeoff, white LED
    set_led(255, 255, 255)
    navigate(x=0, y=0, z=FLIGHT_Z, speed=0.4, frame_id='body', auto_arm=True)
    rospy.sleep(6.0)
    hold_altitude()

    # 2-6. Route
    for dx, dy, color in LEG:
        if color is not None:
            set_led(*color)
        fly_rel(dx, dy)

    # 7. Land
    land()
    rospy.sleep(5.0)
    set_led(0, 0, 0)

except rospy.ROSInterruptException:
    pass
except KeyboardInterrupt:
    try:
        land()
        set_led(0, 0, 0)
    except rospy.ServiceException:
        pass