/**
  **************************************************************************
  * @file     main.c
  * @brief    main program
  **************************************************************************
  *                       Copyright notice & Disclaimer
  *
  * The software Board Support Package (BSP) that is made available to
  * download from Artery official website is the copyrighted work of Artery.
  * Artery authorizes customers to use, copy, and distribute the BSP
  * software and its related documentation for the purpose of design and
  * development in conjunction with Artery microcontrollers. Use of the
  * software is governed by this copyright notice and the following disclaimer.
  *
  * THIS SOFTWARE IS PROVIDED ON "AS IS" BASIS WITHOUT WARRANTIES,
  * GUARANTEES OR REPRESENTATIONS OF ANY KIND. ARTERY EXPRESSLY DISCLAIMS,
  * TO THE FULLEST EXTENT PERMITTED BY LAW, ALL EXPRESS, IMPLIED OR
  * STATUTORY OR OTHER WARRANTIES, GUARANTEES OR REPRESENTATIONS,
  * INCLUDING BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
  * FITNESS FOR A PARTICULAR PURPOSE, OR NON-INFRINGEMENT.
  *
  **************************************************************************
  */

#include "at32f421_board.h"
#include "at32f421_clock.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmcng.h"
#include "TaskLoop.h"
#include <string.h>
#include "storage.h"
#include <stdlib.h>
#include "at32f421_wdt.h"
#include "iap.h"

/** @addtogroup UTILITIES_examples
  * @{
  */
volatile uint8_t usart1_rx_dma_status = 0;
Pkg_Uart g_pkg_uart_1 = { 0, 0, 0, "", 0};
Pkg_Uart g_pkg_uart_report = { 0, 0, 0, "", 0};
static uint8_t tmp_ble_buf[BUFF_STACK_SIZE];
char usart1_rx_buffer[USART1_RX_BUFFER_SIZE];
TaskHandle_t usart_task_handler = NULL;


/** @addtogroup FreeRTOS_demo
  * @{
  */

crm_clocks_freq_type g_crm_clocks_freq_struct = {0};
tmr_input_config_type  g_tmr_input_config_struct;
__IO uint32_t sys_counter = 0;

__IO uint64_t TimingDelayInc;
__IO uint32_t TimingDelayDec;

TaskHandle_t loop_task_handler;

// 看门狗相关
#define WDT_TIMEOUT_MS  3000  // 看门狗超时时间 3秒
void wdt_init(void);
void wdt_feed(void);

tmr_output_config_type tmr_oc_init_structure;

/*
 pb1 output pwm waveform, use Tmr14.
  tmr3 channel1 duty cycle = (tmr3_c1dt/ tmr3_pr)* 100 = 50%
  tmr3 channel2 duty cycle = (tmr3_c2dt/ tmr3_pr)* 100 = 37.5%
  tmr3 channel3 duty cycle = (tmr3_c3dt/ tmr3_pr)* 100 = 25%
  tmr3 channel4 duty cycle = (tmr3_c4dt/ tmr3_pr)* 100 = 12.5%
*/
uint16_t c1dt_val = 333;
//uint16_t c2dt_val = 249;
//uint16_t c3dt_val = 166;
//uint16_t c4dt_val = 83;
uint16_t prescaler_value = 0;
__IO uint16_t pulse=0;//55;   //�������
uint8_t g_pulse_counter = 0;
uint8_t g_flag_pulse = 1;


uint64_t mstick(void){
    return TimingDelayInc;
}

void InitPkgUart(Pkg_Uart * pkg){
    memset(pkg->pkg, 0, BUFF_STACK_SIZE);
    pkg->flag = 0;
    pkg->tick = 0;
    pkg->len = 0;
    pkg->offset = 0;
}






/**
  * @brief  初始化独立看门狗
  * @param  none
  * @retval none
  * @note   超时时间 = (预分频系数 × 重载值) / LSI频率
  *         LSI频率约 40kHz，预分频64，重载值=4687，超时≈ 3秒
  */
void wdt_init(void)
{
    // 使能看门狗寄存器写访问
    wdt_register_write_enable(TRUE);
    
    // 设置预分频系数为 64
    wdt_divider_set(WDT_CLK_DIV_64);
    
    // 设置重载值
    // 超时时间 = (64 × 4687) / 40000 ≈ 7.5秒
    // 如果需要 3秒: (64 × 1875) / 40000 = 3秒
    wdt_reload_value_set(1875);
    
    // 重载计数器
    wdt_counter_reload();
    
    // 使能看门狗
    wdt_enable();
    
    PRINT("Watchdog initialized, timeout: %d ms\n", WDT_TIMEOUT_MS);
}

/**
  * @brief  喂狗（重载看门狗计数器）
  * @param  none
  * @retval none
  */
void wdt_feed(void)
{
    wdt_counter_reload();
}

uint16_t timer_period = 0;
uint16_t channel1_pulse = 0;

