cnc/gpb_GLCanon_arcseg_sim_rtcp_rs274_kins.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
GLCanon 五轴刀具路径解析器 - 完整集成版
包含：RTCP运动学 + 完整G代码解析 + 梯形速度规划 + 虚拟HAL + 完整状态机 + JSON配置支持
集成 LinuxCNC 运动学算法：trtfuncs.c, 5axiskins.c, genhexkins.c, genserfuncs.c, 
pumakins.c, scarakins.c, tripodkins.c, pentakins.c, rotarydeltakins.c, rotatekins.c, 
corexykins.c, scorbot-kins.c, rosekins.c
集成 LinuxCNC 完整参数表系统 (linuxcnc_parameter_table)

版本: 5.0.0 - 完整集成所有 LinuxCNC 运动学算法和参数系统
"""

import os
import sys
import re
import math
import time
import threading
import tempfile
import json
import traceback
from typing import List, Dict, Optional, Tuple, Any, Callable, Union, Set
from enum import IntEnum, Enum
from dataclasses import dataclass, field
from collections import deque, OrderedDict
from abc import ABC, abstractmethod
import dataclasses

# ==================== 导入 LinuxCNC 参数表系统 ====================
# ==================== 导入 LinuxCNC 参数表系统 ====================
try:
    import linuxcnc_parameter_table as lcnc_param
    from linuxcnc_parameter_table import (
        LinuxCNCParameterTable,
        InterpParameterIndex,
        ParameterAttribute,
        ParameterValue,
        ExpressionEvaluator,
        HALVariableManager,
        SystemVariableQuery,
        Context,
        StateTag,
        Block,
        # G代码常量
        G_0, G_1, G_2, G_3, G_4, G_5, G_5_1, G_5_2, G_5_3,
        G_6, G_6_1, G_6_2, G_6_3, G_7, G_8, G_10,
        G_17, G_17_1, G_18, G_18_1, G_19, G_19_1,
        G_20, G_21, G_28, G_28_1, G_30, G_30_1,
        G_33, G_33_1, G_38_2, G_38_3, G_38_4, G_38_5,
        G_40, G_41, G_41_1, G_42, G_42_1,
        G_43, G_43_1, G_43_2, G_49, G_52, G_53,
        G_54, G_55, G_56, G_57, G_58, G_59,
        G_59_1, G_59_2, G_59_3, G_61, G_61_1, G_64,
        G_70, G_71, G_71_1, G_71_2, G_72, G_72_1, G_72_2,
        G_73, G_74, G_76, G_80, G_81, G_82, G_83, G_84,
        G_85, G_86, G_87, G_88, G_89, G_90, G_90_1,
        G_91, G_91_1, G_92, G_92_1, G_92_2, G_92_3,
        G_93, G_94, G_95, G_96, G_97, G_98, G_99,
        # 常量
        RS274NGC_MAX_PARAMETERS, READONLY_PARAMETERS, REQUIRED_PARAMETERS,
        INTERP_SUB_PARAMS, INTERP_SUB_ROUTINE_LEVELS,
        TOLERANCE_EQUAL, TOLERANCE_CONCAVE_CORNER,
        EMCMOT_MAX_AXIS, EMCMOT_MAX_JOINTS, EMCMOT_MAX_SPINDLES,
    )
except ImportError:
    print("错误: 无法导入 linuxcnc_parameter_table 模块")
    print("请确保 linuxcnc_parameter_table.py 在同一目录下")
    sys.exit(1)

# ==================== 常量定义 ====================
VERSION = "5.0.0"

# 数学常量
CART_FUZZ = 1e-5
TO_RAD = math.pi / 180.0
TO_DEG = 180.0 / math.pi
GO_PI_2 = math.pi / 2.0
GO_PI = math.pi
PM_PI = math.pi
PM_2_PI = 2.0 * math.pi
TOLERANCE_EQUAL = 1e-6
TOLERANCE_CONCAVE_CORNER = 1e-5
TINY = 1e-7

# 单位转换
INCH_PER_MM = 0.03937007874016
MM_PER_INCH = 25.4

# 最大轴数
EMCMOT_MAX_AXIS = 9
EMCMOT_MAX_JOINTS = 9
EMCMOT_MAX_SPINDLES = 4

# 刀具表大小
CANON_POCKETS_MAX = 56

# 栈深度
STACK_LEN = 50
STACK_ENTRY_LEN = 80

# 行长度
LINELEN = 256

# 从 linuxcnc_parameter_table 导入常量
RS274NGC_MAX_PARAMETERS = lcnc_param.RS274NGC_MAX_PARAMETERS
READONLY_PARAMETERS = lcnc_param.READONLY_PARAMETERS
REQUIRED_PARAMETERS = lcnc_param.REQUIRED_PARAMETERS
INTERP_SUB_PARAMS = lcnc_param.INTERP_SUB_PARAMS
INTERP_SUB_ROUTINE_LEVELS = lcnc_param.INTERP_SUB_ROUTINE_LEVELS

# G代码常量（从lcnc_param导入）
G_0 = lcnc_param.G_0
G_1 = lcnc_param.G_1
G_2 = lcnc_param.G_2
G_3 = lcnc_param.G_3
G_4 = lcnc_param.G_4
G_5 = lcnc_param.G_5
G_5_1 = lcnc_param.G_5_1
G_5_2 = lcnc_param.G_5_2
G_5_3 = lcnc_param.G_5_3
G_6 = lcnc_param.G_6
G_6_1 = lcnc_param.G_6_1
G_6_2 = lcnc_param.G_6_2
G_6_3 = lcnc_param.G_6_3
G_7 = lcnc_param.G_7
G_8 = lcnc_param.G_8
G_10 = lcnc_param.G_10
G_17 = lcnc_param.G_17
G_17_1 = lcnc_param.G_17_1
G_18 = lcnc_param.G_18
G_18_1 = lcnc_param.G_18_1
G_19 = lcnc_param.G_19
G_19_1 = lcnc_param.G_19_1
G_20 = lcnc_param.G_20
G_21 = lcnc_param.G_21
G_28 = lcnc_param.G_28
G_28_1 = lcnc_param.G_28_1
G_30 = lcnc_param.G_30
G_30_1 = lcnc_param.G_30_1
G_33 = lcnc_param.G_33
G_33_1 = lcnc_param.G_33_1
G_38_2 = lcnc_param.G_38_2
G_38_3 = lcnc_param.G_38_3
G_38_4 = lcnc_param.G_38_4
G_38_5 = lcnc_param.G_38_5
G_40 = lcnc_param.G_40
G_41 = lcnc_param.G_41
G_41_1 = lcnc_param.G_41_1
G_42 = lcnc_param.G_42
G_42_1 = lcnc_param.G_42_1
G_43 = lcnc_param.G_43
G_43_1 = lcnc_param.G_43_1
G_43_2 = lcnc_param.G_43_2
G_49 = lcnc_param.G_49
G_52 = lcnc_param.G_52
G_53 = lcnc_param.G_53
G_54 = lcnc_param.G_54
G_55 = lcnc_param.G_55
G_56 = lcnc_param.G_56
G_57 = lcnc_param.G_57
G_58 = lcnc_param.G_58
G_59 = lcnc_param.G_59
G_59_1 = lcnc_param.G_59_1
G_59_2 = lcnc_param.G_59_2
G_59_3 = lcnc_param.G_59_3
G_61 = lcnc_param.G_61
G_61_1 = lcnc_param.G_61_1
G_64 = lcnc_param.G_64
G_70 = lcnc_param.G_70
G_71 = lcnc_param.G_71
G_71_1 = lcnc_param.G_71_1
G_71_2 = lcnc_param.G_71_2
G_72 = lcnc_param.G_72
G_72_1 = lcnc_param.G_72_1
G_72_2 = lcnc_param.G_72_2
G_73 = lcnc_param.G_73
G_74 = lcnc_param.G_74
G_76 = lcnc_param.G_76
G_80 = lcnc_param.G_80
G_81 = lcnc_param.G_81
G_82 = lcnc_param.G_82
G_83 = lcnc_param.G_83
G_84 = lcnc_param.G_84
G_85 = lcnc_param.G_85
G_86 = lcnc_param.G_86
G_87 = lcnc_param.G_87
G_88 = lcnc_param.G_88
G_89 = lcnc_param.G_89
G_90 = lcnc_param.G_90
G_90_1 = lcnc_param.G_90_1
G_91 = lcnc_param.G_91
G_91_1 = lcnc_param.G_91_1
G_92 = lcnc_param.G_92
G_92_1 = lcnc_param.G_92_1
G_92_2 = lcnc_param.G_92_2
G_92_3 = lcnc_param.G_92_3
G_93 = lcnc_param.G_93
G_94 = lcnc_param.G_94
G_95 = lcnc_param.G_95
G_96 = lcnc_param.G_96
G_97 = lcnc_param.G_97
G_98 = lcnc_param.G_98
G_99 = lcnc_param.G_99

# 默认颜色字典
DEFAULT_COLORS = {
    'back': '#000000',
    'grid': '#404040',
    'axis': '#c0c0c0',
    'rapid': '#00ff00',
    'feed': '#ffff00',
    'arc_cw': '#00ffff',
    'arc_ccw': '#ff00ff',
    'dwell': '#ffffff',
    'probe': '#ff8000',
    'selected': '#ff0000',
    'traverse': '#00ff00',
    'straight_feed': '#ffff00',
    'arc_feed': '#00ffff',
    'm1xx': '#ff8000',
    'rtcp_on': '#00ff88',
    'rtcp_off': '#ff6600',
    'program_end': '#ff0000',
    'g0': '#00ff00',
    'g1': '#ffff00',
    'g2': '#00ffff',
    'g3': '#ff00ff',
    'g38': '#ff8000',
    'g80': '#808080',
    'm6': '#ff0000',
    'm3': '#00ff00',
    'm4': '#ff0000',
    'm5': '#ffff00',
    'm7': '#0088ff',
    'm8': '#0088ff',
    'm9': '#ff0000',
}


# ==================== 纯 Python 实现的包围盒计算 ====================
def calc_extents_python(*lists):
    """
    计算刀具路径的包围盒（纯 Python 实现，替代 gcode.calc_extents）
    """
    INF = 9e99
    
    min_x = min_y = min_z = INF
    max_x = max_y = max_z = -INF
    
    min_xt = min_yt = min_zt = INF
    max_xt = max_yt = max_zt = -INF
    
    for seq in lists:
        if not seq:
            continue
        
        for item in seq:
            if len(item) == 5:
                linenum, start, end, feed, tooloffset = item
            elif len(item) == 4:
                linenum, start, end, tooloffset = item
            else:
                continue
            
            xs, ys, zs = start[0], start[1], start[2]
            xe, ye, ze = end[0], end[1], end[2]
            
            if len(tooloffset) >= 3:
                xt, yt, zt = tooloffset[0], tooloffset[1], tooloffset[2]
            else:
                xt = yt = zt = 0.0
            
            for (x, y, z) in [(xs, ys, zs), (xe, ye, ze)]:
                if x < min_x: min_x = x
                if x > max_x: max_x = x
                if y < min_y: min_y = y
                if y > max_y: max_y = y
                if z < min_z: min_z = z
                if z > max_z: max_z = z
            
            for (x, y, z) in [(xs + xt, ys + yt, zs + zt), (xe + xt, ye + yt, ze + zt)]:
                if x < min_xt: min_xt = x
                if x > max_xt: max_xt = x
                if y < min_yt: min_yt = y
                if y > max_yt: max_yt = y
                if z < min_zt: min_zt = z
                if z > max_zt: max_zt = z
    
    if min_x == INF:
        min_x = min_y = min_z = max_x = max_y = max_z = 0
    if min_xt == INF:
        min_xt = min_yt = min_zt = max_xt = max_yt = max_zt = 0
    
    return (
        [min_x, min_y, min_z], [max_x, max_y, max_z],
        [min_xt, min_yt, min_zt], [max_xt, max_yt, max_zt]
    )


# ==================== 纯 Python 实现的圆弧转线段 ====================
def arcs_to_segments_python(
    canon_obj: Any,
    x1: float, y1: float,
    cx: float, cy: float,
    rot: int,
    z1: float,
    a: float, b: float, c: float,
    u: float, v: float, w: float,
    max_segments: int = 128
) -> List[Tuple[float, ...]]:
    """
    圆弧转线段 - 纯 Python 实现，替代 gcode.arc_to_segments
    """
    if hasattr(canon_obj, 'lo'):
        o = list(canon_obj.lo)
    else:
        o = [0.0] * 9
    
    while len(o) < 9:
        o.append(0.0)
    
    plane = getattr(canon_obj, 'plane', 17)
    
    rotation_cos = getattr(canon_obj, 'rotation_cos', 1.0)
    rotation_sin = getattr(canon_obj, 'rotation_sin', 0.0)
    
    g5x_offset = [
        getattr(canon_obj, 'g5x_offset_x', 0.0),
        getattr(canon_obj, 'g5x_offset_y', 0.0),
        getattr(canon_obj, 'g5x_offset_z', 0.0),
        getattr(canon_obj, 'g5x_offset_a', 0.0),
        getattr(canon_obj, 'g5x_offset_b', 0.0),
        getattr(canon_obj, 'g5x_offset_c', 0.0),
        getattr(canon_obj, 'g5x_offset_u', 0.0),
        getattr(canon_obj, 'g5x_offset_v', 0.0),
        getattr(canon_obj, 'g5x_offset_w', 0.0)
    ]
    
    g92_offset = [
        getattr(canon_obj, 'g92_offset_x', 0.0),
        getattr(canon_obj, 'g92_offset_y', 0.0),
        getattr(canon_obj, 'g92_offset_z', 0.0),
        getattr(canon_obj, 'g92_offset_a', 0.0),
        getattr(canon_obj, 'g92_offset_b', 0.0),
        getattr(canon_obj, 'g92_offset_c', 0.0),
        getattr(canon_obj, 'g92_offset_u', 0.0),
        getattr(canon_obj, 'g92_offset_v', 0.0),
        getattr(canon_obj, 'g92_offset_w', 0.0)
    ]
    
    if plane == 17:
        X, Y, Z = 0, 1, 2
    elif plane == 19:
        X, Y, Z = 2, 0, 1
    else:
        X, Y, Z = 1, 2, 0
    
    n = [0.0] * 9
    n[X] = x1
    n[Y] = y1
    n[Z] = z1
    n[3] = a
    n[4] = b
    n[5] = c
    n[6] = u
    n[7] = v
    n[8] = w
    
    for ax in range(9):
        o[ax] -= g5x_offset[ax]
    
    tx = o[X]
    ty = o[Y]
    o[X] = tx * rotation_cos + ty * rotation_sin
    o[Y] = -tx * rotation_sin + ty * rotation_cos
    
    for ax in range(9):
        o[ax] -= g92_offset[ax]
    
    theta1 = math.atan2(o[Y] - cy, o[X] - cx)
    theta2 = math.atan2(n[Y] - cy, n[X] - cx)
    len_arc = math.hypot(o[X] - n[X], o[Y] - n[Y])
    
    if rot < 0:
        if theta1 < theta2:
            theta2 -= 2.0 * math.pi
        if len_arc < CART_FUZZ:
            theta2 -= 2.0 * math.pi
    else:
        if theta1 > theta2:
            theta2 += 2.0 * math.pi
        if len_arc < CART_FUZZ:
            theta2 += 2.0 * math.pi
    
    if rot < -1:
        theta2 += 2.0 * math.pi * (rot + 1)
    if rot > 1:
        theta2 += 2.0 * math.pi * (rot - 1)
    
    delta_theta = theta2 - theta1
    steps = max(3, int(max_segments * abs(delta_theta) / math.pi))
    rsteps = 1.0 / steps
    
    d = [0.0] * 9
    d[Z] = n[Z] - o[Z]
    d[3] = n[3] - o[3]
    d[4] = n[4] - o[4]
    d[5] = n[5] - o[5]
    d[6] = n[6] - o[6]
    d[7] = n[7] - o[7]
    d[8] = n[8] - o[8]
    
    segs = []
    tx = o[X] - cx
    ty = o[Y] - cy
    dc = math.cos(delta_theta * rsteps)
    ds = math.sin(delta_theta * rsteps)
    
    for i in range(steps - 1):
        f = (i + 1) * rsteps
        new_tx = tx * dc - ty * ds
        new_ty = tx * ds + ty * dc
        tx = new_tx
        ty = new_ty
        
        p = [0.0] * 9
        p[X] = tx + cx
        p[Y] = ty + cy
        p[Z] = o[Z] + d[Z] * f
        p[3] = o[3] + d[3] * f
        p[4] = o[4] + d[4] * f
        p[5] = o[5] + d[5] * f
        p[6] = o[6] + d[6] * f
        p[7] = o[7] + d[7] * f
        p[8] = o[8] + d[8] * f
        
        for ax in range(9):
            p[ax] += g92_offset[ax]
        
        px = p[X]
        py = p[Y]
        p[X] = px * rotation_cos - py * rotation_sin
        p[Y] = px * rotation_sin + py * rotation_cos
        
        for ax in range(9):
            p[ax] += g5x_offset[ax]
        
        segs.append(tuple(p))
    
    for ax in range(9):
        n[ax] += g92_offset[ax]
    
    nx = n[X]
    ny = n[Y]
    n[X] = nx * rotation_cos - ny * rotation_sin
    n[Y] = nx * rotation_sin + ny * rotation_cos
    
    for ax in range(9):
        n[ax] += g5x_offset[ax]
    
    segs.append(tuple(n))
    
    return segs


# ==================== 单位转换函数 ====================
def convert_units(value: float, from_units: int, to_units: int) -> float:
    """单位转换"""
    if from_units == to_units:
        return value
    if from_units == 20 and to_units == 21:  # 英寸 -> 毫米
        return value * MM_PER_INCH
    if from_units == 21 and to_units == 20:  # 毫米 -> 英寸
        return value * INCH_PER_MM
    return value


def program_to_user_len(value: float, units: int) -> float:
    """程序单位 -> 用户单位（长度）"""
    return value


def user_to_program_len(value: float, units: int) -> float:
    """用户单位 -> 程序单位（长度）"""
    return value


def program_to_user_ang(value: float) -> float:
    """程序单位 -> 用户单位（角度）"""
    return value


def user_to_program_ang(value: float) -> float:
    """用户单位 -> 程序单位（角度）"""
    return value


# ==================== JSON配置数据类 ====================
@dataclass
class ProgramConfig:
    """程序配置"""
    name: str = ""
    description: str = ""
    content: str = ""


@dataclass
class SubroutineConfig:
    """子程序配置"""
    name: str = ""
    description: str = ""
    content: str = ""
    parameters: List[Dict[str, Any]] = field(default_factory=list)


@dataclass
class MachineConfig:
    """机床配置数据类"""
    name: str = ""
    type: str = "5axis_mill"
    kinematics_type: str = "TRT_BC"
    kinematics_params: Dict[str, Any] = field(default_factory=dict)
    axes: Dict[str, Dict[str, float]] = field(default_factory=dict)
    spindle: Dict[str, Any] = field(default_factory=dict)
    tool_table: List[Dict[str, Any]] = field(default_factory=list)
    work_offsets: Dict[str, Dict[str, float]] = field(default_factory=dict)


@dataclass
class SimulationConfig:
    """仿真配置"""
    acceleration: float = 500.0
    deceleration: float = 500.0
    max_rapid_rate: float = 10000.0
    max_feed_rate: float = 5000.0
    arc_division: int = 64
    enable_rtcp_debug: bool = False
    max_points: int = 50000
    colors: Dict[str, str] = field(default_factory=dict)
    rapid_override: float = 1.0
    feed_override: float = 1.0
    spindle_override: float = 1.0


@dataclass
class HALPinConfig:
    """HAL引脚配置"""
    name: str = ""
    pin_type: str = "float"
    direction: str = "in"
    default: Any = 0.0
    min_val: Optional[float] = None
    max_val: Optional[float] = None
    description: str = ""


@dataclass
class HALConfig:
    """HAL配置数据类"""
    version: str = "1.0"
    description: str = ""
    pins: Dict[str, HALPinConfig] = field(default_factory=dict)
    connections: List[Dict[str, str]] = field(default_factory=list)


@dataclass
class VisualizationConfig:
    """可视化配置"""
    camera: Dict[str, Any] = field(default_factory=dict)
    grid: Dict[str, Any] = field(default_factory=dict)
    show_tool: bool = True
    show_path: bool = True
    show_coord_systems: bool = True
    show_workpiece: Dict[str, Any] = field(default_factory=dict)
    show_grid: bool = True
    show_axes: bool = True


@dataclass
class ProjectConfig:
    """项目完整配置"""
    project: Dict[str, str] = field(default_factory=dict)
    machine: MachineConfig = field(default_factory=MachineConfig)
    hal_config: HALConfig = field(default_factory=HALConfig)
    main_program: ProgramConfig = field(default_factory=ProgramConfig)
    subroutines: List[SubroutineConfig] = field(default_factory=list)
    simulation_config: SimulationConfig = field(default_factory=SimulationConfig)
    visualization: VisualizationConfig = field(default_factory=VisualizationConfig)
    execution_order: List[Dict[str, str]] = field(default_factory=list)


# ==================== 运动学抽象基类 ====================

@dataclass
class KinematicsParams:
    """运动学参数基类"""
    pass


class BaseKinematics(ABC):
    """运动学抽象基类"""
    
    def __init__(self, params: KinematicsParams = None):
        self.params = params
        self.rtcp_enabled = True
        self.debug = False
        self._tool_length = 0.0
    
    @abstractmethod
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        """
        正运动学：关节坐标 -> 世界坐标 (刀尖点)
        参数: joints = [轴1, 轴2, ...]
        返回: (X, Y, Z, A, B, C)
        """
        pass
    
    @abstractmethod
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        """
        逆运动学：世界坐标 (刀尖点) -> 关节坐标
        参数: world = (X, Y, Z, A, B, C)
        返回: [轴1, 轴2, ...]
        """
        pass
    
    def set_tool_length(self, length: float):
        """设置刀具长度 (用于 RTCP)"""
        self._tool_length = length
    
    def get_tool_length(self) -> float:
        """获取刀具长度"""
        return self._tool_length
    
    def enable_rtcp(self, enable: bool = True):
        """启用/禁用 RTCP"""
        self.rtcp_enabled = enable
    
    def set_params(self, **kwargs):
        """更新运动学参数"""
        if self.params:
            for key, value in kwargs.items():
                if hasattr(self.params, key):
                    setattr(self.params, key, value)
    
    def joints_to_world(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        """关节坐标转世界坐标 (别名)"""
        if not self.rtcp_enabled:
            if len(joints) >= 6:
                return (joints[0], joints[1], joints[2], joints[3], joints[4], joints[5])
            return (joints[0], joints[1], joints[2], 0.0, 0.0, 0.0)
        return self.forward(joints)
    
    def world_to_joints(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        """世界坐标转关节坐标 (别名)"""
        if not self.rtcp_enabled:
            result = [world[0], world[1], world[2], world[3], world[4], world[5]]
            while len(result) < EMCMOT_MAX_JOINTS:
                result.append(0.0)
            return result
        result = self.inverse(world)
        while len(result) < EMCMOT_MAX_JOINTS:
            result.append(0.0)
        return result


# ==================== 运动学类型枚举 ====================

class KinematicsType(Enum):
    """五轴运动学类型 - 基于LinuxCNC实现"""
    IDENTITY = 0
    TRT_AC = 1
    TRT_BC = 2
    MAXKINS_BC = 3
    FIVEAXIS_BC = 4
    HEXAPOD = 5
    SERIAL_DH = 6
    PUMA = 7
    SCARA = 8
    SCORBOT = 9
    LINEAR_DELTA = 10
    ROTARY_DELTA = 11
    TRIPOD = 12
    PENTAPOD = 13
    ROTATE = 14
    COREXY = 15
    ROSE = 16


# 运动学关节配置
KINEMATICS_JOINT_CONFIG = {
    KinematicsType.IDENTITY: {
        'num_joints': 6,
        'joint_names': ['X', 'Y', 'Z', 'A', 'B', 'C'],
        'x_idx': 0, 'y_idx': 1, 'z_idx': 2,
        'a_idx': 3, 'b_idx': 4, 'c_idx': 5,
    },
    KinematicsType.TRT_AC: {
        'num_joints': 5,
        'joint_names': ['X', 'Y', 'Z', 'A', 'C'],
        'x_idx': 0, 'y_idx': 1, 'z_idx': 2,
        'a_idx': 3, 'b_idx': -1, 'c_idx': 4,
    },
    KinematicsType.TRT_BC: {
        'num_joints': 5,
        'joint_names': ['X', 'Y', 'Z', 'B', 'C'],
        'x_idx': 0, 'y_idx': 1, 'z_idx': 2,
        'a_idx': -1, 'b_idx': 3, 'c_idx': 4,
    },
    KinematicsType.MAXKINS_BC: {
        'num_joints': 9,
        'joint_names': ['X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'],
        'x_idx': 0, 'y_idx': 1, 'z_idx': 2,
        'a_idx': 3, 'b_idx': 4, 'c_idx': 5,
    },
    KinematicsType.FIVEAXIS_BC: {
        'num_joints': 6,
        'joint_names': ['X', 'Y', 'Z', 'B', 'C', 'W'],
        'x_idx': 0, 'y_idx': 1, 'z_idx': 2,
        'a_idx': -1, 'b_idx': 3, 'c_idx': 4,
    },
    KinematicsType.HEXAPOD: {
        'num_joints': 6,
        'joint_names': ['S1', 'S2', 'S3', 'S4', 'S5', 'S6'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': -1,
        'a_idx': -1, 'b_idx': -1, 'c_idx': -1,
    },
    KinematicsType.PUMA: {
        'num_joints': 6,
        'joint_names': ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': -1,
        'a_idx': -1, 'b_idx': -1, 'c_idx': -1,
    },
    KinematicsType.SCARA: {
        'num_joints': 4,
        'joint_names': ['J1', 'J2', 'Z', 'J4'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': 2,
        'a_idx': -1, 'b_idx': -1, 'c_idx': 3,
    },
    KinematicsType.SCORBOT: {
        'num_joints': 5,
        'joint_names': ['J0', 'J1', 'J2', 'J3', 'J4'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': -1,
        'a_idx': 3, 'b_idx': 4, 'c_idx': -1,
    },
    KinematicsType.LINEAR_DELTA: {
        'num_joints': 3,
        'joint_names': ['Z1', 'Z2', 'Z3'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': -1,
        'a_idx': -1, 'b_idx': -1, 'c_idx': -1,
    },
    KinematicsType.ROTARY_DELTA: {
        'num_joints': 3,
        'joint_names': ['T1', 'T2', 'T3'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': -1,
        'a_idx': -1, 'b_idx': -1, 'c_idx': -1,
    },
    KinematicsType.TRIPOD: {
        'num_joints': 3,
        'joint_names': ['L1', 'L2', 'L3'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': -1,
        'a_idx': -1, 'b_idx': -1, 'c_idx': -1,
    },
    KinematicsType.PENTAPOD: {
        'num_joints': 5,
        'joint_names': ['L1', 'L2', 'L3', 'L4', 'L5'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': -1,
        'a_idx': -1, 'b_idx': -1, 'c_idx': -1,
    },
    KinematicsType.ROTATE: {
        'num_joints': 9,
        'joint_names': ['X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'],
        'x_idx': 0, 'y_idx': 1, 'z_idx': 2,
        'a_idx': 3, 'b_idx': 4, 'c_idx': 5,
    },
    KinematicsType.COREXY: {
        'num_joints': 9,
        'joint_names': ['X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'],
        'x_idx': 0, 'y_idx': 1, 'z_idx': 2,
        'a_idx': 3, 'b_idx': 4, 'c_idx': 5,
    },
    KinematicsType.ROSE: {
        'num_joints': 3,
        'joint_names': ['R', 'Z', 'Theta'],
        'x_idx': -1, 'y_idx': -1, 'z_idx': 1,
        'a_idx': -1, 'b_idx': -1, 'c_idx': 2,
    },
}


class MoveType(IntEnum):
    """运动类型枚举"""
    RAPID = 0
    FEED = 1
    ARC_CW = 2
    ARC_CCW = 3
    DWELL = 4
    PROBE = 5
    RTCP_ON = 6
    RTCP_OFF = 7
    PROGRAM_END = 8
    TOOL_CHANGE = 9
    SPINDLE_ON = 10
    SPINDLE_OFF = 11
    COOLANT_ON = 12
    COOLANT_OFF = 13
    COMMENT = 14
    MESSAGE = 15
    USER_DEFINED = 16
    WAIT = 17


class CompType(IntEnum):
    """刀具补偿类型"""
    OFF = 0
    LEFT = 1
    RIGHT = 2
    DYNAMIC = 3


class MachineState(Enum):
    """机床状态枚举"""
    OFF = "OFF"
    RESET = "RESET"
    ESTOP = "ESTOP"
    IDLE = "IDLE"
    RUNNING = "RUNNING"
    PAUSED = "PAUSED"
    HOLDING = "HOLDING"
    JOGGING = "JOGGING"
    HOMING = "HOMING"
    ALARM = "ALARM"
    MDI = "MDI"
    PROBING = "PROBING"
    TOOL_CHANGING = "TOOL_CHANGING"


class SpindleState(Enum):
    """主轴状态"""
    OFF = "OFF"
    CW = "CW"
    CCW = "CCW"


class CoolantState(Enum):
    """冷却液状态"""
    OFF = "OFF"
    FLOOD = "FLOOD"
    MIST = "MIST"
    BOTH = "BOTH"


class FeedMode(Enum):
    """进给模式"""
    UNITS_PER_MINUTE = 94
    UNITS_PER_REVOLUTION = 95
    INVERSE_TIME = 93


class DistanceMode(Enum):
    """距离模式"""
    ABSOLUTE = 90
    INCREMENTAL = 91


class PlaneMode(Enum):
    """平面模式"""
    XY = 17
    XZ = 18
    YZ = 19
    UV = 171
    UW = 181
    VW = 191


class UnitsMode(Enum):
    """单位模式"""
    INCHES = 20
    MM = 21


@dataclass
class Point6D:
    """六维点 (X, Y, Z, A, B, C)"""
    x: float = 0.0
    y: float = 0.0
    z: float = 0.0
    a: float = 0.0
    b: float = 0.0
    c: float = 0.0
    u: float = 0.0
    v: float = 0.0
    w: float = 0.0
    
    def to_list(self) -> List[float]:
        return [self.x, self.y, self.z, self.a, self.b, self.c, self.u, self.v, self.w]
    
    def to_tuple(self) -> Tuple[float, ...]:
        return (self.x, self.y, self.z, self.a, self.b, self.c, self.u, self.v, self.w)
    
    def to_xyz_tuple(self) -> Tuple[float, float, float]:
        return (self.x, self.y, self.z)
    
    def to_xyzabc_tuple(self) -> Tuple[float, float, float, float, float, float]:
        return (self.x, self.y, self.z, self.a, self.b, self.c)
    
    def distance_to(self, other: 'Point6D') -> float:
        dx = self.x - other.x
        dy = self.y - other.y
        dz = self.z - other.z
        return math.sqrt(dx*dx + dy*dy + dz*dz)
    
    def distance_to_6d(self, other: 'Point6D') -> float:
        """计算六维距离（包含旋转轴）"""
        dx = self.x - other.x
        dy = self.y - other.y
        dz = self.z - other.z
        da = self.a - other.a
        db = self.b - other.b
        dc = self.c - other.c
        return math.sqrt(dx*dx + dy*dy + dz*dz + da*da + db*db + dc*dc)
    
    def copy(self) -> 'Point6D':
        return Point6D(self.x, self.y, self.z, self.a, self.b, self.c, self.u, self.v, self.w)
    
    def to_dict(self) -> Dict[str, float]:
        return {'x': self.x, 'y': self.y, 'z': self.z, 
                'a': self.a, 'b': self.b, 'c': self.c,
                'u': self.u, 'v': self.v, 'w': self.w}
    
    @classmethod
    def from_list(cls, lst: List[float]) -> 'Point6D':
        if len(lst) >= 9:
            return cls(lst[0], lst[1], lst[2], lst[3], lst[4], lst[5], lst[6], lst[7], lst[8])
        elif len(lst) >= 6:
            return cls(lst[0], lst[1], lst[2], lst[3], lst[4], lst[5], 0, 0, 0)
        elif len(lst) >= 3:
            return cls(lst[0], lst[1], lst[2], 0, 0, 0, 0, 0, 0)
        return cls()
    
    @classmethod
    def from_xyzabc(cls, x: float, y: float, z: float, a: float, b: float, c: float) -> 'Point6D':
        return cls(x, y, z, a, b, c, 0, 0, 0)
    
    def __add__(self, other: 'Point6D') -> 'Point6D':
        return Point6D(
            self.x + other.x, self.y + other.y, self.z + other.z,
            self.a + other.a, self.b + other.b, self.c + other.c,
            self.u + other.u, self.v + other.v, self.w + other.w
        )
    
    def __sub__(self, other: 'Point6D') -> 'Point6D':
        return Point6D(
            self.x - other.x, self.y - other.y, self.z - other.z,
            self.a - other.a, self.b - other.b, self.c - other.c,
            self.u - other.u, self.v - other.v, self.w - other.w
        )
    
    def __mul__(self, scalar: float) -> 'Point6D':
        return Point6D(
            self.x * scalar, self.y * scalar, self.z * scalar,
            self.a * scalar, self.b * scalar, self.c * scalar,
            self.u * scalar, self.v * scalar, self.w * scalar
        )
    
    def __truediv__(self, scalar: float) -> 'Point6D':
        if scalar == 0:
            return self.copy()
        return Point6D(
            self.x / scalar, self.y / scalar, self.z / scalar,
            self.a / scalar, self.b / scalar, self.c / scalar,
            self.u / scalar, self.v / scalar, self.w / scalar
        )
    
    def __eq__(self, other: 'Point6D') -> bool:
        if not isinstance(other, Point6D):
            return False
        return (abs(self.x - other.x) < TOLERANCE_EQUAL and
                abs(self.y - other.y) < TOLERANCE_EQUAL and
                abs(self.z - other.z) < TOLERANCE_EQUAL and
                abs(self.a - other.a) < TOLERANCE_EQUAL and
                abs(self.b - other.b) < TOLERANCE_EQUAL and
                abs(self.c - other.c) < TOLERANCE_EQUAL)
    
    def __str__(self) -> str:
        return f"Point6D(x={self.x:.3f}, y={self.y:.3f}, z={self.z:.3f}, a={self.a:.3f}, b={self.b:.3f}, c={self.c:.3f})"
    
    def __repr__(self) -> str:
        return self.__str__()


@dataclass
class ToolData:
    """刀具数据 - 完整版"""
    tool_number: int = 0
    pocket: int = 0
    diameter: float = 0.0
    radius: float = 0.0
    length_offset: float = 0.0
    offset: Point6D = field(default_factory=Point6D)
    front_angle: float = 0.0
    back_angle: float = 0.0
    orientation: int = 0
    name: str = ""
    comment: str = ""
    
    def __post_init__(self):
        if self.diameter > 0 and self.radius == 0:
            self.radius = self.diameter / 2.0
        elif self.radius > 0 and self.diameter == 0:
            self.diameter = self.radius * 2.0


@dataclass
class MoveSegment:
    """运动段数据"""
    type: MoveType
    start: Point6D
    end: Point6D
    line_number: int = 0
    feedrate: float = 0.0
    center_x: float = 0.0
    center_y: float = 0.0
    center_z: float = 0.0
    radius: float = 0.0
    turn: int = 0
    dwell_time: float = 0.0
    comp_type: CompType = CompType.OFF
    comp_radius: float = 0.0
    d_word: float = 0.0
    duration: float = 0.0
    max_velocity: float = 0.0
    acceleration_time: float = 0.0
    constant_time: float = 0.0
    deceleration_time: float = 0.0
    profile_type: str = ""
    is_rtcp: bool = False
    world_start: Optional[Point6D] = None
    world_end: Optional[Point6D] = None
    gcode: str = ""
    machine_start: Optional[Point6D] = None    # 绝对坐标起点
    machine_end: Optional[Point6D] = None      # 绝对坐标终点
    tool_number: int = 0
    spindle_speed: float = 0.0
    spindle_state: str = ""
    coolant_state: str = ""
    comment: str = ""
    message: str = ""
    probe_tripped: bool = False
    
    # ===== 新增字段 =====
    coord_system: str = "WORLD"  # "WORLD" | "ABSOLUTE" | "MACHINE"
    active_csys: int = 1         # 当前激活的坐标系 (1=G54, 2=G55, ...)
    g92_active: bool = False     # G92 是否激活    
    
    def __post_init__(self):
        """初始化后处理：确保坐标完整性"""
        # 如果 world_start 为空，使用 start 作为默认值
        if self.world_start is None:
            self.world_start = self.start.copy() if self.start else None
        
        # 如果 world_end 为空，使用 end 作为默认值
        if self.world_end is None:
            self.world_end = self.end.copy() if self.end else None
        
        # 确保圆弧段有正确的圆心信息
        if self.type in [MoveType.ARC_CW, MoveType.ARC_CCW]:
            if self.radius == 0 and self.world_start and self.center_x:
                self.radius = math.hypot(
                    self.world_start.x - self.center_x,
                    self.world_start.y - self.center_y
                )    
    
    def get_color(self, colors: Dict[str, str] = None) -> str:
        """获取运动段的显示颜色"""
        if colors is None:
            colors = DEFAULT_COLORS
        
        if self.type == MoveType.RAPID:
            return colors.get('rapid', '#00ff00')
        elif self.type == MoveType.FEED:
            return colors.get('feed', '#ffff00')
        elif self.type == MoveType.ARC_CW:
            return colors.get('arc_cw', '#00ffff')
        elif self.type == MoveType.ARC_CCW:
            return colors.get('arc_ccw', '#ff00ff')
        elif self.type == MoveType.DWELL:
            return colors.get('dwell', '#ffffff')
        elif self.type == MoveType.PROBE:
            return colors.get('probe', '#ff8000')
        elif self.type == MoveType.RTCP_ON:
            return colors.get('rtcp_on', '#00ff88')
        elif self.type == MoveType.RTCP_OFF:
            return colors.get('rtcp_off', '#ff6600')
        elif self.type == MoveType.PROGRAM_END:
            return colors.get('program_end', '#ff0000')
        else:
            return '#ffffff'



    # ===== 新增：坐标系兼容属性 =====
    
    @property
    def effective_start(self) -> Point6D:
        """获取有效的起点（始终返回工件坐标）"""
        if self.world_start is not None:
            return self.world_start
        return self.start
    
    @property
    def effective_end(self) -> Point6D:
        """获取有效的终点（始终返回工件坐标）"""
        if self.world_end is not None:
            return self.world_end
        return self.end
    
    def get_start_in_world(self) -> Point6D:
        """获取工件坐标系下的起点（兼容方法）"""
        if self.world_start is not None:
            return self.world_start
        # 向后兼容：如果没有 world 坐标，假定 start 就是工件坐标
        return self.start
    
    def get_end_in_world(self) -> Point6D:
        """获取工件坐标系下的终点（兼容方法）"""
        if self.world_end is not None:
            return self.world_end
        return self.end
    
    def get_start_in_machine(self) -> Point6D:
        """获取绝对坐标系下的起点"""
        if self.machine_start is not None:
            return self.machine_start
        return self.start
    
    def get_end_in_machine(self) -> Point6D:
        """获取绝对坐标系下的终点"""
        if self.machine_end is not None:
            return self.machine_end
        return self.end
    
    def has_world_coords(self) -> bool:
        """是否有工件坐标"""
        return self.world_start is not None and self.world_end is not None
    
    def has_machine_coords(self) -> bool:
        """是否有绝对坐标"""
        return self.machine_start is not None and self.machine_end is not None
    
    def get_arc_info_in_world(self) -> Dict[str, float]:
        """
        获取工件坐标系下的圆弧信息
        用于几何验证和可视化
        """
        start_pt = self.get_start_in_world()
        end_pt = self.get_end_in_world()
        return {
            'start_x': start_pt.x,
            'start_y': start_pt.y,
            'start_z': start_pt.z,
            'end_x': end_pt.x,
            'end_y': end_pt.y,
            'end_z': end_pt.z,
            'center_x': self.center_x,
            'center_y': self.center_y,
            'center_z': self.center_z,
            'radius': self.radius,
            'turn': self.turn,
        }






@dataclass
class Subroutine:
    """子程序定义"""
    name: str
    lines: List[str] = field(default_factory=list)
    line_numbers: List[int] = field(default_factory=list)
    start_line: int = 0
    parameters: List[str] = field(default_factory=list)
    description: str = ""
    source_file: str = ""


@dataclass
class ToolpathData:
    """刀具路径数据"""
    segments: List[MoveSegment] = field(default_factory=list)
    bounds: Dict[str, float] = field(default_factory=dict)
    total_length: float = 0.0
    total_time: float = 0.0
    point_count: int = 0
    generation_time_ms: float = 0.0
    filename: str = ""
    tool_changes: List[Dict] = field(default_factory=list)
    tool_table: Dict[int, ToolData] = field(default_factory=dict)
    subroutines: Dict[str, Subroutine] = field(default_factory=dict)
    variables: Dict[int, float] = field(default_factory=dict)
    named_variables: Dict[str, float] = field(default_factory=dict)
    rtcp_enabled: bool = False
    kinematics_type: str = ""
    program_comments: List[str] = field(default_factory=list)
    program_messages: List[str] = field(default_factory=list)
    errors: List[str] = field(default_factory=list)
    warnings: List[str] = field(default_factory=list)
    
    def calculate_bounds(self):
        if not self.segments:
            return
        
        min_x = min_y = min_z = min_a = min_b = min_c = float('inf')
        max_x = max_y = max_z = max_a = max_b = max_c = float('-inf')
        
        for seg in self.segments:
            for point in [seg.start, seg.end]:
                min_x = min(min_x, point.x)
                max_x = max(max_x, point.x)
                min_y = min(min_y, point.y)
                max_y = max(max_y, point.y)
                min_z = min(min_z, point.z)
                max_z = max(max_z, point.z)
                min_a = min(min_a, point.a)
                max_a = max(max_a, point.a)
                min_b = min(min_b, point.b)
                max_b = max(max_b, point.b)
                min_c = min(min_c, point.c)
                max_c = max(max_c, point.c)
        
        self.bounds = {
            'minX': min_x if min_x != float('inf') else 0,
            'maxX': max_x if max_x != float('-inf') else 0,
            'minY': min_y if min_y != float('inf') else 0,
            'maxY': max_y if max_y != float('-inf') else 0,
            'minZ': min_z if min_z != float('inf') else 0,
            'maxZ': max_z if max_z != float('-inf') else 0,
            'minA': min_a if min_a != float('inf') else 0,
            'maxA': max_a if max_a != float('-inf') else 0,
            'minB': min_b if min_b != float('inf') else 0,
            'maxB': max_b if max_b != float('-inf') else 0,
            'minC': min_c if min_c != float('inf') else 0,
            'maxC': max_c if max_c != float('-inf') else 0,
            'width': (max_x - min_x) if max_x != float('-inf') else 0,
            'height': (max_y - min_y) if max_y != float('-inf') else 0,
            'depth': (max_z - min_z) if max_z != float('-inf') else 0,
        }
    
    def calculate_length_and_time(self) -> Tuple[float, float]:
        total = 0.0
        total_time = 0.0
        for seg in self.segments:
            if seg.type in [MoveType.RAPID, MoveType.FEED]:
                total += seg.start.distance_to(seg.end)
            elif seg.type in [MoveType.ARC_CW, MoveType.ARC_CCW]:
                total += abs(seg.turn) * seg.radius * math.pi
            total_time += seg.duration
        self.total_length = total
        self.total_time = total_time
        return total, total_time
    
    def get_statistics(self) -> Dict[str, Any]:
        return {
            'total_segments': len(self.segments),
            'rapid_moves': sum(1 for s in self.segments if s.type == MoveType.RAPID),
            'feed_moves': sum(1 for s in self.segments if s.type == MoveType.FEED),
            'arc_moves': sum(1 for s in self.segments if s.type in [MoveType.ARC_CW, MoveType.ARC_CCW]),
            'dwells': sum(1 for s in self.segments if s.type == MoveType.DWELL),
            'probes': sum(1 for s in self.segments if s.type == MoveType.PROBE),
            'rtcp_on_count': sum(1 for s in self.segments if s.type == MoveType.RTCP_ON),
            'rtcp_off_count': sum(1 for s in self.segments if s.type == MoveType.RTCP_OFF),
            'compensated_segments': sum(1 for s in self.segments if s.comp_type != CompType.OFF),
            'tool_changes': len(self.tool_changes),
            'subroutines': len(self.subroutines),
            'variables': len(self.variables),
            'named_variables': len(self.named_variables),
            'rtcp_enabled': self.rtcp_enabled,
            'kinematics_type': self.kinematics_type,
            'errors': len(self.errors),
            'warnings': len(self.warnings),
        }
    
    def get_segments_by_type(self, move_type: MoveType) -> List[MoveSegment]:
        """获取指定类型的运动段"""
        return [s for s in self.segments if s.type == move_type]
    
    def get_segments_by_tool(self, tool_number: int) -> List[MoveSegment]:
        """获取指定刀具的运动段"""
        return [s for s in self.segments if s.tool_number == tool_number]
    
    def get_segments_by_line(self, line_number: int) -> List[MoveSegment]:
        """获取指定行号的运动段"""
        return [s for s in self.segments if s.line_number == line_number]
    
    def add_error(self, error: str):
        """添加错误"""
        self.errors.append(error)
    
    def add_warning(self, warning: str):
        """添加警告"""
        self.warnings.append(warning)
    
    def add_comment(self, comment: str):
        """添加注释"""
        self.program_comments.append(comment)
    
    def add_message(self, message: str):
        """添加消息"""
        self.program_messages.append(message)



# ==================== 关节坐标映射工具 ====================

def joints_to_point6d(joints: List[float], kinematics_type: KinematicsType) -> Point6D:
    """
    根据运动学类型的关节配置，将关节数组转换为 Point6D
    
    参数:
        joints: 关节坐标数组
        kinematics_type: 运动学类型
    
    返回:
        Point6D，其中 x/y/z/a/b/c 根据运动学配置正确映射
    """
    config = KINEMATICS_JOINT_CONFIG.get(kinematics_type, 
                                          KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY])
    
    def safe_get(idx: int, default: float = 0.0) -> float:
        if idx >= 0 and idx < len(joints):
            return joints[idx]
        return default
    
    return Point6D(
        safe_get(config.get('x_idx', 0), 0.0),
        safe_get(config.get('y_idx', 1), 0.0),
        safe_get(config.get('z_idx', 2), 0.0),
        safe_get(config.get('a_idx', 3), 0.0),
        safe_get(config.get('b_idx', 4), 0.0),
        safe_get(config.get('c_idx', 5), 0.0),
    )


def point6d_to_joints(pos: Point6D, kinematics_type: KinematicsType) -> List[float]:
    """
    根据运动学类型的关节配置，将 Point6D 转换为关节数组
    
    参数:
        pos: Point6D 坐标
        kinematics_type: 运动学类型
    
    返回:
        关节坐标数组
    """
    config = KINEMATICS_JOINT_CONFIG.get(kinematics_type, 
                                          KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY])
    num_joints = config.get('num_joints', 6)
    joints = [0.0] * num_joints
    
    # 根据配置将 Point6D 的轴值放到正确位置
    axes_map = {
        'x': ('x_idx', pos.x),
        'y': ('y_idx', pos.y),
        'z': ('z_idx', pos.z),
        'a': ('a_idx', pos.a),
        'b': ('b_idx', pos.b),
        'c': ('c_idx', pos.c),
    }
    
    for key_name, (config_key, value) in axes_map.items():
        idx = config.get(config_key, -1)
        if idx >= 0 and idx < num_joints:
            joints[idx] = value
    
    return joints



# ==================== TRT 双转台运动学参数 ====================

@dataclass
class TRTParams(KinematicsParams):
    """双转台/摆头式五轴参数 (对应 trtfuncs.c 的 haldata)"""
    rot_center_x: float = 0.0
    rot_center_y: float = 0.0
    rot_center_z: float = 0.0
    axis_offset_x: float = 0.0
    axis_offset_y: float = 0.0
    axis_offset_z: float = 0.0
    tool_length: float = 0.0
    conventional_directions: bool = False
    pivot_length: float = 250.0


# ==================== XYZAC_TRTKinematics 类 ====================

class XYZAC_TRTKinematics(BaseKinematics):
    """
    XYZAC 双转台运动学 - 完全参照 LinuxCNC trtfuncs.c
    A 轴绕 X 轴倾斜，C 轴绕 Z 轴旋转。
    
    参数映射 (LinuxCNC -> JSON):
        rot_center_x/y/z = 旋转台回转中心在机床坐标系中的位置
        axis_offset_x/y/z = 直线轴相对于旋转台的安装偏置
        tool_length = 刀具长度补偿值
    """
    
    def __init__(self, params: TRTParams = None, debug: bool = False):
        super().__init__(params or TRTParams())
        self.debug = debug
        self.p = self.params
        
        # 同步刀具长度
        if hasattr(self.p, 'tool_length') and self.p.tool_length > 0:
            self._tool_length = self.p.tool_length
    
    def _get_con(self) -> float:
        """获取旋转方向因子"""
        return 1.0 if self.p.conventional_directions else -1.0
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        """
        正运动学：关节坐标 -> 世界坐标
        
        参照 LinuxCNC xyzacKinematicsForward()：
        1. 从关节坐标中减去旋转中心坐标，把坐标系原点移到旋转中心
        2. 再减去轴安装偏置（axis_offset）
        3. 乘上旋转矩阵（A 轴绕 X，C 轴绕 Z）
        4. 把旋转中心加回来，还原为机床坐标系下的刀尖位置
        
        关节顺序: [X, Y, Z, A, C]
        返回: (X, Y, Z, A, B, C) 刀尖点世界坐标
        """
        if len(joints) < 5:
            joints = list(joints) + [0.0] * (5 - len(joints))
        
        x = joints[0]  # 关节 X
        y = joints[1]  # 关节 Y
        z = joints[2]  # 关节 Z（主轴参考点）
        a = joints[3]  # A 轴角度（度）
        c = joints[4]  # C 轴角度（度）
        
        con = self._get_con()
        a_rad = a * TO_RAD
        c_rad = c * TO_RAD
        
        # 旋转中心 - 对应 LinuxCNC 的 x_rot_point, y_rot_point, z_rot_point
        rx = self.p.rot_center_x
        ry = self.p.rot_center_y
        rz = self.p.rot_center_z
        
        # 轴偏置 - 对应 LinuxCNC 的 x_offset, y_offset, z_offset
        dx = self.p.axis_offset_x
        dy = self.p.axis_offset_y
        dz = self.p.axis_offset_z + self._tool_length  # z_offset + tool_offset
        
        # ===== 正解计算（完全参照 LinuxCNC 官方代码）=====
        
        # 注意：dx (axis_offset_x) 在 AC 结构中用于 X 轴方向
        # 这里的关键是：从关节坐标中减去旋转中心和轴偏置，
        # 然后进行旋转变换，最后再加回旋转中心
        
        pos_x = (
            + math.cos(c_rad) * (x - dx - rx)
            - con * math.sin(c_rad) * math.cos(a_rad) * (y - dy - ry)
            + math.sin(c_rad) * math.sin(a_rad) * (z - dz - rz)
            - con * math.sin(c_rad) * dy
            + rx
        )
        
        pos_y = (
            + con * math.sin(c_rad) * (x - dx - rx)
            + math.cos(c_rad) * math.cos(a_rad) * (y - dy - ry)
            - con * math.cos(c_rad) * math.sin(a_rad) * (z - dz - rz)
            + math.cos(c_rad) * dy
            + ry
        )
        
        pos_z = (
            + con * math.sin(a_rad) * (y - dy - ry)
            + math.cos(a_rad) * (z - dz - rz)
            + dz
            + rz
        )
        
        if self.debug:
            print(f"[XYZAC FWD] joints(x={x:.3f},y={y:.3f},z={z:.3f},a={a:.3f},c={c:.3f}) "
                  f"-> world({pos_x:.3f},{pos_y:.3f},{pos_z:.3f})")
            print(f"[XYZAC FWD]   rot_center=({rx:.1f},{ry:.1f},{rz:.1f}) "
                  f"axis_offset=({dx:.1f},{dy:.1f},{dz:.1f})")
        
        return (pos_x, pos_y, pos_z, a, 0.0, c)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        """
        逆运动学：世界坐标 -> 关节坐标
        
        参照 LinuxCNC xyzacKinematicsInverse()：
        1. 把世界坐标减去旋转中心，把坐标系移到旋转中心
        2. 乘上逆旋转矩阵
        3. 加上轴偏置和旋转中心，还原成关节坐标
        
        输入: (X, Y, Z, A, B, C) 世界坐标
        返回: [X, Y, Z, A, C] 关节坐标
        """
        if len(world) < 6:
            world = list(world) + [0.0] * (6 - len(world))
        
        wx = world[0]  # 世界 X
        wy = world[1]  # 世界 Y
        wz = world[2]  # 世界 Z
        wa = world[3]  # A 轴角度
        wb = world[4]  # B 轴（不使用）
        wc = world[5]  # C 轴角度
        
        con = self._get_con()
        a_rad = wa * TO_RAD
        c_rad = wc * TO_RAD
        
        # 旋转中心
        rx = self.p.rot_center_x
        ry = self.p.rot_center_y
        rz = self.p.rot_center_z
        
        # 轴偏置
        dx = self.p.axis_offset_x
        dy = self.p.axis_offset_y
        dz = self.p.axis_offset_z + self._tool_length
        
        # ===== 逆解计算（完全参照 LinuxCNC 官方代码）=====
        
        # 第一步：先把世界坐标减去旋转中心，计算 P 向量
        Px = (
            + math.cos(c_rad) * (wx - rx)
            + con * math.sin(c_rad) * (wy - ry)
            + rx
        )
        
        Py = (
            - con * math.sin(c_rad) * math.cos(a_rad) * (wx - rx)
            + math.cos(c_rad) * math.cos(a_rad) * (wy - ry)
            + con * math.sin(a_rad) * (wz - rz)
            - math.cos(a_rad) * dy
            - con * math.sin(a_rad) * dz
            + dy
            + ry
        )
        
        Pz = (
            + math.sin(c_rad) * math.sin(a_rad) * (wx - rx)
            - con * math.cos(c_rad) * math.sin(a_rad) * (wy - ry)
            + math.cos(a_rad) * (wz - rz)
            + con * math.sin(a_rad) * dy
            - math.cos(a_rad) * dz
            + dz
            + rz
        )
        
        if self.debug:
            print(f"[XYZAC INV] world({wx:.3f},{wy:.3f},{wz:.3f},a={wa:.1f},c={wc:.1f}) "
                  f"-> joints({Px:.3f},{Py:.3f},{Pz:.3f})")
            print(f"[XYZAC INV]   rot_center=({rx:.1f},{ry:.1f},{rz:.1f}) "
                  f"axis_offset=({dx:.1f},{dy:.1f},{dz:.1f})")
        
        return [Px, Py, Pz, wa, wc]
    
    def set_tool_length(self, length: float):
        """设置刀具长度 - 确保参数同步"""
        self._tool_length = length
        if hasattr(self, 'p') and self.p is not None:
            self.p.tool_length = length
        if self.debug:
            print(f"[XYZAC TLO] set_tool_length({length}) -> "
                  f"_tool_length={self._tool_length}, "
                  f"p.tool_length={self.p.tool_length if hasattr(self.p, 'tool_length') else 'N/A'}")
            
            
# ==================== XYZBC_TRTKinematics 类 ====================

class XYZBC_TRTKinematics(BaseKinematics):
    """
    XYZBC 双转台运动学 (基于 trtfuncs.c 的 xyzbcKinematicsForward/Inverse)
    B 轴绕 Y 轴倾斜，C 轴绕 Z 轴旋转。
    """
    
    def __init__(self, params: TRTParams = None, debug: bool = False):
        super().__init__(params or TRTParams())
        self.debug = debug
        self.p = self.params
        # ★ 同样添加刀具长度同步 ★
        if hasattr(self.p, 'tool_length') and self.p.tool_length > 0:
            self._tool_length = self.p.tool_length
    
    def _get_con(self) -> float:
        """获取方向因子"""
        return 1.0 if self.p.conventional_directions else -1.0
    
    def set_params(self, **kwargs):
        """更新运动学参数"""
        supported = {
            'rot_center_x', 'rot_center_y', 'rot_center_z',
            'axis_offset_x', 'axis_offset_y', 'axis_offset_z',
            'tool_length', 'conventional_directions', 'pivot_length'
        }
        for key, value in kwargs.items():
            if key in supported and hasattr(self.p, key):
                setattr(self.p, key, value)
        
        if self.debug:
            print(f"[XYZBC_TRT] 参数更新: {kwargs}")
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        """
        正运动学：关节坐标 -> 世界坐标 (刀尖点)
        关节顺序: [X, Y, Z, B, C]
        返回: (X, Y, Z, A, B, C)
        """
        if len(joints) < 5:
            joints = list(joints) + [0.0] * (5 - len(joints))
        
        x, y, z, b, c = joints[0], joints[1], joints[2], joints[3], joints[4]
        
        con = self._get_con()
        b_rad = b * TO_RAD
        c_rad = c * TO_RAD
        
        rx = self.p.rot_center_x
        ry = self.p.rot_center_y
        rz = self.p.rot_center_z
        
        dx = self.p.axis_offset_x
        dz = self.p.axis_offset_z + self._tool_length
        
        pos_x = (
            + math.cos(c_rad) * math.cos(b_rad) * (x - dx - rx)
            - con * math.sin(c_rad) * (y - ry)
            + con * math.cos(c_rad) * math.sin(b_rad) * (z - dz - rz)
            + math.cos(c_rad) * dx
            + rx
        )
        
        pos_y = (
            + con * math.sin(c_rad) * math.cos(b_rad) * (x - dx - rx)
            + math.cos(c_rad) * (y - ry)
            + math.sin(c_rad) * math.sin(b_rad) * (z - dz - rz)
            + con * math.sin(c_rad) * dx
            + ry
        )
        
        pos_z = (
            - con * math.sin(b_rad) * (x - dx - rx)
            + math.cos(b_rad) * (z - dz - rz)
            + dz
            + rz
        )
        
        if self.debug:
            print(f"[XYZBC] FWD: joints({x:.3f},{y:.3f},{z:.3f},{b:.3f},{c:.3f}) -> world({pos_x:.3f},{pos_y:.3f},{pos_z:.3f})")
        
        return (pos_x, pos_y, pos_z, 0.0, b, c)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        """
        逆运动学：世界坐标 (刀尖点) -> 关节坐标
        输入: (X, Y, Z, A, B, C)
        返回: [X, Y, Z, B, C]
        """
        if len(world) < 6:
            world = list(world) + [0.0] * (6 - len(world))
        
        wx, wy, wz, wa, wb, wc = world[0], world[1], world[2], world[3], world[4], world[5]
        
        con = self._get_con()
        b_rad = wb * TO_RAD
        c_rad = wc * TO_RAD
        
        rx = self.p.rot_center_x
        ry = self.p.rot_center_y
        rz = self.p.rot_center_z
        
        dx = self.p.axis_offset_x
        dz = self.p.axis_offset_z + self._tool_length
        
        dpx = -math.cos(b_rad) * dx + math.sin(b_rad) * dz + dx
        dpz = -math.sin(b_rad) * dx - math.cos(b_rad) * dz + dz
        
        Px = (
            + math.cos(c_rad) * math.cos(b_rad) * (wx - rx)
            + con * math.sin(c_rad) * math.cos(b_rad) * (wy - ry)
            - con * math.sin(b_rad) * (wz - rz)
            + dpx
            + rx
        )
        
        Py = (
            - con * math.sin(c_rad) * (wx - rx)
            + math.cos(c_rad) * (wy - ry)
            + ry
        )
        
        Pz = (
            + con * math.cos(c_rad) * math.sin(b_rad) * (wx - rx)
            + math.sin(c_rad) * math.sin(b_rad) * (wy - ry)
            + math.cos(b_rad) * (wz - rz)
            + dpz
            + rz
        )
        
        if self.debug:
            print(f"[XYZBC] INV: world({wx:.3f},{wy:.3f},{wz:.3f},{wb:.3f},{wc:.3f}) -> joints({Px:.3f},{Py:.3f},{Pz:.3f})")
        
        return [Px, Py, Pz, wb, wc]
    
    def set_tool_length(self, length: float):
        self._tool_length = length
        self.p.tool_length = length
        if self.debug:
            print(f"[XYZBC] 刀具长度设置为: {length:.3f} mm")
    
    def set_pivot_length(self, length: float):
        """设置枢轴长度（为兼容性保留）"""
        self.p.pivot_length = length
        if self.debug:
            print(f"[XYZBC] 枢轴长度设置为: {length:.3f} mm")


# ==================== 五轴桥式铣床运动学 (5axiskins) ====================

class FiveAxisBCKinematics(BaseKinematics):
    """
    五轴桥式铣床运动学 (基于 5axiskins.c)
    采用球坐标描述刀具指向。
    """
    
    def __init__(self, pivot_length: float = 250.0, tool_length: float = 0.0, debug: bool = False):
        super().__init__()
        self.pivot_length = pivot_length
        self._tool_length = tool_length
        self.debug = debug
    
    def _s2r(self, r: float, t: float, p: float) -> Tuple[float, float, float]:
        """
        球坐标转直角坐标 (来自 5axiskins.c 的 s2r 函数)
        r = 向量长度
        p = phi = 向量与 Z 轴夹角 (度)
        t = theta = 向量在 XY 平面投影与 X 轴夹角 (度)
        """
        t_rad = t * TO_RAD
        p_rad = p * TO_RAD
        x = r * math.sin(p_rad) * math.cos(t_rad)
        y = r * math.sin(p_rad) * math.sin(t_rad)
        z = r * math.cos(p_rad)
        return (x, y, z)
    
    def _r2s(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
        """直角坐标转球坐标"""
        r = math.sqrt(x*x + y*y + z*z)
        if r < CART_FUZZ:
            return (0.0, 0.0, 0.0)
        p = math.acos(z / r) * TO_DEG
        t = math.atan2(y, x) * TO_DEG
        return (r, t, p)
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 6:
            joints = list(joints) + [0.0] * (6 - len(joints))
        
        x, y, z, b, c, w = joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]
        
        rx, ry, rz = self._s2r(self.pivot_length + w, c, 180.0 - b)
        
        pos_x = x + rx
        pos_y = y + ry
        pos_z = z + self.pivot_length + rz
        
        if self.debug:
            print(f"[5AXISBC] FWD: joints({x:.3f},{y:.3f},{z:.3f},{b:.3f},{c:.3f},{w:.3f}) -> world({pos_x:.3f},{pos_y:.3f},{pos_z:.3f})")
        
        return (pos_x, pos_y, pos_z, 0.0, b, c)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        if len(world) < 6:
            world = list(world) + [0.0] * (6 - len(world))
        
        wx, wy, wz, _, wb, wc = world[0], world[1], world[2], world[3], world[4], world[5]
        
        rx, ry, rz = self._s2r(self.pivot_length, wc, 180.0 - wb)
        
        Px = wx - rx
        Py = wy - ry
        Pz = wz - self.pivot_length - rz
        
        if self.debug:
            print(f"[5AXISBC] INV: world({wx:.3f},{wy:.3f},{wz:.3f},{wb:.3f},{wc:.3f}) -> joints({Px:.3f},{Py:.3f},{Pz:.3f})")
        
        return [Px, Py, Pz, wb, wc, 0.0]
    
    def set_tool_length(self, length: float):
        self._tool_length = length
        if self.debug:
            print(f"[5AXISBC] 刀具长度设置为: {length:.3f} mm")


# ==================== Maxkins BC 运动学 ====================

class MaxkinsBCKinematics(BaseKinematics):
    """
    Max 五轴铣床运动学 (基于 maxkins.c)
    摆头 (B轴) + 转台 (C轴) 结构
    """
    
    def __init__(self, pivot_length: float = 0.666, tool_length: float = 0.0,
                 conventional_directions: bool = False, debug: bool = False):
        super().__init__()
        self.pivot_length = pivot_length
        self._tool_length = tool_length
        self.conventional_directions = conventional_directions
        self.debug = debug
    
    def _get_con(self) -> float:
        return 1.0 if self.conventional_directions else -1.0
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 9:
            joints = list(joints) + [0.0] * (9 - len(joints))
        
        x, y, z, a, b, c, u, v, w = joints[0], joints[1], joints[2], joints[3], joints[4], joints[5], joints[6], joints[7], joints[8]
        
        con = self._get_con()
        b_rad = b * TO_RAD
        c_rad = c * TO_RAD
        
        zb = (self.pivot_length + w) * math.cos(b_rad)
        xb = (self.pivot_length + w) * math.sin(b_rad)
        
        xyr = math.hypot(x, y)
        xytheta = math.atan2(y, x) + c_rad
        
        zv = u * math.sin(b_rad)
        xv = u * math.cos(b_rad)
        
        pos_x = xyr * math.cos(xytheta) - con * xb - xv
        pos_y = xyr * math.sin(xytheta) - v
        pos_z = z - zb - con * zv + self.pivot_length
        
        if self.debug:
            print(f"[MAXKINS] FWD: joints -> world({pos_x:.3f},{pos_y:.3f},{pos_z:.3f})")
        
        return (pos_x, pos_y, pos_z, a, b, c)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        if len(world) < 9:
            world = list(world) + [0.0] * (9 - len(world))
        
        wx, wy, wz, wa, wb, wc, wu, wv, ww = world[0], world[1], world[2], world[3], world[4], world[5], world[6], world[7], world[8]
        
        con = self._get_con()
        b_rad = wb * TO_RAD
        c_rad = wc * TO_RAD
        
        zb = (self.pivot_length + ww) * math.cos(b_rad)
        xb = (self.pivot_length + ww) * math.sin(b_rad)
        
        xyr = math.hypot(wx, wy)
        xytheta = math.atan2(wy, wx) - c_rad
        
        xv = wu * math.cos(b_rad)
        zv = wu * math.sin(b_rad)
        
        Px = xyr * math.cos(xytheta) + con * xb + xv
        Py = xyr * math.sin(xytheta) + wv
        Pz = wz + zb - con * zv - self.pivot_length
        
        if self.debug:
            print(f"[MAXKINS] INV: world -> joints({Px:.3f},{Py:.3f},{Pz:.3f})")
        
        return [Px, Py, Pz, wa, wb, wc, wu, wv, ww]
    
    def set_tool_length(self, length: float):
        self._tool_length = length
        if self.debug:
            print(f"[MAXKINS] 刀具长度设置为: {length:.3f} mm")


# ==================== Hexapod 运动学参数 ====================

@dataclass
class HexapodParams(KinematicsParams):
    """Hexapod 参数 (对应 genhexkins.c 的 haldata)"""
    base_joints: List[Tuple[float, float, float]] = field(default_factory=lambda: [
        (-22.95, 13.25, 0), (22.95, 13.25, 0), (22.95, 13.25, 0),
        (0, -26.5, 0), (0, -26.5, 0), (-22.95, 13.25, 0)
    ])
    platform_joints: List[Tuple[float, float, float]] = field(default_factory=lambda: [
        (-1.0, 11.5, 0), (1.0, 11.5, 0), (10.459, -4.884, 0),
        (9.459, -6.616, 0), (-9.459, -6.616, 0), (-10.459, -4.884, 0)
    ])
    base_joint_axes: List[Tuple[float, float, float]] = field(default_factory=lambda: [
        (0.707107, 0.0, 0.707107), (0.0, -0.707107, 0.707107), (-0.707107, 0.0, 0.707107),
        (-0.707107, 0.0, 0.707107), (0.0, 0.707107, 0.707107), (0.707107, 0.0, 0.707107)
    ])
    platform_joint_axes: List[Tuple[float, float, float]] = field(default_factory=lambda: [
        (-1.0, 0.0, 0.0), (0.866025, 0.5, 0.0), (0.866025, 0.5, 0.0),
        (0.866025, -0.5, 0.0), (0.866025, -0.5, 0.0), (-1.0, 0.0, 0.0)
    ])
    spindle_offset: float = 0.0
    tool_offset: float = 0.0
    screw_lead: float = 0.0
    convergence_criterion: float = 1e-9
    max_iterations: int = 120
    max_error: float = 500.0


# ==================== Hexapod 运动学 ====================

class HexapodKinematics(BaseKinematics):
    """
    通用 Hexapod (Stewart 平台) 运动学 - 完整版
    基于 genhexkins.c 完整实现
    """
    
    NUM_STRUTS = 6
    
    def __init__(self, params: HexapodParams = None, debug: bool = False):
        super().__init__(params or HexapodParams())
        self.debug = debug
        self.p = self.params
        self._update_offsets()
        self.last_iterations = 0
        self.max_iterations_used = 0
        self._forward_fail = False
        self._conv_err = 0.0
    
    def _update_offsets(self):
        """更新带有主轴偏移和刀具偏移的坐标"""
        offset_z = self.p.spindle_offset + self._tool_length
        
        self.b = []
        for bx, by, bz in self.p.base_joints:
            self.b.append([bx, by, bz + offset_z])
        
        self.a = []
        for ax, ay, az in self.p.platform_joints:
            self.a.append([ax, ay, az + offset_z])
        
        self.nb1 = [list(n) for n in self.p.base_joint_axes]
        self.na0 = [list(n) for n in self.p.platform_joint_axes]
    
    def set_tool_length(self, length: float):
        self._tool_length = length
        self.p.tool_offset = length
        self._update_offsets()
        if self.debug:
            print(f"[HEXAPOD] 刀具长度设置为: {length:.3f} mm")
    
    def _rotation_matrix(self, a: float, b: float, c: float) -> List[List[float]]:
        """从 RPY 角 (A, B, C) 构建旋转矩阵"""
        a_rad, b_rad, c_rad = a * TO_RAD, b * TO_RAD, c * TO_RAD
        
        ca, sa = math.cos(a_rad), math.sin(a_rad)
        cb, sb = math.cos(b_rad), math.sin(b_rad)
        cc, sc = math.cos(c_rad), math.sin(c_rad)
        
        return [
            [cc*cb, cc*sb*sa - sc*ca, cc*sb*ca + sc*sa],
            [sc*cb, sc*sb*sa + cc*ca, sc*sb*ca - cc*sa],
            [  -sb,            cb*sa,            cb*ca]
        ]
    
    def _cross_product(self, a: List[float], b: List[float]) -> List[float]:
        """三维向量叉积"""
        return [
            a[1]*b[2] - a[2]*b[1],
            a[2]*b[0] - a[0]*b[2],
            a[0]*b[1] - a[1]*b[0]
        ]
    
    def _dot_product(self, a: List[float], b: List[float]) -> float:
        """三维向量点积"""
        return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]
    
    def _vector_magnitude(self, v: List[float]) -> float:
        """向量长度"""
        return math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])
    
    def _normalize(self, v: List[float]) -> List[float]:
        """向量归一化"""
        mag = self._vector_magnitude(v)
        if mag < 1e-12:
            return [0.0, 0.0, 1.0]
        return [v[0]/mag, v[1]/mag, v[2]/mag]
    
    def _mat_vec_mult(self, M: List[List[float]], v: List[float]) -> List[float]:
        """矩阵乘向量"""
        return [
            M[0][0]*v[0] + M[0][1]*v[1] + M[0][2]*v[2],
            M[1][0]*v[0] + M[1][1]*v[1] + M[1][2]*v[2],
            M[2][0]*v[0] + M[2][1]*v[1] + M[2][2]*v[2]
        ]
    
    def _strut_length_correction(self, strut_unit: List[float], R: List[List[float]],
                                  strut_idx: int) -> float:
        """支腿长度修正 (对应 StrutLengthCorrection)"""
        if self.p.screw_lead == 0.0:
            return 0.0
        
        nb2 = self._cross_product(self.nb1[strut_idx], strut_unit)
        nb3 = self._cross_product(strut_unit, nb2)
        nb3 = self._normalize(nb3)
        
        na1 = self._mat_vec_mult(R, self.na0[strut_idx])
        na2 = self._cross_product(na1, strut_unit)
        na2 = self._normalize(na2)
        
        dotprod = self._dot_product(nb3, na2)
        dotprod = max(-1.0, min(1.0, dotprod))
        
        return self.p.screw_lead * math.asin(dotprod) / (2 * math.pi)
    
    def _mat_invert_6x6(self, J: List[List[float]]) -> List[List[float]]:
        """6x6 矩阵求逆 (高斯-约当消元法)"""
        n = 6
        aug = [[0.0] * (2 * n) for _ in range(n)]
        
        for i in range(n):
            for j in range(n):
                aug[i][j] = J[i][j]
            aug[i][i + n] = 1.0
        
        for k in range(n):
            if abs(aug[k][k]) < 0.01:
                for j in range(k + 1, n):
                    if abs(aug[j][k]) > 0.01:
                        aug[k], aug[j] = aug[j], aug[k]
                        break
            
            pivot = aug[k][k]
            if abs(pivot) < 1e-12:
                raise ValueError(f"Matrix is singular at row {k}")
            
            for j in range(2 * n):
                aug[k][j] /= pivot
            
            for i in range(n):
                if i != k:
                    factor = aug[i][k]
                    for j in range(2 * n):
                        aug[i][j] -= factor * aug[k][j]
        
        inv = [[aug[i][j + n] for j in range(n)] for i in range(n)]
        return inv
    
    def _mat_mult_6x6_vec(self, J: List[List[float]], x: List[float]) -> List[float]:
        """6x6 矩阵乘 6x1 向量"""
        result = [0.0] * 6
        for i in range(6):
            for j in range(6):
                result[i] += J[i][j] * x[j]
        return result
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        """正运动学：牛顿-拉夫逊迭代法"""
        if len(joints) < 6:
            raise ValueError("Hexapod requires 6 strut lengths")
        
        if any(j <= 0 for j in joints):
            self._forward_fail = True
            return (-1, -1, -1, 0, 0, 0)
        
        x, y, z = 0.0, 0.0, 0.0
        a, b, c = 0.0, 0.0, 0.0
        
        iteration = 0
        converge = False
        self._forward_fail = False
        
        while iteration < self.p.max_iterations:
            iteration += 1
            
            if abs(self._conv_err) > self.p.max_error:
                self._forward_fail = True
                return (-1, -1, -1, 0, 0, 0)
            
            R = self._rotation_matrix(a, b, c)
            
            strut_diff = []
            jacobian_inv = [[0.0] * 6 for _ in range(6)]
            
            for i in range(self.NUM_STRUTS):
                ax, ay, az = self.a[i]
                bx, by, bz = self.b[i]
                
                aw_x = R[0][0]*ax + R[0][1]*ay + R[0][2]*az + x
                aw_y = R[1][0]*ax + R[1][1]*ay + R[1][2]*az + y
                aw_z = R[2][0]*ax + R[2][1]*ay + R[2][2]*az + z
                
                vx = aw_x - bx
                vy = aw_y - by
                vz = aw_z - bz
                
                length = math.sqrt(vx*vx + vy*vy + vz*vz)
                
                if length > 1e-12:
                    ux, uy, uz = vx/length, vy/length, vz/length
                else:
                    ux, uy, uz = 0.0, 0.0, 1.0
                
                if self.p.screw_lead != 0.0:
                    corr = self._strut_length_correction([ux, uy, uz], R, i)
                    length += corr
                
                strut_diff.append(length - joints[i])
                
                rx = R[0][0]*ax + R[0][1]*ay + R[0][2]*az
                ry = R[1][0]*ax + R[1][1]*ay + R[1][2]*az
                rz = R[2][0]*ax + R[2][1]*ay + R[2][2]*az
                
                cross_x = ry * uz - rz * uy
                cross_y = rz * ux - rx * uz
                cross_z = rx * uy - ry * ux
                
                jacobian_inv[i] = [ux, uy, uz, cross_x, cross_y, cross_z]
            
            self._conv_err = sum(abs(d) for d in strut_diff)
            max_diff = max(abs(d) for d in strut_diff)
            
            if max_diff < self.p.convergence_criterion:
                converge = True
                break
            
            try:
                jacobian = self._mat_invert_6x6(jacobian_inv)
                delta = self._mat_mult_6x6_vec(jacobian, strut_diff)
                
                x -= delta[0]
                y -= delta[1]
                z -= delta[2]
                a -= delta[3] * TO_DEG
                b -= delta[4] * TO_DEG
                c -= delta[5] * TO_DEG
            except ValueError:
                self._forward_fail = True
                return (-1, -1, -1, 0, 0, 0)
        
        self.last_iterations = iteration
        if iteration > self.max_iterations_used:
            self.max_iterations_used = iteration
        
        if not converge:
            return (-1, -1, -1, 0, 0, 0)
        
        if self.debug:
            print(f"[HEXAPOD] FWD: converged after {iteration} iterations")
        
        return (x, y, z, a, b, c)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        """逆运动学：闭式解"""
        if len(world) < 6:
            raise ValueError("Hexapod requires 6 coordinates: X, Y, Z, A, B, C")
        
        x, y, z, a, b, c = world[0], world[1], world[2], world[3], world[4], world[5]
        
        R = self._rotation_matrix(a, b, c)
        
        struts = []
        for i in range(self.NUM_STRUTS):
            ax, ay, az = self.a[i]
            bx, by, bz = self.b[i]
            
            aw_x = R[0][0]*ax + R[0][1]*ay + R[0][2]*az + x
            aw_y = R[1][0]*ax + R[1][1]*ay + R[1][2]*az + y
            aw_z = R[2][0]*ax + R[2][1]*ay + R[2][2]*az + z
            
            vx = aw_x - bx
            vy = aw_y - by
            vz = aw_z - bz
            
            length = math.sqrt(vx*vx + vy*vy + vz*vz)
            
            if self.p.screw_lead != 0.0 and length > 1e-12:
                u = [vx/length, vy/length, vz/length]
                corr = self._strut_length_correction(u, R, i)
                length += corr
            
            struts.append(length)
        
        if self.debug:
            print(f"[HEXAPOD] INV: world -> {len(struts)} struts")
        
        return struts


# ==================== PUMA 运动学参数 ====================

@dataclass
class PUMAParams(KinematicsParams):
    """PUMA 560 参数"""
    a2: float = 300.0
    a3: float = 50.0
    d3: float = 70.0
    d4: float = 400.0
    d6: float = 70.0


# ==================== PUMA 运动学 ====================

class PUMAKinematics(BaseKinematics):
    """
    PUMA 560 运动学 - 完整版
    基于 pumakins.c 完整实现
    """
    
    PUMA_SHOULDER_RIGHT = 0x01
    PUMA_ELBOW_DOWN = 0x02
    PUMA_WRIST_FLIP = 0x04
    PUMA_SINGULAR = 0x08
    PUMA_REACH = 0x01
    
    def __init__(self, params: PUMAParams = None, debug: bool = False):
        super().__init__(params or PUMAParams())
        self.debug = debug
        self.p = self.params
        self.inverse_flags = 0
        self.SINGULAR_FUZZ = 0.000001
        self.FLAG_FUZZ = 0.000001
        self._last_joints = [0.0] * 6
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        """正运动学 - 完整实现"""
        if len(joints) < 6:
            raise ValueError("PUMA requires 6 joints")
        
        s1, s2, s3, s4, s5, s6 = [math.sin(j * TO_RAD) for j in joints[:6]]
        c1, c2, c3, c4, c5, c6 = [math.cos(j * TO_RAD) for j in joints[:6]]
        
        s23 = c2 * s3 + s2 * c3
        c23 = c2 * c3 - s2 * s3
        
        t1 = c4 * c5 * c6 - s4 * s6
        t2 = s23 * s5 * c6
        t3 = s4 * c5 * c6 + c4 * s6
        t4 = c23 * t1 - t2
        
        r11 = c1 * t4 + s1 * t3
        r21 = s1 * t4 - c1 * t3
        r31 = -s23 * t1 - c23 * s5 * c6
        
        t1 = -c4 * c5 * s6 - s4 * c6
        t2 = s23 * s5 * s6
        t3 = c4 * c6 - s4 * c5 * s6
        t4 = c23 * t1 + t2
        
        r12 = c1 * t4 + s1 * t3
        r22 = s1 * t4 - c1 * t3
        r32 = -s23 * t1 + c23 * s5 * s6
        
        t1 = c23 * c4 * s5 + s23 * c5
        r13 = -c1 * t1 - s1 * s4 * s5
        r23 = -s1 * t1 + c1 * s4 * s5
        r33 = s23 * c4 * s5 - c23 * c5
        
        t1_pos = self.p.a2 * c2 + self.p.a3 * c23 - self.p.d4 * s23
        px = c1 * t1_pos - self.p.d3 * s1
        py = s1 * t1_pos + self.p.d3 * c1
        pz = -self.p.a3 * s23 - self.p.a2 * s2 - self.p.d4 * c23
        
        px += r13 * self.p.d6
        py += r23 * self.p.d6
        pz += r33 * self.p.d6
        
        if abs(r31) < 0.99999:
            b = math.atan2(-r31, math.sqrt(r11**2 + r21**2))
            c_angle = math.atan2(r21/math.cos(b), r11/math.cos(b))
            a_angle = math.atan2(r32/math.cos(b), r33/math.cos(b))
        else:
            c_angle = 0
            if r31 <= -1:
                b = math.pi / 2
                a_angle = math.atan2(r12, r13)
            else:
                b = -math.pi / 2
                a_angle = math.atan2(-r12, -r13)
        
        if self.debug:
            print(f"[PUMA] FWD: joints -> world({px:.3f},{py:.3f},{pz:.3f})")
        
        return (px, py, pz, a_angle * TO_DEG, b * TO_DEG, c_angle * TO_DEG)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float],
                flags: int = 0) -> List[float]:
        """逆运动学 - 完整实现"""
        if len(world) < 6:
            raise ValueError("PUMA requires 6 coordinates")
        
        px, py, pz, a_deg, b_deg, c_deg = world[:6]
        
        a_rad, b_rad, c_rad = a_deg * TO_RAD, b_deg * TO_RAD, c_deg * TO_RAD
        
        ca, sa = math.cos(a_rad), math.sin(a_rad)
        cb, sb = math.cos(b_rad), math.sin(b_rad)
        cc, sc = math.cos(c_rad), math.sin(c_rad)
        
        R = [
            [cc*cb, cc*sb*sa - sc*ca, cc*sb*ca + sc*sa],
            [sc*cb, sc*sb*sa + cc*ca, sc*sb*ca - cc*sa],
            [  -sb,            cb*sa,            cb*ca]
        ]
        
        px = px - self.p.d6 * R[0][2]
        py = py - self.p.d6 * R[1][2]
        pz = pz - self.p.d6 * R[2][2]
        
        sumSq = px*px + py*py - self.p.d3*self.p.d3
        if sumSq < 0:
            sumSq = 0
        
        if flags & self.PUMA_SHOULDER_RIGHT:
            th1 = math.atan2(py, px) - math.atan2(self.p.d3, -math.sqrt(sumSq))
        else:
            th1 = math.atan2(py, px) - math.atan2(self.p.d3, math.sqrt(sumSq))
        
        s1 = math.sin(th1)
        c1 = math.cos(th1)
        
        k = (sumSq + pz*pz - self.p.a2*self.p.a2 - self.p.a3*self.p.a3 - self.p.d4*self.p.d4) / (2.0 * self.p.a2)
        k_limit = math.sqrt(self.p.a3*self.p.a3 + self.p.d4*self.p.d4)
        if abs(k) > k_limit:
            k = k_limit if k > 0 else -k_limit
        
        if flags & self.PUMA_ELBOW_DOWN:
            th3 = math.atan2(self.p.a3, self.p.d4) - math.atan2(k, -math.sqrt(self.p.a3*self.p.a3 + self.p.d4*self.p.d4 - k*k))
        else:
            th3 = math.atan2(self.p.a3, self.p.d4) - math.atan2(k, math.sqrt(self.p.a3*self.p.a3 + self.p.d4*self.p.d4 - k*k))
        
        s3 = math.sin(th3)
        c3 = math.cos(th3)
        
        t1 = (-self.p.a3 - self.p.a2 * c3) * pz + (c1*px + s1*py) * (self.p.a2*s3 - self.p.d4)
        t2 = (self.p.a2*s3 - self.p.d4) * pz + (self.p.a3 + self.p.a2*c3) * (c1*px + s1*py)
        t3 = pz*pz + (c1*px + s1*py)*(c1*px + s1*py)
        
        th23 = math.atan2(t1, t2)
        th2 = th23 - th3
        
        s23 = t1 / t3 if t3 != 0 else 0
        c23 = t2 / t3 if t3 != 0 else 0
        
        t1_j4 = -R[2][0] * s1 + R[2][1] * c1
        t2_j4 = -R[2][0] * c1 * c23 - R[2][1] * s1 * c23 + R[2][2] * s23
        
        if abs(t1_j4) < self.SINGULAR_FUZZ and abs(t2_j4) < self.SINGULAR_FUZZ:
            th4 = self._last_joints[3] * TO_RAD if len(self._last_joints) > 3 else 0
        else:
            th4 = math.atan2(t1_j4, t2_j4)
        
        s4 = math.sin(th4)
        c4 = math.cos(th4)
        
        s5 = R[2][2] * (s23*c4) - R[2][0] * (c1*c23*c4 + s1*s4) - R[2][1] * (s1*c23*c4 - c1*s4)
        c5 = -R[2][0] * (c1*s23) - R[2][1] * (s1*s23) - R[2][2] * c23
        th5 = math.atan2(s5, c5)
        
        s6 = R[0][2] * (s23*s4) - R[0][0] * (c1*c23*s4 - s1*c4) - R[0][1] * (s1*c23*s4 + c1*c4)
        c6 = R[0][0] * ((c1*c23*c4 + s1*s4)*c5 - c1*s23*s5) + \
             R[0][1] * ((s1*c23*c4 - c1*s4)*c5 - s1*s23*s5) - \
             R[0][2] * (s23*c4*c5 + c23*s5)
        th6 = math.atan2(s6, c6)
        
        if flags & self.PUMA_WRIST_FLIP:
            th4 = th4 + math.pi
            th5 = -th5
            th6 = th6 + math.pi
        
        joints = [
            th1 * TO_DEG, th2 * TO_DEG, th3 * TO_DEG,
            th4 * TO_DEG, th5 * TO_DEG, th6 * TO_DEG
        ]
        
        joints = [(j + 180) % 360 - 180 for j in joints]
        self._last_joints = joints.copy()
        
        if self.debug:
            print(f"[PUMA] INV: world -> joints({joints[0]:.3f}, {joints[1]:.3f}, ...)")
        
        return joints


# ==================== SCARA 运动学参数 ====================

@dataclass
class SCARAParams(KinematicsParams):
    """SCARA 参数"""
    d1: float = 490.0
    d2: float = 340.0
    d3: float = 50.0
    d4: float = 250.0
    d5: float = 50.0
    d6: float = 50.0


# ==================== SCARA 运动学 ====================

class SCARAKinematics(BaseKinematics):
    """
    SCARA 运动学 (基于 scarakins.c)
    """
    
    def __init__(self, params: SCARAParams = None, debug: bool = False):
        super().__init__(params or SCARAParams())
        self.debug = debug
        self.p = self.params
        self._inverse_flags = 0
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 4:
            raise ValueError("SCARA requires at least 4 joints")
        
        a0 = joints[0] * TO_RAD
        a1 = joints[1] * TO_RAD
        a3 = joints[3] * TO_RAD if len(joints) > 3 else 0
        
        a1_total = a1 + a0
        a3_total = a3 + a1_total
        
        x = self.p.d2 * math.cos(a0) + self.p.d4 * math.cos(a1_total) + self.p.d6 * math.cos(a3_total)
        y = self.p.d2 * math.sin(a0) + self.p.d4 * math.sin(a1_total) + self.p.d6 * math.sin(a3_total)
        z = self.p.d1 + self.p.d3 - joints[2] - self.p.d5
        c = a3_total * TO_DEG
        
        if self.debug:
            print(f"[SCARA] FWD: joints -> world({x:.3f},{y:.3f},{z:.3f})")
        
        return (x, y, z, 0.0, 0.0, c)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        if len(world) < 6:
            raise ValueError("SCARA requires 6 coordinates")
        
        x, y, z, _, _, c = world[:6]
        a3 = c * TO_RAD
        
        xt = x - self.p.d6 * math.cos(a3)
        yt = y - self.p.d6 * math.sin(a3)
        
        rsq = xt*xt + yt*yt
        cc = (rsq - self.p.d2*self.p.d2 - self.p.d4*self.p.d4) / (2 * self.p.d2 * self.p.d4)
        cc = max(-1.0, min(1.0, cc))
        
        q1 = math.acos(cc)
        if self._inverse_flags & 1:
            q1 = -q1
        
        q0 = math.atan2(yt, xt) - math.atan2(self.p.d4 * math.sin(q1), self.p.d2 + self.p.d4 * math.cos(q1))
        
        joints = [
            q0 * TO_DEG,
            q1 * TO_DEG,
            self.p.d1 + self.p.d3 - self.p.d5 - z,
            c - (q0 + q1) * TO_DEG,
            0.0, 0.0
        ]
        
        if self.debug:
            print(f"[SCARA] INV: world({x:.3f},{y:.3f},{z:.3f}) -> joints")
        
        return joints
    
    def set_flags(self, flags: int):
        """设置逆解标志"""
        self._inverse_flags = flags


# ==================== 线性 Delta 运动学 ====================

class LinearDeltaKinematics(BaseKinematics):
    """
    线性 Delta 机器人运动学 (基于 lineardeltakins-common.h)
    Rostock 风格 Delta 机器人
    """
    
    def __init__(self, radius: float = 165.25, rod_length: float = 269.0, debug: bool = False):
        super().__init__()
        self.R = radius
        self.L = rod_length
        self.debug = debug
        self._update_geometry()
    
    def _update_geometry(self):
        self.L2 = self.L * self.L
        SQ3 = math.sqrt(3)
        SIN_60 = SQ3 / 2
        COS_60 = 0.5
        
        self.Ax = 0.0
        self.Ay = self.R
        
        self.Bx = -SIN_60 * self.R
        self.By = -COS_60 * self.R
        
        self.Cx = SIN_60 * self.R
        self.Cy = -COS_60 * self.R
    
    def set_geometry(self, radius: float, rod_length: float):
        self.R = radius
        self.L = rod_length
        self._update_geometry()
        if self.debug:
            print(f"[LINEAR_DELTA] 几何更新: R={radius:.3f}, L={rod_length:.3f}")
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 3:
            raise ValueError("Linear Delta requires 3 joints")
        
        q1, q2, q3 = joints[0], joints[1], joints[2]
        
        den = (self.By - self.Ay) * self.Cx - (self.Cy - self.Ay) * self.Bx
        
        if abs(den) < 1e-12:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        w1 = self.Ay * self.Ay + q1 * q1
        w2 = self.Bx * self.Bx + self.By * self.By + q2 * q2
        w3 = self.Cx * self.Cx + self.Cy * self.Cy + q3 * q3
        
        a1 = (q2 - q1) * (self.Cy - self.Ay) - (q3 - q1) * (self.By - self.Ay)
        b1 = -((w2 - w1) * (self.Cy - self.Ay) - (w3 - w1) * (self.By - self.Ay)) / 2.0
        
        a2 = -(q2 - q1) * self.Cx + (q3 - q1) * self.Bx
        b2 = ((w2 - w1) * self.Cx - (w3 - w1) * self.Bx) / 2.0
        
        a = a1 * a1 + a2 * a2 + den * den
        b = 2 * (a1 * b1 + a2 * (b2 - self.Ay * den) - q1 * den * den)
        c = (b2 - self.Ay * den) * (b2 - self.Ay * den) + b1 * b1 + den * den * (q1 * q1 - self.L2)
        
        discr = b * b - 4.0 * a * c
        if discr < 0:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        z = -0.5 * (b + math.sqrt(discr)) / a
        x = (a1 * z + b1) / den
        y = (a2 * z + b2) / den
        
        if self.debug:
            print(f"[LINEAR_DELTA] FWD: joints({q1:.3f},{q2:.3f},{q3:.3f}) -> world({x:.3f},{y:.3f},{z:.3f})")
        
        return (x, y, z, 0.0, 0.0, 0.0)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        x, y, z = world[0], world[1], world[2]
        
        def sq(v): return v * v
        
        q1 = z + math.sqrt(max(0, self.L2 - sq(self.Ax - x) - sq(self.Ay - y)))
        q2 = z + math.sqrt(max(0, self.L2 - sq(self.Bx - x) - sq(self.By - y)))
        q3 = z + math.sqrt(max(0, self.L2 - sq(self.Cx - x) - sq(self.Cy - y)))
        
        if self.debug:
            print(f"[LINEAR_DELTA] INV: world({x:.3f},{y:.3f},{z:.3f}) -> joints({q1:.3f},{q2:.3f},{q3:.3f})")
        
        return [q1, q2, q3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


# ==================== DH 参数串联运动学 ====================

@dataclass
class DHLink:
    """Denavit-Hartenberg 参数"""
    a: float = 0.0
    alpha: float = 0.0
    d: float = 0.0
    theta: float = 0.0
    is_revolute: bool = True
    min_limit: float = -180.0
    max_limit: float = 180.0


@dataclass
class SerialDHParams(KinematicsParams):
    """串联 DH 参数"""
    links: List[DHLink] = field(default_factory=list)
    max_iterations: int = 100
    convergence_epsilon: float = 1e-6
    use_kinematic_decoupling: bool = False


class SerialDHKinematics(BaseKinematics):
    """
    通用串联机器人运动学 - 完整版
    基于 genserfuncs.c 完整实现
    """
    
    def __init__(self, params: SerialDHParams = None, debug: bool = False):
        super().__init__(params or SerialDHParams())
        self.debug = debug
        self.p = self.params
        self.iterations = 0
        self.GO_REAL_EPSILON = 1e-6
    
    def _dh_transform(self, link: DHLink, joint_value: float) -> List[List[float]]:
        """计算单个连杆的变换矩阵"""
        if link.is_revolute:
            theta = (link.theta + joint_value) * TO_RAD
            d = link.d
        else:
            theta = link.theta * TO_RAD
            d = link.d + joint_value
        
        alpha = link.alpha * TO_RAD
        a = link.a
        
        ct = math.cos(theta)
        st = math.sin(theta)
        ca = math.cos(alpha)
        sa = math.sin(alpha)
        
        return [
            [ct, -st*ca,  st*sa, a*ct],
            [st,  ct*ca, -ct*sa, a*st],
            [ 0,     sa,     ca,    d],
            [ 0,      0,      0,    1]
        ]
    
    def _mat_mult_4x4(self, A: List[List[float]], B: List[List[float]]) -> List[List[float]]:
        """4x4 矩阵乘法"""
        result = [[0.0]*4 for _ in range(4)]
        for i in range(4):
            for j in range(4):
                for k in range(4):
                    result[i][j] += A[i][k] * B[k][j]
        return result
    
    def _mat_mult_3x3_vec(self, M: List[List[float]], v: List[float]) -> List[float]:
        """3x3 矩阵乘 3x1 向量"""
        return [
            M[0][0]*v[0] + M[0][1]*v[1] + M[0][2]*v[2],
            M[1][0]*v[0] + M[1][1]*v[1] + M[1][2]*v[2],
            M[2][0]*v[0] + M[2][1]*v[1] + M[2][2]*v[2]
        ]
    
    def _cross_product_matrix(self, v: List[float]) -> List[List[float]]:
        """向量的叉积矩阵 (用于雅可比计算)"""
        return [
            [    0, -v[2],  v[1]],
            [ v[2],     0, -v[0]],
            [-v[1],  v[0],     0]
        ]
    
    def _mat_mult_3x3(self, A: List[List[float]], B: List[List[float]]) -> List[List[float]]:
        """3x3 矩阵乘法"""
        result = [[0.0]*3 for _ in range(3)]
        for i in range(3):
            for j in range(3):
                for k in range(3):
                    result[i][j] += A[i][k] * B[k][j]
        return result
    
    def _mat_transpose_3x3(self, M: List[List[float]]) -> List[List[float]]:
        """3x3 矩阵转置"""
        return [[M[j][i] for j in range(3)] for i in range(3)]
    
    def _rpy_to_rot_matrix(self, r: float, p: float, y: float) -> List[List[float]]:
        """RPY 角转旋转矩阵"""
        cr, sr = math.cos(r), math.sin(r)
        cp, sp = math.cos(p), math.sin(p)
        cy, sy = math.cos(y), math.sin(y)
        
        return [
            [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
            [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
            [  -sp,            cp*sr,            cp*cr]
        ]
    
    def _rot_matrix_to_rpy(self, R: List[List[float]]) -> Tuple[float, float, float]:
        """旋转矩阵转 RPY 角"""
        if abs(R[2][0]) < 0.99999:
            p = math.atan2(-R[2][0], math.sqrt(R[0][0]**2 + R[1][0]**2))
            y = math.atan2(R[1][0]/math.cos(p), R[0][0]/math.cos(p))
            r = math.atan2(R[2][1]/math.cos(p), R[2][2]/math.cos(p))
        else:
            y = 0
            if R[2][0] <= -1:
                p = math.pi / 2
                r = math.atan2(R[0][1], R[0][2])
            else:
                p = -math.pi / 2
                r = math.atan2(-R[0][1], -R[0][2])
        return (r, p, y)
    
    def _compute_jfwd(self, joints: List[float]) -> Tuple[List[List[float]], List[List[float]]]:
        """计算正向雅可比矩阵"""
        n = len(joints)
        
        Jv = [[0.0] * n for _ in range(3)]
        Jw = [[0.0] * n for _ in range(3)]
        
        if self.p.links[0].is_revolute:
            Jw[2][0] = 1.0
        else:
            Jv[2][0] = 1.0
        
        T_curr = [[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]]
        poses = []
        
        for i, link in enumerate(self.p.links):
            T_link = self._dh_transform(link, joints[i])
            T_curr = self._mat_mult_4x4(T_curr, T_link)
            poses.append(T_curr)
        
        R_end = [[T_curr[i][j] for j in range(3)] for i in range(3)]
        
        for col in range(1, n):
            T_prev = poses[col - 1]
            R_prev = [[T_prev[i][j] for j in range(3)] for i in range(3)]
            
            link = self.p.links[col]
            if link.is_revolute:
                theta = (link.theta + joints[col]) * TO_RAD
                d = link.d
            else:
                theta = link.theta * TO_RAD
                d = link.d + joints[col]
            
            a = link.a
            P_i_ip1 = [a * math.cos(theta), a * math.sin(theta), d]
            P_ip1_0 = self._mat_mult_3x3_vec(R_prev, P_i_ip1)
            
            Jw_col = [0.0, 0.0, 1.0] if link.is_revolute else [0.0, 0.0, 0.0]
            cross = self._cross_product_matrix(P_ip1_0)
            Jv_col = [cross[i][0]*Jw_col[0] + cross[i][1]*Jw_col[1] + cross[i][2]*Jw_col[2] for i in range(3)]
            
            for i in range(3):
                Jv[i][col] = Jv_col[i]
                Jw[i][col] = Jw_col[i]
        
        R_end_inv = self._mat_transpose_3x3(R_end)
        Jv = [self._mat_mult_3x3_vec(R_end_inv, [Jv[i][j] for i in range(3)]) for j in range(n)]
        Jv = [[Jv[j][i] for j in range(n)] for i in range(3)]
        Jw = [self._mat_mult_3x3_vec(R_end_inv, [Jw[i][j] for i in range(3)]) for j in range(n)]
        Jw = [[Jw[j][i] for j in range(n)] for i in range(3)]
        
        Jfwd = [[0.0] * n for _ in range(6)]
        for col in range(n):
            for row in range(3):
                Jfwd[row][col] = Jv[row][col]
                Jfwd[row + 3][col] = Jw[row][col]
        
        return Jfwd, R_end
    
    def _mat_invert_nxn(self, M: List[List[float]], n: int) -> List[List[float]]:
        """nxn 矩阵求逆"""
        aug = [[0.0] * (2 * n) for _ in range(n)]
        
        for i in range(n):
            for j in range(n):
                aug[i][j] = M[i][j]
            aug[i][i + n] = 1.0
        
        for k in range(n):
            pivot = aug[k][k]
            if abs(pivot) < 1e-12:
                raise ValueError(f"Matrix is singular at row {k}")
            
            for j in range(2 * n):
                aug[k][j] /= pivot
            
            for i in range(n):
                if i != k:
                    factor = aug[i][k]
                    for j in range(2 * n):
                        aug[i][j] -= factor * aug[k][j]
        
        return [[aug[i][j + n] for j in range(n)] for i in range(n)]
    
    def _compute_jinv(self, Jfwd: List[List[float]]) -> List[List[float]]:
        """计算雅可比矩阵的逆或伪逆"""
        m = len(Jfwd)
        n = len(Jfwd[0])
        
        if m == n:
            return self._mat_invert_nxn(Jfwd, m)
        elif m < n:
            JJT = [[0.0] * m for _ in range(m)]
            for i in range(m):
                for j in range(m):
                    for k in range(n):
                        JJT[i][j] += Jfwd[i][k] * Jfwd[j][k]
            
            JJT_inv = self._mat_invert_nxn(JJT, m)
            JT = [[Jfwd[j][i] for j in range(m)] for i in range(n)]
            Jinv = [[0.0] * m for _ in range(n)]
            for i in range(n):
                for j in range(m):
                    for k in range(m):
                        Jinv[i][j] += JT[i][k] * JJT_inv[k][j]
            return Jinv
        else:
            JTJ = [[0.0] * n for _ in range(n)]
            for i in range(n):
                for j in range(n):
                    for k in range(m):
                        JTJ[i][j] += Jfwd[k][i] * Jfwd[k][j]
            
            JTJ_inv = self._mat_invert_nxn(JTJ, n)
            JT = [[Jfwd[j][i] for j in range(m)] for i in range(n)]
            Jinv = [[0.0] * m for _ in range(n)]
            for i in range(n):
                for j in range(m):
                    for k in range(n):
                        Jinv[i][j] += JTJ_inv[i][k] * JT[k][j]
            return Jinv
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        """正运动学"""
        if len(joints) != len(self.p.links):
            raise ValueError(f"Expected {len(self.p.links)} joints, got {len(joints)}")
        
        T = [[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]]
        
        for i, link in enumerate(self.p.links):
            T_link = self._dh_transform(link, joints[i])
            T = self._mat_mult_4x4(T, T_link)
        
        x, y, z = T[0][3], T[1][3], T[2][3]
        
        r, p, y_rpy = self._rot_matrix_to_rpy([[T[i][j] for j in range(3)] for i in range(3)])
        
        if self.debug:
            print(f"[SERIAL_DH] FWD: joints -> world({x:.3f},{y:.3f},{z:.3f})")
        
        return (x, y, z, r * TO_DEG, p * TO_DEG, y_rpy * TO_DEG)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float],
                initial_joints: List[float] = None) -> List[float]:
        """逆运动学：牛顿-拉夫逊迭代法"""
        if len(world) < 6:
            raise ValueError("Serial DH requires 6 coordinates")
        
        n = len(self.p.links)
        
        if initial_joints and len(initial_joints) == n:
            jest = list(initial_joints)
        else:
            jest = [0.0] * n
        
        x, y, z, a, b, c = world[:6]
        R_target = self._rpy_to_rot_matrix(a * TO_RAD, b * TO_RAD, c * TO_RAD)
        
        self.iterations = 0
        
        for iteration in range(self.p.max_iterations):
            self.iterations = iteration + 1
            
            Jfwd, R_current = self._compute_jfwd(jest)
            current_pose = self.forward(jest)
            cx, cy, cz = current_pose[0], current_pose[1], current_pose[2]
            
            dx = x - cx
            dy = y - cy
            dz = z - cz
            
            R_diff = self._mat_mult_3x3(R_target, self._mat_transpose_3x3(R_current))
            rx = 0.5 * (R_diff[2][1] - R_diff[1][2])
            ry = 0.5 * (R_diff[0][2] - R_diff[2][0])
            rz = 0.5 * (R_diff[1][0] - R_diff[0][1])
            
            error = [dx, dy, dz, rx, ry, rz]
            
            max_error = max(abs(e) for e in error)
            if max_error < self.p.convergence_epsilon:
                if self.debug:
                    print(f"[SERIAL_DH] INV: converged after {iteration + 1} iterations")
                return jest
            
            try:
                Jinv = self._compute_jinv(Jfwd)
            except ValueError:
                break
            
            dq = [0.0] * n
            for i in range(n):
                for j in range(6):
                    dq[i] += Jinv[i][j] * error[j]
            
            for i in range(n):
                jest[i] += dq[i]
        
        if self.debug:
            print(f"[SERIAL_DH] INV: did not converge after {self.p.max_iterations} iterations")
        
        return jest


# ==================== 新增运动学类 ====================

@dataclass
class TripodParams(KinematicsParams):
    """Tripod 参数"""
    Bx: float = 1.0
    Cx: float = 1.0
    Cy: float = 1.0


class TripodKinematics(BaseKinematics):
    """Tripod运动学 - 基于 tripodkins.c"""
    
    def __init__(self, params: TripodParams = None, debug: bool = False):
        super().__init__(params or TripodParams())
        self.debug = debug
        self.p = self.params
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 3:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        P = joints[0] * joints[0]
        Q = joints[1] * joints[1] - self.p.Bx * self.p.Bx
        R = joints[2] * joints[2] - self.p.Cx * self.p.Cx - self.p.Cy * self.p.Cy
        
        s = -2.0 * self.p.Bx
        t = -2.0 * self.p.Cx
        u = -2.0 * self.p.Cy
        
        if abs(s) < 1e-12 or abs(u) < 1e-12:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        x = (Q - P) / s
        y = (R - Q - (t - s) * x) / u
        z = P - x*x - y*y
        
        if z < 0:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        z = math.sqrt(z)
        
        if self.debug:
            print(f"[TRIPOD] FWD: joints -> world({x:.3f},{y:.3f},{z:.3f})")
        return (x, y, z, 0.0, 0.0, 0.0)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        x, y, z = world[0], world[1], world[2]
        
        L1 = math.sqrt(x*x + y*y + z*z)
        L2 = math.sqrt((x - self.p.Bx)*(x - self.p.Bx) + y*y + z*z)
        L3 = math.sqrt((x - self.p.Cx)*(x - self.p.Cx) + (y - self.p.Cy)*(y - self.p.Cy) + z*z)
        
        if self.debug:
            print(f"[TRIPOD] INV: world({x:.3f},{y:.3f},{z:.3f}) -> joints({L1:.3f},{L2:.3f},{L3:.3f})")
        return [L1, L2, L3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


@dataclass
class PentapodParams(KinematicsParams):
    """Pentapod 参数"""
    base: List[Tuple[float, float, float]] = field(default_factory=lambda: [
        (-418.03, 324.56, 895.56), (417.96, 324.56, 895.56),
        (-418.03, -325.44, 895.56), (417.96, -325.44, 895.56),
        (-0.06, -492.96, 895.56)
    ])
    effector_r: List[float] = field(default_factory=lambda: [80.32] * 5)
    effector_z: List[float] = field(default_factory=lambda: [-185.50, -159.50, -67.50, -41.50, -14.00])
    tool_offset: float = 0.0
    convergence_criterion: float = 1e-9
    max_iterations: int = 120
    max_error: float = 100.0


class PentapodKinematics(BaseKinematics):
    """Pentapod运动学 - 基于 pentakins.c"""
    
    NUM_STRUTS = 5
    
    def __init__(self, params: PentapodParams = None, debug: bool = False):
        super().__init__(params or PentapodParams())
        self.debug = debug
        self.p = self.params
        self.last_iterations = 0
    
    def _rotation_matrix(self, r: float, p: float, y: float) -> List[List[float]]:
        cr, sr = math.cos(r), math.sin(r)
        cp, sp = math.cos(p), math.sin(p)
        return [
            [cp, sp*sr, sp*cr],
            [0, cr, -sr],
            [-sp, cp*sr, cp*cr]
        ]
    
    def _inv_kins(self, coord: List[float]) -> List[float]:
        x, y, z = coord[0], coord[1], coord[2]
        r, p = coord[3], coord[4]
        
        R = self._rotation_matrix(r, p, 0)
        R_inv = [[R[j][i] for j in range(3)] for i in range(3)]
        
        struts = []
        for i in range(self.NUM_STRUTS):
            bx, by, bz = self.p.base[i]
            bz += self.p.tool_offset
            ra = self.p.effector_r[i]
            za = self.p.effector_z[i] + self.p.tool_offset
            
            dx = bx - x
            dy = by - y
            dz = bz - z
            
            tx = R_inv[0][0]*dx + R_inv[0][1]*dy + R_inv[0][2]*dz
            ty = R_inv[1][0]*dx + R_inv[1][1]*dy + R_inv[1][2]*dz
            tz = R_inv[2][0]*dx + R_inv[2][1]*dy + R_inv[2][2]*dz
            
            L = math.sqrt((tz - za)**2 + (math.sqrt(tx*tx + ty*ty) - ra)**2)
            struts.append(L)
        
        return struts
    
    def _mat_invert_5x5(self, J: List[List[float]]) -> List[List[float]]:
        n = 5
        aug = [[0.0] * (2 * n) for _ in range(n)]
        
        for i in range(n):
            for j in range(n):
                aug[i][j] = J[i][j]
            aug[i][i + n] = 1.0
        
        for k in range(n):
            if abs(aug[k][k]) < 0.01:
                for j in range(k + 1, n):
                    if abs(aug[j][k]) > 0.01:
                        aug[k], aug[j] = aug[j], aug[k]
                        break
            
            pivot = aug[k][k]
            if abs(pivot) < 1e-12:
                continue
            
            for j in range(2 * n):
                aug[k][j] /= pivot
            
            for i in range(n):
                if i != k:
                    factor = aug[i][k]
                    for j in range(2 * n):
                        aug[i][j] -= factor * aug[k][j]
        
        return [[aug[i][j + n] for j in range(n)] for i in range(n)]
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 5:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        coord = [0.0, 0.0, 0.0, 0.0, 0.0]
        iteration = 0
        
        while iteration < self.p.max_iterations:
            iteration += 1
            
            struts = self._inv_kins(coord)
            diff = [struts[i] - joints[i] for i in range(5)]
            
            max_diff = max(abs(d) for d in diff)
            if max_diff < self.p.convergence_criterion:
                break
            
            J = [[0.0]*5 for _ in range(5)]
            delta = 1e-4
            for i in range(5):
                orig = coord[i]
                coord[i] += delta
                struts_pert = self._inv_kins(coord)
                coord[i] = orig
                for j in range(5):
                    J[j][i] = (struts_pert[j] - struts[j]) / delta
            
            try:
                Jinv = self._mat_invert_5x5(J)
                dq = [0.0] * 5
                for i in range(5):
                    for j in range(5):
                        dq[i] += Jinv[i][j] * diff[j]
                
                for i in range(5):
                    coord[i] -= dq[i]
            except:
                pass
        
        self.last_iterations = iteration
        
        if self.debug:
            print(f"[PENTAPOD] FWD: converged after {iteration} iterations")
        
        return (coord[0], coord[1], coord[2], coord[3] * TO_DEG, coord[4] * TO_DEG, 0.0)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        x, y, z = world[0], world[1], world[2]
        a, b = world[3] * TO_RAD, world[4] * TO_RAD
        
        coord = [x, y, z, a, b]
        struts = self._inv_kins(coord)
        
        if self.debug:
            print(f"[PENTAPOD] INV: world -> {len(struts)} struts")
        
        return struts + [0.0] * (9 - len(struts))
    
    def set_tool_length(self, length: float):
        self.p.tool_offset = length
        if self.debug:
            print(f"[PENTAPOD] 刀具长度设置为: {length:.3f} mm")


@dataclass
class RotaryDeltaParams(KinematicsParams):
    """Rotary Delta 参数"""
    platform_radius: float = 10.0
    thigh_length: float = 10.0
    shin_length: float = 14.0
    foot_radius: float = 6.0


class RotaryDeltaKinematics(BaseKinematics):
    """Rotary Delta运动学 - 基于 rotarydeltakins.c"""
    
    def __init__(self, params: RotaryDeltaParams = None, debug: bool = False):
        super().__init__(params or RotaryDeltaParams())
        self.debug = debug
        self.p = self.params
    
    def _inverse_j0(self, x: float, y: float, z: float) -> Optional[float]:
        pfr = self.p.platform_radius
        fr = self.p.foot_radius
        tl = self.p.thigh_length
        sl = self.p.shin_length
        
        a = 0.5 * (x*x + (y-fr)*(y-fr) + z*z + tl*tl - sl*sl - pfr*pfr) / z
        b = (fr - pfr - y) / z
        
        d = tl*tl * (b*b + 1) - (a - b*pfr)*(a - b*pfr)
        if d < 0:
            return None
        
        knee_y = (pfr + a*b + math.sqrt(d)) / (b*b + 1)
        knee_z = b * knee_y - a
        theta = math.atan2(knee_z, knee_y - pfr) * TO_DEG
        return theta
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 3:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        j0, j1, j2 = [j * TO_RAD for j in joints[:3]]
        pfr = self.p.platform_radius
        fr = self.p.foot_radius
        tl = self.p.thigh_length
        
        y1 = -(pfr - fr + tl * math.cos(j0))
        z1 = -tl * math.sin(j0)
        
        y2 = (pfr - fr + tl * math.cos(j1)) * 0.5
        x2 = y2 * math.sqrt(3)
        z2 = -tl * math.sin(j1)
        
        y3 = (pfr - fr + tl * math.cos(j2)) * 0.5
        x3 = -y3 * math.sqrt(3)
        z3 = -tl * math.sin(j2)
        
        denom = x3*(y2-y1) - x2*(y3-y1)
        if abs(denom) < 1e-12:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        w1 = y1*y1 + z1*z1
        w2 = x2*x2 + y2*y2 + z2*z2
        w3 = x3*x3 + y3*y3 + z3*z3
        
        a1 = (z2-z1)*(y3-y1) - (z3-z1)*(y2-y1)
        b1 = -((w2-w1)*(y3-y1) - (w3-w1)*(y2-y1)) / 2.0
        a2 = -(z2-z1)*x3 + (z3-z1)*x2
        b2 = ((w2-w1)*x3 - (w3-w1)*x2) / 2.0
        
        a = a1*a1 + a2*a2 + denom*denom
        b = 2 * (a1*b1 + a2*(b2 - y1*denom) - z1*denom*denom)
        c = (b2 - y1*denom)*(b2 - y1*denom) + b1*b1 + denom*denom*(z1*z1 - self.p.shin_length*self.p.shin_length)
        
        d = b*b - 4*a*c
        if d < 0:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        z = (-b - math.sqrt(d)) / (2*a)
        x = (a1*z + b1) / denom
        y = (a2*z + b2) / denom
        
        if self.debug:
            print(f"[ROTARY_DELTA] FWD: joints -> world({x:.3f},{y:.3f},{z:.3f})")
        
        return (x, y, z, 0.0, 0.0, 0.0)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        x, y, z = world[0], world[1], world[2]
        
        def rotate(x: float, y: float, theta: float) -> Tuple[float, float]:
            c, s = math.cos(theta), math.sin(theta)
            return (x*c - y*s, x*s + y*c)
        
        j0 = self._inverse_j0(x, y, z)
        if j0 is None:
            return [0.0] * 9
        
        xr, yr = rotate(x, y, -2*math.pi/3)
        j1 = self._inverse_j0(xr, yr, z)
        if j1 is None:
            return [0.0] * 9
        
        xr, yr = rotate(x, y, 2*math.pi/3)
        j2 = self._inverse_j0(xr, yr, z)
        if j2 is None:
            return [0.0] * 9
        
        if self.debug:
            print(f"[ROTARY_DELTA] INV: world -> joints({j0:.3f},{j1:.3f},{j2:.3f})")
        
        return [j0, j1, j2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


class RotateKinematics(BaseKinematics):
    """Rotary Table运动学 - 基于 rotatekins.c"""
    
    def __init__(self, params: KinematicsParams = None, debug: bool = False):
        super().__init__(params)
        self.debug = debug    
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 6:
            joints = list(joints) + [0.0] * (6 - len(joints))
        
        x, y, z, a, b, c = joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]
        c_rad = -c * TO_RAD
        
        pos_x = x * math.cos(c_rad) - y * math.sin(c_rad)
        pos_y = x * math.sin(c_rad) + y * math.cos(c_rad)
        
        return (pos_x, pos_y, z, a, b, c)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        if len(world) < 6:
            world = list(world) + [0.0] * (6 - len(world))
        
        wx, wy, wz, wa, wb, wc = world[0], world[1], world[2], world[3], world[4], world[5]
        c_rad = wc * TO_RAD
        
        x = wx * math.cos(c_rad) - wy * math.sin(c_rad)
        y = wx * math.sin(c_rad) + wy * math.cos(c_rad)
        
        u = world[6] if len(world) > 6 else 0.0
        v = world[7] if len(world) > 7 else 0.0
        w = world[8] if len(world) > 8 else 0.0
        
        return [x, y, wz, wa, wb, wc, u, v, w]


class CoreXYKinematics(BaseKinematics):
    """CoreXY运动学 - 基于 corexykins.c"""
    
    def __init__(self, params: KinematicsParams = None, debug: bool = False):
        super().__init__(params)
        self.debug = debug    
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 3:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        x = 0.5 * (joints[0] + joints[1])
        y = 0.5 * (joints[0] - joints[1])
        z = joints[2] if len(joints) > 2 else 0.0
        a = joints[3] if len(joints) > 3 else 0.0
        b = joints[4] if len(joints) > 4 else 0.0
        c = joints[5] if len(joints) > 5 else 0.0
        
        return (x, y, z, a, b, c)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        x, y, z, a, b, c = world[0], world[1], world[2], world[3], world[4], world[5]
        
        j0 = x + y
        j1 = x - y
        j2 = z
        j3 = a
        j4 = b
        j5 = c
        
        u = world[6] if len(world) > 6 else 0.0
        v = world[7] if len(world) > 7 else 0.0
        w = world[8] if len(world) > 8 else 0.0
        
        return [j0, j1, j2, j3, j4, j5, u, v, w]


class ScorbotKinematics(BaseKinematics):
    """Scorbot ER3运动学 - 基于 scorbot-kins.c"""
    
    L0_HORIZONTAL = 16.0
    L0_VERTICAL = 140.0
    L1 = 221.0
    L2 = 221.0
    
    def __init__(self, params: KinematicsParams = None, debug: bool = False):
        super().__init__(params)
        self.debug = debug    
    
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 4:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        j0, j1, j2 = joints[0], joints[1], joints[2]
        j0_rad, j1_rad, j2_rad = j0 * TO_RAD, j1 * TO_RAD, j2 * TO_RAD
        
        j1_x = self.L0_HORIZONTAL * math.cos(j0_rad)
        j1_y = self.L0_HORIZONTAL * math.sin(j0_rad)
        j1_z = self.L0_VERTICAL
        
        r1 = self.L1 * math.cos(j1_rad)
        j2_x = r1 * math.cos(j0_rad)
        j2_y = r1 * math.sin(j0_rad)
        j2_z = self.L1 * math.sin(j1_rad)
        
        r2 = self.L2 * math.cos(j2_rad)
        j3_x = r2 * math.cos(j0_rad)
        j3_y = r2 * math.sin(j0_rad)
        j3_z = self.L2 * math.sin(j2_rad)
        
        x = j1_x + j2_x + j3_x
        y = j1_y + j2_y + j3_y
        z = j1_z + j2_z + j3_z
        
        a = joints[3] if len(joints) > 3 else 0.0
        b = joints[4] if len(joints) > 4 else 0.0
        
        return (x, y, z, a, b, 0.0)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        x, y, z = world[0], world[1], world[2]
        
        j0 = math.atan2(y, x) * TO_DEG
        
        r_j1 = self.L0_HORIZONTAL
        z_j1 = self.L0_VERTICAL
        
        r_cp = math.sqrt(x*x + y*y) - r_j1
        z_cp = z - z_j1
        
        dist_to_cp = math.sqrt(r_cp*r_cp + z_cp*z_cp)
        dist_to_center = dist_to_cp / 2.0
        
        angle_to_cp = math.acos(r_cp / dist_to_cp) * TO_DEG if dist_to_cp > 0 else 0
        if z_cp < 0:
            angle_to_cp = -angle_to_cp
        
        if dist_to_center > self.L1:
            dist_to_center = self.L1
        
        j1_angle = math.acos(dist_to_center / self.L1) * TO_DEG
        j1 = angle_to_cp + j1_angle
        
        z_j2 = self.L1 * math.sin(j1 * TO_RAD)
        j2 = -math.asin(max(-1, min(1, (z_j2 - z_cp) / self.L2))) * TO_DEG
        
        j3 = world[3] if len(world) > 3 else 0.0
        j4 = world[4] if len(world) > 4 else 0.0
        
        return [j0, j1, j2, j3, j4, 0.0, 0.0, 0.0, 0.0]


@dataclass
class RoseParams(KinematicsParams):
    """Rose Engine 参数"""
    pass


class RoseKinematics(BaseKinematics):
    """Rose Engine运动学 - 基于 rosekins.c"""
    
    def __init__(self, params: RoseParams = None, debug: bool = False):
        super().__init__(params or RoseParams())
        self.debug = debug
        self._revolutions = 0
        self._old_quad = 0
    
    def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if len(joints) < 3:
            return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        radius, z, theta = joints[0], joints[1], joints[2]
        theta_rad = theta * TO_RAD
        
        x = radius * math.cos(theta_rad)
        y = radius * math.sin(theta_rad)
        
        return (x, y, z, 0.0, 0.0, 0.0)
    
    def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]:
        x, y, z = world[0], world[1], world[2]
        
        radius = math.hypot(x, y)
        theta = math.atan2(y, x) * TO_DEG
        
        if x >= 0 and y >= 0:
            now_quad = 1
        elif x < 0 and y >= 0:
            now_quad = 2
        elif x < 0 and y < 0:
            now_quad = 3
        else:
            now_quad = 4
        
        if self._old_quad == 2 and now_quad == 3:
            self._revolutions += 1
        elif self._old_quad == 3 and now_quad == 2:
            self._revolutions -= 1
        
        big_theta = theta + 360.0 * self._revolutions
        self._old_quad = now_quad
        
        if self.debug:
            print(f"[ROSE] INV: world({x:.3f},{y:.3f},{z:.3f}) -> joints(R={radius:.3f},Z={z:.3f},Theta={big_theta:.3f})")
        
        return [radius, z, big_theta, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


# ==================== 运动学工厂类 ====================

class KinematicsFactory:
    """运动学工厂类，根据类型创建对应的运动学实例"""
    
    _default_params = {
        KinematicsType.TRT_AC: {
            'rot_center_x': 0.0, 'rot_center_y': 0.0, 'rot_center_z': 0.0,
            'axis_offset_x': 0.0, 'axis_offset_y': 0.0, 'axis_offset_z': 0.0,
            'tool_length': 100.0, 'conventional_directions': False
        },
        KinematicsType.TRT_BC: {
            'rot_center_x': 0.0, 'rot_center_y': 0.0, 'rot_center_z': 0.0,
            'axis_offset_x': 0.0, 'axis_offset_y': 0.0, 'axis_offset_z': 0.0,
            'tool_length': 100.0, 'conventional_directions': False
        },
        KinematicsType.MAXKINS_BC: {
            'pivot_length': 250.0, 'tool_length': 100.0, 'conventional_directions': False
        },
        KinematicsType.FIVEAXIS_BC: {
            'pivot_length': 250.0, 'tool_length': 100.0
        },
        KinematicsType.HEXAPOD: {
            'tool_offset': 100.0, 'spindle_offset': 0.0,
            'convergence_criterion': 1e-9, 'max_iterations': 120
        },
        KinematicsType.PUMA: {
            'a2': 300.0, 'a3': 50.0, 'd3': 70.0, 'd4': 400.0, 'd6': 70.0
        },
        KinematicsType.SCARA: {
            'd1': 490.0, 'd2': 340.0, 'd3': 50.0,
            'd4': 250.0, 'd5': 50.0, 'd6': 50.0
        },
        KinematicsType.SCORBOT: {},
        KinematicsType.LINEAR_DELTA: {
            'radius': 165.25, 'rod_length': 269.0
        },
        KinematicsType.ROTARY_DELTA: {
            'platform_radius': 10.0, 'thigh_length': 10.0,
            'shin_length': 14.0, 'foot_radius': 6.0
        },
        KinematicsType.TRIPOD: {
            'Bx': 1.0, 'Cx': 1.0, 'Cy': 1.0
        },
        KinematicsType.PENTAPOD: {},
        KinematicsType.ROTATE: {},
        KinematicsType.COREXY: {},
        KinematicsType.ROSE: {},
    }
    
    @classmethod
    def create(cls, kin_type: KinematicsType, debug: bool = False, **params) -> Optional[BaseKinematics]:
        if kin_type == KinematicsType.IDENTITY:
            return None
        
        merged_params = cls._default_params.get(kin_type, {}).copy()
        merged_params.update(params)
        
        if kin_type == KinematicsType.TRT_AC:
            p = TRTParams(**{k: v for k, v in merged_params.items() if hasattr(TRTParams, k)})
            return XYZAC_TRTKinematics(p, debug=debug)
        elif kin_type == KinematicsType.TRT_BC:
            p = TRTParams(**{k: v for k, v in merged_params.items() if hasattr(TRTParams, k)})
            return XYZBC_TRTKinematics(p, debug=debug)
        elif kin_type == KinematicsType.MAXKINS_BC:
            return MaxkinsBCKinematics(debug=debug, **merged_params)
        elif kin_type == KinematicsType.FIVEAXIS_BC:
            return FiveAxisBCKinematics(debug=debug, **merged_params)
        elif kin_type == KinematicsType.HEXAPOD:
            p = HexapodParams(**{k: v for k, v in merged_params.items() if hasattr(HexapodParams, k)})
            return HexapodKinematics(p, debug=debug)
        elif kin_type == KinematicsType.PUMA:
            p = PUMAParams(**{k: v for k, v in merged_params.items() if hasattr(PUMAParams, k)})
            return PUMAKinematics(p, debug=debug)
        elif kin_type == KinematicsType.SCARA:
            p = SCARAParams(**{k: v for k, v in merged_params.items() if hasattr(SCARAParams, k)})
            return SCARAKinematics(p, debug=debug)
        elif kin_type == KinematicsType.SCORBOT:
            return ScorbotKinematics(debug=debug)
        elif kin_type == KinematicsType.LINEAR_DELTA:
            return LinearDeltaKinematics(debug=debug, **merged_params)
        elif kin_type == KinematicsType.ROTARY_DELTA:
            p = RotaryDeltaParams(**{k: v for k, v in merged_params.items() if hasattr(RotaryDeltaParams, k)})
            return RotaryDeltaKinematics(p, debug=debug)
        elif kin_type == KinematicsType.TRIPOD:
            p = TripodParams(**{k: v for k, v in merged_params.items() if hasattr(TripodParams, k)})
            return TripodKinematics(p, debug=debug)
        elif kin_type == KinematicsType.PENTAPOD:
            p = PentapodParams(**{k: v for k, v in merged_params.items() if hasattr(PentapodParams, k)})
            return PentapodKinematics(p, debug=debug)
        elif kin_type == KinematicsType.ROTATE:
            return RotateKinematics(debug=debug)
        elif kin_type == KinematicsType.COREXY:
            return CoreXYKinematics(debug=debug)
        elif kin_type == KinematicsType.ROSE:
            p = RoseParams()
            return RoseKinematics(p, debug=debug)
        elif kin_type == KinematicsType.SERIAL_DH:
            p = SerialDHParams(**{k: v for k, v in merged_params.items() if hasattr(SerialDHParams, k)})
            return SerialDHKinematics(p, debug=debug)
        
        return None
    
    @classmethod
    def get_supported_types(cls) -> List[KinematicsType]:
        return list(cls._default_params.keys())
    
    @classmethod
    def get_type_info(cls, kin_type: KinematicsType) -> Dict[str, Any]:
        info = {
            KinematicsType.IDENTITY: {'name': 'IDENTITY', 'description': '三轴恒等运动学', 'joints': 3, 'source': 'trivkins.c'},
            KinematicsType.TRT_AC: {'name': 'TRT_AC', 'description': 'XYZAC 双转台五轴', 'joints': 5, 'source': 'trtfuncs.c'},
            KinematicsType.TRT_BC: {'name': 'TRT_BC', 'description': 'XYZBC 双转台五轴', 'joints': 5, 'source': 'trtfuncs.c'},
            KinematicsType.MAXKINS_BC: {'name': 'MAXKINS_BC', 'description': 'Max 五轴铣床', 'joints': 9, 'source': 'maxkins.c'},
            KinematicsType.FIVEAXIS_BC: {'name': 'FIVEAXIS_BC', 'description': '五轴桥式铣床', 'joints': 6, 'source': '5axiskins.c'},
            KinematicsType.HEXAPOD: {'name': 'HEXAPOD', 'description': 'Stewart 六足平台', 'joints': 6, 'source': 'genhexkins.c'},
            KinematicsType.PUMA: {'name': 'PUMA', 'description': 'PUMA 560 机器人', 'joints': 6, 'source': 'pumakins.c'},
            KinematicsType.SCARA: {'name': 'SCARA', 'description': 'SCARA 机器人', 'joints': 4, 'source': 'scarakins.c'},
            KinematicsType.SCORBOT: {'name': 'SCORBOT', 'description': 'Scorbot ER3', 'joints': 5, 'source': 'scorbot-kins.c'},
            KinematicsType.LINEAR_DELTA: {'name': 'LINEAR_DELTA', 'description': '线性Delta机器人', 'joints': 3, 'source': 'lineardeltakins.c'},
            KinematicsType.ROTARY_DELTA: {'name': 'ROTARY_DELTA', 'description': '旋转Delta机器人', 'joints': 3, 'source': 'rotarydeltakins.c'},
            KinematicsType.TRIPOD: {'name': 'TRIPOD', 'description': 'Tripod机器人', 'joints': 3, 'source': 'tripodkins.c'},
            KinematicsType.PENTAPOD: {'name': 'PENTAPOD', 'description': 'Pentapod机器人', 'joints': 5, 'source': 'pentakins.c'},
            KinematicsType.ROTATE: {'name': 'ROTATE', 'description': '旋转工作台', 'joints': 9, 'source': 'rotatekins.c'},
            KinematicsType.COREXY: {'name': 'COREXY', 'description': 'CoreXY运动学', 'joints': 9, 'source': 'corexykins.c'},
            KinematicsType.ROSE: {'name': 'ROSE', 'description': 'Rose Engine', 'joints': 3, 'source': 'rosekins.c'},
        }
        return info.get(kin_type, {'name': 'UNKNOWN', 'description': '未知类型'})


# ==================== 五轴运动学统一接口 (向后兼容) ====================
@dataclass
class KinematicsParamsLegacy:
    rot_center_x: float = 0.0
    rot_center_y: float = 0.0
    rot_center_z: float = 0.0
    axis_offset_x: float = 0.0
    axis_offset_y: float = 0.0
    axis_offset_z: float = 0.0
    pivot_length: float = 250.0
    tool_length: float = 0.0
    conventional_directions: bool = False


class FiveAxisKinematics:
    def __init__(self, kin_type: KinematicsType = KinematicsType.TRT_BC, debug: bool = False):
        self.kin_type = kin_type
        self.debug = debug
        self.rtcp_enabled = False
        self.params = KinematicsParamsLegacy()
        self._kinematics = KinematicsFactory.create(kin_type, debug=debug)
    
    def set_params(self, **kwargs):
        for key, value in kwargs.items():
            if hasattr(self.params, key):
                setattr(self.params, key, value)
        if self._kinematics:
            param_dict = {
                'rot_center_x': self.params.rot_center_x,
                'rot_center_y': self.params.rot_center_y,
                'rot_center_z': self.params.rot_center_z,
                'axis_offset_x': self.params.axis_offset_x,
                'axis_offset_y': self.params.axis_offset_y,
                'axis_offset_z': self.params.axis_offset_z,
                'tool_length': self.params.tool_length,
                'conventional_directions': self.params.conventional_directions,
                'pivot_length': self.params.pivot_length,
            }
            self._kinematics.set_params(**param_dict)
    
    def set_tool_length(self, length: float):
        self.params.tool_length = length
        if self._kinematics:
            self._kinematics.set_tool_length(length)
    
    def enable_rtcp(self, enable: bool = True):
        self.rtcp_enabled = enable
        if self._kinematics:
            self._kinematics.enable_rtcp(enable)
    
    def forward_transform(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]:
        if not self.rtcp_enabled or self._kinematics is None:
            if len(joints) >= 6:
                return (joints[0], joints[1], joints[2], joints[3], joints[4], joints[5])
            return (joints[0], joints[1], joints[2], 0, 0, 0)
        return self._kinematics.forward(joints)
    
    def inverse_transform(self, pos: Tuple[float, float, float, float, float, float]) -> List[float]:
        if not self.rtcp_enabled or self._kinematics is None:
            return [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]]
        return self._kinematics.inverse(pos)
    

# ==================== LineCodeWrapper ====================
class LineCodeWrapper:
    """行代码包装器"""
    def __init__(self, seq_num: int, plane: int = 17):
        self.sequence_number = seq_num
        self.plane = plane
        self.x = 0.0
        self.y = 0.0
        self.z = 0.0
        self.a = 0.0
        self.b = 0.0
        self.c = 0.0
        self.u = 0.0
        self.v = 0.0
        self.w = 0.0
        self.i = 0.0
        self.j = 0.0
        self.k = 0.0
        self.r = 0.0
        self.f = 0.0
        self.s = 0.0
        self.t = 0
        self.h = -1
        self.d = -1
        self.p = -1.0
        self.q = -1.0
        self.l = -1
        self.comment = ""


@dataclass
class CNCRuntimeStatus:
    """CNC运行时状态"""
    position_x: float = 0.0
    position_y: float = 0.0
    position_z: float = 0.0
    position_a: float = 0.0
    position_b: float = 0.0
    position_c: float = 0.0
    position_u: float = 0.0
    position_v: float = 0.0
    position_w: float = 0.0
    
    world_x: float = 0.0
    world_y: float = 0.0
    world_z: float = 0.0
    
    machine_x: float = 0.0
    machine_y: float = 0.0
    machine_z: float = 0.0
    machine_a: float = 0.0
    machine_b: float = 0.0
    machine_c: float = 0.0
    
    spindle_speed: float = 0.0
    spindle_state: str = "OFF"
    spindle_mode: int = 0
    spindle_override_enabled: bool = True
    
    coolant_flood: bool = False
    coolant_mist: bool = False
    coolant_state: str = "OFF"
    
    current_tool: int = 0
    selected_tool: int = 1
    tool_length: float = 0.0
    tool_diameter: float = 0.0
    tool_offset: Point6D = field(default_factory=Point6D)
    
    feedrate: float = 0.0
    feed_override: int = 100
    rapid_override: int = 100
    spindle_override: int = 100
    feed_override_enabled: bool = True
    adaptive_feed_enabled: bool = False
    feed_hold_enabled: bool = True
    
    state: str = MachineState.OFF.value
    current_line: int = 0
    total_lines: int = 0
    progress_percent: float = 0.0
    
    coordinate_mode: str = "G90"
    plane_mode: str = "G17"
    unit_mode: str = "G21"
    cutter_comp: str = "G40"
    current_offset: str = "G54"
    distance_mode: int = 90
    feed_mode: int = 94
    ijk_distance_mode: int = 91
    retract_mode: int = 98
    control_mode: int = 610
    lathe_diameter_mode: bool = False
    
    alarm_code: int = 0
    alarm_message: str = ""
    
    rtcp_enabled: bool = False
    kinematics_type: str = "TRT_BC"
    
    program_name: str = ""
    path_length: float = 0.0
    total_time: float = 0.0
    path_segments: int = 0
    
    g92_applied: bool = False
    g92_x: float = 0.0
    g92_y: float = 0.0
    g92_z: float = 0.0
    g92_a: float = 0.0             # ★ 添加
    g92_b: float = 0.0             # ★ 添加
    g92_c: float = 0.0             # ★ 添加
    
    g5x_index: int = 1
    g5x_offset_x: float = 0.0
    g5x_offset_y: float = 0.0
    g5x_offset_z: float = 0.0
    g5x_offset_a: float = 0.0      # ★ 添加
    g5x_offset_b: float = 0.0      # ★ 添加
    g5x_offset_c: float = 0.0      # ★ 添加
    
    work_offset_x: float = 0.0
    work_offset_y: float = 0.0
    work_offset_z: float = 0.0
    
    rotation_xy: float = 0.0
    
    motion_mode: int = -1
    current_pocket: int = 0
    selected_pocket: int = 0
    
    probe_tripped: bool = False
    probe_x: float = 0.0
    probe_y: float = 0.0
    probe_z: float = 0.0
    
    block_delete: bool = False
    optional_stop: bool = True
    
    cycle_active: bool = False
    cycle_type: int = 0
    cycle_r: float = 0.0
    cycle_z: float = 0.0
    cycle_q: float = 0.0
    cycle_l: int = 1
    
    def to_dict(self) -> Dict[str, Any]:
        """转换为字典"""
        return {
            'position': {'x': self.position_x, 'y': self.position_y, 'z': self.position_z,
                         'a': self.position_a, 'b': self.position_b, 'c': self.position_c},
            'world_position': {'x': self.world_x, 'y': self.world_y, 'z': self.world_z},
            'machine_position': {'x': self.machine_x, 'y': self.machine_y, 'z': self.machine_z},
            'spindle': {'speed': self.spindle_speed, 'state': self.spindle_state},
            'coolant': {'flood': self.coolant_flood, 'mist': self.coolant_mist},
            'tool': {'current': self.current_tool, 'selected': self.selected_tool,
                     'length': self.tool_length, 'diameter': self.tool_diameter},
            'feedrate': self.feedrate,
            'overrides': {'feed': self.feed_override, 'rapid': self.rapid_override,
                          'spindle': self.spindle_override},
            'state': self.state,
            'line': self.current_line,
            'progress': self.progress_percent,
            'modes': {'coordinate': self.coordinate_mode, 'plane': self.plane_mode,
                      'units': self.unit_mode, 'cutter_comp': self.cutter_comp,
                      'offset': self.current_offset},
            'rtcp_enabled': self.rtcp_enabled,
            'kinematics_type': self.kinematics_type,
            'program_name': self.program_name,
        }


# ==================== Translated 类（处理坐标系变换） ====================
class Translated:
    """处理 G5x 和 G92 坐标系偏移以及 XY 旋转"""
    
    def __init__(self):
        self.g92_offset_x = 0.0
        self.g92_offset_y = 0.0
        self.g92_offset_z = 0.0
        self.g92_offset_a = 0.0
        self.g92_offset_b = 0.0
        self.g92_offset_c = 0.0
        self.g92_offset_u = 0.0
        self.g92_offset_v = 0.0
        self.g92_offset_w = 0.0
        
        self.g5x_offset_x = 0.0
        self.g5x_offset_y = 0.0
        self.g5x_offset_z = 0.0
        self.g5x_offset_a = 0.0
        self.g5x_offset_b = 0.0
        self.g5x_offset_c = 0.0
        self.g5x_offset_u = 0.0
        self.g5x_offset_v = 0.0
        self.g5x_offset_w = 0.0
        
        self.g52_offset_x = 0.0
        self.g52_offset_y = 0.0
        self.g52_offset_z = 0.0
        self.g52_offset_a = 0.0
        self.g52_offset_b = 0.0
        self.g52_offset_c = 0.0
        self.g52_offset_u = 0.0
        self.g52_offset_v = 0.0
        self.g52_offset_w = 0.0
        
        self.rotation_xy = 0.0
        self.rotation_sin = 0.0
        self.rotation_cos = 1.0
        
        self.g5x_index = 1
        
        self._g53_active = False
    
    def rotate_and_translate(self, x: float, y: float, z: float, 
                             a: float, b: float, c: float, 
                             u: float, v: float, w: float) -> Tuple[float, ...]:
        """应用 G5x、G92、G52 偏移和 XY 旋转"""
        if self._g53_active:
            # G53 模式下不应用任何偏移
            return (x, y, z, a, b, c, u, v, w)
        
        # 应用 G92 偏移
        x += self.g92_offset_x
        y += self.g92_offset_y
        z += self.g92_offset_z
        a += self.g92_offset_a
        b += self.g92_offset_b
        c += self.g92_offset_c
        u += self.g92_offset_u
        v += self.g92_offset_v
        w += self.g92_offset_w
        
        # 应用 G52 偏移
        x += self.g52_offset_x
        y += self.g52_offset_y
        z += self.g52_offset_z
        a += self.g52_offset_a
        b += self.g52_offset_b
        c += self.g52_offset_c
        
        # XY 旋转
        if self.rotation_xy != 0:
            rotx = x * self.rotation_cos - y * self.rotation_sin
            roty = x * self.rotation_sin + y * self.rotation_cos
            x, y = rotx, roty
        
        # 应用 G5x 偏移
        x += self.g5x_offset_x
        y += self.g5x_offset_y
        z += self.g5x_offset_z
        a += self.g5x_offset_a
        b += self.g5x_offset_b
        c += self.g5x_offset_c
        u += self.g5x_offset_u
        v += self.g5x_offset_v
        w += self.g5x_offset_w
        
        return (x, y, z, a, b, c, u, v, w)
    
    def reverse_rotate_and_translate(self, x: float, y: float, z: float,
                                      a: float, b: float, c: float,
                                      u: float, v: float, w: float) -> Tuple[float, ...]:
        """反向应用偏移（用于从机器坐标计算工件坐标）"""
        if self._g53_active:
            return (x, y, z, a, b, c, u, v, w)
        
        # 减去 G5x 偏移
        x -= self.g5x_offset_x
        y -= self.g5x_offset_y
        z -= self.g5x_offset_z
        a -= self.g5x_offset_a
        b -= self.g5x_offset_b
        c -= self.g5x_offset_c
        u -= self.g5x_offset_u
        v -= self.g5x_offset_v
        w -= self.g5x_offset_w
        
        # 反向 XY 旋转
        if self.rotation_xy != 0:
            rotx = x * self.rotation_cos + y * self.rotation_sin
            roty = -x * self.rotation_sin + y * self.rotation_cos
            x, y = rotx, roty
        
        # 减去 G52 偏移
        x -= self.g52_offset_x
        y -= self.g52_offset_y
        z -= self.g52_offset_z
        a -= self.g52_offset_a
        b -= self.g52_offset_b
        c -= self.g52_offset_c
        
        # 减去 G92 偏移
        x -= self.g92_offset_x
        y -= self.g92_offset_y
        z -= self.g92_offset_z
        a -= self.g92_offset_a
        b -= self.g92_offset_b
        c -= self.g92_offset_c
        u -= self.g92_offset_u
        v -= self.g92_offset_v
        w -= self.g92_offset_w
        
        return (x, y, z, a, b, c, u, v, w)
    
    def set_g5x_offset(self, csys: int, x: float = None, y: float = None, z: float = None,
                       a: float = 0.0, b: float = 0.0, c: float = 0.0,
                       u: float = 0.0, v: float = 0.0, w: float = 0.0):
        """
        G10 L2: 直接设置坐标系偏移
        
        完全参照 LinuxCNC 官方实现：
        rs274ngc.cpp: Interp::convert_setup (G10 L2 分支)
        
        Args:
            csys: 坐标系编号 (1-9)
            x, y, z, a, b, c, u, v, w: 新的偏移值（程序单位）
        """
        if csys < 1 or csys > 9:
            if self.debug:
                print(f"[G10 L2] 错误: 坐标系编号 {csys} 超出范围 (1-9)")
            return
        
        if csys not in self.work_offsets:
            self.work_offsets[csys] = {}
        
        # 获取旧偏移
        old_offsets = self.work_offsets[csys].copy()
        old_g5x_x = old_offsets.get('X', 0.0)
        old_g5x_y = old_offsets.get('Y', 0.0)
        old_g5x_z = old_offsets.get('Z', 0.0)
        old_g5x_a = old_offsets.get('A', 0.0)
        old_g5x_b = old_offsets.get('B', 0.0)
        old_g5x_c = old_offsets.get('C', 0.0)
        
        # 使用新值或保留旧值
        new_x = x if x is not None else old_g5x_x
        new_y = y if y is not None else old_g5x_y
        new_z = z if z is not None else old_g5x_z
        new_a = a if a is not None else old_g5x_a
        new_b = b if b is not None else old_g5x_b
        new_c = c if c is not None else old_g5x_c
        new_u = u if u is not None else old_offsets.get('U', 0.0)
        new_v = v if v is not None else old_offsets.get('V', 0.0)
        new_w = w if w is not None else old_offsets.get('W', 0.0)
        new_r = old_offsets.get('R', 0.0)  # G10 L2 不改变旋转角度
        
        if self.debug:
            print(f"[G10 L2] ========== 设置坐标系 G{53 + csys} ==========")
            print(f"[G10 L2] 旧偏移: X={old_g5x_x:.3f}, Y={old_g5x_y:.3f}, Z={old_g5x_z:.3f}")
            print(f"[G10 L2] 新偏移: X={new_x:.3f}, Y={new_y:.3f}, Z={new_z:.3f}")
            print(f"[G10 L2] 是否为当前坐标系: {csys == self.g5x_index}")
        
        # ===== 如果是当前坐标系，需要调整当前位置 =====
        if csys == self.g5x_index:
            # 第1步：正向旋转当前坐标（去掉旧 XY 旋转）
            if self.rotation_xy != 0:
                rot_rad = math.radians(self.rotation_xy)
                cos_r = math.cos(rot_rad)
                sin_r = math.sin(rot_rad)
                rot_x = self.current_pos.x * cos_r - self.current_pos.y * sin_r
                rot_y = self.current_pos.x * sin_r + self.current_pos.y * cos_r
                self.current_pos.x = rot_x
                self.current_pos.y = rot_y
            
            # 第2步：加上旧 G5x 偏移
            self.current_pos.x += old_g5x_x
            self.current_pos.y += old_g5x_y
            self.current_pos.z += old_g5x_z
            self.current_pos.a += old_g5x_a
            self.current_pos.b += old_g5x_b
            self.current_pos.c += old_g5x_c
            
            # 第3步：设置新 G5x 偏移
            self.g5x_offset_x = new_x
            self.g5x_offset_y = new_y
            self.g5x_offset_z = new_z
            self.g5x_offset_a = new_a
            self.g5x_offset_b = new_b
            self.g5x_offset_c = new_c
            
            # 第4步：减去新 G5x 偏移
            self.current_pos.x -= new_x
            self.current_pos.y -= new_y
            self.current_pos.z -= new_z
            self.current_pos.a -= new_a
            self.current_pos.b -= new_b
            self.current_pos.c -= new_c
            
            if self.debug:
                print(f"[G10 L2] 调整后工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})")
        
        # ===== 存储新偏移 =====
        self.work_offsets[csys] = {
            'X': new_x, 'Y': new_y, 'Z': new_z,
            'A': new_a, 'B': new_b, 'C': new_c,
            'U': new_u, 'V': new_v, 'W': new_w,
            'R': new_r
        }
        
        # ===== 同步到参数表和机器坐标 =====
        if hasattr(self, 'var_manager') and self.var_manager:
            self.var_manager.set_csys_origin(csys, new_x, new_y, new_z, new_a, new_b, new_c)
        
        self._sync_machine_from_current()
        self._sync_parameters_from_state()
        
        if self.debug:
            print(f"[G10 L2] ========== G10 L2 完成 ==========")
    
    def set_g92_offset(self, x: float = 0.0, y: float = 0.0, z: float = 0.0,
                       a: float = 0.0, b: float = 0.0, c: float = 0.0,
                       u: float = 0.0, v: float = 0.0, w: float = 0.0):
        """G92: 设置临时坐标系偏移"""
        self.g92_offset_x = x
        self.g92_offset_y = y
        self.g92_offset_z = z
        self.g92_offset_a = a
        self.g92_offset_b = b
        self.g92_offset_c = c
        self.g92_offset_u = u
        self.g92_offset_v = v
        self.g92_offset_w = w
    
    def apply_g92_from_current(self, target_x: float = None, target_y: float = None, 
                                target_z: float = None, current_x: float = 0.0,
                                current_y: float = 0.0, current_z: float = 0.0):
        """根据当前坐标计算并应用 G92 偏移"""
        if target_x is not None:
            self.g92_offset_x = current_x + self.g92_offset_x - target_x
        if target_y is not None:
            self.g92_offset_y = current_y + self.g92_offset_y - target_y
        if target_z is not None:
            self.g92_offset_z = current_z + self.g92_offset_z - target_z
    
    def clear_g92_offset(self):
        """G92.1: 清除 G92 偏移"""
        self.g92_offset_x = 0.0
        self.g92_offset_y = 0.0
        self.g92_offset_z = 0.0
        self.g92_offset_a = 0.0
        self.g92_offset_b = 0.0
        self.g92_offset_c = 0.0
        self.g92_offset_u = 0.0
        self.g92_offset_v = 0.0
        self.g92_offset_w = 0.0
    
    def set_g52_offset(self, x: float = 0.0, y: float = 0.0, z: float = 0.0,
                       a: float = 0.0, b: float = 0.0, c: float = 0.0):
        """G52: 设置临时工件坐标系偏移"""
        self.g52_offset_x = x
        self.g52_offset_y = y
        self.g52_offset_z = z
        self.g52_offset_a = a
        self.g52_offset_b = b
        self.g52_offset_c = c
    
    def clear_g52_offset(self):
        """清除 G52 偏移"""
        self.g52_offset_x = 0.0
        self.g52_offset_y = 0.0
        self.g52_offset_z = 0.0
        self.g52_offset_a = 0.0
        self.g52_offset_b = 0.0
        self.g52_offset_c = 0.0
    
    def set_xy_rotation(self, theta: float):
        """设置 XY 旋转角度"""
        self.rotation_xy = theta
        t = math.radians(theta)
        self.rotation_sin = math.sin(t)
        self.rotation_cos = math.cos(t)
        
        # ========== 新增：同步到参数表 ==========
        # 如果有 var_manager 属性则同步
        if hasattr(self, 'var_manager') and self.var_manager is not None:
            self.var_manager.set_xy_rotation(theta)     
    
    def set_g53_active(self, active: bool = True):
        """设置 G53 模式"""
        self._g53_active = active


# ==================== ArcsToSegmentsMixin 类 ====================
class ArcsToSegmentsMixin:
    """圆弧转线段的混入类"""
    
    plane = 17
    arcdivision = 64
    
    def set_plane(self, plane: int):
        self.plane = plane
    
    def set_arc_division(self, division: int):
        self.arcdivision = division
    
    def arc_feed_to_segments(self, x1: float, y1: float, cx: float, cy: float, 
                             rot: int, z1: float, a: float, b: float, c: float, 
                             u: float, v: float, w: float):
        """将圆弧转换为线段"""
        segs = arcs_to_segments_python(
            self, x1, y1, cx, cy, rot, z1, a, b, c, u, v, w, self.arcdivision
        )
        self.straight_arcsegments(segs)
    
    def straight_arcsegments(self, segs: List[Tuple[float, ...]]):
        """处理圆弧线段（由子类实现）"""
        raise NotImplementedError


# ==================== 纯 Python GLCanon 类 ====================
class GLCanonPure(Translated, ArcsToSegmentsMixin):
    """纯 Python 实现的 GLCanon 类"""
    
    def __init__(self, colors=None, geometry="XYZ", is_foam=0):
        super().__init__()
        
        if colors is None:
            colors = DEFAULT_COLORS.copy()
        
        self.colors = colors
        self.geometry = geometry
        self.is_foam = is_foam
        
        self.traverse: List[Tuple] = []
        self.feed: List[Tuple] = []
        self.arcfeed: List[Tuple] = []
        self.dwells: List[Tuple] = []
        self.tool_list: List[int] = []
        
        self.preview_zero_rxy: List[Tuple] = []
        
        self.lineno = -1
        self.first_move = True
        self.feedrate = 1.0
        self.lo = [0.0] * 9
        self.xo = self.yo = self.zo = self.ao = self.bo = self.co = self.uo = self.vo = self.wo = 0.0
        
        self.min_extents = [9e99, 9e99, 9e99]
        self.max_extents = [-9e99, -9e99, -9e99]
        self.min_extents_notool = [9e99, 9e99, 9e99]
        self.max_extents_notool = [-9e99, -9e99, -9e99]
        self.min_extents_zero_rxy = [9e99, 9e99, 9e99]
        self.max_extents_zero_rxy = [-9e99, -9e99, -9e99]
        self.min_extents_notool_zero_rxy = [9e99, 9e99, 9e99]
        self.max_extents_notool_zero_rxy = [-9e99, -9e99, -9e99]
        
        self.foam_z = 0.0
        self.foam_w = 1.5
        
        self.in_arc = 0
        self.suppress = 0
        self.dwell_time = 0.0
        self.notify = 0
        self.notify_message = ""
        self.highlight_line = None
        
        self.state = type('State', (), {})()
        self.state.plane = 17
        self.state.feedrate = 1.0
        self.state.spindle_speed = 0.0
        self.state.spindle_mode = 0
        self.state.tool = 0
        self.state.units = 21
        
        self.g92_offset_u = 0.0
        self.g92_offset_v = 0.0
        self.g92_offset_w = 0.0
        
        self.g5x_offset_u = 0.0
        self.g5x_offset_v = 0.0
        self.g5x_offset_w = 0.0
    
    def comment(self, arg: str):
        """处理注释"""
        if arg.startswith("AXIS,") or arg.startswith("PREVIEW,"):
            parts = arg.split(",")
            command = parts[1] if len(parts) > 1 else ""
            if command == "stop":
                raise KeyboardInterrupt("Preview stop requested")
            elif command == "hide":
                self.suppress += 1
            elif command == "show":
                self.suppress -= 1
            elif command == "XY_Z_POS" and len(parts) > 2:
                try:
                    self.foam_z = float(parts[2])
                except ValueError:
                    pass
            elif command == "UV_Z_POS" and len(parts) > 2:
                try:
                    self.foam_w = float(parts[2])
                except ValueError:
                    pass
            elif command == "notify":
                self.notify += 1
                self.notify_message = "(AXIS,notify):" + str(self.notify)
                if len(parts) > 2 and parts[2]:
                    self.notify_message = parts[2]
    
    def message(self, message: str):
        """处理消息"""
        pass
    
    def check_abort(self):
        """检查是否中止"""
        pass
    
    def next_line(self, st: LineCodeWrapper):
        """处理下一行"""
        self.state = st
        self.lineno = getattr(st, 'sequence_number', -1)
    
    def calc_extents(self):
        """计算包围盒"""
        if not self.arcfeed and not self.feed and not self.traverse:
            self.min_extents = self.max_extents = [0, 0, 0]
            self.min_extents_notool = self.max_extents_notool = [0, 0, 0]
            self.min_extents_zero_rxy = self.max_extents_zero_rxy = [0, 0, 0]
            self.min_extents_notool_zero_rxy = self.max_extents_notool_zero_rxy = [0, 0, 0]
            return
        
        (min_p, max_p, min_pt, max_pt) = calc_extents_python(self.arcfeed, self.feed, self.traverse)
        
        self.min_extents = min_p
        self.max_extents = max_p
        self.min_extents_notool = min_pt
        self.max_extents_notool = max_pt
        
        self.unrotate_preview()
        (min_pz, max_pz, min_ptz, max_ptz) = calc_extents_python(self.preview_zero_rxy)
        
        self.min_extents_zero_rxy = min_pz
        self.max_extents_zero_rxy = max_pz
        self.min_extents_notool_zero_rxy = min_ptz
        self.max_extents_notool_zero_rxy = max_ptz
        
        if self.is_foam:
            min_z = min(self.foam_z, self.foam_w)
            max_z = max(self.foam_z, self.foam_w)
            self.min_extents[2] = min_z
            self.max_extents[2] = max_z
            self.min_extents_notool[2] = min_z
            self.max_extents_notool[2] = max_z
    
    def unrotate_preview(self):
        """反向旋转预览数据"""
        angle = math.radians(-self.rotation_xy)
        cos = math.cos(angle)
        sin = math.sin(angle)
        g5x_x = self.g5x_offset_x
        g5x_y = self.g5x_offset_y
        
        self.preview_zero_rxy = []
        
        for lst in [self.feed, self.arcfeed]:
            for item in lst:
                if len(item) == 5:
                    linenum, start, end, feed, tooloffset = item
                else:
                    continue
                
                tsx = start[0] - g5x_x
                tsy = start[1] - g5x_y
                tex = end[0] - g5x_x
                tey = end[1] - g5x_y
                
                rsx = tsx * cos - tsy * sin + g5x_x
                rsy = tsx * sin + tsy * cos + g5x_y
                rex = tex * cos - tey * sin + g5x_x
                rey = tex * sin + tey * cos + g5x_y
                
                self.preview_zero_rxy.append((
                    linenum, (rsx, rsy) + start[2:], (rex, rey) + end[2:], feed, tooloffset
                ))
        
        for item in self.traverse:
            if len(item) == 4:
                linenum, start, end, tooloffset = item
            else:
                continue
            
            tsx = start[0] - g5x_x
            tsy = start[1] - g5x_y
            tex = end[0] - g5x_x
            tey = end[1] - g5x_y
            
            rsx = tsx * cos - tsy * sin + g5x_x
            rsy = tsx * sin + tsy * cos + g5x_y
            rex = tex * cos - tey * sin + g5x_x
            rey = tex * sin + tey * cos + g5x_y
            
            self.preview_zero_rxy.append((
                linenum, (rsx, rsy) + start[2:], (rex, rey) + end[2:], tooloffset
            ))
    
    def tool_offset(self, xo: float, yo: float, zo: float, 
                    ao: float, bo: float, co: float, 
                    uo: float, vo: float, wo: float):
        """设置刀具偏移"""
        self.first_move = True
        x, y, z, a, b, c, u, v, w = self.lo
        self.lo = (
            x - xo + self.xo, y - yo + self.yo, z - zo + self.zo,
            a - ao + self.ao, b - bo + self.bo, c - co + self.co,
            u - uo + self.uo, v - vo + self.vo, w - wo + self.wo
        )
        self.xo, self.yo, self.zo = xo, yo, zo
        self.ao, self.bo, self.co = ao, bo, co
        self.uo, self.vo, self.wo = uo, vo, wo
    
    def set_spindle_rate(self, arg: float):
        """设置主轴转速"""
        pass
    
    def set_feed_rate(self, arg: float):
        """设置进给率"""
        self.feedrate = arg / 60.0
    
    def select_plane(self, plane: int):
        """选择平面"""
        pass
    
    def change_tool(self, arg: int):
        """换刀"""
        self.first_move = True
        try:
            self.tool_list.append(arg)
        except Exception as e:
            print(f"Error in change_tool: {e}")
    
    def straight_traverse(self, x: float, y: float, z: float, 
                          a: float, b: float, c: float, 
                          u: float, v: float, w: float):
        """快速移动"""
        if self.suppress > 0:
            return
        l = self.rotate_and_translate(x, y, z, a, b, c, u, v, w)
        if not self.first_move:
            self.traverse.append((self.lineno, self.lo, l, (self.xo, self.yo, self.zo)))
        self.lo = l
        self.first_move = False
    
    def straight_feed(self, x: float, y: float, z: float, 
                      a: float, b: float, c: float, 
                      u: float, v: float, w: float):
        """直线进给"""
        if self.suppress > 0:
            return
        self.first_move = False
        l = self.rotate_and_translate(x, y, z, a, b, c, u, v, w)
        self.feed.append((self.lineno, self.lo, l, self.feedrate, (self.xo, self.yo, self.zo)))
        self.lo = l
    
    straight_probe = straight_feed
    
    def arc_feed(self, x: float, y: float, z: float, 
                 a: float, b: float, c: float, 
                 i: float, j: float, k: float, turn: int, feed: float, 
                 u: float = 0.0, v: float = 0.0, w: float = 0.0):
        """圆弧进给"""
        if self.suppress > 0:
            return
        self.first_move = False
        self.in_arc = True
        try:
            self.arc_feed_to_segments(x, y, i, j, turn, z, a, b, c, u, v, w)
        finally:
            self.in_arc = False
    
    def straight_arcsegments(self, segs: List[Tuple[float, ...]]):
        """处理圆弧线段"""
        self.first_move = False
        lo = self.lo
        lineno = self.lineno
        feedrate = self.feedrate
        to = (self.xo, self.yo, self.zo)
        
        for l in segs:
            self.arcfeed.append((lineno, lo, l, feedrate, to))
            lo = l
        self.lo = lo
    
    def rigid_tap(self, x: float, y: float, z: float):
        """刚性攻丝"""
        if self.suppress > 0:
            return
        self.first_move = False
        l = self.rotate_and_translate(x, y, z, 0, 0, 0, 0, 0, 0)[:3]
        l += (self.lo[3], self.lo[4], self.lo[5], self.lo[6], self.lo[7], self.lo[8])
        self.feed.append((self.lineno, self.lo, l, self.feedrate, (self.xo, self.yo, self.zo)))
        self.feed.append((self.lineno, l, self.lo, self.feedrate, (self.xo, self.yo, self.zo)))
    
    def user_defined_function(self, i: int, p: float, q: float):
        """用户自定义函数"""
        if self.suppress > 0:
            return
        color = self.colors.get('m1xx', '#ff8000')
        plane = int(getattr(self.state, 'plane', 17) / 10 - 17)
        self.dwells.append((self.lineno, color, self.lo[0], self.lo[1], self.lo[2], plane))
    
    def dwell(self, arg: float):
        """暂停"""
        if self.suppress > 0:
            return
        self.dwell_time += arg
        color = self.colors.get('dwell', '#ffffff')
        plane = int(getattr(self.state, 'plane', 17) / 10 - 17)
        self.dwells.append((self.lineno, color, self.lo[0], self.lo[1], self.lo[2], plane))
    
    def get_extents(self) -> Dict:
        """获取包围盒"""
        self.calc_extents()
        return {
            'min': self.min_extents,
            'max': self.max_extents,
            'min_notool': self.min_extents_notool,
            'max_notool': self.max_extents_notool,
            'min_zero_rxy': self.min_extents_zero_rxy,
            'max_zero_rxy': self.max_extents_zero_rxy,
        }
    
    def get_traverse(self) -> List[Tuple]:
        return self.traverse
    
    def get_feed(self) -> List[Tuple]:
        return self.feed
    
    def get_arcfeed(self) -> List[Tuple]:
        return self.arcfeed
    
    def get_dwells(self) -> List[Tuple]:
        return self.dwells


# ==================== 梯形速度曲线运动规划器 ====================

# ==================== 梯形速度曲线运动规划器（完整版）====================

class MotionProfile:
    """速度曲线规划结果"""
    def __init__(self):
        self.duration: float = 0.0      # 总时间 (s)
        self.v_max: float = 0.0         # 最大速度 (mm/s)
        self.t_acc: float = 0.0         # 加速时间 (s)
        self.t_const: float = 0.0       # 匀速时间 (s)
        self.t_dec: float = 0.0         # 减速时间 (s)
        self.s_acc: float = 0.0         # 加速距离 (mm)
        self.s_const: float = 0.0       # 匀速距离 (mm)
        self.s_dec: float = 0.0         # 减速距离 (mm)
        self.total_distance: float = 0.0 # 总距离 (mm)
        self.profile_type: str = "zero" # zero / trapezoidal / triangular


class TrapezoidalMotionPlanner:
    """梯形速度曲线运动规划器 - 支持实时位置查询"""
    
    def __init__(self, 
                 acceleration: float = 500.0,
                 deceleration: float = 500.0,
                 max_rapid_rate: float = 10000.0,
                 max_feed_rate: float = 5000.0,
                 rapid_override: float = 1.0,
                 feed_override: float = 1.0):
        
        self.acc = acceleration        # mm/s²
        self.dec = deceleration        # mm/s²
        self.max_rapid = max_rapid_rate / 60.0  # mm/s
        self.max_feed = max_feed_rate / 60.0
        self.rapid_override = rapid_override
        self.feed_override = feed_override
        self._jerk = 10000.0  # 加加速度 (mm/s³) - 预留
    
    def plan_motion(self, distance: float, commanded_feedrate: float = None, 
                    is_rapid: bool = False) -> MotionProfile:
        """
        规划运动速度曲线
        
        参数:
            distance: 运动距离 (mm)
            commanded_feedrate: 编程进给率 (mm/min)
            is_rapid: 是否为G0快速移动
        
        返回: MotionProfile 包含完整速度曲线参数
        """
        profile = MotionProfile()
        profile.total_distance = distance
        
        if distance < CART_FUZZ:
            profile.profile_type = "zero"
            return profile
        
        # 确定目标速度 (mm/s)
        if is_rapid:
            v_target = self.max_rapid * self.rapid_override
        else:
            if commanded_feedrate is not None and commanded_feedrate > 0:
                v_target = (commanded_feedrate / 60.0) * self.feed_override
            else:
                v_target = self.max_feed * self.feed_override
        
        max_allowed = self.max_rapid if is_rapid else self.max_feed
        v_target = min(v_target, max_allowed)
        v_target = max(v_target, 0.001)
        
        # 计算加速距离和减速距离
        s_acc = (v_target * v_target) / (2.0 * self.acc)
        s_dec = (v_target * v_target) / (2.0 * self.dec)
        s_cruise = distance - s_acc - s_dec
        
        if s_cruise >= 0:
            # ===== 梯形速度曲线 =====
            profile.profile_type = "trapezoidal"
            profile.t_acc = v_target / self.acc
            profile.t_dec = v_target / self.dec
            profile.t_const = s_cruise / v_target
            profile.v_max = v_target
            profile.s_acc = s_acc
            profile.s_const = s_cruise
            profile.s_dec = s_dec
            profile.duration = profile.t_acc + profile.t_const + profile.t_dec
        else:
            # ===== 三角形速度曲线 =====
            # v_peak = sqrt(2 * s * a1 * a2 / (a1 + a2))
            profile.profile_type = "triangular"
            v_peak = math.sqrt(
                2.0 * distance * self.acc * self.dec / (self.acc + self.dec)
            )
            profile.v_max = v_peak
            profile.t_acc = v_peak / self.acc
            profile.t_dec = v_peak / self.dec
            profile.t_const = 0.0
            profile.s_acc = 0.5 * self.acc * profile.t_acc * profile.t_acc
            profile.s_const = 0.0
            profile.s_dec = 0.5 * self.dec * profile.t_dec * profile.t_dec
            profile.duration = profile.t_acc + profile.t_dec
        
        return profile
    
    def get_position_at_time(self, elapsed: float, profile: MotionProfile) -> float:
        """
        根据速度曲线获取当前位置 (mm)
        
        参数:
            elapsed: 已用时间 (s)
            profile: 速度曲线
        
        返回: 当前位置 (mm)
        """
        if elapsed <= 0:
            return 0.0
        if elapsed >= profile.duration:
            return profile.total_distance
        
        if profile.profile_type == "zero":
            return 0.0
        
        elif profile.profile_type == "trapezoidal":
            if elapsed <= profile.t_acc:
                # 加速段: s = 1/2 * a * t²
                return 0.5 * self.acc * elapsed * elapsed
            elif elapsed <= profile.t_acc + profile.t_const:
                # 匀速段: s = s_acc + v * (t - t_acc)
                return profile.s_acc + profile.v_max * (elapsed - profile.t_acc)
            else:
                # 减速段: s = s_acc + s_const + v*(Δt) - 1/2*dec*(Δt)²
                dt = elapsed - profile.t_acc - profile.t_const
                s = (profile.s_acc + profile.s_const + 
                     profile.v_max * dt - 0.5 * self.dec * dt * dt)
                return min(s, profile.total_distance)
        
        elif profile.profile_type == "triangular":
            if elapsed <= profile.t_acc:
                return 0.5 * self.acc * elapsed * elapsed
            else:
                dt = elapsed - profile.t_acc
                s = (profile.s_acc + 
                     profile.v_max * dt - 0.5 * self.dec * dt * dt)
                return min(s, profile.total_distance)
        
        return 0.0
    
    def get_position_ratio(self, elapsed: float, profile: MotionProfile) -> float:
        """获取当前位置比例 (0.0 ~ 1.0)"""
        if profile.total_distance < CART_FUZZ:
            return 1.0
        pos = self.get_position_at_time(elapsed, profile)
        return min(pos / profile.total_distance, 1.0)
    
    def get_velocity_at_time(self, elapsed: float, profile: MotionProfile) -> float:
        """获取当前速度 (mm/s)"""
        if elapsed <= 0 or elapsed >= profile.duration:
            return 0.0
        
        if profile.profile_type == "zero":
            return 0.0
        
        elif profile.profile_type == "trapezoidal":
            if elapsed <= profile.t_acc:
                return self.acc * elapsed
            elif elapsed <= profile.t_acc + profile.t_const:
                return profile.v_max
            else:
                dt = elapsed - profile.t_acc - profile.t_const
                return max(0.0, profile.v_max - self.dec * dt)
        
        elif profile.profile_type == "triangular":
            if elapsed <= profile.t_acc:
                return self.acc * elapsed
            else:
                dt = elapsed - profile.t_acc
                return max(0.0, profile.v_max - self.dec * dt)
        
        return 0.0
    
    def calculate_time(self, distance: float, commanded_feedrate: float = None, 
                       is_rapid: bool = False) -> Dict[str, float]:
        """向后兼容的 calculate_time 方法"""
        profile = self.plan_motion(distance, commanded_feedrate, is_rapid)
        return {
            'duration': profile.duration,
            'max_velocity': profile.v_max,
            'acceleration_time': profile.t_acc,
            'constant_time': profile.t_const,
            'deceleration_time': profile.t_dec,
            'profile_type': profile.profile_type
        }
    
    def get_position_at_time_legacy(self, distance: float, duration: float, 
                             commanded_feedrate: float = None, 
                             is_rapid: bool = False) -> Callable[[float], float]:
        """向后兼容的旧接口"""
        profile = self.plan_motion(distance, commanded_feedrate, is_rapid)
        
        def pos_func(t: float) -> float:
            return self.get_position_at_time(t, profile)
        
        return pos_func
    
    def get_velocity_at_time_legacy(self, distance: float, duration: float,
                             commanded_feedrate: float = None,
                             is_rapid: bool = False) -> Callable[[float], float]:
        """向后兼容的旧接口"""
        profile = self.plan_motion(distance, commanded_feedrate, is_rapid)
        
        def vel_func(t: float) -> float:
            return self.get_velocity_at_time(t, profile)
        
        return vel_func
    
    def get_profile_summary(self, profile: MotionProfile) -> str:
        """获取速度曲线摘要"""
        if profile.profile_type == "zero":
            return "静止"
        
        v_max_mm_min = profile.v_max * 60.0
        parts = []
        if profile.t_acc > 0.001:
            parts.append(f"加速{profile.t_acc*1000:.0f}ms({profile.s_acc:.1f}mm)")
        if profile.t_const > 0.001:
            parts.append(f"匀速{profile.t_const*1000:.0f}ms({profile.s_const:.1f}mm)")
        if profile.t_dec > 0.001:
            parts.append(f"减速{profile.t_dec*1000:.0f}ms({profile.s_dec:.1f}mm)")
        
        return (f"Vmax={v_max_mm_min:.0f}mm/min "
                f"耗时{profile.duration*1000:.0f}ms [" + " → ".join(parts) + "]")
    
    def set_feed_override(self, value: float):
        """设置进给倍率 (0.0 ~ 2.0)"""
        self.feed_override = max(0.0, min(2.0, value))
    
    def set_rapid_override(self, value: float):
        """设置快速倍率 (0.0 ~ 1.0)"""
        self.rapid_override = max(0.0, min(1.0, value))
    
    def set_acceleration(self, value: float):
        """设置加速度"""
        self.acc = max(1.0, value)
    
    def set_deceleration(self, value: float):
        """设置减速度"""
        self.dec = max(1.0, value)

# ==================== 控制结构处理器 ====================

class ControlStructureHandler:
    """O代码控制结构处理器"""
    
    def __init__(self, var_manager: 'lcnc_param.LinuxCNCParameterTable', debug: bool = False):
        self.var_manager = var_manager
        self.debug = debug
        
        self.while_stack: List[Dict] = []
        self.repeat_stack: List[Dict] = []
        self.if_stack: List[Dict] = []
        self.subroutine_stack: List[Dict] = []
        
        self.jump_targets: Dict[str, int] = {}
        
        self._skip_mode = False
        self._skip_depth = 0
        self._skip_target = None
    
    def evaluate_condition(self, condition: str, local_level: int = 0) -> bool:
        """评估条件表达式"""
        try:
            expanded = self.var_manager.expand_variables(condition, local_level)
            expanded = re.sub(r'(?i)EQ', '==', expanded)
            expanded = re.sub(r'(?i)NE', '!=', expanded)
            expanded = re.sub(r'(?i)GT', '>', expanded)
            expanded = re.sub(r'(?i)GE', '>=', expanded)
            expanded = re.sub(r'(?i)LT', '<', expanded)
            expanded = re.sub(r'(?i)LE', '<=', expanded)
            expanded = re.sub(r'(?i)XOR', '!=', expanded)
            expanded = re.sub(r'(?i)AND', 'and', expanded)
            expanded = re.sub(r'(?i)OR', 'or', expanded)
            expanded = re.sub(r'(?i)NOT', 'not', expanded)
            result = eval(expanded, {"__builtins__": {}}, {"math": math})
            return bool(result)
        except Exception as e:
            if self.debug:
                print(f"  [DEBUG] 条件评估失败: {condition}, 错误: {e}")
            return False
    
    def parse_o_code(self, line: str, line_num: int, current_line_idx: int, 
                    lines: List[str], local_level: int = 0) -> Optional[int]:
        """解析O代码控制结构"""
        line_upper = line.upper().strip()
        
        # 如果在跳过模式
        if self._skip_mode:
            if re.match(r'O<[^>]+>\s+IF\s+\[', line_upper, re.I):
                self._skip_depth += 1
            elif re.match(r'O<[^>]+>\s+ENDIF', line_upper, re.I):
                self._skip_depth -= 1
                if self._skip_depth == 0:
                    self._skip_mode = False
                    self._skip_target = None
            elif re.match(r'O<[^>]+>\s+ELSEIF', line_upper, re.I) and self._skip_depth == 1:
                # 在跳过模式中遇到 ELSEIF，检查条件
                if_match = re.match(r'O<([^>]+)>\s+ELSEIF\s+\[(.+)\]', line_upper, re.I)
                if if_match:
                    condition = if_match.group(2)
                    if self.evaluate_condition(condition, local_level):
                        self._skip_mode = False
                        self._skip_depth = 0
            elif re.match(r'O<[^>]+>\s+ELSE', line_upper, re.I) and self._skip_depth == 1:
                # 在跳过模式中遇到 ELSE，停止跳过
                self._skip_mode = False
                self._skip_depth = 0
            return None
        
        # SUB
        if re.match(r'O<[^>]+>\s+SUB', line_upper, re.I):
            return None
        
        # ENDSUB
        if re.match(r'O<[^>]+>\s+ENDSUB', line_upper, re.I):
            if self.subroutine_stack:
                call_info = self.subroutine_stack.pop()
                return call_info['return_line']
            return None
        
        # CALL
        if re.match(r'O<[^>]+>\s+CALL', line_upper, re.I):
            return None
        
        # IF
        if_match = re.match(r'O<([^>]+)>\s+IF\s+\[(.+)\]', line_upper, re.I)
        if if_match:
            label = if_match.group(1)
            condition = if_match.group(2)
            result = self.evaluate_condition(condition, local_level)
            self.if_stack.append({
                'label': label,
                'condition_result': result,
                'line_num': line_num
            })
            if not result:
                self._skip_mode = True
                self._skip_depth = 1
                self._skip_target = label
                return self._skip_to_endif(current_line_idx, lines)
            return None
        
        # ELSEIF
        elseif_match = re.match(r'O<([^>]+)>\s+ELSEIF\s+\[(.+)\]', line_upper, re.I)
        if elseif_match:
            label = elseif_match.group(1)
            condition = elseif_match.group(2)
            if self.if_stack:
                top = self.if_stack[-1]
                if top['condition_result']:
                    self._skip_mode = True
                    self._skip_depth = 1
                    return self._skip_to_endif(current_line_idx, lines)
                else:
                    result = self.evaluate_condition(condition, local_level)
                    if result:
                        top['condition_result'] = True
                    else:
                        self._skip_mode = True
                        self._skip_depth = 1
                        return self._skip_to_endif(current_line_idx, lines)
            return None
        
        # ELSE
        if re.match(r'O<[^>]+>\s+ELSE', line_upper, re.I):
            if self.if_stack:
                top = self.if_stack[-1]
                if top['condition_result']:
                    self._skip_mode = True
                    self._skip_depth = 1
                    return self._skip_to_endif(current_line_idx, lines)
            return None
        
        # ENDIF
        if re.match(r'O<[^>]+>\s+ENDIF', line_upper, re.I):
            if self.if_stack:
                self.if_stack.pop()
            return None
        
        # WHILE
        while_match = re.match(r'O<([^>]+)>\s+WHILE\s+\[(.+)\]', line_upper, re.I)
        if while_match:
            label = while_match.group(1)
            condition = while_match.group(2)
            result = self.evaluate_condition(condition, local_level)
            self.while_stack.append({
                'label': label,
                'condition': condition,
                'start_line': current_line_idx,
                'line_num': line_num
            })
            if not result:
                return self._skip_to_endwhile(current_line_idx, lines)
            return None
        
        # ENDWHILE
        if re.match(r'O<[^>]+>\s+ENDWHILE', line_upper, re.I):
            if self.while_stack:
                loop_info = self.while_stack[-1]
                result = self.evaluate_condition(loop_info['condition'], local_level)
                if result:
                    return loop_info['start_line']
                else:
                    self.while_stack.pop()
            return None
        
        # REPEAT
        repeat_match = re.match(r'O<([^>]+)>\s+REPEAT\s+\[(.+)\]', line_upper, re.I)
        if repeat_match:
            label = repeat_match.group(1)
            count_expr = repeat_match.group(2)
            try:
                expanded = self.var_manager.expand_variables(count_expr, local_level)
                count = int(eval(expanded, {"__builtins__": {}}, {"math": math}))
            except:
                count = 1
            self.repeat_stack.append({
                'label': label,
                'count': count,
                'remaining': count,
                'start_line': current_line_idx
            })
            return None
        
        # ENDREPEAT
        if re.match(r'O<[^>]+>\s+ENDREPEAT', line_upper, re.I):
            if self.repeat_stack:
                loop_info = self.repeat_stack[-1]
                loop_info['remaining'] -= 1
                if loop_info['remaining'] > 0:
                    return loop_info['start_line']
                else:
                    self.repeat_stack.pop()
            return None
        
        # BREAK
        if re.match(r'O<[^>]+>\s+BREAK', line_upper, re.I):
            if self.while_stack:
                return self._skip_to_endwhile(current_line_idx, lines)
            if self.repeat_stack:
                self.repeat_stack.pop()
            return None
        
        # CONTINUE
        if re.match(r'O<[^>]+>\s+CONTINUE', line_upper, re.I):
            if self.while_stack:
                return self.while_stack[-1]['start_line']
            if self.repeat_stack:
                return self.repeat_stack[-1]['start_line']
            return None
        
        # GOTO
        goto_match = re.match(r'O<[^>]+>\s+GOTO\s+(\d+)', line_upper, re.I)
        if goto_match:
            target_line = int(goto_match.group(1))
            for i, l in enumerate(lines):
                n_match = re.search(r'N(\d+)', l, re.I)
                if n_match and int(n_match.group(1)) == target_line:
                    return i
            if target_line <= len(lines):
                return target_line - 1
            return None
        
        # RETURN
        if re.match(r'O<[^>]+>\s+RETURN', line_upper, re.I):
            if self.subroutine_stack:
                call_info = self.subroutine_stack.pop()
                return call_info['return_line']
            return None
        
        return None
    
    def _skip_to_endif(self, line_idx: int, lines: List[str]) -> int:
        """跳过到对应的 ENDIF"""
        i = line_idx + 1
        depth = 1
        while i < len(lines):
            line_upper = lines[i].upper().strip()
            if re.match(r'O<[^>]+>\s+IF\s+\[', line_upper, re.I):
                depth += 1
            elif re.match(r'O<[^>]+>\s+ENDIF', line_upper, re.I):
                depth -= 1
                if depth == 0:
                    return i
            i += 1
        return len(lines) - 1
    
    def _skip_to_endwhile(self, line_idx: int, lines: List[str]) -> int:
        """跳过到对应的 ENDWHILE"""
        i = line_idx + 1
        depth = 1
        while i < len(lines):
            line_upper = lines[i].upper().strip()
            if re.match(r'O<[^>]+>\s+WHILE\s+\[', line_upper, re.I):
                depth += 1
            elif re.match(r'O<[^>]+>\s+ENDWHILE', line_upper, re.I):
                depth -= 1
                if depth == 0:
                    return i
            i += 1
        return len(lines) - 1
    
    def reset(self):
        """重置状态"""
        self.while_stack.clear()
        self.repeat_stack.clear()
        self.if_stack.clear()
        self.subroutine_stack.clear()
        self.jump_targets.clear()
        self._skip_mode = False
        self._skip_depth = 0
        self._skip_target = None


# ==================== 虚拟HAL系统 ====================

class VirtualHAL:
    """虚拟HAL系统 - 模拟LinuxCNC的硬件抽象层"""
    
    def __init__(self, debug: bool = False):
        self.debug = debug
        self.pins: Dict[str, Any] = {}
        self.signals: Dict[str, Any] = {}
        self.params: Dict[str, Any] = {}
        self.watchers: Dict[str, List[Callable]] = {}
        self._comp_id = 0
        self._initialized = False
        self._init_pins()
    
    def _init_pins(self):
        """初始化默认引脚"""
        # 关节
        for i in range(EMCMOT_MAX_JOINTS):
            self.pins[f'joint.{i}.position'] = 0.0
            self.pins[f'joint.{i}.velocity'] = 0.0
            self.pins[f'joint.{i}.acceleration'] = 0.0
            self.pins[f'joint.{i}.home'] = 0
            self.pins[f'joint.{i}.homing'] = 0
            self.pins[f'joint.{i}.error'] = 0.0
            self.pins[f'joint.{i}.f-error'] = 0.0
            self.pins[f'joint.{i}.enabled'] = 1
            self.pins[f'joint.{i}.max-limit'] = 1000.0
            self.pins[f'joint.{i}.min-limit'] = -1000.0
        
        # 轴
        for axis in ['x', 'y', 'z', 'a', 'b', 'c', 'u', 'v', 'w']:
            self.pins[f'axis.{axis}.position'] = 0.0
            self.pins[f'axis.{axis}.velocity'] = 0.0
            self.pins[f'axis.{axis}.home'] = 0
            self.pins[f'axis.{axis}.homing'] = 0
            self.pins[f'axis.{axis}.enabled'] = 1
            self.pins[f'axis.{axis}.max-limit'] = 1000.0
            self.pins[f'axis.{axis}.min-limit'] = -1000.0
        
        # 主轴
        for i in range(EMCMOT_MAX_SPINDLES):
            self.pins[f'spindle.{i}.speed'] = 0.0
            self.pins[f'spindle.{i}.direction'] = 0
            self.pins[f'spindle.{i}.at-speed'] = 0
            self.pins[f'spindle.{i}.brake'] = 0
            self.pins[f'spindle.{i}.enable'] = 0
            self.pins[f'spindle.{i}.speed-out'] = 0.0
            self.pins[f'spindle.{i}.speed-out-abs'] = 0.0
        
        # 冷却液
        self.pins['coolant.flood'] = 0
        self.pins['coolant.mist'] = 0
        
        # 刀具
        self.pins['tool.number'] = 0
        self.pins['tool.pocket'] = 0
        self.pins['tool.length'] = 0.0
        self.pins['tool.diameter'] = 0.0
        self.pins['tool.prep-number'] = 0
        self.pins['tool.prep-pocket'] = 0
        
        # 倍率
        self.pins['feed-override'] = 1.0
        self.pins['rapid-override'] = 1.0
        self.pins['spindle-override'] = 1.0
        self.pins['max-velocity'] = 10000.0
        
        # 运动
        self.pins['motion.kins-type'] = 2
        self.pins['motion.rtcp-active'] = 0
        self.pins['motion.switchkins-type'] = 0
        self.pins['motion.pivot-length'] = 250.0
        self.pins['motion.tool-length'] = 0.0
        self.pins['motion.rot-center-x'] = 0.0
        self.pins['motion.rot-center-y'] = 0.0
        self.pins['motion.rot-center-z'] = 0.0
        self.pins['motion.enabled'] = 0
        self.pins['motion.in-position'] = 1
        self.pins['motion.feed-hold'] = 0
        self.pins['motion.adaptive-feed'] = 1.0
        
        # 急停
        self.pins['estop.active'] = 0
        self.pins['estop.reset-request'] = 0
        
        # 机床
        self.pins['machine.power-on'] = 0
        self.pins['machine.power-request'] = 0
        self.pins['machine.reset'] = 0
        self.pins['machine.reset-done'] = 1
        self.pins['machine.state'] = 'OFF'
        self.pins['machine.is-on'] = 0
        
        # 回零
        self.pins['home.all'] = 0
        for axis in ['x', 'y', 'z', 'a', 'b', 'c']:
            self.pins[f'home.{axis}'] = 0
            self.pins[f'home.{axis}-done'] = 0
            self.pins[f'home.{axis}-switch'] = 0
        self.pins['home.all-done'] = 0
        
        # 程序
        self.pins['program.run'] = 0
        self.pins['program.pause'] = 0
        self.pins['program.resume'] = 0
        self.pins['program.stop'] = 0
        self.pins['program.status'] = 'IDLE'
        self.pins['program.line'] = 0
        self.pins['program.progress'] = 0.0
        self.pins['program.block-delete'] = 0
        self.pins['program.optional-stop'] = 1
        
        # 告警
        self.pins['alarm.active'] = 0
        self.pins['alarm.code'] = 0
        self.pins['alarm.message'] = ''
        
        # 模拟量
        for i in range(8):
            self.pins[f'analog.out.{i}'] = 0.0
        
        # 数字量
        for i in range(32):
            self.pins[f'digital.in.{i}'] = 0
            self.pins[f'digital.out.{i}'] = 0
        
        # 探测
        self.pins['probe.input'] = 0
        self.pins['probe.tripped'] = 0
        self.pins['probe.position-x'] = 0.0
        self.pins['probe.position-y'] = 0.0
        self.pins['probe.position-z'] = 0.0
        
        # 固定循环
        self.pins['motion.canned-cycle'] = 0
        self.pins['motion.canned-cycle-type'] = 0
        self.pins['motion.canned-cycle-retract'] = 0.0
        self.pins['motion.canned-cycle-clearance'] = 0.0
    
    def hal_init(self, comp_name: str) -> int:
        """初始化HAL组件"""
        self._comp_id = hash(comp_name) & 0x7FFFFFFF
        self._initialized = True
        return self._comp_id
    
    def hal_ready(self, comp_id: int) -> int:
        """标记HAL组件就绪"""
        return 0
    
    def hal_exit(self, comp_id: int):
        """退出HAL"""
        self._initialized = False
    
    def set(self, pin: str, value: Any) -> bool:
        """设置引脚值"""
        old_value = self.pins.get(pin)
        self.pins[pin] = value
        self._notify_watchers(pin, value, old_value)
        return True
    
    def get(self, pin: str, default: Any = 0) -> Any:
        """获取引脚值"""
        return self.pins.get(pin, default)
    
    def add_watcher(self, pin: str, callback: Callable):
        """添加监听器"""
        if pin not in self.watchers:
            self.watchers[pin] = []
        self.watchers[pin].append(callback)
    
    def remove_watcher(self, pin: str, callback: Callable):
        """移除监听器"""
        if pin in self.watchers:
            try:
                self.watchers[pin].remove(callback)
            except ValueError:
                pass
    
    def _notify_watchers(self, pin: str, value: Any, old_value: Any):
        """通知监听器"""
        if pin in self.watchers:
            for callback in self.watchers[pin]:
                try:
                    callback(pin, value, old_value)
                except Exception as e:
                    if self.debug:
                        print(f"[HAL] 监听器回调错误: {e}")
    
    def update_axis_position(self, axis: str, position: float):
        """更新轴位置"""
        self.set(f'axis.{axis}.position', position)
        axis_map = {'x': 0, 'y': 1, 'z': 2, 'a': 3, 'b': 4, 'c': 5, 'u': 6, 'v': 7, 'w': 8}
        if axis.lower() in axis_map:
            self.set(f'joint.{axis_map[axis.lower()]}.position', position)
    
    def update_all_positions(self, positions: Dict[str, float]):
        """更新所有轴位置"""
        for axis, pos in positions.items():
            self.update_axis_position(axis, pos)
    
    def dump_state(self) -> Dict[str, Any]:
        """导出状态"""
        return self.pins.copy()
    
    def load_state(self, state: Dict[str, Any]):
        """加载状态"""
        for pin, value in state.items():
            if pin in self.pins:
                self.pins[pin] = value
    
    def to_json(self) -> str:
        """导出为JSON"""
        serializable = {}
        for k, v in self.pins.items():
            if isinstance(v, (str, int, float, bool, type(None))):
                serializable[k] = v
            else:
                serializable[k] = str(v)
        return json.dumps(serializable, indent=2)
    
    def reset(self):
        """重置"""
        self._init_pins()
        if self.debug:
            print("[HAL] 系统已重置")


# ==================== 机床状态机 ====================

class MachineStateMachine:
    """机床状态机 - 模拟真实CNC控制器的状态转换"""
    
    STATE_OFF = "OFF"
    STATE_RESET = "RESET"
    STATE_ESTOP = "ESTOP"
    STATE_IDLE = "IDLE"
    STATE_RUNNING = "RUNNING"
    STATE_PAUSED = "PAUSED"
    STATE_HOMING = "HOMING"
    STATE_JOGGING = "JOGGING"
    STATE_ALARM = "ALARM"
    STATE_PROBING = "PROBING"
    STATE_MDI = "MDI"
    STATE_TOOL_CHANGING = "TOOL_CHANGING"
    
    TRANSITIONS = {
        STATE_OFF: [STATE_RESET, STATE_ESTOP],
        STATE_RESET: [STATE_IDLE, STATE_ESTOP, STATE_ALARM],
        STATE_ESTOP: [STATE_RESET, STATE_OFF],
        STATE_IDLE: [STATE_RUNNING, STATE_HOMING, STATE_JOGGING, STATE_MDI,
                     STATE_ESTOP, STATE_ALARM, STATE_OFF, STATE_TOOL_CHANGING, STATE_RESET],
        STATE_RUNNING: [STATE_PAUSED, STATE_IDLE, STATE_ESTOP, STATE_ALARM],
        STATE_PAUSED: [STATE_RUNNING, STATE_IDLE, STATE_ESTOP, STATE_ALARM],
        STATE_HOMING: [STATE_IDLE, STATE_ESTOP, STATE_ALARM],
        STATE_JOGGING: [STATE_IDLE, STATE_ESTOP, STATE_ALARM],
        STATE_ALARM: [STATE_RESET, STATE_OFF],
        STATE_PROBING: [STATE_IDLE, STATE_ESTOP],
        STATE_MDI: [STATE_IDLE, STATE_ESTOP, STATE_ALARM],
        STATE_TOOL_CHANGING: [STATE_IDLE, STATE_ESTOP, STATE_ALARM],
    }
    
    def __init__(self, hal: VirtualHAL, debug: bool = False):
        self.hal = hal
        self.debug = debug
        self.current_state = self.STATE_OFF
        self.previous_state = self.STATE_OFF
        self.state_entry_time = time.time()
        self.alarm_reason = ""
        self.alarm_code = 0
        
        self.transition_callbacks: List[Callable] = []
        
        self._update_hal_pins()
        
        if self.debug:
            print(f"[状态机] 初始化: {self.current_state}")
    
    def _update_hal_pins(self):
        """更新 HAL 引脚"""
        self.hal.set('machine.state', self.current_state)
        self.hal.set('estop.active', 1 if self.current_state == self.STATE_ESTOP else 0)
        
        is_powered = self.current_state not in [self.STATE_OFF, self.STATE_ESTOP]
        self.hal.set('machine.power-on', 1 if is_powered else 0)
        self.hal.set('machine.is-on', 1 if is_powered else 0)
        
        self.hal.set('alarm.active', 1 if self.current_state == self.STATE_ALARM else 0)
        self.hal.set('machine.reset-done', 1 if self.current_state == self.STATE_IDLE else 0)
    
    def add_transition_callback(self, callback: Callable):
        """添加状态转换回调"""
        self.transition_callbacks.append(callback)
    
    def _notify_transition(self, from_state: str, to_state: str):
        """通知状态转换"""
        for callback in self.transition_callbacks:
            try:
                callback(from_state, to_state)
            except Exception as e:
                if self.debug:
                    print(f"[状态机] 回调错误: {e}")
    
    def _can_transition(self, target_state: str) -> bool:
        """检查是否可以转换"""
        return target_state in self.TRANSITIONS.get(self.current_state, [])
    
    def _log_transition(self, target_state: str):
        """记录状态转换"""
        if self.debug:
            print(f"[状态机] {self.current_state} -> {target_state}")
    
    def transition_to(self, target_state: str) -> bool:
        """执行状态转换"""
        if not self._can_transition(target_state):
            if self.debug:
                print(f"[状态机] 无效转换: {self.current_state} -> {target_state}")
            return False
        
        old_state = self.current_state
        self.previous_state = old_state
        self.current_state = target_state
        self.state_entry_time = time.time()
        
        self._log_transition(target_state)
        self._update_hal_pins()
        self._notify_transition(old_state, target_state)
        
        return True
    
    def request_power_on(self) -> bool:
        """请求上电"""
        if self.current_state == self.STATE_OFF:
            return self.transition_to(self.STATE_RESET)
        return False
    
    def request_power_off(self) -> bool:
        """请求断电"""
        if self.current_state == self.STATE_IDLE:
            return self.transition_to(self.STATE_OFF)
        return False
    
    def emergency_stop(self) -> bool:
        """急停"""
        if self.current_state != self.STATE_ESTOP:
            self.hal.set('estop.active', 1)
            return self.transition_to(self.STATE_ESTOP)
        return False
    
    def reset(self) -> bool:
        """复位"""
        if self.current_state in [self.STATE_ESTOP, self.STATE_ALARM, self.STATE_IDLE, self.STATE_RESET]:
            self.hal.set('machine.reset', 1)
            result = self.transition_to(self.STATE_RESET)
            self.hal.set('machine.reset', 0)
            return result
        return False
    
    def home_all(self) -> bool:
        """回零"""
        if self.current_state == self.STATE_IDLE:
            self.hal.set('home.all', 1)
            result = self.transition_to(self.STATE_HOMING)
            self.hal.set('home.all', 0)
            return result
        return False
    
    def start_program(self) -> bool:
        """启动程序"""
        if self.current_state == self.STATE_IDLE:
            return self.transition_to(self.STATE_RUNNING)
        return False
    
    def pause_program(self) -> bool:
        """暂停程序"""
        if self.current_state == self.STATE_RUNNING:
            return self.transition_to(self.STATE_PAUSED)
        return False
    
    def resume_program(self) -> bool:
        """恢复程序"""
        if self.current_state == self.STATE_PAUSED:
            return self.transition_to(self.STATE_RUNNING)
        return False
    
    def stop_program(self) -> bool:
        """停止程序"""
        if self.current_state in [self.STATE_RUNNING, self.STATE_PAUSED]:
            return self.transition_to(self.STATE_IDLE)
        return False
    
    def start_jogging(self) -> bool:
        """开始点动"""
        if self.current_state == self.STATE_IDLE:
            return self.transition_to(self.STATE_JOGGING)
        return False
    
    def stop_jogging(self) -> bool:
        """停止点动"""
        if self.current_state == self.STATE_JOGGING:
            return self.transition_to(self.STATE_IDLE)
        return False
    
    def start_mdi(self) -> bool:
        """开始MDI"""
        if self.current_state == self.STATE_IDLE:
            return self.transition_to(self.STATE_MDI)
        return False
    
    def stop_mdi(self) -> bool:
        """停止MDI"""
        if self.current_state == self.STATE_MDI:
            return self.transition_to(self.STATE_IDLE)
        return False
    
    def start_tool_change(self) -> bool:
        """开始换刀"""
        if self.current_state == self.STATE_IDLE:
            return self.transition_to(self.STATE_TOOL_CHANGING)
        return False
    
    def stop_tool_change(self) -> bool:
        """停止换刀"""
        if self.current_state == self.STATE_TOOL_CHANGING:
            return self.transition_to(self.STATE_IDLE)
        return False
    
    def set_alarm(self, code: int, message: str) -> bool:
        """设置告警"""
        if self.current_state not in [self.STATE_ESTOP, self.STATE_OFF]:
            self.alarm_reason = message
            self.alarm_code = code
            self.hal.set('alarm.code', code)
            self.hal.set('alarm.message', message)
            return self.transition_to(self.STATE_ALARM)
        return False
    
    def clear_alarm(self) -> bool:
        """清除告警"""
        if self.current_state == self.STATE_ALARM:
            self.alarm_reason = ""
            self.alarm_code = 0
            self.hal.set('alarm.code', 0)
            self.hal.set('alarm.message', '')
            return self.transition_to(self.STATE_RESET)
        return False
    
    def get_state(self) -> str:
        """获取当前状态"""
        return self.current_state
    
    def get_state_duration(self) -> float:
        """获取当前状态持续时间"""
        return time.time() - self.state_entry_time
    
    def is_operational(self) -> bool:
        """是否可操作"""
        return self.current_state == self.STATE_IDLE
    
    def is_running(self) -> bool:
        """是否正在运行"""
        return self.current_state == self.STATE_RUNNING
    
    def is_paused(self) -> bool:
        """是否已暂停"""
        return self.current_state == self.STATE_PAUSED
    
    def is_estop(self) -> bool:
        """是否急停"""
        return self.current_state == self.STATE_ESTOP
    
    def is_alarm(self) -> bool:
        """是否有告警"""
        return self.current_state == self.STATE_ALARM
    
    def update(self):
        """更新状态机"""
        if self.hal.get('estop.reset-request'):
            self.reset()
            self.hal.set('estop.reset-request', 0)
        
        if self.hal.get('machine.power-request'):
            self.request_power_on()
            self.hal.set('machine.power-request', 0)
        
        if self.hal.get('program.run'):
            self.start_program()
            self.hal.set('program.run', 0)
        
        if self.hal.get('program.pause'):
            self.pause_program()
            self.hal.set('program.pause', 0)
        
        if self.hal.get('program.resume'):
            self.resume_program()
            self.hal.set('program.resume', 0)
        
        if self.hal.get('program.stop'):
            self.stop_program()
            self.hal.set('program.stop', 0)
        
        if self.current_state == self.STATE_RESET:
            if self.get_state_duration() >= 0.5:
                self.transition_to(self.STATE_IDLE)
        
        if self.current_state == self.STATE_HOMING:
            if self.hal.get('home.all-done'):
                self.transition_to(self.STATE_IDLE)
                self.hal.set('home.all-done', 0)
        
        self.hal.set('program.status', self.current_state)
        

# ==================== GLCanonPathCollectorWithRTCP 类 ====================



# ==================== 内置G代码解析器 ====================

class FullGCodeParserWithRTCP:
    """内置G代码解析器 - 支持LinuxCNC宏指令和子程序"""

    def __init__(self, max_points: int = 50000,
                 tool_radius_map: Dict[int, float] = None,
                 tool_length_map: Dict[int, float] = None,
                 acceleration: float = 500.0,
                 max_rapid_rate: float = 10000.0,
                 max_feed_rate: float = 5000.0,
                 kinematics_type: KinematicsType = KinematicsType.TRT_BC,
                 kinematics_params: Dict[str, Any] = None,
                 debug: bool = False):

        self.max_points = max_points
        self.tool_radius_map = tool_radius_map or {1: 2.0, 2: 3.0, 3: 4.0}
        self.tool_length_map = tool_length_map or {1: 100.0, 2: 120.0, 3: 150.0}
        self.acceleration = acceleration
        self.max_rapid_rate = max_rapid_rate
        self.max_feed_rate = max_feed_rate
        self.kinematics_type = kinematics_type
        self.kinematics_params = kinematics_params or {}
        self.debug = debug

        self.collector: Optional[GLCanonPathCollectorWithRTCP] = None
        self.current_plane = 17
        self.current_filename = ""
        self.subroutine_path = ""

        self.subroutines: Dict[str, List[Tuple[int, str]]] = {}
        self.call_level = 0

        self.saved_states: List[Dict] = []
        
        self._line_number_map: Dict[int, int] = {}

    def set_subroutine_path(self, path: str):
        self.subroutine_path = path

    def load_subroutine_file(self, filename: str) -> bool:
        if self.debug:
            print(f"[DEBUG] load_subroutine_file: 尝试加载 '{filename}'")

        search_paths = [filename]
        if self.subroutine_path:
            search_paths.append(os.path.join(self.subroutine_path, os.path.basename(filename)))
        if self.current_filename:
            search_paths.append(os.path.join(os.path.dirname(self.current_filename), os.path.basename(filename)))
        search_paths.append(os.path.join(os.getcwd(), os.path.basename(filename)))

        for path in search_paths:
            if os.path.exists(path):
                try:
                    with open(path, 'r', encoding='utf-8', errors='ignore') as f:
                        content = f.read()
                    lines = content.split('\n')
                    self._collect_subroutines_from_lines(lines, path)
                    return True
                except Exception as e:
                    if self.debug:
                        print(f"[DEBUG]   加载失败: {path}, 错误: {e}")

        return False

    def _collect_subroutines_from_lines(self, lines: List[str], source: str = ""):
        i = 0
        while i < len(lines):
            raw_line = lines[i]
            line = self._preprocess_line(raw_line)

            if not line:
                i += 1
                continue

            sub_match = re.match(r'o<([^>]+)>\s*sub', line, re.I)
            if sub_match:
                sub_name = sub_match.group(1)
                sub_lines = []
                sub_line_nums = []
                i += 1

                while i < len(lines):
                    raw_sub_line = lines[i]
                    sub_line = self._preprocess_line(raw_sub_line)

                    if sub_line and re.match(r'o<[^>]+>\s*endsub', sub_line, re.I):
                        break

                    if sub_line:
                        sub_lines.append(sub_line)
                        sub_line_nums.append(i + 1)
                    i += 1

                self.subroutines[sub_name] = list(zip(sub_line_nums, sub_lines))
            i += 1

    def _preprocess_line(self, line: str) -> Optional[str]:
        """移除注释和行号，返回处理后的字符串或 None"""
        if not line:
            return None
        
        # 移除行号 N...
        line = re.sub(r'^N\d+\s*', '', line)
        
        # 移除分号注释
        if ';' in line:
            line = line.split(';')[0]
        
        # 使用状态机正确移除圆括号注释（处理嵌套和复杂表达式）
        result_line = []
        paren_depth = 0
        for char in line:
            if char == '(':
                paren_depth += 1
            elif char == ')':
                if paren_depth > 0:
                    paren_depth -= 1
            else:
                if paren_depth == 0:
                    result_line.append(char)
        
        line = ''.join(result_line)
        
        # 移除头尾空白
        line = line.strip()
        
        # 如果处理后为空，返回 None
        if not line:
            return None
        
        # 转换为大写进行处理
        return line.upper()

    def _parse_call_params(self, params_str: str) -> List[float]:
        args = []
        if not params_str:
            return args

        pattern = r'\[([^\]]+)\]'
        matches = re.findall(pattern, params_str)

        for match in matches:
            try:
                if self.collector:
                    value = self.collector.var_manager.evaluate_expression(match, self.call_level)
                else:
                    value = float(match)
                args.append(value)
            except Exception:
                args.append(0.0)
        return args



    def expand_named_params(self, expr):
        """
        展开命名参数
        
        将 #<_x> 替换为当前X坐标值
        将 #<_y> 替换为当前Y坐标值
        将 #<_z> 替换为当前Z坐标值
        将 #<_a> 替换为当前A角度值
        将 #<_b> 替换为当前B角度值
        将 #<_c> 替换为当前C角度值
        将 #<r> 替换为子程序参数值
        将 #<dist> 替换为子程序参数值
        
        Args:
            expr: 包含命名参数的表达式字符串
            
        Returns:
            展开后的表达式字符串
        """
        if not isinstance(expr, str):
            return expr
        
        result = expr
        
        # ===== 展开系统变量 #<_x>, #<_y>, #<_z>, #<_a>, #<_b>, #<_c>, #<_w> =====
        if self.collector:
            system_vars = {
                '#<_x>': str(self.collector.current_pos.x),
                '#<_y>': str(self.collector.current_pos.y),
                '#<_z>': str(self.collector.current_pos.z),
                '#<_a>': str(self.collector.current_pos.a),
                '#<_b>': str(self.collector.current_pos.b),
                '#<_c>': str(self.collector.current_pos.c),
                '#<_w>': '0.0',  # W轴默认0
            }
            
            for var, value in system_vars.items():
                result = result.replace(var, value)
        
        # ===== 展开子程序参数 #<zmax>, #<r>, #<dist>, #<frate>, #<n>, #<a>, #<b>, #<c> 等 =====
        if self.collector and hasattr(self.collector, 'var_manager'):
            # 使用参数表系统的 expand_variables 展开剩余变量
            result = self.collector.var_manager.expand_variables(result, self.call_level)
        elif hasattr(self, 'param_stack') and self.param_stack:
            # 备用方案：直接查找子程序参数栈
            current_frame = self.param_stack[-1]
            param_pattern = re.compile(r'#<(\w+)>')
            
            def replace_param(match):
                param_name = match.group(1)
                # 跳过系统变量（已经处理过）
                if param_name in ['_x', '_y', '_z', '_a', '_b', '_c', '_w']:
                    return match.group(0)
                # 跳过HAL变量
                if param_name.startswith('_hal['):
                    return match.group(0)
                # 跳过INI变量
                if param_name.startswith('_ini['):
                    return match.group(0)
                # 查找子程序参数
                if param_name in current_frame:
                    return str(current_frame[param_name])
                return match.group(0)
            
            result = param_pattern.sub(replace_param, result)
        
        return result



    def _parse_line(self, line: str, line_num: int) -> Optional[int]:
        """解析单行G代码 - 完整版，使用参数表系统"""
        if not line or not line.strip():
            return None

        line_upper = line.upper().strip()

        # ==================== G10 通用解析函数 ====================
        # ★★★ 定义在此处，供所有 G10 分支 (L1/L10/L11/L2/L20) 共享 ★★★
        def parse_g10_value(axis, expanded_upper):
            """
            解析 G10 坐标值，支持：
            - X1.234, X=1.234
            - X[#1+#2], X=[#1+#2]
            """
            patterns = [
                rf'{axis}\s*=\s*([+-]?\d*\.?\d+)',
                rf'{axis}\s*=\s*(\[.+?\])',
                rf'{axis}\s*([+-]?\d*\.?\d+)',
                rf'{axis}\s*(\[.+?\])',
                rf'{axis}([+-]?\d*\.?\d+)',
            ]
            for pattern in patterns:
                match = re.search(pattern, expanded_upper, re.I)
                if match:
                    val_str = match.group(1)
                    try:
                        if val_str.startswith('['):
                            return self.collector.var_manager.evaluate_expression(val_str, self.call_level)
                        else:
                            return float(val_str)
                    except (ValueError, Exception) as e:
                        if self.debug:
                            print(f"[DEBUG] G10 值解析失败: {axis}={val_str}, 错误: {e}")
                        continue
            return None

        # ==================== 变量赋值（使用参数表） ====================
        
        # 数字变量赋值: #1 = 10 或 #1 = [1+2] 或 #1 = #2
        var_match = re.match(r'#(\d+)\s*=\s*(.+)', line, re.I)
        if var_match:
            var_num = int(var_match.group(1))
            value_expr = var_match.group(2).strip()
            if self.collector:
                # 检查只读参数
                if lcnc_param.InterpParameterIndex.is_readonly(var_num):
                    if self.debug:
                        print(f"[警告] 参数 #{var_num} 是只读的，不能赋值")
                else:
                    # 使用参数表系统计算表达式
                    value = self.collector.var_manager.evaluate_expression(value_expr, self.call_level)
                    if self.debug:
                        print(f"[DEBUG] 变量赋值: #{var_num} = {value_expr} -> {value}")
                    self.collector.var_manager.set_param(var_num, value, self.call_level > 0)
            return None
        
        # 命名变量赋值: #<zmax> = #1 或 #<zmax> = 100 或 #<zmax> = [1+2]
        named_var_match = re.match(r'#<([^>]+)>\s*=\s*(.+)', line, re.I)
        if named_var_match:
            var_name = named_var_match.group(1)
            value_expr = named_var_match.group(2).strip()
            if self.collector:
                # 使用参数表系统计算表达式
                value = self.collector.var_manager.evaluate_expression(value_expr, self.call_level)
                if self.debug:
                    print(f"[DEBUG] 变量赋值: #<{var_name}> = {value_expr} -> {value}")
                self.collector.var_manager.set_named(var_name, value)
            return None

        # ==================== 子程序调用 ====================
        
        o_call_match = re.match(r'o<([^>]+)>\s*call\s*(.*)', line, re.I)
        if o_call_match:
            sub_name = o_call_match.group(1)
            params_str = o_call_match.group(2).strip()
            
            if self.debug:
                print(f"[DEBUG] ========== 子程序调用 ==========")
                print(f"[DEBUG] 子程序: {sub_name}")
                print(f"[DEBUG] 原始参数字符串: '{params_str}'")
            
            found_sub = None
            for key in self.subroutines.keys():
                if key.lower() == sub_name.lower():
                    found_sub = key
                    break
            
            if not found_sub:
                if self.debug:
                    print(f"[DEBUG] 子程序 {sub_name} 未找到，尝试加载文件...")
                self.load_subroutine_file(f"{sub_name}.ngc")
                for key in self.subroutines.keys():
                    if key.lower() == sub_name.lower():
                        found_sub = key
                        break
            
            if found_sub:
                if self.debug:
                    print(f"[DEBUG] 找到子程序 {found_sub}，开始执行...")
                
                # 使用参数表系统解析调用参数
                args = self._parse_call_params(params_str)
                if self.debug:
                    print(f"[DEBUG] 解析参数: {args}")
                    print(f"[DEBUG] 参数个数: {len(args)}")
                
                self.call_level += 1
                if self.collector:
                    # 推入局部变量帧
                    self.collector.var_manager.push_local_frame()
                    # 设置子程序参数 #1..#30
                    for i, arg in enumerate(args, 1):
                        if i <= lcnc_param.INTERP_SUB_PARAMS:
                            self.collector.var_manager.set_param(i, arg, self.call_level)
                            if self.debug:
                                print(f"[DEBUG] 设置参数 #{i} = {arg}")
                    
                    # 设置 n_args 命名参数
                    self.collector.var_manager.add_named_param("n_args", 
                        lcnc_param.ParameterAttribute.PA_READONLY)
                    self.collector.var_manager.set_named("n_args", float(len(args)))
                
                sub_lines = self.subroutines[found_sub]
                if self.debug:
                    print(f"[DEBUG] 子程序 {found_sub} 共 {len(sub_lines)} 行")
                
                for sub_line_num, sub_line in sub_lines:
                    if self.debug:
                        display_line = sub_line[:80] + "..." if len(sub_line) > 80 else sub_line
                        print(f"[DEBUG] 执行子程序行 {sub_line_num}: {display_line}")
                    self._parse_line(sub_line, sub_line_num)
                
                self.call_level -= 1
                if self.collector:
                    # 弹出局部变量帧
                    self.collector.var_manager.pop_local_frame()
                
                if self.debug:
                    print(f"[DEBUG] ========== 子程序 {found_sub} 执行完成 ==========")
            else:
                if self.debug:
                    print(f"[DEBUG] 子程序 {sub_name} 未找到！")
            
            return None

        # ==================== 跳转指令 ====================
        
        goto_match = re.match(r'goto\s+(\d+)', line, re.I)
        if goto_match:
            target_line = int(goto_match.group(1))
            return target_line
        
        n_match = re.match(r'n(\d+)', line, re.I)
        if n_match:
            label = int(n_match.group(1))
            if self.collector and self.collector.control_handler:
                self.collector.control_handler.jump_targets[str(label)] = line_num
            return None
        
        # ==================== O代码行 ====================
        
        if line_upper.startswith('O'):
            return None
        
        # ==================== 创建行代码包装器 ====================
        
        line_code = LineCodeWrapper(line_num, self.current_plane)
        if self.collector:
            if re.search(r'\bG53\b', line_upper):
                self.collector._g53_active = True
            self.collector.next_line(line_code)
        
        # ==================== 程序结束 ====================
        
        if 'M30' in line_upper or 'M02' in line_upper:
            if self.collector:
                self.collector.program_end()
            return None
        
        if 'M00' in line_upper:
            if self.collector:
                self.collector.program_stop()
            return None
        
        if 'M01' in line_upper:
            if self.collector:
                self.collector.optional_stop()
            return None
        
        # ==================== M70/M71/M72/M73 ====================
        
        if 'M70' in line_upper:
            if self.collector:
                saved_state = self.collector.save_state(restore_on_return=False)
                self.saved_states.append(saved_state)
                if self.debug:
                    print(f"[DEBUG] M70: 保存状态，当前栈深度: {len(self.saved_states)}")
            return None
        
        if 'M73' in line_upper:
            if self.collector:
                saved_state = self.collector.save_state(restore_on_return=True)
                self.saved_states.append(saved_state)
                if self.debug:
                    print(f"[DEBUG] M73: 保存状态（返回时恢复），当前栈深度: {len(self.saved_states)}")
            return None
        
        if 'M72' in line_upper:
            if self.collector and self.saved_states:
                saved_state = self.saved_states.pop()
                self.collector.restore_state(saved_state)
                if self.debug:
                    print(f"[DEBUG] M72: 恢复状态，剩余栈深度: {len(self.saved_states)}")
            return None
        
        if 'M71' in line_upper:
            if self.saved_states:
                self.saved_states.pop()
                if self.debug:
                    print(f"[DEBUG] M71: 丢弃保存的状态，剩余栈深度: {len(self.saved_states)}")
            return None
        
        # ==================== RTCP控制 ====================
        
        # 在 _parse_line 方法中找到 G49 处理部分，修改为：
        if 'G49' in line_upper:
            if self.collector:
                self.collector.set_rtcp_enable(False)
                self.collector.cancel_tool_length_offset()  # ★ 添加此行
            return None
        
        if 'G43.4' in line_upper or 'G43.5' in line_upper:
            if self.collector:
                h_match = re.search(r'H\s*=?\s*(\d+)', line_upper, re.I)
                if not h_match:
                    h_match = re.search(r'H\s*(\d+)', line_upper, re.I)
                if h_match:
                    h_code = int(h_match.group(1))
                    self.collector.set_tool_length_offset(h_code)
                self.collector.set_rtcp_enable(True)
            return None
        
        # ==================== 运动学切换 ====================
        
        if 'M429' in line_upper:
            if self.collector:
                self.collector.set_kinematics_by_type(0)
                if self.debug:
                    print(f"[DEBUG] M429: 切换到三轴运动学 (IDENTITY)")
            return None
        
        if 'M428' in line_upper:
            if self.collector:
                self.collector.set_kinematics_by_type(1)
                if self.debug:
                    print(f"[DEBUG] M428: 恢复到五轴运动学")
            return None
        
        if 'M430' in line_upper:
            if self.collector:
                self.collector.set_kinematics_by_type(2)
                if self.debug:
                    print(f"[DEBUG] M430: 切换到 FIVEAXIS_BC")
            return None
        
        # ==================== G10 坐标系设置（使用参数表） ====================
        
        # ==================== G10 错误检查 ====================
        if 'G10' in line_upper:
            # 检查 L 值
            l_match = re.search(r'L\s*=?\s*(\d+)', line_upper, re.I) or re.search(r'L\s*(\d+)', line_upper, re.I)
            l_val = int(l_match.group(1)) if l_match else 0
            
            if l_val not in [0, 1, 2, 10, 11, 20]:
                if self.debug:
                    print(f"[警告] G10 L{l_val} 不支持，支持的 L 值: 0, 1, 2, 10, 11, 20")
                return None
            
            # 检查 P 值是否为整数
            p_match = re.search(r'P\s*=?\s*([+-]?\d*\.?\d+)', line_upper, re.I) or re.search(r'P\s*([+-]?\d*\.?\d+)', line_upper, re.I)
            p_val = float(p_match.group(1)) if p_match else -1
            p_int = round(p_val)
            
            if abs(p_val - p_int) > 0.0002:
                if self.debug:
                    print(f"[警告] G10 P 值 {p_val} 不是整数")
                return None
            
            # L2/L20: P 值范围 0-9
            if l_val in [2, 20] and (p_int < 0 or p_int > 9):
                if self.debug:
                    print(f"[警告] G10 L{l_val} P 值 {p_int} 超出范围 (0-9)")
                return None
            
            # L1/L10/L11: P 值必须 >= 1
            if l_val in [1, 10, 11] and p_int < 1:
                if self.debug:
                    print(f"[警告] G10 L{l_val} P 值 {p_int} 必须 >= 1")
                return None
            
            # L1: 必须有至少一个偏移值
            if l_val == 1:
                has_offset = False
                for axis in ['X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W', 'R', 'I', 'J', 'Q']:
                    if re.search(rf'{axis}\s*[=]?\s*[\d\[\.]', line_upper, re.I):
                        has_offset = True
                        break
                if not has_offset:
                    if self.debug:
                        print(f"[警告] G10 L1 没有任何偏移值")
        
        # ==================== G10 L20 坐标系设置 ====================
        
        # G10 L20 Pn X_ Y_ Z_ A_ B_ C_  - 必须最先检测，避免被 L2 分支拦截
        if 'G10' in line_upper and 'L20' in line_upper and self.collector:
            expanded_line = self.collector.var_manager.expand_variables(line, self.call_level)
            expanded_upper = expanded_line.upper()
            
            if self.debug:
                print(f"[DEBUG] G10 L20 原始行: '{line}'")
                print(f"[DEBUG] G10 L20 展开后: '{expanded_line}'")
            
            p_match_g10 = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper)
            if p_match_g10:
                p_val = int(p_match_g10.group(1))
                
                # 构建参数字符串，使用表达式求值
                params_parts = []
                for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']:
                    val = parse_g10_value(axis, expanded_upper)
                    if val is not None:
                        params_parts.append(f"{axis}{val}")
                        if self.debug:
                            print(f"[DEBUG] G10 L20 解析: {axis}={val}")
                
                params_str = ' '.join(params_parts)
                
                if self.debug:
                    print(f"[DEBUG] G10 L20 P{p_val}: params='{params_str}'")
                
                self.collector.set_coordinate_system_g10_l20(p_val, params_str)
            return None

        # ==================== G10 L2 坐标系设置 ====================
        
        # G10 L2 Pn X_ Y_ Z_ A_ B_ C_  - 必须在 L20 之后，且排除 L20
        if 'G10' in line_upper and 'L2' in line_upper and 'L20' not in line_upper and self.collector:
            expanded_line = self.collector.var_manager.expand_variables(line, self.call_level)
            expanded_upper = expanded_line.upper()
            
            if self.debug:
                print(f"[DEBUG] G10 L2 原始行: '{line}'")
                print(f"[DEBUG] G10 L2 展开后: '{expanded_line}'")
            
            p_match_g10 = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper)
            if p_match_g10:
                p_val = int(p_match_g10.group(1))
                
                x = y = z = a = b = c = u = v = w = 0.0
                
                x_val = parse_g10_value('X', expanded_upper)
                y_val = parse_g10_value('Y', expanded_upper)
                z_val = parse_g10_value('Z', expanded_upper)
                a_val = parse_g10_value('A', expanded_upper)
                b_val = parse_g10_value('B', expanded_upper)
                c_val = parse_g10_value('C', expanded_upper)
                
                if x_val is not None: x = x_val
                if y_val is not None: y = y_val
                if z_val is not None: z = z_val
                if a_val is not None: a = a_val
                if b_val is not None: b = b_val
                if c_val is not None: c = c_val
                
                if self.debug:
                    print(f"[DEBUG] G10 L2 P{p_val}: X={x} Y={y} Z={z} A={a} B={b} C={c}")
                
                self.collector.set_g5x_offset(p_val, x, y, z, a, b, c, u, v, w)
            return None
        
        # ==================== G10 L1 刀具偏移设置 ====================
        
        if 'G10' in line_upper and 'L1' in line_upper and 'L10' not in line_upper and 'L11' not in line_upper and self.collector:
            expanded_line = self.collector.var_manager.expand_variables(line, self.call_level)
            expanded_upper = expanded_line.upper()
            
            if self.debug:
                print(f"[DEBUG] ========== G10 L1 刀具偏移设置 ==========")
                print(f"[DEBUG] 行号: {line_num}")
                print(f"[DEBUG] 原始行: '{line.strip()}'")
                print(f"[DEBUG] 展开后: '{expanded_line}'")
            
            # 解析 P 值（刀具号）
            p_match = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper)
            if not p_match:
                if self.debug:
                    print(f"[DEBUG] G10 L1 缺少 P 值")
                return None
            
            tool_number = int(p_match.group(1))
            
            # ★ 使用公共 parse_g10_value 函数 ★
            x_val = parse_g10_value('X', expanded_upper)
            y_val = parse_g10_value('Y', expanded_upper)
            z_val = parse_g10_value('Z', expanded_upper)
            a_val = parse_g10_value('A', expanded_upper)
            b_val = parse_g10_value('B', expanded_upper)
            c_val = parse_g10_value('C', expanded_upper)
            u_val = parse_g10_value('U', expanded_upper)
            v_val = parse_g10_value('V', expanded_upper)
            w_val = parse_g10_value('W', expanded_upper)
            
            # 刀具参数
            r_val = parse_g10_value('R', expanded_upper)
            i_val = parse_g10_value('I', expanded_upper)
            j_val = parse_g10_value('J', expanded_upper)
            
            # Q 值
            q_match = re.search(r'Q\s*=?\s*(\d+)', expanded_upper) or re.search(r'Q\s*(\d+)', expanded_upper)
            orientation = int(q_match.group(1)) if q_match else None
            
            # 检查是否有任何偏移值
            has_any_offset = any(v is not None for v in [x_val, y_val, z_val, a_val, b_val, c_val, u_val, v_val, w_val, r_val, i_val, j_val, q_match])
            if not has_any_offset:
                if self.debug:
                    print(f"[DEBUG] G10 L1 没有任何偏移值，跳过")
                return None
            
            # 查找或创建刀具
            tool_idx = self.collector.find_or_create_tool(tool_number)
            if tool_idx < 0:
                if self.debug:
                    print(f"[DEBUG] G10 L1 无法创建刀具 T{tool_number}")
                return None
            
            tool = self.collector.tool_table[tool_idx]
            changed = []
            
            # 更新偏移
            if x_val is not None:
                tool.offset.x = x_val
                changed.append(f'X={x_val:.3f}')
            if y_val is not None:
                tool.offset.y = y_val
                changed.append(f'Y={y_val:.3f}')
            if z_val is not None:
                tool.offset.z = z_val
                self.collector.tool_lengths[tool_number] = z_val
                changed.append(f'Z={z_val:.3f}')
            if a_val is not None:
                tool.offset.a = a_val
                changed.append(f'A={a_val:.3f}')
            if b_val is not None:
                tool.offset.b = b_val
                changed.append(f'B={b_val:.3f}')
            if c_val is not None:
                tool.offset.c = c_val
                changed.append(f'C={c_val:.3f}')
            if u_val is not None:
                tool.offset.u = u_val
                changed.append(f'U={u_val:.3f}')
            if v_val is not None:
                tool.offset.v = v_val
                changed.append(f'V={v_val:.3f}')
            if w_val is not None:
                tool.offset.w = w_val
                changed.append(f'W={w_val:.3f}')
            
            # 更新刀具参数
            if r_val is not None:
                tool.diameter = r_val
                tool.radius = r_val / 2.0
                self.collector.tool_radii[tool_number] = tool.radius
                changed.append(f'D={r_val:.3f}')
            if i_val is not None:
                tool.front_angle = i_val
                changed.append(f'I={i_val:.3f}')
            if j_val is not None:
                tool.back_angle = j_val
                changed.append(f'J={j_val:.3f}')
            if orientation is not None:
                tool.orientation = orientation
                changed.append(f'Q={orientation}')
            
            # 同步到参数表
            self.collector._sync_tool_parameters_to_table(tool_idx)
            
            # 如果是当前刀具，更新补偿
            self.collector._handle_g10_current_tool_update(tool_idx, tool_number)
            
            if self.debug:
                print(f"[DEBUG] G10 L1 T{tool_number}: 修改 {', '.join(changed)}")
            
            return None

        # ==================== G10 L10 刀具偏移设置（工件坐标系基准） ====================
        
        if 'G10' in line_upper and 'L10' in line_upper and self.collector:
            expanded_line = self.collector.var_manager.expand_variables(line, self.call_level)
            expanded_upper = expanded_line.upper()
            
            if self.debug:
                print(f"[DEBUG] ========== G10 L10 刀具偏移设置（工件坐标系） ==========")
                print(f"[DEBUG] 行号: {line_num}")
                print(f"[DEBUG] 原始行: '{line.strip()}'")
                print(f"[DEBUG] 展开后: '{expanded_line}'")
            
            p_match = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper)
            if not p_match:
                if self.debug:
                    print(f"[DEBUG] G10 L10 缺少 P 值")
                return None
            
            tool_number = int(p_match.group(1))
            
            destination_system = self.collector.g5x_index
            tx, ty, tz, ta, tb, tc, tu, tv, tw = self.collector._get_current_in_system_without_tlo(destination_system)
            dest_rotation = self.collector._get_csys_rotation(destination_system)
            
            if self.debug:
                print(f"[DEBUG] G10 L10: 目标坐标系=G{53 + destination_system}, 旋转={dest_rotation}°")
                print(f"[DEBUG] G10 L10: 当前位置(no TLO)=({tx:.3f},{ty:.3f},{tz:.3f})")
            
            # ★ 使用公共 parse_g10_value 函数 ★
            x_val = parse_g10_value('X', expanded_upper)
            y_val = parse_g10_value('Y', expanded_upper)
            z_val = parse_g10_value('Z', expanded_upper)
            a_val = parse_g10_value('A', expanded_upper)
            b_val = parse_g10_value('B', expanded_upper)
            c_val = parse_g10_value('C', expanded_upper)
            u_val = parse_g10_value('U', expanded_upper)
            v_val = parse_g10_value('V', expanded_upper)
            w_val = parse_g10_value('W', expanded_upper)
            
            r_val = parse_g10_value('R', expanded_upper)
            i_val = parse_g10_value('I', expanded_upper)
            j_val = parse_g10_value('J', expanded_upper)
            q_match = re.search(r'Q\s*=?\s*(\d+)', expanded_upper) or re.search(r'Q\s*(\d+)', expanded_upper)
            orientation = int(q_match.group(1)) if q_match else None
            
            tool_idx = self.collector.find_or_create_tool(tool_number)
            if tool_idx < 0:
                return None
            
            tool = self.collector.tool_table[tool_idx]
            changed = []
            
            # X/Y 轴
            if x_val is not None and y_val is not None:
                dx = tx - x_val
                dy = ty - y_val
                rot_rad = math.radians(dest_rotation)
                cos_r = math.cos(rot_rad)
                sin_r = math.sin(rot_rad)
                tool.offset.x = dx * cos_r + dy * sin_r
                tool.offset.y = -dx * sin_r + dy * cos_r
                changed.append(f'X={tool.offset.x:.3f} Y={tool.offset.y:.3f}')
            
            # Z 轴
            if z_val is not None:
                tool.offset.z = tz - z_val
                self.collector.tool_lengths[tool_number] = tool.offset.z
                changed.append(f'Z={tool.offset.z:.3f}')
            
            # 旋转轴
            if a_val is not None:
                tool.offset.a = ta - a_val
                changed.append(f'A={tool.offset.a:.3f}')
            if b_val is not None:
                tool.offset.b = tb - b_val
                changed.append(f'B={tool.offset.b:.3f}')
            if c_val is not None:
                tool.offset.c = tc - c_val
                changed.append(f'C={tool.offset.c:.3f}')
            
            # U/V/W
            if u_val is not None:
                tool.offset.u = tu - u_val
            if v_val is not None:
                tool.offset.v = tv - v_val
            if w_val is not None:
                tool.offset.w = tw - w_val
            
            # 刀具参数
            if r_val is not None:
                tool.diameter = r_val
                tool.radius = r_val / 2.0
                self.collector.tool_radii[tool_number] = tool.radius
            if i_val is not None:
                tool.front_angle = i_val
            if j_val is not None:
                tool.back_angle = j_val
            if orientation is not None:
                tool.orientation = orientation
            
            # 同步
            self.collector._sync_tool_parameters_to_table(tool_idx)
            self.collector._handle_g10_current_tool_update(tool_idx, tool_number)
            
            if self.debug:
                print(f"[DEBUG] G10 L10 T{tool_number}: 修改 {', '.join(changed)}")
            
            return None

        # ==================== G10 L11 刀具偏移设置（夹具坐标系基准） ====================
        
        if 'G10' in line_upper and 'L11' in line_upper and self.collector:
            expanded_line = self.collector.var_manager.expand_variables(line, self.call_level)
            expanded_upper = expanded_line.upper()
            
            if self.debug:
                print(f"[DEBUG] ========== G10 L11 刀具偏移设置（夹具坐标系） ==========")
                print(f"[DEBUG] 行号: {line_num}")
                print(f"[DEBUG] 原始行: '{line.strip()}'")
                print(f"[DEBUG] 展开后: '{expanded_line}'")
            
            p_match = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper)
            if not p_match:
                if self.debug:
                    print(f"[DEBUG] G10 L11 缺少 P 值")
                return None
            
            tool_number = int(p_match.group(1))
            
            fixture_system = 9  # G59.3
            tx, ty, tz, ta, tb, tc, tu, tv, tw = self.collector._get_current_in_system_without_tlo(fixture_system)
            fixture_rotation = self.collector._get_csys_rotation(fixture_system)
            
            if self.debug:
                print(f"[DEBUG] G10 L11: 夹具坐标系 G59.3, 旋转={fixture_rotation}°")
                print(f"[DEBUG] G10 L11: 当前位置(no TLO)=({tx:.3f},{ty:.3f},{tz:.3f})")
            
            # ★ 使用公共 parse_g10_value 函数 ★
            x_val = parse_g10_value('X', expanded_upper)
            y_val = parse_g10_value('Y', expanded_upper)
            z_val = parse_g10_value('Z', expanded_upper)
            a_val = parse_g10_value('A', expanded_upper)
            b_val = parse_g10_value('B', expanded_upper)
            c_val = parse_g10_value('C', expanded_upper)
            u_val = parse_g10_value('U', expanded_upper)
            v_val = parse_g10_value('V', expanded_upper)
            w_val = parse_g10_value('W', expanded_upper)
            
            r_val = parse_g10_value('R', expanded_upper)
            i_val = parse_g10_value('I', expanded_upper)
            j_val = parse_g10_value('J', expanded_upper)
            q_match = re.search(r'Q\s*=?\s*(\d+)', expanded_upper) or re.search(r'Q\s*(\d+)', expanded_upper)
            orientation = int(q_match.group(1)) if q_match else None
            
            tool_idx = self.collector.find_or_create_tool(tool_number)
            if tool_idx < 0:
                return None
            
            tool = self.collector.tool_table[tool_idx]
            changed = []
            
            # X/Y 轴
            if x_val is not None and y_val is not None:
                dx = tx - x_val
                dy = ty - y_val
                rot_rad = math.radians(fixture_rotation)
                cos_r = math.cos(rot_rad)
                sin_r = math.sin(rot_rad)
                tool.offset.x = dx * cos_r + dy * sin_r
                tool.offset.y = -dx * sin_r + dy * cos_r
                changed.append(f'X={tool.offset.x:.3f} Y={tool.offset.y:.3f}')
            
            # Z 轴
            if z_val is not None:
                tool.offset.z = tz - z_val
                self.collector.tool_lengths[tool_number] = tool.offset.z
                changed.append(f'Z={tool.offset.z:.3f}')
            
            # 旋转轴
            if a_val is not None:
                tool.offset.a = ta - a_val
            if b_val is not None:
                tool.offset.b = tb - b_val
            if c_val is not None:
                tool.offset.c = tc - c_val
            
            # U/V/W
            if u_val is not None:
                tool.offset.u = tu - u_val
            if v_val is not None:
                tool.offset.v = tv - v_val
            if w_val is not None:
                tool.offset.w = tw - w_val
            
            # 刀具参数
            if r_val is not None:
                tool.diameter = r_val
                tool.radius = r_val / 2.0
                self.collector.tool_radii[tool_number] = tool.radius
            if i_val is not None:
                tool.front_angle = i_val
            if j_val is not None:
                tool.back_angle = j_val
            if orientation is not None:
                tool.orientation = orientation
            
            # 同步
            self.collector._sync_tool_parameters_to_table(tool_idx)
            self.collector._handle_g10_current_tool_update(tool_idx, tool_number)
            
            if self.debug:
                print(f"[DEBUG] G10 L11 T{tool_number}: 修改 {', '.join(changed)}")
            
            return None

        # ==================== G28.1/G30.1 参考点设置（使用参数表） ====================
        
        if 'G28.1' in line_upper and self.collector:
            expanded_line = self.collector.var_manager.expand_variables(line, self.call_level)
            expanded_upper = expanded_line.upper()
            
            x = y = z = a = b = c = 0.0
            
            def parse_coord_simple(ax):
                patterns = [
                    rf'{ax}\s*=\s*([+-]?\d*\.?\d+)',
                    rf'{ax}\s*([+-]?\d*\.?\d+)',
                    rf'{ax}([+-]?\d*\.?\d+)',
                ]
                for pattern in patterns:
                    match = re.search(pattern, expanded_upper, re.I)
                    if match:
                        try:
                            return float(match.group(1))
                        except ValueError:
                            continue
                return None
            
            x_val = parse_coord_simple('X')
            y_val = parse_coord_simple('Y')
            z_val = parse_coord_simple('Z')
            
            if x_val is not None: x = x_val
            if y_val is not None: y = y_val
            if z_val is not None: z = z_val
            
            self.collector.set_g28(x, y, z, a, b, c)
            return None
        
        if 'G30.1' in line_upper and self.collector:
            expanded_line = self.collector.var_manager.expand_variables(line, self.call_level)
            expanded_upper = expanded_line.upper()
            
            x = y = z = a = b = c = 0.0
            
            x_val = parse_coord_simple('X')
            y_val = parse_coord_simple('Y')
            z_val = parse_coord_simple('Z')
            
            if x_val is not None: x = x_val
            if y_val is not None: y = y_val
            if z_val is not None: z = z_val
            
            self.collector.set_g30(x, y, z, a, b, c)
            return None
        
        # ==================== 解析坐标（使用参数表展开变量） ====================
        
        x = y = z = a = b = c = u = v = w = None
        i = j = k = r_coord = None
        
        if self.collector:

            current = self.collector.current_pos
            
            # 使用参数表展开变量
            expanded_line = self.collector.var_manager.expand_variables(line, self.call_level)
            expanded_upper = expanded_line.upper()
            
            is_g0 = (re.search(r'\bG0\b', expanded_upper) or re.search(r'\bG00\b', expanded_upper) or 
                    expanded_upper.startswith('G0') or expanded_upper.startswith('G00'))
            is_g1 = (re.search(r'\bG1\b', expanded_upper) or re.search(r'\bG01\b', expanded_upper) or 
                    expanded_upper.startswith('G1') or expanded_upper.startswith('G01'))
            is_g2 = ('G2' in expanded_upper or 'G02' in expanded_upper) and not any(cmd in expanded_upper for cmd in ['G20', 'G21', 'G28', 'G92'])
            is_g3 = ('G3' in expanded_upper or 'G03' in expanded_upper) and not any(cmd in expanded_upper for cmd in ['G30', 'G38'])
            is_motion_line = is_g0 or is_g1 or is_g2 or is_g3
            
            if self.debug and is_motion_line:
                print(f"[DEBUG] ========== 运动指令解析 ==========")
                print(f"[DEBUG] 行号: {line_num}")
                print(f"[DEBUG] 原始行: '{line.strip()}'")
                print(f"[DEBUG] 展开后: '{expanded_line}'")
                print(f"[DEBUG] expanded_upper: '{expanded_upper}'")
                print(f"[DEBUG] call_level: {self.call_level}")
                print(f"[DEBUG] 当前坐标: X={current.x:.3f}, Y={current.y:.3f}, Z={current.z:.3f}")
                print(f"[DEBUG] RTCP状态: {self.collector.rtcp_enabled}")
                print(f"[DEBUG] is_g0={is_g0}, is_g1={is_g1}, is_g2={is_g2}, is_g3={is_g3}")
            
            # ===== 修复后的 parse_coord 函数 =====
            def parse_coord(axis):
                """
                解析坐标值
                支持格式:
                - 纯数字: X20, Y-10.5, Z+5
                - 等号格式: X=20, Y=-10.5
                - 方括号表达式: X[#<_x> + #<r>], Y[20 - 10]
                
                参数:
                    axis: 轴名称 ('X', 'Y', 'Z', 'A', 'B', 'C', 'I', 'J', 'K', 'R', 'F', 'S', 'P')
                
                返回:
                    解析后的浮点数值，如果未找到则返回 None
                """
                nonlocal_expanded = expanded_upper
                
                # ===== 先尝试匹配方括号表达式 =====
                bracket_pattern = rf'{axis}\[(.*?)\]'
                bracket_match = re.search(bracket_pattern, nonlocal_expanded, re.IGNORECASE)
                if bracket_match:
                    expr = bracket_match.group(1)
                    try:
                        # 展开命名参数
                        expanded_expr = self.expand_named_params(expr)
                        # 计算表达式
                        if self.collector and hasattr(self.collector, 'var_manager'):
                            result = self.collector.var_manager.evaluate_expression(expanded_expr, self.call_level)
                        else:
                            # 简单eval后备
                            result = eval(expanded_expr, {"__builtins__": {}}, {"math": math})
                        if self.debug:
                            print(f"[DEBUG]   parse_coord({axis}): 模式='BRACKET EXPR', "
                                  f"原始表达式='{expr}', 展开='{expanded_expr}', 解析值={result}")
                        return float(result)
                    except Exception as e:
                        if self.debug:
                            print(f"[DEBUG]   parse_coord({axis}): 方括号求值失败 '{expr}': {e}")
                        return None
                
                # ===== 再匹配带等号的数字: X=20, Y=-10.5 =====
                eq_pattern = rf'{axis}\s*=\s*([+\-]?\d+\.?\d*)'
                eq_match = re.search(eq_pattern, nonlocal_expanded, re.IGNORECASE)
                if eq_match:
                    value = float(eq_match.group(1))
                    if self.debug:
                        print(f"[DEBUG]   parse_coord({axis}): 模式='EQ VALUE', "
                              f"原始匹配='{eq_match.group(1)}', 解析值={value}")
                    return value
                
                # ===== 再匹配普通数字: X20, Y-10.5, Z+5 =====
                number_pattern = rf'{axis}([+\-]?\d+\.?\d*)'
                number_match = re.search(number_pattern, nonlocal_expanded, re.IGNORECASE)
                if number_match:
                    value = float(number_match.group(1))
                    if self.debug:
                        print(f"[DEBUG]   parse_coord({axis}): 模式='AXIS VALUE', "
                              f"原始匹配='{number_match.group(1)}', 解析值={value}")
                    return value
                
                return None
            
            # 解析所有坐标。保留原始轴字，后续 G92/G52 需要区分
            # “未指定该轴”和“指定为 0”两种情况。
            x_word = parse_coord('X')
            y_word = parse_coord('Y')
            z_word = parse_coord('Z')
            a_word = parse_coord('A')
            b_word = parse_coord('B')
            c_word = parse_coord('C')
            u_word = parse_coord('U')
            v_word = parse_coord('V')
            w_word = parse_coord('W')

            x = x_word
            y = y_word
            z = z_word
            a = a_word
            b = b_word
            c = c_word
            u = u_word
            v = v_word
            w = w_word
            
            i = parse_coord('I')
            j = parse_coord('J')
            k = parse_coord('K')
            r_coord = parse_coord('R')

            
            # 进给率
            f_match = re.search(r'F\s*=?\s*([+-]?\d*\.?\d+)', expanded_upper, re.I)
            if not f_match:
                f_match = re.search(r'F\s*([+-]?\d*\.?\d+)', expanded_upper, re.I)
            if f_match:
                feed_val = float(f_match.group(1))
                self.collector.set_feed_rate(feed_val)
                if self.debug:
                    print(f"[DEBUG]   设置进给率: F={feed_val}")
            
            # 主轴速度
            s_match = re.search(r'S\s*=?\s*(\d+)', expanded_upper, re.I)
            if not s_match:
                s_match = re.search(r'S\s*(\d+)', expanded_upper, re.I)
            if s_match:
                self.collector.set_spindle_speed(float(s_match.group(1)))
                if self.debug:
                    print(f"[DEBUG]   设置主轴速度: S={s_match.group(1)}")
            
            # 暂停参数
            p_dwell_match = re.search(r'P\s*=?\s*([+-]?\d*\.?\d+)', expanded_upper, re.I)
            if not p_dwell_match:
                p_dwell_match = re.search(r'P\s*([+-]?\d*\.?\d+)', expanded_upper, re.I)
            
            # 圆弧圈数参数 (P)
            p_turn_match = re.search(r'P\s*=?\s*([+-]?\d*\.?\d+)', expanded_upper, re.I)
            if not p_turn_match:
                p_turn_match = re.search(r'P\s*([+-]?\d*\.?\d+)', expanded_upper, re.I)
            p_turn = float(p_turn_match.group(1)) if p_turn_match else 1.0
            
            # 计算绝对/相对坐标
            if not self.collector.is_absolute:
                x = current.x + x if x is not None else current.x
                y = current.y + y if y is not None else current.y
                z = current.z + z if z is not None else current.z
                a = current.a + (a if a is not None else 0)
                b = current.b + (b if b is not None else 0)
                c = current.c + (c if c is not None else 0)
                u = getattr(current, 'u', 0.0) + (u if u is not None else 0)
                v = getattr(current, 'v', 0.0) + (v if v is not None else 0)
                w = getattr(current, 'w', 0.0) + (w if w is not None else 0)
            else:
                x = x if x is not None else current.x
                y = y if y is not None else current.y
                z = z if z is not None else current.z
                a = a if a is not None else current.a
                b = b if b is not None else current.b
                c = c if c is not None else current.c
                u = u if u is not None else getattr(current, 'u', 0.0)
                v = v if v is not None else getattr(current, 'v', 0.0)
                w = w if w is not None else getattr(current, 'w', 0.0)
        
        # ==================== G代码运动 ====================
        if self.collector:
            # 刀具补偿
            h_match = re.search(r'H\s*=?\s*(\d+)', line_upper, re.I)
            if not h_match:
                h_match = re.search(r'H\s*(\d+)', line_upper, re.I)
            if h_match:
                h_code = int(h_match.group(1))
                self.collector.set_tool_length_offset(h_code)
            
            d_match = re.search(r'D\s*=?\s*(\d+)', line_upper, re.I)
            if not d_match:
                d_match = re.search(r'D\s*(\d+)', line_upper, re.I)
            if d_match:
                self.collector.set_tool_radius_offset(int(d_match.group(1)))
            
            t_match = re.search(r'T\s*=?\s*(\d+)', line_upper, re.I)
            if not t_match:
                t_match = re.search(r'T\s*(\d+)', line_upper, re.I)
            if t_match:
                self.collector.select_tool(int(t_match.group(1)))
            
            # 换刀
            if 'M6' in line_upper:
                tool = int(t_match.group(1)) if t_match else 1
                self.collector.change_tool(tool)
            
            # 主轴控制
            if 'M3' in line_upper:
                self.collector.spindle_control(1)
            elif 'M4' in line_upper:
                self.collector.spindle_control(2)
            elif 'M5' in line_upper:
                self.collector.spindle_control(0)
            
            # 冷却液控制
            if 'M8' in line_upper:
                self.collector.set_flood(True)
            elif 'M7' in line_upper:
                self.collector.set_mist(True)
            elif 'M9' in line_upper:
                self.collector.set_flood(False)
                self.collector.set_mist(False)
            
            # M66 等待输入
            if 'M66' in line_upper:
                e_match = re.search(r'E\s*=?\s*(\d+)', line_upper) or re.search(r'E\s*(\d+)', line_upper)
                l_match = re.search(r'L\s*=?\s*(\d+)', line_upper) or re.search(r'L\s*(\d+)', line_upper)
                if e_match:
                    pin = int(e_match.group(1))
                    mode = int(l_match.group(1)) if l_match else 0
                    self.collector.wait_input(pin, mode)
            
            # M68 模拟量输出
            if 'M68' in line_upper:
                e_match = re.search(r'E\s*=?\s*(\d+)', line_upper) or re.search(r'E\s*(\d+)', line_upper)
                q_match = re.search(r'Q\s*=?\s*([+-]?\d*\.?\d+)', line_upper) or re.search(r'Q\s*([+-]?\d*\.?\d+)', line_upper)
                if e_match and q_match:
                    pin = int(e_match.group(1))
                    value = float(q_match.group(1))
                    self.collector.set_analog_output(pin, value)
            
            current = self.collector.current_pos
            
            # G0 - 快速移动
            if is_g0:
                if x is not None or y is not None or z is not None:
                    if self.debug:
                        print(f"[G0] 执行快速移动: X={x}, Y={y}, Z={z}, A={a}, B={b}, C={c}")
                    self.collector.straight_traverse(
                        x if x is not None else current.x,
                        y if y is not None else current.y,
                        z if z is not None else current.z,
                        a if a is not None else current.a,
                        b if b is not None else current.b,
                        c if c is not None else current.c,
                        u if u is not None else 0,
                        v if v is not None else 0,
                        w if w is not None else 0
                    )
            
            # G1 - 直线进给
            elif is_g1:
                if x is not None or y is not None or z is not None:
                    if self.debug:
                        print(f"[G1] 执行直线进给: X={x}, Y={y}, Z={z}")
                    self.collector.straight_feed(
                        x if x is not None else current.x,
                        y if y is not None else current.y,
                        z if z is not None else current.z,
                        a if a is not None else current.a,
                        b if b is not None else current.b,
                        c if c is not None else current.c,
                        u if u is not None else 0,
                        v if v is not None else 0,
                        w if w is not None else 0
                    )
            
            # G2 - 顺时针圆弧


            # G2 - 顺时针圆弧
            elif is_g2:
                turn = -int(p_turn) if p_turn >= 1 else -1
                
                # 如果只指定IJK没指定X/Y，终点=起点(全圆)
                if x is None and y is None:
                    if i is not None or j is not None:
                        x = current.x
                        y = current.y
                
                if x is not None or y is not None:
                    x = x if x is not None else current.x
                    y = y if y is not None else current.y
                    
                    # ===== R格式圆弧：计算圆心 =====
                    if r_coord is not None and (i is None or j is None):
                        dx = x - current.x
                        dy = y - current.y
                        d = math.sqrt(dx*dx + dy*dy)
                        if d > 0 and abs(r_coord) >= d/2 - 1e-9:
                            h = math.sqrt(max(0, r_coord*r_coord - d*d/4))
                            # G2 (turn < 0)
                            if r_coord > 0:
                                # R>0: 小弧(<180°)
                                cx_calc = current.x + dx/2 - h * dy/d
                                cy_calc = current.y + dy/2 + h * dx/d
                            else:
                                # R<0: 大弧(>180°)
                                cx_calc = current.x + dx/2 + h * dy/d
                                cy_calc = current.y + dy/2 - h * dx/d
                                
                                
                                
                            # 转换为增量IJ
                            i = cx_calc - current.x
                            j = cy_calc - current.y
                        else:
                            # 半圆或无效R
                            i = (x - current.x) / 2
                            j = (y - current.y) / 2
                    
                    # ===== 确保IJ是增量（相对于起点） =====
                    # ===== IJK 处理逻辑（对照 LinuxCNC interp_convert.cc arc_data_ijk）=====
                    # 在 G91.1（增量模式）下，I, J 被视为相对于起点
                    # 在 G90.1（绝对模式）下，I, J 被视为绝对坐标
                    i_final = i if i is not None else 0.0
                    j_final = j if j is not None else 0.0
                    k_final = k if k is not None else 0.0

                    # 如果是绝对 IJK 模式 (G90.1)，转换为增量
                    if self.collector and self.collector.ijk_distance_mode == 90:  # 90 = G90.1 绝对IJK
                        i_final = i_final - current.x
                        j_final = j_final - current.y
                        k_final = k_final - current.z
                    # 在增量模式下（G91.1，默认），IJK 已经是相对于起点的值，无需转换
                    
                    if self.debug:
                        center_x_dbg = current.x + i_final
                        center_y_dbg = current.y + j_final
                        print(f"[G2] 执行圆弧: end=({x},{y}) "
                              f"center=({center_x_dbg:.1f},{center_y_dbg:.1f}) "
                              f"I={i_final:.3f} J={j_final:.3f} turn={turn}")
                    
                    self.collector.arc_feed(
                        x, y,
                        z if z is not None else current.z,
                        a if a is not None else current.a,
                        b if b is not None else current.b,
                        c if c is not None else current.c,
                        i_final if i_final != 0 else 0,
                        j_final if j_final != 0 else 0,
                        k if k is not None else 0,
                        turn, self.collector.feedrate,
                        u if u is not None else 0,
                        v if v is not None else 0,
                        w if w is not None else 0
                    )
         
            # G3 - 逆时针圆弧

            # G3 - 逆时针圆弧
            elif is_g3:
                turn = int(p_turn) if p_turn >= 1 else 1
                
                # 如果只指定IJK没指定X/Y，终点=起点(全圆)
                if x is None and y is None:
                    if i is not None or j is not None:
                        x = current.x
                        y = current.y
                
                if x is not None or y is not None:
                    x = x if x is not None else current.x
                    y = y if y is not None else current.y
                    
                    # ===== R格式圆弧：计算圆心 =====
                    if r_coord is not None and (i is None or j is None):
                        dx = x - current.x
                        dy = y - current.y
                        d = math.sqrt(dx*dx + dy*dy)
                        if d > 0 and abs(r_coord) >= d/2 - 1e-9:
                            h = math.sqrt(max(0, r_coord*r_coord - d*d/4))
                            # G3 (turn > 0)
                            if r_coord > 0:
                                # R>0: 小弧(<180°)
                                cx_calc = current.x + dx/2 + h * dy/d
                                cy_calc = current.y + dy/2 - h * dx/d
                            else:
                                # R<0: 大弧(>180°)
                                cx_calc = current.x + dx/2 - h * dy/d
                                cy_calc = current.y + dy/2 + h * dx/d
                            # 转换为增量IJ
                            i = cx_calc - current.x
                            j = cy_calc - current.y
                        else:
                            # 半圆或无效R
                            i = (x - current.x) / 2
                            j = (y - current.y) / 2
                    
                    # ===== 确保IJ是增量（相对于起点） =====
                    # ===== IJK 处理逻辑（对照 LinuxCNC interp_convert.cc arc_data_ijk）=====
                    # 在 G91.1（增量模式）下，I, J 被视为相对于起点
                    # 在 G90.1（绝对模式）下，I, J 被视为绝对坐标
                    i_final = i if i is not None else 0.0
                    j_final = j if j is not None else 0.0
                    k_final = k if k is not None else 0.0

                    # 如果是绝对 IJK 模式 (G90.1)，转换为增量
                    if self.collector and self.collector.ijk_distance_mode == 90:  # 90 = G90.1 绝对IJK
                        i_final = i_final - current.x
                        j_final = j_final - current.y
                        k_final = k_final - current.z
                    # 在增量模式下（G91.1，默认），IJK 已经是相对于起点的值，无需转换
                    
                    if self.debug:
                        center_x_dbg = current.x + i_final
                        center_y_dbg = current.y + j_final
                        print(f"[G3] 执行圆弧: end=({x},{y}) "
                              f"center=({center_x_dbg:.1f},{center_y_dbg:.1f}) "
                              f"I={i_final:.3f} J={j_final:.3f} turn={turn}")
                    
                    self.collector.arc_feed(
                        x, y,
                        z if z is not None else current.z,
                        a if a is not None else current.a,
                        b if b is not None else current.b,
                        c if c is not None else current.c,
                        i_final if i_final != 0 else 0,
                        j_final if j_final != 0 else 0,
                        k if k is not None else 0,
                        turn, self.collector.feedrate,
                        u if u is not None else 0,
                        v if v is not None else 0,
                        w if w is not None else 0
                    )
         
            # G4 - 暂停
            elif re.search(r'\bG4\b', expanded_upper) or re.search(r'\bG04\b', expanded_upper):
                dwell_time = 0.0
                if p_dwell_match:
                    dwell_time = float(p_dwell_match.group(1)) / 1000.0
                if dwell_time > 0:
                    self.collector.dwell(dwell_time)
            
            # G28/G30 返回参考点
            if 'G28' in line_upper and self.collector and 'G28.1' not in line_upper:
                ref_x, ref_y, ref_z = self.collector.get_g28()
                self.collector.straight_traverse(ref_x, ref_y, ref_z, current.a, current.b, current.c, 0, 0, 0)
            
            if 'G30' in line_upper and self.collector and 'G30.1' not in line_upper:
                ref_x, ref_y, ref_z = self.collector.get_g30()
                self.collector.straight_traverse(ref_x, ref_y, ref_z, current.a, current.b, current.c, 0, 0, 0)
        
        # ==================== 平面选择 ====================
        
        if re.search(r'\bG17\b', line_upper):
            self.current_plane = 17
            if self.collector:
                self.collector.select_plane(17)
        elif re.search(r'\bG18\b', line_upper):
            self.current_plane = 18
            if self.collector:
                self.collector.select_plane(18)
        elif re.search(r'\bG19\b', line_upper):
            self.current_plane = 19
            if self.collector:
                self.collector.select_plane(19)
        
        # ==================== 坐标模式 ====================
        
        if re.search(r'\bG90\b', line_upper) and self.collector:
            self.collector.set_absolute_mode()
        elif re.search(r'\bG91\b', line_upper) and self.collector:
            self.collector.set_relative_mode()
        
        # ==================== 单位 ====================
        
        if re.search(r'\bG20\b', line_upper) and self.collector:
            self.collector.state.units = 20
            if self.debug:
                print(f"[DEBUG] 设置为英制单位 G20")
        elif re.search(r'\bG21\b', line_upper) and self.collector:
            self.collector.state.units = 21
            if self.debug:
                print(f"[DEBUG] 设置为公制单位 G21")
        
        # ==================== 刀具半径补偿 ====================
        
        if re.search(r'\bG40\b', line_upper) and self.collector:
            self.collector.set_cutter_compensation_state(CompType.OFF, 0.0, 0, 0)
        
        if (re.search(r'\bG41\b', line_upper) or re.search(r'\bG41\.1\b', line_upper)) and self.collector:
            d_match = re.search(r'D\s*=?\s*(\d+)', line, re.I) or re.search(r'D\s*(\d+)', line, re.I)
            if d_match:
                d_code = int(d_match.group(1))
            else:
                d_code = self.collector.current_d_code if self.collector.current_d_code > 0 else 1
            
            radius = self.tool_radius_map.get(d_code, 0.0)
            self.collector.set_cutter_compensation_state(CompType.LEFT, radius, 0, d_code)
            self.collector.set_tool_radius_offset(d_code)
        
        elif (re.search(r'\bG42\b', line_upper) or re.search(r'\bG42\.1\b', line_upper)) and self.collector:
            d_match = re.search(r'D\s*=?\s*(\d+)', line, re.I) or re.search(r'D\s*(\d+)', line, re.I)
            if d_match:
                d_code = int(d_match.group(1))
            else:
                d_code = self.collector.current_d_code if self.collector.current_d_code > 0 else 1
            
            radius = self.tool_radius_map.get(d_code, 0.0)
            self.collector.set_cutter_compensation_state(CompType.RIGHT, radius, 0, d_code)
            self.collector.set_tool_radius_offset(d_code)
        
        # ==================== 固定循环 ====================
        
        if re.search(r'\bG80\b', line_upper) and self.collector:
            self.collector.canned_cycle_active = False
        
        # ==================== 坐标系设置 ====================
        
        if re.search(r'\bG92\b', line_upper) and self.collector and 'G92.1' not in line_upper and 'G92.2' not in line_upper and 'G92.3' not in line_upper:
            self.collector.set_g92_offset(
                x_word if 'x_word' in locals() else None,
                y_word if 'y_word' in locals() else None,
                z_word if 'z_word' in locals() else None,
                a_word if 'a_word' in locals() else None,
                b_word if 'b_word' in locals() else None,
                c_word if 'c_word' in locals() else None
            )
        
        if re.search(r'\bG92\.1\b', line_upper) and self.collector:
            self.collector.clear_g92()
        
        if re.search(r'\bG52\b', line_upper) and self.collector:
            self.collector.set_g52_offset(
                x if x is not None else 0,
                y if y is not None else 0,
                z if z is not None else 0
            )
        
        return None



    def parse_string(self, program: str, filename: str = "program.ngc") -> ToolpathData:
        import time

        start_time = time.time()

        self.current_filename = filename

        lines = program.split('\n')
        processed_lines = []
        line_number_map = []

        for line_num, line in enumerate(lines, 1):
            processed = self._preprocess_line(line)
            if processed and processed.strip():
                if re.match(r'o<[^>]+>\s*sub', processed.lower()):
                    continue
                if re.match(r'o<[^>]+>\s*endsub', processed.lower()):
                    continue
                processed_lines.append(processed)
                line_number_map.append(line_num)
                self._line_number_map[len(processed_lines) - 1] = line_num

        self.collector = GLCanonPathCollectorWithRTCP(
            colors=DEFAULT_COLORS.copy(), max_points=self.max_points,
            acceleration=self.acceleration, max_rapid_rate=self.max_rapid_rate,
            max_feed_rate=self.max_feed_rate, kinematics_type=self.kinematics_type,
            kinematics_params=self.kinematics_params, debug=self.debug
        )

        for h_code, length in self.tool_length_map.items():
            self.collector.tool_lengths[h_code] = length
        for d_code, radius in self.tool_radius_map.items():
            self.collector.tool_radii[d_code] = radius

        control_handler = self.collector.control_handler

        i = 0
        loop_count = 0
        max_iterations = len(processed_lines) * 100

        while i < len(processed_lines):
            loop_count += 1
            if loop_count > max_iterations:
                if self.debug:
                    print(f"  [警告] 解析达到最大迭代次数")
                break

            if self.collector and len(self.collector.data.segments) >= self.max_points:
                if self.debug:
                    print(f"  [警告] 达到最大点数限制 {self.max_points}")
                break

            line = processed_lines[i]
            original_line_num = line_number_map[i]

            if line.startswith('/') and self.collector and self.collector.block_delete:
                i += 1
                continue

            if control_handler and line.strip():
                next_idx = control_handler.parse_o_code(
                    line, original_line_num, i, processed_lines, self.call_level
                )
                if next_idx is not None:
                    i = next_idx
                    continue

            self._parse_line(line, original_line_num)
            i += 1

        if self.saved_states:
            for saved_state in reversed(self.saved_states):
                if saved_state.get('restore_on_return', False) and self.collector:
                    self.collector.restore_state(saved_state)

        if self.debug:
            print(f"  [解析完成] 生成 {len(self.collector.data.segments) if self.collector else 0} 个路径段")

        if self.collector:
            self.collector.data.filename = filename
            self.collector.data.kinematics_type = self.kinematics_type.name
            self.collector.data.point_count = len(self.collector.data.segments)
            self.collector.data.generation_time_ms = (time.time() - start_time) * 1000
            self.collector.data.calculate_bounds()
            self.collector.data.calculate_length_and_time()

            return self.collector.data

        return ToolpathData()


# ==================== 完整RS274解析器 ====================

class FullRS274Parser:
    """完整的RS274NGC解释器包装器 - 支持多实例"""

    def __init__(self, max_points: int = 50000,
                 tool_radius_map: Dict[int, float] = None,
                 tool_length_map: Dict[int, float] = None,
                 acceleration: float = 500.0,
                 max_rapid_rate: float = 10000.0,
                 max_feed_rate: float = 5000.0,
                 kinematics_type: KinematicsType = KinematicsType.TRT_BC,
                 kinematics_params: Dict[str, Any] = None,
                 debug: bool = False):

        self.max_points = max_points
        self.tool_radius_map = tool_radius_map or {1: 2.0, 2: 3.0, 3: 4.0}
        self.tool_length_map = tool_length_map or {1: 100.0, 2: 120.0, 3: 150.0}
        self.acceleration = acceleration
        self.max_rapid_rate = max_rapid_rate
        self.max_feed_rate = max_feed_rate
        self.kinematics_type = kinematics_type
        self.kinematics_params = kinematics_params or {}
        self.debug = debug

        self._builtin_parser: Optional[FullGCodeParserWithRTCP] = None
        self.collector: Optional[GLCanonPathCollectorWithRTCP] = None

        self._init_parser()

    def _init_parser(self):
        self._builtin_parser = FullGCodeParserWithRTCP(
            max_points=self.max_points,
            tool_radius_map=self.tool_radius_map,
            tool_length_map=self.tool_length_map,
            acceleration=self.acceleration,
            max_rapid_rate=self.max_rapid_rate,
            max_feed_rate=self.max_feed_rate,
            kinematics_type=self.kinematics_type,
            kinematics_params=self.kinematics_params,
            debug=self.debug
        )

    def register_subroutine(self, name: str, content: str):
        if not self._builtin_parser:
            self._init_parser()

        lines = content.split('\n')
        sub_lines = []
        sub_line_nums = []

        in_sub = False
        for i, line in enumerate(lines):
            processed = self._builtin_parser._preprocess_line(line)
            if processed and processed.strip():
                if re.match(r'o<([^>]+)>\s*sub', processed, re.I):
                    in_sub = True
                    continue
                if re.match(r'o<([^>]+)>\s*endsub', processed, re.I):
                    in_sub = False
                    continue
                if in_sub:
                    sub_lines.append(processed)
                    sub_line_nums.append(i + 1)

        if sub_lines:
            self._builtin_parser.subroutines[name] = list(zip(sub_line_nums, sub_lines))

    def parse_string(self, program: str, filename: str = "program.ngc") -> ToolpathData:
        if not self._builtin_parser:
            self._init_parser()

        return self._builtin_parser.parse_string(program, filename)

    def parse_file(self, filename: str) -> ToolpathData:
        if not os.path.exists(filename):
            raise FileNotFoundError(f"文件不存在: {filename}")

        with open(filename, 'r', encoding='utf-8', errors='ignore') as f:
            program = f.read()

        return self.parse_string(program, os.path.basename(filename))

    def set_subroutine_path(self, path: str):
        if self._builtin_parser:
            self._builtin_parser.set_subroutine_path(path)


# ==================== CNC内核 ====================

class CNCKernel:
    """CNC内核 - 整合所有功能的主控制器"""

    def __init__(self, debug: bool = False, name: str = "CNC1"):
        self.name = name
        self.debug = debug
        self.virtual_hal = VirtualHAL(debug=debug)
        self.state_machine = MachineStateMachine(self.virtual_hal, debug=debug)

        self.parser: Optional[FullRS274Parser] = None
        self.current_toolpath: Optional[ToolpathData] = None
        self.status = CNCRuntimeStatus()

        self.is_running = False
        self.is_paused = False
        self.current_segment_index = 0
        self.program_ended = False
        self.single_step_mode = False

        self.planner = TrapezoidalMotionPlanner()

        self.rtcp_enabled = False
        self._kinematics_type = KinematicsType.TRT_BC
        self._kinematics_instance: Optional[BaseKinematics] = None
        self._original_kinematics_type = KinematicsType.TRT_BC

        self._update_kinematics_instance()

        self.run_thread: Optional[threading.Thread] = None
        self.state_monitor_thread: Optional[threading.Thread] = None
        self.state_monitor_running = False

        self.update_callback: Optional[Callable[[CNCRuntimeStatus], None]] = None

        self.tool_table = {
            1: {"length": 100.0, "diameter": 10.0, "name": "端铣刀"},
            2: {"length": 120.0, "diameter": 8.0, "name": "球头刀"},
            3: {"length": 80.0, "diameter": 12.0, "name": "面铣刀"},
            4: {"length": 150.0, "diameter": 6.0, "name": "钻头"},
            5: {"length": 90.0, "diameter": 10.0, "name": "倒角刀"},
        }

        # 初始化默认工具长度
        self.selected_tool = 1
        if 1 in self.tool_table:
            self.status.tool_length = self.tool_table[1].get('length', 100.0)
            self.status.tool_diameter = self.tool_table[1].get('diameter', 10.0)
            self.status.current_tool = 1
        
        # 设置默认坐标系
        self.status.coordinate_mode = "G54"
        self.status.current_offset = "G54"

        self.work_offsets = {
            54: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0},
            55: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0},
            56: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0},
            57: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0},
            58: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0},
            59: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0},
        }
        self.current_work_offset = 54
        
        
        # 设置默认坐标系显示
        self.status.coordinate_mode = "G54"  # 改为G54而非G90
        self.status.current_offset = "G54"
        
        # 设置默认RTCP状态
        self.status.rtcp_enabled = False  # 默认关闭，等G43.4时开启
        
        

        self._init_hal()
        self.start_state_monitor()
        
        
        print(f"✓ CNC内核 [{self.name}] 初始化完成")

    @property
    def kinematics_type(self) -> KinematicsType:
        return self._kinematics_type

    @kinematics_type.setter
    def kinematics_type(self, value: KinematicsType):
        if self._kinematics_type != value:
            self._kinematics_type = value
            self._update_kinematics_instance()
            self.status.kinematics_type = value.name

    def _update_kinematics_instance(self):
        if self._kinematics_type == KinematicsType.IDENTITY:
            self._kinematics_instance = None
        else:
            self._kinematics_instance = KinematicsFactory.create(
                self._kinematics_type, debug=self.debug
            )
            if self._kinematics_instance:
                self._kinematics_instance.set_tool_length(self.status.tool_length)
                self._kinematics_instance.enable_rtcp(self.rtcp_enabled)

    def _init_hal(self):
        self.virtual_hal.set('motion.kins-type', 2)
        self.virtual_hal.set('motion.pivot-length', 250.0)
        self.virtual_hal.set('motion.tool-length', 100.0)

    def _sync_offsets_to_status(self):
        """从解析器的收集器同步偏移信息到status对象"""
        try:
            if hasattr(self, 'parser') and self.parser:
                parser = self.parser
                if hasattr(parser, '_builtin_parser') and parser._builtin_parser:
                    bp = parser._builtin_parser
                    if bp.collector:
                        c = bp.collector
                        # 同步 G5x 偏移
                        self.status.g5x_offset_x = c.g5x_offset_x
                        self.status.g5x_offset_y = c.g5x_offset_y
                        self.status.g5x_offset_z = c.g5x_offset_z
                        self.status.g5x_offset_a = getattr(c, 'g5x_offset_a', 0.0)
                        self.status.g5x_offset_b = getattr(c, 'g5x_offset_b', 0.0)
                        self.status.g5x_offset_c = getattr(c, 'g5x_offset_c', 0.0)
                        
                        # 同步 G92 偏移
                        self.status.g92_x = c.g92_offset_x
                        self.status.g92_y = c.g92_offset_y
                        self.status.g92_z = c.g92_offset_z
                        self.status.g92_a = getattr(c, 'g92_offset_a', 0.0)
                        self.status.g92_b = getattr(c, 'g92_offset_b', 0.0)
                        self.status.g92_c = getattr(c, 'g92_offset_c', 0.0)
                        
                        # 同步刀具长度
                        self.status.tool_length = c.tool_offset.z
                        
                        # ★ 关键：同步 RTCP 状态 ★
                        self.status.rtcp_enabled = c.rtcp_enabled
                        self.rtcp_enabled = c.rtcp_enabled
                        
                        # 同步坐标系
                        if c.g5x_index:
                            self.status.current_offset = f'G{54 + c.g5x_index - 1}'
                            self.status.g5x_index = c.g5x_index
                        
                        return True
        except Exception as e:
            if self.debug:
                print(f"[{self.name}] 同步偏移信息失败: {e}")
        return False



    def _update_world_position(self):
        if self.status.rtcp_enabled and self._kinematics_instance is not None:
            # 根据运动学类型构建正确顺序的joints数组
            # TRT_AC: [X, Y, Z, A, C] (5个)
            # TRT_BC: [X, Y, Z, B, C] (5个)
            config = KINEMATICS_JOINT_CONFIG.get(
                self._kinematics_type if hasattr(self, '_kinematics_type') else self.kinematics_type,
                KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY]
            )
            num_joints = config.get('num_joints', 6)
            joints = [0.0] * num_joints
            
            x_idx = config.get('x_idx', 0)
            y_idx = config.get('y_idx', 1)
            z_idx = config.get('z_idx', 2)
            a_idx = config.get('a_idx', 3)
            b_idx = config.get('b_idx', 4)
            c_idx = config.get('c_idx', 5)
            
            if x_idx >= 0: joints[x_idx] = self.status.machine_x
            if y_idx >= 0: joints[y_idx] = self.status.machine_y
            if z_idx >= 0: joints[z_idx] = self.status.machine_z
            if a_idx >= 0: joints[a_idx] = self.status.machine_a
            if b_idx >= 0: joints[b_idx] = self.status.machine_b
            if c_idx >= 0: joints[c_idx] = self.status.machine_c
            
            world = self._kinematics_instance.joints_to_world(joints)
            self.status.world_x = world[0]
            self.status.world_y = world[1]
            self.status.world_z = world[2]
        else:
            self.status.world_x = self.status.position_x
            self.status.world_y = self.status.position_y
            self.status.world_z = self.status.position_z



    def _update_world_position_bak(self):
        if self.status.rtcp_enabled and self._kinematics_instance is not None:
            # ★修复: 使用MCS(关节坐标)调用正运动学, 不是WCS(工件坐标)★
            joints = [self.status.machine_x, self.status.machine_y, self.status.machine_z,
                      self.status.machine_a, self.status.machine_b, self.status.machine_c]
            world = self._kinematics_instance.joints_to_world(joints)
            self.status.world_x = world[0]
            self.status.world_y = world[1]
            self.status.world_z = world[2]
        else:
            self.status.world_x = self.status.position_x
            self.status.world_y = self.status.position_y
            self.status.world_z = self.status.position_z

    def _update_current_from_machine(self):
        """
        从机器坐标反向计算工件坐标
        
        逆变换顺序:
        机器坐标 → 正运动学 → 绝对坐标 → -偏移链 → 工件坐标
        
        与 _sync_machine_from_current 互为逆操作：
        - _sync_machine_from_current: 工件→机器
        - _update_current_from_machine: 机器→工件
        """
        # ===== 第1步：正运动学 - 机器坐标→绝对坐标 =====
        if self.rtcp_enabled and self._kinematics_instance is not None:
            # 构建符合运动学类型的关节数组
            config = KINEMATICS_JOINT_CONFIG.get(
                self._kinematics_type if hasattr(self, '_kinematics_type') else self.kinematics_type,
                KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY]
            )
            num_joints = config.get('num_joints', 6)
            joints = [0.0] * num_joints
            
            x_idx = config.get('x_idx', 0)
            y_idx = config.get('y_idx', 1)
            z_idx = config.get('z_idx', 2)
            a_idx = config.get('a_idx', 3)
            b_idx = config.get('b_idx', 4)
            c_idx = config.get('c_idx', 5)
            
            if x_idx >= 0 and x_idx < num_joints:
                joints[x_idx] = self.status.machine_x
            if y_idx >= 0 and y_idx < num_joints:
                joints[y_idx] = self.status.machine_y
            if z_idx >= 0 and z_idx < num_joints:
                joints[z_idx] = self.status.machine_z
            if a_idx >= 0 and a_idx < num_joints:
                joints[a_idx] = self.status.machine_a
            if b_idx >= 0 and b_idx < num_joints:
                joints[b_idx] = self.status.machine_b
            if c_idx >= 0 and c_idx < num_joints:
                joints[c_idx] = self.status.machine_c
            
            try:
                world = self._kinematics_instance.joints_to_world(joints)
                abs_x = world[0]
                abs_y = world[1]
                abs_z = world[2]
                abs_a = world[3] if len(world) > 3 else self.status.machine_a
                abs_b = world[4] if len(world) > 4 else self.status.machine_b
                abs_c = world[5] if len(world) > 5 else self.status.machine_c
            except Exception as e:
                if self.debug:
                    print(f"[{self.name}] 正运动学计算失败: {e}")
                abs_x = self.status.machine_x
                abs_y = self.status.machine_y
                abs_z = self.status.machine_z
                abs_a = self.status.machine_a
                abs_b = self.status.machine_b
                abs_c = self.status.machine_c
        else:
            # RTCP禁用：机器坐标=绝对坐标
            abs_x = self.status.machine_x
            abs_y = self.status.machine_y
            abs_z = self.status.machine_z
            abs_a = self.status.machine_a
            abs_b = self.status.machine_b
            abs_c = self.status.machine_c
        
        # ===== 第2步：逆变换 - 绝对坐标→工件坐标 =====
        # 获取偏移值
        g5x_x = getattr(self.status, 'g5x_offset_x', 0.0)
        g5x_y = getattr(self.status, 'g5x_offset_y', 0.0)
        g5x_z = getattr(self.status, 'g5x_offset_z', 0.0)
        g5x_a = getattr(self.status, 'g5x_offset_a', 0.0)
        g5x_b = getattr(self.status, 'g5x_offset_b', 0.0)
        g5x_c = getattr(self.status, 'g5x_offset_c', 0.0)
        
        g92_x = getattr(self.status, 'g92_x', 0.0)
        g92_y = getattr(self.status, 'g92_y', 0.0)
        g92_z = getattr(self.status, 'g92_z', 0.0)
        g92_a = getattr(self.status, 'g92_a', 0.0)
        g92_b = getattr(self.status, 'g92_b', 0.0)
        g92_c = getattr(self.status, 'g92_c', 0.0)
        
        tlo = getattr(self.status, 'tool_length', 0.0)
        
        # 逆序减去偏移（正变换的逆操作）
        # 正变换: 工件 + G92 + XY旋转 + G5x + TLO = 绝对
        # 逆变换: 绝对 - TLO - G5x - XY旋转 - G92 = 工件
        
        x = abs_x
        y = abs_y
        z = abs_z
        a = abs_a
        b = abs_b
        c = abs_c
        
        # 减去 TLO（刀具长度补偿主要在Z方向）
        z = z - tlo
        
        # 减去 G5x 偏移
        x = x - g5x_x
        y = y - g5x_y
        z = z - g5x_z
        a = a - g5x_a
        b = b - g5x_b
        c = c - g5x_c
        
        # 减去 G92 偏移
        x = x - g92_x
        y = y - g92_y
        z = z - g92_z
        a = a - g92_a
        b = b - g92_b
        c = c - g92_c
        
        # 更新工件坐标
        self.status.position_x = x
        self.status.position_y = y
        self.status.position_z = z
        self.status.position_a = a
        self.status.position_b = b
        self.status.position_c = c
        
        # 更新世界坐标
        self._update_world_position()
        
        if self.debug:
            print(f"[MCS→WCS] machine({self.status.machine_x:.1f},{self.status.machine_y:.1f},"
                  f"{self.status.machine_z:.1f}) → workpiece({x:.1f},{y:.1f},{z:.1f})")
            
            
    def _sync_offsets_to_status(self):
        """从解析器的收集器同步偏移信息到status对象"""
        try:
            if hasattr(self, 'parser') and self.parser:
                parser = self.parser
                if hasattr(parser, '_builtin_parser') and parser._builtin_parser:
                    bp = parser._builtin_parser
                    if bp.collector:
                        c = bp.collector
                        # 同步 G5x 偏移
                        self.status.g5x_offset_x = c.g5x_offset_x
                        self.status.g5x_offset_y = c.g5x_offset_y
                        self.status.g5x_offset_z = c.g5x_offset_z
                        self.status.g5x_offset_a = c.g5x_offset_a
                        self.status.g5x_offset_b = c.g5x_offset_b
                        self.status.g5x_offset_c = c.g5x_offset_c
                        
                        # 同步 G92 偏移
                        self.status.g92_x = c.g92_offset_x
                        self.status.g92_y = c.g92_offset_y
                        self.status.g92_z = c.g92_offset_z
                        self.status.g92_a = c.g92_offset_a
                        self.status.g92_b = c.g92_offset_b
                        self.status.g92_c = c.g92_offset_c
                        
                        # 同步刀具长度
                        self.status.tool_length = c.tool_offset.z
                        
                        # 同步 RTCP 状态
                        self.status.rtcp_enabled = c.rtcp_enabled
                        
                        # 同步坐标系
                        if c.g5x_index:
                            self.status.current_offset = f'G{54 + c.g5x_index - 1}'
                            self.status.g5x_index = c.g5x_index
        except Exception as e:
            if self.debug:
                print(f"[{self.name}] 同步偏移信息失败: {e}")


    def _notify_update(self):
        """通知更新 - 增强版，包含关节坐标"""
        self.virtual_hal.update_axis_position('x', self.status.position_x)
        self.virtual_hal.update_axis_position('y', self.status.position_y)
        self.virtual_hal.update_axis_position('z', self.status.position_z)
        
        # 同时更新HAL中的机器坐标
        self.virtual_hal.set('joint.0.position', self.status.machine_x)
        self.virtual_hal.set('joint.1.position', self.status.machine_y)
        self.virtual_hal.set('joint.2.position', self.status.machine_z)

        self.virtual_hal.set('spindle.0.speed', self.status.spindle_speed)
        self.virtual_hal.set('coolant.flood', 1 if self.status.coolant_flood else 0)
        self.virtual_hal.set('coolant.mist', 1 if self.status.coolant_mist else 0)
        self.virtual_hal.set('tool.number', self.status.current_tool)
        self.virtual_hal.set('motion.rtcp-active', 1 if self.status.rtcp_enabled else 0)

        if self.update_callback:
            try:
                self.update_callback(self.status)
            except Exception as e:
                if self.debug:
                    print(f"[{self.name}] 回调执行错误: {e}")

    def start_state_monitor(self):
        if self.state_monitor_thread is None:
            self.state_monitor_running = True
            self.state_monitor_thread = threading.Thread(target=self._monitor_state, daemon=True)
            self.state_monitor_thread.start()

    def _monitor_state(self):
        while self.state_monitor_running:
            self.state_machine.update()
            self.status.state = self.state_machine.get_state()
            time.sleep(0.1)

    def ensure_operational(self) -> bool:
        current_state = self.state_machine.get_state()

        if current_state == MachineStateMachine.STATE_IDLE:
            return True

        if current_state == MachineStateMachine.STATE_OFF:
            self.power_on()
            time.sleep(0.5)
            return self.state_machine.get_state() == MachineStateMachine.STATE_IDLE

        if current_state == MachineStateMachine.STATE_ESTOP:
            self.reset()
            time.sleep(0.5)
            return self.state_machine.get_state() == MachineStateMachine.STATE_IDLE

        return False

    def power_on(self) -> Dict:
        if self.state_machine.request_power_on():
            time.sleep(0.5)
            self.status.state = self.state_machine.get_state()
            self._notify_update()
            return {"success": True, "message": "上电成功"}
        return {"success": False, "message": "无法上电"}

    def reset(self) -> Dict:
        if self.state_machine.reset():
            time.sleep(0.5)
            self.status.state = self.state_machine.get_state()
            self.status.alarm_code = 0
            self.status.alarm_message = ""
            self._notify_update()
            return {"success": True, "message": "复位成功"}
        return {"success": False, "message": "无法复位"}

    def emergency_stop(self) -> Dict:
        if self.state_machine.emergency_stop():
            self.is_running = False
            self.is_paused = False
            self.status.state = MachineState.ESTOP.value
            self._notify_update()
            return {"success": True, "message": "急停已激活"}
        return {"success": False, "message": "无法急停"}

    def load_program_string(self, program: str, filename: str = "program.ngc") -> Dict:
        try:
            self.parser = FullRS274Parser(
                debug=self.debug, kinematics_type=self.kinematics_type
            )
            self.current_toolpath = self.parser.parse_string(program, filename)

            self.status.total_lines = len(self.current_toolpath.segments)
            self.status.current_line = 0
            self.status.progress_percent = 0
            self.status.rtcp_enabled = self.current_toolpath.rtcp_enabled
            self.status.kinematics_type = self.current_toolpath.kinematics_type
            self.status.program_name = filename
            self.status.path_length = self.current_toolpath.total_length
            self.status.total_time = self.current_toolpath.total_time

            self._notify_update()

            return {"success": True, "message": f"程序加载成功", "segments": len(self.current_toolpath.segments)}
        except Exception as e:
            return {"success": False, "message": f"加载失败: {str(e)}"}

    def start_program(self) -> Dict:
        if not self.ensure_operational():
            return {"success": False, "message": "机床未就绪"}

        if not self.current_toolpath:
            return {"success": False, "message": "没有加载程序"}

        self.state_machine.start_program()
        self.is_running = True
        self.is_paused = False
        self.current_segment_index = 0
        
        # 在这行之后添加:
        self.program_ended = False
        
        self.status.state = MachineState.RUNNING.value

        self.run_thread = threading.Thread(target=self._run_program, daemon=True)
        self.run_thread.start()

        self._notify_update()
        return {"success": True, "message": "程序开始运行"}

    def pause_program(self) -> Dict:
        if not self.is_running:
            return {"success": False, "message": "程序未运行"}

        self.state_machine.pause_program()
        self.is_paused = True
        self.status.state = MachineState.PAUSED.value
        self._notify_update()
        return {"success": True, "message": "程序已暂停"}

    def resume_program(self) -> Dict:
        if not self.is_paused:
            return {"success": False, "message": "程序未暂停"}

        self.state_machine.resume_program()
        self.is_paused = False
        self.status.state = MachineState.RUNNING.value
        self._notify_update()
        return {"success": True, "message": "程序已恢复"}

    def stop_program(self) -> Dict:
        self.state_machine.stop_program()
        self.is_running = False
        self.is_paused = False
        self.status.state = MachineState.IDLE.value
        self._notify_update()
        return {"success": True, "message": "程序已停止"}


    def _run_program(self):
        """按照真实速度曲线执行G代码"""
        if not self.current_toolpath:
            return

        total_segments = len(self.current_toolpath.segments)
        
        if self.debug:
            print(f"\n{'='*60}")
            print(f"[执行] 开始执行程序: {self.status.program_name}")
            print(f"[执行] 总段数: {total_segments}")
            print(f"[执行] 路径总长: {self.current_toolpath.total_length:.2f} mm")
            print(f"[执行] 预估时间: {self.current_toolpath.total_time:.2f} s")
            print(f"{'='*60}\n")
        
        # 预计算所有运动段的速度曲线（用于精确时间估算）
        motion_profiles = []
        for segment in self.current_toolpath.segments:
            if segment.type in [MoveType.RAPID, MoveType.FEED]:
                start = segment.start
                end = segment.end
                dx = end.x - start.x
                dy = end.y - start.y
                dz = end.z - start.z
                # ★ 修复: 将旋转角度变化计入等效距离 ★
                da = end.a - start.a
                db = end.b - start.b
                dc = end.c - start.c
                linear_dist = math.sqrt(dx*dx + dy*dy + dz*dz)
                angular_dist = math.sqrt(da*da + db*db + dc*dc)
                distance = math.sqrt(linear_dist*linear_dist + angular_dist*angular_dist)
                if distance < CART_FUZZ and angular_dist > CART_FUZZ:
                    distance = angular_dist
                
                is_rapid = (segment.type == MoveType.RAPID)
                feedrate = segment.feedrate if segment.type == MoveType.FEED else None
                profile = self.planner.plan_motion(distance, feedrate, is_rapid)
                motion_profiles.append(profile)
            elif segment.type in [MoveType.ARC_CW, MoveType.ARC_CCW]:
                radius = getattr(segment, 'radius', 1.0)
                turn = abs(getattr(segment, 'turn', 1))
                arc_length = turn * 2.0 * math.pi * radius
                profile = self.planner.plan_motion(arc_length, segment.feedrate, False)
                motion_profiles.append(profile)
            else:
                motion_profiles.append(MotionProfile())
        
        # 主执行循环
        for seg_idx in range(total_segments):
            if not self.is_running or self.program_ended:
                break
            
            # 检查急停和报警
            if self.state_machine.is_estop() or self.state_machine.is_alarm():
                self.is_running = False
                if self.debug:
                    print(f"[执行] 紧急停止! 状态={self.state_machine.get_state()}")
                break
            
            # 检查暂停
            while self.is_paused:
                time.sleep(0.05)
                if not self.is_running:
                    break
            
            segment = self.current_toolpath.segments[seg_idx]
            
            
            # ★ 修复: 直接从segment同步RTCP状态 ★
            if segment.type == MoveType.RTCP_ON:
                self.status.rtcp_enabled = True
                self.rtcp_enabled = True
                if self._kinematics_instance:
                    self._kinematics_instance.enable_rtcp(True)
            elif segment.type == MoveType.RTCP_OFF:
                self.status.rtcp_enabled = False
                self.rtcp_enabled = False
                if self._kinematics_instance:
                    self._kinematics_instance.enable_rtcp(False)
            elif hasattr(segment, 'is_rtcp'):
                self.status.rtcp_enabled = segment.is_rtcp
                self.rtcp_enabled = segment.is_rtcp
                if self._kinematics_instance:
                    self._kinematics_instance.enable_rtcp(segment.is_rtcp)            
            
            self.current_segment_index = seg_idx

        
            # ★ 在这里添加偏移同步 ★
            self._sync_offsets_to_status()
            
            # 更新状态
            self.status.current_line = segment.line_number
            self.status.progress_percent = (seg_idx / total_segments) * 100.0
            self.status.feedrate = segment.feedrate if segment.type == MoveType.FEED else 0
            
            # 处理RTCP状态
            # 处理RTCP状态
            if segment.type == MoveType.RTCP_ON:
                self.status.rtcp_enabled = True
                self.rtcp_enabled = True
                if self._kinematics_instance:
                    self._kinematics_instance.enable_rtcp(True)
            elif segment.type == MoveType.RTCP_OFF:
                self.status.rtcp_enabled = False
                self.rtcp_enabled = False
                if self._kinematics_instance:
                    self._kinematics_instance.enable_rtcp(False)

            # ★ 每次运动段执行前同步偏移 ★
            self._sync_offsets_to_status()
            
            self._notify_update()
            
            # 执行运动段
            profile = motion_profiles[seg_idx] if seg_idx < len(motion_profiles) else None
            self._execute_segment_real(segment, profile)
            
            # 更新世界坐标
            if hasattr(segment, 'world_end') and segment.world_end:
                self.status.world_x = segment.world_end.x
                self.status.world_y = segment.world_end.y
                self.status.world_z = segment.world_end.z
            
            self.current_segment_index = seg_idx + 1
            
            # 单步模式
            if self.single_step_mode:
                self.is_paused = True
                self.status.state = MachineState.PAUSED.value
                self.single_step_mode = False
                self._notify_update()
        
        # 程序完成
        self.program_ended = True
        self.status.progress_percent = 100.0
        self.is_running = False
        self.state_machine.transition_to(MachineStateMachine.STATE_IDLE)
        self.status.state = MachineState.IDLE.value
        self._notify_update()
        
        if self.debug:
            print(f"[执行] 程序执行完成! 最终进度=100%")
    
    def _execute_segment_real(self, segment: MoveSegment, profile: MotionProfile = None):
        """按照真实速度曲线执行运动段"""
        
        # ★ 同步偏移信息 ★
        self._sync_offsets_to_status()        
        
        # 暂停
        if segment.type == MoveType.DWELL:
            dwell_time = getattr(segment, 'dwell_time', 0.0)
            if dwell_time > 0:
                if self.debug:
                    print(f"  [G4] 暂停 {dwell_time*1000:.0f}ms")
                start = time.time()
                while time.time() - start < dwell_time:
                    if not self.is_running:
                        return
                    while self.is_paused:
                        time.sleep(0.05)
                        start += 0.05
                    time.sleep(0.01)
            return
        
        # RTCP/程序结束
        if segment.type in [MoveType.RTCP_ON, MoveType.RTCP_OFF, MoveType.PROGRAM_END]:
            time.sleep(0.05)
            return
        
        # 无运动距离
        if not profile or profile.duration <= 0:
            if hasattr(segment, 'end'):
                self._set_position_from_point(segment.end)
            return
        
        # ===== 直线运动 =====
        if segment.type in [MoveType.RAPID, MoveType.FEED]:
            self._execute_linear_real(segment, profile)
        
        # ===== 圆弧运动 =====
        elif segment.type in [MoveType.ARC_CW, MoveType.ARC_CCW]:
            self._execute_arc_real(segment, profile)
    
    def _sync_offsets_to_status(self):
        """从解析器的收集器同步偏移信息到status对象"""
        try:
            if hasattr(self, 'parser') and self.parser:
                parser = self.parser
                if hasattr(parser, '_builtin_parser') and parser._builtin_parser:
                    bp = parser._builtin_parser
                    if bp.collector:
                        c = bp.collector
                        self.status.g5x_offset_x = c.g5x_offset_x
                        self.status.g5x_offset_y = c.g5x_offset_y
                        self.status.g5x_offset_z = c.g5x_offset_z
                        self.status.g92_x = c.g92_offset_x
                        self.status.g92_y = c.g92_offset_y
                        self.status.g92_z = c.g92_offset_z
        except Exception:
            pass  # 如果解析器尚未初始化，使用默认值0
    
    
    def _execute_linear_real(self, segment: MoveSegment, profile: MotionProfile):
        """执行直线运动（真实速度曲线）- 适配工件坐标"""
        
        # ★修复: 从segment同步RTCP状态★
        if hasattr(segment, 'is_rtcp') and self._kinematics_instance:
            expected_rtcp = segment.is_rtcp
            if self.rtcp_enabled != expected_rtcp:
                self.rtcp_enabled = expected_rtcp
                self.status.rtcp_enabled = expected_rtcp
                self._kinematics_instance.enable_rtcp(expected_rtcp)
        
        # ★ 同步偏移信息 ★
        self._sync_offsets_to_status()        
        
        # 使用工件坐标
        world_start = segment.get_start_in_world()
        world_end = segment.get_end_in_world()
        
        start = world_start
        end = world_end
        
        dx = end.x - start.x
        dy = end.y - start.y
        dz = end.z - start.z
        da = end.a - start.a
        db = end.b - start.b
        dc = end.c - start.c
        
        # ★ 修复: 将旋转角度变化计入等效距离，使F值能控制旋转速度 ★
        # 1度角度变化 = 1mm等效直线距离
        angular_dist = math.sqrt(da*da + db*db + dc*dc)
        linear_dist = math.sqrt(dx*dx + dy*dy + dz*dz)
        total_6d_dist = math.sqrt(linear_dist*linear_dist + angular_dist*angular_dist)
        
        distance = max(profile.total_distance, total_6d_dist * 0.5)
        # 确保distance不为零
        if distance < CART_FUZZ and total_6d_dist > CART_FUZZ:
            distance = total_6d_dist
        
        if self.debug:
            is_rapid = (segment.type == MoveType.RAPID)
            gcode = "G0" if is_rapid else "G1"
            print(f"  [{gcode}] {self.planner.get_profile_summary(profile)}")
        
        if profile.duration <= 0:
            self._set_position_from_world_point(end)
            self._update_world_position()
            self._notify_update()
            return
        
        start_time = time.time()
        last_update_time = start_time
        update_interval = 0.03  # ~30Hz更新
        
        while True:
            if not self.is_running:
                return
            if self.state_machine.is_estop() or self.state_machine.is_alarm():
                return
            
            while self.is_paused:
                time.sleep(0.05)
                start_time += 0.05
                last_update_time += 0.05
                if not self.is_running:
                    return
            
            elapsed = time.time() - start_time
            if elapsed >= profile.duration:
                break
            
            s = self.planner.get_position_at_time(elapsed, profile)
            ratio = s / distance if distance > CART_FUZZ else 1.0
            ratio = max(0.0, min(1.0, ratio))
            
            # 插值工件坐标
            wx = start.x + dx * ratio
            wy = start.y + dy * ratio
            wz = start.z + dz * ratio
            wa = start.a + da * ratio
            wb = start.b + db * ratio
            wc = start.c + dc * ratio
            
            self._set_workpiece_position(wx, wy, wz, wa, wb, wc)
            
            # 控制更新频率
            now = time.time()
            if now - last_update_time >= update_interval:
                self._notify_update()
                last_update_time = now
            
            time.sleep(0.005)
        
        # 确保到达终点
        self._set_workpiece_position(end.x, end.y, end.z, end.a, end.b, end.c)
        self._notify_update()

    # 在 gpb_GLCanon_arcseg_sim_rtcp_rs274_kins.py 中找到 _set_workpiece_position 方法
    # 将其替换为以下代码：



    def _set_workpiece_position(self, x, y, z, a, b, c):
        """设置当前位置（工件坐标）并计算世界坐标和机器坐标"""
        # 同步偏移信息
        self._sync_offsets_to_status()
        
        # ★ 调试：检查tool_offset是否正确
        if self.debug:
            tlo = getattr(self.status, 'tool_length', 0.0)
            print(f"[DEBUG] _set_workpiece_position: "
                f"工件({x:.1f},{y:.1f},{z:.1f}) TLO={tlo:.1f} "
                f"RTCP={self.rtcp_enabled}")        
        
        
        # ★修复: 直接使用self.rtcp_enabled★
        rtcp_on = self.rtcp_enabled and self._kinematics_instance is not None
        
        # 更新工件坐标 (WCS)
        self.status.position_x = x
        self.status.position_y = y
        self.status.position_z = z
        self.status.position_a = a
        self.status.position_b = b
        self.status.position_c = c
        
        # 获取偏移值
        g5x_x = getattr(self.status, 'g5x_offset_x', 0.0)
        g5x_y = getattr(self.status, 'g5x_offset_y', 0.0)
        g5x_z = getattr(self.status, 'g5x_offset_z', 0.0)
        g5x_a = getattr(self.status, 'g5x_offset_a', 0.0)
        g5x_b = getattr(self.status, 'g5x_offset_b', 0.0)
        g5x_c = getattr(self.status, 'g5x_offset_c', 0.0)
        
        g92_x = getattr(self.status, 'g92_x', 0.0)
        g92_y = getattr(self.status, 'g92_y', 0.0)
        g92_z = getattr(self.status, 'g92_z', 0.0)
        g92_a = getattr(self.status, 'g92_a', 0.0)
        g92_b = getattr(self.status, 'g92_b', 0.0)
        g92_c = getattr(self.status, 'g92_c', 0.0)
        
        tlo = getattr(self.status, 'tool_length', 0.0)
        
        # ★ 关键修复：计算绝对坐标 ★
        # 工件坐标 + G5x + G92 + TLO = 绝对坐标
        abs_x = x + g5x_x + g92_x
        abs_y = y + g5x_y + g92_y
        abs_z = z + g5x_z + g92_z + tlo  # TLO加在Z上
        abs_a = a + g5x_a + g92_a
        abs_b = b + g5x_b + g92_b
        abs_c = c + g5x_c + g92_c
        
        # ★修复: RTCP逆解 - 使用self.rtcp_enabled★
        rtcp_on = self.rtcp_enabled and self._kinematics_instance is not None
        
        if not rtcp_on:
            # 非RTCP：绝对坐标 = 机器坐标
            self.status.machine_x = abs_x
            self.status.machine_y = abs_y
            self.status.machine_z = abs_z
            self.status.machine_a = abs_a
            self.status.machine_b = abs_b
            self.status.machine_c = abs_c
        else:
            # RTCP开启：调用逆运动学
            try:
                # 构造完整的世界坐标元组 (X, Y, Z, A, B, C)
                world_tuple = (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c)
                
                # ★ 调用逆运动学 ★
                joints = self._kinematics_instance.inverse(world_tuple)
                
                if self.debug:
                    print(f"[RTCP INV] world=({abs_x:.2f},{abs_y:.2f},{abs_z:.2f},"
                        f"A={abs_a:.1f},B={abs_b:.1f},C={abs_c:.1f})")
                    print(f"[RTCP INV] joints={[f'{j:.2f}' for j in joints]}")
                
                # 根据运动学类型赋值关节坐标
                config = KINEMATICS_JOINT_CONFIG.get(
                    self._kinematics_type,
                    KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY]
                )
                
                x_idx = config.get('x_idx', 0)
                y_idx = config.get('y_idx', 1)
                z_idx = config.get('z_idx', 2)
                a_idx = config.get('a_idx', 3)
                b_idx = config.get('b_idx', 4)
                c_idx = config.get('c_idx', 5)
                
                # ★ 关键：确保从joints数组中正确取值 ★
                if x_idx >= 0 and x_idx < len(joints):
                    self.status.machine_x = joints[x_idx]
                else:
                    self.status.machine_x = abs_x
                    
                if y_idx >= 0 and y_idx < len(joints):
                    self.status.machine_y = joints[y_idx]
                else:
                    self.status.machine_y = abs_y
                    
                if z_idx >= 0 and z_idx < len(joints):
                    self.status.machine_z = joints[z_idx]
                else:
                    self.status.machine_z = abs_z
                    
                if a_idx >= 0 and a_idx < len(joints):
                    self.status.machine_a = joints[a_idx]
                else:
                    self.status.machine_a = abs_a
                    
                if b_idx >= 0 and b_idx < len(joints):
                    self.status.machine_b = joints[b_idx]
                else:
                    self.status.machine_b = abs_b
                    
                if c_idx >= 0 and c_idx < len(joints):
                    self.status.machine_c = joints[c_idx]
                else:
                    self.status.machine_c = abs_c
                    
            except Exception as e:
                if self.debug:
                    print(f"[RTCP] 逆运动学计算失败: {e}")
                    import traceback
                    traceback.print_exc()
                # 逆解失败时使用绝对坐标作为后备
                self.status.machine_x = abs_x
                self.status.machine_y = abs_y
                self.status.machine_z = abs_z
                self.status.machine_a = abs_a
                self.status.machine_b = abs_b
                self.status.machine_c = abs_c
        
        # 更新世界坐标
        self._update_world_position()



    def _set_position_from_world_point(self, point):
        """从 Point6D 设置当前位置（工件坐标）"""
        if point is None:
            return
        self.status.position_x = point.x
        self.status.position_y = point.y
        self.status.position_z = point.z
        self.status.position_a = point.a
        self.status.position_b = point.b
        self.status.position_c = point.c
        
        

    def _execute_arc_real(self, segment: MoveSegment, profile: MotionProfile):
        """执行圆弧运动（真实速度曲线）- 适配工件坐标"""
        
        # ★修复: 从segment同步RTCP状态★
        if hasattr(segment, 'is_rtcp') and self._kinematics_instance:
            expected_rtcp = segment.is_rtcp
            if self.rtcp_enabled != expected_rtcp:
                self.rtcp_enabled = expected_rtcp
                self.status.rtcp_enabled = expected_rtcp
                self._kinematics_instance.enable_rtcp(expected_rtcp)
        
        # 使用工件坐标
        world_start = segment.get_start_in_world()
        world_end = segment.get_end_in_world()
        
        start = world_start
        end = world_end
        
        center_x = segment.center_x
        center_y = segment.center_y
        center_z = segment.center_z
        radius = segment.radius
        turn = abs(segment.turn)
        
        arc_length = profile.total_distance
        
        dz = end.z - start.z
        da = end.a - start.a
        db = end.b - start.b
        dc = end.c - start.c
        
        is_cw = (segment.type == MoveType.ARC_CW)
        
        if self.debug:
            gcode = "G2" if is_cw else "G3"
            print(f"  [{gcode}] R={radius:.1f}mm 圈数={turn} "
                  f"{self.planner.get_profile_summary(profile)}")
        
        if profile.duration <= 0:
            self._set_position_from_world_point(end)
            self._update_world_position()
            self._notify_update()
            return
        
        # 计算起始角度（工件坐标系）
        start_angle = math.atan2(start.y - center_y, start.x - center_x)
        
        start_time = time.time()
        last_update_time = start_time
        update_interval = 0.03
        
        while True:
            if not self.is_running:
                return
            if self.state_machine.is_estop() or self.state_machine.is_alarm():
                return
            
            while self.is_paused:
                time.sleep(0.05)
                start_time += 0.05
                last_update_time += 0.05
                if not self.is_running:
                    return
            
            elapsed = time.time() - start_time
            if elapsed >= profile.duration:
                break
            
            s = self.planner.get_position_at_time(elapsed, profile)
            angle_ratio = s / arc_length if arc_length > CART_FUZZ else 1.0
            angle_ratio = max(0.0, min(1.0, angle_ratio))
            
            # 计算当前角度
            if is_cw:
                current_angle = start_angle - angle_ratio * turn * 2.0 * math.pi
            else:
                current_angle = start_angle + angle_ratio * turn * 2.0 * math.pi
            
            # 计算圆弧上的工件坐标点
            wx = center_x + radius * math.cos(current_angle)
            wy = center_y + radius * math.sin(current_angle)
            wz = start.z + dz * angle_ratio
            wa = start.a + da * angle_ratio
            wb = start.b + db * angle_ratio
            wc = start.c + dc * angle_ratio
            
            self._set_workpiece_position(wx, wy, wz, wa, wb, wc)
            
            now = time.time()
            if now - last_update_time >= update_interval:
                self._notify_update()
                last_update_time = now
            
            time.sleep(0.005)
        
        # 确保到达终点
        self._set_workpiece_position(end.x, end.y, end.z, end.a, end.b, end.c)
        self._notify_update()



    def _set_position_from_point(self, point):
        """从 Point6D 设置当前位置（保留向后兼容）"""
        if point is None:
            return
        self._set_workpiece_position(point.x, point.y, point.z, point.a, point.b, point.c)
        
        

    def _execute_segment(self, segment: MoveSegment):
        """兼容旧接口的执行方法 - 适配工件坐标"""
        if segment.type in [MoveType.RAPID, MoveType.FEED]:
            world_start = segment.get_start_in_world()
            world_end = segment.get_end_in_world()
            dx = world_end.x - world_start.x
            dy = world_end.y - world_start.y
            dz = world_end.z - world_start.z
            distance = math.sqrt(dx*dx + dy*dy + dz*dz)
            is_rapid = (segment.type == MoveType.RAPID)
            feedrate = segment.feedrate if segment.type == MoveType.FEED else None
            profile = self.planner.plan_motion(distance, feedrate, is_rapid)
        elif segment.type in [MoveType.ARC_CW, MoveType.ARC_CCW]:
            radius = segment.radius
            turn = abs(segment.turn)
            arc_length = turn * 2.0 * math.pi * radius
            profile = self.planner.plan_motion(arc_length, segment.feedrate, False)
        else:
            profile = MotionProfile()
        
        self._execute_segment_real(segment, profile)


    def enable_rtcp(self, enable: bool = True) -> Dict:
        self.status.rtcp_enabled = enable
        self.rtcp_enabled = enable
        if self._kinematics_instance:
            self._kinematics_instance.enable_rtcp(enable)
        self.virtual_hal.set('motion.rtcp-active', 1 if enable else 0)
        self._update_world_position()
        self._notify_update()
        return {"success": True, "message": f"RTCP {'启用' if enable else '禁用'}"}

    def mdi_execute(self, command: str) -> Dict:
        cmd_upper = command.upper().strip()

        if 'G49' in cmd_upper:
            return self.enable_rtcp(False)

        if 'G43.4' in cmd_upper:
            return self.enable_rtcp(True)

        if cmd_upper == 'M429':
            self._kinematics_type = KinematicsType.IDENTITY
            self.status.kinematics_type = "IDENTITY"
            self.rtcp_enabled = False
            self.status.rtcp_enabled = False
            if self._kinematics_instance:
                self._kinematics_instance.enable_rtcp(False)
            self.virtual_hal.set('motion.rtcp-active', 0)
            self._update_world_position()
            self._notify_update()
            return {"success": True, "message": "切换到三轴运动学"}

        if cmd_upper == 'M428':
            self._kinematics_type = self._original_kinematics_type
            self.status.kinematics_type = self._original_kinematics_type.name
            self.rtcp_enabled = True
            self.status.rtcp_enabled = True
            self._update_kinematics_instance()
            if self._kinematics_instance:
                self._kinematics_instance.enable_rtcp(True)
            self.virtual_hal.set('motion.rtcp-active', 1)
            self._update_world_position()
            self._notify_update()
            return {"success": True, "message": f"切换到五轴运动学 ({self._original_kinematics_type.name})"}

        if cmd_upper == 'M430' or cmd_upper.startswith('M430'):
            self._original_kinematics_type = KinematicsType.FIVEAXIS_BC
            self._kinematics_type = KinematicsType.FIVEAXIS_BC
            self.status.kinematics_type = KinematicsType.FIVEAXIS_BC.name
            self._kinematics_instance = KinematicsFactory.create(
                KinematicsType.FIVEAXIS_BC, debug=self.debug
            )
            self.rtcp_enabled = True
            self.status.rtcp_enabled = True
            if self._kinematics_instance:
                self._kinematics_instance.enable_rtcp(True)
                self._kinematics_instance.set_tool_length(self.status.tool_length)
            self.virtual_hal.set('motion.rtcp-active', 1)
            self.virtual_hal.set('motion.switchkins-type', 2)
            self._update_world_position()
            self._notify_update()
            return {"success": True, "message": "切换到用户运动学 (FIVEAXIS_BC)"}

        return {"success": True, "message": f"MDI: {command}"}

    def get_status(self) -> CNCRuntimeStatus:
        return self.status

    def shutdown(self):
        self.state_monitor_running = False
        self.is_running = False
        print(f"✓ CNC内核 [{self.name}] 已关闭")



    def verify_jog_coordinate_chain(self) -> Dict[str, Any]:
        """
        验证Jog操作的坐标变换链正确性
        
        通过正反运动学验证工件坐标和机器坐标的一致性。
        在RTCP启用时特别重要，确保刀尖跟随正确。
        
        Returns:
            dict: 验证结果，包含各部分坐标和一致性检查
        """
        result = {
            'timestamp': time.time(),
            'rtcp_enabled': self.rtcp_enabled,
            'kinematics_type': self.kinematics_type.name if self.kinematics_type else 'NONE',
            'workpiece_position': {},
            'absolute_position': {},
            'machine_position': {},
            'world_position': {},
            'offsets': {},
            'consistency_checks': {},
        }
        
        # 1. 收集偏移信息
        g5x_x = getattr(self.status, 'g5x_offset_x', 0.0)
        g5x_y = getattr(self.status, 'g5x_offset_y', 0.0)
        g5x_z = getattr(self.status, 'g5x_offset_z', 0.0)
        g92_x = getattr(self.status, 'g92_x', 0.0)
        g92_y = getattr(self.status, 'g92_y', 0.0)
        g92_z = getattr(self.status, 'g92_z', 0.0)
        tlo = getattr(self.status, 'tool_length', 0.0)
        
        result['offsets'] = {
            'g5x': {'x': g5x_x, 'y': g5x_y, 'z': g5x_z},
            'g92': {'x': g92_x, 'y': g92_y, 'z': g92_z},
            'tlo': tlo,
        }
        
        # 2. 工件坐标
        result['workpiece_position'] = {
            'x': self.status.position_x,
            'y': self.status.position_y,
            'z': self.status.position_z,
            'a': self.status.position_a,
            'c': self.status.position_c,
        }
        
        # 3. 计算机器坐标
        result['machine_position'] = {
            'x': self.status.machine_x,
            'y': self.status.machine_y,
            'z': self.status.machine_z,
            'a': self.status.machine_a,
            'c': self.status.machine_c,
        }
        
        # 4. 计算绝对坐标
        abs_x = self.status.position_x + g5x_x + g92_x
        abs_y = self.status.position_y + g5x_y + g92_y
        abs_z = self.status.position_z + g5x_z + g92_z + tlo
        abs_a = self.status.position_a
        abs_c = self.status.position_c
        
        result['absolute_position'] = {
            'x': abs_x, 'y': abs_y, 'z': abs_z,
            'a': abs_a, 'c': abs_c,
        }
        
        # 5. RTCP一致性验证
        if self.rtcp_enabled and self._kinematics_instance:
            try:
                # 正向验证：机器坐标 → 正运动学 → 应等于绝对坐标
                joints = [
                    self.status.machine_x, self.status.machine_y,
                    self.status.machine_z,
                    self.status.machine_a, self.status.machine_b,
                    self.status.machine_c
                ]
                world_fwd = self._kinematics_instance.joints_to_world(joints)
                
                result['world_position'] = {
                    'x': world_fwd[0], 'y': world_fwd[1], 'z': world_fwd[2],
                    'a': world_fwd[3], 'c': world_fwd[5] if len(world_fwd) > 5 else 0,
                }
                
                # 检查正运动学结果是否接近绝对坐标
                tolerance = 0.5  # mm，允许0.5mm误差
                fwd_consistent = True
                if abs(world_fwd[0] - abs_x) > tolerance:
                    fwd_consistent = False
                if abs(world_fwd[1] - abs_y) > tolerance:
                    fwd_consistent = False
                if abs(world_fwd[2] - abs_z) > tolerance:
                    fwd_consistent = False
                
                result['consistency_checks']['forward_kinematics'] = {
                    'consistent': fwd_consistent,
                    'tolerance': tolerance,
                    'error_x': abs(world_fwd[0] - abs_x),
                    'error_y': abs(world_fwd[1] - abs_y),
                    'error_z': abs(world_fwd[2] - abs_z),
                }
                
                # 反向验证：绝对坐标 → 逆运动学 → 应等于机器坐标
                joints_inv = self._kinematics_instance.world_to_joints(
                    (abs_x, abs_y, abs_z, abs_a, 0.0, abs_c)
                )
                
                inv_consistent = True
                if len(joints_inv) > 0:
                    if abs(joints_inv[0] - self.status.machine_x) > tolerance:
                        inv_consistent = False
                if len(joints_inv) > 1:
                    if abs(joints_inv[1] - self.status.machine_y) > tolerance:
                        inv_consistent = False
                if len(joints_inv) > 2:
                    if abs(joints_inv[2] - self.status.machine_z) > tolerance:
                        inv_consistent = False
                
                result['consistency_checks']['inverse_kinematics'] = {
                    'consistent': inv_consistent,
                    'tolerance': tolerance,
                    'joints_calculated': joints_inv[:5] if len(joints_inv) >= 5 else joints_inv,
                }
                
                # 总体一致性
                result['consistency_checks']['overall'] = {
                    'consistent': fwd_consistent and inv_consistent,
                    'message': '坐标链一致 ✓' if (fwd_consistent and inv_consistent) 
                            else '坐标链不一致 ✗ - 请检查运动学参数',
                }
                
            except Exception as e:
                result['consistency_checks']['error'] = str(e)
        else:
            # RTCP禁用时，机器坐标应等于绝对坐标
            machine_eq_absolute = (
                abs(self.status.machine_x - abs_x) < 0.001 and
                abs(self.status.machine_y - abs_y) < 0.001 and
                abs(self.status.machine_z - abs_z) < 0.001
            )
            result['consistency_checks']['machine_equals_absolute'] = machine_eq_absolute
        
        return result



# ==================== 完善后的 GLCanonPathCollectorWithRTCP 类 ====================

class GLCanonPathCollectorWithRTCP(GLCanonPure):
    """增强版路径收集器 - 完整五轴RTCP + 所有LinuxCNC状态管理"""

    def __init__(self, colors=None, geometry=None, is_foam=0, max_points=50000,
                acceleration: float = 500.0,
                max_rapid_rate: float = 10000.0,
                max_feed_rate: float = 5000.0,
                kinematics_type: KinematicsType = KinematicsType.TRT_BC,
                kinematics_params: Dict[str, Any] = None,
                debug: bool = False):
        
        if colors is None:
            colors = DEFAULT_COLORS.copy()
        if geometry is None:
            geometry = "XYZ"
        
        super().__init__(colors, geometry, is_foam)
        
        self.max_points = max_points
        self.debug = debug
        self._point_count = 0
        
        self.data = ToolpathData()
        self.data.filename = ""
        self.data.rtcp_enabled = False
        self.data.kinematics_type = kinematics_type.name
        
        self.rtcp_enabled = False
        self.kinematics_type = kinematics_type
        
        self.kinematics: Optional[BaseKinematics] = KinematicsFactory.create(
            kinematics_type, debug=debug, **(kinematics_params or {})
        )
        
        # ===== 添加这一行 =====
        self._saved_kinematics_type = None  # 用于 M428/M429 切换时保存原始类型
        
        self.planner = TrapezoidalMotionPlanner(
            acceleration=acceleration,
            max_rapid_rate=max_rapid_rate,
            max_feed_rate=max_feed_rate
        )
        
        # 当前位置 - 工件坐标系
        self.current_pos = Point6D(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        
        # 机床坐标
        self.machine_x = 0.0
        self.machine_y = 0.0
        self.machine_z = 0.0
        self.machine_a = 0.0
        self.machine_b = 0.0
        self.machine_c = 0.0
        
        # 工件偏移 (G5x)
        self.work_offsets: Dict[int, Dict[str, float]] = {}
        for i in range(1, 10):
            self.work_offsets[i] = {'X': 0.0, 'Y': 0.0, 'Z': 0.0,
                                     'A': 0.0, 'B': 0.0, 'C': 0.0,
                                     'U': 0.0, 'V': 0.0, 'W': 0.0, 'R': 0.0}
        self.current_work_offset = 1  # G54
        self.g5x_index = 1
        
        # G92偏移
        self.g92_offset_x = 0.0
        self.g92_offset_y = 0.0
        self.g92_offset_z = 0.0
        self.g92_offset_a = 0.0
        self.g92_offset_b = 0.0
        self.g92_offset_c = 0.0
        self.g92_offset_u = 0.0
        self.g92_offset_v = 0.0
        self.g92_offset_w = 0.0
        self.g92_active = False
        
        # G52偏移
        self.g52_offset_x = 0.0
        self.g52_offset_y = 0.0
        self.g52_offset_z = 0.0
        self.g52_offset_a = 0.0
        self.g52_offset_b = 0.0
        self.g52_offset_c = 0.0
        self.g52_offset_u = 0.0
        self.g52_offset_v = 0.0
        self.g52_offset_w = 0.0
        
        # XY旋转
        self.rotation_xy = 0.0
        self.rotation_sin = 0.0
        self.rotation_cos = 1.0
        
        # G53模式
        self._g53_active = False
        
        # 工件偏移（额外的work offset）
        self.work_offset_x = 0.0
        self.work_offset_y = 0.0
        self.work_offset_z = 0.0
        self.work_offset_a = 0.0
        self.work_offset_b = 0.0
        self.work_offset_c = 0.0
        
        # 刀具数据
        self.tool_lengths: Dict[int, float] = {}
        self.tool_radii: Dict[int, float] = {}
        self.tool_table: Dict[int, ToolData] = {}
        self.current_tool: Optional[ToolData] = None
        self.current_h_code = 0
        self.current_d_code = 0
        self.selected_tool = 0
        self.selected_pocket = 0
        self.current_pocket = 0
        
        # 刀具补偿
        self.comp_type = CompType.OFF
        self.comp_radius = 0.0
        self.comp_side = 0
        self.d_word = 0.0
        self.cutter_comp_active = False
        self.cutter_comp_firstmove = True
        self.cutter_comp_orientation = 0
        self.arc_not_allowed = True
        self.program_x = 0.0
        self.program_y = 0.0
        self.program_z = 0.0
        
        # 刀具长度补偿
        self.tool_offset = Point6D(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
        self.g43_with_zero_offset = False
        
        # 模态状态
        self.is_absolute = True
        self.current_plane = 17
        self.feedrate = 1000.0
        self.current_line = 0
        self.cumulative_time = 0.0
        self.motion_mode = -1
        self.retract_mode = 98
        self.ijk_distance_mode = 91
        self.lathe_diameter_mode = False
        self.return_value = 0.0
        self.value_returned = False
        self.remap_level = 0
        self.call_level = 0
        
        # 状态
        self.state = type('State', (), {})()
        self.state.plane = 17
        self.state.distance_mode = 90
        self.state.feedrate = 1000.0
        self.state.spindle_mode = 0
        self.state.spindle_speed = 0.0
        self.state.spindle_state = "OFF"
        self.state.coolant_mode = 9
        self.state.coolant_flood = False
        self.state.coolant_mist = False
        self.state.tool = 0
        self.state.units = 21
        self.state.feed_mode = 94
        
        self.optional_stop = False
        self.block_delete = False
        
        # 固定循环
        self.canned_cycle_active = False
        self.canned_cycle_type = 0
        self.canned_cycle_r = 0.0
        self.canned_cycle_z = 0.0
        self.canned_cycle_q = 0.0
        self.canned_cycle_l = 1
        self.canned_cycle_p = 0.0
        self.canned_cycle_il = 0.0
        self.canned_cycle_il_flag = False
        
        
        # ===== 添加这一行 =====
        self._saved_kinematics_type = None  # 用于 M428/M429 切换时保存原始类型
        
        # ===== 添加旋转轴保存变量 =====
        self._saved_rotation_a = 0.0
        self._saved_rotation_b = 0.0
        self._saved_rotation_c = 0.0        
        
        
        # 参考点
        self.g28_ref: Dict[str, float] = {axis: 0.0 for axis in 'XYZABC'}
        self.g30_ref: Dict[str, float] = {axis: 0.0 for axis in 'XYZABC'}
        
        self.state_machine: Optional[MachineStateMachine] = None
        self.call_counts: Dict[str, int] = {}
        self.saved_states: List[Dict] = []
        
        self._sync_machine_from_current()
        
        # 参数表系统
        self.var_manager = LinuxCNCParameterTable(debug=debug)
        self.var_manager._current_line = self.current_line
        
        self.control_handler = ControlStructureHandler(self.var_manager, debug=debug)
        self._sync_parameters_from_state()
        
        self._gcode_buffer: List[str] = []
        self._parse_errors: List[str] = []
        
        # ===== 新增：坐标系管理 =====
        self._coord_mode = 'WORLD'  # 当前坐标模式: 'WORLD' | 'ABSOLUTE' | 'MACHINE'
        self._last_segment_was_arc = False
        
        # 初始化参数表中的当前位置
        self._init_params_position()        
        
        if self.debug:
            print(f"GLCanonPathCollectorWithRTCP 初始化完成")
            print(f"  运动学类型: {kinematics_type.name}")
            print(f"  RTCP状态: {'启用' if self.rtcp_enabled else '禁用'}")

    def _init_params_position(self):
        """初始化参数表中的当前位置为 (0,0,0,0,0,0)"""
        self.var_manager._current_position = {
            'X': self.current_pos.x,
            'Y': self.current_pos.y,
            'Z': self.current_pos.z,
            'A': self.current_pos.a,
            'B': self.current_pos.b,
            'C': self.current_pos.c,
        }


    def _sync_position_to_params(self):
        """同步当前位置到参数表（5420-5428 相对位置参数）"""
        # 工件坐标
        self.var_manager._current_position['X'] = self.current_pos.x
        self.var_manager._current_position['Y'] = self.current_pos.y
        self.var_manager._current_position['Z'] = self.current_pos.z
        self.var_manager._current_position['A'] = self.current_pos.a
        self.var_manager._current_position['B'] = self.current_pos.b
        self.var_manager._current_position['C'] = self.current_pos.c
        
        # 同步到参数 5420-5428
        self.var_manager._params[5420] = self.current_pos.x
        self.var_manager._params[5421] = self.current_pos.y
        self.var_manager._params[5422] = self.current_pos.z
        self.var_manager._params[5423] = self.current_pos.a
        self.var_manager._params[5424] = self.current_pos.b
        self.var_manager._params[5425] = self.current_pos.c



    def _compute_machine_coords(self, wx: float, wy: float, wz: float,
                                wa: float, wb: float, wc: float,
                                wu: float = 0.0, wv: float = 0.0, ww: float = 0.0
                                ) -> Tuple[float, float, float, float, float, float, float, float, float]:
        """
        计算机器坐标（关节坐标）
        
        参照 LinuxCNC 源码的坐标转换链：
        rs274ngc.cpp: find_current_in_system() + kinematics.cpp: kinematicsInverse()
        
        转换步骤（与 _sync_machine_from_current 保持一致）：
        1. 工件坐标 + G92偏移 + XY旋转 + G5x偏移 + TLO + G52偏移 = 绝对坐标
        2. 如果 RTCP 启用，调用逆运动学计算关节坐标
        3. 如果 RTCP 禁用，直接返回绝对坐标
        
        对应 C++ 中：
        - settings->current_x → 工件坐标
        - settings->tool_offset.tran.z → TLO
        - motion controller 调用 kinematicsInverse() → 关节坐标
        """
        # ===== 第1步：加上 G92 偏移 (axis_offset) =====
        # 对应 C++: *x += s->axis_offset_x
        abs_x = wx + self.g92_offset_x
        abs_y = wy + self.g92_offset_y
        abs_z = wz + self.g92_offset_z
        abs_a = wa + self.g92_offset_a
        abs_b = wb + self.g92_offset_b
        abs_c = wc + self.g92_offset_c
        abs_u = wu + self.g92_offset_u
        abs_v = wv + self.g92_offset_v
        abs_w = ww + self.g92_offset_w
        
        # ===== 第2步：XY 旋转 =====
        # 对应 C++: rotate(x, y, s->rotation_xy)
        if abs(self.rotation_xy) > 1e-9:
            rot = math.radians(self.rotation_xy)
            cos_r = math.cos(rot)
            sin_r = math.sin(rot)
            rx = abs_x * cos_r - abs_y * sin_r
            ry = abs_x * sin_r + abs_y * cos_r
            abs_x, abs_y = rx, ry
        
        # ===== 第3步：加上 G5x 偏移 (origin_offset) =====
        # 对应 C++: *x += s->origin_offset_x
        abs_x += self.g5x_offset_x
        abs_y += self.g5x_offset_y
        abs_z += self.g5x_offset_z
        abs_a += self.g5x_offset_a
        abs_b += self.g5x_offset_b
        abs_c += self.g5x_offset_c
        abs_u += self.g5x_offset_u
        abs_v += self.g5x_offset_v
        abs_w += self.g5x_offset_w
        
        # ===== 第4步：加上刀具长度补偿 (tool_offset) =====
        # 对应 C++: *x += s->tool_offset.tran.x
        # 这是 TLO，在 convert_tool_length_offset 中设置
        abs_x += self.tool_offset.x
        abs_y += self.tool_offset.y
        abs_z += self.tool_offset.z
        abs_a += self.tool_offset.a
        abs_b += self.tool_offset.b
        abs_c += self.tool_offset.c
        abs_u += self.tool_offset.u
        abs_v += self.tool_offset.v
        abs_w += self.tool_offset.w
        
        # ===== 第5步：加上 G52 偏移 =====
        abs_x += self.g52_offset_x
        abs_y += self.g52_offset_y
        abs_z += self.g52_offset_z
        abs_a += self.g52_offset_a
        abs_b += self.g52_offset_b
        abs_c += self.g52_offset_c
        
        # ===== 第6步：RTCP 逆运动学 =====
        # 对应 C++: motion controller 调用 kinematicsInverse()
        # 传入的是包含 TLO 的绝对坐标
        if self.rtcp_enabled and self.kinematics is not None:
            try:
                # 将绝对坐标传给运动学逆解
                world_tuple = (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c)
                joints = self.kinematics.world_to_joints(world_tuple)
                
                # 使用关节映射函数转换为标准9轴格式
                config = KINEMATICS_JOINT_CONFIG.get(
                    self.kinematics_type,
                    KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY]
                )
                
                result = [0.0] * 9
                for i in range(min(len(joints), 9)):
                    result[i] = joints[i]
                
                # 根据运动学类型映射关节
                x_idx = config.get('x_idx', 0)
                y_idx = config.get('y_idx', 1)
                z_idx = config.get('z_idx', 2)
                a_idx = config.get('a_idx', 3)
                b_idx = config.get('b_idx', 4)
                c_idx = config.get('c_idx', 5)
                
                return (
                    joints[x_idx] if (x_idx >= 0 and x_idx < len(joints)) else abs_x,
                    joints[y_idx] if (y_idx >= 0 and y_idx < len(joints)) else abs_y,
                    joints[z_idx] if (z_idx >= 0 and z_idx < len(joints)) else abs_z,
                    joints[a_idx] if (a_idx >= 0 and a_idx < len(joints)) else abs_a,
                    joints[b_idx] if (b_idx >= 0 and b_idx < len(joints)) else abs_b,
                    joints[c_idx] if (c_idx >= 0 and c_idx < len(joints)) else abs_c,
                    result[6], result[7], result[8]
                )
            except Exception as e:
                if self.debug:
                    print(f"[MCS] 逆运动学计算失败: {e}")
                    traceback.print_exc()
                # 逆解失败时返回绝对坐标作为后备
                return (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c, abs_u, abs_v, abs_w)
        else:
            # RTCP 禁用：直接返回绝对坐标
            return (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c, abs_u, abs_v, abs_w)


    def _add_world_segment(self, move_type: MoveType, end_world: Point6D, **kwargs):
        """
        统一的运动段添加方法 - 基于参数表进行坐标管理
        
        所有运动段（直线、圆弧、暂停等）都通过此方法创建。
        
        核心原则（参照 LinuxCNC 源码设计）：
        - segment.start/end 使用工件坐标系 (G代码编程坐标)
        - segment.world_start/world_end 是工件坐标的副本
        - segment.machine_start/machine_end 通过逆运动学计算关节坐标
        - 几何验证在工件坐标系下进行
        
        参数:
            move_type: 运动类型 (RAPID, FEED, DWELL, PROGRAM_END等)
            end_world: 工件坐标系下的目标位置
            **kwargs: 额外参数
                - center_x, center_y, center_z: 圆弧圆心（工件坐标）
                - radius: 圆弧半径
                - turn: 圆弧圈数
                - dwell_time: 暂停时间
                - gcode: G代码字符串
        """
        if self._point_count >= self.max_points:
            return
        
        # 1. 工件坐标系下的起点
        start_world = self.current_pos.copy()
        
        # 2. 计算直线距离
        if move_type in [MoveType.RAPID, MoveType.FEED]:
            linear_distance = start_world.distance_to(end_world)
        else:
            linear_distance = 0.0
        
        # 3. 规划速度曲线
        if move_type == MoveType.RAPID:
            result = self.planner.calculate_time(linear_distance, None, is_rapid=True)
        elif move_type == MoveType.FEED:
            result = self.planner.calculate_time(linear_distance, self.feedrate, is_rapid=False)
        elif move_type == MoveType.DWELL:
            dwell_time = kwargs.get('dwell_time', 0)
            result = {
                'duration': dwell_time,
                'max_velocity': 0.0,
                'acceleration_time': 0.0,
                'constant_time': 0.0,
                'deceleration_time': 0.0,
                'profile_type': 'dwell'
            }
        elif move_type == MoveType.PROGRAM_END:
            dwell_time = kwargs.get('dwell_time', 0.1)
            result = {
                'duration': dwell_time,
                'max_velocity': 0.0,
                'acceleration_time': 0.0,
                'constant_time': 0.0,
                'deceleration_time': 0.0,
                'profile_type': 'program_end'
            }
        else:
            result = {
                'duration': 0.1,
                'max_velocity': 0.0,
                'acceleration_time': 0.0,
                'constant_time': 0.0,
                'deceleration_time': 0.0,
                'profile_type': 'other'
            }
        
        # 4. 计算机器坐标（包含逆运动学）
        # ★ 修改：使用 _compute_machine_coords 替代 program_to_absolute
        # 参照 LinuxCNC: kinematicsInverse() 接收绝对坐标
        abs_start_tuple = self._compute_machine_coords(
            start_world.x, start_world.y, start_world.z,
            start_world.a, start_world.b, start_world.c,
            getattr(start_world, 'u', 0.0),
            getattr(start_world, 'v', 0.0),
            getattr(start_world, 'w', 0.0)
        )
        abs_end_tuple = self._compute_machine_coords(
            end_world.x, end_world.y, end_world.z,
            end_world.a, end_world.b, end_world.c,
            getattr(end_world, 'u', 0.0),
            getattr(end_world, 'v', 0.0),
            getattr(end_world, 'w', 0.0)
        )
        
        abs_start_pt = Point6D(
            abs_start_tuple[0], abs_start_tuple[1], abs_start_tuple[2],
            abs_start_tuple[3], abs_start_tuple[4], abs_start_tuple[5]
        )
        abs_end_pt = Point6D(
            abs_end_tuple[0], abs_end_tuple[1], abs_end_tuple[2],
            abs_end_tuple[3], abs_end_tuple[4], abs_end_tuple[5]
        )
        
        # 5. 创建 MoveSegment
        segment = MoveSegment(
            type=move_type,
            start=start_world.copy(),       # 工件坐标
            end=end_world.copy(),           # 工件坐标
            line_number=self.current_line,
            feedrate=self.feedrate if move_type == MoveType.FEED else 0.0,
            comp_type=self.comp_type,
            comp_radius=self.comp_radius,
            d_word=self.d_word,
            duration=result['duration'],
            max_velocity=result['max_velocity'],
            acceleration_time=result['acceleration_time'],
            constant_time=result['constant_time'],
            deceleration_time=result['deceleration_time'],
            profile_type=result['profile_type'],
            is_rtcp=self.rtcp_enabled,
            world_start=start_world.copy(),
            world_end=end_world.copy(),
            machine_start=abs_start_pt,     # ★ 机器坐标（已通过逆解）
            machine_end=abs_end_pt,         # ★ 机器坐标（已通过逆解）
            tool_number=self.state.tool,
            spindle_speed=self.state.spindle_speed,
            spindle_state=self.state.spindle_state,
            coord_system='WORLD',
            active_csys=self.var_manager.active_csys,
            g92_active=self.var_manager._g92_active,
            **{k: v for k, v in kwargs.items()
            if k in ['center_x', 'center_y', 'center_z', 'radius',
                        'turn', 'dwell_time', 'gcode']}
        )
        
        # 6. 添加到路径数据
        self.data.segments.append(segment)
        
        if move_type in [MoveType.RAPID, MoveType.FEED]:
            self.data.total_length += linear_distance
        
        self.cumulative_time += result['duration']
        self._point_count += 1
        
        # 7. 更新工件坐标位置
        self.current_pos = end_world.copy()
        
        # 8. 同步到参数表
        self._sync_position_to_params()
        
        # 9. 同步机器坐标
        self._sync_machine_from_current()
        
        # 10. 同步所有参数
        self._sync_parameters_from_state()
        
        # 11. 标记运动段类型
        self._last_segment_was_arc = move_type in [MoveType.ARC_CW, MoveType.ARC_CCW]


    def find_or_create_tool(self, tool_number: int) -> int:
        """
        查找或创建刀具，返回刀具索引
        对应 C++ 中的 find_tool_index -> tooldata_find_index_for_tool
        """
        # 先在现有刀具表中查找
        for idx, tool in self.tool_table.items():
            if tool.tool_number == tool_number:
                return idx
        
        # 创建新刀具
        # 查找一个可用的 pocket
        new_pocket = tool_number
        if new_pocket in self.tool_table:
            # 找到一个未使用的 pocket
            for p in range(1, 1000):
                if p not in self.tool_table:
                    new_pocket = p
                    break
        
        new_tool = ToolData(
            tool_number=tool_number,
            pocket=new_pocket,
            diameter=0.0,
            radius=0.0,
            front_angle=0.0,
            back_angle=0.0,
            orientation=0,
            name=f"T{tool_number}"
        )
        self.tool_table[new_pocket] = new_tool
        
        # 初始化刀具长度和半径映射
        if tool_number not in self.tool_lengths:
            self.tool_lengths[tool_number] = 0.0
        if tool_number not in self.tool_radii:
            self.tool_radii[tool_number] = 0.0
        
        if self.debug:
            print(f"[TOOL] 创建新刀具 T{tool_number} in pocket {new_pocket}")
        
        return new_pocket

    def _get_csys_rotation(self, csys_num: int) -> float:
        """
        获取指定坐标系的 XY 旋转角度
        对应 C++ 中 parameters[5210 + (origin * 20)]
        """
        if csys_num in self.work_offsets:
            return self.work_offsets[csys_num].get('R', 0.0)
        return 0.0

    def _get_current_in_system_without_tlo(self, system: int):
        """
        计算当前在指定坐标系中的位置（不带刀具长度补偿）
        
        完全参照 LinuxCNC 官方实现：
        rs274ngc.cpp: Interp::find_current_in_system_without_tlo
        
        变换顺序：
        1. 从工件坐标开始
        2. 加上 G92 偏移 (axis_offset)
        3. 加上 G52 偏移
        4. XY 旋转（正向, rotation_xy）
        5. 加上 G5x 偏移 (origin_offset)
        6. 加上刀具长度补偿 (tool_offset)
        7. 减去目标坐标系的 G5x 偏移
        8. XY 旋转（反向, 目标坐标系的 rotation_xy）
        9. 如果 G92 激活且目标是夹具坐标系(9=G59.3)，减去 G92 偏移
        
        用于 G10 L10/L11 计算刀具偏移
        
        Args:
            system: 坐标系编号 (1-9, 对应 G54-G59.3)
        
        Returns:
            tuple: (x, y, z, a, b, c, u, v, w) 在目标坐标系中的坐标（不含TLO）
        """
        # ===== 第1步：从工件坐标开始 =====
        x = self.current_pos.x
        y = self.current_pos.y
        z = self.current_pos.z
        a = self.current_pos.a
        b = self.current_pos.b
        c = self.current_pos.c
        u = getattr(self.current_pos, 'u', 0.0)
        v = getattr(self.current_pos, 'v', 0.0)
        w = getattr(self.current_pos, 'w', 0.0)
        
        # ===== 第2步：加上 G92 偏移 (axis_offset) =====
        x += self.g92_offset_x
        y += self.g92_offset_y
        z += self.g92_offset_z
        a += self.g92_offset_a
        b += self.g92_offset_b
        c += self.g92_offset_c
        u += self.g92_offset_u
        v += self.g92_offset_v
        w += self.g92_offset_w
        
        # ===== 第3步：加上 G52 偏移 =====
        x += self.g52_offset_x
        y += self.g52_offset_y
        z += self.g52_offset_z
        a += self.g52_offset_a
        b += self.g52_offset_b
        c += self.g52_offset_c
        u += self.g52_offset_u
        v += self.g52_offset_v
        w += self.g52_offset_w
        
        # ===== 第4步：XY 旋转（正向, rotation_xy）=====
        if self.rotation_xy != 0:
            rot_rad = math.radians(self.rotation_xy)
            cos_r = math.cos(rot_rad)
            sin_r = math.sin(rot_rad)
            
            rot_x = x * cos_r - y * sin_r
            rot_y = x * sin_r + y * cos_r
            x, y = rot_x, rot_y
        
        # ===== 第5步：加上 G5x 偏移 (origin_offset) =====
        x += self.g5x_offset_x
        y += self.g5x_offset_y
        z += self.g5x_offset_z
        a += self.g5x_offset_a
        b += self.g5x_offset_b
        c += self.g5x_offset_c
        u += self.g5x_offset_u
        v += self.g5x_offset_v
        w += self.g5x_offset_w
        
        # ===== 第6步：加上刀具长度补偿 (tool_offset) ★ 修复：新增步骤 =====
        x += self.tool_offset.x
        y += self.tool_offset.y
        z += self.tool_offset.z
        a += self.tool_offset.a
        b += self.tool_offset.b
        c += self.tool_offset.c
        u += self.tool_offset.u
        v += self.tool_offset.v
        w += self.tool_offset.w
        
        # ===== 第7步：减去目标坐标系的 G5x 偏移 =====
        target_offsets = self.work_offsets.get(system, {})
        
        x -= target_offsets.get('X', 0.0)
        y -= target_offsets.get('Y', 0.0)
        z -= target_offsets.get('Z', 0.0)
        a -= target_offsets.get('A', 0.0)
        b -= target_offsets.get('B', 0.0)
        c -= target_offsets.get('C', 0.0)
        u -= target_offsets.get('U', 0.0)
        v -= target_offsets.get('V', 0.0)
        w -= target_offsets.get('W', 0.0)
        
        # ===== 第8步：XY 旋转（反向, 目标坐标系的 rotation_xy）=====
        target_rotation = target_offsets.get('R', 0.0)
        if target_rotation != 0:
            rot_rad = math.radians(-target_rotation)
            cos_r = math.cos(rot_rad)
            sin_r = math.sin(rot_rad)
            
            rot_x = x * cos_r - y * sin_r
            rot_y = x * sin_r + y * cos_r
            x, y = rot_x, rot_y
        
        # ===== 第9步：如果 G92 激活，减去 G92 偏移 =====
        if self.g92_active:
            x -= self.g92_offset_x
            y -= self.g92_offset_y
            z -= self.g92_offset_z
            a -= self.g92_offset_a
            b -= self.g92_offset_b
            c -= self.g92_offset_c
            u -= self.g92_offset_u
            v -= self.g92_offset_v
            w -= self.g92_offset_w
        
        if self.debug:
            print(f"[CSYS] _get_current_in_system_without_tlo(system={system}):")
            print(f"       工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})")
            print(f"       目标坐标(不含TLO): ({x:.3f}, {y:.3f}, {z:.3f})")
        
        return (x, y, z, a, b, c, u, v, w)


    def _get_current_in_system(self, system: int) -> Tuple[float, float, float, float, float, float, float, float, float]:
        """
        计算当前在指定坐标系中的位置（带刀具长度补偿）
        
        完全参照 LinuxCNC 官方实现：
        rs274ngc.cpp: Interp::find_current_in_system
        
        变换顺序：
        1. 从工件坐标开始
        2. 加上 G92 偏移 (axis_offset)
        3. 加上 G52 偏移
        4. XY 旋转（正向, rotation_xy）
        5. 加上 G5x 偏移 (origin_offset)
        6. 加上刀具长度补偿 (tool_offset)
        7. 减去目标坐标系的 G5x 偏移
        8. XY 旋转（反向, 目标坐标系的 rotation_xy）
        9. 如果 G92 激活，减去 G92 偏移
        
        与 _get_current_in_system_without_tlo 的区别：
        - 此方法包含刀具长度补偿
        - 用于 G10 L20 和坐标系切换 (G54-G59)
        
        Args:
            system: 坐标系编号 (1-9, 对应 G54-G59.3)
        
        Returns:
            tuple: (x, y, z, a, b, c, u, v, w) 在目标坐标系中的坐标
        """
        # ===== 第1步：从工件坐标开始 =====
        x = self.current_pos.x
        y = self.current_pos.y
        z = self.current_pos.z
        a = self.current_pos.a
        b = self.current_pos.b
        c = self.current_pos.c
        u = getattr(self.current_pos, 'u', 0.0)
        v = getattr(self.current_pos, 'v', 0.0)
        w = getattr(self.current_pos, 'w', 0.0)
        
        # ===== 第2步：加上 G92 偏移 (axis_offset) =====
        # 对应官方: x += s->axis_offset_x; ...
        x += self.g92_offset_x
        y += self.g92_offset_y
        z += self.g92_offset_z
        a += self.g92_offset_a
        b += self.g92_offset_b
        c += self.g92_offset_c
        u += self.g92_offset_u
        v += self.g92_offset_v
        w += self.g92_offset_w
        
        # ===== 第3步：加上 G52 偏移 =====
        x += self.g52_offset_x
        y += self.g52_offset_y
        z += self.g52_offset_z
        a += self.g52_offset_a
        b += self.g52_offset_b
        c += self.g52_offset_c
        u += self.g52_offset_u
        v += self.g52_offset_v
        w += self.g52_offset_w
        
        # ===== 第4步：XY 旋转（正向, rotation_xy）=====
        # 对应官方: rotate(x, y, s->rotation_xy);
        if self.rotation_xy != 0:
            rot_rad = math.radians(self.rotation_xy)
            cos_r = math.cos(rot_rad)
            sin_r = math.sin(rot_rad)
            
            # rotate(&x, &y, theta):
            #   xx = *x * cos(t) - *y * sin(t)
            #   yy = *x * sin(t) + *y * cos(t)
            rot_x = x * cos_r - y * sin_r
            rot_y = x * sin_r + y * cos_r
            x, y = rot_x, rot_y
        
        # ===== 第5步：加上 G5x 偏移 (origin_offset) =====
        # 对应官方: x += s->origin_offset_x; ...
        x += self.g5x_offset_x
        y += self.g5x_offset_y
        z += self.g5x_offset_z
        a += self.g5x_offset_a
        b += self.g5x_offset_b
        c += self.g5x_offset_c
        u += self.g5x_offset_u
        v += self.g5x_offset_v
        w += self.g5x_offset_w
        
        # ===== 第6步：加上刀具长度补偿 (tool_offset) =====
        # 对应官方: x += s->tool_offset.tran.x; ...
        x += self.tool_offset.x
        y += self.tool_offset.y
        z += self.tool_offset.z
        a += self.tool_offset.a
        b += self.tool_offset.b
        c += self.tool_offset.c
        u += self.tool_offset.u
        v += self.tool_offset.v
        w += self.tool_offset.w
        
        # ===== 第7步：减去目标坐标系的 G5x 偏移 =====
        # 对应官方:
        #   *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]);
        #   *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]);
        target_offsets = self.work_offsets.get(system, {})
        
        x -= target_offsets.get('X', 0.0)
        y -= target_offsets.get('Y', 0.0)
        z -= target_offsets.get('Z', 0.0)
        a -= target_offsets.get('A', 0.0)
        b -= target_offsets.get('B', 0.0)
        c -= target_offsets.get('C', 0.0)
        u -= target_offsets.get('U', 0.0)
        v -= target_offsets.get('V', 0.0)
        w -= target_offsets.get('W', 0.0)
        
        # ===== 第8步：XY 旋转（反向, 目标坐标系的 rotation_xy）=====
        # 对应官方: rotate(x, y, -p[5210 + system * 20]);
        target_rotation = target_offsets.get('R', 0.0)
        if target_rotation != 0:
            rot_rad = math.radians(-target_rotation)
            cos_r = math.cos(rot_rad)
            sin_r = math.sin(rot_rad)
            
            rot_x = x * cos_r - y * sin_r
            rot_y = x * sin_r + y * cos_r
            x, y = rot_x, rot_y
        
        # ===== 第9步：如果 G92 激活，减去 G92 偏移 =====
        # 对应官方:
        #   if (p[5210]) {
        #       *x -= USER_TO_PROGRAM_LEN(p[5211]);
        #       ...
        #   }
        if self.g92_active:
            x -= self.g92_offset_x
            y -= self.g92_offset_y
            z -= self.g92_offset_z
            a -= self.g92_offset_a
            b -= self.g92_offset_b
            c -= self.g92_offset_c
            u -= self.g92_offset_u
            v -= self.g92_offset_v
            w -= self.g92_offset_w
        
        if self.debug:
            print(f"[CSYS] _get_current_in_system(system={system}):")
            print(f"       工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})")
            print(f"       目标坐标: ({x:.3f}, {y:.3f}, {z:.3f})")
        
        return (x, y, z, a, b, c, u, v, w)


    def _sync_tool_parameters_to_table(self, tool_idx: int):
        """
        同步刀具参数到参数表（5400-5413）和 HAL
        
        对应 C++ 中 convert_setup_tool 中的：
        - set_tool_parameters()
        - SET_TOOL_TABLE_ENTRY()
        """
        if tool_idx < 0 or tool_idx not in self.tool_table:
            return
        
        tool = self.tool_table[tool_idx]
        
        # ===== 同步到参数表 (#5400-#5413) =====
        # 对应 C++ 中 setup::set_tool_parameters()
        
        # 5400: 刀具号
        # 随机换刀: 5400 = tool_table[0].toolno (如果 >= 0) 否则 -1
        # 非随机换刀: 5400 = tool_table[0].toolno (如果 > 0) 否则 0
        if self.current_tool and self.current_tool.tool_number == tool.tool_number:
            self.var_manager._params[5400] = float(tool.tool_number)
        elif tool_idx == 0:
            self.var_manager._params[5400] = float(tool.tool_number) if tool.tool_number > 0 else 0.0
        else:
            # 非当前刀具，不影响 5400
            pass
        
        # 5401-5409: 刀具偏移
        self.var_manager._params[5401] = tool.offset.x
        self.var_manager._params[5402] = tool.offset.y
        self.var_manager._params[5403] = tool.offset.z
        self.var_manager._params[5404] = tool.offset.a
        self.var_manager._params[5405] = tool.offset.b
        self.var_manager._params[5406] = tool.offset.c
        self.var_manager._params[5407] = tool.offset.u
        self.var_manager._params[5408] = tool.offset.v
        self.var_manager._params[5409] = tool.offset.w
        
        # 5410: 刀具直径
        self.var_manager._params[5410] = tool.diameter
        
        # 5411: 前角
        self.var_manager._params[5411] = tool.front_angle
        
        # 5412: 后角
        self.var_manager._params[5412] = tool.back_angle
        
        # 5413: 刀具方向
        self.var_manager._params[5413] = float(tool.orientation)
        
        # ===== 同步到 HAL 组件 =====
        # 对应 C++ 中 SET_TOOL_TABLE_ENTRY()
        if hasattr(self, 'virtual_hal') and self.virtual_hal:
            # 更新刀具相关 HAL 引脚
            self.virtual_hal.set('tool.number', tool.tool_number)
            self.virtual_hal.set('tool.diameter', tool.diameter)
            self.virtual_hal.set('tool.length', tool.offset.z)
        
        # ===== 如果是当前刀具，更新 tool_offset =====
        if self.current_tool and self.current_tool.tool_number == tool.tool_number:
            self.tool_offset.x = tool.offset.x
            self.tool_offset.y = tool.offset.y
            self.tool_offset.z = tool.offset.z
            self.tool_offset.a = tool.offset.a
            self.tool_offset.b = tool.offset.b
            self.tool_offset.c = tool.offset.c
            
            # 更新运动学的刀具长度
            if self.kinematics and self.current_h_code > 0:
                self.kinematics.set_tool_length(tool.offset.z)
        
        if self.debug:
            tool = self.tool_table[tool_idx]
            print(f"[SYNC] 刀具 T{tool.tool_number} 参数已同步:")
            print(f"       #5400(ToolNo)={self.var_manager._params[5400]:.0f}")
            print(f"       #5403(Z-Off)={self.var_manager._params[5403]:.3f}")
            print(f"       #5410(Dia)={self.var_manager._params[5410]:.3f}")


    def _handle_g10_current_tool_update(self, tool_idx: int, tool_number: int):
        """
        当 G10 修改的是当前刀具时，更新所有相关状态
        
        对应 C++ 中 convert_setup_tool 的最后部分：
        - 更新 tool_table[0]（如果非随机换刀）
        - 更新参数 5400-5413
        - 调用 SET_TOOL_TABLE_ENTRY
        """
        if not self.current_tool or self.current_tool.tool_number != tool_number:
            return
        
        tool = self.tool_table[tool_idx]
        
        # 更新当前刀具偏移
        self.tool_offset.x = tool.offset.x
        self.tool_offset.y = tool.offset.y
        self.tool_offset.z = tool.offset.z
        self.tool_offset.a = tool.offset.a
        self.tool_offset.b = tool.offset.b
        self.tool_offset.c = tool.offset.c
        
        # 更新刀具长度映射
        self.tool_lengths[tool_number] = tool.offset.z
        
        # 更新刀具半径映射
        self.tool_radii[tool_number] = tool.radius
        
        # 如果有长度补偿激活，更新运动学
        if self.current_h_code > 0 and self.kinematics:
            self.kinematics.set_tool_length(tool.offset.z)
        
        # 如果有半径补偿激活，更新半径
        if self.cutter_comp_active:
            self.comp_radius = tool.radius
        
        # 同步到参数表
        self.var_manager.update_tool_params(
            tool.tool_number,
            {
                'x': tool.offset.x, 'y': tool.offset.y, 'z': tool.offset.z,
                'a': tool.offset.a, 'b': tool.offset.b, 'c': tool.offset.c,
                'u': tool.offset.u, 'v': tool.offset.v, 'w': tool.offset.w
            },
            tool.diameter,
            tool.front_angle,
            tool.back_angle,
            tool.orientation
        )
        
        if self.debug:
            print(f"[G10] 当前刀具 T{tool_number} 已更新")








    
    # ==================== 参数同步 ====================
    
    
    
    
    
    
    
    
    
    
    
    
    
    def _sync_parameters_from_state(self):
        """同步状态到参数表"""
        self.var_manager.update_position_params(
            self.current_pos.x, self.current_pos.y, self.current_pos.z,
            self.current_pos.a, self.current_pos.b, self.current_pos.c
        )
        
        self.var_manager.active_csys = self.g5x_index
        
        # 同步 G92 偏移
        self.var_manager._g92_active = self.g92_active
        self.var_manager._g92_offsets['X'] = self.g92_offset_x
        self.var_manager._g92_offsets['Y'] = self.g92_offset_y
        self.var_manager._g92_offsets['Z'] = self.g92_offset_z
        self.var_manager._g92_offsets['A'] = self.g92_offset_a
        self.var_manager._g92_offsets['B'] = self.g92_offset_b
        self.var_manager._g92_offsets['C'] = self.g92_offset_c
        
        # 同步 G52 偏移
        self.var_manager._g52_offsets['X'] = self.g52_offset_x
        self.var_manager._g52_offsets['Y'] = self.g52_offset_y
        self.var_manager._g52_offsets['Z'] = self.g52_offset_z
        
        # 同步刀具偏移
        self.var_manager._tool_offset['X'] = self.tool_offset.x
        self.var_manager._tool_offset['Y'] = self.tool_offset.y
        self.var_manager._tool_offset['Z'] = self.tool_offset.z
        self.var_manager._tool_offset['A'] = self.tool_offset.a
        self.var_manager._tool_offset['B'] = self.tool_offset.b
        self.var_manager._tool_offset['C'] = self.tool_offset.c
        self.var_manager._tool_offset_active = (self.current_h_code > 0)
        
        # 同步 XY 旋转
        self.var_manager.set_xy_rotation(self.rotation_xy)
        
        # 同步 G5x 坐标系偏移
        if self.g5x_index in self.work_offsets:
            wo = self.work_offsets[self.g5x_index]
            self.var_manager.set_csys_origin(
                self.g5x_index,
                wo.get('X', 0.0), wo.get('Y', 0.0), wo.get('Z', 0.0),
                wo.get('A', 0.0), wo.get('B', 0.0), wo.get('C', 0.0)
            )
        
        # 同步模态状态
        self.var_manager._distance_mode = 'ABSOLUTE' if self.is_absolute else 'INCREMENTAL'
        self.var_manager._plane_mode = {17: 'XY', 18: 'XZ', 19: 'YZ'}.get(self.current_plane, 'XY')
        self.var_manager._feed_rate = self.feedrate
        self.var_manager._spindle_speed = self.state.spindle_speed
        
        self.var_manager.set_system('_x', self.current_pos.x)
        self.var_manager.set_system('_y', self.current_pos.y)
        self.var_manager.set_system('_z', self.current_pos.z)
        self.var_manager.set_system('_feed', self.feedrate)
        self.var_manager.set_system('_current_tool', float(self.state.tool))
        self.var_manager.set_system('_rpm', self.state.spindle_speed)
        self.var_manager.set_system('_absolute', 1.0 if self.is_absolute else 0.0)
        self.var_manager.set_system('_metric', 1.0 if self.state.units == 21 else 0.0)
        self.var_manager._current_line = self.current_line
        

    def _sync_machine_from_current(self):
        """从工件坐标同步机器坐标
        
        正变换顺序（与 _compute_machine_coords 严格对应）：
        工件坐标 → +G92偏移 → +XY旋转 → +G5x偏移 → +TLO → +G52偏移 → 逆运动学 → 关节坐标
        
        参照 LinuxCNC 源码：
        - rs274ngc.cpp: find_current_in_system() 计算绝对坐标
        - kinematics.cpp: kinematicsInverse() 计算关节坐标
        """
        # ===== 第1步：加上 G92 偏移 =====
        # 对应 C++: *x += s->axis_offset_x
        abs_x = self.current_pos.x + self.g92_offset_x
        abs_y = self.current_pos.y + self.g92_offset_y
        abs_z = self.current_pos.z + self.g92_offset_z
        abs_a = self.current_pos.a + self.g92_offset_a
        abs_b = self.current_pos.b + self.g92_offset_b
        abs_c = self.current_pos.c + self.g92_offset_c
        
        # ===== 第2步：XY 旋转 =====
        # 对应 C++: rotate(x, y, s->rotation_xy)
        if abs(self.rotation_xy) > 1e-9:
            rot = math.radians(self.rotation_xy)
            cos_r = math.cos(rot)
            sin_r = math.sin(rot)
            rx = abs_x * cos_r - abs_y * sin_r
            ry = abs_x * sin_r + abs_y * cos_r
            abs_x, abs_y = rx, ry
        
        # ===== 第3步：加上 G5x 偏移 =====
        # 对应 C++: *x += s->origin_offset_x
        abs_x += self.g5x_offset_x
        abs_y += self.g5x_offset_y
        abs_z += self.g5x_offset_z
        abs_a += self.g5x_offset_a
        abs_b += self.g5x_offset_b
        abs_c += self.g5x_offset_c
        
        # ===== 第4步：加上刀具长度补偿 (TLO) =====
        # 对应 C++: *x += s->tool_offset.tran.x
        abs_x += self.tool_offset.x
        abs_y += self.tool_offset.y
        abs_z += self.tool_offset.z
        abs_a += self.tool_offset.a
        abs_b += self.tool_offset.b
        abs_c += self.tool_offset.c
        
        # ===== 第5步：加上额外工件偏移 =====
        abs_x += self.work_offset_x
        abs_y += self.work_offset_y
        abs_z += self.work_offset_z
        abs_a += self.work_offset_a
        abs_b += self.work_offset_b
        abs_c += self.work_offset_c
        
        # ===== 第6步：加上 G52 偏移 =====
        abs_x += self.g52_offset_x
        abs_y += self.g52_offset_y
        abs_z += self.g52_offset_z
        abs_a += self.g52_offset_a
        abs_b += self.g52_offset_b
        abs_c += self.g52_offset_c
        
        # ===== 第7步：计算关节坐标（如果需要RTCP）=====
        # 对应 C++: motion controller 调用 kinematicsInverse()
        if self.rtcp_enabled and self.kinematics is not None:
            try:
                # 传入包含 TLO 的绝对坐标
                world_tuple = (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c)
                joints = self.kinematics.world_to_joints(world_tuple)
                
                # 根据运动学类型映射关节坐标
                config = KINEMATICS_JOINT_CONFIG.get(
                    self.kinematics_type,
                    KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY]
                )
                
                x_idx = config.get('x_idx', 0)
                y_idx = config.get('y_idx', 1)
                z_idx = config.get('z_idx', 2)
                a_idx = config.get('a_idx', 3)
                b_idx = config.get('b_idx', 4)
                c_idx = config.get('c_idx', 5)
                
                self.machine_x = joints[x_idx] if (x_idx >= 0 and x_idx < len(joints)) else abs_x
                self.machine_y = joints[y_idx] if (y_idx >= 0 and y_idx < len(joints)) else abs_y
                self.machine_z = joints[z_idx] if (z_idx >= 0 and z_idx < len(joints)) else abs_z
                self.machine_a = joints[a_idx] if (a_idx >= 0 and a_idx < len(joints)) else abs_a
                self.machine_b = joints[b_idx] if (b_idx >= 0 and b_idx < len(joints)) else abs_b
                self.machine_c = joints[c_idx] if (c_idx >= 0 and c_idx < len(joints)) else abs_c
            except Exception as e:
                if self.debug:
                    print(f"[MCS] 逆运动学计算失败: {e}")
                self.machine_x = abs_x
                self.machine_y = abs_y
                self.machine_z = abs_z
                self.machine_a = abs_a
                self.machine_b = abs_b
                self.machine_c = abs_c
        else:
            # RTCP 禁用：直接使用绝对坐标作为机器坐标
            self.machine_x = abs_x
            self.machine_y = abs_y
            self.machine_z = abs_z
            self.machine_a = abs_a
            self.machine_b = abs_b
            self.machine_c = abs_c


    def _update_current_from_machine(self):
        """从机器坐标更新工件坐标
        
        逆变换顺序（与 _sync_machine_from_current 严格对应）：
        绝对坐标 → -G52偏移 → -TLO → -G5x偏移 → -XY旋转 → -G92偏移 → 工件坐标
        
        正变换: 工件 + G92 + XY旋转 + G5x + TLO + G52 = 绝对
        逆变换: 绝对 - G52 - TLO - G5x - XY旋转 - G92 = 工件
        """
        # ===== 第1步：运动学正解（关节→世界）=====
        if self.rtcp_enabled and self.kinematics is not None:
            # 构建符合运动学类型的关节数组
            config = KINEMATICS_JOINT_CONFIG.get(self.kinematics_type, {})
            num_joints = config.get('num_joints', 6)
            joints = [0.0] * num_joints
            
            x_idx = config.get('x_idx', 0)
            y_idx = config.get('y_idx', 1)
            z_idx = config.get('z_idx', 2)
            a_idx = config.get('a_idx', 3)
            b_idx = config.get('b_idx', 4)
            c_idx = config.get('c_idx', 5)
            
            if x_idx >= 0: joints[x_idx] = self.machine_x
            if y_idx >= 0: joints[y_idx] = self.machine_y
            if z_idx >= 0: joints[z_idx] = self.machine_z
            if a_idx >= 0: joints[a_idx] = self.machine_a
            if b_idx >= 0: joints[b_idx] = self.machine_b
            if c_idx >= 0: joints[c_idx] = self.machine_c
            
            world = self.kinematics.joints_to_world(joints)
            
            abs_x, abs_y, abs_z = world[0], world[1], world[2]
            abs_a, abs_b, abs_c = world[3], world[4], world[5]
        else:
            # 非RTCP模式：机器坐标 = 绝对坐标
            abs_x, abs_y, abs_z = self.machine_x, self.machine_y, self.machine_z
            abs_a, abs_b, abs_c = self.machine_a, self.machine_b, self.machine_c
        
        # ===== 第2步：逆变换（与 _sync_machine_from_current 严格对应）=====
        # 正变换: 工件 + G92 + XY旋转 + G5x + TLO + G52 → 绝对
        # 逆变换: 绝对 - G52 - TLO - G5x - XY旋转 - G92 → 工件
        
        # ★ 关键修复：逆变换减法顺序与正变换加法顺序相反 ★
        # Step 2a: 减去 G52 偏移
        x = abs_x - self.g52_offset_x
        y = abs_y - self.g52_offset_y
        z = abs_z - self.g52_offset_z
        a = abs_a - self.g52_offset_a
        b = abs_b - self.g52_offset_b
        c = abs_c - self.g52_offset_c
        
        # Step 2b: 减去额外工件偏移
        x -= self.work_offset_x
        y -= self.work_offset_y
        z -= self.work_offset_z
        a -= self.work_offset_a
        b -= self.work_offset_b
        c -= self.work_offset_c
        
        # Step 2c: 减去刀具长度补偿 (TLO)
        x -= self.tool_offset.x
        y -= self.tool_offset.y
        z -= self.tool_offset.z
        a -= self.tool_offset.a
        b -= self.tool_offset.b
        c -= self.tool_offset.c
        
        # Step 2d: 减去 G5x 坐标系偏移
        x -= self.g5x_offset_x
        y -= self.g5x_offset_y
        z -= self.g5x_offset_z
        a -= self.g5x_offset_a
        b -= self.g5x_offset_b
        c -= self.g5x_offset_c
        
        # Step 2e: 反向 XY 旋转（如果设置了旋转角度）
        # 注意：这里需要在减去G92之前反向旋转
        if abs(self.rotation_xy) > 1e-9:
            rot = math.radians(-self.rotation_xy)  # 反向旋转
            cos_r = math.cos(rot)
            sin_r = math.sin(rot)
            rx = x * cos_r - y * sin_r
            ry = x * sin_r + y * cos_r
            x, y = rx, ry
        
        # Step 2f: 减去 G92 偏移
        x -= self.g92_offset_x
        y -= self.g92_offset_y
        z -= self.g92_offset_z
        a -= self.g92_offset_a
        b -= self.g92_offset_b
        c -= self.g92_offset_c
        
        # ===== 第3步：更新工件坐标 =====
        self.current_pos.x = x
        self.current_pos.y = y
        self.current_pos.z = z
        self.current_pos.a = a
        self.current_pos.b = b
        self.current_pos.c = c
        
        # ===== 第4步：同步参数表 =====
        self._sync_parameters_from_state()    
 
    # ==================== 坐标系管理 ====================
    
 
    def set_coordinate_system(self, csys: int):
        """
        G54-G59.3: 选择坐标系
        
        完全参照 LinuxCNC 官方实现：
        rs274ngc.cpp: Interp::convert_coordinate_system
        
        关键：切换坐标系时，使用 _get_current_in_system 重新计算工件坐标
        
        Args:
            csys: 坐标系编号 (1-9)
        """
        if csys < 1 or csys > 9:
            if self.debug:
                print(f"[CSYS] 错误: 坐标系编号 {csys} 超出范围 (1-9)")
            return
        
        if csys == self.g5x_index:
            if self.debug:
                print(f"[CSYS] 已经是 G{53 + csys}，无需切换")
            return
        
        if self.debug:
            print(f"[CSYS] ========== 切换坐标系 G{53 + self.g5x_index} → G{53 + csys} ==========")
            print(f"[CSYS] 切换前工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})")
        
        # ===== 第1步：计算当前在目标坐标系中的位置 =====
        # 对应官方: find_current_in_system(settings, origin, &settings->current_x, ...)
        (new_x, new_y, new_z, new_a, new_b, new_c, new_u, new_v, new_w) = \
            self._get_current_in_system(csys)
        
        # ===== 第2步：更新当前位置 =====
        self.current_pos.x = new_x
        self.current_pos.y = new_y
        self.current_pos.z = new_z
        self.current_pos.a = new_a
        self.current_pos.b = new_b
        self.current_pos.c = new_c
        
        # ===== 第3步：更新 origin_index =====
        self.g5x_index = csys
        self.current_work_offset = csys
        
        # ===== 第4步：从参数表加载新坐标系偏移 =====
        # 对应官方: parameters[5201 + (origin * 20)]
        new_offsets = self.work_offsets.get(csys, {})
        
        self.g5x_offset_x = new_offsets.get('X', 0.0)
        self.g5x_offset_y = new_offsets.get('Y', 0.0)
        self.g5x_offset_z = new_offsets.get('Z', 0.0)
        self.g5x_offset_a = new_offsets.get('A', 0.0)
        self.g5x_offset_b = new_offsets.get('B', 0.0)
        self.g5x_offset_c = new_offsets.get('C', 0.0)
        self.g5x_offset_u = new_offsets.get('U', 0.0)
        self.g5x_offset_v = new_offsets.get('V', 0.0)
        self.g5x_offset_w = new_offsets.get('W', 0.0)
        
        # ===== 第5步：更新 XY 旋转 =====
        # 对应官方: settings->rotation_xy = parameters[5210 + (origin * 20)];
        new_rotation = new_offsets.get('R', 0.0)
        self.rotation_xy = new_rotation
        t = math.radians(new_rotation)
        self.rotation_sin = math.sin(t)
        self.rotation_cos = math.cos(t)
        
        # ===== 第6步：同步到参数表 =====
        self._sync_parameters_from_state()
        self._sync_machine_from_current()
        
        if self.debug:
            print(f"[CSYS] 切换后工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})")
            print(f"[CSYS] 新G5x偏移: ({self.g5x_offset_x:.3f}, {self.g5x_offset_y:.3f}, {self.g5x_offset_z:.3f})")
            print(f"[CSYS] 新XY旋转: {self.rotation_xy}°")
            print(f"[CSYS] ========== 坐标系切换完成 ==========") 
 
 
    def set_g5x_offset(self, csys: int, x: float = None, y: float = None, z: float = None,
                       a: float = 0.0, b: float = 0.0, c: float = 0.0,
                       u: float = 0.0, v: float = 0.0, w: float = 0.0):
        """G10 L2: 直接设置坐标系偏移"""
        if csys < 1 or csys > 9:
            return
        
        if csys not in self.work_offsets:
            self.work_offsets[csys] = {}
        
        old_offsets = self.work_offsets[csys].copy()
        updates = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'U': u, 'V': v, 'W': w}
        
        for axis, val in updates.items():
            if val is not None:
                self.work_offsets[csys][axis] = val
        
        if csys == self.g5x_index:
            for axis in 'XYZABC':
                if axis in old_offsets:
                    delta = old_offsets[axis] - self.work_offsets[csys].get(axis, 0.0)
                    if axis == 'X': self.current_pos.x += delta
                    elif axis == 'Y': self.current_pos.y += delta
                    elif axis == 'Z': self.current_pos.z += delta
                    elif axis == 'A': self.current_pos.a += delta
                    elif axis == 'B': self.current_pos.b += delta
                    elif axis == 'C': self.current_pos.c += delta
            
            self.g5x_offset_x = self.work_offsets[csys].get('X', 0.0)
            self.g5x_offset_y = self.work_offsets[csys].get('Y', 0.0)
            self.g5x_offset_z = self.work_offsets[csys].get('Z', 0.0)
            self.g5x_offset_a = self.work_offsets[csys].get('A', 0.0)
            self.g5x_offset_b = self.work_offsets[csys].get('B', 0.0)
            self.g5x_offset_c = self.work_offsets[csys].get('C', 0.0)
        
        self.var_manager.set_csys_origin(csys,
            self.work_offsets[csys].get('X', 0.0),
            self.work_offsets[csys].get('Y', 0.0),
            self.work_offsets[csys].get('Z', 0.0),
            self.work_offsets[csys].get('A', 0.0),
            self.work_offsets[csys].get('B', 0.0),
            self.work_offsets[csys].get('C', 0.0))
        
        self._sync_machine_from_current()
        self._sync_parameters_from_state()
    



    def set_coordinate_system_g10_l20_bak(self, csys: int, params_str: str):
        """G10 L20: 设置坐标系使当前位置变为指定值"""
        if csys < 1 or csys > 9:
            return
        
        target_csys = csys
        old_offsets = self.work_offsets.get(target_csys, {})
        
        cx = self.current_pos.x + self.g92_offset_x + self.g52_offset_x
        cy = self.current_pos.y + self.g92_offset_y + self.g52_offset_y
        cz = self.current_pos.z + self.g92_offset_z + self.g52_offset_z
        ca = self.current_pos.a + self.g92_offset_a + self.g52_offset_a
        cb = self.current_pos.b + self.g92_offset_b + self.g52_offset_b
        cc = self.current_pos.c + self.g92_offset_c + self.g52_offset_c
        
        if self.rotation_xy != 0:
            crot = math.cos(math.radians(self.rotation_xy))
            srot = math.sin(math.radians(self.rotation_xy))
            rotx = cx * crot - cy * srot
            roty = cx * srot + cy * crot
            cx, cy = rotx, roty
        
        cx += self.g5x_offset_x
        cy += self.g5x_offset_y
        cz += self.g5x_offset_z
        ca += self.g5x_offset_a
        cb += self.g5x_offset_b
        cc += self.g5x_offset_c
        
        target_g5x_x = old_offsets.get('X', 0.0)
        target_g5x_y = old_offsets.get('Y', 0.0)
        target_g5x_z = old_offsets.get('Z', 0.0)
        
        cx -= target_g5x_x
        cy -= target_g5x_y
        cz -= target_g5x_z
        
        target_x = cx
        target_y = cy
        target_z = cz
        
        for match in re.finditer(r'([XYZ])\s*=\s*([+-]?\d*\.?\d+)', params_str, re.I):
            axis = match.group(1).upper()
            value = float(match.group(2))
            if axis == 'X': target_x = value
            elif axis == 'Y': target_y = value
            elif axis == 'Z': target_z = value
        
        new_g5x_x = target_g5x_x + (cx - target_x)
        new_g5x_y = target_g5x_y + (cy - target_y)
        new_g5x_z = target_g5x_z + (cz - target_z)
        
        self.work_offsets[target_csys]['X'] = new_g5x_x
        self.work_offsets[target_csys]['Y'] = new_g5x_y
        self.work_offsets[target_csys]['Z'] = new_g5x_z
        
        if target_csys == self.g5x_index:
            self.g5x_offset_x = new_g5x_x
            self.g5x_offset_y = new_g5x_y
            self.g5x_offset_z = new_g5x_z
        
        self.current_pos.x = target_x
        self.current_pos.y = target_y
        self.current_pos.z = target_z
        
        self.var_manager.set_csys_origin(target_csys, new_g5x_x, new_g5x_y, new_g5x_z)
        self._sync_machine_from_current()
        self._sync_parameters_from_state()


    def set_coordinate_system_g10_l20(self, csys: int, params_str: str):
        """
        G10 L20: 设置坐标系使当前位置变为指定值
        
        完全参照 LinuxCNC 官方实现：
        rs274ngc.cpp: Interp::convert_setup (G10 L20 分支)
        
        核心逻辑：
        1. 获取目标坐标系的旧偏移和旧旋转
        2. 使用 _get_current_in_system 计算当前在目标坐标系中的位置
        3. 应用用户指定的目标值
        4. 正向 XY 旋转（使用旧 rotation_xy）
        5. 计算新 G5x 偏移：old_position + old_origin - new_position = new_origin
        6. 存储结果
        7. 如果是当前坐标系，更新 rotation_xy 为 0
        
        Args:
            csys: 坐标系编号 (0=当前坐标系, 1-9=指定坐标系)
            params_str: 参数字符串，如 "X0 Y0 Z100 A0 C0"
        """
        # 处理 P0（当前坐标系）
        if csys == 0:
            csys = self.g5x_index
        
        if csys < 1 or csys > 9:
            if self.debug:
                print(f"[G10 L20] 错误: 坐标系编号 {csys} 超出范围 (1-9)")
            return
        
        if self.debug:
            print(f"[G10 L20] ========== 设置坐标系 G{53 + csys} ==========")
            print(f"[G10 L20] 参数: '{params_str}'")
            print(f"[G10 L20] 当前工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})")
            print(f"[G10 L20] 当前G5x索引: {self.g5x_index}, 旋转: {self.rotation_xy}°")
        
        # ===== 第1步：获取目标坐标系的旧偏移和旧旋转 =====
        old_offsets = self.work_offsets.get(csys, {}).copy()
        
        old_g5x_x = old_offsets.get('X', 0.0)
        old_g5x_y = old_offsets.get('Y', 0.0)
        old_g5x_z = old_offsets.get('Z', 0.0)
        old_g5x_a = old_offsets.get('A', 0.0)
        old_g5x_b = old_offsets.get('B', 0.0)
        old_g5x_c = old_offsets.get('C', 0.0)
        old_rotation = old_offsets.get('R', 0.0)
        
        # ===== 第2步：计算当前在目标坐标系中的位置 =====
        # 对应官方: find_current_in_system(settings, p_int, &cx, &cy, &cz, ...)
        (cx, cy, cz, ca, cb, cc, cu, cv, cw) = self._get_current_in_system(csys)
        
        if self.debug:
            print(f"[G10 L20] 当前在G{53+csys}中的位置: ({cx:.3f}, {cy:.3f}, {cz:.3f})")
            print(f"[G10 L20] 旧G5x偏移: ({old_g5x_x:.3f}, {old_g5x_y:.3f}, {old_g5x_z:.3f})")
            print(f"[G10 L20] 旧XY旋转: {old_rotation}°")
        
        # ===== 第3步：记录旧坐标并应用用户指定的值 =====
        # 对应官方: double oldx = cx, oldy = cy; x = cx; y = cy;
        oldx = cx
        oldy = cy
        x = cx
        y = cy
        z = cz
        a = ca
        b = cb
        c_val = cc
        
        # 解析用户指定的坐标值（支持 X=0 和 X0 两种格式）
        for match in re.finditer(r'([XYZABC])\s*=\s*([+-]?\d*\.?\d+)', params_str, re.I):
            axis = match.group(1).upper()
            value = float(match.group(2))
            
            if axis == 'X': x = value
            elif axis == 'Y': y = value
            elif axis == 'Z': z = value
            elif axis == 'A': a = value
            elif axis == 'B': b = value
            elif axis == 'C': c_val = value
        
        if self.debug:
            print(f"[G10 L20] 用户指定值: ({x:.3f}, {y:.3f}, {z:.3f}, A={a}, B={b}, C={c_val})")
        
        # ===== 第4步：正向 XY 旋转（使用旧 rotation_xy）=====
        # 对应官方: rotate(&oldx, &oldy, r); rotate(&x, &y, r);
        if old_rotation != 0:
            rot_rad = math.radians(old_rotation)
            cos_r = math.cos(rot_rad)
            sin_r = math.sin(rot_rad)
            
            rot_oldx = oldx * cos_r - oldy * sin_r
            rot_oldy = oldx * sin_r + oldy * cos_r
            
            rot_x = x * cos_r - y * sin_r
            rot_y = x * sin_r + y * cos_r
            
            oldx, oldy = rot_oldx, rot_oldy
            x, y = rot_x, rot_y
        
        # ===== 第5步：计算新 G5x 偏移 =====
        # 对应官方:
        #   x = oldx + USER_TO_PROGRAM_LEN(parameters[5201 + (p_int * 20)]) - x;
        #   y = oldy + USER_TO_PROGRAM_LEN(parameters[5202 + (p_int * 20)]) - y;
        new_g5x_x = oldx + old_g5x_x - x
        new_g5x_y = oldy + old_g5x_y - y
        new_g5x_z = cz + old_g5x_z - z
        new_g5x_a = ca + old_g5x_a - a
        new_g5x_b = cb + old_g5x_b - b
        new_g5x_c = cc + old_g5x_c - c_val
        
        # ===== 第6步：存储新偏移 =====
        if csys not in self.work_offsets:
            self.work_offsets[csys] = {}
        
        self.work_offsets[csys]['X'] = new_g5x_x
        self.work_offsets[csys]['Y'] = new_g5x_y
        self.work_offsets[csys]['Z'] = new_g5x_z
        self.work_offsets[csys]['A'] = new_g5x_a
        self.work_offsets[csys]['B'] = new_g5x_b
        self.work_offsets[csys]['C'] = new_g5x_c
        self.work_offsets[csys]['U'] = 0.0
        self.work_offsets[csys]['V'] = 0.0
        self.work_offsets[csys]['W'] = 0.0
        self.work_offsets[csys]['R'] = 0.0  # G10 L20 将 XY 旋转清零
        
        # ===== 第7步：如果是当前坐标系，更新 rotation_xy =====
        # 对应官方:
        #   if (p_int == settings->origin_index) {
        #       rotate(&settings->current_x, &settings->current_y, settings->rotation_xy);
        #       settings->rotation_xy = 0;
        #   }
        if csys == self.g5x_index:
            # 先正向旋转当前坐标（去掉旧旋转）
            if self.rotation_xy != 0:
                rot_rad = math.radians(self.rotation_xy)
                cos_r = math.cos(rot_rad)
                sin_r = math.sin(rot_rad)
                rot_x = self.current_pos.x * cos_r - self.current_pos.y * sin_r
                rot_y = self.current_pos.x * sin_r + self.current_pos.y * cos_r
                self.current_pos.x = rot_x
                self.current_pos.y = rot_y
            
            # 加上旧 G5x 偏移
            self.current_pos.x += self.g5x_offset_x
            self.current_pos.y += self.g5x_offset_y
            self.current_pos.z += self.g5x_offset_z
            self.current_pos.a += self.g5x_offset_a
            self.current_pos.b += self.g5x_offset_b
            self.current_pos.c += self.g5x_offset_c
            
            # 减去新 G5x 偏移
            self.current_pos.x -= new_g5x_x
            self.current_pos.y -= new_g5x_y
            self.current_pos.z -= new_g5x_z
            self.current_pos.a -= new_g5x_a
            self.current_pos.b -= new_g5x_b
            self.current_pos.c -= new_g5x_c
            
            # 更新 G5x 偏移变量
            self.g5x_offset_x = new_g5x_x
            self.g5x_offset_y = new_g5x_y
            self.g5x_offset_z = new_g5x_z
            self.g5x_offset_a = new_g5x_a
            self.g5x_offset_b = new_g5x_b
            self.g5x_offset_c = new_g5x_c
            
            # 清零 XY 旋转
            self.rotation_xy = 0.0
            self.rotation_sin = 0.0
            self.rotation_cos = 1.0
        
        # ===== 第8步：同步到参数表 =====
        if hasattr(self, 'var_manager') and self.var_manager:
            self.var_manager.set_csys_origin(
                csys, new_g5x_x, new_g5x_y, new_g5x_z,
                new_g5x_a, new_g5x_b, new_g5x_c
            )
            self.var_manager.set_xy_rotation(
                0.0 if csys == self.g5x_index else self.rotation_xy
            )
        
        # ===== 第9步：同步机器坐标和参数 =====
        self._sync_machine_from_current()
        self._sync_parameters_from_state()
        
        if self.debug:
            print(f"[G10 L20] 新G5x偏移: ({new_g5x_x:.3f}, {new_g5x_y:.3f}, {new_g5x_z:.3f})")
            print(f"[G10 L20] 更新后工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})")
            print(f"[G10 L20] 更新后XY旋转: {self.rotation_xy}°")
            print(f"[G10 L20] ========== G10 L20 完成 ==========")


    
    def set_g92_offset(self, x: float = None, y: float = None, z: float = None,
                       a: float = None, b: float = None, c: float = None,
                       u: float = None, v: float = None, w: float = None):
        """G92: 设置临时坐标系偏移。

        LinuxCNC 的 G92 允许把某个轴设置为 0，因此不能用 0.0
        表示“该轴未指定”。这里用 None 表示未指定，只对实际出现
        在 G92 程序段中的轴计算 offset。
        """
        old_x, old_y, old_z = self.current_pos.x, self.current_pos.y, self.current_pos.z

        any_axis = False

        if x is not None:
            self.g92_offset_x = self.g92_offset_x + self.current_pos.x - x
            self.current_pos.x = x
            any_axis = True
        if y is not None:
            self.g92_offset_y = self.g92_offset_y + self.current_pos.y - y
            self.current_pos.y = y
            any_axis = True
        if z is not None:
            self.g92_offset_z = self.g92_offset_z + self.current_pos.z - z
            self.current_pos.z = z
            any_axis = True
        if a is not None:
            self.g92_offset_a = self.g92_offset_a + self.current_pos.a - a
            self.current_pos.a = a
            any_axis = True
        if b is not None:
            self.g92_offset_b = self.g92_offset_b + self.current_pos.b - b
            self.current_pos.b = b
            any_axis = True
        if c is not None:
            self.g92_offset_c = self.g92_offset_c + self.current_pos.c - c
            self.current_pos.c = c
            any_axis = True
        if u is not None:
            self.g92_offset_u = self.g92_offset_u + getattr(self.current_pos, 'u', 0.0) - u
            self.current_pos.u = u
            any_axis = True
        if v is not None:
            self.g92_offset_v = self.g92_offset_v + getattr(self.current_pos, 'v', 0.0) - v
            self.current_pos.v = v
            any_axis = True
        if w is not None:
            self.g92_offset_w = self.g92_offset_w + getattr(self.current_pos, 'w', 0.0) - w
            self.current_pos.w = w
            any_axis = True

        if not any_axis:
            return

        self.g92_active = True

        # 同步参数表。apply_g92 需要 G92 生效前的当前位置。
        self.var_manager.apply_g92(x, y, z, old_x, old_y, old_z)
        self.var_manager._params[InterpParameterIndex.G92_A] = self.g92_offset_a
        self.var_manager._params[InterpParameterIndex.G92_B] = self.g92_offset_b
        self.var_manager._params[InterpParameterIndex.G92_C] = self.g92_offset_c
        self.var_manager._params[InterpParameterIndex.G92_U] = self.g92_offset_u
        self.var_manager._params[InterpParameterIndex.G92_V] = self.g92_offset_v
        self.var_manager._params[InterpParameterIndex.G92_W] = self.g92_offset_w

        self._sync_machine_from_current()
        self._sync_parameters_from_state()
    
    def clear_g92(self):
        """G92.1: 清除G92偏移"""
        self.current_pos.x += self.g92_offset_x
        self.current_pos.y += self.g92_offset_y
        self.current_pos.z += self.g92_offset_z
        self.current_pos.a += self.g92_offset_a
        self.current_pos.b += self.g92_offset_b
        self.current_pos.c += self.g92_offset_c
        
        self.g92_offset_x = 0.0
        self.g92_offset_y = 0.0
        self.g92_offset_z = 0.0
        self.g92_offset_a = 0.0
        self.g92_offset_b = 0.0
        self.g92_offset_c = 0.0
        self.g92_active = False
        
        self.var_manager.clear_g92()
        self._sync_machine_from_current()
        self._sync_parameters_from_state()
    
    def set_g52_offset(self, x: float = 0.0, y: float = 0.0, z: float = 0.0,
                       a: float = 0.0, b: float = 0.0, c: float = 0.0):
        """G52: 设置临时工件偏移"""
        old_g52_x = self.g52_offset_x
        old_g52_y = self.g52_offset_y
        old_g52_z = self.g52_offset_z
        
        self.g52_offset_x = x
        self.g52_offset_y = y
        self.g52_offset_z = z
        self.g52_offset_a = a
        self.g52_offset_b = b
        self.g52_offset_c = c
        
        self.current_pos.x += old_g52_x - x
        self.current_pos.y += old_g52_y - y
        self.current_pos.z += old_g52_z - z
        self.current_pos.a += a if a != 0.0 else 0.0
        self.current_pos.b += b if b != 0.0 else 0.0
        self.current_pos.c += c if c != 0.0 else 0.0
        
        self._sync_machine_from_current()
        self._sync_parameters_from_state()
    
    def set_xy_rotation(self, theta: float):
        """设置XY旋转（同步到参数表）"""
        self.rotation_xy = theta
        t = math.radians(theta)
        self.rotation_sin = math.sin(t)
        self.rotation_cos = math.cos(t)
        # 同步到参数表
        self.var_manager.set_xy_rotation(theta)
    
    
    # ==================== 运动学切换 ====================
    
    def set_kinematics_by_type(self, kin_type: int):
        """
        运动学切换
        kin_type: 0 = IDENTITY (M429), 1 = 原始五轴 (M428), 2 = FIVEAXIS_BC (M430)
        """
        if kin_type == 0:  # M429: 切换到三轴恒等运动学
            self._switch_to_identity()
        elif kin_type == 1:  # M428: 恢复到原始五轴运动学
            self._switch_to_original()
        elif kin_type == 2:  # M430: 切换到 FIVEAXIS_BC
            self._switch_to_fiveaxis_bc()
        else:
            # 未知的运动学类型，记录警告但不中断
            if self.debug:
                print(f"[KINS] 警告: 未知的运动学切换类型: {kin_type}, 忽略")
    

    def _switch_to_identity(self):
        """
        M429: 切换到三轴恒等运动学 (IDENTITY)
        
        关键修复:
        1. 保存当前运动学类型（用于M428恢复）
        2. 根据运动学类型保存对应的旋转轴角度
        3. 清零所有旋转轴 (A, B, C)
        4. 禁用RTCP
        """
        if self.debug:
            print(f"[KINS] M429: 切换到三轴运动学 (IDENTITY)")
        
        # 保存切换前的运动学类型，以便 M428 恢复
        if self.kinematics_type != KinematicsType.IDENTITY:
            self._saved_kinematics_type = self.kinematics_type
            
            # ★ 修复: 保存当前运动学参数，以便M428恢复 ★
            if self.kinematics and hasattr(self.kinematics, 'p'):
                p = self.kinematics.p
                self._saved_kinematics_params = {
                    'rot_center_x': getattr(p, 'rot_center_x', 0.0),
                    'rot_center_y': getattr(p, 'rot_center_y', 0.0),
                    'rot_center_z': getattr(p, 'rot_center_z', -300.0),
                    'axis_offset_x': getattr(p, 'axis_offset_x', 0.0),
                    'axis_offset_y': getattr(p, 'axis_offset_y', 0.0),
                    'axis_offset_z': getattr(p, 'axis_offset_z', 200.0),
                    'tool_length': self.kinematics._tool_length,
                    'conventional_directions': getattr(p, 'conventional_directions', False),
                }
            
            if self.debug:
                print(f"[KINS] 已保存原始运动学类型: {self._saved_kinematics_type.name}")
        else:
            # 已经是IDENTITY，不需要重复切换
            if self.debug:
                print(f"[KINS] 已经是IDENTITY模式，跳过切换")
            return
        
        # ===== 保存旋转轴位置（根据运动学类型） =====
        # 不同运动学类型使用的旋转轴不同
        self._saved_rotation_a = self.current_pos.a if self.current_pos.a is not None else 0.0
        self._saved_rotation_b = self.current_pos.b if self.current_pos.b is not None else 0.0
        self._saved_rotation_c = self.current_pos.c if self.current_pos.c is not None else 0.0
        
        # 记录当前运动学类型，用于M428恢复时判断哪些轴需要恢复
        self._saved_rotation_kinematics = self.kinematics_type
        
        if self.debug:
            kin_name = self.kinematics_type.name
            # 根据运动学类型显示不同的轴信息
            if self.kinematics_type == KinematicsType.TRT_AC:
                print(f"[KINS] 保存{kin_name}旋转轴: A={self._saved_rotation_a:.3f}, C={self._saved_rotation_c:.3f}")
            elif self.kinematics_type == KinematicsType.TRT_BC:
                print(f"[KINS] 保存{kin_name}旋转轴: B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}")
            elif self.kinematics_type == KinematicsType.FIVEAXIS_BC:
                print(f"[KINS] 保存{kin_name}旋转轴: B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}")
            elif self.kinematics_type == KinematicsType.MAXKINS_BC:
                print(f"[KINS] 保存{kin_name}旋转轴: A={self._saved_rotation_a:.3f}, B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}")
            else:
                print(f"[KINS] 保存{kin_name}旋转轴: A={self._saved_rotation_a:.3f}, B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}")
        
        # 切换到恒等运动学
        self.kinematics_type = KinematicsType.IDENTITY
        self.data.kinematics_type = "IDENTITY"
        self.rtcp_enabled = False
        self.data.rtcp_enabled = False
        
        # 禁用当前运动学实例的 RTCP
        if self.kinematics:
            self.kinematics.enable_rtcp(False)
        
        # 设置恒等模式：直接透传坐标
        self.kinematics = None
        
        # ===== 清零所有旋转轴 =====
        # IDENTITY运动学不支持旋转轴，必须清零
        self.current_pos.a = 0.0
        self.current_pos.b = 0.0
        self.current_pos.c = 0.0
        
        self.machine_a = 0.0
        self.machine_b = 0.0
        self.machine_c = 0.0
        
        if self.debug:
            print(f"[KINS] 清零旋转轴: A→0, B→0, C→0")
        
        # 同步参数表
        self._sync_parameters_from_state()
        
        if self.debug:
            print(f"[KINS] 当前运动学: IDENTITY, RTCP: 禁用")


    
    def _switch_to_original(self):
        """
        M428: 恢复到原始五轴运动学
        
        关键修复:
        1. 恢复保存的运动学类型
        2. 根据运动学类型恢复对应的旋转轴角度
        3. 重新创建运动学实例
        4. 恢复刀具长度补偿
        5. 启用RTCP
        """
        if self.debug:
            print(f"[KINS] M428: 恢复到五轴运动学")
        
        # 恢复保存的运动学类型
        # 优先级1: 使用M429之前通过_switch_to_identity保存的类型
        if hasattr(self, '_saved_kinematics_type') and self._saved_kinematics_type is not None:
            restore_type = self._saved_kinematics_type
            if self.debug:
                print(f"[KINS] 使用保存的运动学类型: {restore_type.name}")
        # 优先级2: 如果当前不是IDENTITY，说明从未切换过，使用当前类型
        elif self.kinematics_type != KinematicsType.IDENTITY:
            restore_type = self.kinematics_type
            if self.debug:
                print(f"[KINS] 使用当前运动学类型: {restore_type.name}")
        # 优先级3: 从data.kinematics_type记录恢复
        else:
            saved_name = self.data.kinematics_type
            type_map = {
                'TRT_AC': KinematicsType.TRT_AC,
                'TRT_BC': KinematicsType.TRT_BC,
                'FIVEAXIS_BC': KinematicsType.FIVEAXIS_BC,
                'MAXKINS_BC': KinematicsType.MAXKINS_BC,
                'IDENTITY': KinematicsType.IDENTITY,
            }
            restore_type = type_map.get(saved_name, KinematicsType.TRT_AC)
            if self.debug:
                print(f"[KINS] 从data记录恢复: {saved_name} -> {restore_type.name}")
        
        # 如果已经是目标类型，不需要重复切换
        if self.kinematics_type == restore_type and self.kinematics_type != KinematicsType.IDENTITY:
            if self.debug:
                print(f"[KINS] 已经是{restore_type.name}模式，跳过切换")
            return
        
        self.kinematics_type = restore_type
        self.data.kinematics_type = restore_type.name
        self.rtcp_enabled = True
        self.data.rtcp_enabled = True
        
        # ★ 修复: 重新创建运动学实例，使用保存的参数 ★
        # 获取保存的参数（如果之前有运动学实例）
        saved_params = {}
        if hasattr(self, '_saved_kinematics_params') and self._saved_kinematics_params:
            saved_params = self._saved_kinematics_params
        else:
            # 从当前work_offsets或默认值重建参数
            saved_params = {
                'rot_center_x': getattr(self, '_saved_rot_center_x', 0.0),
                'rot_center_y': getattr(self, '_saved_rot_center_y', 0.0),
                'rot_center_z': getattr(self, '_saved_rot_center_z', -300.0),
                'axis_offset_x': getattr(self, '_saved_axis_offset_x', 0.0),
                'axis_offset_y': getattr(self, '_saved_axis_offset_y', 0.0),
                'axis_offset_z': getattr(self, '_saved_axis_offset_z', 200.0),
                'tool_length': 100.0,  # 默认值
            }
        
        self.kinematics = KinematicsFactory.create(
            restore_type, debug=self.debug, **saved_params
        )
        
        if self.kinematics:
            self.kinematics.enable_rtcp(True)
            # 重新设置刀具长度
            if self.current_h_code > 0:
                tool_len = self.tool_lengths.get(self.current_h_code, 0.0)
                self.kinematics.set_tool_length(tool_len)
                if self.debug:
                    print(f"[KINS] 恢复刀具长度补偿: H{self.current_h_code} = {tool_len:.1f}mm")
            elif self.debug:
                print(f"[KINS] 注意: 无刀具长度补偿激活 (current_h_code={self.current_h_code})")
        
        # ===== 根据运动学类型恢复旋转轴 =====
        if hasattr(self, '_saved_rotation_a') and hasattr(self, '_saved_rotation_kinematics'):
            saved_kin = self._saved_rotation_kinematics
            
            if self.debug:
                print(f"[KINS] 保存时的运动学类型: {saved_kin.name}, 恢复为: {restore_type.name}")
            
            # ===== TRT_AC: 只有A轴和C轴 =====
            if restore_type == KinematicsType.TRT_AC:
                self.current_pos.a = self._saved_rotation_a
                self.current_pos.b = 0.0  # TRT_AC没有B轴，强制为0
                self.machine_b = 0.0
                self.current_pos.c = self._saved_rotation_c
                if self.debug:
                    print(f"[KINS] 恢复TRT_AC旋转轴: A={self._saved_rotation_a:.3f}, "
                          f"C={self._saved_rotation_c:.3f}, B=0(强制)")
            
            # ===== TRT_BC: 只有B轴和C轴 =====
            elif restore_type == KinematicsType.TRT_BC:
                self.current_pos.a = 0.0  # TRT_BC没有A轴，强制为0
                self.machine_a = 0.0
                self.current_pos.b = self._saved_rotation_b
                self.current_pos.c = self._saved_rotation_c
                if self.debug:
                    print(f"[KINS] 恢复TRT_BC旋转轴: B={self._saved_rotation_b:.3f}, "
                          f"C={self._saved_rotation_c:.3f}, A=0(强制)")
            
            # ===== FIVEAXIS_BC: B轴和C轴 =====
            elif restore_type == KinematicsType.FIVEAXIS_BC:
                self.current_pos.a = 0.0  # FIVEAXIS_BC没有A轴
                self.current_pos.b = self._saved_rotation_b
                self.current_pos.c = self._saved_rotation_c
                if self.debug:
                    print(f"[KINS] 恢复FIVEAXIS_BC旋转轴: B={self._saved_rotation_b:.3f}, "
                          f"C={self._saved_rotation_c:.3f}, A=0(强制)")
            
            # ===== MAXKINS_BC: A, B, C轴都有 =====
            elif restore_type == KinematicsType.MAXKINS_BC:
                self.current_pos.a = self._saved_rotation_a
                self.current_pos.b = self._saved_rotation_b
                self.current_pos.c = self._saved_rotation_c
                if self.debug:
                    print(f"[KINS] 恢复MAXKINS_BC旋转轴: A={self._saved_rotation_a:.3f}, "
                          f"B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}")
            
            # ===== IDENTITY或其他: 保持为0 =====
            else:
                self.current_pos.a = 0.0
                self.current_pos.b = 0.0
                self.current_pos.c = 0.0
                if self.debug:
                    print(f"[KINS] 恢复{restore_type.name}旋转轴: 全部清零")
        else:
            # 没有保存的旋转轴信息，全部清零
            self.current_pos.a = 0.0
            self.current_pos.b = 0.0
            self.current_pos.c = 0.0
            if self.debug:
                print(f"[KINS] 无保存的旋转轴信息，全部清零")
        
        # 重新同步机器坐标
        self._sync_machine_from_current()
        self._sync_parameters_from_state()
        
        if self.debug:
            if self.kinematics:
                world_pos = self.current_pos
                tool_len = getattr(self.kinematics, '_tool_length', 0.0)
                joints = self.kinematics.world_to_joints((
                    world_pos.x, world_pos.y, world_pos.z + tool_len,
                    world_pos.a, world_pos.b, world_pos.c
                ))
                print(f"[XYZAC] INV: world({world_pos.x:.3f},{world_pos.y:.3f},"
                      f"{world_pos.z + tool_len:.3f},{world_pos.a:.3f},{world_pos.c:.3f}) -> "
                      f"joints({joints[0]:.3f},{joints[1]:.3f},{joints[2]:.3f})")
            print(f"[KINS] 当前运动学: {restore_type.name}, RTCP: 启用")
    
    def _switch_to_fiveaxis_bc(self):
        """M430: 切换到五轴桥式铣床运动学 (FIVEAXIS_BC)"""
        if self.debug:
            print(f"[KINS] M430: 切换到五轴桥式铣床运动学 (FIVEAXIS_BC)")
        
        # 保存切换前的运动学类型，以便M428恢复
        # 关键修复：只在当前不是FIVEAXIS_BC时才保存
        if self.kinematics_type != KinematicsType.FIVEAXIS_BC:
            self._saved_kinematics_type = self.kinematics_type
            if self.debug:
                print(f"[KINS] 已保存原始运动学类型: {self._saved_kinematics_type.name}")
        
        # 切换到 FIVEAXIS_BC
        self.kinematics_type = KinematicsType.FIVEAXIS_BC
        self.data.kinematics_type = "FIVEAXIS_BC"
        self.rtcp_enabled = True
        self.data.rtcp_enabled = True
        
        # 创建新的运动学实例
        self.kinematics = KinematicsFactory.create(
            KinematicsType.FIVEAXIS_BC, debug=self.debug
        )
        
        if self.kinematics:
            self.kinematics.enable_rtcp(True)
            # 恢复刀具长度补偿
            if self.current_h_code > 0:
                tool_len = self.tool_lengths.get(self.current_h_code, 0.0)
                self.kinematics.set_tool_length(tool_len)
                if self.debug:
                    print(f"[KINS] 设置刀具长度: H{self.current_h_code} = {tool_len:.1f}mm")
        
        # 重新同步机器坐标
        self._sync_machine_from_current()
        self._sync_parameters_from_state()
        
        if self.debug:
            print(f"[KINS] 当前运动学: FIVEAXIS_BC, RTCP: 启用")

    
    # ==================== RTCP 控制 ====================    
    
    
    # ==================== RTCP 控制 ====================
    
    def set_rtcp_enable(self, enable: bool = True, with_tool_length: bool = True):
        """启用/禁用RTCP"""
        if self.kinematics is None:
            return
        
        old_rtcp = self.rtcp_enabled
        self.rtcp_enabled = enable
        self.data.rtcp_enabled = enable
        self.kinematics.enable_rtcp(enable)
        
        # ===== 新增：生成RTCP状态切换的segment =====
        if enable and not old_rtcp:
            segment = MoveSegment(
                type=MoveType.RTCP_ON,
                start=self.current_pos.copy(),
                end=self.current_pos.copy(),
                line_number=self.current_line,
                is_rtcp=True
            )
            self.data.segments.append(segment)
            self._point_count += 1
        elif not enable and old_rtcp:
            segment = MoveSegment(
                type=MoveType.RTCP_OFF,
                start=self.current_pos.copy(),
                end=self.current_pos.copy(),
                line_number=self.current_line,
                is_rtcp=False
            )
            self.data.segments.append(segment)
            self._point_count += 1
        
        self._sync_parameters_from_state()
    
    def set_tool_length_offset(self, h_code: int):
        """
        G43 H: 设置刀具长度补偿（同步到参数表）
        
        完全参照 LinuxCNC 官方实现：
        rs274ngc.cpp: Interp::convert_tool_length_offset
        
        Args:
            h_code: H 代码编号
        """
        self.current_h_code = h_code
        tool_len = self.tool_lengths.get(h_code, 0.0)
        
        # ★ 关键修复：设置 tool_offset.z ★
        self.tool_offset.z = tool_len
        
        # 同步刀具偏移到参数表
        if hasattr(self, 'var_manager') and self.var_manager:
            self.var_manager._tool_offset['Z'] = tool_len
            self.var_manager._tool_offset_active = True
        
        # 更新运动学实例的刀具长度
        if self.kinematics:
            self.kinematics.set_tool_length(tool_len)
            if self.debug:
                print(f"[TLO] 刀具长度补偿: H{h_code} = {tool_len:.1f}mm, 运动学已更新")
        else:
            if self.debug:
                print(f"[TLO] 刀具长度补偿: H{h_code} = {tool_len:.1f}mm (运动学实例为空, IDENTITY模式)")
    
    def cancel_tool_length_offset(self):
        """
        G49: 取消刀具长度补偿
        
        完全参照 LinuxCNC 官方实现：
        rs274ngc.cpp: Interp::convert_tool_length_offset (G49 分支)
        """
        self.current_h_code = 0
        self.tool_offset.z = 0.0
        
        if hasattr(self, 'var_manager') and self.var_manager:
            self.var_manager._tool_offset['Z'] = 0.0
            self.var_manager._tool_offset_active = False
        
        if self.kinematics:
            self.kinematics.set_tool_length(0.0)
            if self.debug:
                print(f"[TLO] 刀具长度补偿已取消 (G49)")
    
    # ==================== 刀具补偿 ====================

    def set_tool_radius_offset(self, d_code: int):
        """设置刀具半径补偿值（从刀具表读取）
        
        Args:
            d_code: D代码编号，对应刀具表中的刀具半径
        """
        self.current_d_code = d_code
        radius = self.tool_radii.get(d_code, 0.0)
        self.comp_radius = radius
        
        # 同步刀具半径到参数表
        self.var_manager._cutter_comp_radius = radius
        
        if self.debug:
            print(f"[COMP] D{d_code} 刀具半径设置为: {radius:.3f} mm")
    
    def cancel_tool_radius_offset(self):
        """取消刀具半径补偿（G40）"""
        self.current_d_code = 0
        self.comp_radius = 0.0
        self.var_manager._cutter_comp_radius = 0.0
        
        if self.debug:
            print("[COMP] 刀具半径补偿已取消")

    def set_cutter_compensation_state(self, comp_type: CompType, radius: float = 0.0,
                                    side: int = 0, d_word: float = 0.0):
        """设置刀具半径补偿（同步到参数表）"""
        old_type = self.comp_type
        self.comp_type = comp_type
        self.comp_radius = radius
        self.d_word = d_word
        self.cutter_comp_active = (comp_type != CompType.OFF)
        
        # ========== 同步到参数表 ==========
        self.var_manager.set_cutter_compensation_active(self.cutter_comp_active)
        
        if comp_type == CompType.OFF and old_type != CompType.OFF:
            self.cutter_comp_firstmove = True
            self.arc_not_allowed = True
        elif comp_type != CompType.OFF and old_type == CompType.OFF:
            self.cutter_comp_firstmove = True
            self.arc_not_allowed = False
            self.program_x = self.current_pos.x
            self.program_y = self.current_pos.y
            self.program_z = self.current_pos.z
    
  
    def cancel_cutter_compensation(self):
        """G40: 取消刀具半径补偿"""
        self.set_cutter_compensation_state(CompType.OFF, 0.0, 0, 0.0)
    
    # ==================== 运动段添加 ====================
    
    def _add_segment_remove(self, move_type: MoveType, end_world: Point6D, **kwargs):
        """添加运动段"""
        if self._point_count >= self.max_points:
            return
        
        start_world = self.current_pos.copy()
        linear_distance = start_world.distance_to(end_world)
        
        if move_type in [MoveType.RAPID, MoveType.FEED]:
            feed_for_calc = self.feedrate if move_type == MoveType.FEED else None
            result = self.planner.calculate_time(linear_distance, feed_for_calc, 
                                                  is_rapid=(move_type == MoveType.RAPID))
        elif move_type == MoveType.DWELL:
            dwell_time = kwargs.get('dwell_time', 0)
            result = {'duration': dwell_time, 'max_velocity': 0, 'acceleration_time': 0,
                     'constant_time': 0, 'deceleration_time': 0, 'profile_type': 'dwell'}
        else:
            result = {'duration': 0.1, 'max_velocity': 0, 'acceleration_time': 0,
                     'constant_time': 0, 'deceleration_time': 0, 'profile_type': 'other'}
        
        old_machine_x = self.machine_x
        old_machine_y = self.machine_y
        old_machine_z = self.machine_z
        old_machine_a = self.machine_a
        old_machine_b = self.machine_b
        old_machine_c = self.machine_c
        
        self.current_pos = end_world.copy()
        self._sync_machine_from_current()
        
        machine_start = Point6D(old_machine_x, old_machine_y, old_machine_z,
                                old_machine_a, old_machine_b, old_machine_c)
        machine_end = Point6D(self.machine_x, self.machine_y, self.machine_z,
                              self.machine_a, self.machine_b, self.machine_c)
        
        segment = MoveSegment(
            type=move_type, start=machine_start, end=machine_end,
            line_number=self.current_line,
            feedrate=self.feedrate if move_type == MoveType.FEED else 0,
            comp_type=self.comp_type, comp_radius=self.comp_radius, d_word=self.d_word,
            duration=result['duration'], max_velocity=result['max_velocity'],
            acceleration_time=result['acceleration_time'],
            constant_time=result['constant_time'],
            deceleration_time=result['deceleration_time'],
            profile_type=result['profile_type'],
            is_rtcp=self.rtcp_enabled,
            world_start=start_world.copy(), world_end=end_world.copy(),
            tool_number=self.state.tool,
            spindle_speed=self.state.spindle_speed,
            spindle_state=self.state.spindle_state,
            **{k: v for k, v in kwargs.items() 
               if k in ['center_x', 'center_y', 'center_z', 'radius', 'turn', 'dwell_time']}
        )
        
        self.data.segments.append(segment)
        self.data.total_length += linear_distance
        self.cumulative_time += result['duration']
        self._point_count += 1
        self._sync_parameters_from_state()
    
    # ==================== 运动方法 ====================
    
    def straight_traverse(self, x: float, y: float, z: float,
                          a: float, b: float, c: float, u: float, v: float, w: float):
        """G0 快速移动 - 使用工件坐标记录"""
        if self._g53_active:
            self._handle_g53_move(x, y, z, a, b, c, u, v, w, MoveType.RAPID)
            return
        
        # 处理绝对/相对坐标
        if not self.is_absolute:
            x = self.current_pos.x + (x if x is not None else 0)
            y = self.current_pos.y + (y if y is not None else 0)
            z = self.current_pos.z + (z if z is not None else 0)
            a = self.current_pos.a + (a if a is not None else 0)
            b = self.current_pos.b + (b if b is not None else 0)
            c = self.current_pos.c + (c if c is not None else 0)
        else:
            x = x if x is not None else self.current_pos.x
            y = y if y is not None else self.current_pos.y
            z = z if z is not None else self.current_pos.z
            a = a if a is not None else self.current_pos.a
            b = b if b is not None else self.current_pos.b
            c = c if c is not None else self.current_pos.c
        
        # 创建工件坐标目标点
        end = Point6D(x, y, z, a, b, c)
        
        # 使用统一的运动段添加方法
        self._add_world_segment(MoveType.RAPID, end)
        
        # 更新 GLCanonPure 需要的坐标
        self.lo = [x, y, z, a, b, c, u, v, w]
    
    def straight_feed(self, x: float, y: float, z: float,
                      a: float, b: float, c: float, u: float, v: float, w: float):
        """G1 直线进给 - 使用工件坐标记录"""
        if self._g53_active:
            self._handle_g53_move(x, y, z, a, b, c, u, v, w, MoveType.FEED)
            return
        
        # 处理绝对/相对坐标
        if not self.is_absolute:
            x = self.current_pos.x + (x if x is not None else 0)
            y = self.current_pos.y + (y if y is not None else 0)
            z = self.current_pos.z + (z if z is not None else 0)
            a = self.current_pos.a + (a if a is not None else 0)
            b = self.current_pos.b + (b if b is not None else 0)
            c = self.current_pos.c + (c if c is not None else 0)
        else:
            x = x if x is not None else self.current_pos.x
            y = y if y is not None else self.current_pos.y
            z = z if z is not None else self.current_pos.z
            a = a if a is not None else self.current_pos.a
            b = b if b is not None else self.current_pos.b
            c = c if c is not None else self.current_pos.c
        
        # 创建工件坐标目标点
        end = Point6D(x, y, z, a, b, c)
        
        # 处理刀具补偿
        if self.cutter_comp_active and self.comp_radius > 0:
            comp_x, comp_y, comp_z = self._compensate_straight(end.x, end.y, end.z)
            end = Point6D(comp_x, comp_y, comp_z, end.a, end.b, end.c)
        
        # 使用统一的运动段添加方法
        self._add_world_segment(MoveType.FEED, end)
        
        # 更新 GLCanonPure 需要的坐标
        self.lo = [x, y, z, a, b, c, u, v, w]
    

    def _handle_g53_move(self, x: float, y: float, z: float,
                          a: float, b: float, c: float,
                          u: float, v: float, w: float, move_type: MoveType):
        """处理G53模式下的运动
        
        G53使用机床坐标系，刀具长度补偿(TLO)在G53模式下已被取消
        （LinuxCNC标准行为：G53自动取消刀具长度补偿）
        """
        old_mx, old_my, old_mz = self.machine_x, self.machine_y, self.machine_z
        old_ma, old_mb, old_mc = self.machine_a, self.machine_b, self.machine_c
        
        self.machine_x = x if x is not None else self.machine_x
        self.machine_y = y if y is not None else self.machine_y
        self.machine_z = z if z is not None else self.machine_z
        self.machine_a = a if a is not None else self.machine_a
        self.machine_b = b if b is not None else self.machine_b
        self.machine_c = c if c is not None else self.machine_c
        
        # G53模式下，临时清零刀具长度补偿
        saved_tool_offset_z = self.tool_offset.z
        self.tool_offset.z = 0.0
        
        self._update_current_from_machine()
        
        # 恢复刀具长度补偿
        self.tool_offset.z = saved_tool_offset_z
        
        end = self.current_pos.copy()
        
        machine_start = Point6D(old_mx, old_my, old_mz, old_ma, old_mb, old_mc)
        machine_end = Point6D(self.machine_x, self.machine_y, self.machine_z,
                              self.machine_a, self.machine_b, self.machine_c)
        
        segment = MoveSegment(
            type=move_type, start=machine_start, end=machine_end,
            line_number=self.current_line,
            feedrate=self.feedrate if move_type == MoveType.FEED else 0,
            duration=0.1, profile_type='g53',
            is_rtcp=False, world_start=self.current_pos.copy(), world_end=end.copy(),
            tool_number=self.state.tool
        )
        self.data.segments.append(segment)
        self._g53_active = False
        self._sync_parameters_from_state()



    def arc_feed_bak(self, x: float, y: float, z: float,
                 a: float, b: float, c: float,
                 i: float, j: float, k: float, turn: int, feed: float,
                 u: float = 0.0, v: float = 0.0, w: float = 0.0):
        """
        G2/G3 圆弧进给
        
        参数说明:
            x, y, z: 圆弧终点坐标 (工件坐标系)
            a, b, c: 终点旋转轴角度
            i, j, k: 圆心相对于起点的偏移量 (INCREMENTAL IJK)
            turn: 圈数 (负数=G2顺时针, 正数=G3逆时针)
            feed: 进给率 (mm/min)
        
        修复说明:
            I, J, K 始终作为相对于起点的增量处理
            圆心 = 起点 + 偏移量
        """
        start_x, start_y, start_z = self.current_pos.x, self.current_pos.y, self.current_pos.z
        
        # 处理绝对/相对坐标
        if not self.is_absolute:
            x = self.current_pos.x + x
            y = self.current_pos.y + y
            z = self.current_pos.z + z
            a = self.current_pos.a + (a if a is not None else 0)
            b = self.current_pos.b + (b if b is not None else 0)
            c = self.current_pos.c + (c if c is not None else 0)
        else:
            x = x if x is not None else self.current_pos.x
            y = y if y is not None else self.current_pos.y
            z = z if z is not None else self.current_pos.z
            a = a if a is not None else self.current_pos.a
            b = b if b is not None else self.current_pos.b
            c = c if c is not None else self.current_pos.c
        
        # ===== 关键修复：圆心计算 =====
        # I, J, K 始终是相对于起点的增量偏移量
        # 圆心坐标 = 起点坐标 + 偏移量
        i_val = i if i is not None else 0.0
        j_val = j if j is not None else 0.0
        k_val = k if k is not None else 0.0
        
        cx = start_x + i_val
        cy = start_y + j_val
        cz = start_z + k_val
        
        # 计算半径
        radius = math.hypot(i_val, j_val)
        
        if self.debug:
            print(f"[ARC] 起点=({start_x:.3f},{start_y:.3f}) "
                  f"圆心=({cx:.3f},{cy:.3f}) "
                  f"I={i_val:.3f} J={j_val:.3f} R={radius:.3f}")
        
        # 验证终点在圆上
        # 验证终点在圆上（对照 LinuxCNC arc_data_ijk 中的容差检查）
        end_dist = math.hypot(x - cx, y - cy)
        # 使用与 LinuxCNC 一致的容差（spiral_abs_tolerance）
        spiral_tolerance = 0.01  # mm，对于公制单位
        if abs(end_dist - radius) > spiral_tolerance * 100.0:
            # 相对误差检查
            rel_err = abs(end_dist - radius) / max(radius, end_dist) if max(radius, end_dist) > 0 else 0
            if rel_err > 0.001 and abs(end_dist - radius) > spiral_tolerance:
                if self.debug:
                    print(f"[ARC] ⚠ 终点({x:.3f},{y:.3f})不在圆上: "
                        f"距圆心={end_dist:.6f}, R={radius:.6f}, "
                        f"绝对误差={abs(end_dist - radius):.6f}mm, "
                        f"相对误差={rel_err:.6f}")
                # 将终点调整到圆上（容错处理）
                if end_dist > TINY:
                    angle = math.atan2(y - cy, x - cx)
                    x = cx + radius * math.cos(angle)
                    y = cy + radius * math.sin(angle)
                    if self.debug:
                        print(f"[ARC] 调整终点到圆上: ({x:.3f},{y:.3f})")
            else:
                if self.debug:
                    print(f"[ARC] 终点误差在容差范围内，不调整")
        
        # 创建终点
        end = Point6D(x, y, z, a, b, c)
        
        # 保存原始进给率
        original_feedrate = self.feedrate
        if feed > 0:
            self.feedrate = feed
        
        # 计算弧长
        arc_length = abs(turn) * 2.0 * math.pi * radius
        result = self.planner.calculate_time(arc_length, self.feedrate, is_rapid=False)
        
        # 机器坐标
        machine_start = Point6D(self.machine_x, self.machine_y, self.machine_z,
                                self.machine_a, self.machine_b, self.machine_c)
        
        # 计算终点关节坐标
        if self.rtcp_enabled and self.kinematics is not None:
            wx = x + self.work_offset_x + self.g5x_offset_x + self.g92_offset_x + self.g52_offset_x
            wy = y + self.work_offset_y + self.g5x_offset_y + self.g92_offset_y + self.g52_offset_y
            wz = z + self.work_offset_z + self.g5x_offset_z + self.g92_offset_z + self.g52_offset_z
            wa = a + self.work_offset_a + self.g5x_offset_a + self.g92_offset_a + self.g52_offset_a
            wb = b + self.work_offset_b + self.g5x_offset_b + self.g92_offset_b + self.g52_offset_b
            wc = c + self.work_offset_c + self.g5x_offset_c + self.g92_offset_c + self.g52_offset_c
            
            world_tuple = (wx, wy, wz, wa, wb, wc)
            joints = self.kinematics.world_to_joints(world_tuple)
            # 使用统一的关节映射函数
            end_joints = joints_to_point6d(joints, self.kinematics_type)
        else:
            end_joints = Point6D(x, y, z, a, b, c)
        
        machine_end = Point6D(
            self.machine_x + (end_joints.x - self.current_pos.x),
            self.machine_y + (end_joints.y - self.current_pos.y),
            self.machine_z + (end_joints.z - self.current_pos.z),
            end_joints.a, end_joints.b, end_joints.c
        )
        
        # 创建圆弧运动段
        arc_segment = MoveSegment(
            type=MoveType.ARC_CW if turn < 0 else MoveType.ARC_CCW,
            start=machine_start, end=machine_end,
            line_number=self.current_line, feedrate=self.feedrate,
            center_x=cx, center_y=cy, center_z=cz,
            radius=radius, turn=turn,
            comp_type=self.comp_type, comp_radius=self.comp_radius, d_word=self.d_word,
            duration=result['duration'], max_velocity=result['max_velocity'],
            acceleration_time=result['acceleration_time'],
            constant_time=result['constant_time'],
            deceleration_time=result['deceleration_time'],
            profile_type=result['profile_type'],
            is_rtcp=self.rtcp_enabled,
            world_start=self.current_pos.copy(), world_end=end.copy(),
            tool_number=self.state.tool,
            spindle_speed=self.state.spindle_speed,
            spindle_state=self.state.spindle_state
        )
        self.data.segments.append(arc_segment)
        self.data.total_length += arc_length
        self.cumulative_time += result['duration']
        
        # 更新当前位置
        self.current_pos = end_joints.copy()
        self.machine_x = machine_end.x
        self.machine_y = machine_end.y
        self.machine_z = machine_end.z
        self.machine_a = machine_end.a
        self.machine_b = machine_end.b
        self.machine_c = machine_end.c
        
        # 恢复进给率
        self.feedrate = original_feedrate
        self._sync_parameters_from_state()



    def arc_feed(self, x: float, y: float, z: float,
                 a: float, b: float, c: float,
                 i: float, j: float, k: float, turn: int, feed: float,
                 u: float = 0.0, v: float = 0.0, w: float = 0.0):
        """
        G2/G3 圆弧进给 - 基于参数表进行坐标管理
        
        参照 LinuxCNC interp_convert.cc convert_arc2() 的逻辑：
        1. 在工件坐标系中计算圆心
        2. 验证圆弧几何（参照 arc_data_ijk 的容差检查）
        3. 创建 MoveSegment（使用工件坐标）
        4. 更新当前位置（工件坐标）
        
        参数:
            x, y, z: 圆弧终点（工件坐标）
            a, b, c: 终点旋转轴角度（工件坐标）
            i, j, k: 圆心相对起点的偏移（增量，工件坐标）
            turn: 圈数（负数=G2顺时针，正数=G3逆时针）
            feed: 进给率 (mm/min)
        """
        # ===== 第1步：获取起点（工件坐标）=====
        start_x = self.current_pos.x
        start_y = self.current_pos.y
        start_z = self.current_pos.z
        start_a = self.current_pos.a
        start_b = self.current_pos.b
        start_c = self.current_pos.c
        
        # ===== 第2步：处理绝对/相对坐标 =====
        if not self.is_absolute:
            x = start_x + (x if x is not None else 0)
            y = start_y + (y if y is not None else 0)
            z = start_z + (z if z is not None else 0)
            a = start_a + (a if a is not None else 0)
            b = start_b + (b if b is not None else 0)
            c = start_c + (c if c is not None else 0)
        else:
            x = x if x is not None else start_x
            y = y if y is not None else start_y
            z = z if z is not None else start_z
            a = a if a is not None else start_a
            b = b if b is not None else start_b
            c = c if c is not None else start_c
        
        # ===== 第3步：计算圆心（工件坐标系）=====
        # 参照 LinuxCNC arc_data_ijk：IJK 始终是相对于起点的增量
        i_val = i if i is not None else 0.0
        j_val = j if j is not None else 0.0
        k_val = k if k is not None else 0.0
        
        # 圆心 = 起点 + IJK 偏移
        cx = start_x + i_val
        cy = start_y + j_val
        cz = start_z + k_val
        
        # 计算半径
        radius = math.hypot(i_val, j_val)
        
        if self.debug:
            print(f"[ARC] 起点=({start_x:.3f},{start_y:.3f}) "
                  f"圆心=({cx:.3f},{cy:.3f}) "
                  f"I={i_val:.3f} J={j_val:.3f} R={radius:.3f}")
        
        # ===== 第4步：验证圆弧几何（参照 LinuxCNC 容差）=====
        units = getattr(self.state, 'units', 21)
        is_valid, msg, error = self.var_manager.validate_arc_geometry(
            start_x, start_y, x, y, cx, cy, radius, units
        )
        
        if not is_valid and '零半径' in msg:
            if self.debug:
                print(f"[ARC] ⚠ {msg}")
            return
        
        if not is_valid:
            if self.debug:
                print(f"[ARC] ⚠ {msg}")
            # 容错处理：将终点调整到圆上
            end_radius = math.hypot(x - cx, y - cy)
            if end_radius > TINY:
                angle = math.atan2(y - cy, x - cx)
                x = cx + radius * math.cos(angle)
                y = cy + radius * math.sin(angle)
                if self.debug:
                    print(f"[ARC] 调整终点到圆上: ({x:.6f}, {y:.6f})")
        
        # ===== 第5步：计算弧长和速度曲线 =====
        arc_length = abs(turn) * 2.0 * math.pi * radius
        
        # 保存当前进给率
        original_feedrate = self.feedrate
        if feed > 0:
            self.feedrate = feed
        
        result = self.planner.calculate_time(arc_length, self.feedrate, is_rapid=False)
        
        # ===== 第6步：创建工件坐标和绝对坐标点 =====
        world_start = Point6D(start_x, start_y, start_z, start_a, start_b, start_c)
        world_end = Point6D(x, y, z, a, b, c)
        
        # 通过参数表计算绝对坐标
        abs_start = self.var_manager.program_to_absolute(
            start_x, start_y, start_z, start_a, start_b, start_c
        )
        abs_end = self.var_manager.program_to_absolute(x, y, z, a, b, c)
        
        abs_start_pt = Point6D(
            abs_start[0], abs_start[1], abs_start[2],
            abs_start[3], abs_start[4], abs_start[5]
        )
        abs_end_pt = Point6D(
            abs_end[0], abs_end[1], abs_end[2],
            abs_end[3], abs_end[4], abs_end[5]
        )
        
        # ===== 第7步：创建 MoveSegment =====
        arc_segment = MoveSegment(
            type=MoveType.ARC_CW if turn < 0 else MoveType.ARC_CCW,
            start=world_start.copy(),
            end=world_end.copy(),
            line_number=self.current_line,
            feedrate=self.feedrate,
            center_x=cx,
            center_y=cy,
            center_z=cz,
            radius=radius,
            turn=turn,
            comp_type=self.comp_type,
            comp_radius=self.comp_radius,
            d_word=self.d_word,
            duration=result['duration'],
            max_velocity=result['max_velocity'],
            acceleration_time=result['acceleration_time'],
            constant_time=result['constant_time'],
            deceleration_time=result['deceleration_time'],
            profile_type=result['profile_type'],
            is_rtcp=self.rtcp_enabled,
            world_start=world_start.copy(),
            world_end=world_end.copy(),
            machine_start=abs_start_pt,
            machine_end=abs_end_pt,
            tool_number=self.state.tool,
            spindle_speed=self.state.spindle_speed,
            spindle_state=self.state.spindle_state,
            coord_system='WORLD',
            active_csys=self.var_manager.active_csys,
            g92_active=self.var_manager._g92_active,
            gcode=f"G{2 if turn < 0 else 3}"
        )
        
        # ===== 第8步：添加到路径数据 =====
        self.data.segments.append(arc_segment)
        self.data.total_length += arc_length
        self.cumulative_time += result['duration']
        self._point_count += 1
        
        # 更新 GLCanonPure 需要的坐标
        self.lo = [x, y, z, a, b, c, u, v, w]
        
        # ===== 第9步：更新工件坐标位置 =====
        self.current_pos = world_end.copy()
        
        # ===== 第10步：同步到参数表 =====
        self._sync_position_to_params()
        
        # ===== 第11步：同步机器坐标 =====
        self._sync_machine_from_current()
        
        # ===== 第12步：恢复进给率 =====
        self.feedrate = original_feedrate
        
        # ===== 第13步：同步所有参数 =====
        self._sync_parameters_from_state()
        
        # ===== 第14步：标记运动段类型 =====
        self._last_segment_was_arc = True
  
    def dwell(self, seconds: float):
        """G4 暂停 - 使用工件坐标记录"""
        self._add_world_segment(
            MoveType.DWELL,
            self.current_pos.copy(),
            dwell_time=seconds
        )
    
    # ==================== 刀具补偿辅助 ====================
    
    def _compensate_straight(self, end_x: float, end_y: float, end_z: float) -> Tuple[float, float, float]:
        """直线刀具半径补偿"""
        if not self.cutter_comp_active or self.comp_radius <= 0:
            return (end_x, end_y, end_z)
        
        if self.cutter_comp_firstmove:
            dx = end_x - self.program_x
            dy = end_y - self.program_y
            length = math.hypot(dx, dy)
            
            if length > CART_FUZZ:
                angle = math.atan2(dy, dx)
                factor = 1.0 if self.comp_type == CompType.LEFT else -1.0
                alpha = angle + factor * (math.pi / 2)
                comp_end_x = end_x + self.comp_radius * math.cos(alpha)
                comp_end_y = end_y + self.comp_radius * math.sin(alpha)
            else:
                comp_end_x, comp_end_y = end_x, end_y
            
            self.cutter_comp_firstmove = False
            self.program_x = end_x
            self.program_y = end_y
            self.program_z = end_z
            return (comp_end_x, comp_end_y, end_z)
        else:
            dx = end_x - self.program_x
            dy = end_y - self.program_y
            length = math.hypot(dx, dy)
            
            if length > CART_FUZZ:
                ux, uy = dx / length, dy / length
                factor = 1.0 if self.comp_type == CompType.LEFT else -1.0
                perp_x, perp_y = -uy * factor, ux * factor
                comp_end_x = end_x + self.comp_radius * perp_x
                comp_end_y = end_y + self.comp_radius * perp_y
            else:
                comp_end_x, comp_end_y = end_x, end_y
            
            self.program_x = end_x
            self.program_y = end_y
            self.program_z = end_z
            return (comp_end_x, comp_end_y, end_z)
    
    def _world_to_joints(self, world_pos: Point6D) -> Point6D:
        """工件坐标转关节坐标"""
        if not self.rtcp_enabled or self.kinematics is None:
            return world_pos
        
        wx = world_pos.x + self.work_offset_x + self.g5x_offset_x + self.g92_offset_x + self.g52_offset_x
        wy = world_pos.y + self.work_offset_y + self.g5x_offset_y + self.g92_offset_y + self.g52_offset_y
        wz = world_pos.z + self.work_offset_z + self.g5x_offset_z + self.g92_offset_z + self.g52_offset_z
        wa = world_pos.a + self.work_offset_a + self.g5x_offset_a + self.g92_offset_a + self.g52_offset_a
        wb = world_pos.b + self.work_offset_b + self.g5x_offset_b + self.g92_offset_b + self.g52_offset_b
        wc = world_pos.c + self.work_offset_c + self.g5x_offset_c + self.g92_offset_c + self.g52_offset_c
        
        world_tuple = (wx, wy, wz, wa, wb, wc)
        joints = self.kinematics.world_to_joints(world_tuple)
        
        return joints_to_point6d(joints, self.kinematics_type)
    
    def _joints_to_world(self, joints_pos: Point6D) -> Point6D:
        """关节坐标转工件坐标"""
        if not self.rtcp_enabled or self.kinematics is None:
            return joints_pos
        
        # 构建符合运动学类型的关节数组
        config = KINEMATICS_JOINT_CONFIG.get(self.kinematics_type, {})
        num_joints = config.get('num_joints', 6)
        joints = [0.0] * num_joints
        
        x_idx = config.get('x_idx', 0)
        y_idx = config.get('y_idx', 1)
        z_idx = config.get('z_idx', 2)
        a_idx = config.get('a_idx', 3)
        b_idx = config.get('b_idx', 4)
        c_idx = config.get('c_idx', 5)
        
        if x_idx >= 0: joints[x_idx] = joints_pos.x
        if y_idx >= 0: joints[y_idx] = joints_pos.y
        if z_idx >= 0: joints[z_idx] = joints_pos.z
        if a_idx >= 0: joints[a_idx] = joints_pos.a
        if b_idx >= 0: joints[b_idx] = joints_pos.b
        if c_idx >= 0: joints[c_idx] = joints_pos.c
        
        world = self.kinematics.joints_to_world(joints)
        
        wx = world[0] - self.work_offset_x - self.g5x_offset_x - self.g92_offset_x - self.g52_offset_x
        wy = world[1] - self.work_offset_y - self.g5x_offset_y - self.g92_offset_y - self.g52_offset_y
        wz = world[2] - self.work_offset_z - self.g5x_offset_z - self.g92_offset_z - self.g52_offset_z
        wa = world[3] - self.work_offset_a - self.g5x_offset_a - self.g92_offset_a - self.g52_offset_a
        wb = world[4] - self.work_offset_b - self.g5x_offset_b - self.g92_offset_b - self.g52_offset_b
        wc = world[5] - self.work_offset_c - self.g5x_offset_c - self.g92_offset_c - self.g52_offset_c
        
        return Point6D(wx, wy, wz, wa, wb, wc)
    
    # ==================== 其他方法 ====================
    
    def set_feed_rate(self, feed: float):
        self.feedrate = feed
        self.state.feedrate = feed
        self._sync_parameters_from_state()
    
    def set_spindle_speed(self, speed: float):
        self.state.spindle_speed = speed
        self._sync_parameters_from_state()
    
    def change_tool(self, tool: int):
        if tool not in self.tool_table:
            self.tool_table[tool] = ToolData(tool_number=tool)
        self.current_tool = self.tool_table[tool]
        self.state.tool = tool
        self.current_pocket = tool
        
        offsets = {'x': 0.0, 'y': 0.0, 'z': self.tool_lengths.get(tool, 0.0),
                   'a': 0.0, 'b': 0.0, 'c': 0.0,
                   'u': 0.0, 'v': 0.0, 'w': 0.0}
        self.var_manager.update_tool_params(tool, offsets, 
                                           self.current_tool.diameter)
        self._sync_parameters_from_state()
        self.data.tool_changes.append({'tool': tool, 'line': self.current_line,
                                       'position': self.current_pos.to_list()})
    
    def select_tool(self, tool: int):
        self.selected_tool = tool
        self.selected_pocket = tool
    
    def set_absolute_mode(self):
        self.is_absolute = True
        self.state.distance_mode = 90
        self._sync_parameters_from_state()
    
    def set_relative_mode(self):
        self.is_absolute = False
        self.state.distance_mode = 91
        self._sync_parameters_from_state()
    
    def set_plane(self, plane: int):
        self.current_plane = plane
        self.plane = plane
        self.state.plane = plane
        self._sync_parameters_from_state()
    
    def spindle_control(self, mode: int):
        self.state.spindle_mode = mode
        if mode == 1:
            self.state.spindle_state = "CW"
        elif mode == 2:
            self.state.spindle_state = "CCW"
        elif mode == 0:
            self.state.spindle_state = "OFF"
        self._sync_parameters_from_state()
    
    def set_flood(self, enable: bool):
        self.state.coolant_flood = enable
        self._sync_parameters_from_state()
    
    def set_mist(self, enable: bool):
        self.state.coolant_mist = enable
        self._sync_parameters_from_state()
    
    def program_stop(self):
        pass
    
    def optional_stop(self):
        pass
    
    def program_end(self):
        """程序结束 - 使用工件坐标记录"""
        self._add_world_segment(
            MoveType.PROGRAM_END,
            self.current_pos.copy(),
            dwell_time=0.1
        )
    
    def set_g28(self, x: float, y: float, z: float, a: float = 0.0, b: float = 0.0, c: float = 0.0):
        self.g28_ref = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c}
        self.var_manager.set_g28(x, y, z, a, b, c)
    
    def set_g30(self, x: float, y: float, z: float, a: float = 0.0, b: float = 0.0, c: float = 0.0):
        self.g30_ref = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c}
        self.var_manager.set_g30(x, y, z, a, b, c)
    
    def get_g28(self) -> Tuple[float, float, float]:
        return (self.g28_ref.get('X', 0.0), self.g28_ref.get('Y', 0.0), self.g28_ref.get('Z', 0.0))
    
    def get_g30(self) -> Tuple[float, float, float]:
        return (self.g30_ref.get('X', 0.0), self.g30_ref.get('Y', 0.0), self.g30_ref.get('Z', 0.0))
    
    def save_state(self, restore_on_return: bool = False) -> Dict:
        return {
            'motion_mode': self.motion_mode,
            'plane': self.current_plane,
            'distance_mode': 90 if self.is_absolute else 91,
            'current_pos': self.current_pos.copy(),
            'feedrate': self.feedrate,
            'tool': self.state.tool,
            'restore_on_return': restore_on_return
        }
    
    def restore_state(self, saved_state: Dict = None):
        if saved_state is None:
            return
        
        if 'g_codes' in saved_state:
            g = saved_state['g_codes']
            self.current_plane = g.get('plane', self.current_plane)
            self.is_absolute = (g.get('distance_mode', 90) == 90)
        
        if 'positions' in saved_state:
            p = saved_state['positions']
            self.current_pos = Point6D(
                p.get('x', self.current_pos.x), p.get('y', self.current_pos.y),
                p.get('z', self.current_pos.z), p.get('a', self.current_pos.a),
                p.get('b', self.current_pos.b), p.get('c', self.current_pos.c)
            )
            self.machine_x = p.get('machine_x', self.machine_x)
            self.machine_y = p.get('machine_y', self.machine_y)
            self.machine_z = p.get('machine_z', self.machine_z)
        
        self.feedrate = saved_state.get('feedrate', self.feedrate)
        self.state.tool = saved_state.get('tool', self.state.tool)
        self._sync_parameters_from_state()
    
    def next_line(self, st):
        if hasattr(st, 'sequence_number'):
            self.current_line = st.sequence_number
            self.lineno = st.sequence_number
        if hasattr(st, 'plane'):
            self.current_plane = st.plane
            self.plane = st.plane
            self.state.plane = st.plane
        self._sync_parameters_from_state()
    
    def get_total_time(self) -> float:
        return self.cumulative_time


# ==================== CNCKernel 扩展方法 ====================

class CNCKernelExtensions:
    """CNCKernel 类的扩展方法集合"""
    
    @staticmethod
    def add_power_off(kernel_instance):
        def power_off(self) -> Dict:
            if not self.ensure_operational():
                return {"success": False, "message": f"机床未就绪"}
            if self.state_machine.request_power_off():
                self.is_running = False
                self.is_paused = False
                self.status.state = MachineState.OFF.value
                self.status.spindle_state = "OFF"
                self.status.spindle_speed = 0
                self.status.coolant_flood = False
                self.status.coolant_mist = False
                self._notify_update()
                return {"success": True, "message": "断电成功"}
            return {"success": False, "message": "无法断电"}
        kernel_instance.power_off = power_off.__get__(kernel_instance, type(kernel_instance))

    @staticmethod
    def add_home_all(kernel_instance):
        def home_all(self) -> Dict:
            if not self.ensure_operational():
                return {"success": False, "message": "机床未就绪"}
            self.state_machine.home_all()
            self.status.state = MachineState.HOMING.value
            self._notify_update()
            
            def home_motion():
                for axis, current in [('X', self.status.position_x), ('Y', self.status.position_y),
                                       ('Z', self.status.position_z), ('A', self.status.position_a),
                                       ('B', self.status.position_b), ('C', self.status.position_c)]:
                    if abs(current) < 0.001:
                        continue
                    step = current / 20
                    for _ in range(20):
                        if axis == 'X': self.status.position_x -= step
                        elif axis == 'Y': self.status.position_y -= step
                        elif axis == 'Z': self.status.position_z -= step
                        elif axis == 'A': self.status.position_a -= step
                        elif axis == 'B': self.status.position_b -= step
                        elif axis == 'C': self.status.position_c -= step
                        self._update_world_position()
                        self._notify_update()
                        time.sleep(0.05)
                    if axis == 'X': self.status.position_x = 0.0
                    elif axis == 'Y': self.status.position_y = 0.0
                    elif axis == 'Z': self.status.position_z = 0.0
                    elif axis == 'A': self.status.position_a = 0.0
                    elif axis == 'B': self.status.position_b = 0.0
                    elif axis == 'C': self.status.position_c = 0.0
                self.state_machine.transition_to(MachineStateMachine.STATE_IDLE)
                self.status.state = MachineState.IDLE.value
                self._notify_update()
            
            threading.Thread(target=home_motion, daemon=True).start()
            return {"success": True, "message": "回零中..."}
        kernel_instance.home_all = home_all.__get__(kernel_instance, type(kernel_instance))

    @staticmethod
    def add_jog(kernel_instance):
        """添加点动功能 - 支持RTCP刀尖跟随
        
        坐标变换链（与_set_workpiece_position保持一致）：
        工件坐标 → +G92偏移 → +XY旋转 → +G5x偏移 → +TLO → +G52偏移 → 逆运动学 → 关节坐标
        
        参数:
            axis: 轴名称 ('X','Y','Z','A','B','C')
            direction: 方向 (1=正向, -1=负向)
            distance: 移动距离 (mm或度)
        
        返回:
            dict: {'success': bool, 'message': str, 'position': dict}
        """
        def jog(self, axis: str, direction: int, distance: float) -> Dict:
            """执行点动操作"""
            if not self.ensure_operational():
                return {"success": False, "message": "机床未就绪"}
            
            axis_upper = axis.upper()
            
            # 验证轴名称
            valid_axes = {'X', 'Y', 'Z', 'A', 'B', 'C'}
            if axis_upper not in valid_axes:
                return {"success": False, "message": f"无效轴: {axis}, 有效轴: {valid_axes}"}
            
            # 计算移动量
            step = direction * distance
            
            # 记录移动前坐标（用于调试）
            old_pos = {
                'x': self.status.position_x,
                'y': self.status.position_y,
                'z': self.status.position_z,
                'a': self.status.position_a,
                'b': self.status.position_b,
                'c': self.status.position_c,
            }
            old_machine = {
                'x': self.status.machine_x,
                'y': self.status.machine_y,
                'z': self.status.machine_z,
                'a': self.status.machine_a,
                'b': self.status.machine_b,
                'c': self.status.machine_c,
            }
            
            # 计算目标工件坐标
            target_x = self.status.position_x
            target_y = self.status.position_y
            target_z = self.status.position_z
            target_a = self.status.position_a
            target_b = self.status.position_b
            target_c = self.status.position_c
            
            if axis_upper == 'X':
                target_x += step
            elif axis_upper == 'Y':
                target_y += step
            elif axis_upper == 'Z':
                target_z += step
            elif axis_upper == 'A':
                target_a += step
            elif axis_upper == 'B':
                target_b += step
            elif axis_upper == 'C':
                target_c += step
            
            # ★★★ 核心修复：调用_set_workpiece_position处理完整坐标变换链 ★★★
            # 该方法内部会：
            # 1. 更新工件坐标
            # 2. 计算绝对坐标（工件+G5x+G92+TLO）
            # 3. RTCP启用时执行逆运动学得到关节坐标
            # 4. RTCP禁用时直接使用绝对坐标作为关节坐标
            # 5. 更新世界坐标
            self._set_workpiece_position(
                target_x, target_y, target_z,
                target_a, target_b, target_c
            )
            
            # 更新世界坐标（正运动学验证）
            self._update_world_position()
            
            # 通知更新（包括MQTT回调，发送的是machine坐标）
            self._notify_update()
            
            # 调试输出
            if self.debug:
                rtcp_status = "启用" if self.rtcp_enabled else "禁用"
                print(f"\n[JOG] {axis_upper}{'+' if direction > 0 else '-'}{distance} "
                    f"(RTCP: {rtcp_status})")
                print(f"  工件坐标: ({old_pos['x']:.3f}, {old_pos['y']:.3f}, "
                    f"{old_pos['z']:.3f}) → ({target_x:.3f}, {target_y:.3f}, "
                    f"{target_z:.3f})")
                print(f"  机器坐标: ({old_machine['x']:.3f}, {old_machine['y']:.3f}, "
                    f"{old_machine['z']:.3f}) → ({self.status.machine_x:.3f}, "
                    f"{self.status.machine_y:.3f}, {self.status.machine_z:.3f})")
                if axis_upper in ('A', 'B', 'C'):
                    print(f"  旋转轴: {axis_upper}={self.status.__dict__.get(f'position_{axis_upper.lower()}', 0):.2f}°")
                if self.rtcp_enabled:
                    # 计算RTCP补偿量
                    dx = self.status.machine_x - self.status.position_x
                    dy = self.status.machine_y - self.status.position_y
                    dz = self.status.machine_z - self.status.position_z
                    print(f"  RTCP补偿: ΔX={dx:.3f}, ΔY={dy:.3f}, ΔZ={dz:.3f}")
            
            return {
                "success": True,
                "message": f"点动 {axis_upper}{'+' if direction > 0 else '-'}{distance}",
                "data": {
                    "axis": axis_upper,
                    "direction": direction,
                    "distance": distance,
                    "rtcp_enabled": self.rtcp_enabled,
                    "position": {
                        "workpiece": {
                            "x": self.status.position_x,
                            "y": self.status.position_y,
                            "z": self.status.position_z,
                            "a": self.status.position_a,
                            "b": self.status.position_b,
                            "c": self.status.position_c,
                        },
                        "machine": {
                            "x": self.status.machine_x,
                            "y": self.status.machine_y,
                            "z": self.status.machine_z,
                            "a": self.status.machine_a,
                            "b": self.status.machine_b,
                            "c": self.status.machine_c,
                        },
                    }
                }
            }
        
        # 绑定方法到实例
        kernel_instance.jog = jog.__get__(kernel_instance, type(kernel_instance))


    @staticmethod
    def add_jog_machine(kernel_instance):
        """添加机器坐标系点动功能（G53模式）
        
        在机器坐标系下直接点动关节，适用于：
        - 手动调整关节位置
        - 调试运动学参数
        - 紧急情况直接控制关节
        
        注意：机器坐标点动不走RTCP逆解，直接修改关节坐标。
        """
        def jog_machine(self, axis: str, direction: int, distance: float) -> Dict:
            """在机器坐标系下执行点动"""
            if not self.ensure_operational():
                return {"success": False, "message": "机床未就绪"}
            
            axis_upper = axis.upper()
            valid_axes = {'X', 'Y', 'Z', 'A', 'B', 'C'}
            if axis_upper not in valid_axes:
                return {"success": False, "message": f"无效轴: {axis}"}
            
            step = direction * distance
            
            # 直接修改机器坐标
            if axis_upper == 'X':
                self.status.machine_x += step
            elif axis_upper == 'Y':
                self.status.machine_y += step
            elif axis_upper == 'Z':
                self.status.machine_z += step
            elif axis_upper == 'A':
                self.status.machine_a += step
            elif axis_upper == 'B':
                self.status.machine_b += step
            elif axis_upper == 'C':
                self.status.machine_c += step
            
            # 反向计算工件坐标（通过_update_current_from_machine） # ★ 调用新增的方法反向计算工件坐标
            self._update_current_from_machine()
            
            # 更新世界坐标
            self._update_world_position()
            
            # 通知更新
            self._notify_update()
            
            return {
                "success": True,
                "message": f"机器点动 {axis_upper}{'+' if direction > 0 else '-'}{distance}",
                "data": {
                    "axis": axis_upper,
                    "direction": direction,
                    "distance": distance,
                    "rtcp_enabled": self.rtcp_enabled,
                    "position": {
                        "machine": {
                            "x": self.status.machine_x,
                            "y": self.status.machine_y,
                            "z": self.status.machine_z,
                        },
                        "workpiece": {
                            "x": self.status.position_x,
                            "y": self.status.position_y,
                            "z": self.status.position_z,
                        }
                    }
                }
            }
        
        kernel_instance.jog_machine = jog_machine.__get__(kernel_instance, type(kernel_instance))


    @staticmethod
    def add_jog_incremental(kernel_instance):
        """添加增量式点动（明确使用增量模式）
        
        与jog功能相同，但明确表示增量模式（G91）。
        在增量模式下，每次移动都是相对于当前位置。
        """
        def jog_incremental(self, axis: str, direction: int, distance: float) -> Dict:
            """增量式点动"""
            # 直接委托给jog方法
            return self.jog(axis, direction, distance)
        
        kernel_instance.jog_incremental = jog_incremental.__get__(kernel_instance, type(kernel_instance))





    @staticmethod
    def add_spindle_control(kernel_instance):
        def spindle_start(self, direction: str = "CW", speed: float = 1000.0) -> Dict:
            self.status.spindle_speed = speed
            self.status.spindle_state = direction.upper()
            self.virtual_hal.set('spindle.0.speed', speed)
            self.virtual_hal.set('spindle.0.direction', 1 if direction.upper() == "CW" else -1)
            self.virtual_hal.set('spindle.0.enable', 1)
            self._notify_update()
            return {"success": True, "message": f"主轴 {direction} {speed} RPM"}
        
        def spindle_stop(self) -> Dict:
            self.status.spindle_speed = 0
            self.status.spindle_state = "OFF"
            self.virtual_hal.set('spindle.0.speed', 0)
            self.virtual_hal.set('spindle.0.enable', 0)
            self._notify_update()
            return {"success": True, "message": "主轴停止"}
        
        kernel_instance.spindle_start = spindle_start.__get__(kernel_instance, type(kernel_instance))
        kernel_instance.spindle_stop = spindle_stop.__get__(kernel_instance, type(kernel_instance))

    @staticmethod
    def add_coolant_control(kernel_instance):
        def coolant_on(self, coolant_type: str = "flood") -> Dict:
            if coolant_type == "flood": self.status.coolant_flood = True
            elif coolant_type == "mist": self.status.coolant_mist = True
            elif coolant_type == "both": self.status.coolant_flood = self.status.coolant_mist = True
            self._notify_update()
            return {"success": True, "message": f"冷却液开启 ({coolant_type})"}
        
        def coolant_off(self) -> Dict:
            self.status.coolant_flood = False
            self.status.coolant_mist = False
            self._notify_update()
            return {"success": True, "message": "冷却液关闭"}
        
        kernel_instance.coolant_on = coolant_on.__get__(kernel_instance, type(kernel_instance))
        kernel_instance.coolant_off = coolant_off.__get__(kernel_instance, type(kernel_instance))

    @staticmethod
    def add_override_controls(kernel_instance):
        def set_feed_override(self, value: int) -> Dict:
            self.status.feed_override = max(0, min(200, value))
            self.planner.set_feed_override(self.status.feed_override / 100.0)
            self._notify_update()
            return {"success": True, "message": f"进给倍率 {self.status.feed_override}%"}
        
        def set_rapid_override(self, value: int) -> Dict:
            self.status.rapid_override = max(0, min(100, value))
            self.planner.set_rapid_override(self.status.rapid_override / 100.0)
            self._notify_update()
            return {"success": True, "message": f"快速倍率 {self.status.rapid_override}%"}
        
        def set_spindle_override(self, value: int) -> Dict:
            self.status.spindle_override = max(50, min(120, value))
            self._notify_update()
            return {"success": True, "message": f"主轴倍率 {self.status.spindle_override}%"}
        
        kernel_instance.set_feed_override = set_feed_override.__get__(kernel_instance, type(kernel_instance))
        kernel_instance.set_rapid_override = set_rapid_override.__get__(kernel_instance, type(kernel_instance))
        kernel_instance.set_spindle_override = set_spindle_override.__get__(kernel_instance, type(kernel_instance))

    @staticmethod
    def add_get_full_config(kernel_instance):
        def get_full_config(self) -> Dict:
            return {
                'machine_id': self.name,
                'machine_name': self.name,
                'state': self.state_machine.get_state(),
                'status': {
                    'position': {'x': self.status.position_x, 'y': self.status.position_y,
                                 'z': self.status.position_z, 'a': self.status.position_a,
                                 'b': self.status.position_b, 'c': self.status.position_c},
                    'world_position': {'x': self.status.world_x, 'y': self.status.world_y, 'z': self.status.world_z},
                    'spindle': {'speed': self.status.spindle_speed, 'state': self.status.spindle_state},
                    'coolant': {'flood': self.status.coolant_flood, 'mist': self.status.coolant_mist},
                    'tool': {'current': self.status.current_tool, 'length': self.status.tool_length},
                    'feedrate': self.status.feedrate,
                    'overrides': {'feed': self.status.feed_override, 'rapid': self.status.rapid_override},
                    'program': {'name': self.status.program_name, 'progress': self.status.progress_percent},
                    'rtcp_enabled': self.status.rtcp_enabled,
                    'kinematics_type': self.status.kinematics_type
                },
                'tool_table': self.tool_table,
                'work_offsets': self.work_offsets
            }
        kernel_instance.get_full_config = get_full_config.__get__(kernel_instance, type(kernel_instance))

    @staticmethod
    def add_change_tool(kernel_instance):
        def change_tool(self, tool_number: int) -> Dict:
            if tool_number in self.tool_table:
                self.status.current_tool = tool_number
                self.selected_tool = tool_number
                self.status.tool_length = self.tool_table[tool_number]["length"]
                self.status.tool_diameter = self.tool_table[tool_number]["diameter"]
                self._notify_update()
                return {"success": True, "message": f"换刀 T{tool_number}"}
            return {"success": False, "message": f"刀具 T{tool_number} 不存在"}
        kernel_instance.change_tool = change_tool.__get__(kernel_instance, type(kernel_instance))

    @staticmethod
    def extend_all(kernel_instance):
        """一次性扩展所有方法"""
        CNCKernelExtensions.add_power_off(kernel_instance)
        CNCKernelExtensions.add_home_all(kernel_instance)
        CNCKernelExtensions.add_jog(kernel_instance)              # ★ 已修复
        CNCKernelExtensions.add_jog_machine(kernel_instance)      # ★ 新增
        CNCKernelExtensions.add_jog_incremental(kernel_instance)   # ★ 新增
        CNCKernelExtensions.add_spindle_control(kernel_instance)
        CNCKernelExtensions.add_coolant_control(kernel_instance)
        CNCKernelExtensions.add_override_controls(kernel_instance)
        CNCKernelExtensions.add_get_full_config(kernel_instance)
        CNCKernelExtensions.add_change_tool(kernel_instance)


# ==================== MachineManager 类 ====================

class MachineManager:
    """机床管理器 - 简化版"""
    
    def __init__(self, debug: bool = False):
        self.debug = debug
        self._machines: Dict[str, CNCKernel] = {}
        self._machine_configs: Dict[str, Dict] = {}
        self._machine_programs: Dict[str, Dict] = {}
    
    

    def create_machine(self, machine_id: str, config: Optional[Dict] = None) -> CNCKernel:
        if machine_id in self._machines:
            return self._machines[machine_id]
        
        machine_name = config.get('machine_name', machine_id) if config else machine_id
        
        # 使用 CNCKernelWithJSON 替代 CNCKernel
        kernel = CNCKernelWithJSON(debug=self.debug, name=machine_name)
        
        if config:
            self._machine_configs[machine_id] = config
            
            kin_config = config.get('kinematics', {})
            kin_type_str = kin_config.get('type', 'TRT_BC').upper()
            type_map = {
                'TRT_AC': KinematicsType.TRT_AC, 'XYZAC_TRT': KinematicsType.TRT_AC,
                'TRT_BC': KinematicsType.TRT_BC, 'XYZBC_TRT': KinematicsType.TRT_BC,
                'IDENTITY': KinematicsType.IDENTITY, 'FIVEAXIS_BC': KinematicsType.FIVEAXIS_BC,
                'MAXKINS_BC': KinematicsType.MAXKINS_BC, 'HEXAPOD': KinematicsType.HEXAPOD,
                'PUMA': KinematicsType.PUMA, 'SCARA': KinematicsType.SCARA,
                'LINEAR_DELTA': KinematicsType.LINEAR_DELTA,
            }
            kernel.kinematics_type = type_map.get(kin_type_str, KinematicsType.TRT_BC)
            kernel._original_kinematics_type = kernel.kinematics_type
            
            if kernel._kinematics_instance:
                kernel._kinematics_instance.set_params(**kin_config.get('parameters', {}))
            
            for tool in config.get('tool_table', []):
                pocket = tool.get('pocket', 1)
                kernel.tool_table[pocket] = {
                    'length': tool.get('length', 100.0),
                    'diameter': tool.get('diameter', 10.0),
                    'name': tool.get('name', f'T{pocket}')
                }
            
            for offset_name, offset in config.get('work_offsets', {}).items():
                if offset_name.startswith('G'):
                    try:
                        offset_num = int(offset_name[1:])
                        kernel.work_offsets[offset_num] = {
                            'x': offset.get('X', 0.0), 'y': offset.get('Y', 0.0),
                            'z': offset.get('Z', 0.0), 'a': offset.get('A', 0.0),
                            'b': offset.get('B', 0.0), 'c': offset.get('C', 0.0)
                        }
                    except ValueError:
                        pass
            
            for pin_name, pin_value in config.get('hal_pins', {}).items():
                kernel.virtual_hal.set(pin_name, pin_value)
        
        self._machines[machine_id] = kernel
        if self.debug:
            print(f"[MachineManager] 创建机床: {machine_id}")
        return kernel    
    
    
    def create_machine_CNCKernel(self, machine_id: str, config: Optional[Dict] = None) -> CNCKernel:
        if machine_id in self._machines:
            return self._machines[machine_id]
        
        machine_name = config.get('machine_name', machine_id) if config else machine_id
        kernel = CNCKernel(debug=self.debug, name=machine_name)
        CNCKernelExtensions.extend_all(kernel)
        
        if config:
            self._machine_configs[machine_id] = config
            
            # 设置运动学类型
            kin_config = config.get('kinematics', {})
            kin_type_str = kin_config.get('type', 'TRT_BC').upper()
            type_map = {
                'TRT_AC': KinematicsType.TRT_AC, 'XYZAC_TRT': KinematicsType.TRT_AC,
                'TRT_BC': KinematicsType.TRT_BC, 'XYZBC_TRT': KinematicsType.TRT_BC,
                'IDENTITY': KinematicsType.IDENTITY, 'FIVEAXIS_BC': KinematicsType.FIVEAXIS_BC,
                'MAXKINS_BC': KinematicsType.MAXKINS_BC, 'HEXAPOD': KinematicsType.HEXAPOD,
                'PUMA': KinematicsType.PUMA, 'SCARA': KinematicsType.SCARA,
                'LINEAR_DELTA': KinematicsType.LINEAR_DELTA,
            }
            kernel.kinematics_type = type_map.get(kin_type_str, KinematicsType.TRT_BC)
            kernel._original_kinematics_type = kernel.kinematics_type
            
            # 设置运动学参数
            if kernel._kinematics_instance:
                kernel._kinematics_instance.set_params(**kin_config.get('parameters', {}))
            
            # 设置刀具表
            for tool in config.get('tool_table', []):
                pocket = tool.get('pocket', 1)
                kernel.tool_table[pocket] = {
                    'length': tool.get('length', 100.0),
                    'diameter': tool.get('diameter', 10.0),
                    'name': tool.get('name', f'T{pocket}')
                }
            
            # 设置工件坐标系
            for offset_name, offset in config.get('work_offsets', {}).items():
                if offset_name.startswith('G'):
                    try:
                        offset_num = int(offset_name[1:])
                        kernel.work_offsets[offset_num] = {
                            'x': offset.get('X', 0.0), 'y': offset.get('Y', 0.0),
                            'z': offset.get('Z', 0.0), 'a': offset.get('A', 0.0),
                            'b': offset.get('B', 0.0), 'c': offset.get('C', 0.0)
                        }
                    except ValueError:
                        pass
            
            # 设置HAL引脚
            for pin_name, pin_value in config.get('hal_pins', {}).items():
                kernel.virtual_hal.set(pin_name, pin_value)
        
        self._machines[machine_id] = kernel
        if self.debug:
            print(f"[MachineManager] 创建机床: {machine_id}")
        return kernel
    
    def get_machine(self, machine_id: str) -> Optional[CNCKernel]:
        return self._machines.get(machine_id)
    
    def remove_machine(self, machine_id: str) -> bool:
        if machine_id in self._machines:
            self._machines[machine_id].shutdown()
            del self._machines[machine_id]
            self._machine_programs.pop(machine_id, None)
            self._machine_configs.pop(machine_id, None)
            return True
        return False
    
    def list_machines(self) -> List[str]:
        return list(self._machines.keys())
    
    def get_status(self, machine_id: str) -> Optional[Dict]:
        kernel = self.get_machine(machine_id)
        if not kernel:
            return None
        s = kernel.status
        
        # 尝试从收集器获取偏移信息
        g5x_x = g5x_y = g5x_z = 0.0
        g92_x = g92_y = g92_z = 0.0
        
        if hasattr(kernel, 'parser') and kernel.parser:
            parser = kernel.parser
            if hasattr(parser, '_builtin_parser') and parser._builtin_parser:
                bp = parser._builtin_parser
                if bp.collector:
                    c = bp.collector
                    g5x_x = c.g5x_offset_x
                    g5x_y = c.g5x_offset_y
                    g5x_z = c.g5x_offset_z
                    g92_x = c.g92_offset_x
                    g92_y = c.g92_offset_y
                    g92_z = c.g92_offset_z
        
        return {
            'machine_id': machine_id,
            'machine_name': kernel.name,
            'state': kernel.state_machine.get_state(),
            # ... 现有字段 ...
            'g5x_offset_x': g5x_x,
            'g5x_offset_y': g5x_y,
            'g5x_offset_z': g5x_z,
            'g92_x': g92_x,
            'g92_y': g92_y,
            'g92_z': g92_z,
            'tool_length': s.tool_length,
            'coordinate_mode': s.current_offset if s.current_offset else 'G54',
            'rtcp_enabled': s.rtcp_enabled,
            'current_tool': s.current_tool,
            'selected_tool': s.selected_tool,
            'tool_length': s.tool_length,
            'tool_diameter': s.tool_diameter,
            'position': {'x': s.position_x, 'y': s.position_y, 'z': s.position_z,
                        'a': s.position_a, 'b': s.position_b, 'c': s.position_c},
            'world_position': {'x': s.world_x, 'y': s.world_y, 'z': s.world_z},
            'machine_position': {'x': s.machine_x, 'y': s.machine_y, 'z': s.machine_z},
            'spindle_speed': s.spindle_speed,
            'spindle_state': s.spindle_state,
            'coolant_flood': s.coolant_flood,
            'coolant_mist': s.coolant_mist,
            'coolant_state': s.coolant_state,
            'feedrate': s.feedrate,
            'feed_override': s.feed_override,
            'rapid_override': s.rapid_override,
            'spindle_override': s.spindle_override,
            'program_name': s.program_name,
            'program_progress': s.progress_percent,
            'current_line': s.current_line,
            'total_lines': s.total_lines,
            'path_length': s.path_length,
            'total_time': s.total_time,
            'alarm_code': s.alarm_code,
            'alarm_message': s.alarm_message,
            'coordinate_mode': s.coordinate_mode,
            'plane_mode': s.plane_mode,
            'unit_mode': s.unit_mode,
            'cutter_comp': s.cutter_comp,
            'current_offset': s.current_offset,
        }
    
    def get_all_status(self) -> Dict[str, Dict]:
        result = {}
        for mid in self._machines:
            status = self.get_status(mid)
            if status:
                result[mid] = status
        return result
    
    def load_program(self, machine_id: str, program_config: Dict) -> bool:
        kernel = self.get_machine(machine_id)
        if not kernel:
            return False
        
        try:
            self._machine_programs[machine_id] = program_config
            mp = program_config.get('main_program', {})
            content = mp.get('content', '')
            if not content:
                return False
            
            # 获取运动学参数
            kinematics_params = {}
            if kernel._kinematics_instance and kernel._kinematics_instance.params:
                p = kernel._kinematics_instance.params
                kinematics_params = {
                    'rot_center_x': getattr(p, 'rot_center_x', 0.0),
                    'rot_center_y': getattr(p, 'rot_center_y', 0.0),
                    'rot_center_z': getattr(p, 'rot_center_z', 0.0),
                    'pivot_length': getattr(p, 'pivot_length', 250.0),
                    'tool_length': getattr(p, 'tool_length', 0.0),
                    'conventional_directions': getattr(p, 'conventional_directions', False)
                }
            
            parser = FullRS274Parser(
                debug=self.debug,
                kinematics_type=kernel.kinematics_type,
                kinematics_params=kinematics_params,
                max_points=program_config.get('simulation_config', {}).get('max_points', 50000),
                acceleration=program_config.get('simulation_config', {}).get('acceleration', 500.0),
                max_rapid_rate=program_config.get('simulation_config', {}).get('max_rapid_rate', 10000.0),
                max_feed_rate=program_config.get('simulation_config', {}).get('max_feed_rate', 5000.0),
                tool_length_map={k: v['length'] for k, v in kernel.tool_table.items()},
                tool_radius_map={k: v['diameter'] / 2.0 for k, v in kernel.tool_table.items()}
            )
            
            for sub in program_config.get('subroutines', []):
                if sub.get('name') and sub.get('content'):
                    parser.register_subroutine(sub['name'], sub['content'])
            
            toolpath = parser.parse_string(content, mp.get('name', 'program.ngc'))
            if toolpath:
                kernel.current_toolpath = toolpath
                kernel.status.total_lines = len(toolpath.segments)
                kernel.status.path_length = toolpath.total_length
                kernel.status.total_time = toolpath.total_time
                kernel.status.program_name = mp.get('name', '')
                kernel.status.rtcp_enabled = toolpath.rtcp_enabled
                kernel.status.kinematics_type = toolpath.kinematics_type
                if self.debug:
                    print(f"[MachineManager] 程序加载成功: {machine_id}, 段数: {len(toolpath.segments)}")
                return True
            return False
        except Exception as e:
            if self.debug:
                print(f"[MachineManager] 加载程序失败: {e}")
            return False
    
    def get_machine_program_info(self, machine_id: str) -> Optional[Dict]:
        kernel = self.get_machine(machine_id)
        if not kernel or not kernel.current_toolpath:
            return None
        tp = kernel.current_toolpath
        pc = self._machine_programs.get(machine_id, {})
        return {
            'program_id': pc.get('program_id', ''),
            'program_name': pc.get('program_name', tp.filename),
            'filename': tp.filename,
            'total_segments': len(tp.segments),
            'total_length': tp.total_length,
            'total_time': tp.total_time,
            'bounds': tp.bounds,
            'statistics': tp.get_statistics(),
            'rtcp_enabled': tp.rtcp_enabled,
            'kinematics_type': tp.kinematics_type,
            'tool_changes': tp.tool_changes,
            'subroutines': list(tp.subroutines.keys()) if tp.subroutines else [],
            'errors': tp.errors,
            'warnings': tp.warnings,
        }


# ==================== CNCKernelWithJSON 类 ====================

class CNCKernelWithJSON(CNCKernel):
    """支持JSON配置的CNC内核"""
    
    def __init__(self, debug: bool = False, name: str = "CNC1", 
                 json_config: Optional[Union[str, Dict]] = None):
        super().__init__(debug=debug, name=name)
        
        CNCKernelExtensions.extend_all(self)
        
        self.project_config: Optional[ProjectConfig] = None
        self.current_program_config: Optional[Dict] = None
        self.machine_id = name
        
        if json_config:
            self.load_config(json_config)
    
    def load_config(self, json_source: Union[str, Dict]) -> Tuple[ProjectConfig, Optional[ToolpathData]]:
        """加载JSON配置"""
        if isinstance(json_source, str):
            if os.path.exists(json_source):
                with open(json_source, 'r', encoding='utf-8') as f:
                    config_data = json.load(f)
            else:
                config_data = json.loads(json_source)
        else:
            config_data = json_source
        
        config = self._parse_config(config_data)
        self.project_config = config
        self._apply_machine_config(config)
        
        toolpath = self._load_program_from_config(config)
        self.current_toolpath = toolpath
        
        if toolpath:
            self.status.total_lines = len(toolpath.segments)
            self.status.path_length = toolpath.total_length
            self.status.total_time = toolpath.total_time
            self.status.path_segments = len(toolpath.segments)
            self.status.rtcp_enabled = toolpath.rtcp_enabled
            self.status.kinematics_type = toolpath.kinematics_type
            self.status.program_name = config.main_program.name if config.main_program else ''
        
        self._notify_update()
        return config, toolpath
    
    def _parse_config(self, data: Dict) -> ProjectConfig:
        """解析配置数据"""
        config = ProjectConfig()
        
        if 'project' in data:
            config.project = data['project']
        
        if 'machine' in data:
            mc = data['machine']
            config.machine = MachineConfig(
                name=mc.get('name', ''),
                type=mc.get('type', '5axis_mill'),
                kinematics_type=mc.get('kinematics', {}).get('type', 'TRT_BC'),
                kinematics_params=mc.get('kinematics', {}).get('parameters', {}),
                axes=mc.get('axes', {}),
                spindle=mc.get('spindle', {}),
                tool_table=mc.get('tool_table', []),
                work_offsets=mc.get('work_offsets', {})
            )
        
        if 'hal_config' in data:
            hc = data['hal_config']
            pins = {}
            for name, pin in hc.get('pins', {}).items():
                if isinstance(pin, dict):
                    pins[name] = HALPinConfig(
                        name=name,
                        pin_type=pin.get('type', 'float'),
                        direction=pin.get('direction', 'in'),
                        default=pin.get('default', 0.0),
                        description=pin.get('description', '')
                    )
                else:
                    pins[name] = HALPinConfig(
                        name=name,
                        default=float(pin) if isinstance(pin, (int, float)) else 0.0
                    )
            config.hal_config = HALConfig(
                version=hc.get('version', '1.0'),
                description=hc.get('description', ''),
                pins=pins,
                connections=hc.get('connections', [])
            )
        else:
            config.hal_config = HALConfig()
        
        if 'main_program' in data:
            mp = data['main_program']
            config.main_program = ProgramConfig(
                name=mp.get('name', ''),
                description=mp.get('description', ''),
                content=mp.get('content', '')
            )
        
        if 'subroutines' in data:
            config.subroutines = [
                SubroutineConfig(
                    name=sub.get('name', ''),
                    description=sub.get('description', ''),
                    content=sub.get('content', ''),
                    parameters=sub.get('parameters', [])
                )
                for sub in data['subroutines']
            ]
        
        if 'simulation_config' in data:
            sc = data['simulation_config']
            config.simulation_config = SimulationConfig(
                acceleration=sc.get('acceleration', 500.0),
                max_rapid_rate=sc.get('max_rapid_rate', 10000.0),
                max_feed_rate=sc.get('max_feed_rate', 5000.0),
                arc_division=sc.get('arc_division', 64),
                enable_rtcp_debug=sc.get('enable_rtcp_debug', False),
                max_points=sc.get('max_points', 50000),
                colors=sc.get('colors', {})
            )
        else:
            config.simulation_config = SimulationConfig()
        
        return config
    
    def _apply_machine_config(self, config: ProjectConfig):
        """应用机床配置"""
        # 设置HAL引脚
        for pin_name, pin_config in config.hal_config.pins.items():
            if isinstance(pin_config, HALPinConfig):
                self.virtual_hal.set(pin_name, pin_config.default)
            elif isinstance(pin_config, dict):
                self.virtual_hal.set(pin_name, pin_config.get('default', 0))
            else:
                self.virtual_hal.set(pin_name, pin_config)
        
        # 设置刀具表
        for tool in config.machine.tool_table:
            pocket = tool.get('pocket', 1)
            self.tool_table[pocket] = {
                'length': tool.get('length', 100.0),
                'diameter': tool.get('diameter', 10.0),
                'name': tool.get('name', f'T{pocket}')
            }
        
        # 设置工件坐标系
        for offset_name, offset_data in config.machine.work_offsets.items():
            if offset_name.startswith('G'):
                try:
                    offset_num = int(offset_name[1:])
                    self.work_offsets[offset_num] = {
                        'x': offset_data.get('X', 0.0),
                        'y': offset_data.get('Y', 0.0),
                        'z': offset_data.get('Z', 0.0),
                        'a': offset_data.get('A', 0.0),
                        'b': offset_data.get('B', 0.0),
                        'c': offset_data.get('C', 0.0)
                    }
                except ValueError:
                    pass
        
        # 设置运动学类型
        kin_type_str = config.machine.kinematics_type.upper()
        type_map = {
            'IDENTITY': KinematicsType.IDENTITY,
            'TRT_AC': KinematicsType.TRT_AC,
            'XYZAC_TRT': KinematicsType.TRT_AC,
            'TRT_BC': KinematicsType.TRT_BC,
            'XYZBC_TRT': KinematicsType.TRT_BC,
            'MAXKINS_BC': KinematicsType.MAXKINS_BC,
            'FIVEAXIS_BC': KinematicsType.FIVEAXIS_BC,
            'HEXAPOD': KinematicsType.HEXAPOD,
            'PUMA': KinematicsType.PUMA,
            'SCARA': KinematicsType.SCARA,
            'LINEAR_DELTA': KinematicsType.LINEAR_DELTA,
        }
        self._original_kinematics_type = type_map.get(kin_type_str, KinematicsType.TRT_AC)
        self.kinematics_type = self._original_kinematics_type
        self.status.kinematics_type = self.kinematics_type.name
        
        # 设置运动学参数
        kin_params = config.machine.kinematics_params
        if self._kinematics_instance:
            self._kinematics_instance.set_params(**kin_params)
        
        self.virtual_hal.set('motion.pivot-length', kin_params.get('pivot_length', 250.0))
    
    def _load_program_from_config(self, config: ProjectConfig) -> Optional[ToolpathData]:
        """从配置加载程序"""
        sim_config = config.simulation_config
        
        parser = FullRS274Parser(
            max_points=sim_config.max_points,
            acceleration=sim_config.acceleration,
            max_rapid_rate=sim_config.max_rapid_rate,
            max_feed_rate=sim_config.max_feed_rate,
            kinematics_type=self.kinematics_type,
            kinematics_params=config.machine.kinematics_params,
            debug=sim_config.enable_rtcp_debug
        )
        
        # 设置刀具表
        for tool in config.machine.tool_table:
            pocket = tool.get('pocket', 1)
            parser.tool_length_map[pocket] = tool.get('length', 100.0)
            parser.tool_radius_map[pocket] = tool.get('diameter', 10.0) / 2.0
        
        # 注册子程序
        for sub in config.subroutines:
            name = sub.name if hasattr(sub, 'name') else sub.get('name', '')
            content = sub.content if hasattr(sub, 'content') else sub.get('content', '')
            if name and content:
                parser.register_subroutine(name, content)
        
        # 解析主程序
        if config.main_program and config.main_program.content:
            return parser.parse_string(
                config.main_program.content,
                config.main_program.name
            )
        return None
    
    def load_from_json(self, json_source: Union[str, Dict]) -> Tuple[ProjectConfig, Optional[ToolpathData]]:
        return self.load_config(json_source)
    
    def load_from_json_file(self, filepath: str) -> Tuple[ProjectConfig, Optional[ToolpathData]]:
        return self.load_config(filepath)
    
    def load_from_json_string(self, json_str: str) -> Tuple[ProjectConfig, Optional[ToolpathData]]:
        return self.load_config(json_str)
    
    def load_program_config(self, program_config: Dict) -> bool:
        try:
            self.current_program_config = program_config
            full_config = self._build_config_from_program(program_config)
            self.load_config(full_config)
            return True
        except Exception as e:
            if self.debug:
                print(f"加载程序配置失败: {e}")
            return False
    
    def _build_config_from_program(self, program_config: Dict) -> Dict:
        machine_config = {
            "name": self.machine_id,
            "kinematics": {
                "type": self.kinematics_type.name,
                "parameters": {}
            }
        }
        
        if self.project_config:
            machine_config = {
                "name": self.machine_id,
                "type": self.project_config.machine.type,
                "kinematics": {
                    "type": self.project_config.machine.kinematics_type,
                    "parameters": self.project_config.machine.kinematics_params
                },
                "tool_table": self.project_config.machine.tool_table,
                "work_offsets": self.project_config.machine.work_offsets,
                "axes": self.project_config.machine.axes
            }
        
        return {
            "project": {"name": program_config.get('program_id', 'Program')},
            "machine": machine_config,
            "main_program": program_config.get('main_program', {}),
            "subroutines": program_config.get('subroutines', []),
            "simulation_config": program_config.get('simulation_config', {
                "acceleration": 500.0,
                "max_rapid_rate": 10000.0,
                "max_feed_rate": 5000.0,
                "max_points": 50000,
                "enable_rtcp_debug": self.debug
            }),
            "hal_config": {"version": "1.0", "pins": {}}
        }














# ==================== 测试函数 ====================

def test_all_kinematics():
    """测试所有运动学类型"""
    print("=" * 80)
    print("测试所有 LinuxCNC 运动学类型")
    print("=" * 80)

    kin_types = [
        KinematicsType.TRT_AC, KinematicsType.TRT_BC, KinematicsType.MAXKINS_BC,
        KinematicsType.FIVEAXIS_BC, KinematicsType.HEXAPOD, KinematicsType.PUMA,
        KinematicsType.SCARA, KinematicsType.SCORBOT, KinematicsType.LINEAR_DELTA,
        KinematicsType.ROTARY_DELTA, KinematicsType.TRIPOD, KinematicsType.PENTAPOD,
        KinematicsType.ROTATE, KinematicsType.COREXY, KinematicsType.ROSE,
    ]

    for kin_type in kin_types:
        print(f"\n测试 {kin_type.name}:")
        try:
            kin = KinematicsFactory.create(kin_type, debug=False)
            if kin:
                print(f"  ✓ 创建成功")
                info = KinematicsFactory.get_type_info(kin_type)
                print(f"    描述: {info.get('description', 'N/A')}")
                print(f"    来源: {info.get('source', 'N/A')}")
            else:
                print(f"  - IDENTITY类型返回None")
        except Exception as e:
            print(f"  ✗ 创建失败: {e}")


def test_kinematics():
    """测试五轴运动学"""
    print("=" * 60)
    print("测试五轴运动学")
    print("=" * 60)

    kin = FiveAxisKinematics(kin_type=KinematicsType.TRT_BC, debug=True)
    kin.set_tool_length(100.0)
    kin.enable_rtcp(True)

    joints = [0, 0, 0, 30, 0]
    world = kin.forward_transform(joints)
    print(f"关节坐标 {joints} -> 世界坐标 ({world[0]:.3f}, {world[1]:.3f}, {world[2]:.3f})")

    world_pos = (0, 0, 100, 30, 0, 0)
    joints_back = kin.inverse_transform(world_pos)
    print(f"世界坐标 {world_pos} -> 关节坐标 ({joints_back[0]:.3f}, {joints_back[1]:.3f}, {joints_back[2]:.3f})")


def test_parser():
    """测试解析器"""
    print("=" * 60)
    print("测试G代码解析器")
    print("=" * 60)

    test_program = """
G90 G17 G21 G40 G49 G80
G54
T1 M6
G43.4 H1
G0 X0 Y0 Z100
G1 X50 Y0 Z50 F1000
G1 X50 Y50 Z50
G1 X0 Y50 Z50
G1 X0 Y0 Z50
G0 Z100
G49
M30
"""

    parser = FullRS274Parser(debug=True)
    toolpath = parser.parse_string(test_program, "test.ngc")

    print(f"\n解析结果:")
    print(f"  路径段数: {len(toolpath.segments)}")
    print(f"  总长度: {toolpath.total_length:.3f} mm")
    print(f"  总时间: {toolpath.total_time:.3f} s")
    print(f"  边界: {toolpath.bounds}")


def main():
    """主函数"""
    print("=" * 80)
    print("GLCanon 五轴刀具路径解析器 - 完整集成版")
    print(f"版本: {VERSION}")
    print("包含: LinuxCNC 运动学算法完整集成")
    print("包含: LinuxCNC 完整参数表系统 (RS274NGC)")
    print("支持的构型:")
    print("  - TRT_AC (xyzac-trt-kins)")
    print("  - TRT_BC (xyzbc-trt-kins)")
    print("  - MAXKINS_BC (maxkins)")
    print("  - FIVEAXIS_BC (5axiskins)")
    print("  - HEXAPOD (genhexkins)")
    print("  - SERIAL_DH (genserkins)")
    print("  - PUMA (pumakins)")
    print("  - SCARA (scarakins)")
    print("  - SCORBOT (scorbot-kins)")
    print("  - LINEAR_DELTA (lineardeltakins)")
    print("  - ROTARY_DELTA (rotarydeltakins)")
    print("  - TRIPOD (tripodkins)")
    print("  - PENTAPOD (pentakins)")
    print("  - ROTATE (rotatekins)")
    print("  - COREXY (corexykins)")
    print("  - ROSE (rosekins)")
    print("=" * 80)

    test_kinematics()
    print("\n")

    test_all_kinematics()
    print("\n")

    test_parser()
    print("\n")

    print("=" * 80)
    print("所有测试完成!")
    print("=" * 80)


if __name__ == "__main__":
    main()