src/lib/robot/drivers/USBMaster.ts

import type { 
  MasterDriver, 
  ConnectionStatus, 
  RobotCommand, 
  CommandSequence,
  USBMasterConfig,
  CommandCallback,
  SequenceCallback,
  StatusChangeCallback,
  UnsubscribeFn
} from '$lib/types/robotDriver';
import { getRobotPollingConfig, getDataProcessingConfig } from '$lib/configs/performanceConfig';

/**
 * USB Master Driver
 * Reads positions from a physical USB robot and forwards movements to slaves
 * Allows physical robot to act as a master controller for digital twins and other robots
 */
export class USBMaster implements MasterDriver {
  readonly type = "master" as const;
  readonly id: string;
  readonly name: string;
  
  private _status: ConnectionStatus = { isConnected: false };
  private config: USBMasterConfig;
  
  // Robot joint configuration
  private jointIds: number[] = [1, 2, 3, 4, 5, 6]; // Default servo IDs for so-arm100
  private jointNames: string[] = ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll", "Jaw"];
  private lastPositions: Map<number, number> = new Map();
  private restPositions: Map<number, number> = new Map(); // Store initial rest positions
  private positionHistory: Map<number, number[]> = new Map();
  
  // Polling control
  private pollingInterval?: number;
  private isPolling = false;
  private isPaused = false;
  private isReading = false; // Prevent concurrent reads
  
  // Event callbacks
  private commandCallbacks: CommandCallback[] = [];
  private sequenceCallbacks: SequenceCallback[] = [];
  private statusCallbacks: StatusChangeCallback[] = [];
  
  // Import the unlock SDK dynamically
  private sdk: typeof import('$lib/feetech.js/scsServoSDKUnlock.mjs') | null = null;

  constructor(config: USBMasterConfig) {
    this.config = config;
    this.id = `usb-master-${Date.now()}`;
    this.name = `USB Master Robot`;
    
    // Initialize position tracking
    this.jointIds.forEach((id) => {
      this.lastPositions.set(id, 2048); // Center position (will be updated on connect)
      this.restPositions.set(id, 2048); // Rest position baseline (will be updated on connect)
      this.positionHistory.set(id, []);
    });
    
    console.log(`🔌 Created USBMaster with ${this.jointIds.length} joints (unlock mode)`);
  }

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

  async connect(): Promise<void> {
    console.log(`🔌 Connecting ${this.name} to USB robot...`);
    
    try {
      // Dynamically import the unlock SDK
      this.sdk = await import('$lib/feetech.js/scsServoSDKUnlock.mjs');
      
      // Connect to USB robot
      await this.sdk.connect({
        baudRate: this.config.baudRate || 1000000,
        protocolEnd: 0
      });
      
      // Read initial positions
      await this.readInitialPositions();
      
      this._status = { 
        isConnected: true, 
        lastConnected: new Date(),
        error: undefined 
      };
      this.notifyStatusChange();
      
      console.log(`✅ ${this.name} connected successfully`);
      
    } catch (error) {
      this._status = { 
        isConnected: false, 
        error: `Connection failed: ${error}` 
      };
      this.notifyStatusChange();
      throw error;
    }
  }

  async disconnect(): Promise<void> {
    console.log(`🔌 Disconnecting ${this.name}...`);
    
    this.stopPolling();
    
    if (this.sdk && this._status.isConnected) {
      try {
        await this.sdk.disconnect();
      } catch (error) {
        console.warn("Error disconnecting USB robot:", error);
      }
    }
    
    this._status = { isConnected: false };
    this.notifyStatusChange();
    
    console.log(`✅ ${this.name} disconnected`);
  }

  async start(): Promise<void> {
    if (!this._status.isConnected) {
      throw new Error("Cannot start: USB master not connected");
    }
    
    console.log(`🎮 Starting USB master polling...`);
    this.startPolling();
  }

  async stop(): Promise<void> {
    console.log(`⏹️ Stopping USB master polling...`);
    this.stopPolling();
  }

  async pause(): Promise<void> {
    console.log(`⏸️ Pausing USB master polling...`);
    this.isPaused = true;
  }

  async resume(): Promise<void> {
    console.log(`▶️ Resuming USB master polling...`);
    this.isPaused = false;
  }

  // Event subscription methods
  onCommand(callback: CommandCallback): UnsubscribeFn {
    this.commandCallbacks.push(callback);
    return () => {
      const index = this.commandCallbacks.indexOf(callback);
      if (index >= 0) {
        this.commandCallbacks.splice(index, 1);
      }
    };
  }

