#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"
#include "Task.h"
#include "Motor.h"
#include "PID_Controller.h"
#include "IMU.h"

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

static void command_process_manual(uint8_t *data);
static void command_process_debug(uint8_t *data);

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 sof = 0;
    static uint8_t payload_len = 0;
    static uint8_t buf[37]; // 最大payload: 0xA8的37字节(9*4 float + 1 checksum 先读到buf)
    static uint8_t idx = 0;
    uint8_t byte;

    while(fifo_used((fifo_struct*)&command_fifo) > 0)
    {
        switch(state)
        {
            case 0: // 等待SOF
                fifo_read_element((fifo_struct*)&command_fifo, &byte, FIFO_READ_AND_CLEAN);
                if(byte == COMMAND_SOF_MODE)
                {
                    sof = COMMAND_SOF_MODE;
                    payload_len = 2; // cmd(1) + checksum(1)
                    idx = 0;
                    state = 1;
                }
                else if(byte == COMMAND_SOF_MANUAL)
                {
                    sof = COMMAND_SOF_MANUAL;
                    payload_len = 7; // motor_speed_base(2) + angle_velocity(4) + checksum(1)
                    idx = 0;
                    state = 1;
                }
                else if(byte == COMMAND_SOF_DEBUG)
                {
                    sof = COMMAND_SOF_DEBUG;
                    payload_len = 37; // 9*4 float + checksum(1)
                    idx = 0;
                    state = 1;
                }
                break;

            case 1: // 读取payload
                fifo_read_element((fifo_struct*)&command_fifo, &buf[idx], FIFO_READ_AND_CLEAN);
                idx++;
                if(idx >= payload_len)
                {
                    // 校验checksum (payload最后一个字节)
                    uint8_t sum = sof;
                    for(uint8_t i = 0; i < payload_len - 1; i++)
                        sum += buf[i];
                    if(buf[payload_len - 1] == sum)
                    {
                        if(sof == COMMAND_SOF_MODE)
                        {
                            // 命令帧: buf[0]=cmd
                            switch(buf[0])
                            {
                                case 0x01: SwitchToAutoMode();   break;
                                case 0x02: SwitchToManualMode(); break;
                                case 0x03: SwitchToDebugMode();  break;
                            }
                        }
                        else if(sof == COMMAND_SOF_MANUAL)
                        {
                            command_process_manual(buf);
                        }
                        else if(sof == COMMAND_SOF_DEBUG)
                        {
                            command_process_debug(buf);
                        }
                    }
                    state = 0;
                }
                break;
        }
    }
}

//切换到自动模式
void SwitchToAutoMode()
{
    current_mode = COMMAND_MODE_AUTO;

    /* 清理PID状态 */
    PIDClear((PIDInstance *)&pid_speed_right);
    PIDClear((PIDInstance *)&pid_speed_left);
    PIDClear((PIDInstance *)&pid_yaw);
    PIDClear((PIDInstance *)&pid_position);

    /* 初始化输入输出值 */
    //角度参考值设为当前值
    yaw_ref = imu_angle.yaw;
    //角度速率值归零
    angle_velocity = 0;
    //速度参考值归零
    motor_speed_right_ref = 0;
    motor_speed_left_ref = 0;
    //电机PWM归零
    motor_set_pwm_right(0);
    motor_set_pwm_left(0);

    //设定基础直行速度
    motor_speed_base=AUTO_MODE_BASE_SPEED;

    //清理子任务标志
    position_turn_right_90_runing=0;

    /* 开启控制任务 */
    motor_control_task_enable=1;
    angle_control_task_enable=1;
    position_control_task_enable=1;
    beep_on_time(100);
}

//切换到手动模式
void SwitchToManualMode()
{
    current_mode = COMMAND_MODE_MANUAL;

    /* 立即关闭位置环 */
    position_control_task_enable=0;

    /* 清理PID状态 */
    PIDClear((PIDInstance *)&pid_speed_right);
    PIDClear((PIDInstance *)&pid_speed_left);
    PIDClear((PIDInstance *)&pid_yaw);

    /* 初始化输入输出值 */
    //角度参考值设为当前值
    //yaw_ref = imu_angle.yaw;//暂时关掉，方便在调试模式下更改姿态，然后切回手动模式观察响应
    //角度速率归零
    angle_velocity = 0;
    //速度参考值归零
    motor_speed_right_ref = 0;
    motor_speed_left_ref = 0;
    //电机PWM归零
    motor_set_pwm_right(0);
    motor_set_pwm_left(0);

    //基础直行速度归零
    motor_speed_base=0;

    /* 开启控制任务 */
    motor_control_task_enable=1;
    angle_control_task_enable=1;

    beep_on_time(200);
}

//切换到调试模式
void SwitchToDebugMode()
{
    current_mode = COMMAND_MODE_DEBUG;

    /* 立即关闭所有控制任务 */
    motor_control_task_enable=0;
    angle_control_task_enable=0;
    position_control_task_enable=0;

    /* 电机PWM设为0 */
    motor_set_pwm_right(0);
    motor_set_pwm_left(0);

    beep_on_time(300);
}

//解析手动控制帧 0xA6: [motor_speed_base:2 int16_le][angle_velocity:4 float_le]
static void command_process_manual(uint8_t *data)
{
    if(current_mode != COMMAND_MODE_MANUAL)
        return;

    motor_speed_base = (int16_t)(data[0] | (data[1] << 8));
    union { float f; uint8_t b[4]; } u;
    u.b[0] = data[2];
    u.b[1] = data[3];
    u.b[2] = data[4];
    u.b[3] = data[5];
    angle_velocity = u.f;
    command_send_angle_velocity((float *)&angle_velocity);
}

//解析调试调参帧 0xA8: 9个float (KpKiKd_speed + KpKiKd_yaw + KpKiKd_position)
static void command_process_debug(uint8_t *data)
{
    if(current_mode != COMMAND_MODE_DEBUG)
        return;

    union { float f; uint8_t b[4]; } u[9];
    for(int i = 0; i < 9; i++)
    {
        u[i].b[0] = data[i*4 + 0];
        u[i].b[1] = data[i*4 + 1];
        u[i].b[2] = data[i*4 + 2];
        u[i].b[3] = data[i*4 + 3];
    }

    // 电机速度环 KpKiKd (左右共用)
    pid_speed_right.Kp = u[0].f;
    pid_speed_right.Ki = u[1].f;
    pid_speed_right.Kd = u[2].f;
    pid_speed_left.Kp  = u[0].f;
    pid_speed_left.Ki  = u[1].f;
    pid_speed_left.Kd  = u[2].f;

    // 角度环 KpKiKd
    pid_yaw.Kp = u[3].f;
    pid_yaw.Ki = u[4].f;
    pid_yaw.Kd = u[5].f;

    // 位置环 KpKiKd
    pid_position.Kp = u[6].f;
    pid_position.Ki = u[7].f;
    pid_position.Kd = u[8].f;
}
//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;
}