#!/usr/bin/env python3

# Miguel Riem Oliveira, Tiago Tavares, April 2019
import numpy as np
import signal
import sys
import rospy
import actionlib
from actionlib_msgs.msg import GoalStatusArray
from sensor_msgs.msg import JointState
from moveit_msgs.msg import MoveGroupAction, MoveGroupActionGoal, MoveGroupActionFeedback, MoveGroupActionResult, GenericTrajectory
from moveit_msgs.msg import MoveGroupGoal
from moveit_msgs.msg import Constraints, JointConstraint
from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
import time


max_vel_wrist_1 = rospy.get_param("/robot_description_planning/joint_limits/wrist_1_joint/max_velocity")
max_vel_wrist_2 = rospy.get_param("/robot_description_planning/joint_limits/wrist_2_joint/max_velocity")
max_vel_wrist_3 = rospy.get_param("/robot_description_planning/joint_limits/wrist_3_joint/max_velocity")
max_vel_shoulder_pan = rospy.get_param("/robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity")
max_vel_shoulder_lift = rospy.get_param("/robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity")
max_vel_elbow = rospy.get_param("/robot_description_planning/joint_limits/elbow_joint/max_velocity")

max_vel = np.array([max_vel_elbow, max_vel_shoulder_lift, max_vel_shoulder_pan, max_vel_wrist_1, max_vel_wrist_2, max_vel_wrist_3])

max_acel_wrist_1 = rospy.get_param("/robot_description_planning/joint_limits/wrist_1_joint/max_acceleration")
max_acel_wrist_2 = rospy.get_param("/robot_description_planning/joint_limits/wrist_2_joint/max_acceleration")
max_acel_wrist_3 = rospy.get_param("/robot_description_planning/joint_limits/wrist_3_joint/max_acceleration")
max_acel_shoulder_pan = rospy.get_param("/robot_description_planning/joint_limits/shoulder_pan_joint/max_acceleration")
max_acel_shoulder_lift = rospy.get_param("/robot_description_planning/joint_limits/shoulder_lift_joint/max_acceleration")
max_acel_elbow = rospy.get_param("/robot_description_planning/joint_limits/elbow_joint/max_acceleration")

joint_points={
"recovery_block": [1.6980674902545374, -1.1418641370585938, 0.512566089630127, -2.106940885583395, -1.6149757544146937, 0.4761171340942383],
"recovery_block_app": [1.5897014776812952, -1.2063172620585938, 0.505854606628418, -1.9114166698851527, -1.5863073507892054, 0.491818904876709],
"recovery_ball_app": [1.5897014776812952, -1.2063172620585938, 0.505854606628418, -1.9114166698851527, -1.5863073507892054, 0.491818904876709],
"recovery_ball": [1.7983577887164515, -1.0831287664226075, 0.4971494674682617, -2.276367326776022, -1.578881565724508, 0.507758617401123],
"ball_pose_1": [2.3731773535357874, -1.5686639112285157, 0.9675478935241699, -2.3909126720824183, -1.5559447447406214, -0.6098969618426722],
"ball_pose_1_app": [2.1328991095172327, -1.8290783367552699, 0.9399642944335938, -1.894506116906637, -1.5411651770221155, -0.6072114149676722],
"ball_pose_2": [2.403114144002096, -1.5845543346800746, -1.6462433973895472, -2.3943029842772425, -1.5905888716327112, -0.11481124559511358],
"ball_pose_2_app": [2.223088089619772, -1.7530580959715785, -1.6689708868609827, -1.9888812504210414, -1.5642464796649378, -0.2158735434161585],
"block_pose_2": [2.1196497122394007, -1.5631750387004395, -1.2423871199237269, -2.093879362145895, -1.5932601133929651, 0.350522518157959],
"block_pose_2_app": [2.0911291281329554, -1.6656304798521937, -1.242410961781637, -2.0355125866331996, -1.619108025227682, 0.35048627853393555],
"block_pose_1": [2.1818140188800257, -1.6157127819456996, 0.6806135177612305, -2.1608315906920375, -1.543720547352926, -0.931004826222555],
"block_pose_1_app": [2.0072038809405726, -1.697963377038473, 0.6907658576965332, -1.8980370960631312, -1.5576003233539026, -0.8996766249286097],
"passage_safe": [2.167422119771139, -2.218821188012594, -0.025754753743306935, -1.5514825147441407, -1.5466473738299769, 0.018237590789794922],
"passage_fast": [1.7013595739947718, -1.5885659656920375, -0.06869632402528936, -1.802068372766012, -1.5800936857806605, -0.006454292927877248],
"interact": [1.1278675238238733, -0.9531238240054627, -0.08442861238588506, -2.4318977795042933, -1.5508225599872034, 0.026221752166748047]}

global joint_states 

def feedback_callback(feedback):

    # if feedback.status.status == 3:
    print("Received action feedback: {}".format(feedback))

def joint_states_callback(msg):
    global joint_states
    joint_states = msg.position


