#include "debug.h"
#include "stm32f4xx_rcc.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_usart.h"
#include "stm32f4xx_dma.h"

struct debug_data_s debug_data;
float debug_buff[DEBUG_CHAN_MAX + 1] __attribute__((aligned(16)));

void debug_init(void)
{
    GPIO_InitTypeDef  GPIO_InitStructure;
    USART_InitTypeDef USART_InitStructure;
    DMA_InitTypeDef  DMA_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;

    /* config debug uart pin */
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
    GPIO_PinAFConfig(DEBUG_UART_PORT, GPIO_PinSource6, GPIO_AF_USART6);    
    GPIO_InitStructure.GPIO_Pin = DEBUG_UART_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(DEBUG_UART_PORT, &GPIO_InitStructure);
    /* config debug uart function */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART6, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);
    USART_DeInit(USART6);
    USART_OverSampling8Cmd(USART6, DISABLE); 
    USART_InitStructure.USART_BaudRate = DEBUG_UART_BAUDRATE;
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
    USART_InitStructure.USART_Parity = USART_Parity_No;
    USART_InitStructure.USART_StopBits = USART_StopBits_1;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = USART_Mode_Tx;
    USART_Init(USART6, &USART_InitStructure);
    USART_DMACmd(USART6, USART_DMAReq_Tx, ENABLE);
    USART_Cmd(USART6, ENABLE);
    /* Configure DMA controller to manage UART TX request */ 
    DMA_InitStructure.DMA_BufferSize = DEBUG_CHAN_MAX * 4 + 4;
    DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
    DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
    DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
    DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
    DMA_InitStructure.DMA_PeripheralBaseAddr =(uint32_t)(&(USART6->DR));
    DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
    DMA_InitStructure.DMA_Priority = DMA_Priority_Low;
    DMA_InitStructure.DMA_Channel = DMA_Channel_5;
    DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
    DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)debug_buff;
    DMA_Init(DMA2_Stream6, &DMA_InitStructure);
    /* Configure interrupt for USART6 TX */
    NVIC_InitStructure.NVIC_IRQChannel = USART6_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 10;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
    USART_ITConfig(USART6, USART_IT_TC, ENABLE);

    /* debug data init */
    debug_data.enable = DEBUG_ENABLE;
    debug_data.busy = 0;
}

void debug_send_frame(void)
{
    if (debug_data.enable == 0 || debug_data.busy == 1) {
        return;
    }
    debug_data.busy = 1;
    *(uint32_t *)(debug_buff + DEBUG_CHAN_MAX) = 0x7F800000;
    DMA_SetCurrDataCounter(DMA2_Stream6, sizeof(debug_buff));
    USART_ClearITPendingBit(USART6, USART_IT_TC);
    DMA_Cmd(DMA2_Stream6, ENABLE);
}

void debug_write_data(uint16_t ch, float data)
{
    if (debug_data.enable == 0) {
        return;
    }
    if (ch >= DEBUG_CHAN_MAX) {
        while (1) {}
    }
    debug_buff[ch] = data;
}

void USART6_IRQHandler(void)
{
    if (USART_GetITStatus(USART6, USART_IT_TC) != RESET) {
        USART_ClearITPendingBit(USART6, USART_IT_TC);
        DMA_ClearFlag(DMA2_Stream6, DMA_FLAG_TCIF6);
        debug_data.busy = 0;
    }
}
