void bfs_traverse(double road[][MAX_POINT])
{
	int visited[MAX_POINT];
	int i;
	for (i = 1; i < MAX_POINT; i++)
		visited[i] = 0;
	struct queueLK *hq;
	initQueue(hq);
	for (i = 1; i < MAX_POINT; i++) {
		if (!visited[i]) {
			visited[i] = 1;
			visit(road, i);
			enQueue(hq, i);
			while (!emptyQueue(hq)) {
				int u = outQueue(hq);
				int j;
				for (j = 1; j < MAX_POINT; j++) {
					if (road[u][j] > 0.0001 && !visited[j]) {
						visited[j] = 1;
						visit(road, j);
						enQueue(hq, j);
					}
				}
			}
		}
	}
	clearQueue(hq);
}

int isPathBfs(Graph g, Vertex v, Vertex w)
{
	int isPath = FALSE;
	Queue q = newQueue(); // create a new queue
	Vertex curV = 0;
	int *visited = calloc(g->nV, sizeof(int)); // allocate + init to 0
	int i = 0;

	assert(visited != NULL);
	enQueue(q, v);
	visited[v] = TRUE;

	while ( !isEmptyQueue(q) ){ // still have vertices to traverse
		curV = deQueue(q); // get a vertex from the queue
		printf("Visiting: %d.\n", curV);
		visited[curV] = TRUE; // mark it as visited
		if (curV == w) { isPath = TRUE; } 
		for (i = 0; i < g->nV; i++){ // find vertices to add to queue
			if (g->edges[curV][i] && !visited[i]){ // found a vertex to add
				enQueue(q, i); // add it to queue
				visited[i] = TRUE; // mark it as visited
			}
		}
	}	
	free(visited);
	deleteQueue(q);
	return isPath;
}

int checkStackPairwiseOrder(struct sNode *top) {
    struct Queue *q = createQueue();
    struct sNode *s = top;
    
    int pairwiseOrdered = 1;

    while (!isEmpty(s))
        enQueue(q, pop(&s));

    while (!queueIsEmpty(q))
        push(&s, deQueue(q)->key);

    while (!isEmpty(s)) {
        int n = pop(&s);
        enQueue(q, n);
        if (!isEmpty(s)) {
            int m  = pop(&s);
            enQueue(q, m);
            if(abs(n - m) != 1) {
                pairwiseOrdered = 0;
            }    
        }
    }    

    while (!queueIsEmpty(q))
        push(&s, deQueue(q)->key);
    return pairwiseOrdered;
}        

int breadthFirst(struct node * resgraph[], int from , int to, int nV, int parent[])
{
	int i;
	int visitList[nV];
	struct Queue * pop;
	struct node * var;
	for(i = 0;i < nV; i++)
	{
		visitList[i] = 0;
	}
	visitList[from] = 1;
	parent[from] = -1;
	enQueue(from);
	
	while(head != NULL)
	{
		pop = deQueue();
		var = resgraph[pop->nodes];
		while(var != NULL)
		{
			if(visitList[var->toNode] == 0 && var->weight > 0)
			{
				enQueue(var->toNode);
				visitList[var->toNode] = 1;
				parent[var->toNode] = pop->nodes;
			}
			var = var->next;
		}
	}
	
	if(visitList[to] == 1)
		return 1;
	return 0;
}

int main(void){
    
    Queue_t *q;
    Stack_t *stack1 = (Stack_t*)malloc(sizeof(Stack_t));
    Stack_t *stack2 = (Stack_t*)malloc(sizeof(Stack_t));
    
    q->basic = stack1;
    q->forDequeue = stack2;
    
    puts("//enqueue");
    enQueue(q, 1);
    enQueue(q, 2);
    enQueue(q, 3);
    
    printQueue(q);
    
    puts("//deQueue");
    deQueue(q);
    deQueue(q);
    printQueue(q);
    
    deQueue(q);
    printQueue(q);
    
    deQueue(q);
    
    
    
    
    return 0;
    
}

bool isBipartite(int G[][V], int src)
{
    int colorMatrix[V], color, temp, u;

    initColorMatrix(colorMatrix, V);

    struct Queue* queue = createQueue(V);

    color = 1;
    colorMatrix[src] = color;
    enQueue(queue, src);

    while (!isEmpty(queue))
    {
        temp = deQueue(queue);
        // assign alternate color to its neighbor
        color = 1 - colorMatrix[temp];

    	for (u = 0; u < V; ++u)
        {
        	// an edge exists and destination not colored
        	if (G[temp][u] && colorMatrix[u] == -1)
	        {
		        colorMatrix[u] = color;
                enQueue(queue, u);
	        }

            else if (G[temp][u] && colorMatrix[u] == colorMatrix[temp])
                return false;
        }
    }

    return true;
}

