#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "nvs_flash.h"
#include "esp_log.h"
#include "hal/modem_syscon_ll.h"   // modem_syscon_ll_enable_fe_40m_clock() - see initialize_wifi
#include "denm.h"
#include "geonet.h"
#include "dot11p.h"
#include "tx_custom.h" // not called below - kept available for the QoS-Data/tx_custom path if
                        // esp_wifi_80211_tx's non-QoS frame ever proves insufficient again
#include "serial_link.h"
#include "gn_unwrap.h"

static const char *TAG = "obu-tx";

// Phase 03: CAM is no longer built on this chip. The phone fuses its own GNSS+IMU, UPER-encodes
// CAM itself, and hands the finished bytes down over serial_link (SERIAL_MSG_CAM_TX) - this
// firmware's job on transmit shrinks to "GeoNetworking/BTP-wrap + 802.11-wrap + key the PA the
// instant a CAM arrives." There is no on-chip transmit timer anymore; the phone's send cadence
// (1 Hz baseline, faster near intersections/events - all decided app-side) IS the air cadence.
// See cam.c/.h - no longer built (removed from CMakeLists), kept on disk for field-layout
// reference only, since the phone's Kotlin encoder is a byte-exact port of it.
//
// On receive, this firmware now also runs a promiscuous callback (gn_unwrap.c strips
// 802.11/LLC-SNAP/GeoNetworking/BTP-B down to the raw CAM UPER payload) and forwards every CAM
// it hears over the same serial link (SERIAL_MSG_CAM_RX), for the phone's detection engine.
// Single half-duplex radio doing both jobs, same as real ITS-G5 hardware.

// Target frequency: 5900 MHz (ITS-G5 G5-CCH, channel 180). This is what the
// working Rust reference transmits on, proving the C5 PA reaches it despite the
// 5885 datasheet max. The reference sets band-mode 5G, then phy_11p_set +
// phy_change_channel(5900) directly - it does NOT call esp_wifi_set_channel at
// all, so we don't either (channel 180 isn't a normal Wi-Fi channel anyway).
#define TX_FREQ_MHZ  5900

// ---- CAM beacon profile (used for the GeoNetworking layer only now - see below) ----
#define STATION_TYPE            5 // passengerCar (TS 102 894-2 StationType) - matches gn_addr's ST field
#define BTP_PORT_CAM         2001 // BTP-B destination port for CAM (ETSI TS 103 248)

// Bench location, hardcoded since there's no GNSS module wired in yet and the unit is genuinely
// stationary here: 53°33'16.8"N 10°01'20.6"E, in 1/10-microdegree units. Used ONLY for the
// GeoNetworking Source Long Position Vector now (geonet_wrap_shb's own claimed position) - the
// CAM payload's own referencePosition comes from the phone's real GNSS and can legitimately
// differ from this bench placeholder until the GN layer is also given a real position source.
// TODO: feed this from the phone too (e.g. a lightweight position update piggybacked on
// SERIAL_MSG_CAM_TX, or a new small message type) instead of a fixed bench location.
#define BENCH_LATITUDE_TENMICRODEG    535546667
#define BENCH_LONGITUDE_TENMICRODEG   100223889

// Single source of truth for the pseudonym/link-layer address: used both as
// the 802.11 source MAC (Addr2) and as GN_ADDR's MID field, since the GN
// spec defines those as being the same address. Locally-administered bit
// set (0x02) per normal MAC convention. Fixed/non-rotating for now - real
// stacks rotate this every 5-15 min for privacy. Owned entirely by this firmware (not the
// phone) per the Phase 03 design decision - simplest given the phone never needs to know it.
static const uint8_t pseudonym_mac[6] = {0x02, 0x00, 0x00, 0x00, 0x00, 0x01};

// Undocumented libphy.a calls that push the radio into 802.11p OCB mode on
// the 5.9 GHz ITS-G5 band. See docs/04-transmit-setup.md for source + what
// to do if the linker can't find these symbols in your ESP-IDF version.
extern void phy_11p_set(int enable, int unused);
extern void phy_change_channel(int freq_mhz, int bw_mode, int sec_chan_offset, int unused);

// ============================================================================
// ---- TX path: phone -> serial_link -> queue -> radio task -> air ----------
// ============================================================================

