File content as of revision 109:c274780ff609:

#include "debug.h"
#include "ATCmdManager.h"
#include "common_config.h"
#include "common_types.h"
#include "http_request.h"
#include "mbed_memory_status.h"
//#include "mbed_memory_status.h"
#define FILE_CODE       "atcmd"

ATCmdManager::ATCmdManager(PinName tx, PinName rx, SMDevicePeripheral *blePeripheral, 
                           events::EventQueue &event_queue, WiFiManager *wifi, 
                           MemoryPool<wifi_cmd_message_t, 16> *aT2WiFimPool, 
                           Queue<wifi_cmd_message_t, 16>      *aT2WiFiCmdQueue, 
                           MemoryPool<at_resp_message_t, 16>  *wiFi2ATmPool, 
                           Queue<at_resp_message_t, 16>       *wiFi2ATCmdQueue, 
                           MemoryPool<wifi_data_msg_t, PQDSZ>     *aT2WiFiDatamPool, 
                           Queue<wifi_data_msg_t, PQDSZ>          *aT2WiFiDataQueue, 
                           MemoryPool<at_data_msg_t, PQDSZ>       *wiFi2ATDatamPool, 
                           Queue<at_data_msg_t, PQDSZ>            *wiFi2ATDataQueue, 
                           bool debug)
    :
    _serial(tx, rx, DEFAULT_BAUD_RATE),
    blePeripheral(blePeripheral),
    _event_queue(event_queue),
    wiFiManager(wiFiManager),
    _aT2WiFimPool(aT2WiFimPool),
    _aT2WiFiCmdQueue(aT2WiFiCmdQueue),
    
    _wiFi2ATmPool(wiFi2ATmPool),
    _wiFi2ATCmdQueue(wiFi2ATCmdQueue),
    
    _aT2WiFiDatamPool(aT2WiFiDatamPool),
    _aT2WiFiDataQueue(aT2WiFiDataQueue),
    
    _wiFi2ATDatamPool(wiFi2ATDatamPool),
    _wiFi2ATDataQueue(wiFi2ATDataQueue),
    
    _parser(&_serial)
    

{ 
// constructor
    _serial.set_baud(2*DEFAULT_BAUD_RATE);
    //Flow flowCtrl = Flow::Disabled;
    //_serial.set_flow_control(mbed::SerialBase::Disabled);
    _parser.debug_on(debug);
    _parser.set_delimiter("\r\n");
    _parser.send("+STARTUP\r\n\r\n");
    _parser.oob("AT\r", callback(this, &ATCmdManager::_oob_ok_hdlr));
    _parser.oob("ATE0", callback(this, &ATCmdManager::_oob_echo_off));
    _parser.oob("ATE1", callback(this, &ATCmdManager::_oob_echo_on));
    _parser.oob("AT+UMRS", callback(this, &ATCmdManager::_oob_uart_setup));
    
    _parser.oob("ATO", callback(this, &ATCmdManager::_oob_data_mode));
    _parser.oob("AT+UMLA", callback(this, &ATCmdManager::_oob_get_mac_addr));
    _parser.oob("AT+UBTLE?", callback(this, &ATCmdManager::_oob_get_ble_role));
    _parser.oob("AT+UBTLE=2", callback(this, &ATCmdManager::_oob_ena_ble_peri));
    _parser.oob("AT+CPWROFF", callback(this, &ATCmdManager::_oob_reboot));
    _parser.oob("AT+CGMR", callback(this, &ATCmdManager::_oob_get_fw_ver));
    _parser.oob("AT+UWSCAN", callback(this, &ATCmdManager::_oob_scanWiFiNetworks));
    _parser.oob("AT+UWSCA=", callback(this, &ATCmdManager::_oob_WiFiStationConfigAction));
    
    _parser.oob("AT+UMLA=", callback(this, &ATCmdManager::_oob_ok_hdlr));
    _parser.oob("AT+UBTLN", callback(this, &ATCmdManager::_oob_ok_hdlr));
    _parser.oob("AT+UBTSM?", callback(this, &ATCmdManager::_oob_ok_hdlr));
    _parser.oob("AT+UBTPM", callback(this, &ATCmdManager::_oob_ok_hdlr));
    
    _parser.oob("AT+UDSC=", callback(this, &ATCmdManager::_oob_ok_hdlr));
    _parser.oob("AT&W", callback(this, &ATCmdManager::_oob_ok_hdlr));
    //_parser.oob("AT+UBTPM", callback(this, &ATCmdManager::_oob_ok_hdlr));
    //_parser.oob("AT+UBTPM", callback(this, &ATCmdManager::_oob_ok_hdlr));
    //_parser.oob("AT+UWSCD=", callback(this, &ATCmdManager::_oob_disconnectWiFiNetwork));
    _parser.oob("AT+UDDRP", callback(this, &ATCmdManager::_oob_setupInternetConnection));
    _parser.oob("AT+UWSC=0,0,0", callback(this, &ATCmdManager::_oob_ok_hdlr));
    _parser.oob("AT+UWSC=0,2", callback(this, &ATCmdManager::_oob_setWiFiSSID));
    _parser.oob("AT+UWSC=0,8", callback(this, &ATCmdManager::_oob_setWiFiPWD));
    _parser.oob("AT+UWSC=0,5", callback(this, &ATCmdManager::_oob_setWiFiSecurity));
    _parser.oob("AT+UNSTAT=", callback(this, &ATCmdManager::_oob_getNetworkStatus));
    _parser.oob("AT+UWSSTAT", callback(this, &ATCmdManager::_oob_WiFiNetworkStatus));
    
    //_parser.oob("AT+UWSC=0,5", callback(this, &ATCmdManager::_oob_sendHttpMessage));
    //sendAtConfirmation("Testing:: +UBTLE:2\r\nOK\r\n");
    dbg_printf(LOG, "\n --- ATCmdManager constructor completed ---\n");
    at_resp = AT_RESP_NONE;
    dataMode = AT_CMD_DATA_MODE;
    debug_flag = 0;
    wifiStateControl = AT_RESP_NONE;
    //_event_queue.call_in(10, &print_heap_and_isr_stack_info);
    //print_heap_and_isr_stack_info();
    //_event_queue.call_every(3600000,&print_memory_info);
    _event_queue.call_every(10000,&blinkLEDs);
#ifdef BOX_UBLOX_DEMO_TESTING
    check_for_at_cmd = false;
#endif
}


void ATCmdManager::runMain(){
    dbg_printf(LOG, "\r\n [ATCMD MAN]  Thread Id = %X\r\n", (uint32_t)ThisThread::get_id());
    while(true){
        _process_oob(UBLOX_ODIN_W2_RECV_TIMEOUT, true);
        wait_ms(MAIN_LOOP_WAIT_TIME_MS); // allow BTLE/WiFi some time 
        processResponses();
    }
    
}

