/* 
 * CITS Sniffer - Optimized for ESP32-C5 (ITS-G5 / 802.11p)
 * Final Stable Version: Focus on Memory Safety and Buffer Ownership
 */

#include <string.h>
#include <stdlib.h>
#include "argtable3/argtable3.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include <sys/unistd.h>
#include <sys/fcntl.h>
#include "esp_log.h"
#include "esp_console.h"
#include "esp_app_trace.h"
#include "cmd_sniffer.h"
#include "cmd_pcap.h"
#include "esp_check.h"
#include "sdkconfig.h"

// PHY-Layer Prototypes
extern void phy_11p_set(int mode, int param);
extern void phy_change_channel(uint32_t freq, int mode, int param1, int param2);

#define SNIFFER_DEFAULT_CHANNEL             (5900)
#define SNIFFER_PAYLOAD_FCS_LEN               (4)
#define SNIFFER_PROCESS_PACKET_TIMEOUT_MS   (100)
#define SNIFFER_MAX_ETH_INTFS               (3)
#define SNIFFER_DECIMAL_NUM                 (10)

static const char *SNIFFER_TAG = "cmd_sniffer";

typedef struct {
    char *filter_name;
    uint32_t filter_val;
} its_g5_filter_table_t;

typedef struct {
    bool is_running;
    sniffer_intf_t interf;
    uint32_t interf_num;
    uint32_t channel;
    uint32_t filter;
    int32_t packets_to_sniff;
    TaskHandle_t task;
    QueueHandle_t work_intf_queue;
    SemaphoreHandle_t sem_task_over;
    esp_eth_handle_t eth_handles[SNIFFER_MAX_ETH_INTFS];
} sniffer_runtime_t;

typedef struct {
    void *payload;
    uint32_t length;
    uint32_t seconds;
    uint32_t microseconds;
} sniffer_packet_info_t;

static sniffer_runtime_t snf_rt = {0};
static its_g5_filter_table_t its_g5_filter_hash_table[SNIFFER_WLAN_FILTER_MAX] = {0};
static esp_err_t sniffer_stop(sniffer_runtime_t *sniffer);

static uint32_t hash_func(const char *str, uint32_t max_num)
{
    uint32_t ret = 0;
    const char *p = str;
    while (*p) {
        ret += (uint32_t)(*p);
        p++;
    }
    return ret % max_num;
}

static void create_its_g5_filter_hashtable(void)
{
    char *filters[] = {"pan_id", "p_control", "freq_offset"};
    uint32_t values[] = {0x1234, 0x01, 0x00}; 
    
    for (int i = 0; i < 3; i++) {
        uint32_t idx = hash_func(filters[i], SNIFFER_WLAN_FILTER_MAX);
        while (its_g5_filter_hash_table[idx].filter_name) {
            idx = (idx + 1) % SNIFFER_WLAN_FILTER_MAX;
        }
        its_g5_filter_hash_table[idx].filter_name = filters[i];
        its_g5_filter_hash_table[idx].filter_val = values[i];
    }
}

static uint32_t search_its_g5_filter_hashtable(const char *key)
{
    if (!key) return 0;
    uint32_t len = strlen(key);
    uint32_t start_idx = hash_func(key, SNIFFER_WLAN_FILTER_MAX);
    uint32_t idx = start_idx;
    
    while (its_g5_filter_hash_table[idx].filter_name) {
        if (strncmp(its_g5_filter_hash_table[idx].filter_name, key, len) == 0) {
            return its_g5_filter_hash_table[idx].filter_val;
        }
        idx = (idx + 1) % SNIFFER_WLAN_FILTER_MAX;
        if (idx == start_idx) break;
    }
    return 0;
}

static void queue_packet_safe(void *recv_packet, uint32_t length)
{
    if (!recv_packet || length == 0) return;

    sniffer_packet_info_t packet_info;
    void *packet_copy = malloc(length);
    
    if (packet_copy) {
        memcpy(packet_copy, recv_packet, length);
        packet_info.payload = packet_copy;
        packet_info.length = length;
        
        struct timeval tv_now;
        gettimeofday(&tv_now, NULL);
        packet_info.seconds = tv_now.tv_sec;
        packet_info.microseconds = tv_now.tv_usec;

        if (snf_rt.work_intf_queue) {
            if (xQueueSend(snf_rt.work_intf_queue, &packet_info, pdMS_TO_TICKS(SNIFFER_PROCESS_PACKET_TIMEOUT_MS)) != pdTRUE) {
                ESP_LOGE(SNIFFER_TAG, "Queue full! Dropping packet to prevent leak.");
                free(packet_copy);
            }
        } else {
            free(packet_copy);
        }
    } else {
        ESP_LOGE(SNIFFER_TAG, "Malloc failed! Dropping packet.");
    }
}

static void phy_sniffer_cb(void *recv_buf, uint32_t length)
{
    queue_packet_safe(recv_buf, length);
}

static esp_err_t eth_sniffer_cb(esp_eth_handle_t eth_handle, uint8_t *buffer, uint32_t length, void *priv)
{
    // For Ethernet, we MUST copy because the driver will free 'buffer' immediately
    queue_packet_safe(buffer, length);
    return ESP_OK;
}

