static void rt_test_005_005_execute(void) {

  /* [5.5.1] An higher priority thread is created that performs
     non-atomical wait and signal operations on a semaphore.*/
  test_set_step(1);
  {
    threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()+1, thread3, 0);
  }

  /* [5.5.2] The function chSemSignalWait() is invoked by specifying
     the same semaphore for the wait and signal phases. The counter
     value must be one on exit.*/
  test_set_step(2);
  {
    chSemSignalWait(&sem1, &sem1);
    test_assert(queue_isempty(&sem1.queue), "queue not empty");
    test_assert(sem1.cnt == 0, "counter not zero");
  }

  /* [5.5.3] The function chSemSignalWait() is invoked again by
     specifying the same semaphore for the wait and signal phases. The
     counter value must be one on exit.*/
  test_set_step(3);
  {
    chSemSignalWait(&sem1, &sem1);
    test_assert(queue_isempty(&sem1.queue), "queue not empty");
    test_assert(sem1.cnt == 0, "counter not zero");
  }
}

int main()
{
    Queue myQueue;
    queue_init(&myQueue);
    assert(queue_isempty(&myQueue));
    queue_enqueue(&myQueue, 9);
    assert(!queue_isempty(&myQueue));
    printf("%d is top\n", queue_first(&myQueue));
    queue_enqueue(&myQueue, 1);
    queue_enqueue(&myQueue, 2);
    queue_enqueue(&myQueue, 3);
    queue_enqueue(&myQueue, 4);
    queue_enqueue(&myQueue, 5);
    queue_enqueue(&myQueue, 6);
    queue_enqueue(&myQueue, 7);
    queue_print(&myQueue);
    printf("%d dequeued\n", queue_dequeue(&myQueue));
    printf("%d dequeued\n", queue_dequeue(&myQueue));
    printf("%d dequeued\n", queue_dequeue(&myQueue));
    printf("%d dequeued\n", queue_dequeue(&myQueue));
    queue_print(&myQueue);
    
    queue_destroy(&myQueue);
    queue_print(&myQueue);
}

static void test_005_005_execute(void) {
  bool b;
  tprio_t prio;

  /* [5.5.1] Getting current thread priority for later checks.*/
  test_set_step(1);
  {
    prio = chThdGetPriorityX();
  }

  /* [5.5.2] Locking the mutex first time, it must be possible because
     it is not owned.*/
  test_set_step(2);
  {
    b = chMtxTryLock(&m1);
    test_assert(b, "already locked");
  }

  /* [5.5.3] Locking the mutex second time, it must fail because it is
     already owned.*/
  test_set_step(3);
  {
    b = chMtxTryLock(&m1);
    test_assert(!b, "not locked");
  }

  /* [5.5.4] Unlocking the mutex then it must not be owned anymore and
     the queue must be empty.*/
  test_set_step(4);
  {
    chMtxUnlock(&m1);
    test_assert(m1.owner == NULL, "still owned");
    test_assert(queue_isempty(&m1.queue), "queue not empty");
  }

  /* [5.5.5] Testing that priority has not changed after operations.*/
  test_set_step(5);
  {
    test_assert(chThdGetPriorityX() == prio, "wrong priority level");
  }

  /* [5.5.6] Testing chMtxUnlockAll() behavior.*/
  test_set_step(6);
  {
    b = chMtxTryLock(&m1);
    test_assert(b, "already locked");
    b = chMtxTryLock(&m1);
    test_assert(!b, "not locked");

    chMtxUnlockAll();
    test_assert(m1.owner == NULL, "still owned");
    test_assert(queue_isempty(&m1.queue), "queue not empty");
  }

  /* [5.5.7] Testing that priority has not changed after operations.*/
  test_set_step(7);
  {
    test_assert(chThdGetPriorityX() == prio, "wrong priority level");
  }
}

