---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Clover Gazebo. map frame, z=1.0.
LED after arriving. set_altitude + navigate keep offboard alive.

X_FIX: sim offset +1 m on X for right side (x >= 4).
After red: navigate_target hops (no pause = no landing/failsafe).
"""

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

Z = 1.0
SPD = 0.45
SPD_SLOW = 0.32
X_FIX = -1.0
FRAME_MAP = 'map'
FRAME_REL = 'navigate_target'

rospy.init_node('clover_autonomous_mission')
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 led(r, g, b):
    try:
        set_effect(effect='fill', r=r, g=g, b=b)
    except rospy.ServiceException:
        pass


def pulse_altitude(sec):
    end = rospy.Time.now() + rospy.Duration(sec)
    while rospy.Time.now() < end and not rospy.is_shutdown():
        set_altitude(z=Z, frame_id=FRAME_MAP)
        rospy.sleep(0.25)


def pulse_hover(sec, x, y):
    end = rospy.Time.now() + rospy.Duration(sec)
    while rospy.Time.now() < end and not rospy.is_shutdown():
        set_altitude(z=Z, frame_id=FRAME_MAP)
        navigate(x=x, y=y, z=Z, speed=0.15, frame_id=FRAME_MAP)
        rospy.sleep(0.25)


def fix_x(x, y):
    if x >= 4.0:
        return x + X_FIX, y
    return x, y


def go(x, y, t=5.0, speed=SPD):
    set_altitude(z=Z, frame_id=FRAME_MAP)
    navigate(x=x, y=y, z=Z, speed=speed, frame_id=FRAME_MAP)
    pulse_hover(t, x, y)


def go_rel(dx, dy, t=4.0, speed=SPD_SLOW):
    set_altitude(z=Z, frame_id=FRAME_MAP)
    navigate(x=dx, y=dy, z=Z, speed=speed, frame_id=FRAME_REL)
    pulse_altitude(t)


def go_led(x, y, r, g, b, t=5.0, speed=SPD):
    go(x, y, t, speed)
    led(r, g, b)
    pulse_hover(0.6, x, y)


def go_rel_led(dx, dy, r, g, b, t=4.0, speed=SPD_SLOW):
    go_rel(dx, dy, t, speed)
    led(r, g, b)
    pulse_altitude(0.5)


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


try:
    led(255, 255, 255)
    navigate(x=0, y=0, z=Z, speed=0.4, frame_id='body', auto_arm=True)
    rospy.sleep(6.0)
    set_altitude(z=Z, frame_id=FRAME_MAP)
    pulse_altitude(0.5)

    x, y = marker_xy(44)
    go(x, y, 5.0)

    x, y = marker_xy(38)
    go_led(x, y, 255, 0, 255, 5.5)
    pulse_altitude(0.5)

    x, y = fix_x(*mid(26, 33))
    go_led(x, y, 255, 165, 0, 5.5)

    x, y = fix_x(5.0, 4.0)
    go(x, y, 4.5)

    x, y = fix_x(*mid(19, 20))
    go_led(x, y, 0, 255, 255, 6.0)

    x, y = fix_x(*marker_xy(12))
    go_led(x, y, 128, 0, 255, 5.5)

    x, y = fix_x(*marker_xy(11))
    go(x, y, 4.5)

    x, y = marker_xy(17)
    go_led(x, y, 255, 0, 0, 4.5)

    # red -> yellow: relative hops, no pause (prevents landing / climb spike)
    go_rel(-1.0, 0.0, 3.5)
    go_rel(0.0, 1.0, 4.0)
    go_rel_led(-1.0, 0.0, 255, 255, 0, 4.0)

    gx, gy = mid(14, 21)
    go(1.0, gy, 5.0, speed=SPD_SLOW)
    go_led(gx, gy, 0, 255, 0, 5.0, speed=SPD_SLOW)

    bx, by = mid(22, 23)
    go(1.0, by, 4.5, speed=SPD_SLOW)
    go_led(bx, by, 0, 0, 139, 5.0, speed=SPD_SLOW)

    go(1.0, 1.0, 4.5, speed=SPD_SLOW)
    go(2.0, 1.0, 4.5, speed=SPD_SLOW)
    x, y = marker_xy(38)
    go_led(x, y, 255, 0, 255, 5.5, speed=SPD_SLOW)

    go(3.0, 0.0, 4.5, speed=SPD_SLOW)
    go(2.0, 0.0, 4.0, speed=SPD_SLOW)
    go(1.0, 0.0, 4.0, speed=SPD_SLOW)
    go(0.0, 0.0, 6.0, speed=SPD_SLOW)

    land()
    rospy.sleep(2.0)
    led(0, 0, 0)

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