/**
 * Block 4: Integration und Flashen der Firmware auf ESP32-C5
 * 
 * Ziel: C-ITS Sniffer Firmware für ESP32-C5 compilieren und flashen
 */

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <time.h>

#include "sdkconfig.h"
#include "esp_err.h"
#include "esp_log.h"
#include "esp_wifi.h"
#include "nvs_flash.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_console.h"
#include "esp_vfs_dev_usb_serial_jtag.h"
#include "esp_log.h"
#include "arg_table3/arg_table3.h"
#include "driver/usb_serial_jtag.h"

#include "block1_hardware_init.h"
#include "block2_cits_parser.h"
#include "block3_csv_writer.h"

static const char *TAG = "CITS_SNIFFER";

// Globale Puffer für JTAG-Direct-Auslesung
#define MAX_STORED_PACKETS 64
#define MAX_PACKET_SIZE 2000
typedef struct {
    wifi_promiscuous_pkt_t pkt;
    uint32_t timestamp;
} stored_packet_t;

static stored_packet_t stored_packets[MAX_STORED_PACKETS];
static volatile uint32_t packet_write_idx = 0;
static volatile uint32_t packet_count_total = 0;
static bool packets_available = false;

/**
 * Hilfsfunktion: HEX-Daten auf Console (JTAG) ausgeben
 */
static void hex_dump(const char *prefix, const uint8_t *data, int len)
{
    if (!prefix || !data || len <= 0) return;
    
    // Print header with prefix
    char line[128];
    int offset = 0;
    
    ESP_LOGI(TAG, "%s [Len=%d]:", prefix, len);
    
    // First line: raw hex dump
    for (int i = 0; i < len && i < 64; i++) {
        if (i % 16 == 0) {
            if (i > 0) {
                ESP_LOGI(TAG, "  %s", line);
            }
            offset = 0;
            snprintf(line, sizeof(line), "  %04x: ", i);
        }
        offset += snprintf(line + strlen(line), sizeof(line) - strlen(line), "%02x ", data[i]);
    }
    if (len > 0) {
        ESP_LOGI(TAG, "%s", line);
    }
}

/**
 * Sniffer Callback - Wird vom WiFi Promiscuous Mode aufgerufen
 * Jede empfangene 802.11p Nachricht wird hier geloggt UND gespeichert für JTAG-Auslesung
 */
static void sniffer_callback(void *recv_buf, wifi_promiscuous_pkt_type_t type)
{
    wifi_promiscuous_pkt_t *packet = (wifi_promiscuous_pkt_t *)recv_buf;
    
    // Nur MISC Typ für 802.11p verarbeiten
    if (type != WIFI_PKT_MISC) {
        return;
    }
    
    if (packet->payload == NULL || packet->rx_ctrl.sig_len < 20) {
        return;
    }
    
    // RSSI aus dem Paket
    int8_t rssi = packet->rx_ctrl.rssi;
    uint32_t ts = packet->rx_ctrl.timestamp;
    uint16_t pkt_len = packet->rx_ctrl.sig_len;
    
    // Detailiertes Paket-Logging auf JTAG Console
    ESP_LOGI(TAG, "[SNIFFER] === Paket #%lu ===", (unsigned long)packet_count_total);
    ESP_LOGI(TAG, "  Länge: %d Bytes | RSSI: %d dBm | Typ: %d | Chan: %d | Rate: %d",
             pkt_len, rssi, type,
             packet->rx_ctrl.channel,
             packet->rx_ctrl.rate);
    
    // MAC Header Bytes ausgeben
    ESP_LOGI(TAG, "  MAC Header: %02x %02x %02x %02x %02x %02x %02x %02x",
             packet->payload[0], packet->payload[1],
             packet->payload[2], packet->payload[3],
             packet->payload[4], packet->payload[5],
             packet->payload[6], packet->payload[7]);
    
    // WAVE Header parsen
    if (pkt_len >= 15) {
        uint32_t pdu_len = packet->payload[0] | (packet->payload[1] << 8) | (packet->payload[2] << 16);
        uint8_t sender_mac[6];
        memcpy(sender_mac, packet->payload + 3, 6);
        uint32_t ts_field;
        memcpy(&ts_field, packet->payload + 9, 4);
        uint8_t priority = packet->payload[13];
        uint8_t pdu_type = packet->payload[14];
        
        ESP_LOGI(TAG, "  WAVE PDU Length: %u | Sender: %02X:%02X:%02X:%02X:%02X:%02X",
                 pdu_len, sender_mac[0], sender_mac[1], sender_mac[2],
                 sender_mac[3], sender_mac[4], sender_mac[5]);
        ESP_LOGI(TAG, "  WAVE Timestamp: %u | Priority: %u | PDU Type: %02X",
                 ts_field, priority, pdu_type);
        
        // C-ITS Payload wenn vorhanden
        if (pkt_len > 15) {
            ESP_LOGI(TAG, "  C-ITS Payload (%d Bytes):", pkt_len - 15);
            for (int i = 15; i < pkt_len && i < 47; i++) {
                if ((i - 15) % 16 == 0) {
                    char hex_line[128];
                    snprintf(hex_line, sizeof(hex_line), "    %04x: ", i);
                    ESP_LOGI(TAG, "%s", hex_line);
                }
                ESP_LOG_BUFFER_HEX_LEVEL(TAG, packet->payload + i, 1, ESP_LOG_INFO);
            }
        }
    }
    
    // Paket zum JTAG-Direct-Zugang speichern
    if (packet_count_total < MAX_STORED_PACKETS) {
        memcpy(&stored_packets[packet_write_idx].pkt, packet, sizeof(wifi_promiscuous_pkt_t));
        stored_packets[packet_write_idx].timestamp = ts;
        packet_write_idx = (packet_write_idx + 1) % MAX_STORED_PACKETS;
    }
    packet_count_total++;
    packets_available = true;
    
    // Verarbeite C-ITS Nachricht
    cits_process_packet(packet->payload, packet->rx_ctrl.sig_len,
                       packet->rx_ctrl.timestamp / 1000000U,
                       packet->rx_ctrl.timestamp % 1000000U);
}

