static void uv__udp_sendmsg(uv_loop_t* loop,
                            uv__io_t* w,
                            unsigned int revents) {
  uv_udp_t* handle;

  handle = container_of(w, uv_udp_t, io_watcher);
  assert(handle->type == UV_UDP);
  assert(revents & UV__POLLOUT);

  assert(!QUEUE_EMPTY(&handle->write_queue)
      || !QUEUE_EMPTY(&handle->write_completed_queue));

  /* Write out pending data first. */
  uv__udp_run_pending(handle);

  /* Drain 'request completed' queue. */
  uv__udp_run_completed(handle);

  if (!QUEUE_EMPTY(&handle->write_completed_queue)) {
    /* Schedule completion callbacks. */
    uv__io_feed(handle->loop, &handle->io_watcher);
  }
  else if (QUEUE_EMPTY(&handle->write_queue)) {
    /* Pending queue and completion queue empty, stop watcher. */
    uv__io_stop(loop, &handle->io_watcher, UV__POLLOUT);

    if (!uv__io_active(&handle->io_watcher, UV__POLLIN))
      uv__handle_stop(handle);
  }
}

static void uv__cf_loop_cb(void* arg) {
  uv_loop_t* loop;
  QUEUE* item;
  QUEUE split_head;
  uv__cf_loop_signal_t* s;

  loop = arg;

  uv_mutex_lock(&loop->cf_mutex);
  QUEUE_INIT(&split_head);
  if (!QUEUE_EMPTY(&loop->cf_signals)) {
    QUEUE* split_pos = QUEUE_HEAD(&loop->cf_signals);
    QUEUE_SPLIT(&loop->cf_signals, split_pos, &split_head);
  }
  uv_mutex_unlock(&loop->cf_mutex);

  while (!QUEUE_EMPTY(&split_head)) {
    item = QUEUE_HEAD(&split_head);

    s = QUEUE_DATA(item, uv__cf_loop_signal_t, member);

    /* This was a termination signal */
    if (s->cb == NULL)
      CFRunLoopStop(loop->cf_loop);
    else
      s->cb(s->arg);

    QUEUE_REMOVE(item);
    free(s);
  }
}

static void uv__loop_delete(uv_loop_t* loop) {
  uv__signal_loop_cleanup(loop);
  uv__platform_loop_delete(loop);
  uv__async_stop(loop, &loop->async_watcher);

  if (loop->emfile_fd != -1) {
    close(loop->emfile_fd);
    loop->emfile_fd = -1;
  }

  if (loop->backend_fd != -1) {
    close(loop->backend_fd);
    loop->backend_fd = -1;
  }

  uv_mutex_lock(&loop->wq_mutex);
  assert(QUEUE_EMPTY(&loop->wq) && "thread pool work queue not empty!");
  assert(!uv__has_active_reqs(loop));
  uv_mutex_unlock(&loop->wq_mutex);
  uv_mutex_destroy(&loop->wq_mutex);

#if 0
  assert(QUEUE_EMPTY(&loop->pending_queue));
  assert(QUEUE_EMPTY(&loop->watcher_queue));
  assert(loop->nfds == 0);
#endif

  free(loop->watchers);
  loop->watchers = NULL;
  loop->nwatchers = 0;
}

void uv__work_done(uv_async_t* handle) {
  struct uv__work* w;
  uv_loop_t* loop;
  QUEUE* q;
  QUEUE wq;
  int err;

  loop = container_of(handle, uv_loop_t, wq_async);
  QUEUE_INIT(&wq);

  uv_mutex_lock(&loop->wq_mutex);
  if (!QUEUE_EMPTY(&loop->wq)) {
    q = QUEUE_HEAD(&loop->wq);
    QUEUE_SPLIT(&loop->wq, q, &wq);
  }
  uv_mutex_unlock(&loop->wq_mutex);

  while (!QUEUE_EMPTY(&wq)) {
    q = QUEUE_HEAD(&wq);
    QUEUE_REMOVE(q);

    w = container_of(q, struct uv__work, wq);
    err = (w->work == uv__cancelled) ? UV_ECANCELED : 0;
    w->done(w, err);
  }
}

