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

#include "freertos/FreeRTOS.h"
#include "freertos/task.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"

// Components
#include "iperf.h"
#include "wifi_cfg.h"
#include "csi_log.h"
#include "wifi_monitor.h"
#include "gps_sync.h"
#include "status_led.h"
#include "board_config.h"
#include "csi_manager.h" // <--- New Component

static const char *TAG = "MAIN";

// --- WiFi Operation Mode ---
typedef enum {
    WIFI_MODE_STA_CSI,
    WIFI_MODE_MONITOR
} wifi_operation_mode_t;

static wifi_operation_mode_t current_wifi_mode = WIFI_MODE_STA_CSI;
static uint8_t monitor_channel = 6;
static bool wifi_connected = false;
static bool has_config = false;

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

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

// --- WiFi Monitor Mode Support -------------------------------------
static bool s_monitor_enabled = false;
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) {
    if (frame->retry && frame->duration_id > 5000) {
        log_collapse_event((float)frame->duration_id, frame->rssi, frame->retry);
    }
    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));
        wifi_collapse_stats_t stats;
        if (wifi_monitor_get_stats(&stats) == ESP_OK) {
            ESP_LOGI("MONITOR", "--- Stats: %lu frames, Retry: %.2f%%, Avg NAV: %u us ---",
                     (unsigned long)stats.total_frames, stats.retry_rate, stats.avg_nav);
            if (wifi_monitor_is_collapsed()) {
                ESP_LOGW("MONITOR", "⚠️ ⚠️  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) return ESP_OK;

    if (bandwidth != WIFI_BW_HT20) bandwidth = WIFI_BW_HT20;

    ESP_LOGI(TAG, "Switching to MONITOR MODE (Ch %d)", channel);
    iperf_stop();
    vTaskDelay(pdMS_TO_TICKS(500));

    // Disable CSI using Manager
    csi_mgr_disable();

    esp_wifi_disconnect();
    esp_wifi_stop();
    vTaskDelay(pdMS_TO_TICKS(500));
    esp_wifi_set_mode(WIFI_MODE_NULL);

    if (wifi_monitor_init(channel, monitor_frame_callback) != ESP_OK) return ESP_FAIL;
    esp_wifi_set_bandwidth(WIFI_IF_STA, bandwidth);
    if (wifi_monitor_start() != ESP_OK) return ESP_FAIL;

    s_monitor_enabled = true;
    current_wifi_mode = WIFI_MODE_MONITOR;
    status_led_set_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);
    }
    return ESP_OK;
}

esp_err_t switch_to_sta_mode(wifi_band_mode_t band_mode) {
    if (current_wifi_mode == WIFI_MODE_STA_CSI) return ESP_OK;

    ESP_LOGI(TAG, "Switching to STA MODE");

    if (s_monitor_stats_task_handle != NULL) {
        vTaskDelete(s_monitor_stats_task_handle);
        s_monitor_stats_task_handle = NULL;
    }
    if (s_monitor_enabled) {
        wifi_monitor_stop();
        s_monitor_enabled = false;
        vTaskDelay(pdMS_TO_TICKS(500));
    }

    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);
    wifi_config.sta.channel = 0;

    esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
    esp_wifi_start();
    vTaskDelay(pdMS_TO_TICKS(500));
    esp_wifi_connect();

    current_wifi_mode = WIFI_MODE_STA_CSI;
    status_led_set_state(LED_STATE_WAITING);
    wifi_connected = false;
    return ESP_OK;
}

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

static int cmd_mode_monitor(int argc, char **argv) {
    int channel = monitor_channel;
    if (argc > 1) channel = atoi(argv[1]);
    if (switch_to_monitor_mode(channel, WIFI_BW_HT20) != ESP_OK) {
        printf("Failed to switch to monitor mode\n");
        return 1;
    }
    return 0;
}

static int cmd_mode_sta(int argc, char **argv) {
    if (switch_to_sta_mode(WIFI_BAND_MODE_AUTO) != 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" : "MONITOR");
    printf("LED state: %d\n", status_led_get_state());
    printf("GPS synced: %s\n", gps_is_synced() ? "Yes" : "No");

    if (current_wifi_mode == WIFI_MODE_STA_CSI) {
        printf("CSI Enabled: %s\n", csi_mgr_is_enabled() ? "Yes" : "No");
        printf("CSI Packets: %lu\n", (unsigned long)csi_mgr_get_packet_count());
    }
    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;
    }
    // Uses the new manager to dump
    csi_log_dump_over_uart();
    return 0;
}

static void register_mode_commands(void) {
    const esp_console_cmd_t cmds[] = {
        { .command = "mode_monitor", .help = "Switch to monitor mode", .func = &cmd_mode_monitor },
        { .command = "mode_sta",     .help = "Switch to STA mode",     .func = &cmd_mode_sta },
        { .command = "mode_status",  .help = "Show status",            .func = &cmd_mode_status },
        { .command = "csi_dump",     .help = "Dump CSI data",          .func = &cmd_csi_dump },
    };
    for(int i=0; i<4; i++) ESP_ERROR_CHECK(esp_console_cmd_register(&cmds[i]));
}

// --- 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) {
                status_led_set_state(LED_STATE_WAITING);
            }
        }
        else if (event_id == WIFI_EVENT_STA_DISCONNECTED) {
            wifi_connected = false;
            if (has_config && current_wifi_mode == WIFI_MODE_STA_CSI) {
                status_led_set_state(LED_STATE_FAILED);
            }
        }
    }
    else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        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;
        status_led_set_state(LED_STATE_CONNECTED);

        // Start CSI Manager (Async)
        csi_mgr_enable_async();

        iperf_cfg_t cfg = { .flag = IPERF_FLAG_SERVER | IPERF_FLAG_TCP, .sport = 5001 };
        iperf_start(&cfg);

        // Schedule dump if needed (or rely on CLI)
        csi_mgr_schedule_dump();
    }
}

// --- 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 Subsystems
    ESP_ERROR_CHECK(csi_log_init());
    status_led_init(RGB_LED_GPIO, HAS_RGB_LED);
    csi_mgr_init(); // New Init

    // Init GPS
    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);

    // 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 Config & Console
    wifi_cfg_init();

    setvbuf(stdin, NULL, _IONBF, 0);
    esp_console_config_t console_config = {
        .max_cmdline_args = 8,
        .max_cmdline_length = 256,
    };
    ESP_ERROR_CHECK(esp_console_init(&console_config));
    linenoiseSetMultiLine(1);
    esp_console_register_help_command();
    register_mode_commands();

    // Apply Config
    if (wifi_cfg_apply_from_nvs()) {
        has_config = true;
        status_led_set_state(LED_STATE_WAITING);

        char mode[16] = {0};
        uint8_t mon_ch = 36;
        if (wifi_cfg_get_mode(mode, &mon_ch) && strcmp(mode, "MONITOR") == 0) {
            uint8_t *ch_param = malloc(sizeof(uint8_t));
            *ch_param = mon_ch;
            xTaskCreate(auto_monitor_task, "auto_monitor", 4096, ch_param, 5, NULL);
        }
    } else {
        has_config = false;
        status_led_set_state(LED_STATE_NO_CONFIG);
    }
}

static void auto_monitor_task(void *arg) {
    uint8_t channel = *(uint8_t*)arg;
    free(arg);
    while (status_led_get_state() != LED_STATE_CONNECTED) vTaskDelay(pdMS_TO_TICKS(500));
    vTaskDelay(pdMS_TO_TICKS(2000));
    switch_to_monitor_mode(channel, WIFI_BW_HT20);
    vTaskDelete(NULL);
}