src/lib/robot/RobotManager.svelte.ts

import { Robot, type ManagedJointState } from "./Robot.svelte";
import type {
	MasterDriver,
	SlaveDriver,
	DriverJointState,
	MasterDriverConfig,
	SlaveDriverConfig
} from "$lib/types/robotDriver";
import { MockSequenceMaster, DEMO_SEQUENCES } from "./drivers/MockSequenceMaster";
import { MockSlave } from "./drivers/MockSlave";
import { USBSlave } from "./drivers/USBSlave";
import { RemoteServerMaster } from "./drivers/RemoteServerMaster";
import { RemoteServerSlave } from "./drivers/RemoteServerSlave";
import { USBMaster } from "./drivers/USBMaster";
import { createRobot } from "@/components/3d/robot/URDF/createRobot.svelte";
import type { RobotUrdfConfig } from "$lib/types/urdf";
import {
	getCommunicationConfig,
	getRobotPollingConfig,
	getDataProcessingConfig
} from "$lib/configs/performanceConfig";

/**
 * Central manager for all robots with master-slave architecture
 *
 * Masters: Command sources (remote servers, scripts, manual control)
 * Slaves: Execution targets (physical robots, simulators)
 */
export class RobotManager {
	private _robots = $state<Robot[]>([]);

	// Reactive getters
	get robots(): Robot[] {
		return this._robots;
	}

	get robotCount(): number {
		return this._robots.length;
	}

	get robotsWithMaster(): Robot[] {
		return this._robots.filter((robot) => robot.master !== undefined);
	}

	get robotsWithSlaves(): Robot[] {
		return this._robots.filter((robot) => robot.slaves.length > 0);
	}

	/**
	 * Create a new robot from URDF configuration
	 */
	async createRobot(id: string, urdfConfig: RobotUrdfConfig): Promise<Robot> {
		// Check if robot already exists
		if (this._robots.find((r) => r.id === id)) {
			throw new Error(`Robot with ID ${id} already exists`);
		}

		// Create robot state from URDF
		const robotState = await createRobot(urdfConfig);

		// Create managed robot
		const robot = new Robot(id, robotState);

		// Add to reactive array
		this._robots.push(robot);

		console.log(`Created robot ${id}. Total robots: ${this._robots.length}`);

		return robot;
	}

	/**
	 * Get robot by ID
	 */
	getRobot(id: string): Robot | undefined {
		return this._robots.find((r) => r.id === id);
	}

	/**
	 * Remove a robot
	 */
	async removeRobot(id: string): Promise<void> {
		const robotIndex = this._robots.findIndex((r) => r.id === id);
		if (robotIndex === -1) return;

		const robot = this._robots[robotIndex];

		// Move to rest position before removal if has connected slaves
		if (robot.connectedSlaves.length > 0) {
			try {
				console.log(`Removing robot ${id}: moving to rest position first`);
				await robot.moveToRestPosition(3000);
				await new Promise((resolve) => setTimeout(resolve, 500));
			} catch (error) {
				console.warn(`Failed to move robot ${id} to rest position before removal:`, error);
			}
		}

		// Clean up robot resources
		await robot.destroy();

		// Remove from reactive array
		this._robots.splice(robotIndex, 1);

		console.log(`Removed robot ${id}. Remaining robots: ${this._robots.length}`);
	}

	// ============= MASTER MANAGEMENT =============

	/**
	 * Connect a master driver to a robot
	 */
	async connectMaster(robotId: string, masterConfig: MasterDriverConfig): Promise<void> {
		const robot = this._robots.find((r) => r.id === robotId);
		if (!robot) {
			throw new Error(`Robot ${robotId} not found`);
		}

		// Create master driver instance
		const master = this.createMaster(masterConfig, robotId);

		// Connect the master
		await master.connect();

		// Attach to robot
		await robot.setMaster(master);

		console.log(`Master ${master.name} connected to robot ${robotId}`);
	}

	/**
	 * Disconnect a robot's master
	 */
	async disconnectMaster(robotId: string): Promise<void> {
		const robot = this._robots.find((r) => r.id === robotId);
		if (!robot) {
			throw new Error(`Robot ${robotId} not found`);
		}

		await robot.removeMaster();
		console.log(`Master disconnected from robot ${robotId}. Manual control restored.`);
	}

	// ============= SLAVE MANAGEMENT =============

	/**
	 * Connect a slave driver to a robot
	 */
	async connectSlave(robotId: string, slaveConfig: SlaveDriverConfig): Promise<void> {
		const robot = this._robots.find((r) => r.id === robotId);
		if (!robot) {
			throw new Error(`Robot ${robotId} not found`);
		}

		// Create slave driver instance
		const slave = this.createSlave(slaveConfig, robot);

		// Add to robot (this handles connection and initialization)
		await robot.addSlave(slave);

		console.log(`Slave ${slave.name} connected to robot ${robotId}`);
	}

	/**
	 * Disconnect a slave driver from a robot
	 */
	async disconnectSlave(robotId: string, slaveId: string): Promise<void> {
		const robot = this._robots.find((r) => r.id === robotId);
		if (!robot) {
			throw new Error(`Robot ${robotId} not found`);
		}

		await robot.removeSlave(slaveId);
		console.log(`Slave ${slaveId} disconnected from robot ${robotId}`);
	}

	// ============= CONVENIENCE METHODS =============

