/*
 * dbn_ble_srv.c
 *
 *  Created on: Aug 27, 2024
 *      Author: Thinkpad
 */

#include "dbn_ble_srv.h"
#include "cmcng.h"
#include "storage.h"
#include "CH59x_common.h"


uint8_t g_flag_notify_temp = 0;             //临时通知notify flag, 0 disable, 1 enable

BLE_Notify_Buf  g_notify_buftemp = {0,0, ""};
BLE_Notify_Buf  g_ble_rcv_buf  = {0,0, ""};
BLE_Notify_Buf  g_tmp_report_buf = {0,0, ""};

Buf_DBN_BLE g_buf_ble_response = {0,0,0,0,0,0,0,""};

uint8_t tmp_ble_buf[MAX_BLE_TMP_BUF_LEN] = "";

extern uint8_t loop1_FLAG_CUT;


void clear_ble_notify_buf(BLE_Notify_Buf * buf)
{
    buf->len = 0;
    buf->flag = 0;
    memset(buf, 0, MAX_BLE_Notify_Buf_LEN);
}

void clear_buf_dbn_ble(Buf_DBN_BLE * buf)
{
    uint8_t i = 0;


    buf->flag = 0;
    buf->magic = 0;
    buf->cmd = 0;
    memset(buf->dat, 0, buf->dat_len);
    buf->dat_len = 0;
    buf->dat_offset = 0;
    buf->pkg_amount = 0;
    buf->pkg_seq = 0;
}

void clear_buf_dbn_ble_all(Buf_DBN_BLE * buf)
{
    uint8_t i = 0;
//    if(g_flag_debug)
    {
//        PRINT("\nclear_buf_dbn_ble_flag:%02X,magic:%02X,pkg_amount:%02X,seq:%02X,cmd:%02X, dat_len:%d,offset:%d\n",
//        buf->flag,buf->magic,buf->pkg_amount, buf->pkg_seq,buf->cmd, buf->dat_len,buf->dat_offset);
//        for(i = 0; i < buf->dat_len; i++)
//        {
//            PRINT(" %02X", buf->dat[i]);
//        }
//        PRINT("\n");
    }
    buf->flag = 0;
    buf->magic = 0;
    buf->cmd = 0;
    memset(buf->dat, 0, MAX_BLE_BUF_LEN);
    buf->dat_len = 0;
    buf->dat_offset = 0;
    buf->pkg_amount = 0;
    buf->pkg_seq = 0;
}


// 0: 包检测异常
//检测包是否完整，Magic Byte, Header Byte, CK Byte,   校验字节（异或校验，以及和校验）
uint8_t check_pkg(uint8_t * pkg, uint8_t len)
{
    if(len < 6)     //min length of ble pkg is 6
        return 0;
    uint8_t magic = pkg[0];
    uint8_t _len = pkg[2];   //data_len
    if(_len + 5 < len)
    {
        return 0;
    }
    uint8_t _dxor = 0;
    uint8_t _dsum = 0;
    uint8_t _cklen = _len + 3;  //不计算 校验字节
    uint8_t i = 0;
    for(i = 1; i < _cklen; i++)
    {
        _dxor ^= pkg[i];
        _dsum += pkg[i];
    }
    if((_dxor != pkg[_cklen]) || (_dsum != pkg[_cklen+1]))
    {
//        if(g_flag_debug)
//        {
//            PRINT("_dxor:0x%02X - 0x%02X, _dsum:0x%02X - 0x%02X\r\n", _dxor, pkg[_cklen], _dsum, pkg[_cklen + 1] );
//        }
        return 0;
    }

    return magic;
}

static uint8_t set_dev_info_to_ready(uint8_t *dat_dst)
{
    uint8_t i = 0, j = 0;
    for(i = 0; i < 6; i++)
    {
        dat_dst[i] = MACAddr[i];
//        PRINT(" %02X",dat_dst[i] );
    }
//    memcpy(dat_dst, MACAddr, sizeof(MACAddr));
//    i += sizeof(MACAddr);
    dat_dst[i++] = HARDWARE_VER_MAIN;
    dat_dst[i++] = HARDWARE_VER_SUB;
    dat_dst[i++] = FIRMWARE_VER_MAIN;
    dat_dst[i++] = FIRMWARE_VER_SUB;

    // Dev Model
    dat_dst[i++] = strlen(PRODUCT_MODEL);
    memcpy(&dat_dst[i], PRODUCT_MODEL, strlen(PRODUCT_MODEL));
    i += strlen(PRODUCT_MODEL);

    // Dev Number
    for(j = 0; j < 6; j++, i++)
    {
        dat_dst[i] = g_dev_number[j];
    }
    // sub_code
//    memcpy(&dat_dst[i], &(g_sub_code_enable.code_set), sizeof(g_sub_code_enable.code_set));
//    i += sizeof(g_sub_code_enable.code_set);
    dat_dst[i++] = 0;
    dat_dst[i++] = 0;


    dat_dst[i++] = 0; // g_bus_dus_amount_stored.bus1_amount;
    dat_dst[i++] = 0; //g_bus_dus_amount_stored.bus2_amount;
    return i;
}

