enum playlist_result
playlist_delete_range(struct playlist *playlist, unsigned start, unsigned end)
{
	const struct song *queued;

	if (start >= queue_length(&playlist->queue))
		return PLAYLIST_RESULT_BAD_RANGE;

	if (end > queue_length(&playlist->queue))
		end = queue_length(&playlist->queue);

	if (start >= end)
		return PLAYLIST_RESULT_SUCCESS;

	queued = playlist_get_queued_song(playlist);

	do {
		playlist_delete_internal(playlist, --end, &queued);
	} while (end != start);

	playlist_increment_version(playlist);
	playlist_update_queued_song(playlist, queued);

	return PLAYLIST_RESULT_SUCCESS;
}

static VALUE
queue_do_pop(VALUE self, int should_block)
{
    struct waiting_delete args;
    args.waiting = GET_QUEUE_WAITERS(self);
    args.th	 = rb_thread_current();

    while (queue_length(self) == 0) {
	if (!should_block) {
	    rb_raise(rb_eThreadError, "queue empty");
	}
	else if (queue_closed_p(self)) {
	    return queue_closed_result(self);
	}
	else {
	    assert(queue_length(self) == 0);
	    assert(queue_closed_p(self) == 0);

	    rb_ary_push(args.waiting, args.th);
	    rb_ensure(queue_sleep, (VALUE)0, queue_delete_from_waiting, (VALUE)&args);
	}
    }

    return rb_ary_shift(GET_QUEUE_QUE(self));
}

enum playlist_result
playlist_move_range(struct playlist *playlist,
		    unsigned start, unsigned end, int to)
{
	const struct song *queued;
	int currentSong;

	if (!queue_valid_position(&playlist->queue, start) ||
		!queue_valid_position(&playlist->queue, end - 1))
		return PLAYLIST_RESULT_BAD_RANGE;

	if ((to >= 0 && to + end - start - 1 >= queue_length(&playlist->queue)) ||
	    (to < 0 && abs(to) > (int)queue_length(&playlist->queue)))
		return PLAYLIST_RESULT_BAD_RANGE;

	if ((int)start == to)
		/* nothing happens */
		return PLAYLIST_RESULT_SUCCESS;

	queued = playlist_get_queued_song(playlist);

	/*
	 * (to < 0) => move to offset from current song
	 * (-playlist.length == to) => move to position BEFORE current song
	 */
	currentSong = playlist->current >= 0
		? (int)queue_order_to_position(&playlist->queue,
					      playlist->current)
		: -1;
	if (to < 0 && playlist->current >= 0) {
		if (start <= (unsigned)currentSong && (unsigned)currentSong <= end)
			/* no-op, can't be moved to offset of itself */
			return PLAYLIST_RESULT_SUCCESS;
		to = (currentSong + abs(to)) % queue_length(&playlist->queue);
		if (start < (unsigned)to)
			to--;
	}

	queue_move_range(&playlist->queue, start, end, to);

	if (!playlist->queue.random) {
		/* update current/queued */
		if ((int)start <= playlist->current &&
		    (unsigned)playlist->current < end)
			playlist->current += to - start;
		else if (playlist->current >= (int)end &&
			 playlist->current <= to) {
			playlist->current -= end - start;
		} else if (playlist->current >= to &&
			   playlist->current < (int)start) {
			playlist->current += end - start;
		}
	}

	playlist_increment_version(playlist);

	playlist_update_queued_song(playlist, queued);

	return PLAYLIST_RESULT_SUCCESS;
}

G_GNUC_UNUSED
static void
dump_order(const struct queue *queue)
{
	g_printerr("queue length=%u, order:\n", queue_length(queue));
	for (unsigned i = 0; i < queue_length(queue); ++i)
		g_printerr("  [%u] -> %u (prio=%u)\n", i, queue->order[i],
			   queue->items[queue->order[i]].priority);
}

