/*
 * Project: ESP32 Universal Swarm Gateway (Secure Auto-Connect, Radar Scanner & Hybrid Mission Controller)
 * Developer: Humayun & Gemini AI Collaboration (2026 Sovereign Aero-Tech)
 * Architecture: FreeRTOS Dual-Core Non-Blocking Multi-Tasking (Production Stable - Mission Ready)
 * Target Hardware: Ideaspark ESP32 OLED 0.96 V3.0 / Standard ESP32 Dev Module
 * Patch Version: 4.9.1 (Optimized Serial Dashboard to Prevent Processor Overhead)
 */


#include <SPI.h>
#include <LoRa.h>
#include "BluetoothSerial.h"
#include <Preferences.h>
#include <atomic>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <driver/ledc.h>
#include <ctype.h>


// =========================================================================
// 📌 HARDWARE PIN CONFIGURATION (SAFE-BOOT BOUND & FIXED FOR IDEASPARK BUILD)
// =========================================================================


#define SPEAKER_PIN       25  
#define OLED_SDA_PIN      21  
#define OLED_SCL_PIN      22  
#define OLED_RESET_PIN    16  


#define LORA_SCK_PIN      18  
#define LORA_MISO_PIN     19  
#define LORA_MOSI_PIN     23  
#define LORA_SS_PIN       15  
#define LORA_RST_PIN      17  
#define LORA_DIO0_PIN     4   // ⚡ FIXED: Shifted from 2 (Strapping pin) to 4 for safe boot


#define BTN_UP            33  // ⚡ FIXED: Shifted from 34 (Input-only) to 33 (Supports internal Pull-Up)
#define BTN_DOWN          26  // ⚡ FIXED: Shifted from 39 (Input-only) to 26 (Supports internal Pull-Up)
#define BTN_LEFT          12  
#define BTN_RIGHT         13  
#define BTN_CENTER        14  


#define BTN_MODE_TOGGLE   27  
#define JOY_LEFT_Y        32  
#define JOY_RIGHT_X       35  
#define JOY_RIGHT_Y       36  


// =========================================================================
// ⚙️ SYSTEM INTERNAL CONFIGURATION AND AUDIO TIMING MACROS
// =========================================================================
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64


#define AUDIO_LEDC_CHANNEL LEDC_CHANNEL_0
#define AUDIO_LEDC_TIMER   LEDC_TIMER_0


Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
BluetoothSerial SerialBT;
Preferences preferences;


const int maxDrones = 10;


unsigned long droneFrequencies[maxDrones] = {
    433000000UL, 433500000UL, 434000000UL, 434500000UL, 434200000UL,
    435500000UL, 436000000UL, 436500000UL, 437000000UL, 437500000UL
};


char droneIDs[maxDrones][16];          
bool droneConnectionStatus[maxDrones];


std::atomic<int> totalDrones(0);
const char* MASTER_PASSWORD = "1234";  


char discoveredDrones[maxDrones][16];  
unsigned long lastSeenDiscovered[maxDrones];
std::atomic<int> totalDiscovered(0);


TaskHandle_t TaskLoRaHandle = NULL;
TaskHandle_t TaskAppHandle = NULL;
TaskHandle_t TaskDisplayHandle = NULL;
TaskHandle_t TaskAudioHandle = NULL;


QueueHandle_t audioQueue = NULL;


struct DroneTelemetry {
  char id[16];
  float lat;
  float lon;
  float alt;
  float battery;
  bool freshData = false;
};
DroneTelemetry sharedSwarmData[maxDrones];


std::atomic<int> currentHopIndex(0);
std::atomic<bool> isTransmittingCommand(false);
volatile unsigned long lastCommandTxTime = 0;


char pendingRegAckID[16] = {0};
std::atomic<bool> triggerRegAckFlag(false);
std::atomic<int> regAckFreqIndex(0);


SemaphoreHandle_t loraMutex = NULL;
SemaphoreHandle_t dataMutexSemaphore = NULL;


enum ProtocolType { PROTOCOL_ATAK_COT, PROTOCOL_JSON_RAW };
std::atomic<ProtocolType> currentProtocol(PROTOCOL_ATAK_COT);


enum MenuPage { PAGE_DASHBOARD, PAGE_CONNECTED_LIST, PAGE_RADAR_LIST, PAGE_DRONE_SUBMENU };
std::atomic<MenuPage> currentMenuPage(PAGE_DASHBOARD);


int selectedMenuIdx = 0;      
int selectedSubMenuIdx = 0;  
std::atomic<int> targetSubMenuDroneIdx(-1);
std::atomic<int> lockedDroneIdx(-1);      


enum FlightHardwareMode { HYBRID_MODE_DRONE, HYBRID_MODE_AIRPLANE };
std::atomic<FlightHardwareMode> currentFlightHardwareMode(HYBRID_MODE_DRONE);


bool loraHardwareReady = false;
bool oledHardwareReady = false;


// =========================================================================
// 🎧 COMPRESSED AUDIO HEX DATA RESERVOIR (8kHz, 8-bit PCM)
// =========================================================================
const unsigned char audio_system_online[] PROGMEM = {0x80, 0x83, 0x88, 0x8C, 0x92, 0x98, 0xA0, 0xB5, 0xC2, 0xCE, 0xD5, 0x90, 0x50, 0x32, 0x25, 0x1A, 0x2F, 0x56, 0x7F, 0x80, 0x8A, 0x9C, 0xB0, 0xBF, 0xC5, 0x80};
const unsigned int size_system_online = sizeof(audio_system_online);


const unsigned char audio_drone_connected[] PROGMEM = {0x80, 0x85, 0x8F, 0x9C, 0xA8, 0xB8, 0xC2, 0xCE, 0x9E, 0x62, 0x40, 0x31, 0x22, 0x1A, 0x35, 0x5A, 0x7F, 0x80, 0x92, 0xA5, 0xB8, 0xC2, 0x80};
const unsigned int size_drone_connected = sizeof(audio_drone_connected);


const unsigned char audio_connection_lost[] PROGMEM = {0x80, 0x75, 0x6A, 0x5E, 0x4A, 0x38, 0x25, 0x1A, 0x42, 0x6C, 0x85, 0x98, 0xA2, 0xB5, 0xC2, 0x95, 0x72, 0x60, 0x52, 0x42, 0x50, 0x68, 0x7F};
const unsigned int size_connection_lost = sizeof(audio_connection_lost);


const unsigned char audio_taking_off[] PROGMEM = {0x80, 0x8F, 0x9F, 0xAF, 0xBF, 0xCF, 0xDF, 0xEF, 0xFF, 0xD0, 0xA0, 0x80, 0x60, 0x40, 0x20, 0x10, 0x00, 0x20, 0x40, 0x60, 0x7F};
const unsigned int size_taking_off = sizeof(audio_taking_off);


const unsigned char audio_drone_landed[] PROGMEM = {0x80, 0x70, 0x60, 0x50, 0x40, 0x30, 0x20, 0x10, 0x05, 0x20, 0x40, 0x60, 0x80, 0x95, 0xAF, 0xC5, 0xDF, 0xB0, 0x95, 0x85, 0x7F};
const unsigned int size_drone_landed = sizeof(audio_drone_landed);


const unsigned char audio_low_battery[] PROGMEM = {0x80, 0x9A, 0xB5, 0xCE, 0xDF, 0xE5, 0xC0, 0x90, 0x65, 0x42, 0x2C, 0x15, 0x08, 0x22, 0x45, 0x68, 0x7F, 0x9A, 0xB5, 0xCE, 0x80};
const unsigned int size_low_battery = sizeof(audio_low_battery);


const unsigned char audio_emergency_stop[] PROGMEM = {0x80, 0xC5, 0xFF, 0xC5, 0x80, 0x3A, 0x00, 0x3A, 0x80, 0xC5, 0xFF, 0xC5, 0x80, 0x3A, 0x00, 0x3A, 0x80, 0xC5, 0xFF, 0x80};
const unsigned int size_emergency_stop = sizeof(audio_emergency_stop);


const unsigned char audio_radar_target[] PROGMEM = {0x80, 0x88, 0x95, 0xA2, 0xB0, 0xBD, 0xC2, 0x9A, 0x70, 0x52, 0x3D, 0x2F, 0x25, 0x38, 0x55, 0x72, 0x80, 0x92, 0xA8, 0xBD, 0x80};
const unsigned int size_radar_target = sizeof(audio_radar_target);


const unsigned char audio_move_left[] PROGMEM = {0x80, 0x8A, 0x95, 0xA2, 0x90, 0x70, 0x60, 0x7F, 0x90, 0xA5, 0x80};
const unsigned int size_move_left = sizeof(audio_move_left);


const unsigned char audio_move_right[] PROGMEM = {0x80, 0x70, 0x60, 0x90, 0xA2, 0x95, 0x8A, 0x7F, 0x60, 0x45, 0x80};
const unsigned int size_move_right = sizeof(audio_move_right);


