try:
    from dobot_api import DobotApiFeedBack,DobotApiDashboard
except:
    from hardware.dobot_api import DobotApiFeedBack,DobotApiDashboard
import threading
from time import sleep,time
import re
import copy
import numpy as np
from scipy.spatial.transform import Rotation as R
import math
'''

    MODE    DESCRIPTION    CN

    1       INIT            初始化
    2       BREAK_OPEN      抱闸松开
    3       POWER_OFF       本体下电
    4       DISABLED        未使能(抱闸未松)
    5       ENABLE          使能(空闲)
    6       BACKDRIVE       拖拽
    7       RUNNING         运行状态(脚本/TCP)
    8       SINGLE_MOVE     单次运动(点动)
    9       ERROR           错误状态
    10      PAUSE           暂停状态
    11      COLLISION       碰撞状态

    MODE    RANK

    报错    I
    下电    II
    碰撞    III
    开抱闸  IV

'''

class dobot_nova5:
    def __init__(self,ip):
        self.ip = ip
        self.dashboardPort = 29999 ## 一发一收-设置-控制及运动端口号
        self.feedFourPort = 30004 ## 第四实时反馈(8ms)反馈数据端口号
        self.dashboard = None ## 初始化控制及运动端口实例
        self.feedFour = None ## 初始化数据反馈端口实例
        self.feedInfo = []
        self.__globalLockValue = threading.Lock() # 多线程安全

        class feedbackItem:
            def __init__(self):
                self.robotMode = -1
                self.robotCurrentCommandID = 0
                self.MessageSize = -1
                self.DigitalInputs = -1
                self.DigitalOutputs = -1
                self.QActual = -1
                self.ActualQuaternion = -1
                self.ToolVectorActual = -1
                # ... 自定义反馈数据
                '''
                self.UserValue = -1
                self.ToolValue = -1
                '''
        self.feedbackData = feedbackItem() # 自定义反馈数据结构体

    def parseResultId(self,valueRecv):
        # 解析返回值
        if "Not Tcp" in valueRecv:
            print("Control Mode is not TCP")
            print("机械臂不在二次开发模式下")
            return [1]
        return [int(num) for num in re.findall(r'-?\d+',valueRecv)] or [2]
    
    def GetFeedback(self):
        while True:
            feedInfo = self.feedFour.feedBackData()
            with self.__globalLockValue:
                if feedInfo is not None:
                    if hex((feedInfo['TestValue'][0]))=='0x123456789abcdef': ## DOBOT官方的数据魔数验证
                       # 基础字段
                        self.feedbackData.MessageSize = feedInfo['len'][0]
                        self.feedbackData.robotMode = feedInfo['RobotMode'][0]
                        self.feedbackData.DigitalInputs = feedInfo['DigitalInputs'][0]
                        self.feedbackData.DigitalOutputs = feedInfo['DigitalOutputs'][0]
                        self.feedbackData.robotCurrentCommandID = feedInfo['CurrentCommandId'][0]
                        self.feedbackData.QActual = feedInfo['QActual'][0]
                        self.feedbackData.ActualQuaternion = feedInfo['ActualQuaternion'][0]
                        self.feedbackData.ToolVectorActual = feedInfo['ToolVectorActual'][0]
                        # 自定义添加所需反馈数据
                        '''
                        self.feedData.DigitalOutputs = int(feedInfo['DigitalOutputs'][0])
                        self.feedData.RobotMode = int(feedInfo['RobotMode'][0])
                        self.feedData.TimeStamp = int(feedInfo['TimeStamp'][0])
                        self.feedbackData.UserValue = feedInfo['UserValue[6]'][0]
                        self.feedbackData.ToolValue = feedInfo['ToolValue[6]'][0]
                        ''' 

    def start(self):
        self.dashboard = DobotApiDashboard(self.ip,self.dashboardPort)
        self.feedFour = DobotApiFeedBack(self.ip,self.feedFourPort)
        self.dashboard.EmergencyStop(mode=0) # 松开急停
        self.clearError() # 清除报警
        if self.parseResultId(self.dashboard.EnableRobot())[0] != 0:
            print("使能失败:检查29999端口是否被占用")
            return
        print("机械臂使能成功")

        # 状态反馈线程
        feedback_thread = threading.Thread(
            target=self.GetFeedback # 机器状态反馈线程
        )
        feedback_thread.daemon = True
        feedback_thread.start()
    
    def RunPoint_P_inPose(self,poses_list):
        '''
            RUNPOINT_P 以关节运动方式运动到目标点
            MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: mm/deg
        '''
        # 走点
        recvmovemess = self.dashboard.MovJ(*poses_list,0)
        print("MovJ:",recvmovemess)
        print(self.parseResultId(recvmovemess))
        currentCommandID = self.parseResultId(recvmovemess)[1]
        print("指令 ID:", currentCommandID)
        sleep(0.02)
        while True: # 完成判断循环
            ########## 调试注释 #############
            # print("robot mode",self.feedbackData.robotMode)
            # print("feedback current ID", self.feedbackData.robotCurrentCommandID)
            # print("指令 ID:", currentCommandID)
            ########## 调试注释 #############
            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                self.dashboard.Stop()
                self.dashboard.EmergencyStop(mode=1)
                print("当前工作停止，机械臂处于急停状态")
            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                print("机械臂为暂停状态")
                res = self.dashboard.Continue()
                code = self.parseResultId(res)[0]
                if code == 0:
                    print("机械臂继续已暂停的运动指令")
            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                print("该次运动完成")
                break
            sleep(0.1)
        return 0
    
    def RunPoint_P_inPose_M_RAD(self,poses_list):
            '''
                RUNPOINT_P 以关节运动方式运动到目标点
                MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: M/RAD
            '''
            # 单位 m/rad -> mm/deg
            poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
            # 走点
            recvmovemess = self.dashboard.MovJ(*poses_list_mm_deg,0)
            print("MovJ:",recvmovemess)
            print(self.parseResultId(recvmovemess))
            currentCommandID = self.parseResultId(recvmovemess)[1]
            print("指令 ID:", currentCommandID)
            sleep(0.02)
            while True: # 完成判断循环
                ########## 调试注释 #############
                # print("robot mode",self.feedbackData.robotMode)
                # print("feedback current ID", self.feedbackData.robotCurrentCommandID)
                # print("指令 ID:", currentCommandID)
                ########## 调试注释 #############
                currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
                if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                    self.dashboard.Stop()
                    self.dashboard.EmergencyStop(mode=1)
                    print("当前工作停止，机械臂处于急停状态")
                if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                    print("机械臂为暂停状态")
                    res = self.dashboard.Continue()
                    code = self.parseResultId(res)[0]
                    if code == 0:
                        print("机械臂继续已暂停的运动指令")
                if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                    print("该次运动完成")
                    break
                sleep(0.1)
            return 0
    
    def RunPoint_P_inJoint(self,joints_list):
        '''
            RUNPOINT_P 以关节运动方式运动到目标点
            MovJ flag = 1 通过关节角度走点，输入为[J1 J2 J3 J4 J5 J6]  unit: deg
        '''
        # 走点
        recvmovemess = self.dashboard.MovJ(*joints_list,1)
        print("MovJ:",recvmovemess)
        print(self.parseResultId(recvmovemess))
        currentCommandID = self.parseResultId(recvmovemess)[1]
        print("指令 ID:", currentCommandID)
        sleep(0.02)
        while True: # 完成判断循环
            ########## 调试注释 #############
            # print("robot mode",self.feedbackData.robotMode)
            # print("feedback current ID", self.feedbackData.robotCurrentCommandID)
            # print("指令 ID:", currentCommandID)
            # print("30004",self.dashboard.GetCurrentCommandID())
            ########## 调试注释 #############
            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                self.dashboard.Stop()
                self.dashboard.EmergencyStop(mode=1)
                print("当前工作停止，机械臂处于急停状态")
            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                print("机械臂为暂停状态")
                res = self.dashboard.Continue()
                code = self.parseResultId(res)[0]
                if code == 0:
                    print("机械臂继续已暂停的运动指令")
            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                print("该次运动完成")
                break
            sleep(0.1)  # 避免 CPU 占用过高
        return 0
    
    def RunPoint_P_inJoint_RAD(self,joints_list):
        '''
            RUNPOINT_P 以关节运动方式运动到目标点
            MovJ flag = 1 通过关节角度走点，输入为[J1 J2 J3 J4 J5 J6]  unit: rad
        '''
        # 单位 rad -> deg
        joints_list_deg = self.__transform_rad_2_deg(joints_list)
        # 走点
        recvmovemess = self.dashboard.MovJ(*joints_list_deg,1)
        print("MovJ:",recvmovemess)
        print(self.parseResultId(recvmovemess))
        currentCommandID = self.parseResultId(recvmovemess)[1]
        print("指令 ID:", currentCommandID)
        sleep(0.02)
        while True: # 完成判断循环
            ########## 调试注释 #############
            # print("robot mode",self.feedbackData.robotMode)
            # print("feedback current ID", self.feedbackData.robotCurrentCommandID)
            # print("指令 ID:", currentCommandID)
            # print("30004",self.dashboard.GetCurrentCommandID())
            ########## 调试注释 #############
            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                self.dashboard.Stop()
                self.dashboard.EmergencyStop(mode=1)
                print("当前工作停止，机械臂处于急停状态")
            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                print("机械臂为暂停状态")
                res = self.dashboard.Continue()
                code = self.parseResultId(res)[0]
                if code == 0:
                    print("机械臂继续已暂停的运动指令")
            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                print("该次运动完成")
                break
            sleep(0.1)  # 避免 CPU 占用过高
        return 0
    
    def RunPoint_L_inPose(self,poses_list):
        '''
            RUNPOINT_L 以直线运动方式运动到目标点
            MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: mm/deg
        '''
        # 走点
        recvmovemess = self.dashboard.MovL(*poses_list,0)
        print("MovL:",recvmovemess)
        print(self.parseResultId(recvmovemess))
        currentCommandID = self.parseResultId(recvmovemess)[1]
        print("指令 ID:", currentCommandID)
        while True:
            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                self.dashboard.Stop()
                self.dashboard.EmergencyStop(mode=1)
                print("当前工作停止，机械臂处于急停状态")
            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                print("机械臂为暂停状态")
                res = self.dashboard.Continue()
                code = self.parseResultId(res)[0]
                if code == 0:
                    print("机械臂继续已暂停的运动指令")
            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                print("该次运动完成")
                break
            sleep(0.1)
        return 0
    
    def RunPoint_L_inPose_M_RAD(self,poses_list):
        '''
            RUNPOINT_L 以直线运动方式运动到目标点
            MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: m/rad
        '''
        # 单位 m/rad -> mm/deg
        poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
        # 走点
        recvmovemess = self.dashboard.MovL(*poses_list_mm_deg,0)
        print("MovL:",recvmovemess)
        print(self.parseResultId(recvmovemess))
        currentCommandID = self.parseResultId(recvmovemess)[1]
        print("指令 ID:", currentCommandID)
        while True:
            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                self.dashboard.Stop()
                self.dashboard.EmergencyStop(mode=1)
                print("当前工作停止，机械臂处于急停状态")
            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                print("机械臂为暂停状态")
                res = self.dashboard.Continue()
                code = self.parseResultId(res)[0]
                if code == 0:
                    print("机械臂继续已暂停的运动指令")
            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                print("该次运动完成")
                break
            sleep(0.1)
        return 0
    
    def RunPoint_L_inJoint(self,joints_list):
        '''
            RUNPOINT_L 以直线运动方式运动到目标点
            MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: mm/deg
        '''
        # 走点
        recvmovemess = self.dashboard.MovL(*joints_list,0)
        print("MovL:",recvmovemess)
        print(self.parseResultId(recvmovemess))
        currentCommandID = self.parseResultId(recvmovemess)[1]
        print("指令 ID:", currentCommandID)
        while True:
            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                self.dashboard.Stop()
                self.dashboard.EmergencyStop(mode=1)
                print("当前工作停止，机械臂处于急停状态")
            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                print("机械臂为暂停状态")
                res = self.dashboard.Continue()
                code = self.parseResultId(res)[0]
                if code == 0:
                    print("机械臂继续已暂停的运动指令")
            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                print("该次运动完成")
                break
            sleep(0.1)
        return 0
    
    def RunPoint_L_inJoint_RAD(self,joints_list):
        '''
            RUNPOINT_L 以直线运动方式运动到目标点
            MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: rad
        '''
        # 单位 rad -> deg
        joints_list_deg = self.__transform_rad_2_deg(joints_list)
        # 走点
        recvmovemess = self.dashboard.MovL(*joints_list_deg,0)
        print("MovL:",recvmovemess)
        print(self.parseResultId(recvmovemess))
        currentCommandID = self.parseResultId(recvmovemess)[1]
        print("指令 ID:", currentCommandID)
        while True:
            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
                self.dashboard.Stop()
                self.dashboard.EmergencyStop(mode=1)
                print("当前工作停止，机械臂处于急停状态")
            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
                print("机械臂为暂停状态")
                res = self.dashboard.Continue()
                code = self.parseResultId(res)[0]
                if code == 0:
                    print("机械臂继续已暂停的运动指令")
            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
                print("该次运动完成")
                break
            sleep(0.1)
        return 0

    def ServoJoint(self,joints_list):
        '''
        关节角度伺服
        输入为[J1 J2 J3 J4 J5 J6]  unit: deg
        '''
        tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]}, t=0.01, aheadtime=20, gain=200)"
        self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
        print("ServoJ:",tcp_command)
        if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
            self.dashboard.Stop()
            self.dashboard.EmergencyStop(mode=1)
            print("当前工作停止，机械臂处于急停状态")
            return -1
        if self.feedbackData.robotMode == 10: # 10 当前工程暂停
            print("机械臂为暂停状态")
            res = self.dashboard.Continue()
            code = self.parseResultId(res)[0]
            if code == 0:
                print("机械臂继续已暂停的运动指令")
        return 0
    
    def ServoJoint_RAD(self,joints_list):
        '''
        关节角度伺服
        输入为[J1 J2 J3 J4 J5 J6]  unit: rad
        '''
        # 单位转换 rad -> deg
        joints_list_deg = self.__transform_rad_2_deg(joints_list)
        # 构造字符串
        tcp_command = f"servoj({joints_list_deg[0]},{joints_list_deg[1]},{joints_list_deg[2]},{joints_list_deg[3]},{joints_list_deg[4]},{joints_list_deg[5]}, t=0.01, aheadtime=20, gain=200)"
        self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
        print("ServoJ:",tcp_command)
        if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
            self.dashboard.Stop()
            self.dashboard.EmergencyStop(mode=1)
            print("当前工作停止，机械臂处于急停状态")
            return -1
        if self.feedbackData.robotMode == 10: # 10 当前工程暂停
            print("机械臂为暂停状态")
            res = self.dashboard.Continue()
            code = self.parseResultId(res)[0]
            if code == 0:
                print("机械臂继续已暂停的运动指令")
        return 0
            
    def ServoPose(self,poses_list):
        '''
        位姿伺服
        输入为[X Y Z RX RY RZ]  unit: mm/deg
        '''
        poses_list = list(map(float, poses_list))
        tcp_command = f"ServoP({poses_list[0]},{poses_list[1]},{poses_list[2]},{poses_list[3]},{poses_list[4]},{poses_list[5]}, t=0.01, aheadtime=20, gain=300)"
        # res = self.dashboard.sendRecvMsg(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
        self.dashboard.send_data(tcp_command)
        # print("ServoP msg:",res)
        if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
            self.dashboard.Stop()
            self.dashboard.EmergencyStop(mode=1)
            print("当前工作停止，机械臂处于急停状态")
        if self.feedbackData.robotMode == 10: # 10 当前工程暂停
            print("机械臂为暂停状态")
            res = self.dashboard.Continue()
            code = self.parseResultId(res)[0]
            if code == 0:
                print("机械臂继续已暂停的运动指令")
    
    def ServoPose_M_RAD(self,poses_list):
        '''
        位姿伺服
        输入为[X Y Z RX RY RZ]  unit: M/RAD
        '''
        # 单位 m/rad -> mm/deg
        poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
        # 确保数据类型正确
        poses_list_mm_deg = list(map(float, poses_list_mm_deg))
        # 构造字符串
        tcp_command = f"ServoP({poses_list[0]},{poses_list[1]},{poses_list[2]},{poses_list[3]},{poses_list[4]},{poses_list[5]}, t=0.01, aheadtime=20, gain=200)"
        res = self.dashboard.sendRecvMsg(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
        print("ServoP msg:",res)
        if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
            self.dashboard.Stop()
            self.dashboard.EmergencyStop(mode=1)
            print("当前工作停止，机械臂处于急停状态")
        if self.feedbackData.robotMode == 10: # 10 当前工程暂停
            print("机械臂为暂停状态")
            res = self.dashboard.Continue()
            code = self.parseResultId(res)[0]
            if code == 0:
                print("机械臂继续已暂停的运动指令")

    def setEndEffector(self,i,tool_i):
        '''
            设置工具坐标系在索引i. i!=0
            tool_i 需要以字符串形式输入 "{x,y,z,rx,ry,rz}"
        '''
        if i == 0:
            print("不可修改工具0(初始法兰坐标系)")
            return
        
        if i not in {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}:
            print("工具坐标系序号超出范围")
            return
        
        res = self.dashboard.SetTool(i,tool_i)
        print(res)
        code = self.parseResultId(res)[0]
        if code == 0:
            print(f"设置工具坐标系{i}的偏移量成功")
        else:
            print(f"设置工具坐标系{i}失败")
            return 
    
    def chooseEndEffector(self,i):
        '''
            切换工具坐标系, 0~9
        '''
        if i not in {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}:
            print("工具坐标系序号超出范围")
            return
        res = self.dashboard.Tool(i)
        print(res)
        code = self.parseResultId(res)[0]
        if code == 0:
            print(f"切换为工具坐标系{i}成功")
        else:
            print("切换工具坐标系失败")
            return 
    
    def setBaseFrame(self,i,base_i):
        '''
            设置工具坐标系在索引i. i!=0
            base_i 修改后的用户坐标系(非偏移量!) 需要以字符串形式输入 "{x,y,z,rx,ry,rz}"
        '''
        if i == 0:
            print("不可修改用户坐标系0(初始基坐标系)")
            return
        
        if i not in {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}:
            print("用户坐标系序号超出范围")
            return
        
        res = self.dashboard.SetUser(i,base_i)
        code = self.parseResultId(res)[0]
        if code == 0:
            print(f"设置用户坐标系{i}成功")
        else:
            print(f"设置用户坐标系{i}失败")
            return 
    
    def chooseBaseFrame(self,i):
        '''
            切换用户基坐标系, 0~9
        '''
        if i not in {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}:
            print("工具坐标系序号超出范围")
            return
        res = self.dashboard.User(i)
        code = self.parseResultId(res)[0]
        if code == 0:
            print(f"切换为用户基坐标系{i}成功")
        else:
            print("切换用户基坐标系失败")
            return 
    
    def getPose(self,user=-1,tool=-1):
        if (user == -1) != (tool == -1): ## XOR 必须同时设置或者同时不设置
            print("必须同时设置或同时不设置坐标系参数")
        res = self.dashboard.GetPose(user,tool) # 返回字符串
        print("get pose",res)
        code = self.parseResultId(res)[0]
        if code == 0:
            pass
            # print("success")
        else:
            print("获取位姿失败")
            return
        # 处理字符串
        start = res.find("{")+1
        end = res.find("}")
        pose_str = res[start:end]
        pose = [float(x.strip()) for x in pose_str.split(",")]
        return pose
    
    def getAngel(self):
        res = self.dashboard.GetAngle()
        code = self.parseResultId(res)[0]
        if code == 0:
            pass
            # print("success")
        else:
            print("获取关节角度失败")
            return
        # 处理字符串
        start = res.find("{")+1
        end = res.find("}")
        angle_str = res[start:end]
        print("res:",res)
        angle = [float(x.strip()) for x in angle_str.split(",")]
        return angle

    def getForwardKin(self,joints_list,user=-1,tool=-1):
        res = self.dashboard.PositiveKin(*joints_list,user,tool)
        code = self.parseResultId(res)[0]
        if code == 0:
            pass
            # print("success")
        else:
            print("获取正向运动学解失败")
            return
        # 处理字符串
        start = res.find("{")+1
        end = res.find("}")
        pose_str = res[start:end]
        pose = [float(x.strip()) for x in pose_str.split(",")]
        return pose
    
    def getInverseKin(self,poses_list,user=-1,tool=-1,useJointNear=-1,JointNear=''):
        '''
            poses_list    X Y Z RX RY RZ
            useJointNear 0 或不携带 表示 JointNear无效
                         1 表示根据JointNear就近选解
            jointNear string "jointNear={j1,j2,j3,j4,j5,j6}"   
        '''
        res = self.dashboard.InverseKin(*poses_list,user,tool,useJointNear,JointNear)
        print(res)
        code = self.parseResultId(res)[0]
        if code == 0:
            pass
            # print("success")
        else:
            print("获取逆向运动学解失败")
            return
        # 处理字符串
        start = res.find("{")+1
        end = res.find("}")
        joint_str = res[start:end]
        joint = [float(x.strip()) for x in joint_str.split(",")]
        return joint
    
    def disableRobot(self):
        res = self.dashboard.DisableRobot()
        code = self.parseResultId(res)[0]
        if code == 0:
            print("下使能机械臂成功")
        else:
            print("下使能机械臂失败")
        return
    
    def clearError(self):
        res = self.dashboard.ClearError()
        code = self.parseResultId(res)[0]
        if code == 0:
            print("清楚报警成功")
        else:
            print("清楚报警失败")
        return

    def start_drag(self):
        res = self.dashboard.StartDrag()
        code = self.parseResultId(res)[0]
        if code == 0:
            print("拖拽模式开启成功")
        else:
            print("拖拽模式开启失败")
        return
    
    def stop_drag(self):
        res = self.dashboard.StopDrag()
        code = self.parseResultId(res)[0]
        if code == 0:
            print("拖拽模式关闭成功")
        else:
            print("拖拽模式关闭失败")
        return
    
    def stop_motion(self):
        res = self.dashboard.Stop()
        code = self.parseResultId(res)[0]
        if code == 0:
            print("机械臂停止下发运动指令队列")
        else:
            print("停止下发运动失败,需立即拍下急停")
        return

    def __del__(self):
        del self.dashboard
        del self.feedFour
    ## 内部工具函数

    def __transform_m_rad_2_mm_deg(self,input_list):
        '''
        接受列表 [X Y Z RX RY RZ]
        将单位 m/rad 转为 mm/deg
        '''
        if len(input_list) != 6:
            raise ValueError("Input list must contain exactly 6 elements [X, Y, Z, RX, RY, RZ]")
        
        # 转换位置单位：m -> mm (乘以 1000)
        x_mm = input_list[0] * 1000.0
        y_mm = input_list[1] * 1000.0
        z_mm = input_list[2] * 1000.0
        
        # 转换角度单位：rad -> deg (乘以 180/pi)
        rx_deg = math.degrees(input_list[3])
        ry_deg = math.degrees(input_list[4])
        rz_deg = math.degrees(input_list[5])
        
        return [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg]
    
    def __transform_rad_2_deg(self,input_list):
        '''
        接受列表 [J1 J2 J3 J4 J5 J6]
        将单位rad转为deg
        '''
        if len(input_list) != 6:
            raise ValueError("Input list must contain exactly 6 elements [J1, J2, J3, J4, J5, J6]")
        # 转换角度单位: rad -> deg
        J_deg = []
        for i in range(6):
            J_deg.append(math.degrees(input_list[i]))
        return J_deg


    ## 为反馈线程封装
    
    def Get_feedInfo_angle(self):
        # print("反馈当前角度",self.feedbackData.QActual)
        return self.feedbackData.QActual
    
    def Get_feedInfo_pose(self):
        # print("反馈当前TCP位置",self.feedbackData.ToolVectorActual)
        return self.feedbackData.ToolVectorActual
    
    ## 适配中间层的再封装接口
    
    # 为状态管理而封装的初始化函数
    def init(self):
        self.is_exit = False
        self.traj_list = None
        self.last_pose_command = np.zeros(6)
        self.last_input_command = None
        self.tcp_offset = None
        self.init_J = [0,30,-120,0,90,0]
        self.filter_matirx = np.zeros((1,7)) # 位姿伺服用
        self.filter_matrix = np.zeros((1,6)) # 角度伺服用
        sleep(1)
        self.is_standby = False
        code = self.RunPoint_P_inJoint(self.init_J)
        if code == 0:
            print("机械臂初始化成功且到达初始位置")
        else:
            print("机械臂初始化失败")

        

    def get_arm_position(self):
        '''
        输出pos,quat
        pos: [x,y,z] in m
        quat: [qx,qy,qz,qw] 四元数
        '''
        pose = self.Get_feedInfo_pose()
        # print("pose",pose)
        x,y,z,rx,ry,rz = pose # x y z in mm; rx ry rz in degree
        x /= 1000
        y /= 1000
        z /= 1000
        pos = np.array([x,y,z])
        quat_rot = R.from_euler('xyz', [rx, ry, rz], degrees=True).as_quat()
        return pos,quat_rot

    def __circular_mean(self,angles):
        # 将角度转换为复数单位圆上的点，再计算均值
        # 为处理周期性问题
        angles_rad = np.deg2rad(angles)
        x = np.mean(np.cos(angles_rad))
        y = np.mean(np.sin(angles_rad))
        return np.rad2deg(np.arctan2(y, x))
    
    def process_command_angle(self,arm_joint_command):
        if (not self.is_exit) and (not self.is_standby):
            current_joint = np.asarray(arm_joint_command) # degree
            # FIFO
            self.filter_matrix = np.append(self.filter_matrix,[current_joint],axis=0)
            if len(self.filter_matrix)<7:
                return 0,current_joint
            self.filter_matrix = np.delete(self.filter_matrix,0,axis = 0)
            avg_joint = np.array([self.__circular_mean(self.filter_matrix[:, i]) for i in range(len(current_joint))])
            return 0,avg_joint
        return -1,None
    
    def process_command(self, arm_position_command, arm_orientation_command):
        if (not self.is_exit) and (not self.is_standby):
            # 构造当前位姿（位置 + 四元数）
            current_pose = np.concatenate([
                arm_position_command,      # [x, y, z]
                arm_orientation_command  # [qx, qy, qz, qw]
            ])
            
            # 更新滑动窗口（FIFO）
            self.filter_matirx = np.append(self.filter_matirx, [current_pose], axis=0)
            
            # 如果窗口未填满，直接返回当前值（不滤波）
            if len(self.filter_matirx) < 7:
                rot_euler = R.from_quat(arm_orientation_command).as_euler('xyz', degrees=True)
                result = np.concatenate([arm_position_command, rot_euler])
                return 0, result
            
            # 窗口已填满，删除最旧数据
            self.filter_matirx = np.delete(self.filter_matirx, 0, axis=0)
            
            # --- 位置分量：滑动平均（仅对有效数据）---
            avg_position = np.mean(self.filter_matirx[:, :3], axis=0)
            
            # --- 四元数分量：加权平均（处理符号歧义）---
            quats = self.filter_matirx[:, 3:7]
            for i in range(1, len(quats)):
                if np.dot(quats[0], quats[i]) < 0:
                    quats[i] = -quats[i]  # 确保所有四元数在同一半球
            avg_quat = np.mean(quats, axis=0)
            avg_quat = avg_quat / np.linalg.norm(avg_quat)  # 归一化
            
            # 转换回欧拉角
            rot_euler = R.from_quat(avg_quat).as_euler('xyz', degrees=True)
            
            # 合并结果 [x, y, z, roll, pitch, yaw]
            result = np.concatenate([avg_position, rot_euler])
            return 0, result
        
        return -1, None


if __name__ == "__main__":
    # sleep(5)
    dobot = dobot_nova5("192.168.5.1")
    dobot.start()
    posJ = [0,30,-120,0,90,0]
    dobot.RunPoint_P_inJoint(posJ)
    sleep(1)
    dobot.setEndEffector(i=9,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}")
    dobot.chooseEndEffector(i=9)
    print("Arm pose:",dobot.getPose())

    # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90])

    # dobot.start_drag()
    dobot.disableRobot()