/*
 * uart_protocol.c — UART command protocol for Jetson-STM32 communication (Issue #629)
 *
 * Physical: UART5, PC12 (TX, AF8) / PD2 (RX, AF8), 115200 baud, 8N1, no flow control.
 * NOTE: Spec requested USART1 @ 115200, but USART1 is already used by JLink @ 921600.
 *       Implemented on UART5 (PC12/PD2) instead.
 *
 * RX: DMA1_Stream0 (Channel 4), 256-byte circular buffer, no interrupt needed.
 * TX: Polled (HAL_UART_Transmit), frames are short (<20 B) so blocking is acceptable.
 *
 * CRC: CRC8-SMBUS — poly 0x07, init 0x00, computed over CMD+PAYLOAD bytes only.
 */

#include "uart_protocol.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>

/* ── Configuration ─────────────────────────────────────────────────────────── */
#define RX_BUF_SIZE  256u   /* must be power-of-two for wrap math */
#define TX_TIMEOUT   5u     /* HAL_UART_Transmit timeout ms */

/* ── Peripheral handles ─────────────────────────────────────────────────────  */
static UART_HandleTypeDef  huart5;
static DMA_HandleTypeDef   hdma_rx;

/* ── DMA ring buffer ────────────────────────────────────────────────────────  */
static uint8_t rx_buf[RX_BUF_SIZE];
static uint32_t rx_head = 0u;   /* next byte to consume */

/* ── Shared state (read by main.c) ─────────────────────────────────────────  */
UartProtState uart_prot_state;

/* ── Parser state machine ───────────────────────────────────────────────────  */
typedef enum {
    PS_IDLE,
    PS_LEN,
    PS_CMD,
    PS_PAYLOAD,
    PS_CRC,
    PS_ETX
} ParseState;

static ParseState ps        = PS_IDLE;
static uint8_t    ps_len    = 0u;   /* expected payload bytes */
static uint8_t    ps_cmd    = 0u;   /* command byte */
static uint8_t    ps_payload[12];   /* max payload = SET_PID = 12 B */
static uint8_t    ps_pi     = 0u;   /* payload index */
static uint8_t    ps_crc    = 0u;   /* received CRC byte */

/* ── CRC8-SMBUS ─────────────────────────────────────────────────────────────  */
static uint8_t crc8(const uint8_t *data, uint8_t len)
{
    uint8_t crc = 0x00u;
    while (len--) {
        crc ^= *data++;
        for (uint8_t i = 0u; i < 8u; i++) {
            if (crc & 0x80u)
                crc = (uint8_t)((crc << 1) ^ 0x07u);
            else
                crc <<= 1;
        }
    }
    return crc;
}

/* ── TX helper ──────────────────────────────────────────────────────────────  */
static void tx_frame(uint8_t cmd, const uint8_t *payload, uint8_t plen)
{
    uint8_t frame[20];
    uint8_t fi = 0u;
    frame[fi++] = UPROT_STX;
    frame[fi++] = plen;
    frame[fi++] = cmd;
    for (uint8_t i = 0u; i < plen; i++)
        frame[fi++] = payload[i];
    /* CRC over CMD + PAYLOAD */
    frame[fi++] = crc8(&frame[2], (uint8_t)(1u + plen));
    frame[fi++] = UPROT_ETX;
    HAL_UART_Transmit(&huart5, frame, fi, TX_TIMEOUT);
}

static void send_ack(uint8_t cmd)
{
    tx_frame(URESP_ACK, &cmd, 1u);
}

static void send_nack(uint8_t cmd, uint8_t err)
{
    uint8_t p[2] = { cmd, err };
    tx_frame(URESP_NACK, p, 2u);
}

/* ── Command dispatcher ─────────────────────────────────────────────────────  */
static void dispatch(uint8_t cmd, const uint8_t *payload, uint8_t plen)
{
    /* Validate CRC (computed over cmd + payload) */
    uint8_t buf[13];
    buf[0] = cmd;
    memcpy(&buf[1], payload, plen);
    if (crc8(buf, (uint8_t)(1u + plen)) != ps_crc) {
        send_nack(cmd, UERR_BAD_CRC);
        return;
    }

    uart_prot_state.last_rx_ms = HAL_GetTick();

    switch (cmd) {
        case UCMD_SET_VELOCITY:
            if (plen != 4u) { send_nack(cmd, UERR_BAD_LEN); break; }
            {
                int16_t lrpm, rrpm;
                memcpy(&lrpm, &payload[0], 2u);
                memcpy(&rrpm, &payload[2], 2u);
                uart_prot_state.left_rpm    = lrpm;
                uart_prot_state.right_rpm   = rrpm;
                uart_prot_state.vel_updated = 1u;
                send_ack(cmd);
            }
            break;

        case UCMD_GET_STATUS:
            if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
            /* ACK immediately; main.c sends URESP_STATUS at next 10 Hz tick */
            send_ack(cmd);
            break;

        case UCMD_SET_PID:
            if (plen != 12u) { send_nack(cmd, UERR_BAD_LEN); break; }
            {
                float kp, ki, kd;
                memcpy(&kp, &payload[0], 4u);
                memcpy(&ki, &payload[4], 4u);
                memcpy(&kd, &payload[8], 4u);
                uart_prot_state.pid_kp      = kp;
                uart_prot_state.pid_ki      = ki;
                uart_prot_state.pid_kd      = kd;
                uart_prot_state.pid_updated = 1u;
                send_ack(cmd);
            }
            break;

        case UCMD_ESTOP:
            if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
            uart_prot_state.estop_req = 1u;
            send_ack(cmd);
            break;

        case UCMD_CLEAR_ESTOP:
            if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
            uart_prot_state.estop_clear_req = 1u;
            send_ack(cmd);
            break;

        default:
            send_nack(cmd, UERR_BAD_LEN);
            break;
    }
}

