// Copyright: Jon Wilkening, 1999.

#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <limits.h>
#include <float.h>
#include <time.h>
#include <math.h>
#include "sym_mat.h"

#define ARPACK 0
#define DOTS 78
#define RE_ORDER 1

#define MAX(X,Y)  ((X) > (Y) ? (X) : (Y))
#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
#define TEST_FOR_ZERO  0
// TEST_FOR_ZERO affects  sym_mat_add_entry  and  sym_mat_unwrap_column 
//   if you do test for zero, roundoff can lead to
//   slightly different non-zero patterns which can lead to completely
//   different re-orderings and may be confusing to people using the code

const sym_mat_row_elt       sym_mat_row_elt0 = {-1, 0};
const sym_mat_hit_list_elt  sym_mat_hit_list_elt0 = {-1, -1, 0.0};

void sym_mat_list_init(sym_mat_elt_list *v, int n);
void sym_mat_list_free(sym_mat_elt_list *v);
void sym_mat_list_grow(sym_mat_elt_list *v);
void sym_mat_list_insert(sym_mat *A, int col, sym_mat_elt b);
void sym_mat_init(sym_mat *A, int n);
void sym_mat_free(sym_mat *A);

void sym_mat_add_entry(sym_mat *A, int i, int j, double w);
double sym_mat_get_entry(sym_mat *A, int i, int j);
void sym_mat_fill_a(sym_mat *A);
void sym_mat_get_permutations(sym_mat *A);
void sym_mat_get_perm(int n, int *a, int *p, int *perm); // external
void sym_mat_permute_vector(int n, double *x, int *px);
void sym_mat_re_order(sym_mat *A);
void sym_mat_fill_b(sym_mat *A);

int sym_mat_linked_list_insertion_sort
  (sym_mat_hit_list_elt *hits, sym_mat_elt *w, int k, int start);
int sym_mat_unwrap_column(sym_mat *A, int j);

void sym_mat_move_down_col(sym_mat *A, sym_mat_row_elt *g, int k);
int sym_mat_zipper(sym_mat *A, int j);
void sym_mat_factor1(sym_mat *A);
int sym_mat_find_row(sym_mat_elt *v, int low, int high, int x);

void sym_mat_equilibrate(sym_mat *A);
void sym_mat_factor(sym_mat *A, const char *equil, const char *re_order);
void sym_mat_apply(sym_mat *A, double *x, double *b);
void sym_mat_forward_solve(sym_mat *A, double *x);
void sym_mat_back_solve(sym_mat *A, double *x);
void sym_mat_solve1(sym_mat *A, double *x);
void sym_mat_solve(sym_mat *A, double *b, double *x);
void sym_mat_dlacon(int n, double *v, double *x,
		    int *sgn, double *est, int *kase);
double sym_mat_rcond(sym_mat *A);
void sym_mat_refine(sym_mat *A, double *b, double *x,
		    double *ferr, double *berr);

void sym_mat_list_dump(sym_mat_elt_list *v);
void sym_mat_full_matrix_dump(double *B, int m, int n);
void sym_mat_dump_A(sym_mat *A,
		    const char *filename_A, const char *filename_perm,
		    const char *filename_matlab, int precision);
void sym_mat_dump_everything(sym_mat *A, int showB);

void sym_mat_list_init(sym_mat_elt_list *v, int n) {
    int i;

    v->n = n;
    v->size = n;
    v->maxsize = 5 * n;
    v->space = (sym_mat_list_elt *)
	calloc(v->maxsize, sizeof(sym_mat_list_elt));
    for (i=0; i<n; i++) {
	// diagonal entry
	v->space[i].next = -1;
	v->space[i].row = i;
	v->space[i].val = 0.0;
    }
}

void sym_mat_list_free(sym_mat_elt_list *v) {
    free(v->space);
}

void sym_mat_list_grow(sym_mat_elt_list *v) {
    v->maxsize = 2*v->maxsize;
    v->space = (sym_mat_list_elt *)
	realloc(v->space, v->maxsize*sizeof(sym_mat_list_elt));
}

void sym_mat_list_insert(sym_mat *A, int col, sym_mat_elt b)
{
    int p=col, q=p;
    sym_mat_elt_list *v = &A->v;

    while (p!=-1) {
	if (v->space[p].row == b.row) {
	    v->space[p].val += b.val;
	    return;
	}
	q=p;
	p=v->space[p].next;
    }
    if (v->size == v->maxsize)
	sym_mat_list_grow(v);
    v->space[v->size].next = -1;
    v->space[v->size].row = b.row;
    v->space[v->size].val = b.val;
    v->space[q].next = v->size++;
    A->nnzA++;
}

void sym_mat_init(sym_mat *A, int n) {
    int i;
    if (n<=0) {
	printf("error: n must be positive in sym_mat_init\n");
	exit(1);
    }
    A->n = n;
    A->flops = 0.0;
    A->nnzA = n;
    A->nnzL = 0;
    A->scond = 1.0; // smallest eq / largest eq

    A->no_more_entries = 0;
    A->already_factored = 0;
    A->b_is_set_up = 0;

    A->col = (sym_mat_elt *) calloc(n, sizeof(sym_mat_elt));
    A->hits = (sym_mat_hit_list_elt *) calloc(n, sizeof(sym_mat_hit_list_elt));
    A->row = (sym_mat_row_elt *) calloc(n, sizeof(sym_mat_row_elt));
    A->pa = (int *) calloc(n+1, sizeof(int));
    A->a = (int *) calloc(1, sizeof(int));
    A->pb = (int *) calloc(n+1, sizeof(int));
    A->b = (sym_mat_elt *) calloc(1, sizeof(sym_mat_elt));
    A->pc = (int *) calloc(n+1, sizeof(int));
    A->c = (sym_mat_elt *) calloc(1, sizeof(sym_mat_elt));
    A->perm1 = (int *) calloc(n+1, sizeof(int));
    A->perm2 = (int *) calloc(n, sizeof(int));
    A->eq = (double *) calloc(n, sizeof(double));
    sym_mat_list_init(&A->v, n);
    for (i=0; i<n; i++) {
	A->perm1[i] = i;
	A->perm2[i] = i;
	A->eq[i] = 1.0;
    }
    for (i=0; i<n; i++) {
	A->row[i] = sym_mat_row_elt0;
	A->hits[i] = sym_mat_hit_list_elt0;
    }
}