void ATCmdManager::processResponses(){
        dequeueATresponse();
        dequeueWiFidataResponse();
        updateWiFiMgrStatus();
        char * respStr;
        //if(at_resp > AT_COMMAND_FAILED)
        //return;
        switch(at_resp){
            case AT_RESP_NONE:
                // IDLE response state
                break;
            case AT_SCAN_RESP:
                // AT_SCAN_RESP response state
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI SCAN RESPONSE RECEIVED!!\r\n");
                //_smutex.lock();
                //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                respStr = (char *) resp_data->buffer;
                sendAtConfirmationFreeMpool(respStr);
                at_resp = AT_RESP_NONE;
                break;
            case AT_DETAILED_SCAN_RESP:
                // AT_DETAILED_SCAN_RESP response state
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI DETAILED SCAN RESPONSE RECEIVED!!\r\n");
                //_smutex.lock();
                //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                respStr = (char *) resp_data->buffer;
                sendAtConfirmationFreeMpool(respStr);
                at_resp = AT_RESP_NONE;
                break;
            case AT_CONNECT_RESP:  
            {
                // AT_CONNECT_RESP response state 
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI CONNECT RESPONSE RECEIVED!!\r\n");
                //_smutex.lock();
                sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                respStr = (char *) resp_data->buffer;
                sendAtConfirmationFreeMpool(respStr);
                char * urc = new char[20];
                sprintf(urc, "\r\n%s:%d\r\n", NETWORK_UP_URC, WIFI_INTERFACE_ID);
                sendAtEvent(urc);
                delete urc;
                //_smutex.unlock();
                at_resp = AT_RESP_NONE;
                break;
            }
            case AT_DISCONNECT_RESP:  
                // AT_DISCONNECT_RESP response state 
                //////_smutex.lock();
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI DISCONNECT RESPONSE RECEIVED!!\r\n");
                //_smutex.lock();
                //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                respStr = (char *) resp_data->buffer;
                sendAtConfirmationFreeMpool(respStr);
                at_resp = AT_RESP_NONE;
                break;
            case AT_CONFIG_RESP:  
                // AT_CONFIG_RESP response state 
                //////_smutex.lock();
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI CONFIG RESPONSE RECEIVED!!\r\n");
                //_smutex.lock();
                sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                at_resp = AT_RESP_NONE;
                break;
            case AT_NETWORK_STATUS_RESP:  
                // AT_CONFIG_RESP response state 
                //////_smutex.lock();
                dbg_printf(LOG, "\n [ATCMD MAN] NETWORK STATUS RESPONSE RECEIVED!!\r\n");
                respStr = (char *) resp_data->buffer;
                sendAtConfirmationFreeMpool(respStr);
                at_resp = AT_RESP_NONE;
                break;
            case AT_WIFI_STATUS_RESP:  
                // AT_CONFIG_RESP response state 
                //////_smutex.lock();
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI RESPONSE RECEIVED!!\r\n");
                respStr = (char *) resp_data->buffer;
                sendAtConfirmationFreeMpool(respStr);
                at_resp = AT_RESP_NONE;
                break;
            case AT_INTERNET_CONFIG_RESP:  
                // AT_CONFIG_RESP response state 
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI INTERNET_CONFIG RESPONSE RECEIVED!!\r\n");
                //_smutex.lock();
                sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                //respStr = (char *) resp_data->buffer;
                //sendAtConfirmationFreeMpool(respStr);
                at_resp = AT_RESP_NONE;
                break;
            case AT_HTTPS_RESP:  
                // AT_HTTP_RESP response state 
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI HTTPS RESPONSE RECEIVED!!\r\n");
                return_response();
                //_smutex.lock();
                sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                at_resp = AT_RESP_NONE;
                break;
            case AT_HTTPS_RESP_DOWNLOAD:  
                // AT_HTTPS_RESP_DOWNLOAD response state 
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI HTTPS DOWNLOAD RESPONSE RECEIVED!!\r\n");
                return_response(true); // set download paramter to true
                //_smutex.lock();
                //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                at_resp = AT_RESP_NONE;
                break;
            case AT_HTTP_RESP:  
                // AT_HTTP_RESP response state 
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI HTTP RESPONSE RECEIVED!!\r\n");
                return_response(); 
                //_smutex.lock();
                //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                at_resp = AT_RESP_NONE;
                break;
            case AT_HTTP_RESP_DOWNLOAD:  
                // AT_HTTP_RESP response state 
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI HTTP RESPONSE RECEIVED!!\r\n");
                return_response(true); // set download paramter to true
                //_smutex.lock();
                //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                at_resp = AT_RESP_NONE;
                break;
            case CONNECT_EVENT:  
            {
                // Connect Event state 
                dbg_printf(LOG, "\n [ATCMD MAN] CONNECT EVENT RECEIVED!!\r\n");
                sendConnectEvent(resp_data->buffer, resp_data->dataLen);
                at_resp = AT_RESP_NONE;
                break;
            }
            case AT_EVENT:  
            {
                // AT Event state 
                dbg_printf(LOG, "\n [ATCMD MAN] AT EVENT RECEIVED!!\r\n");
                respStr = (char *) resp_data->buffer;
                sendAtEvent(respStr);
                at_resp = AT_RESP_NONE;
                break;
            }
            case WIFI_WATCH_DOG:  
            {
                // AT_HTTP_RESP response state 
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI Watchdog message!!\r\n");
                //_smutex.lock();
                //_parser.send("\r\nNACK\r\n");
                //_smutex.unlock();
                respStr = (char *) resp_data->buffer;
                sendConnectIndicationFreeMpool(respStr);
                at_resp = AT_RESP_NONE;
                break;
            }
            case AT_COMMAND_FAILED:  
            {
                // AT_HTTP_RESP response state 
                dbg_printf(LOG, "\n [ATCMD MAN] WIFI COMMAND FAILED!!\r\n");
                //_smutex.lock();
                //_parser.send("\r\nNACK\r\n");
                //_smutex.unlock();
                respStr = (char *) resp_data->buffer;
                sendAtConfirmationFreeMpool(respStr);
                at_resp = AT_RESP_NONE;
                break;
            }
            default:
                 //UNKNOWN response state
                //_smutex.lock();
                dbg_printf(LOG, "\n [ATCMD MAN] UNKNOWN RESPONSE RECEIVED!!\r\n");
                sendAtConfirmationFreeMpool(ERROR_RESP);   //_parser.send(OK_RESP);
                //_smutex.unlock();
                at_resp = AT_RESP_NONE;
                break;
        }
}