/**
 * @brief   Performs atomic signal and wait operations on two semaphores.
 *
 * @param[in] sps       pointer to a @p semaphore_t structure to be signaled
 * @param[in] spw       pointer to a @p semaphore_t structure to wait on
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval MSG_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval MSG_RESET    if the semaphore has been reset using @p chSemReset().
 *
 * @api
 */
msg_t chSemSignalWait(semaphore_t *sps, semaphore_t *spw) {
  msg_t msg;

  chDbgCheck((sps != NULL) && (spw != NULL));
  chDbgAssert(((sps->s_cnt >= (cnt_t)0) && queue_isempty(&sps->s_queue)) ||
              ((sps->s_cnt < (cnt_t)0) && queue_notempty(&sps->s_queue)),
              "inconsistent semaphore");
  chDbgAssert(((spw->s_cnt >= (cnt_t)0) && queue_isempty(&spw->s_queue)) ||
              ((spw->s_cnt < (cnt_t)0) && queue_notempty(&spw->s_queue)),
              "inconsistent semaphore");

  chSysLock();
  if (++sps->s_cnt <= (cnt_t)0) {
    chSchReadyI(queue_fifo_remove(&sps->s_queue))->p_u.rdymsg = MSG_OK;
  }
  if (--spw->s_cnt < (cnt_t)0) {
    thread_t *ctp = currp;
    sem_insert(ctp, &spw->s_queue);
    ctp->p_u.wtsemp = spw;
    chSchGoSleepS(CH_STATE_WTSEM);
    msg = ctp->p_u.rdymsg;
  }
  else {
    chSchRescheduleS();
    msg = MSG_OK;
  }
  chSysUnlock();

  return msg;
}

static void mtx5_execute(void) {

#if !CH_CFG_USE_MUTEXES_RECURSIVE
  bool b;
  tprio_t prio = chThdGetPriorityX();

  b = chMtxTryLock(&m1);
  test_assert(1, b, "already locked");

  b = chMtxTryLock(&m1);
  test_assert(2, !b, "not locked");

  chSysLock();
  chMtxUnlockS(&m1);
  chSysUnlock();

  test_assert(3, queue_isempty(&m1.m_queue), "queue not empty");
  test_assert(4, m1.m_owner == NULL, "still owned");
  test_assert(5, chThdGetPriorityX() == prio, "wrong priority level");
#endif /* !CH_CFG_USE_MUTEXES_RECURSIVE */
  
  chMtxLock(&m1);
  chMtxUnlockAll();
  test_assert(6, queue_isempty(&m1.m_queue), "queue not empty");
  test_assert(7, m1.m_owner == NULL, "still owned");
}

/* 
 * Move entire contents of one queue to the other
 *
 * Parameters:
 *  src     Pointer to source queue
 *  dst     Pointer to destination queue
 */
void queue_move(QUEUE *dst, QUEUE *src)
{
    if (queue_isempty(src))
        return;
    
    if (queue_isempty(dst))
        dst->head = src->head;
    else
        dst->tail->next = src->head;

    dst->tail = src->tail;
    src->head = NULL;
    return;
}

static void wakeup_writer(struct mgmt *mgmt)
{
	if (!queue_isempty(mgmt->pending_list)) {
		/* only queued reply commands trigger wakeup */
		if (queue_isempty(mgmt->reply_queue))
			return;
	}

	if (mgmt->writer_active)
		return;

	io_set_write_handler(mgmt->io, can_write_data, mgmt,
						write_watch_destroy);
}

int queue_remove(queue *q)
{
	if(queue_isempty(q))
	{
		fprintf(stderr,"Error: dequeue: Queue is empty\n");
		exit(1);
	}
	node *p=q->head;
	q->head=q->head->next;
	q->size--;
	if(queue_isempty(q)) q->tail=NULL;
	int x=p->data;
	free(p);
	return x;
}

