
  async def binary_interpreter(
    self,
    node_id: int,
    cmd: str,
    cmd_index: int,
    cmd_type: eCmdType,
    value_ref: str,  # Simulates ref string
    val_type: eValType = eValType.Int,
    response_time: int = 0,  # Simulates ref long
    low_priority: bool = False,
  ) -> int:
    """
    Interprets and sends binary commands to the motor.

    This is a critical and complex method that involves:
    1. Formatting command and data into bytes.
    2. Sending bytes via CAN_Write.
    3. Waiting for a response (polling `query_wait_buffer`).
    4. Parsing the response based on CmdType and ValType.
    """

    if self.grp_node_id is None:  # C# `if (this.GrpNodeID == null)`
      return 0  # Or handle appropriately

    # C# dynamic array `numArray1` (int[8]) used for msg bytes
    # C# `flag1` is for ValQuery, `flag2` for Float types, `Execute` for Execute CmdType
    is_val_query = cmd_type == eCmdType.ValQuery
    is_float_type = val_type == eValType.Float
    is_execute_cmd = cmd_type == eCmdType.Execute

    query_idx = 0
    group_query_indices: List[int] = []  # For NodeID 10 (broadcast group query)

    if node_id != 10:  # C# MAGIC NUMBER for Group broadcast (0x0A)
      query_idx = self.get_next_query_wait_buffer_index(node_id)
    else:
      if self.grp_node_id:
        for grp_node in self.grp_node_id:
          group_query_indices.append(self.get_next_query_wait_buffer_index(grp_node))

    for retry in range(5):
      if not value_ref:  # C# `Operators.CompareString(Value, "", false) == 0`
        value_ref = "0"

      # Prepare QueryWaitBuffer entry
      if node_id != 10:
        node_idx = self.get_node_id_index(node_id)
        self.query_wait_buffer[node_idx][query_idx].node_id = node_id
        self.query_wait_buffer[node_idx][query_idx].msg_index = cmd_index
        self.query_wait_buffer[node_idx][query_idx].msg_type = cmd
        # self.query_wait_buffer[node_idx][query_idx].response = ""
      else:
        if self.grp_node_id:
          for i, grp_node in enumerate(self.grp_node_id):
            node_idx = self.get_node_id_index(grp_node)
            current_query_idx = group_query_indices[i]
            self.query_wait_buffer[node_idx][current_query_idx].node_id = grp_node
            self.query_wait_buffer[node_idx][current_query_idx].msg_index = cmd_index
            self.query_wait_buffer[node_idx][current_query_idx].msg_type = cmd
            # self.query_wait_buffer[node_idx][current_query_idx].response = ""

      # Construct CAN message data bytes (8 bytes total for RPDO2)
      # C# `numArray1` (int[8]) used for msg bytes
      can_data: List[int] = [0] * 8
      can_data[0] = ord(cmd[:1])
      can_data[1] = ord(vb_right(cmd, 1))

      binary_cmd_index = int_to_binary(cmd_index, 14)  # C# fixed length 14
      can_data[2] = binary_to_int(vb_right(binary_cmd_index, 8))  # Lower 8 bits

      # Upper 6 bits of cmd_index, plus flags for query (bit 6) and float (bit 7)
      upper_6_bits_cmd_idx = binary_to_int(binary_cmd_index[:6])

      # C# uses Math.Pow(2.0, checked(2 * Math.Abs(-(flag1 ? 1 : 0))), 6.0)))
      # If flag1 is true (is_val_query), then 2 * 1 = 2. pow(2,6) -> bit 6 is set
      # This is bit 6 for query (0x40) and bit 7 for float (0x80)
      if is_val_query:  # Bit 6 (0x40)
        can_data[3] |= 0x40
      if is_float_type:  # Bit 7 (0x80)
        can_data[3] |= 0x80

      can_data[3] |= upper_6_bits_cmd_idx  # Merge with upper 6 bits of cmd_index (0-5)

      if is_float_type:
        # C# use BitConverter.GetBytes(Conversions.ToSingle(Value))
        # Python's struct.pack for float (little-endian)
        packed_float = struct.pack("<f", float(value_ref))
        can_data[4] = packed_float[0]
        can_data[5] = packed_float[1]
        can_data[6] = packed_float[2]
        can_data[7] = packed_float[3]
      else:
        # C# GetByte(Conversions.ToLong(Value), byte_num)
        val_long = int(value_ref)
        can_data[4] = (val_long >> 0) & 0xFF
        can_data[5] = (val_long >> 8) & 0xFF
        can_data[6] = (val_long >> 16) & 0xFF
        can_data[7] = (val_long >> 24) & 0xFF

      # Wait for any ongoing high-priority writes if low_priority is true
      if low_priority:
        start_hp_wait = time.monotonic()
        while (
          self.can_msg_buffer_first_index != self.can_msg_buffer_last_index
          and (time.monotonic() - start_hp_wait) < 10.0
        ):  # C# 10000ms
          await asyncio.sleep(0.001)

      tx_start_time = time.monotonic()
      err_code = await self.can_write(
        eCOBType.RPDO2,
        node_id,
        byte0=can_data[0],
        byte1=can_data[1],
        byte2=can_data[2],
        byte3=can_data[3],
        byte4=can_data[4],
        byte5=can_data[5],
        byte6=can_data[6],
        byte7=can_data[7],
        execute=is_execute_cmd,
        low_priority=low_priority,
      )
      if err_code != 0:
        self.sending_sv_command = False
        return err_code

      timeout_ms = 1000  # Default C# timeout
      if cmd == "SV":
        timeout_ms = 10000  # Specific timeout for SV command in C#

      timeout_sec = timeout_ms / 1000.0

      # Wait for response (different logic for single node vs. group)
      if node_id != 10:
        node_idx = self.get_node_id_index(node_id)
        response_start_time = time.monotonic()
        while (time.monotonic() - response_start_time) < timeout_sec:
          if self.query_wait_buffer[node_idx][query_idx].response != "":
            break
          await asyncio.sleep(0.001)

        response_time = int((time.monotonic() - tx_start_time) * 1000)  # milliseconds

        if not is_val_query and not is_execute_cmd:  # Value Set command, check if response matches
          is_match = False
          if (
            is_float_type
            and vb_isnumeric(value_ref)
            and vb_isnumeric(self.query_wait_buffer[node_idx][query_idx].response)
          ):
            val1 = float(value_ref)
            val2 = float(self.query_wait_buffer[node_idx][query_idx].response)
            if val1 == val2 or (val1 / val2 > 0.99 and val1 / val2 < 1.01):
              is_match = True
          elif self.query_wait_buffer[node_idx][query_idx].response.upper() == value_ref.upper():
            is_match = True

          if not is_match:
            if retry == 4:  # Last retry
              self.error_code[
                4
              ] = f"Unexpected CAN response: Attempted to send {cmd}[{cmd_index}]={value_ref}, but received response = {self.query_wait_buffer[node_idx][query_idx].response}"
              return 4
            continue  # Retry
        elif (
          self.query_wait_buffer[node_idx][query_idx].response == ""
        ):  # ValQuery or Execute, but no response
          if retry == 4:
            return 45  # Failed to receive response from motor
          continue  # Retry
        else:  # Success for ValQuery/Execute or value matched for ValSet
          break  # Exit retry loop

      else:  # Group query (NodeID == 10)
        # Need to wait for all nodes in the group to respond for *ValQuery* or *Execute*.
        # For ValSet, we need all nodes to confirm the value.
        all_responded = False
        response_start_time = time.monotonic()
        while (time.monotonic() - response_start_time) < timeout_sec:
          all_responded = True
          if self.grp_node_id:
            for i, grp_node in enumerate(self.grp_node_id):
              node_idx = self.get_node_id_index(grp_node)
              current_query_idx = group_query_indices[i]
              if self.query_wait_buffer[node_idx][current_query_idx].response == "":
                all_responded = False
                break
          if all_responded:
            break
          await asyncio.sleep(0.001)

        response_time = int((time.monotonic() - tx_start_time) * 1000)

        if not is_val_query and not is_execute_cmd:  # Value Set for group
          group_match = True
          if self.grp_node_id:
            for i, grp_node in enumerate(self.grp_node_id):
              node_idx = self.get_node_id_index(grp_node)
              current_query_idx = group_query_indices[i]
              is_match_node = False
              if (
                is_float_type
                and vb_isnumeric(value_ref)
                and vb_isnumeric(self.query_wait_buffer[node_idx][current_query_idx].response)
              ):
                val1 = float(value_ref)
                val2 = float(self.query_wait_buffer[node_idx][current_query_idx].response)
                if val1 == val2 or (val1 / val2 > 0.99 and val1 / val2 < 1.01):
                  is_match_node = True
              elif (
                self.query_wait_buffer[node_idx][current_query_idx].response.upper()
                == value_ref.upper()
              ):
                is_match_node = True

              if not is_match_node:
                group_match = False
                break
          if not group_match:
            if retry == 4:
              self.error_code[
                4
              ] = f"Unexpected CAN response (group): Attempted to send {cmd}[{cmd_index}]={value_ref}, but responses did not match."
              return 4
            continue  # Retry
          else:
            break  # Success
        elif not all_responded:  # No response from all nodes in group for ValQuery/Execute
          if retry == 4:
            return 45
          continue
        else:
          break

    if is_val_query:
      if node_id != 10:
        node_idx = self.get_node_id_index(node_id)
        value_ref = self.query_wait_buffer[node_idx][query_idx].response
      else:
        collected_responses: List[str] = []
        if self.grp_node_id:
          for i, grp_node in enumerate(self.grp_node_id):
            node_idx = self.get_node_id_index(grp_node)
            current_query_idx = group_query_indices[i]
            collected_responses.append(self.query_wait_buffer[node_idx][current_query_idx].response)
        value_ref = ",".join(collected_responses)

    self.sending_sv_command = False
    if retry == 4:  # If loop finished by exhausting retries, it means an error occurred
      return 4  # C# returned 4 for "unexpected CAN response" (already set in self.error_code)

    return 0