uint8_t set_response_buf(Buf_DBN_BLE *response_dst, uint8_t magic, uint8_t cmd, uint8_t * dat, uint8_t dat_len)
{
    uint8_t ret = 0;
    uint8_t i = 0;
    clear_buf_dbn_ble(response_dst);
    response_dst->magic = magic;
    response_dst->cmd = cmd;

    uint8_t _amount = dat_len / MAX_BLE_DAT_RESPONSE_LEN;
    if((dat_len % MAX_BLE_DAT_RESPONSE_LEN) > 0)
    {
        _amount++;
    }
    response_dst->pkg_amount = _amount;
    response_dst->pkg_seq = 0;
    response_dst->dat_len = dat_len;
    for(i = 0; i < dat_len; i++)
    {
        response_dst->dat[i] = dat[i];
    }
    response_dst->flag = 1;

    return ret;
}

uint16_t compute_ckb(uint8_t header_0, uint8_t header_1, uint8_t header_cmd, uint8_t *dat, uint8_t dat_len)
{
    uint16_t ret = 0;
    uint8_t i = 0;
    uint8_t _dxor = header_0 ^ header_1 ^ header_cmd;
    uint8_t _dsum = header_0 + header_1 + header_cmd;

    for(i = 0; i < dat_len; i++)
    {
        _dxor ^= dat[i];
        _dsum += dat[i];
    }
    ret = _dxor;
    ret = (ret << 8) + (_dsum % 0x100);

    return ret;
}


//response_ori  must set_response_buf first
uint8_t set_response_to_notify(Buf_DBN_BLE *response_ori, BLE_Notify_Buf * notify_dst)
{
    uint8_t ret = 0;
    uint8_t i = 0;
    uint16_t _ckbl = 0;
    if(notify_dst->flag)
    {   //dst not idle
        return ret;
    }
    if(response_ori->flag == 0)
    {   //ori idle
        return ret;
    }
    if(response_ori->pkg_amount <= 1)
    {
        //没有分包的情况
        notify_dst->buf[0] = response_ori->magic;
        if(response_ori->magic == MAGIC_BYTE_DBN_TRANSPARENT)
        {
            notify_dst->buf[1] = response_ori->pkg_seq;
        }
        else
        {
            notify_dst->buf[1] = 0;
        }
        notify_dst->buf[2] = response_ori->dat_len + 1; // 1 is cmd_byte
        notify_dst->buf[3] = response_ori->cmd;
        _ckbl = compute_ckb(notify_dst->buf[1], notify_dst->buf[2], notify_dst->buf[3], response_ori->dat, response_ori->dat_len);
        for(i = 0; i < response_ori->dat_len; i++)
        {
            notify_dst->buf[i + 4] = response_ori->dat[i];
        }
        notify_dst->buf[i + 4] = (uint8_t)(_ckbl >> 8);
        i++;
        notify_dst->buf[i + 4] = (uint8_t)(_ckbl);
        i++;
        notify_dst->len = i + 4;
//        if(g_flag_debug)
//        {
//            PRINT("set_response_to_notify_dst__\n");
//            for(i = 0; i < notify_dst->len; i++)
//            {
//                PRINT(" %02X", notify_dst->buf[i]);
//            }
//        }
        notify_dst->flag = 1;

        clear_buf_dbn_ble(response_ori);
        return 1;
    }
    else
    {
        notify_dst->buf[0] = response_ori->magic;
        uint8_t _pkg_amount = response_ori->pkg_amount;
        uint8_t _pkg_seq = response_ori->pkg_seq;
        uint8_t _remain_len = response_ori->dat_len - response_ori->dat_offset;
        if(_remain_len > MAX_BLE_DAT_RESPONSE_LEN)
        {
            _remain_len = MAX_BLE_DAT_RESPONSE_LEN;
        }
        _pkg_seq += 1;
        notify_dst->buf[1] = (_pkg_amount << 4)|(_pkg_seq);
        notify_dst->buf[2] = _remain_len + 1;   // 1 is cmd_byte
        notify_dst->buf[3] = response_ori->cmd;

        _ckbl = compute_ckb(notify_dst->buf[1], notify_dst->buf[2], notify_dst->buf[3],
            &(response_ori->dat[response_ori->dat_offset]), _remain_len);
        for(i = 0; i < _remain_len; i++)
        {
            notify_dst->buf[i + 4] = response_ori->dat[response_ori->dat_offset + i];
        }
        notify_dst->buf[i + 4] = (uint8_t)(_ckbl >> 8);
        i++;
        notify_dst->buf[i + 4] = (uint8_t)(_ckbl);
        i++;

        response_ori->dat_offset += _remain_len;
        response_ori->pkg_seq += 1;
        notify_dst->len = i + 4;


//        if(g_flag_debug)
//        {
//            PRINT("\nset_notify_dst,amount:%d,seq:%d,offset:%d\n", response_ori->pkg_amount,response_ori->pkg_seq, response_ori->dat_offset);
//            for(i = 0; i < notify_dst->len; i++)
//            {
//                PRINT(" %02X", notify_dst->buf[i]);
//            }
//            PRINT("\n");
//        }

        notify_dst->flag = 1;
        if(response_ori->pkg_amount == response_ori->pkg_seq)
        {
            clear_buf_dbn_ble(response_ori);
        }
        return 1;

    }

}


