/***************************************************************************
 *
 * Copyright 2015-2019 BES.
 * All rights reserved. All unpublished rights reserved.
 *
 * No part of this work may be used or reproduced in any form or by any
 * means, or stored in a database or retrieval system, without prior written
 * permission of BES.
 *
 * Use of this work is governed by a license granted by BES.
 * This work contains confidential and proprietary information of
 * BES. which is protected by copyright, trade secret,
 * trademark and other intellectual property rights.
 *
 ****************************************************************************/
//#include "mbed.h"
#if defined(IBRT)
#include <stdio.h>
#include <assert.h>

#include "hal_trace.h"
#include "app_ibrt_if.h"
#include "app_tws_ctrl_thread.h"
#include "app_ibrt_rssi.h"
#include "a2dp_decoder.h"

#define RSSI_WINDOW_SIZE 30

static tota_stutter_t g_stutter = {0,};
extern float a2dp_audio_get_sample_reference(void);
extern int8_t a2dp_audio_get_current_buf_size(void);
extern diff_us_t * app_tws_ibrt_audio_sync_get_diff_us(void);

struct rssi_window_struct
{
    int8_t buf[RSSI_WINDOW_SIZE];
    uint8_t index;
};

extern osTimerId ibrt_ui_check_roleswitch_timer_id;
static struct rssi_window_struct tws_rssi_window = {0};
static struct rssi_window_struct mobile_rssi_window = {0};
static bool need_roleswitch_with_rssi_bak = false;

static void rssi_window_push(struct rssi_window_struct *p, int8_t data)
{
    if(p == NULL)
    {
        return;
    }
    
    if(p->index < RSSI_WINDOW_SIZE)
    {
        for(uint8_t i=p->index; i>0; i--)
        {
            p->buf[i] = p->buf[i-1];
        }
        p->buf[0] = data;
        p->index++;
    }
    else
    {
        for(uint8_t i=p->index-1; i>0; i--)
        {
            p->buf[i] = p->buf[i-1];
        }
        p->buf[0] = data;
    }
}

void app_ibrt_ui_rssi_reset(void)
{
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
    memset(&tws_rssi_window,0,sizeof(rssi_window_struct));
    memset(&mobile_rssi_window,0,sizeof(rssi_window_struct));
    need_roleswitch_with_rssi_bak = false;
    p_ibrt_ctrl->raw_rssi.agc_idx0 = 0;
    p_ibrt_ctrl->raw_rssi.rssi0 = 0;
    p_ibrt_ctrl->raw_rssi.rssi0_max = 0x7f;
    p_ibrt_ctrl->raw_rssi.rssi0_min = 0x80;
    p_ibrt_ctrl->raw_rssi.agc_idx1 = 0;
    p_ibrt_ctrl->raw_rssi.rssi1 = 0;
    p_ibrt_ctrl->raw_rssi.rssi1_max = 0x7f;
    p_ibrt_ctrl->raw_rssi.rssi1_min = 0x80;
    p_ibrt_ctrl->raw_rssi.rssi2 = 0;
    p_ibrt_ctrl->raw_rssi.rssi2_max = 0x7f;
    p_ibrt_ctrl->raw_rssi.rssi2_min = 0x80;
    p_ibrt_ctrl->raw_rssi.ser = 0;
    p_ibrt_ctrl->raw_rssi.rx_data_sum = 0;

    p_ibrt_ctrl->peer_raw_rssi.agc_idx0 = 0;
    p_ibrt_ctrl->peer_raw_rssi.rssi0 = 0;
    p_ibrt_ctrl->peer_raw_rssi.rssi0_max = 0x7f;
    p_ibrt_ctrl->peer_raw_rssi.rssi0_min = 0x80;
    p_ibrt_ctrl->peer_raw_rssi.agc_idx1 = 0;
    p_ibrt_ctrl->peer_raw_rssi.rssi1 = 0;
    p_ibrt_ctrl->peer_raw_rssi.rssi1_max = 0x7f;
    p_ibrt_ctrl->peer_raw_rssi.rssi1_min =0x80;
    p_ibrt_ctrl->peer_raw_rssi.rssi2 = 0;
    p_ibrt_ctrl->peer_raw_rssi.rssi2_max = 0x7f;
    p_ibrt_ctrl->peer_raw_rssi.rssi2_min =0x80;
    p_ibrt_ctrl->peer_raw_rssi.ser = 0;
    p_ibrt_ctrl->peer_raw_rssi.rx_data_sum = 0;
    p_ibrt_ctrl->role_switch_debonce_time = 0;
}