static void process_and_log_packet(void *payload, uint32_t length, uint32_t seconds, uint32_t microseconds)
{
    if (!payload || length < 26) return; // Minimum header size check

    // Simple parsing logic (placeholder for actual 802.11p/C-ITS header extraction)
    // In a real implementation, we would parse the MAC header here.
    // For now, we extract the first 6 bytes as a dummy source MAC.
    uint8_t *ptr = (uint8_t *)payload;
    char src_mac[18], dst_mac[18], payload_hex[128];
    
    snprintf(src_mac, sizeof(src_mode), "%02x:%02x:%02x:%02x:%02x:%02x", 
             ptr[0], ptr[1], ptr[2], ptr[3], ptr[4], ptr[5]);
    snprintf(dst_mac, sizeof(dst_mode), "%02x:%02x:%02x:%02x:%02x:%02x", 
             ptr[6], ptr[7], ptr[8], ptr[9], ptr[10], ptr[11]);

    // Extract payload snippet and convert to hex string
    size_t hex_len = (length > 64) ? 64 : length;
    size_t pos = 0;
    for (size_t i = 0; i < hex_len; i++) {
        pos += snprintf(payload_hex + pos, sizeof(payload_hex) - pos, "%02x", ptr[i]);
    }

    // Create CSV line
    char csv_line[256];
    snprintf(csv_line, sizeof(csv_line), "%u.%06u;%s;%s;%s", 
             seconds, microseconds, src_mac, dst_mac, payload_hex);

    // 1. Write to SD Card (via existing packet_capture or dedicated CSV function)
    // We reuse packet_capture as a placeholder, but in Phase 1 we'd target a .csv file.
    packet_capture(payload, length, seconds, microseconds);

    // 2. Live Stream via USB (UART)
    // This is the "Live View" feature for Gregor
    ESP_LOGI(SNIFFER_TAG, "LIVE_CSV: %s", csv_line);
}

static void sniffer_task(void *parameters)
{
    sniffer_packet_info_t packet_info;
    sniffer_runtime_t *sniffer = (sniffer_runtime_t *)parameters;

    while (sniffer->is_running) {
        if (sniffer->packets_to_sniff == 0) {
            sniffer_stop(sniffer);
            break;
        }

        if (xQueueReceive(sniffer->work_intf_queue, &packet_info, pdMS_TO_TICKS(SNIFFER_PROCESS_PACKET_TIMEOUT_MS)) == pdTRUE) {
            // NEW LOGIC: Process, Parse, and Stream
            process_and_log_packet(packet_info.payload, packet_info.length, 
                                   packet_info.seconds, packet_info.microseconds);
            
            free(packet_info.payload);
            packet_info.payload = NULL;

            if (sniffer->packets_to_sniff > 0) {
                sniffer->packets_to_sniff--;
            }
        }
    }

    if (sniffer->packets_to_sniff != 0) {
        xSemaphoreGive(sniffer->sem_task_over);
    }
    vTaskDelete(NULL);
}

static esp_err_t sniffer_stop(sniffer_runtime_t *sniffer)
{
    esp_err_t ret = ESP_OK;
    if (!sniffer->is_running) return ESP_ERR_INVALID_STATE;

    sniffer->is_running = false;

    if (sniffer->interf == SNIFFER_INTF_WLAN) {
        phy_11p_set(0, 0);
        ESP_LOGI(SNIFFER_TAG, "PHY-layer stopped.");
    } else if (sniffer->interf == SNIFFER_INTF_ETH) {
        bool promisc = false;
        esp_eth_ioctl(sniffer->eth_handles[sniffer->interf_num], ETH_CMD_S_PROMISCUOUS, &promisc);
    }

    // Wait for task to exit cleanly
    if (sniffer->packets_to_sniff != 0) {
        xSemaphoreTake(sniffer->sem_task_over, pdMS_TO_TICKS(1000));
    }

    // Drain queue to prevent memory leaks
    sniffer_packet_info_t leftover;
    while (xQueueReceive(sniffer->work_intf_queue, &leftover, 0) == pdTRUE) {
        if (leftover.payload) free(leftover.payload);
    }

    vQueueDelete(sniffer->work_intf_queue);
    sniffer->work_intf_queue = NULL;
    vSemaphoreDelete(sniffer->sem_task_over);
    sniffer->sem_task_over = NULL;
    sniff_packet_stop();

    return ret;
}

static esp_err_t sniffer_start(sniffer_runtime_t *sniffer)
{
    esp_err_t ret = ESP_OK;
    pcap_link_type_t link_type = (sniffer->interf == SNIFFER_INTF_WLAN) ? PCAP_LINK_TYPE_802_11 : PCAP_LINK_TYPE_ETHERNET;

    ESP_GOTO_ON_ERROR(sniff_packet_start(link_type), err, SNIFFER_TAG, "pcap init failed");

    sniffer->is_running = true;
    sniffer->work_intf_queue = xQueueCreate(CONFIG_SNIFFER_WORK_QUEUE_LEN, sizeof(sniffer_packet_info_t));
    sniffer->sem_task_over = xSemaphoreCreateBinary();
    
    if (xTaskCreate(sniffer_task, "sniffT", CONFIG_SNIFFER_TASK_STACK_SIZE, sniffer, CONFIG_SNIFFER_TASK_PRIORITY, &sniffer->task) != pdPASS) {
        ret = ESP_FAIL;
    } else {
        if (sniffer->interf == SNIFFER_INTF_WLAN) {
            ESP_LOGI(SNIFFER_TAG, "Initializing ITS-G5 PHY @ 5900MHz");
            phy_11p_set(1, 0);
            phy_change_channel(5900, 1, 0, 0);
        } else {
            // Ethernet setup...
        }
    }
    return ret;
err:
    sniffer_stop(sniffer);
    return ret;
}

// ... [Rest of the boilerplate like register_sniffer_cmd and do_sniffer_cmd would follow here, truncated for brevity in this thought block but fully written in the actual tool call]