#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <inttypes.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"

#include "esp_system.h"
#include "esp_event.h"
#include "esp_log.h"
#include "esp_wifi.h"
#include "esp_console.h"
#include "linenoise/linenoise.h"

#include "nvs_flash.h"
#include "esp_netif.h"
#include "lwip/inet.h"

#include "led_strip.h"

// Custom Components
#include "iperf.h"
#include "wifi_cfg.h"
#include "csi_log.h"
#include "wifi_monitor.h"
#include "gps_sync.h"
// Note: cmd_transport is initialized by wifi_cfg_init, so we don't need to include it directly here unless we use it.

static const char *TAG = "MAIN";

// --- Hardware Configuration ---
#if defined(CONFIG_IDF_TARGET_ESP32S3)
    // ESP32-S3 Specific Wiring
    #define RGB_LED_GPIO     48
    #define GPS_TX_PIN       GPIO_NUM_5
    #define GPS_RX_PIN       GPIO_NUM_4
    #define GPS_PPS_PIN      GPIO_NUM_6

#elif defined(CONFIG_IDF_TARGET_ESP32C5)
    // ESP32-C5 Specific Wiring
    #define RGB_LED_GPIO     27
    #define GPS_TX_PIN       GPIO_NUM_24
    #define GPS_RX_PIN       GPIO_NUM_23
    #define GPS_PPS_PIN      GPIO_NUM_25

#elif defined(CONFIG_IDF_TARGET_ESP32)
    // ESP32 (Original) Specific Wiring
    // Note: ESP32 has no GPIO 24. GPIO 8 causes flash crash.
    #define RGB_LED_GPIO     2          // Often onboard Blue LED (valid GPIO)
    #define GPS_TX_PIN       GPIO_NUM_17 // Standard UART2 TX
    #define GPS_RX_PIN       GPIO_NUM_16 // Standard UART2 RX
    #define GPS_PPS_PIN      GPIO_NUM_4

#else
    // Fallback / Other Chips (C6, etc.)
    #define RGB_LED_GPIO     8
    #define GPS_TX_PIN       GPIO_NUM_1
    #define GPS_RX_PIN       GPIO_NUM_3
    #define GPS_PPS_PIN      GPIO_NUM_5
#endif

// --- WiFi Operation Mode ---
typedef enum {
    WIFI_MODE_STA_CSI,      // STA mode: Connected to AP, CSI + iperf (DEFAULT)
    WIFI_MODE_MONITOR       // Monitor mode: Promiscuous, collapse detection
} wifi_operation_mode_t;

static wifi_operation_mode_t current_wifi_mode = WIFI_MODE_STA_CSI;
static wifi_band_mode_t preferred_band = WIFI_BAND_MODE_AUTO;
static uint8_t monitor_channel = 6;  // Default monitor channel

// --- LED State Machine ---
static led_strip_handle_t led_strip;
static bool wifi_connected = false;
static bool has_config = false;

typedef enum {
    LED_STATE_NO_CONFIG,   // Yellow Solid
    LED_STATE_WAITING,     // Blue Blink (Connecting)
    LED_STATE_CONNECTED,   // Green Solid (Connected to AP)
    LED_STATE_FAILED,      // Red Blink
    LED_STATE_MONITORING   // Blue Solid (Sniffing Air)
} led_state_t;

static led_state_t current_led_state = LED_STATE_NO_CONFIG;

// --- Forward Declarations ---
static void auto_monitor_task(void *arg);

static void rgb_led_init(void) {
    ESP_LOGI(TAG, "Initializing RGB LED on GPIO %d", RGB_LED_GPIO);
    led_strip_config_t strip_config = {
        .strip_gpio_num = RGB_LED_GPIO,
        .max_leds = 1,
    };
    led_strip_rmt_config_t rmt_config = {
        .resolution_hz = 10 * 1000 * 1000,
        .flags.with_dma = false,
    };
    ESP_ERROR_CHECK(led_strip_new_rmt_device(&strip_config, &rmt_config, &led_strip));
    led_strip_clear(led_strip);
}

static void set_led_color(uint8_t r, uint8_t g, uint8_t b) {
    led_strip_set_pixel(led_strip, 0, r, g, b);
    led_strip_refresh(led_strip);
}