void bfs(struct Graph *graph, int s)
{

    int visited[graph->v];
    for (int i = 0; i < graph->v; i++)
        visited[i] = G_FALSE;
    visited[s] = G_TRUE;
    
    struct Queue *queue = newQueue(10);
    enQueue(queue, s);
    
    fprintf(stderr, "BFS\n");
    while (!isEmpty(queue)) {
        int temp = deQueue(queue);
        fprintf(stderr, "%d\n", temp);
        struct adjListNode *cur = graph->array[temp].head;
        while (cur){
            if (!visited[cur->dest]) {
                visited[cur->dest] = G_TRUE;
                enQueue(queue,cur->dest);
            }
            cur = cur->next;
        }
    }
    return;
}

int main(void){

	printf("FIFO: First In First Out\n");

	printf("Queue is %s\n", (isEmpty()? "Empty": "Not Empty"));
	printf("Queue Size = %d\n", queueSize());

	enQueue(1);
	enQueue(2);
	enQueue(13);
	enQueue(6);

	printQueue();
	printf("Queue Size = %d\n", queueSize());


	printf("1st DeQueue = 1 || deQueue Result = %d\n", deQueue());
	printf("2nd DeQueue = 2 || deQueue Result = %d\n", deQueue());
	printf("Queue Size = %d\n", queueSize());

	printQueue();

	printf("3rd DeQueue = 13 || deQueue Result = %d\n", deQueue());
	printf("4th DeQueue = 6  || deQueue Result = %d\n", deQueue());

	printf("Add 6 to the queue\n");
	enQueue(6);
	printQueue();

	printf("Queue Size = %d\n", queueSize());
	printf("Queue is %s\n", (isEmpty()? "Empty": "Not Empty"));


	return 0;
}

void
start(void)
{
	int i;
	//exercise 4A
	//sys_priority (PRIORITY);
	//exercise 7
	queue_t frontground = initQueue();
	queue_t background = initQueue();
	sys_multilevelq (QUEUE);
	sys_yield();
	for (i = 0; i < RUNCOUNT; i++) {
		if (current->queue_name == q_foreground)
		{
			enQueue (frontground, pid_t current->p_pid);
		}
		else if (current->queue_name == q_background)
		{
			enQueue (background, pid_t current->p_pid);
		}
		else
		{
			console_printf(cursorpos, 0x100, "error on enqueue");
		}
		// Write characters to the console, yielding after each one.
		*cursorpos++ = PRINTCHAR;
		sys_yield();
	}

	// Yield forever.
	//while (1)
		//sys_yield();
	sys_exit(0);
}

void configure_AccelRange(accelRange range){
	mpu_regs->accel_cfg &= ~BITS_FSR;
	mpu_regs->accel_cfg |= range;
	enQueue(mpuTxQueue, reg.accel_cfg);				// enqueue the register number
	enQueue(mpuTxQueue, mpu_regs->accel_cfg);	// enqueue the new value 
	mpu_writeRegister(1, reg.accel_cfg, false); // send to i2c interface
	
}

void test_inserting_elements_equal_to_size_of_queue(){
	CircularQueue* cQueue = create(sizeof(int),2);
	int element1 = 10;
	int element2 = 20;
	int res = enQueue(cQueue,&element1);
	res = enQueue(cQueue,&element2);
	ASSERT(1 == res);
};

void test_inserts_the_element_first_having_higher_priority_float(){
	void* pQueue = createPQueue();
	float data1 = 10.0f , data2 = 20.0f;
	ASSERT(1 == enQueue(pQueue, &data2, 2));
	ASSERT(1 == enQueue(pQueue, &data1, 1));
	ASSERT(1 == deQueue(pQueue));
	ASSERT(1 == deQueue(pQueue));
	ASSERT(0 == deQueue(pQueue));
};

void test_to_enqueue_an_int_element_when_queue_is_full(){
	int element = 4;
	Queue* queue = create(sizeof(int), 2);
	enQueue(queue, &element);
	enQueue(queue, &element);
	ASSERT(0==enQueue(queue, &element));
	free(queue->base);
	free(queue);	
}

void test_to_check_the_behaviour_on_full_capacity(){
	int element = 4;
	queue = create(sizeof(int), 4);
	ASSERT(enQueue(queue, &element));
	ASSERT(enQueue(queue, &element));
	ASSERT(enQueue(queue, &element));
	ASSERT(enQueue(queue, &element));
	ASSERT(false==enQueue(queue, &element));
}

void test_to_enter_element_in_queue_when_it_is_full_11(){
	Queue* actual = createQueue(2,sizeof(int));
	int value[3] = {1,2,3};
	int* data = (int*)actual->elements;
	enQueue(actual,&value[0]);
	enQueue(actual,&value[1]);
	ASSERT(0==enQueue(actual,&value[2]));
	free(actual);
}