static uint8_t unpack_packs(uint8_t *pkg, uint8_t len)
{
    static uint8_t _offset = 0;
    uint8_t _header_0 = pkg[1];
    uint8_t _header_0_high = _header_0 >> 4;
    uint8_t _header_0_low = _header_0 % 0x10;
    uint8_t _len = pkg[2];
    uint8_t _cmd = pkg[3];
    uint8_t i = 0, j = 0, k = 0;
    uint8_t _amount = 0;

    if(_header_0_high)
    {
        if(_header_0_low == 1)
        {
            g_buf_ble_response.magic = pkg[0];
            g_buf_ble_response.pkg_amount = _header_0_high;
            g_buf_ble_response.pkg_seq = _header_0_low;
            g_buf_ble_response.cmd = _cmd;
            memcpy(&g_buf_ble_response.dat, &pkg[4], _len - 1);
            _offset = _len - 1;
            g_buf_ble_response.dat_len = _len - 1;
        }
        else
        {
            if((g_buf_ble_response.pkg_amount != _header_0_high) || (g_buf_ble_response.cmd != _cmd))
            {
                //printf("\nNot_the_right_sub_pack!\n");
                clear_buf_dbn_ble_all(&g_buf_ble_response);
                return 0;
            }
            if(g_buf_ble_response.pkg_seq != (_header_0_low -1))
            {
                //printf("\nNot_the_right_sub_pack!!\n");
                clear_buf_dbn_ble_all(&g_buf_ble_response);
                return 0;
            }
            g_buf_ble_response.pkg_seq = _header_0_low;
            g_buf_ble_response.dat_len += (_len - 1);  // not include cmd_byte
            memcpy(&g_buf_ble_response.dat[_offset], &pkg[4], _len - 1);
            _offset += (_len - 1);
        }
    }
    else
    {
        clear_buf_dbn_ble(&g_buf_ble_response);
        g_buf_ble_response.magic = pkg[0];
        g_buf_ble_response.pkg_amount = 1;
        g_buf_ble_response.pkg_seq = 0;
        g_buf_ble_response.cmd = _cmd;
        g_buf_ble_response.dat_len = (_len - 1);
        memcpy(g_buf_ble_response.dat, &pkg[4], _len - 1);
    }

    if(_header_0_high == _header_0_low)
    {
        // received end.
//        if(g_flag_debug)
//        {
//            printf("\nReceive_pack_over:\n");
//            for(i = 0; i < g_buf_ble_response.dat_len; i++)
//            {
//                printf(" %02X", g_buf_ble_response.dat[i]);
//            }
//            printf("\nEnd\n");
//        }


        if(g_buf_ble_response.cmd == CMD_DBN_CHECK_PASS)
        {
//            memset(tmp_ble_buf, 0, MAX_BLE_TMP_BUF_LEN);
//            if(g_buf_ble_response.dat_len != 6)
//            {
//                //response pass failed
//                tmp_ble_buf[0] = 0x01;
//                set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, 1);
//            }
//            else
//            {
//                char _pass = check_ble_safe_pass(g_buf_ble_response.dat);
//                if(_pass)
//                {
//                    //response ok
//                    tmp_ble_buf[0] = 0x0;
//                    set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, 1);
//                }
//                else
//                {
//                    //response failed
//                    tmp_ble_buf[0] = 0x01;
//                    set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, 1);
//                }
//            }

        }   //end if g_buf_ble_response.cmd == CMD_DBN_CHECK_PASS
        else if(g_buf_ble_response.cmd == CMD_DBN_MODIFY_PASS)
        {
//            memset(tmp_ble_buf, 0, MAX_BLE_TMP_BUF_LEN);
//            if(g_buf_ble_response.dat_len != 12)
//            {
//                //response failed
//                tmp_ble_buf[0] = 0x01;
//            }
//            else
//            {
//                char _pass = check_ble_safe_pass(g_buf_ble_response.dat);
//                if(_pass)
//                {
//                    //response ok
//                    set_ble_safe_pass(&g_buf_ble_response.dat[6]);
//                    tmp_ble_buf[0] = 0x00;
//                }
//                else
//                {
//                    //response failed
//                    tmp_ble_buf[0] = 0x01;
//                }
//            }
            set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, 1);
        }   //end if g_buf_ble_response.cmd == CMD_DBN_MODIFY_PASS
    }

    return 0;
}


