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

#define DBG if(0)

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 direction;     // направление движения с энкодера (0 - вперед, 1 - назад)
uint32_t forw_turns;    // счетчик энкодера (вперед) 
uint32_t back_turns;    // счетчик энкодера (назад) 

uint32_t forw_turns_old;
uint32_t back_turns_old;

// -------------------------------------------------------------------------- //
// Вычисляемые параметры.
int total_turns;        // счетчик энкодера с учетом реверса
move_t move = m_stop;   // статус робота (движение вперед, назад, остановка)
float distance;         // пройденное расстояние в метрах (выводится на экран)


//
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) {
            DBG 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);

                calculation(forw_turns, back_turns, direction);
                
                begin = false;
                cnt = 0;
                
                forw_turns_old = forw_turns;
                back_turns_old = back_turns;
            }
            
            if (cnt == DATA_BUF_SIZE) {
                cnt = 0;
            }
        }
    }
}

// На вход поступают сырые данные из протокола
void calculation(uint32_t forward, uint32_t back, uint32_t dir)
{
    forw_turns = forward;
    back_turns = back;
    direction = dir;
    
    // Расчет пройденного пути
    if (settings.revers == false) {
        total_turns = forw_turns - back_turns;
    }
    else {
        total_turns = back_turns - forw_turns;
    }
    
    // Расчет пройденного пути в метрах (преобразуем total_turns)
    distance = total_turns/settings.factor_2;
    
    // Направление движения и остановка
    calc_move();
}

//
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++;
    DBG printf("error cb\r\n");
}

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

}

// -------------------------------------------------------------------------- //
// Расчет параметров

// Направление движения и остановка (в зависимости от реверса).
void calc_move(void)
{
    if (direction == 0) {
        if (forw_turns > forw_turns_old)
            if (settings.revers == 0)
                move = m_forward;
            else
                move = m_back;
        else if (forw_turns == forw_turns_old)
            move = m_stop;
    }
    else
    {
        if (back_turns > back_turns_old)
            if (settings.revers == 0)
                move = m_back;
            else
                move = m_forward;
        else if (back_turns == back_turns_old)
            move = m_stop;
    }
}

// -------------------------------------------------------------------------- //
// API

// Сырые счетчики и направления из канала связи без обработки.
void get_raw_params(uint32_t *f_turns, uint32_t *b_turns, uint32_t *dir)
{
    *f_turns = forw_turns;
    *b_turns = back_turns;
    *dir = direction;
}

//
void get_total(int *val)
{
    *val = total_turns;
}

//
float get_distance(void)
{
    return distance;
}

//
move_t get_move(void)
{
    return move;
}

//
void set_raw_params(uint32_t f_turns, uint32_t b_turns, uint32_t dir)
{
    forw_turns = f_turns;
    back_turns = b_turns;
    direction = dir;
}

// 
void set_raw_counters(uint32_t f_turns, uint32_t b_turns, int tot)
{
    forw_turns = f_turns;
    back_turns = b_turns;
    total_turns = tot;
}

// Эмулятор. Функция для тестов.
void inc_raw_counters(void)
{
    static uint32_t f_cnt = 0;
    static uint32_t b_cnt = 0;
    static uint32_t dir = 0;
      
    calculation(f_cnt, b_cnt, dir);

    f_cnt++;
            
}