src/lib/elements/robot/Robot.svelte.ts

import type {
	JointState,
	RobotCommand,
	ConnectionStatus,
	USBDriverConfig,
	RemoteDriverConfig,
	Consumer,
	Producer
} from "./models.js";
import type { Positionable, Position3D } from "$lib/types/positionable.js";
import { USBConsumer } from "./drivers/USBConsumer.js";
import { USBProducer } from "./drivers/USBProducer.js";
import { RemoteConsumer } from "./drivers/RemoteConsumer.js";
import { RemoteProducer } from "./drivers/RemoteProducer.js";
import { USBServoDriver } from "./drivers/USBServoDriver.js";

import { ROBOT_CONFIG } from "./config.js";
import type IUrdfRobot from "@/components/3d/elements/robot/URDF/interfaces/IUrdfRobot.js";

export class Robot implements Positionable {
	// Core robot data
	readonly id: string;
	private unsubscribeFns: (() => void)[] = [];

	// Command synchronization to prevent state conflicts
	private commandMutex = $state(false);
	private pendingCommands: RobotCommand[] = [];

	// Command deduplication to prevent rapid duplicate commands
	private lastCommandTime = 0;
	private lastCommandValues: Record<string, number> = {};

	// Memory management
	private lastCleanup = 0;

	// Single consumer and multiple producers using Svelte 5 runes - PUBLIC for reactive access
	consumer = $state<Consumer | null>(null);
	producers = $state<Producer[]>([]);

	// Reactive state using Svelte 5 runes - PUBLIC for reactive access
	joints = $state<Record<string, JointState>>({});
	position = $state<Position3D>({ x: 0, y: 0, z: 0 });
	isManualControlEnabled = $state(true);
	connectionStatus = $state<ConnectionStatus>({ isConnected: false });

	// URDF robot state for 3D visualization - PUBLIC for reactive access
	urdfRobotState = $state<IUrdfRobot | null>(null);

	// Derived reactive values for components
	jointArray = $derived(Object.values(this.joints));
	hasProducers = $derived(this.producers.length > 0);
	hasConsumer = $derived(this.consumer !== null && this.consumer.status.isConnected);
	outputDriverCount = $derived(this.producers.filter((d) => d.status.isConnected).length);

	constructor(id: string, initialJoints: JointState[], urdfRobotState?: IUrdfRobot) {
		this.id = id;

		// Store URDF robot state if provided
		this.urdfRobotState = urdfRobotState || null;

		// Initialize joints with normalized values
		initialJoints.forEach((joint) => {
			const isGripper =
				joint.name.toLowerCase() === "jaw" || joint.name.toLowerCase() === "gripper";
			this.joints[joint.name] = {
				...joint,
				value: isGripper ? 0 : 0 // Start at neutral position
			};
		});
	}

	// Method to set URDF robot state after creation (for async loading)
	setUrdfRobotState(urdfRobotState: any): void {
		this.urdfRobotState = urdfRobotState;
	}

	/**
	 * Update position (implements Positionable interface)
	 */
	updatePosition(newPosition: Position3D): void {
		this.position = { ...newPosition };
	}

	// Get all USB drivers (both consumer and producers) for calibration
	getUSBDrivers(): USBServoDriver[] {
		const usbDrivers: USBServoDriver[] = [];

		// Check consumer
		if (this.consumer && USBServoDriver.isUSBDriver(this.consumer)) {
			usbDrivers.push(this.consumer);
		}

		// Check producers
		this.producers.forEach((producer) => {
			if (USBServoDriver.isUSBDriver(producer)) {
				usbDrivers.push(producer);
			}
		});

		return usbDrivers;
	}

	// Get uncalibrated USB drivers that need calibration
	getUncalibratedUSBDrivers(): USBServoDriver[] {
		return this.getUSBDrivers().filter((driver) => driver.needsCalibration);
	}

	// Check if robot has any USB drivers
	hasUSBDrivers(): boolean {
		return this.getUSBDrivers().length > 0;
	}

	// Check if all USB drivers are calibrated
	areAllUSBDriversCalibrated(): boolean {
		const usbDrivers = this.getUSBDrivers();
		return usbDrivers.length > 0 && usbDrivers.every((driver) => driver.isCalibrated);
	}