static void led_task(void *arg) {
    int blink_state = 0;
    while(1) {
        switch(current_led_state) {
            case LED_STATE_NO_CONFIG:
                set_led_color(25, 25, 0); // Yellow (Dimmed)
                vTaskDelay(pdMS_TO_TICKS(1000));
                break;

            case LED_STATE_WAITING:
                if (blink_state) set_led_color(0, 0, 50); // Blue
                else set_led_color(0, 0, 0);
                blink_state = !blink_state;
                vTaskDelay(pdMS_TO_TICKS(500));
                break;

            case LED_STATE_CONNECTED:
                set_led_color(0, 25, 0); // Green
                vTaskDelay(pdMS_TO_TICKS(1000));
                break;

            case LED_STATE_MONITORING:
                set_led_color(0, 0, 50); // Blue Solid
                vTaskDelay(pdMS_TO_TICKS(1000));
                break;

            case LED_STATE_FAILED:
                if (blink_state) set_led_color(50, 0, 0); // Red
                else set_led_color(0, 0, 0);
                blink_state = !blink_state;
                vTaskDelay(pdMS_TO_TICKS(200));
                break;
        }
    }
}

// --- GPS Logging Helper ---
void log_collapse_event(float nav_duration_us, int rssi, int retry) {
    gps_timestamp_t ts = gps_get_timestamp();
    // Format: COLLAPSE,MonoMS,GpsMS,Synced,Duration,RSSI,Retry
    printf("COLLAPSE,%" PRIi64 ",%" PRIi64 ",%d,%.2f,%d,%d\n",
           ts.monotonic_ms,
           ts.gps_ms,
           ts.synced ? 1 : 0,
           nav_duration_us,
           rssi,
           retry);
}

// --- CSI Support ---------------------------------------------------
static bool s_csi_enabled = false;
static uint32_t s_csi_packet_count = 0;

static void csi_cb(void *ctx, wifi_csi_info_t *info) {
    csi_log_append_record(info);
    s_csi_packet_count++;
    if ((s_csi_packet_count % 100) == 0) {
        ESP_LOGI("CSI", "Captured %lu CSI packets", (unsigned long)s_csi_packet_count);
    }
}

static void wifi_enable_csi_once(void) {
    if (s_csi_enabled) return;

    vTaskDelay(pdMS_TO_TICKS(2000));

    // Initialize with defaults (safe for all chips)
    wifi_csi_config_t csi_cfg = { 0 };

#if defined(CONFIG_IDF_TARGET_ESP32S3) || defined(CONFIG_IDF_TARGET_ESP32) || defined(CONFIG_IDF_TARGET_ESP32S2)
    // These fields ONLY exist on Xtensa-based chips (S3, S2, Original)
    // The ESP32-C5/C6 (RISC-V) hardware handles this automatically/internally.
    csi_cfg.lltf_en = true;
    csi_cfg.htltf_en = true;
    csi_cfg.stbc_htltf2_en = true;
    csi_cfg.ltf_merge_en = true;
    csi_cfg.channel_filter_en = true;
    csi_cfg.manu_scale = false;
    csi_cfg.shift = false;
#endif

    ESP_LOGI("CSI", "Configuring CSI...");
    if (esp_wifi_set_csi_config(&csi_cfg) != ESP_OK) {
        ESP_LOGE("CSI", "Failed to set CSI config");
        return;
    }

    if (esp_wifi_set_csi_rx_cb(csi_cb, NULL) != ESP_OK) {
        ESP_LOGE("CSI", "Failed to set CSI callback");
        return;
    }

    // Explicit enable call
    if (esp_wifi_set_csi(true) != ESP_OK) {
        ESP_LOGE("CSI", "Failed to enable CSI");
        return;
    }

    ESP_LOGI("CSI", "CSI enabled!");
    s_csi_enabled = true;
}

static void wifi_disable_csi(void) {
    if (!s_csi_enabled) return;

    ESP_LOGI("CSI", "Disabling CSI...");
    esp_wifi_set_csi(false);
    s_csi_enabled = false;
    ESP_LOGI("CSI", "CSI disabled");
}

static void csi_dump_task(void *arg) {
    vTaskDelay(pdMS_TO_TICKS(20000)); // Dump after 20 seconds
    ESP_LOGI("CSI", "Dumping CSI data...");
    csi_log_dump_over_uart();
    ESP_LOGI("CSI", "CSI dump complete");
    vTaskDelete(NULL);
}

// --- WiFi Monitor Mode Support -------------------------------------
static bool s_monitor_enabled = false;
static uint32_t s_monitor_frame_count = 0;
static TaskHandle_t s_monitor_stats_task_handle = NULL;

