src/lib/robot/drivers/USBSlave.ts

import type {
	SlaveDriver,
	DriverJointState,
	ConnectionStatus,
	RobotCommand,
	USBSlaveConfig,
	StateUpdateCallback,
	StatusChangeCallback,
	UnsubscribeFn
} from "$lib/types/robotDriver";
// import { scsServoSDK } from '../../../../feetech.js/index.mjs';
import { scsServoSDK } from "feetech.js";
import { servoPositionToAngle, degreesToServoPosition } from "$lib/utils";

/**
 * USB Slave Driver
 * Controls physical robots via feetech.js USB connection
 */
export class USBSlave implements SlaveDriver {
	readonly type = "slave" as const;
	readonly id: string;
	readonly name: string;

	private _status: ConnectionStatus = { isConnected: false };
	private config: USBSlaveConfig;

	// Joint states
	private jointStates: DriverJointState[] = [];
	private initialPositions: number[] = [];

	// Event callbacks
	private stateCallbacks: StateUpdateCallback[] = [];
	private statusCallbacks: StatusChangeCallback[] = [];

	constructor(config: USBSlaveConfig, initialJointStates: DriverJointState[]) {
		this.config = config;
		this.id = `usb-slave-${Date.now()}`;
		this.name = `USB Slave Robot`;

		// Initialize joint states
		this.jointStates = initialJointStates.map((state) => ({ ...state }));

		// Validate that all active joints have servo IDs
		const missingServos = this.jointStates.filter((j) => !j.servoId);
		if (missingServos.length > 0) {
			console.warn(
				`USB slave: Some joints missing servo IDs: ${missingServos.map((j) => j.name).join(", ")}`
			);
		}

		console.log(`Created USBSlave with ${this.jointStates.length} joints`);
	}

	get status(): ConnectionStatus {
		return this._status;
	}

	async connect(): Promise<void> {
		console.log(`Connecting ${this.name}...`);

		try {
			this._status = { isConnected: false };
			this.notifyStatusChange();

			await scsServoSDK.connect();

			// Initialize each servo
			const initialPositions: number[] = [];

			for (const joint of this.jointStates) {
				if (!joint.servoId) {
					initialPositions.push(0);
					continue;
				}

				try {
					if (joint.type === "continuous") {
						// Configure for speed control
						await scsServoSDK.setWheelMode(joint.servoId);
						await scsServoSDK.writeWheelSpeed(joint.servoId, 0);
						initialPositions.push(0);

						// Update joint state
						joint.realValue = 0;
					} else {
						// Configure for position control
						await scsServoSDK.setPositionMode(joint.servoId);

						// Read initial position
						const servoPosition = await scsServoSDK.readPosition(joint.servoId);
						const positionInDegrees = servoPositionToAngle(servoPosition);
						initialPositions.push(positionInDegrees);

						// Enable torque
						await scsServoSDK.writeTorqueEnable(joint.servoId, true);

						// Update joint state
						joint.realValue = positionInDegrees;
					}
				} catch (error) {
					console.error(`Failed to initialize servo ${joint.servoId} (${joint.name}):`, error);
					initialPositions.push(0);
					joint.realValue = undefined;
				}
			}

			this.initialPositions = initialPositions;

			this._status = {
				isConnected: true,
				lastConnected: new Date(),
				error: undefined
			};
			this.notifyStatusChange();

			console.log(`${this.name} connected successfully`);
		} catch (error) {
			this._status = {
				isConnected: false,
				error: `Connection failed: ${error}`
			};
			this.notifyStatusChange();
			throw error;
		}
	}

	async disconnect(): Promise<void> {
		console.log(`Disconnecting ${this.name}...`);

		if (!this._status.isConnected) return;

		try {
			// Stop all joints safely
			for (const joint of this.jointStates) {
				if (!joint.servoId) continue;

				try {
					if (joint.type === "continuous") {
						await scsServoSDK.writeWheelSpeed(joint.servoId, 0);
					}
					await scsServoSDK.writeTorqueEnable(joint.servoId, false);
				} catch (error) {
					console.error(`Failed to stop servo ${joint.servoId}:`, error);
				}
			}

			await scsServoSDK.disconnect();

			// Clear real values
			this.jointStates.forEach((joint) => {
				joint.realValue = undefined;
			});

			this._status = { isConnected: false };
			this.notifyStatusChange();

			console.log(`${this.name} disconnected`);
		} catch (error) {
			this._status = {
				isConnected: false,
				error: `Disconnect failed: ${error}`
			};
			this.notifyStatusChange();
			throw error;
		}
	}

