# -*- coding: utf-8 -*-
"""
# @FileName      : parse_data
# @Time          : 2025-03-20 15:07:34
# @description   : 非实时数据获取示例
# @detail        :
"""
import struct
from dataclasses import dataclass
from typing import List
import socket
import setup_path
import platform
# 根据操作系统导入相应的模块
if platform.system() == 'Windows':
    from Release.windows import xCoreSDK_python
elif platform.system() == 'Linux':
    from Release.linux import xCoreSDK_python
else:
    raise ImportError("Unsupported operating system")
from log import print_log, print_separator
from datetime import timedelta
import time


@dataclass
class XCoreStatus:
    # ----- 基础信息 -----
    msg_size: int  # 0-3
    ts: int  # 4-11
    power_st: int  # 12
    robot_busy: int  # 13
    operation_st: int  # 14
    operate_mode: int  # 15
    collision_st: int  # 16
    collaborate_mode: int  # 17
    sdk_connection: int  # 18
    log_code: int  # 19-22
    # ----- 关节数据 -----
    q_m: List[float]  # 23-78 (7 doubles)
    dq_m: List[float]  # 79-134 (7 doubles)
    tau_m: List[float]  # 135-190 (7 doubles)
    # ----- 外部轴数据 -----
    ex_q_m: List[float]  # 191-238 (6 doubles)
    ex_dq_m: List[float]  # 239-286 (6 doubles)
    # ----- 坐标系数据 -----
    pos_eir_m: List[float]  # 287-334 (6 doubles)
    pos_fib_m: List[float]  # 335-382 (6 doubles)
    tool_frame: List[float]  # 383-430 (6 doubles)
    wobj_frame: List[float]  # 431-478 (6 doubles)
    # ----- IO信号 -----
    dio_board0_di: int  # 479-480 (小端int16)
    dio_board0_do: int  # 481-482 (小端int16)
    dio_board1_di: int  # 483-484 (小端int16)
    dio_board1_do: int  # 485-486 (小端int16)
    dio_board2_di: int  # 487-488 (小端int16)
    dio_board2_do: int  # 489-490 (小端int16)
    # ----- 将IO信号按位存成数组 -----
    dio_board0_di_array: List[int]
    dio_board0_do_array: List[int]
    dio_board1_di_array: List[int]
    dio_board1_do_array: List[int]
    dio_board2_di_array: List[int]
    dio_board2_do_array: List[int]
    # ----- 模拟信号 -----
    aio_board0_ai: List[float]  # 491-538 (6 doubles)
    aio_board0_ao: List[float]  # 539-586 (6 doubles)
    aio_board1_ai: List[float]  # 587-634 (6 doubles)
    aio_board1_ao: List[float]  # 635-682 (6 doubles)
    # ----- 状态信号 -----
    Xpanel_keypad: int  # 683
    fault_sr: int  # 684
    servo_alarm: List[int]  # 685-712 (7 int32)
    rl_prog_running: int  # 713
    speed_ratio: float  # 714-721 (double)
    V: bytes  # 722 (结束符)