void Timr_msic_Init_Pwm(void)
{
    gpio_init_type  gpio_init_struct = {0};
    tmr_output_config_type tmr_output_struct;
    /* enable tmr1/gpioa/gpiob clock */
    crm_periph_clock_enable(CRM_TMR1_PERIPH_CLOCK, TRUE);
    crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE);

    /* timer1 output pin Configuration */
    gpio_init_struct.gpio_pins = GPIO_PINS_8 ;
    gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
    gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
    gpio_init_struct.gpio_pull = GPIO_PULL_NONE;
    gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
    gpio_init(GPIOA, &gpio_init_struct);


    gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE8, GPIO_MUX_2);

    /* tmr1 configuration generate 7 pwm signals with 4 different duty cycles:
    prescaler = 0, tmr1 counter clock = system_core_clock

    the objective is to generate 7 pwm signal at 17.57 khz:
        - tim1_period = (system_core_clock / 17570) - 1
    the channel 1 and channel 1c duty cycle is set to 50%
    the channel 2 and channel 2c duty cycle is set to 37.5%
    the channel 3 and channel 3c duty cycle is set to 25%
    the channel 4 duty cycle is set to 12.5%
    the timer pulse is calculated as follows:
        - channelxpulse = duty_cycle * (tim1_period - 1) / 100 */

    /* compute the value to be set in arr regiter to generate signal frequency at 17.57 khz */
    timer_period = (g_crm_clocks_freq_struct.sclk_freq / 17570 ) - 1;

    /* compute c1dt value to generate a duty cycle at 50% for channel 1 and 1c */
//    channel1_pulse = (uint16_t)(((uint32_t) 5 * (timer_period - 1)) / 10);
    /* compute c2dt value to generate a duty cycle at 37.5%  for channel 2 and 2c */
    channel1_pulse = (uint16_t)(((uint32_t) 375 * (timer_period - 1)) / 1000);

    /* compute c3dt value to generate a duty cycle at 25%  for channel 3 and 3c */
//    channel1_pulse = (uint16_t)(((uint32_t) 25 * (timer_period - 1)) / 100);

    /* compute c4dt value to generate a duty cycle at 12.5%  for channel 4 */
//    channel1_pulse = (uint16_t)(((uint32_t) 125 * (timer_period- 1)) / 1000);

    channel1_pulse = (uint16_t)(((uint32_t) 999 * (timer_period - 1)) / 1000);

    tmr_base_init(TMR1, timer_period, 0);
    tmr_cnt_dir_set(TMR1, TMR_COUNT_UP);

    /* channel 1, 2, 3 and 4 configuration in output mode */
    tmr_output_default_para_init(&tmr_output_struct);
    tmr_output_struct.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_B;
    tmr_output_struct.oc_output_state = TRUE;
    tmr_output_struct.oc_polarity = TMR_OUTPUT_ACTIVE_LOW;
    tmr_output_struct.oc_idle_state = TRUE;
    tmr_output_struct.occ_output_state = TRUE;
    tmr_output_struct.occ_polarity = TMR_OUTPUT_ACTIVE_HIGH;
    tmr_output_struct.occ_idle_state = FALSE;

    /* channel 1 */
    tmr_output_channel_config(TMR1, TMR_SELECT_CHANNEL_1, &tmr_output_struct);
    tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, channel1_pulse);


    /* output enable */
    tmr_output_enable(TMR1, TRUE);

    /* enable tmr1 */
    tmr_counter_enable(TMR1, TRUE);

}


/* Basic timr msic, other timer init  */
void Timr_msic_Init(void)
{
    crm_periph_clock_enable(CRM_TMR6_PERIPH_CLOCK, TRUE);

    //TODO: ��ʱ�ļ��㷽��
//    tmr_base_init(TMR6, 9999, (g_crm_clocks_freq_struct.ahb_freq / 10000) - 1); // 1s
    tmr_base_init(TMR6, 999, (g_crm_clocks_freq_struct.ahb_freq / 1000000) - 1);  // 1ms


    tmr_cnt_dir_set(TMR6, TMR_COUNT_UP);

    tmr_interrupt_enable(TMR6, TMR_OVF_INT, TRUE);

    nvic_irq_enable(TMR6_GLOBAL_IRQn, 2, 0);    // 较低：系统节拍，上报计时、一般计数器

    tmr_counter_enable(TMR6, TRUE);


    crm_periph_clock_enable(CRM_TMR15_PERIPH_CLOCK, TRUE);

    //TODO: ��ʱ�ļ��㷽��
//    tmr_base_init(TMR6, 9999, (g_crm_clocks_freq_struct.ahb_freq / 10000) - 1); // 1s
    tmr_base_init(TMR15, (5000-1), (g_crm_clocks_freq_struct.ahb_freq / 1000000) - 1);  // 5ms
//    tmr_base_init(TMR14, 9999, (g_crm_clocks_freq_struct.ahb_freq / 1000000) - 1);  // 10ms


    tmr_cnt_dir_set(TMR15, TMR_COUNT_UP);

    tmr_interrupt_enable(TMR15, TMR_OVF_INT, TRUE);

    nvic_irq_enable(TMR15_GLOBAL_IRQn, 2, 0);   // 较低：定时任务，线圈任务定时器，5ms

    tmr_counter_enable(TMR15, TRUE);

    Timr_msic_Init_Pwm();

}