/**
 * C-ITS Sniffer aufsetzen
 */
static esp_err_t setup_cits_sniffer(void)
{
    // WiFi Promiscuous Mode starten
    esp_err_t ret = start_promiscuous_mode(CITS_CHANNEL_0_MHZ);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to start promiscuous mode");
        return ret;
    }
    
    // C-ITS Parser initialisieren
    ret = cits_parser_init();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to initialize C-ITS parser");
        return ret;
    }
    
    // CSV Writer initialisieren
    ret = csv_writer_init();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to initialize CSV writer");
        return ret;
    }
    
    ESP_LOGI(TAG, "C-ITS Sniffer Setup abgeschlossen");
    return ESP_OK;
}

/**
 * JTAG Direct Read - Gepackete Pakete auf Console (JTAG) ausgeben
 */
static int cmd_jtag_read(int argc, char **argv)
{
    int count = 10; // Default 10 Pakete
    
    if (argc > 0) {
        count = atoi(argv[0]);
        if (count < 1) count = 1;
        if (count > MAX_STORED_PACKETS) count = MAX_STORED_PACKETS;
    }
    
    if (!packets_available) {
        ESP_LOGI(TAG, "Keine Pakete empfangen. Warte auf C-ITS Nachrichten...");
        return 0;
    }
    
    ESP_LOGI(TAG, "[JTAG-READ] Empfangene Pakete (%d vom %lu insgesamt):", count, (unsigned long)packet_count_total);
    
    uint32_t idx = (packet_write_idx - 1 + MAX_STORED_PACKETS) % MAX_STORED_PACKETS;
    int printed = 0;
    
    for (int i = 0; i < count; i++) {
        if (printed >= packet_count_total) break;
        
        stored_packet_t *p = &stored_packets[idx];
        if (p->pkt.payload == NULL) {
            idx = (idx - 1 + MAX_STORED_PACKETS) % MAX_STORED_PACKETS;
            continue;
        }
        
        uint16_t pkt_len = p->pkt.rx_ctrl.sig_len;
        int8_t rssi = p->pkt.rx_ctrl.rssi;
        
        char mac_str[20];
        if (pkt_len >= 12) {
            snprintf(mac_str, sizeof(mac_str), "%02x:%02x:%02x:%02x:%02x:%02x",
                     p->pkt.payload[0], p->pkt.payload[1],
                     p->pkt.payload[2], p->pkt.payload[3],
                     p->pkt.payload[4], p->pkt.payload[5]);
        } else {
            strcpy(mac_str, "N/A");
        }
        
        ESP_LOGI(TAG, "[Paket %d] L=%d RSSI=%d MAC=%s", 
                 i + 1, pkt_len, rssi, mac_str);
        ESP_LOGI(TAG, "[Paket %d] Raw: ", i + 1);
        ESP_LOG_BUFFER_HEX_LEVEL(TAG, p->pkt.payload, pkt_len < 64 ? pkt_len : 64, ESP_LOG_INFO);
        
        printed++;
        idx = (idx - 1 + MAX_STORED_PACKETS) % MAX_STORED_PACKETS;
    }
    
    return 0;
}

/**
 * Sniffer Status - Aktuelle Statistik ausgeben
 */