	/**
	 * Connect demo sequence master to a robot
	 */
	async connectDemoSequences(robotId: string, loopMode: boolean = true): Promise<void> {
		const config: MasterDriverConfig = {
			type: "mock-sequence",
			sequences: DEMO_SEQUENCES,
			autoStart: true,
			loopMode
		};

		await this.connectMaster(robotId, config);
	}

	/**
	 * Connect mock slave to a robot
	 */
	async connectMockSlave(robotId: string, simulateLatency: number = 50): Promise<void> {
		const config: SlaveDriverConfig = {
			type: "mock-slave",
			simulateLatency,
			simulateErrors: false,
			responseDelay: 20
		};

		await this.connectSlave(robotId, config);
	}

	/**
	 * Connect USB slave to a robot (when implemented)
	 */
	async connectUSBSlave(robotId: string, port?: string): Promise<void> {
		const config: SlaveDriverConfig = {
			type: "usb-slave",
			port,
			baudRate: 115200
		};

		await this.connectSlave(robotId, config);
	}

	/**
	 * Connect remote server slave to a robot
	 */
	async connectRemoteServerSlave(
		robotId: string,
		url: string = "ws://localhost:8080",
		apiKey?: string,
		targetRobotId?: string
	): Promise<void> {
		const config: SlaveDriverConfig = {
			type: "remote-server-slave",
			url,
			apiKey,
			robotId: targetRobotId || robotId // Use targetRobotId if provided, otherwise use local robotId
		};

		await this.connectSlave(robotId, config);
	}

	/**
	 * Connect USB master to a robot
	 */
	async connectUSBMaster(
		robotId: string,
		options: { port?: string; baudRate?: number; pollInterval?: number; smoothing?: boolean } = {}
	): Promise<void> {
		const config: MasterDriverConfig = {
			type: "usb-master",
			port: options.port,
			baudRate: options.baudRate || getCommunicationConfig().USB_BAUD_RATE,
			pollInterval: options.pollInterval || getRobotPollingConfig().USB_MASTER_POLL_INTERVAL_MS,
			smoothing: options.smoothing ?? getDataProcessingConfig().ENABLE_SMOOTHING
		};

		await this.connectMaster(robotId, config);
	}

	/**
	 * Get detailed robot status
	 */
	getRobotStatus(robotId: string):
		| {
				id: string;
				hasActiveMaster: boolean;
				masterName?: string;
				manualControlEnabled: boolean;
				connectedSlaves: number;
				totalSlaves: number;
				lastCommandSource: string;
		  }
		| undefined {
		const robot = this._robots.find((r) => r.id === robotId);
		if (!robot) return undefined;

		return {
			id: robot.id,
			hasActiveMaster: robot.controlState.hasActiveMaster,
			masterName: robot.controlState.masterName,
			manualControlEnabled: robot.manualControlEnabled,
			connectedSlaves: robot.connectedSlaves.length,
			totalSlaves: robot.slaves.length,
			lastCommandSource: robot.controlState.lastCommandSource
		};
	}

	/**
	 * Get joint states from all robots
	 */
	getAllJointStates(): { robotId: string; joints: ManagedJointState[] }[] {
		return this._robots.map((robot) => ({
			robotId: robot.id,
			joints: robot.joints
		}));
	}

	/**
	 * Clean up all robots
	 */
	async destroy(): Promise<void> {
		const cleanupPromises = this._robots.map((robot) => robot.destroy());
		await Promise.allSettled(cleanupPromises);
		this._robots.length = 0;
	}

	// ============= DRIVER FACTORIES =============

	/**
	 * Create a master driver instance
	 */
	private createMaster(config: MasterDriverConfig, robotId: string): MasterDriver {
		switch (config.type) {
			case "mock-sequence":
				return new MockSequenceMaster(config);

			case "remote-server":
				return new RemoteServerMaster(config, robotId);

			case "script-player":
				// TODO: Implement ScriptPlayerMaster
				throw new Error("Script player master not implemented yet");

			case "usb-master":
				return new USBMaster(config);

			default: {
				// TypeScript exhaustiveness check
				const _exhaustive: never = config;
				throw new Error(
					`Unknown master driver type: ${(_exhaustive as unknown as { type: string }).type}`
				);
			}
		}
	}

	/**
	 * Create a slave driver instance
	 */
	private createSlave(config: SlaveDriverConfig, robot: Robot): SlaveDriver {
		// Convert robot joints to driver joint states
		const driverJointStates: DriverJointState[] = robot.joints.map((joint) => ({
			name: joint.name,
			servoId: joint.servoId || 0,
			type: joint.urdfJoint.type as "revolute" | "continuous",
			virtualValue: joint.virtualValue,
			realValue: joint.realValue,
			limits: joint.urdfJoint.limit
				? {
						lower: joint.urdfJoint.limit.lower,
						upper: joint.urdfJoint.limit.upper,
						velocity: joint.urdfJoint.limit.velocity,
						effort: joint.urdfJoint.limit.effort
					}
				: undefined
		}));

		switch (config.type) {
			case "mock-slave":
				return new MockSlave(config, driverJointStates);

			case "usb-slave":
				return new USBSlave(config, driverJointStates);

			case "simulation-slave":
				// TODO: Implement SimulationSlave
				throw new Error("Simulation slave driver not implemented yet");

			case "remote-server-slave":
				return new RemoteServerSlave(config, driverJointStates);

			default: {
				// TypeScript exhaustiveness check
				const _exhaustive: never = config;
				throw new Error(
					`Unknown slave driver type: ${(_exhaustive as unknown as { type: string }).type}`
				);
			}
		}
	}
}

// Global robot manager instance
export const robotManager = new RobotManager();