void* helper_thread(void* args)
{
    //Unpack args
    ARG_HOLDER* argholder = (ARG_HOLDER*)(args);
    rule_node_t* queue = argholder->rule_queue;
    int queue_size = argholder->max_queue_length;
    argholder->threads_not_done++;
    int threadno = argholder->threads_not_done;
	int ql = queue_length(queue) - 1;
    while (!argholder->finished_adding || ql > 0)
    {
        //Check queue and "execute" targets on it
	    int full_queue = 0;
	    while (!argholder->done && ql == 0)
	    {
	        //Wait for the queue length to go up
	        cond_wait(&queue_empty, &mutex);
	        ql = queue_length(queue) - 1;
	    }
	    if (ql == queue_size)
	    {
	        full_queue = 1;
	    }
	    if (ql > 0 && !argholder->done)
        {
            //Remove the first target from the queue and "execute" it
            sem_wait(&sem_lock);
            rule_node_t* first_node = queue->next;
            rule_node_t* qnode = first_node->next;
            rule_t* cur_rule = first_node->rule;
            queue->next = qnode;
            free(first_node);
            sem_post(&sem_lock);
            //If the queue was full, signal the main thread that it is not
            if (full_queue)
            {
                pthread_cond_broadcast(&queue_full);
            }
            
            fake_exec(cur_rule);
            
            //Put rule on output queue
            rule_node_t* out_first = argholder->output_queue;
            rule_node_t* output = (rule_node_t*)malloc(sizeof(rule_node_t));
            output->rule = cur_rule;
            output->next = out_first;
            argholder->output_queue = output;
        }
        ql = queue_length(queue) - 1;
    }
    argholder->done = 1;
    argholder->threads_not_done--;
    pthread_cond_broadcast(&queue_empty);
    pthread_cond_signal(&finished_execution);
    //printf("Thread %d exiting.\n", threadno);
    return NULL;
}

static int decide_frame_length(PullupContext *s)
{
    PullupField *f0 = s->first;
    PullupField *f1 = f0->next;
    PullupField *f2 = f1->next;
    PullupField *f;
    int i, l, n;

    if (queue_length(s->first, s->last) < 4)
        return 0;

    f = s->first;
    n = queue_length(f, s->last);
    for (i = 0; i < n - 1; i++) {
        if (i < n - 3)
            compute_breaks(s, f);

        compute_affinity(s, f);

        f = f->next;
    }

    if (f0->affinity == -1)
        return 1;

    l = find_first_break(f0, 3);

    if (l == 1 && s->strict_breaks < 0)
        l = 0;

    switch (l) {
    case 1:
        return 1 + (s->strict_breaks < 1 && f0->affinity == 1 && f1->affinity == -1);
    case 2:
        /* FIXME: strictly speaking, f0->prev is no longer valid... :) */
        if (s->strict_pairs
            && (f0->prev->breaks & BREAK_RIGHT) && (f2->breaks & BREAK_LEFT)
            && (f0->affinity != 1 || f1->affinity != -1) )
            return 1;
        return 1 + (f1->affinity != 1);
    case 3:
        return 2 + (f2->affinity != 1);
    default:
        /* 9 possibilities covered before switch */
        if (f1->affinity == 1)
            return 1; /* covers 6 */
        else if (f1->affinity == -1)
            return 2; /* covers 6 */
        else if (f2->affinity == -1) { /* covers 2 */
            return (f0->affinity == 1) ? 3 : 1;
        } else {
            return 2; /* the remaining 6 */
        }
    }
}

static void
check_descending_priority(G_GNUC_UNUSED const struct queue *queue,
			 unsigned start_order)
{
	assert(start_order < queue_length(queue));

	uint8_t last_priority = 0xff;
	for (unsigned order = start_order; order < queue_length(queue); ++order) {
		unsigned position = queue_order_to_position(queue, order);
		uint8_t priority = queue->items[position].priority;
		assert(priority <= last_priority);
		(void)last_priority;
		last_priority = priority;
	}
}

