File content as of revision 19:e2de6c1776eb:

/**
 * File: ble-cli.c
 * Description: BLE CLI commands used by all applications BLE of profile.
 *
 * Copyright 2014 by CYNTEC Corporation.  All rights reserved.
 */

#include "ble_cli.h"   //  All #include define in ble_cli.h

// Genral configuration parameters
#define BLE_DEBUG  0
#define MAX_DEVNAME_LEN 32
#define CLI_FWVERION "DELTA_CLI_V1.15"

//0920 Silvia modify
#if defined(TARGET_DELTA_DFBM_NQ620)
#define MODULE_NAME "DFBM-NQ620"
#else if defined(TARGET_DELTA_DFCM_NNN40) 
#define MODULE_NAME "DFCM-NNN40-DT1R"
#endif
//0920 Silvia modify

// Advertising configuration parameters
#define APP_ADV_INTERVAL           40                     /**< The advertising interval (in units of ms). */
#define APP_ADV_TIMEOUT_IN_SECONDS 180                    /**< The advertising timeout (in units of seconds). */
#define APP_ADV_INTERVAL_MAX       10240 									// correspond to 10.24s
#define APP_ADV_INTERVAL_MIN       20		 									// correspond to 20ms
#define APP_ADV_DATA_OFFSET 2                        // Offset for Advertising Data.
//#define ADVERTISING_LED_PIN_NO LED1
// Peripheral mode operation parameters
#define CLI_SERVICE_MAX_NUM 10
#define CLI_CHAR_MAX_NUM 10
#define MAX_VALUE_LENGTH 20
// Central mode operation parameters
#define DEFAULT_SCAN_INTERVAL 500
#define DEFAULT_SCAN_WINDOW 400
#define DEFAULT_SCAN_TIMEOUT 0x0005
#define BLE_MAX_ADDRESS_NUMBER 10
#define TARGET_DEVNAME_LEN 30

typedef struct bufferGattChar {
    uint8_t props;
    UUID char_uuid;
    uint8_t value[MAX_VALUE_LENGTH];
    uint16_t char_value_handle;
    uint16_t valueLength;
} bufferGattChar_t;

typedef struct bufferGattService {
    UUID ser_uuid;
    bufferGattChar_t bufferGattChar[CLI_CHAR_MAX_NUM];
    //uint16_t service_handle;
} bufferGattService_t;

// gill 20150916 for scan
typedef struct {
    const uint8_t     * p_data;                                                      /**< Pointer to data. */
    uint8_t      data_len;                                                    /**< Length of data. */
} data_t;

/*************** General parameters**********************************/
bufferGattService_t bufferService[CLI_SERVICE_MAX_NUM]; /* save entry services */
static GattCharacteristic *charAry[CLI_SERVICE_MAX_NUM][CLI_CHAR_MAX_NUM];
DiscoveredService discoverServiceArr[CLI_SERVICE_MAX_NUM]; //Silvia add
DiscoveredCharacteristic discoverCharArr[CLI_SERVICE_MAX_NUM][CLI_CHAR_MAX_NUM]; //Silvia add
extern Serial console;
BLE deltaBLE; //gill 0904
extern const char* cyntecCommandErrorNames[];
//extern uint8_t isValidGPIO(uint8_t);

/*************** GATT Configuration parameters************************/
static bool connState = false; // gill 0904, define currently connecting state
static uint8_t service_count=0;
static uint8_t char_count=0;
static uint8_t discoverService_count=0; //Silvia add
static uint8_t discoverChar_count=0; //Silvia add
static uint16_t test_conn_handle; //Connection handle, assign after trigger onConnectionCallback
//extern bool advState; // currently no use
static Gap::Address_t saveAddr[BLE_MAX_ADDRESS_NUMBER]; // check in advertisementCallback
static uint8_t bleDevInd;
//static const unsigned ADDR_LEN = BLEProtocol::ADDR_LEN; //Silvia add, Length (in octets) of the BLE MAC address
static BLEProtocol::AddressBytes_t targetAddr; //Silvia add, For connect target device
static char targetDevName[TARGET_DEVNAME_LEN]; // For connect target device
//static char DEVICE_NAME[] = "nRF5x"; // default device name, same as defined in Gap.h
static bool conn_action = false; // Gill add 20151015
static ble_gap_addr_t m_peer_addr;
//DigitalOut led1(p7);
//DigitalOut led2(p13);
DigitalOut BLEWriteInt(p13); // used in OnDataWritten()


/******************************************************
 *               Function Definitions
 ******************************************************/

//DiscoveredCharacteristic ledCharacteristic;

void serviceDiscoveryCallback(const DiscoveredService *service)
{
    console.printf("serviceDiscoveryCallback\r\n");
    if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        console.printf("S UUID-%x attrs[%u %u]\r\n", service->getUUID().getShortUUID(), service->getStartHandle(), service->getEndHandle());
    } else {
        console.printf("S UUID-");
        const uint8_t *longUUIDBytes = service->getUUID().getBaseUUID();
        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
            console.printf("%02x", longUUIDBytes[i]);
        }
        console.printf(" attrs[%u %u]\r\n", service->getStartHandle(), service->getEndHandle());
    }
    
    discoverServiceArr[discoverService_count] = *service;
    discoverService_count++;
    discoverChar_count = 0;
}

void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP)
{
    //Silvia modify
    if (characteristicP->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        console.printf("  C UUID-%x valueAttr[%u] props[%x]\r\n", characteristicP->getUUID().getShortUUID(), characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());
    }
    else {
        console.printf("  C UUID-");
        const uint8_t *longUUIDBytes = characteristicP->getUUID().getBaseUUID();
        for (unsigned i = (UUID::LENGTH_OF_LONG_UUID) - 1; i < UUID::LENGTH_OF_LONG_UUID; i--) {
            console.printf("%02x ", longUUIDBytes[i]);
        }
        console.printf(" valueAttr[%u] props[%x]\r\n", characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());
//        uvCharacteristic        = *characteristicP;
    }
    
    discoverCharArr[discoverService_count - 1][discoverChar_count] = *characteristicP;
    discoverChar_count++;
}
void discoveryTerminationCallback(Gap::Handle_t connectionHandle)
{
    console.printf("terminated SD for handle %u\r\n", connectionHandle);
}

void onConnectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    test_conn_handle = params->handle;
    connState = true;
#if BLE_DEBUG
    console.printf("Connect: connState write to %d\r\n",connState);
#endif
    //Silvia modify
    if (params->role == Gap::CENTRAL) {
        deltaBLE.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
        deltaBLE.gattClient().launchServiceDiscovery(test_conn_handle, serviceDiscoveryCallback, characteristicDiscoveryCallback);
    }
    //Silvia modify
}

void onTimeoutCallback(Gap::TimeoutSource_t source)
{
    //led1 = !led1;
    
    //Silvia add
    switch (source) {
    	case Gap::TIMEOUT_SRC_ADVERTISING:
    		console.printf("Advertising timeout\r\n");
    		break;	
    	case Gap::TIMEOUT_SRC_SECURITY_REQUEST:
    		console.printf("Security request timeout\r\n");
    		break;
    	case Gap::TIMEOUT_SRC_SCAN:
    		console.printf("Scanning timeout\r\n");
    		break;
    	case Gap::TIMEOUT_SRC_CONN:
    		console.printf("Connection timeout\r\n");
    		break;
    }
    //Silvia add
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    console.printf("Disconnected\r\n"); //Silvia add
    //console.printf("connState:%d\r\n",connState);
    //Gap::GapState_t->connected = 0;
    connState = false;
}

