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

export class Robot {
  constructor(name, firmwareVersionId) {
    this.deviceName = name;
    this.firmwareVersion = { major: 0, minor: 0 };

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

  static fromBytes(bytes) {
    console.log("Loading Robot from:");
    console.log(bytes);
    const data = bytes instanceof Uint8Array ? bytes : Uint8Array.from(bytes);
    let offset = 0;

    // Device name
    const nameLen = data[offset++];
    const deviceName = String.fromCharCode(...data.slice(offset, offset + nameLen));
    offset += nameLen;

    // Firmware version
    const firmwareMajor = data[offset++];
    const firmwareMinor = data[offset++];

    // Create Robot instance
    const robot = new Robot(deviceName, `${firmwareMajor}.${firmwareMinor}`);
    robot.firmwareVersion.major = firmwareMajor;
    robot.firmwareVersion.minor = firmwareMinor;

    // Motor count
    const motorCount = data[offset++];

    for (let i = 0; i < motorCount; i++) {
      const motorID = data[offset++];
      const nameLen = data[offset++];
      const name = String.fromCharCode(...data.slice(offset, offset + nameLen));
      offset += nameLen;

      const position = (data[offset++] << 8) | data[offset++];
      const motor = new ServoMotor(0, motorID, null, name);
      robot.assignMotor(motorID, motor); // motorID used as position ID
    }

    return robot;
  };
}