---

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

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

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', srv.SetEffect)

def fly_to_marker(marker_id, x=0.0, y=0.0, z=1.0, speed=0.5):
    navigate(x=x, y=y, z=z, speed=speed, frame_id=f'aruco_{marker_id}', auto_arm=True)
    rospy.sleep(4.5)

def set_led_color(r, g, b):
    set_effect(effect='solid', r=r, g=g, b=b)
    rospy.sleep(0.5)

try:
    set_led_color(255, 255, 255)
    fly_to_marker(42, z=1.0)

    fly_to_marker(44, z=1.0)
    fly_to_marker(38, z=1.0)
    set_led_color(128, 0, 128)
    rospy.sleep(1.0)

    fly_to_marker(48, z=1.0)
    fly_to_marker(41, z=1.0)
    set_led_color(0, 191, 255)
    
    fly_to_marker(27, z=1.0)
    set_led_color(128, 0, 128)
    
    fly_to_marker(13, z=1.0)
    fly_to_marker(12, z=1.0)
    set_led_color(255, 255, 255)

    fly_to_marker(11, z=1.0)
    fly_to_marker(17, z=1.0)
    set_led_color(255, 0, 0)

    fly_to_marker(9, z=1.0)
    fly_to_marker(8, z=1.0)
    set_led_color(0, 255, 0)

    fly_to_marker(0, z=1.0)
    fly_to_marker(7, z=1.0)
    set_led_color(255, 255, 0)
    
    fly_to_marker(21, z=1.0)
    set_led_color(0, 0, 255)
    
    fly_to_marker(35, z=1.0)
    fly_to_marker(38, z=1.0)
    set_led_color(128, 0, 128)

    fly_to_marker(42, z=1.0)
    rospy.sleep(1.0)
    
    land()
    rospy.sleep(4.0)
    
    set_led_color(0, 0, 0)

except rospy.ROSInterruptException:
    pass