/*****************************************************************************
 Prototype    : app_ibrt_ui_check_roleswitch_timer_cb
 Description  : close bt scan when timeout
 Input        : current_evt
 Output       : None
 Return Value :
 Calls        :
 Called By    :

 History        :
 Date         : 2019/4/17
 Author       : bestechnic
 Modification : Created function

*****************************************************************************/
void app_ibrt_ui_rssi_process(void)
{
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();

    if(app_tws_ibrt_tws_link_connected()){
		rx_agc_t tws_agc = {0};
        bt_drv_reg_op_read_rssi_in_dbm(p_ibrt_ctrl->tws_conhandle,&tws_agc);
        rssi_window_push(&tws_rssi_window, tws_agc.rssi);
        if(tws_rssi_window.index >= RSSI_WINDOW_SIZE){
            int32_t tws_rssi_sum = 0;
            for(uint8_t i=0; i<RSSI_WINDOW_SIZE; i++){
                tws_rssi_sum += tws_rssi_window.buf[i];
            }

            p_ibrt_ctrl->raw_rssi.rssi1 = tws_rssi_sum/RSSI_WINDOW_SIZE;
            p_ibrt_ctrl->raw_rssi.agc_idx1 = tws_agc.rxgain;

            if(p_ibrt_ctrl->raw_rssi.rssi1 >= p_ibrt_ctrl->raw_rssi.rssi1_max)
                p_ibrt_ctrl->raw_rssi.rssi1_max = p_ibrt_ctrl->raw_rssi.rssi1;
            if(p_ibrt_ctrl->raw_rssi.rssi1 <= p_ibrt_ctrl->raw_rssi.rssi1_min)
                p_ibrt_ctrl->raw_rssi.rssi1_min = p_ibrt_ctrl->raw_rssi.rssi1;
        }else{
            p_ibrt_ctrl->raw_rssi.rssi1 = 0;
            p_ibrt_ctrl->raw_rssi.rssi1_max = 0x80;
            p_ibrt_ctrl->raw_rssi.rssi1_min = 0x7f;
            p_ibrt_ctrl->raw_rssi.agc_idx1 = 0;
        }
    }

	bool mobile_link_flag = app_tws_ibrt_mobile_link_connected();
	bool slave_ibrt_link_flag = app_tws_ibrt_slave_ibrt_link_connected();

    if(mobile_link_flag||slave_ibrt_link_flag){
		rx_agc_t mobile_agc= {0};
        if(mobile_link_flag){
            bt_drv_reg_op_read_rssi_in_dbm(p_ibrt_ctrl->mobile_conhandle,&mobile_agc);
        }else if(slave_ibrt_link_flag){
            bt_drv_reg_op_read_rssi_in_dbm(p_ibrt_ctrl->ibrt_conhandle,&mobile_agc);
        }
        rssi_window_push(&mobile_rssi_window, mobile_agc.rssi);
        if(mobile_rssi_window.index >= RSSI_WINDOW_SIZE){
            int32_t mobile_rssi_sum = 0;
            for(uint8_t i=0; i<RSSI_WINDOW_SIZE; i++){
                mobile_rssi_sum += mobile_rssi_window.buf[i];
            }

            p_ibrt_ctrl->raw_rssi.rssi0 = mobile_rssi_sum/RSSI_WINDOW_SIZE;
            p_ibrt_ctrl->raw_rssi.agc_idx0 = mobile_agc.rxgain;

            if(p_ibrt_ctrl->raw_rssi.rssi0 >= p_ibrt_ctrl->raw_rssi.rssi0_max)
                p_ibrt_ctrl->raw_rssi.rssi0_max = p_ibrt_ctrl->raw_rssi.rssi0;
            if(p_ibrt_ctrl->raw_rssi.rssi0 <= p_ibrt_ctrl->raw_rssi.rssi0_min)
                p_ibrt_ctrl->raw_rssi.rssi0_min = p_ibrt_ctrl->raw_rssi.rssi0;
        }else{
            p_ibrt_ctrl->raw_rssi.rssi0 = 0;
            p_ibrt_ctrl->raw_rssi.rssi0_max = 0x80;
            p_ibrt_ctrl->raw_rssi.rssi0_min = 0x7f;
            p_ibrt_ctrl->raw_rssi.agc_idx0 = 0;
        }
    }
    //TRACE(5,"mobile -> %d:%d tws -> %d:%d SER:%d/100", p_ibrt_ctrl->raw_rssi.rssi0, p_ibrt_ctrl->raw_rssi.agc_idx0, p_ibrt_ctrl->raw_rssi.rssi1, p_ibrt_ctrl->raw_rssi.agc_idx1,p_ibrt_ctrl->raw_rssi.ser);
}
/*****************************************************************************
 Prototype    : app_ibrt_ui_tws_switch_according_rssi_needed
 Description  :
 Input        : void
 Output       : bool
 Return Value :
 Calls        :
 Called By    :

 History        :
 Date         : 2019/6/5
 Author       : bestechnic
 Modification : Created function

*****************************************************************************/
bool app_ibrt_ui_tws_switch_according_rssi_needed(void)
{
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
    app_ibrt_ui_t *p_ibrt_ui = app_ibrt_ui_get_ctx();

    if ((p_ibrt_ctrl->raw_rssi.rssi0 != IBRT_UI_INVALID_RSSI)&&
        (p_ibrt_ctrl->peer_raw_rssi.rssi0 != IBRT_UI_INVALID_RSSI)&&
        (p_ibrt_ctrl->peer_raw_rssi.rssi0 >= IBRT_UI_MIN_RSSI) &&
        (p_ibrt_ctrl->raw_rssi.rssi0 >= IBRT_UI_MIN_RSSI))
    {
        int8_t rssi_d_value_diff = p_ibrt_ctrl->peer_raw_rssi.rssi0 - p_ibrt_ctrl->raw_rssi.rssi0;
		
		if(rssi_d_value_diff >= p_ibrt_ui->config.rssi_threshold)
		{
            //local RSSI is stronger than peer and local role is SLAVE
            return true;
		}
    }
    return false;
}