// One CAM-to-transmit item. Fixed-size (no malloc) since SERIAL_LINK_MAX_PAYLOAD bounds it -
// simplest safe option for a queue this small and this hot.
typedef struct {
    uint8_t data[SERIAL_LINK_MAX_PAYLOAD];
    int len;
} cam_tx_item_t;

static QueueHandle_t s_tx_queue;

// Called directly from serial_link's UART RX task the instant a checksummed SERIAL_MSG_CAM_TX
// frame arrives - MUST be fast (documented in serial_link.h), so this only copies into a queue
// item and returns; the actual GeoNetworking-wrap + 802.11-wrap + radio TX happens in
// tx_radio_task below, off the UART parsing path entirely. xQueueSend with 0 timeout: if the
// radio task is somehow behind, drop this CAM rather than stall UART frame parsing - the next
// one is only ~1s (or less, at elevated rate) away regardless.
static void on_cam_tx_from_phone(const uint8_t *cam_uper, int cam_len)
{
    if (cam_len <= 0 || cam_len > SERIAL_LINK_MAX_PAYLOAD) {
        ESP_LOGW(TAG, "on_cam_tx_from_phone: bad length %d", cam_len);
        return;
    }
    cam_tx_item_t item;
    item.len = cam_len;
    memcpy(item.data, cam_uper, (size_t)cam_len);
    if (xQueueSend(s_tx_queue, &item, 0) != pdTRUE) {
        ESP_LOGW(TAG, "tx queue full, dropping CAM from phone");
    }
}

static void tx_radio_task(void *arg)
{
    (void)arg;
    cam_tx_item_t item;

    while (1) {
        if (xQueueReceive(s_tx_queue, &item, portMAX_DELAY) != pdTRUE) {
            continue;
        }

        // static, not stack: these are sized off SERIAL_LINK_MAX_PAYLOAD (raised to 512), so on
        // the stack they'd be ~1.2 KB of this task's 4 KB. Safe as statics - tx_radio_task is a
        // singleton, created once in app_main. Both wrap functions bounds-check against the size
        // passed in and return <= 0 on overflow, so an oversized CAM is rejected, not written past.
        static uint8_t gn_payload[SERIAL_LINK_MAX_PAYLOAD + 64];
        int gn_len = geonet_wrap_shb(item.data, item.len, pseudonym_mac, STATION_TYPE,
                                     BENCH_LATITUDE_TENMICRODEG, BENCH_LONGITUDE_TENMICRODEG,
                                     BTP_PORT_CAM, gn_payload, sizeof(gn_payload));
        if (gn_len <= 0) {
            ESP_LOGW(TAG, "geonet_wrap_shb failed (cam_len=%d)", item.len);
            continue;
        }

        static uint8_t frame[SERIAL_LINK_MAX_PAYLOAD + 192];
        int frame_len = dot11p_build_frame(gn_payload, gn_len, pseudonym_mac, frame,
                                           sizeof(frame), false);
        if (frame_len <= 0) {
            ESP_LOGW(TAG, "dot11p_build_frame failed (gn_len=%d)", gn_len);
            continue;
        }

        // Standard, well-tested raw-TX API with a non-QoS Data frame - same path validated
        // during Phase 2 bring-up (see git history for the tx_custom.c A/B test that led here).
        esp_err_t err = esp_wifi_80211_tx(WIFI_IF_STA, frame, frame_len, true);
        if (err != ESP_OK) {
            // Also counted into the heartbeat so the phone can see it - from the app's side a CAM
            // that reached the radio but didn't go out otherwise looks identical to one that did.
            serial_link_note_tx_failure();
            ESP_LOGW(TAG, "esp_wifi_80211_tx failed: %d", err);
        } else {
            ESP_LOGI(TAG, "CAM sent (%d bytes) @ %d MHz", frame_len, TX_FREQ_MHZ);
        }
    }
}

// ============================================================================
// ---- RX path: air -> promiscuous cb -> queue -> forward task -> serial_link
// ============================================================================

// Promiscuous RX callbacks run in the WiFi driver's own task context and must stay short - so,
// same pattern as the TX side and as the reference sniffer firmware (cmd_sniffer.c's
// queue_packet), this just copies the frame and queues it; gn_unwrap_cam() and the serial write
// both happen in rx_forward_task instead.
// Capture buffer per queued frame. 800 bytes because real traffic is much larger than our own
// TX: a CiT One CAM measures 286-355 bytes on air and its GeoBroadcast DENM measures 528
// (measured 2026-08-17). The previous 400 silently truncated every DENM mid-payload, which no
// amount of correct unwrapping downstream could have recovered from. Raise this before adding
// MAPEM, which is larger again.
#define RX_FRAME_MAX_LEN 800