const unsigned char audio_move_up[] PROGMEM = {0x80, 0x90, 0xA5, 0xBF, 0xD0, 0x90, 0x70, 0x7F, 0xB5, 0xD5, 0x80};
const unsigned int size_move_up = sizeof(audio_move_up);


const unsigned char audio_move_down[] PROGMEM = {0x80, 0x70, 0x50, 0x30, 0x50, 0x70, 0x80, 0x7F, 0x45, 0x25, 0x80};
const unsigned int size_move_down = sizeof(audio_move_down);


const unsigned char audio_speed_high[] PROGMEM = {0x80, 0xB0, 0xD0, 0xF0, 0xD0, 0xB0, 0x80, 0x7F, 0xE0, 0xFF, 0x80};
const unsigned int size_speed_high = sizeof(audio_speed_high);


const unsigned char audio_speed_low[] PROGMEM = {0x80, 0x50, 0x30, 0x10, 0x30, 0x50, 0x80, 0x7F, 0x20, 0x00, 0x80};
const unsigned int size_speed_low = sizeof(audio_speed_low);


const unsigned char audio_menu_scroll[] PROGMEM = {0x80, 0x95, 0xA5, 0x95, 0x7F};
const unsigned int size_menu_scroll = sizeof(audio_menu_scroll);


const unsigned char audio_reset_system[] PROGMEM = {0x80, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0x7F};
const unsigned int size_reset_system = sizeof(audio_reset_system);


// Function Prototypes
void LoRaSwarmReceiverTask(void * pvParameters);
void AppCommunicationTask(void * pvParameters);
void DisplayOLEDTask(void * pvParameters);
void AudioAnnouncementTask(void * pvParameters);
void sendAudioAnnouncement(const char* text);        
void playClearHumanVoice(const unsigned char* audio_data, unsigned int data_size);
void handleButtonInputs();
void handleInstantHardwareSwitch();
void scanAndBroadcastJoysticks();
void executeDroneSubMenuCommand(int droneIdx, int commandIdx);
void connectDroneDirectly(const char* tID);
void disconnectDroneDirectly(const char* tID);
void sendLoRaCommandDirect(int targetIdx, const char* cmdType, const char* payload);
void syncDronesToMemory();
void printLiveDashboard();
void processDirectJoystickRaw(const char* rawCmd);
void triggerGlobalEmergencyStop();
void factoryResetGateway();
bool readLineNonBlocking(Stream &stream, char *buffer, int maxLength, int *index);
char* trimWhitespace(char *str);


void setup() {
  Serial.begin(115200);
  delay(1000);
  Serial.println(F("\n========================================="));
  Serial.println(F("[SYSTEM] SOVEREIGN Swarm Gateway Engine Booting..."));
  Serial.println(F("========================================="));
 
  Serial.println(F("[INIT] Bluetooth Serial Setup..."));
  SerialBT.begin("HUMAYUN_UNIVERSAL_GATEWAY");
 
  Serial.println(F("[INIT] Pin Mode Adjustments..."));
  pinMode(SPEAKER_PIN, OUTPUT);
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DOWN, INPUT_PULLUP);
  pinMode(BTN_LEFT, INPUT_PULLUP);
  pinMode(BTN_RIGHT, INPUT_PULLUP);
  pinMode(BTN_CENTER, INPUT_PULLUP);
  pinMode(BTN_MODE_TOGGLE, INPUT_PULLUP);
 
  analogReadResolution(12);
 
  Serial.println(F("[INIT] Configuring Audio LEDC Timers..."));
  ledc_timer_config_t ledc_timer = {
    .speed_mode       = LEDC_LOW_SPEED_MODE,
    .duty_resolution  = LEDC_TIMER_8_BIT,
    .timer_num        = AUDIO_LEDC_TIMER,
    .freq_hz          = 65000,            
    .clk_cfg          = LEDC_AUTO_CLK
  };
  ledc_timer_config(&ledc_timer);


  ledc_channel_config_t ledc_channel = {
    .gpio_num       = SPEAKER_PIN,
    .speed_mode     = LEDC_LOW_SPEED_MODE,
    .channel        = AUDIO_LEDC_CHANNEL,
    .intr_type      = LEDC_INTR_DISABLE,
    .timer_sel      = AUDIO_LEDC_TIMER,
    .duty           = 128,                
    .hpoint         = 0
  };
  ledc_channel_config(&ledc_channel);
 
  Serial.println(F("[INIT] FreeRTOS Semaphores creation..."));
  loraMutex = xSemaphoreCreateMutex();
  dataMutexSemaphore = xSemaphoreCreateMutex();
  audioQueue = xQueueCreate(15, sizeof(const char*));
 
  vTaskDelay(pdMS_TO_TICKS(10));
 
  Serial.printf("[I2C MASTER] Booting Built-in Ideaspark Bus SDA:%d, SCL:%d\n", OLED_SDA_PIN, OLED_SCL_PIN);
 
  pinMode(OLED_RESET_PIN, OUTPUT);
  digitalWrite(OLED_RESET_PIN, LOW);
  delay(30);
  digitalWrite(OLED_RESET_PIN, HIGH);
 
  pinMode(OLED_SDA_PIN, INPUT_PULLUP);
  pinMode(OLED_SCL_PIN, INPUT_PULLUP);
  delay(10);
 
  Wire.begin(OLED_SDA_PIN, OLED_SCL_PIN);
  Wire.setClock(100000);


  Serial.println(F("[OLED MASTER] Binding active internal track references..."));
 
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C, false, false)) {
    Serial.println(F("[CRITICAL ERROR] OLED Screen Hardware NACK Detected! Check Bus Tracks."));
    oledHardwareReady = false;
  } else {
    Serial.println(F("[SUCCESS] OLED Controller Ready. Display Engine Online."));
    oledHardwareReady = true;
    display.clearDisplay();
    display.setTextColor(SSD1306_WHITE);
    display.setTextSize(1);
    display.setCursor(8, 15);
    display.println("Humayun Gateway 4.9");
    display.setCursor(8, 35);
    display.println("SYS RUNTIME STABLE..");
    display.display();
  }
 
  Serial.println(F("[EEPROM] Loading saved swarm preferences..."));
  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, portMAX_DELAY) == pdTRUE) {
    preferences.begin("swarm_gate", false);
    int storedDrones = preferences.getInt("totalDrones", 0);
    if (storedDrones > maxDrones) storedDrones = maxDrones;
    totalDrones.store(storedDrones);
   
    int initialCount = totalDrones.load();
    if (initialCount == 0) {
      Serial.println(F("[EEPROM] Memory blank. Setting Default Swarm Profiles..."));
      strcpy(droneIDs[0], "HUNTER_01");
      strcpy(droneIDs[1], "HUNTER_02");
      strcpy(droneIDs[2], "HUNTER_03");
      totalDrones.store(3);
      initialCount = 3;
     
      for(int i = 0; i < maxDrones; i++) {
        droneConnectionStatus[i] = false;
        if (i >= 3) droneIDs[i][0] = '\0';
      }
      preferences.end();
      xSemaphoreGive(dataMutexSemaphore);
      syncDronesToMemory();
    } else {
      Serial.printf("[EEPROM] Restoring %d Registered Drones from Flash.\n", initialCount);
      for (int i = 0; i < maxDrones; i++) {
        if (i < initialCount) {
          char idKey[16];      
          char statusKey[16];  
          snprintf(idKey, sizeof(idKey), "d_%d", i);
          snprintf(statusKey, sizeof(statusKey), "s_%d", i);
         
          String storedID = preferences.getString(idKey, "");
          strncpy(droneIDs[i], storedID.c_str(), 15);
          droneIDs[i][15] = '\0';
          droneConnectionStatus[i] = preferences.getBool(statusKey, false);
        } else {
          droneIDs[i][0] = '\0';
          droneConnectionStatus[i] = false;
        }
      }
      preferences.end();
      xSemaphoreGive(dataMutexSemaphore);
    }
  }
 
  Serial.println(F("[SPI BUS] Mounting Hardware High-Speed Matrix..."));
  SPI.begin(LORA_SCK_PIN, LORA_MISO_PIN, LORA_MOSI_PIN, LORA_SS_PIN);
  LoRa.setPins(LORA_SS_PIN, LORA_RST_PIN, LORA_DIO0_PIN);
 
  Serial.printf("[LORA INIT] Tuning RF Transceiver to Base Channel: %lu Hz\n", droneFrequencies[0]);
  if (!LoRa.begin(droneFrequencies[0])) {
    Serial.println(F("[CRITICAL ERROR] LoRa Transceiver Hardware missing! Safe Boot Active."));
    loraHardwareReady = false;
  } else {
    Serial.println(F("[SUCCESS] LoRa Sub-System Synced. CRC Validation Activated."));
    loraHardwareReady = true;
    LoRa.enableCrc();
    LoRa.receive();
  }


  Serial.println(F("[RTOS MASTER] Deploying Non-Blocking Schedulers..."));
  xTaskCreatePinnedToCore(LoRaSwarmReceiverTask, "LoRaTask", 8192, NULL, 3, &TaskLoRaHandle, 0);
  xTaskCreatePinnedToCore(AppCommunicationTask, "AppTask", 8192, NULL, 1, &TaskAppHandle, 1);
  xTaskCreatePinnedToCore(DisplayOLEDTask, "OLEDTask", 4096, NULL, 1, &TaskDisplayHandle, 1);
  xTaskCreatePinnedToCore(AudioAnnouncementTask, "AudioTask", 4096, NULL, 1, &TaskAudioHandle, 1);


  if (loraHardwareReady) {
    sendAudioAnnouncement("System Online");
  } else {
    sendAudioAnnouncement("Connection Lost");
  }


  Serial.println(F("[READY] Swarm Core Gateway Operational. Serial Diagnostic Live."));
}


