---

#!/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.SetLEDEffect)

def fly_to_coordinates(x, y, z=1.0, speed=0.5):
    navigate(x=x, y=y, z=z, speed=speed, frame_id='navigate_target', auto_arm=True)
    rospy.sleep(6.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)
    navigate(x=0.0, y=0.0, z=1.0, speed=0.5, frame_id='body', auto_arm=True)
    rospy.sleep(5.0)

    fly_to_coordinates(x=2.0, y=0.0)
    fly_to_coordinates(x=3.0, y=0.0)
    set_led_color(128, 0, 128)
    rospy.sleep(1.0)

    fly_to_coordinates(x=3.0, y=-3.0)
    fly_to_coordinates(x=4.0, y=-3.0)
    set_led_color(0, 191, 255)
    
    fly_to_coordinates(x=6.0, y=-3.0)
    set_led_color(128, 0, 128)
    
    fly_to_coordinates(x=6.0, y=-2.0)
    set_led_color(255, 255, 255)

    fly_to_coordinates(x=5.0, y=-2.0)
    fly_to_coordinates(x=4.0, y=-3.0)
    set_led_color(255, 0, 0)

    fly_to_coordinates(x=2.0, y=-2.0)
    fly_to_coordinates(x=2.0, y=-3.0)
    set_led_color(0, 255, 0)

    fly_to_coordinates(x=0.0, y=0.0)
    fly_to_coordinates(x=1.0, y=0.0)
    set_led_color(255, 255, 0)
    
    fly_to_coordinates(x=3.0, y=0.0)
    set_led_color(0, 0, 255)
    
    fly_to_coordinates(x=3.0, y=-1.0)
    set_led_color(128, 0, 128)

    fly_to_coordinates(x=0.0, y=0.0)
    rospy.sleep(1.0)
    
    land()
    rospy.sleep(4.0)
    
    set_led_color(0, 0, 0)

except rospy.ROSInterruptException:
    pass