import {
	PortHandler,
	PacketHandler,
	COMM_SUCCESS,
	COMM_RX_TIMEOUT,
	COMM_RX_CORRUPT,
	COMM_TX_FAIL,
	COMM_NOT_AVAILABLE,
	SCS_LOBYTE,
	SCS_HIBYTE,
	SCS_MAKEWORD,
	GroupSyncRead, // Import GroupSyncRead
	GroupSyncWrite // Import GroupSyncWrite
} from "./lowLevelSDK.mjs";

// Import address constants from the correct file
import {
	ADDR_SCS_PRESENT_POSITION,
	ADDR_SCS_GOAL_POSITION,
	ADDR_SCS_TORQUE_ENABLE,
	ADDR_SCS_GOAL_ACC,
	ADDR_SCS_GOAL_SPEED
} from "./scsservo_constants.mjs";

// Define constants not present in scsservo_constants.mjs
const ADDR_SCS_MODE = 33;
const ADDR_SCS_LOCK = 55;
const ADDR_SCS_ID = 5; // Address for Servo ID
const ADDR_SCS_BAUD_RATE = 6; // Address for Baud Rate

// 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.");
		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); // Attempt cleanup
			portHandler = null;
			throw new Error(`Failed to open port at baudrate ${baudRate}.`);
		}

		packetHandler = new PacketHandler(protocolEnd);
		console.log(`Connected to serial port at ${baudRate} baud, protocol end: ${protocolEnd}.`);
		return true;
	} catch (err) {
		console.error("Error during connection:", err);
		if (portHandler) {
			try {
				await portHandler.closePort();
			} catch (closeErr) {
				console.error("Error closing port after connection failure:", closeErr);
			}
		}
		portHandler = null;
		packetHandler = null;
		// Re-throw the original or a new error
		throw new Error(`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.");
		return true;
	}

	try {
		await portHandler.closePort();
		portHandler = null;
		packetHandler = null;
		console.log("Disconnected from serial port.");
		return true;
	} catch (err) {
		console.error("Error during disconnection:", err);
		// Attempt to nullify handlers even if close fails
		portHandler = null;
		packetHandler = null;
		throw new Error(`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. 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; // Ensure it's within 16 bits
	} 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 baud rate index of a servo.
 * @param {number} servoId - The ID of the servo (1-252).
 * @returns {Promise<number>} Resolves with the baud rate index (0-7).
 * @throws {Error} If not connected, read fails, or an exception occurs.
 */
export async function readBaudRate(servoId) {
	checkConnection();
	try {
		const [baudIndex, result, error] = await packetHandler.read1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_BAUD_RATE
		);

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

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

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

/**
 * Writes a target position to a servo.
 * @param {number} servoId - The ID of the servo (1-252).
 * @param {number} position - The target position value (0-4095).
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, position is out of range, write fails, or an exception occurs.
 */
export async function writePosition(servoId, position) {
	checkConnection();
	try {
		// Validate position range
		if (position < 0 || position > 4095) {
			throw new Error(
				`Invalid position value ${position} for servo ${servoId}. Must be between 0 and 4095.`
			);
		}
		const targetPosition = Math.round(position); // Ensure integer value

		const [result, error] = await packetHandler.write2ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_GOAL_POSITION,
			targetPosition
		);

		if (result !== COMM_SUCCESS) {
			throw new Error(
				`Error writing position to servo ${servoId}: ${packetHandler.getTxRxResult(
					result
				)}, Error code: ${error}`
			);
		}
		return "success";
	} catch (err) {
		console.error(`Exception writing position to servo ${servoId}:`, err);
		// Re-throw the original error or a new one wrapping it
		throw new Error(`Failed to write position to servo ${servoId}: ${err.message}`);
	}
}

/**
 * Enables or disables the torque of a servo.
 * @param {number} servoId - The ID of the servo (1-252).
 * @param {boolean} enable - True to enable torque, false to disable.
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, write fails, or an exception occurs.
 */
export async function writeTorqueEnable(servoId, enable) {
	checkConnection();
	try {
		const enableValue = enable ? 1 : 0;
		const [result, error] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_TORQUE_ENABLE,
			enableValue
		);

		if (result !== COMM_SUCCESS) {
			throw new Error(
				`Error setting torque for servo ${servoId}: ${packetHandler.getTxRxResult(
					result
				)}, Error code: ${error}`
			);
		}
		return "success";
	} catch (err) {
		console.error(`Exception setting torque for servo ${servoId}:`, err);
		throw new Error(`Exception setting torque for servo ${servoId}: ${err.message}`);
	}
}

