#ifndef FEETECH_H
#define FEETECH_H

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

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 getPosition(uint8_t id);
  void sendRequest(uint8_t id, uint8_t instruction);
  void sendWritePos(uint8_t id, uint16_t position);
  void pingAll();
  void waitOnReply(unsigned long timeout);
  void waitOnReply(unsigned long timeout, uint8_t expectedReturnType);
  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 REQUEST_POSITION = 0x38;   // Current Position request

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