static int gsm_atCmd_waitResponse(char * cmd, char *resp, char * resp1, int cmdSize, int timeout, char **response, int size)
{
    int len, res = 1, idx = 0, tot = 0, timeoutCnt = 0;
    char *sresp = malloc( 256 );
    char *data = malloc( 256 );

    if( (sresp == NULL) || (data == NULL) )
    {
        return 0;
    }

    memset( sresp, 0, 256 );
    memset( data, 0, 256 );

    // ** Send command to GSM
    vTaskDelay(100 / portTICK_PERIOD_MS);
    uart_flush( gsm_uartNum );

    // Send command
    if (cmd != NULL)
    {
        if (cmdSize == -1)
        {
            cmdSize = strlen(cmd);
        }
        #if GSM_DEBUG
        gsm_infoCommand(cmd, cmdSize, "AT COMMAND:");
        #endif
        uart_write_bytes(gsm_uartNum, (const char*)cmd, cmdSize);
        uart_wait_tx_done(gsm_uartNum, 100 / portTICK_RATE_MS);
    }

    if (response != NULL)
    {
        // Read GSM response into buffer
        char *pbuf = *response;
        len = uart_read_bytes(gsm_uartNum, (uint8_t*)data, 256, timeout / portTICK_RATE_MS);

        while (len > 0)
        {
            if ((tot + len) >= size)
            {
                char *ptemp = realloc(pbuf, size+512);
                if (ptemp == NULL) return 0;
                size += 512;
                pbuf = ptemp;
            }
            memcpy(pbuf + tot, data, len);
            tot += len;
            response[tot] = '\0';
            len = uart_read_bytes(gsm_uartNum, (uint8_t*)data, 256, 100 / portTICK_RATE_MS);
        }
        *response = pbuf;
        return tot;
    }

    // ** Wait for and check the response
    idx = 0;
    while(1)
    {
        memset(data, 0, 256);
        len = 0;
        len = uart_read_bytes(gsm_uartNum, (uint8_t*)data, 256, 10 / portTICK_RATE_MS);
        if (len > 0)
        {
            for (int i=0; i<len;i++)
            {
                if (idx < 256)
                {
                    if ((data[i] >= 0x20) && (data[i] < 0x80)) sresp[idx++] = data[i];
                    else sresp[idx++] = 0x2e;
                }
            }
            tot += len;
        }
        else
        {
            if (tot > 0)
            {
                // Check the response
                if (strstr(sresp, resp) != NULL)
                {
                    #if GSM_DEBUG
                    snprintf( debugBuffer, sizeof(debugBuffer), "AT RESPONSE: [%s]\n", sresp);
                    uart_write_bytes( UART_NUM_2, debugBuffer, strlen(debugBuffer) );
                    #endif
                    break;
                }
                else
                {
                    if (resp1 != NULL)
                    {
                        if (strstr(sresp, resp1) != NULL)
                        {
                            #if GSM_DEBUG
                            snprintf( debugBuffer, sizeof(debugBuffer), "AT RESPONSE (1): [%s]\n", sresp);
                            uart_write_bytes( UART_NUM_2, debugBuffer, strlen(debugBuffer) );
                            #endif
                            res = 2;
                            break;
                        }
                    }
                    // no match
                    #if GSM_DEBUG
                    snprintf( debugBuffer, sizeof(debugBuffer), "AT BAD RESPONSE: [%s]\n", sresp);
                    uart_write_bytes( UART_NUM_2, debugBuffer, strlen(debugBuffer) );
                    #endif
                    res = 0;
                    break;
                }
            }
        }

        timeoutCnt += 10;
        if (timeoutCnt > timeout)
        {
            // timeout
            #if GSM_DEBUG
            sprintf( debugBuffer, "AT: TIMEOUT\n");
            uart_write_bytes( UART_NUM_2, debugBuffer, strlen(debugBuffer) );
            #endif
            res = 0;
            break;
        }
    }

    free( sresp );
    free( data );

    return res;
}