static int _thread_cond_signal(cond_t *cond, int broadcast)
{
    /* consistency checks */
    if (cond == NULL)
        return_errno(FALSE, EINVAL);
    if (!(cond->cn_state & THREAD_COND_INITIALIZED))
        return_errno(FALSE, EDEADLK);

    // do something only if there is at least one waiters (POSIX semantics)
    if (cond->cn_waiters > 0) {
      // signal the condition
      do {
	thread_t *t = dequeue(&cond->wait_queue);
	assert (t != NULL);
	thread_resume(t);   // t could also be a timed out thread, but it doesn't matter
	cond->cn_waiters--;
      } while (broadcast && !queue_isempty(&cond->wait_queue));
      
      // and give other threads a chance to grab the CPU 
      CAP_SET_SYSCALL();
      thread_yield();
      CAP_CLEAR_SYSCALL();
    }

    /* return to caller */
    return TRUE;
}

int main (int argc, char **argv) {
   execname = basename(argv[0]);
   queue *the_queue = queue_new();

   if (argc < 2) {
      putinqueue(the_queue, stdin, "-");
   } else {
      for (int argi = 1; argi < argc; ++argi) {
         if (strcmp(argv[argi], "-") == 0) {
            putinqueue(the_queue, stdin, "-");
         } else {
            putfileinqueue(the_queue, argv[argi]);
         }
      }
   }

   while (!queue_isempty(the_queue)) {
      queue_item_t temp = queue_remove(the_queue);
      printf("%s\n", temp);
      free(temp);
   }
   queue_free(the_queue);

   return exit_status;
}

int main (int argc, char **argv) {
   execname = basename(argv[0]);
   queue *the_queue = queue_new();

   if (argc < 2) {
      putinqueue(the_queue, stdin, "-");
   } else {
      for (int argi = 1; argi < argc; ++argi) {
         if (strcmp(argv[argi], "-") == 0) {
            putinqueue(the_queue, stdin, "-");
         } else {
            putfileinqueue(the_queue, argv[argi]);
         }
      }
   }

   while (!queue_isempty(the_queue)) {
	   char* tmp = queue_remove(the_queue);  //assign a temp char pointer to point at the item
      printf("%s\n", tmp);                  //print the item
	   free(tmp);                            //free the pointer
   }

   queue_free(the_queue);                   //free the queue
   return exit_status;
}

void queue_destroy (queue_t *queue)
{
	void *tmp;

	if (queue != NULL)
	{
		//Tell our queue manager that we're done.
		stop_managing_queue(queue);

		CHECK_AND_LOCK_MUTEX(queue->mutex);
		while (!queue_isempty (queue))
		{
			if ((tmp = queue_pop(queue)) == NULL)
			{
				break;
			}
			else
			{
				free(tmp);
			}
		}
		/* Free the dummy node too */
		free(queue->front);
		CHECK_AND_UNLOCK_MUTEX(queue->mutex);
		SDL_DestroyMutex(queue->mutex);
#ifdef	NEW_TEXTURES
		SDL_DestroyCond(queue->condition);
#endif	/* NEW_TEXTURES */
		free (queue);
	}
}

bool gatt_db_isempty(struct gatt_db *db)
{
	if (!db)
		return true;

	return queue_isempty(db->services);
}

void *queue_pop (queue_t *queue)
{
	node_t *oldnode;
	void *item;

	if (queue == NULL || queue_isempty(queue))
	{
		return NULL;
	}
	else
	{
		CHECK_AND_LOCK_MUTEX(queue->mutex);
		oldnode = queue->front->next;
		item = oldnode->data;
		/* Check if removing the last node from the queue */
		if (queue->front->next->next == NULL)
		{
			queue->rear = queue->front;
		}
		else
		{
			queue->front->next = queue->front->next->next;
		}
		free(oldnode);
		queue->nodes--;
		CHECK_AND_UNLOCK_MUTEX(queue->mutex);
		return item;
	}
}

object* dequeue(queue *q) {
    int sz;
    queue_node *p;
    object *obj = NULL;