void ATCmdManager::updateWiFiMgrStatus()
{
    static int wifiBusyMonitor = 0;
    // if next response is the expected response, 
    // then last command complete, reset wifi status
    // pass or fail!!
    if(at_resp == wifiStateControl || at_resp == AT_COMMAND_FAILED)
    {
        wifiStateControl = AT_RESP_NONE;
    }
    if(at_resp == WIFI_WATCH_DOG && strstr((char *) resp_data->buffer, "WiFi Main Loop inActive") != NULL)
    {
        wifiBusyMonitor++;
        if(wifiBusyMonitor >= 4)
        {
            wifiStateControl = AT_RESP_NONE;
            wifiBusyMonitor = 0;
        }
    }
}
void ATCmdManager::sendConnectIndicationFreeMpool(const char *buf)
{
    int len =  strlen(buf);
    outputEDMdata((const uint8_t *) buf, len, AT_MSG_ID, CONFIRMATION_MSG_TYPE, NO_CHANNEL);
    _wiFi2ATDatamPool->free(resp_data);
    resp_data = NULL;
}

void ATCmdManager::sendAtConfirmationFreeMpool(const char *buf)
{
    sendAtConfirmation(buf);
    _wiFi2ATDatamPool->free(resp_data);
    resp_data = NULL;
}


void ATCmdManager::printBufferInHex(const uint8_t *buf, int pLen)
{
    print_debug_hex(buf, pLen);
}

bool ATCmdManager::validate(edm_header_t edm_header)
{
    if(edm_header.startByte != EDM_START_BYTE)return false; // unexpected start byte found!
    if(edm_header.payloadID != CONNECT_EVENT_ID &&
       edm_header.payloadID != DISCONNECT_EVENT_ID &&
       edm_header.payloadID != DATA_EVENT_ID &&
       edm_header.payloadID != DATA_COMMAND_ID &&
       edm_header.payloadID != AT_REQUEST_ID &&
       edm_header.payloadID != AT_CONFIRMATION_ID &&
       edm_header.payloadID != AT_EVENT_ID 
       )return false; // unexpected payload ID found!
    if(edm_header.payloadLen > MAX_EDM_PAYLOAD_LEN ||
       edm_header.payloadLen < MIN_EDM_PAYLOAD_LEN 
       ) return false; // unexpected length received!
    if(edm_header.channel_id != WIFI_CHANNEL &&
       edm_header.channel_id != BLE_CHANNEL 
       ) return false; // unexpected channel_id received!
    return true;
}