int main(int argc, char const *argv[])
{
	void * queue;
	queue = queue_create();
	
	teacher_t t[50];

	for (int i = 0 ; i < 50; i++)
	{
		t[i].age = i;
		queue_insert(queue, &t[i]);
	}
	
	teacher_t * p;
	int k = queue_length(queue);
	for (int i = 0; i < k-1; i++)
	{
		p = (teacher_t *)queue_delete(queue);
		fprintf(stdout, "%d ", p->age);
	} 

	fprintf(stdout, "\n");

	p = (teacher_t *)queue_head(queue);
	fprintf(stdout, "%d ", p->age);
	fprintf(stdout, "\n");
	queue_delete(queue);

	for (int i = 0 ; i < 50; i++)
	{
		t[i].age = i + 100;
		queue_insert(queue, &t[i]);
	}
	if (!queue_empty(queue))
		fprintf(stdout, "queue is not empty\n");
	k = queue_length(queue);
	for (int i = 0; i < k; i++)
	{
		p = (teacher_t *)queue_delete(queue);
		fprintf(stdout, "%d ", p->age);
	} 
	fprintf(stdout, "\n");
	if (queue_empty(queue))
		fprintf(stdout, "queue not empty\n");

	queue_destroy(queue);
	return 0;
}

int threadpool_add_task(threadpool *tp, task_function task_fn, void *arg) {
	int ret, retval;
	task *next_task;

	if (!tp || !task_fn) {
		tp_error("got a NULL argument: tp=%p, task_fn=%p\n", tp, task_fn);
		return -1;
	}

	kp_kpalloc((void **)(&next_task), sizeof(task), tp->use_nvm);
	if (!next_task) {
		tp_error("malloc(next_task) failed\n");
		return -1;
	}
	next_task->task_fn = task_fn;
	next_task->arg = arg;
	next_task->use_nvm = tp->use_nvm;

	retval = 0;
	kp_mutex_lock("add task", tp->lock);

	ret = queue_enqueue(tp->task_queue, (void *)next_task, task_free_fn);
	if (ret != 0) {
		tp_error("queue_enqueue returned error=%d\n", ret);
		retval = -1;
	} else {
		ret = pthread_cond_signal(tp->task_available);
		if (ret != 0) {
			tp_error("pthread_cond_signal(task_available) returned error=%d\n",
					ret);
			retval = -1;
		} else {
			tp_debug("successfully added a task and signaled task_available "
					"condition\n");
		}
	}
	tp->tasks_pending++;
#ifdef TP_ASSERT
	if (tp->tasks_pending != queue_length(tp->task_queue)) {
		tp_die("mismatch: tasks_pending=%u, but task_queue length=%u\n",
				tp->tasks_pending, queue_length(tp->task_queue));
	}
#endif

	kp_mutex_unlock("add task", tp->lock);

	return retval;
}

int main()
{
    Queue* q = queue_new("example", "localhost", 6379, 0, NULL);
    queue_connect(q);
    printf("Current Queue Length: %lld\n", queue_length(q));

    /*
    char *data = calloc(20, sizeof(char));
    strncpy(data, "\"ac\"", 7);
    Task *t = task_new(data, "0");

    Job *j = queue_enqueue(q, t);

    printf("Got job: %s\n", j->urn);

    task_destroy(t);
    job_destroy(j);
    */

    Task* t = queue_wait(q, 0);


    printf("Received: %s URN: %s \n", t->data, t->urn);


    task_destroy(t);    
    queue_destroy(q);
    return 0;
}

int service_mqlen(uint32_t handle) {
	struct service *s = service_grab(handle);
	if (!s) return 0;
	int mqlen = queue_length(s->queue);
	service_release(handle);
	return mqlen;
}