    if (q == NULL) {
        return NULL;
    }

    if (!queue_isempty(q)) {
        p = q->head;
        q->head = q->head->next;
        if (q->head == NULL) {
            q->rear = NULL;
        }
        obj = p->elem;

        sz = q->cache_size;
        if (sz < QUEUE_CACHE_SIZE) {
            /* cache this node */
            q->cache[sz++] = p;
            q->cache_size = sz;
        } else {
            sc_free(p);
        }
    }
    return obj;
}

const Article* queue_peek(const Queue *queue) {
    if (!queue_isempty(queue)) {
        const Article *const_ptr = node_get_const_ptr(queue->bottom);
        return const_ptr;
    }
    return 0;
}

static void export_service(struct gatt_db_attribute *attr, void *user_data)
{
	struct btd_gatt_client *client = user_data;
	struct service *service;

	if (gatt_db_service_get_claimed(attr))
		return;

	service = service_create(attr, client);
	if (!service)
		return;

	if (!create_characteristics(attr, service)) {
		error("Exporting characteristics failed");
		unregister_service(service);
		return;
	}

	queue_push_tail(client->services, service);

	/*
	 * Asynchronously update the "Characteristics" property of the service.
	 * If there are any pending reads to obtain the value of the "Extended
	 * Properties" descriptor then wait until they are complete.
	 */
	if (!service->chrcs_ready && queue_isempty(service->pending_ext_props))
		service->idle_id = g_idle_add(set_chrcs_ready, service);
}

static char *test_queue_empty() {
	queue_t queue;
	queue_init(&queue);

	mu_assert("Error: queue not empty", queue_isempty(&queue));
	queue_destroy(&queue);
	return 0;
}

static void* mevent_start_base_entry(void *arg)
{
    int rv;
    struct timespec ts;
    struct queue_entry *q;

    struct event_entry *e = (struct event_entry*)arg;
    int *mypos = _tls_get_mypos();
    *mypos = e->cur_thread;

    mtc_dbg("I'm %s %dnd thread", e->name, *mypos);

    for (;;) {
        /* Condition waits are specified with absolute timeouts, see
         * pthread_cond_timedwait()'s SUSv3 specification for more
         * information. We need to calculate it each time.
         * We sleep for 1 sec. There's no real need for it to be too
         * fast (it's only used so that stop detection doesn't take
         * long), but we don't want it to be too slow either. */
        mutil_utc_time(&ts);
        ts.tv_sec += 1;

        rv = 0;
        queue_lock(e->op_queue[*mypos]);
        while (queue_isempty(e->op_queue[*mypos]) && rv == 0) {
            rv = queue_timedwait(e->op_queue[*mypos], &ts);
        }

        if (rv != 0 && rv != ETIMEDOUT) {
            errlog("Error in queue_timedwait()");
            /* When the timedwait fails the lock is released, so
             * we need to properly annotate this case. */
            __release(e->op_queue[*mypos]->lock);
            continue;
        }

        q = queue_get(e->op_queue[*mypos]);
        queue_unlock(e->op_queue[*mypos]);

        if (q == NULL) {
            if (e->loop_should_stop) {
                break;
            } else {
                continue;
            }
        }

        mtc_dbg("%s %d process", e->name, *mypos);

        e->process_driver(e, q, *mypos);

        /* Free the entry that was allocated when tipc queued the
         * operation. This also frees it's components. */
        queue_entry_free(q);
    }

    return NULL;
}

static inline void job_send_status(jobpool *jp,uint32_t jobid,uint8_t status) {
	zassert(pthread_mutex_lock(&(jp->pipelock)));
	if (queue_isempty(jp->statusqueue)) {	// first status
		eassert(write(jp->wpipe,&status,1)==1);	// write anything to wake up select
	}
	queue_put(jp->statusqueue,jobid,status,NULL,1);
	zassert(pthread_mutex_unlock(&(jp->pipelock)));
	return;
}