---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Clover + Gazebo. Relative flight via navigate_target (works in this sim).
No set_altitude, no map frame. Short pauses (battery / offboard failsafe).

Route from marker 42 (0,0), step 1 m:
  44 -> 38[pink] -> orange -> cyan -> 12[purple] -> 11 ->
  17[red] -> 9 -> 8[yellow] -> green -> darkblue -> 38[pink] -> 42
"""

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.45

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


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


def fly_rel(dx, dy, extra_wait=0.0):
    """Relative move in navigate_target, fixed z=1.0."""
    navigate(x=dx, y=dy, z=FLIGHT_Z, speed=SPEED, frame_id='navigate_target')
    dist = math.hypot(dx, dy)
    wait = dist / SPEED + 2.0 + extra_wait
    deadline = rospy.Time.now() + rospy.Duration(min(wait, 20.0))
    while rospy.Time.now() < deadline and not rospy.is_shutdown():
        t = get_telemetry(frame_id='navigate_target')
        if math.sqrt(t.x ** 2 + t.y ** 2 + t.z ** 2) < 0.4:
            break
        rospy.sleep(0.15)
    rospy.sleep(0.5)


# (dx, dy, LED before leg or None, extra_wait sec)
LEG = [
    ( 2.0,  0.0, None,              0),   # 44
    ( 1.0,  1.0, (255,   0, 255),   0),   # 38 pink
    ( 2.0,  1.5, (255, 165,   0),   0),   # orange
    ( 0.5,  1.5, (  0, 255, 255),   0),   # cyan
    (-0.5,  1.0, (128,   0, 255),   1),   # 12 purple (+1s, was short leg)
    (-1.0,  0.0, None,              0),   # 11
    (-1.0, -1.0, (255,   0,   0),   0),   # 17 red
    (-1.0,  1.0, None,              0),   # 9
    (-1.0,  0.0, (255, 255,   0),   0),   # 8 yellow
    (-1.0, -1.5, (  0, 255,   0),   0),   # green
    ( 1.5, -0.5, (  0,   0, 139),   0),   # dark blue
    ( 1.5, -2.0, (255,   0, 255),   0),   # 38 pink
    (-3.0, -1.0, None,              0),   # 42 home
]


try:
    set_led(255, 255, 255)
    navigate(x=0, y=0, z=FLIGHT_Z, speed=0.4, frame_id='body', auto_arm=True)
    rospy.sleep(5.0)

    for dx, dy, color, extra in LEG:
        if color:
            set_led(*color)
        fly_rel(dx, dy, extra_wait=extra)

    land()
    rospy.sleep(3.0)
    set_led(0, 0, 0)

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