/**
 * @file gps_assist.c
 * @brief AssistNow UDP forwarder — receives UBX binary from server, writes to UART.
 *
 * Listens on a UDP socket.  The first datagram whose first two bytes are the
 * UBX sync characters (0xB5 0x62) is forwarded verbatim to the GPS UART.
 * After forwarding, the task closes the socket and deletes itself so it
 * consumes no further resources once assistance data has been loaded.
 *
 * UART1 must already be running at 115200 baud (gps_nmea_init() does this).
 */

#include "gps_assist.h"

#include <string.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/uart.h"
#include "esp_log.h"
#include "lwip/sockets.h"
#include "lwip/netdb.h"

/* ---- Constants ---- */

#define TAG                 "GPS_ASSIST"
#define GPS_ASSIST_BUF_SIZE 4096
#define GPS_UART_NUM        UART_NUM_1

/* UBX sync bytes */
#define UBX_SYNC1  0xB5u
#define UBX_SYNC2  0x62u

/* Task parameters */
#define ASSIST_TASK_STACK     4096
#define ASSIST_TASK_PRIORITY  4

/* ---- Module state ---- */

static TaskHandle_t s_task_handle = NULL;
static uint16_t     s_udp_port    = 4212;

/* ---- Task implementation ---- */

static void gps_assist_task(void *arg)
{
    (void)arg;
    uint8_t *buf = malloc(GPS_ASSIST_BUF_SIZE);
    if (!buf) {
        ESP_LOGE(TAG, "Failed to allocate receive buffer");
        vTaskDelete(NULL);
        return;
    }

    int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
    if (sock < 0) {
        ESP_LOGE(TAG, "socket() failed: errno %d", errno);
        free(buf);
        vTaskDelete(NULL);
        return;
    }

    /* Set receive timeout so the task can be deleted cleanly */
    struct timeval tv = { .tv_sec = 5, .tv_usec = 0 };
    setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));

    struct sockaddr_in bind_addr = {
        .sin_family      = AF_INET,
        .sin_port        = htons(s_udp_port),
        .sin_addr.s_addr = htonl(INADDR_ANY),
    };
    if (bind(sock, (struct sockaddr *)&bind_addr, sizeof(bind_addr)) < 0) {
        ESP_LOGE(TAG, "bind() failed: errno %d", errno);
        close(sock);
        free(buf);
        vTaskDelete(NULL);
        return;
    }

    ESP_LOGI(TAG, "Listening for AssistNow data on UDP port %u", s_udp_port);

    while (true) {
        struct sockaddr_in src;
        socklen_t src_len = sizeof(src);
        int n = recvfrom(sock, buf, GPS_ASSIST_BUF_SIZE, 0,
                         (struct sockaddr *)&src, &src_len);
        if (n < 0) {
            if (errno == EAGAIN || errno == EWOULDBLOCK) {
                continue;
            }
            ESP_LOGW(TAG, "recvfrom error: errno %d", errno);
            continue;
        }

        if (n < 2) {
            ESP_LOGD(TAG, "Packet too short (%d bytes), discarding", n);
            continue;
        }

        /* Validate UBX sync header */
        if (buf[0] != UBX_SYNC1 || buf[1] != UBX_SYNC2) {
            ESP_LOGW(TAG, "Non-UBX packet (%02X %02X), discarding",
                     buf[0], buf[1]);
            continue;
        }

        /* Forward raw UBX bytes to GPS chip */
        int written = uart_write_bytes(GPS_UART_NUM, (const char *)buf, n);
        uart_wait_tx_done(GPS_UART_NUM, pdMS_TO_TICKS(500));

        ESP_LOGI(TAG, "Forwarded %d UBX bytes to GPS UART (wrote %d) — position seed applied",
                 n, written);

        /* Keep listening — server may send improved seeds as GPS accuracy improves */
    }

    close(sock);
    free(buf);
    s_task_handle = NULL;
    vTaskDelete(NULL);
}

/* ---- Public API ---- */

void gps_assist_init(uint16_t udp_port)
{
    if (s_task_handle) {
        ESP_LOGW(TAG, "gps_assist_init called while task already running");
        return;
    }
    s_udp_port = udp_port;
    xTaskCreate(gps_assist_task, "gps_assist", ASSIST_TASK_STACK, NULL,
                ASSIST_TASK_PRIORITY, &s_task_handle);
    ESP_LOGI(TAG, "AssistNow receiver spawned (port %u)", udp_port);
}

void gps_assist_deinit(void)
{
    if (s_task_handle) {
        vTaskDelete(s_task_handle);
        s_task_handle = NULL;
        ESP_LOGI(TAG, "AssistNow task deleted");
    }
}