http_method ATCmdManager::str2HttpMethod(const char * methodStr)
{
    http_method _http_method;
    if(strstr(methodStr, "POST")!= NULL){
        _http_method = HTTP_POST;
    }
    else if(strstr(methodStr, "GET")!= NULL){
        _http_method = HTTP_GET;
    }
    else{
        _http_method = HTTP_DELETE; // unsupported method - set to HTTP_DELETE
    }
    return _http_method;
}
bool ATCmdManager::createHttpRequest()
{
    static int wifiBusyTimeOut = 0;
    if(wifiStateControl != AT_RESP_NONE && wifiBusyTimeOut < 10) // wifi busy!
    {
        wifiBusyTimeOut++;
        return false;
    }
    wifiBusyTimeOut = 0;
    http_request_t *http_req; // = new http_request_t;
    wifi_data_msg_t data_msg;
    http_req = (http_request_t *)data_msg.buffer;
    char s1[32];
    char s2[32];
    char s3[32];
    int len = 0;
    int n;
    char * strPtr = (char *)rx_buf_ptr;
    char * p;
    char * p2 = strstr(strPtr, "\r\n\r\n");
    char * nxtPtr = strPtr;
    char * outPtr;
    char * bodyPtr = p2+4;
    dbg_printf(LOG, "\nstrPtr address= %x",strPtr);
    dbg_printf(LOG, "\np2 address= %x", p2);
    for(int i = 0; i < 5; i++){
        if(i == 0)// firstline scan method uri and http_ver
        {
            n = sscanf(nxtPtr,"%s %s %s", s1, s2, s3);
            if(n!=3) return false; // error in input abort
#ifdef FULL_DEBUG_ENABLED
            dbg_printf(LOG, "\nmethod = %s\nuri = %s\nhttp_ver = %s",s1, s2, s3 );
#endif    
            //http_req.method = str2HttpMethod(s1.c_str());
            http_req->method = str2HttpMethod(s1);
            http_req->request_URI = (char *) http_req->buffer; // point 1st string to start of buffer
            len = strlen(s2)+1;
            strncpy(http_req->request_URI, s2, len);
            http_req->http_version = http_req->request_URI + len; // point 2nd string to after 1st
            len = strlen(s3)+1;
            strncpy(http_req->http_version, s3, len);
#ifdef FULL_DEBUG_ENABLED
            dbg_printf(LOG, "\nhttp_request 1st line:\n method = %d\nuri = %s\nhttp_ver = %s",http_req->method, 
                                                                      http_req->request_URI, 
                                                                      http_req->http_version );
            dbg_printf(LOG, "\nhttp_request str ptrs\nuri = %X\nhttp_ver = %X",http_req->request_URI, 
                                                                      http_req->http_version );
#endif
            outPtr = http_req->http_version + len; // point output buffer ptr to after 2nd string
        }
        else{ // scan header pairs
            //n = sscanf(nxtPtr,"%s %s", &s1[0], &s2[0]);
            n = sscanf(nxtPtr,"%s %s", s1, s2);
            if(n!=2) return false; // error in input abort
            p = strstr(s1,":");
            *p = NULL;
#ifdef FULL_DEBUG_ENABLED
            dbg_printf(LOG, "\nname = %s value = %s",s1, s2);
#endif
            if(strstr(s1, "Host")!= NULL){
                http_req->hostName = outPtr;
#ifdef BOX_UBLOX_DEMO_TESTING
                p = strstr(s2, "//");
                if(p == NULL)
                 p = s2;
                else
                 p+=2;
                len = strlen(p)+1;                
                strncpy(outPtr, p, len);
#else
                len = strlen(s2)+1;                
                strncpy(outPtr, s2, len);
#endif
#ifdef FULL_DEBUG_ENABLED
                dbg_printf(LOG, "\nname = %s value = %s",s1, outPtr);
#endif
                outPtr += len; // point output buffer ptr to after current string
            }
            else if(strstr(s1, "Accept")!= NULL){
                http_req->AcceptVal = outPtr;
                len = strlen(s2)+1;                
                strncpy(outPtr, s2, len);
#ifdef FULL_DEBUG_ENABLED
                dbg_printf(LOG, "\nname = %s value = %s",s1, outPtr);
#endif
                outPtr += len; // point output buffer ptr to after current string
            }
            else if(strstr(s1, "Content-Type")!= NULL){
                http_req->contentType = outPtr;
                len = strlen(s2)+1;                
                strncpy(outPtr, s2, len);
#ifdef FULL_DEBUG_ENABLED
                dbg_printf(LOG, "\nname = %s value = %s",s1, outPtr);
#endif
                outPtr += len; // point output buffer ptr to after current string
            }
            else if(strstr(s1, "Content-Length")!= NULL){
                //http_req.contentLen.assign(s2);
                http_req->contentLen = outPtr;
                len = strlen(s2)+1;                
                strncpy(outPtr, s2, len);
#ifdef FULL_DEBUG_ENABLED
                dbg_printf(LOG, "\nname = %s value = %s",s1, outPtr);
#endif
                outPtr += len; // point output buffer ptr to after current string
            }
            
#ifdef FULL_DEBUG_ENABLED
            dbg_printf(LOG, "\noutPtr = %X len = %d\n", outPtr, len);
#endif
        }
        nxtPtr = strstr(nxtPtr, "\r\n")+2; // goto next line
        if(nxtPtr >= p2) break;
    }
    // print header from http_req_struct
#ifdef FULL_DEBUG_ENABLED
    dbg_printf(LOG, "\nhttp request header: \n %s\n", http_req->buffer);
#endif
    int bodyLen = edm_hdr.payloadLen -(p2+7-strPtr);
#ifdef FULL_DEBUG_ENABLED
    dbg_printf(LOG, "\nLen = %d\n", bodyLen);
#endif
    http_req->body = (uint8_t *) outPtr;
    memcpy(outPtr, bodyPtr, bodyLen);
    if(bodyLen > 10){
        dbg_printf(LOG, "\n Message Body:\n");
        printBufferInHex(http_req->body, bodyLen);
    }
    outPtr += bodyLen; // move output pointer to end output (header + body)
    // package and send on wifi data queue
    data_msg.wifi_cmd = WIFI_CMD_SEND_HTTPS_REQ;
    data_msg.dataLen = (uint8_t *)outPtr - http_req->buffer;//sizeof(http_request_t);
#ifdef FULL_DEBUG_ENABLED
    dbg_printf(LOG, "\nsizeof(http_req) on population = %d\n", sizeof(*http_req));
    dbg_printf(LOG, "\ndata_msg.dataLen = %d\n", data_msg.dataLen);
#endif
    // queue next data request
    if(queueWiFiDataRequest(data_msg) == true)
    {
        wifiStateControl = AT_HTTPS_RESP_DOWNLOAD;
    }
    return true;
}
// OOB processing
void ATCmdManager::_process_oob(uint32_t timeout, bool all){
    set_timeout(timeout);
    //int cnt = 0;
    int start;
    //channel_id_t chan_id;
    if(dataMode == AT_EXT_DATA_MODE)
    {
        int n;
        //if(cnt++ % 10 == 0)dbg_printf(LOG, "In EDM mode\n");
        uint8_t edm[EDM_HDR_LEN];
        char cmdType[16];
        // Poll for edm packets
        do{
            n = _parser.read((char *)edm, EDM_HDR_LEN-1);
            edm_hdr.startByte = edm[0];
            edm_hdr.payloadLen = edm[1]*256 + edm[2];
            edm_hdr.payloadID = edm[3]*256 + edm[4];
            edm_hdr.channel_id = (channel_id_t) 0;
            start = Kernel::get_ms_count();
            if(n == -1) break; // break if it times out
            int pT;
            if(edm_hdr.payloadID < AT_REQUEST_ID) //
            {
                n += _parser.read((char *)edm, 1);
                edm_hdr.channel_id = (channel_id_t) edm[0];
                pT = 0;
                strcpy(cmdType, "DATA COMMAND");
            }
            else
            {
                pT = 1;
                strcpy(cmdType, "AT REQUEST");
            }
            //dbg_printf(LOG, "%d bytes read! : type = %s\n", n, cmdType2str(edm_hdr.payloadID));
            dbg_printf(LOG, "%d bytes read! : CMD type = %s\n", n, cmdType);
            if(n==(EDM_HDR_LEN-pT))
            dbg_printf(LOG, "Start = %d, payloadID = %d len = %d chan_id = %d\n", edm_hdr.startByte, 
                                                                         edm_hdr.payloadID, 
                                                                         edm_hdr.payloadLen,
                                                                         edm_hdr.channel_id);
            if(n == (EDM_HDR_LEN-pT) && validate(edm_hdr)) // if AT command use process oob to decode
            {
                if(edm_hdr.payloadID == AT_REQUEST_ID)
                {
                    //_parser.process_oob();
                    while (_parser.process_oob() && all) {
                        if(debug_flag>0){
                            dbg_printf(LOG, "finished queuing WIFI CONNECT CMD -- back to process_oob loop \r\n");
                        }
                    }
                    break;
                }
                else
                {
                    int pLen = edm_hdr.payloadLen-2-pT;
                    rx_buf_ptr = (uint8_t *) malloc (pLen); // we already read 2 bytes from payload but expect 1 stop byte
                    rx_buf_ptr[pLen-1] = 0x00; // clear last byte so the readback value is as expected
                    n = _parser.read((char *)rx_buf_ptr, pLen);
                    if(n == -1) break; // timeout!
                    dbg_printf(LOG, "%d bytes read - expected %d!\n", n, pLen);
                    printBufferInHex(rx_buf_ptr, pLen);
                    dbg_printf(LOG, "rx_buf_ptr[pLen-1] = %0x\n",rx_buf_ptr[pLen-1]);
                    if(rx_buf_ptr[pLen-1] != EDM_STOP_BYTE) {
                        _smutex.lock();
                        _parser.send("ERR");
                        _smutex.unlock();
                        free(rx_buf_ptr); // make sure to free buffer
                        rx_buf_ptr = NULL;
                        break; // exit if stop byte not found - possible data corruption!
                    }
                    switch(edm_hdr.payloadID)
                    {
                        case CONNECT_EVENT_ID:
                            dbg_printf(LOG, "Connection Event received!\n");
                            break;
                        case DISCONNECT_EVENT_ID:
                            dbg_printf(LOG, "DISCONNECT_EVENT received!\n");
                            break;
                        case DATA_EVENT_ID:
                            dbg_printf(LOG, "DATA_EVENT received!\n");
                            break;
                        case DATA_COMMAND_ID:
                        {
                            dbg_printf(LOG, "DATA_COMMAND received!\n");
                            //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
                            if(createHttpRequest() == false)
                            {
#ifdef SEND_DEBUG_MESSAGES
                                sendAtConfirmation(WIFI_BUSY_RESP);
#endif
                            }
                            free(rx_buf_ptr);
                            rx_buf_ptr = NULL;
                            int stop = Kernel::get_ms_count();
                            dbg_printf(LOG, "\n Time Elapsed = %d\n", stop-start);
                            break;
                        }
                        case AT_REQUEST_ID:
                            dbg_printf(LOG, "AT_REQUEST received!\n");
                            break;
                        case AT_CONFIRMATION_ID:
                            dbg_printf(LOG, "AT_CONFIRMATION received!\n");
                            break;
                        case AT_EVENT_ID:
                            dbg_printf(LOG, "AT_EVENT received!\n");
                            break;
                        default:
                            dbg_printf(LOG, "UNKNOWN MESSAGE received!\n");
                            break;
                    }
                }
                //_parser.process_oob();
            }
            else // incorrect # of bytes received abort!!
            {
                break;
            }
        }while (all);  // continue to process until timeout
    }
    else
    {
        // Poll for inbound packets
        static int msgCnt = 0;
        static char * ATCMD = new char[32];
        int n = 0;
#ifdef BOX_UBLOX_DEMO_TESTING
        if(check_for_at_cmd)
        {
            while (n>=0 && all) {
                n = readAtCommandString(ATCMD, 32);
                if(strstr(ATCMD,"AT\r") != NULL)
                {
                    _oob_ok_hdlr();
                    check_for_at_cmd = false;
                    break;
                }
            }
        }
        else
#endif
        {
        while (_parser.process_oob() && all) {
                        dbg_printf(LOG, "inbound message found... msgCnt = %d \r\n", ++msgCnt);
                        if(debug_flag>0){
                            dbg_printf(LOG, "finished queuing WIFI CONNECT CMD -- back to process_oob loop \r\n");
                        }
        }
        }
    }        
    set_timeout();
}


