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

import type { ConnectionStatus, USBDriverConfig } from "../models.js";
import { CalibrationState } from "../calibration/CalibrationState.svelte.js";
import { ScsServoSDK } from "feetech.js";
import { ROBOT_CONFIG } from "../config.js";

export abstract class USBServoDriver {
	readonly id: string;
	readonly name: string;
	readonly config: USBDriverConfig;

	protected _status: ConnectionStatus = { isConnected: false };
	protected statusCallbacks: ((status: ConnectionStatus) => void)[] = [];

	protected scsServoSDK: ScsServoSDK | null = null;

	// Calibration state - directly embedded
	readonly calibrationState: CalibrationState;

	// Calibration polling
	private calibrationPollingInterval: ReturnType<typeof setInterval> | null = null;

	// Joint to servo ID mapping for SO-100 arm
	protected readonly jointToServoMap = {
		Rotation: 1,
		Pitch: 2,
		Elbow: 3,
		Wrist_Pitch: 4,
		Wrist_Roll: 5,
		Jaw: 6
	};

	constructor(config: USBDriverConfig, driverType: string) {
		this.config = config;
		this.id = `usb-${driverType}-${Date.now()}`;
		this.name = `USB ${driverType}`;
		this.calibrationState = new CalibrationState();
	}

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

	get needsCalibration(): boolean {
		return this.calibrationState.needsCalibration;
	}

	get isCalibrating(): boolean {
		return this.calibrationState.isCalibrating;
	}

	get isCalibrated(): boolean {
		return this.calibrationState.isCalibrated;
	}

	// Type guard to check if a driver is a USB driver
	static isUSBDriver(driver: any): driver is USBServoDriver {
		return (
			driver &&
			typeof driver.calibrationState === "object" &&
			typeof driver.needsCalibration === "boolean" &&
			typeof driver.isCalibrated === "boolean" &&
			typeof driver.startCalibration === "function"
		);
	}

	// Type-safe method to get calibration interface
	getCalibrationInterface(): {
		needsCalibration: boolean;
		isCalibrating: boolean;
		isCalibrated: boolean;
		startCalibration: () => Promise<void>;
		completeCalibration: () => Promise<Record<string, number>>;
		skipCalibration: () => void;
		cancelCalibration: () => void;
		onCalibrationCompleteWithPositions: (
			callback: (positions: Record<string, number>) => void
		) => () => void;
	} {
		return {
			needsCalibration: this.needsCalibration,
			isCalibrating: this.isCalibrating,
			isCalibrated: this.isCalibrated,
			startCalibration: () => this.startCalibration(),
			completeCalibration: () => this.completeCalibration(),
			skipCalibration: () => this.skipCalibration(),
			cancelCalibration: () => this.cancelCalibration(),
			onCalibrationCompleteWithPositions: (callback) =>
				this.onCalibrationCompleteWithPositions(callback)
		};
	}

	// Abstract methods that subclasses must implement
	abstract connect(): Promise<void>;
	abstract disconnect(): Promise<void>;

	// Common connection logic
	protected async connectToUSB(): Promise<void> {
		if (this._status.isConnected) {
			console.log(`[${this.name}] Already connected`);
			return;
		}

		try {
			console.log(`[${this.name}] Connecting...`);

			// Create a new SDK instance for this driver instead of using the singleton
			// This allows multiple drivers to connect to different ports simultaneously
			this.scsServoSDK = new ScsServoSDK();

			await this.scsServoSDK.connect({
				baudRate: this.config.baudRate || ROBOT_CONFIG.usb.baudRate,
				protocolEnd: 0 // STS/SMS protocol
			});

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

			console.log(`[${this.name}] Connected successfully`);

			// Debug: Log SDK instance methods to identify the issue
			console.log(
				`[${this.name}] SDK instance methods:`,
				Object.getOwnPropertyNames(this.scsServoSDK)
			);
			console.log(
				`[${this.name}] SDK prototype methods:`,
				Object.getOwnPropertyNames(Object.getPrototypeOf(this.scsServoSDK))
			);
			console.log(
				`[${this.name}] writeTorqueEnable available:`,
				typeof this.scsServoSDK.writeTorqueEnable
			);
			console.log(
				`[${this.name}] syncReadPositions available:`,
				typeof this.scsServoSDK.syncReadPositions
			);
		} catch (error) {
			console.error(`[${this.name}] Connection failed:`, error);
			this._status = { isConnected: false, error: `Connection failed: ${error}` };
			this.notifyStatusChange();
			throw error;
		}
	}

