/**
 * Block 2: C-ITS Parser für ESP32-C5 C-ITS Sniffer
 * 
 * Ziel: C-ITS DATENSATZ parsen und extrahieren
 * 
 * WAVE/DSRC Paket Struktur (IEEE 1609.3):
 * - MAC Header: 14 Bytes
 * - LLC/SNAP: 8 Bytes
 * - WAVE Short Message (WSM): 6 Bytes Header
 * - C-ITS DATENSATZ (Payload)
 */

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

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

static const char *TAG = "CITS_PARSER";

// WAVE Short Message Header Struktur
typedef struct __attribute__((packed)) {
    uint8_t pduLength[3];      // PDU Länge (3 Bytes)
    uint8_t senderId[6];       // Sender MAC Adresse
    uint32_t timestamp;        // Zeitstempel
    uint8_t priority;          // Priorität
    uint8_t pduType;           // PDU Typ
} wave_header_t;

// C-ITS Message Type (BSI Format)
typedef struct __attribute__((packed)) {
    uint8_t msgId;            // Message ID
    uint8_t msgCnt;           // Message Count
    uint8_t sectionNum;       // Section Number
    uint8_t partId;           // Part ID
    uint8_t reserved[8];      // Reserviert
    uint8_t payload[];        // Payload
} cits_bsi_header_t;

// C-ITS Message Types (ETSI TS 103 097)
#define CITS_MSGID_DECEVENT     0x01  // Dezisionsunterstützung
#define CITS_MSGID_DSECN        0x02  // Dekstruktive Umgebung
#define CITS_MSGID_DMM          0x03  // Dynamisches Map-Matching
#define CITS_MSGID_MAP         0x04  // Kartendaten
#define CITS_MSGID_MAPDATA      0x05  // Kartendaten
#define CITS_MSGID_MAPSELDATA   0x06  // Kartendaten Auswahl
#define CITS_MSGID_RSM          0x07  // Road Safety Message
#define CITS_MSGID_CAM         0x08  // Cooperative Awareness Message
#define CITS_MSGID_DENM         0x09  // Decentralized Environmental Notification
#define CITS_MSGID_SPA          0x0A  // Signal Phase and Timing
#define CITS_MSGID_MAM    0x0B  // Map Alignment Message
#define CITS_MSGID_VIT         0x0C  // Vehicle Information Message
#define CITS_MSGID_VSL         0x0D  // Variable Speed Limit
#define CITS_MSGID_SPA         0x0E  // Signal Phase and Timing
#define CITS_MSGID_CSP         0x0F  // Cross-Signal Phase

/**
 * MAC Adresse als String formatieren
 */
static void format_mac(const uint8_t *mac, char *mac_str, size_t size)
{
    snprintf(mac_str, size, "%02X:%02X:%02X:%02X:%02X:%02X",
             mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}

/**
 * WAVE Header parsen
 */
static esp_err_t parse_wave_header(const uint8_t *packet, wave_header_t *wave_hdr)
{
    if (packet == NULL || wave_hdr == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    memcpy(wave_hdr->pduLength, packet, 3);
    memcpy(wave_hdr->senderId, packet + 3, 6);
    memcpy(&wave_hdr->timestamp, packet + 9, 4);
    wave_hdr->priority = packet[13];
    wave_hdr->pduType = packet[14];
    
    return ESP_OK;
}

/**
 * C-ITS BSI Header parsen
 */
static esp_err_t parse_cits_bsi(const uint8_t *packet, cits_bsi_header_t *bsi_hdr)
{
    if (packet == NULL || bsi_hdr == NULL) {
        return ESP_ERR_INVALID_ARG;
    }
    
    bsi_hdr->msgId = packet[0];
    bsi_hdr->msgCnt = packet[1];
    bsi_hdr->sectionNum = packet[2];
    bsi_hdr->partId = packet[3];
    memcpy(bsi_hdr->reserved, packet + 4, 8);
    
    return ESP_OK;
}

/**
 * Nachrichtstyp identifizieren
 */
static const char* get_message_type(uint8_t msgId)
{
    switch (msgId) {
        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";
        default: return "UNKNOWN";
    }
}

/**
 * C-ITS Nachricht verarbeiten
 */
static void process_cits_packet(const uint8_t *packet, uint32_t length, 
                               uint32_t timestamp_sec, uint32_t timestamp_usec)
{
    if (packet == NULL || length < 20) {
        return;
    }
    
    wave_header_t wave_hdr;
    esp_err_t ret = parse_wave_header(packet, &wave_hdr);
    if (ret != ESP_OK) {
        ESP_LOGD(TAG, "Failed to parse WAVE header");
        return;
    }
    
    char mac_str[18];
    format_mac(wave_hdr.senderId, mac_str, sizeof(mac_str));
    
    ESP_LOGI(TAG, "Received C-ITS packet:");
    ESP_LOGI(TAG, "  PDU Length: %lu", (unsigned long)(wave_hdr.pduLength[0] | wave_hdr.pduLength[1] << 8 | wave_hdr.pduLength[2] << 16));
    ESP_LOGI(TAG, "  Sender MAC: %s", mac_str);
    ESP_LOGI(TAG, "  Timestamp: %u.%06u", timestamp_sec, timestamp_usec);
    ESP_LOGI(TAG, "  Priority: %d", wave_hdr.priority);
    ESP_LOGI(TAG, "  PDU Type: %d", wave_hdr.pduType);
    
    // C-ITS BSI Header parsen
    cits_bsi_header_t bsi_hdr;
    ret = parse_cits_bsi(packet + 15, &bsi_hdr);
    if (ret == ESP_OK) {
        ESP_LOGI(TAG, "  Message ID: %d (%s)", bsi_hdr.msgId, get_message_type(bsi_hdr.msgId));
        ESP_LOGI(TAG, "  Message Count: %d", bsi_hdr.msgCnt);
        ESP_LOGI(TAG, "  Section Num: %d", bsi_hdr.sectionNum);
        ESP_LOGI(TAG, "  Part ID: %d", bsi_hdr.partId);
    }
    
    // TODO: C-ITS spezifische Payload parsen
    // Hier können die spezifischen C-ITS Nachrichteninhalt extrahiert werden
}

/**
 * C-ITS Parser initialisieren
 */
esp_err_t cits_parser_init(void)
{
    ESP_LOGI(TAG, "C-ITS Parser initialisiert");
    return ESP_OK;
}

/**
 * C-ITS Parser beenden
 */
void cits_parser_deinit(void)
{
    ESP_LOGI(TAG, "C-ITS Parser beendet");
}

/**
 * Hauptverarbeitungsfunktion
 * Diese wird von der sniffer_task aufgerufen
 */
void cits_process_packet(sniffer_packet_info_t *packet_info)
{
    if (packet_info == NULL || packet_info->payload == NULL) {
        return;
    }
    
    process_cits_packet(packet_info->payload, packet_info->length,
                       packet_info->seconds, packet_info->microseconds);
}