def parse_xcore_data(data: bytes) -> XCoreStatus:
    """解析723字节的协议数据包"""
    if len(data) != 723:
        raise ValueError(f"数据长度错误，期望723字节，实际收到{len(data)}字节")

    # 辅助函数：解析数组
    def parse_array(fmt, offset, count):
        ret = list()
        step = 0
        if "d" in fmt:
            step = 8
        elif "i" in fmt:
            step = 4
        for i in range(count):
            ret.append(struct.unpack_from(fmt, data, offset + i * step)[0])
        return ret

    # 辅助函数：处理小端数据
    def parse_little_endian(fmt, offset):
        return struct.unpack_from('<' + fmt, data, offset)[0]

    def get_io_array(di_do: int) -> List[int]:
        bit_list = []

        for i in range(16):
            bit = (di_do >> i) & 1
            bit_list.append(bit)

        return bit_list

    return XCoreStatus(
        # ----- 基础信息 -----
        msg_size=struct.unpack_from('<i', data, 0)[0],
        ts=struct.unpack_from('<Q', data, 4)[0],
        power_st=data[12],
        robot_busy=data[13],
        operation_st=data[14],
        operate_mode=data[15],
        collision_st=data[16],
        collaborate_mode=data[17],
        sdk_connection=data[18],
        log_code=struct.unpack_from('<i', data, 19)[0],
        # ----- 关节数据 -----
        # q_m=list[struct.unpack_from('<d', data, 23)[0]],
        q_m=parse_array('<d', 23, 7),
        dq_m=parse_array('<d', 79, 7),
        tau_m=parse_array('<d', 135, 7),
        # ----- 外部轴数据 -----
        ex_q_m=parse_array('<d', 191, 6),
        ex_dq_m=parse_array('<d', 239, 6),
        # ----- 坐标系数据 -----
        pos_eir_m=parse_array('<d', 287, 6),
        pos_fib_m=parse_array('<d', 335, 6),
        tool_frame=parse_array('<d', 383, 6),
        wobj_frame=parse_array('<d', 431, 6),
        # ----- IO信号 -----
        dio_board0_di=parse_little_endian('H', 479),
        dio_board0_do=parse_little_endian('H', 481),
        dio_board1_di=parse_little_endian('H', 483),
        dio_board1_do=parse_little_endian('H', 485),
        dio_board2_di=parse_little_endian('H', 487),
        dio_board2_do=parse_little_endian('H', 489),
        dio_board0_di_array=get_io_array(parse_little_endian('H', 479)),
        dio_board0_do_array=get_io_array(parse_little_endian('H', 481)),
        dio_board1_di_array=get_io_array(parse_little_endian('H', 483)),
        dio_board1_do_array=get_io_array(parse_little_endian('H', 485)),
        dio_board2_di_array=get_io_array(parse_little_endian('H', 487)),
        dio_board2_do_array=get_io_array(parse_little_endian('H', 489)),
        # ----- 模拟信号 -----
        aio_board0_ai=parse_array('<d', 491, 6),
        aio_board0_ao=parse_array('<d', 539, 6),
        aio_board1_ai=parse_array('<d', 587, 6),
        aio_board1_ao=parse_array('<d', 635, 6),
        # ----- 状态信号 -----
        Xpanel_keypad=data[683],
        fault_sr=data[684],
        servo_alarm=parse_array('<i', 685, 7),
        rl_prog_running=data[713],
        speed_ratio=struct.unpack_from('<d', data, 714)[0],
        V=data[722:723])


# 使用示例
if __name__ == "__main__":
    REMOTE_IP = "192.168.110.129"  # 替换为实际IP
    REMOTE_PORT = 16666  # 当前固定为16666端口，不支持替换
    host = REMOTE_IP
    port = REMOTE_PORT
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
        try:
            robot = xCoreSDK_python.xMateRobot(host)
            ec = {}
            robot.stopReceiveRobotState_Nrt(ec)
            print_log("stopReceiveRobotState_Nrt", ec)
            robot.startReceiveRobotState_Nrt(timedelta(seconds=0.1), ec)
            print_log("startReceiveRobotState_Nrt", ec)
            l = list()
            ret = robot.queryStateDataPublisherServerInfo(l, ec)
            if ret == 0:
                print("服务端监听中")
            elif ret == 2:
                print("服务端已连接")
            elif ret == 3:
                print("服务端已关闭")
            else:
                print("发生错误")

            s.connect((host, port))
            print(f"成功连接到 {host}:{port}")

            buffer = b''
            while True:
                buffer = s.recv(723)
                status = parse_xcore_data(buffer)

                # 打印部分字段示例
                print(f"时间戳: {status.ts}")
                print(f"关节角度: {status.q_m}")
                print(f"IO板0 DI信号: {bin(status.dio_board0_di)}")
                # 打印所有属性
                for attr_name, attr_value in status.__dict__.items():
                    print(f"{attr_name}: {attr_value}")
                time.sleep(0.1)
        except ConnectionRefusedError:
            print(f"无法连接到 {host}:{port}")
        except Exception as e:
            print(f"发生错误: {str(e)}")