unsigned int threadpool_get_task_count_active(threadpool *tp) {
	if (!tp) {
		tp_error("got a NULL argument: tp=%p\n", tp);
		return UINT32_MAX;
	}
	return queue_length(tp->task_queue);
}

static int
get_ports(ClientData clientData,
              Tcl_Interp *interp,
              int argc,
              char *argv[])
{
    char msg[255];
    channel_t *chan;
    int i, n;

    memset(msg, 0, 255);
    n = queue_length(priv_c);
    Tcl_SetResult(interp,NULL,TCL_STATIC);
    for(i = 0; i < n; i++) {
        chan = (channel_t*)queue_get(priv_c, i, Q_KEEP);
        sprintf(msg, "%d", chan->port);
        Tcl_AppendElement(interp,msg);
    }

    UNUSED(clientData);
    UNUSED(argc);
    UNUSED(argv);

    return TCL_OK;
}

void*
queue_dequeue(
    queue_holder_t      *queue,
    size_t              offset)
{
    queue_node_t    *return_node; 
    
    if (queue == NULL) {
        return NULL;
    } 
    
    if (queue_length(queue) == 0) {
        return NULL;
    }
    
    return_node = queue->first;
    queue->first = queue->first->next;
    return_node->next = NULL;
    queue->size--;
    
    if (queue->size == 0) {
        queue->last = NULL;
    }
#if QUEUE_DEBUG() > 0
    printf("return_node: %p, offset: %lu, return_node-offset: %p\n",
            return_node, offset, (void*)return_node - offset);
#endif /* QUEUE_DEBUG() > 0 */

    /* need to cast to void prior to returning since
     * the ptr_math should not be done on the
     * queue_node_t
     */
    return (void *)return_node - offset; 
}

enum playlist_result
spl_save_queue(const char *name_utf8, const struct queue *queue)
{
	char *path_fs;
	FILE *file;

	if (map_spl_path() == NULL)
		return PLAYLIST_RESULT_DISABLED;

	if (!spl_valid_name(name_utf8))
		return PLAYLIST_RESULT_BAD_NAME;

	path_fs = map_spl_utf8_to_fs(name_utf8);
	if (path_fs == NULL)
		return PLAYLIST_RESULT_BAD_NAME;

	if (g_file_test(path_fs, G_FILE_TEST_EXISTS)) {
		g_free(path_fs);
		return PLAYLIST_RESULT_LIST_EXISTS;
	}

	file = fopen(path_fs, "w");
	g_free(path_fs);

	if (file == NULL)
		return PLAYLIST_RESULT_ERRNO;

	for (unsigned i = 0; i < queue_length(queue); i++)
		playlist_print_song(file, queue_get(queue, i));

	fclose(file);

	idle_add(IDLE_STORED_PLAYLIST);
	return PLAYLIST_RESULT_SUCCESS;
}

static VALUE
rb_szqueue_push(int argc, VALUE *argv, VALUE self)
{
    struct waiting_delete args;
    int should_block = szqueue_push_should_block(argc, argv);
    args.waiting = GET_SZQUEUE_WAITERS(self);
    args.th      = rb_thread_current();

    while (queue_length(self) >= GET_SZQUEUE_ULONGMAX(self)) {
	if (!should_block) {
	    rb_raise(rb_eThreadError, "queue full");
	}
	else if (queue_closed_p(self)) {
	    goto closed;
	}
	else {
	    rb_ary_push(args.waiting, args.th);
	    rb_ensure((VALUE (*)())rb_thread_sleep_deadly, (VALUE)0, queue_delete_from_waiting, (VALUE)&args);
	}
    }

    if (queue_closed_p(self)) {
      closed:
	raise_closed_queue_error(self);
    }

    return queue_do_push(self, argv[0]);
}

