---

#!/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_grid(x, y, z=1.0, speed=0.4, delay=7.0):
    navigate(x=x, y=y, z=z, speed=speed, frame_id='navigate_target', auto_arm=True)
    rospy.sleep(delay)

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

try:
    set_led_color(255, 255, 255)
    navigate(x=0.0, y=0.0, z=1.0, speed=0.4, frame_id='body', auto_arm=True)
    rospy.sleep(6.0)

    fly_grid(x=2.0, y=0.0, delay=6.0)
    fly_grid(x=2.0, y=-1.0, delay=5.0)
    set_led_color(128, 0, 128)

    fly_grid(x=1.0, y=-5.0, delay=8.0)
    
    fly_grid(x=2.5, y=-5.0, delay=6.0)
    set_led_color(0, 191, 255)
    
    fly_grid(x=4.5, y=-5.0, delay=6.0)
    set_led_color(128, 0, 128)
    
    fly_grid(x=6.0, y=-5.0, delay=6.0)
    
    fly_grid(x=6.0, y=-4.0, delay=5.0)
    set_led_color(255, 255, 255)

    fly_grid(x=5.0, y=-4.0, delay=5.0)
    fly_grid(x=4.0, y=-4.0, delay=5.0)
    set_led_color(255, 0, 0)

    fly_grid(x=3.0, y=-4.0, delay=5.0)
    fly_grid(x=3.0, y=-3.0, delay=5.0)
    fly_body_target(x=2.0, y=-3.0, delay=5.0)
    set_led_color(0, 255, 0)

    fly_grid(x=1.0, y=-1.0, delay=7.0)
    
    fly_grid(x=1.0, y=-2.5, delay=6.0)
    set_led_color(255, 255, 0)
    
    fly_grid(x=1.0, y=-4.5, delay=6.0)
    set_led_color(0, 0, 255)
    
    fly_grid(x=1.0, y=-6.0, delay=6.0)
    
    fly_grid(x=2.0, y=-6.0, delay=5.0)
    fly_grid(x=2.0, y=-3.0, delay=6.0)
    set_led_color(128, 0, 128)

    fly_grid(x=0.0, y=0.0, delay=7.0)

    land()
    rospy.sleep(5.0)
    set_led_color(0, 0, 0)

except rospy.ROSInterruptException:
    pass