void onDataWrittenCallback(const GattWriteCallbackParams *params)
{
#if BLE_DEBUG
    console.printf("onDataWritten\r\n");
#endif
    // trigger Host GPIO interrupt
    BLEWriteInt = !BLEWriteInt;
    BLEWriteInt = !BLEWriteInt;
#if BLE_DEBUG
    console.printf("handle:%04X\r\n",params->handle);
    console.printf("conn_handle:%04X,writeOp:%d,offset:%04X\r\n",params->connHandle,params->writeOp,params->offset);
#endif
    console.printf("w%d,",params->writeOp);
    const uint8_t* cpSerBaseUUID;
    const uint8_t* cpCharBaseUUID;

    // Find specific handle Characteristic
    for ( int i = 0 ; i < service_count ; i++ ) {
        for (int j = 0 ; j < CLI_CHAR_MAX_NUM ; j++ ) {
            GattAttribute& tempValueAttr = charAry[i][j]->getValueAttribute();
            if (tempValueAttr.getHandle()==params->handle) {
#if BLE_DEBUG
                console.printf("ser_count,char_count:%d,%d\r\n",i,j);
#endif
                if (bufferService[i].ser_uuid.shortOrLong()==0)
                    console.printf("%04X",bufferService[i].ser_uuid.getShortUUID());
                else {
                    cpSerBaseUUID = bufferService[i].ser_uuid.getBaseUUID();
                    for (int i=15; i>=0; i--) {
                        console.printf("%02X",cpSerBaseUUID[i]);
                    }
                }
                console.printf(",");
                //console.printf("char_uuid:");
                if (bufferService[i].bufferGattChar[j].char_uuid.shortOrLong()==0) // Short UUID
                    console.printf("%04X",bufferService[i].bufferGattChar[j].char_uuid.getShortUUID());
                else { // Long UUID
                    cpCharBaseUUID = bufferService[i].bufferGattChar[j].char_uuid.getBaseUUID();
                    for (int i=15; i>=0; i--) {
                        console.printf("%02X",cpCharBaseUUID[i]);
                    }
                }
                console.printf(",");
								// update char value length
								bufferService[i].bufferGattChar[j].valueLength = params->len;
                break;
            }
        }
    }
    console.printf("%d,",params->len);
    for(int i=0; i<params->len; i++) {
        console.printf("%02X",params->data[i]);
    }
    console.printf(";\r\n");
}

static void cyntecPrintOk(void)
{
    console.printf("\r\nOK\r\n\r\n");
}

static void cyntecPrintError(uint8_t errIdx)
{
    console.printf("\r\nERROR;");
    console.printf(cyntecCommandErrorNames[errIdx]);
    console.printf("\r\n\r\n");
}

static void cynAdvertiseStartCommand(void)
{
    uint8_t argLen = 0;
    uint8_t *arg;
    uint16_t advInterval;
    uint16_t advTimeout;

    switch (cyntecGetCommandTokenCnt()) {
        case 2:
            advInterval = APP_ADV_INTERVAL;
            advTimeout = APP_ADV_TIMEOUT_IN_SECONDS;
            break;
        case 3:
            /* arg1 is adv interval parameter */
            arg = cyntecGetCommandArgument(0,&argLen);
            advInterval = cyntecAtoiUint16( arg, argLen );
            advTimeout = APP_ADV_TIMEOUT_IN_SECONDS;
            break;
        case 4:
            arg = cyntecGetCommandArgument(0,&argLen);
            advInterval = cyntecAtoiUint16( arg, argLen );
            arg = cyntecGetCommandArgument(1,&argLen);
            advTimeout = cyntecAtoiUint16( arg, argLen );
            break;
        default:
            advInterval = APP_ADV_INTERVAL;
            advTimeout = APP_ADV_TIMEOUT_IN_SECONDS;
            break;
    }

    //if ( advInterval< APP_ADV_INTERVAL_MIN | advInterval> APP_ADV_INTERVAL_MAX | advTimeout < 1| advTimeout > BLE_GAP_ADV_TIMEOUT_LIMITED_MAX ) {
//        cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_OUT_OF_RANGE);
//        return;
//    }
#if BLE_DEBUG
    console.printf("advInterval:%d,advTimeout:%d",advInterval,advTimeout);
#endif
    uint8_t bleName[BLE_GAP_DEVNAME_MAX_LEN] = {"\0"};
    uint16_t bleLen = BLE_GAP_DEVNAME_MAX_LEN - APP_ADV_DATA_OFFSET;
    uint32_t err_code;

    err_code = sd_ble_gap_device_name_get(&bleName[APP_ADV_DATA_OFFSET], &bleLen);
#if BLE_DEBUG
    console.printf("%08X,%s\r\n",err_code,bleName+APP_ADV_DATA_OFFSET);
#endif
    deltaBLE.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    //deltaBLE.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    deltaBLE.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, bleName+APP_ADV_DATA_OFFSET, bleLen);
    deltaBLE.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    deltaBLE.gap().setAdvertisingInterval(advInterval); /* minisecond. */
    deltaBLE.gap().setAdvertisingTimeout(advTimeout); /* second. */
    deltaBLE.gap().startAdvertising();
    cyntecPrintOk();
    //nrf_gpio_pin_set(ADVERTISING_LED_PIN_NO);
}

/**@brief Function for stop advertising.
 */
static void cynAdvertiseStopCommand(void)
{
    deltaBLE.gap().stopAdvertising();
    cyntecPrintOk();
    //nrf_gpio_pin_clear(ADVERTISING_LED_PIN_NO);
}

//Silvia modify
static void cynBLENameCommand(void)
{
    uint8_t nameLen = 0; // Name char number, max is TARGET_DEVNAME_LEN
    uint8_t nameLenCharNum = 0; // Name length number is 1 or 2
    uint8_t *nameLeng = cyntecGetCommandArgument(0,&nameLenCharNum);
    nameLen = cyntecAtoi(nameLeng,nameLenCharNum);
#ifdef BLE_DEUG
    console.printf("nameLenCharNum:%i\r\nOK;",nameLenCharNum);
    console.printf("nameLen:%i\r\nOK;",nameLen);
#endif
    uint8_t bleName[BLE_GAP_DEVNAME_MAX_LEN] = {"\0"};
    uint16_t bleLen = BLE_GAP_DEVNAME_MAX_LEN - APP_ADV_DATA_OFFSET;
    ble_gap_conn_sec_mode_t sec_mode;
    uint32_t err_code;
    uint8_t * argName2 = cyntecGetCommandTotalBuffer();

    // Get name string
    if (cyntecGetCommandTokenCnt() == 2) {
        err_code = sd_ble_gap_device_name_get(&bleName[APP_ADV_DATA_OFFSET], &bleLen);
        console.printf("\r\nOK;");
        console.printf("%s",bleName+APP_ADV_DATA_OFFSET);
        console.printf(";\r\n");
    }
    // Set name
    if (cyntecGetCommandTokenCnt() >= 3) {
        if (nameLen>TARGET_DEVNAME_LEN || nameLen <=0)
            cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_OUT_OF_RANGE);
        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);
        err_code = sd_ble_gap_device_name_set(&sec_mode,
                                              (const uint8_t *) &argName2[11+nameLenCharNum],
                                              (uint16_t) nameLen);//(uint16_t) (cyntecGetTotalIndex()-11-nameLenCharNum));
        if (err_code == NRF_SUCCESS)
        	cyntecPrintOk();
        else
        	cyntecPrintError(CYNTEC_CMD_ERR_CALL_FAIL);
#ifdef BLE_DEUG
        console.printf("LengNum:%i,string:%s,index:%i\r\n",nameLenCharNum,(const uint8_t *) &argName2[11+nameLenCharNum],cyntecGetTotalIndex());
#endif
    }
}

static void cynBLEInfoCommand(void)
{
    console.printf("\r\nOK;%s;%s\r\n\r\n", CLI_FWVERION, MODULE_NAME);
}
//Silvia modify

/**@brief Set the radio's transmit power.
 *
 * @param[in] tx_power Radio transmit power in dBm (accepted values are -40, -30, -20, -16, -12, -8, -4, 0, and 4 dBm).
 *
 * @note -40 dBm will not actually give -40 dBm, but will instead be remapped to -30 dBm.
 */
