src/lib/feetech.js/scsServoSDKUnlock.mjs

/**
 * Read-Only Servo SDK for USB Master
 * Simplified version that only reads positions without any locking
 */

import {
  PortHandler,
  PacketHandler,
  COMM_SUCCESS,
  GroupSyncRead,
} from "./lowLevelSDK.mjs";

import {
  ADDR_SCS_PRESENT_POSITION,
} from "./scsservo_constants.mjs";

// Module-level variables for handlers
let portHandler = null;
let packetHandler = null;

/**
 * Connects to the serial port and initializes handlers.
 * @param {object} [options] - Connection options.
 * @param {number} [options.baudRate=1000000] - The baud rate for the serial connection.
 * @param {number} [options.protocolEnd=0] - The protocol end setting (0 for STS/SMS, 1 for SCS).
 * @returns {Promise<true>} Resolves with true on successful connection.
 * @throws {Error} If connection fails or port cannot be opened/selected.
 */
export async function connect(options = {}) {
  if (portHandler && portHandler.isOpen) {
    console.log("🔓 Already connected to USB robot (read-only mode).");
    return true;
  }

  const { baudRate = 1000000, protocolEnd = 0 } = options;

  try {
    portHandler = new PortHandler();
    const portRequested = await portHandler.requestPort();
    if (!portRequested) {
      portHandler = null;
      throw new Error("Failed to select a serial port.");
    }

    portHandler.setBaudRate(baudRate);
    const portOpened = await portHandler.openPort();
    if (!portOpened) {
      await portHandler.closePort().catch(console.error);
      portHandler = null;
      throw new Error(`Failed to open port at baudrate ${baudRate}.`);
    }

    packetHandler = new PacketHandler(protocolEnd);
    console.log(
      `🔓 Connected to USB robot (read-only mode) at ${baudRate} baud, protocol end: ${protocolEnd}.`
    );
    return true;
  } catch (err) {
    console.error("Error during USB robot connection:", err);
    if (portHandler) {
      try {
        await portHandler.closePort();
      } catch (closeErr) {
        console.error("Error closing port after connection failure:", closeErr);
      }
    }
    portHandler = null;
    packetHandler = null;
    throw new Error(`USB robot connection failed: ${err.message}`);
  }
}

/**
 * Disconnects from the serial port.
 * @returns {Promise<true>} Resolves with true on successful disconnection.
 * @throws {Error} If disconnection fails.
 */
export async function disconnect() {
  if (!portHandler || !portHandler.isOpen) {
    console.log("Already disconnected from USB robot.");
    return true;
  }

  try {
    await portHandler.closePort();
    portHandler = null;
    packetHandler = null;
    console.log("🔓 Disconnected from USB robot (read-only mode).");
    return true;
  } catch (err) {
    console.error("Error during USB robot disconnection:", err);
    portHandler = null;
    packetHandler = null;
    throw new Error(`USB robot disconnection failed: ${err.message}`);
  }
}

/**
 * Checks if the SDK is connected. Throws an error if not.
 * @throws {Error} If not connected.
 */
function checkConnection() {
  if (!portHandler || !packetHandler) {
    throw new Error("Not connected to USB robot. Call connect() first.");
  }
}

/**
 * Reads the current position of a servo.
 * @param {number} servoId - The ID of the servo (1-252).
 * @returns {Promise<number>} Resolves with the position (0-4095).
 * @throws {Error} If not connected, read fails, or an exception occurs.
 */
export async function readPosition(servoId) {
  checkConnection();
  try {
    const [position, result, error] = await packetHandler.read2ByteTxRx(
      portHandler,
      servoId,
      ADDR_SCS_PRESENT_POSITION
    );

    if (result !== COMM_SUCCESS) {
      throw new Error(
        `Error reading position from servo ${servoId}: ${packetHandler.getTxRxResult(
          result
        )}, Error code: ${error}`
      );
    }
    return position & 0xffff;
  } catch (err) {
    console.error(`Exception reading position from servo ${servoId}:`, err);
    throw new Error(
      `Exception reading position from servo ${servoId}: ${err.message}`
    );
  }
}

/**
 * Reads the current position of multiple servos synchronously.
 * Returns positions for all servos that respond, skipping failed ones gracefully.
 * @param {number[]} servoIds - An array of servo IDs (1-252) to read from.
 * @returns {Promise<Map<number, number>>} Resolves with a Map where keys are servo IDs and values are positions (0-4095).
 * @throws {Error} If not connected or transmission fails completely.
 */
export async function syncReadPositions(servoIds) {
  checkConnection();
  if (!Array.isArray(servoIds) || servoIds.length === 0) {
    console.log("Sync Read: No servo IDs provided.");
    return new Map();
  }

  const startAddress = ADDR_SCS_PRESENT_POSITION;
  const dataLength = 2;
  const groupSyncRead = new GroupSyncRead(
    portHandler,
    packetHandler,
    startAddress,
    dataLength
  );
  const positions = new Map();
  const validIds = [];

  // Add parameters for each valid servo ID
  servoIds.forEach((id) => {
    if (id >= 1 && id <= 252) {
      if (groupSyncRead.addParam(id)) {
        validIds.push(id);
      } else {
        console.warn(
          `Sync Read: Failed to add param for servo ID ${id} (maybe duplicate or invalid).`
        );
      }
    } else {
      console.warn(`Sync Read: Invalid servo ID ${id} skipped.`);
    }
  });

  if (validIds.length === 0) {
    console.log("Sync Read: No valid servo IDs to read.");
    return new Map();
  }

  try {
    // Send the Sync Read instruction packet
    let txResult = await groupSyncRead.txPacket();
    if (txResult !== COMM_SUCCESS) {
      throw new Error(
        `Sync Read txPacket failed: ${packetHandler.getTxRxResult(txResult)}`
      );
    }

    // Receive the response packets
    let rxResult = await groupSyncRead.rxPacket();
    if (rxResult !== COMM_SUCCESS) {
      console.warn(
        `Sync Read rxPacket overall result: ${packetHandler.getTxRxResult(
          rxResult
        )}. Checking individual servos.`
      );
    }

    // Check data availability and retrieve data for each servo
    const failedIds = [];
    validIds.forEach((id) => {
      const isAvailable = groupSyncRead.isAvailable(
        id,
        startAddress,
        dataLength
      );
      if (isAvailable) {
        const position = groupSyncRead.getData(id, startAddress, dataLength);
        const finalPosition = position & 0xffff;
        console.log(`🔍 Debug Servo ${id}: raw=${position}, final=${finalPosition}, hex=${position.toString(16)}`);
        positions.set(id, finalPosition);
      } else {
        failedIds.push(id);
      }
    });

    // Log failed servos but don't throw error - return available data
    if (failedIds.length > 0) {
      console.warn(
        `Sync Read: Data not available for servo IDs: ${failedIds.join(
          ", "
        )}. Got ${positions.size}/${validIds.length} servos successfully.`
      );
    }

    return positions;
  } catch (err) {
    console.error("Exception during syncReadPositions:", err);
    throw new Error(`Sync Read failed: ${err.message}`);
  }
}