bool PRINT_SENT_PACKETS = false;
#define DE_PIN 20
#define RE_PIN 10
#define TX_PIN 21
#define RX_PIN 9


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

void setup() {
  Serial.begin(115200);
  Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
  pinMode(DE_PIN, OUTPUT);
  pinMode(RE_PIN, OUTPUT);
  SetModeReceive();
  delay(1000);
}

// Send move command to servo id:0-255, position:0-4095
void sendWritePos(uint8_t id, uint16_t position) {
  uint8_t packet[9];

  packet[0] = 0xFF;                    // Header
  packet[1] = 0xFF;                    // Header
  packet[2] = id;                      // Servo ID
  packet[3] = 5;                       // Length = instruction + address + 2 bytes + checksum
  packet[4] = WRITE_DATA;              // Instruction: WRITE
  packet[5] = 0x2A;                    // Address: Goal Position
  packet[6] = position & 0xFF;         // Low byte
  packet[7] = (position >> 8) & 0xFF;  // High byte

  // Calculate checksum
  uint8_t sum = 0;
  for (int i = 2; i < 8; i++) sum += packet[i];
  packet[8] = ~sum;

  // Send packet
  SetModeTransmit();
  Serial1.write(packet, sizeof(packet));
  Serial1.flush();
  SetModeReceive();

  digitalWrite(DE_PIN, LOW);  // Back to receive
  if (PRINT_SENT_PACKETS) {
    Serial.print("Sent packet: ");

    for (int i = 0; i < 9; i++) {
      Serial.print("0x");
      if (packet[i] < 0x10) Serial.print("0");  // Leading zero for single-digit hex
      Serial.print(packet[i], HEX);
      Serial.print(" ");
    }
    Serial.println();
  }
}

void syncWriteTwoServos(uint8_t id1, uint16_t pos1, uint8_t id2, uint16_t pos2) {
  const uint8_t SYNC_WRITE = 0x83;
  const uint8_t START_BYTE = 0xFF;
  const uint8_t DATA_LEN = 0x05; // 2 bytes for position + 1 byte for speed (optional)

  // Build packet
  uint8_t packet[] = {
    START_BYTE, START_BYTE, 0xFE, // Broadcast ID
    0x0F,                         // Length = 15 bytes total
    SYNC_WRITE,
    0x2A,                         // Starting address for Goal Position (0x2A)
    0x04,                         // Data length per servo (2 bytes pos + 2 bytes speed)
    id1,
    (uint8_t)(pos1 & 0xFF),       // Position low byte
    (uint8_t)((pos1 >> 8) & 0xFF),// Position high byte
    0x00, 0x00,                   // Speed (optional, set to 0)
    id2,
    (uint8_t)(pos2 & 0xFF),
    (uint8_t)((pos2 >> 8) & 0xFF),
    0x00, 0x00
  };

  // Calculate checksum
  uint8_t checksum = 0;
  for (int i = 2; i < sizeof(packet); i++) {
    checksum += packet[i];
  }
  checksum = ~checksum;

// Send packet
  SetModeTransmit();
  Serial1.write(packet, sizeof(packet));
  Serial1.write(checksum);
  Serial1.flush();
  SetModeReceive();
}
void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
  const uint8_t SYNC_WRITE = 0x83;
  const uint8_t START_BYTE = 0xFF;
  const uint8_t BROADCAST_ID = 0xFE;
  const uint8_t START_ADDR = 0x2A;     // Goal Position address
  const uint8_t DATA_LEN_PER_SERVO = 0x04; // 2 bytes pos + 2 bytes speed

  // Calculate packet length: instruction + address + data length + (count × data per servo)
  uint8_t packetLen = 4 + (count * (1 + DATA_LEN_PER_SERVO)); // instruction + addr + len + servo data

  // Create packet buffer
  uint8_t packet[3 + packetLen]; // 3 header bytes + payload
  uint8_t index = 0;

  // Header
  packet[index++] = START_BYTE;
  packet[index++] = START_BYTE;
  packet[index++] = BROADCAST_ID;
  packet[index++] = packetLen;
  packet[index++] = SYNC_WRITE;
  packet[index++] = START_ADDR;
  packet[index++] = DATA_LEN_PER_SERVO;

  // Servo data
  for (uint8_t i = 0; i < count; i++) {
    packet[index++] = ids[i];
    packet[index++] = (uint8_t)(positions[i] & 0xFF);        // Low byte
    packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte
    packet[index++] = 0x00; // Speed low byte
    packet[index++] = 0x00; // Speed high byte
  }

  // Checksum
  uint8_t checksum = 0;
  for (uint8_t i = 2; i < index; i++) {
    checksum += packet[i];
  }
  checksum = ~checksum;

  // Send packet
  SetModeTransmit();
  Serial1.write(packet, index);
  Serial1.write(checksum);
  Serial1.flush();
  SetModeReceive();
}