static void cynBLESetTxPowerCommand (void)
{
    if (cyntecGetCommandTokenCnt() != 3) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    uint32_t err_code;
    uint8_t txLen=0;
    uint8_t *arg = cyntecGetCommandArgument(0,&txLen);
    const uint8_t NUM_TXPOW = 9;
    bool inputSwitch = false; // if find matched case in TxPow, assign true

    int8_t validTxPow[NUM_TXPOW] = {-40, -30, -20, -16, -12, -8, -4, 0, 4};
    uint8_t i;
    int8_t setValue;

    if ( arg != NULL ) {
        setValue = atoi((const char *)arg);
        for (i = 0; i < NUM_TXPOW; i++) {
            if (setValue == validTxPow[i]) {
                err_code = sd_ble_gap_tx_power_set(setValue);
                //APP_ERROR_CHECK(err_code);
                inputSwitch = true;
                break;
            }
        }

        if ( inputSwitch )
            cyntecPrintOk();
        else
            cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_OUT_OF_RANGE);

    }
    clearBuffer(); // clear the commandState.buffer
    return;
}

static void cynBLEAddressCommand(void)
{
    if (cyntecGetCommandTokenCnt() == 3) {
        uint8_t argLen = 0;
        uint8_t *arg = cyntecGetCommandArgument(0, &argLen);
        uint32_t err_code;
        uint8_t addr,i;

        /* should with "0x" + 12 len addr */
        if (argLen == 14) {
            if (arg[0] != '0' || arg[1] != 'x') {
                cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_SYNTAX_ERROR);
                return;
            }

            m_peer_addr.addr_type = BLE_GAP_ADDR_TYPE_PUBLIC;

            for ( i = 1 ; i < 7 ; i++) {
                addr = cyntecArgToUint8(arg+2*i, 2);
                /* 5 - (i-1) */
                m_peer_addr.addr[6-i] = addr;
            }

            err_code = sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_NONE, &m_peer_addr);
            //APP_ERROR_CHECK(err_code);

        } else {  //argLen != 14
            cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_SYNTAX_ERROR);
            return;
        }

//#if BLE_DEBUG
//		uint8_t buf2[20];
//		console.printf("\r\nSet BLE Address: ");
//		sprintf((char *)&buf2[0],(const char *)"[%02X %02X %02X %02X %02X %02X]",
//						m_peer_addr.addr[0], m_peer_addr.addr[1], m_peer_addr.addr[2],
//						m_peer_addr.addr[3], m_peer_addr.addr[4], m_peer_addr.addr[5]);
//		console.printf(buf2);
//#endif

        cyntecPrintOk();
    } else if (cyntecGetCommandTokenCnt() == 2) {

        uint32_t err_code;
        err_code = sd_ble_gap_address_get(&m_peer_addr);
        //APP_ERROR_CHECK(err_code);

//        cyntecPrintOk();

        console.printf("OK;[%02X %02X %02X %02X %02X %02X];\r\n",
                       m_peer_addr.addr[5], m_peer_addr.addr[4], m_peer_addr.addr[3],
                       m_peer_addr.addr[2], m_peer_addr.addr[1], m_peer_addr.addr[0]);
    } else { //cyntecGetCommandTokenCnt() not equal to 2 or 3
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
    }
    return;
}

static uint32_t adv_report_parse(uint8_t type, data_t * p_advdata, data_t * p_typedata)
{
    uint32_t index = 0;
    const uint8_t * p_data;

    p_data = p_advdata->p_data;

    while (index < p_advdata->data_len) {
        uint8_t field_length = p_data[index];
        uint8_t field_type   = p_data[index+1];

        if (field_type == type) {
            p_typedata->p_data   = &p_data[index+2];
            p_typedata->data_len = field_length-1;
            return NRF_SUCCESS;
        }
        index += field_length+1;
    }
    return NRF_ERROR_NOT_FOUND;
}

void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{
    //Silvia modify
    Gap::Address_t newAddr[6];
    memcpy(newAddr,params->peerAddr,6);
    
    if (conn_action == true) {
        conn_action = false;
        
        if (memcmp(targetAddr,newAddr,6)==0) {
			deltaBLE.gap().connect(params->peerAddr, Gap::ADDR_TYPE_PUBLIC, NULL, NULL); //0920 Silvia change address type
            return;
        }
    } else {
    	bool flagRepeat=false;
    	
    	for(int i=0; i<BLE_MAX_ADDRESS_NUMBER; i++) {
        	if (memcmp(newAddr,saveAddr[i],6)==0) {
				#if BLE_DEBUG
            	console.printf("Repeated\r\n");
				#endif
            	flagRepeat = true;
            	//return;
        	}
    	}

		#if BLE_DEBUG
    	console.printf("addr cmp result :%i\r\n",memcmp(newAddr,params->peerAddr,6));
    	console.printf("ADV data:%X,%i\r\n",&(params->advertisingData),params->advertisingDataLen);
		#endif
    	data_t adv_data;
    	data_t type_data;
    	//Initialize advertisement report for parsing.
    	adv_data.p_data = params->advertisingData;
    	adv_data.data_len = params->advertisingDataLen;
    	// Parsing Device Name
    	uint32_t err_code = adv_report_parse(BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME,
                                         	&adv_data,
                                         	&type_data);
		#if BLE_DEBUG
    	console.printf("error code:%X\r\n",err_code);
    	console.printf("type_data.data_len:%i\r\n",type_data.data_len);
		#endif
    	if (flagRepeat == false) {
        	if (err_code == 0) {
            	for (int i=0; i<type_data.data_len; i++) {
                	console.printf("%c",type_data.p_data[i]);
            	}
            	//console.printf("\r\n");
        	}
        	console.printf(",ADV,[%02X %02X %02X %02X %02X %02X],%d,%u\r\n",params->peerAddr[5], params->peerAddr[4], params->peerAddr[3],
                       		params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],params->rssi,params->type);
        	memcpy(saveAddr[bleDevInd],params->peerAddr,6);
        	bleDevInd++;
    	}
    }

    /*
    Check short name, not implemented
    */
    //else
//    {
//    	uint32_t err_code_2 = adv_report_parse(BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME,
//                                         &adv_data,
//                                         &type_data);
//        console.printf("error code:%X\r\n",err_code_2);
//        console.printf("%i\r\n",type_data.data_len);
//    	console.printf("%s\r\n",type_data.p_data);
//
//    }
}


/**@brief Function to start scanning.
 */
static void scan_start(void)
{
    memset(saveAddr,0,sizeof(saveAddr)); //Silvia add, Clear saving address set
    deltaBLE.gap().startScan(advertisementCallback);
    //APP_ERROR_CHECK(err_code);
}

static void scan_stop(void)
{
    deltaBLE.stopScan();
    //APP_ERROR_CHECK(err_code);
}

// gill 20150916 modify
static void cynBLEScanCommand(void)
{
    uint16_t setInterval = DEFAULT_SCAN_INTERVAL;
    uint16_t setWindow = DEFAULT_SCAN_WINDOW;
    uint16_t setTimeout = DEFAULT_SCAN_TIMEOUT;
    if (cyntecGetCommandTokenCnt()!= 2 & cyntecGetCommandTokenCnt()!= 5) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    // If user input scan parameters, overwrite the default value
    if (cyntecGetCommandTokenCnt()== 5) {
        uint8_t argLen = 0;
        uint8_t *arg = cyntecGetCommandArgument(0,&argLen);
        setInterval = cyntecAtoiUint16( arg, argLen );
        arg = cyntecGetCommandArgument(1,&argLen);
        setWindow = cyntecAtoiUint16( arg, argLen );
        arg = cyntecGetCommandArgument(2,&argLen);
        setTimeout = cyntecAtoiUint16( arg, argLen );
    }
#if BLE_DEBUG
    console.printf("Interval:%d,Window:%d,timeout:%d\r\n",setInterval,setWindow,setTimeout);
#endif
//    memset(saveAddr,0,sizeof(saveAddr)); // Clear saving address set
    //deltaBLE.gap().setScanParams(setInterval,setWindow,setTimeout,false);
    deltaBLE.gap().setScanInterval(setInterval);
    deltaBLE.gap().setScanWindow(setWindow);
    deltaBLE.gap().setScanTimeout(setTimeout);
    console.printf("Start Scan\r\n");
    scan_start();

}