void sym_mat_free(sym_mat *A) {
    free(A->col);
    free(A->hits);
    free(A->row);
    free(A->pa);
    free(A->a);
    free(A->pb);
    free(A->b);
    free(A->pc);
    free(A->c);
    free(A->perm1);
    free(A->perm2);
    free(A->eq);
    sym_mat_list_free(&A->v);
}

void sym_mat_add_entry(sym_mat *A, int i, int j, double w) {
    if (i<0 || i>=A->n || j<0 || j>=A->n)
	fprintf(stderr, "error: index out of bounds (i=%d, j=%d, n=%d) "
		"(ignoring)\n", i, j, A->n);
    else if (A->no_more_entries == 1)
	fprintf(stderr, "error: can't add entries after factoring\n");
    else if (i>=j && (!TEST_FOR_ZERO || w!=0.0)) {
	sym_mat_elt b;
	b.row = i;
	b.val = w;
	sym_mat_list_insert(A, j, b);
	A->b_is_set_up = 0;
    }
}

double sym_mat_get_entry(sym_mat *A, int i, int j) {
    int ii, jj, ss;
    double equil;

    if (A->b_is_set_up == 0)
	sym_mat_fill_b(A);
  
    if (i<0 || i>=A->n || j<0 || j>=A->n) {
	fprintf(stderr, "error: index out of bounds (i=%d, j=%d, n=%d) "
		"(ignoring)\n", i, j, A->n);
	return 0.0;
    }
    ii = A->perm2[i];
    jj = A->perm2[j];
    if (ii < jj) {
	int temp;
	SWAP(ii, jj);
    }
    equil = 1.0 / (A->eq[i] * A->eq[j]);

    ss = sym_mat_find_row(A->b, A->pb[jj], A->pb[jj+1]-1, ii);
    if (ss==-1)
	return 0.0;
    return A->b[ss].val * equil;
}

void sym_mat_fill_a(sym_mat *A) {
    int k, j, t=0;

    A->a = (int *) realloc(A->a, A->v.size * sizeof(int));
    for (j=0; j<A->n; j++) {
	int colsize = sym_mat_unwrap_column(A, j);
	for (k=0; k<colsize; k++)
	    A->a[t++] = A->col[k].row;
	A->pa[j+1]=t;
    }
    A->a = (int *) realloc(A->a, A->pa[A->n] * sizeof(int));
}

void sym_mat_get_permutations(sym_mat *A) {
    // PAP' = LL'  <-- ultimate goal
    // PAP'(i,j) =    A(perm1[i], perm1[j])
    //    A(i,j) = PAP'(perm2[i], perm2[j])

    int j;
    sym_mat_fill_a(A);
    sym_mat_get_perm(A->n, A->a, A->pa, A->perm1);

    for (j=0; j<A->n; j++)
	A->perm2[A->perm1[j]] = j;
    free(A->a);
    A->a = (int *) calloc(1, sizeof(int));
    for (j=0; j<=A->n; j++)
	A->pa[j] = 0;
}

void sym_mat_permute_vector(int n, double *x, int *px) {
    int start=0, i, old_i;
    double tmp;
    while (start < n) {
	old_i = start;
	i = px[old_i];
	if (i >= n) {
	    start++;
	    continue;
	}
	tmp = x[start];
	while (1) {
	    x[old_i] = x[i];
	    px[old_i] = i + n;
	    old_i = i;
	    i = px[old_i];
	    if (i >= n)
		break; // occurs when perm[start] = start
	    if (i == start) {
		x[old_i] = tmp;
		px[old_i] = i + n;
		break;
	    }
	}
	start++;
    } // end while
    for (i=0; i<n; i++)
	px[i] = px[i] - n;
}

void sym_mat_re_order(sym_mat *A) {
    int j;

    // put columns in correct order (v[i] = oldv[perm1[i]])
    A->no_more_entries = 1;
    {
	int start=0, i, old_i, n=A->n, *px = A->perm1;
	sym_mat_list_elt tmp, *v = A->v.space;
	while (start < n) {
	    old_i = start;
	    i = px[old_i];
	    if (i >= n) {
		start++;
		continue;
	    }
	    tmp = v[start];
	    while (1) {
		v[old_i] = v[i];
		px[old_i] = i + n;
		old_i = i;
		i = px[old_i];
		if (i >= n)
		    break;
		if (i == start) {
		    v[old_i] = tmp;
		    px[old_i] = i + n;
		    break;
		}
	    }
	    start++;
	} // end while
	for (i=0; i<n; i++)
	    px[i] = px[i] - n;
    }
    
    for (j=0; j<A->n; j++) {
	// rows in correct order for first j-1 columns
	// so it's OK to flip elements across diag into them
	int p=j, nextp, oldp = -1;
	sym_mat_list_elt *v = A->v.space;
	while (p!=-1) {
	    int i = A->perm2[v[p].row];
	    nextp = v[p].next;
	    if (i < j) {
		// flip across diagonal
		v[p].row = j;
		if (oldp == -1)
		    // can't happen (0 occupies diags if nothing else)
		    fprintf(stderr, "flip error: i=%d, j=%d, orig col: %d\n",
			    i, j, A->perm1[j]);
		else
		    v[oldp].next = nextp;
		v[p].next = v[i].next;
		v[i].next = p;
		p = nextp;
	    }
	    else {
		v[p].row = i;
		oldp = p;
		p = nextp;
	    }
	}
    } // end for j
}