static void unpack_pkg_set_cjq_param(uint8_t *pkg, uint8_t len)
{
    uint8_t i = 4, k = 0;
    uint8_t tmp[7] = {0};
    k = pkg[i++];
    g_loop_cng_unit.sensitvity = k & 0x0F;
    g_loop_cng_unit.loopFreq_Level = k >> 4;
    g_loop_cng_unit.delay_time = pkg[i++];
    k = pkg[i++];
    g_loop_cng_unit.output_mode = k & 0x03;
    g_loop_cng_unit.loopSafe_Timeout = k >> 3;
    g_loop_cng_unit.exist_mode = pkg[i++];
    g_loop_cng_unit.direction_mode = pkg[i++];

    g_function_mode = (g_loop_cng_unit.direction_mode >> 4) & 0x0F;

    g_hold_time = g_loop_cng_unit.exist_mode * 30 * 20;

    i = 0;
    tmp[i++] = g_loop_cng_unit.sensitvity;
    tmp[i++] = g_loop_cng_unit.delay_time;
    tmp[i++] = g_loop_cng_unit.output_mode;
    tmp[i++] = g_loop_cng_unit.direction_mode;  //默认 0
    tmp[i++] = g_loop_cng_unit.loopFreq_Level;
    tmp[i++] = g_loop_cng_unit.loopSafe_Timeout;
    tmp[i++] = g_loop_cng_unit.exist_mode;

    write_cfg_to_store(4,  tmp, 7);

}


