src/lib/elements/robot/drivers/USBConsumer.ts

import type { Consumer, RobotCommand } from "../models.js";
import { USBServoDriver } from "./USBServoDriver.js";
import { ROBOT_CONFIG } from "../config.js";

export class USBConsumer extends USBServoDriver implements Consumer {
	private commandCallbacks: ((command: RobotCommand) => void)[] = [];
	private pollingInterval: ReturnType<typeof setInterval> | null = null;
	private lastPositions: Record<number, number> = {};
	private errorCount = 0;

	constructor(config: any) {
		super(config, "Consumer");
	}

	async connect(): Promise<void> {
		// Connect to USB first (this triggers browser's device selection dialog)
		await this.connectToUSB();

		// Unlock servos for manual movement (consumer mode)
		await this.unlockAllServos();

		// Note: Calibration is checked when operations are actually needed
	}

	async disconnect(): Promise<void> {
		await this.stopListening();
		await this.disconnectFromUSB();
	}

	async startListening(): Promise<void> {
		if (!this._status.isConnected || this.pollingInterval !== null) {
			return;
		}

		if (!this.isCalibrated) {
			throw new Error("Cannot start listening: not calibrated");
		}

		console.log(`[${this.name}] Starting position listening...`);
		this.errorCount = 0;

		this.pollingInterval = setInterval(async () => {
			try {
				await this.pollAndBroadcastPositions();
				this.errorCount = 0;
			} catch (error) {
				this.errorCount++;
				console.warn(`[${this.name}] Polling error (${this.errorCount}):`, error);

				if (this.errorCount >= ROBOT_CONFIG.polling.maxPollingErrors) {
					console.warn(`[${this.name}] Too many polling errors, slowing down...`);
					await this.stopListening();
					setTimeout(() => this.startListening(), ROBOT_CONFIG.polling.errorBackoffRate);
				}
			}
		}, ROBOT_CONFIG.polling.consumerPollingRate);
	}

	async stopListening(): Promise<void> {
		if (this.pollingInterval !== null) {
			clearInterval(this.pollingInterval);
			this.pollingInterval = null;
			console.log(`[${this.name}] Stopped position listening`);
		}
	}

	// Event handlers already in base class

	onCommand(callback: (command: RobotCommand) => void): () => void {
		this.commandCallbacks.push(callback);
		return () => {
			const index = this.commandCallbacks.indexOf(callback);
			if (index >= 0) {
				this.commandCallbacks.splice(index, 1);
			}
		};
	}

	// Private methods
	private async pollAndBroadcastPositions(): Promise<void> {
		if (!this.scsServoSDK || !this._status.isConnected) {
			return;
		}

		try {
			// Read positions for all servos
			const servoIds = Object.values(this.jointToServoMap);
			const positions = await this.scsServoSDK.syncReadPositions(servoIds);

			const jointsWithChanges: { name: string; value: number }[] = [];

			// Check for position changes and convert to normalized values
			Object.entries(this.jointToServoMap).forEach(([jointName, servoId]) => {
				const currentPosition = positions.get(servoId);
				const lastPosition = this.lastPositions[servoId];

				if (
					currentPosition !== undefined &&
					(lastPosition === undefined ||
						Math.abs(currentPosition - lastPosition) >
							ROBOT_CONFIG.performance.jointUpdateThreshold)
				) {
					this.lastPositions[servoId] = currentPosition;

					// Convert to normalized value using calibration (required)
					const normalizedValue = this.normalizeValue(currentPosition, jointName);

					jointsWithChanges.push({
						name: jointName,
						value: normalizedValue
					});
				}
			});

			// Broadcast changes if any
			if (jointsWithChanges.length > 0) {
				const command: RobotCommand = {
					timestamp: Date.now(),
					joints: jointsWithChanges
				};

				this.notifyCommand(command);
			}
		} catch (error) {
			throw error; // Re-throw for error handling in polling loop
		}
	}

	private notifyCommand(command: RobotCommand): void {
		this.commandCallbacks.forEach((callback) => {
			try {
				callback(command);
			} catch (error) {
				console.error(`[${this.name}] Error in command callback:`, error);
			}
		});
	}
}