#include <stdio.h>
#include <arpa/inet.h>
#include <errno.h>
#include <sys/time.h>
#include <time.h>
#include <math.h>
#include "data.h"

void host_q__check(char *s, struct host_q * host_q, char on_iter){
	if(host_q == NULL || 
			(on_iter && host_q->w_iter_current==NULL)){
		fprintf(stderr, "Invalid host_q for host_q_%s call.", s);
		exit(0);
	}
}

void host_q_new(struct host_q* host_q){
	host_q->w_last = host_q->w_first = NULL;
	host_q->length = 0;
	host_q_reset_iter(host_q);
}


void host_q_enqueue(struct host_q* host_q, struct host* host){
	host_q__check("enqueue", host_q, 0);
	struct host_q_wrapper *p_wrapper = (void *)
		malloc(sizeof(struct host_q_wrapper));
	p_wrapper->host = host;
	/* if there is no first, update the first property */
	if(host_q->w_first == NULL)
		host_q->w_first = p_wrapper;
	p_wrapper->prev = host_q->w_last;
	p_wrapper->next = NULL; /* its added to the end, so next is NULL */
	/* if there WAS a previous (is not first), then update its next */
	if(p_wrapper->prev)
		p_wrapper->prev->next = p_wrapper;
	host_q->w_last = p_wrapper;
	host_q->length++;
}



/*

int host_q_remove(struct host_q* host_q, struct host* host){
	struct host_q_wrapper *p_wrapper = p_wrapper->w_first;
	if(p_wrapper == NULL) return 0;
	do {
	} while(p_wrapper = p_wrapper->next);
	malloc(sizeof(struct host_q_wrapper));
	host_q->length--;
}*/
/*
**
** example usage:
** struct host_q q;
** host_q_new(&q);
** [...]
** host_q_enqueue(&q, &some_host);
** [...]
** To loop through:
** struct host *host;
** host_q_reset_iter(&q);
** while(host = host_q_next(&q)){
**    //do stuff with host..
**    // To remove from the host list:
**    if(host->sequence == 0xBAD){
**        host_q_remove_current(&q);
**        free(host);
**    }
** }
**
**
** */


void host_q_reset_iter(struct host_q* host_q) {
	host_q__check("reset_iter", host_q, 0);
	host_q->w_iter_next = host_q->w_first;
	host_q->w_iter_current = NULL;
}

void host_q__remove_current(struct host_q* host_q){
	if(host_q->w_first == host_q->w_iter_current)
		host_q->w_first = host_q->w_iter_current->next;
	else host_q->w_iter_current->prev->next = 
			host_q->w_iter_current->next;
	if(host_q->w_last == host_q->w_iter_current)
		host_q->w_last  = host_q->w_iter_current->prev;
	else host_q->w_iter_current->next->prev = 
		host_q->w_iter_current->prev;
	/* patch it around iter_current */
	free(host_q->w_iter_current);
	host_q->w_iter_current = NULL;
	host_q->length--;
}

struct host* host_q_peak(struct host_q* q){
	return q->w_first->host;
}
/* 
** 
**
**/ 
struct host* host_q_fetch_or_make(struct host_q* q, struct sockaddr_in* addr){
	struct host* host;
	host_q_reset_iter(q);
	while((host = host_q_next(q))){
		if(host_cmp(&host->address, addr))
			return host;
	}
	host = malloc(sizeof(struct host));
	host_init(host);
	host->address = *addr;
	host_q_enqueue(q, host);
	// It may be working now? 
	return host;
}

void host_init(struct host* host){
	host->is_enabled = 1;
	host->last_time = get_time();
	host->last_sequence = -1;
	host->edge_count = 0;
}

void host_q_reset_current(struct host_q* q){
	struct host *current;
	current = q->w_iter_current->host;
	host_q__remove_current(q);
	host_q_enqueue(q, current);
}

struct host* host_q_next(struct host_q* host_q){
	host_q__check("next", host_q, 0);
	host_q->w_iter_current = host_q->w_iter_next;
	if(host_q->w_iter_current==NULL) return NULL;
	host_q->w_iter_next = host_q->w_iter_current->next;
	return host_q->w_iter_current->host;
}