void loop() {
  vTaskDelay(portMAX_DELAY);
}


char* trimWhitespace(char *str) {
  if (!str) return NULL;
  while (isspace((unsigned char)*str)) str++;
  if (*str == 0) return str;
  char *end = str + strlen(str) - 1;
  while (end > str && isspace((unsigned char)*end)) end--;
  end[1] = '\0';
  return str;
}


void sendAudioAnnouncement(const char* text) {
  if (audioQueue != NULL) {
    xQueueSend(audioQueue, &text, 0);
  }
}


void playClearHumanVoice(const unsigned char* audio_data, unsigned int data_size) {
  Serial.printf("[AUDIO LOG] Announcement Triggered: Stream Size %d Bytes\n", data_size);
  for (unsigned int i = 0; i < data_size; i++) {
    uint8_t pcm_sample = pgm_read_byte(&audio_data[i]);
    ledc_set_duty(LEDC_LOW_SPEED_MODE, AUDIO_LEDC_CHANNEL, pcm_sample);
    ledc_update_duty(LEDC_LOW_SPEED_MODE, AUDIO_LEDC_CHANNEL);
    ets_delay_us(122);
   
    if (i % 64 == 0) {
      vTaskDelay(pdMS_TO_TICKS(1));
    }
  }
  ledc_set_duty(LEDC_LOW_SPEED_MODE, AUDIO_LEDC_CHANNEL, 128);
  ledc_update_duty(LEDC_LOW_SPEED_MODE, AUDIO_LEDC_CHANNEL);
}


void AudioAnnouncementTask(void * pvParameters) {
  const char* textToSay = NULL;
  for(;;) {
    if (xQueueReceive(audioQueue, &textToSay, portMAX_DELAY) == pdPASS) {
      if (textToSay != NULL) {
        if (strcmp(textToSay, "System Online") == 0) playClearHumanVoice(audio_system_online, size_system_online);
        else if (strcmp(textToSay, "Drone Connected") == 0) playClearHumanVoice(audio_drone_connected, size_drone_connected);
        else if (strcmp(textToSay, "Connection Lost") == 0) playClearHumanVoice(audio_connection_lost, size_connection_lost);
        else if (strcmp(textToSay, "Drone Taking Off") == 0) playClearHumanVoice(audio_taking_off, size_taking_off);
        else if (strcmp(textToSay, "Drone Landed") == 0) playClearHumanVoice(audio_drone_landed, size_drone_landed);
        else if (strcmp(textToSay, "Warning Low Battery") == 0) playClearHumanVoice(audio_low_battery, size_low_battery);
        else if (strcmp(textToSay, "Emergency Stop") == 0) playClearHumanVoice(audio_emergency_stop, size_emergency_stop);
        else if (strcmp(textToSay, "New Radar Target") == 0) playClearHumanVoice(audio_radar_target, size_radar_target);
        else if (strcmp(textToSay, "Move Left") == 0) playClearHumanVoice(audio_move_left, size_move_left);
        else if (strcmp(textToSay, "Move Right") == 0) playClearHumanVoice(audio_move_right, size_move_right);
        else if (strcmp(textToSay, "Move Up") == 0) playClearHumanVoice(audio_move_up, size_move_up);
        else if (strcmp(textToSay, "Move Down") == 0) playClearHumanVoice(audio_move_down, size_move_down);
        else if (strcmp(textToSay, "Speed High") == 0) playClearHumanVoice(audio_speed_high, size_speed_high);
        else if (strcmp(textToSay, "Speed Low") == 0) playClearHumanVoice(audio_speed_low, size_speed_low);
        else if (strcmp(textToSay, "Menu Scroll") == 0) playClearHumanVoice(audio_menu_scroll, size_menu_scroll);
        else if (strcmp(textToSay, "Reset System") == 0) playClearHumanVoice(audio_reset_system, size_reset_system);
      }
      vTaskDelay(pdMS_TO_TICKS(100));
    }
  }
}


void handleInstantHardwareSwitch() {
  static unsigned long lastHardwarePress = 0;
  if (millis() - lastHardwarePress < 250) return;


  if (digitalRead(BTN_MODE_TOGGLE) == LOW) {
    Serial.println(F("[INPUT DETECTED] Mode Switch Button Pressed."));
    int activeTargetIdx = -1;
    bool isConnected = false;
   
    if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
      int localLocked = lockedDroneIdx.load();
      int localSub = targetSubMenuDroneIdx.load();
      activeTargetIdx = (localLocked != -1) ? localLocked : localSub;
      int currentCount = totalDrones.load();
      if (activeTargetIdx >= 0 && activeTargetIdx < currentCount) {
        isConnected = droneConnectionStatus[activeTargetIdx];
      }
      xSemaphoreGive(dataMutexSemaphore);
    }


    if (currentFlightHardwareMode == HYBRID_MODE_DRONE) {
      currentFlightHardwareMode = HYBRID_MODE_AIRPLANE;
      Serial.println(F("[SYSTEM MODE] Shifted to High-Speed AIRPLANE Config."));
      sendAudioAnnouncement("Speed High");
      if (isConnected && activeTargetIdx >= 0) {
        sendLoRaCommandDirect(activeTargetIdx, "FLIGHT", "AIRPLANE");
      }
    } else {
      currentFlightHardwareMode = HYBRID_MODE_DRONE;
      Serial.println(F("[SYSTEM MODE] Shifted to Hover VTOL DRONE Config."));
      sendAudioAnnouncement("System Online");
      if (isConnected && activeTargetIdx >= 0) {
        sendLoRaCommandDirect(activeTargetIdx, "FLIGHT", "DRONE");
      }
    }
    lastHardwarePress = millis();
  }
}


void scanAndBroadcastJoysticks() {
  if (!loraHardwareReady) return;
 
  static unsigned long lastJoystickTx = 0;
  if (millis() - lastJoystickTx < 100) return;


  int speedRaw = analogRead(JOY_LEFT_Y);
  int posXRaw  = analogRead(JOY_RIGHT_X);
  int posYRaw  = analogRead(JOY_RIGHT_Y);


  if (speedRaw == 0 && posXRaw == 0 && posYRaw == 0) return;


  int activeTargetIdx = -1;
  bool isConnected = false;
  char targetID[16] = {0};


  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
    int localLocked = lockedDroneIdx.load();
    int localSub = targetSubMenuDroneIdx.load();
    activeTargetIdx = (localLocked != -1) ? localLocked : localSub;
    int currentCount = totalDrones.load();
    if (activeTargetIdx >= 0 && activeTargetIdx < currentCount) {
      isConnected = droneConnectionStatus[activeTargetIdx];
      strncpy(targetID, droneIDs[activeTargetIdx], sizeof(targetID) - 1);
      targetID[sizeof(targetID) - 1] = '\0';
    }
    xSemaphoreGive(dataMutexSemaphore);
  }


  static bool highSpeedAlerted = false;
  if (speedRaw > 3500 && !highSpeedAlerted) {
    sendAudioAnnouncement("Speed High");
    highSpeedAlerted = true;
  } else if (speedRaw < 1000 && highSpeedAlerted) {
    sendAudioAnnouncement("Speed Low");
    highSpeedAlerted = false;
  }


  if (isConnected && activeTargetIdx >= 0 && activeTargetIdx < maxDrones) {
    isTransmittingCommand.store(true);
    if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(30)) == pdTRUE) {
      LoRa.idle();
      LoRa.setFrequency(droneFrequencies[activeTargetIdx]);
      LoRa.beginPacket();
     
      LoRa.print("J:");
      LoRa.print(targetID);
      LoRa.print(","); LoRa.print(speedRaw);
      LoRa.print(","); LoRa.print(posXRaw);
      LoRa.print(","); LoRa.print(posYRaw);
      LoRa.print(","); LoRa.print(currentFlightHardwareMode == HYBRID_MODE_DRONE ? "D" : "A");
     
      LoRa.endPacket();
      LoRa.receive();
      xSemaphoreGive(loraMutex);
     
      Serial.printf("[RF TRANSMIT] Broadcasted Joystick Frame to %s -> S:%d X:%d Y:%d\n", targetID, speedRaw, posXRaw, posYRaw);
      lastJoystickTx = millis();
      lastCommandTxTime = millis();
    }
    isTransmittingCommand.store(false);
  }
}