static void cynBLEScanStopCommand(void)
{
    console.printf("\r\nStop Scanning\r\n");
    scan_stop();
}

static void cynBLEConnectCommand(void)
{
    //Silvia modify for specific address
    if (cyntecGetCommandTokenCnt() != 3) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0, &argLen);
    ble_error_t err_code;
    uint8_t addr,i;

    /* should with "0x" + 12 len addr */
    if (argLen == 14) {
        if (arg[0] != '0' || arg[1] != 'x') {
            cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_SYNTAX_ERROR);
            return;
        }
        memset(targetAddr,0,sizeof(targetAddr));

        for ( i = 1 ; i < 7 ; i++) {
            addr = cyntecArgToUint8(arg+2*i, 2);
            /* 5 - (i-1) */
            targetAddr[6-i] = addr;
        }
        
        for(int i=0; i<BLE_MAX_ADDRESS_NUMBER; i++) {
        	if (memcmp(targetAddr,saveAddr[i],6)==0) {
				err_code = deltaBLE.gap().connect(targetAddr, Gap::ADDR_TYPE_PUBLIC, NULL, NULL); //0920 Silvia change address type
        
        		if (err_code != 0) {
        			console.printf("Error:%d\r\n",err_code);
        		} else {
        			cyntecPrintOk();
        		}
        		
        		return;
        	}
    	}
    	
    	conn_action = true;
    	scan_start();  
    } else {  //argLen != 14
        cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_SYNTAX_ERROR);
        return;
    } 
    
    //if (cyntecGetCommandTokenCnt() != 3) {
//        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
//        return;
//    }
//
//    uint8_t devNameLen = 0;
//    uint8_t *argDevName = cyntecGetCommandArgument(0, &devNameLen);
//
//    //if ( argDevName != NULL ) {
////			uint8_t i;
////			for (i = 0; i < devNameLen; i++) {
////				targetDevName[i] = argDevName[i];
////			}
////			if (i < devNameLen)
////				targetDevName[i] = '\0';
////		}
//
//    memset( targetDevName , 0, TARGET_DEVNAME_LEN);
//    memcpy( targetDevName, argDevName, devNameLen);
//#if BLE_DEBUG
//    console.printf("Search for device name:%s\r\n",argDevName);
//    console.printf("Target:%s\r\n",targetDevName);
//#endif
//    conn_action = true;
//    scan_start();

}


static void cynBLEDisconnectCommand(void)
{
    ble_error_t err_code;
    err_code = deltaBLE.gap().disconnect(Gap::REMOTE_USER_TERMINATED_CONNECTION);
    //err_code = sd_ble_gap_disconnect(test_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
    //err_code = sd_ble_gap_connect_cancel();	// No function defined currently
#if BLE_DEBUG
    console.printf("Error:%d\r\n",err_code);
#endif
    cyntecPrintOk();
}

//uint8_t isValidGPIO(uint8_t num)
//{
//    uint8_t cfgPin[] = {0,4,5,6,7,13,21,23,24,25,29,30,31};
//    uint8_t i;
//
//    for ( i = 0; i < sizeof(cfgPin) ; i++ ) {
//        if ( num == cfgPin[i] )
//            return 1;
//    }
//
//    return 0;
//}

static void cynBLESystemOffCommand(void)
{
    if (cyntecGetCommandTokenCnt() == 3) {
        uint8_t argLen = 0;
        uint8_t *arg = cyntecGetCommandArgument(0,&argLen);
        uint8_t gpioNum = cyntecAtoi( arg, argLen );
        // Can only use 0,1,2,3,4,5,6,7,13,16,17,23,24,25,29
        if (!isValidGPIO(gpioNum)) {
            cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_OUT_OF_RANGE);
            return;
        }
        nrf_gpio_cfg_sense_input(gpioNum,
                                 NRF_GPIO_PIN_PULLUP,
                                 NRF_GPIO_PIN_SENSE_LOW);
        cyntecPrintOk();
    } else if (cyntecGetCommandTokenCnt() == 2) {
        /* default wake up pin is uart Rx pin */
        nrf_gpio_cfg_sense_input(RX_PIN_NUMBER,
                                 NRF_GPIO_PIN_PULLUP,
                                 NRF_GPIO_PIN_SENSE_LOW);
        cyntecPrintOk();
    } else {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    sd_power_system_off();
}

static void cynBLEGPIOCommand(void)
{

    if (cyntecGetCommandTokenCnt() == 4) {
        uint8_t argLen = 0;
        uint8_t *arg = cyntecGetCommandArgument(0,&argLen);
        uint8_t gpioNum = cyntecAtoi( arg, argLen );

        if (!isValidGPIO(gpioNum)) {
            cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_OUT_OF_RANGE);
            return;
        }

        /* arg2 is set or clear */
        uint8_t *action = cyntecGetCommandArgument(1,&argLen);

        if ( cyntecStrCmp(action,(unsigned char *)"set",argLen) == 1 ) {
            nrf_gpio_cfg_output(gpioNum);
            nrf_gpio_pin_set(gpioNum);
            cyntecPrintOk();
        } else if ( cyntecStrCmp(action,(unsigned char *)"clear",argLen) == 1 ) {
            nrf_gpio_cfg_output(gpioNum);
            nrf_gpio_pin_clear(gpioNum);
            cyntecPrintOk();
        } else {
            cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_SYNTAX_ERROR);
        }
    } else {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
    }
}

static void cynResetCommand(void)
{
    cyntecPrintOk();
    // On assert, the system can only recover with a reset.
    NVIC_SystemReset();
}

void triggerRead(const GattReadCallbackParams *response)
{
    //if (response->handle == ledCharacteristic.getValueHandle()) {
#if DUMP_READ_DATA
    printf("triggerToggledWrite: handle %u, offset %u, len %u\r\n", response->handle, response->offset, response->len);
    for (unsigned index = 0; index < response->len; index++) {
        printf("%c[%02x]", response->data[index], response->data[index]);
    }
    printf("\r\n");
#endif
//
//        uint8_t toggledValue = response->data[0] ^ 0x1;
//        ledCharacteristic.write(1, &toggledValue);
//    }
}

//Silvia modify
void centralReadCallback(const GattReadCallbackParams *response) {
//    printf("centralReadCallback Handle: %d\r\n", response->handle);
    
    for (unsigned index = 0; index < response->len; index++) {
        console.printf("[%02x]", response->data[index]);
    }
    console.printf("\r\n");
}

void centralWriteCallback(const GattWriteCallbackParams *response) {
//    printf("centralWriteCallback: %d\r\n", response->len);
    
    for (unsigned index = 0; index < response->len; index++) {
        console.printf("[%02x]", response->data[index]);
    }
    console.printf("\r\n");
}

void centralHvxCallback(const GattHVXCallbackParams *response) {
//	printf("centralHvxCallback handle %u, type %02x, len %u\r\n", response->handle, response->type, response->len);
    for (unsigned index = 0; index < response->len; index++) {
        console.printf("[%02x]", response->data[index]);
    }
    console.printf("\r\n");
}
//Silvia modify

