#include "b6x.h"
#include "bledef.h"
#include "drvs.h"

#include "app.h"
#include "prf_sess.h"
#include "app_uart.h"
#include "app_ws2812.h"

extern BLE_GRB_Data_t BLE_WS2812_GRBs;
extern radar_daraframe_t radar_daraframe;

#define BLE_MAX_LEN           (BLE_MTU - 3)
#define NULL_CNT              20

static uint8_t buff[BLE_MAX_LEN];


bool speed_test = 0;


// 透传接收回调
void sess_cb_rxd(uint8_t conidx, uint16_t len, const uint8_t *data)
{
    uart_send(UART1_PORT, len, data);
    if(len == 9 && 0xa5 ==data[0] &&  0x5a ==data[len-1]){

        //app_set_speed(*(int8_t *)(&data[2]));

        // Set_Status(OUT_High_brake,(0x01 & (data[1] >> 0)));
        // Set_Status(OUT_Low_brake,(0x01 & (data[1] >> 1)));
        // Set_Status(OUT_Push_key,(0x01 & (data[1] >> 2)));
        // Set_Status(OUT_High_speed,(0x01 & (data[1] >> 3)));
        // Set_Status(OUT_Low_speed,(0x01 & (data[1] >> 4)));
        // Set_Status(OUT_Door_lock,(0x01 & (data[1] >> 5)));
        // Set_Status(OUT_Switch_gear,(0x01 & (data[1] >> 6)));

        BLE_WS2812_GRBs.len = data[3];
        BLE_WS2812_GRBs.GRB = ((uint32_t)data[4]<<17)|((uint32_t)data[5]<<9)|((uint32_t)data[6]<<1);
        buff[0]=0xa5;
        buff[1]=data[1];
        buff[2]=(*(int8_t *)(&data[2]))<0?-data[2]:data[2];
        buff[3]=radar_daraframe.Left_data+1;
        buff[4]=radar_daraframe.Right_data+2;
        buff[5]=buff[1]+buff[2]+buff[3]+buff[4];
        buff[6]=0x5a;
        sess_txd_send(app_env.curidx, 7, buff);


    }
    
}

/// Uart Data procedure
static void data_proc(void)
{
//     // Todo Loop-Proc
//     static uint8_t null_cnt = 0;
//     uint16_t len;

//     len = uart1Rb_Read(&buff[buff_len], BLE_MAX_LEN - buff_len);
//     if (len > 0)
//     {
//         buff_len += len;
//         if (buff_len < BLE_MAX_LEN)
//         {
//             return; // wait full
//         }
//     }
//     else
//     {
//         if ((buff_len > 0) && (null_cnt++ > NULL_CNT))
//         {
//             //finish = true;
//             null_cnt = 0;
//         }
//         else
//         {
//             return; // wait again
//         }
//     }

//     if (app_state_get() == APP_CONNECTED)
//     {
//         if (buff[0] == 0xAA)
//         {
//             speed_test = false;
//             DEBUG("GAP Disc!\r\n");
//             gapc_disconnect(app_env.curidx);
//             buff_len = 0;
//         }
//         else if (buff[0] == 'S')
//         {
//             speed_test = true;
//             buff_len = 0;
//         }
//         else if (sess_txd_send(app_env.curidx, buff_len, buff) == LE_SUCCESS)
//         {
//             debugHex(buff, buff_len);
//             buff_len = 0;
//         }
//     }
//     else
//     {
//         // goto reset
//         if (buff[0] == 0xAA)
//         {
//             DEBUG("GAP Reset!\r\n");
//             gapm_reset();
//         }

//         buff_len = 0;
//     }
}

#if (CFG_SLEEP)
static void sleep_proc(void)
{
    uint8_t lpsta = ble_sleep(BLE_SLP_TWOSC, BLE_SLP_DURMAX);

    if (lpsta == BLE_IN_SLEEP)
    {
        uint16_t lpret = core_sleep(CFG_WKUP_BLE_EN);
        //DEBUG("ble sta:%d, wksrc:%X", lpsta, lpret);
    }
    else
    {
        //DEBUG("ble sta:%d", lpsta);
    }
}
#endif //(CFG_SLEEP)

void app_ble_procedure(void)
{
    #if (CFG_SLEEP)
    sleep_proc();
    #endif //(CFG_SLEEP)

    data_proc();

    if ((app_state_get() == APP_CONNECTED) && (speed_test))
    {
        sess_txd_send(app_env.curidx, BLE_MAX_LEN, buff);
    }
}