#include <stdbool.h>
#include <FreeRTOS.h>
#include <queue.h>
#include "stm32f10x.h"
#include "irq.h"
#include "utils.h"
#include "io_gsm.h"
#include "io_base.h"
#include "syslog.h"
#include <string.h>
#include "gpio.h"

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

#define RXNEIE_ON() set_bit(USART3->CR1, USART_CR1_RXNEIE)
#define RXNEIE_OFF() clear_bit(USART3->CR1, USART_CR1_RXNEIE)

static xQueueHandle gsm_rxqueue;
static xQueueHandle gsm_txqueue;

DECLARE_GPIO(tx, GPIOB, 10);
DECLARE_GPIO(rx, GPIOB, 11);
DECLARE_GPIO(cts, GPIOB, 13);
DECLARE_GPIO(rts, GPIOB, 14);

// Initialize USART3 for GSM module
void io_gsm_init()
{
    gsm_rxqueue = xQueueCreate(IO_GSM_RXQUEUE_SIZE, 1);
    gsm_txqueue = xQueueCreate(IO_GSM_TXQUEUE_SIZE, 1);
    
    RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
    
    // 115200bps 8N1 RTS/CTS
    USART3->BRR = (round_div(configCPU_CLOCK_HZ, 16 * 115200) << 4) | 1;
    USART3->CR1 = USART_CR1_UE | USART_CR1_TE | USART_CR1_RE | \
                  USART_CR1_RXNEIE;
    USART3->CR3 = USART_CR3_CTSE | USART_CR3_RTSE;
    
    // Configure IO pins
    gpio_tx_mode(GPIO_AFOUT_10);
    gpio_rx_mode(GPIO_HIGHZ_INPUT);
    gpio_cts_mode(GPIO_HIGHZ_INPUT);
    gpio_rts_mode(GPIO_AFOUT_10);
    
    // Interrupt on RX (TX interrupt enabled on demand)
    NVIC_EnableIRQ(USART3_IRQn);
    NVIC_SetPriority(USART3_IRQn, 14);
}

void __irq__ io_gsm_isr()
{
    portBASE_TYPE woken = pdFALSE;
    char byte;
    
    if (USART3->SR & USART_SR_RXNE)
    {
        if (uxQueueMessagesWaiting(gsm_rxqueue) < IO_GSM_RXQUEUE_SIZE)
        {
            if (USART3->SR & (USART_SR_ORE | USART_SR_NE | USART_SR_FE))
                counter_inc(COUNTER_io_gsm_errors);
                
            byte = USART3->DR;
            
            if (xQueueSendFromISR(gsm_rxqueue, &byte, &woken) != pdTRUE)
                counter_inc(COUNTER_io_gsm_errors);
        }
        else
        {
            RXNEIE_OFF(); // Queue full
        }
    }
    
    if (USART3->SR & USART_SR_TXE)
    {
        if (xQueueReceiveFromISR(gsm_txqueue, &byte, &woken) == pdTRUE)
            USART3->DR = byte;
        else
            TXEIE_OFF(); // Queue empty
    }
    
    if (woken == pdTRUE) {
        vPortYieldFromISR();
    }
}

bool io_gsm_poweron()
{
    char buffer[64];
    
    // Just in case the module is already on, return back to command mode
    vTaskDelayMS(1100);
    fputs("+++", FILE_GSM);
    vTaskDelayMS(1100);
    fputs("ATH\r\n", FILE_GSM);
    
    // Send the power on signal (active low) to the module
    // This has to be long enough, so rs232 break is not enough.
    gpio_tx_mode(GPIO_OUT_10);
    gpio_tx_set(0);
    vTaskDelayMS(50);
    gpio_tx_mode(GPIO_AFOUT_10);
    
    // Wait until the module responds
    for (int tries = 0; tries < 10; tries++)
    {
        vTaskDelayMS(1000);
        while (fread2(buffer, sizeof(buffer), FILE_GSM, 0)); // Flush buffers
        fputs("ATZ\r\n", FILE_GSM);
        int bytes = fread2(buffer, sizeof(buffer) - 1, FILE_GSM, 100);
        
        buffer[bytes] = '\0';
        if (strstr(buffer, "OK"))
            return true;
    }
    
    return false;
}

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

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