int ATCmdManager::readAtCommandString(char *strbuf, size_t bufLen)
{
    int n;
    bool atstr_found = false;
    int i = 0;
    while(n != -1)
    {
        char c;
        n = _parser.read(&c, 1);
        if(n)
        strbuf[i++] = c;
        if(c=='\r')
        {
            strbuf[i++] = NULL; // terminate string and exit
            atstr_found = true;
            break;
        }
    }
    if(atstr_found)
    {
        return i;
    }
    else
    {
        return -1;
    }
}

// OOB message handlers
void ATCmdManager::_oob_startup_hdlr(){
}

void ATCmdManager::_oob_ok_hdlr(){
    //_smutex.lock();
    sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    //_smutex.unlock();
}


void ATCmdManager::_oob_bleRole_hdlr(){
}


void ATCmdManager::_oob_wifiMode_err(){
}


void ATCmdManager::_oob_conn_already(){
}


void ATCmdManager::_oob_err(){
}

void ATCmdManager::_oob_get_fw_ver()
{
#ifdef MBED_MAJOR_VERSION
    char * fw_ver_str = new char[20];
    sprintf(fw_ver_str, "Mbed OS version %d.%d.%d.0\n\n", MBED_MAJOR_VERSION, MBED_MINOR_VERSION, MBED_PATCH_VERSION);
    sendAtConfirmation(fw_ver_str);
    delete fw_ver_str;
#endif
}

#define TTP_DEBUGGING
#define TTP_DEBUGGING_UART_NOCHANGE
void ATCmdManager::_oob_uart_setup(){
    int uOpts[NUM_UART_OPTIONS];
    //if(_parser.recv("=%d,%d,%d,%d,%d,%d", &uOpts[0], &uOpts[1], &uOpts[2], &uOpts[3], &uOpts[4], &uOpts[5])) {
    if(_parser.scanf("=%d,%d,%d,%d,%d,%d", &uOpts[0], &uOpts[1], &uOpts[2], &uOpts[3], &uOpts[4], &uOpts[5]) >0) {
        dbg_printf(LOG, "\nATCmdParser: Uart Options=%d,%d,%d,%d,%d,%d\n", uOpts[0], uOpts[1], uOpts[2], uOpts[3], uOpts[4], uOpts[5]);
        //printf("\nATCmdParser: Uart Options=%d,%d,%d,%d,%d,%d\n", uOpts[0], uOpts[1], uOpts[2], uOpts[3], uOpts[4], uOpts[5]);
       //AT+UMRS=230400,2,8,1,1,1
        dbg_printf(LOG, "\n Changing Baud Rate to %d\n", uOpts[0]);
#ifdef TTP_DEBUGGING_UART_NOCHANGE
    sendAtConfirmation(OK_RESP);
#else

        sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
        //wait(0.1);
        _serial.set_baud(uOpts[0]);
        wait(0.1);
        dbg_printf(LOG, "\n Baud Rate now %d\n", uOpts[0]);
#endif
#ifdef BOX_UBLOX_DEMO_TESTING
    check_for_at_cmd = true;
#endif

    } else {
        dbg_printf(LOG, "\nATCmdParser: Retrieving Uart Options failed");
    }
}

void ATCmdManager::set_timeout(uint32_t timeout_ms)
{
    _parser.set_timeout(timeout_ms);
}


void ATCmdManager::_oob_echo_off()
{
    //_smutex.lock();
    dbg_printf(LOG, "\n Received ATEO OOB command!!\n");
    dbg_printf(LOG, "\n turning echo OFF!!\n");
    //printf("\n turning echo OFF!!\n");
    _parser.debug_on(false);
    sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    //_smutex.unlock();
}


void ATCmdManager::_oob_echo_on()
{
    //_smutex.lock();
    dbg_printf(LOG, "\n Received ATE1 OOB command!!\n");
    dbg_printf(LOG, "\n turning echo ON!!\n");
    _parser.debug_on(true);
    sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    //_smutex.unlock();
}


void ATCmdManager::_oob_data_mode(){
    //_smutex.lock();
    dbg_printf(LOG, "\n Received EDM mode command!!\n");
    int dmode;
    if(_parser.scanf("%d", &dmode) >0) {
        dbg_printf(LOG, "\nATCmdParser: Data mode=%d\r\n", dataMode);
        sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
        switch(dmode)
        {
            case 0:
                dbg_printf(LOG, "\nATCmdParser: Command Mode request received\n");
                dataMode = AT_CMD_DATA_MODE;
                break;
            case 1:
                dbg_printf(LOG, "\nATCmdParser: Data Mode request received\n");
                dataMode = AT_STD_DATA_MODE;
                break;
            case 2:
                dbg_printf(LOG, "\nATCmdParser: Extended data Mode request received\n");
                dataMode = AT_EXT_DATA_MODE;
                _event_queue.call_in(10, &print_heap_and_isr_stack_info);
                //print_heap_and_isr_stack_info();
                break;
            default:
                dbg_printf(LOG, "\nATCmdParser: ERROR - UNKNOWN DATA MODE RECEIVED!!! \n");
                break;
        }
    } else {
        dbg_printf(LOG, "\nATCmdParser: Retrieving Uart Options failed\n");
    }
    //_smutex.unlock();
}

void ATCmdManager::_oob_get_mac_addr(){
    int bleOrWifi;
    //_smutex.lock();
    if(_parser.scanf("=%d", &bleOrWifi) >0) {
        switch(bleOrWifi)
        {
            case 1:
                dbg_printf(LOG, "\nATCmdParser: BLE MAC Address request received");
                break;
            case 2:
                dbg_printf(LOG, "\nATCmdParser: WiFi MAC Address request received");
                break;
            default:
                dbg_printf(LOG, "\nATCmdParser: ERROR - UNKNOWN MAC ADDRESS REQUEST RECEIVED!!! \n");
                break;
        }
    } else {
        dbg_printf(LOG, "\nATCmdParser: Retrieving Uart Options failed");
    }
    sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    //_smutex.unlock();
}

