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

#define N 56
#define K 7
#define M 5000

typedef struct node_struct node;
struct node_struct {
    int v;
    node* next;
};

typedef struct {
    int n;
    node** neighbours;
} graph;

typedef struct entry_struct entry;
struct entry_struct {
    void* value;
    entry* next;
};

typedef struct {
    entry* first;
} set;

int random_int(int n) {
    int res;
    if (n == 0) return 0;
    while ((res = (int) ((((double) rand()) / ((double) RAND_MAX)) * (n + 1))) 
	   >= n);
    return res;
}

double **make_matrix(int n) {
    int i;
    double **A;

    A = (double**) malloc(n * sizeof(double*));
    for (i=0;i<n;i++) 
	A[i] = (double*) malloc(n * sizeof(double));
    return A;
}

void free_matrix(double **A, int n) {
    int i;
    for (i=0;i<n;i++) 
	free(A[i]);
    free(A);
}

double **copy_matrix(double **A, int n) {
    double **B;
    int i,j;

    B = make_matrix(n);
    for (i=0;i<n;i++) 
	for (j=0;j<n;j++) 
	    B[i][j] = A[i][j];
    return B;
}

int min(int a, int b) {
    return a > b ? b : a;
}

double **insert_symmetric_random_block(double **A, int n, int k) {
    int i,j;
    double r;
    for (i=0;i<k;i++) {
	for (j=0;j<=i;j++) {
	    r = ((double) rand()) / ((double) RAND_MAX);
	    A[n+i][n+j] = r;
	    A[n+j][n+i] = r;
	}
    }
    return A;
}

double **make_random_block_matrix(int n, int k) {
    double **A;
    int i, l;
    
    if (n % k != 0) {
	fprintf(stderr, "%d not dividable by %d: no such blocked matrix\n",
		n,k);
	return NULL;
    }
    l = n / k;
    A = make_matrix(n);
    for (i=0;i<k;i++) {
	insert_symmetric_random_block(A,i*l,l);
    }
    return A;
}

double **make_permutation_matrix(int *perm, int n) {
    double **P;
    int i;

    P = make_matrix(n);
    for (i=0;i<n;i++) {
	P[i][perm[i]] = 1.0;
    }
    return P;
}

double **transpose_matrix(double **A, int n) {
    double **T;
    int i, j;

    T = make_matrix(n);
    for (i=0;i<n;i++) {
	for (j=0;j<n;j++) {
	    T[i][j] = A[j][i];
	}
    }
    return T;
}

double **multiply_matrix_with_matrix(double **A, double **B, int n) {
    double **C;
    int i,j,k;

    C = make_matrix(n);
    for (i=0;i<n;i++) {
	for (j=0;j<n;j++) {
	    C[i][j] = 0.0;
	    for (k=0;k<n;k++) {
		C[i][j] += A[i][k] * B[k][j]; 
	    }
	}
    }
    return C;
}


double **swap_rows(double **A, int n, int i, int j) {
    int k;
    double a;
    if (i==j) return A;
    for (k=0;k<n;k++) {
	a = A[k][i];
	A[k][i] = A[k][j];
	A[k][j] = a;
    }
    return A;
}

double **swap_columns(double **A, int n, int i, int j) {
    int k;
    double a;
    if (i==j) return A;
    for (k=0;k<n;k++) {
	a = A[i][k];
	A[i][k] = A[j][k];
	A[j][k] = a;
    }
    return A;
}

void print_matrix(double **A, int n) {
    int i, j;

    for (i=0;i<n;i++) {
	for (j=0;j<n;j++) {
	    printf("%e ",A[i][j]);
	}
	printf("\n");
    }
    printf("\n");
}

void print_matrix_pattern(double **A, int n) {
    int i, j;

    for (i=0;i<n;i++) {
	for (j=0;j<n;j++) {
	    if (A[i][j] != 0.0) printf("#"); else printf("."); 
	}
	printf("\n");
    }
    printf("\n");
}


