void test_queue_new_free(void)
{
	int i;
	Queue *queue;

	/* Create and destroy a queue */

	queue = queue_new();

	queue_free(queue);

	/* Add lots of values and then destroy */

	queue = queue_new();

	for (i=0; i<1000; ++i) {
		queue_push_head(queue, &variable1);
	}

	queue_free(queue);

	/* Test allocation when there is no free memory */

	alloc_test_set_limit(0);
	queue = queue_new();
	assert(queue == NULL);
}

/**
  @brief Serial Bridge task initialize
  @param[in] port: network port
*/
MEMSPACE
bridge_task_init(int port)
{
	static struct espconn esp_data_config;
	static esp_tcp esp_data_tcp_config;

	if(!(uart_send_queue = queue_new(BUFFER_SIZE)))
		reset();

	if(!(uart_receive_queue = queue_new(BUFFER_SIZE)))
		reset();

	if(!(tcp_data_send_buffer = malloc(BUFFER_SIZE)))
		reset();

	wifi_set_sleep_type(NONE_SLEEP_T);

	tcp_accept(&esp_data_config, &esp_data_tcp_config, port, tcp_data_connect_callback);
	espconn_regist_time(&esp_data_config, 0, 0);
	esp_data_tcp_connection = 0;

	system_os_task(bridge_task, bridge_task_id, bridge_task_queue, bridge_task_queue_length);
	system_os_post(bridge_task_id, 0, 0);

	printf("\nbridge task init done\n");
}

queueSet_t queueSet_new(void){
	queueSet_t set = malloc(sizeof(struct queueSet_s));
	set->queue1 = queue_new();
	set->queue2 = queue_new();
	set->numOfSubscribers = 0;
	return set;
}

void* job_pool_new(uint32_t jobs) {
	int fd[2];
	uint32_t i;
	jobpool* jp;

	if (pipe(fd)<0) {
		return NULL;
	}
       	jp=malloc(sizeof(jobpool));
	passert(jp);
//	syslog(LOG_WARNING,"new pool of workers (%p:%"PRIu8")",(void*)jp,workers);
	jp->rpipe = fd[0];
	jp->wpipe = fd[1];
	jp->workers_avail = 0;
	jp->workers_total = 0;
	jp->workers_term_waiting = 0;
	zassert(pthread_cond_init(&(jp->worker_term_cond),NULL));
	zassert(pthread_mutex_init(&(jp->pipelock),NULL));
	zassert(pthread_mutex_init(&(jp->jobslock),NULL));
	jp->jobqueue = queue_new(jobs);
//	syslog(LOG_WARNING,"new jobqueue: %p",jp->jobqueue);
	jp->statusqueue = queue_new(0);
	for (i=0 ; i<JHASHSIZE ; i++) {
		jp->jobhash[i]=NULL;
	}
	jp->nextjobid = 1;
	zassert(pthread_mutex_lock(&(jp->jobslock)));
	job_spawn_worker(jp);
	zassert(pthread_mutex_unlock(&(jp->jobslock)));
	return jp;
}

void pipeline_create(
        pipeline_data_create_t create,
        pipeline_data_free_t destroy,
        pipeline_split_t split,
        pipeline_process_t process,
        size_t max_procs) {
    gPLFreer = destroy;
    gPLSplit = split;
    gPLProcess = process;

    gPipelineStartQ = queue_new(pipeline_qfree);
    gPipelineSplitQ = queue_new(pipeline_qfree);
    gPipelineMergeQ = queue_new(pipeline_qfree);

    gPLSplitSeq = 0;
    gPLMergeSeq = 0;
    gPLMergedItems = NULL;

    gPLProcessCount = num_threads(max_procs);
    gPLProcessThreads = malloc(gPLProcessCount * sizeof(pthread_t));
    for (size_t i = 0; i < (int)(gPLProcessCount * 2 + 3); ++i) {
        // create blocks, including a margin of error
        pipeline_item_t *item = malloc(sizeof(pipeline_item_t));
        item->data = create();
        // seq and next are garbage
        queue_push(gPipelineStartQ, PIPELINE_ITEM, item);
    }
    for (size_t i = 0; i < gPLProcessCount; ++i) {
        if (pthread_create(&gPLProcessThreads[i], NULL,
                &pipeline_thread_process, (void*)(uintptr_t)i))
            die("Error creating encode thread");
    }
    if (pthread_create(&gPLSplitThread, NULL, &pipeline_thread_split, NULL))
        die("Error creating read thread");
}