void test_to_dequeue_an_int_element(){
	int element = 4;
	queue = create(sizeof(int), 3);
	ASSERT(enQueue(queue, &element));
	ASSERT(enQueue(queue, &element));
	deQueue(queue);
	deQueue(queue);
	ASSERT(1==queue->queueInfo.front);
}

void test_inserts_the_element_first_having_higher_priority_String(){
	void* pQueue = createPQueue();
	String data1 = "a" , data2 = "b";
	ASSERT(1 == enQueue(pQueue, &data2, 2));
	ASSERT(1 == enQueue(pQueue, &data1, 1));
	ASSERT(1 == deQueue(pQueue));
	ASSERT(1 == deQueue(pQueue));
	ASSERT(0 == deQueue(pQueue));
};

void test_inserts_the_element_first_having_lower_priority_char(){
	void* pQueue = createPQueue();
	char data1 = 'a' , data2 = 'b';
	ASSERT(1 == enQueue(pQueue, &data1, 1));
	ASSERT(1 == enQueue(pQueue, &data2, 2));
	ASSERT(1 == deQueue(pQueue));
	ASSERT(1 == deQueue(pQueue));
	ASSERT(0 == deQueue(pQueue));
};

void test_inserts_the_element_first_having_lower_priority_double(){
	void* pQueue = createPQueue();
	double data1 = 10.0 , data2 = 20.0;
	ASSERT(1 == enQueue(pQueue, &data1, 1));
	ASSERT(1 == enQueue(pQueue, &data2, 2));
	ASSERT(1 == deQueue(pQueue));
	ASSERT(1 == deQueue(pQueue));
	ASSERT(0 == deQueue(pQueue));
};

void mpu_init(void){
	mpu_setup_s *startup_config;
	boolean is_Started;
	
	// allocate memory and set to zero
	mpu_regs = malloc(sizeof *mpu_regs);
	if(!mpu_regs){
		while(1);
	}
	/* Configure mpu_regs to be a copy  of onboard registers  */
	// mpu actually starts up with two non-zero registers
	mpu_regs->who_am_i = MPU_I2C_ADDRESS;	// initialized with known address
	mpu_regs->pwr_mgmt_1 = BIT_SLEEP; // starts in sleep mode
	/* End configure of mpu_regs */

	NVIC_SetPriority (EXTI9_5_IRQn, 0x07); 			/* set priority to lower than i2c */
	NVIC_DisableIRQ(EXTI9_5_IRQn);							/* we don't want to interrupt during setup */
	
	/* Check to see if MPU is already loaded (from pre-CPU reset) */
	is_Started = mpu_isInitialized();
	
	// restart device 
	enQueue(mpuTxQueue, reg.pwr_mgmt_1);				// enqueue the register number
	enQueue(mpuTxQueue, BIT_RESET);							// enqueue the new value 
	mpu_writeRegister(1, reg.pwr_mgmt_1, false); // send to i2c interface
	
	Delay(1000);// wait for reset
	// wakeup device on restart
	mpu_regs->pwr_mgmt_1 &= MPU_WAKE_UP;
	enQueue(mpuTxQueue, reg.pwr_mgmt_1);				// enqueue the register number
	enQueue(mpuTxQueue, mpu_regs->pwr_mgmt_1);	// enqueue the new value 
	mpu_writeRegister(1, reg.pwr_mgmt_1, false); // send to i2c interface
	
	Delay(500);// wait for wakeup
	mpu_readRegister(1, reg.pwr_mgmt_1);
	while(queue_isEmpty(mpuRxQueue)){ }
	display_Int(deQueue(mpuRxQueue), 0, 0, true);
	
	/* Once device is on and awake, set it to default config */
	startup_config = malloc(sizeof *startup_config);
	startup_config->rate_div = STARTUP_RATE_DIV;
	startup_config->lpf = STARTUP_LPF;
	startup_config->user_ctrl = STARTUP_USERCTRL;
	startup_config->accel_cfg = STARTUP_ACCELCFG;
	startup_config->fifo_en = STARTUP_FIFOEN;
	startup_config->gyro_cfg = STARTUP_GYROCFG;
	startup_config->int_enable = STARTUP_INTNENABLE;
	startup_config->int_pin_cfg = STARTUP_INTPINCFG;
	
	configure_Mpu(startup_config);			/* call the function to write values */
	
	free(startup_config);								/* release memory */
	//NVIC_ClearPendingIRQ(EXTI9_5_IRQn);	/* clear any initial interrupts */
	NVIC_EnableIRQ(EXTI9_5_IRQn);				/* turn interrupts back on */

}