uint8_t g_cnt_pwm_red_low_timeout = 0;
void poll_red_pwm(void)
{
    g_pulse_counter++;
    if(g_pulse_counter >= 12){
        g_pulse_counter = 0;
        if(g_flag_pulse){
            if(pulse < 6000){
                pulse += 400;
            }
            else{
                pulse += 100;
            }

            if(pulse >= 6800)
            {
                if(g_cnt_pwm_red_low_timeout < 3){
                    pulse = 6800;
                    g_cnt_pwm_red_low_timeout++;
                }
                else{
                    g_flag_pulse = 0;
                    g_cnt_pwm_red_low_timeout = 0;
                    pulse = 6800;
                }

            }
        }
        else{
            if(pulse <= 6800)
            {
                pulse -= 400;
            }
            // else{
            //     pulse -= 20;
            // }

            if(pulse < 500){
                g_flag_pulse = 1;
            }
        }
        tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pulse);
    }
}


void usart1_sendstring(uint8_t *buf, uint16_t len)
{
    uint16_t _len = len;
    while(_len){
        while(usart_flag_get(USART1, USART_TDBE_FLAG) == RESET);
        usart_data_transmit(USART1, *buf++);
        _len--;
    }
}


/**
  * @brief  main function.
  * @param  none
  * @retval none
  */
int main(void)
{
    nvic_vector_table_set(NVIC_VECTTAB_FLASH, APP_START_ADDR - FLASH_BASE);

  nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);

  system_clock_config();

    /* get system clock */
  crm_clocks_freq_get(&g_crm_clocks_freq_struct);

    at32_board_init();

  /* init usart1 */
    //2250000
    // 115200
#ifdef DEBUG
//  uart_print_init(230400);  //115200
//    uart_print_init(115200);  //115200
    usart_configuration();

    PRINT("sys_clock:%d, ahb_freq:%d, sclk_freq:%d\n", system_core_clock, g_crm_clocks_freq_struct.ahb_freq, g_crm_clocks_freq_struct.sclk_freq);
#else
    usart_configuration();
    #endif

    para_store_init();
        
    loop_timer_io_init();
    Timr_msic_Init();
        
    // 初始化看门狗
    wdt_init();



  /* enter critical */
  taskENTER_CRITICAL();



  /* create loop task */
  if(xTaskCreate((TaskFunction_t )loop_task_function,
                 (const char*    )"Loop_task",
                 (uint16_t       )512,
                 (void*          )NULL,
                 (UBaseType_t    )2,
                 (TaskHandle_t*  )&loop_task_handler) != pdPASS)
  {
    PRINT("loop task could not be created as there was insufficient heap memory remaining.\r\n");
  }
  else
  {
    PRINT("loop task was created successfully.\r\n");
  }

  /* create usart task */
  if(xTaskCreate((TaskFunction_t )usart_task_function,
                 (const char*    )"Usart_task",
                 (uint16_t       )256,
                 (void*          )NULL,
                 (UBaseType_t    )3,
                 (TaskHandle_t*  )&usart_task_handler) != pdPASS)
  {
    PRINT("usart task could not be created as there was insufficient heap memory remaining.\r\n");
  }
  else
  {
    PRINT("usart task was created successfully.\r\n");
  }

  /* create led indicator task */
  if(xTaskCreate((TaskFunction_t )led_indicator_task_function,
                 (const char*    )"LED_Task",
                 (uint16_t       )256,
                 (void*          )NULL,
                 (UBaseType_t    )1,
                 (TaskHandle_t*  )NULL) != pdPASS)
  {
    PRINT("led task could not be created as there was insufficient heap memory remaining.\r\n");
  }
  else
  {
    PRINT("led task was created successfully.\r\n");
  }

  /* exit critical */
  taskEXIT_CRITICAL();

  /* start scheduler */
  vTaskStartScheduler();
}


