#include "enumerate.h"

#include <stdlib.h>

#define LOOP(i,n) for(int i = 0; i < (n); i++)
// #define INFO(msg, ...) fprintf(stderr, "[%10.3f] " msg, runtime(), ##__VA_ARGS__)
#define INFO(msg, ...)

static int compare_int(const void *a, const void *b)
{
    const int *aa = a;
    const int *bb = b;
    return (*aa > *bb) - (*aa < *bb);
}

static int index_in_list(const int *list, int n, int key)
{
	int *ptr = bsearch(&key, list, n, sizeof(int), compare_int);
	if(ptr)
		return ptr - list;
	else
		return -1;
}

// idlist is assumed to be sorted and symmetric
void enumerate_coxeter_group(group_t *group, mat *gen, mat *matrices, const int *idlist, int nlist)
{
	TYPE type = GETTYPE(gen[0]->x[0]);
	mat_workspace *ws;

	// mark elements which we need to compute
	int ncompute = 0;
	LOOP(i, group->size) group->elements[i].need_to_compute = 0;
	LOOP(i, nlist) {
		groupelement_t *cur = &group->elements[idlist[i]];
		while(cur && cur->need_to_compute == 0) {
			cur->need_to_compute = 1;
			ncompute++;
			cur = cur->parent;
		}
	}
	INFO("need to compute %d elements for %d elements in list\n", ncompute, nlist);

	// compute them depth first
	int maxlen = group->elements[group->size-1].length;

	groupelement_t **stack = malloc((maxlen+1)*sizeof(groupelement_t));
	int *letter_stack = malloc((maxlen+1)*sizeof(int));
	mat *matrix_stack = malloc((maxlen+1)*sizeof(mat));
	int level = 0;

	LOOP(i, maxlen+1) mat_init(matrix_stack[i], 3, type);
	ws = mat_workspace_init(3, type);

	stack[0] = &group->elements[0];
	mat_identity(matrix_stack[0]);
	letter_stack[0] = 0;

	while(level >= 0) {
		int letter = letter_stack[level];
		groupelement_t *next = stack[level]->left[letter];
		if(next && next->length > level && next->need_to_compute) {
			mat_multiply(ws, matrix_stack[level+1], gen[letter], matrix_stack[level]);
			int id = next->id;
			int listid = index_in_list(idlist, nlist, id);
			if(listid != -1)
				mat_copy(matrices[listid], matrix_stack[level+1]);
			next->need_to_compute = 0;
			stack[level+1] = next;
			letter_stack[level+1] = 0;
			level++;
		} else {
			letter = ++letter_stack[level];
			while(letter >= group->rank) { // done with this level, go back down
				level--;
				if(level < 0)
					break;
				letter = ++letter_stack[level];
			}
		}
	}

	LOOP(i, maxlen+1) mat_clear(matrix_stack[i]);
	mat_workspace_clear(ws);
}

static int compare_tracedata(const void *a_, const void *b_)
{
        int d = 0;

        struct tracedata **a = (struct tracedata **)a_;
        struct tracedata **b = (struct tracedata **)b_;

        d = CMP((*a)->tr,(*b)->tr);
        if(d == 0) {
                d = CMP((*a)->trinv, (*b)->trinv);
        }

        return d;
}

static int compare_tracedata_id(const void *a, const void *b)
{
        int ida = (*(struct tracedata **)a)->id;
        int idb = (*(struct tracedata **)b)->id;

        return ida > idb ? 1 : ida < idb ? -1 : 0;
}

