/**
 * Block 2: C-ITS Parser für ESP32-C5 C-ITS Sniffer
 *
 * Ziel: 802.11p / ITS-G5 Pakete empfangen, WAVE-Header parsen,
 *       C-ITS BSI (Basic Security Element) Header extrahieren
 *       und Nachrichtentyp identifizieren.
 *
 * Paket-Struktur (ITS-G5 / IEEE 1609.3 WSM):
 *   [802.11 MAC Header ~14-36B] [LLC/SNAP 8B] [WSM Header 6B] [C-ITS Payload]
 *
 * WSM Header (IEEE 1609.3):
 *   Bytes 0-2:   PDU Length (24-bit)
 *   Bytes 3-8:   Sender MAC (6 Bytes)
 *   Bytes 9-12:  Timestamp (32-bit)
 *   Byte 13:     Priority (0-255)
 *   Byte 14:     PDU Type (WSM)
 *
 * C-ITS BSI (ETSI TS 103 097 / EN 15754):
 *   Byte 0:  Message ID (msgId)
 *   Byte 1:  Message Count (msgCnt)
 *   Byte 2:  Section Number (sectionNum)
 *   Byte 3:  Part ID (partId)
 *   Bytes 4-11: Station ID (8 bytes)
 *   Bytes 12+:  DERSignedCertificate / Payload
 */

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

#include "sdkconfig.h"
#include "esp_err.h"
#include "esp_log.h"
#include "esp_wifi.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "block3_csv_writer.h"

static const char *TAG = "CITS_PARSER";

/* ---------- WAVE Short Message Header ---------- */
typedef struct __attribute__((packed)) {
    uint8_t pdu_length[3];     // PDU Length (little-endian, 24-bit)
    uint8_t sender_id[6];      // Sender MAC
    uint32_t timestamp;        // Timestamp
    uint8_t priority;          // Priority (0=lowest, 255=highest)
    uint8_t pdu_type;          // WSM PDU Type
} wave_header_t;

/* ---------- C-ITS BSI Header (ETSI TS 103 097) ---------- */
typedef struct __attribute__((packed)) {
    uint8_t msg_id;            // Message ID
    uint8_t msg_cnt;           // Message Count
    uint8_t section_num;       // Section Number
    uint8_t part_id;           // Part ID
    uint8_t station_id[8];     // Station ID
} cits_bsi_header_t;

/* ---------- Message IDs nach ETSI TS 103 097 ---------- */
#define CITS_MSGID_DECEVENT     0x01
#define CITS_MSGID_DSECN        0x02
#define CITS_MSGID_DMM          0x03
#define CITS_MSGID_MAP          0x04
#define CITS_MSGID_MAPDATA      0x05
#define CITS_MSGID_MAPSELDATA   0x06
#define CITS_MSGID_RSM          0x07
#define CITS_MSGID_CAM          0x08
#define CITS_MSGID_DENM         0x09
#define CITS_MSGID_SPA          0x0A
#define CITS_MSGID_MAM          0x0B
#define CITS_MSGID_VIT          0x0C
#define CITS_MSGID_VSL          0x0D
#define CITS_MSGID_CSP          0x0E
#define CITS_MSGID_CAMsub       0x10
#define CITS_MSGID_DENMsub      0x11
#define CITS_MSGID_DCC          0x12
#define CITS_MSGID_SSM          0x13
#define CITS_MSGID_IVIM         0x14
#define CITS_MSGID_MAC          0x15
#define CITS_MSGID_CLM          0x16
#define CITS_MSGID_CLMrev       0x17
#define CITS_MSGID_DDM          0x18

static const char *msg_id_str(uint8_t msg_id)
{
    switch (msg_id) {
        case CITS_MSGID_DECEVENT:  return "DECEVENT";
        case CITS_MSGID_DSECN:     return "DSECN";
        case CITS_MSGID_DMM:       return "DMM";
        case CITS_MSGID_MAP:       return "MAP";
        case CITS_MSGID_MAPDATA:   return "MAPDATA";
        case CITS_MSGID_MAPSELDATA:return "MAPSELDATA";
        case CITS_MSGID_RSM:       return "RSM";
        case CITS_MSGID_CAM:       return "CAM";
        case CITS_MSGID_DENM:      return "DENM";
        case CITS_MSGID_SPA:       return "SPA";
        case CITS_MSGID_MAM:       return "MAM";
        case CITS_MSGID_VIT:       return "VIT";
        case CITS_MSGID_VSL:       return "VSL";
        case CITS_MSGID_CSP:       return "CSP";
        case CITS_MSGID_CAMsub:    return "CAMsub";
        case CITS_MSGID_DENMsub:   return "DENMsub";
        case CITS_MSGID_DCC:       return "DCC";
        case CITS_MSGID_SSM:       return "SSM";
        case CITS_MSGID_IVIM:      return "IVIM";
        case CITS_MSGID_MAC:       return "MAC";
        case CITS_MSGID_CLM:       return "CLM";
        case CITS_MSGID_CLMrev:    return "CLMrev";
        case CITS_MSGID_DDM:       return "DDM";
        default:                   return "UNKWN";
    }
}