double **apply_permutation(double **A, int n, int* perm) {
    double **temp;
    int i,j;

    temp = copy_matrix(A,n);
    for (i=0;i<n;i++) {
	for (j=0;j<n;j++) {
	    A[perm[i]][perm[j]] = temp[i][j];
	}
    }
    free_matrix(temp,n);
    return A;
}




double **permute_chaotically(double **A, int n, int rounds) {
    int i, j, k;
    for (i=0;i<rounds;i++) {
	j = random_int(n);
	k = random_int(n);
	swap_rows(A,n,j,k);
	swap_columns(A,n,j,k);
    }
    return A;
}

graph *make_graph(int n) {
    graph *G;
    int i;

    G = (graph *) malloc(sizeof(graph));
    G->n = n;
    G->neighbours = (node**) malloc(n * sizeof(node*));
    for (i=0;i<n;i++)
	G->neighbours[i] = NULL;
    return G;
}

void free_graph(graph *G) {
    int i;
    node *n;
    for (i=0;i<G->n;i++) {
	while (G->neighbours[i] != NULL) {
	    n = G->neighbours[i]->next;
	    free(G->neighbours[i]);
	    G->neighbours[i] = n;
	}
    }
    free(G->neighbours);
    free(G);
}

void print_graph(graph *G) {
    int i;
    node *n;

    printf("%d nodes in the graph\n",G->n);
    for (i=0;i<G->n;i++) {
	printf("[%d]->",i);
	n = G->neighbours[i];
	while (n != NULL) {
	    printf("%d->",n->v);
	    n = n->next;
	}
	printf("null\n");
    }
    printf("\n");
}

graph *add_edge(graph *G, int i, int j) {
    node *n;
    if (i >= G->n) return G;
    if (j >= G->n) return G;
    n = malloc(sizeof(node));
    n->v = j;
    n->next = G->neighbours[i];
    G->neighbours[i] = n;
    if (i != j) {
	n = malloc(sizeof(node));
	n->v = i;
	n->next = G->neighbours[j];
	G->neighbours[j] = n;
    }
    return G;
}

graph *make_graph_from_symmetric_matrix(double **A, int n) {
    graph *G;
    int i,j;
    
    G = make_graph(n);
    for (i=0;i<n;i++) 
	for (j=0;j<=i;j++) {
	    if (A[i][j] != A[j][i]) {
		fprintf(stderr,"Matrix is not symmetric!\n");
		return G;
	    }
	    if (A[i][j] != 0.0) {
		add_edge(G,i,j);
	    }
	}
    return G;
}

set *make_set() {
    set *s;
    s = (set*) malloc(sizeof(set));
    s->first = NULL;
    return s;
}

void free_set_of_int(set *s) {
    entry *e;
    while (s->first != NULL) {
	free((int*) s->first->value);
	e = s->first->next;
	free(s->first);
	s->first = e;
    }
    free(s);
}

void free_set_of_sets_of_int(set *s) {
    entry *e;
    while (s->first != NULL) {
	free_set_of_int((set *) s->first->value);
	e = s->first->next;
	free(s->first);
	s->first = e;
    }
    free(s);
}

void print_set_of_int(set *s) {
    entry *e;
    printf("{");
    e = s->first;
    while (e != NULL) {
	printf("%d",*((int*)e->value));
	if (e->next != NULL) 
	    printf(", ");
	e = e->next;
    }
    printf("}");
}

void print_set_of_sets_of_int(set *s) {
    entry *e;
    printf("{");
    e = s->first;
    while (e != NULL) {
	print_set_of_int((set*) e->value);
	if (e->next != NULL) 
	    printf(", ");
	e = e->next;
    }
    printf("}\n\n");
}

set *add_entry(set* s, void *v) {
    entry *e;
    e = (entry*) malloc(sizeof(entry));
    e->value = v;
    e->next = s->first;
    s->first = e;
    return s;
}