void processUsbSendQueue(UsbDevice* usbDevice) {
    while(usbDevice->configured &&
            !QUEUE_EMPTY(uint8_t, &usbDevice->sendQueue)) {
        // Make sure the USB write is 100% complete before messing with this buffer
        // after we copy the message into it - the Microchip library doesn't copy
        // the data to its own internal buffer. See #171 for background on this
        // issue.
        if(!waitForHandle(usbDevice)) {
            return;
        }

        int byteCount = 0;
        while(!QUEUE_EMPTY(uint8_t, &usbDevice->sendQueue) && byteCount < 64) {
            usbDevice->sendBuffer[byteCount++] = QUEUE_POP(uint8_t, &usbDevice->sendQueue);
        }

        int nextByteIndex = 0;
        while(nextByteIndex < byteCount) {
            if(!waitForHandle(usbDevice)) {
                return;
            }
            int bytesToTransfer = min(USB_PACKET_SIZE, byteCount - nextByteIndex);
            usbDevice->deviceToHostHandle = usbDevice->device.GenWrite(
                    usbDevice->inEndpoint, &usbDevice->sendBuffer[nextByteIndex], bytesToTransfer);
            nextByteIndex += bytesToTransfer;
        }
    }
}

static void uv__write_callbacks(uv_stream_t* stream) {
  uv_write_t* req;
  QUEUE* q;

  while (!QUEUE_EMPTY(&stream->write_completed_queue)) {
    /* Pop a req off write_completed_queue. */
    q = QUEUE_HEAD(&stream->write_completed_queue);
    req = QUEUE_DATA(q, uv_write_t, queue);
    QUEUE_REMOVE(q);
    uv__req_unregister(stream->loop, req);

    if (req->bufs != NULL) {
      stream->write_queue_size -= uv__write_req_size(req);
      if (req->bufs != req->bufsml)
        free(req->bufs);
      req->bufs = NULL;
    }

    /* NOTE: call callback AFTER freeing the request data. */
    if (req->cb)
      req->cb(req, req->error);
  }

  assert(QUEUE_EMPTY(&stream->write_completed_queue));
}

void uv__io_start(uv_loop_t* loop, uv__io_t* w, unsigned int events) {
  assert(0 == (events & ~(POLLIN | POLLOUT | UV__POLLRDHUP)));
  assert(0 != events);
  assert(w->fd >= 0);
  assert(w->fd < INT_MAX);

  w->pevents |= events;
  maybe_resize(loop, w->fd + 1);

#if !defined(__sun)
  /* The event ports backend needs to rearm all file descriptors on each and
   * every tick of the event loop but the other backends allow us to
   * short-circuit here if the event mask is unchanged.
   */
  if (w->events == w->pevents) {
    if (w->events == 0 && !QUEUE_EMPTY(&w->watcher_queue)) {
      QUEUE_REMOVE(&w->watcher_queue);
      QUEUE_INIT(&w->watcher_queue);
    }
    return;
  }
#endif

  if (QUEUE_EMPTY(&w->watcher_queue))
    QUEUE_INSERT_TAIL(&loop->watcher_queue, &w->watcher_queue);

  if (loop->watchers[w->fd] == NULL) {
    loop->watchers[w->fd] = w;
    loop->nfds++;
  }
}

void uv__cf_loop_cb(void* arg) {
  uv_loop_t* loop;
  QUEUE* item;
  QUEUE split_head;
  uv__cf_loop_signal_t* s;

  loop = arg;

  uv_mutex_lock(&loop->cf_mutex);
  QUEUE_INIT(&split_head);
  if (!QUEUE_EMPTY(&loop->cf_signals)) {
    QUEUE* split_pos = QUEUE_HEAD(&loop->cf_signals);
    QUEUE_SPLIT(&loop->cf_signals, split_pos, &split_head);
  }
  uv_mutex_unlock(&loop->cf_mutex);

  while (!QUEUE_EMPTY(&split_head)) {
    item = QUEUE_HEAD(&split_head);

    s = QUEUE_DATA(item, uv__cf_loop_signal_t, member);
    s->cb(s->arg);

    QUEUE_REMOVE(item);
    free(s);
  }
}