/* ---------- MAC als hex-String ---------- */
static void mac_to_str(const uint8_t *mac, char *buf, size_t buflen)
{
    snprintf(buf, buflen, "%02X:%02X:%02X:%02X:%02X:%02X",
             mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}

/* ---------- PDU Length (24-bit little-endian) ---------- */
static uint32_t pdu_length_bytes(const uint8_t *pdu_length_bytes)
{
    return pdu_length_bytes[0] | ((uint32_t)pdu_length_bytes[1] << 8)
           | ((uint32_t)pdu_length_bytes[2] << 16);
}

/* ---------- Paket verarbeiten ---------- */
static uint32_t g_packet_count = 0;

void cits_process_packet(const uint8_t *data, uint16_t data_len,
                         int8_t rssi, uint32_t channel_mhz,
                         uint32_t timestamp_sec, uint32_t timestamp_usec)
{
    if (!data || data_len < 30) {
        // Nicht genug Daten für WSM Header
        return;
    }

    g_packet_count++;

    // WSM Header ab offset 12 (14 B MAC + 8 B LLC/SNAP = 22; WSM startet bei 12 im WSM-Offset-Modus)
    // Wir lesen WSM Header direkt ab data[12]
    if (data_len < 12 + sizeof(wave_header_t)) {
        return;
    }

    wave_header_t wave;
    memcpy(&wave, data + 12, sizeof(wave_header_t));

    uint32_t pdu_len = pdu_length_bytes(wave.pdu_length);
    char mac_str[18];
    mac_to_str(wave.sender_id, mac_str, sizeof(mac_str));

    // Prüfen ob C-ITS BSI Header im Payload ist
    uint8_t msg_id = 0;
    uint8_t msg_cnt = 0;
    uint8_t sec_num = 0;
    uint8_t part_id = 0;
    bool has_bsi = false;

    if (data_len >= 12 + sizeof(wave_header_t) + sizeof(cits_bsi_header_t)) {
        const uint8_t *bsi_ptr = data + 12 + sizeof(wave_header_t);
        msg_id     = bsi_ptr[0];
        msg_cnt    = bsi_ptr[1];
        sec_num    = bsi_ptr[2];
        part_id    = bsi_ptr[3];
        has_bsi = true;
    }

    // Log alle empfangenen Pakete
    ESP_LOGI(TAG, "[PKT#%lu] len=%u rssi=%d ch=%u | WAVE PDU=%u MAC=%s pri=%u type=0x%02X",
             (unsigned long)g_packet_count, data_len, rssi, channel_mhz,
             pdu_len, mac_str, wave.priority, wave.pdu_type);

    // Wenn C-ITS BSI gefunden → Detail-Log
    if (has_bsi) {
        char sta_str[24];
        snprintf(sta_str, sizeof(sta_str), "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X",
                 data[12 + sizeof(wave_header_t) + 4],
                 data[12 + sizeof(wave_header_t) + 5],
                 data[12 + sizeof(wave_header_t) + 6],
                 data[12 + sizeof(wave_header_t) + 7],
                 data[12 + sizeof(wave_header_t) + 8],
                 data[12 + sizeof(wave_header_t) + 9],
                 data[12 + sizeof(wave_header_t) + 10],
                 data[12 + sizeof(wave_header_t) + 11]);

        ESP_LOGI(TAG, "  >>> C-ITS %s (msg=0x%02X cnt=%u sec=%u part=%u STA=%s)",
                 msg_id_str(msg_id), msg_id, msg_cnt, sec_num, part_id, sta_str);
    }

    // In CSV schreiben
    cits_message_t csv_msg;
    csv_msg.timestamp_sec = timestamp_sec;
    csv_msg.timestamp_usec = timestamp_usec;
    csv_msg.length = data_len;
    memcpy(csv_msg.sender_id, wave.sender_id, 6);
    csv_msg.message_type = has_bsi ? msg_id : wave.pdu_type;
    csv_msg.channel = channel_mhz;
    csv_msg.rssi = rssi;
    csv_store_cits_message(&csv_msg);
}

/* ---------- Init / Deinit ---------- */
esp_err_t cits_parser_init(void)
{
    g_packet_count = 0;
    ESP_LOGI(TAG, "C-ITS Parser initialisiert");
    return ESP_OK;
}

void cits_parser_deinit(void)
{
    ESP_LOGI(TAG, "C-ITS Parser beendet. Total Pakete: %lu",
             (unsigned long)g_packet_count);
}