static void cynBLEUpdateDataCommand(void)
{
    if (cyntecGetCommandTokenCnt() != 6) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    UUID buf_ser_uuid ;
    uint8_t bufferUuidVs[16];
    UUID buf_char_uuid;
    uint8_t bufVal[MAX_VALUE_LENGTH] = {0};
    uint16_t valueLen = 0;
    bool readmatchFlag = false;
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0,&argLen);

    // Handle input parameter - Service UUID
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_ser_uuid = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_ser_uuid.setupLong(bufferUuidVs);
    }

    // Handle input parameter -  Characteristic UUID
    argLen = 0;
    arg = cyntecGetCommandArgument(1,&argLen);
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_char_uuid  = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_char_uuid.setupLong(bufferUuidVs);
    }
		// Input Value Type
		arg = cyntecGetCommandArgument(2,&argLen);
		uint8_t type = *arg;
		//type = cyntecArgToUint8(arg,2);
		//printf("type:%d\r\n",type);
		if(type != '0' && type != '1') // Original Data
	 {
		 cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_OUT_OF_RANGE);
	 }
		
    // Input value
    arg = cyntecGetCommandArgument(3,&argLen);
	 if(type == '0') // Original Data
	 {
			valueLen = argLen; 
			memcpy(bufVal,arg,argLen);
	 }
	 if(type == '1') // Char Data
	 {
			valueLen = (argLen-2)/2; // uint8_t to uint16_t transform
			for (int i=0 ; i < valueLen; i++) {
				bufVal[i] = cyntecArgToUint8(arg+2+2*i,2);
			}
	 }
#if BLE_DEBUG
	 printf("type:%i,bufVal:",type);
	 printf("valueLength:%i\r\n",valueLen);
		for (int i=0 ; i < valueLen; i++) {
				printf("%02X ",bufVal[i]);
			}
		printf("\r\n");
#endif

    for ( int i = 0 ; i < service_count ; i++ ) {
        if ( (bufferService[i].ser_uuid == buf_ser_uuid) & (readmatchFlag == false)) {
            for (int j = 0 ; j < CLI_CHAR_MAX_NUM ; j++ ) {
                if (( bufferService[i].bufferGattChar[j].char_uuid == buf_char_uuid)& (readmatchFlag == false)) {
                    GattAttribute& valueAttr = charAry[i][j]->getValueAttribute();
                    ble_error_t err;
                    if (!connState)
                        err = deltaBLE.gattServer().write(valueAttr.getHandle(),bufVal,valueLen,true);
                    else
                        err = deltaBLE.gattServer().write(test_conn_handle,valueAttr.getHandle(),bufVal,valueLen,false);
										
										// update valueLength
										bufferService[i].bufferGattChar[j].valueLength = valueLen;

#if BLE_DEBUG
                    console.printf("Write ERR:%i\r\n",err);
										printf("valueLen:%i\r\n",valueLen);
#endif
                    console.printf("\r\nOK;");
                    for (uint16_t n=0; n<valueLen; n++) {
                        console.printf("%02X",bufVal[n]);
                    }
                    console.printf("\r\n");
                    readmatchFlag = true;
                    return;
                }
            }
        }
    }
    // no matched case, can not read
    if (readmatchFlag == false) {
        cyntecPrintError(CYNTEC_CMD_ERR_NO_MATCHED_ARGUMENT);
    }
}

static void cynBLEReadDataCommand(void)
{
//#if BLE_DEBUG
//    printf("Saved Char:\r\n");
//    for (int i=0; i<service_count; i++) {
//        console.printf("ser_uuid:%04X",bufferService[service_count-1].ser_uuid.getShortUUID());
//        for (int j=0; j<CLI_CHAR_MAX_NUM; j++) {
//            printf(" %i,char_uuid%04X,",j,bufferService[service_count-1].bufferGattChar[char_count-1].char_uuid.getShortUUID());
//            printf("len%d,",bufferService[service_count-1].bufferGattChar[char_count-1].valueLength);
//            printf("val%02X;",bufferService[service_count-1].bufferGattChar[char_count-1].value[0]);
//        }
//    }
//    printf("\r\n");
//#endif
    if (cyntecGetCommandTokenCnt() != 4) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    UUID buf_ser_uuid ;
    uint8_t bufferUuidVs[16];
    UUID buf_char_uuid;
    uint8_t bufVal[MAX_VALUE_LENGTH] = {0};
    uint16_t valueLen=0;
    //uint16_t * valueLenPtr;
    bool readmatchFlag = false;
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0,&argLen);

    // Handle input parameter - Service UUID
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_ser_uuid = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_ser_uuid.setupLong(bufferUuidVs);
    }

    // Handle input parameter -  Characteristic UUID
    argLen = 0;
    arg = cyntecGetCommandArgument(1,&argLen);
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_char_uuid  = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_char_uuid.setupLong(bufferUuidVs);
    }
		
    for ( int i = 0 ; i < service_count ; i++ ) {
        if ( (bufferService[i].ser_uuid == buf_ser_uuid) & (readmatchFlag == false)) {
            for (int j = 0 ; j < CLI_CHAR_MAX_NUM ; j++ ) {
                if (( bufferService[i].bufferGattChar[j].char_uuid == buf_char_uuid)& (readmatchFlag == false)) {
                    GattAttribute& valueAttr = charAry[i][j]->getValueAttribute();
//                    valueLenPtr = valueAttr.getLengthPtr();
                  	valueLen = bufferService[i].bufferGattChar[j].valueLength;
					uint16_t * valueLenPtr = &bufferService[i].bufferGattChar[j].valueLength;
                    ble_error_t err = deltaBLE.gattServer().read(valueAttr.getHandle(),bufVal,valueLenPtr);
#if BLE_DEBUG
                    console.printf("Read ERR:%i\r\n",err);
					printf("valueLen:%i\r\n",valueLen);
#endif
                    console.printf("\r\nOK;");
                    console.printf("0x");
                    for (uint16_t n=0; n<valueLen; n++) {
                        console.printf("%02X",bufVal[n]);
                    }
                    console.printf("\r\n");
                    readmatchFlag = true;
                    return;
                }
            }
        }

    }
    // no matched case, can not read
    if (readmatchFlag == false) {
        cyntecPrintError(CYNTEC_CMD_ERR_NO_MATCHED_ARGUMENT);
    }
}

static void cynGattCharCommand(void)
{
    uint8_t 			i;
    uint8_t 			valueLengthBuffer;

    if (cyntecGetCommandTokenCnt() != 5) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    
    //Silvia add
    if (service_count == 0) {
    	cyntecPrintError(CYNTEC_CMD_ERR_INVALID_STATE_TO_PERFORM_OPERATION);
        return;
    }
    //Silvia add

    /* handle parameter -  UUID */
    /* Only support 16-bit or 128-bit UUID type */
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0,&argLen);
    if (arg[0] != '0' || arg[1] != 'x' || (argLen != 6 && argLen != 34)) {
        cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_SYNTAX_ERROR);
        return;
    }
    /* len: 6 => 0xXXXX */
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        UUID::ShortUUIDBytes_t buffer_uuid_short;
        buffer_uuid_short = cyntecArgToUint16( (arg + 2), (argLen - 2));
        UUID uuid_short(buffer_uuid_short);
        bufferService[service_count-1].bufferGattChar[char_count].char_uuid = uuid_short;
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        // Initialize LongUUIDBytes_t, then use default constructor to setupLong(longUUID)
        UUID::LongUUIDBytes_t buffer_uuid_vs;
        for (uint8_t i=0; i<16; i++) {
            buffer_uuid_vs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        bufferService[service_count-1].bufferGattChar[char_count].char_uuid.setupLong(buffer_uuid_vs);
    }

    /* handle 3rd parameter -  attribute mode */
    argLen = 0;
    arg = cyntecGetCommandArgument(1,&argLen);
    if (arg[0] != '0' || arg[1] != 'x' ) {
        cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_SYNTAX_ERROR);
        return;
    }
    uint8_t prop = cyntecArgToUint8(arg+2,2);

    /* handle 4th parameter -  attribute value */
    argLen = 0;
    arg = cyntecGetCommandArgument(2,&argLen);
    if (arg[0] != '0' || arg[1] != 'x' | argLen%2 != 0) {
        cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_SYNTAX_ERROR);
        return;
    }
    valueLengthBuffer = (argLen-2)/2;
    memset(bufferService[service_count-1].bufferGattChar[char_count].value,0,MAX_VALUE_LENGTH);
    for (i=0 ; i < (argLen-2)/2; i++) {
        bufferService[service_count-1].bufferGattChar[char_count].value[i]  = cyntecArgToUint8(arg+2*(i+1), 2);
        //console.printf("%02X ",bufferService[service_count-1].bufferGattChar[char_count].value[i]);
    }
