---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Autonomous flight: 7x7 ArUco field, marker 42 = origin (0,0).
Clover + Gazebo.

Navigation: absolute coords in 'map' frame (no drift like navigate_target).
Altitude: fixed z=1.0 in every navigate call (no set_altitude).
LED: turns on BEFORE flying to each colored point.

Route check (x,y from marker 42, step 1 m):
  42(0,0) -> 44(2,0) -> 38(3,1) pink ->
  orange(5,2.5) -> cyan(5.5,4) -> 12(5,5) purple ->
  11(4,5) -> 17(3,4) red -> 9(2,5) -> 8(1,5) yellow ->
  green(0,3.5) -> darkblue(1.5,3) -> 38(3,1) pink -> 42(0,0) land
"""

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
ARRIVE_DIST = 0.35
ARRIVE_TIMEOUT = 70.0
LED_PAUSE = 1.5
HOVER_PAUSE = 2.0

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(LED_PAUSE)


def wait_arrival():
    start = rospy.Time.now()
    while not rospy.is_shutdown():
        telem = get_telemetry(frame_id='navigate_target')
        dist = math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2)
        if dist < ARRIVE_DIST:
            rospy.sleep(HOVER_PAUSE)
            return True
        if (rospy.Time.now() - start).to_sec() > ARRIVE_TIMEOUT:
            rospy.logwarn('arrival timeout, dist=%.2f', dist)
            return False
        rospy.sleep(0.2)


def fly_to(x, y, label=''):
    if label:
        rospy.loginfo('fly -> %s (%.1f, %.1f, %.1f)', label, x, y, FLIGHT_Z)
    navigate(x=x, y=y, z=FLIGHT_Z, speed=SPEED, frame_id='map')
    wait_arrival()


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


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


# (x, y, name, LED rgb or None)  -- LED before this leg
MISSION = [
    (marker_xy(44)[0], marker_xy(44)[1], 'marker 44', None),
    (marker_xy(38)[0], marker_xy(38)[1], 'marker 38 pink', (255, 0, 255)),
    (mid(26, 33)[0], mid(26, 33)[1], 'orange block', (255, 165, 0)),
    (mid(19, 20)[0], mid(19, 20)[1], 'cyan block', (0, 255, 255)),
    (marker_xy(12)[0], marker_xy(12)[1], 'marker 12 purple', (128, 0, 255)),
    (marker_xy(11)[0], marker_xy(11)[1], 'marker 11', None),
    (marker_xy(17)[0], marker_xy(17)[1], 'marker 17 red', (255, 0, 0)),
    (marker_xy(9)[0], marker_xy(9)[1], 'marker 9', None),
    (marker_xy(8)[0], marker_xy(8)[1], 'marker 8 yellow', (255, 255, 0)),
    (mid(14, 21)[0], mid(14, 21)[1], 'green block', (0, 255, 0)),
    (mid(22, 23)[0], mid(22, 23)[1], 'dark blue block', (0, 0, 139)),
    (marker_xy(38)[0], marker_xy(38)[1], 'marker 38 pink', (255, 0, 255)),
    (marker_xy(42)[0], marker_xy(42)[1], 'marker 42 home', None),
]


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(6.0)

    for x, y, name, color in MISSION:
        if color is not None:
            set_led(*color)
        fly_to(x, y, name)

    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