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