def send_goal_joint_states(client, point_name):
    # print("Moving to " + point_name)
    # joint_values = joint_points[point_name]
    # print(joint_values)
    goal = MoveGroupGoal()
    constraints = Constraints()
    goal_contraints = []
    waypoints = JointTrajectory()
    reference_traj = GenericTrajectory()

    # joint_names = ["shoulder_pan_joint",
    #                "shoulder_lift_joint",
    #                "elbow_joint",
    #                "wrist_1_joint",
    #                "wrist_2_joint",
    #                "wrist_3_joint"]

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

    joint_traj_points = [joint_points["passage_fast"], joint_points["ball_pose_1_app"]]
    joint_values = joint_points["ball_pose_1_app"]
    joint_contraint_values = joint_points["passage_fast"]

    # for joint_values in waypoints:
    #     print(joint_values)
    for joint_name, joint_value in zip(joint_names, joint_values):
        constraint = JointConstraint()
        constraint.joint_name = joint_name
        constraint.position = joint_value
        constraint.tolerance_above = 0.0001
        constraint.tolerance_below = 0.0001
        constraint.weight = 1.0
        constraints.joint_constraints.append(constraint)
    
    # goal_contraints.append(constraints)
    waypoints.joint_names = joint_names
    for joint_point in joint_traj_points:
        joint_trajectory_point = JointTrajectoryPoint()
        joint_trajectory_point.positions = joint_point
        waypoints.points.append(joint_trajectory_point)

    reference_traj.joint_trajectory.append(waypoints)

    goal.request.goal_constraints = [constraints]
    goal.request.group_name = 'manipulator'
    goal.request.num_planning_attempts = 10
    goal.request.allowed_planning_time = 10
    goal.request.max_velocity_scaling_factor = 0.3
    goal.request.max_acceleration_scaling_factor = 0.3
    goal.request.workspace_parameters.header.stamp = rospy.Time.now()
    goal.request.workspace_parameters.header.frame_id = 'world'
    goal.request.reference_trajectories = [reference_traj]

    # Sends the goal to the action server.
    client.send_goal(goal)
    
    current_status = client.get_state()
    # print("Current action status: {}".format(current_status))

def send_trajectory_goal_joint_states(client, points, vel = 1, acel = 1, vel_waypoint = 0.2):
    # print("Moving to " + point_name)

    # print(joint_values)
    # goal = MoveGroupGoal()
    # constraints = Constraints()
    goal = FollowJointTrajectoryGoal()
    print(joint_states)
    initial_point = joint_states
    
    points.insert(0, initial_point)
    # joint_names = ["shoulder_pan_joint",
    
    #                "shoulder_lift_joint",
    #                "elbow_joint",
    #                "wrist_1_joint",
    #                "wrist_2_joint",
    #                "wrist_3_joint"]

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

    goal.trajectory.joint_names = joint_names
    count = 0.1
    for i, point in enumerate(points):
        point1 = np.array(point)
        point2 = np.array(points[i-1])
        delta_theta = point[2] - points[i-1][2]


        if i>0 and i<len(points) - 1:
            # point1 = np.array(point)
            # point2 = np.array(points[i-1])
            point3 = np.array(points[i+1])

            # delta_theta = point[2] - points[i-1][2]
            # print("---------------------------------------")
            # print(np.max(np.abs(point2 - point1)/ max_vel))
            # print(delta_theta)
            time_span = abs(2 * np.max(np.abs(point2 - point1)/ (max_vel * vel)))

            delta_theta_objective = point3 - point1
            vel_mid = delta_theta_objective / time_span
            vel_middle = (vel_mid / max(abs(vel_mid))) * vel_waypoint
            # print(f"delta_theta_objective: {delta_theta_objective}")
            # print(f"point3: {point3}")
            # print(f"point1: {point1}")
            # print(f"vel_mid: {vel_mid}")
        elif i == len(points) - 1:
            # point1 = np.array(point) 
            # point2 = np.array(points[i-1])
            # delta_theta = point[2] - points[i-1][2]
            time_span = abs(2 * np.max(np.abs(point2 - point1)/ (max_vel * vel)))
            vel_middle = [0] * 6
        else:
            time_span = 0.1
            vel_middle = [0] * 6

        point_msg = JointTrajectoryPoint()
        point_msg.velocities = vel_middle
        # for name in joint_names:
        #     tolerance = JointTolerance()
            
        #     goal.path_tolerance.append(tolerance)
        #     goal.goal_tolerance.append(tolerance)
        # point_msg.accelerations = [0] * 6
        # point_msg.velocities = [0] * 6


        # if i==0 or i == 1 or i == len(points) - 1:
        #     point_msg.accelerations = [0] * 6
        #     point_msg.velocities = [0] * 6
        # else:

        #     point_msg.velocities = vel_middle
        
        point_msg.positions = point
        count += time_span
        print(count)
        print(time_span)
        point_msg.time_from_start = rospy.Time(count)
        goal.trajectory.points.append(point_msg)
        
    goal.goal_time_tolerance = rospy.Time(count)
    print(goal)
    goal.trajectory.header.stamp = rospy.Time.now()
    goal.trajectory.header.frame_id = 'world'
    client.send_goal(goal)

    client.wait_for_result()
    print(joint_states)


    # for joint_name, joint_value in zip(joint_names, joint_values):
    #     constraint = JointConstraint()
    #     constraint.joint_name = joint_name
    #     constraint.position = joint_value
    #     constraint.tolerance_above = 0.0001
    #     constraint.tolerance_below = 0.0001
    #     constraint.weight = 1.0
    #     constraints.joint_constraints.append(constraint)

    # goal.request.goal_constraints = [constraints]
    # goal.request.group_name = 'manipulator'
    # goal.request.num_planning_attempts = 10
    # goal.request.allowed_planning_time = 10
    # goal.request.max_velocity_scaling_factor = 0.3
    # goal.request.max_acceleration_scaling_factor = 0.3
    # goal.request.workspace_parameters.header.stamp = rospy.Time.now()
    # goal.request.workspace_parameters.header.frame_id = 'world'

    # # Sends the goal to the action server.
    # client.send_goal(goal)
    
    # current_status = client.get_state()
    # # print("Current action status: {}".format(current_status))

