import numpy as np
from time import sleep
from robodk import Pose_2_UR, UR_2_Pose
from robodk.robolink import *
import robodk.robomath as rm
import tools

# the setup
RDK = Robolink()
RDK.setRunMode(RUNMODE_SIMULATE)
UR5 = RDK.Item("UR5", ITEM_TYPE_ROBOT)

tls = tools.Tools(RDK)

ur_T_ms_np = np.array(          [[ 0.500000,    0.866025,     0.000000,   439.4 ],
                                  [  -0.866025,   0.5,     0.000000,  -277.9 ],
                                  [  0.000000,     0.000000,     1.000000,     41.9 ],
                                  [  0.000000,     0.000000,     0.000000,     1.000000 ]])

tcp_T_rt_np =  np.array([[ np.cos(np.deg2rad(-50)),   -np.sin(np.deg2rad(-50)),    0.000000,       0 ],
                      [ np.sin(np.deg2rad(-50)),    np.cos(np.deg2rad(-50)),    0.000000,       0 ],
                      [  0.000000,                  0.000000,                   1.000000,       0 ],
                      [  0.000000,                  0.000000,                   0.000000,       1.000000 ]])

rt_T_bi_np =  np.array([[ 1.000000,    0.000000,    0.000000,       -32.00 ],
                      [ 0.000000,   1.000000,    0.000000,       0 ],
                      [ 0.000000,   0.000000,    1.000000,       28.07 ],
                      [ 0.000000,   0.000000,    0.000000,       1.000000 ]])

ms_T_bi_np = np.array([[ 0.000000, 0.000000, 1.000000, -12.1 ],
                       [ 0.000000, -1.000000, 0.000000, -19.0 ],
                       [ 1.000000, 0.000000, 0.000000, 14.5 ],
                       [ 0.000000, 0.000000, 0.000000, 1.000000 ]])


ur_T_tcp_np = ur_T_ms_np @ ms_T_bi_np @ np.linalg.inv(rt_T_bi_np) @ np.linalg.inv(tcp_T_rt_np)

# reset the sim
robot_program = RDK.Item("Reset_Simulation_R", ITEM_TYPE_PROGRAM)
robot_program.RunCode()
robot_program.WaitFinished()

# pick up mazzer tool
tls.rancilio_tool_attach_r_ati()

# Convert numpy 4x4 array to robomath.Mat
ur_T_tcp = rm.Mat(ur_T_tcp_np.tolist())

# Note that these two HT matrices appear to be EXACTLY the same when printed.
print(ur_T_tcp)
print(UR_2_Pose(Pose_2_UR(ur_T_tcp)))

# The first MoveJ fails due to a target reach error.
UR5.MoveJ(ur_T_tcp, blocking=True)
# This one works - even though it should be EAXCTLY the same matrix!!!
# UR5.MoveJ(UR_2_Pose(Pose_2_UR(ur_T_tcp)), blocking=True)