static int cmd_sniffer_status(int argc, char **argv)
{
    ESP_LOGI(TAG, "[Status] Gesamt empfangene Pakete: %lu",
             (unsigned long)packet_count_total);
    ESP_LOGI(TAG, "[Status] Gespeichert: %d / %d",
             (packet_write_idx < MAX_STORED_PACKETS) ? 
             (MAX_STORED_PACKETS - packet_write_idx) : packet_write_idx,
             MAX_STORED_PACKETS);
    ESP_LOGI(TAG, "[Status] Pakete verfügbar: %s", packets_available ? "JA" : "NEIN");
    return 0;
}

/**
 * Kanal wechseln
 */
static int cmd_change_channel(int argc, char **argv)
{
    if (argc < 1) {
        ESP_LOGI(TAG, "Nutzung: change_channel <freq_mhz>");
        return 1;
    }
    
    uint32_t freq = atoi(argv[0]);
    if (freq < 5800 || freq > 5900) {
        ESP_LOGE(TAG, "Frequenz muss zwischen 5800 und 5900 MHz liegen");
        return 1;
    }
    
    phy_11p_set(1, 0);
    phy_change_channel(freq, 1, 0, 0);
    ESP_LOGI(TAG, "Kanal auf %u MHz geändert", freq);
    return 0;
}

/**
 * Sniffer starten (neu)
 */
static int cmd_start_sniffer(int argc, char **argv)
{
    ESP_LOGI(TAG, "Sniffer wird neu gestartet...");
    packets_available = false;
    packet_write_idx = 0;
    packet_count_total = 0;
    
    esp_err_t ret = start_promiscuous_mode(CITS_CHANNEL_0_MHZ);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Sniffer Start fehlgeschlagen");
        return 1;
    }
    
    ESP_LOGI(TAG, "Sniffer läuft auf %u MHz", CITS_CHANNEL_0_MHZ);
    return 0;
}

/**
 * Hauptfunktion
 */
void app_main(void)
{
    // USB-Serial/JTAG Console initialisieren
    usb_serial_jtag_driver_install(0, 0, 0);
    esp_vfs_dev_usb_serial_jtag_set_console();
    
    ESP_LOGI(TAG, "=== C-ITS Sniffer für ESP32-C5 startet ===");
    ESP_LOGI(TAG, "Console: USB-Serial/JTAG (115200 bps)");
    
    // NVS initialisieren
    esp_err_t ret = init_nvs_flash();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to initialize NVS");
        return;
    }
    
    // WiFi initialisieren
    ret = initialize_wifi();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to initialize WiFi");
        return;
    }
    
    // C-ITS Sniffer aufsetzen
    ret = setup_cits_sniffer();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to setup C-ITS Sniffer");
        return;
    }
    
    // Console Commands registrieren
    const esp_console_cmd_t cmd_jtag_read_cmd = {
        .command = "jtag_read",
        .help = "Read captured packets via JTAG (optional count)",
        .hint = NULL,
        .func = &cmd_jtag_read,
    };
    esp_console_cmd_register(&cmd_jtag_read_cmd);
    
    const esp_console_cmd_t cmd_status_cmd = {
        .command = "sniffer_status",
        .help = "Show sniffer statistics",
        .hint = NULL,
        .func = &cmd_sniffer_status,
    };
    esp_console_cmd_register(&cmd_status_cmd);
    
    const esp_console_cmd_t cmd_chan_cmd = {
        .command = "chan",
        .help = "Change C-ITS channel (5800-5900 MHz)",
        .hint = NULL,
        .func = &cmd_change_channel,
    };
    esp_console_cmd_register(&cmd_chan_cmd);
    
    const esp_console_cmd_t cmd_start_cmd = {
        .command = "start_sniffer",
        .help = "Restart sniffer",
        .hint = NULL,
        .func = &cmd_start_sniffer,
    };
    esp_console_cmd_register(&cmd_start_cmd);
    
    register_cits_commands();
    
    // Hauptschleife - C-ITS Nachrichten werden im Callback verarbeitet
    ESP_LOGI(TAG, "C-ITS Sniffer läuft - Empfange DSRC/C-ITS Nachrichten auf %u MHz...",
             CITS_CHANNEL_0_MHZ);
    ESP_LOGI(TAG, "Verwende USB-Serial/JTAG für Console/Ausgabe");
    ESP_LOGI(TAG, "Commands: jtag_read [count], sniffer_status, chan <freq>, start_sniffer");
    
    // Zähler für Statistik
    uint32_t packet_count = 0;
    uint32_t last_report = 0;
    
    while (1) {
        vTaskDelay(pdMS_TO_TICKS(60000)); // Alle 60 Sekunden Statistik
        last_report++;
        ESP_LOGI(TAG, "Laufzeit: %lu Min. | Warte auf C-ITS Nachrichten...",
                 (unsigned long)last_report);
    }
}