/* orin_serial.c — Orin↔ESP32-S3 serial protocol implementation (bd-66hx)
 *
 * Implements the binary framing protocol matching bd-wim1 (Orin side).
 * CRC8-SMBUS: poly=0x07, init=0x00, covers LEN+TYPE+PAYLOAD bytes.
 */

#include "orin_serial.h"
#include "config.h"
#include "driver/uart.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include <string.h>

static const char *TAG = "orin";

/* ── Shared state ── */
orin_drive_t   g_orin_drive   = {0};
orin_pid_t     g_orin_pid     = {0};
orin_control_t g_orin_ctrl    = {.armed = false, .estop = false, .hb_last_ms = 0};

/* ── CRC8-SMBUS (poly=0x07, init=0x00) ── */
static uint8_t crc8(const uint8_t *data, uint8_t len)
{
    uint8_t crc = 0x00u;
    for (uint8_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (uint8_t b = 0; b < 8u; b++) {
            crc = (crc & 0x80u) ? (uint8_t)((crc << 1u) ^ 0x07u) : (uint8_t)(crc << 1u);
        }
    }
    return crc;
}

/* ── Frame builder ── */
static void build_frame(orin_tx_frame_t *f, uint8_t out[/* ORIN_MAX_PAYLOAD + 4 */], uint8_t *out_len)
{
    /* [SYNC][LEN][TYPE][PAYLOAD...][CRC] */
    uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD];
    crc_buf[0] = f->len;
    crc_buf[1] = f->type;
    memcpy(&crc_buf[2], f->payload, f->len);
    uint8_t crc = crc8(crc_buf, (uint8_t)(2u + f->len));

    out[0] = ORIN_SYNC;
    out[1] = f->len;
    out[2] = f->type;
    memcpy(&out[3], f->payload, f->len);
    out[3u + f->len] = crc;
    *out_len = (uint8_t)(4u + f->len);
}

/* ── Enqueue helpers ── */
static void enqueue(QueueHandle_t q, uint8_t type, const uint8_t *payload, uint8_t len)
{
    orin_tx_frame_t f = {.type = type, .len = len};
    if (len > 0u && payload) {
        memcpy(f.payload, payload, len);
    }
    if (xQueueSend(q, &f, 0) != pdTRUE) {
        ESP_LOGW(TAG, "tx queue full, dropped type=0x%02x", type);
    }
}

void orin_send_ack(QueueHandle_t q, uint8_t cmd_type)
{
    enqueue(q, RESP_ACK, &cmd_type, 1u);
}

void orin_send_nack(QueueHandle_t q, uint8_t cmd_type, uint8_t err)
{
    uint8_t p[2] = {cmd_type, err};
    enqueue(q, RESP_NACK, p, 2u);
}

void orin_send_status(QueueHandle_t q,
                      int16_t pitch_x10, int16_t motor_cmd,
                      uint16_t vbat_mv, bal_state_t state, uint8_t flags)
{
    /* int16 pitch_x10, int16 motor_cmd, uint16 vbat_mv, uint8 state, uint8 flags — BE */
    uint8_t p[8];
    p[0] = (uint8_t)((uint16_t)pitch_x10 >> 8u);
    p[1] = (uint8_t)((uint16_t)pitch_x10);
    p[2] = (uint8_t)((uint16_t)motor_cmd >> 8u);
    p[3] = (uint8_t)((uint16_t)motor_cmd);
    p[4] = (uint8_t)(vbat_mv >> 8u);
    p[5] = (uint8_t)(vbat_mv);
    p[6] = (uint8_t)state;
    p[7] = flags;
    enqueue(q, TELEM_STATUS, p, 8u);
}

