#include "Command.h"
#include "zf_driver_uart.h"
#include "zf_common_interrupt.h"
#include "ti/driverlib/dl_uart_main.h"
#include "ti/driverlib/dl_dma.h"
#include "zf_common_fifo.h"
#include "Beep.h"

volatile fifo_struct command_fifo = {0};
volatile uint8_t command_fifo_buffer[COMMAND_FIFO_BUFFER_SIZE] = {0};
uint8_t command_sum(uint8_t* data,int len);

void command_callback(uint32 state, void *ptr)
{
    while(false == DL_UART_isRXFIFOEmpty(UART2))//读取到一个数据
    {
        fifo_write_element((fifo_struct*)&command_fifo,DL_UART_Main_receiveData(UART2));//写入软件FIFO
    }
}
void command_init()
{
    uart_init(UART_2,115200,UART2_TX_B15,UART2_RX_B16);
    uart_set_interrupt_config(UART_2,UART_INTERRUPT_CONFIG_RX_ENABLE);
    interrupt_set_priority(UART2_INT_IRQn,2);//设置为次于主要任务的优先级

    //开启发送DMA
    static const DL_DMA_Config dma_tx_config =
    {
        .trigger       = DMA_UART2_TX_TRIG,
        .triggerType   = DL_DMA_TRIGGER_TYPE_EXTERNAL,
        .transferMode  = DL_DMA_SINGLE_TRANSFER_MODE,
        .extendedMode  = DL_DMA_NORMAL_MODE,
        .srcWidth      = DL_DMA_WIDTH_BYTE,
        .destWidth     = DL_DMA_WIDTH_BYTE,
        .srcIncrement  = DL_DMA_ADDR_INCREMENT,
        .destIncrement = DL_DMA_ADDR_UNCHANGED,
    };
    DL_DMA_initChannel(DMA, 0, &dma_tx_config);
    DL_UART_Main_enableDMATransmitEvent(UART2);

    //开启硬件FIFO
    DL_UART_Main_enableFIFOs(UART2);
    DL_UART_Main_setRXFIFOThreshold(UART2,DL_UART_MAIN_RX_FIFO_LEVEL_ONE_ENTRY);
    DL_UART_Main_setTXFIFOThreshold(UART2,DL_UART_MAIN_TX_FIFO_LEVEL_ONE_ENTRY);

    //设置软件FIFO
    fifo_init((fifo_struct *)&command_fifo,FIFO_DATA_8BIT,(void*)command_fifo_buffer,COMMAND_FIFO_BUFFER_SIZE);
    //设置中断回调
    uart_set_callback(UART_2,command_callback,NULL);
}
//从软件FIFO中解析数据包并执行
void command_task()
{
    static uint8_t state = 0;
    static uint8_t cmd;
    uint8_t byte;

    while(fifo_used((fifo_struct*)&command_fifo) > 0)
    {
        switch(state)
        {
            case 0: // 等待SOF 0xA7
                fifo_read_element((fifo_struct*)&command_fifo, &byte, FIFO_READ_AND_CLEAN);
                if(byte == 0xA7) state = 1;
                break;
            case 1: // 读取cmd
                fifo_read_element((fifo_struct*)&command_fifo, &cmd, FIFO_READ_AND_CLEAN);
                state = 2;
                break;
            case 2: // 校验checksum = SOF + cmd
                fifo_read_element((fifo_struct*)&command_fifo, &byte, FIFO_READ_AND_CLEAN);
                if(byte == (uint8_t)(0xA7 + cmd))
                {
                    switch(cmd)
                    {
                        case 0x01: SwitchToAutoMode();   break;
                        case 0x02: SwitchToManualMode(); break;
                        case 0x03: SwitchToDebugMode();  break;
                    }
                }
                state = 0;
                break;
        }
    }
}

//切换到自动模式
void SwitchToAutoMode()
{
    beep_on_time(100);
}

//切换到手动模式
void SwitchToManualMode()
{
    beep_on_time(200);
}

//切换到调试模式
void SwitchToDebugMode()
{
    beep_on_time(300);
}
//DMA发送使用的地址
uint8_t angle_velocity_data[6] = {0};
//发送速度前馈
void command_send_angle_velocity(float* pointer_angle_velocity)
{
    //等待上一次DMA发送完成
    while(DL_DMA_isChannelEnabled(DMA, 0));
    //等待UART物理发送完成
    while(!DL_UART_isTXFIFOEmpty(UART2));

    //填充数据
    uint8_t* p = (uint8_t*)pointer_angle_velocity;
    angle_velocity_data[0]=0xA9;
    angle_velocity_data[1]=*(p++);
    angle_velocity_data[2]=*(p++);
    angle_velocity_data[3]=*(p++);
    angle_velocity_data[4]=*(p++);
    angle_velocity_data[5]=command_sum(angle_velocity_data,5);
    //DMA发送（非阻塞）
    DL_DMA_setSrcAddr(DMA, 0, (uint32_t)angle_velocity_data);
    DL_DMA_setDestAddr(DMA, 0, (uint32_t)&UART2->TXDATA);
    DL_DMA_setTransferSize(DMA, 0, 6);
    DL_DMA_enableChannel(DMA, 0);
    // UART TX FIFO为空，DMA触发条件已满足，使能通道即自动开始传输

}
uint8_t command_sum(uint8_t* data,int len)
{
    uint8_t sum = 0;
    for(int i=0;i<len;i++)
    {
        sum+=data[i];
    }
    return sum;
}