void sym_mat_fill_b(sym_mat *A) {
    int k, j, t=0;
    
    if (A->v.n == -1)
	return;
    free(A->b);
    A->b = (sym_mat_elt *)
	malloc(A->v.size * sizeof(sym_mat_elt));
    for (j=0; j<A->n; j++) {
	int colsize = sym_mat_unwrap_column(A, j);
	for (k=0; k<colsize; k++)
	    A->b[t++] = A->col[k];
	A->pb[j+1]=t;
    }
    A->b = (sym_mat_elt *)
	realloc(A->b, A->pb[A->n] * sizeof(sym_mat_elt));
    A->b_is_set_up = 1;
    A->nnzA = A->pb[A->n];
}

int sym_mat_linked_list_insertion_sort
(sym_mat_hit_list_elt *hits, sym_mat_elt *w, int k, int end)
{
    // if end == -2 then use w for initial list
    // otherwise list already set up (-2 terminates list)
    // This is an insertion sort algorithm from back to front which
    // uses a direct map and linked list to avoid shifting regions of
    // data by 1.  It also keeps track of the last insertion location
    // for a first guess at where the next value belongs. For FE data,
    // I am finding this at least 10 times faster than qsort.
    
    int i, l, current, guess, prev, temp;
    
    if (end == -2) {
	if (k<=0)
	    return 0;
	for (i=0; i<k; i++) {
	    l = w[i].row;
	    hits[l].prev = end;
	    hits[l].val = w[i].val;
	    end = l;
	}
    }

    i=1;
    current = end;
    guess = end;
    temp = hits[guess].prev;
    prev = hits[current].prev;
    while (prev != -2) {
	i++;
	if (prev < current) {
	    current = prev;
	    prev = hits[current].prev;
	}
	else {
	    // insert prev
	    hits[current].prev = hits[prev].prev;
	    if (prev > guess) {
		if (prev > end) {
		    hits[prev].prev = end;
		    end = prev;
		    prev = hits[current].prev;
		    continue;
		}
		guess = end;
		temp = hits[guess].prev;
	    }
	    while (temp > prev) {
		guess = temp;
		temp = hits[guess].prev;
	    }
	    hits[guess].prev = prev;
	    hits[prev].prev = temp;
	    temp = prev;
	    prev = hits[current].prev;
	}
    }
    k=i;
    current = end;
    while (current != -2) {
	temp = hits[current].prev;
	w[--i].row = current;
	w[i].val = hits[current].val;
	hits[current].prev = -1;
	hits[current].val = 0.0;
	current = temp;
    }
    return k;
}

int sym_mat_unwrap_column(sym_mat *A, int j) {
    int colsize = 0;
    int p=j;
    while (p!=-1) {
	if (!TEST_FOR_ZERO  ||  A->v.space[p].val != 0.0  ||  p==j) {
	    A->col[colsize].row = A->v.space[p].row;
	    A->col[colsize++].val = A->v.space[p].val;
	}
	p=A->v.space[p].next;
    }
    if (colsize>0)
	sym_mat_linked_list_insertion_sort(A->hits, A->col, colsize, -2);
    return colsize;
}

void sym_mat_move_down_col(sym_mat *A, sym_mat_row_elt *g, int k) {
    // insert column k into first position in list for row next_row
    if (++g[k].idx >= A->pc[k+1])
	g[k] = sym_mat_row_elt0;
    else {
	int next_row = A->c[g[k].idx].row;
	g[k].next = g[next_row].next;
	g[next_row].next = k;
    }
}

int sym_mat_zipper(sym_mat *A, int j) {
    int k, next_k, s, s0, s1, colsize, end;
    sym_mat_row_elt *g = A->row;
    sym_mat_elt *z;
    sym_mat_hit_list_elt *hits = A->hits;
    double alpha;

    end = -2;
    z = A->b;
    s1 = A->pb[j+1];
    for (s = A->pb[j]; s<s1; s++) {
	int l = z[s].row;
	hits[l].val = z[s].val;
	hits[l].prev = end;
	end = l;
    }

    z = A->c;
    k = g[j].next;
    g[j].next = -1;
    while (k != -1) {
	next_k = g[k].next;
	s0 = g[k].idx;
	s1 = A->pc[k+1];
	alpha = -z[s0].val; // LL'
	for (s=s0; s<s1; s++) {
	    int l = z[s].row;
	    hits[l].val += alpha * z[s].val; // LL'
	    if (hits[l].prev == -1) {
		hits[l].prev = end;
		end = l;
	    }
	}
	A->flops += 2.0*(s1 - s0);
	sym_mat_move_down_col(A, g, k);
	k = next_k;
    }
    colsize =
	sym_mat_linked_list_insertion_sort(hits, A->col, 0, end);
    return colsize;
}