void uart_report_packet_loop_acs(uint8_t flag)
{
    uint8_t i,j, _flag_event = 0, _len = 0, _ckb = 0, _sum = 0;
    uint8_t _misc = 0;

    for(i = 0; i < LOOP_CAPTURE_MAX; i++)
    {
        Loop154_Unit *unit = &g_loop_states.loop_unit[i];
        if(unit->loop_FLAG_IN || unit->loop_FLAG_OUT) _flag_event = 1;
    }

    if(_flag_event == 0)
    {
        if(g_loop_states.report_counter < 200) return;
    }
    else
    {
        if(g_loop_states.report_counter < 30) return;
    }
    InitPkgUart(&g_pkg_uart_report);

    i = 0;
    g_pkg_uart_report.pkg[i++] = 0x7F;
    g_pkg_uart_report.pkg[i++] = 0;
    g_pkg_uart_report.pkg[i++] = _len;
    g_pkg_uart_report.pkg[i++] = CMD_SUB_SENS_REPORT;
    g_pkg_uart_report.pkg[i++] = SENS_MULTI_LOOP_DYNAMIC;
    g_pkg_uart_report.pkg[i++] = 0x00;

    for(j = 0; j < LOOP_CAPTURE_MAX; j++)
    {
        Loop154_Unit *unit = &g_loop_states.loop_unit[j];

        g_pkg_uart_report.pkg[i++] = (g_loop_cng_info.loop_cng[j].loopFreq_Level << 6) | (1 << 4) | (unit->loop_SensLevel & 0x03);
        g_pkg_uart_report.pkg[i++] = 0 | ((!unit->loop_LOOP_OK) << 3) | (unit->loop_VD_FLAG << 2) | (_misc);
        // 上报频率 = sclk_freq * input_div * LPCNT / CAPVD  (先乘后除避免精度损失)
        uint32_t _frequent = 0;
        if (unit->loop_LPCNT > 0 && unit->loop_CAPVD > 0) {
            _frequent = (uint32_t)((uint64_t)g_crm_clocks_freq_struct.sclk_freq * g_input_div * unit->loop_LPCNT / unit->loop_CAPVD);
        }
        g_pkg_uart_report.pkg[i++] = (uint8_t)(_frequent & 0xFF);
        g_pkg_uart_report.pkg[i++] = (uint8_t)((_frequent >> 8) & 0xFF);
        g_pkg_uart_report.pkg[i++] = (uint8_t)((_frequent >> 16) & 0xFF);

        uint32_t variation = (unit->loop_Origin > unit->loop_CAPVD) ?
            (unit->loop_Origin - unit->loop_CAPVD) : (unit->loop_CAPVD - unit->loop_Origin);
        g_pkg_uart_report.pkg[i++] = (uint8_t)(variation & 0xFF);
        g_pkg_uart_report.pkg[i++] = (uint8_t)((variation >> 8) & 0xFF);

        // time val: pass_time (placeholder, not tracked in DLD154V4B algo)
        uint32_t _tmp = 0;
        g_pkg_uart_report.pkg[i++] = (uint8_t)(_tmp & 0xFF);
        g_pkg_uart_report.pkg[i++] = (uint8_t)((_tmp >> 8) & 0xFF);
        g_pkg_uart_report.pkg[i++] = (uint8_t)((_tmp >> 16) & 0xFF);
        g_pkg_uart_report.pkg[i++] = (uint8_t)((_tmp >> 24) & 0xFF);
    }
    g_pkg_uart_report.pkg[2] = i - 3;

    for(j = 1; j < i; j++){
        _ckb ^= g_pkg_uart_report.pkg[j];
        _sum += g_pkg_uart_report.pkg[j];
    }
    g_pkg_uart_report.pkg[i++] = _ckb;
    g_pkg_uart_report.pkg[i++] = _sum & 0xFF;

    usart1_sendstring(g_pkg_uart_report.pkg, i);
    g_loop_states.report_counter = 0;
}


// 7F
void response_uart_pkg(uint8_t cmd, uint8_t *dat, uint8_t dat_len)
{
    uint8_t i,j, _ckb = 0, _sum = 0;
    uint8_t *sBuf = (uint8_t *)malloc(BUFF_STACK_SIZE);

    if(sBuf == NULL){
        PRINT("Not enough memory!!");
    }
    memset(sBuf, 0, BUFF_STACK_SIZE);
    i = 0;
    sBuf[i++] = 0x7F;
    sBuf[i++] = 0x80;
    sBuf[i++] = 1 + dat_len;
    sBuf[i++] = cmd;
    memcpy(&sBuf[i], dat, dat_len);

    i += dat_len;
    for(j = 1; j < i; j++)
    {
        _ckb ^= sBuf[j];
        _sum += sBuf[j];
    }
    sBuf[i++] = _ckb;
    sBuf[i++] = _sum & 0xFF;

    usart1_sendstring(sBuf, i);

    free(sBuf);
}


// multip loop cjq param
static void unpack_pkg_set_mcjq_param(uint8_t *pkg, uint8_t len)
{
    uint8_t i = 4, j, k = 0;
    uint8_t _amount = 0;

    g_loop_cng_info.smart_mode = pkg[i++];
    _amount = pkg[i++];
    Loop_Cng_Unit *unit = g_loop_cng_info.loop_cng;
    for(j = 0; j < _amount; j++)
    {
        k = pkg[i++];
        unit->sensitvity = k & 0x0F;
        unit->loopFreq_Level = k >> 4;
        unit->delay_time = pkg[i++];
        k = pkg[i++];
        unit->output_mode = k & 0x03;
        unit->loopSafe_Timeout = k >> 3;
        unit->exist_mode = pkg[i++];
        unit->direction_mode = pkg[i++];

        unit++;
    }

    storage_dev();
}

/**
  * @brief  USART1 串口数据包校验函数
  * @param  pkg: 接收到的数据包指针
  * @param  len: 数据包实际长度
  * @retval 0: 校验成功；其他：校验失败
  */
