int main() {
        queue_t *q;
        task_t t1 = { test1, (void *) 0};
        task_t t2 = { test2, (void *) 0};
        task_t t;

        int rc = queue_init(&q);
        die(rc);
        assert(q != NULL);

        rc = queue_add(q, &t1);
        die(rc);
        rc = queue_add(q, &t2);
        die(rc);
        assert(!queue_is_empty(q));
        
        rc = queue_remove(q, &t);
        die(rc);
        assert(t.task == t1.task);
        rc = queue_remove(q, &t);
        die(rc);
        assert(t.task == t2.task);

        rc = queue_remove(q, &t);
        assert(rc < 0);
        assert(queue_is_empty(q));

        queue_destroy(&q);

        printf("Success!\n");
        return 0;

}

static void traverse(queue *q, hash_table *h, int fsid, stream_fn sfn ) {

	void obj_callback(int fsid, struct mfs_subobj_header *obj,
			  struct mfs_attr_header *attr, void *data)
	{
		int i;
		char *p = data;
		struct mfs_obj_attr *objattr;
	
		if (!attr) {
			return;
		}

		switch (attr->eltype>>6) {
		case TYPE_FILE:
			// Save on queue of objects to process later
			for (i=0;i<(attr->len-4)/4;i++)
				queue_add(q, ntohl(*(int *)&p[i*4]));
			break;

		case TYPE_OBJECT:
			objattr = (struct mfs_obj_attr *)p;
			for (i=0;i<(attr->len-4)/sizeof(*objattr);i++) {
				int  id = ntohl(objattr->fsid);
				if (id != fsid)
					queue_add(q, id);
				objattr++;
			}
			break;
		}
	}

static void do_graph_bfs(Graph g, Vertex v, Visitor visit)
{
	uint8_t vert_state[MAX_VERTS] = {1};
    QueueResult res;
    uint8_t u = v.id, w;

    Queue queue = queue_new(0);
    Queue *q = &queue;

    queue_add(q, u, &res);

    while (!queue_empty(q)) {
        queue_remove(q, &res);
        assert(res.status == QUEUE_OK);
        
        u = res.data;
        if (!VISITED_VERTEX(vert_state[u])) {
            vert_state[u] = MARK_VISITED_VERTEX(vert_state[u]);
            
            /* call the function that is interested in the visited vertex */
            visit(vertex_new(u, g.labels[u], 0));
            
            /* push each neighbors of vertex u on the stack */
            for (w = 0; w < g.vc; ++w) {
            	if (w != u && g.adj[u][w]) {
            	    queue_add(q, w, &res);
            	    assert(res.status == QUEUE_OK);
                }
            }
        }
    }
}

static void test_more_than_size(void)
{
  queue *q = queue_new(3);

  queue_add(q, NULL);
  queue_add(q, NULL);
  queue_add(q, NULL);

  info("count = %d", queue_count(q));
  assert(queue_count(q) == 3);

  queue_remove(q);
  info("count = %d", queue_count(q));
  assert(queue_count(q) == 2);

  queue_add(q, NULL);
  info("count = %d", queue_count(q));
  assert(queue_count(q) == 3);

  queue_remove(q);
  info("count = %d", queue_count(q));
  assert(queue_count(q) == 2);

  queue_remove(q);
  info("count = %d", queue_count(q));
  assert(queue_count(q) == 1);

  queue_remove(q);
  info("count = %d", queue_count(q));
  assert(queue_count(q) == 0);
}

static void test_right_value(void)
{
  int a = 10;
  int b = 20;
  int c = 30;
  int *item;
  queue *q = queue_new(3);

  queue_add(q, &a);
  queue_add(q, &b);
  queue_add(q, &c);

  item = queue_remove(q);
  info("item = %d", *item);
  assert(*item == 10);

  queue_add(q, &a);

  item = queue_remove(q);
  info("item = %d", *item);
  assert(*item == 20);

  item = queue_remove(q);
  info("item = %d", *item);
  assert(*item == 30);

  item = queue_remove(q);
  info("item = %d", *item);
  assert(*item == 10);

  info("count = %d", queue_count(q));
  assert(queue_count(q) == 0);
}

void graph_bfs(graph g,int source) {
    int i,j,u;
    queue q = queue_new(g->size);
    for(i = 0; i < g->size; i++) {
        g->vertices[i].state = UNVISITED;
        g->vertices[i].distance = -1;
        g->vertices[i].predecessor = -1;
    }
    g->vertices[source].state = VISITED_SELF;
    g->vertices[source].distance = 0;

    queue_add(q,source);
    while(queue_empty(q) == 0) {
        u = queue_remove(q);
        for(j = 0; j < g->size; j++) {
            if(g->edges[u][j] == 1 && g->vertices[j].state == UNVISITED) {
                g->vertices[j].state = VISITED_SELF;
                g->vertices[j].distance = 1 + g->vertices[u].distance;
                g->vertices[j].predecessor = u;
                queue_add(q,j);
            }
            g->vertices[u].state = VISITED_DESCENDANTS;
        }
    }
}

void handle_combination(char * buff){


    int i, j;
    int app[4];

    for(i=0;i<4;i++)
        app[i]=comb[i];

 

    wrong = 0;
    correct = 0;
    

    for(i=0; i<4; i++){
        if(buff[i]==app[i]){
            correct++;
            app[i] = -1;
        }

        else{
            for(j=0; j<4; j++){
                if(app[i]==buff[j] && app[i]!=-1){
                    wrong++;
                    app[i] = -1;
                }
            }
        }
    }

   

    if(correct==4){
        queue_add(&queue_l, cd, CL_WIN, 0, 0 );
        FD_SET(cd, &write_set);

        printf("mi dispiace, hai perso\n");
        command_mode = 1;
        cprintf("");




    }
    
    else{

    snprintf(bb, 100, "%s dice: cifre giuste al posto giusto: %d , cifre giuste al posto sbagliato %d ", player_info.name, correct, wrong);

    queue_add(&queue_l, cd, CL_ANS, sizeof(bb), bb);
    FD_SET(cd, &write_set);
    }
       

    
}

/**
 * Funzione di gestione del missile lanciato dalla navicella
 */
void *missile_task(void *args)
{	
	object_data_t missile;

	// Riempio la struttura con le info del missile
	missile = *((object_data_t *) (args));
	missile.type = OT_MISSILE;
	missile.size = 1;
	
	// Invia al controllo la posizione iniziale
	queue_add(missile);
	
	// Indica se il missile e' in vita	
	int alive = 1;
		
	// Eseguo sino a che l'oggetto non esce dallo schermo, o sino a che non riceve un segnale d'uscita
	while((! (missile.x < 0 || missile.y < 0 || missile.x > SCREEN_WIDTH || missile.y > SCREEN_HEIGHT)) && alive)
	{	
		// Leggo lo stato delle collisioni
		object_type_t coll_type = get_collision_state(missile.id);
		
		if(coll_type == OT_DELETE)
			alive = 0;
		
		// Faccio salire il missile di una posizione y			
		missile.y -= 1;
		
		// A seconda della direzione, mi sposto anche in orizzontale
		switch(missile.dir)
		{
			case LEFT:
				missile.x -= 1;
				break;
				
			case RIGHT:
				missile.x += 1;
				break;
				
		}
	
		// Invia al controllo la posizione attuale
		queue_add(missile);
		
		// Attende un tempo prefissato, prima di fare un altra iterazione
		usleep(SLEEP_UTIME);
	}
	
	// Siamo fuori dal ciclo, diciamo al controllo di cancellare il missile
	missile.type = OT_DELETE;
	queue_add(missile);
	
	// Termino il thread
	pthread_exit(NULL);
}

static void test_queue_full(void)
{
  char *a = "hello";
  char *b = "goodbye";
  queue *q = queue_new(1);

  assert(queue_add(q, a));
  info("count = %d", queue_count(q));
  assert(queue_count(q) == 1);
  assert(!queue_add(q, b));
  info("count = %d", queue_count(q));
  assert(queue_count(q) == 1);

  queue_destroy(q);
}

void camd_process_packet(struct ts *ts, struct camd_msg *msg) {
	if (!msg)
		return;
	if (ts->camd.constant_codeword)
		return;
	msg->ts = ts;
	if (ts->camd.thread) {
		if (msg->type == EMM_MSG)
			queue_add(ts->camd.emm_queue, msg);
		if (msg->type == ECM_MSG)
			queue_add(ts->camd.ecm_queue, msg);
		queue_add(ts->camd.req_queue, msg);
	} else {
		camd_do_msg(msg);
	}
}

void send_frames(int link){
	//printf("Send frames called for link : %d\n", link);
	size_t len = sizeof(FRAME);
	CnetTime timeout;
	FRAME *f = queue_remove(links[link].sender, &len);
	switch(f->payload.kind){
	case DL_DATA :
		if(!links[link].ack_received[f->payload.A]) {
			CHECK(CNET_write_physical(link, (char*)f, &len));
			timeout = (len*((CnetTime)8000000)/linkinfo[link].bandwidth + linkinfo[link].propagationdelay);
			start_timer(link, timeout);
			queue_add(links[link].sender, f, len);
		}
		else {
			if(queue_nitems(links[link].sender) > 0)
				send_frames(link);
		}
		break;
	case DL_ACK :
		CHECK(CNET_write_physical(link, (char*)f, &len));
                timeout = (len*((CnetTime)8000000)/linkinfo[link].bandwidth + linkinfo[link].propagationdelay);
                start_timer(link, timeout);
		break;
	case RT_DATA:
	//printf("RT packet sending on link : %d\n", link);
		CHECK(CNET_write_physical(link, (char*)f, &len));
                timeout = (len*((CnetTime)8000000)/linkinfo[link].bandwidth + linkinfo[link].propagationdelay);
                start_timer(link, timeout);
		break;
	}
	free(f);
}

/*
 * Create a new thread.
 *
 * This function allocates a new context, and a new Thread
 * structure, and it adds the thread to the Ready queue.
 */
Thread * sthread_create(void (*f)(void *arg), void *arg) {
    Thread *threadp;
    void *memory;

    /* Create a stack for use by the thread */
    memory = (void *) malloc(DEFAULT_STACKSIZE);
    if (memory == NULL) {
        fprintf(stderr, "Can't allocate a stack for the new thread\n");
        exit(1);
    }

    /* Create a thread struct */
    threadp = (Thread *) malloc(sizeof(Thread));
    if (threadp == NULL) {
        fprintf(stderr, "Can't allocate a thread context\n");
        exit(1);
    }

    /* Initialize the thread */
    threadp->state = ThreadReady;
    threadp->memory = memory;
    threadp->context = __sthread_initialize_context(
        (char *) memory + DEFAULT_STACKSIZE, f, arg);
    queue_add(threadp);

    return threadp;
}

static void queue_add_text(char *txt, size_t length)
{
	struct espeak_entry_t *entry;
	int added = 0;

	entry = allocMem(sizeof(struct espeak_entry_t));
	entry->cmd = CMD_SPEAK_TEXT;
	entry->adjust = ADJ_SET;
	entry->buf = strdup(txt);
	if (!entry->buf) {
		perror("unable to allocate space for text");
		free(entry);
		return;
	}
	entry->len = length;
	pthread_mutex_lock(&queue_guard);
	added = queue_add(synth_queue, (void *) entry);
	if (!added) {
		free(entry->buf);
		free(entry);
	} else {
		pthread_cond_signal(&runner_awake);
	}

	pthread_mutex_unlock(&queue_guard);
}

/*
 * Create a new thread.
 *
 * This function allocates a new context, and a new Thread
 * structure, and it adds the thread to the Ready queue.
 */
Thread * sthread_create(void (*f)(void *arg), void *arg) {
    /* Allocate memory for a new thread. */
    Thread * new_thread = (Thread *) malloc(sizeof(Thread));

    /* Allocate memory for a stack for the thread. */
    void * new_stack = (void *) malloc(DEFAULT_STACKSIZE);

    /* Make sure mallocs worked. */
    if (new_thread == NULL) {
        printf("Thread was not allocated.\n");
    }

    if (new_stack == NULL) {
        printf("Stack was not allocated.\n");
    }

    /* Set thread's stack. */
    new_thread->memory = new_stack;

    /* Set thread to ready. */
    new_thread->state = ThreadReady;

    /* Set contect to end of stack (because it grows down). */
    new_thread->context = __sthread_initialize_context((char *) new_stack +
        DEFAULT_STACKSIZE, f, arg);

    /* Add to queue. */
    queue_add(new_thread);

    return new_thread;
}

int producer()
{
	pthread_mutex_lock(ProgressIndicatorsMutex_m_spinlock, ProgressIndicatorsMutex_m_count);
	NumBlocks = 0;
	InBytesProduced = 0;
	pthread_mutex_unlock(ProgressIndicatorsMutex_m_spinlock, ProgressIndicatorsMutex_m_count);

	while (1)
	{
		if (syncGetTerminateFlag() != 0)
		{
			return -1;
		}
		pthread_mutex_lock(fifo_mut_m_spinlock, fifo_mut_m_count);
		while (fifo_full)
		{
			pthread_cond_wait(fifo_mut_m_spinlock, fifo_mut_m_count);

			if (syncGetTerminateFlag() != 0)
			{
				pthread_mutex_unlock(fifo_mut_m_spinlock, fifo_mut_m_count);
				return -1;
			}
		}
		queue_add(fifo);
		pthread_cond_signal();

		pthread_mutex_lock(ProgressIndicatorsMutex_m_spinlock, ProgressIndicatorsMutex_m_count);
		++NumBlocks;
		InBytesProduced += inSize;
		pthread_mutex_unlock(ProgressIndicatorsMutex_m_spinlock, ProgressIndicatorsMutex_m_count);
		
		pthread_mutex_unlock(fifo_mut_m_spinlock, fifo_mut_m_count);
	} // while
}

static int create_main_tcb(){

  mythread_t main_tcb = (mythread_t)malloc(sizeof(struct mythread));

  if(main_tcb == NULL){
    return -1;
  }

  /* Initialize main TCB */

  main_tcb -> tid = (pid_t) mythread_gettid();
  main_tcb -> start_func = NULL;
  main_tcb -> arg = NULL;
  main_tcb -> returnValue = NULL;
  main_tcb -> state = READY;
  

  /* Initialize futex for main thread */
  futex_init(&main_tcb->futex,0);
 
  /* Addition of this entry into the global queue is required */
  queue_add(main_tcb);
  
  return 0;  
}

boolean forward_to_packet_queue(struct sniffed_packet_descr_t *descr)
{	
	pid_t p;
	
	/* put timestamp on packet */

	getnstimeofday(&descr->time_received);	
			
	write_lock(&packet_queue_lock);
	if(packet_receiver.sniffed_packet_queue)
		queue_add(packet_receiver.sniffed_packet_queue,
			  descr);
	write_unlock(&packet_queue_lock);
	
	/* wake up scan-job-manager */
		
	spin_lock(&scan_job_man_thread_lock);
	if(scan_job_man_thread)
		p = scan_job_man_thread->pid;
	else{
		spin_unlock(&scan_job_man_thread_lock);
		return FALSE;
	}
	spin_unlock(&scan_job_man_thread_lock);
		
	send_sig(SIGINT, scan_job_man_thread, 1);
	return TRUE;	
	
}

static void dns_handle_local() {
    struct sockaddr *src_addr = malloc(sizeof(struct sockaddr));
    socklen_t src_addrlen = sizeof(struct sockaddr);
    uint16_t query_id;
    ssize_t len;
    int i;
    const char *question_hostname;
    ns_msg msg;
    len = recvfrom(local_sock, global_buf, BUF_SIZE, 0, src_addr, &src_addrlen);
    if (len > 0) {
        if (ns_initparse((const u_char *)global_buf, len, &msg) < 0) {
            ERR("ns_initparse");
            free(src_addr);
            return;
        }
        // parse DNS query id
        // TODO generate id for each request to avoid conflicts
        query_id = ns_msg_id(msg);
        question_hostname = hostname_from_question(msg);
        LOG("request %s\n", question_hostname);
        id_addr_t id_addr;
        id_addr.id = query_id;
        id_addr.addr = src_addr;
        id_addr.addrlen = src_addrlen;
        queue_add(id_addr);
        for (i = 0; i < dns_servers_len; i++) {
            if (-1 == sendto(remote_sock, global_buf, len, 0,
                             dns_server_addrs[i].addr, dns_server_addrs[i].addrlen))
                ERR("sendto");
        }
    }
    else
        ERR("recvfrom");
}

/* function-decoder with 14-bit address */
int comp_nmra_fb14(int address, int group, int* f) {

   char addrbyte1[9] = {0};
   char addrbyte2[9] = {0};
   char funcbyte[9] = {0};
   char funcbyte2[9] = {0};
   char errdbyte[9] = {0};
   char dummy[9] = {0};
   char bitstream[BUFFERSIZE];
   char packetstream[PKTSIZE];

   int adr       = 0;
   int i,j;

   adr=address;

   /* no special error handling, it's job of the clients */
   if (address<1 || address>10239)
      return 1;

   calc_14bit_address_byte(addrbyte1, addrbyte2, address);
   calc_function_group(funcbyte, funcbyte2, group, f);

   xor_two_bytes(dummy, addrbyte1, addrbyte2);
   xor_two_bytes(errdbyte, dummy, funcbyte);


   /* putting all together in a 'bitstream' (char array) (functions) */
   memset(bitstream, 0, 100);
   strcat(bitstream, preamble);
   strcat(bitstream, "0");
   strcat(bitstream, addrbyte1);
   strcat(bitstream, "0");
   strcat(bitstream, addrbyte2);
   strcat(bitstream, "0");
   strcat(bitstream, funcbyte);
   strcat(bitstream, "0");
   if(funcbyte2[0] != 0 ) {
     char tmp[9] = {0};
     strcpy( tmp, errdbyte );
     xor_two_bytes(errdbyte, tmp, funcbyte2);
     strcat(bitstream, funcbyte2);
     strcat(bitstream, "0");
   }
   strcat(bitstream, errdbyte);
   strcat(bitstream, "1");

   TraceOp.trc( "nmra", TRCLEVEL_BYTE, __LINE__, 9999,
       "14 bit addr bitstream: %s", bitstream);

   j=translateBitstream2Packetstream(bitstream, packetstream);

   if (j>0) {
      update_NMRAPacketPool(adr+ADDR14BIT_OFFSET,NULL,0,packetstream,j);
      queue_add(adr+ADDR14BIT_OFFSET,packetstream,QNBLOCOPKT,j);
      return 0;
   }

   return 1;
}

int main() {
    
    bt_node *tmp;
    queue *q = queue_init();
    binary_tree *bt = malloc(sizeof(bt_node));
    bt->Element = 0;
    bt->left = malloc(sizeof(bt_node));
    bt->left->Element = 1;
    bt->right = malloc(sizeof(bt_node));
    bt->right->Element = 2;
    tmp = bt->left;
    tmp->left = malloc(sizeof(bt_node));
    tmp->left->Element = 3;
    tmp->right = malloc(sizeof(bt_node));
    tmp->right->Element = 4;
    tmp = tmp->left;
    tmp->left = nil;
    tmp->right = malloc(sizeof(bt_node));
    tmp->right->Element = 6;
    tmp = tmp->right;
    tmp->left = nil;
    tmp->right = nil;
    tmp = bt->left->right;
    tmp->left = nil;
    tmp->right = nil;
    tmp = bt->right;
    tmp->left = malloc(sizeof(bt_node));
    tmp->left->Element = 5;
    tmp->right = nil;
    tmp = tmp->left;
    tmp->left = nil;
    tmp->right = nil;
    
    queue_add(bt, q);
    while (!setjmp(buf)) {
        tmp = queue_del(q);
        printf("visited node index: %d\n", tmp->Element);
        if (tmp->left != nil) {
            queue_add(tmp->left, q);
        }
        if (tmp->right != nil) {
            queue_add(tmp->right, q);
        }
    }
    
    return 0;
}