void ATCmdManager::_oob_get_ble_role(){
    //_smutex.lock();
    dbg_printf(LOG, "\n Received get BLE role command!!\n");
    //printf("\n Received get BLE role command!!\n");
    sendAtConfirmation("+UBTLE:2\r\nOK\r\n");   //_parser.send(OK_RESP);
    //_smutex.unlock();
}

void ATCmdManager::_oob_ena_ble_peri(){
    //_smutex.lock();
    dbg_printf(LOG, "\n Received enable BLE Peripheral command!!\n");
    sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    //_smutex.unlock();
}

void ATCmdManager::_oob_reboot(){
    //_smutex.lock();
    dbg_printf(LOG, "\n Received reboot command!!\n");
    sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    //_parser.send("System Resetting....\n");
    system_reset();
    //_smutex.unlock();
}

const char * ATCmdManager::sec2str(nsapi_security_t sec)
{
    switch (sec) {
        case NSAPI_SECURITY_NONE:
            return "None";
        case NSAPI_SECURITY_WEP:
            return "WEP";
        case NSAPI_SECURITY_WPA:
            return "WPA";
        case NSAPI_SECURITY_WPA2:
            return "WPA2";
        case NSAPI_SECURITY_WPA_WPA2:
            return "WPA/WPA2";
        case NSAPI_SECURITY_UNKNOWN:
        default:
            return "Unknown";
    }
}

bool ATCmdManager::setNextResponse(at_cmd_resp_t resp)
{
     if(at_resp == AT_RESP_NONE){
        at_resp = resp;
        return true; // success
    }
    return false; // wiFiManager busy
}


void ATCmdManager::_oob_scanWiFiNetworks(){
    //_smutex.lock();
    dbg_printf(LOG, "\n Received scanWiFiNetworks command!!\n");
    sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    //_smutex.unlock();
    wifi_cmd_t cmd = WIFI_CMD_SCAN;
    // queue next command
    queueWiFiCommand(cmd);
    return;    
}



void ATCmdManager::_oob_WiFiStationConfigAction()
{
    int if_id; // interface id for request
    int aId; // interface id for request
    _parser.recv("%d,%d", &if_id, &aId);
    //_smutex.lock();
    dbg_printf(LOG, "\n Received WiFi Configuration Action command %d %d!!\r\n", if_id, aId);
    //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    if(if_id == WIFI_CONFIG_ID && aId <5){
        wifi_cmd_t cmd;
        action_id_t action_id = (action_id_t) aId;
        switch(action_id){
            case WIFI_CONFIG_RESET:
                break;
            case WIFI_CONFIG_STORE:
                break;
            case WIFI_CONFIG_LOAD:
                break;
            case WIFI_CONFIG_ACTIVATE:
                cmd = WIFI_CMD_CONNECT;
                dbg_printf(LOG, "\n About to Queue wifi cmd = %d!!\n", cmd);
                //_smutex.unlock();
                // queue next command
                queueWiFiCommand(cmd);
                break;
            case WIFI_CONFIG_DEACTIVATE:
                cmd = WIFI_CMD_DISCONNECT;
                dbg_printf(LOG, "\n About to Queue wifi cmd = %d!!\n", cmd);
                //_smutex.unlock();
                // queue next command
                queueWiFiCommand(cmd);
                break;
            default:
                break;
        }
    }
    if(debug_flag>0){
        dbg_printf(LOG, "finished queuing WIFI CONNECT CMD \r\n");
    }
    return;
}


void ATCmdManager::_oob_disconnectWiFiNetwork()
{
    //_smutex.lock();
    dbg_printf(LOG, "\n Received WiFi Disconnect command!!\n");
    sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    wifi_cmd_t cmd = WIFI_CMD_DISCONNECT;
    //_smutex.unlock();
    // queue next command
    queueWiFiCommand(cmd);
    return;
}

void ATCmdManager::_oob_setupInternetConnection()
{
    char str[MAX_URL_LEN];
    char url[MAX_URL_LEN];
    int n;
    internet_config_t internet_config;
    //_smutex.lock();
    dbg_printf(LOG, "sizeof internet_config_t = %d\n", sizeof(internet_config_t));
    n = ReadBytes((uint8_t *)str, MAX_URL_LEN);
    str[n]=NULL;
    dbg_printf(LOG, "\n read string = %s , n = %d\n", str, n);
    //n = sscanf(str, "=%1d,%199[^,],%1d", &(uint8_t)internet_config.peer_id, 
    n = sscanf(str, "=%1d,%99[^,],%1d", &internet_config.peer_id, 
                                         url, //internet_config.url,    
                                        &internet_config.connectionScheme);
#ifdef BOX_UBLOX_DEMO_TESTING
    char *p = strstr(url,"/https:"); 
#else
    char *p = strstr(url,"\""); 
#endif
    if(p!=NULL)
    {
        strncpy(internet_config.url, &p[1], strlen(url));
        p = strstr(internet_config.url,"\"");
        *p = NULL;
    }
    else
    {
        strncpy(internet_config.url, url, strlen(url)+1);
    }
    dbg_printf(LOG, "\n read string = %s , n = %d -- strlen(url) = %d\n", internet_config.url, n, strlen(internet_config.url));
    if(n>0) 
    {
        dbg_printf(LOG, "peer_id = %1d, url = %s, connScheme = %1d\n", internet_config.peer_id, 
                                                              internet_config.url, 
                                                              internet_config.connectionScheme);
        // package and send on wifi data queue
        wifi_data_msg_t data_msg; 
        data_msg.wifi_cmd = WIFI_CMD_INTERNET_CONFIG;
        data_msg.dataLen = sizeof(internet_config_t); // + strlen(internet_config.url);
        dbg_printf(LOG, "\n url size = %d url str = %s\n",strlen(internet_config.url), internet_config.url );
        memcpy(data_msg.buffer,&internet_config, data_msg.dataLen);
        //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
        // queue next data request
        queueWiFiDataRequest(data_msg);
        print_memory_info();
    } else {
        dbg_printf(LOG, "\n[ATCMD MAN]: internet configuration failed %d fields parsed \r\n", n);
        _parser.send("NAK\n");
    }
    //_smutex.unlock();
}

wifi_config_t ATCmdManager::init_wifi_config()
{ 
    wifi_config_t wifi_cfg;
    wifi_cfg.ssid[0]  = NULL;
    wifi_cfg.pass[0]  = NULL;
    wifi_cfg.security = NSAPI_SECURITY_UNKNOWN;
    return wifi_cfg;    
}
/*  read ASCII characters into buffer and null terminate */
int ATCmdManager::readStringBytes(uint8_t *buf, int maxBytes)
{ 
    int c;
    int sptr = 0;
    int quoteCnt = 0;
    for(int i=0;i<maxBytes;i++){
        c = _parser.getc();
        if(c== '"')quoteCnt++;
        if(c==-1 || quoteCnt==2){
            buf[sptr] = NULL; // null terminate if string
            return i;
        }
        if(c != ',' && c!= '"'){
            buf[sptr++] = (uint8_t) c;
        }
    }
    return maxBytes;
}

