src/lib/robot/drivers/USBMaster.ts

import type {
	MasterDriver,
	ConnectionStatus,
	RobotCommand,
	CommandSequence,
	USBMasterConfig,
	CommandCallback,
	SequenceCallback,
	StatusChangeCallback,
	UnsubscribeFn
} from "$lib/types/robotDriver";
import { getRobotPollingConfig, getDataProcessingConfig } from "$lib/configs/performanceConfig";
import { scsServoSDKUnlock } from "feetech.js";

/**
 * USB Master Driver
 * Reads positions from a physical USB robot and forwards movements to slaves
 * Allows physical robot to act as a master controller for digital twins and other robots
 */
export class USBMaster implements MasterDriver {
	readonly type = "master" as const;
	readonly id: string;
	readonly name: string;

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

	// Robot joint configuration
	private jointIds: number[] = [1, 2, 3, 4, 5, 6]; // Default servo IDs for so-arm100
	private jointNames: string[] = ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll", "Jaw"];
	private lastPositions: Map<number, number> = new Map();
	private restPositions: Map<number, number> = new Map(); // Store initial rest positions
	private positionHistory: Map<number, number[]> = new Map();

	// Polling control
	private pollingInterval?: number;
	private isPolling = false;
	private isPaused = false;
	private isReading = false; // Prevent concurrent reads

	// Event callbacks
	private commandCallbacks: CommandCallback[] = [];
	private sequenceCallbacks: SequenceCallback[] = [];
	private statusCallbacks: StatusChangeCallback[] = [];

	constructor(config: USBMasterConfig) {
		this.config = config;
		this.id = `usb-master-${Date.now()}`;
		this.name = `USB Master Robot`;

		// Initialize position tracking
		this.jointIds.forEach((id) => {
			this.lastPositions.set(id, 2048); // Center position (will be updated on connect)
			this.restPositions.set(id, 2048); // Rest position baseline (will be updated on connect)
			this.positionHistory.set(id, []);
		});

		console.log(`🔌 Created USBMaster with ${this.jointIds.length} joints (unlock mode)`);
	}

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

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

