#ifndef FEETECH_H
#define FEETECH_H

#include <vector>
#include <Arduino.h>
#include <HardwareSerial.h>


// SCS & HLS big-endian (high byte first)
// STS little-endian    (low byte first)

class Feetech {
public:
  Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN);
  void begin();
  void sendPing(uint8_t id);
  uint16_t getModel(uint8_t id);
  uint8_t getID(uint8_t id);
  uint8_t setID(uint8_t id, uint8_t newId);
  uint8_t getBaudRate(uint8_t id);
  uint16_t getMinAngleLimit(uint8_t id);
  uint16_t getMaxAngleLimit(uint8_t id);
  uint8_t getCWDeadZone(uint8_t id);
  uint8_t getCCWDeadZone(uint8_t id);
  uint16_t getOffset(uint8_t id);
  uint8_t getMode(uint8_t id);
  uint8_t getTorqueEnable(uint8_t id);
  uint8_t getAcceleration(uint8_t id);
  uint16_t getGoalPosition(uint8_t id);
  uint16_t getGoalTime(uint8_t id);
  uint16_t getGoalSpeed(uint8_t id);
  uint8_t getLock(uint8_t id);
  uint8_t setLock(uint8_t id, uint8_t lockEnabled);
  uint8_t setLockSTS(uint8_t id, uint8_t lockEnabled);
  uint16_t getPosition(uint8_t id);

  int16_t getSpeed(uint8_t id);
  uint16_t getLoad(uint8_t id);
  uint8_t getTemperature(uint8_t id);


  uint8_t getMoving(uint8_t id);
  float getCurrent(uint8_t id);

  float getVoltage(uint8_t id);
  void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount);
  void sendWritePos(uint8_t id, uint16_t position);
  void syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count);
  void write1Byte(uint8_t id, byte instruction, uint8_t data);
  void write2Bytes(uint8_t id, byte instruction, uint16_t data);
  void pingAll(std::vector<uint8_t>& successfulAddresses);
  void waitOnReply(unsigned long timeout);
  uint8_t waitOnData1Byte(unsigned long timeout);
  uint16_t waitOnData2Bytes(unsigned long timeout);
  void sendData(const byte* data, size_t length);
  size_t receiveData(byte* buffer, size_t bufferSize);
  void setModeReceive();
  void setModeTransmit();
  void update();
  void testRequest();
  void enableTorque(uint8_t id);

  static const byte PING = 0x01;            // QUERY THE WORKING STATUS
  static const byte READ_DATA = 0x02;       // READ DATA
  static const byte WRITE_DATA = 0x03;      // WRITE DATA
  static const byte REGWRITE_DATA = 0x04;   // QUEUES MOVES FOR ACTION COMMAND
  static const byte ACTION = 0x05;          // TRIGGERS REG WRITE WRITES
  static const byte SYNCWRITE_DATA = 0x83;  // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
  static const byte RESET = 0x06;           // RESET TO FACTORY DEFAULT
  static const byte BROADCAST_ID = 0xFE;


  // MEMORY TABLE LOCATIONS SMS-STS
  static const byte REQUEST_MODEL = 0x03;            // 2 bytes
  static const byte REQUEST_ID = 0x05;               // 1 byte
  static const byte REQUEST_BAUD_RATE = 0x06;        // 1 byte
  static const byte REQUEST_MIN_ANGLE_LIMIT = 0x09;  // 2 bytes
  static const byte REQUEST_MAX_ANGLE_LIMIT = 0x0B;  // 2 bytes
  static const byte REQUEST_CW_DEAD_ZONE = 0x1A;     // 1 byte
  static const byte REQUEST_CCW_DEAD_ZONE = 0x1B;    // 1 byte
  static const byte REQUEST_OFFSET = 0x1F;           // 2 bytes
  static const byte REQUEST_MODE = 0x21;             // 1 byte

  static const byte REQUEST_TORQUE_ENABLE = 0x28;  // 1 byte
  static const byte REQUEST_ACCELERATION = 0x29;   // 1 byte
  static const byte REQUEST_GOAL_POSITION = 0x2A;  // 2 byte
  static const byte REQUEST_GOAL_TIME = 0x2C;      // 2 byte
  static const byte REQUEST_GOAL_SPEED = 0x2E;     // 2 byte
  static const byte REQUEST_LOCK = 0x30;           // 1 byte

  static const byte REQUEST_POSITION = 0x38;  // 2 bytes

  static const byte REQUEST_CURRENT_SPEED = 0x3A;  // 2 bytes
  static const byte REQUEST_CURRENT_LOAD = 0x3C;   // 2 bytes

  static const byte REQUEST_VOLTAGE = 0x3E;  // 1 byte

  static const byte REQUEST_TEMPERATURE = 0x3F;      // 1 byte
  static const byte REQUEST_MOVING = 0x42;           // 1 byte
  static const byte REQUEST_CURRENT_CURRENT = 0x45;  // 2 bytes


  // BAUD RATES (stored as 1 byte)
  static const byte SMS_STS_1M = 0;
  static const byte SMS_STS_0_5M = 1;
  static const byte SMS_STS_250K = 2;
  static const byte SMS_STS_128K = 3;
  static const byte SMS_STS_115200 = 4;
  static const byte SMS_STS_76800 = 5;
  static const byte SMS_STS_57600 = 6;
  static const byte SMS_STS_38400 = 7;

  uint8_t lastSentPacket[32];
  int lastSentLength = 0;


private:
  HardwareSerial& serial;  // Reference to the HardwareSerial object
  uint8_t DE_PIN = 20;
  uint8_t RE_PIN = 10;
  uint8_t TX_PIN = 21;
  uint8_t RX_PIN = 9;
};

#endif  // FEETECH_H