---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Clover Gazebo. navigate_target + fixed delays (stable in VM).
LED ON before each colored leg. Pause over pink before orange.
"""

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

Z = 1.0
SPD = 0.4

rospy.init_node('clover_autonomous_mission')
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
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
    rospy.sleep(1.0)


def step(dx, dy, t=6.0):
    navigate(x=dx, y=dy, z=Z, speed=SPD, frame_id='navigate_target')
    rospy.sleep(t)


try:
    # 1. takeoff, white
    led(255, 255, 255)
    navigate(x=0, y=0, z=Z, speed=0.4, frame_id='body', auto_arm=True)
    rospy.sleep(6.0)

    # 2. marker 44
    step(2.0, 0.0, 5.0)

    # 3. marker 38 pink
    led(255, 0, 255)
    step(1.0, 1.0, 6.0)
    rospy.sleep(3.0)          # hover on pink, do NOT switch orange yet

    # 4. orange block (split into 2 short legs)
    led(255, 165, 0)
    step(1.0, 0.75, 5.0)
    step(1.0, 0.75, 5.0)

    # 5. cyan block
    led(0, 255, 255)
    step(0.5, 1.5, 6.0)

    # 6. marker 12 purple
    led(128, 0, 255)
    step(-0.5, 1.0, 7.0)

    # 7. marker 11
    step(-1.0, 0.0, 5.0)

    # 8. marker 17 red
    led(255, 0, 0)
    step(-1.0, -1.0, 6.0)

    # 9. marker 9
    step(-1.0, 1.0, 5.0)

    # 10. marker 8 yellow
    led(255, 255, 0)
    step(-1.0, 0.0, 5.0)

    # 11. green block
    led(0, 255, 0)
    step(-1.0, -1.5, 6.0)

    # 12. dark blue block
    led(0, 0, 139)
    step(1.5, -0.5, 6.0)

    # 13. marker 38 pink again
    led(255, 0, 255)
    step(1.5, -2.0, 7.0)

    # 14. home marker 42
    step(-3.0, -1.0, 7.0)

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

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