#!/usr/bin/env python
from xml.etree.ElementTree import PI

from soupsieve import select
import rospy

import numpy as np
import tf2_ros
import tf_conversions
import ros_numpy
import geometry_msgs.msg
import actionlib

from std_srvs.srv import Trigger, SetBool
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint
from controller_manager_msgs.srv import SwitchControllerRequest, SwitchController
from controller_manager_msgs.srv import LoadControllerRequest, LoadController

JOINT_NAMES = [
    "shoulder_pan_joint",
    "shoulder_lift_joint",
    "elbow_joint",
    "wrist_1_joint",
    "wrist_2_joint",
    "wrist_3_joint",
]

class MoveAway:
    def __init__(self) -> None:
        # switch controller
        rospy.init_node("move_away")
        rospy.Service("/move_away", SetBool, self.move_away_callback)
        # self.joint_controller = "scaled_pos_joint_traj_controller"
        self.joint_controller = "forward_joint_traj_controller"
        self.cartesian_controller = "pose_based_cartesian_traj_controller"
        # self.cartesian_controller = "forward_cartesian_traj_controller"

        self.trajectory_client = actionlib.SimpleActionClient(
                    self.joint_controller + "/follow_joint_trajectory",
                    FollowJointTrajectoryAction,)

        # load needed controllers
        srv = LoadControllerRequest()
        srv.name = self.joint_controller
        load_srv = rospy.ServiceProxy(
                    "controller_manager/load_controller", LoadController)
        load_srv(srv)

        srv = LoadControllerRequest()
        srv.name = self.cartesian_controller
        load_srv = rospy.ServiceProxy(
                    "controller_manager/load_controller", LoadController)
        load_srv(srv)

        self.joint_positions = [0, -120/180*np.pi, 75/180*np.pi, -60/180*np.pi, 40/180*np.pi, -80/180*np.pi]

    def move_away_callback(self, req):
        if req.data:
            if not self.ask_confirmation():
                return False, "Motion not confirmed by user"
        self.move_away()
        return True, "move away finished"

    def move_away(self):
        switch_srv = rospy.ServiceProxy(
                    "controller_manager/switch_controller", SwitchController)
        switch_srv.wait_for_service(rospy.Duration(5).to_sec())
        srv = SwitchControllerRequest()
        srv.stop_controllers = [self.cartesian_controller]
        srv.start_controllers = [self.joint_controller]
        srv.strictness = SwitchControllerRequest.BEST_EFFORT
        switch_srv(srv)

        # create trajectory

        rospy.sleep(1)
        # Create and fill trajectory goal
        goal = FollowJointTrajectoryGoal()
        goal.trajectory.joint_names = JOINT_NAMES
        point = JointTrajectoryPoint()
        point.positions = self.joint_positions
        point.time_from_start = rospy.Duration(5)
        goal.trajectory.points.append(point)
        goal.goal_time_tolerance = rospy.Duration(2)
        rospy.loginfo("sending goal")
        self.trajectory_client.send_goal_and_wait(goal)

        # switch the controller back
        switch_srv = rospy.ServiceProxy(
                    "controller_manager/switch_controller", SwitchController)
        switch_srv.wait_for_service(rospy.Duration(5).to_sec())
        srv = SwitchControllerRequest()
        srv.stop_controllers = [self.joint_controller]
        srv.start_controllers = [self.cartesian_controller]
        srv.strictness = SwitchControllerRequest.BEST_EFFORT
        switch_srv(srv)

    def ask_confirmation(self):
        rospy.logwarn(f"The robot will move to the joint positions: {self.joint_positions}")
        confirmed = False
        valid = False
        while not valid:
            input_str = input(
                "Please confirm that you want the robot to move \n"
                "Keep the EM-Stop available at all times.\n"
                "Please type 'y' to proceed or 'n' to abort: "
            )
            valid = input_str in ["y", "n"]
            if not valid:
                rospy.loginfo("Please confirm by entering 'y' or abort by entering 'n'")
            else:
                confirmed = input_str == "y"
            return confirmed

if __name__ == "__main__":
    client = MoveAway()
    rospy.spin()