	// Joint value updates (normalized) - for manual control
	updateJoint(name: string, normalizedValue: number): void {
		if (!this.isManualControlEnabled) {
			console.warn("Manual control is disabled");
			return;
		}

		this.updateJointValue(name, normalizedValue, true);
	}

	// Internal joint value update (used by both manual control and USB calibration sync)
	updateJointValue(name: string, normalizedValue: number, sendToProducers: boolean = false): void {
		const joint = this.joints[name];
		if (!joint) {
			console.warn(`Joint ${name} not found`);
			return;
		}

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

		console.debug(
			`[Robot ${this.id}] Update joint ${name} to ${normalizedValue} (normalized, sendToProducers: ${sendToProducers})`
		);

		// Create a new joint object to ensure reactivity
		this.joints[name] = { ...joint, value: normalizedValue };

		// Send normalized command to producers if requested
		if (sendToProducers) {
			this.sendToProducers({ joints: [{ name, value: normalizedValue }] });
		}
	}

	executeCommand(command: RobotCommand): void {
		// Command deduplication - skip if same values sent within dedup window
		const now = Date.now();
		if (now - this.lastCommandTime < ROBOT_CONFIG.commands.dedupWindow) {
			const hasChanges = command.joints.some(
				(joint) => Math.abs((this.lastCommandValues[joint.name] || 0) - joint.value) > 0.5
			);
			if (!hasChanges) {
				console.debug(
					`[Robot ${this.id}] 🔄 Skipping duplicate command within ${ROBOT_CONFIG.commands.dedupWindow}ms window`
				);
				return;
			}
		}

		// Update deduplication tracking
		this.lastCommandTime = now;
		command.joints.forEach((joint) => {
			this.lastCommandValues[joint.name] = joint.value;
		});

		// Queue command if mutex is locked to prevent race conditions
		if (this.commandMutex) {
			if (this.pendingCommands.length >= ROBOT_CONFIG.commands.maxQueueSize) {
				console.warn(`[Robot ${this.id}] Command queue full, dropping oldest command`);
				this.pendingCommands.shift();
			}
			this.pendingCommands.push(command);
			return;
		}

		this.commandMutex = true;

		try {
			console.debug(
				`[Robot ${this.id}] Executing command with ${command.joints.length} joints:`,
				command.joints.map((j) => `${j.name}=${j.value}`).join(", ")
			);

			// Update virtual robot joints with normalized values
			command.joints.forEach((jointCmd) => {
				const joint = this.joints[jointCmd.name];
				if (joint) {
					// Clamp to appropriate normalized range based on joint type
					let normalizedValue: number;
					if (jointCmd.name.toLowerCase() === "jaw" || jointCmd.name.toLowerCase() === "gripper") {
						normalizedValue = Math.max(0, Math.min(100, jointCmd.value));
					} else {
						normalizedValue = Math.max(-100, Math.min(100, jointCmd.value));
					}

					console.debug(
						`[Robot ${this.id}] Joint ${jointCmd.name}: ${jointCmd.value} -> ${normalizedValue} (normalized)`
					);

					// Create a new joint object to ensure reactivity
					this.joints[jointCmd.name] = { ...joint, value: normalizedValue };
				} else {
					console.warn(`[Robot ${this.id}] Joint ${jointCmd.name} not found`);
				}
			});

			// Send normalized command to producers
			this.sendToProducers(command);
		} finally {
			this.commandMutex = false;

			// Periodic cleanup to prevent memory leaks
			const now = Date.now();
			if (now - this.lastCleanup > ROBOT_CONFIG.performance.memoryCleanupInterval) {
				// Clear old command values that haven't been updated recently
				Object.keys(this.lastCommandValues).forEach((jointName) => {
					if (now - this.lastCommandTime > ROBOT_CONFIG.performance.memoryCleanupInterval) {
						delete this.lastCommandValues[jointName];
					}
				});
				this.lastCleanup = now;
			}

			// Process any pending commands
			if (this.pendingCommands.length > 0) {
				const nextCommand = this.pendingCommands.shift();
				if (nextCommand) {
					// Use setTimeout to prevent stack overflow with rapid commands
					setTimeout(() => this.executeCommand(nextCommand), 0);
				}
			}
		}
	}

	// Consumer management (input driver) - SINGLE consumer only
	async setConsumer(config: USBDriverConfig | RemoteDriverConfig): Promise<string> {
		return this._setConsumer(config, false);
	}