	protected async disconnectFromUSB(): Promise<void> {
		if (this.scsServoSDK) {
			try {
				await this.unlockAllServos();
				await this.scsServoSDK.disconnect();
			} catch (error) {
				console.warn(`[${this.name}] Error during disconnect:`, error);
			}
			this.scsServoSDK = null;
		}

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

		console.log(`[${this.name}] Disconnected`);
	}

	// Calibration methods
	async startCalibration(): Promise<void> {
		if (!this._status.isConnected) {
			await this.connectToUSB();
		}

		if (!this._status.isConnected) {
			throw new Error("Cannot start calibration: not connected");
		}

		console.log(`[${this.name}] Starting calibration...`);
		this.calibrationState.startCalibration();

		// Unlock servos for manual movement during calibration
		await this.unlockAllServos();

		// Start polling positions during calibration
		await this.startCalibrationPolling();
	}

	async completeCalibration(): Promise<Record<string, number>> {
		if (!this.isCalibrating) {
			throw new Error("Not currently calibrating");
		}

		// Stop polling
		this.stopCalibrationPolling();

		// Read final positions
		const finalPositions = await this.readCurrentPositions();

		// Complete calibration state
		this.calibrationState.completeCalibration();

		console.log(`[${this.name}] Calibration completed`);
		return finalPositions;
	}

	skipCalibration(): void {
		// Stop polling if active
		this.stopCalibrationPolling();
		this.calibrationState.skipCalibration();
	}

	async setPredefinedCalibration(): Promise<void> {
		// Stop polling if active
		this.stopCalibrationPolling();
		this.skipCalibration();
	}

	// Cancel calibration
	cancelCalibration(): void {
		// Stop polling if active
		this.stopCalibrationPolling();
		this.calibrationState.cancelCalibration();
	}