uint8_t usart_packet_validate(uint8_t *pkg, uint8_t len)
{
    uint8_t _len = pkg[2];
    uint8_t _cmd = pkg[3];
    uint8_t _addr = pkg[1];
    uint8_t j;

    // ===== 数据包校验 =====
    // 1. 检查起始字节 STX (0x7F)
//    if(pkg[0] != 0x7F && pkg[0] != 0x8F)
//    {
//        PRINT("Error: Invalid STX 0x%02X\n", pkg[0]);
//        return 1;  // 校验失败
//    }

    // 2. 检查长度字段
    // LEN = CMD(1) + Data(LEN-1), 总长度应该是 LEN+3(STX+ADDR+LEN+XOR+SUM)
    if(_len < 1)  // 至少要有 CMD
    {
        PRINT("Error: Invalid length %d\n", _len);
        return 2;  // 校验失败
    }

    // 3. 检查接收到的数据长度是否匹配
    if(len < (_len + 3))  // STX(1) + ADDR(1) + LEN(1) + LEN bytes + XOR(1) + SUM(1) = LEN+4，但 pkg 从 0 开始，所以是 len >= _len + 3
    {
        PRINT("Error: Received length %d doesn't match declared length %d\n", len, _len);
        return 3;  // 校验失败
    }

    // 4. 提取并验证 XOR 校验
    uint8_t received_xor = pkg[3 + _len];  // XOR 位置：STX+ADDR+LEN+CMD+Data
    uint8_t calculated_xor = _addr;  // ADDR
    calculated_xor ^= _len;  // LEN
    calculated_xor ^= _cmd;  // CMD
    for(j = 0; j < (_len - 1); j++)  // Data (LEN-1 bytes)
    {
        calculated_xor ^= pkg[4 + j];
    }

    if(received_xor != calculated_xor)
    {
        PRINT("Error: XOR check failed! Received: 0x%02X, Calculated: 0x%02X\n", received_xor, calculated_xor);
        return 4;  // 校验失败
    }

    // 5. 提取并验证 SUM 校验
    uint8_t received_sum = pkg[3 + _len + 1];  // SUM 位置：XOR 后一个字节
    uint8_t calculated_sum = _addr;  // ADDR
    calculated_sum += _len;  // LEN
    calculated_sum += _cmd;  // CMD
    for(j = 0; j < (_len - 1); j++)  // Data (LEN-1 bytes)
    {
        calculated_sum += pkg[4 + j];
    }

    if(received_sum != calculated_sum)
    {
        PRINT("Error: SUM check failed! Received: 0x%02X, Calculated: 0x%02X\n", received_sum, calculated_sum);
        return 5;  // 校验失败
    }

    // ===== 所有校验通过 =====
    return 0;  // 校验成功
}

uint8_t usart_packet_9F_validate(uint8_t *pkg, uint8_t len)
{
    uint8_t _sub1 = pkg[1];
    uint8_t _sub2 = pkg[2];
    uint8_t _len = pkg[3];
    uint8_t _cmd = pkg[4];
    uint8_t j;

    // ===== 数据包校验 =====
    // 1. 检查起始字节 STX (0x7F)
    if(pkg[0] != 0x9F)
    {
        PRINT("Error: Invalid STX 0x%02X\n", pkg[0]);
        return 1;  // 校验失败
    }

//    // 2. 检查长度字段
//    // LEN = CMD(1) + Data(LEN-1), 总长度应该是 LEN+3(STX+ADDR+LEN+XOR+SUM)
//    if(_len < 1)  // 至少要有 CMD
//    {
//        PRINT("Error: Invalid length %d\n", _len);
//        return 2;  // 校验失败
//    }
//
//    // 3. 检查接收到的数据长度是否匹配
//    if(len < (_len + 2))  // STX(1) + ADDR(1) + LEN(1) + LEN bytes + SUM(1) = LEN+3，但 pkg 从 0 开始，所以是 len >= _len + 2
//    {
//        PRINT("Error: Received length %d doesn't match declared length %d\n", len, _len);
//        return 3;  // 校验失败
//    }
//
//    // 5. 提取并验证 SUM 校验
//    uint8_t received_sum = pkg[4 + _len];  // SUM 位置：XOR 后一个字节
//    uint8_t calculated_sum = _sub1 + _sub2;  // ADDR
//    calculated_sum += _len;  // LEN
//    calculated_sum += _cmd;  // CMD
//    for(j = 0; j < (_len - 1); j++)  // Data (LEN-1 bytes)
//    {
//        calculated_sum += pkg[4 + j];
//    }
//
//    if(received_sum != calculated_sum)
//    {
//        PRINT("Error: SUM check failed! Received: 0x%02X, Calculated: 0x%02X\n", received_sum, calculated_sum);
//        return 5;  // 校验失败
//    }

    // ===== 所有校验通过 =====
    return 0;  // 校验成功
}


void iap_command_handle(void)
{
    // IAP_REV_FLAG_DONE
    flash_unlock();
    flash_sector_erase(IAP_UPGRADE_FLAG_ADDR);
    flash_word_program(IAP_UPGRADE_FLAG_ADDR, IAP_UPGRADE_FLAG_9F);
    flash_lock();
    nvic_system_reset();
}