/**
 * Sets the acceleration profile for a servo's movement.
 * @param {number} servoId - The ID of the servo (1-252).
 * @param {number} acceleration - The acceleration value (0-254).
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, write fails, or an exception occurs.
 */
export async function writeAcceleration(servoId, acceleration) {
	checkConnection();
	try {
		const clampedAcceleration = Math.max(0, Math.min(254, Math.round(acceleration)));
		const [result, error] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_GOAL_ACC,
			clampedAcceleration
		);

		if (result !== COMM_SUCCESS) {
			throw new Error(
				`Error writing acceleration to servo ${servoId}: ${packetHandler.getTxRxResult(
					result
				)}, Error code: ${error}`
			);
		}
		return "success";
	} catch (err) {
		console.error(`Exception writing acceleration to servo ${servoId}:`, err);
		throw new Error(`Exception writing acceleration to servo ${servoId}: ${err.message}`);
	}
}

/**
 * Helper to attempt locking a servo, logging errors without throwing.
 * @param {number} servoId
 */
async function tryLockServo(servoId) {
	try {
		await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
	} catch (lockErr) {
		console.error(`Failed to re-lock servo ${servoId}:`, lockErr);
	}
}

/**
 * Sets a servo to wheel mode (continuous rotation).
 * Requires unlocking, setting mode, and locking the configuration.
 * @param {number} servoId - The ID of the servo (1-252).
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, any step fails, or an exception occurs.
 */
export async function setWheelMode(servoId) {
	checkConnection();
	let unlocked = false;
	try {
		console.log(`Setting servo ${servoId} to wheel mode...`);

		// 1. Unlock servo configuration
		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_LOCK,
			0
		);
		if (resUnlock !== COMM_SUCCESS) {
			throw new Error(
				`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
					resUnlock
				)}, Error: ${errUnlock}`
			);
		}
		unlocked = true;

		// 2. Set mode to 1 (Wheel/Speed mode)
		const [resMode, errMode] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_MODE,
			1
		);
		if (resMode !== COMM_SUCCESS) {
			throw new Error(
				`Failed to set wheel mode for servo ${servoId}: ${packetHandler.getTxRxResult(
					resMode
				)}, Error: ${errMode}`
			);
		}

		// 3. Lock servo configuration
		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_LOCK,
			1
		);
		if (resLock !== COMM_SUCCESS) {
			// Mode was set, but lock failed. Still an error state.
			throw new Error(
				`Failed to lock servo ${servoId} after setting mode: ${packetHandler.getTxRxResult(
					resLock
				)}, Error: ${errLock}`
			);
		}
		unlocked = false; // Successfully locked

		console.log(`Successfully set servo ${servoId} to wheel mode.`);
		return "success";
	} catch (err) {
		console.error(`Exception setting wheel mode for servo ${servoId}:`, err);
		if (unlocked) {
			// Attempt to re-lock if an error occurred after unlocking
			await tryLockServo(servoId);
		}
		// Re-throw the original error or a new one wrapping it
		throw new Error(`Failed to set wheel mode for servo ${servoId}: ${err.message}`);
	}
}

