#!/usr/bin/env python

import roslib; roslib.load_manifest('c3_trajectory_generator')
import rospy

import actionlib
import tf
from tf import transformations
from std_msgs.msg import Header
from visualization_msgs.msg import InteractiveMarker, InteractiveMarkerControl, MenuEntry
from interactive_markers.interactive_marker_server import InteractiveMarkerServer
from geometry_msgs.msg import Quaternion, Twist, Pose, Point
from uf_common.msg import MoveToAction, MoveToGoal, PoseTwist
from kill_handling.listener import KillListener

CENTER_TO_VEHICLE = 1
SET_DEPTH = 9
SET_DEPTH_VAL = 10

depth_vals = [0.3, 0.5, 1, 1.5, 2, 3, 4]

class Node(object):
    def __init__(self):
        self._tf_listener = tf.TransformListener()
        self._server = InteractiveMarkerServer("moveto_marker")
        self._move_client = actionlib.SimpleActionClient('moveto', MoveToAction)
        self._move_client.wait_for_server()
        self._kill_listener = KillListener(self.on_killed)
        self._marker_pose = None
        self._marker_pose_time = None
        self._marker_pose_sent = False
        self.setup_markers()

    def setup_markers(self):
        while not rospy.is_shutdown():
            try:
                self._tf_listener.waitForTransform('map', 'base_link',
                                                   rospy.Time(0), rospy.Duration(1))
                trans, rot = self._tf_listener.lookupTransform('map', 'base_link',
                                                               rospy.Time(0))
                break
            except tf.Exception as ex:
                print ex
                continue
        self._marker_pose = Pose(position=Point(*trans),
                               orientation=Quaternion(*rot))

        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "/map"
        int_marker.name = "moveto"
        int_marker.description = "moveto"
        int_marker.pose = self._marker_pose

        for axis, dir in [('x', (1, 0, 0)),
                          ('y', (0, 1, 0)),
                          ('z', (0, 0, 1))]:
            control = InteractiveMarkerControl()
            control.name = axis
            control.orientation = Quaternion(*(dir + (1,))) # w must be 1?
            control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
            int_marker.controls.append(control);

        yaw_control = InteractiveMarkerControl()
        yaw_control.name = "yaw"
        yaw_control.orientation = Quaternion(0, 1, 0, 1)
        yaw_control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(yaw_control);

        reset_menu = MenuEntry()
        reset_menu.id = CENTER_TO_VEHICLE
        reset_menu.title = 'Center to vehicle'
        int_marker.menu_entries.append(reset_menu)

        depth_menu = MenuEntry()
        depth_menu.id = SET_DEPTH
        depth_menu.title = 'Set depth'
        int_marker.menu_entries.append(depth_menu)

        for i, depth in enumerate(depth_vals):
            menu = MenuEntry()
            menu.id = SET_DEPTH_VAL + i
            menu.parent_id = SET_DEPTH
            menu.title = '%.1f m' % depth
            int_marker.menu_entries.append(menu)

        self._server.insert(int_marker, self.on_feedback)
        self._server.applyChanges()

    def on_feedback(self, feedback):
        applyChanges = True

        if feedback.menu_entry_id == CENTER_TO_VEHICLE:
            trans, rot = self._tf_listener.lookupTransform('map', 'base_link',
                                                           rospy.Time(0))
            yaw = transformations.euler_from_quaternion(rot)[2]
            self._marker_pose = Pose(position=Point(*trans),
                                     orientation=Quaternion(*transformations.quaternion_from_euler(0, 0, yaw)))
        elif feedback.menu_entry_id >= SET_DEPTH_VAL:
            self._marker_pose.position.z = -depth_vals[feedback.menu_entry_id - SET_DEPTH_VAL]
        else:
            self._marker_pose = feedback.pose
            applyChanges = False # Don't need to apply feedback back to rviz

        if not self._kill_listener.get_killed():
            self._marker_pose_time = rospy.Time.now()
            self._marker_pose_sent = False
        if applyChanges:
            self._server.setPose('moveto', self._marker_pose)
            self._server.applyChanges()

    def run(self):
        while not rospy.is_shutdown():
            rospy.sleep(.2)
            if self._marker_pose_time is None:
                continue
            if self._marker_pose_sent:
                continue
            if rospy.Time.now() - self._marker_pose_time > rospy.Duration(.5):
                self.send_goal()

    def on_killed(self):
        self._move_client.cancel_goal()

    def send_goal(self):
        self._move_client.send_goal(MoveToGoal(
                header=Header(frame_id='/map'),
                posetwist=PoseTwist(
                    pose=self._marker_pose)))
        self._marker_pose_sent = True

if __name__ == "__main__":
    rospy.init_node("interactive_marker_moveto")
    node = Node()
    node.run()