bool_e VisionCamFrameManager::Set(VisionCamFrame *fr)
{
    bool_e ret = false_e;
    FrameQueueNode_t *fNode = NULL;
    if( fr && qFrames )
    {
        return true_e;
        Lock(frm, ReadLock_e );
        while( queue_length(qFrames ) )
        {
            queue_read( qFrames, false_e, fNode);

            if( !fNode )
                continue;

            if( fNode && fNode->frame && fNode->frame->mFrameBuff == fr->mFrameBuff )
            {
                memcpy(fNode->frame, fr, sizeof(VisionCamFrame));/// @todo check other possibilities
            }
        }
        Unlock(frm, ReadLock_e );
    }

    return ret;
}

void
playlist_shuffle(struct playlist *playlist, struct player_control *pc,
		 unsigned start, unsigned end)
{
	const struct song *queued;

	if (end > queue_length(&playlist->queue))
		/* correct the "end" offset */
		end = queue_length(&playlist->queue);

	if ((start+1) >= end)
		/* needs at least two entries. */
		return;

	queued = playlist_get_queued_song(playlist);
	if (playlist->playing && playlist->current >= 0) {
		unsigned current_position;
		current_position = queue_order_to_position(&playlist->queue,
	                                                   playlist->current);

		if (current_position >= start && current_position < end)
		{
			/* put current playing song first */
			queue_swap(&playlist->queue, start, current_position);

			if (playlist->queue.random) {
				playlist->current =
					queue_position_to_order(&playlist->queue, start);
			} else
				playlist->current = start;

			/* start shuffle after the current song */
			start++;
		}
	} else {
		/* no playback currently: reset playlist->current */

		playlist->current = -1;
	}

	queue_shuffle_range(&playlist->queue, start, end);

	playlist_increment_version(playlist);

	playlist_update_queued_song(playlist, pc, queued);
}

/**
  Prints the initial conditions of the raection system.
*/
void print_initial_conditions()
{
  int i;
  
  printf( "\nThe initial conditions are:\n\n" );
  for( i=0; i<sys.Ncomp; i++ ) 
  {
    if( X[i] != 0 )
      printf( "[%s] \t= %d\n", Xname[i], X[i] );
  }
  if( queue_length() > 0 )
  {
    printf( "The length of the queue is %d.\n", queue_length() );
  }
  printf( "The origin of the timeline is %f.\n", sys.tau_init );
  printf( "\n" );
}

static void exec_write_cb(uint8_t opcode, const void *pdu,
					uint16_t length, void *user_data)
{
	struct bt_gatt_server *server = user_data;
	uint8_t flags;
	uint8_t ecode;
	bool write;
	uint16_t ehandle = 0;

	if (length != 1) {
		ecode = BT_ATT_ERROR_INVALID_PDU;
		goto error;
	}

	flags = ((uint8_t *) pdu)[0];

	util_debug(server->debug_callback, server->debug_data,
				"Exec Write Req - flags: 0x%02x", flags);

	if (flags == 0x00)
		write = false;
	else if (flags == 0x01)
		write = true;
	else {
		ecode = BT_ATT_ERROR_INVALID_PDU;
		goto error;
	}

	if (!write) {
		queue_remove_all(server->prep_queue, NULL, NULL,
						prep_write_data_destroy);
		bt_att_send(server->att, BT_ATT_OP_EXEC_WRITE_RSP, NULL, 0,
							NULL, NULL, NULL);
		return;
	}

	/* If there is more than one prep request, we are in reliable session */
	if (queue_length(server->prep_queue) > 1) {
		struct prep_write_data *prep_data;

		prep_data = queue_find(server->prep_queue,
					find_no_reliable_characteristic, NULL);
		if (prep_data) {
			ecode = BT_ATT_ERROR_REQUEST_NOT_SUPPORTED;
			ehandle = prep_data->handle;
			goto error;
		}
	}

	exec_next_prep_write(server, 0, 0);

	return;

error:
	queue_remove_all(server->prep_queue, NULL, NULL,
						prep_write_data_destroy);
	bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
}