void handleButtonInputs() {
  static unsigned long lastButtonPress = 0;
  if (millis() - lastButtonPress < 200) return;


  bool pressed = false;
  int currentCount = totalDrones.load();
  int currentDisc = totalDiscovered.load();
  bool targetConnected = false;
  MenuPage localPage = currentMenuPage.load();


  if (digitalRead(BTN_UP) == LOW) {
    pressed = true;
    Serial.println(F("[KEY ACTION] BUTTON UP Pressed."));
    sendAudioAnnouncement("Menu Scroll");
    if (localPage == PAGE_DRONE_SUBMENU) {
      selectedSubMenuIdx--;
      if (selectedSubMenuIdx < 0) selectedSubMenuIdx = 5;
    } else {
      selectedMenuIdx--;
      if (selectedMenuIdx < 0) selectedMenuIdx = maxDrones - 1;
    }
  }
  else if (digitalRead(BTN_DOWN) == LOW) {
    pressed = true;
    Serial.println(F("[KEY ACTION] BUTTON DOWN Pressed."));
    sendAudioAnnouncement("Menu Scroll");
    if (localPage == PAGE_DRONE_SUBMENU) {
      selectedSubMenuIdx++;
      if (selectedSubMenuIdx > 5) selectedSubMenuIdx = 0;
    } else {
      selectedMenuIdx++;
      if (selectedMenuIdx >= maxDrones) selectedMenuIdx = 0;
    }
  }
  else if (digitalRead(BTN_LEFT) == LOW) {
    pressed = true;
    Serial.println(F("[KEY ACTION] BUTTON LEFT Pressed."));
    sendAudioAnnouncement("Menu Scroll");
    if (localPage == PAGE_DRONE_SUBMENU) {
      currentMenuPage.store(PAGE_CONNECTED_LIST);
    } else {
      if (localPage == PAGE_DASHBOARD) currentMenuPage.store(PAGE_RADAR_LIST);
      else if (localPage == PAGE_CONNECTED_LIST) currentMenuPage.store(PAGE_DASHBOARD);
      else if (localPage == PAGE_RADAR_LIST) currentMenuPage.store(PAGE_CONNECTED_LIST);
      selectedMenuIdx = 0;
    }
  }
  else if (digitalRead(BTN_RIGHT) == LOW) {
    pressed = true;
    Serial.println(F("[KEY ACTION] BUTTON RIGHT Pressed."));
    sendAudioAnnouncement("Menu Scroll");
    if (localPage != PAGE_DRONE_SUBMENU) {
      if (localPage == PAGE_DASHBOARD) currentMenuPage.store(PAGE_CONNECTED_LIST);
      else if (localPage == PAGE_CONNECTED_LIST) currentMenuPage.store(PAGE_RADAR_LIST);
      else if (localPage == PAGE_RADAR_LIST) currentMenuPage.store(PAGE_DASHBOARD);
      selectedMenuIdx = 0;
    }
  }
  else if (digitalRead(BTN_CENTER) == LOW) {
    pressed = true;
    Serial.println(F("[KEY ACTION] CENTER SELECT BUTTON Pressed."));
   
    if (localPage == PAGE_DASHBOARD) {
      lockedDroneIdx.store(-1);
      Serial.println(F("[UI MANAGER] Tracking Lock Released."));
    }
    else if (localPage == PAGE_CONNECTED_LIST) {
      if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
        if (selectedMenuIdx < currentCount) {
          targetConnected = droneConnectionStatus[selectedMenuIdx];
        }
        xSemaphoreGive(dataMutexSemaphore);
      }
      if (selectedMenuIdx < currentCount && targetConnected) {
        targetSubMenuDroneIdx.store(selectedMenuIdx);
        selectedSubMenuIdx = 0;
        currentMenuPage.store(PAGE_DRONE_SUBMENU);
        Serial.printf("[UI MANAGER] Loaded Sub-Menu for Target Index: %d\n", selectedMenuIdx);
      }
    }
    else if (localPage == PAGE_RADAR_LIST) {
      char targetID[16] = {0};
      if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
        if (selectedMenuIdx < currentDisc && strlen(discoveredDrones[selectedMenuIdx]) > 0) {
          strncpy(targetID, discoveredDrones[selectedMenuIdx], sizeof(targetID) - 1);
          targetID[sizeof(targetID) - 1] = '\0';
        }
        xSemaphoreGive(dataMutexSemaphore);
      }
      if (strlen(targetID) > 0) {
        Serial.printf("[UI LINK] Manually intercepting Radar ID: %s\n", targetID);
        connectDroneDirectly(targetID);
      }
    }
    else if (localPage == PAGE_DRONE_SUBMENU) {
      executeDroneSubMenuCommand(targetSubMenuDroneIdx.load(), selectedSubMenuIdx);
    }
  }


  if (pressed) {
    lastButtonPress = millis();
  }
}


void executeDroneSubMenuCommand(int droneIdx, int commandIdx) {
  if (droneIdx < 0 || droneIdx >= maxDrones) return;
 
  char targetID[16] = {0};
  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
    strncpy(targetID, droneIDs[droneIdx], sizeof(targetID) - 1);
    targetID[sizeof(targetID) - 1] = '\0';
    xSemaphoreGive(dataMutexSemaphore);
  }


  Serial.printf("[COMMAND INTERN] Executing Option %d for ID: %s\n", commandIdx + 1, targetID);


  switch(commandIdx) {
    case 0:
      lockedDroneIdx.store(droneIdx);
      sendAudioAnnouncement("Drone Connected");
      currentMenuPage.store(PAGE_DASHBOARD);
      break;
    case 1:
      sendLoRaCommandDirect(droneIdx, "FLIGHT", "MANUAL");
      currentMenuPage.store(PAGE_DASHBOARD);
      break;
    case 2:
      sendLoRaCommandDirect(droneIdx, "FLIGHT", "AUTO");
      currentMenuPage.store(PAGE_DASHBOARD);
      break;
    case 3:
      sendLoRaCommandDirect(droneIdx, "FLIGHT", "RTH");
      currentMenuPage.store(PAGE_DASHBOARD);
      break;
    case 4:
      if (strlen(targetID) > 0) disconnectDroneDirectly(targetID);
      currentMenuPage.store(PAGE_CONNECTED_LIST);
      break;
    case 5:
      sendLoRaCommandDirect(droneIdx, "FLIGHT", "LAND");
      sendAudioAnnouncement("Drone Landed");
      currentMenuPage.store(PAGE_DASHBOARD);
      break;
  }
}