void manage_dbn_ble_default(uint8_t *pkg, uint8_t len)
{
    uint8_t _header_0 = pkg[1];
    uint8_t _header_0_high = 0;
    uint8_t _header_0_low = _header_0 % 0x10;
    uint8_t _len = pkg[2];
    uint8_t _cmd = pkg[3];
    uint8_t _tmp = 0;
    uint8_t i = 0, j,k = 0;
    uint8_t _ota_pre_flag = 0;

    if(pkg[0] == 0x7F || pkg[0] == 0x8F)
    {
        // ===== 调用校验函数进行数据包校验 =====
        if(usart_packet_validate(pkg, len) != 0)
        {
            // 校验失败，直接返回
            return;
        }
    }
    else if(pkg[0] == 0x9F)
    {
        // ===== 调用校验函数进行数据包校验 =====
        if(usart_packet_9F_validate(pkg, len) != 0)
        {
            // 校验失败，直接返回
            return;
        }
        _ota_pre_flag = 1;
    }
    else{
        PRINT("Error: Invalid STX 0x%02X\n", pkg[0]);
        return;
    }

    if(_ota_pre_flag){
        PRINT("OTA Pre Flag: %d\n", _ota_pre_flag);
        if(pkg[1] == 0x01 && pkg[2] == 0x00 && pkg[3] == 0x01 && pkg[4] == 0xA5 && pkg[5] == 0xA7){
            PRINT("OTA Pre Flag: %d\n", _ota_pre_flag);
            iap_command_handle();
        }

        return;
    }

    switch(_cmd)
    {
        case CMD_DBN_DEV_RESET:
        {
            //Restart Dev
            nvic_system_reset();
        } break;
        case CMD_DEV_Ver:
        {
            i = 0;
            tmp_ble_buf[i++] = 0x00;
            tmp_ble_buf[i++] = HARDWARE_VER_MAIN;
            tmp_ble_buf[i++] = HARDWARE_VER_SUB;
            tmp_ble_buf[i++] = HARDWARE_VER_SSUB;
            tmp_ble_buf[i++] = FIRMWARE_VER_MAIN;
            tmp_ble_buf[i++] = FIRMWARE_VER_SUB;
            tmp_ble_buf[i++] = FIRMWARE_VER_SSUB;
            response_uart_pkg(_cmd, tmp_ble_buf, i);
        } break;
        case CMD_DBN_SET_MCJQ_PARAM:
        {
            unpack_pkg_set_mcjq_param(pkg, len);
            tmp_ble_buf[0] = 0;
            // set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, 1);

            response_uart_pkg(_cmd, tmp_ble_buf, 1);

            delay_ms(30);

            NVIC_SystemReset(); // 自动复位
            // para_store_init();
        } break;
        case CMD_DBN_GET_MCJQ_PARAM:
        {
            i = 0;
            tmp_ble_buf[i++] = g_loop_cng_info.smart_mode;
            tmp_ble_buf[i++] = LOOP_CAPTURE_MAX;
            Loop_Cng_Unit *_cng = g_loop_cng_info.loop_cng;
            for(j = 0; j < LOOP_CAPTURE_MAX; j++)
            {
                tmp_ble_buf[i++] = (_cng->sensitvity) | (_cng->loopFreq_Level << 4);
                tmp_ble_buf[i++] = _cng->delay_time;
                tmp_ble_buf[i++] = (_cng->output_mode) | (_cng->loopSafe_Timeout << 3);
                tmp_ble_buf[i++] = _cng->exist_mode;
                tmp_ble_buf[i++] = _cng->direction_mode;
                // 上报频率 = sclk_freq * input_div * LPCNT / CAPVD  (先乘后除避免精度损失)
                uint32_t _freq = 0;
                if (g_loop_states.loop_unit[j].loop_LPCNT > 0 && g_loop_states.loop_unit[j].loop_CAPVD > 0) {
                    _freq = (uint32_t)((uint64_t)g_crm_clocks_freq_struct.sclk_freq * g_input_div
                        * g_loop_states.loop_unit[j].loop_LPCNT / g_loop_states.loop_unit[j].loop_CAPVD);
                }
                tmp_ble_buf[i++] = (uint8_t)(_freq & 0xFF);
                tmp_ble_buf[i++] = (uint8_t)((_freq >> 8) & 0xFF);
                tmp_ble_buf[i++] = (uint8_t)((_freq >> 16) & 0xFF);
                _cng++;
            }

            response_uart_pkg(_cmd, tmp_ble_buf, i);

        } break;
//        case CMD_DBN_LOOP_SAMPLE_PARAM:{
//            // 获取/设置 地感采样参数
////            PRINT("SAMPLE_CNG: %02X\n", pkg[4]);
//            if(pkg[4] == 0){
//                // get param
//                i = 0;
//                tmp_ble_buf[i++] = 0x10;
//                tmp_ble_buf[i++] = g_loop_balance_planB.sample_cng.flag;
//                tmp_ble_buf[i++] = g_loop_balance_planB.sample_cng.max_amplitude;
//                tmp_ble_buf[i++] = (g_loop_balance_planB.sample_cng.max_amplitude >> 8) & 0xFF;
//                set_response_buf(&g_buf_ble_response,  MAGIC_BYTE_DBN_DEFAULT,  _cmd, tmp_ble_buf, i);
//            }
//            else {
//                // set param
//                i = 0;
//                k = 4;
//                tmp_ble_buf[i++] = 0x11;
//                tmp_ble_buf[i++] = pkg[k++];
//                tmp_ble_buf[i++] = pkg[k++];
//                tmp_ble_buf[i++] = pkg[k++];
//
//                g_loop_balance_planB.sample_cng.flag = tmp_ble_buf[1];
//                g_loop_balance_planB.sample_cng.max_amplitude = tmp_ble_buf[2] | ((tmp_ble_buf[3] << 8 ) & 0xFF00);
//
//                write_cfg_to_store(Addr_Loop_PlanB_Cng_Offset, &tmp_ble_buf[1], sizeof(Loop_Sample_Cng));
//
//                set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, i);
//
//                //TODO: 不整个设备复位，复位除了蓝牙的部分，即蓝牙正常，其他部分重新初始化
//                para_store_init();
//            }
//
//        } break;
//        case CMD_DBN_LOOP_BALANCE_PARAM: {
//            // 获取/设置 漂移补偿参数
////            PRINT("Balance_CNG: %02X\n", pkg[4]);
//            if(pkg[4] == 0){
//                // get param
//                i = 0;
//                tmp_ble_buf[i++] = 0x10;
//                tmp_ble_buf[i++] = g_loop_balance_planB.balance_ori_cng.flag;
//                tmp_ble_buf[i++] = g_loop_balance_planB.balance_ori_cng.max_cnt;
//                tmp_ble_buf[i++] = (g_loop_balance_planB.balance_ori_cng.max_cnt >> 8) & 0xFF;
//
//                set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, i);
//            }
//            else{
//                // set param
//                i = 0;
//                k = 4;
//                tmp_ble_buf[i++] = 0x11;
//                tmp_ble_buf[i++] = pkg[k++];
//                tmp_ble_buf[i++] = pkg[k++];
//                tmp_ble_buf[i++] = pkg[k++];
//
//                g_loop_balance_planB.balance_ori_cng.flag = tmp_ble_buf[1];
//                g_loop_balance_planB.balance_ori_cng.max_cnt = tmp_ble_buf[2] | ((tmp_ble_buf[3] << 8 ) & 0xFF00);
//
//                write_cfg_to_store(Addr_Loop_PlanB_Cng_Offset + sizeof(Loop_Sample_Cng), &tmp_ble_buf[1], sizeof(Loop_Balance_Ori_Cng));
//
//                set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, i);
//
//                //TODO: 不整个设备复位，复位除了蓝牙的部分，即蓝牙正常，其他部分重新初始化
//                para_store_init();
//
//            }
//
//        } break;
//        case CMD_DBN_LOOP_RELEASE_PLANB: {
//            // 获取/设置 释放去抖动算法参数
////            PRINT("Release2_CNG: %02X\n", pkg[4]);
//            if(pkg[4] == 0){
//                // get param
//                i = 0;
//                tmp_ble_buf[i++] = 0x10;
//                tmp_ble_buf[i++] = g_loop_balance_planB.release_ori_planB.flag_weight;
//                tmp_ble_buf[i++] = g_loop_balance_planB.release_ori_planB.max_amplitude;
//                tmp_ble_buf[i++] = (g_loop_balance_planB.release_ori_planB.max_amplitude >> 8) & 0xFF;
//                tmp_ble_buf[i++] = g_loop_balance_planB.release_ori_planB.timeout;
//                tmp_ble_buf[i++] = g_loop_balance_planB.release_change_rate.flag_weight;
//                tmp_ble_buf[i++] = g_loop_balance_planB.release_change_rate.rate_first;
//                tmp_ble_buf[i++] = g_loop_balance_planB.release_change_rate.rate_second;
//                tmp_ble_buf[i++] = g_loop_balance_planB.release_change_rate.mode;
//
//                set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, i);
//            }
//            else{
//                // set param
//                i = 0; k = 0;
//                tmp_ble_buf[i++] = 0x11;
//                memcpy(&tmp_ble_buf[1], &pkg[4], 8);
//                i += 8;
//
//                write_cfg_to_store(Addr_Loop_PlanB_Cng_Offset + sizeof(Loop_Sample_Cng) + sizeof(Loop_Balance_Ori_Cng), &tmp_ble_buf[1], 8);
//
//                set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, i);
//
//                //TODO: 不整个设备复位，复位除了蓝牙的部分，即蓝牙正常，其他部分重新初始化
//                para_store_init();
//            }
//        } break;
        case CMD_DBN_LOOP_SENS_LIST: {
            PRINT("SENS_CNG: %02X\n", pkg[4]);
            if(pkg[4] == 0x00){
                // get param
                i = 0;
                tmp_ble_buf[i++] = 0x10;
                tmp_ble_buf[i++] = g_loop_sens_list.total;
                for(k = 0; k < g_loop_sens_list.total; k++){
                    tmp_ble_buf[i++] = g_loop_sens_list.sens[k].sens_in;
                    tmp_ble_buf[i++] = g_loop_sens_list.sens[k].sens_in >> 8;
                    tmp_ble_buf[i++] = g_loop_sens_list.sens[k].sens_out;
                    tmp_ble_buf[i++] = g_loop_sens_list.sens[k].sens_out >> 8;
                }
                response_uart_pkg(_cmd, tmp_ble_buf, i);
            }
            else {
                // set param
                i = 0; k = 5;

                tmp_ble_buf[i++] = 0x11;
                g_loop_sens_list.total = pkg[k];
                if(g_loop_sens_list.total > MAX_LOOP_SENS_AMOUNT){
                    g_loop_sens_list.total = MAX_LOOP_SENS_AMOUNT;
                }
                k =  1+ g_loop_sens_list.total * 4;
                memcpy(&tmp_ble_buf[i], &pkg[5], k);
                memcpy(&g_loop_sens_list.sens, &pkg[6], k - 1);
                i += ( k );

                storage_dev();

                response_uart_pkg(_cmd, tmp_ble_buf, i);


                //TODO: 不整个设备复位，复位除了蓝牙的部分，即蓝牙正常，其他部分重新初始化
                para_store_init();
            }
        } break;
        case CMD_DBN_SET_CJQ_FACTORY: {
            // 出厂初始化
            tmp_ble_buf[0] = 0;
            set_factory_param();
            storage_dev();

            response_uart_pkg(_cmd, tmp_ble_buf, 1);

            //TODO: 不整个设备复位，复位除了蓝牙的部分，即蓝牙正常，其他部分重新初始化
            para_store_init();
        } break;
        case CMD_SENS_ACS_ENABLE: {
            // 线圈动态上报使能（简化版：仅处理 enable flag）
            // g_dbn_ble_state_acs_enable.enable = pkg[5];
        } break;
        default:
        {
        } break;
    }

}