void uv__work_done(uv_async_t* handle) {
  struct uv__work* w;
  uv_loop_t* loop;
  QUEUE* q;
  QUEUE wq;
  int err;

  loop = container_of(handle, uv_loop_t, wq_async);
  QUEUE_INIT(&wq);

  // uv_mutex_lock(&loop->wq_mutex);
  if (!QUEUE_EMPTY(&loop->wq)) {
    q = QUEUE_HEAD(&loop->wq);
    QUEUE_SPLIT(&loop->wq, q, &wq);
  }
  // uv_mutex_unlock(&loop->wq_mutex);

  while (!QUEUE_EMPTY(&wq)) {
    q = QUEUE_HEAD(&wq);
    QUEUE_REMOVE(q);

    w = container_of(q, struct uv__work, wq);
    w->done(w, 0, NULL, 0);
  }
}

void processUsbSendQueue(UsbDevice* usbDevice) {
    if(usbDevice->configured && vbusEnabled()) {
        // if there's nothing attached to the analog input it floats at ~828, so
        // if we're powering the board from micro-USB (and the jumper is going
        // to 5v and not the analog input), this is still OK.
        debug("USB no longer detected - marking unconfigured");
        usbDevice->configured = false;
    }

    // Don't touch usbDevice->sendBuffer if there's still a pending transfer
    if(!waitForHandle(usbDevice)) {
        return;
    }

    while(usbDevice->configured &&
            !QUEUE_EMPTY(uint8_t, &usbDevice->sendQueue)) {
        int byteCount = 0;
        while(!QUEUE_EMPTY(uint8_t, &usbDevice->sendQueue) &&
                byteCount < USB_SEND_BUFFER_SIZE) {
            usbDevice->sendBuffer[byteCount++] = QUEUE_POP(uint8_t,
                    &usbDevice->sendQueue);
        }

        int nextByteIndex = 0;
        while(nextByteIndex < byteCount) {
            // Make sure the USB write is 100% complete before messing with this
            // buffer after we copy the message into it - the Microchip library
            // doesn't copy the data to its own internal buffer. See #171 for
            // background on this issue.
            // TODO instead of dropping, replace POP above with a SNAPSHOT
            // and POP off only exactly how many bytes were sent after the
            // fact.
            // TODO in order for this not to fail too often I had to increase
            // the USB_HANDLE_MAX_WAIT_COUNT. that may be OK since now we have
            // VBUS detection.
            if(!waitForHandle(usbDevice)) {
                debug("USB not responding in a timely fashion, dropped data");
                return;
            }

            int bytesToTransfer = min(MAX_USB_PACKET_SIZE_BYTES,
                    byteCount - nextByteIndex);
            usbDevice->deviceToHostHandle = usbDevice->device.GenWrite(
                    usbDevice->inEndpoint,
                    &usbDevice->sendBuffer[nextByteIndex], bytesToTransfer);
            nextByteIndex += bytesToTransfer;
        }
    }
}

static void uv__udp_run_completed(uv_udp_t* handle) {
  uv_udp_send_t* req;
  QUEUE* q;

  while (!QUEUE_EMPTY(&handle->write_completed_queue)) {
    q = QUEUE_HEAD(&handle->write_completed_queue);
    QUEUE_REMOVE(q);

    req = QUEUE_DATA(q, uv_udp_send_t, queue);
    uv__req_unregister(handle->loop, req);

    if (req->bufs != req->bufsml)
      free(req->bufs);
    req->bufs = NULL;

    if (req->send_cb == NULL)
      continue;

    /* req->status >= 0 == bytes written
     * req->status <  0 == errno
     */
    if (req->status >= 0)
      req->send_cb(req, 0);
    else
      req->send_cb(req, req->status);
  }
}

