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

/*
 * servo_bus.c — Feetech/ST3215 serial bus servo driver (Issue #547)
 *
 * Half-duplex on USART3 PB10 (USART3_TX, AF7) at 1 Mbps.
 * HDSEL bit (CR3.3) routes TX and RX to the TX pin.
 * TX direction: TX active, RX gated.
 * RX direction: TX idle, RX active — enabled after TC interrupt.
 *
 * All operations are blocking with short timeouts; the servo bus
 * is polled at 50 Hz from the main loop (not from an ISR).
 */

static UART_HandleTypeDef s_uart;

/* ---- Init ---- */

void servo_bus_init(void)
{
    /* GPIO: PB10 open-drain AF7 (USART3_TX) with external pull-up on bus */
    __HAL_RCC_GPIOB_CLK_ENABLE();
    GPIO_InitTypeDef gpio = {0};
    gpio.Pin       = SERVO_BUS_PIN;
    gpio.Mode      = GPIO_MODE_AF_OD;      /* open-drain for half-duplex bus */
    gpio.Pull      = GPIO_PULLUP;
    gpio.Speed     = GPIO_SPEED_FREQ_VERY_HIGH;
    gpio.Alternate = GPIO_AF7_USART3;
    HAL_GPIO_Init(SERVO_BUS_PORT, &gpio);

    /* USART3 at SERVO_BUS_BAUD (1 Mbps), 8N1 */
    __HAL_RCC_USART3_CLK_ENABLE();
    s_uart.Instance          = USART3;
    s_uart.Init.BaudRate     = SERVO_BUS_BAUD;
    s_uart.Init.WordLength   = UART_WORDLENGTH_8B;
    s_uart.Init.StopBits     = UART_STOPBITS_1;
    s_uart.Init.Parity       = UART_PARITY_NONE;
    s_uart.Init.Mode         = UART_MODE_TX_RX;
    s_uart.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
    s_uart.Init.OverSampling = UART_OVERSAMPLING_8;  /* required for 1 Mbps @ APB1 54 MHz */
    HAL_UART_Init(&s_uart);

    /* Enable half-duplex: HDSEL bit in CR3 routes TX/RX to TX pin */
    SET_BIT(s_uart.Instance->CR3, USART_CR3_HDSEL);
}

/* ---- Checksum ---- */

static uint8_t _cksum(uint8_t id, uint8_t len, uint8_t instr,
                      const uint8_t *params, uint8_t nparams)
{
    uint16_t sum = (uint16_t)id + len + instr;
    for (uint8_t i = 0; i < nparams; i++) sum += params[i];
    return (uint8_t)(~sum & 0xFFu);
}

/* ---- Transmit a raw packet ---- */

static bool _tx(uint8_t id, uint8_t instr, const uint8_t *params, uint8_t nparams)
{
    uint8_t  len = nparams + 2u;  /* instr + params + cksum */
    uint8_t  buf[32];
    uint8_t  n = 0;

    buf[n++] = 0xFFu;
    buf[n++] = 0xFFu;
    buf[n++] = id;
    buf[n++] = len;
    buf[n++] = instr;
    for (uint8_t i = 0; i < nparams; i++) buf[n++] = params[i];
    buf[n++] = _cksum(id, len, instr, params, nparams);

    return HAL_UART_Transmit(&s_uart, buf, n, 10u) == HAL_OK;
}

/* ---- Receive response packet ---- */

/*
 * Read one byte with timeout. In half-duplex the TX line is still driving
 * after the last stop bit; we must wait ~1 bit-time before the servo begins
 * its response. HAL_UART_Receive handles the timing via the timeout.
 */