void sym_mat_factor1(sym_mat *A) {
    int j, s, t, n, colsize;
    sym_mat_elt *col = A->col;
    
    if (A->already_factored) {
	fprintf(stderr, "already factored\n");
	return;
    }
    A->already_factored = 1;
    n = A->n;
    A->pc[n] = 2 * A->pb[n];
    A->c = (sym_mat_elt *)
	realloc(A->c, A->pc[n] * sizeof(sym_mat_elt));
    for (j=0; j<n; j++) {
	colsize = sym_mat_zipper(A, j);
	if (colsize==0  ||  col[0].row != j  ||  col[0].val <= 0.0) {
	    fprintf(stderr, "not positive definite\n");
	    exit(1); // LL'
	}
	col[0].val = sqrt(col[0].val); // LL'
	for (s=1; s<colsize; s++)
	    col[s].val /= col[0].val; // LL'
	if ( A->pc[j]+colsize > A->pc[n] ) {
	    A->pc[n] += MAX(colsize, A->pc[n]);
	    A->c = (sym_mat_elt *)
		realloc(A->c, A->pc[n] * sizeof(sym_mat_elt));
	}
	for (s=0, t=A->pc[j]; s<colsize; s++)
	    A->c[t++] = col[s];
	A->pc[j+1] = t;
	if (colsize > 1) {
	    sym_mat_row_elt *g = A->row;
	    g[j].idx = A->pc[j]+1;
	    g[j].next = g[col[1].row].next;
	    g[col[1].row].next = j;
	}
	A->flops += colsize;
    }
    A->c = (sym_mat_elt *)
	realloc(A->c, A->pc[n] * sizeof(sym_mat_elt));
    A->nnzL = A->pc[n];
}

int sym_mat_find_row(sym_mat_elt *v, int low, int high, int x) {
    int mid;
    while (low<=high) {
	mid = (low+high)/2;
	if (x < v[mid].row)
	    high = mid - 1;
	else if (x > v[mid].row)
	    low = mid + 1;
	else
	    return mid;
    }
    return -1;
}

void sym_mat_equilibrate(sym_mat *A) {
    int j;
    double smin = DBL_MAX;
    double smax = -1;
    // constants chosen to agree with LAPACK on my machine
    double scond;
    double thresh = 0.1;
    double small = 2.2250738585072e-308 / 4.44089209850063e-16;
    double large = 1.0/small;
    double temp;
    sym_mat_list_elt *v = A->v.space;

    if (A->no_more_entries == 1) {
	fprintf(stderr, "equilibrate called after factoring "
	    "(or called twice)\nnothing done\n");
	return;
    }
    for (j=0; j<A->n; j++) {
	temp = v[j].val;
	if (temp <= 0.0) {
	    fprintf(stderr, "non-positive diagonal entry in column %d, %f\n",
		    j, temp);
	    // smin = 1.0; smax = 1.0; break;
	    exit(1); // LL'
	}
	if (temp < smin)
	    smin = temp;
	if (temp > smax)
	    smax = temp;
    }
    scond = sqrt(smin/smax);
    if (scond >= thresh  &&  smax >= small  &&  smax <= large)
	; // no equilibration necessary
    else {
	A->no_more_entries = 1;
	A->scond = scond;
	for (j=0; j<A->n; j++) {
	    A->eq[j] = 1.0/sqrt(v[j].val);
	    v[j].val = 1.0;
	}
	for (j=0; j<A->n; j++) {
	    double temp2 = A->eq[j];
	    int p=v[j].next;
	    while (p != -1) {
		v[p].val *= temp2;
		v[p].val *= A->eq[v[p].row];
		p = v[p].next;
	    }
	}
    }
}

void sym_mat_factor(sym_mat *A, const char *equil, const char *re_order) {

    if (equil[0] != 'N' && equil[0] != 'n')
	sym_mat_equilibrate(A);
    
    if (re_order[0] != 'N' && re_order[0] != 'n') {
	sym_mat_get_permutations(A);
	sym_mat_re_order(A);
    }
    sym_mat_fill_b(A);

    // free up list space
    A->no_more_entries = 1;
    A->v.n = A->v.size = A->v.maxsize = -1;
    free(A->v.space);
    A->v.space = (sym_mat_list_elt *)
	calloc(1, sizeof(sym_mat_list_elt));

    //for (i=0; i<DOTS; i++)
    //  fprintf(stderr, ".");
    //fprintf(stderr,"\n");
    //fprintf(stderr,"factoring A\n");

    sym_mat_factor1(A);

    //fprintf(stderr, "non-zero entries in L: %d, flops to factor A: %d\n",
    //	 A->pc[A->n], A->flops);
    //for (i=0; i<DOTS; i++)
    //  fprintf(stderr, ".");
    //fprintf(stderr,"\n");
}

void sym_mat_apply(sym_mat *A, double *x, double *b) {
    // replace b with A*x. in-situ OK
    int n = A->n;
    int j, s, s1;
    double bbj, xj, temp;
    double *bb = b;
    
    if (b==x)
	bb = (double *) malloc(A->n * sizeof(double));
    for (j=0; j<n; j++)
	bb[j] = 0.0;

    for (j=0; j<n; j++)
	x[j] /= A->eq[j];
    sym_mat_permute_vector(n, x, A->perm1);
  
    if (A->b_is_set_up == 0)
	sym_mat_fill_b(A);
    
    for (j=0; j<n; j++) {
	s = A->pb[j];
	s1 = A->pb[j+1];
	xj = x[j];
	bbj = A->b[s].val * xj;
	while (++s < s1) {
	    temp = A->b[s].val;
	    bbj += temp * x[A->b[s].row];
	    bb[A->b[s].row] += temp * xj;
	}
	bb[j] += bbj;
    }

    sym_mat_permute_vector(n, bb, A->perm2);
    for (j=0; j<n; j++)
	bb[j] /= A->eq[j];
    
    if (b==x) {
	for (j=0; j<n; j++)
	    b[j] = bb[j];
	free(bb);
    }
    else { // unscale and unpermute x
	sym_mat_permute_vector(n, x, A->perm2);
	for (j=0; j<n; j++)
	    x[j] *= A->eq[j];
    }
}