/*
 * Initialization.
 *
 *      minithread_system_initialize:
 *       This procedure should be called from your C main procedure
 *       to turn a single threaded UNIX process into a multithreaded
 *       program.
 *
 *       Initialize any private data structures.
 *       Create the idle thread.
 *       Fork the thread which should call mainproc(mainarg)
 *       Start scheduling.
 *
 */
void
minithread_system_initialize(proc_t mainproc, arg_t mainarg) {
  minithread_t clean_up_thread = NULL;
  int a = 0;
  void* dummy_ptr = NULL;
  minithread_t tmp = NULL;
  tmp = NULL;
  dummy_ptr = (void*)&a;
  current_id = 0; // the next thread id to be assigned
  id_lock = semaphore_create();
  semaphore_initialize(id_lock,1); 
  runnable_q = multilevel_queue_new(4);
  blocked_q = queue_new();
  blocked_q_lock = semaphore_create();
  semaphore_initialize(blocked_q_lock,1);
  dead_q = queue_new();
  dead_q_lock = semaphore_create();
  semaphore_initialize(dead_q_lock,1);
  dead_sem = semaphore_create();
  semaphore_initialize(dead_sem,0);    
  runnable_q_lock = semaphore_create();
  semaphore_initialize(runnable_q_lock,1);
  clean_up_thread = minithread_create(clean_up, NULL);
  multilevel_queue_enqueue(runnable_q,
    clean_up_thread->priority,clean_up_thread);
  runnable_count++;
  minithread_clock_init(TIME_QUANTA, (interrupt_handler_t)clock_handler);
  init_alarm();
  current_thread = minithread_create(mainproc, mainarg);
  minithread_switch(&dummy_ptr, &(current_thread->stacktop));
  return;
}

static void multiplex(const char *exe, const char *tx_addr, int port)
{
    Queue *runq = queue_new();
    Queue *waitq = queue_new();

    for (int i = 0; i < THREADS; ++i) {
        Exec *e = mem_alloc(sizeof(Exec));
        str_cpy(e->exe, exe);
        str_cpy(e->tx, tx_addr);
        e->runq = runq;
        e->waitq = waitq;

        sys_thread(exec_thread, e);

        if (i == 0) /* only one waitq thread (reuses the same operand) */
            sys_thread(waitq_thread, e);
    }

    IO *sio = sys_socket(&port);

    sys_log('E', "started port=%d, tx=%s\n", port, tx_addr);

    for (;;) {
        IO *io = sys_accept(sio, IO_STREAM);
        queue_put(waitq, conn_new(io));
    }

    queue_free(waitq);
    queue_free(runq);
}

int main(void)
{
    Student s1 = {45, "hello"},
            s2 = {49, "world"}, * sptr;
    pQueue q = queue_new(sizeof(int));
    pQueue ps = queue_new(sizeof(Student));

    int value = 10, v = 12, *ip;
    queue_enqueue(q, &value);
    queue_enqueue(q, &v);
    ip = (int *)queue_front(q);
    printf("%d\n", *ip);
    queue_dequeue(q);
    ip = (int *)queue_front(q);
    printf("%d\n", *ip);
    queue_free(q);

    queue_enqueue(ps, &s1);
    queue_enqueue(ps, &s2);
    printf("queue size: %ld\n", queue_size(ps));
    sptr = (Student *)queue_front(ps);
    printf("no: %d, name: %s\n", sptr->no, sptr->name);
    queue_dequeue(ps);
    printf("queue size: %ld\n", queue_size(ps));
    sptr = (Student *)queue_front(ps);
    printf("no: %d, name: %s\n", sptr->no, sptr->name);
    queue_free(ps);
    exit(EXIT_SUCCESS);
}

