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();
  }
}

// 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 loop() {

  sendWritePos(2, random(0, 4096));
  WaitOnReply(1000);
  delay(1000);
  sendPing(200);
  Serial.println("WAITING...");
  WaitOnReply(1000);
  delay(1000);
  sendPing(5);
  Serial.println("WAITING...");
  WaitOnReply(1000);
  delay(1000);
}