async def _move_thread(self):
  print("Move Thread Started")
  self.b_move_thread_running = True
  tick_count = time.monotonic()  # High-resolution timer for time differences

  while self.b_move_thread_running:
    # check the move buffer
    if self.move_buffer_first_index != self.move_buffer_last_index:
      if (time.monotonic() - tick_count) >= 1.0:  # 1000ms
        tick_count = time.monotonic()

      if self.move_buffer_first_index == 99:
        self.move_buffer_first_index = 0
      else:
        self.move_buffer_first_index += 1

      current_move = self.move_buffer[self.move_buffer_first_index]

      if current_move.pending:
        current_move.pending = False  # Mark as processed
        self.move_error_code = 0
        self.emcy_move_error_received = False

        err_code = 0
        if current_move.move_type == eMoveType.MotorMove:
          param = self.motor_move_param_buffer[current_move.index]
          err_code = await self.motor_move_execute(
            param.node_id,
            param.position,
            param.velocity,
            param.acceleration,
            current_move.index,
            param.relative,
            current_move.timeout_msec,
            param.mtr_joint_move_direction,
            current_move.wait_until_done,
          )
        elif current_move.move_type == eMoveType.MotorsMoveAbsolute:
          param = self.motors_move_absolute_param_buffer[current_move.index]
          if (
            param.node_id
            and param.position
            and param.velocity
            and param.acceleration
            and param.mtrs_joint_move_direction
          ):
            err_code = await self.motors_move_absolute_execute(
              param.node_id,
              param.position,
              param.velocity,
              param.acceleration,
              current_move.index,
              param.mtrs_joint_move_direction,
              current_move.timeout_msec,
            )
        elif current_move.move_type == eMoveType.MotorsMovePath:
          param = self.motors_move_path_param_buffer[current_move.index]
          if param.pva_point and param.stop_deceleration and param.skip_ax:
            err_code = await self.motors_move_path_execute(
              param.pva_point,
              param.time_interval_msec,
              param.stop_deceleration,
              current_move.index,
              param.skip_ax,
              current_move.timeout_msec,
            )

        current_move.error_code = err_code

      if current_move.error_code > 0:
        event_data = sEventData(
          event_type=eEventType.MoveError,
          me_error_code=current_move.error_code,
          me_index=current_move.index,
        )
        self.move_buffer_first_index = self.move_buffer_last_index  # Clear buffer on error
        for i in range(100):
          self.move_buffer[i].pending = False
        await self._raise_an_event(event_data)

    await asyncio.sleep(0.001)
  print("Move Thread Stopped")