/*****************************************************************************
 Prototype    : app_ibrt_ui_check_roleswitch_timer_cb
 Description  : close bt scan when timeout
 Input        : current_evt
 Output       : None
 Return Value :
 Calls        :
 Called By    :

 History        :
 Date         : 2019/4/17
 Author       : bestechnic
 Modification : Created function

*****************************************************************************/
void app_ibrt_ui_check_roleswitch_timer_cb(void const *current_evt)
{
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
    app_ibrt_ui_t *p_ibrt_ui = app_ibrt_ui_get_ctx();

    //app_ibrt_ui_rssi_process();

    if (app_tws_ibrt_mobile_link_connected() &&
        app_ibrt_ui_is_profile_exchanged() &&
        (p_ibrt_ctrl->tws_mode  == BTIF_BLM_ACTIVE_MODE))
    {
        tws_ctrl_send_cmd(APP_TWS_CMD_GET_PEER_MOBILE_RSSI, NULL, 0);
    }

    osTimerStart(ibrt_ui_check_roleswitch_timer_id, p_ibrt_ui->config.rssi_monitor_timeout);
}

/*****************************************************************************
 Prototype    : app_ibrt_get_peer_mobile_rssi
 Description  :
 Input        : uint8_t *p_buff
                uint16_t length
 Output       : None
 Return Value :
 Calls        :
 Called By    :

 History        :
 Date         : 2019/6/4
 Author       : bestechnic
 Modification : Created function

*****************************************************************************/
void app_ibrt_get_peer_mobile_rssi(uint8_t *p_buff, uint16_t length)
{
    app_ibrt_send_cmd_without_rsp(APP_TWS_CMD_GET_PEER_MOBILE_RSSI, p_buff, length);
}

