#include "feetech.h"
// ESP32 S2 PINOUT
#define RX_PIN 17  // DI
#define TX_PIN 18  // RO
#define DE_PIN 33  // Driver Enable
#define RE_PIN 3   // Receiver Enable


Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);

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

uint8_t ids[] = { 1, 10, 11, 15 };
uint16_t pos1[] = { 0, 0, 0, 0 };
uint16_t pos2[] = { 1023, 1023, 1023, 4095 };


void setup() {
  Serial.begin(115200);
  for (int i = 0; i < 10; i++) {
    Serial.println(i);
    delay(500);
  }

  pos2[3] = flipBytes(pos2[3]);

  servos.begin();

}

void SetID(uint8_t oldID, uint8_t newID) {
  Serial.println("Setting Lock to 0");
  Serial.println(servos.setLock(oldID, 0));
  delay(1000);
  Serial.print("Changing ID ");
  Serial.print(oldID);
  Serial.print(" to ");
  Serial.println(newID);
  Serial.println(servos.setID(oldID, newID));
  delay(1000);
  Serial.println("Setting Lock to 1");
  Serial.println(servos.setLock(newID, 1));
  delay(1000);
}

void loop() {
  // put your main code here, to run repeatedly:
  PingAll();

}

void PingAll() {
  std::vector<uint8_t> successfulAddresses;
  servos.pingAll(successfulAddresses);

  // Now successfulAddresses contains all successful pings
  Serial.println("Successful Addresses:");
  for (uint8_t address : successfulAddresses) {
    Serial.print(address);
    Serial.print(" ");
  }
  Serial.println();
}