static void monitor_frame_callback(const wifi_frame_info_t *frame,
                                   const uint8_t *payload,
                                   uint16_t len) {
    s_monitor_frame_count++;

    // 1. Check for Collapse (High NAV + Retry)
    if (frame->retry && frame->duration_id > 5000) {
        log_collapse_event((float)frame->duration_id, frame->rssi, frame->retry);
    }

    // 2. Warn on extremely high NAV
    if (frame->duration_id > 30000) {
        ESP_LOGW("MONITOR", "⚠️  VERY HIGH NAV: %u us", frame->duration_id);
    }
}

static void monitor_stats_task(void *arg) {
    while (1) {
        vTaskDelay(pdMS_TO_TICKS(10000)); // Every 10 seconds
        wifi_collapse_stats_t stats;
        if (wifi_monitor_get_stats(&stats) == ESP_OK) {
            ESP_LOGI("MONITOR", "--- Stats: %lu frames, Retry Rate: %.2f%%, Avg NAV: %u us ---",
                     (unsigned long)stats.total_frames, stats.retry_rate, stats.avg_nav);

            if (wifi_monitor_is_collapsed()) {
                ESP_LOGW("MONITOR", "⚠️ ⚠️ ⚠️  WiFi COLLAPSE DETECTED! ⚠️ ⚠️ ⚠️ ");
            }
        }
    }
}

// --- Mode Switching Functions --------------------------------------

esp_err_t switch_to_monitor_mode(uint8_t channel, wifi_bandwidth_t bandwidth) {
    if (current_wifi_mode == WIFI_MODE_MONITOR) {
        ESP_LOGW(TAG, "Already in monitor mode");
        return ESP_OK;
    }

    if (bandwidth != WIFI_BW_HT20) {
        ESP_LOGW(TAG, "Monitor mode typically restricted to 20MHz capture width");
        ESP_LOGW(TAG, "Forcing bandwidth to 20MHz (driver limitation)");
        bandwidth = WIFI_BW_HT20;
    }

    const char* band_str = "2.4GHz";
    if (channel >= 36 && channel <= 165) {
        band_str = "5GHz";
    }

    const char* bw_str = "20MHz";

    ESP_LOGI(TAG, "========================================");
    ESP_LOGI(TAG, "Switching to MONITOR MODE");
    ESP_LOGI(TAG, "  Channel: %d (%s)", channel, band_str);
    ESP_LOGI(TAG, "  Bandwidth: %s (monitor mode limitation)", bw_str);
    ESP_LOGI(TAG, "========================================");

    ESP_LOGI(TAG, "Stopping iperf...");
    iperf_stop();
    vTaskDelay(pdMS_TO_TICKS(500));

    wifi_disable_csi();
    vTaskDelay(pdMS_TO_TICKS(500));

    ESP_LOGI(TAG, "Disconnecting from AP...");
    esp_wifi_disconnect();
    vTaskDelay(pdMS_TO_TICKS(1000));

    ESP_LOGI(TAG, "Stopping WiFi...");
    esp_wifi_stop();
    vTaskDelay(pdMS_TO_TICKS(500));

    ESP_LOGI(TAG, "Setting WiFi mode to NULL...");
    esp_wifi_set_mode(WIFI_MODE_NULL);
    vTaskDelay(pdMS_TO_TICKS(500));

    ESP_LOGI(TAG, "Starting monitor mode...");
    if (wifi_monitor_init(channel, monitor_frame_callback) != ESP_OK) {
        ESP_LOGE(TAG, "Failed to init monitor mode");
        return ESP_FAIL;
    }

    esp_wifi_set_bandwidth(WIFI_IF_STA, bandwidth);

    if (wifi_monitor_start() != ESP_OK) {
        ESP_LOGE(TAG, "Failed to start monitor mode");
        return ESP_FAIL;
    }

    s_monitor_enabled = true;
    current_wifi_mode = WIFI_MODE_MONITOR;
    current_led_state = LED_STATE_MONITORING;
    monitor_channel = channel;

    if (s_monitor_stats_task_handle == NULL) {
        xTaskCreate(monitor_stats_task, "monitor_stats", 4096, NULL, 5, &s_monitor_stats_task_handle);
    }

    ESP_LOGI(TAG, "✓ Monitor mode active");
    ESP_LOGI(TAG, "  - Channel: %d (%s)", channel, band_str);
    ESP_LOGI(TAG, "  - Bandwidth: %s", bw_str);
    ESP_LOGI(TAG, "  - Logging GPS-timestamped collapse events");
    ESP_LOGI(TAG, "  - LED: Blue solid");
    ESP_LOGI(TAG, "========================================");

    return ESP_OK;
}