void orin_send_vesc(QueueHandle_t q, uint8_t telem_type,
                    int32_t erpm, uint16_t voltage_mv,
                    int16_t current_ma, uint16_t temp_c_x10)
{
    /* int32 erpm, uint16 voltage_mv, int16 current_ma, uint16 temp_c_x10 — BE */
    uint8_t p[10];
    uint32_t u = (uint32_t)erpm;
    p[0] = (uint8_t)(u >> 24u);
    p[1] = (uint8_t)(u >> 16u);
    p[2] = (uint8_t)(u >>  8u);
    p[3] = (uint8_t)(u);
    p[4] = (uint8_t)(voltage_mv >> 8u);
    p[5] = (uint8_t)(voltage_mv);
    p[6] = (uint8_t)((uint16_t)current_ma >> 8u);
    p[7] = (uint8_t)((uint16_t)current_ma);
    p[8] = (uint8_t)(temp_c_x10 >> 8u);
    p[9] = (uint8_t)(temp_c_x10);
    enqueue(q, telem_type, p, 10u);
}

/* ── UART init ── */
void orin_serial_init(void)
{
    uart_config_t cfg = {
        .baud_rate  = ORIN_UART_BAUD,
        .data_bits  = UART_DATA_8_BITS,
        .parity     = UART_PARITY_DISABLE,
        .stop_bits  = UART_STOP_BITS_1,
        .flow_ctrl  = UART_HW_FLOWCTRL_DISABLE,
    };
    ESP_ERROR_CHECK(uart_param_config(ORIN_UART_PORT, &cfg));
    ESP_ERROR_CHECK(uart_set_pin(ORIN_UART_PORT,
                                  ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO,
                                  UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
    ESP_ERROR_CHECK(uart_driver_install(ORIN_UART_PORT, ORIN_UART_RX_BUF, 0,
                                        0, NULL, 0));
    ESP_LOGI(TAG, "UART%d init OK: tx=%d rx=%d baud=%d",
             ORIN_UART_PORT, ORIN_UART_TX_GPIO, ORIN_UART_RX_GPIO, ORIN_UART_BAUD);
}

/* ── RX parser state machine ── */
typedef enum {
    WAIT_SYNC,
    WAIT_LEN,
    WAIT_TYPE,
    WAIT_PAYLOAD,
    WAIT_CRC,
} rx_state_t;

static void dispatch_cmd(uint8_t type, const uint8_t *payload, uint8_t len,
                          QueueHandle_t tx_q)
{
    uint32_t now_ms = (uint32_t)(esp_timer_get_time() / 1000LL);

    switch (type) {
    case CMD_HEARTBEAT:
        g_orin_ctrl.hb_last_ms = now_ms;
        orin_send_ack(tx_q, type);
        break;

    case CMD_DRIVE:
        if (len < 4u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
        if (g_orin_ctrl.estop) { orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE); break; }
        if (!g_orin_ctrl.armed) { orin_send_nack(tx_q, type, ERR_DISARMED); break; }
        g_orin_drive.speed      = (int16_t)(((uint16_t)payload[0] << 8u) | payload[1]);
        g_orin_drive.steer      = (int16_t)(((uint16_t)payload[2] << 8u) | payload[3]);
        g_orin_drive.updated_ms = now_ms;
        g_orin_ctrl.hb_last_ms  = now_ms;  /* drive counts as heartbeat */
        orin_send_ack(tx_q, type);
        break;

    case CMD_ESTOP:
        if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
        g_orin_ctrl.estop = (payload[0] != 0u);
        if (g_orin_ctrl.estop) {
            g_orin_drive.speed = 0;
            g_orin_drive.steer = 0;
        }
        orin_send_ack(tx_q, type);
        break;

    case CMD_ARM:
        if (len < 1u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
        if (g_orin_ctrl.estop && payload[0] != 0u) {
            /* cannot arm while estop is active */
            orin_send_nack(tx_q, type, ERR_ESTOP_ACTIVE);
            break;
        }
        g_orin_ctrl.armed = (payload[0] != 0u);
        if (!g_orin_ctrl.armed) {
            g_orin_drive.speed = 0;
            g_orin_drive.steer = 0;
        }
        orin_send_ack(tx_q, type);
        break;

    case CMD_PID:
        if (len < 12u) { orin_send_nack(tx_q, type, ERR_BAD_LEN); break; }
        /* float32 big-endian: copy and swap bytes */
        {
            uint32_t raw;
            raw = ((uint32_t)payload[0] << 24u) | ((uint32_t)payload[1] << 16u) |
                  ((uint32_t)payload[2] <<  8u) |  (uint32_t)payload[3];
            memcpy((void*)&g_orin_pid.kp, &raw, 4u);
            raw = ((uint32_t)payload[4] << 24u) | ((uint32_t)payload[5] << 16u) |
                  ((uint32_t)payload[6] <<  8u) |  (uint32_t)payload[7];
            memcpy((void*)&g_orin_pid.ki, &raw, 4u);
            raw = ((uint32_t)payload[8] << 24u) | ((uint32_t)payload[9] << 16u) |
                  ((uint32_t)payload[10] << 8u) |  (uint32_t)payload[11];
            memcpy((void*)&g_orin_pid.kd, &raw, 4u);
            g_orin_pid.updated = true;
        }
        orin_send_ack(tx_q, type);
        break;

    default:
        ESP_LOGW(TAG, "unknown cmd type=0x%02x", type);
        break;
    }
}

void orin_serial_rx_task(void *arg)
{
    QueueHandle_t tx_q = (QueueHandle_t)arg;
    rx_state_t state   = WAIT_SYNC;
    uint8_t    rx_len  = 0;
    uint8_t    rx_type = 0;
    uint8_t    payload[ORIN_MAX_PAYLOAD];
    uint8_t    pay_idx = 0;

    uint8_t byte;
    for (;;) {
        int r = uart_read_bytes(ORIN_UART_PORT, &byte, 1, pdMS_TO_TICKS(10));
        if (r <= 0) {
            continue;
        }

        switch (state) {
        case WAIT_SYNC:
            if (byte == ORIN_SYNC) { state = WAIT_LEN; }
            break;

        case WAIT_LEN:
            if (byte > ORIN_MAX_PAYLOAD) {
                /* oversize — send NACK and reset */
                orin_send_nack(tx_q, 0x00u, ERR_BAD_LEN);
                state = WAIT_SYNC;
            } else {
                rx_len = byte;
                state  = WAIT_TYPE;
            }
            break;

        case WAIT_TYPE:
            rx_type = byte;
            pay_idx = 0u;
            state   = (rx_len == 0u) ? WAIT_CRC : WAIT_PAYLOAD;
            break;

        case WAIT_PAYLOAD:
            payload[pay_idx++] = byte;
            if (pay_idx == rx_len) { state = WAIT_CRC; }
            break;

        case WAIT_CRC: {
            /* Verify CRC over [LEN, TYPE, PAYLOAD] */
            uint8_t crc_buf[2u + ORIN_MAX_PAYLOAD];
            crc_buf[0] = rx_len;
            crc_buf[1] = rx_type;
            memcpy(&crc_buf[2], payload, rx_len);
            uint8_t expected = crc8(crc_buf, (uint8_t)(2u + rx_len));
            if (byte != expected) {
                ESP_LOGW(TAG, "CRC fail type=0x%02x got=0x%02x exp=0x%02x",
                         rx_type, byte, expected);
                orin_send_nack(tx_q, rx_type, ERR_BAD_CRC);
            } else {
                dispatch_cmd(rx_type, payload, rx_len, tx_q);
            }
            state = WAIT_SYNC;
            break;
        }
        }
    }
}

void orin_serial_tx_task(void *arg)
{
    QueueHandle_t tx_q = (QueueHandle_t)arg;
    orin_tx_frame_t f;
    uint8_t wire[4u + ORIN_MAX_PAYLOAD];
    uint8_t wire_len;

    for (;;) {
        if (xQueueReceive(tx_q, &f, portMAX_DELAY) == pdTRUE) {
            build_frame(&f, wire, &wire_len);
            uart_write_bytes(ORIN_UART_PORT, (const char *)wire, wire_len);
        }
    }
}