#include "fracture.h"

// I implemented a fibonacci heap because wikipedia said it would be fast

struct fibn {
	uint_t value;
	uint_t priority;
	uint_t rank;
	bool marked;
	struct fibn *first_child;
	struct fibn *parent;
	struct fibn *prev;
	struct fibn *next;
};

typedef struct {
	struct fibn *min;
	uint_t rank;
	uint_t trees;
	struct fibn **vertex_to_node;
} fibh;

uint_t fib_findmin(fibh *heap) {
	return heap->min->value;
}

void ll_setparent(struct fibn *ll, struct fibn *parent) {
	struct fibn *curnode = ll->next;
	ll->parent = parent;
	while (curnode != ll) {
		curnode->parent = parent;
		curnode = curnode->next;
	}
}

struct fibn *ll_merge(struct fibn *ll1, struct fibn *ll2) {
	if (ll1 == NULL)
		return ll2;
	if (ll2 == NULL)
		return ll1;

	// link the beginning of list one to the end of list two and vice versa
	struct fibn *ll1_beg = ll1;
	struct fibn *ll1_end = ll1->prev;
	struct fibn *ll2_beg = ll2;
	struct fibn *ll2_end = ll2->prev;

	ll1_beg->prev = ll2_end;
	ll1_end->next = ll2_beg;
	ll2_beg->prev = ll1_end;
	ll2_end->next = ll1_beg;

	return ll1;
}

struct fibn *ll_delroot(struct fibn *ll) {
	if (ll == NULL)
		return NULL;

	if (ll->next == ll) {
		return NULL;
	}

	struct fibn *ll_beg = ll->next;
	struct fibn *ll_end = ll->prev;

	ll_beg->prev = ll_end;
	ll_end->next = ll_beg;

	ll->next = ll;
	ll->prev = ll;

	return ll_beg;
}

void fib_insert(fibh *heap, uint_t value, uint_t priority) {
	struct fibn *newnode = calloc(1, sizeof(struct fibn));
	newnode->value = value;
	newnode->priority = priority;
	newnode->next = newnode;
	newnode->prev = newnode;

	heap->min = ll_merge(heap->min, newnode);

	if (priority < heap->min->priority) {
		heap->min = newnode;
	}

	heap->vertex_to_node[value] = newnode;

	heap->trees++;
}

void fib_deletemin(fibh *heap) {
	uint_t min_rank = heap->min->rank;
	struct fibn *min_children = heap->min->first_child;

	struct fibn *trees = ll_delroot(heap->min);
	heap->vertex_to_node[heap->min->value] = NULL;
	free(heap->min);

	if (min_children != NULL)
		ll_setparent(min_children, NULL);
	trees = ll_merge(trees, min_children);

	heap->trees += min_rank - 1;

	if (trees == NULL) {
		// min had no children and was only tree, return empty heap
		heap->min = NULL;
		heap->rank = 0;
	} else {
		// min had children or there were other trees
		uint_t min_val = UINT_MAX;
		struct fibn *min_ptr = NULL;
		uint_t max_rank = 0;
		struct fibn *curnode = trees;
		for (uint_t i = 0; i < heap->trees; i++) {
			if (curnode->priority < min_val) {
				min_ptr = curnode;
				min_val = curnode->priority;
			}
			if (curnode->rank > max_rank)
				max_rank = curnode->rank;
			curnode = curnode->next;
		}

		if (min_ptr == NULL)
			min_ptr = trees;
		heap->min = min_ptr;
		heap->rank = max_rank;

		struct fibn **rankslots = calloc(max_rank + 1, sizeof(struct fibn *));
		curnode = heap->min;
		while (curnode != rankslots[curnode->rank]) {
			if (rankslots[curnode->rank] == NULL) {
				rankslots[curnode->rank] = curnode;
				curnode = curnode->next;
			} else {
				struct fibn *oldnode = rankslots[curnode->rank];
				rankslots[curnode->rank] = NULL;
				struct fibn *smaller =
						curnode->priority < oldnode->priority || curnode == heap->min
								? curnode
								: oldnode;
				struct fibn *larger = smaller == curnode ? oldnode : curnode;
				ll_delroot(larger);
				ll_setparent(larger, smaller);
				struct fibn *smaller_children = smaller->first_child;
				smaller->first_child = ll_merge(smaller_children, larger);
				heap->trees--;
				smaller->rank++;
				if (smaller->rank > heap->rank) {
					heap->rank = smaller->rank;
					rankslots =
							realloc(rankslots, (heap->rank + 1) * sizeof(struct fibn *));
					rankslots[heap->rank] = NULL;
				}
				curnode = smaller;
			}
		}
		free(rankslots);
	}
}