hb_value_t*
hb_plist_parse(const char *buf, size_t len)
{
    xmlSAXHandler parser;
    parse_data_t pd;

    pd.stack = queue_new();
    pd.tag_stack = queue_new();
    pd.key = NULL;
    pd.value = NULL;
    pd.plist = NULL;
    pd.closed_top = 0;

    memset(&parser, 0, sizeof(parser));
    parser.initialized = XML_SAX2_MAGIC;
    parser.startElement = start_element;
    parser.endElement = end_element;
    parser.characters = text_data;
    parser.warning = parse_warning;
    parser.error = parse_error;
    int result = xmlSAXUserParseMemory(&parser, &pd, buf, len);
    if (result != 0)
    {
        hb_error("Plist parse failed");
        return NULL;
    }
    xmlCleanupParser();

    if (pd.key) free(pd.key);
    if (pd.value) free(pd.value);
    queue_free(&pd.stack);
    queue_free(&pd.tag_stack);

    return pd.plist;
}

static struct gatt_db_attribute *new_attribute(struct gatt_db_service *service,
							uint16_t handle,
							const bt_uuid_t *type,
							const uint8_t *val,
							uint16_t len)
{
	struct gatt_db_attribute *attribute;

	attribute = new0(struct gatt_db_attribute, 1);

	attribute->service = service;
	attribute->handle = handle;
	attribute->uuid = *type;
	attribute->value_len = len;
	if (len) {
		attribute->value = malloc0(len);
		if (!attribute->value)
			goto failed;

		memcpy(attribute->value, val, len);
	}

	attribute->pending_reads = queue_new();
	attribute->pending_writes = queue_new();

	return attribute;

failed:
	attribute_destroy(attribute);
	return NULL;
}

static struct service *service_create(struct gatt_db_attribute *attr,
						struct btd_gatt_client *client)
{
	struct service *service;
	const char *device_path = device_get_path(client->device);
	bt_uuid_t uuid;

	service = new0(struct service, 1);
	if (!service)
		return NULL;

	service->chrcs = queue_new();
	if (!service->chrcs) {
		free(service);
		return NULL;
	}

	service->pending_ext_props = queue_new();
	if (!service->pending_ext_props) {
		queue_destroy(service->chrcs, NULL);
		free(service);
		return NULL;
	}

	service->client = client;

	gatt_db_attribute_get_service_data(attr, &service->start_handle,
							&service->end_handle,
							&service->primary,
							&uuid);
	bt_uuid_to_uuid128(&uuid, &service->uuid);

	service->path = g_strdup_printf("%s/service%04x", device_path,
							service->start_handle);

	if (!g_dbus_register_interface(btd_get_dbus_connection(), service->path,
						GATT_SERVICE_IFACE,
						NULL, NULL,
						service_properties,
						service, service_free)) {
		error("Unable to register GATT service with handle 0x%04x for "
							"device %s",
							service->start_handle,
							client->devaddr);
		service_free(service);

		return NULL;
	}

	DBG("Exported GATT service: %s", service->path);

	/* Set service active so we can skip discovering next time */
	gatt_db_service_set_active(attr, true);

	/* Mark the service as claimed since it going to be exported */
	gatt_db_service_set_claimed(attr, true);

	return service;
}

static struct characteristic *characteristic_create(
						struct gatt_db_attribute *attr,
						struct service *service)
{
	struct characteristic *chrc;
	bt_uuid_t uuid;

	chrc = new0(struct characteristic, 1);
	if (!chrc)
		return NULL;

	chrc->descs = queue_new();
	if (!chrc->descs) {
		free(chrc);
		return NULL;
	}