/*****************************************************************************
 Prototype    : app_ibrt_get_peer_mobile_rssi_handler
 Description  :
 Input        : uint8_t *p_buff
                uint16_t length
 Output       : None
 Return Value :
 Calls        :
 Called By    :

 History        :
 Date         : 2019/6/4
 Author       : bestechnic
 Modification : Created function

*****************************************************************************/
void app_ibrt_get_peer_mobile_rssi_handler(uint16_t rsp_seq, uint8_t *p_buff, uint16_t length)
{
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
    diff_us_t * p_diff_us = app_tws_ibrt_audio_sync_get_diff_us();

    if (app_tws_ibrt_slave_ibrt_link_connected() &&
        app_ibrt_ui_is_profile_exchanged() &&
        (p_ibrt_ctrl->tws_mode  == BTIF_BLM_ACTIVE_MODE))
    {
	    app_ui_rssi_battery_info_t  rssi_battery_buffer;
	    rssi_battery_buffer.battery_volt = p_ibrt_ctrl->local_battery_volt;
	    rssi_battery_buffer.raw_rssi = p_ibrt_ctrl->raw_rssi;
        rssi_battery_buffer.mobile_conhandle = p_ibrt_ctrl->ibrt_conhandle;
        rssi_battery_buffer.tws_conhandle = p_ibrt_ctrl->tws_conhandle;
        rssi_battery_buffer.mobile_diff_us = p_diff_us->mobile_diff_us;
        rssi_battery_buffer.tws_diff_us = p_diff_us->tws_diff_us;
        rssi_battery_buffer.cur_buf_size = a2dp_audio_get_current_buf_size();

        tws_ctrl_send_rsp(APP_TWS_CMD_GET_PEER_MOBILE_RSSI, \
                          rsp_seq, \
                          (uint8_t *)&rssi_battery_buffer, \
                          sizeof(app_ui_rssi_battery_info_t));
    }
}

/*****************************************************************************
 Prototype    : app_ibrt_get_peer_mobile_rssi_rsp_handler
 Description  :
 Input        : uint8_t *p_buff
                uint16_t length
 Output       : None
 Return Value :
 Calls        :
 Called By    :

 History        :
 Date         : 2019/6/4
 Author       : bestechnic
 Modification : Created function

*****************************************************************************/
static void app_ibrt_print_rssi_info(void)
{
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
    TRACE(0, "RSSI local mobile : local tws <> peer mobile : peer tws");
    TRACE(19,"RSSI %d %d %d %d : %d %d %d %d %d/100 <> %d %d %d %d : %d %d %d %d %d/100 %d",
          p_ibrt_ctrl->raw_rssi.agc_idx0,
          p_ibrt_ctrl->raw_rssi.rssi0,
          p_ibrt_ctrl->raw_rssi.rssi0_max,
          p_ibrt_ctrl->raw_rssi.rssi0_min,
          p_ibrt_ctrl->raw_rssi.agc_idx1,
          p_ibrt_ctrl->raw_rssi.rssi1,
          p_ibrt_ctrl->raw_rssi.rssi1_max,
          p_ibrt_ctrl->raw_rssi.rssi1_min,
          p_ibrt_ctrl->raw_rssi.ser,
          p_ibrt_ctrl->peer_raw_rssi.agc_idx0,
          p_ibrt_ctrl->peer_raw_rssi.rssi0,
          p_ibrt_ctrl->peer_raw_rssi.rssi0_max,
          p_ibrt_ctrl->peer_raw_rssi.rssi0_min,
          p_ibrt_ctrl->peer_raw_rssi.agc_idx1,
          p_ibrt_ctrl->peer_raw_rssi.rssi1,
          p_ibrt_ctrl->peer_raw_rssi.rssi1_max,
          p_ibrt_ctrl->peer_raw_rssi.rssi1_min,
          p_ibrt_ctrl->peer_raw_rssi.ser,
          p_ibrt_ctrl->current_role);
}