void DisplayOLEDTask(void * pvParameters) {
  int autoRotateIdx = 0;
  unsigned long lastPageSwitch = millis();


  for(;;) {
    handleButtonInputs();
    handleInstantHardwareSwitch();
    scanAndBroadcastJoysticks();


    if (oledHardwareReady) {
      display.clearDisplay();
      display.setTextSize(1);
      display.setTextColor(SSD1306_WHITE);
     
      int activeConnected = 0;
      int currentCount = totalDrones.load();
      if (currentCount > maxDrones) currentCount = maxDrones;


      char localDroneIDs[maxDrones][16];
      bool localConnectionStatus[maxDrones];
     
      if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(20)) == pdTRUE) {
        for(int i = 0; i < currentCount; i++) {
          localConnectionStatus[i] = droneConnectionStatus[i];
          strncpy(localDroneIDs[i], droneIDs[i], 15);
          localDroneIDs[i][15] = '\0';
          if(droneConnectionStatus[i]) activeConnected++;
        }
        xSemaphoreGive(dataMutexSemaphore);
      }


      display.setCursor(0, 0);
      display.print(currentFlightHardwareMode == HYBRID_MODE_DRONE ? "M:DRONE" : "M:PLANE");
      display.setCursor(54, 0);
      display.printf("CON:%d/%d", activeConnected, currentCount);
      display.setCursor(104, 0);
      display.print(currentProtocol == PROTOCOL_ATAK_COT ? "ATAK" : "JSON");
      display.drawFastHLine(0, 10, 128, SSD1306_WHITE);
     
      if (!loraHardwareReady) {
        display.setCursor(0, 56);
        display.print("W: LORA HW ERROR!");
      }


      switch(currentMenuPage.load()) {
        case PAGE_DASHBOARD: {
          if (activeConnected == 0) {
            display.setCursor(18, 22);
            display.print("No Active Drone");
            display.setCursor(15, 36);
            display.printf("Radar Scanned: %d", totalDiscovered.load());
          }
          else {
            if (lockedDroneIdx.load() == -1) {
              if (millis() - lastPageSwitch > 3000) {
                lastPageSwitch = millis();
                int attempts = 0;
                do {
                  autoRotateIdx = (autoRotateIdx + 1) % currentCount;
                  attempts++;
                } while ((autoRotateIdx < 0 || autoRotateIdx >= maxDrones || !localConnectionStatus[autoRotateIdx]) && attempts < currentCount);
              }
            } else {
              int localLocked = lockedDroneIdx.load();
              if (localLocked >= 0 && localLocked < maxDrones && localConnectionStatus[localLocked]) {
                autoRotateIdx = localLocked;
              } else {
                lockedDroneIdx.store(-1);
              }
            }


            if (autoRotateIdx >= 0 && autoRotateIdx < maxDrones && localConnectionStatus[autoRotateIdx]) {
              DroneTelemetry currentDrone;
              if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
                currentDrone = sharedSwarmData[autoRotateIdx];
                xSemaphoreGive(dataMutexSemaphore);
              }


              display.setCursor(0, 14);
              display.printf("%s%s", (lockedDroneIdx.load() != -1) ? "[LKD] " : "ID: ", localDroneIDs[autoRotateIdx]);
              display.setCursor(85, 14);
              display.printf("%d%%", (int)currentDrone.battery);
              display.drawRect(110, 14, 16, 8, SSD1306_WHITE);
              int battFill = (int)(14 * (currentDrone.battery / 100.0));
              if(battFill < 0) battFill = 0;
              if(battFill > 14) battFill = 14;
              display.fillRect(111, 15, battFill, 6, SSD1306_WHITE);


              display.setCursor(0, 26);
              display.printf("ALT: %.1f M", currentDrone.alt);
              display.setCursor(80, 26);
              display.print(currentDrone.alt > 1.8 ? "FLYING" : "GROUND");


              char latBuf[16];
              char lonBuf[16];
              dtostrf(currentDrone.lat, 2, 5, latBuf);
              dtostrf(currentDrone.lon, 2, 5, lonBuf);


              display.setCursor(0, 37);
              display.printf("LAT: %s", latBuf);
              display.setCursor(0, 46);
              display.printf("LON: %s", lonBuf);
            }
          }
          break;
        }


        case PAGE_CONNECTED_LIST: {
          display.setCursor(0, 13);
          display.print("--- CONNECTED LIST ---");
          int yPos = 24;
          for(int i = 0; i < currentCount; i++) {
            if (yPos > 46) break;
            if (localConnectionStatus[i]) {
              display.setCursor(0, yPos);
              display.printf("%s %s", (selectedMenuIdx == i) ? ">" : " ", localDroneIDs[i]);
              yPos += 11;
            }
          }
          if(activeConnected == 0) {
            display.setCursor(10, 32);
            display.print("No Drone Linked yet");
          }
          break;
        }


        case PAGE_RADAR_LIST: {
          display.setCursor(0, 13);
          display.print("--- RADAR SCANNER ---");
          int yPos = 24;
          int discCount = totalDiscovered.load();
          if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
            for(int i = 0; i < discCount; i++) {
              if (yPos > 46) break;
              if (strlen(discoveredDrones[i]) > 0) {
                display.setCursor(0, yPos);
                display.printf("%s %s", (selectedMenuIdx == i) ? ">" : " ", discoveredDrones[i]);
                yPos += 11;
              }
            }
            xSemaphoreGive(dataMutexSemaphore);
          }
          if(discCount == 0) {
            display.setCursor(10, 32);
            display.print("Scanning Airspace...");
          }
          break;
        }


        case PAGE_DRONE_SUBMENU: {
          int localSubIdx = targetSubMenuDroneIdx.load();
          if(localSubIdx >= 0 && localSubIdx < currentCount) {
            display.setCursor(0, 12);
            display.printf("CMD: %s", localDroneIDs[localSubIdx]);
            display.drawFastHLine(0, 21, 128, SSD1306_WHITE);


            const char* subMenuOptions[] = {
              "1. Track Lock Display",
              "2. Mode: MANUAL Flight",
              "3. Mode: AUTO Mission",
              "4. Cmd: Return Home(RTH)",
              "5. Cmd: DISCONNECT Drone",
              "6. Cmd: AUTO LANDING"
            };


            int startLoop = selectedSubMenuIdx - 1;
            if (startLoop < 0) startLoop = 0;
            if (startLoop > 3) startLoop = 3;


            int printY = 24;
            for(int k = startLoop; k < startLoop + 3 && k < 6; k++) {
              display.setCursor(0, printY);
              display.printf("%s%s", (selectedSubMenuIdx == k) ? ">" : " ", subMenuOptions[k]);
              printY += 12;
            }
          }
          break;
        }
      }
      display.display();
    }
    vTaskDelay(pdMS_TO_TICKS(40));
  }
}


void LoRaSwarmReceiverTask(void * pvParameters) {
  unsigned long lastHop = millis();
  char rxBuffer[128];
  char parseBuffer[128];
  char activeAckID[16] = {0}; // ⚡ INITIALIZED TO PREVENT GARBAGE DATA
 
  bool droneWasFlying[maxDrones] = {false};
  unsigned long lastBatteryWarningTime[maxDrones] = {0};


  for(;;) {
    if (!loraHardwareReady) {
      vTaskDelay(pdMS_TO_TICKS(100));
      continue;
    }


    if (triggerRegAckFlag.load() && !isTransmittingCommand.load()) {
      if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
        LoRa.idle();
        int localRegIndex = 0;
       
        if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
          strncpy(activeAckID, pendingRegAckID, sizeof(activeAckID) - 1);
          activeAckID[sizeof(activeAckID) - 1] = '\0';
          localRegIndex = regAckFreqIndex.load();
          xSemaphoreGive(dataMutexSemaphore);
        }


        if (localRegIndex >= 0 && localRegIndex < maxDrones) {
          LoRa.setFrequency(droneFrequencies[localRegIndex]);
        }


        LoRa.beginPacket();
        LoRa.print("REG_OK:");
        LoRa.print(activeAckID);
        LoRa.endPacket();
        LoRa.receive();
       
        Serial.printf("[REG SYSTEM] Acknowledged Node Registry for Node: %s\n", activeAckID);
       
        int localHop = currentHopIndex.load();
        if (localHop >= 0 && localHop < maxDrones) {
          LoRa.setFrequency(droneFrequencies[localHop]);
        }
        triggerRegAckFlag.store(false);
        xSemaphoreGive(loraMutex);
      }
    }


    if (!isTransmittingCommand.load() && !triggerRegAckFlag.load() && (millis() - lastCommandTxTime > 60)) {
      if (millis() - lastHop > 150) {
        if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
          lastHop = millis();
          int nextHop = (currentHopIndex.load() + 1) % maxDrones;
          currentHopIndex.store(nextHop);
          LoRa.setFrequency(droneFrequencies[nextHop]);
          LoRa.receive();
          xSemaphoreGive(loraMutex);
        }
      }
    }


    int packetSize = 0;
    if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(20)) == pdTRUE) {
      packetSize = LoRa.parsePacket();
      if (packetSize > 0) {
        int bytesRead = 0;
        int maxRead = (packetSize < 127) ? packetSize : 127;
       
        while (LoRa.available() && bytesRead < maxRead) {
          char incomingByte = (char)LoRa.read();
          if (incomingByte != '\0') {
            rxBuffer[bytesRead++] = incomingByte;
          }
        }
        rxBuffer[bytesRead] = '\0';
      }
      xSemaphoreGive(loraMutex);
    }


    if (packetSize > 0) {
      strncpy(parseBuffer, rxBuffer, sizeof(parseBuffer) - 1);
      parseBuffer[sizeof(parseBuffer) - 1] = '\0';


      if (strncmp(parseBuffer, "REG:", 4) == 0) {
        char newID[16];
        int idx = 0;
        for(int i = 4; parseBuffer[i] != '\0' && i < 128 && idx < 15; i++) {
          if(parseBuffer[i] != ' ' && parseBuffer[i] != '\r' && parseBuffer[i] != '\n') {
            newID[idx++] = parseBuffer[i];
          }
        }
        newID[idx] = '\0';


        if(strlen(newID) > 0) {
          bool exists = false;
          int currentCount = totalDrones.load();
          if (currentCount > maxDrones) currentCount = maxDrones;
          bool triggerRegAck = false;
          int assignedIndex = 0;
         
          if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(50)) == pdTRUE) {
            for (int i = 0; i < currentCount; i++) {
              if (strcmp(droneIDs[i], newID) == 0) { exists = true; break; }
            }
           
            if (!exists && currentCount < maxDrones && !triggerRegAckFlag.load()) {
              strncpy(droneIDs[currentCount], newID, 15);
              droneIDs[currentCount][15] = '\0';
              droneConnectionStatus[currentCount] = false;
              assignedIndex = currentCount;
              totalDrones.fetch_add(1);
              triggerRegAck = true;
             
              strncpy(pendingRegAckID, newID, sizeof(pendingRegAckID) - 1);
              pendingRegAckID[sizeof(pendingRegAckID) - 1] = '\0';
              regAckFreqIndex.store(assignedIndex);
              Serial.printf("[RF RADAR] Discovered Unregistered Swarm Signature: %s. Saving to Slots.\n", newID);
            }
            xSemaphoreGive(dataMutexSemaphore);
           
            if (triggerRegAck) {
              syncDronesToMemory();
              triggerRegAckFlag.store(true);
            }
          }
        }
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      char *strtokCtx = NULL;
      // ⚡ FIXED: 'sttokCtx' টাইপো এররটি সংশোধন করে 'strtokCtx' করা হয়েছে
      char *token = strtok_r(parseBuffer, ",", &strtokCtx);
      if (!token) { vTaskDelay(pdMS_TO_TICKS(5)); continue; }


      char parsedID[16];
      token = trimWhitespace(token);
      if(token) {
        strncpy(parsedID, token, 15);
        parsedID[15] = '\0';
      } else { vTaskDelay(pdMS_TO_TICKS(5)); continue; }


      char *latToken = strtok_r(NULL, ",", &strtokCtx);
      char *lonToken = strtok_r(NULL, ",", &strtokCtx);
      char *altToken = strtok_r(NULL, ",", &strtokCtx);
      char *battToken = strtok_r(NULL, ",", &strtokCtx);


      if (!latToken || !lonToken || !altToken || !battToken) {
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      latToken = trimWhitespace(latToken);
      lonToken = trimWhitespace(lonToken);
      altToken = trimWhitespace(altToken);
      battToken = trimWhitespace(battToken);


      float lat = atof(latToken);
      float lon = atof(lonToken);


      if (lat < -90.0 || lat > 90.0 || lon < -180.0 || lon > 180.0) {
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      bool isAllowed = false;
      int targetIndex = -1;
     
      if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(50)) == pdTRUE) {
        int currentCount = totalDrones.load();
        if (currentCount > maxDrones) currentCount = maxDrones;
        for(int i = 0; i < currentCount; i++) {
          if(strcmp(droneIDs[i], parsedID) == 0) {
            isAllowed = droneConnectionStatus[i];
            targetIndex = i;
            break;
          }
        }


        if (!isAllowed) {
          bool foundInDiscovered = false;
          int currentDiscoveredCount = totalDiscovered.load();
          if (currentDiscoveredCount > maxDrones) currentDiscoveredCount = maxDrones;
          for(int j = 0; j < currentDiscoveredCount; j++) {
            if(strcmp(discoveredDrones[j], parsedID) == 0) {
              lastSeenDiscovered[j] = millis();
              foundInDiscovered = true;
              break;
            }
          }
          if(!foundInDiscovered && currentDiscoveredCount < maxDrones) {
            strncpy(discoveredDrones[currentDiscoveredCount], parsedID, 15);
            discoveredDrones[currentDiscoveredCount][15] = '\0';
            lastSeenDiscovered[currentDiscoveredCount] = millis();
            totalDiscovered.fetch_add(1);
            Serial.printf("[RADAR TRACK] Added target to Live Airspace: %s\n", parsedID);
            sendAudioAnnouncement("New Radar Target");
          }
        }


        if (isAllowed && targetIndex >= 0 && targetIndex < maxDrones) {
          float alt = atof(altToken);
          float batt = atof(battToken);


          if (alt > 1.8 && !droneWasFlying[targetIndex]) {
            droneWasFlying[targetIndex] = true;
            sendAudioAnnouncement("Drone Taking Off");
          } else if (alt <= 1.5 && droneWasFlying[targetIndex]) {
            droneWasFlying[targetIndex] = false;
            sendAudioAnnouncement("Drone Landed");
          }


          if (batt < 20.0 && (millis() - lastBatteryWarningTime[targetIndex] > 30000)) {
            lastBatteryWarningTime[targetIndex] = millis();
            sendAudioAnnouncement("Warning Low Battery");
          }


          memset(sharedSwarmData[targetIndex].id, 0, sizeof(sharedSwarmData[targetIndex].id));
          strncpy(sharedSwarmData[targetIndex].id, parsedID, sizeof(sharedSwarmData[targetIndex].id) - 1);
          sharedSwarmData[targetIndex].id[sizeof(sharedSwarmData[targetIndex].id) - 1] = '\0';
          sharedSwarmData[targetIndex].lat = lat;
          sharedSwarmData[targetIndex].lon = lon;
          sharedSwarmData[targetIndex].alt = alt;
          sharedSwarmData[targetIndex].battery = batt;
          sharedSwarmData[targetIndex].freshData = true;
        }
        xSemaphoreGive(dataMutexSemaphore);
      }
    }
    vTaskDelay(pdMS_TO_TICKS(10));
  }
}


