src/lib/elements/robot/drivers/USBProducer.ts

import type { Producer, RobotCommand } from '../models.js';
import { ROBOT_CONFIG } from '../config.js';
import { USBServoDriver } from './USBServoDriver.js';

export class USBProducer extends USBServoDriver implements Producer {
  private commandQueue: RobotCommand[] = [];
  private isProcessingCommands = false;

  constructor(config: any) {
    super(config, 'Producer');
  }

  async connect(): Promise<void> {
    // Connect to USB first (this triggers browser's device selection dialog)
    await this.connectToUSB();
    
    // Lock servos for software control (producer mode)
    await this.lockAllServos();
    
    // Note: Calibration is checked when operations are actually needed
  }

  async disconnect(): Promise<void> {
    // Stop command processing
    this.isProcessingCommands = false;
    this.commandQueue = [];

    await this.disconnectFromUSB();
  }

  async sendCommand(command: RobotCommand): Promise<void> {
    if (!this._status.isConnected) {
      throw new Error(`[${this.name}] Cannot send command: not connected`);
    }

    if (!this.isCalibrated) {
      throw new Error(`[${this.name}] Cannot send command: not calibrated`);
    }

    // Add command to queue for processing
    this.commandQueue.push(command);
    
    // Limit queue size to prevent memory issues
    if (this.commandQueue.length > ROBOT_CONFIG.commands.maxQueueSize) {
      this.commandQueue.shift(); // Remove oldest command
    }

    // Start processing if not already running
    if (!this.isProcessingCommands) {
      this.processCommandQueue();
    }
  }

  // Event handlers already in base class

  // Private methods
  private async processCommandQueue(): Promise<void> {
    if (this.isProcessingCommands || !this._status.isConnected) {
      return;
    }

    this.isProcessingCommands = true;

    while (this.commandQueue.length > 0 && this._status.isConnected) {
      const command = this.commandQueue.shift();
      if (command) {
        try {
          await this.executeCommand(command);
        } catch (error) {
          console.error(`[${this.name}] Command execution failed:`, error);
          // Continue processing other commands
        }
      }
    }

    this.isProcessingCommands = false;
  }

  private async executeCommand(command: RobotCommand): Promise<void> {
    if (!this.scsServoSDK || !this._status.isConnected) {
      return;
    }

    try {
      // Convert normalized values to servo positions using calibration (required)
      const servoCommands = new Map<number, number>();
      
      command.joints.forEach(joint => {
        const servoId = this.jointToServoMap[joint.name as keyof typeof this.jointToServoMap];
        if (servoId) {
          const servoPosition = this.denormalizeValue(joint.value, joint.name);
          
          // Clamp to valid servo range
          const clampedPosition = Math.max(0, Math.min(4095, Math.round(servoPosition)));
          servoCommands.set(servoId, clampedPosition);
        }
      });

      if (servoCommands.size > 0) {
        // Use batch commands when possible for better performance
        if (servoCommands.size > 1) {
          await this.scsServoSDK.syncWritePositions(servoCommands);
        } else {
          // Single servo command
          const entry = servoCommands.entries().next().value;
          if (entry) {
            const [servoId, position] = entry;
            await this.scsServoSDK.writePosition(servoId, position);
          }
        }
        
        console.debug(`[${this.name}] Sent positions to ${servoCommands.size} servos`);
      }
      
    } catch (error) {
      console.error(`[${this.name}] Failed to execute command:`, error);
      throw error;
    }
  }


}