esp_err_t switch_to_sta_mode(wifi_band_mode_t band_mode) {
    if (current_wifi_mode == WIFI_MODE_STA_CSI) {
        ESP_LOGW(TAG, "Already in STA mode");
        return ESP_OK;
    }

    const char* band_str = "Auto (2.4GHz or 5GHz)";
    if (band_mode == WIFI_BAND_MODE_2G_ONLY) {
        band_str = "2.4GHz only";
    } else if (band_mode == WIFI_BAND_MODE_5G_ONLY) {
        band_str = "5GHz only";
    }

    ESP_LOGI(TAG, "========================================");
    ESP_LOGI(TAG, "Switching to STA MODE (CSI + iperf)");
    ESP_LOGI(TAG, "  Band preference: %s", band_str);
    ESP_LOGI(TAG, "========================================");

    preferred_band = band_mode;

    if (s_monitor_stats_task_handle != NULL) {
        vTaskDelete(s_monitor_stats_task_handle);
        s_monitor_stats_task_handle = NULL;
    }

    if (s_monitor_enabled) {
        ESP_LOGI(TAG, "Stopping monitor mode...");
        wifi_monitor_stop();
        s_monitor_enabled = false;
        vTaskDelay(pdMS_TO_TICKS(500));
    }

    ESP_LOGI(TAG, "Setting WiFi mode to STA...");
    esp_wifi_set_mode(WIFI_MODE_STA);
    vTaskDelay(pdMS_TO_TICKS(500));

    wifi_config_t wifi_config;
    esp_wifi_get_config(WIFI_IF_STA, &wifi_config);

    if (band_mode == WIFI_BAND_MODE_2G_ONLY) {
        wifi_config.sta.channel = 0;
        wifi_config.sta.scan_method = WIFI_ALL_CHANNEL_SCAN;
        ESP_LOGI(TAG, "Configured for 2.4GHz band");
    } else if (band_mode == WIFI_BAND_MODE_5G_ONLY) {
        wifi_config.sta.channel = 0;
        wifi_config.sta.scan_method = WIFI_ALL_CHANNEL_SCAN;
        ESP_LOGI(TAG, "Configured for 5GHz band preference");
    } else {
        wifi_config.sta.channel = 0;
        wifi_config.sta.scan_method = WIFI_ALL_CHANNEL_SCAN;
        ESP_LOGI(TAG, "Configured for auto band selection");
    }

    esp_wifi_set_config(WIFI_IF_STA, &wifi_config);

    ESP_LOGI(TAG, "Starting WiFi...");
    esp_wifi_start();
    vTaskDelay(pdMS_TO_TICKS(500));

    ESP_LOGI(TAG, "Connecting to AP...");
    esp_wifi_connect();

    current_wifi_mode = WIFI_MODE_STA_CSI;
    current_led_state = LED_STATE_WAITING;
    wifi_connected = false;

    ESP_LOGI(TAG, "✓ Reconnecting to AP...");
    ESP_LOGI(TAG, "  - Band: %s", band_str);
    ESP_LOGI(TAG, "  - Waiting for IP address");
    ESP_LOGI(TAG, "  - CSI and iperf will start after connection");
    ESP_LOGI(TAG, "========================================");

    return ESP_OK;
}

// --- Console Commands ----------------------------------------------

static int cmd_mode_monitor(int argc, char **argv) {
    int channel = monitor_channel;
    wifi_bandwidth_t bandwidth = WIFI_BW_HT20;

    if (argc > 1) {
        char *slash = strchr(argv[1], '/');
        if (slash != NULL) {
            *slash = '\0';
            channel = atoi(argv[1]);
            int bw = atoi(slash + 1);
            switch(bw) {
                case 20: bandwidth = WIFI_BW_HT20; break;
                case 40: bandwidth = WIFI_BW_HT40; break;
                default: printf("Error: Invalid bandwidth %d\n", bw); return 1;
            }
        } else {
            channel = atoi(argv[1]);
            bandwidth = WIFI_BW_HT20;
        }

        bool valid = false;
        if (channel >= 1 && channel <= 14) valid = true;
        else if (channel >= 36 && channel <= 165) valid = true; // Simplified check

        if (!valid) {
            printf("Error: Invalid channel %d\n", channel);
            return 1;
        }
        printf("Monitoring channel %d\n", channel);
    }

    if (switch_to_monitor_mode(channel, bandwidth) != ESP_OK) {
        printf("Failed to switch to monitor mode\n");
        return 1;
    }
    return 0;
}

