#include <stdio.h>
#include <stdlib.h>

#include "local_queue.h"
#include "localsys.h"



typedef struct tag_queue {
  elev_direction_t button;
  int floor;
  struct tag_queue *next;
} queue_t;

queue_t *head = NULL;
queue_t *last = NULL;



void print_queue(){
	queue_t *it = head;
	printf("Local queue: ");
	while(it != NULL){
		printf("floor %d, type %d  |  ",it->floor+1, it->button);
		it = it->next;
	}
	printf("\n");
}


void move_to_front(queue_t *node){  	
	if (node == head) return;
	
	queue_t *prev = head;
	queue_t *it = head->next;
	
	while (it != NULL) {
		if (it != node) continue;
		
		if(prev->next == last) {
			prev->next = NULL;
			last->next = head;
			head = last;
			last = prev;
			return;
		}
		else if(prev->next == it){
			prev->next = it->next;
			it->next = head;
			head = it;
			it = prev; 
			return;
		}
		it = it->next;
		prev = prev->next;
	}
	
}



void passing_by(queue_t *node) {
	int end_dest = get_end_dest();
	int floor = get_floor();
	int dir = end_dest-floor;
	
	if (dir > 0) {
		if ((node->floor < end_dest) && (node->floor > floor) && (node->button != ELEV_DIR_DOWN)) {
			move_to_front(node);
			set_dest(node->floor);
			printf("New destination: %d\n", node->floor+1);
		}
	}
	else if (dir < 0) {
		if ((node->floor > end_dest) && (node->floor < floor) && (node->button != ELEV_DIR_UP)) {
			move_to_front(node);
			set_dest(node->floor);
			printf("New destination: %d\n", node->floor+1);
		}
	}
}



void find_end_dest() {
	queue_t *it = head;
	int dest = get_dest();
	int floor = get_floor();
	int dir = dest-floor;
	
	while (it != NULL) {
		if (((dir > 0) && (it->floor > get_end_dest()) && (it->button != ELEV_DIR_DOWN)) || 
			((dir < 0) && (it->floor < get_end_dest()) && (it->button != ELEV_DIR_UP))) {
			set_end_dest(it->floor);
		}
		it = it->next;
	}
	printf("End destination: %d\n", get_end_dest()+1);
	
}



void pop_order() {
	if(head == NULL) return;
	elev_set_button_lamp(head->button, head->floor, 0);
	
	queue_t *new_head = head->next;
	free(head);
	head = new_head;

	if(head == NULL) {
		set_dest(-1);
		set_end_dest(-1);
		last = NULL;
	}
	else {
		set_dest(head->floor);
		printf("Next destination: %d\n", head->floor+1);
		set_end_dest(head->floor);
		find_end_dest();
	}
	print_queue();
}


void add_to_queue(elev_direction_t b, int f){
	queue_t *it = head;
	while (it != NULL) {
		if ((it->button == b) && (it->floor == f)) return;
		it = it->next;
	}
	
	queue_t *new_order;
	new_order = malloc(sizeof(queue_t));
	new_order->button = b;
	new_order->floor = f;
	new_order->next = NULL;
	
	if (head == NULL){
		set_end_dest(f);
		set_dest(f);
		printf("Next destination: %d\n", f+1);
		head = new_order;
		last = new_order;
		elev_set_button_lamp(b, f, 1);
	}
	else {
		last->next = new_order;
		last = new_order;
		elev_set_button_lamp(b, f, 1);
		
		passing_by(last);
		find_end_dest();
	}
	print_queue();
}

void remove_orders() {
	if (head == NULL) return;
	queue_t *it = head;	
	int found_call = 0;
	
	int dest = get_dest();
	int floor = get_floor();
	int dir = dest-floor;
	while(it != NULL){
		if (floor == it->floor) {
			if (((dir > 0) && (it->button != ELEV_DIR_DOWN)) || 
				((dir < 0) && (it->button != ELEV_DIR_UP))) {
				move_to_front(it);
				pop_order();
			}
			else if (it->button == ELEV_DIR_COMMAND) {
				move_to_front(it);
				pop_order();
				it = it->next;
				continue;
			}
			else if ((dir == 0) && (!found_call)) {
				found_call = 1;
				move_to_front(it);
				pop_order();
			}
		}
		it = it->next;
	}
}

void clear_queue() {
	queue_t *it = head;
	while (it != NULL) {
		pop_order();
	}
}