src/lib/robot/drivers/USBSlave.ts

import type { 
  SlaveDriver, 
  DriverJointState, 
  ConnectionStatus, 
  RobotCommand,
  USBSlaveConfig,
  StateUpdateCallback,
  StatusChangeCallback,
  UnsubscribeFn
} from '$lib/types/robotDriver';
import { scsServoSDK } from '$lib/feetech.js';
import { servoPositionToAngle, degreesToServoPosition } from '$lib/utils';

/**
 * USB Slave Driver
 * Controls physical robots via feetech.js USB connection
 */
export class USBSlave implements SlaveDriver {
  readonly type = "slave" as const;
  readonly id: string;
  readonly name: string;
  
  private _status: ConnectionStatus = { isConnected: false };
  private config: USBSlaveConfig;
  
  // Joint states
  private jointStates: DriverJointState[] = [];
  private initialPositions: number[] = [];
  
  // Event callbacks
  private stateCallbacks: StateUpdateCallback[] = [];
  private statusCallbacks: StatusChangeCallback[] = [];

  constructor(config: USBSlaveConfig, initialJointStates: DriverJointState[]) {
    this.config = config;
    this.id = `usb-slave-${Date.now()}`;
    this.name = `USB Slave Robot`;
    
    // Initialize joint states
    this.jointStates = initialJointStates.map(state => ({ ...state }));
    
    // Validate that all active joints have servo IDs
    const missingServos = this.jointStates.filter(j => !j.servoId);
    if (missingServos.length > 0) {
      console.warn(
        `USB slave: Some joints missing servo IDs: ${missingServos.map(j => j.name).join(', ')}`
      );
    }
    
    console.log(`Created USBSlave with ${this.jointStates.length} joints`);
  }

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

  async connect(): Promise<void> {
    console.log(`Connecting ${this.name}...`);
    
    try {
      this._status = { isConnected: false };
      this.notifyStatusChange();
      
      await scsServoSDK.connect();
      
      // Initialize each servo
      const initialPositions: number[] = [];
      
      for (const joint of this.jointStates) {
        if (!joint.servoId) {
          initialPositions.push(0);
          continue;
        }

        try {
          if (joint.type === "continuous") {
            // Configure for speed control
            await scsServoSDK.setWheelMode(joint.servoId);
            await scsServoSDK.writeWheelSpeed(joint.servoId, 0);
            initialPositions.push(0);
            
            // Update joint state
            joint.realValue = 0;
          } else {
            // Configure for position control
            await scsServoSDK.setPositionMode(joint.servoId);
            
            // Read initial position
            const servoPosition = await scsServoSDK.readPosition(joint.servoId);
            const positionInDegrees = servoPositionToAngle(servoPosition);
            initialPositions.push(positionInDegrees);
            
            // Enable torque
            await scsServoSDK.writeTorqueEnable(joint.servoId, true);
            
            // Update joint state
            joint.realValue = positionInDegrees;
          }
        } catch (error) {
          console.error(`Failed to initialize servo ${joint.servoId} (${joint.name}):`, error);
          initialPositions.push(0);
          joint.realValue = undefined;
        }
      }
      
      this.initialPositions = initialPositions;
      
      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}...`);
    
    if (!this._status.isConnected) return;

    try {
      // Stop all joints safely
      for (const joint of this.jointStates) {
        if (!joint.servoId) continue;
        
        try {
          if (joint.type === "continuous") {
            await scsServoSDK.writeWheelSpeed(joint.servoId, 0);
          }
          await scsServoSDK.writeTorqueEnable(joint.servoId, false);
        } catch (error) {
          console.error(`Failed to stop servo ${joint.servoId}:`, error);
        }
      }

      await scsServoSDK.disconnect();
      
      // Clear real values
      this.jointStates.forEach(joint => {
        joint.realValue = undefined;
      });
      
      this._status = { isConnected: false };
      this.notifyStatusChange();
      
      console.log(`${this.name} disconnected`);
      
    } catch (error) {
      this._status = { 
        isConnected: false, 
        error: `Disconnect failed: ${error}` 
      };
      this.notifyStatusChange();
      throw error;
    }
  }

  async executeCommand(command: RobotCommand): Promise<void> {
    if (!this._status.isConnected) {
      throw new Error("Cannot execute command: USB slave not connected");
    }
    
    console.log(`USBSlave executing command with ${command.joints.length} joint updates`);
    
    for (const jointUpdate of command.joints) {
      const joint = this.jointStates.find(j => j.name === jointUpdate.name);
      if (!joint || !joint.servoId) continue;

      try {
        if (joint.type === "continuous") {
          await scsServoSDK.writeWheelSpeed(joint.servoId, jointUpdate.value);
          joint.realValue = jointUpdate.value;
        } else {
          // Use relative positioning for revolute joints
          const jointIndex = this.jointStates.indexOf(joint);
          const relativeValue = (this.initialPositions[jointIndex] || 0) + jointUpdate.value;
          
          if (relativeValue >= 0 && relativeValue <= 360) {
            const servoPosition = degreesToServoPosition(relativeValue);
            await scsServoSDK.writePosition(joint.servoId, Math.round(servoPosition));
            joint.realValue = relativeValue;
          } else {
            throw new Error(`Position ${relativeValue}° out of range (0-360°)`);
          }
        }

        // Update virtual value to match what we sent
        joint.virtualValue = jointUpdate.value;
        
      } catch (error) {
        console.error(`Failed to execute command for joint ${joint.name}:`, error);
        joint.realValue = undefined;
        throw error;
      }
    }
    
    // Notify state update
    this.notifyStateUpdate();
  }

  async executeCommands(commands: RobotCommand[]): Promise<void> {
    console.log(`USBSlave executing batch of ${commands.length} commands`);
    
    for (const command of commands) {
      await this.executeCommand(command);
      
      // Small delay between commands to avoid overwhelming servos
      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: USB slave not connected");
    }

    const states: DriverJointState[] = [];
    
    for (let i = 0; i < this.jointStates.length; i++) {
      const joint = this.jointStates[i];
      
      if (!joint.servoId) {
        states.push({ ...joint });
        continue;
      }

      try {
        if (joint.type === "revolute") {
          const servoPosition = await scsServoSDK.readPosition(joint.servoId);
          const positionInDegrees = servoPositionToAngle(servoPosition);
          
          states.push({
            ...joint,
            realValue: positionInDegrees
          });
        } else {
          // For continuous joints, we don't read speed (not available in feetech)
          // Keep the last known value
          states.push({ ...joint });
        }
      } catch (error) {
        console.error(`Failed to read servo ${joint.servoId}:`, error);
        states.push({
          ...joint,
          realValue: undefined
        });
      }
    }

    return states;
  }

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