typedef struct {
    uint8_t data[RX_FRAME_MAX_LEN];
    int len;
    int8_t rssi;
} rx_item_t;

static QueueHandle_t s_rx_queue;

static void wifi_promisc_rx_cb(void *recv_buf, wifi_promiscuous_pkt_type_t type)
{
    if (type == WIFI_PKT_MISC) {
        return; // no payload of interest, mirrors cmd_sniffer.c's handling
    }
    wifi_promiscuous_pkt_t *packet = (wifi_promiscuous_pkt_t *)recv_buf;
    if (packet->rx_ctrl.rx_state) {
        return; // frame had an error (mirrors cmd_sniffer.c)
    }

#if CONFIG_SOC_WIFI_HE_SUPPORT
    int length = packet->rx_ctrl.dump_len;
#else
    int length = packet->rx_ctrl.sig_len - 4 /* FCS */;
#endif
    if (length <= 0) {
        return;
    }

    rx_item_t item;
    item.len = length > (int)sizeof(item.data) ? (int)sizeof(item.data) : length;
    memcpy(item.data, packet->payload, (size_t)item.len);
    item.rssi = packet->rx_ctrl.rssi;

    // 0 timeout: never block the WiFi driver's own task waiting for queue space.
    xQueueSend(s_rx_queue, &item, 0);
}

static void rx_forward_task(void *arg)
{
    (void)arg;
    rx_item_t item;

    while (1) {
        if (xQueueReceive(s_rx_queue, &item, portMAX_DELAY) != pdTRUE) {
            continue;
        }

        // Most promiscuously-captured frames are NOT ITS traffic we handle (management/control
        // frames, other message types, our own loopback if the driver echoes it) - gn_unwrap_its
        // returning false here is the common case, not an error, so it isn't logged per frame.
        gn_rx_t rx;
        if (gn_unwrap_its(item.data, item.len, &rx)) {
            serial_link_send_v2x_rx(rx.btp_dest_port, item.rssi,
                                    rx.has_geo_area,
                                    rx.geo_area_lat_tenmicrodeg,
                                    rx.geo_area_lon_tenmicrodeg,
                                    rx.geo_area_distance_a_m,
                                    rx.payload, rx.payload_len);
        }
    }
}

// ============================================================================