/**
 * Sets a servo back to position control mode from wheel mode.
 * Requires unlocking, setting mode, and locking the configuration.
 * @param {number} servoId - The ID of the servo (1-252).
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, any step fails, or an exception occurs.
 */
export async function setPositionMode(servoId) {
	checkConnection();
	let unlocked = false;
	try {
		console.log(`Setting servo ${servoId} back to position mode...`);

		// 1. Unlock servo configuration
		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_LOCK,
			0
		);
		if (resUnlock !== COMM_SUCCESS) {
			throw new Error(
				`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
					resUnlock
				)}, Error: ${errUnlock}`
			);
		}
		unlocked = true;

		// 2. Set mode to 0 (Position/Servo mode)
		const [resMode, errMode] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_MODE,
			0 // 0 for position mode
		);
		if (resMode !== COMM_SUCCESS) {
			throw new Error(
				`Failed to set position mode for servo ${servoId}: ${packetHandler.getTxRxResult(
					resMode
				)}, Error: ${errMode}`
			);
		}

		// 3. Lock servo configuration
		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_LOCK,
			1
		);
		if (resLock !== COMM_SUCCESS) {
			throw new Error(
				`Failed to lock servo ${servoId} after setting mode: ${packetHandler.getTxRxResult(
					resLock
				)}, Error: ${errLock}`
			);
		}
		unlocked = false; // Successfully locked

		console.log(`Successfully set servo ${servoId} back to position mode.`);
		return "success";
	} catch (err) {
		console.error(`Exception setting position mode for servo ${servoId}:`, err);
		if (unlocked) {
			// Attempt to re-lock if an error occurred after unlocking
			await tryLockServo(servoId);
		}
		throw new Error(`Failed to set position mode for servo ${servoId}: ${err.message}`);
	}
}

/**
 * Writes a target speed for a servo in wheel mode.
 * @param {number} servoId - The ID of the servo
 * @param {number} speed - The target speed value (-10000 to 10000). Negative values indicate reverse direction. 0 stops the wheel.
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, either write fails, or an exception occurs.
 */
export async function writeWheelSpeed(servoId, speed) {
	checkConnection();
	try {
		// Validate and clamp the speed to the new range
		const clampedSpeed = Math.max(-10000, Math.min(10000, Math.round(speed)));
		let speedValue = Math.abs(clampedSpeed) & 0x7fff; // Get absolute value, ensure within 15 bits

		// Set the direction bit (MSB of the 16-bit value) if speed is negative
		if (clampedSpeed < 0) {
			speedValue |= 0x8000; // Set the 16th bit for reverse direction
		}

		// Use write2ByteTxRx to write the 16-bit speed value
		const [result, error] = await packetHandler.write2ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_GOAL_SPEED, // Starting address for the 2-byte speed value
			speedValue
		);

		if (result !== COMM_SUCCESS) {
			throw new Error(
				`Error writing wheel speed to servo ${servoId}: ${packetHandler.getTxRxResult(
					result
				)}, Error: ${error}`
			);
		}

		return "success";
	} catch (err) {
		console.error(`Exception writing wheel speed to servo ${servoId}:`, err);
		throw new Error(`Exception writing wheel speed to servo ${servoId}: ${err.message}`);
	}
}

/**
 * Writes target speeds to multiple servos in wheel mode synchronously.
 * @param {Map<number, number> | object} servoSpeeds - A Map or object where keys are servo IDs (1-252) and values are target speeds (-10000 to 10000).
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, any speed is out of range, transmission fails, or an exception occurs.
 */