	// Start polling servo positions during calibration
	private async startCalibrationPolling(): Promise<void> {
		if (this.calibrationPollingInterval !== null) {
			return; // Already polling
		}

		console.log(`[${this.name}] Starting calibration position polling...`);

		// Poll positions every 100ms during calibration
		this.calibrationPollingInterval = setInterval(async () => {
			if (!this.isCalibrating || !this._status.isConnected || !this.scsServoSDK) {
				this.stopCalibrationPolling();
				return;
			}

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

				// Update calibration state with current positions
				Object.entries(this.jointToServoMap).forEach(([jointName, servoId]) => {
					const position = positions.get(servoId);
					if (position !== undefined) {
						this.calibrationState.updateCurrentValue(jointName, position);
						console.debug(`[${this.name}] ${jointName} (servo ${servoId}): ${position}`);
					}
				});
			} catch (error) {
				console.warn(`[${this.name}] Calibration polling error:`, error);
				// Continue polling despite errors - user might be moving servos rapidly
			}
		}, 100); // Poll every 100ms
	}

	// Stop polling servo positions
	private stopCalibrationPolling(): void {
		if (this.calibrationPollingInterval !== null) {
			clearInterval(this.calibrationPollingInterval);
			this.calibrationPollingInterval = null;
			console.log(`[${this.name}] Stopped calibration position polling`);
		}
	}

	// Servo position reading (for calibration)
	async readCurrentPositions(): Promise<Record<string, number>> {
		if (!this.scsServoSDK || !this._status.isConnected) {
			throw new Error("Cannot read positions: not connected");
		}

		const positions: Record<string, number> = {};

		try {
			const servoIds = Object.values(this.jointToServoMap);
			const servoPositions = await this.scsServoSDK.syncReadPositions(servoIds);

			Object.entries(this.jointToServoMap).forEach(([jointName, servoId]) => {
				const position = servoPositions.get(servoId);
				if (position !== undefined) {
					positions[jointName] = position;
					// Update calibration state with current position
					this.calibrationState.updateCurrentValue(jointName, position);
				}
			});
		} catch (error) {
			console.error(`[${this.name}] Error reading positions:`, error);
			throw error;
		}

		return positions;
	}

	// Value conversion methods
	normalizeValue(rawValue: number, jointName: string): number {
		if (!this.isCalibrated) {
			throw new Error("Cannot normalize value: not calibrated");
		}
		return this.calibrationState.normalizeValue(rawValue, jointName);
	}

	denormalizeValue(normalizedValue: number, jointName: string): number {
		if (!this.isCalibrated) {
			throw new Error("Cannot denormalize value: not calibrated");
		}
		return this.calibrationState.denormalizeValue(normalizedValue, jointName);
	}

	// Servo control methods
	protected async lockAllServos(): Promise<void> {
		if (!this.scsServoSDK) return;

		try {
			console.log(`[${this.name}] Locking all servos...`);

			const servoIds = Object.values(this.jointToServoMap);

			for (const servoId of servoIds) {
				try {
					// Check if writeTorqueEnable method exists before calling
					if (typeof this.scsServoSDK.writeTorqueEnable !== "function") {
						console.warn(
							`[${this.name}] writeTorqueEnable method not available on SDK instance for servo ${servoId}`
						);
						continue;
					}

					await this.scsServoSDK.writeTorqueEnable(servoId, true);
					await new Promise((resolve) => setTimeout(resolve, ROBOT_CONFIG.usb.servoWriteDelay));
				} catch (error) {
					console.warn(`[${this.name}] Failed to lock servo ${servoId}:`, error);
				}
			}

			console.log(`[${this.name}] All servos locked`);
		} catch (error) {
			console.error(`[${this.name}] Error locking servos:`, error);
		}
	}

	protected async unlockAllServos(): Promise<void> {
		if (!this.scsServoSDK) return;

		try {
			console.log(`[${this.name}] Unlocking all servos...`);

			const servoIds = Object.values(this.jointToServoMap);

			for (const servoId of servoIds) {
				try {
					// Check if writeTorqueEnable method exists before calling
					if (typeof this.scsServoSDK.writeTorqueEnable !== "function") {
						console.warn(
							`[${this.name}] writeTorqueEnable method not available on SDK instance for servo ${servoId}`
						);
						continue;
					}

					await this.scsServoSDK.writeTorqueEnable(servoId, false);
					await new Promise((resolve) => setTimeout(resolve, ROBOT_CONFIG.usb.servoWriteDelay));
				} catch (error) {
					console.warn(`[${this.name}] Failed to unlock servo ${servoId}:`, error);
				}
			}

			console.log(`[${this.name}] All servos unlocked`);
		} catch (error) {
			console.error(`[${this.name}] Error unlocking servos:`, error);
		}
	}

	// Event handlers
	onStatusChange(callback: (status: ConnectionStatus) => void): () => void {
		this.statusCallbacks.push(callback);
		return () => {
			const index = this.statusCallbacks.indexOf(callback);
			if (index >= 0) {
				this.statusCallbacks.splice(index, 1);
			}
		};
	}

	protected notifyStatusChange(): void {
		this.statusCallbacks.forEach((callback) => {
			try {
				callback(this._status);
			} catch (error) {
				console.error(`[${this.name}] Error in status callback:`, error);
			}
		});
	}

	// Register callback for calibration completion with positions
	onCalibrationCompleteWithPositions(
		callback: (positions: Record<string, number>) => void
	): () => void {
		return this.calibrationState.onCalibrationCompleteWithPositions(callback);
	}

	// Sync robot joint positions using normalized values from calibration
	syncRobotPositions(
		finalPositions: Record<string, number>,
		updateRobotCallback?: (jointName: string, normalizedValue: number) => void
	): void {
		if (!updateRobotCallback) return;

		console.log(`[${this.name}] 🔄 Syncing robot to final calibration positions...`);

		Object.entries(finalPositions).forEach(([jointName, rawPosition]) => {
			try {
				// Convert raw servo position to normalized value using calibration
				const normalizedValue = this.normalizeValue(rawPosition, jointName);

				// Clamp to appropriate normalized range based on joint type
				let clampedValue: number;
				if (jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper") {
					clampedValue = Math.max(0, Math.min(100, normalizedValue));
				} else {
					clampedValue = Math.max(-100, Math.min(100, normalizedValue));
				}

				console.log(
					`[${this.name}] ${jointName}: ${rawPosition} (raw) -> ${normalizedValue.toFixed(1)} -> ${clampedValue.toFixed(1)} (normalized)`
				);

				// Update robot joint through callback
				updateRobotCallback(jointName, clampedValue);
			} catch (error) {
				console.warn(`[${this.name}] Failed to sync position for joint ${jointName}:`, error);
			}
		});

		console.log(`[${this.name}] ✅ Robot synced to calibration positions`);
	}

	// Cleanup
	async destroy(): Promise<void> {
		this.stopCalibrationPolling();
		await this.disconnect();
		this.calibrationState.destroy();
	}
}