#   conda activate coppeliasim
#   env fix, in terminal:  fix_robotics_env.sh

from rm75_kinematics import rm75_kinematics

from math import pi
import numpy as np

#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
tools_in_ee = {
    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
}

# joint limit
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
lb = -ub

tool_name = "scissor"

def main():
    """Demonstrate pure position control"""

    # Create controller

    robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',mesh_dir='./urdf_rm75',
                                 tcps=["scissor_tcp", "camera_tcp"],tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)

    ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
                                                 initial_guess=[0.1] * 7, tool=tool_name)

    self_collision_sts = robot_kine.get_self_collision(q)

    p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)

    print(f'self_collision_sts: {self_collision_sts}')

    import numpy as np

    ik_suc = 0

    for i in range(100):
        joint_rand = np.random.uniform(ub, lb)

        p_t = robot_kine.get_fk_result(joint_angles=joint_rand.tolist(), tool=tool_name)

        joint_rand_init = np.random.uniform(ub, lb)

        ret_ik, q = robot_kine.get_ik_result(target_position=p_t[0:3], target_rpy=p_t[3:6],
                                             initial_guess=joint_rand_init, tool=tool_name)




        p_fk = robot_kine.get_fk_result(joint_angles=q, tool=tool_name)

        d_p_ik = cal_pose_deviation(pose1=p_t, pose2=p_fk)

        coll_sts = robot_kine.get_self_collision(joint_angles=q)
        if ret_ik == True:

            print(f'\n---- success, in the ik, j_t = {joint_rand}, q = {q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')

            ik_suc += 1
        else:
            print(f'\n**** ik failed, in the ik, j_t = {joint_rand}, q={q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')


    print(f'ik_suc: {ik_suc}')


def cal_pose_deviation(pose1, pose2):
    d_fk_p1 = np.array(pose1) - np.array(pose2)
    for j in [3, 4, 5]:
        while d_fk_p1[j] > pi:
            d_fk_p1[j] -= 2 * pi
        while d_fk_p1[j] < -pi:
            d_fk_p1[j] += 2 * pi
    d_fk = np.linalg.norm(d_fk_p1)
    return d_fk



if __name__ == "__main__":
    main()