// micrOBU firmware: the ESP32-C5 half of the VRU ITS-S on vanetza-idf.
// One station task owns stack, security entity and radio; the serial reader task only
// enqueues frames. Link messages: implementation/station-link/README.md.
#include "link_protocol.hpp"
#include "link_service.hpp"
#include "board_controls.hpp"
#include "simple_ble.hpp"
#include "serial_link.hpp"
#include "station.hpp"
#if CONFIG_MICROBU_TEST_CHANNEL
#include "test_channel.hpp"
#endif
#include <esp_log.h>
#include <esp_task_wdt.h>
#include <esp_timer.h>
#include <nvs_flash.h>
#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <freertos/task.h>
#include <algorithm>
#include <cstdio>
#include <new>

namespace {
const char* TAG = "microbu";

/// @brief Represents one frame received from a transport, to be queued for the station task.
struct Incoming {
    microbu::LinkTransport transport;
    microbu::serial::Frame frame;
};

QueueHandle_t frames = nullptr; // FIFO of Incoming* (8 bytes each); frame bytes never enter the queue itself

/// @brief Attempts to heap-allocate an Incoming so it can be pushed onto the frames queue.
/// @param transport Transport the frame arrived on.
/// @param frame Decoded frame to take ownership of.
/// @return Owning pointer to push onto the queue, or nullptr if allocation failed.
Incoming* try_new_incoming(microbu::LinkTransport transport, microbu::serial::Frame frame) noexcept {
    try {
        // std::move here steals frame's payload buffer into the heap Incoming (no byte copy);
        // it has nothing to do with the queue below, which only ever transports the resulting pointer.
        return new Incoming {transport, std::move(frame)};
    } catch (const std::bad_alloc&) {
        return nullptr;
    }
}

/// @brief Dispatches one queued frame to the link service (or the test channel); logs and drops it on failure.
void handle_incoming(microbu::Station& station, microbu::LinkService& link, const Incoming& incoming) {
    try {
        if (incoming.frame.type == microbu::serial::FrameType::LINK)
            link.handle(incoming.frame.payload, incoming.transport);
#if CONFIG_MICROBU_TEST_CHANNEL
        else if (incoming.transport == microbu::LinkTransport::serial &&
                 incoming.frame.type == microbu::serial::FrameType::TEST)
            microbu::serial::write(microbu::serial::FrameType::TEST,
                                   microbu::test_channel_execute(station, incoming.frame.payload));
#endif
    } catch (const std::bad_alloc&) {
        ESP_LOGE(TAG, "out of memory while handling a frame");
    } catch (const std::exception& e) {
        ESP_LOGE(TAG, "frame handling failed: %s", e.what());
    }
}

/// @brief MicrOBU: BLE and ITS-G5 share the C5's one RF front end. While the phone is talking over
/// USB there is no reason for BLE to take airtime, so advertising stops until the USB link has been
/// quiet for CONFIG_MICROBU_BLE_USB_IDLE_MS. A phone already connected over BLE is left connected.
void pause_ble_while_usb_in_use() {
    const auto last = microbu::serial::last_frame_ms();
    const auto now = static_cast<std::uint32_t>(esp_timer_get_time() / 1000);
    const bool usb_in_use = last != 0 && now - last < CONFIG_MICROBU_BLE_USB_IDLE_MS;
    microbu::ble::set_advertising_allowed(!usb_in_use);
}

/// @brief FreeRTOS task body: owns the station, link service and board controls. Drains the incoming queue.
void station_task(void*) {
    microbu::Station station;
    microbu::LinkService link(station);
    microbu::BoardControls controls;
    station.on_disseminated([&controls] { controls.blink(); });
    station.on_received([&controls] { controls.blink(); });
    ESP_LOGI(TAG, "station task ready; the phone/PC configures the station over BLE or USB Serial-JTAG");
    for (;;) { /// main loop: drain the incoming queue, tick the station and link service, tick the controls
        Incoming* incoming = nullptr;
        // MicrOBU: at least one tick. At CONFIG_FREERTOS_HZ=100 pdMS_TO_TICKS(5) is 0, so the wait
        // never blocked: this priority-10 task spun on the single core, starved every lower-priority
        // task and the idle task (task watchdog), for as long as the board ran.
        if (xQueueReceive(frames, &incoming, std::max<TickType_t>(1, pdMS_TO_TICKS(5))) == pdTRUE && incoming) {
            handle_incoming(station, link, *incoming);
            delete incoming;
        }
        station.tick();
        link.tick();
        controls.tick();
        pause_ble_while_usb_in_use();
    }
}
}

/// @brief Firmware entry point: initializes NVS, watchdog, serial/BLE transports, and starts the station task.
extern "C" void app_main() {
    // NVS: the credential store (and the Wi-Fi driver's calibration data)
    esp_err_t nvs = nvs_flash_init();
    if (nvs == ESP_ERR_NVS_NO_FREE_PAGES || nvs == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        nvs = nvs_flash_init();
    }
    ESP_ERROR_CHECK(nvs);
    // Signing and verification take tens of milliseconds each on the C5. This keeps the idle
    // watchdog from reporting a busy station task (5 s default) while a credential bundle applies.
    esp_task_wdt_config_t watchdog = {};
    watchdog.timeout_ms = 30000;
    watchdog.idle_core_mask = (1 << portNUM_PROCESSORS) - 1;
    watchdog.trigger_panic = false; // log, don't reboot, on timeout
    esp_task_wdt_reconfigure(&watchdog);

    frames = xQueueCreate(32, sizeof(Incoming*));
    microbu::serial::start([](microbu::serial::Frame frame) {
        auto* copy = try_new_incoming(microbu::LinkTransport::serial, std::move(frame));
        if (!copy) return;
        // xQueueSend copies the 8-byte pointer value into the queue, not *copy itself. 
        // station_task then pops pointers FIFO and owns/deletes whatever it receives.
        if (xQueueSend(frames, &copy, 0) != pdTRUE) delete copy; // the phone retries on a missing RESULT
    });
    if (!microbu::ble::start([](microbu::link::Bytes message) {
        microbu::serial::Frame frame {microbu::serial::FrameType::LINK, std::move(message)};
        auto* copy = try_new_incoming(microbu::LinkTransport::ble, std::move(frame));
        if (!copy) return;
        if (xQueueSend(frames, &copy, 0) != pdTRUE) delete copy; // same hand-off as the serial callback above
    })) {
        ESP_LOGE(TAG, "BLE station link failed to start. USB remains available");
    }
    ESP_LOGI(TAG, "MicrOBU obu-firmware on vanetza-idf, station-link v1 + V2X_RX, console on UART0%s",
#if CONFIG_MICROBU_TEST_CHANNEL
             ", test channel enabled");
#else
             "");
#endif
    xTaskCreate(station_task, "station", 12288, nullptr, 10, nullptr);
}