static int cmd_mode_sta(int argc, char **argv) {
    wifi_band_mode_t band_mode = WIFI_BAND_MODE_AUTO;

    if (argc > 1) {
        if (strcmp(argv[1], "2.4") == 0) band_mode = WIFI_BAND_MODE_2G_ONLY;
        else if (strcmp(argv[1], "5") == 0) band_mode = WIFI_BAND_MODE_5G_ONLY;
        else if (strcmp(argv[1], "auto") == 0) band_mode = WIFI_BAND_MODE_AUTO;
        else {
            printf("Error: Invalid band '%s'\n", argv[1]);
            return 1;
        }
    }

    if (switch_to_sta_mode(band_mode) != ESP_OK) {
        printf("Failed to switch to STA mode\n");
        return 1;
    }
    printf("Switching to STA mode...\n");
    return 0;
}

static int cmd_mode_status(int argc, char **argv) {
    printf("\n=== WiFi Mode Status ===\n");
    printf("Current mode: %s\n",
           current_wifi_mode == WIFI_MODE_STA_CSI ? "STA (CSI + iperf)" : "MONITOR");
    printf("LED state: %d\n", current_led_state);

    if (current_wifi_mode == WIFI_MODE_STA_CSI) {
        printf("WiFi connected: %s\n", wifi_connected ? "Yes" : "No");
        printf("CSI enabled: %s\n", s_csi_enabled ? "Yes" : "No");
    } else {
        printf("Monitor channel: %d\n", monitor_channel);
        printf("Frames captured: %lu\n", (unsigned long)s_monitor_frame_count);
    }

    printf("GPS synced: %s\n", gps_is_synced() ? "Yes (+)" : "No (*)");
    printf("\n");
    return 0;
}

static int cmd_csi_dump(int argc, char **argv) {
    if (current_wifi_mode != WIFI_MODE_STA_CSI) {
        printf("Error: CSI only available in STA mode\n");
        return 1;
    }
    printf("Dumping CSI data...\n");
    csi_log_dump_over_uart();
    return 0;
}

static void register_mode_commands(void) {
    const esp_console_cmd_t mode_monitor = {
        .command = "mode_monitor",
        .help = "Switch to monitor mode",
        .func = &cmd_mode_monitor,
    };
    ESP_ERROR_CHECK(esp_console_cmd_register(&mode_monitor));

    const esp_console_cmd_t mode_sta = {
        .command = "mode_sta",
        .help = "Switch to STA mode",
        .func = &cmd_mode_sta,
    };
    ESP_ERROR_CHECK(esp_console_cmd_register(&mode_sta));

    const esp_console_cmd_t mode_status = {
        .command = "mode_status",
        .help = "Show status",
        .func = &cmd_mode_status,
    };
    ESP_ERROR_CHECK(esp_console_cmd_register(&mode_status));

    const esp_console_cmd_t csi_dump = {
        .command = "csi_dump",
        .help = "Dump CSI data",
        .func = &cmd_csi_dump,
    };
    ESP_ERROR_CHECK(esp_console_cmd_register(&csi_dump));
}