	// Join existing room as consumer (for Inference Session integration)
	async joinAsConsumer(config: RemoteDriverConfig): Promise<string> {
		if (config.type !== "remote") {
			throw new Error("joinAsConsumer only supports remote drivers");
		}
		return this._setConsumer(config, true);
	}

	private async _setConsumer(
		config: USBDriverConfig | RemoteDriverConfig,
		joinExistingRoom: boolean
	): Promise<string> {
		// Remove existing consumer if any
		if (this.consumer) {
			await this.removeConsumer();
		}

		const consumer = this.createConsumer(config);

		// Set up calibration completion callback for USB drivers
		if (USBServoDriver.isUSBDriver(consumer)) {
			const calibrationUnsubscribe = consumer.onCalibrationCompleteWithPositions(
				async (finalPositions: Record<string, number>) => {
					console.log(`[Robot ${this.id}] Calibration complete, syncing robot to final positions`);
					consumer.syncRobotPositions(
						finalPositions,
						(jointName: string, normalizedValue: number) => {
							this.updateJointValue(jointName, normalizedValue, false); // Don't send to producers to avoid feedback loop
						}
					);

					// Start listening now that calibration is complete
					if ("startListening" in consumer && consumer.startListening) {
						try {
							await consumer.startListening();
							console.log(`[Robot ${this.id}] Started listening after calibration completion`);
						} catch (error) {
							console.error(
								`[Robot ${this.id}] Failed to start listening after calibration:`,
								error
							);
						}
					}
				}
			);
			this.unsubscribeFns.push(calibrationUnsubscribe);
		}

		// Only pass joinExistingRoom to remote drivers
		if (config.type === "remote") {
			await (consumer as RemoteConsumer).connect(joinExistingRoom);
		} else {
			await consumer.connect();
		}

		// Set up command listening
		const commandUnsubscribe = consumer.onCommand((command: RobotCommand) => {
			this.executeCommand(command);
		});
		this.unsubscribeFns.push(commandUnsubscribe);

		// Monitor status changes
		const statusUnsubscribe = consumer.onStatusChange(() => {
			this.updateStates();
		});
		this.unsubscribeFns.push(statusUnsubscribe);

		// Start listening for consumers with this capability (only if calibrated for USB)
		if ("startListening" in consumer && consumer.startListening) {
			// For USB consumers, only start listening if calibrated
			if (USBServoDriver.isUSBDriver(consumer)) {
				if (consumer.isCalibrated) {
					await consumer.startListening();
				}
				// If not calibrated, startListening will be called after calibration completion
			} else {
				// For non-USB consumers, start listening immediately
				await consumer.startListening();
			}
		}

		this.consumer = consumer;
		this.updateStates();

		return consumer.id;
	}

	// Producer management (output drivers) - MULTIPLE allowed
	async addProducer(config: USBDriverConfig | RemoteDriverConfig): Promise<string> {
		return this._addProducer(config, false);
	}

	// Join existing room as producer (for Inference Session integration)
	async joinAsProducer(config: RemoteDriverConfig): Promise<string> {
		if (config.type !== "remote") {
			throw new Error("joinAsProducer only supports remote drivers");
		}
		return this._addProducer(config, true);
	}

	private async _addProducer(
		config: USBDriverConfig | RemoteDriverConfig,
		joinExistingRoom: boolean
	): Promise<string> {
		const producer = this.createProducer(config);

		// Set up calibration completion callback for USB drivers
		if (USBServoDriver.isUSBDriver(producer)) {
			const calibrationUnsubscribe = producer.onCalibrationCompleteWithPositions(
				async (finalPositions: Record<string, number>) => {
					console.log(`[Robot ${this.id}] Calibration complete, syncing robot to final positions`);
					producer.syncRobotPositions(
						finalPositions,
						(jointName: string, normalizedValue: number) => {
							this.updateJointValue(jointName, normalizedValue, false); // Don't send to producers to avoid feedback loop
						}
					);

					console.log(
						`[Robot ${this.id}] USB Producer calibration completed and ready for commands`
					);
				}
			);
			this.unsubscribeFns.push(calibrationUnsubscribe);
		}

		// Only pass joinExistingRoom to remote drivers
		if (config.type === "remote") {
			await (producer as RemoteProducer).connect(joinExistingRoom);
		} else {
			await producer.connect();
		}

		// Monitor status changes
		const statusUnsubscribe = producer.onStatusChange(() => {
			this.updateStates();
		});
		this.unsubscribeFns.push(statusUnsubscribe);

		this.producers.push(producer);
		this.updateStates();

		return producer.id;
	}