#if BLE_DEBUG
	printf("prop:%02X\r\n",prop);
    console.printf("valueLengthBuffer:%d\r\n",valueLengthBuffer);
    console.printf("value:");
    for (i=0 ; i < valueLengthBuffer; i++) {
        console.printf("%02X",bufferService[service_count-1].bufferGattChar[char_count].value[i]);
    }
#endif
    bufferService[service_count-1].bufferGattChar[char_count].valueLength = valueLengthBuffer;
    bufferService[service_count-1].bufferGattChar[char_count].props = prop;
    //bufferService[service_count-1].bufferGattChar[char_count].char_value_handle = testHandle;
    if (char_count>CLI_CHAR_MAX_NUM) {
        cyntecPrintError(CYNTEC_CMD_ERR_CALL_FAIL);
#if BLE_DEBUG
        console.printf("Error: char_count>CLI_CHAR_MAX_NUM\r\n");
#endif
        return;
    }
    cyntecPrintOk();
    char_count++;
}

static void cynRegServiceCommand(void)
{
    //GattCharacteristic *charAry[char_count];
    //GattCharacteristic **charAry;
    //charAry = new GattCharacteristic *[char_count];
#if BLE_DEBUG
    console.printf("Current char_count:%d\r\n",char_count);
    console.printf("Current service_count:%d\r\n",service_count);
#endif
    for (uint8_t i=0; i<char_count; i++) {
        charAry[service_count-1][i] = new GattCharacteristic (
            bufferService[service_count-1].bufferGattChar[i].char_uuid,
            bufferService[service_count-1].bufferGattChar[i].value,
            bufferService[service_count-1].bufferGattChar[i].valueLength,
            MAX_VALUE_LENGTH,
            bufferService[service_count-1].bufferGattChar[i].props
        );
    }
    GattService newService(bufferService[service_count-1].ser_uuid, charAry[service_count-1], char_count );
    ble_error_t err_code;
    err_code = deltaBLE.addService(newService);
    if (err_code != 0) {
#if BLE_DEBUG
        console.printf("addService error:%d\r\n",err_code);
#endif
        cyntecPrintError(CYNTEC_CMD_ERR_CALL_FAIL);
    } else
        cyntecPrintOk();
    //char_count = 0; // already did in gattService
}

static void cynGattServiceCommand(void)
{
    if (cyntecGetCommandTokenCnt() != 3) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    /* handle first parameter - Service UUID */
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0,&argLen);

    /* Service uuid is 16 bits */
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        UUID::ShortUUIDBytes_t buffer_uuid_short;
        buffer_uuid_short = cyntecArgToUint16( (arg + 2), (argLen - 2));
#if BLE_DEBUG
        console.printf("%4X",buffer_uuid_short);
#endif
        UUID uuid_short(buffer_uuid_short);
        bufferService[service_count].ser_uuid = uuid_short;
        //buffer_uuid = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }

    /* Service uuid is 128 bits */
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        // Initialize LongUUIDBytes_t, then use default constructor to setupLong(longUUID)
        UUID::LongUUIDBytes_t buffer_uuid_vs;
        for (uint8_t i=0; i<16; i++) {
            buffer_uuid_vs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
#if BLE_DEBUG
            console.printf("%2X ",buffer_uuid_vs[i]);
#endif
        }
        UUID uuid_long(buffer_uuid_vs);
        bufferService[service_count].ser_uuid = uuid_long;
    }
    cyntecPrintOk();
    service_count++;
    char_count = 0;
}

/*Tsungta, @0913 modify to compatiable with mbed os 5.0*/
#include <mbed-events/events.h>
void onBleInitError(BLE &ble, ble_error_t error)
{
    (void)ble;
    (void)error;
   /* Initialization error handling should go here */
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{

}

static EventQueue eventQueue(
    /* event count */ 16 * /* event size */ 32
);

void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
    BLE &deltaBLE = BLE::Instance();
    eventQueue.post(Callback<void()>(&deltaBLE, &BLE::processEvents));
}

void cynBLEInitCommand(void)
{   
    BLE &deltaBLE = BLE::Instance();

    deltaBLE.onEventsToProcess(scheduleBleEventsProcessing);
    deltaBLE.init(bleInitComplete);
    deltaBLE.onDisconnection(disconnectionCallback);
    deltaBLE.onConnection(onConnectionCallback);
    deltaBLE.onTimeout(onTimeoutCallback);
    deltaBLE.gattServer().onDataRead(triggerRead);

    console.printf("\r\nOK;%s;%s\r\n\r\n", CLI_FWVERION, MODULE_NAME); //0920 Silvia add
    while (true) {
        eventQueue.dispatch();
    }
}
/*Tsungta, end of @0913 modification*/

//Silvia modify
static void cynBLECenInitCommand(void)
{
    deltaBLE.init();
    deltaBLE.onDisconnection(disconnectionCallback);
    deltaBLE.onConnection(onConnectionCallback);
    deltaBLE.onTimeout(onTimeoutCallback);
    deltaBLE.gattClient().onDataRead(centralReadCallback);
    deltaBLE.gattClient().onDataWrite(centralWriteCallback);
    deltaBLE.gattClient().onHVX(centralHvxCallback);

    cyntecPrintOk();
}

static void cynBLECenReadDataCommand(void)
{
	if (cyntecGetCommandTokenCnt() != 4) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    UUID buf_ser_uuid ;
    uint8_t bufferUuidVs[16];
    UUID buf_char_uuid;
    bool readmatchFlag = false;
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0,&argLen);

    // Handle input parameter - Service UUID
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_ser_uuid = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_ser_uuid.setupLong(bufferUuidVs);
    }

    // Handle input parameter -  Characteristic UUID
    argLen = 0;
    arg = cyntecGetCommandArgument(1,&argLen);
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_char_uuid  = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_char_uuid.setupLong(bufferUuidVs);
    }
    
    for ( int i = 0 ; i < discoverService_count ; i++ ) {
    	UUID sUUID, cUUID;
    	if (discoverServiceArr[i].getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        	sUUID = UUID(discoverServiceArr[i].getUUID().getShortUUID());
    	} else {
    		const uint8_t *longUUIDBytes = discoverServiceArr[i].getUUID().getBaseUUID();
    		sUUID = UUID(longUUIDBytes);
        }
        if ((sUUID == buf_ser_uuid) & (readmatchFlag == false)) {
        	for (int j = 0 ; j < CLI_CHAR_MAX_NUM ; j++ ) {
        		if (discoverCharArr[i][j].getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        			cUUID = UUID(discoverCharArr[i][j].getUUID().getShortUUID());
    			} else {
    				const uint8_t *longUUIDBytes = discoverCharArr[i][j].getUUID().getBaseUUID();
    				cUUID = UUID(longUUIDBytes);
    			}
    			if ((cUUID == buf_char_uuid) & (readmatchFlag == false)) {
    				cyntecPrintOk();
    				discoverCharArr[i][j].read();
    				
    				readmatchFlag = true;
                    return;
    			}
        	}
        }
    }
    	
    // no matched case, can not read
    if (readmatchFlag == false) {
        cyntecPrintError(CYNTEC_CMD_ERR_NO_MATCHED_ARGUMENT);
    }	
}

static void cynBLECenWriteDataCommand(void) {
	if (cyntecGetCommandTokenCnt() != 6) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    UUID buf_ser_uuid ;
    uint8_t bufferUuidVs[16];
    UUID buf_char_uuid;
    bool readmatchFlag = false;
    uint16_t valueLen = 0;
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0,&argLen);
    uint8_t bufVal[MAX_VALUE_LENGTH] = {0};

    // Handle input parameter - Service UUID
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_ser_uuid = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_ser_uuid.setupLong(bufferUuidVs);
    }

    // Handle input parameter -  Characteristic UUID
    argLen = 0;
    arg = cyntecGetCommandArgument(1,&argLen);
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_char_uuid  = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_char_uuid.setupLong(bufferUuidVs);
    }
    
    //// Input Value Type
	arg = cyntecGetCommandArgument(2,&argLen);
	uint8_t type = *arg;
	//type = cyntecArgToUint8(arg,2);