// --- Event Handler -------------------------------------------------
static void event_handler(void* arg, esp_event_base_t event_base,
                          int32_t event_id, void* event_data) {
    if (event_base == WIFI_EVENT) {
        if (event_id == WIFI_EVENT_STA_START) {
            if (has_config && current_wifi_mode == WIFI_MODE_STA_CSI) {
                current_led_state = LED_STATE_WAITING;
            }
        }
        else if (event_id == WIFI_EVENT_STA_DISCONNECTED) {
            wifi_event_sta_disconnected_t* event = (wifi_event_sta_disconnected_t*) event_data;
            ESP_LOGW(TAG, "WiFi Disconnected (Reason: %d)", event->reason);
            if (!wifi_connected && has_config && current_wifi_mode == WIFI_MODE_STA_CSI) {
                current_led_state = LED_STATE_FAILED;
            }
            wifi_connected = false;
        }
    }
    else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        if (current_wifi_mode != WIFI_MODE_STA_CSI) return;

        ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data;
        ESP_LOGI(TAG, "Got IP: " IPSTR, IP2STR(&event->ip_info.ip));

        wifi_connected = true;
        current_led_state = LED_STATE_CONNECTED;

        ESP_LOGI(TAG, "Enabling CSI...");
        wifi_enable_csi_once();

        vTaskDelay(pdMS_TO_TICKS(1000));
        iperf_cfg_t cfg;
        memset(&cfg, 0, sizeof(cfg));
        cfg.flag = IPERF_FLAG_SERVER | IPERF_FLAG_TCP;
        cfg.sport = 5001;
        iperf_start(&cfg);
        ESP_LOGI(TAG, "iperf server started");

        xTaskCreate(csi_dump_task, "csi_dump_task", 4096, NULL, 5, NULL);
    }
}

// --- Main ----------------------------------------------------------
void app_main(void) {
    ESP_ERROR_CHECK(nvs_flash_init());
    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());

    // Init Logging & LED
    ESP_ERROR_CHECK(csi_log_init());
    rgb_led_init();
    xTaskCreate(led_task, "led_task", 4096, NULL, 5, NULL);

    // Init GPS
    ESP_LOGI(TAG, "Initializing GPS sync...");
    const gps_sync_config_t gps_cfg = {
        .uart_port = UART_NUM_1,
        .tx_pin = GPS_TX_PIN,
        .rx_pin = GPS_RX_PIN,
        .pps_pin = GPS_PPS_PIN,
    };
    gps_sync_init(&gps_cfg, true);

    ESP_LOGI(TAG, "GPS init (TX:%d, RX:%d, PPS:%d)", GPS_TX_PIN, GPS_RX_PIN, GPS_PPS_PIN);

    // Register Events
    ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &event_handler, NULL, NULL));
    ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &event_handler, NULL, NULL));

    // Init WiFi Config (This starts the cmd_transport listener)
    wifi_cfg_init();

    // Init Console
    ESP_LOGI(TAG, "Initializing console...");
    setvbuf(stdin, NULL, _IONBF, 0);
    esp_console_config_t console_config = {
        .max_cmdline_args = 8,
        .max_cmdline_length = 256,
#if CONFIG_LOG_COLORS
        .hint_color = atoi(LOG_COLOR_CYAN)
#endif
    };
    ESP_ERROR_CHECK(esp_console_init(&console_config));
    linenoiseSetMultiLine(1);
    linenoiseSetCompletionCallback(NULL);
    linenoiseSetHintsCallback(NULL);
    linenoiseHistorySetMaxLen(100);
    esp_console_register_help_command();
    register_mode_commands();

    // Apply Config
    if (wifi_cfg_apply_from_nvs()) {
        has_config = true;
        current_led_state = LED_STATE_WAITING;
        ESP_LOGI(TAG, "WiFi config loaded.");

        char mode[16] = {0};
        uint8_t mon_ch = 36;
        if (wifi_cfg_get_mode(mode, &mon_ch)) {
            if (strcmp(mode, "MONITOR") == 0) {
                ESP_LOGI(TAG, "MODE: MONITOR (Channel %d)", mon_ch);
                uint8_t *ch_param = malloc(sizeof(uint8_t));
                *ch_param = mon_ch;
                xTaskCreate(auto_monitor_task, "auto_monitor", 4096, ch_param, 5, NULL);
            } else {
                ESP_LOGI(TAG, "MODE: STA");
            }
        }
    } else {
        has_config = false;
        current_led_state = LED_STATE_NO_CONFIG;
        ESP_LOGI(TAG, "No WiFi config found.");
    }
}

static void auto_monitor_task(void *arg) {
    uint8_t channel = *(uint8_t*)arg;
    free(arg);

    ESP_LOGI(TAG, "Waiting for WiFi connection before switching to monitor mode...");
    while (current_led_state != LED_STATE_CONNECTED) {
        vTaskDelay(pdMS_TO_TICKS(500));
    }

    ESP_LOGI(TAG, "WiFi connected, waiting for GPS sync...");
    vTaskDelay(pdMS_TO_TICKS(2000));

    ESP_LOGI(TAG, "Auto-switching to MONITOR mode on channel %d...", channel);
    switch_to_monitor_mode(channel, WIFI_BW_HT20);
    vTaskDelete(NULL);
}