src/lib/robot/drivers/MockSlave.ts

import type {
	SlaveDriver,
	DriverJointState,
	ConnectionStatus,
	RobotCommand,
	MockSlaveConfig,
	StateUpdateCallback,
	StatusChangeCallback,
	UnsubscribeFn
} from "$lib/types/robotDriver";
import { getRobotPollingConfig } from "$lib/configs/performanceConfig";

/**
 * Mock Slave Driver
 * Simulates a physical robot for testing slave command execution
 */
export class MockSlave implements SlaveDriver {
	readonly type = "slave" as const;
	readonly id: string;
	readonly name: string;

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

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

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

	// Simulation control
	private simulationIntervalId?: number;

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

		// Initialize joint states
		this.jointStates = initialJointStates.map((state) => ({
			...state,
			virtualValue: state.virtualValue,
			realValue: state.virtualValue // Mock starts with perfect sync
		}));

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

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

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

		// Simulate connection delay
		if (this.config.simulateLatency) {
			await new Promise((resolve) => setTimeout(resolve, this.config.simulateLatency));
		}

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

		// Start simulation loop
		this.startSimulation();

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

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

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

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

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

		// Simulate command error if configured
		if (this.config.simulateErrors && Math.random() < 0.1) {
			throw new Error("Simulated command execution error");
		}

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

		// Apply joint updates
		for (const jointUpdate of command.joints) {
			const joint = this.jointStates.find((j) => j.name === jointUpdate.name);
			if (joint) {
				joint.virtualValue = jointUpdate.value;
				// Mock robot has perfect response (real = virtual)
				joint.realValue = jointUpdate.value;
			}
		}

		// Simulate response delay
		if (this.config.responseDelay) {
			await new Promise((resolve) => setTimeout(resolve, this.config.responseDelay));
		}

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

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

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

			// Small delay between commands
			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: slave not connected");
		}

		// Simulate read latency
		if (this.config.simulateLatency) {
			await new Promise((resolve) => setTimeout(resolve, this.config.simulateLatency));
		}

		return [...this.jointStates];
	}

	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 startSimulation(): void {
		if (this.simulationIntervalId) {
			clearInterval(this.simulationIntervalId);
		}

		// Use performance config for update rate
		this.simulationIntervalId = setInterval(() => {
			this.notifyStateUpdate();
		}, getRobotPollingConfig().STATE_UPDATE_INTERVAL_MS);
	}

	private stopSimulation(): void {
		if (this.simulationIntervalId) {
			clearInterval(this.simulationIntervalId);
			this.simulationIntervalId = undefined;
		}
	}

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