// Send a ping command to a specific servo
void sendPing(uint8_t id) {
  uint8_t packet[6];

  packet[0] = 0xFF;  // Header
  packet[1] = 0xFF;  // Header
  packet[2] = id;    // Servo ID
  packet[3] = 0x02;  // Length = instruction + address + checksum
  packet[4] = PING;  // Instruction: PING

  // Calculate checksum
  uint8_t sum = 0;
  for (int i = 2; i < 5; i++) sum += packet[i];
  packet[5] = ~sum;  // Checksum

  // Send packet
  SetModeTransmit();
  Serial1.write(packet, sizeof(packet));
  Serial1.flush();
  SetModeReceive();

  if (PRINT_SENT_PACKETS) {
    Serial.print("Sent ping packet: ");
    for (int i = 0; i < 6; i++) {
      Serial.print("0x");
      if (packet[i] < 0x10) Serial.print("0");  // Leading zero for single-digit hex
      Serial.print(packet[i], HEX);
      Serial.print(" ");
    }
    Serial.println();
  }
}

void WaitOnReply(unsigned long timeout) {
  unsigned long startTime = millis();  // Record the start time

  while (millis() - startTime < timeout) {  // Loop until timeout
    if (Serial1.available()) {
      uint8_t buffer[32];
      int count = 0;

      // Read all available bytes
      while (Serial1.available() && count < sizeof(buffer)) {
        buffer[count++] = Serial1.read();
      }

      // Display on Serial
      Serial.print("recv: ");
      Serial.print(buffer[2]);
      Serial.print(" ");
      if (buffer[4] == 0x00) {
        Serial.print("OK");
      } else {
        Serial.print("NOK");
      }
      Serial.print("\t");
      for (int i = 0; i < count; i++) {
        Serial.print("0x");
        if (buffer[i] < 0x10) Serial.print("0");
        Serial.print(buffer[i], HEX);
        Serial.print(" ");
      }
      Serial.println();


      break;  // Exit the loop after processing the reply
    }
    delay(10);  // Small delay to prevent busy-waiting
  }
}

void SetModeTransmit() {
  digitalWrite(DE_PIN, HIGH);
  digitalWrite(RE_PIN, HIGH);
  delay(10);
}

void SetModeReceive() {
  digitalWrite(DE_PIN, LOW);
  digitalWrite(RE_PIN, LOW);
  delay(10);
}

void PingAll() {
  Serial.println("PINGING ALL 0-255");
  for (int i = 0; i < 255; i++) {

    sendPing(i);

    WaitOnReply(100);
  }
  Serial.println("PINGING COMPLETE");
}

void TestRange(uint8_t id) {
  for (int i = 0; i < 4095; i += 256) {
    sendWritePos(id, i);
    WaitOnReply(100);
    delay(100);
  }
  for (int i = 4095; i > 0; i -= 256) {

    sendWritePos(id, i);
    WaitOnReply(100);
    delay(100);
  }
}

uint8_t ids[] = { 0, 4, 10, 15, 40, 41, 43, 44};
uint16_t pos[8];

void loop() {
  //PingAll();
  for (int i = 0; i < 4; i += 1) {
    for (int x = 0; x < 8; x++){
    pos[x] = i;
    }
    syncWriteServos(ids, pos, 8);
    Serial.println(i);
    //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
    delay(100);
  }
  for (int i = 4; i > 0; i -= 8) {
    for (int x = 0; x < 8; x++){
    pos[x] = i;
    }
    syncWriteServos(ids, pos, 2);
    Serial.println(i);
    //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
    delay(100);
  }
  // TestRange(0);
  // TestRange(4);
  // TestRange(10);
  // TestRange(15);
  // TestRange(40);
  // TestRange(41);
  // TestRange(43);
  // TestRange(44);

  // sendWritePos(123, 1000);
  //delay(2000);

  // sendWritePos(123, 2000);
  // delay(2000);



  // sendWritePos(34, 4095);
  // WaitOnReply(50);
  // delay(100);
  // sendWritePos(34, 0);
  // WaitOnReply(50);
  // delay(100);

  // WaitOnReply(1000);
  // delay(1000);
  // delay(1000);
  // sendPing(5);
  // Serial.println("WAITING...");
  // WaitOnReply(1000);
  // delay(1000);
}