/**
 * Block 1: Hardware Initialisierung für ESP32-C5 C-ITS Sniffer (ITS-G5 / 802.11p)
 *
 * Ziel: WiFi als NULL-Mode + Promiscuous Mode auf 802.11p konfigurieren
 * ESP32-C5: WiFi unterstützt ITS-G5/802.11p via interne PHY-Register
 *
 * C-ITS-Kanäle (5,9 GHz DSRC nach ETSI EN 302 571):
 *   Kanal 0: 5900 MHz (Primary, Channel 183)
 *   Kanal 1: 5890 MHz (Channel 182)
 *   Kanal 2: 5880 MHz (Channel 181)
 *   Kanal 3: 5870 MHz (Channel 180)
 */

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

#include "sdkconfig.h"
#include "esp_err.h"
#include "esp_log.h"
#include "esp_wifi.h"
#include "esp_timer.h"
#include "nvs_flash.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"

static const char *TAG = "CITS_BLOCK1";

/* ---------- Konfiguration ---------- */
#define CITS_CHANNEL_0_MHZ    5900
#define CITS_CHANNEL_1_MHZ    5890
#define CITS_CHANNEL_2_MHZ    5880
#define CITS_CHANNEL_3_MHZ    5870
#define LED_PIN               GPIO_NUM_28

/* ---------- Externe Callback (wird von block4 definiert) ---------- */
/* Wird vom WiFi-Treiber bei jedem empfangenen Paket aufgerufen. */
void sniffer_on_raw_packet(void *buf, wifi_promiscuous_pkt_type_t type);

/* ---------- WiFi initialisieren ---------- */
static esp_err_t initialize_wifi(void)
{
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
    ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM));
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_NULL));
    ESP_ERROR_CHECK(esp_wifi_start());

    ESP_LOGI(TAG, "WiFi initialisiert (NULL-Mode / Promiscuous)");
    return ESP_OK;
}

/* ---------- 802.11p PHY aktivieren ---------- */
/* ESP32-C5 unterstützt ITS-G5 über interne WiFi-Register.
   Die PHY-Konfiguration erfolgt durch den ESP-IDF WiFi-Treiber.
   Wichtig: Kanal muss korrekt gesetzt werden. */
static void init_80211p_phy(void)
{
    // ESP32-C5: ITS-G5-Modus wird durch den Kanal und den PHY-Modus
    // automatisch aktiviert. Kein separater Aufruf nötig.
    ESP_LOGI(TAG, "802.11p PHY aktiv (ESP32-C5 nativ unterstützt)");
}

/* ---------- C-ITS Kanal setzen ---------- */
/* Channel 183 = 5900 MHz, 182 = 5890 MHz, etc. */
esp_err_t set_cits_channel(uint32_t freq_mhz)
{
    // WiFi channel numbers for 5 GHz ITS-G5:
    // 5900 MHz → channel 183
    // 5890 MHz → channel 182
    // 5880 MHz → channel 181
    // 5870 MHz → channel 180
    uint8_t channel = (uint8_t)((freq_mhz - 5600) / 5);

    esp_err_t ret = esp_wifi_set_channel(channel, WIFI_SECOND_CHAN_NONE);
    if (ret == ESP_OK) {
        ESP_LOGI(TAG, "Kanal auf %u MHz (WiFi channel %d) gesetzt", freq_mhz, channel);
    } else {
        ESP_LOGE(TAG, "Kanal-Wechsel zu %u MHz fehlgeschlagen: %s", freq_mhz, esp_err_to_name(ret));
    }
    return ret;
}

/* ---------- Promiscuous Mode starten ---------- */
esp_err_t start_promiscuous_mode(uint32_t channel_mhz)
{
    wifi_promiscuous_filter_t filter = {
        .filter_mask = WIFI_PROMIS_FILTER_MASK_ALL,
    };

    ESP_ERROR_CHECK(esp_wifi_set_promiscuous_filter(&filter));
    ESP_ERROR_CHECK(esp_wifi_set_promiscuous_rx_cb(sniffer_on_raw_packet));
    ESP_ERROR_CHECK(esp_wifi_set_promiscuous(true));

    init_80211p_phy();
    set_cits_channel(channel_mhz);

    ESP_LOGI(TAG, "Promiscuous Mode aktiv auf %u MHz", channel_mhz);
    return ESP_OK;
}

/* ---------- Promiscuous Mode stoppen ---------- */
void stop_promiscuous_mode(void)
{
    esp_wifi_set_promiscuous(false);
    esp_wifi_set_promiscuous_filter(NULL);
    ESP_LOGI(TAG, "Promiscuous Mode gestoppt");
}

/* ---------- NVS Flash initialisieren ---------- */
static esp_err_t init_nvs_flash(void)
{
    esp_err_t err = nvs_flash_init();
    if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        err = nvs_flash_init();
    }
    ESP_ERROR_CHECK(err);
    ESP_LOGI(TAG, "NVS Flash initialisiert");
    return ESP_OK;
}

/* ---------- LED Pin ---------- */
static void init_led(void)
{
    gpio_reset_pin(LED_PIN);
    gpio_set_direction(LED_PIN, GPIO_MODE_OUTPUT);
    gpio_set_level(LED_PIN, 0);
    ESP_LOGI(TAG, "LED auf GPIO%d initialisiert", LED_PIN);
}

/* ---------- Console-Befehle (nur wenn Console verfügbar) ---------- */

#ifdef CONFIG_ESP_CONSOLE_USB_SERIAL_JTAG
#include "esp_console.h"

static int cmd_set_channel(int argc, char **argv)
{
    if (argc < 1) {
        printf("Nutzung: set_channel <freq_mhz>\n");
        return 0;
    }
    uint32_t freq = atoi(argv[0]);
    if (freq < 5800 || freq > 5900) {
        printf("Frequenz muss zwischen 5800 und 5900 MHz liegen\n");
        return 1;
    }
    set_cits_channel(freq);
    printf("Kanal auf %" PRIu32 " MHz gesetzt\n", freq);
    return 0;
}

static int cmd_stop_sniffer_cmd(int argc, char **argv)
{
    (void)argc; (void)argv;
    stop_promiscuous_mode();
    printf("Sniffer gestoppt\n");
    return 0;
}

void register_cits_commands(void)
{
    const esp_console_cmd_t cmd_set_ch = {
        .command = "set_channel",
        .help    = "Set C-ITS channel (5800-5900 MHz)",
        .hint    = NULL,
        .func    = &cmd_set_channel,
    };
    esp_console_cmd_register(&cmd_set_ch);

    const esp_console_cmd_t cmd_stop = {
        .command = "stop_sniffer",
        .help    = "Stop promiscuous sniffer",
        .hint    = NULL,
        .func    = &cmd_stop_sniffer_cmd,
    };
    esp_console_cmd_register(&cmd_stop);
}
#endif