import asyncio
import math
import struct
import time
from dataclasses import dataclass, field
from enum import Enum, IntEnum
from typing import Dict, List, Optional, Tuple, Any, Union

import can

def _is_number(s: str) -> bool:
    try:
        float(str(s).strip())
        return True
    except Exception:
        return False

def _to_float(s: str, default: float = 0.0) -> float:
    try:
        return float(str(s).strip())
    except Exception:
        print("Error converting to float, returning default")
        return default

def _to_int(v, name: str = None) -> int:
    try:
        return int(float(str(v).strip()))
    except Exception as e:
        raise CanError(f"Expected {name} to be an int-ish value, got {v!r}") from e


def vb_isnumeric(value: Any) -> bool:
  """Approximation of VB.NET's IsNumeric"""
  if isinstance(value, (int, float)):
    return True
  if isinstance(value, str):
    try:
      float(value)
      return True
    except ValueError:
      return False
  return False


def ushort(value: Any) -> int:
  return int(value) & 0xFFFF


def uint(value: Any) -> int:
  return int(value) & 0xFFFFFFFF


def ulong(value: Any) -> int:
  return int(value) & 0xFFFFFFFFFFFFFFFF


def to_byte(value: Any) -> int:
  """Equivalent to VB.NET's Conversions.ToByte"""
  return int(value) & 0xFF

def _u32_le(value: int) -> List[int]:
  return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False))


# --- Enums ---


class eJointMoveDirection(IntEnum):
  Normal = 0
  Clockwise = 1
  Counterclockwise = 2
  ShortestWay = 3


class eHomeStatus(IntEnum):
  NotHomed = 0
  Homed = 1
  InitializedWithoutHoming = 2


class eInputLogic(IntEnum):
  GeneralPurpose = 0
  EnableForwardOnly = 1
  EnableReverseOnly = 2


class eEventType(IntEnum):
  MotorPositionChanged = 1
  MotionStatusReceived = 2
  MotorEnabledStatusReceived = 3
  DigitalInputChangedState = 4
  CANEmergencyMessageReceived = 5
  MoveError = 6
  MotorMoveDone = 7
  MotorsMoveDone = 8
  MotorsMovePathDone = 9


class eMoveType(IntEnum):
  MotorMove = 0
  MotorsMoveAbsolute = 1
  MotorsMovePath = 2


class eCmdType(IntEnum):
  ValQuery = 1
  ValSet = 2
  Execute = 3


class eValType(IntEnum):
  Int = 1
  Float = 2


class eCOBType(IntEnum):
  NMT = 0
  EMCY = 1
  SYNC = 1
  TIMESTAMP = 2
  TPDO1 = 3
  RPDO1 = 4
  TPDO2 = 5
  RPDO2 = 6
  TPDO3 = 7
  RPDO3 = 8
  TPDO4 = 9
  RPDO4 = 10
  TSDO = 11
  RSDO = 12
  ERRCTRL = 14
  HEARTBEAT = 14


class eRPDO(IntEnum):
  RPDO1 = 1
  RPDO3 = 3
  RPDO4 = 4


class ePDOTransmissionType(IntEnum):
  SynchronousAcyclic = 0
  SynchronousCyclic = 1
  EventDrivenManf = 254  # 0xFE
  EventDrivenDev = 255  # 0xFF


class eRPDOMappedObject(IntEnum):
  NotMapped = 0
  ControlWord = 0x60400010
  TargetTorque = 0x60710010
  MaxTorque = 0x60720010
  TargetPosition = 0x607A0020
  VelocityOffset = 0x60B10020
  TargetPositionIP = 0x60C10120
  TargetVelocityIP = 0x60C10220
  DigitalOutputs = 0x60FE0020
  TargetVelocity = 0x60FF0020


class eTPDO(IntEnum):
  TPDO1 = 1
  TPDO3 = 3
  TPDO4 = 4


class eTPDOTrigger(IntEnum):
  MotionComplete = 0
  MainHomingComplete = 1
  AuxiliaryHomingComplete = 2
  MotorShutDownByException = 3
  MotorStarted = 4
  UserProgramEmitCommand = 5
  OSInterpreterExecutionComplete = 6
  MotionStartedEvent = 8
  PDODataChanged = 24
  DigitalInputEvent = 26
  StatusWordEvent = 27
  BinaryInterpreterCommandComplete = 31


class eTPDOMappedObject(IntEnum):
  NotMapped = 0
  Timestamp = 0x20410020
  PVTHeadPointer = 0x2F110010
  PVTTailPointer = 0x2F120010
  StatusWord = 0x60410010
  PositionActualValue = 0x60640020
  VelocityDemandValue = 0x606B0020
  VelocityActualValue = 0x606C0020
  TorqueDemandValue = 0x60740010
  TorqueActualValue = 0x60770010
  IPBufferPosition = 0x60C40410
  DigitalInputs = 0x60FD0020


class eElmoObject(IntEnum):
  DeviceType = 0x1000
  ErrorRegister = 0x1001
  ManfStatusRegister = 0x1002
  ErrorField = 0x1003
  CommCyclePeriod = 0x1006
  ManfDeviceName = 0x1008
  ManfHWVersion = 0x1009
  ManfSWVersion = 0x100A
  NodeID = 0x100B
  StoreParameters = 0x1010
  RestoreParameters = 0x1011
  CustomerHeartbeatTime = 0x1016
  ProducerHeartbeatTime = 0x1017
  IdentityObject = 0x1018
  OSCommand = 0x1023
  OSCommandMode = 0x1024
  ErrorBehavior = 0x1029
  RPDO1CommParam = 0x1400
  RPDO2CommParam = 0x1401
  RPDO3CommParam = 0x1402
  RPDO4CommParam = 0x1403
  RPDO1Mapping = 0x1600
  RPDO2Mapping = 0x1601
  RPDO3Mapping = 0x1602
  RPDO4Mapping = 0x1603
  TPDO1CommParam = 0x1800
  TPDO2CommParam = 0x1801
  TPDO3CommParam = 0x1802
  TPDO4CommParam = 0x1803
  TPDO1Mapping = 0x1A00
  TPDO2Mapping = 0x1A01
  TPDO3Mapping = 0x1A02
  TPDO4Mapping = 0x1A03
  FastReference = 0x2005
  BinaryInterpreterInput = 0x2012
  BinaryInterpreterOutput = 0x2013
  FilteredRMSCurrent = 0x201B
  HomeOnBlockLimitParams = 0x2020
  RecorderData = 0x2030
  TimeStamp = 0x2040
  DriveParamsChecksum = 0x2060
  AdditionalPositionRangeLimit = 0x207B
  ExtendedErrorCode = 0x2081
  CANControllerStatus = 0x2082
  SerialEncoderStatus = 0x2084
  ExtraStatusRegister = 0x2085
  STOStatusRegister = 0x2086
  PALVersion = 0x2087
  AuxPositionActualValue = 0x20A0
  SocketAdditionalFunction = 0x20B0
  AbsoluteSensorFunctions = 0x20FC
  DigitalInputs = 0x20FD
  DigitalInputLowByte = 0x2201
  ExtendedInputs = 0x2202
  Application = 0x2203
  AnalogInputs = 0x2205
  Supply5VDC = 0x2206
  DigitalOutputs = 0x22A0
  ExtendedOutputs = 0x22A1
  DriveTemperature = 0x22A2
  DriveTemperature2 = 0x22A3
  MotorTemperature = 0x22A4
  GainSchedulingIndex = 0x2E00
  TorqueWindow = 0x2E06
  TorqueWindowTime = 0x2E07
  HomeOnTouchProbe = 0x2E10
  GantryYawOffset = 0x2E15
  UserInteger = 0x2F00
  UserFloat = 0x2F01
  GetCtrlBoardType = 0x2F05
  TPDOAsyncEvents = 0x2F20
  EmergencyEvents = 0x2F21
  DS402Config = 0x2F41
  ThresholdParam = 0x2F45
  CANEncoderRange = 0x2F70
  ExtrapolationCyclesTimeout = 0x2F75
  ElmoParamBG = 0x3020
  ElmoParamCA = 0x3034
  ElmoParamCL = 0x303F
  ElmoParamDC = 0x3050
  ElmoParamEC = 0x306A
  ElmoParamER = 0x3079
  ElmoParamHL = 0x30C1
  ElmoParamHP = 0x30C5
  ElmoParamHT = 0x30C9
  ElmoParamIB = 0x30D1
  ElmoParamID = 0x30D3
  ElmoParamIF = 0x30D5
  ElmoParamIL = 0x30DB
  ElmoParamIP = 0x30DF
  ElmoParamIQ = 0x30E0
  ElmoParamJV = 0x30FF
  ElmoParamKI = 0x310C
  ElmoParamKL = 0x310F
  ElmoParamKP = 0x3113
  ElmoParamKV = 0x3119
  ElmoParamLL = 0x3129
  ElmoParamMC = 0x313A
  ElmoParamMO = 0x3146
  ElmoParamMS = 0x314A
  ElmoParamOB = 0x316D
  ElmoParamOL = 0x3177
  ElmoParamOP = 0x317B
  ElmoParamPA = 0x3186
  ElmoParamPE = 0x318A
  ElmoParamPL = 0x3191
  ElmoParamPP = 0x3195
  ElmoParamPR = 0x3197
  ElmoParamPS = 0x3198
  ElmoParamPX = 0x319D
  ElmoParamSD = 0x31D7
  ElmoParamSF = 0x31D9
  ElmoParamSR = 0x31E5
  ElmoParamSV = 0x31E9
  ElmoParamSW = 0x31EA
  ElmoParamTC = 0x31F0
  ElmoParamTM = 0x31FA
  ElmoParamUC = 0x320D
  ElmoParamUI = 0x3210
  ElmoParamVE = 0x3226
  ElmoParamVH = 0x3229
  ElmoParamVL = 0x322D


class eElmoObjectDataType(IntEnum):
  UNSIGNED8 = 0
  UNSIGNED16 = 1
  UNSIGNED32 = 2
  UNSIGNED64 = 3
  INTEGER8 = 4
  INTEGER16 = 5
  INTEGER32 = 6
  INTEGER64 = 7
  STR = 8


# --- Structs (dataclasses) ---


@dataclass
class sEventData:
  event_type: eEventType = eEventType.MotorPositionChanged
  pending: bool = False
  node_id: int = 0
  cmr_msg_type: str = ""
  cmr_data_type: str = ""
  cmr_data: str = ""
  mpc_position: Optional[List[int]] = None  # Array
  msr_status: bool = False
  msr_status_word: int = 0
  mesr_status: int = 0
  dics_state: Optional[List[bool]] = None  # Array
  cemr_emcy_msg: Any = None  # sEmcy
  cemr_description: str = ""
  cemr_disable_motors: bool = False
  me_error_code: int = 0
  me_index: int = 0
  mmd_all_moves_done: bool = False


@dataclass
class sErrCtrl:
  data_byte: Optional[List[int]] = None  # Array


@dataclass
class sMoveData:
  move_type: eMoveType = eMoveType.MotorMove
  index: int = 0
  pending: bool = False
  motion_started: bool = False
  error_code: int = 0
  wait_until_done: bool = False
  timeout_msec: int = 0


@dataclass
class sMotorMoveParam:
  node_id: int = 0
  position: int = 0
  velocity: int = 0
  acceleration: int = 0
  relative: bool = False
  mtr_joint_move_direction: eJointMoveDirection = eJointMoveDirection.ShortestWay


@dataclass
class sMotorsMoveAbsoluteParam:
  node_id: Optional[List[int]] = None  # Array
  position: Optional[List[int]] = None  # Array
  velocity: Optional[List[int]] = None  # Array
  acceleration: Optional[List[int]] = None  # Array
  mtrs_joint_move_direction: Optional[List[eJointMoveDirection]] = None  # Array


@dataclass
class sPVAPoint:
  p: int = 0
  v: int = 0
  a: int = 0


@dataclass
class sMotorsMovePathParam:
  pva_point: Optional[List[List[sPVAPoint]]] = None  # 2D Array
  time_interval_msec: int = 0
  stop_deceleration: Optional[List[int]] = None  # Array
  skip_ax: Optional[List[bool]] = None  # Array


@dataclass
class sPVT_EMCY_QueueLow:
  state: bool = False
  write_pointer: int = 0
  read_pointer: int = 0


@dataclass
class sPVT_EMCY_QueueFull:
  state: bool = False
  failed_write_pointer: int = 0


@dataclass
class sPVT_EMCY:
  queue_low: sPVT_EMCY_QueueLow = field(default_factory=sPVT_EMCY_QueueLow)
  queue_full: sPVT_EMCY_QueueFull = field(default_factory=sPVT_EMCY_QueueFull)
  bad_head_pointer: bool = False
  bad_mode_init_data: bool = False
  motion_terminated: bool = False
  out_of_modulo: bool = False


@dataclass
class sEmcy:
  err_code: int = 0
  err_reg: int = 0
  elmo_err_code: int = 0
  err_code_data1: int = 0
  err_code_data2: int = 0


@dataclass
class sCAN_DeviceType:
  code: int = 0
  description: str = ""
  available: bool = False


@dataclass
class sQuery:
  node_id: int = 0
  object_byte0: int = 0
  object_byte1: int = 0
  sub_index: int = 0
  msg_type: str = ""  # "SDODI" "CHR" "STAT" "SDOUA" "SDOSU"
  msg_index: int = 0
  # response: str = ""


@dataclass
class sCAN_Msg:
  cob: eCOBType = eCOBType.NMT
  node_id: int = 0
  byte0: int = 0
  byte1: int = 0
  byte2: int = 0
  byte3: int = 0
  byte4: int = 0
  byte5: int = 0
  byte6: int = 0
  byte7: int = 0
  execute: bool = False
  data_length: int = 8
  error_msg: str = ""
  pending: bool = False
  time_stamp: int = 0
  fut: Optional[asyncio.Future] = None


@dataclass
class sMotorDriveParam:
  cmd: str = ""
  array_lower_bound: int = 0
  array_upper_bound: int = 0
  is_float: bool = False


@dataclass
class sNodeInputConfig:
  logic: eInputLogic = eInputLogic.GeneralPurpose
  logic_high: bool = True


class CanError(Exception):
  """Custom exception for CAN motor errors."""