void sym_mat_forward_solve(sym_mat *A, double *x) {
    int k, s, s1, n=A->n;

    // forward solve
    for (k=0; k<n; k++) {
	s = A->pc[k];
	s1 = A->pc[k+1];
	x[k] /= A->c[s].val; // LL'
	while (++s < s1)
	    x[A->c[s].row] -= A->c[s].val * x[k]; // LL'
    }
}  

void sym_mat_back_solve(sym_mat *A, double *x) {
    int k, s, s0, n=A->n;

    // back solve
    for (k=n-1; k>=0; k--) {
	s = A->pc[k+1];
	s0 = A->pc[k];
	while (--s > s0)
	    x[k] -= A->c[s].val * x[A->c[s].row]; // LL'
	x[k] /= A->c[s].val; // LL'
    }
}

void sym_mat_solve1(sym_mat *A, double *x) {
    sym_mat_forward_solve(A, x);
    sym_mat_back_solve(A, x);
}

void sym_mat_solve(sym_mat *A, double *b, double *x) {
    int i, n=A->n;

    if (!A->already_factored) {
	fprintf(stderr, "A must be factored to call sym_mat_solve\n");
	return;
    }
    // scale, permute and copy RHS
    if (x==b) {
	for (i=0; i<n; i++)
	    x[i] *= A->eq[i];
	sym_mat_permute_vector(n, x, A->perm1);
    }
    else {
	for (i=0; i<n; i++)
	    x[i] = b[A->perm1[i]] * A->eq[A->perm1[i]];
	// I used to alter b to agree with LAPACK (see sym_mat_refine later)
	// but I think it's better to leave b alone
	// for (i=0; i<n; i++)
	//   b[i] = x[i];
    }

    sym_mat_solve1(A, x);

    // un-scale and re-order solution
    // oldx = x, x[i] = A->eq[i] * oldx[A->perm2[i]]
    sym_mat_permute_vector(n, x, A->perm2);
    for (i=0; i<n; i++)
	x[i] *= A->eq[i];
}

void sym_mat_apply_invL(sym_mat *A, double *b, double *x) {
    int i, n=A->n;

    if (!A->already_factored) {
	fprintf(stderr, "A must be factored to call sym_mat_apply_invL\n");
	return;
    }
    // scale, permute and copy RHS
    if (x==b) {
	for (i=0; i<n; i++)
	    x[i] *= A->eq[i];
	sym_mat_permute_vector(n, x, A->perm1);
    }
    else {
	for (i=0; i<n; i++)
	    x[i] = b[A->perm1[i]] * A->eq[A->perm1[i]];
    }
    
    sym_mat_forward_solve(A, x);

    // un-scale and re-order solution
    // oldx = x, x[i] = A->eq[i] * oldx[A->perm2[i]]
    sym_mat_permute_vector(n, x, A->perm2);
    for (i=0; i<n; i++)
	x[i] *= A->eq[i];
}

void sym_mat_dlacon(register int n, double *v, double *x,
		    int *sgn, double *est, int *kase) {
    // from higham p. 295  and  dlacon.f
    // I more or less copied the LAPACK implementation, although
    // it surely could be done in a less convoluted way.  It returns
    // to the calling program every time it needs to compute Ax or A'x
    
    const int itmax = 5;
    register int i;
    register double temp;
    static int iter, j, jump;
    static double altsgn, estold, xj;

    if (*kase == 0) {
	for (i=0; i<n; i++)
	    x[i] = 1.0/n;
	*kase = 1;
	jump = 1;
	return;
    }
    if (jump==1) {
	if (n==1) {
	    v[0] = x[0];
	    *est = fabs(v[0]);
	    *kase = 0;
	    return;
	}
	*est = 0;
	for (i=0; i<n; i++)
	    *est += fabs(x[i]);
	for (i=0; i<n; i++) {
	    sgn[i] = (x[i] < 0.0) ? -1 : 1;
	    x[i] = sgn[i];
	}
	*kase = 2;
	jump = 2;
	return;
    }
    if (jump==2) {
	iter = 2;
	temp = 0.0;
	j = 0;
	for (i=0; i<n; i++) {
	    if (temp < fabs(x[i])) {
		temp = fabs(x[i]);
		j = i;
	    }
	    x[i] = 0.0;
	}
	x[j] = 1.0;
	*kase = 1;
	jump = 3;
	return;
    }
    if (jump==3) {
	estold = *est;
	*est = 0;
	for (i=0; i<n; i++) {
	    v[i] = x[i];
	    *est += fabs(x[i]);
	}
	for (i=0; i<n; i++)
	    if (x[i] < 0.0  &&  sgn[i]==1)
		break;
	if (i==n  ||  *est < estold)
	    goto L120;
	for (i=0; i<n; i++) {
	    sgn[i] = (x[i] < 0.0) ? -1 : 1;
	    x[i] = sgn[i];
	}
	*kase = 2;
	jump = 4;
	return;
    }
    if (jump==4) {
	xj = x[j];
	temp = 0.0;
	j = 0;
	for (i=0; i<n; i++) {
	    if (temp < fabs(x[i])) {
		temp = fabs(x[i]);
		j = i;
	    }
	    x[i] = 0.0;
	}
	if (xj != temp  &&  iter < itmax) {
	    iter++;
	    x[j] = 1.0;
	    *kase = 1;
	    jump = 3;
	    return;
	}
	// otherwise falls through to L120
    }
    if (jump==5) {
	temp = 0.0;
	for (i=0; i<n; i++)
	    if (temp < fabs(x[i]))
		temp = fabs(x[i]);
	temp *= 2.0/(3*n);
	if (temp > *est) {
	    for (i=0; i<n; i++)
		v[i] = x[i];
	    *est = temp;
	}
	*kase = 0;
	return;
    }

L120:
    altsgn = 1.0;
    for (i=0; i<n; i++) {
	x[i] = altsgn*i/(n-1);
	altsgn = -altsgn;
    }
    *kase = 1;
    jump = 5;
    return;
}

