// Copyright: Jon Wilkening, 1999.

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

// to do: Try printing out info on clique one larger than previously
//   accepted clique to find out how important band-like behavior is
//   in an initially unbanded problem.  Experiment with compute_score()
//   Think about how you should do the supercolumns in the numerical
//   factorization.  Probably you can write a much faster symbolic
//   factorization code mimicking your original sp_chol method
//   for the second pass when the order is determined and you just
//   want to know the structure of each column/supercolumn
//   Also: once a column has become an unnecessary clique, merge the
//   space into the previous clique.  Also: rather than copying so much
//   when a new column is accepted, try doling out the new entries to
//   the largest accepted clique rather than the other way around
//   Also: check for lone cliques that have only 1 node than the next
//   accepted clique.  This may require storing m and the number of
//   fill_ins when updating the score.  if m=1 and fil_ins = 0 and
//   the previously accepted column has thickness=1 then reverse the order
//   Also: think about how to optimally register block a banded matrix.
//   will the above approach work well (i.e. put it in the order 103254...)

//#define error(s) {printf( s "\n"); exit(1);}
#define error(s) local_error(s);
#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}

typedef struct symb_chol_elt_struct {
    // later: can use hits for dead to save space
    int dead; // 0 - alive, pos size - head of clique, -1 - absorbed clique
    int aa;
    int bb;
    int cc;
} col_head;

typedef struct heap_elt_struct {
    int col;
    int score;
} heap_elt;

void local_error(const char *s) {
    printf("%s\n", s);
    exit(1);
}

#ifdef __cplusplus
#include "fence_symb_chol.cc"
#else
#define int_star int *
#define heap_elt_star heap_elt *
#define col_head_star col_head *
#endif

typedef struct heap_struct {
    int           size;
    int_star      lookup;
    heap_elt_star p;
} heap;

typedef struct symb_struct {
    int        accepted;
    int        n;
    int        *p;        // size n+1
    int        *a;        // size nnz_lower
    int        *hits;     // size n
    col_head   *v;        // size n+1
    int        *rows;     // size nnz
    int        *perm;
    heap        h;        // size n+1 (heaps start at 1, lookup at 0)
} symb;

typedef struct symb2_struct {
    // for later:
    //   we will take a given ordering and compute the symbolic factorization
    //   and supercolumn information
    int        n;
    int        *hits;     // size n
    col_head   *v;        // size n+1
    int        *rows;     // size nnz
} symb2;

void lower_to_full(symb *A) {
    int j, s, s1, n=A->n;
    int *a = A->a;
    int *p = A->p;
    col_head *v = A->v;
    int *r;
    
    for (j=0; j<n; j++) {
	s = p[j];
	s1 = p[j+1];
	if (s>=s1 || a[s]!=j)
	    error ("s>=s1 in lower_to_full");
	v[j].bb++;
	while (++s < s1) {
	    v[j].bb++;
	    v[a[s]].bb++;
	}
    }
    for (j=1; j<=n; j++) {
	v[j].aa = v[j-1].aa + v[j-1].bb;
	v[j-1].bb = v[j-1].aa;
    }
    if (2*p[n]-n != v[n].aa)
	error("2*p[n]-n != v[n].aa in lower_to_full");

    r = A->rows = (int *) malloc (v[n].aa * sizeof(int));
  
    for (j=0; j<n; j++) {
	s = p[j];
	s1 = p[j+1];
	r[v[j].bb++] = j;
	while (++s < s1) {
	    r[v[j].bb++] = a[s];
	    r[v[a[s]].cc + v[a[s]].bb++] = j;
	}
	v[j].cc = v[j].bb;
    }
}

void dump_ap(int n, int *a, int *p) {
    int j, s;
    for (j=0; j<n; j++) {
	s = p[j];
	for (s=p[j]; s<p[j+1]; s++)
	    fprintf(stderr, "%d %d, ", j, a[s]);
	fprintf(stderr, "\n");
    }
    fprintf(stderr, "\n");
}

void dump_A(symb *A) {
    int j, s, n=A->n;
    fprintf(stderr, "n = %d\n", n);
    fprintf(stderr, "hits = ");
    for (j=0; j<n; j++)
	fprintf(stderr, "%d ", A->hits[j]);
    fprintf(stderr, "\n");
    fprintf(stderr, "perm = ");
    for (j=0; j<n; j++)
	fprintf(stderr, "%d ", A->perm[j]);
    fprintf(stderr, "\n");
    fprintf(stderr, "   p = ");
    for (j=0; j<n; j++)
	fprintf(stderr, "%d ", A->p[A->perm[j]]);
    fprintf(stderr, "\n");
    fprintf(stderr, "heap(size = %d):  ", A->h.size);
    for (j=0; j<n; j++)
	fprintf(stderr, "%d ", A->h.lookup[j]);
    fprintf(stderr, ", ");
    for (j=0; j<n; j++)
	fprintf(stderr, "%d ", A->h.p[j+1].col);
    fprintf(stderr, ", ");
    for (j=0; j<n; j++)
	fprintf(stderr, "%d ", A->h.p[j+1].score);
    fprintf(stderr, "\n\n");

    for (j=0; j<n; j++) {
	fprintf(stderr, "%3d%3d%3d%3d%3d:  ", j, A->v[j].dead, A->v[j].aa,
		A->v[j].bb, A->v[j].cc);
	for (s=A->v[j].aa; s<A->v[j+1].aa; s++)
	    fprintf(stderr, "%d ", A->rows[s]);
	fprintf(stderr, "\n");
    }
    fprintf(stderr, "%3d%3d%3d%3d%3d:  ", j, A->v[j].dead, A->v[n].aa,
	    A->v[n].bb, A->v[n].cc);
    fprintf(stderr, "\n\n");
}