class KX2Can:
  def __init__(self):
    # Lock objects
    self.query_wait_locker = asyncio.Lock()
    self.can_write_locker = asyncio.Lock()
    self.event_locker = asyncio.Lock()

    self.connecting: bool = False
    self.write_to_debug_trace_file: bool = False
    self.debug_trace_file: str = ""
    self.grp_id: int = 0
    self.grp_node_id: Optional[List[int]] = None

    # Event handling
    self.event_buffer: List[sEventData] = [sEventData() for _ in range(100)]
    self.event_buffer_first_index: int = 0
    self.event_buffer_last_index: int = 0

    # Error control
    self.err_ctrl: List[sErrCtrl] = [sErrCtrl() for _ in range(8)]

    # Move buffers
    self.move_buffer: List[sMoveData] = [sMoveData() for _ in range(100)]
    self.move_buffer_first_index: int = 0
    self.move_buffer_last_index: int = 0

    self.motor_move_param_buffer: List[sMotorMoveParam] = [sMotorMoveParam() for _ in range(100)]
    self.motor_move_param_buffer_index: int = 0

    self.motors_move_absolute_param_buffer: List[sMotorsMoveAbsoluteParam] = [
      sMotorsMoveAbsoluteParam() for _ in range(100)
    ]
    self.motors_move_absolute_param_buffer_index: int = 0

    self.emcy_move_error_received: bool = False
    self.emcy_move_error: str = ""
    self.emcy_move_error_node_id: int = 0
    self.move_error_code: int = 0
    self.move_error_index: int = 0

    self.motors_move_path_param_buffer: List[sMotorsMovePathParam] = [
      sMotorsMovePathParam() for _ in range(100)
    ]
    self.motors_move_path_param_buffer_index: int = 0

    self.pvt_time_interval_msec: int = 0
    self.pvt_stop: bool = False

    self.gc_joint_move_direction: Optional[List[eJointMoveDirection]] = None  # Array
    self.joint_move_direction_initialized: Optional[List[bool]] = None  # Array

    self.pvt_emcy: List[sPVT_EMCY] = [sPVT_EMCY() for _ in range(128)]
    self._pvt_mode: bool = False

    # CAN Device (python-can bus object)
    self._can_device: Optional[can.BusABC] = None
    self.can_device_type: List[int] = [  # Mapped from C# ushort values
      33,
      34,
      35,
      36,
      37,
      38,
      39,
      40,
      49,
      65,
      66,
      67,
      68,
      69,
      70,
      71,
      72,
      1033,
      1034,
      1035,
      1036,
      1037,
      1038,
      1039,
      81,
      82,
      83,
      84,
      85,
      86,
      87,
      88,
      1289,
      1290,
      1291,
      1292,
      1293,
      1294,
      1295,
      1296,
      97,
      98,
      2049,
      2050,
      2051,
      2052,
      2053,
      2054,
      2055,
      2056,
      2057,
      2058,
      2059,
      2060,
      2061,
      2062,
      2063,
      2064,
    ]

    # Asyncio event for message reception
    self.ev_msg_received = asyncio.Event()

    # Threading flags (for asyncio tasks)
    self._can_write_task_running: bool = False
    self.b_pvt_thread_started: bool = False
    self.b_motor_position_query_thread_running: bool = False
    self.b_event_thread_running: bool = False  # Added for event thread

    # Can message queues
    self._can_msg_queue_hp: asyncio.Queue[sCAN_Msg] = asyncio.Queue()
    self._can_msg_queue_lp: asyncio.Queue[sCAN_Msg] = asyncio.Queue()

    self.move_done_response: List[bool] = []  # Array
    self.move_done_time: Optional[List[int]] = None  # Array
    self.encoder_position: List[int] = []  # Array
    self.input_state: List[int] = []  # Array
    self.last_time_stamp: int = 0
    self.time_stamp_offset: int = 0
    self.node_id_list: Optional[List[int]] = None  # Array

    self.error_code: List[str] = [""] * 101

    # 3D array in C#: TPDOMappedObject[checked ((int) num1 + 1 + 1), 5, 1]
    # Equivalent to list of lists of lists, or dict-based access for sparse.
    # For now, initialize based on maximum possible node_id (assume 127 + 1 + 1, so 129), 5, 1
    self.tpdo_mapped_object: List[List[List[eTPDOMappedObject]]] = [
      [[eTPDOMappedObject.NotMapped for _ in range(1)] for _ in range(5)] for _ in range(129)
    ]

    self.motor_drive_param: List[sMotorDriveParam] = [sMotorDriveParam() for _ in range(56)]
    self.sending_move_command: bool = False
    self.sending_sv_command: bool = False

    # C#: NodeInputConfig[checked ((int) num1 + 1), 7] (max_node_id + 1, 7)
    # Assuming max_node_id is implicitly small, or determined later.
    # Initialize for typical node IDs (e.g., up to 8 max, for example)
    self.node_input_config: List[List[sNodeInputConfig]] = [
      [sNodeInputConfig() for _ in range(7)] for _ in range(129)
    ]  # Assuming max_Node_ID 128

    self.next_encoder_position: List[int] = [0] * 8

    self._init_error_codes()
    self._init_motor_drive_params()

    # Events (asyncio queues for simplicity, or signal dispatchers)
    self.on_motor_position_changed = asyncio.Queue()
    self.on_motion_status_received = asyncio.Queue()
    self.on_motor_enabled_status_received = asyncio.Queue()
    self.on_digital_input_changed_state = asyncio.Queue()
    self.on_can_emergency_message_received = asyncio.Queue()
    self.on_move_error = asyncio.Queue()
    self.on_drive_parameter_file_transfer_status = asyncio.Queue()
    self.on_motor_move_done = asyncio.Queue()
    self.on_motors_move_done = asyncio.Queue()
    self.on_motors_move_path_done = asyncio.Queue()

    #  Reading thread
    self._can_read_task_running: bool = True

    # OSQueryWaitBuffer
    self._os_query_wait_buffer: Dict[
      int, List[Tuple[sQuery, asyncio.Future]]
    ] = {}  # Keyed by node_id
    self._os_query_wait_buffer_lock = asyncio.Lock()

    self._query_wait_buffer: Dict[
      int, List[Tuple[sQuery, asyncio.Future]]
    ] = {}  # Keyed by node_id
    self._query_wait_buffer_lock = asyncio.Lock()

  @property
  def can_device(self) -> can.BusABC:
    if self._can_device is None:
      raise CanError("CAN device is not initialized.")
    return self._can_device

  def _init_error_codes(self):
    self.error_code[4] = "Unexpected CAN response: "
    self.error_code[5] = "CAN Status: "
    self.error_code[10] = "Motor fault.  "
    self.error_code[15] = "PVT fault.  "
    self.error_code[20] = "Motor failed to enable."
    self.error_code[25] = "Motor failed to disable."
    self.error_code[30] = "Motion timed out."
    self.error_code[35] = "Homing Failure 1: Failed to set Hard Stop Search Deceleration."
    self.error_code[36] = "Homing Failure 2: Failed to find hard stop."
    self.error_code[37] = "Homing Failure 3: Failed to stop after finding hard stop."
    self.error_code[38] = "Homing Failure 4: Hard stop search timed out."
    self.error_code[39] = "Homing Failure 5: Failed to move away from hard stop."
    self.error_code[40] = "Homing Failure 6: Failed to finish index pulse search."
    self.error_code[41] = "Homing Failure 7: Failed to move to index offset."
    self.error_code[43] = "Homing an axis equipped with a BiSS absolute encoder is not allowed."
    self.error_code[44] = "No CAN devices found."
    self.error_code[45] = "Failed to receive response from motor."
    self.error_code[46] = "Connection to CAN device is closed."
    self.error_code[
      50
    ] = "Total linear move time cannot exceed 16 seconds (255msec per time interval)."
    self.error_code[55] = "Unable to find the library PCANBasic.dll."
    self.error_code[60] = "Maximum allowed command length is 500 characters."
    self.error_code[65] = "Unexpected SDO Transfer Toggle value."
    self.error_code[70] = "Move aborted due to preexisting error state."

  def _init_motor_drive_params(self):
    params = [
      ("AB", 1, 6, False),
      ("AC", 0, 0, False),
      ("AG", 1, 2, True),
      ("AS", 1, 2, True),
      ("BP", 1, 2, False),
      ("CA", 1, 6, False),
      ("CA", 7, 7, True),
      ("CA", 8, 119, False),
      ("CL", 1, 3, True),
      ("DC", 0, 0, False),
      ("EF", 1, 2, False),
      ("EM", 1, 7, False),
      ("ER", 2, 3, False),
      ("ET", 1, 10, False),
      ("FF", 1, 2, True),
      ("FR", 1, 3, True),
      ("GS", 0, 2, False),
      ("GS", 4, 5, False),
      ("GS", 9, 10, False),
      ("GS", 14, 15, False),
      ("HL", 2, 3, False),
      ("IF", 1, 10, True),
      ("IL", 1, 10, False),
      ("KG", 1, 10, True),
      ("KG", 190, 191, True),
      ("KI", 1, 2, True),
      ("KP", 1, 3, True),
      ("KV", 1, 99, True),
      ("LL", 2, 3, False),
      ("OL", 1, 6, False),
      ("PL", 1, 2, True),
      ("PM", 0, 0, False),
      ("PP", 1, 2, False),
      ("PP", 4, 4, False),
      ("PP", 13, 15, False),
      ("PW", 1, 2, True),
      ("RM", 0, 0, False),
      ("SD", 0, 0, False),
      ("SF", 0, 0, False),
      ("SP", 1, 4, False),
      ("TR", 1, 2, False),
      ("TS", 0, 0, False),
      ("UF", 1, 24, True),
      ("UI", 1, 24, False),
      ("UM", 0, 0, False),
      ("US", 1, 4, False),
      ("VH", 2, 3, False),
      ("VL", 2, 3, False),
      ("XA", 1, 4, False),
      ("XM", 1, 2, False),
      ("XP", 1, 2, False),
      ("XP", 4, 7, False),
      ("YA", 1, 1, False),
      ("YA", 4, 5, False),
      ("YM", 1, 2, False),
      ("ZF", 1, 24, True),
    ]
    for i, (cmd, lower, upper, is_float) in enumerate(params):
      self.motor_drive_param[i] = sMotorDriveParam(cmd, lower, upper, is_float)

  async def _can_write_task(self):
    print("CAN Write Task started")
    self._can_write_task_running = True

    while self._can_write_task_running:
      message: Optional[sCAN_Msg] = None
      try:
        message = self._can_msg_queue_hp.get_nowait()
      except asyncio.QueueEmpty:
        try:
          message = self._can_msg_queue_lp.get_nowait()
        except asyncio.QueueEmpty:  # No messages to send
          await asyncio.sleep(0.001)
          continue

      if message.fut is not None and message.fut.done():
        continue  # Skip if future is already done

      msg_id = (message.cob.value << 7) + message.node_id
      data_bytes = [
        message.byte0,
        message.byte1,
        message.byte2,
        message.byte3,
        message.byte4,
        message.byte5,
        message.byte6,
        message.byte7,
      ][: message.data_length]

      try:
        can_msg = can.Message(
          arbitration_id=msg_id,
          data=data_bytes,
          is_extended_id=False,  # Assuming standard IDs for CANopen
        )

        await asyncio.to_thread(self.can_device.send, can_msg)
        if message.fut and not message.fut.done():
          message.fut.set_result(None)
      except Exception as e:
        error_msg = f"CAN Write Error: {e}"
        print(f"CAN Write Error: {e}")
        if message.fut and not message.fut.done():
          message.fut.set_exception(CanError(error_msg))

  async def _process_emcy_message(self, node_id: int, message: can.Message) -> None:
    print("EMCY received!!")

    data = message.data
    if not data or len(data) < 8:
      print(f"EMCY malformed: expected 8 bytes, got {0 if not data else len(data)}")
      return

    def u16_le(i: int) -> int:
      return int(data[i]) | (int(data[i + 1]) << 8)

    emcy = sEmcy()
    emcy.err_code = u16_le(0)
    emcy.err_reg = int(data[2])
    emcy.elmo_err_code = int(data[3])
    emcy.err_code_data1 = u16_le(4)
    emcy.err_code_data2 = u16_le(6)

    self.last_emcy = emcy

    desc = ""
    disable_motors = False   # VB flag1
    suppress_event = False   # VB flag2

    err = emcy.err_code
    elmo = emcy.elmo_err_code

    # Simple (err_code -> (description, disable_motors))
    ERR_MAP: dict[int, tuple[str, bool]] = {
      0x8110: ("CAN message lost (corrupted or overrun)", False),
      0x8200: ("Protocol error (unrecognized NMT request)", False),
      0x8210: ("Attempt to access an unconfigured RPDO", False),
      0x8130: ("Heartbeat event", False),
      0x6180: ("Fatal CPU error: stack overflow", False),  # de-duped from VB snippet
      0x6181: ("CPU exception: fatal exception", False),
      0x6200: ("User program aborted by an error", False),
      0xFF01: ("Request by user program 'emit' function", False),
      0x6300: ("Object mapped to an RPDO returned an error during interpretation or a referenced motion failed to be performed", False),

      0x7300: ("Resolver or Analog Encoder feedback failed", True),
      0x7380: ("Feedback loss: no match between encoder and Hall locations.", True),
      0x8311: ("Peak current has been exceeded due to drive malfunction or badly-tuned current controller", True),
      0x5441: ("E-stop button was pressed", True),
      0x5280: ("ECAM table problem", False),
      0x7381: ("Two digital Hall sensors changed at once; only one sensor can be changed at a time", True),
      0x8480: ("Speed tracking error", True),
      0x8611: ("Position tracking error", True),
      0x6320: ("Cannot start due to inconsistent database", False),
      0x8380: ("Cannot find electrical zero of motor when attempting to start motor", False),
      0x8481: ("Speed limit exceeded", True),
      0x5281: ("Timing error", False),
      0x7121: ("Motor stuck: motor powered but not moving", True),
      0x8680: ("Position limit exceeded", True),
      0x8381: ("Cannot tune current offsets", False),
      0xFF10: ("Cannot start motor", False),
      0x5400: ("Cannot start motor", False),
      0x3120: ("Under-voltage: power supply is shut down or it has too high an output impedance", True),
      0x3310: ("Over-voltage: power supply voltage is too high or servo driver could not absorb kinetic energy while braking a load", True),
      0x2340: ("Short circuit: motor or its wiring may be defective, or drive is faulty", True),
      0x4310: ("Temperature: drive overheating", True),
      0xFF20: ("Safety switch is sensed - Drive in safety state", True),
    }

    if err == 0xFF00:
      if elmo == 0x56:
        st = self.pvt_emcy[node_id]
        st.queue_low.state = True
        st.queue_low.write_pointer = emcy.err_code_data1
        st.queue_low.read_pointer = emcy.err_code_data2
        desc = "Queue Low"
      elif elmo == 0x5B:
        self.pvt_emcy[node_id].bad_head_pointer = True
        desc = "Bad Head Pointer"
      elif elmo == 0x34:
        st = self.pvt_emcy[node_id]
        st.queue_full.state = True
        st.queue_full.failed_write_pointer = emcy.err_code_data1
        desc = "Queue Full"
      elif elmo == 0x07:
        self.pvt_emcy[node_id].bad_mode_init_data = True
        desc = "Bad Mode Init Data"
      elif elmo == 0x08:
        self.pvt_emcy[node_id].motion_terminated = True
        desc = "Motion Terminated"
      elif elmo == 0xA6:
        self.pvt_emcy[node_id].out_of_modulo = True
        desc = "Out Of Modulo"
      else:
        desc = f"DS402 IP Error {elmo}"

    elif err == 0xFF02:
      if elmo == 0x07:
        self.pvt_emcy[node_id].bad_mode_init_data = True
        desc = "Bad Mode Init Data"
      elif elmo == 0x08:
        self.pvt_emcy[node_id].motion_terminated = True
        desc = "Motion Terminated"
      elif elmo == 0x34:
        st = self.pvt_emcy[node_id]
        st.queue_full.state = True
        st.queue_full.failed_write_pointer = emcy.err_code_data1
        desc = "Queue Full"
      elif elmo == 0x56:
        st = self.pvt_emcy[node_id]
        st.queue_low.state = True
        st.queue_low.write_pointer = emcy.err_code_data1
        st.queue_low.read_pointer = emcy.err_code_data2
        desc = "Queue Low"
      elif elmo == 0x5B:
        self.pvt_emcy[node_id].bad_head_pointer = True
        desc = "Bad Head Pointer"
      elif elmo == 0x8A:
        self.pvt_emcy[node_id].queue_low.state = True
        desc = "Position Interpolation buffer underflow"
        suppress_event = True
      elif elmo == 0xA6:
        self.pvt_emcy[node_id].out_of_modulo = True
        desc = "Out Of Modulo"
      elif elmo == 0xBA:
        self.pvt_emcy[node_id].queue_full.state = True
        desc = "Interpolation queue is full"
      elif elmo == 0xBB:
        desc = "Incorrect interpolation sub-mode"
      else:
        desc = f"DS402 IP Error {elmo}"

    else:
      mapped = ERR_MAP.get(err)
      if mapped:
        desc, disable_motors = mapped
      else:
        desc = f"Unknown EMCY err={err} elmo={elmo}"

    if disable_motors:
      self.emcy_move_error_received = True
      self.emcy_move_error = desc
      self.emcy_move_error_node_id = node_id

    if not suppress_event:
      event = EventData()
      event.event_type = eEventType.CANEmergencyMessageReceived
      event.node_id = node_id
      event.cemr_description = desc
      event.cemr_emcy_msg = emcy
      event.cemr_disable_motors = disable_motors
      self.raise_an_event(event)

    print(emcy)
    print(f"EMCY node={node_id} desc='{desc}' disable_motors={disable_motors} suppress_event={suppress_event}")

  async def _process_tpdo_message(self, node_id: int, message: can.Message, response_type: int):
    tpdo_index = {0: eTPDO.TPDO1, 4: eTPDO.TPDO3, 6: eTPDO.TPDO4}[response_type - 3]

    if self.node_id_list is None or node_id not in self.node_id_list:
      return

    if self.tpdo_mapped_object is None:
      return

    num2 = 0

    node_idx = self.node_id_list.index(node_id)

    for i in range(len(self.tpdo_mapped_object[node_id][tpdo_index])):
      mapped = self.tpdo_mapped_object[node_id][tpdo_index][i]

      # read 16-bit or 32-bit value depending on flags
      num4_: Optional[int] = None
      if mapped.value & 0x10 == 0x10:  # 16 bit
        num4_ = (message.data[num2 + 1] << 8) | message.data[num2]
        num2 += 2
      if mapped.value & 0x20 == 0x20:  # 32 bit
        num4_ = (
          (message.data[num2 + 3] << 24)
          | (message.data[num2 + 2] << 16)
          | (message.data[num2 + 1] << 8)
          | message.data[num2]
        )
        num2 += 4
      if num4_ is None:
        # print("Failed to read TPDO mapped object value, probably fine....")
        # raise CanError("Failed to read TPDO mapped object value.")
        continue
      num4 = num4_

      if mapped == eTPDOMappedObject.Timestamp:
        self.last_time_stamp = num4
      elif mapped == eTPDOMappedObject.StatusWord:
        print(f"StatusWord received from node {node_id}: motor move done", time.time())
        self.move_done_response[node_idx] = True

        event_data = sEventData(
          event_type=eEventType.MotionStatusReceived,
          node_id=node_id,
          msr_status=True,
          msr_status_word=int(num4 & 0xFFFF),  # short
        )
        await self._raise_an_event(event_data)
      elif mapped == eTPDOMappedObject.PositionActualValue:
        # signed 32-bit conversion
        if num4 <= 0x7FFFFFFF:
          num5 = int(num4)
        else:
          num5 = int(num4 - 0x100000000)

        if self.encoder_position[node_idx] != num5:
          self.encoder_position[node_idx] = num5

          positions = list(self.encoder_position)
          positions[node_idx] = num5
          event_data = sEventData(
            event_type=eEventType.MotorPositionChanged,
            node_id=node_id,
            mpc_position=positions,
          )

          await self._raise_an_event(event_data)
      elif mapped == eTPDOMappedObject.DigitalInputs:
        # temp array for digital inputs (indices 16..21 used)
        num_array = [0] * 22  # 0..21

        # clear bits 16..21??
        for i in range(16, 21 + 1):
          num_array[i] = 0

        # set bits from num4
        if num4 & (1 << 16):
          num_array[16] = 1
        if num4 & (1 << 17):
          num_array[17] = 1
        if num4 & (1 << 18):
          num_array[18] = 1
        if num4 & (1 << 19):
          num_array[19] = 1
        if num4 & (1 << 20):
          num_array[20] = 1
        if num4 & (1 << 21):
          num_array[21] = 1

        # num6: packed 6-bit value
        num6 = (
          num_array[16]
          + num_array[17] * 2
          + num_array[18] * 4
          + num_array[19] * 8
          + num_array[20] * 16
          + num_array[21] * 32
        )

        if self.input_state[node_idx] != num6:
          # check for transitions that imply move done
          for index4 in range(1, 7):  # 1..6
            # previous bit for this input (0/1)
            prev_bit = 1 if (self.input_state[node_idx] & (1 << (index4 - 1))) else 0
            new_bit = num_array[15 + index4]

            cfg = self.node_input_config[node_idx][index4]
            logic = cfg.logic

            edge = prev_bit != new_bit
            logic_enabled = logic in (
              eInputLogic.EnableForwardOnly,
              eInputLogic.EnableReverseOnly,
            )

            if edge and logic_enabled and new_bit == 1:
              self.next_encoder_position[node_id] = -2000000000
              # this.MoveDoneResponse[GetNodeIDIndex(index1)] = true;
              self.move_done_response[node_idx] = True
              print(f"Digital input {index4} enabled motor move done for node {node_id}")

          self.input_state[node_idx] = num6

          # index 0 unused, 1..6 valid
          dics_state = [False] * 7
          dics_state[1] = num_array[16] > 0
          dics_state[2] = num_array[17] > 0
          dics_state[3] = num_array[18] > 0
          dics_state[4] = num_array[19] > 0
          dics_state[5] = num_array[20] > 0
          dics_state[6] = num_array[21] > 0

          # raise digital input change event
          event_data = sEventData(
            event_type=eEventType.DigitalInputChangedState,
            node_id=node_id,
            dics_state=dics_state,
          )

          await self._raise_an_event(event_data)
  
  async def _process_binary_interpreter_response(self, node_id: int, message: can.Message) -> None:
    data = message.data
    if not data or len(data) < 8:
      raise CanError("Invalid binary interpreter response data.")

    msg_type = chr(data[0]) + chr(data[1])

    # Build index: lower 8 bits from DATA[2], upper 6 bits from DATA[3]
    msg_index = (((data[3] & 0b0011_1111) << 8) | data[2])

    is_int = (data[3] & 0x80) == 0

    if is_int:
      raw = bytes(data[4:8])
      # value_int = struct.unpack("<I", raw)[0]
      value_int = struct.unpack("<i", raw)[0]
      value_str = str(value_int)
    else:
      raw = bytes(data[4:8])
      (value_float,) = struct.unpack("<f", raw)
      value_str = str(value_float)

    async with self._query_wait_buffer_lock:
      entries = self._query_wait_buffer.get(node_id)
      if not entries:
        return

      def iter_entries():
        for query, fut in entries:
          if fut.done():
            continue
          if query.node_id != node_id:
            continue
          yield query, fut

      for query, fut in iter_entries():
        if (
          query.msg_index == msg_index
          and query.msg_type == msg_type
        ):
          # Original code put the string result into Response
          fut.set_result(value_str)

      # Optional: cleanup finished futures
      self._query_wait_buffer[node_id] = [
        (q, f) for (q, f) in entries if not f.done()
      ]
      if not self._query_wait_buffer[node_id]:
        del self._query_wait_buffer[node_id]

  async def _process_sdo_response(self, node_id: int, message: can.Message) -> None:
    data = message.data
    if not data:
      return

    cmd = data[0]
    length = len(data)

    async with self._os_query_wait_buffer_lock:
      entries = self._os_query_wait_buffer.get(node_id)
      if not entries:
        return

      # Helper: find all matching queries (we may set multiple futures)
      def iter_entries():
        for query, fut in entries:
          if fut.done():
            continue
          if query.node_id != node_id:
            continue
          yield query, fut

      # ---- 1) SDO abort (0x80) ----
      if cmd == 0x80 and length >= 8:
        abort_code = (
          data[4]
          | (data[5] << 8)
          | (data[6] << 16)
          | (data[7] << 24)
        )

        try:
          # If you have an equivalent of GetSDOAbortMessage(...)
          msg = self.get_sdo_abort_message(abort_code)  # type: ignore[attr-defined]
        except AttributeError:
          msg = f"SDO abort 0x{abort_code:08X}"

        for query, fut in iter_entries():
          if (
            query.object_byte0 == data[2]
            and query.object_byte1 == data[1]
            and query.sub_index == data[3]
          ):
            fut.set_exception(CanError(msg))

      # ---- 2) SDO download initiate response (0x60) -> "SDODI" ----
      if cmd == 0x60 and length >= 4:
        for query, fut in iter_entries():
          if (
            query.object_byte0 == data[2]
            and query.object_byte1 == data[1]
            and query.sub_index == data[3]
            and query.msg_type == "SDODI"
          ):
            fut.set_result("SDODI")

      # ---- 3) "CHR" response: 0x20/0x30 and index bytes zero ----
      if cmd in (0x20, 0x30) and length >= 4 and data[1] == 0 and data[2] == 0 and data[3] == 0:
        for query, fut in iter_entries():
          if query.msg_type == "CHR":
            fut.set_result("CHR")

      # ---- 4) "STAT" response ----
      # DATA[0] == 79 (0x4F), DATA[1] == 35 (0x23), DATA[2] == 0x10, DATA[3] == 2
      if length >= 5 and cmd == 0x4F and data[1] == 0x23 and data[2] == 0x10 and data[3] == 2:
        stat_value = data[4]
        for query, fut in iter_entries():
          if query.msg_type == "STAT":
            fut.set_result(f"STAT{stat_value}")

      # ---- 5) "VAL" response ----
      # DATA[0] == 67 (0x43), DATA[1] == 35 (0x23), DATA[2] == 0x10, DATA[3] == 3
      if length >= 4 and cmd == 0x43 and data[1] == 0x23 and data[2] == 0x10 and data[3] == 3:
        if length > 4:
          tail = ",".join(str(b) for b in data[4:length])
          val_response = f"VAL,{tail}"
        else:
          val_response = "VAL"

        for query, fut in iter_entries():
          if query.msg_type == "VAL":
            fut.set_result(val_response)

      # ---- 6) SDO upload (expedited) "SDOUA" ----
      if (cmd & 0x40) == 0x40 and length >= 4:
        # Decode bits according to CANopen-style layout:
        # n = number of unused bytes in 4-byte data field
        # e = (usually) expedited bit; here used to choose numeric vs string
        n = (cmd >> 2) & 0x03
        e = (cmd >> 1) & 0x01
        # s = cmd & 0x01  # not needed here

        used = max(0, min(4 - n, length - 4))
        raw_bytes = data[4 : 4 + used]

        for query, fut in iter_entries():
          if (
            query.msg_type == "SDOUA"
            and query.object_byte0 == data[2]
            and query.object_byte1 == data[1]
            and query.sub_index == data[3]
          ):
            if e == 0:
              # Numeric value: sum(data[i] * 256^(i-1))
              value = 0
              for i, b in enumerate(raw_bytes):
                value |= int(b) << (8 * i)
              fut.set_result(f"SDOUAN{value}")
            else:
              # String value: concatenate chars from data bytes
              text = "".join(chr(b) for b in raw_bytes)
              fut.set_result(f"SDOUAE{text}")

      # ---- 7) SDO segmented upload "SDOSU" ----
      if (cmd & 0x20) == 0 and (cmd & 0x40) == 0 and (cmd & 0x80) == 0 and length >= 1:
        # Again, follow CANopen-ish semantics:
        # toggle bit, n = number of unused bytes (among 7), last-segment bit.
        toggle = (cmd >> 4) & 0x01
        n = (cmd >> 1) & 0x07
        last = cmd & 0x01

        used = max(0, min(7 - n, length - 1))
        payload = data[1 : 1 + used]
        payload_str = "".join(chr(b) for b in payload)

        for query, fut in iter_entries():
          if query.msg_type == "SDOSU":
            prefix = "SDOSUC" if last == 0 else "SDOSUD"
            fut.set_result(f"{prefix}{toggle}{payload_str}")

      # ---- Cleanup: drop finished futures from buffer ----
      self._os_query_wait_buffer[node_id] = [
        (q, f) for (q, f) in entries if not f.done()
      ]
      if not self._os_query_wait_buffer[node_id]:
        del self._os_query_wait_buffer[node_id]
  
  async def _process_motor_drive_restarted(self, node_id: int, message: can.Message):
    print(f"Motor drive restarted message received from Node {node_id}")

    self.err_ctrl[node_id].data_byte = list(message.data)
    if not self.connecting:
      event = sEventData(
        event_type=eEventType.CANEmergencyMessageReceived,
        node_id=node_id,
        cemr_description="Node Guarding error. Motor drive restarted spontaneously.",
        cemr_disable_motors=True,
      )
      await self._raise_an_event(event)

      await self.can_write(cob=eCOBType.NMT, node_id=0, byte0=0x01, byte1=node_id)
      await self.can_tpdo_unmap(tpdo=eTPDO.TPDO1, node_id=node_id)
      await self.can_tpdo3_map(node_id=node_id)
      await self.can_tpdo4_map(node_id=node_id)

  async def _can_read_task(
    self,
  ):
    print("CAN Read Task started")

    while self._can_read_task_running:
      # receive message from CAN bus
      try:
        message = await asyncio.to_thread(self.can_device.recv, timeout=1.0)
      except Exception as e:
        print(f"CAN Read Error: {e}")
        raise CanError(f"CAN Read Error: {e}")

      if message is None:  # timeout
        continue

      response_type = message.arbitration_id >> 7
      response_type_c = round(message.arbitration_id / 128)
      assert (
        response_type == response_type_c
      ), f"Response type calculation mismatch: {response_type} != {response_type_c}"
      node_id = message.arbitration_id & 0x7F
      node_id_c = message.arbitration_id - (response_type * 128)
      assert node_id == node_id_c, f"Node index calculation mismatch: {node_id} != {node_id_c}"

      if response_type == 0:
        print("NMT message received, ignoring")
      elif response_type == 1:
        await self._process_emcy_message(node_id=node_id, message=message)
      elif response_type in {3, 7, 9}:
        await self._process_tpdo_message(
          node_id=node_id, message=message, response_type=response_type
        )
      elif response_type == 5:
        await self._process_binary_interpreter_response(node_id=node_id, message=message)
      elif response_type == 11:
        await self._process_sdo_response(node_id=node_id, message=message)
      elif response_type == 14:
        await self._process_motor_drive_restarted(node_id=node_id, message=message)
      else:
        # ?
        print(f"Unknown CAN message type received: {response_type}")

  async def can_write(
    self,
    cob: eCOBType,
    node_id: int,
    byte0: int = 0,
    byte1: int = 0,
    byte2: int = 0,
    byte3: int = 0,
    byte4: int = 0,
    byte5: int = 0,
    byte6: int = 0,
    byte7: int = 0,
    execute: bool = False,
    data_length: int = 8,
    low_priority: bool = False,
    time_stamp: int = 0,
  ) -> None:
    """Queues a CAN message for transmission."""

    fut = asyncio.get_event_loop().create_future()

    msg_entry = sCAN_Msg(
      cob=cob,
      node_id=node_id,
      byte0=byte0,
      byte1=byte1,
      byte2=byte2,
      byte3=byte3,
      byte4=byte4,
      byte5=byte5,
      byte6=byte6,
      byte7=byte7,
      execute=execute,
      data_length=data_length,
      error_msg="",
      pending=True, # Still use pending for internal state if needed elsewhere, but fut is primary
      time_stamp=time_stamp,
      fut=fut,
    )

    if low_priority:
      await self._can_msg_queue_lp.put(msg_entry)
    else:
      await self._can_msg_queue_hp.put(msg_entry)

    timeout_sec = 5.0  # 5000ms
    try:
      await asyncio.wait_for(fut, timeout=timeout_sec)
    except asyncio.TimeoutError:
      raise CanError(
        f"Failed to send CAN message {cob.name},{node_id},... Low Priority = {low_priority} (timeout)"
      )
    except Exception as e:
      raise CanError(str(e))

  async def connect(
    self,
    device: int = 0,
    group_id: int = 0,
    group_node_id: Optional[List[int]] = None,
    baud_rate: int = 500000,
    nodes_found: bool = False,
  ) -> None:
    """
    Connects or disconnects to/from the CAN bus.
    """
    self.emcy_move_error_received = False
    self.emcy_move_error = ""
    self.sending_move_command = False
    self.sending_sv_command = False
    self.move_error_code = 0
    self.grp_id = group_id
    self.pvt_time_interval_msec = 0

    # Handle NodeID parameter list
    self.node_id_list = [1, 2, 3, 4, 5, 6]

    if group_node_id is None:
      self.grp_node_id = [1, 2, 3, 4, 5]
    else:
      self.grp_node_id = list(group_node_id)

    # Determine max_node_id for array sizing if dynamic behavior is needed
    max_node_id = max(self.node_id_list)
    if self.grp_node_id:
      max_node_id = max(max_node_id, max(self.grp_node_id))

    # Resize buffers based on max_node_id
    buffer_size = (
      max_node_id + 1 if max_node_id > 0 else 8
    )

    self.move_done_response = [False] * (buffer_size + 1)
    self.move_done_time = [0] * (buffer_size + 1)
    self.input_state = [0] * (buffer_size + 1)
    self.encoder_position = [0] * (buffer_size + 1)
    self.gc_joint_move_direction = [eJointMoveDirection.ShortestWay] * (buffer_size + 1)
    self.joint_move_direction_initialized = [False] * (buffer_size + 1)
    self.node_input_config = [
      [sNodeInputConfig() for _ in range(7)] for _ in range(buffer_size + 1)
    ]
    for i in range(buffer_size + 1):
      for j in range(1, 7):
        self.node_input_config[i][j].logic = eInputLogic.GeneralPurpose
        self.node_input_config[i][j].logic_high = True

    # Abort existing threads/tasks if running
    if self.b_motor_position_query_thread_running:
      self.b_motor_position_query_thread_running = False
      # The actual asyncio task needs to be cancelled.
      # Assuming task handle is stored, e.g., self._motor_position_query_task.cancel()

    # Initialize python-can bus
    try:
      self._can_device = can.Bus(
        interface="pcan",  # Or 'usbcan', 'kvaser', etc. based on setup
        channel=None,  # e.g., 'PCAN_USBBUS1' or int 0 for default
        bitrate=baud_rate,
        is_extended_id=False,
      )
    except Exception as e:
      raise CanError(f"Failed to initialize CAN device: {e}")

    # Start asyncio tasks
    # self.move_buffer_first_index = self.move_buffer_last_index  # Reset buffer
    # for i in range(100):  # Reset pending flags
    #   self.move_buffer[i].pending = False

    # EventLaunchThread
    asyncio.create_task(self._event_thread())

    self.ev_msg_received.clear()  # Clear initial state

    # CAN_ReadThread
    asyncio.create_task(self._can_read_task())

    # CAN_WriteThread
    asyncio.create_task(self._can_write_task())

    await asyncio.sleep(0.01)

    # Error control
    # self.err_ctrl is already initialized based on max_node_id

    # --- CANopen Initialization Sequence ---
    self.connecting = True
    # NMT Reset Nodes (0x80)
    await self.can_write(cob=eCOBType.NMT, node_id=0, byte0=0x82)

    await asyncio.sleep(0.5)

    # NMT Start Nodes (0x01)
    await self.can_write(cob=eCOBType.NMT, node_id=0, byte0=0x01)
    await asyncio.sleep(0.1)
    self.connecting = False

    arySrc = []

    for i in range(len(self.err_ctrl)):
      if self.err_ctrl[i].data_byte is not None:
        arySrc.append(i)

    if len(arySrc) == 0:
      raise CanError("No nodes found on the CAN bus.")
    else:
      self.grp_node_id = arySrc
      self.node_id_list = arySrc

  async def connect_part_two(self):
    """After setting up the threads and can connection, and receiving node IDs, map the things. this is flag=True"""

    max_node_id = max(self.node_id_list)

    self.tpdo_mapped_object = [
      [[eTPDOMappedObject.NotMapped for _ in range(1)] for _ in range(5)]
      for _ in range(max_node_id + 2)
    ]

    for node_id in self.node_id_list:
      await self.can_tpdo_unmap(eTPDO.TPDO1, node_id)

      await self.can_tpdo3_map(node_id)
      await self.can_tpdo4_map(node_id)

    # MotorPositionQueryThread
    # if self.node_id_list:
    #   asyncio.create_task(self._motor_position_thread())

    # Configure Elmo objects if group_node_id is set
    # This section involves can_sdo_download_ElmoObject which needs to be translated first.
    for idx, node_id in enumerate(self.grp_node_id):
      # 24768, sub 0, INTEGER16, data = "-1"
      await self.can_sdo_download_elmo_object(
        node_id=int(node_id),
        elmo_object_int=24768,
        sub_index=0,
        data=str(-1),
        data_type=eElmoObjectDataType.INTEGER16,
      )

      # 24772, sub 2, UNSIGNED32, data = "16"
      await self.can_sdo_download_elmo_object(
        node_id=int(node_id),
        elmo_object_int=24772,
        sub_index=2,
        data=str(16),
        data_type=eElmoObjectDataType.UNSIGNED32,
      )

      # 24772, sub 3, UNSIGNED8, data = "0"
      await self.can_sdo_download_elmo_object(
        node_id=int(node_id),
        elmo_object_int=24772,
        sub_index=3,
        data=str(0),
        data_type=eElmoObjectDataType.UNSIGNED8,
      )

      # 24772, sub 5, UNSIGNED8, data = "8"
      await self.can_sdo_download_elmo_object(
        node_id=int(node_id),
        elmo_object_int=24772,
        sub_index=5,
        data=str(8),
        data_type=eElmoObjectDataType.UNSIGNED8,
      )

      node_id5 = int(self.grp_node_id[idx])
      await self.can_sdo_download_elmo_object(
        node_id=node_id5,
        elmo_object_int=24770,
        sub_index=2,
        data="-3",
        data_type=eElmoObjectDataType.INTEGER8,
      )

      # 24669, sub 0, INTEGER16, data = "1"
      await self.can_sdo_download_elmo_object(
        node_id=int(node_id),
        elmo_object_int=24669,
        sub_index=0,
        data=str(1),
        data_type=eElmoObjectDataType.INTEGER16,
      )

    for node_id in self.grp_node_id:
      await self.can_rpdo1_map(node_id)
      await self.can_rpdo3_map(node_id)

    # PVT Mode setup
    self._pvt_mode = True
    await self.pvt_select_mode(False)

  async def disconnect(self) -> None:
    self.b_motor_position_query_thread_running = False
    self._can_read_task_running = False
    self._can_write_task_running = False
    self.b_event_thread_running = False

    if self._can_device is not None:
      self._can_device.shutdown()
      self._can_device = None

  async def _event_thread(self):
    print("Event Thread Started")
    self.b_event_thread_running = True
    while self.b_event_thread_running:
      while self.event_buffer_first_index != self.event_buffer_last_index:
        if self.event_buffer_first_index == 99:
          self.event_buffer_first_index = 0
        else:
          self.event_buffer_first_index += 1

        event_data = self.event_buffer[self.event_buffer_first_index]

        if event_data.pending:
          event_data.pending = False  # Mark as processed

          if event_data.event_type == eEventType.MotorPositionChanged:
            await self.on_motor_position_changed.put((event_data.node_id, event_data.mpc_position))
          elif event_data.event_type == eEventType.MotionStatusReceived:
            await self.on_motion_status_received.put(
              (event_data.node_id, event_data.msr_status, event_data.msr_status_word)
            )
          elif event_data.event_type == eEventType.MotorEnabledStatusReceived:
            await self.on_motor_enabled_status_received.put(
              (event_data.node_id, event_data.mesr_status)
            )
          elif event_data.event_type == eEventType.DigitalInputChangedState:
            await self.on_digital_input_changed_state.put(
              (event_data.node_id, event_data.dics_state)
            )
          elif event_data.event_type == eEventType.CANEmergencyMessageReceived:
            await self.on_can_emergency_message_received.put(
              (
                event_data.node_id,
                event_data.cemr_emcy_msg,
                event_data.cemr_description,
                event_data.cemr_disable_motors,
              )
            )
          elif event_data.event_type == eEventType.MoveError:
            await self.on_move_error.put((event_data.me_error_code, event_data.me_index))
          elif event_data.event_type == eEventType.MotorMoveDone:
            await self.on_motor_move_done.put(
              (event_data.node_id, event_data.me_index, event_data.mmd_all_moves_done)
            )
          elif event_data.event_type == eEventType.MotorsMoveDone:
            await self.on_motors_move_done.put((event_data.me_index, event_data.mmd_all_moves_done))
          elif event_data.event_type == eEventType.MotorsMovePathDone:
            await self.on_motors_move_path_done.put(
              (event_data.me_index, event_data.mmd_all_moves_done)
            )
      await asyncio.sleep(0.01)
    print("Event Thread Stopped")

  async def _raise_an_event(self, event_data: sEventData):
    """
    Queues an event to be processed by the event thread.
    """
    print(f"Raising event: {event_data}")

    async with self.event_locker:
      new_index = (self.event_buffer_last_index + 1) % 100

      # Check if buffer is full, for simplicity, overwrite oldest if full, or log error
      if new_index == self.event_buffer_first_index and self.event_buffer[new_index].pending:
        print("Event buffer full, overwriting oldest event.")
        # In a real system, you might want to increase buffer size or handle differently

      self.event_buffer[new_index] = event_data
      event_data.pending = True  # Mark the queued event as pending
      self.event_buffer_last_index = new_index

  # --- Next, translate helper methods and core functionalities ---

  def get_sdo_abort_message(self, code: int) -> str:
    sdo_abort_message = f"Unknown error {code:#010x}."  # Print hex directly {code:X8} in C#
    if code == 0x05030000:
      sdo_abort_message = "Toggle bit not alternated."
    elif code == 0x05040001:
      sdo_abort_message = "Invalid or unknown client/server command specifier."
    elif code == 0x05040002:
      sdo_abort_message = "Invalid block size."
    elif code == 0x05040003:
      sdo_abort_message = "Invalid sequence number in SDO block upload."
    elif code == 0x05040005:
      sdo_abort_message = "Out of memory."
    elif code == 0x06010000:
      sdo_abort_message = "Unsupported access to an object."
    elif code == 0x06010001:
      sdo_abort_message = "Attempt to read a write-only object."
    elif code == 0x06010002:
      sdo_abort_message = "Attempt to write a read-only object."
    elif code == 0x06020000:
      sdo_abort_message = "Object does not exist in object dictionary."
    elif code == 0x06040041:
      sdo_abort_message = "Object cannot be mapped to PDO."
    elif code == 0x06040042:
      sdo_abort_message = "Number and length of objects to be mapped exceeds PDO length."
    elif code == 0x06040043:
      sdo_abort_message = "General parameter incompatibility."
    elif code == 0x06060000:
      sdo_abort_message = "Access failed due to hardware error."
    elif code == 0x06070012:
      sdo_abort_message = "Data type does not match. Service parameter too long."
    elif code == 0x06090011:
      sdo_abort_message = "Sub-index does not exist."
    elif code == 0x06090030:
      sdo_abort_message = "Value range of parameter exceeded (only for write access)."
    elif code == 0x06090031:
      sdo_abort_message = "Value of parameter written too high."
    elif code == 0x06090032:
      sdo_abort_message = "Value of parameter written too low."
    elif code == 0x06090036:
      sdo_abort_message = "Maximum value is less than minimum value."
    elif code == 0x08000000:
      sdo_abort_message = "General error. Use the EC command to retrieve the actual error."
    elif code == 0x08000020:
      sdo_abort_message = "Data cannot be transferred to or stored in application."
    elif code == 0x08000022:
      sdo_abort_message = (
        "Data cannot be transferred to or stored in application due to present device state."
      )
    elif code == 0x08000024:
      sdo_abort_message = "There is no data available to transmit."
    return sdo_abort_message

  async def _add_os_query_wait_buffer(self, query):
    """
    Adds a new query to the OS query wait buffer and returns a Future for awaiting the response.
    The Future will be set by the reading thread when the response is received.
    """
    fut = asyncio.get_event_loop().create_future()
    async with self._os_query_wait_buffer_lock:
      if query.node_id not in self._os_query_wait_buffer:
        self._os_query_wait_buffer[query.node_id] = []
      self._os_query_wait_buffer[query.node_id].append((query, fut))
    return fut

  async def _add_query_wait_buffer(self, query):
    """
    Adds a new query to the query wait buffer and returns a Future for awaiting the response.
    The Future will be set by the reading thread when the response is received.
    """
    fut = asyncio.get_event_loop().create_future()
    async with self._query_wait_buffer_lock:
      if query.node_id not in self._query_wait_buffer:
        self._query_wait_buffer[query.node_id] = []
      elif self._query_wait_buffer[query.node_id]:
        print("NODE ID already in query wait buffer", self._query_wait_buffer[query.node_id])
        # exit()
      self._query_wait_buffer[query.node_id].append((query, fut))
    return fut

  async def can_sdo_upload(
    self,
    node_id: int,
    object_byte0: int,
    object_byte1: int,
    sub_index: int,
  ) -> bytes:
    """can_sdo_upload (read). Sends an SDO Upload request and waits for the response."""

    query = sQuery(
      node_id=node_id,
      object_byte0=object_byte0,
      object_byte1=object_byte1,
      sub_index=sub_index,
      msg_type="SDOUA",  # SDO Upload Acknowledge
    )
    fut = await self._add_os_query_wait_buffer(query)

    # Command for SDO_UPLOAD_INITIATE (0x40)
    await self.can_write(
      cob=eCOBType.RSDO,
      node_id=node_id,
      byte0=0x40,
      byte1=object_byte1,
      byte2=object_byte0,
      byte3=sub_index,
    )

    # Wait for response
    resp = await asyncio.wait_for(fut, timeout=1.0)  # 1000ms timeout

    if "SDOUA" not in resp:
      raise CanError(
        f"Failed to receive Initiate SDO Upload acknowledgement from motor drive {node_id}"
      )

    if "ABORT" in resp:
      abort_code_start = resp.index("ABORT")
      abort_msg = resp[abort_code_start + 4 :]
      raise CanError(
        f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index}. Abort code received from motor drive {node_id}. {abort_msg}"
      )

    # At this point, `resp` should contain SDOUANxxx or SDOUAExxx or actual data
    # This is where segmented transfer or direct values are extracted.
    # This part is highly complex in C# and depends on `SDOUA` command.
    # TODO: more work here.

    # Simplified conversion from query.response to data_byte_ref
    if "SDOUAN" in resp:
      val_str = resp[len("SDOUAN") :]
      if vb_isnumeric(val_str):
        return int(val_str).to_bytes(8, "little")[:4]  # Max 4 bytes for expedited
      else:
        raise CanError(
          f"Failed to receive numeric response to {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} from motor drive {node_id}"
        )
    if "SDOUAE" in resp:
      data_str = resp[len("SDOUAE") :]
      return [ord(c) for c in data_str]
    raise CanError(f"Failed to interpret SDO Upload response: {resp}")

  async def can_sdo_download(
    self,
    node_id: int,
    object_byte0: int,
    object_byte1: int,
    sub_index: int,
    data_byte: List[int],
  ) -> None:
    """
    can_sdo_download (write).
    Sends an SDO message and waits for a response.
    """
    if len(data_byte) <= 4:  # Expedited SDO Download
      query = sQuery(
        node_id=node_id,
        object_byte0=object_byte0,
        object_byte1=object_byte1,
        sub_index=sub_index,
        msg_type="SDODI",  # SDO Download Initiate Acknowledge
      )
      fut = await self._add_os_query_wait_buffer(query)

      # 0x21: initiate download, data length indicated by n = 4 - len(data) in bytes
      # If len 1 => 0x2F, len 2 => 0x2B, len 3 => 0x27, len 4 => 0x23
      cmd_byte = 0x23 | ((4 - len(data_byte)) << 2) | 0x01  # 0x01 is probably a fixed bit
      filled_data = list(data_byte) + [0] * (
        4 - len(data_byte)
      )  # Pad with zeros to 4 bytes

      await self.can_write(
        eCOBType.RSDO,
        node_id,
        byte0=cmd_byte,
        byte1=object_byte1,
        byte2=object_byte0,
        byte3=sub_index,
        byte4=filled_data[0],
        byte5=filled_data[1],
        byte6=filled_data[2],
        byte7=filled_data[3],
      )

      resp = await asyncio.wait_for(fut, timeout=1.0)

      if "SDODI" not in resp:
        raise CanError(
          f"Failed to receive Expedited SDO Download acknowledgement from motor drive {node_id}"
        )

      if "ABORT" in resp:
        abort_code_start = resp.index("ABORT")
        abort_msg = resp[abort_code_start + 5 :]
        raise CanError(
          f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} abort code received from motor drive {node_id}. {abort_msg}"
        )

    else:  # Segmented SDO Download (more than 4 bytes)
      # This is much more involved, involving multiple CAN messages for data transfer.
      query = sQuery(
        node_id=node_id,
        object_byte0=object_byte0,
        object_byte1=object_byte1,
        sub_index=sub_index,
        msg_type="SDODI",  # SDO Download Initiate Acknowledge
      )
      fut = await self._add_os_query_wait_buffer(query)

      # Initiate Segmented SDO Download (0x21): Object, Subindex, Data size (total len in bytes)
      # Data payload for this initiate message: object bytes, subindex, and total data len.
      # `CAN_Write(eCOBType.RSDO, NodeID, (byte) 33, ObjectByte1, ObjectByte0, SubIndex)`
      # Byte0=0x21 (initiate segment download), then object/subindex for the SDO.
      # The data length is sent with this message if known.
      # For `segmented` method, the total size is determined.

      # Byte0 = (byte)33 -- (CSID = 001, E=0, S=0) -- initiate Segmented SDO Download
      # Followed by Object and Subindex
      await self.can_write(
        eCOBType.RSDO, node_id, byte0=0x21, byte1=object_byte1, byte2=object_byte0, byte3=sub_index
      )  # Missing length

      # Wait for response on initiated segmented download.
      resp = await asyncio.wait_for(fut, timeout=1.0)  # 1000ms timeout

      if "SDODI" not in resp:
        raise CanError(
          f"Failed to receive Segmented SDO Download acknowledgement from motor drive {node_id}"
        )

      if "ABORT" in resp:
        abort_code_start = resp.index("ABORT")
        abort_msg = resp[abort_code_start + 5 :]
        raise CanError(
          f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} abort code received from motor drive {node_id}. {abort_msg}"
        )

      # Token byte toggling and sending segments. This is a loops over data_byte_ref.
      toggle_bit = 0
      bytes_sent = 0
      while bytes_sent < len(data_byte):
        query_frag = sQuery(
          node_id=node_id,
          object_byte0=object_byte0,
          object_byte1=object_byte1,
          sub_index=sub_index,
          msg_type="CHR",
        )
        fut_frag = await self._add_os_query_wait_buffer(query_frag)

        remaining_data = len(data_byte) - bytes_sent
        segment_len = min(remaining_data, 7)  # Max 7 bytes payload for SDO segment

        # `num1` is toggle bit, `num10` is len_indicator. Last bit set if last fragment.
        # Base is 0x00, toggle bit is 0x10. N=num_bytes_not_used.
        # If last segment: add 0x01.
        cmd_sdo_seg = (toggle_bit << 4) | ((7 - segment_len) << 1)
        if bytes_sent + segment_len >= len(data_byte):  # This is the last segment
          cmd_sdo_seg |= 0x01  # Set last segment bit (C=1)

        segment_data = data_byte[bytes_sent : bytes_sent + segment_len]

        # Pad segment_data to 7 bytes for can_write
        padded_segment = segment_data + [0] * (7 - len(segment_data))

        await self.can_write(
          eCOBType.RSDO,
          node_id,
          byte0=cmd_sdo_seg,
          byte1=padded_segment[0],
          byte2=padded_segment[1],
          byte3=padded_segment[2],
          byte4=padded_segment[3],
          byte5=padded_segment[4],
          byte6=padded_segment[5],
          byte7=padded_segment[6],
          data_length=segment_len + 1,
        )  # cmd_sdo_seg itself + data

        resp = await asyncio.wait_for(fut_frag, timeout=1.0)  # 1000ms timeout

        if "CHR" not in resp:
          raise CanError(
            f"Failed to receive OS Character Transfer acknowledgement for segment from drive {node_id}"
          )

        bytes_sent += segment_len
        toggle_bit = 1 - toggle_bit  # Toggle

  async def os_interpreter(
    self,
    node_id: int,
    cmd: str,
    *,
    query: bool = False,
  ) -> str:
    def _resp_text(resp: Any) -> str:
      """
      Best-effort extraction of a response "text" from whatever your wait-buffer returns.
      Adjust this to your actual response object shape.
      """

      if resp is None:
        return ""
      if isinstance(resp, str):
        return resp
      for attr in ("response", "text", "raw", "payload"):
        v = getattr(resp, attr, None)
        if isinstance(v, str):
          return v
      return str(resp)

    def _abort_detail(resp_txt: str) -> Optional[str]:
      i = resp_txt.upper().find("ABORT")
      if i < 0:
        return None
      # VB did: Right(resp, Len(resp) - i - 4) with 1-based InStr
      # Here: everything after "ABORT"
      return resp_txt[i + 5 :].strip()

    _node_idx = self.node_id_list.index(node_id)

    async def send_and_wait(
      *,
      object_byte0: int,
      object_byte1: int,
      sub_index: int,
      msg_type: str,
      write_kwargs: dict,
      timeout_s: float = 1.0,
    ) -> str:
      q = sQuery(
        node_id=node_id,
        object_byte0=object_byte0,
        object_byte1=object_byte1,
        sub_index=sub_index,
        msg_type=msg_type,
      )
      fut = await self._add_os_query_wait_buffer(query=q)
      await self.can_write(**write_kwargs)
      resp = await asyncio.wait_for(fut, timeout=timeout_s)
      return _resp_text(resp)

    t0 = time.perf_counter()

    # --- 1) "Evaluate Immediately mode" acknowledge (SDODI), write: 35,36,0x10
    try:
      r = await send_and_wait(
        object_byte0=0x10,
        object_byte1=0x24,  # 36
        sub_index=0,
        msg_type="SDODI",
        write_kwargs=dict(
          cob=eCOBType.RSDO,
          node_id=node_id,
          byte0=0x23,  # 35
          byte1=0x24,  # 36
          byte2=0x10,
        ),
      )
    except Exception as e:
      raise CanError(f"Failed to send OS evaluate-immediately command to motor drive {node_id}: {e}")

    if "SDODI" not in r:
      raise CanError(f"Failed to receive OS Evaluate Immediately mode acknowledgement from motor drive {node_id}")

    ab = _abort_detail(r)
    if ab is not None:
      raise CanError(f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}")

    # --- 2) Initiate segmented SDO download, sub=1, write: 33,35,0x10,1
    r = await send_and_wait(
      object_byte0=0x10,
      object_byte1=0x23,  # 35
      sub_index=1,
      msg_type="SDODI",
      write_kwargs=dict(
        cob=eCOBType.RSDO,
        node_id=node_id,
        byte0=0x21,  # 33
        byte1=0x23,  # 35
        byte2=0x10,
        byte3=0x01,
      ),
    )

    if "SDODI" not in r:
      raise CanError(f"Failed to receive Segmented SDO Download acknowledgement from motor drive {node_id}")

    ab = _abort_detail(r)
    if ab is not None:
      raise CanError(f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}")

    # --- 3) Send command as 7-byte segments with CANopen toggle/unused/last bits.
    cmd_bytes = cmd.encode("ascii", errors="strict")
    chunks: List[bytes] = [cmd_bytes[i : i + 7] for i in range(0, len(cmd_bytes), 7)]
    if not chunks:
      chunks = [b""]

    toggle = 1  # VB starts at 1 and flips before first send => first segment uses toggle=0

    for idx, chunk in enumerate(chunks):
      toggle = 0 if toggle != 0 else 1  # flip each segment
      unused = 7 - len(chunk)
      last = (idx == len(chunks) - 1)

      # Byte0: [toggle in bit4] + [unused in bits1-3] + [last in bit0]
      byte0 = (toggle << 4) | ((unused & 0x07) << 1) | (1 if last else 0)

      # pad with ASCII '0' (48) exactly like VB did
      padded = chunk + (b"0" * unused)
      b1, b2, b3, b4, b5, b6, b7 = (padded[i] for i in range(7))

      r = await send_and_wait(
        object_byte0=0,
        object_byte1=0,
        sub_index=0,
        msg_type="CHR",
        write_kwargs=dict(
          cob=eCOBType.RSDO,
          node_id=node_id,
          byte0=byte0,
          byte1=b1,
          byte2=b2,
          byte3=b3,
          byte4=b4,
          byte5=b5,
          byte6=b6,
          byte7=b7,
        ),
      )

      if "CHR" not in r:
        raise CanError(f"Failed to receive OS Character Transfer acknowledgement from motor drive {node_id}")

      ab = _abort_detail(r)
      if ab is not None:
        raise CanError(f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}")

    # --- 4) Read OS command status (STAT), sub=2, write: 0x40,35,0x10,2
    r = await send_and_wait(
      object_byte0=0x10,
      object_byte1=0x23,  # 35
      sub_index=2,
      msg_type="STAT",
      write_kwargs=dict(
        cob=eCOBType.RSDO,
        node_id=node_id,
        byte0=0x40,
        byte1=0x23,
        byte2=0x10,
        byte3=0x02,
      ),
    )

    if "STAT" not in r:
      raise CanError(f"Failed to receive OS command status from motor drive {node_id}")

    ab = _abort_detail(r)
    if ab is not None:
      raise CanError(f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}")

    # If not querying a response, done.
    if not query:
      return ""

    # --- 5) Initiate SDO upload (SDOUA), sub=3, write: 0x40,35,0x10,3
    r = await send_and_wait(
      object_byte0=0x10,
      object_byte1=0x23,
      sub_index=3,
      msg_type="SDOUA",
      write_kwargs=dict(
        cob=eCOBType.RSDO,
        node_id=node_id,
        byte0=0x40,
        byte1=0x23,
        byte2=0x10,
        byte3=0x03,
      ),
    )

    if "SDOUA" not in r:
      raise CanError(f"Failed to receive Initiate SDO Upload acknowledgement from motor drive {node_id}")

    ab = _abort_detail(r)
    if ab is not None:
      raise CanError(f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}")

    # VB string protocol parsing:
    # expects r like: "SDOUA" + ("N" or "E") + payload
    response_out = ""

    if len(r) >= 6 and r.startswith("SDOUA"):
      kind = r[5:6]      # 6th char (1-based)
      payload = r[6:]    # everything after that char

      if kind == "E":
        response_out = payload
        return 0, response_out, int((time.perf_counter() - t0) * 1000)

      if kind == "N":
        # numeric check in VB; if non-numeric => error
        if not payload.strip().lstrip("+-").isdigit():
          raise CanError(f"Failed to receive a response to '{cmd}' from motor drive {node_id}")
        # else fall through to segmented upload reading below

    # --- 6) Segmented SDO upload (SDOSU)
    seg_idx = 0
    response_out = ""

    while True:
      # client request byte0 toggles between 0x60 and 0x70
      req0 = 0x60 if (seg_idx % 2) == 0 else 0x70

      r = await send_and_wait(
        object_byte0=0,
        object_byte1=0,
        sub_index=0,
        msg_type="SDOSU",
        write_kwargs=dict(
          cob=eCOBType.RSDO,
          node_id=node_id,
          byte0=req0,
        ),
      )

      if "SDOSU" not in r:
        raise CanError(f"Failed to receive OS response value from motor drive {node_id}")

      ab = _abort_detail(r)
      if ab is not None:
        raise CanError(f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}")

      if len(r) <= 7:
        raise CanError(f"Failed to extract data response for '{cmd}' from motor drive {node_id}")

      # VB protocol:
      # r: "SDOSU" + (pos6: 'C'/'D') + (pos7: toggle bit '0'/'1') + data...
      cd_flag = r[5:6]
      toggle_bit = r[6:7]
      data_part = r[7:]

      response_out += data_part

      # toggle check (mirrors VB). If mismatch -> 65.
      if (req0 == 0x60 and toggle_bit == "1") or (req0 == 0x70 and toggle_bit == "0"):
        return 65, response_out, int((time.perf_counter() - t0) * 1000)

      if cd_flag == "C":
        seg_idx += 1
        continue

      if cd_flag == "D":
        return 0, response_out, int((time.perf_counter() - t0) * 1000)

      raise CanError(f"Failed to receive data response for '{cmd}' from motor drive {node_id}")

  async def can_sync(self) -> None:
    await self.can_write(cob=eCOBType.SYNC, node_id=0)

  async def can_sdo_download_elmo_object(
    self,
    node_id: int,
    elmo_object_int: int,
    sub_index: int,
    data: str,
    data_type: eElmoObjectDataType,
  ) -> None:
    """
    Wrapper around can_sdo_download for Elmo specific objects, handling data type conversions.
    """
    data_bytes: List[int] = []

    # Convert input 'data' string to byte array based on data_type
    if data:  # Only if data string is provided (i.e. this is a write operation)
      if data_type == eElmoObjectDataType.UNSIGNED8:
        data_bytes = list(int(data).to_bytes(1, "little"))
      elif data_type == eElmoObjectDataType.UNSIGNED16:
        data_bytes = list(int(data).to_bytes(2, "little"))
      elif data_type == eElmoObjectDataType.UNSIGNED32:
        data_bytes = list(int(float(data)).to_bytes(4, "little"))
      elif data_type == eElmoObjectDataType.UNSIGNED64:
        data_bytes = list(int(data).to_bytes(8, "little"))
      elif data_type == eElmoObjectDataType.INTEGER8:
        data_bytes = list(int(data).to_bytes(1, "little", signed=True))
      elif data_type == eElmoObjectDataType.INTEGER16:
        data_bytes = list(int(data).to_bytes(2, "little", signed=True))
      elif data_type == eElmoObjectDataType.INTEGER32:
        data_bytes = list(int(float(data)).to_bytes(4, "little", signed=True))
      elif data_type == eElmoObjectDataType.INTEGER64:
        data_bytes = list(int(data).to_bytes(8, "little", signed=True))
      elif data_type == eElmoObjectDataType.STR:
        data_bytes = [ord(c) for c in data]
      else:
        raise CanError(f"Unsupported data type for SDO Write: {data_type.name}")

    obj_byte0 = elmo_object_int >> 8
    obj_byte1 = elmo_object_int & 0xFF

    await self.can_sdo_download(node_id, obj_byte0, obj_byte1, sub_index, data_bytes)

    if not data:  # If it was a read operation (data string was empty before SDO send)
      # data_bytes should now contain the read data. Convert it back to a string.
      if len(data_bytes) == 0:
        data = ""
      elif data_type == eElmoObjectDataType.UNSIGNED8:
        data = str(int.from_bytes(data_bytes[:1], "little", signed=False))
      elif data_type == eElmoObjectDataType.UNSIGNED16:
        data = str(int.from_bytes(data_bytes[:2], "little", signed=False))
      elif data_type == eElmoObjectDataType.UNSIGNED32:
        data = str(int.from_bytes(data_bytes[:4], "little", signed=False))
      elif data_type == eElmoObjectDataType.UNSIGNED64:
        data = str(int.from_bytes(data_bytes[:8], "little", signed=False))
      elif data_type == eElmoObjectDataType.INTEGER16:
        data = str(int.from_bytes(data_bytes[:2], "little", signed=True))
      elif data_type == eElmoObjectDataType.INTEGER32:
        data = str(int.from_bytes(data_bytes[:4], "little", signed=True))
      elif data_type == eElmoObjectDataType.INTEGER64:
        data = str(int.from_bytes(data_bytes[:8], "little", signed=True))
      elif data_type == eElmoObjectDataType.STR:
        data = "".join([chr(b) for b in data_bytes])
      else:
        raise CanError(f"Unsupported data type for SDO Read conversion: {data_type.name}")

  # --- PDO Mapping methods ---

  async def can_tpdo_unmap(self, tpdo: eTPDO, node_id: int):
    cob_type_int = {
      eTPDO.TPDO1: eCOBType.TPDO1.value,
      eTPDO.TPDO3: eCOBType.TPDO3.value,
      eTPDO.TPDO4: eCOBType.TPDO4.value,
    }[tpdo]

    if not (0 <= node_id <= 0x7F):
      raise ValueError(f"node_id must be 0..127, got {node_id}")

    node_id = node_id & 0x7F
    num1 = ((cob_type_int & 0x01) << 7) | node_id
    num2 = (cob_type_int >> 1) & 0x07

    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=24,
      object_byte1=tpdo.value - 1,
      sub_index=1,
      data_byte=[
        num1,
        num2,
        0,
        0xC0,
      ],
    )

    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=26,
      object_byte1=tpdo.value - 1,
      sub_index=0,
      data_byte=[0, 0, 0, 0],
    )

    for index in range(len(self.tpdo_mapped_object[node_id][tpdo])):
      self.tpdo_mapped_object[node_id][tpdo][index] = eTPDOMappedObject.NotMapped

  async def can_tpdo3_map(self, node_id: int) -> None:
    mapped_objects = [eTPDOMappedObject.StatusWord]
    # EventTimerMs=0, Delay100us=0, TransmissionType=EventDrivenDev
    await self.can_tpdo_map(
      tpdo=eTPDO.TPDO3, node_id=node_id, mapped_objects=mapped_objects, event_trigger=eTPDOTrigger.MotionComplete
    )

  async def can_tpdo4_map(self, node_id: int) -> None:
    mapped_objects = [eTPDOMappedObject.DigitalInputs]
    await self.can_tpdo_map(
      tpdo=eTPDO.TPDO4, node_id=node_id, mapped_objects=mapped_objects, event_trigger=eTPDOTrigger.DigitalInputEvent
    )

  async def can_rpdo1_map(self, node_id: int) -> None:
    mapped_objects = [eRPDOMappedObject.ControlWord]
    await self.can_rpdo_map(
      rpdo=eRPDO.RPDO1, node_id=node_id, mapped_objects=mapped_objects, transmission_type=ePDOTransmissionType.SynchronousCyclic
    )

  async def can_rpdo3_map(self, node_id: int) -> None:
    mapped_objects = [eRPDOMappedObject.TargetPositionIP, eRPDOMappedObject.TargetVelocityIP]
    await self.can_rpdo_map(
      rpdo=eRPDO.RPDO3, node_id=node_id, mapped_objects=mapped_objects, transmission_type=ePDOTransmissionType.EventDrivenDev
    )

  async def can_rpdo_map(
    self,
    rpdo: eRPDO,
    node_id: int,
    mapped_objects: List[eRPDOMappedObject],
    transmission_type: ePDOTransmissionType,
  ) -> None:
    """Maps RPDOs for incoming messages."""

    rpdo_num = int(rpdo)         # expects 1..4
    rpdo_idx = (rpdo_num - 1) & 0xFF   # original passes (byte)(RPDO - 1)

    # Map RPDO -> COB function code (4-bit) used to build the 11-bit COB-ID
    # (original decompilation only showed 1,3,4; RPDO2 is included here)
    if rpdo == eRPDO.RPDO1:
      cob_type = eCOBType.RPDO1
    elif rpdo == eRPDO.RPDO3:
      cob_type = eCOBType.RPDO3
    elif rpdo == eRPDO.RPDO4:
      cob_type = eCOBType.RPDO4
    else:
      raise ValueError(f"Unsupported RPDO: {rpdo!r}")

    # CANopen 11-bit COB-ID: (function_code << 7) | node_id(7 bits)
    cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F)

    # 1) Disable PDO (set bit 31) while configuring: 0x80000000 | cob_id
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x14,
      object_byte1=rpdo_idx,
      sub_index=0x01,
      data_byte=_u32_le(0x80000000 | cob_id_11),
    )

    # 2) Clear mapping count (sub 0) at 0x1600 + rpdo_idx
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x16,      # 0x1600 + rpdo_idx
      object_byte1=rpdo_idx,
      sub_index=0x00,
      data_byte=[0, 0, 0, 0],
    )

    # 3) Set transmission type (sub 2) at 0x1400 + rpdo_idx
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x14,
      object_byte1=rpdo_idx,
      sub_index=0x02,
      data_byte=[int(transmission_type) & 0xFF, 0, 0, 0],
    )

    # 4) Write each mapped object (sub 1..n) at 0x1600 + rpdo_idx
    for i, mo in enumerate(mapped_objects):
      await self.can_sdo_download(
        node_id=node_id,
        object_byte0=0x16,
        object_byte1=rpdo_idx,
        sub_index=(i + 1) & 0xFF,
        data_byte=_u32_le(int(mo)),
      )

    # 5) Set mapping count (sub 0)
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x16,
      object_byte1=rpdo_idx,
      sub_index=0x00,
      data_byte=[len(mapped_objects) & 0xFF, 0, 0, 0],
    )

    # 6) Enable PDO (clear bit 31): write cob_id only
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x14,
      object_byte1=rpdo_idx,
      sub_index=0x01,
      data_byte=_u32_le(cob_id_11),
    )

  async def can_tpdo_map(
    self,
    tpdo: eTPDO,
    node_id: int,
    mapped_objects: List[eTPDOMappedObject],
    event_trigger: eTPDOTrigger,
    event_timer_ms: int = 0,
    delay_100_us: int = 0,
    transmission_type: ePDOTransmissionType = ePDOTransmissionType.EventDrivenDev,
  ) -> None:
    """Maps TPDOs for outgoing messages."""

    tpdo_num = int(tpdo)         # expects 1..4
    tpdo_idx = (tpdo_num - 1) & 0xFF   # (byte)(TPDO - 1)

    if tpdo == eTPDO.TPDO1:
      cob_type = eCOBType.TPDO1
    elif tpdo == eTPDO.TPDO3:
      cob_type = eCOBType.TPDO3
    elif tpdo == eTPDO.TPDO4:
      cob_type = eCOBType.TPDO4
    else:
      raise ValueError(f"Unsupported TPDO: {tpdo!r}")

    # CANopen 11-bit COB-ID
    cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F)

    # Event trigger mask: 2**EventTrigger, stored as u32
    event_mask = 1 << int(event_trigger)

    # 1) Disable TPDO while configuring: 0xC0000000 | cob_id
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x18,      # 0x1800 + tpdo_idx
      object_byte1=tpdo_idx,
      sub_index=0x01,
      data_byte=_u32_le(0xC0000000 | cob_id_11),
    )

    # 2) Clear mapping count: 0x1A00 + tpdo_idx, sub 0
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x1A,      # 0x1A00 + tpdo_idx
      object_byte1=tpdo_idx,
      sub_index=0x00,
      data_byte=[0, 0, 0, 0],
    )

    # 3) Transmission type: 0x1800 + tpdo_idx, sub 2
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x18,
      object_byte1=tpdo_idx,
      sub_index=0x02,
      data_byte=[int(transmission_type) & 0xFF, 0, 0, 0],
    )

    # 4) Inhibit time / delay (100us units): sub 3
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x18,
      object_byte1=tpdo_idx,
      sub_index=0x03,
      data_byte=[delay_100_us & 0xFF, 0, 0, 0],
    )

    # 5) Event timer (ms): sub 5
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x18,
      object_byte1=tpdo_idx,
      sub_index=0x05,
      data_byte=[event_timer_ms & 0xFF, 0, 0, 0],
    )

    # 6) Write event trigger mask to 0x2F20 sub = TPDO (matches can_sdo_download(NodeID, 0x2F, 0x20, TPDO, ...))
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x2F,
      object_byte1=0x20,
      sub_index=tpdo_num & 0xFF,
      data_byte=_u32_le(event_mask),
    )

    # 7) Write mapped objects into 0x1A00 + tpdo_idx, sub 1..n
    for i, mo in enumerate(mapped_objects):
      await self.can_sdo_download(
        node_id=node_id,
        object_byte0=0x1A,
        object_byte1=tpdo_idx,
        sub_index=(i + 1) & 0xFF,
        data_byte=_u32_le(int(mo)),
      )

    # 8) Set mapping count (sub 0)
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x1A,
      object_byte1=tpdo_idx,
      sub_index=0x00,
      data_byte=[len(mapped_objects) & 0xFF, 0, 0, 0],
    )

    # 9) Re-enable TPDO: write 0x40000000 | cob_id
    await self.can_sdo_download(
      node_id=node_id,
      object_byte0=0x18,
      object_byte1=tpdo_idx,
      sub_index=0x01,
      data_byte=_u32_le(0x40000000 | cob_id_11),
    )

    # 10) Mirror the original side-effect: store mapping in self.tpdo_mapped_object
    node_idx = self.node_id_list.index(node_id)
    self.tpdo_mapped_object[node_idx][tpdo] = mapped_objects

  async def pvt_select_mode(self, enable: bool) -> None:
    """Enables or disables PVT mode on all motors in the group."""

    if enable:
      if not self._pvt_mode:
        for node_id in self.grp_node_id:
          await self.can_sdo_download(
            node_id=node_id,
            object_byte0=0x60,
            object_byte1=0xC4,
            sub_index=0x06,
            data_byte=[0],
          )
          await self.can_sdo_download(
            node_id=node_id,
            object_byte0=0x60,
            object_byte1=0x60,
            sub_index=0x00,
            data_byte=[7],
          )
        self._pvt_mode = True
      else:
        for node_id in self.grp_node_id:
          await self.can_sdo_download(
            node_id=node_id,
            object_byte0=0x60,
            object_byte1=0x60,
            sub_index=0x00,
            data_byte=[1],
          )
          await self.can_sdo_download(
            node_id=node_id,
            object_byte0=0x60,
            object_byte1=0xC4,
            sub_index=0x06,
            data_byte=[0],
          )
          await self.can_sdo_download(
            node_id=node_id,
            object_byte0=0x60,
            object_byte1=0x60,
            sub_index=0x00,
            data_byte=[7],
          )
    elif self._pvt_mode:
      for node_id in self.grp_node_id:
        await self.can_sdo_download(
          node_id=node_id,
          object_byte0=0x60,
          object_byte1=0x60,
          sub_index=0x00,
          data_byte=[1],
        )
      self._pvt_mode = False

  async def binary_interpreter(
    self,
    node_id: int,
    cmd: str,
    cmd_index: int,
    cmd_type: eCmdType,
    value: str = "0",
    val_type: eValType = eValType.Int,
    low_priority: bool = False,
  ) -> Union[str, float]:

    if self.grp_node_id is None:
      # Original code just returned 0; here we assume this is programmer error
      raise CanError("GrpNodeID is not set")

    # if not self.connected:
    # raise CanError("Not connected")

    # Max wait time (seconds)
    # Original: 1000 ms normally, 10000 ms if cmd == 'SV'
    timeout_s = 10.0 if cmd.upper() == "SV" else 1.0
    if cmd.upper() == "SV":
      self.SendingSVCommand = True

    is_float = (val_type == eValType.Float)
    is_query = (cmd_type == eCmdType.ValQuery)
    is_execute = (cmd_type == eCmdType.Execute)

    # Helper: build command bytes
    def _build_bytes() -> tuple[int, int, int, int, int, int, int, int]:
      # byte0, byte1: ASCII of first and last char of cmd
      byte0 = ord(cmd[0])
      byte1 = ord(cmd[-1])

      # Encode cmd_index into 14 bits:
      # - low 8 bits -> byte2
      # - high 6 bits -> lower bits of byte3
      byte2 = cmd_index & 0xFF
      byte3 = (cmd_index >> 8) & 0x3F  # keep only 6 bits

      # Set flags in bits 6 and 7 of byte3
      if is_query:
        byte3 |= 0x40  # bit 6
      if is_float:
        byte3 |= 0x80  # bit 7

      # Encode value
      if is_float:
        fv = float(value)
        b4, b5, b6, b7 = struct.pack("<f", fv)
        byte4 = b4
        byte5 = b5
        byte6 = b6
        byte7 = b7
      else:
        iv = int(round(float(value)))
        byte4 = iv & 0xFF
        byte5 = (iv >> 8) & 0xFF
        byte6 = (iv >> 16) & 0xFF
        byte7 = (iv >> 24) & 0xFF

      return (
        byte0 & 0xFF,
        byte1 & 0xFF,
        byte2 & 0xFF,
        byte3 & 0xFF,
        byte4 & 0xFF,
        byte5 & 0xFF,
        byte6 & 0xFF,
        byte7 & 0xFF,
      )

    # Helper: numeric comparison with ±1% tolerance like the original
    def _float_matches(expected_str: str, actual_str: str) -> bool:
      try:
        expected = float(expected_str)
        actual = float(actual_str)
      except ValueError:
        return False
      if actual == 0.0:
        return expected == 0.0
      ratio = expected / actual
      return expected == actual or (0.99 < ratio < 1.01)

    max_attempts = 1
    last_response_time_ms = 0

    # Single-node path (NodeID != 10)
    if node_id != 10:
      for attempt in range(1, max_attempts + 1):
        if value == "":
          value = "0"

        (
          byte0,
          byte1,
          byte2,
          byte3,
          byte4,
          byte5,
          byte6,
          byte7,
        ) = _build_bytes()

        # Build a minimal query object compatible with your buffer/reader
        # We don't know your exact type, so SimpleNamespace gives us attributes.
        query = sQuery(
          node_id=node_id,
          msg_index=cmd_index,
          msg_type=cmd,
        )

        fut = await self._add_query_wait_buffer(query)

        await self.can_write(
          eCOBType.RPDO2,
          node_id,
          byte0,
          byte1,
          byte2,
          byte3,
          byte4,
          byte5,
          byte6,
          byte7,
          execute=is_execute,
          low_priority=low_priority,
          data_length=4 if is_execute else 8,
        )

        try:
          resp = await asyncio.wait_for(fut, timeout=timeout_s)
        except asyncio.TimeoutError:
          if attempt == max_attempts:
            self.SendingSVCommand = False
            raise CanError(
              f"Timeout waiting for response to {cmd}[{cmd_index}] from node {node_id}"
            )
          # retry
          continue

        # Query: just return the response
        if is_query:
          value = str(resp)
          self.SendingSVCommand = False
          return float(value) if is_float else int(float(value))

        # Execute: only care that we got *some* response
        if is_execute:
          if resp == "" and attempt == max_attempts:
            self.SendingSVCommand = False
            raise CanError(
              f"No response for execute command {cmd}[{cmd_index}] from node {node_id}"
            )
          if resp != "":
            self.SendingSVCommand = False
            return float(value) if is_float else int(float(value))
          # else retry
          continue

        # Write: verify echoed value
        if is_float:
          ok = _float_matches(value, str(resp))
        else:
          ok = (str(int(float(resp))) == str(int(float(value))))
          # ok = (str(resp) == str(value))

        if ok:
          self.SendingSVCommand = False
          return float(value) if is_float else int(float(value))

        if attempt == max_attempts:
          self.SendingSVCommand = False
          raise CanError(
            f"Unexpected CAN response: attempted to send {cmd}[{cmd_index}]={value}, "
            f"but received response={resp} from node {node_id}"
          )
        # else retry

      # Should never get here
      self.SendingSVCommand = False
      raise CanError("Internal error in binary_interpreter (single-node)")

    # Group path (NodeID == 10)
    grp_ids = list(self.grp_node_id)

    for attempt in range(1, max_attempts + 1):
      if value == "":
        value = "0"

      (
        byte0,
        byte1,
        byte2,
        byte3,
        byte4,
        byte5,
        byte6,
        byte7,
      ) = _build_bytes()

      # One query per node in group
      queries = []
      futures = []
      for gid in grp_ids:
        q = sQuery(
          node_id=gid,
          msg_index=cmd_index,
          msg_type=cmd,
        )
        queries.append(q)
        fut = await self._add_query_wait_buffer(q)
        futures.append(fut)

      await self.can_write(
        eCOBType.RPDO2,
        node_id,  # broadcast/group node (10)
        byte0,
        byte1,
        byte2,
        byte3,
        byte4,
        byte5,
        byte6,
        byte7,
        execute=is_execute,
        low_priority=low_priority,
        data_length=4 if is_execute else 8,
      )

      try:
        # Wait for *all* group responses
        resps = await asyncio.wait_for(
          asyncio.gather(*futures, return_exceptions=False),
          timeout=timeout_s,
        )
      except asyncio.TimeoutError:
        if attempt == max_attempts:
          self.SendingSVCommand = False
          raise CanError(
            f"Timeout waiting for group response to {cmd}[{cmd_index}] "
            f"from nodes {grp_ids}"
          )
        # retry
        continue

      # Query: concatenate responses with commas
      if is_query:
        if any(r == "" for r in resps):
          if attempt == max_attempts:
            self.SendingSVCommand = False
            raise CanError(
              f"Incomplete group query response for {cmd}[{cmd_index}] "
              f"from nodes {grp_ids}"
            )
          # retry
          continue

        value = ",".join(str(r) for r in resps)
        self.SendingSVCommand = False
        return float(value) if is_float else int(float(value))

      # Execute: just require all responses non-empty
      if is_execute:
        if all(r != "" for r in resps):
          self.SendingSVCommand = False
          return float(value) if is_float else int(float(value))
        if attempt == max_attempts:
          missing_nodes = [
            gid for gid, r in zip(grp_ids, resps) if r == ""
          ]
          self.SendingSVCommand = False
          raise CanError(
            f"No execute response from nodes {missing_nodes} "
            f"for {cmd}[{cmd_index}]"
          )
        # retry
        continue

      # Write: verify each node's echoed value
      mismatch_node = None
      mismatch_resp = None

      for gid, resp in zip(grp_ids, resps):
        if is_float:
          ok = _float_matches(value, str(resp))
        else:
          ok = (str(resp) == str(value))

        if not ok:
          mismatch_node = gid
          mismatch_resp = resp
          break

      if mismatch_node is None:
        # everyone matched
        self.SendingSVCommand = False
        return float(value) if is_float else int(float(value))

      if attempt == max_attempts:
        self.SendingSVCommand = False
        raise CanError(
          f"Unexpected CAN response: attempted to send {cmd}[{cmd_index}]={value}, "
          f"but received response={mismatch_resp} from node {mismatch_node}"
        )
      # else retry

    # Should never get here
    self.SendingSVCommand = False
    raise CanError("Internal error in binary_interpreter (group)")

  # --- Converted Functions ---

  async def motor_move_execute(
    self,
    node_id: int,
    position: int,
    velocity: int,
    acceleration: int,
    relative: bool = False,
    timeout: int = 10000,
    joint_move_direction: eJointMoveDirection = eJointMoveDirection.ShortestWay,
  ) -> None:
    print("motor_move_execute", node_id, position, velocity, acceleration, relative, timeout, joint_move_direction)

    # event_data = {} # clsCANMotor.sEventData()
    num1 = 0x40 if relative else 0

    node_idx = self.node_id_list.index(node_id)
    # TODO: wait for previous move done?
    # if not self.move_done_response[node_idx]:
    #   await self.motor_wait_for_move_done(node_id=node_id, timeout_msec=int(timeout_remaining))

    self.SendingMoveCommand = True

    await self.pvt_select_mode(enable=False)

    await self.motor_set_move_direction(node_id=node_id, direction=joint_move_direction)

    # Set Position
    await self.can_sdo_download_elmo_object(
      node_id=node_id, 
      elmo_object_int=24698, 
      sub_index=0, 
      data=str(position), 
      data_type=eElmoObjectDataType.INTEGER32
    )
    # Set Velocity
    await self.can_sdo_download_elmo_object(
      node_id=node_id, 
      elmo_object_int=24705, 
      sub_index=0, 
      data=str(velocity), 
      data_type=eElmoObjectDataType.UNSIGNED32
    )
    # Set Accel
    await self.can_sdo_download_elmo_object(
      node_id=node_id, 
      elmo_object_int=24707, 
      sub_index=0, 
      data=str(acceleration), 
      data_type=eElmoObjectDataType.UNSIGNED32
    )
    # Set Accel again (Decel?)
    await self.can_sdo_download_elmo_object(
      node_id=node_id, 
      elmo_object_int=24708, 
      sub_index=0, 
      data=str(acceleration), 
      data_type=eElmoObjectDataType.UNSIGNED32
    )

    # Set Control Word (0x6040)
    await self.control_word_set(node_id=node_id, value=(47 + num1))
    await self.control_word_set(node_id=node_id, value=(47 + num1 + 16))

    # ... starts motion ...
    self.move_done_response[node_idx] = False

    self.SendingMoveCommand = False

    await self.motor_wait_for_move_done(node_id=node_id, timeout=timeout)

    # Raise Event Logic
    # event_data['EventType'] = eEventType.MotorMoveDone
    # event_data['NodeID'] = node_id
    # event_data['ME_Index'] = index
    # event_data['MMD_AllMovesDone'] = (self.MoveBufferFirstIndex == self.MoveBufferLastIndex)
    # await self._raise_an_event(event_data)

  # async def motors_move_absolute(
  #   self,
  #   node_ids: List[int],
  #   positions: List[int],
  #   velocities: List[int],
  #   accelerations: List[int],
  #   wait_until_done: bool,
  #   joint_move_directions: List[eJointMoveDirection],
  #   timeout_msec: int = 10000,
  #   wait_until_started: bool = False
  # ) -> int:
  #   """ Returns the index. """
  #   index1 = (self.MoveBufferLastIndex + 1) % 100
  #   returned_index = index1
  #   num1 = (self.MotorsMoveAbsoluteParamBufferIndex + 1) % 100
    
  #   # self.MotorsMoveAbsoluteParamBuffer[num1].NodeID = node_ids
  #   # self.MotorsMoveAbsoluteParamBuffer[num1].Position = positions
  #   # self.MotorsMoveAbsoluteParamBuffer[num1].Velocity = velocities
  #   # self.MotorsMoveAbsoluteParamBuffer[num1].Acceleration = accelerations
  #   # self.MotorsMoveAbsoluteParamBuffer[num1].MtrsJointMoveDirection = joint_move_directions
    
  #   # self.MoveBuffer[index1].ErrorCode = -1
  #   # self.MoveBuffer[index1].MoveType = eMoveType.MotorsMoveAbsolute
  #   # self.MoveBuffer[index1].TimeoutMsec = timeout_msec
  #   # self.MoveBuffer[index1].MotionStarted = False
  #   # self.MoveBuffer[index1].WaitUntilDone = wait_until_done
  #   # self.MoveBuffer[index1].index = num1
  #   # self.MoveBuffer[index1].Pending = True
    
  #   self.MotorsMoveAbsoluteParamBufferIndex = num1
  #   self.MoveBufferLastIndex = index1

  #   if wait_until_started:
  #      while not self.MoveBuffer[index1].MotionStarted and self.bMoveThreadRunning:
  #       if self.EmcyMoveErrorReceived:
  #         returned_index = self.MoveBufferFirstIndex
  #         self.ErrorCode[1] = self.EmcyMoveError
  #         raise RuntimeError("Emergency Move Error")
  #       if self.MoveErrorCode != 0:
  #         returned_index = self.MoveErrorIndex
  #         raise RuntimeError(f"Move Error {self.MoveErrorCode}")
  #       await asyncio.sleep(0.001)

  #   if not wait_until_done:
  #     return returned_index

  #   while self.MoveBuffer[index1].Pending and self.bMoveThreadRunning:
  #     if self.EmcyMoveErrorReceived:
  #       returned_index = self.MoveBufferFirstIndex
  #       self.ErrorCode[1] = self.EmcyMoveError
  #       raise RuntimeError("Emergency Move Error")
  #     if self.MoveErrorCode != 0:
  #       returned_index = self.MoveErrorIndex
  #       raise RuntimeError(f"Move Error {self.MoveErrorCode}")
  #     await asyncio.sleep(0.001)

  #   if self.MoveBuffer[index1].ErrorCode > 0:
  #      raise RuntimeError(f"Buffer Error {self.MoveBuffer[index1].ErrorCode}")

  #   if not self.bMoveThreadRunning:
  #      raise RuntimeError("Thread not running (Error 70)")

  #   start_tick = time.time() * 1000
  #   while (time.time() * 1000 - start_tick) < (timeout_msec + 500) and self.MoveBuffer[index1].ErrorCode == -1:
  #     await asyncio.sleep(0.001)
    
  #   if self.MoveBuffer[index1].index == 255: # byte.MaxValue
  #      raise TimeoutError("Error 30")
  #   elif self.MoveBuffer[index1].ErrorCode != -1:
  #      raise RuntimeError(f"Error {self.MoveBuffer[index1].ErrorCode}")
    
  #   return returned_index

  # async def motors_move_absolute_execute(
  #   self,
  #   node_ids: List[int],
  #   positions: List[int],
  #   velocities: List[int],
  #   accelerations: List[int],
  #   index: int,
  #   joint_move_directions: List[eJointMoveDirection],
  #   timeout_msec: int = 10000
  # ) -> None:
  #   event_data = {}
  #   flag = True
  #   max_done_time = -2147483647
    
  #   # Check if previous moves are done
  #   for i, _ in enumerate(self.grp_node_id):
  #     if not self.move_done_response[i]:
  #       flag = False
  #       if self.MoveDoneTime[i] > max_done_time:
  #         max_done_time = self.MoveDoneTime[i]

  #   if not flag:
  #     time_diff = max_done_time - (time.time() * 1000)
  #     try:
  #       await self.motors_wait_for_move_done(timeout_msec=time_diff)
  #     except Exception as e:
  #       self.MoveErrorIndex = index
  #       self.MoveErrorCode = str(e)
  #       raise e

  #   self.SendingMoveCommand = True
  #   await self.pvt_select_mode(enable=False)
    
  #   try:
  #     for i, nid in enumerate(node_ids):
  #       # Set Direction
  #       await self.motor_set_move_direction(node_id=nid, direction=joint_move_directions[nid]) # CAUTION: C# index logic `JointMoveDirection[(int) NodeID[(int) index2]]` implies array size based on NodeID
        
  #       # Send Elmo Objects
  #       await self.can_sdo_download_elmo_object(nid, 24698, 0, str(positions[i]), eElmoObjectDataType.INTEGER32)
  #       await self.can_sdo_download_elmo_object(nid, 24705, 0, str(velocities[i]), eElmoObjectDataType.UNSIGNED32)
        
  #       acc = accelerations[i]
  #       if acc < 100: acc = 100
        
  #       await self.can_sdo_download_elmo_object(nid, 24707, 0, str(acc), eElmoObjectDataType.UNSIGNED32)
  #       await self.can_sdo_download_elmo_object(nid, 24708, 0, str(acc), eElmoObjectDataType.UNSIGNED32)

  #     await self.motors_move_start(node_ids=node_ids, wait_until_done=True, timeout_msec=timeout_msec, index=index)
      
  #   except Exception as e:
  #     self.MoveErrorIndex = index
  #     self.MoveErrorCode = str(e)
  #     raise e

  #   event_data['EventType'] = eEventType.MotorsMoveDone
  #   event_data['ME_Index'] = index
  #   event_data['MMD_AllMovesDone'] = (self.MoveBufferFirstIndex == self.MoveBufferLastIndex)
  #   self.raise_an_event(event_data)

  # async def motors_move_path(
  #   self,
  #   pva_points: List[List['sPVAPoint']], # 2D array in C#
  #   time_interval_msec: int,
  #   stop_deceleration: List[int],
  #   wait_until_done: bool,
  #   skip_ax: List[bool],
  #   timeout_msec: int = 10000,
  #   wait_until_started: bool = False
  # ) -> int:
  #   """ Returns Index. """
  #   self.PVTStop = False
  #   index1 = (self.MoveBufferLastIndex + 1) % 100
  #   returned_index = index1
  #   num1 = (self.MotorsMovePathParamBufferIndex + 1) % 100
    
  #   self.MotorsMovePathParamBuffer[num1].PVAPoint = pva_points
  #   self.MotorsMovePathParamBuffer[num1].TimeIntervalMsec = time_interval_msec
  #   self.MotorsMovePathParamBuffer[num1].StopDeceleration = stop_deceleration
  #   self.MotorsMovePathParamBuffer[num1].SkipAx = skip_ax
    
  #   self.MoveBuffer[index1].ErrorCode = -1
  #   self.MoveBuffer[index1].MoveType = eMoveType.MotorsMovePath
  #   self.MoveBuffer[index1].TimeoutMsec = timeout_msec
  #   self.MoveBuffer[index1].MotionStarted = False
  #   self.MoveBuffer[index1].WaitUntilDone = wait_until_done
  #   self.MoveBuffer[index1].index = num1
  #   self.MoveBuffer[index1].Pending = True
    
  #   self.MotorsMovePathParamBufferIndex = num1
  #   self.MoveBufferLastIndex = index1

  #   if wait_until_started:
  #      while not self.MoveBuffer[index1].MotionStarted and self.bMoveThreadRunning:
  #       if self.EmcyMoveErrorReceived:
  #         returned_index = self.MoveBufferFirstIndex
  #         self.ErrorCode[1] = self.EmcyMoveError
  #         raise RuntimeError("Emergency Move Error")
  #       if self.MoveErrorCode != 0:
  #         returned_index = self.MoveErrorIndex
  #         raise RuntimeError(f"Error {self.MoveErrorCode}")
  #       await asyncio.sleep(0.001)
    
  #   if not wait_until_done:
  #     return returned_index

  #   while self.MoveBuffer[index1].Pending and self.bMoveThreadRunning:
  #     if self.EmcyMoveErrorReceived:
  #       returned_index = self.MoveBufferFirstIndex
  #       self.ErrorCode[1] = self.EmcyMoveError
  #       raise RuntimeError("Emergency Move Error")
  #     if self.MoveErrorCode != 0:
  #       returned_index = self.MoveErrorIndex
  #       raise RuntimeError(f"Error {self.MoveErrorCode}")
  #     await asyncio.sleep(0.001)

  #   if self.MoveBuffer[index1].ErrorCode > 0:
  #     raise RuntimeError(f"Buffer Error {self.MoveBuffer[index1].ErrorCode}")

  #   if not self.bMoveThreadRunning:
  #      raise RuntimeError("Error 70")

  #   start_tick = time.time() * 1000
  #   while (time.time() * 1000 - start_tick) < (timeout_msec + 500) and self.MoveBuffer[index1].ErrorCode == -1:
  #     await asyncio.sleep(0.001)

  #   if self.MoveBuffer[index1].index == 255:
  #     raise TimeoutError("Error 30")
  #   elif self.MoveBuffer[index1].ErrorCode != -1:
  #     raise RuntimeError(f"Error {self.MoveBuffer[index1].ErrorCode}")
      
  #   return returned_index

  async def motors_move_path_execute(
    self,
    pva_points: List[List], # 2D structure access: pva_points[node_id][step]
    time_interval_msec: int,
    stop_deceleration: List[int],
    index: int,
    skip_ax: List[bool],
    timeout_msec: int = 10000
  ) -> None:
    event_data = {}
    grp_len = len(self.grp_node_id)
    num_array1 = [0] * grp_len
    num_array2 = [0] * grp_len
    num1 = 0
    flag1 = True
    max_done_time = -2147483647
    
    for i in range(grp_len):
      if not self.move_done_response[i]:
        flag1 = False
        if self.MoveDoneTime[i] > max_done_time:
          max_done_time = self.MoveDoneTime[i]
    
    if not flag1:
      try:
        await self.motors_wait_for_move_done(timeout_msec=max_done_time - (time.time() * 1000))
      except Exception as e:
        self.MoveErrorIndex = index
        self.MoveErrorCode = str(e)
        raise e
    
    self.SendingMoveCommand = True
    if self.PVTTimeIntervalMsec != time_interval_msec:
      await self.pvt_set_time_interval(time_interval_msec)
      self.PVTTimeIntervalMsec = time_interval_msec
      
    await self.pvt_select_mode(enable=True)
    
    for i in range(grp_len):
      if not skip_ax[i]:
        self.move_done_response[self.node_id_list.index(self.grp_node_id[i])] = False
        num1 = i
    
    # Pre-fill buffer (first 8 points or fewer)
    pva_cols = len(pva_points[0]) # Assuming rectangular
    
    index3 = 0
    while index3 < pva_cols:
      for i in range(grp_len):
        if not skip_ax[i]:
          nid = self.grp_node_id[i]
          # Accessing pva_points[nid][index3] based on C#
          await self.pvt_send_data_point(nid, pva_points[nid][index3].P, pva_points[nid][index3].V)
      index3 += 1
      if index3 > 7:
        break
    
    for i in range(grp_len):
      nid = self.grp_node_id[i]
      # Reset queue status flags in internal structures
      self.PVT_EMCY[nid].QueueFull.State = False
      self.PVT_EMCY[nid].QueueLow.State = False
      
    await self.pvt_begin_motion(begin=True, skip_ax=skip_ax)
    
    tick_count1 = time.time() * 1000
    self.MoveBuffer[index].MotionStarted = True
    self.SendingMoveCommand = False
    
    index6 = 8
    while index6 < pva_cols:
      for i in range(grp_len):
        if not skip_ax[i]:
          nid = self.grp_node_id[i]
          await self.pvt_send_data_point(nid, pva_points[nid][index6].P, pva_points[nid][index6].V)
      
      # Timing loop
      while (time.time() * 1000 - tick_count1) < ((index6 - 7) * time_interval_msec):
        if self.PVTStop:
          # Handle Emergency Stop Calculation
          elapsed = (time.time() * 1000) - tick_count1
          a = elapsed / time_interval_msec
          index8 = int(round(a))
          if index8 < a: index8 += 1
          
          for i in range(grp_len):
            nid = self.grp_node_id[i]
            vel = pva_points[nid][index8].V
            denom = 2 * stop_deceleration[nid]
            num_array1[i] = int(round((vel ** 2) / denom))
          
          # Search for stop point
          index11 = 0 # Placeholder for found index
          found_stop = False
          index10 = index8 + 1
          
          while index10 < pva_cols:
            found_stop = True
            for i in range(grp_len):
               nid = self.grp_node_id[i]
               diff = abs(pva_points[nid][index10].P - pva_points[nid][index8].P)
               if diff < num_array1[i]:
                 found_stop = False
            if found_stop:
              index11 = index10
              break
            index10 += 1
          
          if found_stop:
            for i in range(grp_len):
              nid = self.grp_node_id[i]
              dist = abs(pva_points[nid][index11].P - pva_points[nid][index8].P)
              vel = pva_points[nid][index8].V
              if dist == 0:
                val = 1000
              else:
                val = int(round((vel ** 2) / (2 * dist)))
              
              num_array2[i] = val
              await self.can_sdo_download_elmo_object(nid, 24708, 0, str(val), eElmoObjectDataType.UNSIGNED32)
            
            # Wait out the calculated time
            target_time = index8 * time_interval_msec
            while (time.time() * 1000 - tick_count1) < target_time:
              await asyncio.sleep(0.001)
            
            # Stop Logic
            for i in range(grp_len):
              if not skip_ax[i]:
                nid = self.grp_node_id[i]
                sync = (i == num1)
                await self.control_word_set(nid, 271, sync)
            
            # Jump to end label logic
            self.SendingMoveCommand = False
            # Wait for move done
            for i in range(grp_len):
              curr = time.time() * 1000
              remaining = timeout_msec - (curr - tick_count1) + 1000
              if remaining < 1000: remaining = 1000
              if not skip_ax[i]:
                await self.motor_wait_for_move_done(self.grp_node_id[i], remaining)
            
            await self.pvt_begin_motion(begin=False, skip_ax=skip_ax)
            # Event trigger omitted for brevity logic break
            return

        await asyncio.sleep(0.001)

      # Check Queue Health
      for i in range(grp_len):
        nid = self.grp_node_id[i]
        if (self.PVT_EMCY[nid].QueueFull.State or (self.PVT_EMCY[nid].QueueLow.State and not skip_ax[i])):
           status = "full" if self.PVT_EMCY[nid].QueueFull.State else "empty"
           self.ErrorCode[15] = f"PVT fault. Axis {nid} queue {status}."
           raise RuntimeError(self.ErrorCode[15])
      
      index6 += 1
    
    # End of motion sequence (label_82 equivalent logic for normal finish)
    self.SendingMoveCommand = False
    for i in range(grp_len):
      curr_tick = time.time() * 1000
      rem = timeout_msec if timeout_msec <= (curr_tick - tick_count1) else (timeout_msec - (curr_tick - tick_count1) + 1000)
      if rem < 1000: rem = 1000
      if not skip_ax[i]:
        await self.motor_wait_for_move_done(self.grp_node_id[i], int(rem))
    
    await self.pvt_begin_motion(begin=False, skip_ax=skip_ax)
    
    event_data['EventType'] = eEventType.MotorsMovePathDone
    event_data['ME_Index'] = index
    event_data['MMD_AllMovesDone'] = (self.MoveBufferFirstIndex == self.MoveBufferLastIndex)
    self.raise_an_event(event_data)

  async def configure_input_logic(
    self,
    node_id: int,
    input_num: int,
    logic_high: bool,
    logic_type: eInputLogic,
    ignore_op_mode_error: bool = False
  ) -> None:
    val_to_set = logic_type.value
    if logic_high:
      val_to_set += 1
      
    right = await self.binary_interpreter(node_id, "IL", input_num, eCmdType.ValQuery)
    
    if val_to_set != right:
      await self.binary_interpreter(node_id, "IL", input_num, eCmdType.ValSet, str(val_to_set))
      await asyncio.sleep(0.25)
      
    # Update local config cache
    # Assuming self.NodeInputConfig is a 2D structure/list
    # idx = self.get_node_id_index(node_id)
    # self.NodeInputConfig[idx][input_num].Logic = logic_type
    # self.NodeInputConfig[idx][input_num].LogicHigh = logic_high

  async def read_input(self, node_id: int, input_num: int) -> bool:
    """ Returns the State (bool). """
    left = await self.binary_interpreter(node_id, "IB", input_num, eCmdType.ValQuery)
    return left == 1

  async def read_analog_input(self, node_id: int, input_num: int) -> float:
    """ Returns the Voltage. """
    val = await self.binary_interpreter(node_id, "AN", input_num, eCmdType.ValQuery)
    try:
      return float(val)
    except ValueError:
      return 0.0

  async def read_output(self, node_id: int, output_num: int) -> bool:
    """ Returns the State. """
    expression = await self.binary_interpreter(node_id, "OP", 0, eCmdType.ValQuery)
    val = int(expression)
      
    mask = int(round(math.pow(2, output_num - 1)))
    return (val & mask) == mask

  async def set_output(self, node_id: int, output_num: int, state: bool) -> str:
    val = "1" if state else "0"
    return await self.binary_interpreter(node_id, "OB", output_num, eCmdType.ValSet, val)

  async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int:
    """ Returns the Position (int). """
    cmd = "PU" if pu else "PX"
    val_str = await self.binary_interpreter(node_id, cmd, 0, eCmdType.ValQuery)
    try:
      return int(round(float(val_str)))
    except ValueError:
      return 0

  def motor_position_thread_stop(self):
    self.bMotorPositionQueryThreadRunning = False
    # Thread abort logic not applicable in Python async

  async def motor_position_thread(self):
    # Converted to an async loop
    self.bMotorPositionQueryThreadRunning = True
    
    while self.bMotorPositionQueryThreadRunning:
      if not self.SendingMoveCommand and not self.SendingSVCommand:
        grp_id = self.grp_id
        try:
          expression = await self.binary_interpreter(grp_id, "PX", 0, eCmdType.ValQuery, low_priority=True)
        except:
          self.bMotorPositionQueryThreadRunning = False
          break
        
        # Logic for extra node check
        val = ""
        if len(self.grp_node_id) < len(self.NodeIDList):
          last_node = self.NodeIDList[-1]
          try:
            val = await self.binary_interpreter(last_node, "PX", 0, eCmdType.ValQuery, low_priority=True)
          except:
            self.bMotorPositionQueryThreadRunning = False
            break
        
        ary_src = expression.split(',')
        if len(self.grp_node_id) < len(self.NodeIDList):
           ary_src.append(val)
        
        try:
          num_array = [int(float(x)) for x in ary_src]
        except ValueError:
          continue # Goto label_27 equivalent
        
        if len(self.EncoderPosition) != len(num_array):
          self.bMotorPositionQueryThreadRunning = False
          break
        
        flag = False
        changed_node_id = 0
        for i in range(len(num_array)):
          if num_array[i] != self.EncoderPosition[i]:
            self.EncoderPosition[i] = num_array[i]
            flag = True
            if changed_node_id == 0:
              changed_node_id = i + 1
        
        if flag:
          event_data = {}
          event_data['EventType'] = eEventType.MotorPositionChanged
          event_data['NodeID'] = changed_node_id
          event_data['MPC_Position'] = self.EncoderPosition
          self.raise_an_event(event_data)
        
        await asyncio.sleep(1.0)
      else:
        await asyncio.sleep(0.1)

  async def motor_get_motion_status(self, node_id: int) -> int:
    """ Returns MotionStatus. """
    val = await self.binary_interpreter(node_id, "MS", 0, eCmdType.ValQuery)
    return int(round(float(val)))

  async def measure_drive_timer(self, node_id: int, measurement_period: int) -> float:
    """ Returns PCtoDriveRatio. """
    val = await self.binary_interpreter(node_id, "TM", 0, eCmdType.ValQuery)
    val1 = int(float(val))
    
    t1 = time.time() * 1000
    await asyncio.sleep(measurement_period / 1000.0)
    t2 = time.time() * 1000
    
    val = await self.binary_interpreter(node_id, "TM", 0, eCmdType.ValQuery)
    val2 = int(float(val))
    
    pc_diff = t2 - t1
    drive_diff = round((val2 - val1) / 1000.0)
    
    return pc_diff / drive_diff

  async def motor_enable(self, node_id: int, state: bool) -> None:
    self.SendingMoveCommand = False
    flag = not (node_id in self.grp_node_id or node_id == self.grp_id)

    if state:
      self.EmcyMoveErrorReceived = False
      if flag:
        await self.binary_interpreter(node_id, "MO", 0, eCmdType.ValSet, "1")
      else:
        # Standard DS402 Enable Sequence
        await self.control_word_set(node_id, 0)
        await self.control_word_set(node_id, 128)
        await self.control_word_set(node_id, 6)
        await self.control_word_set(node_id, 7)
        await self.control_word_set(node_id, 15)

      await asyncio.sleep(0.1)
      left = await self.binary_interpreter(node_id=node_id, cmd="MO", cmd_index=0, cmd_type=eCmdType.ValQuery, val_type=eValType.Int)
      if left != 1:
         raise CanError(f"Motor failed to enable (NodeID = {node_id})")
    else:
      if flag:
        try:
          await self.binary_interpreter(node_id, "MO", 0, eCmdType.ValSet, "0")
        except Exception as e:
          pass
      else:
         await self.control_word_set(node_id, 7)
         await self.control_word_set(node_id, 6)
      
      await asyncio.sleep(0.1)
      left = await self.binary_interpreter(node_id, "MO", 0, eCmdType.ValQuery)
      if left != 0:
         raise RuntimeError(f"Motor failed to disable (NodeID = {node_id}")

  async def motor_set_move_direction(self, node_id: int, direction: eJointMoveDirection) -> None:
    idx = self.node_id_list.index(node_id)
    # if self.gcJointMoveDirection[idx] == direction and self.JointMoveDirectionInitialized[idx]:
    #   return

    val_str = "1"
    if direction == eJointMoveDirection.Clockwise: val_str = "65"
    elif direction == eJointMoveDirection.Counterclockwise: val_str = "129"
    elif direction == eJointMoveDirection.ShortestWay: val_str = "193"
    
    await self.can_sdo_download_elmo_object(node_id, 24818, 0, val_str, eElmoObjectDataType.UNSIGNED16)
    
    # self.gcJointMoveDirection[idx] = direction
    # self.JointMoveDirectionInitialized[idx] = True

  async def motor_emergency_stop(self, node_id: int) -> None:
    await self.binary_interpreter(node_id, "MO", 0, eCmdType.ValSet, "0")

  async def motor_set_homing_deceleration(self, node_id: int, deceleration: int) -> None:
    left2 = await self.binary_interpreter(node_id, "SD", 0, eCmdType.ValQuery)
    if deceleration == left2:
      return
      
    left1 = await self.binary_interpreter(node_id, "MO", 0, eCmdType.ValQuery)
    if left1 != 0:
      await self.motor_enable(node_id, False)
      
    await self.binary_interpreter(node_id, "SD", 0, eCmdType.ValSet, str(deceleration))
    
    if left1 == 1:
      await self.motor_enable(node_id, True)

  async def user_program_run(
    self,
    axis: int,
    user_function: str,
    params = None,
    timeout_sec: int = 0,
    wait_until_done: bool = False,
  ) -> int:
    """
    Runs a user program on `axis` and optionally waits for completion.

    Returns:
      last_line_completed (0 if unknown / not provided by controller)
    Raises:
      CanError on any controller/protocol/runtime failure or timeout.
    """
    if not isinstance(axis, int):
      raise ValueError("axis must be int")
    if axis < 0 or axis > 255:
      raise ValueError("axis must be in [0, 255]")

    node_id = axis

    # PS query
    ps_raw = await self.binary_interpreter(node_id, "PS", 0, eCmdType.ValQuery)
    ps = _to_int(ps_raw, "PS")

    if ps == -2:
      raise CanError(f"Axis {axis}: controller reported PS=-2 (not ready / unavailable)")

    # If not idle (-1), request idle by setting UI[1]=0 and wait up to 3s for PS=-1
    if ps != -1:
      await self.binary_interpreter(
        node_id, "UI", 1, eCmdType.ValSet,
        value=0,  # don't stringify bytes; pass normal ints
        val_type=eValType.Int,
      )

      t0 = time.monotonic()
      while (time.monotonic() - t0) < 3.0:
        ps_raw = await self.binary_interpreter(node_id, "PS", 0, eCmdType.ValQuery)
        ps = _to_int(ps_raw, "PS")
        if ps == -1:
          break
        await asyncio.sleep(0.01)
      else:
        raise CanError(f"Axis {axis}: did not reach idle state (PS=-1) within 3s (last PS={ps})")

    # Build "(a,b,c)" argument list
    arg_str = ""
    if params:
      parts = [str(p) for p in params]
      print(parts)
      if parts:
        arg_str = f"({','.join(parts)})"
      print(arg_str)

    # Arm UI[1]=1 then execute XQ
    await self.binary_interpreter(
      node_id, "UI", 1, eCmdType.ValSet,
      value="1",
      val_type=eValType.Int,
    )

    cmd = f"XQ##{user_function}{arg_str}"
    print(cmd)
    await self.os_interpreter(node_id, cmd, query=False)

    last_line_completed = 0

    if wait_until_done:
      # Wait while PS==1 and UI[1]==1, or until timeout
      t0 = time.monotonic()
      ps = 1
      ui1 = 1
      while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < float(timeout_sec):
        ps = _to_int(await self.binary_interpreter(node_id, "PS", 0, eCmdType.ValQuery), "PS")
        ui1 = _to_int( await self.binary_interpreter(node_id, "UI", 1, eCmdType.ValQuery),"UI[1]" )
        await asyncio.sleep(0.01)

      # Grab UI[2] (last line completed) after wait loop (matches original behavior)
      expr_raw = await self.binary_interpreter(node_id, "UI", 2, eCmdType.ValQuery)
      try:
        last_line_completed = int(str(expr_raw).strip())
      except Exception:
        last_line_completed = 0

      # UI[1] should be "0" on successful completion
      if ui1 != 0:
        raise CanError(f"Axis {axis}: user program ended with UI[1]={ui1} (expected 0), last_line={last_line_completed}")

      # Timeout condition: still stuck in running state
      if ps == 1 and ui1 == 1:
        raise CanError(f"Axis {axis}: timeout waiting for '{user_function}' after {timeout_sec}s, last_line={last_line_completed}")

    return 0

  async def home_motor(
    self,
    node_id: int,
    hs_offset: int,
    ind_offset: int,
    home_pos: int,
    srch_vel: int,
    srch_acc: int,
    max_pe: int,
    hs_pe: int,
    offset_vel: int,
    offset_acc: int,
    timeout: int
  ) -> None:
    left = await self.binary_interpreter(node_id, "CA", 41, eCmdType.ValQuery)
    if left == 24: raise RuntimeError("Error 43")

    try:
      await self.motor_hard_stop_search(node_id, srch_vel, srch_acc, max_pe, hs_pe, timeout)
    except Exception as e:
      # Check fault
      fault = await self.motor_get_fault(node_id)
      if fault is not None: raise RuntimeError(fault)
      raise e
      
    await self.motor_enable(node_id=node_id, state=True)
    
    # dummy index ref
    await self.motor_move_execute(node_id=node_id, position=hs_offset, velocity=offset_vel, acceleration=offset_acc, relative=False, timeout=timeout)
    
    is_positive = hs_offset > 0
    await self.motor_index_search(node_id, abs(srch_vel), srch_acc, is_positive, timeout)
    
    await self.motor_move_execute(node_id=node_id, position=ind_offset, velocity=offset_vel, acceleration=offset_acc, relative=False, timeout=timeout)
    await self.motor_reset_encoder_position(node_id, home_pos)
    await self.motor_set_homed_status(node_id, eHomeStatus.Homed)

  async def motor_hard_stop_search(self, axis: int, srch_vel: int, srch_acc: int, max_pe: int, hs_pe: int, timeout: int) -> None:
    self.SendingMoveCommand = True
    await self.binary_interpreter(axis, "ER", 3, eCmdType.ValSet, str(max_pe * 10))
    await self.binary_interpreter(axis, "AC", 0, eCmdType.ValSet, str(srch_acc))
    await self.binary_interpreter(axis, "DC", 0, eCmdType.ValSet, str(srch_acc))
    # Clear homing params
    for i in [3, 4, 5, 2]:
      await self.binary_interpreter(axis, "HM", i, eCmdType.ValSet, "0")

    await self.binary_interpreter(node_id=axis, cmd="JV", cmd_index=0, cmd_type=eCmdType.ValSet, value=str(srch_vel))

    try:
      params = [str(int(hs_pe)), str(int(timeout))]
      try:
        last_line = await self.user_program_run(axis, "Home", params, int(timeout/1000), True)
        if last_line in [1, 2, 3]:
          raise RuntimeError(f"Homing Script Error {34 + last_line}")
      except Exception as e:
        # Re-raise unless specific handling needed
        raise e
        
      curr_pos = await self.motor_get_current_position(axis)
      print(f"Homing complete, current position: {curr_pos}")
      
      await self.binary_interpreter(axis, "PA", 0, eCmdType.ValSet, str(curr_pos))
      await self.binary_interpreter(axis, "SP", 0, eCmdType.ValSet, str(srch_vel))
      await self.binary_interpreter(axis, "AC", 0, eCmdType.ValSet, str(srch_acc))
      await self.binary_interpreter(axis, "DC", 0, eCmdType.ValSet, str(srch_acc))
      
      # await asyncio.sleep(0.3)
      # await self.binary_interpreter(axis, "BG", 0, eCmdType.Execute, value="0")
      # await asyncio.sleep(0.3)
      # await self.binary_interpreter(axis, "ER", 3, eCmdType.ValSet, str(max_pe))
    finally:
      # Stop any lingering motion/scripts
      await asyncio.sleep(0.3)
      await self.binary_interpreter(axis, "BG", 0, eCmdType.Execute, value="0") 
      await asyncio.sleep(0.3)

      # Restore Error Range to normal safety limits
      await self.binary_interpreter(axis, "ER", 3, eCmdType.ValSet, str(int(max_pe)))

      # Force Motor Off to kill any zombie scripts holding the state machine
      # if self.move_error_code != 0: # Only if we had an error
      #   await self.binary_interpreter(axis, "MO", 0, eCmdType.ValSet, "0")

      self.SendingMoveCommand = False

  async def motor_index_search(
    self,
    axis: int,
    srch_vel: int,
    srch_acc: int,
    positive_direction: bool,
    timeout: int
  ) -> Tuple[int, int]:
    """ Returns (OneRevolution, CapturedPosition). """
    self.SendingMoveCommand = True
    await self.binary_interpreter(axis, "HM", 1, eCmdType.ValSet, "0")
    
    rev = await self.binary_interpreter(axis, "CA", 18, eCmdType.ValQuery)
    one_revolution = int(float(rev))
    if not positive_direction:
      one_revolution *= -1
      
    await self.binary_interpreter(axis, "PR", 1, eCmdType.ValSet, str(one_revolution))
    await self.binary_interpreter(axis, "SP", 0, eCmdType.ValSet, str(srch_vel))
    await self.binary_interpreter(axis, "AC", 0, eCmdType.ValSet, str(srch_acc))
    await self.binary_interpreter(axis, "DC", 0, eCmdType.ValSet, str(srch_acc))
    
    await self.binary_interpreter(axis, "HM", 3, eCmdType.ValSet, "3") # Index only
    await self.binary_interpreter(axis, "HM", 4, eCmdType.ValSet, "0")
    await self.binary_interpreter(axis, "HM", 5, eCmdType.ValSet, "0")
    await self.binary_interpreter(axis, "HM", 2, eCmdType.ValSet, "0")
    await self.binary_interpreter(axis, "HM", 1, eCmdType.ValSet, "1") # Arm
    
    self.move_done_response[self.node_id_list.index(axis)] = False
    await self.binary_interpreter(axis, "BG", 0, eCmdType.Execute)
    
    await self.motor_wait_for_move_done(axis, timeout)
    
    left = await self.binary_interpreter(axis, "HM", 1, eCmdType.ValQuery)
    if left != 0: raise RuntimeError("Error 40")
    
    cap = await self.binary_interpreter(axis, "HM", 7, eCmdType.ValQuery)
    captured_position = int(float(cap))
    
    self.SendingMoveCommand = False
    return one_revolution, captured_position

  async def motor_reset_encoder_position(self, node_id: int, position: float) -> None:
    await self.binary_interpreter(node_id, "HM", 1, eCmdType.ValSet, "0")
    await self.binary_interpreter(node_id, "HM", 3, eCmdType.ValSet, "0")
    await self.binary_interpreter(node_id, "HM", 4, eCmdType.ValSet, "0")
    await self.binary_interpreter(node_id, "HM", 5, eCmdType.ValSet, "0")
    await self.binary_interpreter(node_id, "HM", 2, eCmdType.ValSet, str(position))
    await self.binary_interpreter(node_id, "HM", 1, eCmdType.ValSet, "1")

  async def motor_set_homed_status(self, node_id: int, status: eHomeStatus) -> None:
    val = "0"
    if status == eHomeStatus.Homed: val = "1"
    elif status == eHomeStatus.InitializedWithoutHoming: val = "2"
    await self.binary_interpreter(node_id, "UI", 3, eCmdType.ValSet, val)

  async def motor_get_homed_status(self, node_id: int) -> eHomeStatus:
    left = await self.binary_interpreter(node_id, "UI", 3, eCmdType.ValQuery)
    if left == 1: return eHomeStatus.Homed
    if left == 2: return eHomeStatus.InitializedWithoutHoming
    return eHomeStatus.NotHomed

  async def motor_stop(self, node_id: int) -> None:
    await self.control_word_set(node_id, 271)
    if self.move_done_response[self.node_id_list.index(node_id)]:
      return
    await self.motor_wait_for_move_done(node_id, 2500)
    self.NextEncoderPosition[node_id] = -2000000000
    self.move_done_response[self.node_id_list.index(node_id)] = True
    
    # NMT Stop
    data = [7]
    await self.can_sdo_download(node_id, 96, 96, 0, data)
    data = [1]
    await self.can_sdo_download(node_id, 96, 96, 0, data)

  async def motors_stop(self, wait_until_done: bool = False) -> None:
    # Assuming DataByte handling logic from C# where it checks for '7' return
    # Here we just implement the ControlWord set loop
    for i, nid in enumerate(self.grp_node_id):
       is_sync = (i == len(self.grp_node_id) - 1)
       await self.control_word_set(nid, 271, is_sync)
       
    if wait_until_done:
       for nid in self.grp_node_id:
         await self.motor_wait_for_move_done(nid, 2500)

  async def motors_stop_path(self, wait_until_done: bool = False) -> None:
    self.PVTStop = True
    if wait_until_done:
      for nid in self.grp_node_id:
         await self.motor_wait_for_move_done(nid, 2500)

  async def motors_move_start(self, node_ids: List[int], wait_until_done: bool, timeout_msec: int, index: int) -> None:
    for nid in node_ids:
      self.move_done_response[self.node_id_list.index(nid)] = False
    
    # Set 0x002F (47)
    for i, nid in enumerate(self.grp_node_id):
      sync = (i == len(self.grp_node_id) - 1)
      await self.control_word_set(nid, 47, sync)
      
    # Set 0x003F (63)
    for i, nid in enumerate(self.grp_node_id):
      sync = (i == len(self.grp_node_id) - 1)
      await self.control_word_set(nid, 63, sync)
      
    self.MoveBuffer[index].MotionStarted = True
    self.SendingMoveCommand = False
    
    if wait_until_done:
       start = time.time() * 1000
       for nid in node_ids:
         curr = time.time() * 1000
         rem = timeout_msec if timeout_msec <= (curr - start) else (timeout_msec - (curr - start) + 1000)
         if rem < 1000: rem = 1000
         await self.motor_wait_for_move_done(nid, int(rem))

  async def motor_check_if_move_done(self, node_id: int) -> bool:
    """ Returns Done status. Raises error on fault. """
    ms_val = await self.binary_interpreter(node_id, "MS", 0, eCmdType.ValQuery)
    
    if ms_val == 0:
      return True
    elif ms_val == 1:
      mo_val = await self.binary_interpreter(node_id, "MO", 0, eCmdType.ValQuery)
      if mo_val == 1:
        return True
      else:
        fault = await self.motor_get_fault(node_id)
        if fault is not None:
           raise RuntimeError(f"Motor Fault: {fault}")
        raise RuntimeError("Motor Fault (Unknown)")
    elif ms_val == 2:
      return False

    return False

  async def motors_check_if_move_done(self) -> bool:
    grp_id = self.grp_id
    print("probably won't work because expr is not a list")
    expr = await self.binary_interpreter(grp_id, "MS", 0, eCmdType.ValQuery)
    parts = expr.split(',')
    if len(parts) != len(self.grp_node_id):
      return False
      
    # Logic follows similar pattern to single motor check but iterated
    # Omitted full iteration for brevity, essentially checks "0" or "1" & MO=1
    return True # Stub

  async def motor_wait_for_move_done(self, node_id: int, timeout: int) -> None:
    done = False
    start = time.time() * 1000
    print("motor_wait_for_move_done", timeout)
    
    while (time.time() * 1000 - start) <= (timeout + 2000) and not done:
      if self.move_done_response[self.node_id_list.index(node_id)]:
        done = True
      if self.EmcyMoveErrorReceived:
        raise RuntimeError("Emergency Move Error")
      await asyncio.sleep(0.01)
      
    if not done:
      done = await self.motor_check_if_move_done(node_id)
      if not done:
        print("Timeout waiting for move done", time.time())
        raise TimeoutError("Error 30")

  async def motors_wait_for_move_done(self, timeout_msec: int) -> None:
    # Loop checking MoveDoneResponse array
    done = False
    start = time.time() * 1000
    
    while (time.time() * 1000 - start) <= (timeout_msec + 2000) and not done:
      done = True
      for nid in self.grp_node_id:
        if not self.move_done_response[nid]:
          done = False
      
      if self.EmcyMoveError:
        # Fault handling logic
        raise RuntimeError(f"Emergency Error: {self.EmcyMoveError}")
      
      await asyncio.sleep(0.01)
      
    if not done:
       # Check individual statuses
       pass

  async def motor_get_fault(self, node_id: int) -> Optional[str]:
    val = await self.binary_interpreter(node_id, "MF", 0, eCmdType.ValQuery)

    assert isinstance(val, int), "Expected integer fault code"

    if val == 0:
      return None
    faults = []
    if val & 1: faults.append("Hall sensor feedback angle not found")
    if val & 4: faults.append("Feedback loss")
    if val & 8: faults.append("Peak current exceeded")
    # ... map remaining bits from C# ...
    return "; ".join(faults)

  async def pvt_send_data_point(self, node_id: int, position: int, velocity: int) -> None:
    p_bytes = position.to_bytes(4, byteorder='little', signed=True)
    v_bytes = velocity.to_bytes(4, byteorder='little', signed=True)
    # RPDO3 is used for PVT data usually
    await self.can_write(eCOBType.RPDO3, node_id, p_bytes[0], p_bytes[1], p_bytes[2], p_bytes[3], v_bytes[0], v_bytes[1], v_bytes[2], v_bytes[3])

  # async def pvt_get_buffer_position(self, node_id: int) -> int:
  #   # Stub for CAN read logic, returns byte
    # return 0

  async def pvt_set_buffer_position(self, node_id: int, buffer_position: int) -> None:
    data = [buffer_position]
    await self.can_sdo_download(node_id, 96, 196, 4, data)

  async def pvt_get_buffer_size(self, node_id: int) -> int:
    return 0

  async def pvt_set_buffer_size(self, node_id: int, buffer_size: int) -> None:
    await self.can_sdo_download_elmo_object(node_id, 24772, 2, str(buffer_size), eElmoObjectDataType.UNSIGNED32)

  async def pvt_set_time_interval(self, time_interval_msec: int) -> None:
    for nid in self.grp_node_id:
      await self.can_sdo_download_elmo_object(nid, 24770, 1, str(time_interval_msec), eElmoObjectDataType.UNSIGNED8)

  async def pvt_begin_motion(self, begin: bool, skip_ax: List[bool]) -> None:
    target_idx = 0
    for i, skip in enumerate(skip_ax):
      if not skip: target_idx = i
      
    if begin:
      for i, nid in enumerate(self.grp_node_id):
        sync = (i == target_idx)
        if not skip_ax[i]:
          await self.control_word_set(nid, 31, sync)
    else:
      for i, nid in enumerate(self.grp_node_id):
        sync = (i == len(self.grp_node_id) - 1)
        await self.control_word_set(nid, 15, sync)

  async def control_word_set(self, node_id: int, value: int, sync: bool = True) -> None:
    val_bytes = value.to_bytes(2, byteorder='little')
    await self.can_write(eCOBType.RPDO1, node_id, val_bytes[0], val_bytes[1])
    if sync:
      await self.can_sync()


  async def motors_check_if_move_done(self) -> bool:
    """ Returns True if all done, False otherwise. Raises RuntimeError on fault. """
    grp_id = self.grp_id
    # Query Motion Status for the group
    print("will probably break because expression is an int not a string.")
    expression = await self.binary_interpreter(grp_id, "MS", 0, eCmdType.ValQuery)
    
    str_array = expression.split(',')
    
    # Verify response length matches group size
    if len(str_array) != len(self.grp_node_id):
      return False
    
    is_done = True
    
    for i, val_str in enumerate(str_array):
      if int(val_str) == 0:
        # Axis Done
        pass 
      elif val_str == "1":
        # Check Motor Enable Status (MO)
        node_id = self.grp_node_id[i]
        mo_val = await self.binary_interpreter(node_id, "MO", 0, eCmdType.ValQuery)
        
        if mo_val != "1":
          # If MS=1 (Moving) but MO!=1 (Enabled), likely a fault
          is_done = False
          fault_str = await self.motor_get_fault(node_id)
          if fault_str is not None:
             raise RuntimeError(f"Motor {node_id} Fault: {fault_str}")

          raise RuntimeError(f"Motor {node_id} fault (Unknown).")
      elif val_str == "2":
        # Not Done / Error state in Elmo?
        is_done = False
      else:
        is_done = False

    return is_done

  async def motors_wait_for_move_done(self, timeout_msec: int) -> None:
    tick_count1 = time.time() * 1000
    start_time = tick_count1
    done = False
    fault = ""
    
    while (time.time() * 1000 - start_time) <= (timeout_msec + 2000) and not done:
      # Check internal Done Response flags
      done = True
      for i, nid in enumerate(self.grp_node_id):
        if not self.move_done_response[i]: # Assumes MoveDoneResponse is indexed by group index here based on C#
          done = False
      
      # Check Emergency Errors
      if self.EmcyMoveError:
        self.ErrorCode[1] = ""
        await asyncio.sleep(1.0)
        
        if "Unknown error 0." not in self.EmcyMoveError:
          self.ErrorCode[1] += f"Axis {self.EmcyMoveErrorNodeID} Fault: {self.EmcyMoveError} "
          if "E-stop button was pressed" in self.EmcyMoveError:
            raise RuntimeError(self.ErrorCode[1])
        
        # Check individual motor faults
        for nid in self.grp_node_id:
          fault = await self.motor_get_fault(nid)
          if fault is not None:
            raise RuntimeError(fault)
        
        if self.ErrorCode[1]:
          raise RuntimeError(self.ErrorCode[1])

      await asyncio.sleep(0.01)

    if not done:
      # Final check using SDO query
      done = await self.motors_check_if_move_done()
      if done:
        return

      # If still not done, compile status for error message
      status_msg = "Motion timed out. Motion Status"
      for i, nid in enumerate(self.grp_node_id):
        ms = await self.motor_get_motion_status(nid)
        status_msg += f" Axis{i}={ms}"
        if i < len(self.grp_node_id) - 1:
          status_msg += ", "
      
      raise RuntimeError(status_msg)