export async function syncWriteWheelSpeed(servoSpeeds) {
	checkConnection();

	const groupSyncWrite = new GroupSyncWrite(
		portHandler,
		packetHandler,
		ADDR_SCS_GOAL_SPEED,
		2 // Data length for speed (2 bytes)
	);
	let paramAdded = false;

	const entries = servoSpeeds instanceof Map ? servoSpeeds.entries() : Object.entries(servoSpeeds);

	// Second pass: Add valid parameters
	for (const [idStr, speed] of entries) {
		const servoId = parseInt(idStr, 10); // Already validated

		if (isNaN(servoId) || servoId < 1 || servoId > 252) {
			throw new Error(`Invalid servo ID "${idStr}" in syncWriteWheelSpeed.`);
		}
		if (speed < -10000 || speed > 10000) {
			throw new Error(
				`Invalid speed value ${speed} for servo ${servoId} in syncWriteWheelSpeed. Must be between -10000 and 10000.`
			);
		}

		const clampedSpeed = Math.max(-10000, Math.min(10000, Math.round(speed))); // Ensure integer, already validated range
		let speedValue = Math.abs(clampedSpeed) & 0x7fff; // Get absolute value, ensure within 15 bits

		// Set the direction bit (MSB of the 16-bit value) if speed is negative
		if (clampedSpeed < 0) {
			speedValue |= 0x8000; // Set the 16th bit for reverse direction
		}

		const data = [SCS_LOBYTE(speedValue), SCS_HIBYTE(speedValue)];

		if (groupSyncWrite.addParam(servoId, data)) {
			paramAdded = true;
		} else {
			// This should ideally not happen if IDs are unique, but handle defensively
			console.warn(
				`Failed to add servo ${servoId} to sync write speed group (possibly duplicate).`
			);
		}
	}

	if (!paramAdded) {
		console.log("Sync Write Speed: No valid servo speeds provided or added.");
		return "success"; // Nothing to write is considered success
	}

	try {
		// Send the Sync Write instruction
		const result = await groupSyncWrite.txPacket();
		if (result !== COMM_SUCCESS) {
			throw new Error(`Sync Write Speed txPacket failed: ${packetHandler.getTxRxResult(result)}`);
		}
		return "success";
	} catch (err) {
		console.error("Exception during syncWriteWheelSpeed:", err);
		// Re-throw the original error or a new one wrapping it
		throw new Error(`Sync Write Speed failed: ${err.message}`);
	}
}

/**
 * Reads the current position of multiple servos synchronously.
 * @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, transmission fails, reception fails, or data for any requested servo is unavailable.
 */
export async function syncReadPositions(servoIds) {
	checkConnection();
	if (!Array.isArray(servoIds) || servoIds.length === 0) {
		console.log("Sync Read: No servo IDs provided.");
		return new Map(); // Return empty map for empty input
	}

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

	// 1. 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(); // Return empty map if no valid IDs
	}

	try {
		// 2. 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)}`);
		}

		// 3. Receive the response packets
		let rxResult = await groupSyncRead.rxPacket();
		// Even if rxPacket reports an overall issue (like timeout), we still check individual servos.
		// A specific error will be thrown later if any servo data is missing.
		if (rxResult !== COMM_SUCCESS) {
			console.warn(
				`Sync Read rxPacket overall result: ${packetHandler.getTxRxResult(
					rxResult
				)}. Checking individual servos.`
			);
		}

		// 4. 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);
				positions.set(id, position & 0xffff);
			} else {
				failedIds.push(id);
			}
		});

		// 5. Check if all requested servos responded
		if (failedIds.length > 0) {
			throw new Error(
				`Sync Read failed: Data not available for servo IDs: ${failedIds.join(
					", "
				)}. Overall RX result: ${packetHandler.getTxRxResult(rxResult)}`
			);
		}

		return positions;
	} catch (err) {
		console.error("Exception or failure during syncReadPositions:", err);
		// Re-throw the caught error or a new one wrapping it
		throw new Error(`Sync Read failed: ${err.message}`);
	}
}

/**
 * Writes target positions to multiple servos synchronously.
 * @param {Map<number, number> | object} servoPositions - A Map or object where keys are servo IDs (1-252) and values are target positions (0-4095).
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, any position is out of range, transmission fails, or an exception occurs.
 */