void app_ibrt_get_peer_mobile_rssi_rsp_handler(uint16_t rsp_seq, uint8_t *p_buff, uint16_t length)
{
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();

    if (app_tws_ibrt_mobile_link_connected() &&
        app_ibrt_ui_is_profile_exchanged() &&
        (p_ibrt_ctrl->tws_mode  == BTIF_BLM_ACTIVE_MODE))
    {
	    if(p_ibrt_ctrl->role_switch_debonce_time == 0)
	    {
		    app_ibrt_ui_t *p_ibrt_ui = app_ibrt_ui_get_ctx();
    		app_ui_rssi_battery_info_t rssi_battery_info = *(app_ui_rssi_battery_info_t *)p_buff;
		    p_ibrt_ctrl->peer_mobile_conhandle = rssi_battery_info.mobile_conhandle;
		    p_ibrt_ctrl->peer_tws_conhandle = rssi_battery_info.tws_conhandle;
		    p_ibrt_ctrl->peer_battery_volt = rssi_battery_info.battery_volt;
		    p_ibrt_ctrl->peer_raw_rssi = rssi_battery_info.raw_rssi;
            p_ibrt_ctrl->mobile_diff_us = rssi_battery_info.mobile_diff_us;
            p_ibrt_ctrl->tws_diff_us = rssi_battery_info.tws_diff_us;
            p_ibrt_ctrl->cur_buf_size = rssi_battery_info.cur_buf_size;

            app_ibrt_print_rssi_info();
            
	        if(p_ibrt_ui->config.tws_switch_according_to_rssi_value)
	        {
	            bool need_roleswitch_with_rssi = app_ibrt_ui_tws_switch_according_rssi_needed();

	            if(need_roleswitch_with_rssi != need_roleswitch_with_rssi_bak){
	                if(need_roleswitch_with_rssi){
	                    TRACE(2,"ibrt_ui_log:one headset far away cause tws switch used rssi %d %d",p_ibrt_ctrl->raw_rssi.rssi0,p_ibrt_ctrl->peer_raw_rssi.rssi0);
	                    app_ibrt_ui_tws_switch();
	                    p_ibrt_ctrl->role_switch_debonce_time = p_ibrt_ui->config.role_switch_timer_threshold;
	                }
	                need_roleswitch_with_rssi_bak = need_roleswitch_with_rssi;
	            }
	        }
	    }
	    else
	    {
	        p_ibrt_ctrl->role_switch_debonce_time--;
	    }
	}	
}