void connectDroneDirectly(const char* tID) {
  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(50)) == pdTRUE) {
    int currentCount = totalDrones.load();
    if (currentCount > maxDrones) currentCount = maxDrones;
    for(int i = 0; i < currentCount; i++) {
      if(strcmp(droneIDs[i], tID) == 0) {
        droneConnectionStatus[i] = true;
        sendAudioAnnouncement("Drone Connected");
        Serial.printf("[LINK HANDSHAKE] Connected secure uplink with Node: %s\n", tID);
       
        int currentDiscoveredCount = totalDiscovered.load();
        if (currentDiscoveredCount > maxDrones) currentDiscoveredCount = maxDrones;
        for(int j = 0; j < currentDiscoveredCount; j++) {
          if(strcmp(discoveredDrones[j], tID) == 0) {
            int lastIdx = currentDiscoveredCount - 1;
            if (j != lastIdx && lastIdx >= 0 && lastIdx < maxDrones) {
              memset(discoveredDrones[j], 0, 16);
              strncpy(discoveredDrones[j], discoveredDrones[lastIdx], 15);
              discoveredDrones[j][15] = '\0';
              lastSeenDiscovered[j] = lastSeenDiscovered[lastIdx];
            }
            if (lastIdx >= 0 && lastIdx < maxDrones) {
              memset(discoveredDrones[lastIdx], 0, 16);
              lastSeenDiscovered[lastIdx] = 0;
            }
            totalDiscovered.store(lastIdx >= 0 ? lastIdx : 0);
            break;
          }
        }
        break;
      }
    }
    xSemaphoreGive(dataMutexSemaphore);
    syncDronesToMemory();
    SerialBT.printf("BUTTON_SUCCESS:Connected -> %s\n", tID);
  }
}


void disconnectDroneDirectly(const char* tID) {
  bool disconnectedSuccessfully = false;
  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(50)) == pdTRUE) {
    int currentCount = totalDrones.load();
    if (currentCount > maxDrones) currentCount = maxDrones;
    for(int i = 0; i < currentCount; i++) {
      if(strcmp(droneIDs[i], tID) == 0) {
        droneConnectionStatus[i] = false;
        disconnectedSuccessfully = true;
        sendAudioAnnouncement("Connection Lost");
        Serial.printf("[LINK BREAK] Terminated secure uplink for Node: %s\n", tID);
        break;
      }
    }
    xSemaphoreGive(dataMutexSemaphore);
    if (disconnectedSuccessfully) {
      syncDronesToMemory();
      SerialBT.printf("BUTTON_SUCCESS:Disconnected -> %s\n", tID);
    }
  }
}


void sendLoRaCommandDirect(int targetIdx, const char* cmdType, const char* payload) {
  if (!loraHardwareReady || targetIdx < 0 || targetIdx >= maxDrones) return;
 
  char targetID[16] = {0};
  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(10)) == pdTRUE) {
    strncpy(targetID, droneIDs[targetIdx], sizeof(targetID) - 1);
    targetID[sizeof(targetID) - 1] = '\0';
    xSemaphoreGive(dataMutexSemaphore);
  }


  isTransmittingCommand.store(true);
  if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
    LoRa.idle();
    LoRa.setFrequency(droneFrequencies[targetIdx]);
    LoRa.beginPacket();
    LoRa.print("CMD:"); LoRa.print(targetID); LoRa.print(","); LoRa.print(cmdType); LoRa.print(","); LoRa.print(payload);
    LoRa.endPacket();
    LoRa.receive();
    lastCommandTxTime = millis();
    xSemaphoreGive(loraMutex);
    Serial.printf("[RF COMMAND] Dispatched packet to %s -> Type:%s Payload:%s\n", targetID, cmdType, payload);
    vTaskDelay(pdMS_TO_TICKS(10));
  }
  isTransmittingCommand.store(false);
}


