#!/usr/bin/env python
import math
import rospy

import actionlib
from cras_relative_positional_controller.msg import RelativeMoveFeedback, RelativeMoveResult, RelativeMoveAction, \
    RelativeMoveGoal
from geometry_msgs.msg import Twist


class PositionController:
    _feedback = RelativeMoveFeedback()
    _result = RelativeMoveResult()

    def __init__(self, name):
        self._action_name = name
        self._pub = rospy.Publisher("~cmd_vel", Twist, queue_size=10)
        self._max_lin_vel = rospy.get_param("~max_lin_vel", 1.0)
        self._max_ang_vel = rospy.get_param("~max_ang_vel", math.pi/2)
        self._max_age = rospy.get_param("~max_age", 3.0)
        self._as = actionlib.SimpleActionServer(self._action_name, RelativeMoveAction, execute_cb=self.execute_cb,
                                                auto_start=False)
        self._as.start()

    def execute_cb(self, goal):
        assert isinstance(goal, RelativeMoveGoal)

        age = rospy.Time.now() - goal.header.stamp
        if age.to_sec() > self._max_age:
            rospy.logwarn("Too old positional command received, ignoring (age " + str(age) + ")")
            return

        # helper variables
        r = rospy.Rate(10)
        dur = r.sleep_dur
        success = True

        remaining = [goal.target_x_change, goal.target_yaw_change]

        # if the target is zero, we do nothing
        if goal.target_x_change == 0:
            goal.linear_speed = 0

        if goal.target_yaw_change == 0:
            goal.angular_speed = 0

        vel_x = abs(goal.linear_speed)
        if vel_x > self._max_lin_vel:
            vel_x = self._max_lin_vel
        if goal.target_x_change < 0:
            vel_x = -vel_x

        vel_yaw = abs(goal.angular_speed)
        if vel_yaw > self._max_ang_vel:
            vel_yaw = self._max_ang_vel
        if goal.target_yaw_change < 0:
            vel_yaw = -vel_yaw

        cmd = Twist()
        cmd.linear.x = vel_x
        cmd.linear.y = cmd.linear.z = 0
        cmd.angular.x = cmd.angular.y = 0
        cmd.angular.z = vel_yaw

        motion_length_lin = vel_x * dur.to_sec()
        motion_length_ang = vel_yaw * dur.to_sec()
        i = 0
        while not rospy.is_shutdown() and \
                (abs(remaining[0]) > abs(motion_length_lin) or abs(remaining[1]) > abs(motion_length_ang)):

            if self._as.is_preempt_requested():
                rospy.loginfo('%s: Preempted' % self._action_name)
                self._as.set_preempted()
                success = False
                break

            if abs(remaining[0]) < abs(motion_length_lin):
                cmd.linear.x = 0
            if abs(remaining[1]) < abs(motion_length_ang):
                cmd.angular.z = 0

            self._pub.publish(cmd)

            r.sleep()

            remaining[0] -= motion_length_lin
            remaining[1] -= motion_length_ang
            self._feedback.remaining_x = remaining[0]
            self._feedback.remaining_yaw = remaining[1]
            self._feedback.header.stamp = rospy.Time.now()
            # publish the feedback
            self._as.publish_feedback(self._feedback)

            i += 1

        cmd.linear.x = 0
        cmd.angular.z = 0
        self._pub.publish(cmd)

        if success:
            self._result.remaining_x = self._feedback.remaining_x
            self._result.remaining_yaw = self._feedback.remaining_yaw
            self._result.header.stamp = rospy.Time.now()
            rospy.loginfo('%s: Succeeded' % self._action_name)
            self._as.set_succeeded(self._result)


if __name__ == '__main__':
    rospy.init_node('position_controller')
    server = PositionController(rospy.get_name())
    rospy.spin()
