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 { RemoteServerMaster } from './drivers/RemoteServerMaster'; // TODO: Implement
import { createRobot } from '$lib/components/scene/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();