void app_ibrt_rssi_get_stutter(uint8_t * data,uint32_t * data_len)
{
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
    uint8_t temp;

    data[0] = p_ibrt_ctrl->raw_rssi.agc_idx0;
    data[1] = p_ibrt_ctrl->raw_rssi.rssi0;
    data[2] = p_ibrt_ctrl->raw_rssi.rssi0_max;
    data[3] = p_ibrt_ctrl->raw_rssi.rssi0_min;

    data[4] = p_ibrt_ctrl->raw_rssi.agc_idx1;
    data[5] = p_ibrt_ctrl->raw_rssi.rssi1;
    data[6] = p_ibrt_ctrl->raw_rssi.rssi1_max;
    data[7] = p_ibrt_ctrl->raw_rssi.rssi1_min;
    data[8] = p_ibrt_ctrl->raw_rssi.ser;

    data[9] = p_ibrt_ctrl->peer_raw_rssi.agc_idx0;
    data[10] = p_ibrt_ctrl->peer_raw_rssi.rssi0;
    data[11] = p_ibrt_ctrl->peer_raw_rssi.rssi0_max;
    data[12] = p_ibrt_ctrl->peer_raw_rssi.rssi0_min;

    data[13] = p_ibrt_ctrl->peer_raw_rssi.agc_idx1;
    data[14] = p_ibrt_ctrl->peer_raw_rssi.rssi1;
    data[15] = p_ibrt_ctrl->peer_raw_rssi.rssi1_max;
    data[16] = p_ibrt_ctrl->peer_raw_rssi.rssi1_min;
    data[17] = p_ibrt_ctrl->peer_raw_rssi.ser;

    g_stutter.sample_ref = a2dp_audio_get_sample_reference();
    g_stutter.cur_buf_size_l = a2dp_audio_get_current_buf_size();

    TRACE(2,"diff us: mobile:%d,tws:%d.", p_ibrt_ctrl->mobile_diff_us, p_ibrt_ctrl->tws_diff_us);

    TRACE(19,"%d %d %d %d : %d %d %d %d %d/100 <> %d %d %d %d : %d %d %d %d %d/100 %d",
          p_ibrt_ctrl->raw_rssi.agc_idx0,
          p_ibrt_ctrl->raw_rssi.rssi0,
          p_ibrt_ctrl->raw_rssi.rssi0_max,
          p_ibrt_ctrl->raw_rssi.rssi0_min,
          p_ibrt_ctrl->raw_rssi.agc_idx1,
          p_ibrt_ctrl->raw_rssi.rssi1,
          p_ibrt_ctrl->raw_rssi.rssi1_max,
          p_ibrt_ctrl->raw_rssi.rssi1_min,
          p_ibrt_ctrl->raw_rssi.ser,
          p_ibrt_ctrl->peer_raw_rssi.agc_idx0,
          p_ibrt_ctrl->peer_raw_rssi.rssi0,
          p_ibrt_ctrl->peer_raw_rssi.rssi0_max,
          p_ibrt_ctrl->peer_raw_rssi.rssi0_min,
          p_ibrt_ctrl->peer_raw_rssi.agc_idx1,
          p_ibrt_ctrl->peer_raw_rssi.rssi1,
          p_ibrt_ctrl->peer_raw_rssi.rssi1_max,
          p_ibrt_ctrl->peer_raw_rssi.rssi1_min,
          p_ibrt_ctrl->peer_raw_rssi.ser,
          p_ibrt_ctrl->current_role);

    if (p_ibrt_ctrl->current_role == IBRT_SLAVE)
    {
        for (int i = 0; i < 9; i++)
        {
            temp = data[i + 9];
            data[i + 9] = data[i];
            data[i] = temp;
        }
    }
    data[18] = p_ibrt_ctrl->current_role;

    data[19] = bt_drv_reg_op_fa_gain_direct_get();
    data[20] = g_stutter.cur_buf_size_l;  //L buffer size
    data[21] = p_ibrt_ctrl->cur_buf_size; //R buffer size
    //sample_reference
    data[22] = *((int8_t *)&g_stutter.sample_ref);
    data[23] = *((int8_t *)&g_stutter.sample_ref + 1);
    data[24] = *((int8_t *)&g_stutter.sample_ref + 2);
    data[25] = *((int8_t *)&g_stutter.sample_ref + 3);
    //mobile diff / us
    data[26] = *((int8_t *)&p_ibrt_ctrl->mobile_diff_us);
    data[27] = *((int8_t *)&p_ibrt_ctrl->mobile_diff_us + 1);
    data[28] = *((int8_t *)&p_ibrt_ctrl->mobile_diff_us + 2);
    data[29] = *((int8_t *)&p_ibrt_ctrl->mobile_diff_us + 3);
    //tws diff / us
    data[30] = *((int8_t *)&p_ibrt_ctrl->tws_diff_us);
    data[31] = *((int8_t *)&p_ibrt_ctrl->tws_diff_us + 1);
    data[32] = *((int8_t *)&p_ibrt_ctrl->tws_diff_us + 2);
    data[33] = *((int8_t *)&p_ibrt_ctrl->tws_diff_us + 3);

    //L error_sum
    data[34] = *((int8_t *)&p_ibrt_ctrl->raw_rssi.ser);
    data[35] = *((int8_t *)&p_ibrt_ctrl->raw_rssi.ser+1);
    //L Total_sum
    data[36] = *((int8_t *)&p_ibrt_ctrl->raw_rssi.rx_data_sum);
    data[37] = *((int8_t *)&p_ibrt_ctrl->raw_rssi.rx_data_sum+1);
    //R error_sum
    data[38] = *((int8_t *)&p_ibrt_ctrl->peer_raw_rssi.ser);
    data[39] = *((int8_t *)&p_ibrt_ctrl->peer_raw_rssi.ser+1);
    //R Total_sum
    data[40] = *((int8_t *)&p_ibrt_ctrl->peer_raw_rssi.rx_data_sum);
    data[41] = *((int8_t *)&p_ibrt_ctrl->peer_raw_rssi.rx_data_sum+1);

    TRACE(1,"0x%08x", p_ibrt_ctrl->mobile_diff_us);
    TRACE(1,"0x%08x", p_ibrt_ctrl->tws_diff_us);
    TRACE(1,"0x%08x", p_ibrt_ctrl->raw_rssi.ser);
    TRACE(1,"0x%08x", p_ibrt_ctrl->raw_rssi.rx_data_sum);
    TRACE(1,"0x%08x", p_ibrt_ctrl->peer_raw_rssi.ser);
    TRACE(1,"0x%08x", p_ibrt_ctrl->peer_raw_rssi.rx_data_sum);

    TRACE(23,"%d %d %d %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
          data[19],
          data[20],
          data[21],
          data[22],
          data[23],
          data[24],
          data[25],
          data[26],
          data[27],
          data[28],
          data[29],
          data[30],
          data[31],
          data[32],
          data[33],
          data[34],
          data[35],
          data[36],
          data[37],
          data[38],
          data[39],
          data[40],
          data[41]);
    *data_len = 42;
}