  onSequence(callback: SequenceCallback): UnsubscribeFn {
    this.sequenceCallbacks.push(callback);
    return () => {
      const index = this.sequenceCallbacks.indexOf(callback);
      if (index >= 0) {
        this.sequenceCallbacks.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 async readInitialPositions(): Promise<void> {
    if (!this.sdk) return;
    
    try {
      console.log("📖 Reading initial REST positions from USB robot (individual reads)...");
      
      let successCount = 0;
      for (const servoId of this.jointIds) {
        try {
          const position = await this.sdk.readPosition(servoId);
          
          // Store as both last position and rest position baseline
          this.lastPositions.set(servoId, position);
          this.restPositions.set(servoId, position);
          
          console.log(`Joint ${servoId}: REST position = ${position} (servo units)`);
          successCount++;
        } catch (err) {
          console.warn(`Failed to read servo ${servoId}:`, err);
          // Keep default center positions for failed reads
        }
      }
      
      console.log(`📖 Successfully read ${successCount}/${this.jointIds.length} REST positions`);
      console.log("🏠 Robot is now calibrated to REST position baseline");
      
    } catch (error) {
      console.warn("Could not read initial positions:", error);
      console.log("🏠 Using default center positions as REST baseline");
    }
  }

  private startPolling(): void {
    if (this.isPolling) return;
    
    this.isPolling = true;
    // Use performance config instead of hardcoded value
    const pollInterval = this.config.pollInterval || getRobotPollingConfig().USB_MASTER_POLL_INTERVAL_MS;
    
    this.pollingInterval = setInterval(async () => {
      if (this.isPaused || !this._status.isConnected || this.isReading) return;
      
      try {
        await this.pollRobotPositions();
      } catch (error) {
        console.error("Error polling robot positions:", error);
        
        // Handle connection errors
        if (error instanceof Error && error.message?.includes('Not connected')) {
          this._status = { 
            isConnected: false, 
            error: `Connection lost: ${error.message}` 
          };
          this.notifyStatusChange();
          this.stopPolling();
        }
      }
    }, pollInterval);
    
    console.log(`🔄 USB master polling started (${pollInterval}ms interval)`);
  }

  private stopPolling(): void {
    if (this.pollingInterval) {
      clearInterval(this.pollingInterval);
      this.pollingInterval = undefined;
    }
    this.isPolling = false;
  }

  private async pollRobotPositions(): Promise<void> {
    if (!this.sdk || this.isReading) return;
    
    this.isReading = true; // Prevent concurrent reads
    
    try {
      const currentPositions: Map<number, number> = new Map();
      const changedJoints: { name: string; value: number }[] = [];
      let validReads = 0;
      
      // Read all servo positions
      for (const servoId of this.jointIds) {
        try {
          const position = await this.sdk.readPosition(servoId);
          if (position !== null && position !== undefined) {
            currentPositions.set(servoId, position);
            validReads++;
          }
        } catch {
          // Skip individual servo read errors
        }
      }
      
      currentPositions.forEach((position: number, servoId: number) => {
        // Process all readings - position 0 is valid
        const lastPosition = this.lastPositions.get(servoId) || 2048;
        // Use performance config for threshold
        const threshold = getDataProcessingConfig().ENABLE_SMOOTHING 
          ? getRobotPollingConfig().POSITION_CHANGE_THRESHOLD 
          : 1; // Maximum sensitivity when smoothing disabled
        
        if (Math.abs(position - lastPosition) > threshold) {
          const jointIndex = this.jointIds.indexOf(servoId);
          if (jointIndex >= 0) {
            const jointName = this.jointNames[jointIndex];
            const degrees = this.servoToRelativeDegrees(position, servoId);
            
            console.log(`🎯 Joint ${jointName} moved: ${lastPosition} → ${position} (${degrees.toFixed(1)}°)`);
            
            // Apply smoothing if enabled in performance config
            if (this.config.smoothing && getDataProcessingConfig().ENABLE_SMOOTHING) {
              const history = this.positionHistory.get(servoId) || [];
              history.push(degrees);
              const maxHistory = getDataProcessingConfig().SMOOTHING_HISTORY_SIZE;
              if (history.length > maxHistory) history.shift();
              this.positionHistory.set(servoId, history);
              
              // Use average of recent positions
              const smoothedDegrees = history.reduce((a, b) => a + b, 0) / history.length;
              changedJoints.push({ name: jointName, value: smoothedDegrees });
            } else {
              changedJoints.push({ name: jointName, value: degrees });
            }
            
            this.lastPositions.set(servoId, position);
          }
        }
      });
      
      // Log polling status occasionally (less frequent with faster polling)
      if (Date.now() % 5000 < getRobotPollingConfig().USB_MASTER_POLL_INTERVAL_MS) {
        console.log(`🔄 USB polling: ${validReads}/${this.jointIds.length} joints read successfully`);
      }
      
      // If there are changes, emit command
      if (changedJoints.length > 0) {
        const command: RobotCommand = {
          timestamp: Date.now(),
          joints: changedJoints,
          metadata: { source: "usb_master_polling" }
        };
        
        console.log(`📤 Emitting command with ${changedJoints.length} joint changes`);
        this.notifyCommand([command]);
      }
      
    } catch (error) {
      console.error("Polling error:", error);
      
      // Don't re-throw port access errors, just log them
      if (error instanceof Error && error.message?.includes('Releasing Default reader')) {
        console.warn("Serial port access conflict, skipping this poll cycle");
        return;
      }
      
      throw error; // Re-throw other errors
    } finally {
      // Always release the reading lock
      this.isReading = false;
    }
  }

  private servoToRelativeDegrees(servoPosition: number, servoId: number): number {
    // Get the rest position for this servo
    const restPosition = this.restPositions.get(servoId) || 2048;
    
    // Calculate relative movement from rest position
    const deltaServoUnits = servoPosition - restPosition;
    
    // Convert servo units to degrees (assuming full range 0-4095 = 360°)
    const relativeDegrees = (deltaServoUnits / 4095) * 360;
    
    return relativeDegrees;
  }

  private notifyCommand(commands: RobotCommand[]): void {
    this.commandCallbacks.forEach(callback => {
      try {
        callback(commands);
      } catch (error) {
        console.error("Error in command callback:", error);
      }
    });
  }

  private notifySequence(sequence: CommandSequence): void {
    this.sequenceCallbacks.forEach(callback => {
      try {
        callback(sequence);
      } catch (error) {
        console.error("Error in sequence callback:", error);
      }
    });
  }

  private notifyStatusChange(): void {
    this.statusCallbacks.forEach(callback => {
      try {
        callback(this._status);
      } catch (error) {
        console.error("Error in status callback:", error);
      }
    });
  }
}