#!/usr/bin/env python

import rospy
import sys

# Brings in the SimpleActionClient
import actionlib

# Brings in the messages used by the fibonacci action, including the
# goal message and the result message.
import bwi_planning_common.msg
import bwi_msgs.msg

def execute_logical_goal():
    # Creates the SimpleActionClient, passing the type of the action
    # (FibonacciAction) to the constructor.
    client = actionlib.SimpleActionClient('execute_logical_goal', bwi_msgs.msg.LogicalNavigationAction)

    # Waits until the action server has started up and started
    # listening for goals.
    client.wait_for_server()

    # Creates a goal to send to the action server.
    atom = bwi_planning_common.msg.PlannerAtom()
    atom.name = sys.argv[1]
    atom.value = sys.argv[2:]
    goal = bwi_msgs.msg.LogicalNavigationGoal(atom)

    # Sends the goal to the action server.
    client.send_goal(goal)

    # Waits for the server to finish performing the action.
    client.wait_for_result()

    # Prints out the result of executing the action
    return client.get_result()  # A FibonacciResult

if __name__ == '__main__':
    if len(sys.argv) < 2:
        print "Need atleast one argument."
    else:
        try:
            # Initializes a rospy node so that the SimpleActionClient can
            # publish and subscribe over ROS.
            rospy.init_node('execute_logical_goal')
            result = execute_logical_goal()
            if result.success:
                print "Success!"
            else:
                print "Failed because " + result.status
            for obs in result.observations:
                print obs.name,
                if len(obs.value) > 0:
                    print "(" + ",".join(obs.value) + ")"
                else:
                    print " "
        except rospy.ROSInterruptException:
            pass