static bool _rx(uint8_t id, uint8_t *data, uint8_t ndata)
{
    uint8_t hdr[4];   /* 0xFF 0xFF ID LEN */
    uint8_t tail[2];  /* ERROR CKSUM (or just CKSUM if ndata==0) */

    /* Read header */
    if (HAL_UART_Receive(&s_uart, hdr, 4u, SB_RX_TIMEOUT_MS) != HAL_OK)
        return false;

    /* Validate header */
    if (hdr[0] != 0xFFu || hdr[1] != 0xFFu || hdr[2] != id)
        return false;

    uint8_t pkt_len = hdr[3];              /* ERROR + DATA... + CKSUM */
    if (pkt_len < 2u || pkt_len > 20u)     /* sanity */
        return false;

    /* Read payload: ERROR + DATA bytes + CKSUM */
    uint8_t  payload[20];
    uint8_t  payload_len = pkt_len;        /* includes error byte and cksum */
    if (HAL_UART_Receive(&s_uart, payload, payload_len, SB_RX_TIMEOUT_MS) != HAL_OK)
        return false;

    /* Verify checksum: ~(ID + LEN + ERROR + DATA) */
    uint16_t sum = (uint16_t)id + pkt_len;
    for (uint8_t i = 0; i < (uint8_t)(pkt_len - 1u); i++) sum += payload[i];
    uint8_t expected_cksum = (uint8_t)(~sum & 0xFFu);
    if (payload[pkt_len - 1u] != expected_cksum)
        return false;

    /* payload[0] = ERROR; payload[1..] = data */
    if (payload[0] != 0u) return false;    /* servo reported error */

    uint8_t n_data_bytes = pkt_len - 2u;   /* minus ERROR and CKSUM */
    if (n_data_bytes < ndata) return false;

    for (uint8_t i = 0; i < ndata; i++) data[i] = payload[1u + i];
    return true;
}

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

bool servo_bus_write_pos(uint8_t id, uint16_t raw_pos, uint16_t speed)
{
    if (raw_pos > SB_POS_MAX) raw_pos = SB_POS_MAX;
    if (speed   > SB_SPEED_MAX) speed = SB_SPEED_MAX;

    /* Write 4 bytes starting at SB_REG_GOAL_POS_L:
     * [addr][POS_L][POS_H][SPD_L][SPD_H] */
    uint8_t params[5] = {
        SB_REG_GOAL_POS_L,
        (uint8_t)(raw_pos & 0xFFu),
        (uint8_t)(raw_pos >> 8),
        (uint8_t)(speed   & 0xFFu),
        (uint8_t)(speed   >> 8),
    };
    return _tx(id, SB_INSTR_WRITE, params, 5u);
    /* WRITE does not generate a status packet by default */
}

bool servo_bus_write_torque(uint8_t id, bool enable)
{
    uint8_t params[2] = { SB_REG_TORQUE_EN, enable ? 1u : 0u };
    return _tx(id, SB_INSTR_WRITE, params, 2u);
}

bool servo_bus_read_pos(uint8_t id, uint16_t *raw_pos)
{
    /* READ instruction: [addr][count] */
    uint8_t params[2] = { SB_REG_PRES_POS_L, 2u };
    if (!_tx(id, SB_INSTR_READ, params, 2u)) return false;

    uint8_t data[2];
    if (!_rx(id, data, 2u)) return false;

    *raw_pos = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
    return true;
}

bool servo_bus_read_speed(uint8_t id, uint16_t *speed)
{
    uint8_t params[2] = { SB_REG_PRES_SPD_L, 2u };
    if (!_tx(id, SB_INSTR_READ, params, 2u)) return false;

    uint8_t data[2];
    if (!_rx(id, data, 2u)) return false;

    *speed = (uint16_t)data[0] | ((uint16_t)data[1] << 8);
    return true;
}

uint16_t servo_bus_deg_to_raw(float deg)
{
    /* Center = 0 deg = 2048 raw; 1 deg = 4096/360 raw */
    int32_t raw = (int32_t)(SB_POS_CENTER + (int32_t)(deg * (4096.0f / 360.0f)));
    if (raw < 0)             raw = 0;
    if (raw > (int32_t)SB_POS_MAX) raw = (int32_t)SB_POS_MAX;
    return (uint16_t)raw;
}

float servo_bus_raw_to_deg(uint16_t raw)
{
    return ((float)raw - (float)SB_POS_CENTER) * (360.0f / 4096.0f);
}