#! /usr/bin/env python
import rospy
import time
import actionlib
from ardrone_as.msg import ArdroneAction, ArdroneGoal, ArdroneResult, ArdroneFeedback
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty

# We create some constants with the corresponding values from the SimpleGoalState class
PENDING = 0
ACTIVE = 1
DONE = 2
WARN = 3
ERROR = 4

nImage = 1

# definition of the feedback callback, This will be called when feedback
# is received from the action server
# it just prints a message indicting a new messae has been received
def feedback_callback(feedback):

    global nImage
    print('[Feedback] image n.%d received'%nImage)
    nImage += 1

# initializes the action client node
rospy.init_node('drone_action_client')

action_server_name = '/ardrone_action_server'
client = actionlib.SimpleActionClient(action_server_name,ArdroneAction)
move = rospy.Publisher('/cmd_vel',Twist,queue_size=1) # Create a Publisher to move the drone
move_msg = Twist() # create the message to move the drone
takeoff = rospy.Publisher('/drone/takeoff',Empty,queue_size=1) # create a publisher to takeoff the drone
takeoff_msg = Empty() # Create the message to takeoff the drone
land = rospy.Publisher('/drone/land',Empty,queue_size=1) # create a Publisher to land the drone
land_msg = Empty() # Create the message to land the drone


# waits until the action server is up and running
rospy.loginfo('Waiting for action Server'+action_server_name)
client.wait_for_server()
rospy.loginfo('Action Server Found...'+action_server_name)

# create a goal to send to the action server
goal = ArdroneGoal()
goal.nseconds = 10 # indicates ,take pictures along 10 seconds

client.send_goal(goal,feedback_cb=feedback_callback)

# you can access the SimpleAction Variable "simple_state", that will be 1 if active, and 2 when finished.
# Its a variable, better use a function like get_state
# state = client.simple_state
# state_result will give the FINAL STATE. Will be 1 when Active, and 2 if NO ERROR, 3 If Any Warning, and 3 if ERROR
state_result = client.get_state()

rate = rospy.Rate(1)

rospy.loginfo("state_result:"+str(state_result))

# We takeoff the drone during the first 3 seconds
i=0
while not i==3:
    takeoff.publish(takeoff_msg)
    rospy.loginfo('Taking off...')
    time.sleep(1)
    i += 1

# We move the drone in a circle while the state of the Action is not DONE yet
while state_result < DONE:
    move_msg.linear.x  = 1
    move_msg.angular.z = 1
    move.publish(move_msg)
    rate.sleep()
    state_result = client.get_state()
    rospy.loginfo('Moving around...')
    rospy.loginfo("state_result:"+str(state_result))

rospy.loginfo("[Result] State:"+str(state_result))
if state_result == ERROR:
    rospy.logerr("something went wrong in the Server Side")
if state_result == WARN:
    rospy.logwarn("There is a warning in the Server Side")

# We land the drone when the action is finished
i=0
while not i==3:
    move_msg.linear.x = 0
    move_msg.angular.z = 0
    move.publish(move_msg)
    land.publish(land_msg)
    rospy.loginfo('Landing...')
    time.sleep(1)
    i += 1
    