double sym_mat_rcond(sym_mat *A) {
    // based on dpocon.f  and  dlansy.f

    double anorm = 0.0, ainv_norm, temp;
    int j, n=A->n, s, s1, kase, count;
    int *iwork;
    double *work;

    if (!A->already_factored) {
	fprintf(stderr, "A must be factored to call sym_mat_rcond\n");
	return 0;
    }
    iwork = (int *) calloc (n, sizeof(int));
    work = (double *) calloc (2*n, sizeof(double));

    // compute 1 norm of A
    for (j=0; j<n; j++) {
	s = A->pb[j], s1 = A->pb[j+1];
	work[j] += A->b[s].val;
	while (++s < s1) {
	    temp = fabs(A->b[s].val);
	    work[j] += temp;
	    work[A->b[s].row] += temp;
	}
    }
    for (j=0; j<n; j++)
	if (work[j] > anorm)
	    anorm = work[j];

    // estimate 1 norm of inv(A)
    kase = 0;
    count = 0;
    while (1) {
	sym_mat_dlacon(n, work+n, work, iwork, &ainv_norm, &kase);
	if (kase == 0)
	    break;
	sym_mat_solve1(A, work);
	count++;
    }
    // printf("cost of rcond estimate: %d applications of inv(A)\n", count);
    free(iwork);
    free(work);
    return 1.0/anorm/ainv_norm;
}

void sym_mat_refine(sym_mat *A, double *b, double *x,
		    double *ferr_ptr, double *berr_ptr) {
    // modeled on LAPACK's dporfs.f
    
    int count=0, itmax=4, i, k, kase, s, s1, n=A->n;
    int nz = n+1;
    int *iwork;
    double eps = 2.22044604925031e-16;
    double safmin = 2.2250738585072e-308;
    double safe1 = nz*safmin;
    double safe2 = safe1 / eps;
    double lstres = 0.0;
    double *resid, *denom, *w, *v;
    double temp, xk, rk, dk, berr, ferr;
    
    if (!A->already_factored) {
	fprintf(stderr, "A must be factored to call refine\n");
	return;
    }

    // I'm mimicking the workspace of dporfs.f here:
    iwork = (int *) calloc(n, sizeof(int));
    denom = (double *) calloc(3*n, sizeof(double));
    w = denom;
    resid = denom + n;
    v = denom + 2*n;
    
    // re-scale and re-order x
    for (i=0; i<n; i++)
	x[i] /= A->eq[i];
    sym_mat_permute_vector(n, x, A->perm1);
    
    // re-scale and re-order b
    // (in LAPACK b is already in this state; see comment in sym_mat_solve)
    for (i=0; i<n; i++)
	b[i] *= A->eq[i];
    sym_mat_permute_vector(n, b, A->perm1);
    
    // do iterative refinement
    while (1) {
	// compute residual b-Ax  and  denom |b|+|A||x|
	for (i=0; i<n; i++) {
	    resid[i] = b[i];
	    denom[i] = fabs(b[i]);
	}
	for (k=0; k<n; k++) {
	    s = A->pb[k], s1 = A->pb[k+1];
	    xk = x[k];
	    rk = resid[k];
	    dk = denom[k];
	    temp = A->b[s].val * xk;
	    rk -= temp;
	    dk += fabs(temp);
	    while (++s < s1) {
		temp = A->b[s].val * x[A->b[s].row];
		rk -= temp;
		dk += fabs(temp);
		temp = A->b[s].val * xk;
		resid[A->b[s].row] -= temp;
		denom[A->b[s].row] += fabs(temp);
	    }
	    resid[k] = rk;
	    denom[k] = dk;
	}
	berr = 0.0;
	for (i=0; i<n; i++) {
	    if (denom[i]>safe2)
		temp = fabs(resid[i])/denom[i];
	    else
		temp = (fabs(resid[i]) + safe1)/(denom[i] + safe1);
	    if (temp>berr)
		berr = temp;
	}
	if (berr>eps  &&  (count==0 || 2*berr<lstres)  &&  count<itmax) {
	    sym_mat_solve1(A, resid);
	    for (i=0; i<n; i++)
		x[i] += resid[i];
	    lstres = berr;
	    count++;
	}
	else break;
    }

    // estimate infinity norm of inv(A) * diag(w),
    // w = abs(R) + nz*eps*(abs(A)*abs(x)+abs(b))
    for (i=0; i<n; i++) {
	if (w[i] > safe2)
	    w[i] = fabs(resid[i]) + nz*eps*w[i];
	else
	    w[i] = fabs(resid[i]) + nz*eps*w[i] + safe1;
    }

    kase = 0;
    while (1) {
	sym_mat_dlacon(n, v, resid, iwork, &ferr, &kase);
	if (kase == 0)
	    break;
	if (kase == 1) {
	    // multiply by diag(w)*inv(A')
	    sym_mat_solve1(A, resid);
	    for (i=0; i<n; i++)
		resid[i] *= w[i];
	}
	else { // kase == 2
	    // multiply by inv(A)*diag(w)
	    for (i=0; i<n; i++)
		resid[i] *= w[i];
	    sym_mat_solve1(A, resid);
	}
    }

    // normalize the error
    lstres = 0.0;
    for (i=0; i<n; i++) {
	temp = fabs(x[i]);
	if (lstres < temp)
	    lstres = temp;
    }
    if (lstres != 0.0)
	ferr /= lstres * A->scond;

    // unscale and re-order x
    sym_mat_permute_vector(n, x, A->perm2);
    for (i=0; i<n; i++)
	x[i] *= A->eq[i];
    
    // unscale and re-order b (not done in LAPACK)
    sym_mat_permute_vector(n, b, A->perm2);
    for (i=0; i<n; i++)
	b[i] /= A->eq[i];

    *ferr_ptr = ferr;
    *berr_ptr = berr;
    free(denom);
    free(iwork);
}