	chrc->notify_clients = queue_new();
	if (!chrc->notify_clients) {
		queue_destroy(chrc->descs, NULL);
		free(chrc);
		return NULL;
	}

	chrc->service = service;

	gatt_db_attribute_get_char_data(attr, &chrc->handle,
							&chrc->value_handle,
							&chrc->props, &uuid);

	chrc->attr = gatt_db_get_attribute(service->client->db,
							chrc->value_handle);
	if (!chrc->attr) {
		error("Attribute 0x%04x not found", chrc->value_handle);
		characteristic_free(chrc);
		return NULL;
	}

	bt_uuid_to_uuid128(&uuid, &chrc->uuid);

	chrc->path = g_strdup_printf("%s/char%04x", service->path,
								chrc->handle);

	if (!g_dbus_register_interface(btd_get_dbus_connection(), chrc->path,
						GATT_CHARACTERISTIC_IFACE,
						characteristic_methods, NULL,
						characteristic_properties,
						chrc, characteristic_free)) {
		error("Unable to register GATT characteristic with handle "
							"0x%04x", chrc->handle);
		characteristic_free(chrc);

		return NULL;
	}

	DBG("Exported GATT characteristic: %s", chrc->path);

	return chrc;
}

inline PathFinder *pathfinder_new (void)
{
	PathFinder *pf = calloc (1, sizeof(PathFinder));
	pf->queue1 = queue_new (1024);
	pf->queue2 = queue_new (1024);
	pf->tileset = queue_new (1024);
	return pf;
}

static struct bt_hci *create_hci(int fd)
{
	struct bt_hci *hci;

	if (fd < 0)
		return NULL;

	hci = new0(struct bt_hci, 1);
	if (!hci)
		return NULL;

	hci->io = io_new(fd);
	if (!hci->io) {
		free(hci);
		return NULL;
	}

	hci->is_stream = true;
	hci->writer_active = false;
	hci->num_cmds = 1;
	hci->next_cmd_id = 1;
	hci->next_evt_id = 1;

	hci->cmd_queue = queue_new();
	if (!hci->cmd_queue) {
		io_destroy(hci->io);
		free(hci);
		return NULL;
	}

	hci->rsp_queue = queue_new();
	if (!hci->rsp_queue) {
		queue_destroy(hci->cmd_queue, NULL);
		io_destroy(hci->io);
		free(hci);
		return NULL;
	}

	hci->evt_list = queue_new();
	if (!hci->evt_list) {
		queue_destroy(hci->rsp_queue, NULL);
		queue_destroy(hci->cmd_queue, NULL);
		io_destroy(hci->io);
		free(hci);
		return NULL;
	}

	if (!io_set_read_handler(hci->io, io_read_callback, hci, NULL)) {
		queue_destroy(hci->evt_list, NULL);
		queue_destroy(hci->rsp_queue, NULL);
		queue_destroy(hci->cmd_queue, NULL);
		io_destroy(hci->io);
		free(hci);
		return NULL;
	}

	return bt_hci_ref(hci);
}

struct bt_att *bt_att_new(int fd)
{
	struct bt_att *att;

	if (fd < 0)
		return NULL;

	att = new0(struct bt_att, 1);
	if (!att)
		return NULL;

	att->fd = fd;

	att->mtu = ATT_DEFAULT_LE_MTU;
	att->buf = malloc(att->mtu);
	if (!att->buf)
		goto fail;

	att->io = io_new(fd);
	if (!att->io)
		goto fail;

	att->req_queue = queue_new();
	if (!att->req_queue)
		goto fail;

	att->ind_queue = queue_new();
	if (!att->ind_queue)
		goto fail;

	att->write_queue = queue_new();
	if (!att->write_queue)
		goto fail;

	att->notify_list = queue_new();
	if (!att->notify_list)
		goto fail;

	if (!io_set_read_handler(att->io, can_read_data, att, NULL))
		goto fail;