set *depth_first(graph *G, int* reached, int x, set *v) {
    int *x_ptr;
    node *n;
    int y;

    reached[x] = 1;
    x_ptr = (int*) malloc(sizeof(int));
    *x_ptr = x;
    add_entry(v,(void *) x_ptr);
    n = G->neighbours[x];
    while (n != NULL) {
	y = n->v;
	if (!reached[y]) {
	    depth_first(G,reached,y,v);
	}
	n = n->next;
    }
    return v;
}

set *connectivity_components(graph *G) {
    int *reached;
    int i;
    set *cc, *v;

    reached = (int*) malloc(G->n * sizeof(int));
    for (i=0;i<G->n;i++) 
	reached[i] = 0;
    cc = make_set();
    for (i=0;i<G->n;i++) {
	if (!reached[i]) {
	    v = make_set();
	    depth_first(G,reached,i,v);
	    add_entry(cc,(void*) v);
	}
    }
    free(reached);
    return cc;
}

int *calculate_permutation(set *s, int n) {
    int *perm;
    int i;
    entry *e, *f;

    perm = (int *) malloc(n * sizeof(int));
    i = 0;
    e = s->first;
    while (e != NULL) {
	f = ((set *) e->value)->first;
	while (f != NULL) {
	    perm[*((int *) f->value)] = i;
	    i++;
	    f = f->next;
	}
	e = e->next;
    }
    return perm;
}

void print_permutation(int *perm, int n) {
    int i;
    for (i=0;i<n;i++) 
	printf("%d -> %d\n",i,perm[i]);
    printf("\n");
}

int main(int argc, char** argv) {
    double **A, **P, **PT, **AP, **PTAP;
    graph *G;
    set *cc;
    int *perm;

    printf("Creating a %dx%d block matrix with %d blocks...\n",N,N,K);
    A = make_random_block_matrix(N,K);
    printf("\"Sparsity pattern\" of the created matrix:\n");
    print_matrix_pattern(A,N);
    printf("Permuting randomly rows and columns of the matrix by %d transpositions...\n",M);
    permute_chaotically(A,N,M);
    printf("Sparsity pattern of the resulting matrix:\n");
    print_matrix_pattern(A,N);
    printf("Calculating the sparsity graph of the matrix...\n");
    G = make_graph_from_symmetric_matrix(A,N);
    printf("Adjacency list of the resulting graph:\n");
    print_graph(G);
    printf("Calculating the weak connectivity components of the graph...\n");
    cc = connectivity_components(G);
    printf("Set of the connectivity components:\n");
    print_set_of_sets_of_int(cc);
    printf("Computing a permutation on the matrix such that the sparsity pattern is regular...\n");
    perm = calculate_permutation(cc,N);
    printf("Resulting permutation:\n");
    print_permutation(perm,N);
    printf("Calculating a permutation matrix corresponding to the permutation...\n");
    P = make_permutation_matrix(perm,N);
    printf("Sparsity pattern of the permutation matrix:\n");
    print_matrix_pattern(P,N);
    printf("Computing P^T * A * P...\n");
    AP = multiply_matrix_with_matrix(A,P,N);
    PT = transpose_matrix(P,N);
    PTAP = multiply_matrix_with_matrix(PT,AP,N);
    printf("\"Sparsity pattern\" of the resulting matrix:\n");
    print_matrix_pattern(PTAP,N);
    free_matrix(PTAP,N);
    free_matrix(AP,N);
    free_matrix(PT,N);
    free_matrix(P,N);
    printf("Applying the permutation directly on the rows and columns of the matrix...\n");
    apply_permutation(A,N,perm);
    printf("\"Sparsity pattern\" of the reordered matrix:\n");
    print_matrix_pattern(A,N);
    free(perm);
    free_set_of_sets_of_int(cc);
    free_matrix(A,N);
    free_graph(G);

    return 0;
}