void initialize_A(int n, int *a, int *p, int *perm, symb *A) {
    A->accepted = 0;
    A->n = n;
    A->a = a;
    A->p = p;
    A->perm = perm;
    // calloc relied on later in some of these:
    A->h.size = n;
    A->h.p = (heap_elt *) calloc(n+1, sizeof(heap_elt));
    A->h.lookup = (int *) calloc(n, sizeof(int));
    A->v = (col_head *) calloc(n+1, sizeof(col_head));
    A->hits = (int *) calloc(n, sizeof(int));
    lower_to_full(A);
}

void heapify(heap *h, int i) {
    int ll=2*i, rr=2*i+1, smallest=i;

    if (ll <= h->size  &&  h->p[ll].score < h->p[smallest].score)
	smallest = ll;
    if (rr <= h->size  &&  h->p[rr].score < h->p[smallest].score)
	smallest = rr;
    if (smallest!=i) {
	heap_elt temp;
	SWAP(h->p[smallest], h->p[i]);
	h->lookup[h->p[smallest].col] = smallest;
	h->lookup[h->p[    i   ].col] = i;
	heapify(h, smallest);
    }
}

void fix_heap(heap *h, int i) {
    // assumes h was a heap before h[i].score was changed
    if (h->p[i/2].score < h->p[i].score)
	heapify(h, i);
    else while (i>1  &&  h->p[i/2].score > h->p[i].score) {
	heap_elt temp;
	SWAP(h->p[i], h->p[i/2]);
	h->lookup[h->p[i/2].col] = i/2;
	h->lookup[h->p[ i ].col] = i;
	i=i/2;
    }
}

void heap_delete(heap *h, int i) {
    int size = h->size;
    h->lookup[h->p[size].col] = i;
    h->lookup[h->p[i].col] = -1;
    h->p[i] = h->p[size];
    h->p[size].col = -1;
    h->p[size].score = -1;
    h->size--;
    heapify(h, i); // OK to call heapify even if i > size
}

int compute_score(int m, int m0) {
    return (m-1)*(m-1) - (m0-1)*(m0-1);
}

void compute_initial_scores(symb *A) {
    int j, m, n=A->n;
    heap *h = &A->h;
    for (j=0; j<n; j++) {
	m = A->v[j+1].aa - A->v[j].aa;
	h->lookup[j] = j+1;
	h->p[j+1].col = j;
	h->p[j+1].score = compute_score(m, 1);
    }
    for (j = n/2; j>=1; j--)
	heapify(h, j);
}

int get_next_col(symb *A) {
    // pop_heap -> col j -> collapse into one clique, fill hits[]
    int j, q, r, s, t, temp, tot=0;
    col_head_star v = A->v;
    int_star hits = A->hits;
    int_star rows = A->rows;
  
    j = A->h.p[1].col;

    if (v[j].cc <= v[j].bb)
	v[j].cc = -1;
    else {
	for (s=v[j].cc-1; s>v[j].bb; s--) {
	    // join all cliques into a linked list
	    q = rows[s-1];
	    while (v[q].cc != -1)
		q = v[q].cc;
	    v[q].cc = rows[s];
	    v[rows[s]].dead = -1;
	    rows[s] = -1;
	}
	// put the clique into col_head list
	v[j].cc = rows[s]; // s = v[j].bb
	v[rows[s]].dead = -1;
	rows[s] = -1;
    }
    r = j;
    t = v[j].aa - 1;
    for (q=j; q!=-1; q=v[q].cc) {
	// wipe out all repeats
	for (s=v[q].aa; s<v[q].bb; s++) {
	    if (hits[rows[s]] == 0) {
		hits[rows[s]] = -1;
		if (++t == v[r+1].aa) {
		    v[r].bb = v[r+1].aa;
		    r = v[r].cc;
		    t = v[r].aa;
		}
		rows[t] = rows[s];
		tot++;
	    }
	}
    }
    v[r].bb = ++t;
    while (t<v[r+1].aa)
	rows[t++] = -1;
    temp = v[r].cc;
    v[r].cc = -1;
    while ((r=temp) != -1) {
	temp = v[r].cc;
	v[r].cc = -1;
	for (t=v[r].aa; t<v[r].bb; t++)
	    rows[t] = -1;
	v[r].bb = v[r].aa;
    }
    v[j].dead = tot;
    A->p[j] = tot;
    return j;
}

