#include "lamp_uart.h"
#include "lamp_tx_queue.h"
#include "stm32g0xx.h"

#define RX_RING_SIZE 256u
#define RX_RING_MASK (RX_RING_SIZE - 1u)

_Static_assert((RX_RING_SIZE & RX_RING_MASK) == 0u, "RX ring size must be a power of two");

static uint8_t rx_ring[RX_RING_SIZE];
static volatile uint8_t rx_head;
static volatile uint8_t rx_tail;
static volatile uint32_t rx_dropped_bytes;
static volatile uint32_t rx_hardware_errors;
static volatile uint32_t tx_dropped_packets;
static lamp_tx_queue tx_queue;

bool lamp_uart_init(uint32_t peripheral_clock_hz, uint32_t baud) {
    if (peripheral_clock_hz == 0u || baud == 0u) return false;
    const uint32_t brr = (peripheral_clock_hz + baud / 2u) / baud;
    if (brr < 16u || brr > 0xFFFFu) return false;

    RCC->IOPENR |= RCC_IOPENR_GPIOAEN;
    RCC->APBENR1 |= RCC_APBENR1_USART2EN;
    (void)RCC->APBENR1;

    /* PA2 USART2_TX and PA3 USART2_RX use AF1 on STM32G030F6P6. */
    const uint32_t pin_mask = (3u << 4u) | (3u << 6u);
    GPIOA->MODER = (GPIOA->MODER & ~pin_mask) | (2u << 4u) | (2u << 6u);
    GPIOA->PUPDR = (GPIOA->PUPDR & ~pin_mask) | (1u << 6u);
    GPIOA->AFR[0] = (GPIOA->AFR[0] & ~((0xFu << 8u) | (0xFu << 12u))) |
                    (1u << 8u) | (1u << 12u);

    USART2->CR1 = 0u;
    USART2->CR2 = 0u;
    USART2->CR3 = USART_CR3_EIE;
    USART2->PRESC = 0u;
    USART2->BRR = brr;
    USART2->ICR = USART_ICR_PECF | USART_ICR_FECF |
                  USART_ICR_NECF | USART_ICR_ORECF;
    rx_head = 0u;
    rx_tail = 0u;
    rx_dropped_bytes = 0u;
    rx_hardware_errors = 0u;
    tx_dropped_packets = 0u;
    lamp_tx_queue_init(&tx_queue);
    NVIC_ClearPendingIRQ(USART2_IRQn);
    USART2->CR1 = USART_CR1_UE | USART_CR1_RE | USART_CR1_TE |
                  USART_CR1_RXNEIE_RXFNEIE;
    NVIC_EnableIRQ(USART2_IRQn);
    return true;
}

bool lamp_uart_rx_read(uint8_t *byte) {
    if (!byte) return false;
    const uint8_t tail = rx_tail;
    if (tail == rx_head) return false;
    __DMB();
    *byte = rx_ring[tail];
    __DMB();
    rx_tail = (uint8_t)((tail + 1u) & RX_RING_MASK);
    return true;
}

bool lamp_uart_tx_enqueue(void *context, const uint8_t *packet, size_t length) {
    (void)context;
    if (!lamp_tx_queue_push(&tx_queue, packet, length)) {
        ++tx_dropped_packets;
        return false;
    }
    USART2->CR1 |= USART_CR1_TXEIE_TXFNFIE;
    return true;
}

void lamp_uart_get_stats(lamp_uart_stats *stats) {
    if (!stats) return;
    stats->dropped_bytes = rx_dropped_bytes;
    stats->dropped_packets = tx_dropped_packets;
    stats->hardware_errors = rx_hardware_errors;
    stats->tx_queued_bytes = (uint32_t)lamp_tx_queue_count(&tx_queue);
}

void USART2_IRQHandler(void) {
    const uint32_t status = USART2->ISR;
    const uint32_t error_mask = USART_ISR_PE | USART_ISR_FE |
                                USART_ISR_NE | USART_ISR_ORE;
    const bool receive_ready = (status & USART_ISR_RXNE_RXFNE) != 0u;
    const bool has_error = (status & error_mask) != 0u;

    uint8_t byte = 0u;
    if (receive_ready) byte = (uint8_t)USART2->RDR;
    if (has_error) {
        USART2->ICR = USART_ICR_PECF | USART_ICR_FECF |
                      USART_ICR_NECF | USART_ICR_ORECF;
        ++rx_hardware_errors;
    }
    if (receive_ready && !has_error) {
        const uint8_t head = rx_head;
        const uint8_t next = (uint8_t)((head + 1u) & RX_RING_MASK);
        if (next == rx_tail) {
            ++rx_dropped_bytes;
        } else {
            rx_ring[head] = byte;
            __DMB();
            rx_head = next;
        }
    }

    if ((status & USART_ISR_TXE_TXFNF) != 0u) {
        uint8_t tx_byte;
        if (lamp_tx_queue_pop(&tx_queue, &tx_byte)) {
            USART2->TDR = tx_byte;
        } else {
            USART2->CR1 &= ~USART_CR1_TXEIE_TXFNFIE;
        }
    }
}