void  usart_packet_processing_acs_loop(uint8_t flag)
{
    static uint32_t _current_freq = 0;
    static uint32_t _last_freq = 0;

    clear_ble_notify_buf(&g_tmp_report_buf);

    uint8_t i = 0,j;
    uint32_t _loop_capvd = g_loop_acs_info.loop_capvd;
    g_tmp_report_buf.flag = flag;
    

    g_loop_acs_info.condition = (g_env_activity > 150) ? 15 : (g_env_activity / 10);
    if(loop1_FLAG_CUT){
        g_loop_acs_info.condition = 15;
    }
    g_tmp_report_buf.buf[i++] = SENS_LOOP_DYNAMIC;

    g_tmp_report_buf.buf[i++] = (g_loop_acs_info.loop_num << 6) | (g_freq_level << 4) | (g_loop_acs_info.condition & 0x0F);
    g_tmp_report_buf.buf[i++] = (g_loop_acs_info.relay2_state << 6) | (g_loop_acs_info.relay2_cng << 4) | (g_loop_acs_info.relay1_state << 2) | (g_loop_acs_info.relay1_cng & 0x3);
    g_tmp_report_buf.buf[i++] = (loop1_FLAG_CUT << 7) | (g_loop_acs_info.car_state << 4) | (g_loop_acs_info.direction & 0x0F);

    g_freq_tmp = (float)(g_sys_freq )/((float)(_loop_capvd)/((float)(loop1_LPCNT)));     // ?? ???????

    // why + 360?   balance value  ,补偿，经验值，可以根据示波器比较再进行调整
    _current_freq = (uint32_t)(g_freq_tmp) + 360 ;// * 32);
    // if(flag)
    {
        g_loop_acs_info.frequent = _current_freq;
        _last_freq = _current_freq;
    }
    // else
    // {
    //     if(_current_freq > _last_freq)
    //     {
    //         if(_current_freq - _last_freq > 30)
    //         {
    //             //稳定显示，过滤低数值的变动
    //             g_loop_acs_info.frequent = _current_freq;
    //             _last_freq = _current_freq;
    //         }
    //     }
    //     else
    //     {
    //         if(_last_freq - _current_freq > 30)
    //         {
    //             // 稳定显示，过滤低数值的变动
    //             g_loop_acs_info.frequent = _current_freq;
    //             _last_freq = _current_freq;
    //         }
    //     }
    // }

    // g_tmp_report_buf.buf[i++] = (uint8_t)flt_mgr.init_freq;
    // g_tmp_report_buf.buf[i++] = (flt_mgr.init_freq >> 8) % 0x100;
    // g_tmp_report_buf.buf[i++] = (flt_mgr.init_freq >> 16) % 0x100;

    if(g_loop_acs_info.flag_change){
        g_loop_acs_info.flag_change = 0;
    }
    g_tmp_report_buf.buf[i++] = (g_loop_power_up_state == 0) ? 1 : ((uint8_t)g_loop_cut_amount);
    g_tmp_report_buf.buf[i++] = (g_loop_power_up_state == 0) ? 0 : ((g_loop_cut_amount >> 8) % 0x100);
    g_tmp_report_buf.buf[i++] = (g_loop_power_up_state == 0) ? 0 : ((g_loop_cut_amount >> 16) % 0x100);

    g_tmp_report_buf.buf[i++] = (uint8_t)g_loop_acs_info.frequent;
    g_tmp_report_buf.buf[i++] = (g_loop_acs_info.frequent >> 8) % 0x100;
    g_tmp_report_buf.buf[i++] = (g_loop_acs_info.frequent >> 16) % 0x100;

    g_tmp_report_buf.buf[i++] = (uint8_t)g_loop_acs_info.variation;
    g_tmp_report_buf.buf[i++] = (g_loop_acs_info.variation >> 8) % 0x100;
    g_tmp_report_buf.buf[i++] = (g_loop_acs_info.variation >> 16) % 0x100;


    g_tmp_report_buf.len = i;

    set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, CMD_SUB_SENS_REPORT, g_tmp_report_buf.buf, g_tmp_report_buf.len);
}


void report_sens_acs(void)
{
    uint8_t _ret = 0;

    if(g_dbn_ble_state_acs_enable.enable)
    {
        // 上报线圈动态数据


//        switch(g_dbn_ble_state_acs_enable.sens_type)
//        {
//            //case SENS_LOOP_DYNAMIC:
//            default:
//            {
                // 线圈动态信息
                if(g_loop_acs_info.flag_event)
                {
//                    if(g_loop_acs_info.report_counter >= 3)
                    {
//                        refresh_loop_acs_info();
                        g_loop_acs_info.flag_event = 0;
                        g_loop_acs_info.report_counter = 0;

                        // report
                        usart_packet_processing_acs_loop(1);
                    }
                }

                if(g_loop_acs_info.report_counter >= 120)
                {
//                        refresh_loop_acs_info();
                    g_loop_acs_info.report_counter = 0;

                    // report
                    usart_packet_processing_acs_loop(0);
//                    response_bt_dat(usart_packet_processing_acs_loop(0));
                }

                if(g_dbn_ble_state_acs_enable.timeout_min)
                {
                    if(mstick() - g_dbn_ble_state_acs_enable.counter >= g_dbn_ble_state_acs_enable.timeout_counter)
                    {
                        g_dbn_ble_state_acs_enable.flag = 0;
                        g_dbn_ble_state_acs_enable.enable = 0;

                    }
                }

//            } break;
//        }
    }
}




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, k = 0;

    memset(tmp_ble_buf, 0, MAX_BLE_TMP_BUF_LEN);

    switch(_cmd)
    {
        case CMD_DBN_GET_DEV_INFO:
        {
            i = set_dev_info_to_ready(tmp_ble_buf);

            set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, i);

            // printf("\nGet_dev_info\n");
        } break;
        case CMD_DBN_RESET_DEV:
        {
            //Restart Dev
            SYS_ResetExecute();
        } break;