void fib_decreasekey(fibh *heap, uint_t value, uint_t new_priority) {
	struct fibn *node = heap->vertex_to_node[value];
	if (node != NULL) {
		node->priority = new_priority;
		if (node->parent != NULL) {
			if (node->priority < node->parent->priority) {

				struct fibn *curnode = node;
				curnode->marked = true;
				while (curnode->parent != NULL && curnode->marked) {
					struct fibn *oldparent = curnode->parent;
					oldparent->rank--;
					oldparent->first_child = ll_delroot(curnode);
					ll_setparent(curnode, NULL);
					ll_merge(heap->min, curnode);
					heap->trees++;
					if (curnode->marked) {
						curnode->marked = false;
					}

					if (oldparent->marked) {
						curnode = oldparent;
					} else {
						oldparent->marked = true;
						break;
					}
				}
			}
		}

		if (new_priority < heap->min->priority) {
			heap->min = node;
		}
	}
}

uint_t *dijkstra(const graph_t *network, uint_t source) {
	uint_t nv = network->dnv;
	uint_t *vei = network->dvei;
	uint_t *ve = network->dve;
	uint_t *ev = network->dev;

	uint_t *dist = (uint_t *)calloc(nv, sizeof(uint_t));
	fibh *Q = (fibh *)calloc(1, sizeof(fibh));
	Q->vertex_to_node = (struct fibn **)calloc(nv, sizeof(struct fibn *));

	for (uint_t i = 0; i < nv; i++) {
		if (i != source) {
			dist[i] = UINT_MAX;
		}

		fib_insert(Q, i, dist[i]);
	}

	while (Q->min != NULL) {
		uint_t u = fib_findmin(Q);
		fib_deletemin(Q);

		for (uint_t i = 0; i < vei[u + 1] - vei[u]; i++) {
			uint_t e = ve[vei[u] + i];
			uint_t v = ev[2 * e] == u ? ev[2 * e + 1] : ev[2 * e];
			uint_t alt = dist[u] + 1;
			if (alt < dist[v]) {
				dist[v] = alt;
				fib_decreasekey(Q, v, alt);
			}
		}
	}

	free(Q->vertex_to_node);
	free(Q);

	return dist;
}

uint_t **get_dists(const graph_t *network) {
	uint_t nv = network->dnv;
	uint_t **dists = (uint_t **)malloc(nv * sizeof(uint_t *));

	#pragma omp parallel for
	for (uint_t i = 0; i < nv; i++) {
		dists[i] = dijkstra(network, i);
	}

	return dists;
}

double *get_corr(net_t *instance, uint_t **dists, cholmod_common *c) {
	uint_t nv = instance->graph->dnv;
	uint_t ne = instance->graph->ne;
	uint_t *ev = instance->graph->dev;
	bool nulldists = false;
	if (dists == NULL) {
		dists = get_dists(instance->graph);
		nulldists = true;
	}
	double *corr = calloc(nv, sizeof(double));
	uint_t *marks = get_clusters(instance, c);
	uint_t *numat = calloc(nv, sizeof(uint_t));

	for (uint_t i = 0; i < ne; i++) {
		uint_t v1 = ev[2 * i];
		uint_t v2 = ev[2 * i + 1];
		for (uint_t j = 0; j < ne; j++) {
			uint_t v3 = ev[2 * j];
			uint_t v4 = ev[2 * j + 1];
			uint_t dist1 = dists[v1][v3];
			uint_t dist2 = dists[v1][v4];
			uint_t dist3 = dists[v2][v3];
			uint_t dist4 = dists[v2][v4];
			uint_t dist = (dist1 + dist2 + dist3 + dist4) / 4;
			corr[dist] += instance->fuses[i] && instance->fuses[j];
			numat[dist]++;
		}
	}

	for (uint_t i = 0; i < nv; i++) {
		if (numat[i] > 0) {
			corr[i] /= numat[i];
		}
	}

	if (nulldists) {
		for (int i = 0; i < nv; i++) {
			free(dists[i]);
		}
		free(dists);
	}

	free(marks);

	return corr;
}