#pragma once
#include <Arduino.h>
#include "feetech.h"
#include "robotconfig.h"

// Pin definitions
namespace Pins {
  // Channel 0 (SCS servos)
  constexpr int CH0_RX = 13;
  constexpr int CH0_TX = 12;
  
  // Channel 1 (STS servos)  
  constexpr int CH1_RX = 11;
  constexpr int CH1_TX = 10;
  
  // RS485 control
  constexpr int DE = 7;  // Driver Enable
  constexpr int RE = 8;  // Receiver Enable
}

// Servo model constants
constexpr uint8_t MODEL_STS = 9;

// Utility functions
uint16_t flipBytes(uint16_t value);

// Prepare motor positions for transmission (handles SCS vs STS byte ordering)
void prepareMotorPositions(
  const uint8_t* ids,
  uint16_t* positions,
  uint16_t* speeds,
  uint8_t count,
  RobotConfig& config
);

// Servo manager class for cleaner access
class ServoManager {
public:
  void init();
  
  Feetech* channel(uint8_t ch) { return servos[ch]; }
  Feetech* operator[](uint8_t ch) { return servos[ch]; }
  
  // Convenience methods
  void syncWritePositions(
    const uint8_t* ids,
    uint16_t* positions,
    uint16_t* speeds,
    uint8_t count,
    RobotConfig& config,
    uint8_t channel = 0
  );

private:
  Feetech* servos[2] = {nullptr, nullptr};
};

// Global servo manager
extern ServoManager servoManager;