int enumerate_coxeter_group_traces(group_t *group, mat *gen, struct tracedata **traces_out, const int *idlist, int nlist, int unique)
{
	TYPE t = GETTYPE(gen[0]->x[0]);
	int nuniq;

	mat *matrices = malloc(nlist*sizeof(mat));
	struct tracedata *traces = malloc(nlist*sizeof(struct tracedata));
	struct tracedata **distinct_traces = malloc(nlist*sizeof(struct tracedata*));

	LOOP(i, nlist) mat_init(matrices[i], 3, t);
	enumerate_coxeter_group(group, gen, matrices, idlist, nlist);

	LOOP(i, nlist) {
		INIT(traces[i].tr, t);
		INIT(traces[i].trinv, t);
		int inv_id_in_list = index_in_list(idlist, nlist, group->elements[idlist[i]].inverse->id);
		mat_trace(traces[i].tr, matrices[i]);
		mat_trace(traces[i].trinv, matrices[inv_id_in_list]);
		traces[i].id = idlist[i];
		distinct_traces[i] = &traces[i];
	}

	if(unique) {
		qsort(distinct_traces, nlist, sizeof(struct tracedata*), compare_tracedata);

		nuniq = 0;
		LOOP(i, nlist) {
			if(i == 0 || compare_tracedata(&distinct_traces[i], &distinct_traces[nuniq-1]) != 0) {
				distinct_traces[nuniq] = distinct_traces[i];
				nuniq++;
			} else {
				int oldlength = group->elements[distinct_traces[nuniq-1]->id].length;
				int newlength = group->elements[distinct_traces[i]->id].length;
				if(newlength < oldlength)
					distinct_traces[nuniq-1]->id = distinct_traces[i]->id;
			}
		}
	} else {
		nuniq = nlist;
	}

	qsort(distinct_traces, nuniq, sizeof(struct tracedata*), compare_tracedata_id);

	struct tracedata *unique_traces = malloc(nuniq*sizeof(struct tracedata));
	LOOP(i, nuniq) {
		INIT(unique_traces[i].tr, t);
		INIT(unique_traces[i].trinv, t);
		SET(unique_traces[i].tr, distinct_traces[i]->tr);
		SET(unique_traces[i].trinv, distinct_traces[i]->trinv);
		unique_traces[i].id = distinct_traces[i]->id;
	}

	LOOP(i, nlist) mat_clear(matrices[i]);
	LOOP(i, nlist) {
		CLEAR(traces[i].tr);
		CLEAR(traces[i].trinv);
	}
	free(matrices);
	free(traces);
	free(distinct_traces);

	*traces_out = unique_traces;
	return nuniq;
}

void enumerate_tracedata_clear(struct tracedata *traces, int n)
{
	LOOP(i, n) {
		CLEAR(traces[i].tr);
		CLEAR(traces[i].trinv);
	}
	free(traces);
}

// returns squares of absolute values
int solve_characteristic_polynomial_d(mps_context *solv, mps_monomial_poly *poly, double tr_real, double tr_imag, double trinv_real, double trinv_imag, double *eigenvalues_real, double *eigenvalues_imag)
{
//        mpq_t coeff1, coeff2, zero;
        cplx_t *roots;
        double radii[3];
        double *radii_p[3];
        mps_boolean errors;
        int result = 0;

        /*
        mpq_inits(coeff1, coeff2, zero, NULL);
        mpq_set(coeff1, trinv);
        mpq_sub(coeff2, zero, tr);
        */

        mps_monomial_poly_set_coefficient_d(solv, poly, 0, -1, 0);
        mps_monomial_poly_set_coefficient_d(solv, poly, 1, trinv_real, trinv_imag);
        mps_monomial_poly_set_coefficient_d(solv, poly, 2, -tr_real, -tr_imag);
//        mps_monomial_poly_set_coefficient_q(solv, poly, 1, coeff1, zero);
//        mps_monomial_poly_set_coefficient_q(solv, poly, 2, coeff2, zero);
        mps_monomial_poly_set_coefficient_d(solv, poly, 3, 1, 0);

        mps_context_set_input_poly(solv, (mps_polynomial*)poly);
        mps_mpsolve(solv);

        roots = cplx_valloc(3);
        for(int i = 0; i < 3; i++)
                radii_p[i] = &(radii[i]);
        mps_context_get_roots_d(solv, &roots, radii_p);
        errors = mps_context_has_errors(solv);

        if(errors) {
                result = 1;
        } else {
                for(int i = 0; i < 3; i++) {
                        eigenvalues_real[i] = cplx_Re(roots[i]);
                        eigenvalues_imag[i] = cplx_Im(roots[i]);
//                        if(fabs(cplx_Im(roots[i])) > radii[i]) // non-real root
//                                result = 2;
                }
        }

        cplx_vfree(roots);
//        mpq_clears(coeff1, coeff2, zero, NULL);

        return result;
}