/* ── Parser byte handler ────────────────────────────────────────────────────  */
static void parse_byte(uint8_t b)
{
    switch (ps) {
        case PS_IDLE:
            if (b == UPROT_STX) ps = PS_LEN;
            break;

        case PS_LEN:
            if (b > 12u) { ps = PS_IDLE; break; }  /* sanity: max payload 12 B */
            ps_len = b;
            ps = PS_CMD;
            break;

        case PS_CMD:
            ps_cmd = b;
            ps_pi  = 0u;
            ps     = (ps_len == 0u) ? PS_CRC : PS_PAYLOAD;
            break;

        case PS_PAYLOAD:
            ps_payload[ps_pi++] = b;
            if (ps_pi >= ps_len) ps = PS_CRC;
            break;

        case PS_CRC:
            ps_crc = b;
            ps     = PS_ETX;
            break;

        case PS_ETX:
            if (b == UPROT_ETX)
                dispatch(ps_cmd, ps_payload, ps_len);
            else
                send_nack(ps_cmd, UERR_BAD_ETX);
            ps = PS_IDLE;
            break;

        default:
            ps = PS_IDLE;
            break;
    }
}

/* ── Public API ─────────────────────────────────────────────────────────────  */

void uart_protocol_init(void)
{
    memset(&uart_prot_state, 0, sizeof(uart_prot_state));
    ps = PS_IDLE;

    /* GPIO: PC12 (TX, AF8) and PD2 (RX, AF8) */
    __HAL_RCC_GPIOC_CLK_ENABLE();
    __HAL_RCC_GPIOD_CLK_ENABLE();
    GPIO_InitTypeDef gpio = {0};
    gpio.Mode      = GPIO_MODE_AF_PP;
    gpio.Pull      = GPIO_NOPULL;
    gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
    gpio.Alternate = GPIO_AF8_UART5;
    gpio.Pin       = GPIO_PIN_12;
    HAL_GPIO_Init(GPIOC, &gpio);
    gpio.Pin       = GPIO_PIN_2;
    HAL_GPIO_Init(GPIOD, &gpio);

    /* UART5 */
    __HAL_RCC_UART5_CLK_ENABLE();
    huart5.Instance          = UART5;
    huart5.Init.BaudRate     = UART_PROT_BAUD;
    huart5.Init.WordLength   = UART_WORDLENGTH_8B;
    huart5.Init.StopBits     = UART_STOPBITS_1;
    huart5.Init.Parity       = UART_PARITY_NONE;
    huart5.Init.Mode         = UART_MODE_TX_RX;
    huart5.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
    huart5.Init.OverSampling = UART_OVERSAMPLING_16;
    if (HAL_UART_Init(&huart5) != HAL_OK) return;

    /* DMA1_Stream0, Channel 4 — UART5_RX */
    __HAL_RCC_DMA1_CLK_ENABLE();
    hdma_rx.Instance                 = DMA1_Stream0;
    hdma_rx.Init.Channel             = DMA_CHANNEL_4;
    hdma_rx.Init.Direction           = DMA_PERIPH_TO_MEMORY;
    hdma_rx.Init.PeriphInc           = DMA_PINC_DISABLE;
    hdma_rx.Init.MemInc              = DMA_MINC_ENABLE;
    hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
    hdma_rx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
    hdma_rx.Init.Mode                = DMA_CIRCULAR;
    hdma_rx.Init.Priority            = DMA_PRIORITY_LOW;
    hdma_rx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
    HAL_DMA_Init(&hdma_rx);
    __HAL_LINKDMA(&huart5, hdmarx, hdma_rx);

    /* Start circular DMA receive */
    HAL_UART_Receive_DMA(&huart5, rx_buf, RX_BUF_SIZE);
}

void uart_protocol_process(void)
{
    /* DMA writes forward; NDTR counts down from RX_BUF_SIZE */
    uint32_t ndtr = __HAL_DMA_GET_COUNTER(&hdma_rx);
    uint32_t tail = (RX_BUF_SIZE - ndtr) & (RX_BUF_SIZE - 1u);
    while (rx_head != tail) {
        parse_byte(rx_buf[rx_head]);
        rx_head = (rx_head + 1u) & (RX_BUF_SIZE - 1u);
    }
}

void uart_protocol_send_status(const uart_prot_status_t *s)
{
    tx_frame(URESP_STATUS, (const uint8_t *)s, (uint8_t)sizeof(*s));
}