void AppCommunicationTask(void * pvParameters) {
  unsigned long lastDashboardTime = 0;
  char btBuffer[256];
  char serBuffer[256];
  static int btIndex = 0;
  static int serIndex = 0;


  for(;;) {
    if (millis() - lastDashboardTime > 4000) {
      lastDashboardTime = millis();
      printLiveDashboard();
    }


    char cmd[256];
    cmd[0] = '\0';
    bool hasCmd = false;


    if (SerialBT.available()) {
      int firstByte = SerialBT.peek();
      if (firstByte == '<') { currentProtocol.store(PROTOCOL_ATAK_COT); }
      else if (firstByte == '{') { currentProtocol.store(PROTOCOL_JSON_RAW); }
     
      if (readLineNonBlocking(SerialBT, btBuffer, sizeof(btBuffer), &btIndex)) {
        strncpy(cmd, btBuffer, sizeof(cmd)-1);
        cmd[sizeof(cmd)-1] = '\0';
        hasCmd = true;
      }
    }
   
    if (!hasCmd && Serial.available()) {
      int firstByte = Serial.peek();
      if (firstByte == '<') { currentProtocol.store(PROTOCOL_ATAK_COT); }
      else if (firstByte == '{') { currentProtocol.store(PROTOCOL_JSON_RAW); }
     
      if (readLineNonBlocking(Serial, serBuffer, sizeof(serBuffer), &serIndex)) {
        strncpy(cmd, serBuffer, sizeof(serBuffer)-1);
        cmd[sizeof(cmd)-1] = '\0';
        hasCmd = true;
      }
    }


    if (hasCmd) {
      int len = strlen(cmd);
      while(len > 0 && (cmd[len-1] == ' ' || cmd[len-1] == '\r' || cmd[len-1] == '\n')) {
        cmd[len-1] = '\0';
        len--;
      }


      Serial.printf("[COM INPUT] Received inbound frame: %s\n", cmd);


      if (strcmp(cmd, "EMERGENCY_STOP") == 0) { triggerGlobalEmergencyStop(); continue; }
      if (strcmp(cmd, "RESET_GATEWAY") == 0) { factoryResetGateway(); continue; }
      if (strncmp(cmd, "JOY:", 4) == 0) { processDirectJoystickRaw(cmd); continue; }


      if (strncmp(cmd, "CONNECT:", 8) == 0) {
        const char* dataStr = cmd + 8;
        char* commaIndex = (char*)strchr(dataStr, ',');
        if (commaIndex != NULL) {
          char targetID[16];
          char enteredPassword[16];
          int idLength = commaIndex - dataStr;
          if (idLength > 15) idLength = 15;
          strncpy(targetID, dataStr, idLength);
          targetID[idLength] = '\0';
          strncpy(enteredPassword, commaIndex + 1, sizeof(enteredPassword)-1);
          enteredPassword[sizeof(enteredPassword)-1] = '\0';
         
          if (strcmp(enteredPassword, MASTER_PASSWORD) == 0) {
            connectDroneDirectly(targetID);
          } else {
            SerialBT.println(F("ERROR:Invalid Password!"));
            Serial.println(F("[SECURITY WARNING] Breach Attempt! Wrong App Password."));
          }
        }
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      else if (strncmp(cmd, "DISCONNECT:", 11) == 0) {
        disconnectDroneDirectly(cmd + 11);
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      else if (strncmp(cmd, "MOVE:", 5) == 0) {
        const char* dataStr = cmd + 5;
        char* commaIndex = (char*)strchr(dataStr, ',');
        if (commaIndex != NULL) {
          char targetID[16];
          int idLength = commaIndex - dataStr;
          if(idLength > 15) idLength = 15;
          strncpy(targetID, dataStr, idLength);
          targetID[idLength] = '\0';
          const char* direction = commaIndex + 1;


          int currentCount = totalDrones.load();
          if (currentCount > maxDrones) currentCount = maxDrones;
          bool isTargetConnected = false;
          int targetIdx = -1;
         
          if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(20)) == pdTRUE) {
            for (int i = 0; i < currentCount; i++) {
              if (strcmp(droneIDs[i], targetID) == 0 && droneConnectionStatus[i] == true) {
                isTargetConnected = true; targetIdx = i; break;
              }
            }
            xSemaphoreGive(dataMutexSemaphore);
          }
          if (isTargetConnected && targetIdx >= 0) {
            sendLoRaCommandDirect(targetIdx, "MOVE", direction);
            if (strcmp(direction, "LEFT") == 0) sendAudioAnnouncement("Move Left");
            else if (strcmp(direction, "RIGHT") == 0) sendAudioAnnouncement("Move Right");
          }
        }
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      else if (strncmp(cmd, "ALT:", 4) == 0) {
        const char* dataStr = cmd + 4;
        char* commaIndex = (char*)strchr(dataStr, ',');
        if (commaIndex != NULL) {
          char targetID[16];
          int idLength = commaIndex - dataStr;
          if(idLength > 15) idLength = 15;
          strncpy(targetID, dataStr, idLength);
          targetID[idLength] = '\0';
          const char* action = commaIndex + 1;


          int currentCount = totalDrones.load();
          if (currentCount > maxDrones) currentCount = maxDrones;
          bool isTargetConnected = false;
          int targetIdx = -1;
         
          if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(20)) == pdTRUE) {
            for (int i = 0; i < currentCount; i++) {
              if (strcmp(droneIDs[i], targetID) == 0 && droneConnectionStatus[i] == true) {
                isTargetConnected = true; targetIdx = i; break;
              }
            }
            xSemaphoreGive(dataMutexSemaphore);
          }
          if (isTargetConnected && targetIdx >= 0) {
            sendLoRaCommandDirect(targetIdx, "ALT", action);
            if (strcmp(action, "UP") == 0) sendAudioAnnouncement("Move Up");
            else if (strcmp(action, "DOWN") == 0) sendAudioAnnouncement("Move Down");
          }
        }
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      else if (strncmp(cmd, "FLIGHT:", 7) == 0) {
        const char* dataStr = cmd + 7;
        char* commaIndex = (char*)strchr(dataStr, ',');
        if (commaIndex != NULL) {
          char targetID[16];
          int idLength = commaIndex - dataStr;
          if(idLength > 15) idLength = 15;
          strncpy(targetID, dataStr, idLength);
          targetID[idLength] = '\0';
          const char* flightCmd = commaIndex + 1;


          int currentCount = totalDrones.load();
          if (currentCount > maxDrones) currentCount = maxDrones;
          bool isTargetConnected = false;
          int targetIdx = -1;
         
          if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(20)) == pdTRUE) {
            for (int i = 0; i < currentCount; i++) {
              if (strcmp(droneIDs[i], targetID) == 0 && droneConnectionStatus[i] == true) {
                isTargetConnected = true; targetIdx = i; break;
              }
            }
            xSemaphoreGive(dataMutexSemaphore);
          }
          if (isTargetConnected && targetIdx >= 0) {
            sendLoRaCommandDirect(targetIdx, "FLIGHT", flightCmd);
          }
        }
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      else if (strncmp(cmd, "MODE_AUTO:", 10) == 0) {
        const char* dataStr = cmd + 10;
        char* commaIndex = (char*)strchr(dataStr, ',');
        if (commaIndex != NULL) {
          char targetID[16];
          int idLength = commaIndex - dataStr;
          if(idLength > 15) idLength = 15;
          strncpy(targetID, dataStr, idLength);
          targetID[idLength] = '\0';
          const char* missionData = commaIndex + 1;


          int currentCount = totalDrones.load();
          if (currentCount > maxDrones) currentCount = maxDrones;
          bool isTargetConnected = false;
          int targetIdx = -1;
         
          if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(20)) == pdTRUE) {
            for (int i = 0; i < currentCount; i++) {
              if (strcmp(droneIDs[i], targetID) == 0 && droneConnectionStatus[i] == true) {
                isTargetConnected = true; targetIdx = i; break;
              }
            }
            xSemaphoreGive(dataMutexSemaphore);
          }


          if (isTargetConnected && targetIdx >= 0 && targetIdx < maxDrones && loraHardwareReady) {
            isTransmittingCommand.store(true);
            if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(100)) == pdTRUE) {
              LoRa.idle();
              LoRa.setFrequency(droneFrequencies[targetIdx]);
              LoRa.beginPacket();
              LoRa.print("ROUTE:"); LoRa.print(targetID); LoRa.print(","); LoRa.print(missionData);
              LoRa.endPacket();
              LoRa.receive();
              lastCommandTxTime = millis();
              xSemaphoreGive(loraMutex);
              Serial.printf("[ROUTE ENCODER] Streamed autonomous grid paths to %s\n", targetID);
              vTaskDelay(pdMS_TO_TICKS(10));
            }
            isTransmittingCommand.store(false);
          }
        }
        vTaskDelay(pdMS_TO_TICKS(5));
        continue;
      }


      if (strcmp(cmd, "SET_MODE_ATAK") == 0) { currentProtocol.store(PROTOCOL_ATAK_COT); Serial.println(F("[PROTOCOL] Changed Stream to ATAK CoT.")); }
      else if (strcmp(cmd, "SET_MODE_JSON") == 0) { currentProtocol.store(PROTOCOL_JSON_RAW); Serial.println(F("[PROTOCOL] Changed Stream to Raw JSON.")); }
    }


    DroneTelemetry localBuffer[maxDrones];
    int currentCount = 0;
    ProtocolType localProtocol = PROTOCOL_ATAK_COT;


    if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(20)) == pdTRUE) {
      currentCount = totalDrones.load();
      if (currentCount > maxDrones) currentCount = maxDrones;
      localProtocol = currentProtocol.load();
      for (int i = 0; i < currentCount; i++) {
        localBuffer[i] = sharedSwarmData[i];
        if (droneConnectionStatus[i] && sharedSwarmData[i].freshData) {
          sharedSwarmData[i].freshData = false;
        } else {
          localBuffer[i].freshData = false;
        }
      }
      xSemaphoreGive(dataMutexSemaphore);
    }


    for (int i = 0; i < currentCount; i++) {
      if (localBuffer[i].freshData) {
        switch (localProtocol) {
          case PROTOCOL_ATAK_COT: {
            SerialBT.print("<event version='2.0' uid='"); SerialBT.print(localBuffer[i].id); SerialBT.print("' type='a-f-A-M-F' how='m-g'>");
            SerialBT.print("<point lat='"); SerialBT.print(localBuffer[i].lat, 6); SerialBT.print("' lon='"); SerialBT.print(localBuffer[i].lon, 6);
            SerialBT.print("' hae='"); SerialBT.print(localBuffer[i].alt, 1); SerialBT.print("' ce='1.0' le='1.0'/>");
            SerialBT.println("</event>");
            break;
          }
          case PROTOCOL_JSON_RAW: {
            SerialBT.print("{\"id\":\""); SerialBT.print(localBuffer[i].id); SerialBT.print("\",\"lat\":"); SerialBT.print(localBuffer[i].lat, 6);
            SerialBT.print(",\"lon\":"); SerialBT.print(localBuffer[i].lon, 6); SerialBT.print(",\"alt\":"); SerialBT.print(localBuffer[i].alt, 1);
            SerialBT.print(",\"batt\":"); SerialBT.print(localBuffer[i].battery, 1); SerialBT.println("}");
            break;
          }
        }
      }
    }
    vTaskDelay(pdMS_TO_TICKS(15));
  }
}