class KX2:
  def __init__(self):
    self.can = KX2Can()
    self.should_ax = 1
    self.z_ax = 2
    self.elbow_ax = 3
    self.wrist_ax = 4
    self.rail_ax = 5
    self.servo_gripper_ax = 6

    self.num_axes = 4

    self.DigitalInputAssignment = {}
    self.AnalogInputAssignment = {}
    self.OutputAssignment = {}
    self.drive_serial_number = {}
    self.motor_conversion_factor_ax = {}
    self.max_travel_ax = {}
    self.min_travel_ax = {}
    self.unlimited_travel_ax = {}
    self.absolute_encoder_ax = {}
    self.max_vel_ax = {}
    self.max_accel_ax = {}
    self.ErrorCode = {}
    self.drive_serial_number = {}

    self.g_joint_move_direction = {
      1: eJointMoveDirection.Normal,
      2: eJointMoveDirection.Normal,
      3: eJointMoveDirection.Normal,
      4: eJointMoveDirection.Normal,
      6: eJointMoveDirection.Normal,
    }

    self.registry_get_settings()

    self.node_id_list = [1, 2, 3, 4, 6]

  def registry_get_settings(self):
    self.master_velocity_scale = 100.0

    self.use_default_gripper_state = 1
    self.default_gripper_shutdown_state = None
    self.default_gripper_state = 0
    self.modeless_sub_windows = False
    self.vel_lim_enabled = 0
    self.vel_lim_max = 10
    self.vel_linear_lim_max = 100
    self.vel_lim_input_axis = 0
    self.vel_lim_input_num = 4
    self.vel_lim_input_state = 1
    self.automatic_move_error_recovery = False
    self.automatic_move_error_recovery_attempts = 1
    self.automatic_move_error_recovery_use_button = False
    self.jog_velocity_scale_shoulder = 100
    self.jog_accel_shoulder = 100
    self.jog_velocity_scale_z = 100
    self.jog_accel_z = 50
    self.jog_velocity_scale_elbow = 100
    self.jog_accel_elbow = 100
    self.jog_velocity_scale_wrist = 100
    self.jog_accel_wrist = 100
    self.warning_before_move = True
    self.warning_duration = 5
    self.warning_after_idle_time = 30
    self.servo_gripper_closed_position = 0
    self.servo_gripper_open_position = 5
    self.servo_gripper_force_percent = 100
  
  async def initialize(self):
    await self.can.connect()  # just to get the node IDs
    await self.drive_get_parameters(self.can.node_id_list)
    print(self.can.grp_node_id)
    self.can.grp_node_id = [1, 2, 3, 4]
    print(self.can.grp_node_id)
    await self.can.connect_part_two()

    for node in range(1, self.num_axes+1):
      if self.unlimited_travel_ax[node]:
        self.g_joint_move_direction[node] = eJointMoveDirection.ShortestWay
    
    # MoveDataSyncRegistryToDrives

    for node in range(1, self.num_axes+1):
      try:
        await self.can.motor_enable(node_id=node, state=True)
      except Exception as e:
        print(f"Error enabling motor on node {node}: {e}")
    
    # await self.servo_gripper_initialize()
    
  async def servo_gripper_initialize(self):
    try:
      await self.can.motor_enable(node_id=self.servo_gripper_ax, state=True)
    except Exception as e:
      print(f"Error enabling servo gripper motor on node {self.servo_gripper_ax}: {e}")
    
    await self.servo_gripper_home()

    await self.servo_gripper_close()

  async def servo_gripper_home(self) -> None:
    await self.motor_send_command(
      axis=self.servo_gripper_ax,
      motor_command="PL",
      index=1,
      value=str(self.servo_gripper_peak_current),
      val_type=eValType.Float,
    )

    await self.motor_send_command(
      axis=self.servo_gripper_ax,
      motor_command="CL",
      index=1,
      value=str(self.servo_gripper_continuous_current),
      val_type=eValType.Float,
    )

    await self.can.home_motor(
      node_id=self.servo_gripper_ax,
      hs_offset=self.servo_gripper_home_hard_stop_offset,
      ind_offset=self.ServoGripperHomeIndexOffset,
      home_pos=self.servo_gripper_home_pos,
      srch_vel=self.servo_gripper_home_search_vel,
      srch_acc=self.servo_gripper_home_search_accel,
      max_pe=self.servo_gripper_home_default_position_error,
      hs_pe=self.servo_gripper_home_hard_stop_position_error,
      offset_vel=self.ServoGripperHomeOffsetVel,
      offset_acc=self.ServoGripperHomeOffsetAccel,
      timeout=self.ServoGripperHomeTimeoutMsec,
    )

    await self.servo_gripper_set_default_gripping_force(100)
  
  async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) -> None:
    if max_force_percent < 10:
      max_force_percent = 10
    elif max_force_percent > 100:
      max_force_percent = 100

    cont_current = float(self.servo_gripper_continuous_current) * max_force_percent / 100.0
    peak_current = float(self.servo_gripper_peak_current) * max_force_percent / 100.0

    axis = int(self.servo_gripper_ax)

    # 1) PL with unscaled peak current
    await self.motor_send_command(
      axis, "PL", 1, str(self.servo_gripper_peak_current), val_type=eValType.Float
    )

    # 2) CL with scaled continuous current
    await self.motor_send_command(
      axis, "CL", 1, str(cont_current), val_type=eValType.Float
    )

    # 3) PL with scaled peak current
    await self.motor_send_command(
      axis, "PL", 1, str(peak_current), val_type=eValType.Float
    )

    self.servo_gripper_force_percent = max_force_percent
  
  async def servo_gripper_close(self) -> None:
    calc_timeout = await self.move_absolute_single_axis(
      axis=self.servo_gripper_ax,
      position=self.servo_gripper_closed_position,
      vel=abs(100),
      accel=abs(100),  # matches velocity
    )
    # move_absolute_single_axis does not wait for done in c#
    # for us it does wait
    # await asyncio.sleep(calc_timeout / 1000.0 + 1)

    # TODO: calculate whether something is gripped based on the gripping force feedback
  
  async def drive_set_move_count_parameters(
    self,
    move_count: int,
    travel: List[float],
    last_maintenance_performed: float,
    maintenance_required: bool,
    last_maintenance_performed_date: int,
    last_maintenance_performed_rail: float,
    maintenance_required_rail: bool,
    last_maintenance_performed_date_rail: int,
  ) -> None:
    # MoveCount -> Z axis, UI index 22
    await self.motor_send_command(
      axis=self.z_ax,
      motor_command="UI",
      index=22,
      value=str(int(move_count)),
      val_type=eValType.Int,
      low_priority=False,
    )

    # Travel[] -> each node, UF index 5
    # The source looked 1-based for Travel and 0-based for NodeIDList; handle both cleanly.
    if len(travel) == len(self.node_id_list) + 1:
      pairs = zip(self.node_id_list, travel[1:])
    else:
      pairs = zip(self.node_id_list, travel)

    for node_id, dist in pairs:
      await self.motor_send_command(
        axis=int(node_id),
        motor_command="UF",
        index=5,
        value=str(float(dist)),
        val_type=eValType.Float,
        low_priority=True,
      )

    # LastMaintenancePerformed -> Z axis, UF index 6
    await self.motor_send_command(
      axis=self.z_ax,
      motor_command="UF",
      index=6,
      value=str(float(last_maintenance_performed)),
      val_type=eValType.Float,
      low_priority=True,
    )

    # MaintenanceRequired -> Z axis, UI index 23
    await self.motor_send_command(
      axis=self.z_ax,
      motor_command="UI",
      index=23,
      value="1" if maintenance_required else "0",
      val_type=eValType.Int,
      low_priority=False,
    )

    # LastMaintenancePerformedDate -> Z axis, UI index 21
    await self.motor_send_command(
      axis=self.z_ax,
      motor_command="UI",
      index=21,
      value=str(int(last_maintenance_performed_date)),
      val_type=eValType.Int,
      low_priority=False,
    )

    # Rail (if present)
    if self.robot_on_rail:
      await self.motor_send_command(
        axis=self.rail_ax,
        motor_command="UF",
        index=6,
        value=str(float(last_maintenance_performed_rail)),
        val_type=eValType.Float,
        low_priority=True,
      )

      await self.motor_send_command(
        axis=self.rail_ax,
        motor_command="UI",
        index=23,
        value="1" if maintenance_required_rail else "0",
        val_type=eValType.Int,
        low_priority=False,
      )

      await self.motor_send_command(
        axis=self.rail_ax,
        motor_command="UI",
        index=21,
        value=str(int(last_maintenance_performed_date_rail)),
        val_type=eValType.Int,
        low_priority=False,
      )

  
  async def drive_get_parameters(self, node_ids) -> None:
    self.robot_on_rail = False
    self.has_servo_gripper = False

    nodes = [int(b) for b in node_ids] if isinstance(node_ids, (bytes, bytearray)) else [int(x) for x in node_ids]

    def set2d(store: dict, axis: int, ch: int, value: str) -> None:
      store.setdefault(axis, {})[ch] = value

    # Pass 1: identify axes by UI[4]
    for axis in nodes:
      ret = await self.motor_send_command(axis, "UI", 4, val_type=eValType.Int)
      if ret == 1:
        self.should_ax = axis
        self.shoulder_node_id = axis
      elif ret == 2:
        self.z_ax = axis
        self.z_node_id = axis
      elif ret == 3:
        self.elbow_ax = axis
        self.elbow_node_id = axis
      elif ret == 4:
        self.wrist_ax = axis
        self.wrist_node_id = axis
      elif ret == 5:
        self.rail_ax = axis
        self.rail_node_id = axis
        self.robot_on_rail = True
      elif ret == 6:
        self.servo_gripper_ax = axis
        self.servo_gripper_node_id = axis
        self.has_servo_gripper = True

    # Pass 2: per-axis parameters
    for axis in nodes:
      print()
      print("axis", axis)

      # UI[5..10] digital inputs
      for ui_idx in range(5, 11):
        ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=eValType.Int)
        ch = (ui_idx - 5) + 1
        if ret == "101":
          set2d(self.DigitalInputAssignment, axis, ch, "GripperSensor")
          self.GripperSensorAxis = axis
          self.GripperSensorInput = ch
        elif ret == "102":
          set2d(self.DigitalInputAssignment, axis, ch, "TeachButton")
          self.TeachButtonAxis = axis
          self.TeachButtonInput = ch
        else:
          set2d(
            self.DigitalInputAssignment,
            axis,
            ch,
            "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}",
          )

      # UI[11..12] analog inputs
      for ui_idx in range(11, 13):
        ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=eValType.Int)
        ch = (ui_idx - 11) + 1
        set2d(
          self.AnalogInputAssignment,
          axis,
          ch,
          "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}",
        )

      # UI[13..16] outputs
      for ui_idx in range(13, 17):
        ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=eValType.Int)
        ch = (ui_idx - 13) + 1
        
        if ret == 101:
          set2d(self.OutputAssignment, axis, ch, "IndicatorLightRed")
          self.IndicatorLightRedAxis = axis
          self.IndicatorLightRedOutput = ch
        elif ret == 102:
          set2d(self.OutputAssignment, axis, ch, "IndicatorLightGreen")
          self.IndicatorLightGreenAxis = axis
          self.IndicatorLightGreenOutput = ch
        elif ret == 103:
          set2d(self.OutputAssignment, axis, ch, "IndicatorLightBlue")
          self.IndicatorLightBlueAxis = axis
          self.IndicatorLightBlueOutput = ch
        elif ret == 104:
          set2d(self.OutputAssignment, axis, ch, "IndicatorLight")
          self.IndicatorLightAxis = axis
          self.IndicatorLightOutput = ch
        elif ret == 105:
          set2d(self.OutputAssignment, axis, ch, "Buzzer")
          self.BuzzerAxis = axis
          self.BuzzerOutput = ch
        else:
          set2d(
            self.OutputAssignment,
            axis,
            ch,
            "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}",
          )

      # UI[24] drive serial number
      ret = await self.motor_send_command(axis, "UI", 24, val_type=eValType.Int)
      if _is_number(ret):
        self.drive_serial_number[axis] = int(ret)
        # _to_int

      # UF[1], UF[2] conversion factor
      uf1 = await self.motor_send_command(axis, "UF", 1, val_type=eValType.Float)
      uf2 = await self.motor_send_command(axis, "UF", 2, val_type=eValType.Float)
      if not (_is_number(uf1) and _is_number(uf2)) or _to_float(uf1) == 0.0 or _to_float(uf2) == 0.0:
        raise CanError(f"Invalid Motor Conversion Factor for axis {axis}. UF[1]={uf1}, UF[2]={uf2}")
      self.motor_conversion_factor_ax[axis] = _to_float(uf1) / _to_float(uf2)

      # XM / travel
      xm1 = await self.motor_send_command(axis, "XM", 1, val_type=eValType.Int)
      xm2 = await self.motor_send_command(axis, "XM", 2, val_type=eValType.Int)
      uf3 = await self.motor_send_command(axis, "UF", 3, val_type=eValType.Float)
      uf4 = await self.motor_send_command(axis, "UF", 4, val_type=eValType.Float)
      vh3 = await self.motor_send_command(axis, "VH", 3, val_type=eValType.Int)
      vl3 = await self.motor_send_command(axis, "VL", 3, val_type=eValType.Int)

      self.max_travel_ax[axis] = _to_float(uf3)
      self.min_travel_ax[axis] = _to_float(uf4)

      if not all(_is_number(x) for x in (xm1, xm2, vh3, vl3)):
        raise CanError(
          f"Invalid travel limits or modulo settings for axis {axis}. "
          f"VH[3]={vh3}, VL[3]={vl3}, XM[1]={xm1}, XM[2]={xm2}"
        )

      xm1v, xm2v, vh3v, vl3v = map(_to_float, (xm1, xm2, vh3, vl3))
      if ((xm1v == 0.0) and (xm2v == 0.0)) or ((xm1v <= vl3v) and (xm2v >= vh3v)):
        self.unlimited_travel_ax[axis] = False
      elif (xm1v > vl3v) and (xm2v < vh3v):
        self.unlimited_travel_ax[axis] = True
      else:
        raise CanError(
          f"Invalid travel limits or modulo settings for axis {axis}. "
          f"VH[3]={vh3}, VL[3]={vl3}, XM[1]={xm1}, XM[2]={xm2}"
        )

      # Encoder socket/type
      ca45 = await self.motor_send_command(axis, "CA", 45, val_type=eValType.Int)
      ca45v = _to_float(ca45, 0.0)
      print(type(ca45v), ca45v, type(ca45), ca45)
      if (not _is_number(ca45)) or not (0.0 < ca45v <= 4.0):
        raise CanError(f"Invalid encoder socket number received from axis {axis}. CA[45]={ca45}")

      enc_type = await self.motor_send_command(axis, "CA", int(round(40.0 + ca45v)), val_type=eValType.Int)
      if enc_type in (1, 2):
        self.absolute_encoder_ax[axis] = False
      elif enc_type == 24:
        self.absolute_encoder_ax[axis] = True
      else:
        raise CanError(f"Unsupported encoder type specified for axis {axis}. CA[4{ca45}]={enc_type}")

      ca46 = await self.motor_send_command(axis, "CA", 46, val_type=eValType.Int)
      if ca45 == ca46:
        num3 = 1.0
      else:
        ff3 = await self.motor_send_command(axis, "FF", 3, val_type=eValType.Float)
        num3 = _to_float(ff3, 1.0)

      denom = (self.motor_conversion_factor_ax[axis] * num3) # or 1.0

      sp2 = await self.motor_send_command(axis, "SP", 2, val_type=eValType.Int)
      if sp2 == 100000:
        vh2 = await self.motor_send_command(axis, "VH", 2, val_type=eValType.Int)
        self.max_vel_ax[axis] = _to_float(vh2) / 1.01 / denom
      else:
        self.max_vel_ax[axis] = _to_float(sp2) / denom

      sd0 = await self.motor_send_command(axis, "SD", 0, val_type=eValType.Int)
      self.max_accel_ax[axis] = _to_float(sd0) / 1.01 / denom

    # Robot-level params from ShoulderAx
    shoulder = self.should_ax

    self.base_to_gripper_clearance_z = _to_float(await self.motor_send_command(shoulder, "UF", 6,  val_type=eValType.Float))
    self.base_to_gripper_clearance_arm = _to_float(await self.motor_send_command(shoulder, "UF", 7,  val_type=eValType.Float))
    self.wrist_offset = _to_float(await self.motor_send_command(shoulder, "UF", 8,  val_type=eValType.Float))
    self.elbow_offset = _to_float(await self.motor_send_command(shoulder, "UF", 9,  val_type=eValType.Float))
    self.elbow_zero_offset = _to_float(await self.motor_send_command(shoulder, "UF", 10, val_type=eValType.Float))
    self.MaxLinearVelMMPerSec = _to_float(await self.motor_send_command(shoulder, "UF", 11, val_type=eValType.Float))
    self.MaxLinearAccelMMPerSec2 = _to_float(await self.motor_send_command(shoulder, "UF", 12, val_type=eValType.Float))
    self.MaxLinearJerkMMPerSec3 = _to_float(await self.motor_send_command(shoulder, "UF", 13, val_type=eValType.Float))
    self.MaxRotaryVelDegPerSec = _to_float(await self.motor_send_command(shoulder, "UF", 14, val_type=eValType.Float))
    self.MaxRotaryAccelDegPerSec2 = _to_float(await self.motor_send_command(shoulder, "UF", 15, val_type=eValType.Float))

    ui17 = await self.motor_send_command(shoulder, "UI", 17, val_type=eValType.Int)
    self.PVTTimeIntervalMSec = 25 if (not _is_number(ui17) or _to_float(ui17) <= 0.0 or _to_float(ui17) > 255.0) else int(_to_float(ui17))

    # Servo gripper params (only if present)
    sg = self.servo_gripper_ax
    self.servo_gripper_home_pos = _to_int(await self.motor_send_command(sg, "UF", 6,  val_type=eValType.Float))
    self.servo_gripper_home_search_vel = _to_int(await self.motor_send_command(sg, "UF", 7,  val_type=eValType.Float))
    self.servo_gripper_home_search_accel = _to_int(await self.motor_send_command(sg, "UF", 8,  val_type=eValType.Float))
    self.servo_gripper_home_default_position_error = _to_int(await self.motor_send_command(sg, "UF", 9,  val_type=eValType.Float))
    self.servo_gripper_home_hard_stop_position_error = _to_int(await self.motor_send_command(sg, "UF", 10, val_type=eValType.Float))
    self.servo_gripper_home_hard_stop_offset = _to_int(await self.motor_send_command(sg, "UF", 11, val_type=eValType.Float))
    self.ServoGripperHomeIndexOffset = _to_int(await self.motor_send_command(sg, "UF", 12, val_type=eValType.Float))
    self.ServoGripperHomeOffsetVel = _to_int(await self.motor_send_command(sg, "UF", 13, val_type=eValType.Float))
    self.ServoGripperHomeOffsetAccel = _to_int(await self.motor_send_command(sg, "UF", 14, val_type=eValType.Float))
    self.ServoGripperHomeTimeoutMsec = _to_int(await self.motor_send_command(sg, "UF", 15, val_type=eValType.Float))
    self.servo_gripper_continuous_current = _to_float(await self.motor_send_command(sg, "UF", 16, val_type=eValType.Float))
    self.servo_gripper_peak_current = _to_float(await self.motor_send_command(sg, "UF", 17, val_type=eValType.Float))

  async def get_estop_state(self) -> bool:
    """Return True if in estop, False otherwise."""
    r = await self.motor_send_command(
      axis=self.should_ax,
      motor_command="SR",
      index=1,
      value="",
    )
    num2 = not (r & 0x4000 == 0x4000)
    num3 = not (r & 0x8000 == 0x8000)
    if not r == 8438016: print("!!! not the same as c#")
    return num2 == False and num3 == False

  async def motor_send_command(
    self,
    axis: int,
    motor_command: str,
    index: int,
    value: str = "",
    val_type: eValType = eValType.Int,
    *,
    low_priority: bool = False,
  ) -> str:
    print("motor send command", axis, motor_command, index, value, val_type == eValType.Float, f"({val_type})")

    cmd_u = motor_command.upper()
    OS_CMDS = {"VR", "CD", "LS", "DL", "DF", "BH"}

    has_xc = "XC##" in cmd_u
    has_xq = "XQ##" in cmd_u
    use_os = (cmd_u in OS_CMDS) or has_xc or has_xq

    returned_data = ""

    cmd_u = motor_command.upper()

    NO_QUERY_CMDS = {
      "BG", "CP", "EI", "EO", "HP", "HX", "KL", "KR", "LD", "MI", "PB", "RS", "SV", "XC##",
    }

    if value == "":
      if (cmd_u in NO_QUERY_CMDS) or ("XQ##" in cmd_u):
        cmd_type = eCmdType.Execute
      else:
        cmd_type = eCmdType.ValQuery
    else:
      cmd_type = eCmdType.ValSet

    if use_os:
      # VB flag starts True, but forced False for XC## / XQ##
      query_flag = not (has_xc or has_xq)

      # OSInterpreter writes into `str` and can also write a long; you can ignore the long if you don't use it.
      s = await self.can.os_interpreter(
        node_id=int(axis),
        cmd=motor_command,
        query=query_flag,
      )

      if cmd_type == eCmdType.ValQuery:
        returned_data = s if s is not None else ""
    else:
      s = await self.can.binary_interpreter(
        node_id=int(axis),
        cmd=motor_command,
        cmd_index=int(index),
        cmd_type=cmd_type,
        value=value,
        val_type=val_type,
        low_priority=low_priority,
      )

      if cmd_type == eCmdType.ValQuery:
        returned_data = s

    print("motor send command returned", returned_data)
    return returned_data
  
  def convert_elbow_position_to_angle(self, elbow_pos: float) -> float:
    max_travel = self.max_travel_ax[int(self.elbow_ax)]
    denom = max_travel + self.elbow_zero_offset

    if elbow_pos > max_travel:
      x = (2.0 * max_travel - elbow_pos + self.elbow_zero_offset) / denom
      angle = math.asin(x) * (180.0 / math.pi)
      elbow_angle = 90.0 + angle
    else:
      x = (elbow_pos + self.elbow_zero_offset) / denom
      angle = math.asin(x) * (180.0 / math.pi)
      elbow_angle = angle

    return elbow_angle

  def convert_elbow_angle_to_position(self, elbow_angle_deg: float) -> float:
    elbow_pos = (
        (self.max_travel_ax[self.elbow_ax] + self.elbow_zero_offset)
        * math.sin(elbow_angle_deg * (math.pi / 180.0))
        - self.elbow_zero_offset
    )

    if elbow_angle_deg > 90.0:
        elbow_pos = 2.0 * self.max_travel_ax[self.elbow_ax] - elbow_pos

    print("convert_elbow_angle_to_position:", elbow_angle_deg, "->", elbow_pos)
    return elbow_pos
  
  async def motor_get_current_position(self, axis: int) -> float:
    raw = await self.can.motor_get_current_position(node_id=axis, pu=self.unlimited_travel_ax[axis])
    c = self.motor_conversion_factor_ax[axis]
    if axis == self.elbow_ax:
      return self.convert_elbow_angle_to_position(elbow_angle_deg=raw / c)
    else:
      if c == 0:
        print("node", axis, "has conversion factor of 0")
        return 0
      else:
        return raw / c
  
  async def read_input(self, axis: int, input_num: int) -> bool:
    r = await self.can.read_input(node_id=axis, input_num=0x10 + input_num)
    return r

  async def move_absolute_single_axis(
    self,
    axis: int,
    position: float,
    vel: float,
    accel: float,
    calculated_move_time_msec: Optional[List[int]] = None,
    send_event_when_move_done: bool = False,
  ) -> int:
    print(f"move_absolute_single_axis: axis={axis}, position={position}, vel={vel}, accel={accel}")

    # self.jog_stopped[axis] = False

    # Master velocity scaling
    vel = vel * self.master_velocity_scale * 0.01

    # Optional velocity limit input gating
    if self.vel_lim_enabled and vel > self.vel_lim_max:
      print("vel_lim_enabled shoudl be off")
      state = await (self.read_input(self.vel_lim_input_axis, self.vel_lim_input_num))
      # C# logic: (State & VelLimInputState==1) | (!State & VelLimInputState==0)
      if ((state) and self.vel_lim_input_state == 1) or ((not (state)) and self.vel_lim_input_state == 0):
        vel = self.vel_lim_max

    # Axis validity (1-based axes, plus optional servo gripper axis)
    allowed = (1 <= axis <= self.num_axes) or (self.has_servo_gripper and axis == self.servo_gripper_ax)
    if not allowed:
      raise CanError(30, f"Invalid axis {axis}")

    # Parameter ranges
    if vel > 100.0 or vel <= 0.0:
      raise CanError(35, f"Velocity percent out of range: {vel}")

    if accel > 100.0 or accel <= 0.0:
      raise CanError(40, f"Accel percent out of range: {accel}")

    # Travel limits (with 0.1 tolerance snap)
    if position > self.max_travel_ax[axis] and not self.unlimited_travel_ax[axis]:
      if (position - self.max_travel_ax[axis]) < 0.1:
        position = self.max_travel_ax[axis]
      else:
        msg = f"Axis {axis}, max. limit = {self.max_travel_ax[axis]}, min. limit = {self.min_travel_ax[axis]}"
        raise CanError(45, msg)

    if position < self.min_travel_ax[axis] and not self.unlimited_travel_ax[axis]:
      if (self.min_travel_ax[axis] - position) < 0.1:
        position = self.min_travel_ax[axis]
      else:
        msg = f"Axis {axis}, max. limit = {self.max_travel_ax[axis]}, min. limit = {self.min_travel_ax[axis]}"
        raise CanError(45, msg)

    # Convert requested vel/accel percentages into axis-native units, then into motor units
    v_abs = (vel / 100.0) * self.max_vel_ax[axis]
    a_abs = (accel / 100.0) * self.max_accel_ax[axis]

    conv = self.motor_conversion_factor_ax[axis]
    a1 = float(v_abs * abs(conv))  # motor velocity units
    a2 = float(a_abs * abs(conv))  # motor accel units

    # Clearance interlocks (Z vs Elbow)
    if axis == self.z_ax:
      elbow_pos = await (self.motor_get_current_position(self.elbow_ax))
      if elbow_pos < self.base_to_gripper_clearance_arm and position < (self.min_travel_ax[self.z_ax] + self.base_to_gripper_clearance_z):
        raise CanError(110, "Base-to-gripper clearance violated (Z move)")

    if axis == self.elbow_ax:
      z_pos = await (self.motor_get_current_position(self.z_ax))
      if z_pos < (self.min_travel_ax[self.z_ax] + self.base_to_gripper_clearance_z) and position < self.base_to_gripper_clearance_arm:
        raise CanError(110, "Base-to-gripper clearance violated (elbow move)")

    # Current position of this axis
    current_pos = await self.motor_get_current_position(axis)

    # Elbow position special handling: convert to angle before wrapping/travel calc
    if axis == self.elbow_ax:
      position = (self.convert_elbow_position_to_angle(position))
      current_pos = (self.convert_elbow_position_to_angle(current_pos))

    # If unlimited travel and direction not "Normal", wrap commanded position into [min, max)
    if self.unlimited_travel_ax[axis] and self.g_joint_move_direction[axis] != eJointMoveDirection.Normal:
      span = self.max_travel_ax[axis] - self.min_travel_ax[axis]
      k = int(position / span)
      position -= k * span
      if position < self.min_travel_ax[axis]:
        position += span
      if position == self.max_travel_ax[axis]:
        position -= span

    # Travel/time estimation with trapezoid/triangle profile
    v_abs2 = (vel / 100.0) * self.max_vel_ax[axis]
    a_abs2 = (accel / 100.0) * self.max_accel_ax[axis]
    x = v_abs2 / a_abs2
    dist_accel = 0.5 * a_abs2 * (x ** 2)

    if self.unlimited_travel_ax[axis]:
      delta = position - current_pos
      span = self.max_travel_ax[axis] - self.min_travel_ax[axis]
      d = self.g_joint_move_direction[axis]
      if d == eJointMoveDirection.Clockwise:
        if delta > 0.0:
          delta -= span
      elif d == eJointMoveDirection.Counterclockwise:
        if delta < 0.0:
          delta += span
      elif d == eJointMoveDirection.ShortestWay:
        if delta > 180.0:
          delta -= span
        if delta < -180.0:
          delta += span
      travel = abs(delta)
    else:
      travel = abs(position - current_pos)

    if 2.0 * dist_accel > travel:
      t = math.sqrt(2.0 * (travel / 2.0) / a_abs2)
      move_time_s = 2.0 * t
    else:
      move_time_s = (travel - 2.0 * dist_accel) / v_abs2 + 2.0 * x

    calc_ms = int(round(move_time_s * 1000.0))
    if calculated_move_time_msec is not None:
      if len(calculated_move_time_msec) == 0:
        calculated_move_time_msec.append(calc_ms)
      else:
        calculated_move_time_msec[0] = calc_ms

    # Motor target in motor units
    motor_target = position * conv

    # Encoder "next position" uses wrapped position when unlimited
    if self.unlimited_travel_ax[axis]:
      span = self.max_travel_ax[axis] - self.min_travel_ax[axis]
      k = int(position / span)
      enc_pos = position - k * span
      if enc_pos < self.min_travel_ax[axis]:
        enc_pos += span
      if enc_pos == self.max_travel_ax[axis]:
        enc_pos -= span
    else:
      enc_pos = position

    self.can.next_encoder_position[axis] = int(round(enc_pos * conv))

    self.last_move_time = int(calc_ms)
    # self.move_done[axis] = False

    await self.can.motor_move_execute(
      node_id=int(axis),
      position=int(round(motor_target)),
      velocity=int(round(a1)),
      acceleration=int(round(a2)),
      relative=False,
      timeout=int(calc_ms),
      joint_move_direction=self.g_joint_move_direction[axis],
    )

    # Counters
  #   if travel > 0.01:
  #     await (self.move_count_increment())
  #     await (self.move_distance_count_increment(axis, travel))

    return calc_ms
  