void
uTaskMessageLoop(
    void
    )
{
    Tcb_T *pTcb;

    if (!gCore.Flags & CORE_FLAGS_INIT)
    {
        return;
    }

    for ( ; ; )
    {
        /* Has a shutdown request occurred */
        if (gCore.Flags & CORE_FLAGS_SHUTDOWN)
        {
            DBG_MSG(DBG_WARN, "Shutdown request\n");
            break;
        }

        /* If the are any isr queue items, move them into tcb queue */
        if (!QUEUE_EMPTY(gCore.IsrQ))
        {
            pTcb = TcbAlloc();

            if (pTcb)
            {
                QUEUE_GET(gCore.IsrQ, *pTcb);

                TcbEnqueue(pTcb);
            }
        }

        pTcb = TcbFront();

        if (pTcb)
        {
            /* Has pTcb expired */
            if (TIME_AFTER_EQ(uTaskGetTick(), pTcb->Expire))
            {
                pTcb = TcbDequeue();

                DBG_MSG(DBG_TRACE, "Delay(%ld) Task %p Id %d pMsg %p\n",
                                   uTaskGetTick()-pTcb->Expire,
                                   pTcb->pTask,
                                   pTcb->Id,
                                   pTcb->pMsg);

                /* Send the message to the task */
                pTcb->pTask->Handler(pTcb->pTask, pTcb->Id, pTcb->pMsg);

                /* Free the message structure */
                uTaskFree(pTcb->pMsg);

                TcbFree(pTcb);
            }
        }
    }
}

static void uv__drain(uv_stream_t* stream) {
  uv_shutdown_t* req;
  int err;

  assert(QUEUE_EMPTY(&stream->write_queue));
  uv__io_stop(stream->loop, &stream->io_watcher, UV__POLLOUT);

  /* Shutdown? */
  if ((stream->flags & UV_STREAM_SHUTTING) &&
      !(stream->flags & UV_CLOSING) &&
      !(stream->flags & UV_STREAM_SHUT)) {
    assert(stream->shutdown_req);

    req = stream->shutdown_req;
    stream->shutdown_req = NULL;
    stream->flags &= ~UV_STREAM_SHUTTING;
    uv__req_unregister(stream->loop, req);

    err = 0;
    if (tuvp_shutdown(uv__stream_fd(stream), SHUT_WR))
      err = -errno;

    if (err == 0)
      stream->flags |= UV_STREAM_SHUT;

    if (req->cb != NULL)
      req->cb(req, err);
  }
}

void uv__loop_close(uv_loop_t* loop) {
  size_t i;

  uv__loops_remove(loop);

  /* close the async handle without needing an extra loop iteration */
  assert(!loop->wq_async.async_sent);
  loop->wq_async.close_cb = NULL;
  uv__handle_closing(&loop->wq_async);
  uv__handle_close(&loop->wq_async);

  for (i = 0; i < ARRAY_SIZE(loop->poll_peer_sockets); i++) {
    SOCKET sock = loop->poll_peer_sockets[i];
    if (sock != 0 && sock != INVALID_SOCKET)
      closesocket(sock);
  }

  uv_mutex_lock(&loop->wq_mutex);
  assert(QUEUE_EMPTY(&loop->wq) && "thread pool work queue not empty!");
  assert(!uv__has_active_reqs(loop));
  uv_mutex_unlock(&loop->wq_mutex);
  uv_mutex_destroy(&loop->wq_mutex);

  uv__free(loop->timer_heap);
  loop->timer_heap = NULL;

  CloseHandle(loop->iocp);
}

void uv__udp_finish_close(uv_udp_t* handle) {
  uv_udp_send_t* req;
  QUEUE* q;

  assert(!uv__io_active(&handle->io_watcher, UV__POLLIN | UV__POLLOUT));
  assert(handle->io_watcher.fd == -1);

  uv__udp_run_completed(handle);

  while (!QUEUE_EMPTY(&handle->write_queue)) {
    q = QUEUE_HEAD(&handle->write_queue);
    QUEUE_REMOVE(q);

    req = QUEUE_DATA(q, uv_udp_send_t, queue);
    uv__req_unregister(handle->loop, req);

    if (req->bufs != req->bufsml)
      free(req->bufs);
    req->bufs = NULL;

    if (req->send_cb != NULL)
      req->send_cb(req, -ECANCELED);
  }

  /* Now tear down the handle. */
  handle->recv_cb = NULL;
  handle->alloc_cb = NULL;
  /* but _do not_ touch close_cb */
}

