---

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

Drone spawns on marker 42 (origin 0,0).
Each fly_rel() moves relative to the previous target point.
"""

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

rospy.init_node('clover_autonomous_mission')

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)  # optional debug
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):
    set_effect(effect='fill', r=r, g=g, b=b)
    rospy.sleep(0.5)


def fly_rel(dx, dy, z=FLIGHT_Z, speed=SPEED):
    """Fly dx/dy meters from current navigate_target."""
    navigate(x=dx, y=dy, z=z, speed=speed, frame_id='navigate_target')
    dist = math.hypot(dx, dy)
    rospy.sleep(max(dist / speed + 2.0, 3.5))


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


def rel_steps(points):
    """Absolute map points -> list of (dx, dy) relative steps."""
    steps = []
    px, py = points[0]
    for x, y in points[1:]:
        steps.append((x - px, y - py))
        px, py = x, y
    return steps


# --- Waypoints (absolute coords, origin = marker 42) ---
HOME = marker_xy(42)
M44 = marker_xy(44)
M38 = marker_xy(38)
ORANGE = midpoint(26, 33)
CYAN = midpoint(19, 20)
M12 = marker_xy(12)
M11 = marker_xy(11)
M17 = marker_xy(17)
M9 = marker_xy(9)
M8 = marker_xy(8)
GREEN = midpoint(14, 21)
DARK_BLUE = midpoint(22, 23)

_route_points = [
    M44,
    M38,
    ORANGE,
    CYAN,
    M12,
    M11,
    M17,
    M9,
    M8,
    GREEN,
    DARK_BLUE,
    M38,
    HOME,
]
_route_steps = rel_steps([HOME] + _route_points)

LED_AT = {
    1:  (255,   0, 255),   # pink  - marker 38
    2:  (255, 165,   0),   # orange block
    3:  (  0, 255, 255),   # cyan block
    4:  (128,   0, 255),   # purple - marker 12
    6:  (255,   0,   0),   # red - marker 17
    8:  (255, 255,   0),   # yellow - marker 8
    9:  (  0, 255,   0),   # green block
    10: (  0,   0, 139),   # dark blue block
    11: (255,   0, 255),   # pink - marker 38 again
}


try:
    # 1. Takeoff from marker 42, 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)

    # 2-6. Fly route
    for i, (dx, dy) in enumerate(_route_steps):
        fly_rel(dx, dy)
        if i in LED_AT:
            set_led(*LED_AT[i])

    # 7. Land on marker 42
    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