//	printf("type:%d\r\n",type);
	if(type != '0' && type != '1') {
		cyntecPrintError(CYNTEC_CMD_ERR_ARGUMENT_OUT_OF_RANGE);
	}
		
    // Input value
    arg = cyntecGetCommandArgument(3,&argLen);
	if(type == '0') {
		valueLen = argLen; 
		memcpy(bufVal,arg,argLen);
	}
	if(type == '1') {
		valueLen = (argLen-2)/2; // uint8_t to uint16_t transform
		for (int i=0 ; i < valueLen; i++) {
			bufVal[i] = cyntecArgToUint8(arg+2+2*i,2);
		}
	}
#if BLE_DEBUG
	 printf("type:%i,bufVal:",type);
	 printf("valueLength:%i\r\n",valueLen);
	 for (int i=0 ; i < valueLen; i++) {
		printf("%02X ",bufVal[i]);
	 }
	 printf("\r\n");
#endif
    
    for ( int i = 0 ; i < discoverService_count ; i++ ) {
    	UUID sUUID, cUUID;
    	if (discoverServiceArr[i].getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        	sUUID = UUID(discoverServiceArr[i].getUUID().getShortUUID());
    	} else {
    		const uint8_t *longUUIDBytes = discoverServiceArr[i].getUUID().getBaseUUID();
    		sUUID = UUID(longUUIDBytes);
        }
        if ((sUUID == buf_ser_uuid) & (readmatchFlag == false)) {
        	for (int j = 0 ; j < CLI_CHAR_MAX_NUM ; j++ ) {
        		if (discoverCharArr[i][j].getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        			cUUID = UUID(discoverCharArr[i][j].getUUID().getShortUUID());
    			} else {
    				const uint8_t *longUUIDBytes = discoverCharArr[i][j].getUUID().getBaseUUID();
    				cUUID = UUID(longUUIDBytes);
    			}
    			if ((cUUID == buf_char_uuid) & (readmatchFlag == false)) {
    				console.printf("OK;");
	 				for (int i=0 ; i < valueLen; i++) {
					console.printf("%02X",bufVal[i]);
	 				}
	 				console.printf("\r\n");
    				deltaBLE.gattClient().write(GattClient::GATT_OP_WRITE_REQ, discoverCharArr[i][j].getConnectionHandle(), discoverCharArr[i][j].getValueHandle(), valueLen, bufVal);
    				readmatchFlag = true;
                    return;
    			}
        	}
        }
    }
    	
    // no matched case, can not read
    if (readmatchFlag == false) {
        cyntecPrintError(CYNTEC_CMD_ERR_NO_MATCHED_ARGUMENT);
    }
}

static void cynBLECenEnNotifyCommand(void)
{
	if (cyntecGetCommandTokenCnt() != 4) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    UUID buf_ser_uuid ;
    uint8_t bufferUuidVs[16];
    UUID buf_char_uuid;
    bool readmatchFlag = false;
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0,&argLen);

    // Handle input parameter - Service UUID
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_ser_uuid = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_ser_uuid.setupLong(bufferUuidVs);
    }

    // Handle input parameter -  Characteristic UUID
    argLen = 0;
    arg = cyntecGetCommandArgument(1,&argLen);
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_char_uuid  = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_char_uuid.setupLong(bufferUuidVs);
    }
    
    for ( int i = 0 ; i < discoverService_count ; i++ ) {
    	UUID sUUID, cUUID;
    	if (discoverServiceArr[i].getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        	sUUID = UUID(discoverServiceArr[i].getUUID().getShortUUID());
    	} else {
    		const uint8_t *longUUIDBytes = discoverServiceArr[i].getUUID().getBaseUUID();
    		sUUID = UUID(longUUIDBytes);
        }
        if ((sUUID == buf_ser_uuid) & (readmatchFlag == false)) {
        	for (int j = 0 ; j < CLI_CHAR_MAX_NUM ; j++ ) {
        		if (discoverCharArr[i][j].getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        			cUUID = UUID(discoverCharArr[i][j].getUUID().getShortUUID());
    			} else {
    				const uint8_t *longUUIDBytes = discoverCharArr[i][j].getUUID().getBaseUUID();
    				cUUID = UUID(longUUIDBytes);
    			}
    			if ((cUUID == buf_char_uuid) & (readmatchFlag == false)) {
    				cyntecPrintOk();
    				uint16_t value = BLE_HVX_NOTIFICATION;
    				deltaBLE.gattClient().write(GattClient::GATT_OP_WRITE_REQ, discoverCharArr[i][j].getConnectionHandle(), discoverCharArr[i][j].getValueHandle() + 1, 2, (uint8_t *)&value);
    				readmatchFlag = true;
                    return;
    			}
        	}
        }
    }
    	
    // no matched case, can not read
    if (readmatchFlag == false) {
        cyntecPrintError(CYNTEC_CMD_ERR_NO_MATCHED_ARGUMENT);
    }	
}

static void cynBLECenDisNotifyCommand(void) {
	if (cyntecGetCommandTokenCnt() != 4) {
        cyntecPrintError(CYNTEC_CMD_ERR_WRONG_NUMBER_OF_ARGUMENTS);
        return;
    }
    UUID buf_ser_uuid ;
    uint8_t bufferUuidVs[16];
    UUID buf_char_uuid;
    bool readmatchFlag = false;
    uint8_t argLen = 0;
    uint8_t *arg = cyntecGetCommandArgument(0,&argLen);

    // Handle input parameter - Service UUID
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_ser_uuid = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_ser_uuid.setupLong(bufferUuidVs);
    }

    // Handle input parameter -  Characteristic UUID
    argLen = 0;
    arg = cyntecGetCommandArgument(1,&argLen);
    if ( argLen == 6 && arg[0] == '0' && arg[1] == 'x') {
        buf_char_uuid  = cyntecArgToUint16( (arg + 2), (argLen - 2));
    }
    if ( argLen == 34 && arg[0] == '0' && arg[1] == 'x' ) {
        for (uint8_t i=0; i<16; i++) {
            bufferUuidVs[i] = cyntecArgToUint8( (arg + 2+2*i), 2);
        }
        buf_char_uuid.setupLong(bufferUuidVs);
    }
    
    for ( int i = 0 ; i < discoverService_count ; i++ ) {
    	UUID sUUID, cUUID;
    	if (discoverServiceArr[i].getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        	sUUID = UUID(discoverServiceArr[i].getUUID().getShortUUID());
    	} else {
    		const uint8_t *longUUIDBytes = discoverServiceArr[i].getUUID().getBaseUUID();
    		sUUID = UUID(longUUIDBytes);
        }
        if ((sUUID == buf_ser_uuid) & (readmatchFlag == false)) {
        	for (int j = 0 ; j < CLI_CHAR_MAX_NUM ; j++ ) {
        		if (discoverCharArr[i][j].getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        			cUUID = UUID(discoverCharArr[i][j].getUUID().getShortUUID());
    			} else {
    				const uint8_t *longUUIDBytes = discoverCharArr[i][j].getUUID().getBaseUUID();
    				cUUID = UUID(longUUIDBytes);
    			}
    			if ((cUUID == buf_char_uuid) & (readmatchFlag == false)) {
    				cyntecPrintOk();
    				uint16_t value = 0x00;
    				deltaBLE.gattClient().write(GattClient::GATT_OP_WRITE_REQ, discoverCharArr[i][j].getConnectionHandle(), discoverCharArr[i][j].getValueHandle() + 1, 2, (uint8_t *)&value);
    				readmatchFlag = true;
                    return;
    			}
        	}
        }
    }
    	
    // no matched case, can not read
    if (readmatchFlag == false) {
        cyntecPrintError(CYNTEC_CMD_ERR_NO_MATCHED_ARGUMENT);
    }
}
//Silvia modify