void app_ibrt_debug_parse(uint8_t *data, uint32_t data_len)
{
    bool do_it = false;
    ibrt_ctrl_t *p_ibrt_ctrl = app_tws_ibrt_get_bt_ctrl_ctx();
    uint8_t *parse_p = data;

    TRACE(2,"%s len:%d", __func__, data_len);
    DUMP8("%02x ",data, data_len);

    if (p_ibrt_ctrl->audio_chnl_sel == A2DP_AUDIO_CHANNEL_SELECT_LCHNL){
        if (*parse_p & 0x01){
            do_it = true;
        }
    }
    if (p_ibrt_ctrl->audio_chnl_sel == A2DP_AUDIO_CHANNEL_SELECT_RCHNL){
        if (*parse_p & 0x02){
            do_it = true;
        }
    }
    parse_p++;
    if (do_it){
        uint8_t funcCode = *parse_p;
        parse_p++;
        switch (funcCode)
        {
            case 1:
                if (app_tws_ibrt_mobile_link_connected()){
                    bt_drv_reg_op_dgb_link_gain_ctrl_set(p_ibrt_ctrl->mobile_conhandle, 0, *parse_p, *parse_p == 0xff ? 0 : 1);
                }else if (app_tws_ibrt_slave_ibrt_link_connected()){
                    bt_drv_reg_op_dgb_link_gain_ctrl_set(p_ibrt_ctrl->ibrt_conhandle, 0, *parse_p, *parse_p == 0xff ? 0 : 1);
                }
                break;
            case 2:
                bt_drv_reg_op_dgb_link_gain_ctrl_set(p_ibrt_ctrl->tws_conhandle, 0, *parse_p, *parse_p == 0xff ? 0 : 1);
                break;
            case 3:
                bt_drv_reg_op_fa_gain_direct_set(*parse_p);
                break;
            case 4:
                {
                    uint32_t data_format = 0;
                    data_format = be_to_host32(parse_p);
                    TRACE(1,"lowlayer_monitor len:%d", data_format);
                    if (app_tws_ibrt_mobile_link_connected()){
                        btif_me_set_link_lowlayer_monitor(btif_me_get_remote_device_by_handle(p_ibrt_ctrl->mobile_conhandle), FLAG_START_DATA, REP_FORMAT_PACKET, data_format, 0);
                    }else if (app_tws_ibrt_slave_ibrt_link_connected()){
                        btif_me_set_link_lowlayer_monitor(btif_me_get_remote_device_by_handle(p_ibrt_ctrl->ibrt_conhandle), FLAG_START_DATA, REP_FORMAT_PACKET, data_format, 0);
                    }
                }
                break;
            default:
                TRACE(1,"wrong cmd 0x%x",funcCode);
                break;
        }
    }
}
#endif

