#include <esp_now.h>
#include <WiFi.h>

// จำนวนโดรน
#define DRONE_COUNT 5

// เก็บ MAC Address ของโดรน
uint8_t droneMacs[DRONE_COUNT][6] = {
  {0xCC,0xDB,0xA7,0x3F,0xB0,0x74}, // Drone 1
  {0x24, 0x6F, 0x28, 0x1A, 0x4D, 0x8E}, // Drone 2
  {0x24, 0x6F, 0x28, 0x1A, 0x59, 0xAF}, // Drone 3
  {0x24, 0x6F, 0x28, 0x1B, 0x11, 0xC0}, // Drone 4
  {0x24, 0x6F, 0x28, 0x1B, 0x22, 0xD1}  // Drone 5
};

void OnSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("ส่งไปที่: ");
  for (int i = 0; i < 6; i++) {
    Serial.printf("%02X", mac_addr[i]);
    if (i < 5) Serial.print(":");
  }
  Serial.print(" สถานะ: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "สำเร็จ" : "ล้มเหลว");
}

void setup() {
  Serial.begin(115200);

  WiFi.mode(WIFI_STA);

  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW Init ล้มเหลว!");
    return;
  }

  esp_now_register_send_cb(OnSent);

  // เพิ่ม Peer (โดรนแต่ละตัว)
  for (int i = 0; i < DRONE_COUNT; i++) {
    esp_now_peer_info_t peerInfo = {};
    memcpy(peerInfo.peer_addr, droneMacs[i], 6);
    peerInfo.channel = 0;
    peerInfo.encrypt = false;
    if (esp_now_add_peer(&peerInfo) != ESP_OK) {
      Serial.printf("เพิ่ม Peer ล้มเหลว %d\n", i+1);
    }
  }
}

void loop() {
  // ตัวอย่างคำสั่ง
  String command = "TAKEOFF"; // เปลี่ยนเป็น MOVE_LEFT, LAND ฯลฯ
  for (int i = 0; i < DRONE_COUNT; i++) {
    esp_now_send(droneMacs[i], (uint8_t *)command.c_str(), command.length() + 1);
  }
  delay(2000); // ส่งทุก 2 วินาที
}