from casadi import print_operator

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

from rm75_kine_qp import KinematicsSolver as kine_qp
from rm75_kine_rm import rm75_kine_api as kine_rm
from rm75_mjc import MuJoCoPositionController
from Robotic_Arm.rm_robot_interface import *

import time
from math import radians, degrees, pi, cos, sin
import numpy as np

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

    urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"



    # Create controller
    robot_mjk = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
    robot_mjk.start()
    time.sleep(1)

    print("\n[Test 1] Move joint 1 to 45 degrees")
    robot_mjk.send_command([0.785, 0, 0, 0, 0, 0, 0])
    robot_mjk.wait_until_reached()
    robot_mjk.print_state()
    time.sleep(0.5)

    # print("\n[Test 2] Move joint 2 to -30 degrees")
    # robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
    # robot_mjk.wait_until_reached()
    # robot_mjk.print_state()
    # time.sleep(0.5)
    #
    # print("\n[Test 3] Move multiple joints simultaneously")
    # robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
    # robot_mjk.wait_until_reached()
    # robot_mjk.print_state()
    # time.sleep(0.5)

    print("\n[Test 4] Return home\n")
    robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
    robot_mjk.wait_until_reached()
    robot_mjk.print_state()




#---------------------------------------------------------------------------


    joints = [10, 20, -30, -40, 50, 60, 70]
    joints_rad = [radians(j)  for j in joints] #radians(joints)
    # target_position = [0.3, 0.2, 0.4]
    # target_rpy = [0.0, 0.0, 3.14*0.25]
    target_position = [0.17892041, 0.25274317, 0.83107248]
    target_rpy = [0.78576018, 0.67554633, 1.86302226]
    target_p = target_position + target_rpy
    # target_p_rad = [radians(pos) for pos in target_position] + target_rpy
    initial_guess = [11, 20, -30, -40, 50, 60, 71] # [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0] #
    initial_guess_rad = [ radians(j) for j in initial_guess ]
    tool_name = "scissor"


    robot_kine_qp = kine_qp()
    print(f'the forward kinematics result: {robot_kine_qp.forward_kinematics(joints_rad , tool=tool_name)}')

    joint_solution, success, error = robot_kine_qp.inverse_kinematics(
            target_p[0:3], target_rpy=target_p[3:6], initial_guess=initial_guess_rad,
            max_iter=500, debug=False, tool=tool_name
        )

    print(f'the qp based kinematics result: {joint_solution}, success: {success}, error: {error}\n')
    if success:
        print(f'forward result of the ik solution is {robot_kine_qp.forward_kinematics(joint_solution , tool=tool_name)}\n')



    #   ----------      rm75 official algorithm         -----------
    robot_kine_rm = kine_rm()
    print(f'forward kine pose is {robot_kine_rm.forward_kinematics(q=joints, tool=tool_name)}')
    ret, q = robot_kine_rm.inverse_kinematics(target_position=target_p[0:3], target_rpy=target_p[3:6],initial_guess=initial_guess, tool=tool_name)

    print(f'the ik result is ret ={ret}, q = {[radians(q_s) for q_s in q]}')
    if ret == 0:
        print(f'forward result of ik rm ik solution is {robot_kine_rm.forward_kinematics(q=q, tool=tool_name)} ')










    print(f'\nDone\n')


    # try:
    #     while robot_mjk.viewer and robot_mjk.viewer.is_running():
    #         time.sleep(0.1)
    # except KeyboardInterrupt:
    #     pass
    robot_mjk.stop()



def main():

    demo_position_control()



if __name__ == "__main__":
    main()