#include "motorcontrol.h"

// Global servo manager instance
ServoManager servoManager;

uint16_t flipBytes(uint16_t value) {
  return (value >> 8) | (value << 8);
}

void prepareMotorPositions(
  const uint8_t* ids,
  uint16_t* positions,
  uint16_t* speeds,
  uint8_t count,
  RobotConfig& config
) {
  for (uint8_t i = 0; i < count; i++) {
    if (config.getMotorModel(ids[i]) == MODEL_STS) {
      // STS servos: flip bytes, different speed range
      positions[i] = flipBytes(positions[i]);
      speeds[i] = map(speeds[i], 0, 4095, 0, 254);
      speeds[i] = flipBytes(speeds[i]);
    } else {
      // SCS servos: map to 10-bit range
      positions[i] = map(positions[i], 0, 4095, 0, 1023);
      speeds[i] = map(speeds[i], 0, 4095, 0, 1000);
    }
  }
}

void ServoManager::init() {
  servos[0] = new Feetech(Serial1, Pins::DE, Pins::RE, Pins::CH0_TX, Pins::CH0_RX);
  servos[0]->setFeetechMode(Feetech::MODE_SCS);
  servos[0]->begin();
  
  servos[1] = new Feetech(Serial2, Pins::DE, Pins::RE, Pins::CH1_TX, Pins::CH1_RX);
  servos[1]->setFeetechMode(Feetech::MODE_SCS);
  // servos[1]->begin();  // Uncomment when using channel 1
}

void ServoManager::syncWritePositions(
  const uint8_t* ids,
  uint16_t* positions,
  uint16_t* speeds,
  uint8_t count,
  RobotConfig& config,
  uint8_t channel
) {
  // Prepare positions (handles SCS vs STS conversion)
  prepareMotorPositions(ids, positions, speeds, count, config);
  
  // Send to servos
  servos[channel]->syncWritePos(
    const_cast<uint8_t*>(ids),
    positions,
    speeds,
    count
  );
}