		try {
			// Connect to USB robot
			await scsServoSDKUnlock.connect({
				baudRate: this.config.baudRate || 1000000,
				protocolEnd: 0
			});

			// Read initial positions
			await this.readInitialPositions();

			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}...`);

		this.stopPolling();

		if (scsServoSDKUnlock && this._status.isConnected) {
			try {
				await scsServoSDKUnlock.disconnect();
			} catch (error) {
				console.warn("Error disconnecting USB robot:", error);
			}
		}

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

		console.log(`✅ ${this.name} disconnected`);
	}

	async start(): Promise<void> {
		if (!this._status.isConnected) {
			throw new Error("Cannot start: USB master not connected");
		}

		console.log(`🎮 Starting USB master polling...`);
		this.startPolling();
	}

	async stop(): Promise<void> {
		console.log(`⏹️ Stopping USB master polling...`);
		this.stopPolling();
	}

	async pause(): Promise<void> {
		console.log(`⏸️ Pausing USB master polling...`);
		this.isPaused = true;
	}

	async resume(): Promise<void> {
		console.log(`▶️ Resuming USB master polling...`);
		this.isPaused = false;
	}

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

	onSequence(callback: SequenceCallback): UnsubscribeFn {
		this.sequenceCallbacks.push(callback);
		return () => {
			const index = this.sequenceCallbacks.indexOf(callback);
			if (index >= 0) {
				this.sequenceCallbacks.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 async readInitialPositions(): Promise<void> {
		if (!scsServoSDKUnlock) return;

		try {
			console.log("📖 Reading initial REST positions from USB robot (individual reads)...");

			let successCount = 0;
			for (const servoId of this.jointIds) {
				try {
					const position = await scsServoSDKUnlock.readPosition(servoId);

					// Store as both last position and rest position baseline
					this.lastPositions.set(servoId, position);
					this.restPositions.set(servoId, position);

					console.log(`Joint ${servoId}: REST position = ${position} (servo units)`);
					successCount++;
				} catch (err) {
					console.warn(`Failed to read servo ${servoId}:`, err);
					// Keep default center positions for failed reads
				}
			}

			console.log(`📖 Successfully read ${successCount}/${this.jointIds.length} REST positions`);
			console.log("🏠 Robot is now calibrated to REST position baseline");
		} catch (error) {
			console.warn("Could not read initial positions:", error);
			console.log("🏠 Using default center positions as REST baseline");
		}
	}

	private startPolling(): void {
		if (this.isPolling) return;

		this.isPolling = true;
		// Use performance config instead of hardcoded value
		const pollInterval =
			this.config.pollInterval || getRobotPollingConfig().USB_MASTER_POLL_INTERVAL_MS;

		this.pollingInterval = setInterval(async () => {
			if (this.isPaused || !this._status.isConnected || this.isReading) return;

			try {
				await this.pollRobotPositions();
			} catch (error) {
				console.error("Error polling robot positions:", error);

				// Handle connection errors
				if (error instanceof Error && error.message?.includes("Not connected")) {
					this._status = {
						isConnected: false,
						error: `Connection lost: ${error.message}`
					};
					this.notifyStatusChange();
					this.stopPolling();
				}
			}
		}, pollInterval);

		console.log(`🔄 USB master polling started (${pollInterval}ms interval)`);
	}

	private stopPolling(): void {
		if (this.pollingInterval) {
			clearInterval(this.pollingInterval);
			this.pollingInterval = undefined;
		}
		this.isPolling = false;
	}

	private async pollRobotPositions(): Promise<void> {
		if (!scsServoSDKUnlock || this.isReading) return;

		this.isReading = true; // Prevent concurrent reads

		try {
			const currentPositions: Map<number, number> = new Map();
			const changedJoints: { name: string; value: number }[] = [];
			let validReads = 0;

			// Read all servo positions
			for (const servoId of this.jointIds) {
				try {
					const position = await scsServoSDKUnlock.readPosition(servoId);
					if (position !== null && position !== undefined) {
						currentPositions.set(servoId, position);
						validReads++;
					}
				} catch {
					// Skip individual servo read errors
				}
			}

			currentPositions.forEach((position: number, servoId: number) => {
				// Process all readings - position 0 is valid
				const lastPosition = this.lastPositions.get(servoId) || 2048;
				// Use performance config for threshold
				const threshold = getDataProcessingConfig().ENABLE_SMOOTHING
					? getRobotPollingConfig().POSITION_CHANGE_THRESHOLD
					: 1; // Maximum sensitivity when smoothing disabled

				if (Math.abs(position - lastPosition) > threshold) {
					const jointIndex = this.jointIds.indexOf(servoId);
					if (jointIndex >= 0) {
						const jointName = this.jointNames[jointIndex];
						const degrees = this.servoToRelativeDegrees(position, servoId);

						console.log(
							`🎯 Joint ${jointName} moved: ${lastPosition} → ${position} (${degrees.toFixed(1)}°)`
						);

						// Apply smoothing if enabled in performance config
						if (this.config.smoothing && getDataProcessingConfig().ENABLE_SMOOTHING) {
							const history = this.positionHistory.get(servoId) || [];
							history.push(degrees);
							const maxHistory = getDataProcessingConfig().SMOOTHING_HISTORY_SIZE;
							if (history.length > maxHistory) history.shift();
							this.positionHistory.set(servoId, history);

							// Use average of recent positions
							const smoothedDegrees = history.reduce((a, b) => a + b, 0) / history.length;
							changedJoints.push({ name: jointName, value: smoothedDegrees });
						} else {
							changedJoints.push({ name: jointName, value: degrees });
						}

						this.lastPositions.set(servoId, position);
					}
				}
			});

			// Log polling status occasionally (less frequent with faster polling)
			if (Date.now() % 5000 < getRobotPollingConfig().USB_MASTER_POLL_INTERVAL_MS) {
				console.log(
					`🔄 USB polling: ${validReads}/${this.jointIds.length} joints read successfully`
				);
			}

			// If there are changes, emit command
			if (changedJoints.length > 0) {
				const command: RobotCommand = {
					timestamp: Date.now(),
					joints: changedJoints,
					metadata: { source: "usb_master_polling" }
				};

				console.log(`📤 Emitting command with ${changedJoints.length} joint changes`);
				this.notifyCommand([command]);
			}
		} catch (error) {
			console.error("Polling error:", error);

			// Don't re-throw port access errors, just log them
			if (error instanceof Error && error.message?.includes("Releasing Default reader")) {
				console.warn("Serial port access conflict, skipping this poll cycle");
				return;
			}

			throw error; // Re-throw other errors
		} finally {
			// Always release the reading lock
			this.isReading = false;
		}
	}

	private servoToRelativeDegrees(servoPosition: number, servoId: number): number {
		// Get the rest position for this servo
		const restPosition = this.restPositions.get(servoId) || 2048;

		// Calculate relative movement from rest position
		const deltaServoUnits = servoPosition - restPosition;

		// Convert servo units to degrees (assuming full range 0-4095 = 360°)
		const relativeDegrees = (deltaServoUnits / 4095) * 360;

		return relativeDegrees;
	}

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

	private notifySequence(sequence: CommandSequence): void {
		this.sequenceCallbacks.forEach((callback) => {
			try {
				callback(sequence);
			} catch (error) {
				console.error("Error in sequence callback:", error);
			}
		});
	}

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