	return bt_att_ref(att);

fail:
	queue_destroy(att->req_queue, NULL);
	queue_destroy(att->ind_queue, NULL);
	queue_destroy(att->write_queue, NULL);
	io_destroy(att->io);
	free(att->buf);
	free(att);

	return NULL;
}

void camd_start(struct ts *ts) {
	struct camd *c = &ts->camd;
	if (c->constant_codeword)
		return;
	c->ops.connect(c);
	// The input is not file, process messages using async thread
	if (ts->threaded) {
		c->req_queue = queue_new();
		c->ecm_queue = queue_new();
		c->emm_queue = queue_new();
		pthread_create(&c->thread, &ts->thread_attr , &camd_thread, ts);
	}
}

static struct scheduler *
_new_scheduler(uint32_t id) {
    struct scheduler *s = calloc(1, sizeof(*s));
    s->id = id;
    s->self_queue = queue_new(255);
    s->lock_queue = queue_new(255);
    s->empty_queue = queue_new(255);
    s->stack.ss_sp = valloc(kStackSize);
    s->stack.ss_size = kStackSize;
    pthread_mutex_init(&s->queue_mutex, NULL);
    pthread_cond_init(&s->queue_signal, NULL);
    return s;
}

static struct health_app *create_health_app(const char *app_name,
				const char *provider, const char *srv_name,
				const char *srv_descr, uint8_t mdeps)
{
	struct health_app *app;
	static unsigned int app_id = 1;

	DBG("");

	app = new0(struct health_app, 1);
	if (!app)
		return NULL;

	app->id = app_id++;
	app->num_of_mdep = mdeps;
	app->app_name = strdup(app_name);

	if (provider) {
		app->provider_name = strdup(provider);
		if (!app->provider_name)
			goto fail;
	}

	if (srv_name) {
		app->service_name = strdup(srv_name);
		if (!app->service_name)
			goto fail;
	}

	if (srv_descr) {
		app->service_descr = strdup(srv_descr);
		if (!app->service_descr)
			goto fail;
	}

	app->mdeps = queue_new();
	if (!app->mdeps)
		goto fail;

	app->devices = queue_new();
	if (!app->devices)
		goto fail;

	return app;

fail:
	free_health_app(app);
	return NULL;
}

GAttrib *g_attrib_new(GIOChannel *io, guint16 mtu, bool ext_signed)
{
	gint fd;
	GAttrib *attr;

	if (!io)
		return NULL;

	fd = g_io_channel_unix_get_fd(io);
	attr = new0(GAttrib, 1);
	if (!attr)
		return NULL;

	g_io_channel_ref(io);
	attr->io = io;

	attr->att = bt_att_new(fd, ext_signed);
	if (!attr->att)
		goto fail;

	bt_att_set_close_on_unref(attr->att, true);
	g_io_channel_set_close_on_unref(io, FALSE);

	if (!bt_att_set_mtu(attr->att, mtu))
		goto fail;

	attr->buf = malloc0(mtu);
	attr->buflen = mtu;
	if (!attr->buf)
		goto fail;

	attr->callbacks = queue_new();
	if (!attr->callbacks)
		goto fail;

	attr->track_ids = queue_new();
	if (!attr->track_ids)
		goto fail;

	return g_attrib_ref(attr);

fail:
	free(attr->buf);
	bt_att_unref(attr->att);
	g_io_channel_unref(io);
	free(attr);
	return NULL;
}

struct btd_gatt_client *btd_gatt_client_new(struct btd_device *device)
{
	struct btd_gatt_client *client;
	struct gatt_db *db;

	if (!device)
		return NULL;

	db = btd_device_get_gatt_db(device);
	if (!db)
		return NULL;

	client = new0(struct btd_gatt_client, 1);
	if (!client)
		return NULL;

	client->services = queue_new();
	if (!client->services) {
		free(client);
		return NULL;
	}

	client->device = device;
	ba2str(device_get_address(device), client->devaddr);

	client->db = gatt_db_ref(db);

	return client;
}