void print_host_q(struct host_q* q){
	struct host* next; 
	host_q_reset_iter(q);
	fprintf(stderr, "################\n# HOST Q (len=%02i) \n", q->length);
	fprintf(stderr, "################\n");
	while((next=host_q_next(q))){
		print_host(next);
	}
	fprintf(stderr, "$$$$$$$$$$$$$$$$\n$ END HOST Q (len=%02i) \n", q->length);
	fprintf(stderr, "$$$$$$$$$$$$$$$$\n");
}

void print_host(struct host* host){
	struct host** edges = host->edges; 
	struct host* next; 
	if(host==NULL) {
		fprintf(stderr, "------- host       NULL    ----\n");
		fprintf(stderr, "---------------------------------------\n\n");
		fflush(stderr);
		return;
	}
	fprintf(stderr, "------- host #%02i, %016lx, port %04i ----\n", host->number, 
			(unsigned long) host, ntohs(host->address.sin_port));
	fprintf(stderr, "edges (%02i): ", host->edge_count);
	while((next = *(edges++))){
		fprintf(stderr, "[%02i, %016lx] ", next->number, 
				(unsigned long) next);
	}
	if(host->next_hop)
		fprintf(stderr, "\nnext hop: [%02i, %016lx]\n", host->next_hop->number, 
				(unsigned long)host->next_hop);
	else fprintf(stderr, "\nnext hop: NULL\n");
	fprintf(stderr, "---------------------------------------\n\n");
	fflush(stderr);
}

/******************************************************************
**                            _                                  **
**       __ _ _ __ __ _ _ __ | |__                               **
**      / _` | '__/ _` | '_ \| '_ \                              **
**     | (_| | | | (_| | |_) | | | |                             **
**      \__, |_|  \__,_| .__/|_| |_|                             **
**      |___/          |_|                                       **
******************************************************************/
void host_graph_new(struct host_graph* graph,  struct host_q* q){
	struct host* host = NULL;
	graph->node_count = 0;
	host_q_reset_iter(q);
	while((host = host_q_next(q))){
		graph->nodes[graph->node_count] = host;
		host->number = graph->node_count;
		graph->node_count = graph->node_count + 1;
	}
	graph->nodes[graph->node_count] = NULL;
}

void host_graph_dijkstra(struct host_graph* graph){
	int dist[32];
	struct host **hosts = NULL;
	struct host *host = NULL;
	int hosts_unoptimized = graph->node_count;

	struct host *minhost;
	int mindist;

	int new_dist;

	struct host *previous_host;
	/* initialize array */
	hosts = graph->nodes;
	while((host = *(hosts++))) {
		dist[host->number] = INT_MAX;
		host->optimized = 0;
		host->previous = NULL;
	}
	dist[graph->self->number] = 0;
	/* loop through while hosts have been accounted for */
	while(hosts_unoptimized > 0) {
		/* search for host with smallest dist[] */ 
		minhost = NULL;
		mindist = INT_MAX;	
		hosts = graph->nodes;
		while((host = *(hosts++))) {
			/* check if its optimized, and ignore it if it is */
			if(host->optimized) continue;
			if(dist[host->number] < mindist) {
				mindist = dist[host->number];
				minhost = host;
			}
		}
		/* minhost now has the host which is smallest */
		if(mindist == INT_MAX) /* if the minimum distance is the max, */
			break; /* then nothing more is reachable */
		
		/* mark host as optimized and decrement unoptmized counter */
		minhost->optimized = 1;
		hosts_unoptimized--;
		
		/* loop through neighbors of minhost */
		hosts = minhost->edges;
		while((host = *(hosts++))) {
			new_dist = dist[minhost->number] + 1; // TODO add checking to see if node is up
			if(new_dist < dist[host->number] && host->is_enabled){
				dist[host->number] = new_dist;
				host->previous = minhost;
			}
		}
	}
	/* Here we fill out actual next hop information, by walking back on 
	** the previus fields of the host struct. */
	hosts = graph->nodes;
	while((host = *(hosts++))) {
		previous_host = host->previous;
		if(!previous_host || previous_host == graph->self) {
			host->next_hop = previous_host == graph->self ? host : NULL;
			continue;
		}
		while(previous_host && previous_host->previous != graph->self)
			previous_host = previous_host->previous;
		host->next_hop = previous_host;
	}
}