void uv__udp_finish_close(uv_udp_t* handle) {
  uv_udp_send_t* req;
  QUEUE* q;

  assert(!uv__io_active(&handle->io_watcher, UV__POLLIN | UV__POLLOUT));
  assert(handle->io_watcher.fd == -1);

  while (!QUEUE_EMPTY(&handle->write_queue)) {
    q = QUEUE_HEAD(&handle->write_queue);
    QUEUE_REMOVE(q);

    req = QUEUE_DATA(q, uv_udp_send_t, queue);
    req->status = -ECANCELED;
    QUEUE_INSERT_TAIL(&handle->write_completed_queue, &req->queue);
  }

  uv__udp_run_completed(handle);

  assert(handle->send_queue_size == 0);
  assert(handle->send_queue_count == 0);

  /* Now tear down the handle. */
  handle->recv_cb = NULL;
  handle->alloc_cb = NULL;
  /* but _do not_ touch close_cb */
}

void uv__io_stop(uv_loop_t* loop, uv__io_t* w, unsigned int events) {
  assert(0 == (events & ~(POLLIN | POLLOUT | UV__POLLRDHUP)));
  assert(0 != events);

  if (w->fd == -1)
    return;

  assert(w->fd >= 0);

  /* Happens when uv__io_stop() is called on a handle that was never started. */
  if ((unsigned) w->fd >= loop->nwatchers)
    return;

  w->pevents &= ~events;

  if (w->pevents == 0) {
    QUEUE_REMOVE(&w->watcher_queue);
    QUEUE_INIT(&w->watcher_queue);

    if (loop->watchers[w->fd] != NULL) {
      assert(loop->watchers[w->fd] == w);
      assert(loop->nfds > 0);
      loop->watchers[w->fd] = NULL;
      loop->nfds--;
      w->events = 0;
    }
  }
  else if (QUEUE_EMPTY(&w->watcher_queue))
    QUEUE_INSERT_TAIL(&loop->watcher_queue, &w->watcher_queue);
}

int uv_loop_fork(uv_loop_t* loop) {
  int err;
  unsigned int i;
  uv__io_t* w;

  err = uv__io_fork(loop);
  if (err)
    return err;

  err = uv__async_fork(loop);
  if (err)
    return err;

  err = uv__signal_loop_fork(loop);
  if (err)
    return err;

  /* Rearm all the watchers that aren't re-queued by the above. */
  for (i = 0; i < loop->nwatchers; i++) {
    w = loop->watchers[i];
    if (w == NULL)
      continue;

    if (w->pevents != 0 && QUEUE_EMPTY(&w->watcher_queue)) {
      w->events = 0; /* Force re-registration in uv__io_poll. */
      QUEUE_INSERT_TAIL(&loop->watcher_queue, &w->watcher_queue);
    }
  }

  return 0;
}

/* ----------------------------------------------------------------------------*/
void* queue_out(Queue *queue)
{
    if (queue == NULL){
	ERROR("Null pointer!");
	return NULL;
    }

    if (QUEUE_EMPTY(queue)){
	INFO("Queue is empty!");
	return NULL;
    }

    void *ret = queue->head->element;
    QueueNode *node = queue->head;

    /**
     * only one node in the queue or not
     */ 
    if (queue->tail == node){
	queue->head = NULL;
	queue->tail = NULL;
    }else{
	queue->head = node->next;
    }
    queue->queue_size--;

    /**
     * insert one node into the spare list(in the head)
     */ 
    node->element = NULL;
    node->next = queue->spare;
    queue->spare = node;

    return ret;
}

/* ----------------------------------------------------------------------------*/
int queue_delete(Queue *queue, handle destroy_node)
{
    if (queue == NULL){
	ERROR("Null pointer!");
	return -1;
    }

    int ret = 0;
    if (!QUEUE_EMPTY(queue))
	queue_clear(queue, destroy_node);

    /**
     * free spare list
     */ 
    QueueNode *node = queue->spare;
    QueueNode *temp;
    while (node){
	temp = node;
	node = node->next;
	temp->next = NULL;
	free(temp);
    }

    ret = queue->node_count;
    queue->spare = NULL;
    queue->node_count = 0;

    return ret;
}