int ATCmdManager::ReadBytes(uint8_t *buf, int maxBytes)
{ 
    int c;
    int sptr = 0;
    for(int i=0;i<maxBytes;i++){
        c = _parser.getc();
        if(c==-1){
            return i;
        }
        buf[sptr++] = (uint8_t) c;
    }
    return maxBytes;
}

void ATCmdManager::_oob_setWiFiSSID()
{
    int n;
    wifi_config_t wifi_cfg = init_wifi_config();
    //_smutex.lock();
    n = readStringBytes((uint8_t *)wifi_cfg.ssid, 32);
    dbg_printf(LOG, "[ATCMD MAN]: number of bytes read = %d\n", n);
    if(n>0) 
    {
        dbg_printf(LOG, "[ATCMD MAN]: wifi_cfg.ssid = %s\n", wifi_cfg.ssid);
        // package and send on wifi data queue
        wifi_data_msg_t data_msg;
        data_msg.wifi_cmd = WIFI_CMD_CONFIG;
        data_msg.dataLen = sizeof(wifi_config_t);
        memcpy(data_msg.buffer,&wifi_cfg, data_msg.dataLen);
        //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
        queueWiFiDataRequest(data_msg);
    } else {
        dbg_printf(LOG, "\n[ATCMD MAN]: wifi configuration failed  \n");
        _parser.send("NAK\n");
    }
    //_smutex.unlock();
}

void ATCmdManager::_oob_setWiFiPWD()
{
    int n;
    wifi_config_t wifi_cfg = init_wifi_config();
    //_smutex.lock();
    //n = _parser.scanf("%31[^\r\n]", wifi_cfg.pass);
    n = readStringBytes((uint8_t *)wifi_cfg.pass, 32);
    if(n>0) 
    {
        dbg_printf(LOG, "ATCMD MAN]: wifi_cfg.pass = %s\n", "****************");
        // package and send on wifi data queue
        wifi_data_msg_t data_msg;
        data_msg.wifi_cmd = WIFI_CMD_CONFIG;
        data_msg.dataLen = sizeof(wifi_config_t);
        memcpy(data_msg.buffer,&wifi_cfg, data_msg.dataLen);
        //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
        queueWiFiDataRequest(data_msg);
    } else {
        dbg_printf(LOG, "\n[ATCMD MAN]: wifi configuration failed  \n");
        _parser.send("NAK\n");
    }
    //_smutex.unlock();
}

void ATCmdManager::_oob_setWiFiSecurity()
{
    int n;
    wifi_config_t wifi_cfg = init_wifi_config();
    _smutex.lock();
    n = _parser.scanf(",%d", &wifi_cfg.security);
    _smutex.unlock();
    if(n>0) 
    {
        dbg_printf(LOG, "ATCMD MAN]: wifi_cfg.security = %s\n", sec2str(wifi_cfg.security));
        // package and send on wifi data queue
        wifi_data_msg_t data_msg;
        data_msg.wifi_cmd = WIFI_CMD_CONFIG;
        data_msg.dataLen = sizeof(wifi_config_t);
        memcpy(data_msg.buffer,&wifi_cfg, data_msg.dataLen);
        //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
        queueWiFiDataRequest(data_msg);
    } else {
        dbg_printf(LOG, "\n[ATCMD MAN]: wifi configuration failed  \n");
        _smutex.lock();
        _parser.send("NAK\n");
        _smutex.unlock();
    }
    //_smutex.unlock();
}



void ATCmdManager::_oob_getNetworkStatus()
{
    uint8_t if_id; // interface id for request
    _parser.scanf(",%d", &if_id);
    //_smutex.lock();
    dbg_printf(LOG, "\n Received Get Network Status command!!\n");
    //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    if(if_id == WIFI_INTERFACE_ID){
        wifi_cmd_t cmd = WIFI_CMD_NETWORK_STATUS;
        dbg_printf(LOG, "\n About to Queue wifi cmd = %d!!\n", cmd);
        //_smutex.unlock();
        // queue next command
        queueWiFiCommand(cmd);
    }
    return;
}


void ATCmdManager::_oob_WiFiNetworkStatus()
{
    //_smutex.lock();
    dbg_printf(LOG, "\n Received Get WiFi Network Status command!!\n");
    //sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    wifi_cmd_t cmd = WIFI_CMD_WIFI_STATUS;
    dbg_printf(LOG, "\n About to Queue wifi cmd = %d!!\n", cmd);
    //_smutex.unlock();
    // queue next command
    queueWiFiCommand(cmd);
    return;
}

void ATCmdManager::_oob_sendHttpMessage()
{
    int n;
    http_request_t http_post_request;
    //_smutex.lock();
    /*
    n = _parser.scanf(",%d", &wifi_cfg.security);
    if(n>0) 
    {
        dbg_printf(LOG, "ATCMD MAN]: wifi_cfg.security = %s\n", sec2str(wifi_cfg.security));
        // package and send on wifi data queue
        wifi_data_msg_t data_msg;
        data_msg.wifi_cmd = WIFI_CMD_CONFIG;
        data_msg.dataLen = sizeof(wifi_config_t);
        memcpy(data_msg.buffer,&wifi_cfg, data_msg.dataLen);
        queueWiFiDataRequest(data_msg);
        sendAtConfirmation(OK_RESP);   //_parser.send(OK_RESP);
    } else {
        dbg_printf(LOG, "\n[ATCMD MAN]: wifi configuration failed  \n");
        _parser.send("NAK\n");
    }
    */
    //_smutex.unlock();

}

bool  ATCmdManager::queueWiFiCommand(wifi_cmd_t cmd){
    dbg_printf(LOG, "[ATCMD MAN] about to be queued with wifi_cmd = %d\n", cmd);
#ifndef USE_MALLOC_FOR_COMMAND_MEMORY_POOL
    wifi_cmd_message_t *wifiCmd = _aT2WiFimPool->alloc();
    if(wifiCmd == NULL){
        dbg_printf(LOG, "[ATCMD MAN] queued memory allocation failed\n");
        return false;
    }
#else
    wifi_cmd_message_t *wifiCmd = (wifi_cmd_message_t *) malloc(sizeof(wifi_cmd_message_t));
    if(wifiCmd == NULL){
        dbg_printf(LOG, "[ATCMD MAN] try malloc() : queued memory allocation failed\n");
        //_event_queue.call_in(10, &print_memory_info);
        _event_queue.call_in(10, &print_heap_and_isr_stack_info);
        //print_memory_info();
        //print_heap_and_isr_stack_info();
        return false;
    }
#endif
    wifiCmd->wifi_cmd            = cmd;
    _aT2WiFiCmdQueue->put(wifiCmd);
    dbg_printf(LOG, "[ATCMD MAN] queued wifi_cmd = %d\n", wifiCmd->wifi_cmd);
    if((int)cmd == 5)
    debug_flag = 3;
    return true;
}

