#include "fracture.h"

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

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

unsigned int 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, unsigned int value, unsigned int 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) {
	unsigned int 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
		unsigned int min_val = UINT_MAX;
		struct fibn *min_ptr = NULL;
		unsigned int max_rank = 0;
		struct fibn *curnode = trees;
		for (unsigned int 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, unsigned int value,
										 unsigned int 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;
		}
	}
}

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

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

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

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

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

		for (unsigned int i = 0; i < vei[u + 1] - vei[u]; i++) {
			unsigned int e = ve[vei[u] + i];
			unsigned int v = ev[2 * e] == u ? ev[2 * e + 1] : ev[2 * e];
			unsigned int 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;
}

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

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

	return dists;
}

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

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

	for (unsigned int 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;
}

/*
double *get_space_corr(net_t *instance, cholmod_common *c) {
	unsigned int nv = instance->graph->dnv;
	double *vc = instance->graph->dual_vert_coords;
	unsigned int ne = instance->graph->ne;
	unsigned int *ev = instance->graph->dev;
	double *corr = calloc(nv, sizeof(unsigned int));
	unsigned int *numat = calloc(nv, sizeof(unsigned int));

	for (unsigned int i = 0; i < ne; i++) {
		unsigned int v1 = ev[2 * i];
		unsigned int v2 = ev[2 * i + 1];
		double v1x = vc[2 * v1];
		double v1y = vc[2 * v1 + 1];
		double v2x = vc[2 * v2];
		double v2y = vc[2 * v2 + 1];
		double e1x = (v1x + v2x) / 2;
		double e1y = (v1y + v2y) / 2;
		for (unsigned int j = 0; j < ne; j++) {
			unsigned int v3 = ev[2 * j];
			unsigned int v4 = ev[2 * j + 1];
			double v1x = vc[2 * v1];
			double v1y = vc[2 * v1 + 1];
			double v2x = vc[2 * v2];
			double v2y = vc[2 * v2 + 1];
			double e1x = (v1x + v2x) / 2;
			double e1y = (v1y + v2y) / 2;
			corr[dist] += instance->fuses[i] && instance->fuses[j];
			numat[dist]++;
		}
	}

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

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

	free(marks);
	free(dists);

	return corr;
}
*/