#/usr/bin/env python

import rospy
import actionlib

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped

from actionlib_msgs.msg import GoalStatus       as g_state
from pyrois.RoIS_Common import Component_Status as c_state

def pose_to_goal(pose):
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'
    goal.target_pose.pose.position.x = pose[0][0]
    goal.target_pose.pose.position.y = pose[0][1]
    goal.target_pose.pose.position.z = pose[0][2]
    goal.target_pose.pose.orientation.x = pose[1][0]
    goal.target_pose.pose.orientation.y = pose[1][1]
    goal.target_pose.pose.orientation.z = pose[1][2]
    goal.target_pose.pose.orientation.w = pose[1][3]

    return goal


class GoalSenderROS(object):
    # Table for conversion from GoalStatus to Component_Status
    # http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html
    GoalSenderState = {
        g_state.PENDING:    c_state.READY,
        g_state.ACTIVE:     c_state.BUSY,
        g_state.PREEMPTED:  c_state.READY,
        g_state.SUCCEEDED:  c_state.READY,
        g_state.ABORTED:    c_state.ERROR,
        g_state.PREEMPTING: c_state.BUSY,
        g_state.RECALLING:  c_state.BUSY,
        g_state.RECALLED:   c_state.READY,
        g_state.LOST:       c_state.READY
    }

    def __init__(self, robot_name):
        self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction)
        self.action_client.wait_for_server()

    def send_goal(self, pose_xy):
        pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)]
        goal = pose_to_goal(pose)
        self.action_client.send_goal(goal)

    def send_goal_and_wait(self, pose_xy):
        pose = [pose_xy, (0.0, 0.0, 0.0, 1.0)]
        goal = pose_to_goal(pose)
        state = self.action_client.send_goal_and_wait(goal)
        return GoalSenderROS.GoalSenderState[state]

    def get_goalsender_state(self):
        state = self.action_client.get_state()
        return GoalSenderROS.GoalSenderState[state]

#  If you remove this comment out, it does not work.
#  rospy.init_node('goal_sender')
