---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Clover Gazebo. frame_id='map', absolute coords, z=1.0 always.

Fix after yellow (marker 8): no single jump to (0, 3.5) -
first go down in Y, then small step left (avoids uncontrolled left flyaway).
"""

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   # left side after marker 8

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


def go(x, y, t=5.5, speed=SPD):
    navigate(x=x, y=y, z=Z, speed=speed, frame_id='map')
    rospy.sleep(t)


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

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

    led(255, 0, 255)
    x, y = marker_xy(38)
    go(x, y, 6.0)
    rospy.sleep(2.0)

    led(255, 165, 0)
    x, y = mid(26, 33)
    go(x, y, 6.0)

    led(0, 255, 255)
    x, y = mid(19, 20)
    go(x, y, 6.0)

    led(128, 0, 255)
    x, y = marker_xy(12)
    go(x, y, 7.0)

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

    led(255, 0, 0)
    x, y = marker_xy(17)
    go(x, y, 6.0)

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

    led(255, 255, 0)
    x, y = marker_xy(8)
    go(x, y, 6.0)
    rospy.sleep(2.0)          # hover on yellow

    # --- left side: small steps, slow speed ---
    led(0, 255, 0)
    gx, gy = mid(14, 21)      # green block (0, 3.5)
    go(1.0, gy, 6.0, speed=SPD_SLOW)   # down in Y first, keep x=1
    go(gx, gy, 6.0, speed=SPD_SLOW)     # small step left to green

    led(0, 0, 139)
    bx, by = mid(22, 23)      # dark blue (1.5, 3)
    go(bx, by, 6.0, speed=SPD_SLOW)

    led(255, 0, 255)
    x, y = marker_xy(38)
    go(2.0, 2.0, 5.0, speed=SPD_SLOW)  # via midpoint to 38
    go(x, y, 7.0, speed=SPD_SLOW)

    x, y = marker_xy(42)
    go(1.0, 0.0, 5.0, speed=SPD_SLOW)   # via (1,0) not straight diagonal
    go(x, y, 6.0, speed=SPD_SLOW)

    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