/**
  * @brief  USART1串口接收任务
  * @param  pvParameters: 任务参数
  * @retval None
  */
void usart_task_function(void *pvParameters)
{
    uint8_t received_data;

    while(1)
    {

        if(g_pkg_uart_1.flag){
//            PRINT("Rcv_uart:%s\n", g_pkg_uart_1.pkg);
            manage_dbn_ble_default(g_pkg_uart_1.pkg, g_pkg_uart_1.offset);

            InitPkgUart(&g_pkg_uart_1);
        }

       uart_report_packet_loop_acs(0);

        #ifdef DEBUG
        if(g_flag_output)
        {
            g_flag_output = 0;
            PRINT("LPCNT:%d,%d,%d,%d CAPVD:%d,%d,%d,%d Origin:%d,%d,%d,%d VD:%d,%d,%d,%d\n",
                g_loop_states.loop_unit[0].loop_LPCNT, g_loop_states.loop_unit[1].loop_LPCNT,
                g_loop_states.loop_unit[2].loop_LPCNT, g_loop_states.loop_unit[3].loop_LPCNT,
                g_loop_states.loop_unit[0].loop_CAPVD, g_loop_states.loop_unit[1].loop_CAPVD,
                g_loop_states.loop_unit[2].loop_CAPVD, g_loop_states.loop_unit[3].loop_CAPVD,
                g_loop_states.loop_unit[0].loop_Origin, g_loop_states.loop_unit[1].loop_Origin,
                g_loop_states.loop_unit[2].loop_Origin, g_loop_states.loop_unit[3].loop_Origin,
                g_loop_states.loop_unit[0].loop_VD_FLAG, g_loop_states.loop_unit[1].loop_VD_FLAG,
                g_loop_states.loop_unit[2].loop_VD_FLAG, g_loop_states.loop_unit[3].loop_VD_FLAG);
        }
        #endif

        // 短暂延迟，避免过度占用CPU
        vTaskDelay(10);
    }
}