static void cynBLEDataIntCommand(void)
{
    cyntecPrintOk();
    deltaBLE.gattServer().onDataWritten().add(onDataWrittenCallback);
}
static void cynBLEDisDataIntCommand(void)
{
    cyntecPrintOk();
    deltaBLE.gattServer().onDataWritten().detach(onDataWrittenCallback);
}

#if BLE_DEBUG
static void cynBLETestCommand(void)
{
    // gill test 1021
    uint8_t bufVal[20] = {0};
    //uint8_t valWrite[2] = {0x22,0x33};
    //uint16_t bufhandle = 0x0001;
    //uint8_t valRead[2] = {0,0};

//	uint16_t handle = charAry[0][0]->getValueHandle();  // No used?
    GattAttribute& valueAttr = charAry[0][0]->getValueAttribute();
    //valueAttr.setHandle(bufhandle);
    console.printf("Handle:%04X ",valueAttr.getHandle());
    console.printf("UUID:%04X ",valueAttr.getUUID().getShortUUID());

    uint16_t* valueLenPtr = valueAttr.getLengthPtr();
    deltaBLE.gattServer().read(valueAttr.getHandle(),bufVal,valueLenPtr);
    console.printf("gatt val[0][1]:%02X %02X\r\n",bufVal[0],bufVal[1]);
}
#endif

CyntecCommandEntry bleCommandSets[] = {
// General
#if BLE_DEBUG
    {"TEST", cynBLETestCommand, NULL, "test"},
#endif
//
#if SIMPLE_CMD_NAME
    //{"INT", cynBLEInitCommand, NULL, "Init BLE stack"},//Tsungta, INT is called in main.cpp
    {"GIO", cynBLEGPIOCommand, NULL, "Config gpio, Usage: <GPIO NO> <set|clear>"},
    {"SLP", cynBLESystemOffCommand, NULL, "System off mode, Usage: <GPIO NO>"},
    {"RST", cynResetCommand, NULL, "Soft reset"},
    {"INF", cynBLEInfoCommand, NULL, "Get module information"},
    {"POW", cynBLESetTxPowerCommand, NULL, "Set BLE tx power, Usage: <TX POWER>"},
    {"NAM", cynBLENameCommand, NULL, "Set/Get friendly for BLE module, Usage: <LENGTH> <NAME>"},
// GATT
    {"GRS", cynRegServiceCommand, NULL, "Register standby service"},
    {"GAC", cynGattCharCommand, NULL, "Set SIG defined characteristic or Create your own,Usage: <CHAR UUID> <PROPS> <VALUE>"},
    {"GAS", cynGattServiceCommand, NULL, "Set SIG defined service or Create your own,Usage: <SERVICE UUID>"},
    {"ADS", cynAdvertiseStartCommand, NULL, "Start broadcast advertise packet,Usage: <INTERVAL> <WINDOW>"},
    {"ADP", cynAdvertiseStopCommand, NULL, "Stop broadcast advertise packet"},
    {"SCS", cynBLEScanCommand, NULL, "Start to scan BLE device, Usage: <INTERVAL> <WINDOW> <TIMEOUT>"},
    {"SCP", cynBLEScanStopCommand, NULL, "Stop to scan BLE device"},
    {"CON", cynBLEConnectCommand, NULL, "Connect to specific BLE device by Device Address, Usage: <ADDR>"},
    {"DCN", cynBLEDisconnectCommand, NULL, "Disconnection"},
    {"ADR", cynBLEAddressCommand, NULL, "Set/Get Bluetooth address, Usage: <ADDR>"},
    {"WRT", cynBLEUpdateDataCommand, NULL, "Update value of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID> <TYPE> <VALUE>"},
    {"RED", cynBLEReadDataCommand, NULL, "Read value of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID>"},
    {"EDI", cynBLEDataIntCommand, NULL, "Trigger remote write detection, give interrupt to Host."},
    {"DDI", cynBLEDisDataIntCommand, NULL, "Disable remote write detection."},
    //Silvia add BLE central commands
    {"CIN", cynBLECenInitCommand, NULL, "Init BLE Central stack"},
    {"CRD", cynBLECenReadDataCommand, NULL, "BLE Central read value of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID>"},
    {"CWD", cynBLECenWriteDataCommand, NULL, "BLE Central write value to specific characteristics, Usage: <SERVICE UUID> <CHAR UUID> <TYPE> <VALUE>"},
    {"CEN", cynBLECenEnNotifyCommand, NULL, "BLE Central enable notification of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID>"},
    {"CDN", cynBLECenDisNotifyCommand, NULL, "BLE Central disable notification of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID>"},
    {NULL, NULL, NULL, NULL},
#else
    //{"init", cynBLEInitCommand, NULL, "Init BLE stack"},//Tsungta, init is called in main.cpp
    {"gpio", cynBLEGPIOCommand, NULL, "Config gpio, Usage: <GPIO NO> <set|clear>"},
    {"sleep", cynBLESystemOffCommand, NULL, "System off mode, Usage: <GPIO NO>"},
    {"reset", cynResetCommand, NULL, "Soft reset"},
    {"info", cynBLEInfoCommand, NULL, "Get module information"},
    {"txPow", cynBLESetTxPowerCommand, NULL, "Set BLE tx power, Usage: <TX POWER>"},
    {"name", cynBLENameCommand, NULL, "Set/Get friendly for BLE module, Usage: <LENGTH> <NAME>"},
// GATT
    {"regService", cynRegServiceCommand, NULL, "Register standby service"},
    {"gattChar", cynGattCharCommand, NULL, "Set SIG defined characteristic or Create your own,Usage: <CHAR UUID> <PROPS> <VALUE>"},
    {"gattService", cynGattServiceCommand, NULL, "Set SIG defined service or Create your own,Usage: <SERVICE UUID>"},
    {"advStart", cynAdvertiseStartCommand, NULL, "Start broadcast advertise packet,Usage: <INTERVAL> <WINDOW>"},
    {"advStop", cynAdvertiseStopCommand, NULL, "Stop broadcast advertise packet"},
    {"scanStart", cynBLEScanCommand, NULL, "Start to scan BLE device, Usage: <INTERVAL> <WINDOW> <TIMEOUT>"}, //BLE central
    {"scanStop", cynBLEScanStopCommand, NULL, "Stop to scan BLE device"}, //BLE central
    {"connect", cynBLEConnectCommand, NULL, "Connect to specific BLE device by Device Address, Usage: <ADDR>"}, //Silvia modify, BLE central
    {"disconn", cynBLEDisconnectCommand, NULL, "Disconnection, Usage: cynb disconn"},
    {"bleAddr", cynBLEAddressCommand, NULL, "Set/Get Bluetooth address, Usage: <ADDR>"},
    {"update", cynBLEUpdateDataCommand, NULL, "Update value of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID> <TYPE> <VALUE>"},
    {"readData", cynBLEReadDataCommand, NULL, "Read value of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID>"},
    {"enInt", cynBLEDataIntCommand, NULL, "Trigger remote write detection, give interrupt to Host."},
    {"disInt", cynBLEDisDataIntCommand, NULL, "Disable remote write detection."},
    //Silvia add BLE central commands
    {"initBleCen", cynBLECenInitCommand, NULL, "Init BLE Central stack"},
    {"cenReadData", cynBLECenReadDataCommand, NULL, "BLE Central read value of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID>"},
    {"cenWriteData", cynBLECenWriteDataCommand, NULL, "BLE Central write value to specific characteristics, Usage: <SERVICE UUID> <CHAR UUID> <TYPE> <VALUE>"},
    {"cenEnNotify", cynBLECenEnNotifyCommand, NULL, "BLE Central enable notification of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID>"},
    {"cenDisNotify", cynBLECenDisNotifyCommand, NULL, "BLE Central disable notification of specific characteristics, Usage: <SERVICE UUID> <CHAR UUID>"},
    {NULL, NULL, NULL, NULL},
#endif
};