	async executeCommand(command: RobotCommand): Promise<void> {
		if (!this._status.isConnected) {
			throw new Error("Cannot execute command: USB slave not connected");
		}

		console.log(`USBSlave executing command with ${command.joints.length} joint updates`);

		for (const jointUpdate of command.joints) {
			const joint = this.jointStates.find((j) => j.name === jointUpdate.name);
			if (!joint || !joint.servoId) continue;

			try {
				if (joint.type === "continuous") {
					await scsServoSDK.writeWheelSpeed(joint.servoId, jointUpdate.value);
					joint.realValue = jointUpdate.value;
				} else {
					// Use relative positioning for revolute joints
					const jointIndex = this.jointStates.indexOf(joint);
					const relativeValue = (this.initialPositions[jointIndex] || 0) + jointUpdate.value;

					if (relativeValue >= 0 && relativeValue <= 360) {
						const servoPosition = degreesToServoPosition(relativeValue);
						await scsServoSDK.writePosition(joint.servoId, Math.round(servoPosition));
						joint.realValue = relativeValue;
					} else {
						throw new Error(`Position ${relativeValue}° out of range (0-360°)`);
					}
				}

				// Update virtual value to match what we sent
				joint.virtualValue = jointUpdate.value;
			} catch (error) {
				console.error(`Failed to execute command for joint ${joint.name}:`, error);
				joint.realValue = undefined;
				throw error;
			}
		}

		// Notify state update
		this.notifyStateUpdate();
	}

	async executeCommands(commands: RobotCommand[]): Promise<void> {
		console.log(`USBSlave executing batch of ${commands.length} commands`);

		for (const command of commands) {
			await this.executeCommand(command);

			// Small delay between commands to avoid overwhelming servos
			if (commands.length > 1) {
				await new Promise((resolve) => setTimeout(resolve, 50));
			}
		}
	}

	async readJointStates(): Promise<DriverJointState[]> {
		if (!this._status.isConnected) {
			throw new Error("Cannot read states: USB slave not connected");
		}

		const states: DriverJointState[] = [];

		for (let i = 0; i < this.jointStates.length; i++) {
			const joint = this.jointStates[i];

			if (!joint.servoId) {
				states.push({ ...joint });
				continue;
			}

			try {
				if (joint.type === "revolute") {
					const servoPosition = await scsServoSDK.readPosition(joint.servoId);
					const positionInDegrees = servoPositionToAngle(servoPosition);

					states.push({
						...joint,
						realValue: positionInDegrees
					});
				} else {
					// For continuous joints, we don't read speed (not available in feetech)
					// Keep the last known value
					states.push({ ...joint });
				}
			} catch (error) {
				console.error(`Failed to read servo ${joint.servoId}:`, error);
				states.push({
					...joint,
					realValue: undefined
				});
			}
		}

		return states;
	}

	async writeJointState(jointName: string, value: number): Promise<void> {
		const command: RobotCommand = {
			timestamp: Date.now(),
			joints: [{ name: jointName, value }]
		};

		await this.executeCommand(command);
	}

	async writeJointStates(updates: { jointName: string; value: number }[]): Promise<void> {
		const command: RobotCommand = {
			timestamp: Date.now(),
			joints: updates.map((update) => ({ name: update.jointName, value: update.value }))
		};

		await this.executeCommand(command);
	}

	// Event subscription methods
	onStateUpdate(callback: StateUpdateCallback): UnsubscribeFn {
		this.stateCallbacks.push(callback);
		return () => {
			const index = this.stateCallbacks.indexOf(callback);
			if (index >= 0) {
				this.stateCallbacks.splice(index, 1);
			}
		};
	}

	onStatusChange(callback: StatusChangeCallback): UnsubscribeFn {
		this.statusCallbacks.push(callback);
		return () => {
			const index = this.statusCallbacks.indexOf(callback);
			if (index >= 0) {
				this.statusCallbacks.splice(index, 1);
			}
		};
	}

	// Private methods
	private notifyStateUpdate(): void {
		this.stateCallbacks.forEach((callback) => {
			try {
				callback([...this.jointStates]);
			} catch (error) {
				console.error("Error in state update callback:", error);
			}
		});
	}

	private notifyStatusChange(): void {
		this.statusCallbacks.forEach((callback) => {
			try {
				callback(this._status);
			} catch (error) {
				console.error("Error in status change callback:", error);
			}
		});
	}
}