void update_neighbors(symb *A, int j) {
    // include new clique into all neighbors, make j a supercolumn
    // by finding all absorbed neighbors, remove j and absorbed i's
    // from remaining neighbors, remove absorbed cliques,
    // compute new scores, update heap, clear hits
    // don't forget to clear j and absorbed i's from hits
    
    int q, r, i, s, t, u, temp, m, m0;
    heap *h = &A->h;
    col_head_star v = A->v;
    int_star rows = A->rows;
    int_star hits = A->hits;

    hits[j] = -2;
    v[j].dead--;

    for (q=j; q!=-1; q=v[q].cc)
       for (s=v[q].aa; s<v[q].bb; s++) {
	  i = rows[s]; // a neighbor
	  if (i==j) {
	     // put j first in the list so it's accepted first below
	     // (do this to get p[j] = tot, p[i] = -1)
	     int temp;
	     SWAP(rows[v[j].aa], rows[s]);
	  }
	  else {
	     m = 0;
	     for (t=v[i].cc-1; t>=v[i].bb; t--) {
		 if (v[rows[t]].dead == -1) {
		     rows[t] = rows[--v[i].cc];
		     rows[v[i].cc] = -1;
		 }
		 else {
		    // clique can't contain j or it would be dead
		    // if it contains i's that get absorbed into supercol j
		    // then m0 will be 0 and it will die.
		    // it will never happen that we need to remove j's or i's
		    // from a clique that survives
		    m0 = 0;
		    for (r=rows[t]; r!=-1; r=v[r].cc)
			for (u=v[r].aa; u<v[r].bb; u++)
			    if (hits[rows[u]] == 0) {
				m++;
				m0++;
			    }
		    if (m0 == 0) {
			// clique dead; not needed by j -- destroy completely
			v[rows[t]].dead = -1;
			for (r=rows[t]; r!=-1; r=temp) {
			    for (u=v[r].aa; u<v[r].bb; u++)
				rows[u] = -1;
			    v[r].bb = v[r].aa;
			    temp = v[r].cc;
			    v[r].cc = -1;
			}
			rows[t] = rows[--v[i].cc];
			rows[v[i].cc] = -1;
		    }
		 }
	     }
	     for (t=v[i].bb-1; t>=v[i].aa; t--) {
		 if (hits[rows[t]] != 0) {
		     rows[t] = rows[--v[i].bb];
		     rows[v[i].bb] = rows[--v[i].cc];
		     rows[v[i].cc] = -1;
		 }
		 else m++;
	     }
	  
	     if (m == 0) {
		 // j is a supercolumn.  remove column i
		 hits[i] = -2;
		 v[j].dead--;
		 // every clique that i belongs to is already dead
		 if (v[i].cc != v[i].aa) {
		     printf("error: v[%d].cc = %d, v[%d].aa = %d\n",
			    i, v[i].cc, i, v[i].aa);
		     exit(1);
		 }
		 v[i].dead = -1;
		 v[i].cc = -1;
		 A->p[i] = -1;
	     }
	     else {
		 // hook in clique j (a space is present since j was removed)
		 rows[v[i].cc++] = j;
		 hits[i] = m;
	     }
	  } // end if i!=j
       } // end for s
    
    // clear hits, record rows of supercolumn, update scores
    // remove j and i's from clique, update size of clique
    r = j;
    t = v[j].aa - 1;
    for (q=j; q!=-1; q=v[q].cc) {
	// wipe out all repeats
	for (s=v[q].aa; s<v[q].bb; s++) {
	    i = rows[s];
	    m = hits[i];
	    if (m == -2) {
		A->perm[A->accepted++] = i;
		heap_delete(h, h->lookup[i]);
	    }
	    else if (m <= 0) {
		error("m < 1");
	    }
	    else { // hits >= 1
		if (++t == v[r+1].aa) {
		    v[r].bb = v[r+1].aa;
		    r = v[r].cc;
		    t = v[r].aa;
		}
		rows[t] = i;
		m0 = v[j].dead;
		h->p[h->lookup[i]].score = compute_score(m+m0, m0);
		fix_heap(h, h->lookup[i]);
	    }      
	    hits[rows[s]] = 0;
	}
    }
    v[r].bb = ++t;
    while (t<v[r+1].aa)
	rows[t++] = -1;
    temp = v[r].cc;
    v[r].cc = -1;
    while ((r=temp) != -1) {
	temp = v[r].cc;
	v[r].cc = -1;
	for (t=v[r].aa; t<v[r].bb; t++)
	    rows[t] = -1;
	v[r].bb = v[r].aa;
    }
}

int sym_mat_get_perm(int n, int *a, int *p, int *perm) {
    // a size nnz = p[n]
    // p size n+1
    // perm size n+1 (last space workspace)
    // replace p with supercolumn info
    
    int j;
    symb A;
    
    // dump_ap(n, a, p);
    initialize_A(n, a, p, perm, &A);
    compute_initial_scores(&A);
    // dump_A(&A);
    while (A.accepted < n) {
	j = get_next_col(&A);
	update_neighbors(&A, j);
	// dump_A(&A);
    }
    return 1;
}