def MyActionClient(cancel):


    # calculate_t_final(0, 0, 3.14, 0, 10)
    client = actionlib.SimpleActionClient('/move_group', MoveGroupAction)
    client_traj = actionlib.SimpleActionClient('/scaled_pos_joint_traj_controller/follow_joint_trajectory', 
                                               FollowJointTrajectoryAction)

    rospy.Subscriber('/move_group/feedback', MoveGroupActionFeedback, feedback_callback)
    rospy.Subscriber('/joint_states', JointState, joint_states_callback)

    from functools import partial

    def exitProgram(sig, frame, client):
        print('you pressed ctrl+c')
        client.cancel_goal()
        # client.cancel_all_goals()
        # result = MoveGroupActionResult()
        # result = "Action Canceled"
        rospy.signal_shutdown("you pressed ctrl+c")
        # return result

    # signal.signal(signal.SIGINT, partial(exitProgram, client=client))

    # Waits until the action server has started up and started
    # listening for goals.
    # client.wait_for_server()
    print("here 1")
    client_traj.wait_for_server()
    print("here 2")
    # trajectory_goal = FollowJointTrajectoryGoal()

    # Creates a goal to send to the action server.
    # goal = MoveGroupGoal()

    points = [joint_points["passage_safe"]]
    send_trajectory_goal_joint_states(client_traj, points, vel = 0.5, vel_waypoint = 0.5)
    client_traj.wait_for_result()
    

    points = [joint_points["passage_fast"] ,joint_points["ball_pose_2_app"]]
    send_trajectory_goal_joint_states(client_traj, points, vel=1, vel_waypoint=1)
    client_traj.wait_for_result()

    points = [joint_points["passage_fast"] ,joint_points["interact"]]
    send_trajectory_goal_joint_states(client_traj, points, vel = 0.5, vel_waypoint = 0.5)
    client_traj.wait_for_result()
    
    
    #TODO fill in the goal message
    # goal.goal_id = 0
    # goal.header.stamp = rospy.Time.now()

    # send_goal_joint_states(client=client, point_name= "ball_pose_1_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, point_name= "ball_pose_1")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name= "ball_pose_1_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="block_pose_1_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="block_pose_1")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="block_pose_1_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal,  point_name="passage_safe")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, point_name="ball_pose_2_app")
    # client.wait_for_result()
    # time.sleep(1)
    # cancel_goal = FollowJointTrajectoryGoal()    
    # client.send_goal(cancel_goal)
    # client.wait_for_result()

    # send_goal_joint_states(client=client, point_name="recovery_block_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="ball_pose_2")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="ball_pose_2_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="block_pose_2_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="block_pose_2")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="block_pose_2_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="passage_fast")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="interact")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="passage_fast")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="recovery_block_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="recovery_block")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="recovery_block_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="recovery_ball_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="recovery_ball")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="recovery_ball_app")
    # client.wait_for_result()
    # send_goal_joint_states(client=client, goal=goal, point_name="passage_fast")
    # client.wait_for_result()



    # # Waits for the server to finish performing the action.
    # if cancel != 1:
    #     client.wait_for_result()
    #     # Prints out the result of executing the action
    #     return client.get_result()  # Result
    # else:
    #     time.sleep(3)
    #     client.cancel_goal()
    #     #client.cancel_all_goals()
    #     result = MoveGroupActionResult()
    #     result= "Action Canceled"
    #     return result


if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.

        rospy.init_node('test_action_client')
        cancel=0 # 1 to cancel goal with 5 delay secs
        result = MyActionClient(cancel)
        print ("Result:" + str(result) )
    except rospy.ROSInterruptException:
        print("program interrupted before completion")