text: /**
   * @file ATcodec_api.c
   * @author aurelien.morelle@parrot.fr
   * @date 2006/12/06
  
   * modified on 2010/07/19 by stephane.piskorski.ext@parrot.fr (bug fix+comments)
   */
  
  #include <VP_Os/vp_os_types.h>
  #include <VP_Os/vp_os_delay.h>
  #include <VP_Os/vp_os_assert.h>
  #include <VP_Os/vp_os_malloc.h>
  #include <VP_Os/vp_os_signal.h>
  #include <VP_Os/vp_os_print.h>
  
  #if !defined(TARGET_OS_IPHONE) && !defined(TARGET_IPHONE_SIMULATOR)
  # include <VP_Api/vp_api_thread_helper.h>
  #endif // ! TARGET_OS_IPHONE && ! TARGET_IPHONE_SIMULATOR
  
  #include <ATcodec/ATcodec_api.h>
  #include <ATcodec/ATcodec_Buffer.h>
  #include <ATcodec/ATcodec.h>
  #include <ATcodec/ATcodec_Tree.h>
  
  
  static AT_CODEC_READING_MODE at_codec_reading_mode = ATCODEC_READ_FROM_STREAM;
  
  static AT_CODEC_FUNCTIONS_PTRS func_ptrs;
  static ATcodec_Tree_t default_tree;
  static int atcodec_lib_init_ok = 0;
  
  // only for client
  static vp_os_mutex_t ATcodec_cond_mutex;
  static vp_os_cond_t  ATcodec_wait_cond;
  static int8_t        ATcodec_Message_Buffer[INTERNAL_BUFFER_SIZE];
  static int32_t       ATcodec_Message_len = 0;
  
  static int32_t v_continue = 0;
  
  
  static ATCODEC_RET
  test_dyn_strs_one_node(ATcodec_Tree_t *tree, ATcodec_Tree_Node_t *node, int depth, const char *dynamic_str, ATcodec_Memory_t *memory, AT_CODEC_Message_Received *ptr, int *len_dec)
  {
    char *static_str, *dyn_str, *mem_index;
    int len;
    ATcodec_Memory_t str, fmt;
  	
    static_str = (char *)ATcodec_Buffer_getElement(&tree->strs, ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data))->static_str);
    if((len = strlen(static_str)) > depth)
      {
        if(strncmp(static_str+depth, dynamic_str, (len-depth)*sizeof(char)))
  	return ATCODEC_FALSE;
      }
  	
    dyn_str = (char *)ATcodec_Buffer_getElement(&tree->strs, ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data))->dynamic_str);
    ATcodec_Memory_Init(&str, dynamic_str+(len-depth), 0, 1, NULL, NULL);
    ATcodec_Memory_Init(&fmt, dyn_str, 0, 1, NULL, NULL);
  	
    mem_index = memory->current;
    if(vp_atcodec_sscanf(&str, &fmt, memory, len_dec) == ATCODEC_TRUE)
      {
        memory->current = mem_index;
        *ptr = ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data))->func_ptr;
        *len_dec += len;
        return ATCODEC_TRUE;
      }
  	
    return ATCODEC_FALSE;
  }
  
  
  // We assume we know that current->isleaf != -1 here
  static ATCODEC_RET
  test_dyn_strs(ATcodec_Tree_t *tree, ATcodec_Tree_Node_t *current, int depth, const char *dynamic_str, ATcodec_Memory_t *memory, AT_CODEC_Message_Received *ptr, int *len_dec)
  {
    ATcodec_Tree_Node_t *son;
    int i;
  	
    if(current->type == ATCODEC_TREE_NODE_TYPE_LEAF)
      {
        // just current to be tested
        return test_dyn_strs_one_node(tree, current, depth, dynamic_str, memory, ptr, len_dec);
      }
    else
      {
        VP_OS_ASSERT(current->type == ATCODEC_TREE_NODE_TYPE_MULTILEAVES);
        // all sons have to be tested
        for(i = 0 ; i < current->nb_sons ; i++)
  	{
  	  son = (ATcodec_Tree_Node_t *)ATcodec_Buffer_getElement(&tree->sons, current->sons[i]);
  	  if(test_dyn_strs_one_node(tree, son, depth, dynamic_str, memory, ptr, len_dec) == ATCODEC_TRUE)
  	    return ATCODEC_TRUE;
  	}
      }
  	
    return ATCODEC_FALSE;
  }
  
  
  static ATCODEC_RET
  test_process_node(ATcodec_Tree_t *tree, ATcodec_Tree_Node_t *node, const char *cpy, ATcodec_Memory_t *memory, int *len_dec, int depth)
  {
    ATcodec_Tree_Node_t *son;
    AT_CODEC_Message_Received ptr;
    AT_CODEC_MSG_ID id = (AT_CODEC_MSG_ID)-1;
    char *fmt_str;
    int32_t output_len;
    char output_params[INTERNAL_BUFFER_SIZE];
    int8_t output_str[INTERNAL_BUFFER_SIZE];
    ATcodec_Memory_t output, dest, fmt;
  	
    if(test_dyn_strs(tree, node, depth, cpy, memory, &ptr, len_dec) == ATCODEC_TRUE)
      {
        ATcodec_Memory_Init(&output, &output_params[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
        VP_OS_ASSERT(ptr);
        ptr(memory, &output, &id);
        if((int)id != -1)
  	{
  	  son = ATcodec_Tree_Node_get(tree, id);
  	  fmt_str = (char *)ATcodec_Buffer_getElement(&tree->strs, ((ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, son->data))->total_str);
  			
  	  ATcodec_Memory_Init(&dest, (char*)&output_str[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
  	  ATcodec_Memory_Init(&fmt, fmt_str, 0, 1, NULL, NULL);
  			
  	  output.current = (char *)output.start;
  	  if(vp_atcodec_sprintf_params(&dest, &output_len, &fmt, &output) != ATCODEC_TRUE)
  	    return ATCODEC_FALSE;
  			
  	  if(func_ptrs.write(&output_str[0], &output_len) != AT_CODEC_WRITE_OK)
  	    return ATCODEC_FALSE;
  	}
        return ATCODEC_TRUE;
      }
  	
    return ATCODEC_FALSE;
  }
  
  
  static ATCODEC_RET
  find_process_node(ATcodec_Tree_t *tree, const char *message, ATcodec_Memory_t *memory, int *len_dec)
  {
    ATcodec_Tree_Node_t *current = ATcodec_Tree_Node_get(tree, tree->root);
    char *cpy = (char *)message;
    int depth = 0;
  	
    while(current->type == ATCODEC_TREE_NODE_TYPE_NODE)
      {
        if(current->sons[0] != -1)
  	{
  	  if(test_process_node(tree, ATcodec_Tree_Node_get(tree, current->sons[0]), cpy, memory, len_dec, depth) == ATCODEC_TRUE)
  	    return ATCODEC_TRUE;
  	}
        if(!*cpy || current->sons[(int)(*cpy)] == -1)
  	{
  	  return ATCODEC_FALSE;
  	}
        else
  	{
  	  current = ATcodec_Tree_Node_get(tree, current->sons[(int)(*cpy++)]);
  	  depth++;
  	}
      }
  	
    return test_process_node(tree, current, cpy, memory, len_dec, depth);
  }
  
  /********************************************************************
   * @param[in] str String to a raw AT command (with % standing for parameters).
   * @brief Computes the length of the non-changing part (before the parameters) of an AT command string.
  */
  static int
  static_len(const char *str)
  {
    int len = 0;
    int percent = 0;
    int escaped = 1;
  	
    do
      {
        switch(*str)
  	{
  	case '%':
  	  if(escaped)
  	    percent = 1-percent;
  	  escaped = 1;
  	  break;
  	case 's':
  	case 'd':
  	case 'c':
  	case 'l':
  	  if(percent)
  	    {
  	      return (len-1);
  	    }
  	  escaped = 1;
  	  percent = 0;
  	  break;
  	case '[':
  	  if(escaped)
  	    {
  	      return len;
  	    }
  	  break;
  	case '\\':
  	  escaped = 1-escaped;
  	  percent = 0;
  	  break;
  	case '\0':
  	  return len;
  	  break;
  	default:
  	  escaped = 1;
  	  percent = 0;
  	  break;
  	}
  		
        len++;
      }
    while(*str++);
  	
    return len;
  }
  
  
  AT_CODEC_ERROR_CODE at_default_cb(ATcodec_Memory_t *input, ATcodec_Memory_t *output, int *id)
  {
    ATCODEC_PRINT("< AT message received >\n");
  	
    return AT_CODEC_GENERAL_OK;
  }
  
  
  AT_CODEC_MSG_ID
  ATcodec_Add_Hashed_Message (const char *str, AT_CODEC_MSG_ID from_cmd, AT_CODEC_Message_Received func_ptr, int priority)
  {
    return ATcodec_Add_Hashed_Message_Tree  (&default_tree, str, from_cmd, func_ptr, priority);
  }
  
  
  AT_CODEC_MSG_ID
  ATcodec_Add_Hashed_Message_Tree  (ATcodec_Tree_t *tree, const char *str, AT_CODEC_MSG_ID from_cmd, AT_CODEC_Message_Received func_ptr, int priority)
  {
    ATcodec_Message_Data_t data, *p_data;
    ATcodec_Tree_Node_t *node;
    int len_s, len_d, node_i;
    char buffer[1024];
  	
    data.static_str = -1;
    data.dynamic_str = -1;
    data.total_str = -1;
  	
    data.from_cmd = from_cmd;
    data.func_ptr = func_ptr;
    data.priority = priority;
  
    /* Retrieves the length of the never-changing part of the AT command */
    len_s = static_len(str);
    VP_OS_ASSERT(len_s < 1024);
    vp_os_memcpy(&buffer[0], str, len_s);
    buffer[len_s] = 0;
    node_i = ATcodec_Tree_insert(tree, &buffer[0], &data);
    node = ATcodec_Tree_Node_get(tree, node_i);
  	
    p_data = ATcodec_Buffer_getElement(&tree->leaves, node->data);
    p_data->static_str = node->strkey;
  	
    // backup dynamic_str
    p_data->dynamic_str = tree->strs.nbElements;
    len_d = strlen(str+len_s)+1;
    ATcodec_Buffer_pushElements (&tree->strs, str+len_s, len_d);
  	
    // backup concatenation of static_str and dynamic_str
    p_data->total_str = tree->strs.nbElements;
    ATcodec_Buffer_pushElements (&tree->strs, str, len_s+len_d);
  	
    return (AT_CODEC_MSG_ID)node_i;
  }
  
  
  AT_CODEC_MSG_ID
  ATcodec_Add_Defined_Message (const char *str)
  {
    return ATcodec_Add_Defined_Message_Tree (&default_tree, str);
  }
  
  
  AT_CODEC_MSG_ID
  ATcodec_Add_Defined_Message_Tree (ATcodec_Tree_t *tree, const char *str)
  {
    return ATcodec_Add_Hashed_Message_Tree (tree, str, 0, NULL, 0);
  }
  
  
  void
  ATcodec_Init_Library        (AT_CODEC_FUNCTIONS_PTRS *funcs)
  {
    ATcodec_Init_Library_Tree   (&default_tree, funcs);
  }
  
  void
  ATcodec_Set_Reading_Mode(AT_CODEC_READING_MODE _mode)
  {
  	if (_mode==ATCODEC_READ_FROM_STREAM || _mode==ATCODEC_READ_FROM_PACKETS)
  	{
  		at_codec_reading_mode = _mode;
  	}
  }
  
  void
  ATcodec_Init_Library_Tree   (ATcodec_Tree_t *tree, AT_CODEC_FUNCTIONS_PTRS *funcs)
  {
    VP_OS_ASSERT(funcs);
    VP_OS_ASSERT(funcs->open);
    VP_OS_ASSERT(funcs->read);
    VP_OS_ASSERT(funcs->enable);
    VP_OS_ASSERT(funcs->write);
    VP_OS_ASSERT(funcs->close);
    VP_OS_ASSERT(funcs->init);
    VP_OS_ASSERT(funcs->shutdown);
  	
    ATcodec_Tree_init(tree, sizeof(ATcodec_Message_Data_t), 1);
  	
    memcpy(&func_ptrs, funcs, sizeof(*funcs));
  	
    vp_os_mutex_init(&ATcodec_cond_mutex);
    vp_os_cond_init(&ATcodec_wait_cond, &ATcodec_cond_mutex);
  	
    if(func_ptrs.init() != AT_CODEC_INIT_OK)
      {
        ATCODEC_PRINT("ATcodec Init error\n");
      }
    else
      {
        ATcodec_Tree_print(tree);
        atcodec_lib_init_ok = 1;
        ATcodec_Message_len = 0;
      }
  }
  
  
  void
  ATcodec_Shutdown_Library        (void)
  {
    ATcodec_Shutdown_Library_Tree(&default_tree);
  }
  
  
  void
  ATcodec_Shutdown_Library_Tree   (ATcodec_Tree_t *tree)
  {
    /*   ATcodec_Buffer_destroy(tree->strs); */
    /*   ATcodec_Buffer_destroy(tree->sons); */
    /*   ATcodec_Buffer_destroy(tree->leaves); */
    atcodec_lib_init_ok = 0;
  }
  
  
  // \todo CHANGE THE '\r'
  static int append_reception(char *buffer, int len, char *global_buffer, int *global_len, int global_buffer_limit)
  {
    int res = 0, i;
  	
    for(i = 0 ; i < len ; i++)
      {
        global_buffer[(*global_len)++] = buffer[i];
        if(*global_len >= /*INTERNAL_BUFFER_SIZE*/global_buffer_limit)
  	{
  	  // buffer overflow => purge
  	  *global_len = 0;
  	  return -1;
  	}
        if(buffer[i] == '\r')
  	{
  	  res++;
  	}
      }
  	
    global_buffer[*global_len] = '\0';
  	
    return res;
  }
  
  
  static ATCODEC_RET process_received_data(ATcodec_Tree_t *tree, int nb, char *global_buffer, int *global_len)
  {
    ATCODEC_RET res = ATCODEC_TRUE;
    int len_dec;
    char memory[INTERNAL_BUFFER_SIZE];
    ATcodec_Memory_t mem;
  
    while(nb--) /* nb is the number of \r in the buffer, ie. the number of potential AT commands */
      {
        ATcodec_Memory_Init(&mem, &memory[0], sizeof(memory), 1, NULL, NULL);
        if(find_process_node(tree, global_buffer, &mem, &len_dec) != ATCODEC_TRUE)
  	{
        /* Go to the next potential command */
  	  for(len_dec = 0 ; len_dec < *global_len && global_buffer[len_dec] != '\r' ; len_dec++)
  	    {
  	      // nothing
  	    }
  	  ATCODEC_PRINT("PURGE\n");
  	  if(global_buffer[len_dec++] == '\r')
  	    {
  	      if(global_buffer[len_dec++] == '\0')
  		len_dec++;
  	    }
  	}
  		
        if(--len_dec >= *global_len)
  	len_dec = *global_len;
  		
      if(*global_len == len_dec)
  	{
  	  ATCODEC_ZERO_MEMSET(global_buffer, 0, *global_len*sizeof(char));
  	  *global_len = 0;
  	}
        else
  	{
  	  memmove(/*dest*/global_buffer, /*src*/global_buffer+len_dec, /*nb bytes*/*global_len-len_dec);
  	  ATCODEC_ZERO_MEMSET(global_buffer+*global_len-len_dec, 0, (len_dec)*sizeof(char));
  	  *global_len -= len_dec;
  	}
      }
  	
    return res;
  }
  
  
  
  
  /**
  * AT Command receiving thread. ( ** AT Command Server ** )
  * This thread keeps calling a callback function stored
  * in 'func_ptrs.read' to fetch data, accumulates these data until
  * a full AT command has been received, and then call the AT decoder.
  * */
  #if !defined(TARGET_OS_IPHONE) && !defined(TARGET_IPHONE_SIMULATOR)
  DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Server,data,ATCODEC_STACK_SIZE)
  {
    ATcodec_Tree_t *tree = &default_tree;
    AT_CODEC_ERROR_CODE res;
    int32_t v_loop, v_read, len, nb_cmd = 0;
    char buffer[INTERNAL_BUFFER_SIZE]; // user-defined
    char global_buffer[INTERNAL_BUFFER_SIZE];
    char safety[16];  // Absorbs data overflowing from global_buffer.
    int global_len=0;
  	
    v_continue = 1;
    PRINT("Thread AT Commands Server Start\n");
  	
  
  
    while(!atcodec_lib_init_ok)
      {
        vp_os_thread_yield();
      }
  	
    while(v_continue)
      {
  	  vp_os_memset(buffer,0,sizeof(buffer));
  	  vp_os_memset(global_buffer,0,sizeof(global_buffer));global_len=0;
  	  vp_os_memset(safety,0,sizeof(safety));
  
        // open and init
  		  if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK){
  			  v_continue = 0;
  		  }
  		
        for ( v_loop = 1 ; v_loop && func_ptrs.enable() == AT_CODEC_ENABLE_OK; )
  		{
  		  v_read = 1;
  		  do
  			{
  			  // wait so that thread can give the hand : delay user-defined / OS-dependent
  			  // ...
  				  vp_os_delay(ATCODEC_SERVER_YIELD_DELAY);
  				  //vp_os_thread_yield();
  
  
  
  			  /* In case of reading from packets, we clear the incoming buffer.
  			   * Splitting AT commands into several packets would be a bad idea since packet order in not guaranteed in UDP.
  			   */
  				  if (at_codec_reading_mode==ATCODEC_READ_FROM_PACKETS){
  					  vp_os_memset(global_buffer,0,sizeof(global_buffer));
  					  global_len=0;
  				  }
  
  			  /*
  			   * Read some bytes; this function blocks until some data are made
  			   * available by the VP_COM thread.
  			   */
  				  len = sizeof(buffer); //INTERNAL_BUFFER_SIZE/*/2*/; // user-defined
  				  res = func_ptrs.read((int8_t*)&buffer[0], (int32_t*)&len);
  
  				if(res == AT_CODEC_READ_OK)
  				{
  				  if(len > 0)
  					{
  					  // process characters and update v_read
  					  // \todo Do not use nb_cmd ?
  
  					  /* Data are accumulated in the global buffer until at least one '\r' is found. */
  					  if((nb_cmd = append_reception(&buffer[0], len, &global_buffer[0], &global_len,sizeof(global_buffer))) > 0)
  						{
  						  v_read = 0;
  						}
  					  else if(nb_cmd == -1) /* no \r found in the global_buffer*/
  						{
  						  // a buffer overflow occurs
  							  switch(at_codec_reading_mode)
  							  {
  								  case ATCODEC_READ_FROM_STREAM:
  									  PRINT("AT Codec buffer was filled before a full AT commands was received.");
  									  break;
  								  case ATCODEC_READ_FROM_PACKETS:
  									 PRINT("AT Codec received a packet with no complete AT command or buffer was too small to store the whole packet.");
  									 break;
  							  }
  							  //ATCODEC_PRINT("Overflow\n");
  
  						/* In case of overflow, a TCP connection should be reinitialized in order to resynchronize
  						 * the client and the server. Otherwise there is no way to find the beginning of the next AT Command.
  						 * For a UDP connection, we assume all packets begin with an AT Command, and we just wait
  						 * for the next packet to arrive.
  						 */
  							  if (at_codec_reading_mode==ATCODEC_READ_FROM_STREAM) { v_loop = 0; }
  						}
  					  else
  						{
  						  v_read = 1;
  						}
  					}
  				  else
  					{
  					  if(len < 0)
  					{
  					  ATCODEC_PRINT("read returns a neg length\n");
  					  v_loop = 0;
  					}
  					}
  				}
  			  else /* if (res == AT_CODEC_READ_OK) */
  				{
  				  // an error occurred
  				  ATCODEC_PRINT("an error occurs\n");
  				  v_loop = 0;
  				}
  			}
  		  while (v_read && v_loop);
  
  		  // process what has been received if no error occurs
  		  if(v_loop)
  			{
  			  // ...
  			  if(process_received_data(tree, nb_cmd, &global_buffer[0], &global_len) != ATCODEC_TRUE)
  			{
  			  ATCODEC_PRINT("process_received returns false\n");
  			  v_loop = 0;
  			}
  			}
  		}/*for*/
  		
        // close and un-init : user-defined
  		  if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)
  		  v_continue = 0;
  
      }/* while */
  	
    if((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)
      {
        ATCODEC_PRINT("ATcodec Shutdown error\n");
      }
  	
    return((THREAD_RET)0);
  }
  #endif // ! TARGET_OS_IPHONE && ! TARGET_IPHONE_SIMULATOR
  
  
  static ATCODEC_RET
  valist_ATcodec_Queue_Message_valist_Tree(ATcodec_Tree_t *tree, AT_CODEC_MSG_ID id, va_list *va)
  {
    int32_t len;
    ATCODEC_RET res;
    ATcodec_Tree_Node_t *node = ATcodec_Tree_Node_get(tree, (int)id);
    ATcodec_Message_Data_t *data = (ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data);
    char *total_str = (char *)ATcodec_Buffer_getElement(&tree->strs, data->total_str);
    ATcodec_Memory_t msg, fmt;
  	char buffer[INTERNAL_BUFFER_SIZE];
  	
  	if(!atcodec_lib_init_ok)
      return ATCODEC_FALSE;
  
    
    vp_os_mutex_lock(&ATcodec_cond_mutex);
  
    ATcodec_Memory_Init(&msg, (char*)&buffer[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
    ATcodec_Memory_Init(&fmt, total_str, 0, 1, NULL, NULL);
  		
    if((res = vp_atcodec_sprintf_valist(&msg, &len, &fmt, va)) !=  ATCODEC_TRUE)
      {
  		vp_os_mutex_unlock(&ATcodec_cond_mutex);
        va_end(*va);
        return res;
      }
  
  	if(ATcodec_Message_len + len < INTERNAL_BUFFER_SIZE)
  	{
  		memcpy(&ATcodec_Message_Buffer[ATcodec_Message_len], &buffer[0], len);
  		ATcodec_Message_len += len;
  	}
  	
    //vp_os_cond_signal(&ATcodec_wait_cond);
    vp_os_mutex_unlock(&ATcodec_cond_mutex);
    va_end(*va);
  	
    return ATCODEC_TRUE;
  }
  
  
  
  /**
  * Push an AT Command in the AT command output queue.
  * This is the function called by the ardrone_at.c API functions.
  * */
  ATCODEC_RET
  ATcodec_Queue_Message_valist(AT_CODEC_MSG_ID id, ...)
  {
    ATCODEC_RET res;
    va_list va;
  
    va_start(va, id);
  
    res = valist_ATcodec_Queue_Message_valist_Tree(&default_tree, id, &va);
  	
    va_end(va);
  	
    return res;
  }
  
  
  ATCODEC_RET
  ATcodec_Queue_Message_valist_Tree(ATcodec_Tree_t *tree, AT_CODEC_MSG_ID id, ...)
  {
    ATCODEC_RET res;
    va_list va;
  
    va_start(va, id);
  
    res = valist_ATcodec_Queue_Message_valist_Tree(tree, id, &va);
  	
    va_end(va);
  	
    return res;
  }
  
  
  ATCODEC_RET
  ATcodec_Queue_Message_params(AT_CODEC_MSG_ID id, ATcodec_Memory_t *params)
  {
    return ATcodec_Queue_Message_params_Tree(&default_tree, id, params);
  }
  
  
  ATCODEC_RET
  ATcodec_Queue_Message_params_Tree(ATcodec_Tree_t *tree, AT_CODEC_MSG_ID id, ATcodec_Memory_t *params)
  {
    int32_t len;
    ATCODEC_RET res;
    ATcodec_Tree_Node_t *node = ATcodec_Tree_Node_get(tree, (int)id);
    ATcodec_Message_Data_t *data = (ATcodec_Message_Data_t *)ATcodec_Buffer_getElement(&tree->leaves, node->data);
    char *total_str = (char *)ATcodec_Buffer_getElement(&tree->strs, data->total_str);
    ATcodec_Memory_t msg, fmt;
  	char buffer[INTERNAL_BUFFER_SIZE];
    if(!atcodec_lib_init_ok)
      return ATCODEC_FALSE;
  	
    ATcodec_Memory_Init(&msg, (char*)&buffer[0], INTERNAL_BUFFER_SIZE, 1, NULL, NULL);
    ATcodec_Memory_Init(&fmt, total_str, 0, 1, NULL, NULL);
  	
    params->current = (char *)params->start;
    if((res = vp_atcodec_sprintf_params(&msg, &len, &fmt, params)) !=  ATCODEC_TRUE)
      {
        vp_os_mutex_unlock(&ATcodec_cond_mutex);
        return res;
      }
  	
  
  	if(ATcodec_Message_len + len < INTERNAL_BUFFER_SIZE)
  	{
  		memcpy(&ATcodec_Message_Buffer[ATcodec_Message_len], &buffer[0], len);
  		ATcodec_Message_len += len;
  	}
  	
    //vp_os_cond_signal(&ATcodec_wait_cond);
    vp_os_mutex_unlock(&ATcodec_cond_mutex);
  	
    return ATCODEC_TRUE;
  }
  
  
  ATCODEC_RET
  ATcodec_Send_Messages()
  {
    ATCODEC_RET res = ATCODEC_TRUE;
  
    if(!atcodec_lib_init_ok)
      return ATCODEC_FALSE;
  	
    vp_os_mutex_lock(&ATcodec_cond_mutex);
   
    if(ATcodec_Message_len > INTERNAL_BUFFER_SIZE)
  	  printf("ATcodec_Send_Messages : buf=%s, len=%d\n", &ATcodec_Message_Buffer[0], ATcodec_Message_len);
  	
    if(ATcodec_Message_len && func_ptrs.write((int8_t*)&ATcodec_Message_Buffer[0], (int32_t*)&ATcodec_Message_len) != AT_CODEC_WRITE_OK)
      res = ATCODEC_FALSE;
  	
    ATcodec_Message_len = 0;
  	
    vp_os_mutex_unlock(&ATcodec_cond_mutex);
  	
    return res;
  }
  
  #if !defined(TARGET_OS_IPHONE) && !defined(TARGET_IPHONE_SIMULATOR)
  DEFINE_THREAD_ROUTINE_STACK(ATcodec_Commands_Client,data,ATCODEC_STACK_SIZE)
  {
    AT_CODEC_ERROR_CODE res;
    int32_t v_loop;
  	
    v_continue = 1;
    PRINT("Thread AT Commands Client Start\n");
  	
    while(!atcodec_lib_init_ok)
      {
        vp_os_thread_yield();
      }
  	
    while(v_continue)
      {
        // open and init
        if((res = func_ptrs.open()) != AT_CODEC_OPEN_OK)
  	v_continue = 0;
  		
        for ( v_loop = 1 ; v_loop ; )
  	{
  	  // vp_os_delay(ATCODEC_SERVER_YIELD_DELAY);
  	  vp_os_thread_yield();
  			
  	  // wait a successful ATcodec_Queue_... has been called
  	  vp_os_mutex_lock(&ATcodec_cond_mutex);
  	  //vp_os_cond_wait(&ATcodec_wait_cond);
  			
  	  //ATCODEC_PRINT("Must send \"%s\"\n", &ATcodec_Message_Buffer[0]);
  	  if(ATcodec_Message_len && func_ptrs.write((int8_t*)&ATcodec_Message_Buffer[0], (int32_t*)&ATcodec_Message_len) != AT_CODEC_WRITE_OK)
  	    v_loop = 0;
  			
  	  ATcodec_Message_len = 0;
  	  vp_os_mutex_unlock(&ATcodec_cond_mutex);
  	}
  		
        // close and un-init : user-defined
        if((res = func_ptrs.close()) != AT_CODEC_CLOSE_OK)
  	v_continue = 0;
      }
  	
    if((res = func_ptrs.shutdown()) != AT_CODEC_SHUTDOWN_OK)
      {
        ATCODEC_PRINT("ATcodec Shutdown error\n");
      }
  	
    return((THREAD_RET)0);
  }
  #endif // ! TARGET_OS_IPHONE && ! TARGET_IPHONE_SIMULATOR
  
  
  void ATcodec_exit_thread(void)
  {
    v_continue = 0;
  }
  