void syncDronesToMemory() {
  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(50)) == pdTRUE) {
    preferences.begin("swarm_gate", false);
    int currentCount = totalDrones.load();
    preferences.putInt("totalDrones", currentCount);
   
    for (int i = 0; i < currentCount; i++) {
      char idKey[16];      
      char statusKey[16];  
      snprintf(idKey, sizeof(idKey), "d_%d", i);
      snprintf(statusKey, sizeof(statusKey), "s_%d", i);
     
      String currentSavedID = preferences.getString(idKey, "");
      bool currentSavedStatus = preferences.getBool(statusKey, false);


      if (currentSavedID != String(droneIDs[i])) {
        preferences.putString(idKey, String(droneIDs[i]));
      }
      if (currentSavedStatus != droneConnectionStatus[i]) {
        preferences.putBool(statusKey, droneConnectionStatus[i]);
      }
    }
    preferences.end();
    xSemaphoreGive(dataMutexSemaphore);
  }
  Serial.println(F("[SYSTEM-EEPROM] Database State Sync Completed."));
}


void printLiveDashboard() {
  // ⚡ STATE TRACKING VARIABLES TO PREVENT PROCESSOR OVERHEAD
  static int lastLoggedActiveCount = -1;
  static int lastLoggedDiscCount = -1;
  static bool firstRun = true;


  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(50)) == pdTRUE) {
    int currentDiscoveredCount = totalDiscovered.load();
    if (currentDiscoveredCount > maxDrones) currentDiscoveredCount = maxDrones;
   
    for(int j = 0; j < currentDiscoveredCount; j++) {
      if(millis() - lastSeenDiscovered[j] > 12000) {
        int lastIdx = currentDiscoveredCount - 1;
        if (j != lastIdx && lastIdx >= 0 && lastIdx < maxDrones) {
          memset(discoveredDrones[j], 0, 16);
          strncpy(discoveredDrones[j], discoveredDrones[lastIdx], 15);
          discoveredDrones[j][15] = '\0';
          lastSeenDiscovered[j] = lastSeenDiscovered[lastIdx];
        }
        if (lastIdx >= 0 && lastIdx < maxDrones) {
          memset(discoveredDrones[lastIdx], 0, 16);
          lastSeenDiscovered[lastIdx] = 0;
        }
        totalDiscovered.store(lastIdx >= 0 ? lastIdx : 0);
        currentDiscoveredCount--;
        j--;
      }
    }


    int currentCount = totalDrones.load();
    if (currentCount > maxDrones) currentCount = maxDrones;
   
    int loggedActive = 0;
    for(int i = 0; i < currentCount; i++) {
      if(droneConnectionStatus[i]) {
        loggedActive++;
      }
    }


    // ⚡ STATE CHANGE LOGIC: ডাটার পরিবর্তন হলেই কেবল প্রিন্ট হবে
    if (firstRun || loggedActive != lastLoggedActiveCount || currentDiscoveredCount != lastLoggedDiscCount) {
     
      Serial.println(F("\n==================================="));
      Serial.println(F("🛸 SWARM RADAR SCANNER DASHBOARD 🛸"));
      Serial.println(F("==================================="));
     
      int actualActivePrint = 0;
      for(int i = 0; i < currentCount; i++) {
        if(droneConnectionStatus[i]) {
          Serial.printf("Active Tracking: %s\n", droneIDs[i]);
          actualActivePrint++;
        }
      }
     
      if (actualActivePrint == 0) {
        Serial.println(F("[INFO] No active localized links established."));
      }
     
      Serial.printf("Peripheral Scan Count: %d Nodes Found\n", currentDiscoveredCount);
      Serial.println(F("==================================="));


      // নতুন স্টেট সেভ করে রাখা হচ্ছে পরবর্তী লুপের জন্য
      lastLoggedActiveCount = loggedActive;
      lastLoggedDiscCount = currentDiscoveredCount;
      firstRun = false;
    }
   
    xSemaphoreGive(dataMutexSemaphore);
  }
}


void triggerGlobalEmergencyStop() {
  if (!loraHardwareReady) return;
 
  isTransmittingCommand.store(true);
  sendAudioAnnouncement("Emergency Stop");
  Serial.println(F("[CRITICAL INTERCEPT] EMERGENCY STOP SIGNALS DISPATCHED CRITICAL FREQS!"));
 
  for(int i = 0; i < maxDrones; i++) {
    for(int j = 0; j < 3; j++) {
      if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
        LoRa.idle();
        LoRa.setFrequency(droneFrequencies[i]);
        LoRa.beginPacket();
        LoRa.print("BCAST:EMERGENCY_HOLD");
        LoRa.endPacket();
        xSemaphoreGive(loraMutex);
      }
      vTaskDelay(pdMS_TO_TICKS(5));
    }
  }


  if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(50)) == pdTRUE) {
    int localHop = currentHopIndex.load();
    if (localHop >= 0 && localHop < maxDrones) {
      LoRa.setFrequency(droneFrequencies[localHop]);
    }
    LoRa.receive();
    xSemaphoreGive(loraMutex);
  }
 
  isTransmittingCommand.store(false);
}


void factoryResetGateway() {
  Serial.println(F("[FACTORY RESET] Clearing EEPROM Namespaces and Rebooting..."));
  sendAudioAnnouncement("Reset System");
  vTaskDelay(pdMS_TO_TICKS(1000));
  preferences.begin("swarm_gate", false);
  preferences.clear();
  preferences.end();
  vTaskDelay(pdMS_TO_TICKS(500));
  ESP.restart();
}


void processDirectJoystickRaw(const char* rawCmd) {
  if (!loraHardwareReady || strncmp(rawCmd, "JOY:", 4) != 0) return;
  const char* joyData = rawCmd + 4;
 
  int currentCount = totalDrones.load();
  if (currentCount > maxDrones) currentCount = maxDrones;


  char tempIDs[maxDrones][16];
  bool tempStatus[maxDrones];
 
  if (dataMutexSemaphore != NULL && xSemaphoreTake(dataMutexSemaphore, pdMS_TO_TICKS(20)) == pdTRUE) {
    for (int i = 0; i < currentCount; i++) {
      tempStatus[i] = droneConnectionStatus[i];
      if (tempStatus[i]) {
        strncpy(tempIDs[i], droneIDs[i], 15);
        tempIDs[i][15] = '\0';
      }
    }
    xSemaphoreGive(dataMutexSemaphore);
  }
 
  isTransmittingCommand.store(true);
 
  if (loraMutex != NULL && xSemaphoreTake(loraMutex, pdMS_TO_TICKS(100)) == pdTRUE) {
    LoRa.idle();
    for (int i = 0; i < currentCount; i++) {
      if (tempStatus[i]) {
        LoRa.setFrequency(droneFrequencies[i]);
        LoRa.beginPacket();
        LoRa.print("J:"); LoRa.print(tempIDs[i]); LoRa.print(","); LoRa.print(joyData);
        LoRa.endPacket();
      }
    }
    int localHop = currentHopIndex.load();
    if (localHop >= 0 && localHop < maxDrones) {
      LoRa.setFrequency(droneFrequencies[localHop]);
    }
    LoRa.receive();
    lastCommandTxTime = millis();
    xSemaphoreGive(loraMutex);
  }
 
  isTransmittingCommand.store(false);
}


bool readLineNonBlocking(Stream &stream, char *buffer, int maxLength, int *index) {
  while (stream.available()) {
    char c = stream.read();
    if (c == '\n') {
      buffer[*index] = '\0';
      *index = 0;
      return true;
    } else if (c != '\r') {
      if (*index < maxLength - 1) {
        buffer[*index] = c;
        (*index)++;
      } else {
        buffer[maxLength - 1] = '\0';
        *index = 0;
        return true;
      }
    }
  }
  return false;
}