#include "ble_conn_params.h"
#include "softdevice_handler.h"
#include "app_error.h"
#include "nrf_delay.h"
#include "type.h"
#include "spi.h"
#include "queue.h"
#include "ble_dtm.h"
#include "proc.h"
#include "dtm.h"


extern uint8_t dtm_flag;
uint16_t dtm_command = 0x8004;   //command_code:freqency:length:payload in 2:6:6:2 bits.

static void ble_stack_disable(void)
{
    uint32_t         err_code;

//    err_code = sd_nvic_DisableIRQ(SWI2_IRQn);
//    APP_ERROR_CHECK(err_code);

    err_code = softdevice_handler_sd_disable();
    APP_ERROR_CHECK(err_code);
}


void reset_prepare(void)
{
    uint32_t err_code;


//      err_code = sd_ble_gap_disconnect(g_s_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
//      APP_ERROR_CHECK(err_code);

    err_code = ble_conn_params_stop();
    APP_ERROR_CHECK(err_code);
    //????
    sd_ble_gap_adv_stop();

    //disable???
    ble_stack_disable();
    nrf_delay_ms(1000);
}

static uint32_t dtm_cmd_put(uint16_t command)
{
    dtm_cmd_t      command_code = (command >> 14) & 0x03;
    dtm_freq_t     freq         = (command >> 8) & 0x3F;
    uint32_t       length       = (command >> 2) & 0x3F;
    dtm_pkt_type_t payload      = command & 0x03;

    // Check for Vendor Specific payload.
    if (payload == 0x03)
    {
        /* Note that in a HCI adaption layer, as well as in the DTM PDU format,
           the value 0x03 is a distinct bit pattern (PRBS15). Even though BLE does not
           support PRBS15, this implementation re-maps 0x03 to DTM_PKT_VENDORSPECIFIC,
           to avoid the risk of confusion, should the code be extended to greater coverage.
        */
        payload = DTM_PKT_VENDORSPECIFIC;
    }
    return dtm_cmd(command_code, freq, length, payload);
}

void dtm_process_data(queue_list_t* queue_send, queue_list_t* queue_recv)
{
    uint32_t    dtm_error_code;
    reset_prepare();
    dtm_error_code = dtm_init();
    if (dtm_error_code != DTM_SUCCESS)
    {
        spis_transfer_data(BLE_CMD_DTM_STACK_CLOSE, NULL, 0);
    }
    else
    {
        spis_transfer_data(BLE_CMD_DTM_STACK_CLOSE, NULL, 0);
    }
    while(QUEUE_MSG_NOT_EMPTY(queue_send))
    {
        QUEUE_MSG_PROCESS(queue_send,spis_send_data);
    }

    for(;;)
    {
        QUEUE_MSG_PROCESS(queue_recv,spis_process_data);
        if(dtm_flag == 2)
        {
            dtm_flag = 0;
            if (dtm_cmd_put(dtm_command) != DTM_SUCCESS)
            {
                spis_transfer_data(BLE_CMD_DTM_TX_ON, NULL, 0);
            }
            else
            {
                spis_transfer_data(BLE_CMD_DTM_TX_ON, NULL, 0);
            }
        }
        else if(dtm_flag == 3)  //dtm tx off
        {
            dtm_flag = 0;
            if (dtm_cmd_put(0xC000) != DTM_SUCCESS)
            {
                spis_transfer_data(BLE_CMD_DTM_TX_OFF, NULL, 0);
            }
            else
            {
                spis_transfer_data(BLE_CMD_DTM_TX_OFF, NULL, 0);
            }
        }
        QUEUE_MSG_PROCESS(queue_send,spis_send_data);
        dtm_wait();
    }
}