import rospy, sys
import numpy as np
from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply

# def traj_validity_check(trajectory):
#     if type (trajectory)is moveit_msgs.msg._RobotTrajectory.RobotTrajectory:
#         point_num=len(trajectory.joint_trajectory.point)
#         trajectory.joint_trajectory.point.positions
        
#         # for i
#         trajectory.joint_trajectory.point.velocities

#         trajectory.joint_trajectory.point.accelerations
        
#         return 0
#     # else:
#     #         raise MoveItCommanderException(
#     #             "Expected value in the range from 0 to 1 for scaling factor"
#     #         )
#     def scale_trajectory_speed(traj, scale):
#        # Create a new trajectory object
#        new_traj = RobotTrajectory()
       
#        # Initialize the new trajectory to be the same as the input trajectory
#        new_traj.joint_trajectory = traj.joint_trajectory
       
#        # Get the number of joints involved
#        n_joints = len(traj.joint_trajectory.joint_names)
       
#        # Get the number of points on the trajectory
#        n_points = len(traj.joint_trajectory.points)
        
#        # Store the trajectory points
#        points = list(traj.joint_trajectory.points)
       
#        # Cycle through all points and joints and scale the time from start,
#        # as well as joint speed and acceleration
#        for i in range(n_points):
#            point = JointTrajectoryPoint()
           
#            # The joint positions are not scaled so pull them out first
#            point.positions = list(traj.joint_trajectory.points[i].positions)

#            # Next, scale the time_from_start for this point
#             # point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
#            # Get the joint velocities for this point
#            point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
#            # Get the joint accelerations for this point
#            point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
#            # Scale the velocity and acceleration for each joint at this point
#            for j in range(n_joints):
#             #    if point.positions[j]
#                if has_velocity_limits:
#                 if point.velocities[j] > joint_max_velocity[j]:
#                     vel_exception_point
#                     rospy.loginfo("velocities Test OK") 
#                 else:
#                     raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor")     
#                if has_acceleration_limits:
#                 point.accelerations[j] = point.accelerations[j] * scale * scale
        
#            # Store the scaled trajectory point
#            points[i] = point

#        rospy.loginfo("velocities Check OK") 

#        # Assign the modified points to the new trajectory
#        new_traj.joint_trajectory.points = points

#        # Return the new trajecotry
#        return new_traj

#     else:
#         print("traj type is not std")
    
def check_joint_positions(joint_trajectory):
    is_limited = False
    Limit_margin = True
    percentage = 0
    for i in range(len(joint_trajectory.joint_trajectory.points) - 1):
        if  not is_limited:
            joint_positions1 = joint_trajectory.joint_trajectory.points[i].positions
            joint_positions2 = joint_trajectory.joint_trajectory.points[i + 1].positions 

            for j in range(len(joint_positions1)):
                position_diff = abs(joint_positions1[j] - joint_positions2[j])
                if position_diff > 6:
                    rospy.loginfo("point{}-joint{}:{}".format(i,j+1,joint_positions1))
                    new_start_point = list(joint_trajectory.joint_trajectory.points[0].positions)
                    
                    #关节限位值
                    joint_limt =[[],[],[],[-3.314,3.314],[],[-3.838,3.838]] 
                    #检查关节限位余量
                    # midpoint = (joint_limt[j][0] + joint_limt[j][1])/2
                    # distance_to_midpoint = abs(joint_trajectory.joint_trajectory.points[0].positions[j]- midpoint)
                    Limit_point = joint_positions1[j]
                    distance_to_Limit_point = abs(joint_trajectory.joint_trajectory.points[0].positions[j]- Limit_point)
                    Joint_range = abs(joint_limt[j][0] - joint_limt[j][1])
                    margin = distance_to_Limit_point / Joint_range
                    if margin > 0.4:
                        Limit_margin = True
                    else:
                        Limit_margin = False
                    rospy.loginfo("margin = {}".format(margin))
                    # if joint_positions1[j]>0:
                    #     new_start_point[j] -= np.pi*2
                    # else:
                    #     new_start_point[j] += np.pi*2
                    # rospy.loginfo("new point{}:{}".format(i,new_start_point))
                    percentage = i/len(joint_trajectory.joint_trajectory.points)
                    is_limited = True
                    break
                if position_diff > 3:
                    rospy.loginfo("point{}-joint{}:{}".format(i,j+1,joint_positions1))
                    
                    
                    
                    Limit_margin = True
                    
                    
                    is_limited = True
                    break
        if  is_limited:
            break
    if not is_limited:
        rospy.loginfo("joint positions check OK")         
    return is_limited,Limit_margin

def angle_adjustment(pose,axis,angle):
    if len(pose) == 6:
        q_initial = quaternion_from_euler(pose[3], pose[4], pose[5])
    else:
        q_initial = (pose[3], pose[4], pose[5],pose[6])
    if axis == 'z':
        q_rotation = quaternion_from_euler(0, 0, angle)
    if axis == 'y':
        q_rotation = quaternion_from_euler(0, angle, 0)
    if axis == 'x':
        q_rotation = quaternion_from_euler(angle, 0, 0)    
    q_new = quaternion_multiply(q_initial, q_rotation)
    pose[3:] = q_new
    return pose