#include <stdbool.h>
#include <FreeRTOS.h>
#include <queue.h>
#include "stm32f10x.h"
#include "irq.h"
#include "utils.h"
#include "io_cli.h"
#include "syslog.h"

// Enable & disable transmission interrupt
#define TXEIE_ON() set_bit(USART1->CR1, USART_CR1_TXEIE)
#define TXEIE_OFF() clear_bit(USART1->CR1, USART_CR1_TXEIE)

static xQueueHandle cli_rxqueue;
static xQueueHandle cli_txqueue;

void io_cli_init()
{
    // USART1: Command line debug interface
    // 115200bps 8N1
    cli_rxqueue = xQueueCreate(IO_CLI_RXQUEUE_SIZE, 1);
    cli_txqueue = xQueueCreate(IO_CLI_TXQUEUE_SIZE, 1);
    RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
    USART1->BRR = (round_div(configCPU_CLOCK_HZ, 16 * 115200) << 4) | 1;
    USART1->CR1 = USART_CR1_UE | USART_CR1_TE | USART_CR1_RE | \
                  USART_CR1_RXNEIE;
    NVIC_EnableIRQ(USART1_IRQn);
    NVIC_SetPriority(USART1_IRQn, 14);
}

void __irq__ io_cli_isr()
{
    portBASE_TYPE woken = pdFALSE;
    char byte;
    
    if (USART1->SR & USART_SR_RXNE)
    {
        if (USART1->SR & (USART_SR_ORE | USART_SR_NE | USART_SR_FE))
            counter_inc(COUNTER_io_cli_errors);
        
        byte = USART1->DR;
        
        if (xQueueSendFromISR(cli_rxqueue, &byte, &woken) != pdTRUE)
            counter_inc(COUNTER_io_cli_errors);
    }
    
    if (USART1->SR & USART_SR_TXE)
    {
        if (xQueueReceiveFromISR(cli_txqueue, &byte, &woken) == pdTRUE)
            USART1->DR = byte;
        else
            TXEIE_OFF(); // Queue empty
    }
    
    if (woken == pdTRUE) {
        vPortYieldFromISR();
    }
}

unsigned io_cli_read(char *buf, unsigned count, portTickType timeout)
{
    unsigned rxcount = 0;
    while (rxcount < count)
    {
        if (xQueueReceive(cli_rxqueue, buf++, timeout) != pdTRUE)
            break;
        rxcount++;
    }
    return rxcount;
}

unsigned io_cli_write(const char *buf, unsigned count, portTickType timeout)
{
    unsigned txcount = 0;
    while (txcount < count)
    {
        if (xQueueSend(cli_txqueue, buf++, timeout) != pdTRUE)
            break;
        
        TXEIE_ON();
        txcount++;
    }
    return txcount;
}