#include "stm32g4xx_hal.h"
#include "uart_bridge.h"
#include "uart_bridge_cfg.h"
#include "cmsis_os.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>


UART_HandleTypeDef huart_bridge;
osMessageQId mb_rx_queue;

osThreadId ub_task_handle;
void vUartBridge(void const *params);

static uint8_t rx_byte;
static uint32_t error_cnt = 0;
static char rx_buf[DATA_BUF_SIZE];

uint32_t forw_turns;
uint32_t back_turns;
uint32_t direction;

//
void init_usart(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_USART2_CLK_ENABLE();
        
    GPIO_InitStruct.Pin       = GPIO_PIN_2 | GPIO_PIN_3;
    GPIO_InitStruct.Mode      = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull      = GPIO_PULLUP;
    GPIO_InitStruct.Speed     = GPIO_SPEED_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF7_USART2;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
    
    GPIO_InitStruct.Pin = GPIO_PIN_1;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
    
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);
    
    huart_bridge.Instance           = USART2;
    huart_bridge.Init.BaudRate      = UB_BAUDRATE;
    huart_bridge.Init.WordLength    = UART_WORDLENGTH_8B;
    huart_bridge.Init.StopBits      = UART_STOPBITS_1;
    huart_bridge.Init.Parity        = UART_PARITY_NONE;
    huart_bridge.Init.Mode          = UART_MODE_TX_RX;
    huart_bridge.Init.HwFlowCtl     = UART_HWCONTROL_NONE;
    huart_bridge.Init.OverSampling  = UART_OVERSAMPLING_8;
    huart_bridge.Init.OneBitSampling= UART_ONE_BIT_SAMPLE_DISABLE;
    HAL_UART_Init(&huart_bridge);
    
    //HAL_RS485Ex_Init(&huart_bridge, UART_DE_POLARITY_HIGH, 0, 0);

    HAL_NVIC_SetPriority(USART2_IRQn, 6, 0);
    HAL_NVIC_EnableIRQ(USART2_IRQn);
    HAL_UART_Receive_IT(&huart_bridge, &rx_byte, 1);
}

//
void ub_init_os(void)
{
    osMessageQDef(ub_queue, 20, uint8_t);
    mb_rx_queue = osMessageCreate(osMessageQ(ub_queue), NULL);
    
    osThreadDef(UB, vUartBridge, osPriorityNormal, 0, 2*128);
    ub_task_handle = osThreadCreate(osThread(UB), NULL);
}

//
void vUartBridge(void const *params)
{
    osEvent event;
    uint8_t cnt = 0;
    bool begin = false;
        
    for (;;)
    {
        event = osMessageGet(mb_rx_queue, 1000);
       
        if (event.status == osEventMessage) {
            printf("%c", event.value.v);
            if (event.value.v == '{') {
                rx_buf[cnt++] = event.value.v;
                begin = true;
                continue;
            }
            if (begin)
                rx_buf[cnt++] = event.value.v;
            
            if ((event.value.v == '\n') && (begin)) {
                
                forw_turns = get_uint_param(rx_buf, 0);
                back_turns = get_uint_param(rx_buf, 1);
                direction = get_uint_param(rx_buf, 2);

                begin = false;
                cnt = 0;
            }
            
            if (cnt == DATA_BUF_SIZE) {
                cnt = 0;
            }
        }
    }
}

//
uint32_t get_uint_param(char* buf, uint8_t index)
{
    char str_val[10] = {0};
    char *end, *begin;
        
    if (index == 0) {
        end = strchr(buf, ',');
        memcpy(str_val, &buf[1], end - buf - 1);
    }
    else {
        begin = strchr(buf, ',');
        *begin = ' ';
        end = strchr(buf, ',');
        memcpy(str_val, begin + 1, end - begin - 1);
    }
    return atoi(str_val);
}

//
void usart_bridge_rx_cb(void)
{
    //printf("%c", rx_byte);
    osMessagePut(mb_rx_queue, rx_byte, 0);
    HAL_UART_Receive_IT(&huart_bridge, &rx_byte, 1);
}

//
void usart_error_cb(void)
{
    error_cnt++;
    printf("error cb\r\n");
}

extern "C" {
  
void USART2_IRQHandler(void)
{
    HAL_UART_IRQHandler(&huart_bridge);
}

}