import { ServoMotor } from './feetechDefinitions.js';

export class Robot {
  constructor(name, firmwareVersionId) {
    this.name = name;
    this.firmwareVersionId = firmwareVersionId;

    // Map of position ID → ServoMotor
    this.positionMap = new Map();
  }

  // Assign a motor to a position
  assignMotor(positionId, motor) {
    if (!(motor instanceof ServoMotor)) {
      throw new Error('Assigned motor must be a ServoMotor instance');
    }
    this.positionMap.set(positionId, motor);
  }

  // Get motor assigned to a position
  getMotor(positionId) {
    return this.positionMap.get(positionId);
  }

  // Remove motor from a position
  removeMotor(positionId) {
    this.positionMap.delete(positionId);
  }

  // Get all position assignments
  getAllAssignments() {
    const assignments = [];
    for (const [position, motor] of this.positionMap.entries()) {
      assignments.push({ position, motor });
    }
    return assignments;
  }

  // Optional: Find position by motor ID
  findPositionByMotorId(id) {
    for (const [position, motor] of this.positionMap.entries()) {
      if (motor.ID === id) return position;
    }
    return null;
  }
}