uint8_t g_flag_output = 0;
uint8_t g_flag_output2 = 0;
// 1ms interval
void TMR6_GLOBAL_IRQHandler(void)
{
    static uint16_t _cnt_5ms = 0;
    static uint32_t _cnt = 0;
    if(tmr_interrupt_flag_get(TMR6, TMR_OVF_FLAG) != RESET)
    {
        #ifdef DEBUG
        _cnt++;

        if(_cnt >= 2000){
            g_flag_output = 1;
            _cnt = 0;

        }
        #endif

        TimingDelayInc++;

        if(g_pkg_uart_1.offset){
            if(g_pkg_uart_1.flag == 0){
                g_pkg_uart_1.tick++;
                if(g_pkg_uart_1.tick > 8){
                    g_pkg_uart_1.flag = 1;
                }
            }

            g_loop_states.report_counter = 0;
        }




        tmr_flag_clear(TMR6, TMR_OVF_FLAG);
    }
}


/**
  * @brief  this function handles usart1 handler.
  * @param  none
  * @retval none
  */
void USART1_IRQHandler(void)
{
  if(usart_interrupt_flag_get(USART1, USART_RDBF_FLAG) != RESET)
  {
    /* read one byte from the receive data register */
      g_pkg_uart_1.pkg[g_pkg_uart_1.offset++] = usart_data_receive(USART1);
      g_pkg_uart_1.tick = 0;
//    if(usart1_rx_counter == usart2_tx_buffer_size)
//    {
//      /* disable the usart1 receive interrupt */
//      usart_interrupt_enable(USART1, USART_RDBF_INT, FALSE);
//    }
  }

}


/**
  * @}
  */

/**
  * @}
  */