void app_main(void)
{
    ESP_ERROR_CHECK(nvs_flash_init());
    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());

    s_tx_queue = xQueueCreate(4, sizeof(cam_tx_item_t));
    s_rx_queue = xQueueCreate(8, sizeof(rx_item_t));
    if (!s_tx_queue || !s_rx_queue) {
        ESP_LOGE(TAG, "queue creation failed - halting");
        return;
    }

    // Enable the modem FRONT-END 40 MHz clock BEFORE esp_wifi_init(). This is
    // the one step the proven-working receiver firmware
    // (its-g5-receiver-firmware_txenabled, main/main.c -> initialize_wifi())
    // performs that this OBU was missing. Without the FE clock enabled the
    // 5 GHz front-end / transmit chain is not fully clocked - which matches the
    // exact symptom here: the radio calibrates (boot RF ping) and receives
    // fine, but data frames are accepted by the API and never actually key the
    // PA. This is a low-level modem_syscon register write via the HAL LL layer,
    // copied verbatim from the reference firmware.
    modem_syscon_ll_enable_fe_40m_clock(&MODEM_SYSCON, 1);

    wifi_init_config_t wifi_cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&wifi_cfg));
    ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); // match reference initialize_wifi()
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
    ESP_ERROR_CHECK(esp_wifi_start());

    // ---- Regulatory / TX-authorization override -----------------------------
    // THE fix for "RX works but TX is silent". By default the driver uses
    // WIFI_COUNTRY_POLICY_AUTO, whose 5 GHz regulatory table does NOT authorize
    // transmit on the 5.9 GHz ITS band (and treats DFS channels as no-IR /
    // radar-gated). Receiving is never gated - which is exactly why the sniffer
    // hears traffic but our own frames never key the PA, and why the only RF
    // seen from this board is the uninhibited PHY-calibration burst at boot.
    //
    // Switching to WIFI_COUNTRY_POLICY_MANUAL with an explicit 5 GHz channel
    // mask (wifi_5g_channel_mask, which only takes effect under manual policy)
    // tells the driver these channels are permitted and lifts the transmit
    // gate. Manual policy = the operator asserts regulatory responsibility, which is
    // appropriate for licensed/university research on the ITS band.
    wifi_country_t ctry = {
        .cc = "US",                            // nominal under manual policy
        .schan = 1,
        .nchan = 11,
        .policy = WIFI_COUNTRY_POLICY_MANUAL,
        .wifi_5g_channel_mask = 0x1FFFFFFE,    // all 5 GHz channels, bits 1..28 (incl. 140 and 177)
    };
    esp_err_t ctry_err = esp_wifi_set_country(&ctry);
    if (ctry_err != ESP_OK) {
        ESP_LOGW(TAG, "esp_wifi_set_country(MANUAL) failed: %d (continuing)", ctry_err);
    }
    // Ensure the PA runs at full configured power (not a reduced regulatory
    // default). Units are 0.25 dBm; 80 = 20 dBm.
    esp_wifi_set_max_tx_power(80);
    // -------------------------------------------------------------------------

    // Force the dual-band C5 onto its 5 GHz PHY. This MUST be called after
    // esp_wifi_start() - calling it before returns ESP_ERR_WIFI_NOT_STARTED
    // (0x3002 / 12290). Locking the band to 5G explicitly keeps the driver
    // from ever falling back to 2.4 GHz ch1 (the old "stuck at primary=1"
    // symptom), which would key the wrong PHY and make us inaudible to a
    // 5.9 GHz sniffer/peer. Valid 5 GHz channels on the C5 are 36..177. Not
    // ESP_ERROR_CHECK'd: log and continue if a given IDF build differs.
    esp_err_t band_err = esp_wifi_set_band_mode(WIFI_BAND_MODE_5G_ONLY);
    if (band_err != ESP_OK) {
        ESP_LOGW(TAG, "esp_wifi_set_band_mode(5G_ONLY) failed: %d (continuing)", band_err);
    }

    // Disable Wi-Fi power save. An unassociated STA with the default
    // WIFI_PS_MIN_MODEM power save sleeps its radio between beacons it will
    // never receive (we're not joined to any AP), and drops outbound raw
    // frames while asleep. Also matters for RX now: a sleeping radio misses
    // incoming CAMs just as easily as it drops outbound ones. Must be called
    // after esp_wifi_start().
    ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));

    // Register the promiscuous RX callback BEFORE enabling promiscuous mode, so there's no
    // window where promiscuous mode is on but nothing is registered to receive frames from it.
    ESP_ERROR_CHECK(esp_wifi_set_promiscuous_rx_cb(wifi_promisc_rx_cb));

    // Enable promiscuous mode. Doubles as the fix for raw-TX being silently dropped
    // (ESP-IDF only actually emits raw frames when the MAC is promiscuous or associated to an
    // AP - plain unassociated STA is neither) AND as what makes RX possible at all outside a
    // joined BSS. One radio, one mode, both jobs - see file header comment.
    ESP_ERROR_CHECK(esp_wifi_set_promiscuous(true));

    // Force 802.11p OCB mode on the ITS-G5 channel, exactly like the working
    // Rust reference (esp32-c_its-companion, src/radio.rs setup_wifi_sniffer):
    // enable 802.11p, then jump straight to the target frequency. With band-mode
    // already locked to 5 GHz above, NO esp_wifi_set_channel priming is needed -
    // channel 180 (5900 MHz) isn't a normal Wi-Fi channel anyway. phy_change_channel
    // takes the frequency in MHz.
    ESP_LOGI(TAG, "about to call phy_11p_set...");
    phy_11p_set(1, 0);
    ESP_LOGI(TAG, "phy_11p_set returned, about to call phy_change_channel(%d)...", TX_FREQ_MHZ);
    phy_change_channel(TX_FREQ_MHZ, 1, 0, 0);
    ESP_LOGI(TAG, "phy_change_channel returned");

    xTaskCreate(tx_radio_task, "tx_radio", 4096, NULL, 6, NULL);
    xTaskCreate(rx_forward_task, "rx_forward", 4096, NULL, 5, NULL);
    serial_link_init(on_cam_tx_from_phone);

    ESP_LOGW(TAG, "OCB @ %d MHz - TX/RX armed, driven by serial_link (no on-chip TX timer)",
             TX_FREQ_MHZ);
}