void sym_mat_list_dump(sym_mat_elt_list *v) {
    int i;
    printf("n = %d,  size = %d,  maxsize = %d\n",
	   v->n, v->size, v->maxsize);
    for (i=0; i<v->size; i++)
	printf("%d  %d  %10.3e\n", v->space[i].next,
	       v->space[i].row, v->space[i].val);
}

void sym_mat_full_matrix_dump(double *B, int m, int n) {
    int i, j;
    for (i=0; i<m; i++) {
	for (j=0; j<n; j++)
	    printf("%11.5g ", B[j*m+i]);
	printf("\n");
    }
}

// fixme: filename_matlab
void sym_mat_dump_A(sym_mat *A,
		    const char *filename_A, const char *filename_perm,
		    const char *filename_matlab, int precision) {
    // dumps lower triangle of B=PAP' (equilibration is reversed if necessary)
    // dumps permutation P to get A back (in matlab: A = B(P,P))
    // adds one to all indices so matlab can read the files
    // fixme: the choice of filenames needs to be re-thought.
    int i, j, idx, max=-1, ave=0;
    double alpha;
    char buf[80];
    FILE *fpA, *fpPerm, *fpMatlab;
    
    fpA = fopen(filename_A, "w");
    fpPerm = fopen(filename_perm, "w");
    fpMatlab = fopen(filename_matlab, "w");

    if (A->b_is_set_up == 0)
	sym_mat_fill_b(A);
    for (j=0; j<A->n; j++) {
	int temp = A->pb[j+1] - A->pb[j];
	ave += temp;
	if (temp > max)
	    max = temp;
	for (idx=A->pb[j]; idx<A->pb[j+1]; idx++) {
	    i = A->b[idx].row;
	    alpha = 1.0 / A->eq[A->perm2[i]] / A->eq[A->perm2[j]];
	    sprintf(buf,"%%d %%d %%.%dg\n", precision);
	    fprintf(fpA, buf, i+1, j+1, alpha * A->b[idx].val);
	}
    }
    for (j=0; j<A->n; j++)
	fprintf(fpPerm, "%d\n", A->perm2[j]+1);
    
    fprintf(fpMatlab, "load %s\nload %s\n",
	    filename_A, filename_perm);
    fprintf(fpMatlab, "%s1 = spconvert(%s);\n%sP = %s;\n",
	    filename_A, filename_A, filename_A, filename_perm);
    fprintf(fpMatlab, "%s = %s1(%sP,%sP) + %s1(%sP,%sP)' "
	    "- diag(diag(%s1(%sP,%sP)));\n",
	    filename_A, filename_A, filename_A, filename_A,
	    filename_A, filename_A, filename_A, filename_A, 
	    filename_A, filename_A);
    
    // fprintf(stderr, "max entries per column (of lower triangle): %d\n"
    //	     "average entries per column %f\n", max, 1.0*ave/A->n);
    
    fclose(fpA);
    fclose(fpPerm);
    fclose(fpMatlab);
}

void sym_mat_dump_L(sym_mat *A, char *filename_L, char *filename_equil,
		    char *filename_perm, char *filename_matlab, int precision)
{
    // dumps lower triangle of L, LL'=PAP'
    // dumps equilibration E and permutation P
    // (in matlab: diag(E)*A*diag(E) = L(P,P)*L(P,P)'
    // adds one to all indices so matlab can read the files
    int i, j, idx, max=-1, ave=0;
    char buf[80];
    FILE *fpL, *fpEquil, *fpPerm, *fpMatlab;
    
    fpL = fopen(filename_L, "w");
    fpEquil = fopen(filename_equil, "w");
    fpPerm = fopen(filename_perm, "w");
    fpMatlab = fopen(filename_matlab, "w");
    
    if (!A->already_factored) {
	printf("A must be factored to call sym_mat_dump_L\n");
	return;
    }
    for (j=0; j<A->n; j++) {
	int temp = A->pc[j+1] - A->pc[j];
	ave += temp;
	if (temp > max)
	    max = temp;
	for (idx=A->pc[j]; idx<A->pc[j+1]; idx++) {
	    i = A->c[idx].row;
	    sprintf(buf,"%%d %%d %%.%dg\n", precision);
	    fprintf(fpL, buf, i+1, j+1, A->c[idx].val);
	}
    }
    for (j=0; j<A->n; j++)
	fprintf(fpPerm, "%d\n", A->perm2[j]+1);
    
    sprintf(buf,"%%.%dg\n", precision);
    for (j=0; j<A->n; j++)
	fprintf(fpEquil, buf, A->eq[j]);
    
    fprintf(fpMatlab, "load %s\nload %s\nload %s\n",
	    filename_L, filename_equil, filename_perm);
    fprintf(fpMatlab, "L = spconvert(%s)\nE = %s\nP = %s\n",
	    filename_L, filename_equil, filename_perm);
    fprintf(fpMatlab, "clear %s\nclear %s\nclear %s\n",
	    filename_L, filename_equil, filename_perm);
    fprintf(fpMatlab, "A = diag(1.0 ./ E)*L(P,P)*L(P,P)'*diag(1.0 ./ E)\n");
    
    // fprintf(stderr, "max entries per column (of lower triangle): %d\n"
    //	     "average entries per column %f\n", max, 1.0*ave/A->n);
    
    fclose(fpL);
    fclose(fpEquil);
    fclose(fpPerm);
    fclose(fpMatlab);
}

