try:
    import vrep
except:
    print ('--------------------------------------------------------------')
    print ('"vrep.py" could not be imported. This means very probably that')
    print ('either "vrep.py" or the remoteApi library could not be found.')
    print ('Make sure both are in the same folder as this file,')
    print ('or appropriately adjust the file "vrep.py"')
    print ('--------------------------------------------------------------')
    print ('')

import time
import sys
import numpy as np
import math
from time import sleep
from serialCommunication import *
import re
#python端与V-rep建立连接
# print ('Program started')
# vrep.simxFinish(-1)  # 闭所有与VREP之前的连接
# clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)  # 参数解释可查看API文档
# if clientID != -1:  # 检查连接是否成功
#     print('Connected to remote API server')
# else:
#     print('Connection not successful')
#     sys.exit('Could not connect')
#
# vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)     #启动仿真
# print("Simulation start")
# #####检索对象句柄
# jointNum = 4
# jointName = 'PhantomXPincher_joint'
# baseName = 'PhantomXPincher'
# jointHandle = np.zeros((jointNum,), dtype=int)  # 注意是整型
# for i in range(jointNum):
#     errorCode, returnHandle = vrep.simxGetObjectHandle(clientID, jointName + str(i + 1), vrep.simx_opmode_blocking)
#     print(errorCode, returnHandle)
#     jointHandle[i] = returnHandle
#     #print(jointHandle[i])
#
# # 读取每个关节角度
# jointConfig = np.zeros((jointNum, 1))
# for i in range(jointNum):
#     _, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i], vrep.simx_opmode_blocking)
#     jointConfig[i] = jpos
#     print(jointConfig[i])
#
# # 驱动ur机械臂关节旋转角度
# joint_angle = [0,90,0,0]     #机械臂每一个关节的旋转角度
# RAD2DEG = 180 / math.pi         # transform radian to degrees
#
# vrep.simxPauseCommunication(clientID, True)
# for i in range(jointNum):
#     print(f"现在第{i}个关节移动到：",joint_angle[i])
#     vrep.simxSetJointTargetPosition(clientID, jointHandle[i], joint_angle[i]/RAD2DEG, vrep.simx_opmode_oneshot)
#     sleep(0.5)
# vrep.simxPauseCommunication(clientID, False)

class Vrep_simulation(object):
    def __init__(self,link):
        vrep.simxFinish(-1)  # 关闭所有与VREP之前的连接
        self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)  # 参数解释可查看API文档
        self.com = link
        if self.clientID != -1:  # 检查连接是否成功
            print('Connected to remote API server')
        else:
            print('Connection not successful')
            sys.exit('Could not connect')
        vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)  # 启动仿真
        print("Simulation start")
        self.jointNum = 4,
        self.jointName = 'PhantomXPincher_joint'
        self.baseName = 'PhantomXPincher'
        #获得对象句柄
        self.jointHandle = np.zeros((4,), dtype=int)  # 注意是整型
        for i in range(4):
            errorCode, returnHandle = vrep.simxGetObjectHandle(self.clientID, self.jointName + str(i + 1),
                                                               vrep.simx_opmode_blocking)
            # print(errorCode, returnHandle)
            self.jointHandle[i] = returnHandle
        # print(self.jointHandle)

    def get_joints(self):
        self.jointConfig = np.zeros((self.jointNum, 1))
        for i in range(self.jointNum):
            _, jpos = vrep.simxGetJointPosition(self.clientID, self.jointHandle[i], vrep.simx_opmode_blocking)
            self.jointConfig[i] = jpos
            print(self.jointConfig[i])
        return self.jointConfig

    def drive_joints(self):
        RAD2DEG = 180 / math.pi
        sleep(1)
        self.com.Send_hex_data("1".encode())
        joint_angle = [0]*4
        # sleep(0.05)
        # while True:
        joint_data=self.com.Read_Size(17).decode()
        print(joint_data)
        joints = [int(x) for x in re.findall(r'[-+]?\d+', joint_data)]
        print(joints)
        joint_angle[1:4]=joints
        joint_angle[1]+=90
        #这里的angle是以pi度量
        vrep.simxPauseCommunication(self.clientID, True)
        for i in range(4):
            print(f"现在第{i}个关节移动到：", joint_angle[i])
            vrep.simxSetJointTargetPosition(self.clientID, self.jointHandle[i], joint_angle[i]/RAD2DEG,
                                            vrep.simx_opmode_oneshot)
        vrep.simxPauseCommunication(self.clientID, False)

            # if 0xFF == ord('e'):
            #     break