	async removeConsumer(): Promise<void> {
		if (this.consumer) {
			// Stop listening for consumers with this capability
			if ("stopListening" in this.consumer && this.consumer.stopListening) {
				await this.consumer.stopListening();
			}
			await this.consumer.disconnect();

			this.consumer = null;
			this.updateStates();
		}
	}

	async removeProducer(driverId: string): Promise<void> {
		const driverIndex = this.producers.findIndex((d) => d.id === driverId);
		if (driverIndex >= 0) {
			const driver = this.producers[driverIndex];
			await driver.disconnect();

			this.producers.splice(driverIndex, 1);
			this.updateStates();
		}
	}

	// Private methods
	private createConsumer(config: USBDriverConfig | RemoteDriverConfig): Consumer {
		switch (config.type) {
			case "usb":
				return new USBConsumer(config);
			case "remote":
				return new RemoteConsumer(config);
			default:
				const _exhaustive: never = config;
				throw new Error(`Unknown consumer type: ${JSON.stringify(_exhaustive)}`);
		}
	}

	private createProducer(config: USBDriverConfig | RemoteDriverConfig): Producer {
		switch (config.type) {
			case "usb":
				return new USBProducer(config);
			case "remote":
				return new RemoteProducer(config);
			default:
				const _exhaustive: never = config;
				throw new Error(`Unknown producer type: ${JSON.stringify(_exhaustive)}`);
		}
	}

	// Convert normalized values to URDF radians for 3D visualization
	convertNormalizedToUrdfRadians(jointName: string, normalizedValue: number): number {
		const joint = this.joints[jointName];
		if (!joint?.limits || joint.limits.lower === undefined || joint.limits.upper === undefined) {
			// Default ranges
			if (jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper") {
				return (normalizedValue / 100) * Math.PI;
			} else {
				return (normalizedValue / 100) * Math.PI;
			}
		}

		const { lower, upper } = joint.limits;

		// Map normalized value to URDF range
		let normalizedRatio: number;
		if (jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper") {
			normalizedRatio = normalizedValue / 100; // 0-100 -> 0-1
		} else {
			normalizedRatio = (normalizedValue + 100) / 200; // -100-+100 -> 0-1
		}

		const urdfRadians = lower + normalizedRatio * (upper - lower);

		console.debug(
			`[Robot ${this.id}] Joint ${jointName}: ${normalizedValue} (norm) -> ${urdfRadians.toFixed(3)} (rad)`
		);

		return urdfRadians;
	}

	private async sendToProducers(command: RobotCommand): Promise<void> {
		const connectedProducers = this.producers.filter((d) => d.status.isConnected);

		console.debug(
			`[Robot ${this.id}] Sending command to ${connectedProducers.length} producers:`,
			command
		);

		// Send to all connected producers
		await Promise.all(
			connectedProducers.map(async (producer) => {
				try {
					await producer.sendCommand(command);
				} catch (error) {
					console.error(
						`[Robot ${this.id}] Failed to send command to producer ${producer.id}:`,
						error
					);
				}
			})
		);
	}

	private updateStates(): void {
		// Update connection status
		const hasConnectedDrivers =
			this.consumer?.status.isConnected || this.producers.some((d) => d.status.isConnected);

		this.connectionStatus = {
			isConnected: hasConnectedDrivers,
			lastConnected: hasConnectedDrivers ? new Date() : this.connectionStatus.lastConnected
		};

		// Manual control is enabled when no connected consumer
		this.isManualControlEnabled = !this.consumer?.status.isConnected;
	}

	// Cleanup
	async destroy(): Promise<void> {
		// Unsubscribe from all callbacks
		this.unsubscribeFns.forEach((fn) => fn());
		this.unsubscribeFns = [];

		// Disconnect all drivers
		const allDrivers = [this.consumer, ...this.producers].filter(Boolean) as (
			| Consumer
			| Producer
		)[];
		await Promise.allSettled(
			allDrivers.map(async (driver) => {
				try {
					await driver.disconnect();
				} catch (error) {
					console.error(`Error disconnecting driver ${driver.id}:`, error);
				}
			})
		);

		// Calibration cleanup is handled by individual USB drivers
	}
}