void sym_mat_dump_everything(sym_mat *A, int showB) {
    int i, j, n=A->n;
    double *B;
    
    B = (double *) malloc(n*n*sizeof(double));
    
    for (i=0; i<A->n; i++)
	for (j=0; j<A->n; j++)
	    B[j*n+i] = 0;
    
    printf("n = %d\n", A->n);
    
    printf("list: ");
    sym_mat_list_dump(&A->v);
    printf("\n");

    printf("perm1 = ");
    for (j=0; j<A->n; j++)
	printf("%d ", A->perm1[j]);
    
    printf("\nperm2 = ");
    for (j=0; j<A->n; j++)
	printf("%d ", A->perm2[j]);

    printf("\neq = ");
    for (j=0; j<A->n; j++)
	printf("%.2g ", A->eq[j]);

    printf("\npa = ");
    for (j=0; j<=A->n; j++)
	printf("%d ", A->pa[j]);

    printf("\na = ");
    for (j=0; j<A->pa[A->n]; j++)
	printf("%d ", A->a[j]);

    printf("\npb = ");
    for (j=0; j<=A->n; j++)
	printf("%d ", A->pb[j]);

    printf("\nb = ");
    for (j=0; j<A->pb[A->n]; j++)
	printf("%d %g, ", A->b[j].row, A->b[j].val);

    printf("\npc = ");
    for (j=0; j<=A->n; j++)
	printf("%d ", A->pc[j]);

    printf("\nc = ");
    for (j=0; j<A->pc[A->n]; j++)
	printf("%d %g, ", A->c[j].row, A->c[j].val);
    printf("\n");

    for (j=0; j<A->n; j++) {
	for (i=A->pc[j]; i<A->pc[j+1]; i++)
	    B[j*n + A->c[i].row] = A->c[i].val;
    }
    if (showB)
	sym_mat_full_matrix_dump(B, A->n, A->n);
    free(B);
}

#if ARPACK

#include "lapack.h" // there's a daxpy in the code below

void dsaupd_(int *ido, char *bmat, int *n, char *which, int *nev,
	     double *tol, double *resid,
	     int *ncv, double *v, int *ldv, int *iparam,
	     int *ipntr, double *workd, double *workl,
	     int *lworkl, int *info);

void dseupd_( int *rvec, char *howmny, int *select, double *d,
	      double *z, int *ldz, double *sigma, 
	      char *bmat, int *n, char *which, int *nev, double *tol,
	      double *resid, int *ncv, double *v, int *ldv,
	      int *iparam, int *ipntr, double *workd, double *workl,
	      int *lworkl, int *info );

void sym_mat_arpack(sym_mat *A, int mode) {
    double *v, *workl, *workd, *d, *resid, *ax;
    int *select, iparam[11], ipntr[11];
    char *bmat = "I", *which = "LM"; // "BE";
    int ido = 0, n = A->n, nev = 2, ncv = 8, lworkl = ncv*(ncv+8);
    int i, j, nconv, rvec = 1, info = 0, ierr=0, count=0;
    double tol = 0.0, sigma;

    v = (double *) malloc(n * ncv * sizeof(double));
    workl = (double *) malloc(lworkl * sizeof(double));
    workd = (double *) malloc(3 * n * sizeof(double));
    d = (double *) malloc(2 * nev * sizeof(double));
    resid = (double *) malloc(n * sizeof(double));
    ax = (double *) malloc(n * sizeof(double));
    select = (int *) malloc(ncv * sizeof(int));

    iparam[0] = 1; // ishfts
    iparam[2] = 300; // maxitr
    iparam[6] = 1; //mode1

    while (1) {
	dsaupd_(&ido, bmat, &n, which, &nev, &tol, resid,
		&ncv, v, &n, iparam, ipntr, workd, workl,
		&lworkl, &info);
	count++;
	if (ido == -1 || ido == 1)
	    if (mode == 1)
		sym_mat_apply(A, workd + ipntr[0] - 1, workd + ipntr[1] - 1);
	    else
		sym_mat_solve(A, workd + ipntr[0] - 1, workd + ipntr[1] - 1);
	else
	    break;
    }
    if (info < 0)
	printf("info = %d", info);
    dseupd_( &rvec, "All", select, d, v, &n, &sigma, 
	     bmat, &n, which, &nev, &tol, resid, &ncv, v, &n, 
	     iparam, ipntr, workd, workl, &lworkl, &ierr );

    nconv = iparam[4];

    printf("count = %d\n", count);
    for (j=0; j<nconv; j++) {
	if (mode == 1)
	    sym_mat_apply(A, v+j*n, ax);
	else
	    sym_mat_solve(A, v+j*n, ax);
	daxpy(n, -d[j], v+j*n, 1, ax, 1);
	for (i=0; i<n; i++) ; // printf("%g ", ax[i]); printf("\n");
	d[j+nev] = dnrm2(n, ax, 1) / fabs(d[j]);
	printf("%g   %g\n", d[j], d[j+nev]);
    }

    free(v);
    free(workl);
    free(workd);
    free(d);
    free(resid);
    free(ax);
    free(select);
}

// to be done: (or not...)
//   put in a check to make sure it was initialized before other
//     operations have been done
//   have get_entry return a pointer either into the list or into b
//     how can you disallow changing values after factor is called?
//   make arpack calls run faster by avoiding re-ordering and re-scaling
//     each time it calls solve or apply (or make it optional)

#endif