//        case CMD_DBN_CHECK_PASS:
//        {
//            unpack_packs(pkg, len);
//        } break;
        case CMD_DBN_SET_CJQ_PARAM:
        {
//            PRINT("\nSet_Cjq_param\n");
            unpack_pkg_set_cjq_param(pkg, len);
            tmp_ble_buf[0] = 0;
            set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, 1);

            para_store_init();
        } break;
        case CMD_DBN_GET_CJQ_PARAM:
        {
            i = 0;
            tmp_ble_buf[i++] = (g_freq_sens + 1) | (g_loop_cng_unit.loopFreq_Level << 4);
            tmp_ble_buf[i++] = g_loop_cng_unit.delay_time;
            tmp_ble_buf[i++] = g_loop_cng_unit.output_mode | (g_loop_cng_unit.loopSafe_Timeout << 3);
            tmp_ble_buf[i++] = g_loop_cng_unit.exist_mode;
            tmp_ble_buf[i++] = g_loop_sens_list.total << 4;
            tmp_ble_buf[i++] = (g_loop_power_up_state == 0) ? 1 : ((uint8_t)(g_loop_cut_amount));
            tmp_ble_buf[i++] = (uint8_t)(g_loop_cut_amount >> 8);
            tmp_ble_buf[i++] = g_function_mode;

            set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, i);
//            PRINT("\nGet_Cjq_param\n");
        } 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]);
            memset(tmp_ble_buf, 0, MAX_BLE_TMP_BUF_LEN);
            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;
                }
                set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _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);
                i += k;

                write_cfg_to_store(Addr_Loop_Sens_List_Offset,  &tmp_ble_buf[1], (i - 1));
                DelayMs(10);
                
                set_response_buf(&g_buf_ble_response, MAGIC_BYTE_DBN_DEFAULT, _cmd, tmp_ble_buf, i);
                write_cfg_to_store(Addr_Loop_Sens_List_Offset,  &tmp_ble_buf[1], (i - 1));
                DelayMs(10);

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

            set_factory_param();
            factory_dev();
            DelayMs(10);
            //TODO: 不整个设备复位，复位除了蓝牙的部分，即蓝牙正常，其他部分重新初始化
            para_store_init();
        } break;
        case CMD_SENS_ACS_ENABLE: {
            // 线圈动态
            g_dbn_ble_state_acs_enable.enable = pkg[5];

            if(g_dbn_ble_state_acs_enable.enable){
                g_loop_acs_info.condition = 0;
                g_dbn_ble_state_acs_enable.interval = pkg[6] * 0x7F;
                g_dbn_ble_state_acs_enable.timeout_min = pkg[7];
                if(g_dbn_ble_state_acs_enable.timeout_min){
                    g_dbn_ble_state_acs_enable.counter = mstick();
                    g_dbn_ble_state_acs_enable.timeout_counter = g_dbn_ble_state_acs_enable.timeout_min * 60 * 100;     // TODO: min --> ms
                }
                g_dbn_ble_state_acs_enable.flag = 1;
            }
            else{
                g_dbn_ble_state_acs_enable.flag = 0;
            }
        } break;
        default:
        {
        } break;
    }

}


void poll_dbn_ble(void)
{
    uint8_t i = 0;
    uint8_t magic = 0;
    if(g_ble_rcv_buf.flag)
    {
//        if(g_flag_debug)
//        {
//            PRINT("profile ChangeCB CHAR1?..:%s, len:%d \n", g_ble_rcv_buf.buf, g_ble_rcv_buf.len);
//            for(i = 0; i < g_ble_rcv_buf.len; i++)
//            {
//                PRINT(" %02X", g_ble_rcv_buf.buf[i]);
//            }
//            PRINT("\n");
//        }
        magic = check_pkg(g_ble_rcv_buf.buf, g_ble_rcv_buf.len);
        // printf("_magic: %02X\n", magic);
        if(magic != 0)
        {
//            if(magic == MAGIC_BYTE_DBN_DEFAULT)
            {
                manage_dbn_ble_default(g_ble_rcv_buf.buf, g_ble_rcv_buf.len);
            }
        }

//        memcpy(g_notify_buftemp.buf, g_ble_rcv_buf.buf, g_ble_rcv_buf.len);
//        g_notify_buftemp.len = g_ble_rcv_buf.len;
//        g_flag_notify_temp = 1;
        clear_ble_notify_buf(&g_ble_rcv_buf);
    }
    if(g_notify_buftemp.flag == 0)
    {
        report_sens_acs();
        g_flag_notify_temp = set_response_to_notify(&g_buf_ble_response, &g_notify_buftemp);
    }
}