bool  ATCmdManager::dequeueATresponse(){
    if(debug_flag>0){
        debug_flag--;
        dbg_printf(LOG, "Called dequeueATresponse after WIFI CONNECT CMD \r\n");
    }
    if(at_resp != AT_RESP_NONE) return false; // busy
    osEvent evt = _wiFi2ATCmdQueue->get(0);
    if(evt.status == osEventMessage){
        at_resp_message_t *resp = (at_resp_message_t*)evt.value.p;
        setNextResponse(resp->at_resp);
        dbg_printf(LOG, "[ATCMD MAN] dequeued AT CMD : at_resp = %d\n", resp->at_resp);
#ifndef USE_MALLOC_FOR_COMMAND_MEMORY_POOL
        _wiFi2ATmPool->free(resp);
        resp = NULL;
#else
        free(resp);
        resp = NULL;
#endif
    }
    return true;
}

bool  ATCmdManager::queueWiFiDataRequest(wifi_data_msg_t data_req){
    static bool memFull = false;
    //print_memory_info();
    //_event_queue.call_in(10, &print_heap_and_isr_stack_info);
    //print_heap_and_isr_stack_info();
    wifi_data_msg_t *wifiData = _aT2WiFiDatamPool->alloc();
    if(wifiData == NULL)
    {
        //_parser.send("\r\nQUEUE MEMORY FULL\r\n");
#ifdef SEND_DEBUG_MESSAGES
        sendAtConfirmation("\r\nQUEUE MEMORY FULL\r\n");
#endif
        memFull = true;
        return false;
    }
    if(memFull)
    {
        memFull = false;
#ifdef SEND_DEBUG_MESSAGES
        sendAtConfirmation("\r\n[ATCMD-MAN] memory released...\r\n");
#endif
    }
    wifiData->wifi_cmd        = data_req.wifi_cmd;
    wifiData->dataLen        = data_req.dataLen;
    memcpy(wifiData->buffer, data_req.buffer, data_req.dataLen);
    _aT2WiFiDataQueue->put(wifiData);
    dbg_printf(LOG, "[ATCMD MAN] queued data size = %d : wifi_cmd = %d\n", data_req.dataLen, data_req.wifi_cmd);
    //free(&data_req);
    return true;
}

bool  ATCmdManager::dequeueWiFidataResponse(){
    if(debug_flag>0){
        debug_flag--;
        dbg_printf(LOG, "Called dequeueWiFidataResponse after WIFI CONNECT CMD \r\n");
    }
    if(at_resp != AT_RESP_NONE) return false; // busy
    osEvent evt = _wiFi2ATDataQueue->get(0);
    if(evt.status == osEventMessage){
        resp_data = (at_data_msg_t*)evt.value.p;
        setNextResponse(resp_data->at_resp);
        dbg_printf(LOG, "[ATCMD MAN] dequeued data size = %d : at_resp = %d\n", resp_data->dataLen, resp_data->at_resp);
        _event_queue.call_in(10, &print_heap_and_isr_stack_info);
        //print_heap_and_isr_stack_info();
    }
    return true;
}


void ATCmdManager::sendAtConfirmation(const char *buf)
{
    _smutex.lock();
    switch(dataMode){
        case AT_CMD_DATA_MODE:
            _parser.send("%s", buf);
            break;
        case AT_STD_DATA_MODE:
            _parser.send("%s", buf);
            break;
        case AT_EXT_DATA_MODE:
        {
            int len =  strlen(buf);
            outputEDMdata((const uint8_t *) buf, len, AT_MSG_ID, CONFIRMATION_MSG_TYPE, NO_CHANNEL);
            break;
        }
        default:
            _parser.send("%s", buf);
            break;
    }
    //wait_ms(1);
    _smutex.unlock();
}
void ATCmdManager::sendAtEvent(const char *buf)
{
    _smutex.lock();
    switch(dataMode){
        case AT_CMD_DATA_MODE:
            _parser.send(buf);
            break;
        case AT_STD_DATA_MODE:
            _parser.send(buf);
            break;
        case AT_EXT_DATA_MODE:
        {
            int len =  strlen(buf);
            outputEDMdata((const uint8_t *) buf, len, AT_MSG_ID, EVENT_MSG_TYPE, NO_CHANNEL);
            break;
        }
        default:
            _parser.send(buf);
            break;
    }
    _smutex.unlock();
    _wiFi2ATDatamPool->free(resp_data);
    resp_data = NULL;
}


void ATCmdManager::sendConnectEvent(const uint8_t *buf, int len)
{
    //_smutex.lock();
    switch(dataMode){
        case AT_CMD_DATA_MODE:
            _parser.send((const char*) buf);
            break;
        case AT_STD_DATA_MODE:
            _parser.send((const char*)buf);
            break;
        case AT_EXT_DATA_MODE:
            outputEDMdata((const uint8_t *) buf, len, CONNECT_MSG_ID, EVENT_MSG_TYPE, WIFI_CHANNEL);
            break;
    }
    _wiFi2ATDatamPool->free(resp_data);
    resp_data = NULL;
    
}


void ATCmdManager::outputEDMdata(const uint8_t *buf, int pLen, 
                                 edm_msg_id_t identifier, edm_msg_type_t type,
                                 channel_id_t channel_id)
{
    int epLen = pLen + 2; // edm payload length = data length + 2
    if(channel_id != NO_CHANNEL)
        epLen += 1;
    _smutex.lock();
    // send EDM Message start byte
    _parser.putc(EDM_START_BYTE);
    // send EDM Message length
    _parser.putc(epLen>>8);
    _parser.putc(epLen%256);
    // send EDM Identifier + Type
    _parser.putc(identifier>>8);
    _parser.putc(identifier%256 | type);
    // send channel id if valid
    if(channel_id != NO_CHANNEL)
        _parser.putc(channel_id);
    // send the data
    _parser.write((const char *)buf, pLen);
    // send EDM Message stop byte
    _parser.putc(EDM_STOP_BYTE);
    _smutex.unlock();
    int msWait = (pLen + 5+20)/20;
    wait_ms(msWait);
}

void ATCmdManager::return_response(bool download) {
    char * resp = (char *) resp_data->buffer;
    //_smutex.lock();
    dbg_printf(LOG, "\n[ATCMD MAN] received response:\n");
    if(download == false) // not download must be ascii header
    {
        dbg_printf(LOG, "%.*s\r\n", resp_data->dataLen, resp);
    }
    else // dump payload as hex
    {
        printBufferInHex((uint8_t *)resp, resp_data->dataLen);
    }
    //_parser.write(resp, resp_data->dataLen);
    outputEDMdata((const uint8_t *)resp, resp_data->dataLen, DATA_MSG_ID, 
                  EVENT_MSG_TYPE, WIFI_CHANNEL);
    //_smutex.unlock();
    _wiFi2ATDatamPool->free(resp_data);
    resp_data = NULL;
    //print_memory_info();
    _event_queue.call_in(10, &print_heap_and_isr_stack_info);
    //print_heap_and_isr_stack_info();   
}