import { getModelType, 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();
    this.motors = [];
  }

  // 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);
    this.motors.push(motor);
  }

  getMotor(motorID) {
  for (let i = 0; i < this.motors.length; i++) {
    console.log(motorID, this.motors[i].ID);
    if (String(this.motors[i].ID) === String(motorID)) {
      return this.motors[i];
    }
  }
  return null;
}


  // Remove motor from a position
  removeMotor(motorID) {
    this.motors = this.motors.filter(motor => motor.ID !== motorID);
  }


  // 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 modelMajor = data[offset++];
      const modelMinor = data[offset++];
      const modelType = getModelType(modelMajor, modelMinor);

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

    return robot;
  };
}