export async function syncWritePositions(servoPositions) {
	checkConnection();

	const groupSyncWrite = new GroupSyncWrite(
		portHandler,
		packetHandler,
		ADDR_SCS_GOAL_POSITION,
		2 // Data length for position
	);
	let paramAdded = false;

	const entries =
		servoPositions instanceof Map ? servoPositions.entries() : Object.entries(servoPositions);

	// Second pass: Add valid parameters
	for (const [idStr, position] of entries) {
		const servoId = parseInt(idStr, 10); // Already validated
		if (isNaN(servoId) || servoId < 1 || servoId > 252) {
			throw new Error(`Invalid servo ID "${idStr}" in syncWritePositions.`);
		}
		if (position < 0 || position > 4095) {
			throw new Error(
				`Invalid position value ${position} for servo ${servoId} in syncWritePositions. Must be between 0 and 4095.`
			);
		}
		const targetPosition = Math.round(position); // Ensure integer, already validated range
		const data = [SCS_LOBYTE(targetPosition), SCS_HIBYTE(targetPosition)];

		if (groupSyncWrite.addParam(servoId, data)) {
			paramAdded = true;
		} else {
			// This should ideally not happen if IDs are unique, but handle defensively
			console.warn(`Failed to add servo ${servoId} to sync write group (possibly duplicate).`);
		}
	}

	if (!paramAdded) {
		console.log("Sync Write: No valid servo positions provided or added.");
		return "success"; // Nothing to write is considered success
	}

	try {
		// Send the Sync Write instruction
		const result = await groupSyncWrite.txPacket();
		if (result !== COMM_SUCCESS) {
			throw new Error(`Sync Write txPacket failed: ${packetHandler.getTxRxResult(result)}`);
		}
		return "success";
	} catch (err) {
		console.error("Exception during syncWritePositions:", err);
		// Re-throw the original error or a new one wrapping it
		throw new Error(`Sync Write failed: ${err.message}`);
	}
}

/**
 * Sets the Baud Rate of a servo.
 * NOTE: After changing the baud rate, you might need to disconnect and reconnect
 *       at the new baud rate to communicate with the servo further.
 * @param {number} servoId - The current ID of the servo to configure (1-252).
 * @param {number} baudRateIndex - The index representing the new baud rate (0-7).
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, input is invalid, any step fails, or an exception occurs.
 */
export async function setBaudRate(servoId, baudRateIndex) {
	checkConnection();

	// Validate inputs
	if (servoId < 1 || servoId > 252) {
		throw new Error(`Invalid servo ID provided: ${servoId}. Must be between 1 and 252.`);
	}
	if (baudRateIndex < 0 || baudRateIndex > 7) {
		throw new Error(`Invalid baudRateIndex: ${baudRateIndex}. Must be between 0 and 7.`);
	}

	let unlocked = false;
	try {
		console.log(`Setting baud rate for servo ${servoId}: Index=${baudRateIndex}`);

		// 1. Unlock servo configuration
		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_LOCK,
			0 // 0 to unlock
		);
		if (resUnlock !== COMM_SUCCESS) {
			throw new Error(
				`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
					resUnlock
				)}, Error: ${errUnlock}`
			);
		}
		unlocked = true;

		// 2. Write new Baud Rate index
		const [resBaud, errBaud] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_BAUD_RATE,
			baudRateIndex
		);
		if (resBaud !== COMM_SUCCESS) {
			throw new Error(
				`Failed to write baud rate index ${baudRateIndex} to servo ${servoId}: ${packetHandler.getTxRxResult(
					resBaud
				)}, Error: ${errBaud}`
			);
		}

		// 3. Lock servo configuration
		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
			portHandler,
			servoId,
			ADDR_SCS_LOCK,
			1
		);
		if (resLock !== COMM_SUCCESS) {
			throw new Error(
				`Failed to lock servo ${servoId} after setting baud rate: ${packetHandler.getTxRxResult(
					resLock
				)}, Error: ${errLock}.`
			);
		}
		unlocked = false; // Successfully locked

		console.log(
			`Successfully set baud rate for servo ${servoId}. Index: ${baudRateIndex}. Remember to potentially reconnect with the new baud rate.`
		);
		return "success";
	} catch (err) {
		console.error(`Exception during setBaudRate for servo ID ${servoId}:`, err);
		if (unlocked) {
			await tryLockServo(servoId);
		}
		throw new Error(`Failed to set baud rate for servo ${servoId}: ${err.message}`);
	}
}

/**
 * Sets the ID of a servo.
 * NOTE: Changing the ID requires using the new ID for subsequent commands.
 * @param {number} currentServoId - The current ID of the servo to configure (1-252).
 * @param {number} newServoId - The new ID to set for the servo (1-252).
 * @returns {Promise<"success">} Resolves with "success".
 * @throws {Error} If not connected, input is invalid, any step fails, or an exception occurs.
 */
export async function setServoId(currentServoId, newServoId) {
	checkConnection();

	// Validate inputs
	if (currentServoId < 1 || currentServoId > 252 || newServoId < 1 || newServoId > 252) {
		throw new Error(
			`Invalid servo ID provided. Current: ${currentServoId}, New: ${newServoId}. Must be between 1 and 252.`
		);
	}

	if (currentServoId === newServoId) {
		console.log(`Servo ID is already ${newServoId}. No change needed.`);
		return "success";
	}

	let unlocked = false;
	let idWritten = false;
	try {
		console.log(`Setting servo ID: From ${currentServoId} to ${newServoId}`);

		// 1. Unlock servo configuration (using current ID)
		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
			portHandler,
			currentServoId,
			ADDR_SCS_LOCK,
			0 // 0 to unlock
		);
		if (resUnlock !== COMM_SUCCESS) {
			throw new Error(
				`Failed to unlock servo ${currentServoId}: ${packetHandler.getTxRxResult(
					resUnlock
				)}, Error: ${errUnlock}`
			);
		}
		unlocked = true;

		// 2. Write new Servo ID (using current ID)
		const [resId, errId] = await packetHandler.write1ByteTxRx(
			portHandler,
			currentServoId,
			ADDR_SCS_ID,
			newServoId
		);
		if (resId !== COMM_SUCCESS) {
			throw new Error(
				`Failed to write new ID ${newServoId} to servo ${currentServoId}: ${packetHandler.getTxRxResult(
					resId
				)}, Error: ${errId}`
			);
		}
		idWritten = true;

		// 3. Lock servo configuration (using NEW ID)
		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
			portHandler,
			newServoId, // Use NEW ID here
			ADDR_SCS_LOCK,
			1 // 1 to lock
		);
		if (resLock !== COMM_SUCCESS) {
			// ID was likely changed, but lock failed. Critical state.
			throw new Error(
				`Failed to lock servo with new ID ${newServoId}: ${packetHandler.getTxRxResult(
					resLock
				)}, Error: ${errLock}. Configuration might be incomplete.`
			);
		}
		unlocked = false; // Successfully locked with new ID

		console.log(
			`Successfully set servo ID from ${currentServoId} to ${newServoId}. Remember to use the new ID for future commands.`
		);
		return "success";
	} catch (err) {
		console.error(`Exception during setServoId for current ID ${currentServoId}:`, err);
		if (unlocked) {
			// If unlock succeeded but subsequent steps failed, attempt to re-lock.
			// If ID write failed, use current ID. If ID write succeeded but lock failed, use new ID.
			const idToLock = idWritten ? newServoId : currentServoId;
			console.warn(`Attempting to re-lock servo using ID ${idToLock}...`);
			await tryLockServo(idToLock);
		}
		throw new Error(
			`Failed to set servo ID from ${currentServoId} to ${newServoId}: ${err.message}`
		);
	}
}