#include "coxeter.h"
#include "linalg.h"
#include "mat.h"

#include <gsl/gsl_poly.h>
#include <mps/mps.h>

#define SWAP(t,x,y) do { t _tmp = (x); (x) = (y); (y) = _tmp; } while (0);
#define DEBUG(msg, ...) do { print_time(); fprintf(stderr, msg, ##__VA_ARGS__); } while (0);
//#define DEBUG(msg, ...)

//#define OUTPUT_POINTS
#define OUTPUT_SUMMARY

struct timespec starttime;

void print_time()
{
	double diff;
	struct timespec current;

    clock_gettime(CLOCK_REALTIME, &current);

    diff = (current.tv_sec - starttime.tv_sec) + (current.tv_nsec - starttime.tv_nsec)*1e-9;

    fprintf(stderr, "[%.3f] ", diff);
}


struct result {
	mpq_t tr;
	mpq_t trinv;
};

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

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

	d = mpq_cmp((*a)->tr,(*b)->tr);
	if(d == 0)
		d = mpq_cmp((*a)->trinv, (*b)->trinv);

	return d;
}

int solve_characteristic_polynomial(mps_context *solv, mpq_t tr, mpq_t trinv, double *eigenvalues)
{
	mpq_t coeff1, coeff2, zero;
	cplx_t *roots;
	double radii[3];
	double *radii_p[3];
	mps_monomial_poly *poly;
	mps_boolean errors;
	int result = 0;

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

	poly = mps_monomial_poly_new(solv, 3);
	mps_monomial_poly_set_coefficient_int(solv, poly, 0, -1, 0);
	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_int(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[i] = cplx_Re(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;
}

void continued_fraction_approximation(mpq_t out, double in, int level)
{
	mpq_t tmp;

	if(in < 0) {
		mpq_init(tmp);
		mpq_set_ui(tmp, 0, 1);
		continued_fraction_approximation(out, -in, level);
		mpq_sub(out, tmp, out);
		mpq_clear(tmp);
		return;
	}

	if(level == 0) {
		mpq_set_si(out, (signed long int)round(in), 1); // floor(in)
	} else {
		continued_fraction_approximation(out, 1/(in - floor(in)), level - 1);
		mpq_init(tmp);
		mpq_set_ui(tmp, 1, 1);
		mpq_div(out, tmp, out); // out -> 1/out
		mpq_set_si(tmp, (signed long int)in, 1); // floor(in)
		mpq_add(out, out, tmp);
		mpq_clear(tmp);
	}
}

void quartic(mpq_t out, mpq_t in, int a, int b, int c, int d, int e)
{
	mpq_t tmp;
	mpq_init(tmp);

	mpq_set_si(out, a, 1);
	mpq_mul(out, out, in);
	mpq_set_si(tmp, b, 1);
	mpq_add(out, out, tmp);
	mpq_mul(out, out, in);
	mpq_set_si(tmp, c, 1);
	mpq_add(out, out, tmp);
	mpq_mul(out, out, in);
	mpq_set_si(tmp, d, 1);
	mpq_add(out, out, tmp);
	mpq_mul(out, out, in);
	mpq_set_si(tmp, e, 1);
	mpq_add(out, out, tmp);

	mpq_clear(tmp);
}

// this version is only for the (4,4,4) group
void initialize_triangle_generators(mat_workspace *ws, mat *gen, mpq_t m, mpq_t t)
{
	mpq_t s,sinv,q,x,y;
	mpq_t zero, one, two;
	mpq_t tmp;

	mpq_inits(s,sinv,q,x,y,zero,one,two,tmp,NULL);
	mpq_set_ui(zero, 0, 1);
	mpq_set_ui(one, 1, 1);
	mpq_set_ui(two, 2, 1);

	// s = (1-m^2)/2m
	mpq_mul(s, m, m);
	mpq_sub(s, one, s);
	mpq_div(s, s, m);
	mpq_div(s, s, two);
	mpq_div(sinv, one, s);

	// q = (1+m^2)/(1-m^2) = 2/(1-m^2) - 1
	mpq_mul(q, m, m);
	mpq_sub(q, one, q);
	mpq_div(q, two, q);
	mpq_sub(q, q, one);

	// x = -tq, y = -q/t
	mpq_mul(x, q, t);
	mpq_sub(x, zero, x);
	mpq_div(y, q, t);
	mpq_sub(y, zero, y);

	// q^2 = xy = 1 + 1/s^2
	// [         -s         s*y           0]
	// [       -s*x s*x*y - 1/s           0]
	// [       -s*y   s*y^2 - x           1]
	LOOP(i,3) {
		mat_zero(gen[i]);
		mpq_sub(tmp, zero, s);
		mat_set(gen[i%3], i%3, i%3, tmp);
		mpq_mul(tmp, s, y);
		mat_set(gen[i%3], i%3, (i+1)%3, tmp);
		mpq_mul(tmp, s, x);
		mpq_sub(tmp, zero, tmp);
		mat_set(gen[i%3], (i+1)%3, i%3, tmp);
		mpq_mul(tmp, s, x);
		mpq_mul(tmp, tmp, y);
		mpq_sub(tmp, tmp, sinv);
		mat_set(gen[i%3], (i+1)%3, (i+1)%3, tmp);
		mpq_mul(tmp, s, y);
		mpq_sub(tmp, zero, tmp);
		mat_set(gen[i%3], (i+2)%3, i%3, tmp);
		mpq_mul(tmp, s, y);
		mpq_mul(tmp, tmp, y);
		mpq_sub(tmp, tmp, x);
		mat_set(gen[i%3], (i+2)%3, (i+1)%3, tmp);
		mat_set(gen[i%3], (i+2)%3, (i+2)%3, one);
	}

	LOOP(i,3) mat_pseudoinverse(ws, gen[i+3], gen[i]);

	// debug output
	/*
	gmp_printf("m = %Qd, s = %Qd, t = %Qd, q = %Qd, x = %Qd, y = %Qd\n", m, s, t, q, x, y);
	mat_print(gen[0]);
	mat_print(gen[1]);
	mat_print(gen[2]);
	*/

	mpq_inits(s,sinv,q,x,y,zero,one,two,tmp,NULL);
}

char *print_word(groupelement_t *g, char *str)
{
  int i = g->length - 1;

  str[g->length] = 0;
  while(g->parent) {
    str[i--] = 'a' + g->letter;
    g = g->parent;
  }

  return str;
}

void enumerate(group_t *group, mat *matrices, mpq_t m, mpq_t t)
{
	mat_workspace *ws;
	mat tmp;
	mat gen[6];
	char buf[100], buf2[100], buf3[100];

	// allocate stuff
	ws = mat_workspace_init(3);
	for(int i = 0; i < 6; i++)
		mat_init(gen[i], 3);
	mat_init(tmp, 3);

	initialize_triangle_generators(ws, gen, m, t);

	mat_identity(matrices[0]);
	for(int i = 1; i < group->size; i++) {
		if(group->elements[i].length % 2 != 0)
			continue;
		if(!group->elements[i].inverse)
			continue;

		int parent = group->elements[i].parent->id;
		int grandparent = group->elements[i].parent->parent->id;
		int letter;

		if(group->elements[parent].letter == 1 && group->elements[i].letter == 2)
			letter = 0; // p = bc
		else if(group->elements[parent].letter == 2 && group->elements[i].letter == 0)
			letter = 1; // q = ca
		else if(group->elements[parent].letter == 0 && group->elements[i].letter == 1)
			letter = 2; // r = ab
		if(group->elements[parent].letter == 2 && group->elements[i].letter == 1)
			letter = 3; // p^{-1} = cb
		else if(group->elements[parent].letter == 0 && group->elements[i].letter == 2)
			letter = 4; // q^{-1} = ac
		else if(group->elements[parent].letter == 1 && group->elements[i].letter == 0)
			letter = 5; // r^{-1} = ba

		mat_multiply(ws, matrices[i], matrices[grandparent], gen[letter]);
	}

	// free stuff
	for(int i = 0; i < 6; i++)
		mat_clear(gen[i]);
	mat_clear(tmp);
	mat_workspace_clear(ws);
}

void output_invariants(group_t *group, mat *matrices, mpq_t s, mpq_t q, mps_context *solver)
{
	mpq_t tr, trinv;
	char buf[100];
	double evs[3];
	int retval;

	mpq_inits(tr, trinv, NULL);

	for(int i = 0; i < group->size; i++) {
		if(group->elements[i].length % 2 != 0 || !group->elements[i].inverse)
			continue;

		mat_trace(tr, matrices[i]);
		mat_trace(trinv, matrices[group->elements[i].inverse->id]);

		retval = solve_characteristic_polynomial(solver, tr, trinv, evs);
		if(retval == 1) {
			fprintf(stderr, "Error! Could not solve polynomial.\n");
			continue;
		} else if(retval == 2) {
			continue;
		}

		if(fabs(evs[0]) < fabs(evs[1]))
			SWAP(double, evs[0], evs[1]);
		if(fabs(evs[1]) < fabs(evs[2]))
			SWAP(double, evs[1], evs[2]);
		if(fabs(evs[0]) < fabs(evs[1]))
			SWAP(double, evs[0], evs[1]);

		gmp_printf("%d %d %s %Qd %Qd %f %f\n", i, group->elements[i].length, print_word(&group->elements[i], buf), tr, trinv, log(evs[0]), -log(evs[2]));
	}

	mpq_clears(tr, trinv, NULL);
}

/*
double max_slope(groupelement_t *group, mat *matrices, mpq_t s, mpq_t t, int *index)
{
	double max = 0;
	double slope;

	mpq_t tr, trinv;
	char buf[100];

	mpq_inits(tr, trinv, NULL);

	for(int i = 0; i < MAX_ELEMENTS; i++) {
		if(group[i].length % 2 != 0 || !group[i].inverse)
			continue;

		mat_trace(tr, matrices[i]);
		mat_trace(trinv, matrices[group[i].inverse->id]);

		slope = log(mpq_get_d(trinv))/log(mpq_get_d(tr));
		if(slope > max)
		{
			*index = i;
			max = slope;
		}
	}

	mpq_clears(tr, trinv, NULL);

	return max;
}
*/

int main(int argc, char *argv[])
{
	mpq_t m, t, tmp;
	double s;
	mat *matrices;
	group_t *group;
	int index;
	mps_context *solver;
	int acc = 100;
	int n, nuniq, nmax;
	int retval;
	double evs[3];
	double max_slope;
	char buf[100];
	char buf2[100];

	struct result *invariants;
	struct result **distinct_invariants;

    clock_gettime(CLOCK_REALTIME, &starttime);

	nmax = atoi(argv[1]);

	DEBUG("Allocate\n");

	mpq_inits(m, t, tmp, NULL);
	matrices = malloc(nmax*sizeof(mat));
	for(int i = 0; i < nmax; i++)
		mat_init(matrices[i], 3);
	invariants = malloc(nmax*sizeof(struct result));
	distinct_invariants = malloc(nmax*sizeof(struct result));
	for(int i = 0; i < nmax; i++) {
		mpq_init(invariants[i].tr);
		mpq_init(invariants[i].trinv);
		distinct_invariants[i] = &invariants[i];
	}

	solver = mps_context_new();
	mps_context_set_output_prec(solver, 20); // relative precision
	mps_context_set_output_goal(solver, MPS_OUTPUT_GOAL_APPROXIMATE);

	/*
	DEBUG("Approximate parameters\n");

	// get approximate s and q values
	sapprox = atof(argv[2]);
	tapprox = atof(argv[3]);
	tqfactor = pow((sapprox*sapprox-sapprox+1)*(sapprox*sapprox-sapprox+1)*(sapprox*sapprox+1), 1/6.0);
	qapprox = tapprox/tqfactor;

	for(int i = 0; ; i++) {
		continued_fraction_approximation(tmp, sapprox, i);
		if(fabs(mpq_get_d(t)-sapprox) < 1e-10
		   || (mpz_cmpabs_ui(mpq_numref(tmp),acc) > 0 && mpz_cmpabs_ui(mpq_denref(tmp),acc) > 0))
			break;
		mpq_set(s, tmp);
	}
	mpq_canonicalize(s);

	for(int i = 0; ; i++) {
		continued_fraction_approximation(tmp, qapprox, i);
		if(fabs(mpq_get_d(t)-qapprox) < 1e-10
		   || (mpz_cmpabs_ui(mpq_numref(tmp),acc) > 0 && mpz_cmpabs_ui(mpq_denref(tmp),acc) > 0))
			break;
		mpq_set(q, tmp);
	}
	mpq_canonicalize(q);

	tqfactor = pow((mpq_get_d(s)*mpq_get_d(s)-mpq_get_d(s)+1)*(mpq_get_d(s)*mpq_get_d(s)-mpq_get_d(s)+1)*(mpq_get_d(s)*mpq_get_d(s)+1), 1/6.0);

#ifdef OUTPUT_POINTS
	gmp_fprintf(stdout, "\"s = %Qd = %.3f, q = %Qd, t = %.3f\"\n", s, mpq_get_d(s), q, mpq_get_d(q)*tqfactor);
#endif
	*/

	// group
	DEBUG("Generate group\n");
	group = coxeter_init_triangle(4, 4, 4, nmax);

	fprintf(stderr, "max word length = %d\n", group->elements[nmax-1].length);

	for(int ttick = 45; ttick <= 65; ttick++) {
		for(int mtick = 45; mtick < 65; mtick++) {
			mpq_set_ui(t, ttick, 100);
			mpq_set_ui(m, mtick, 100); // 414/1000 ~ sqrt(2)-1 <-> s=1
			s = (1-mpq_get_d(m)*mpq_get_d(m))/(2*mpq_get_d(m));

			DEBUG("Compute matrices\n");
			enumerate(group, matrices, m, t);

			n = 0;
			DEBUG("Compute traces\n");
			for(int i = 0; i < nmax; i++) {
				if(group->elements[i].length % 2 != 0 || !group->elements[i].inverse)
					continue;

				mat_trace(invariants[i].tr, matrices[i]);
				mat_trace(invariants[i].trinv, matrices[group->elements[i].inverse->id]);

				distinct_invariants[n++] = &invariants[i];

//		gmp_printf("%Qd %Qd %d %s\n", invariants[i].tr, invariants[i].trinv, i, print_word(&group->elements[i], buf));
			}

			DEBUG("Get unique traces\n");

			qsort(distinct_invariants, n, sizeof(struct result*), compare_result);

			nuniq = 0;
			for(int i = 0; i < n; i++) {
				if(i == 0 || compare_result(&distinct_invariants[i], &distinct_invariants[nuniq-1]) != 0)
					distinct_invariants[nuniq++] = distinct_invariants[i];
			}

			max_slope = 0;
			int max_slope_index;

			DEBUG("Solve characteristic polynomials\n");
			for(int i = 0; i < nuniq; i++) {
				retval = solve_characteristic_polynomial(solver, distinct_invariants[i]->tr, distinct_invariants[i]->trinv, evs);
				if(retval == 1) {
					fprintf(stderr, "Error! Could not solve polynomial.\n");
					continue;
				} else if(retval == 2) {
					continue;
				}

				if(fabs(evs[0]) < fabs(evs[1]))
			SWAP(double, evs[0], evs[1]);
				if(fabs(evs[1]) < fabs(evs[2]))
					SWAP(double, evs[1], evs[2]);
				if(fabs(evs[0]) < fabs(evs[1]))
					SWAP(double, evs[0], evs[1]);

				double x = log(fabs(evs[0]));
				double y = -log(fabs(evs[2]));

				if(y/x > max_slope && (x > 0.1 || y > 0.1)) {
					max_slope_index = distinct_invariants[i] - invariants;
					max_slope = y/x;
				}

#ifdef OUTPUT_POINTS
				gmp_printf("%Qd %Qd %f %f %f\n", distinct_invariants[i]->tr, distinct_invariants[i]->trinv, x, y, y/x);
#endif
			}

#ifdef OUTPUT_SUMMARY
//	fprintf(stdout, "%.5f %.5f %.5f %f %s\n", mpq_get_d(t), mpq_get_d(m), s, max_slope, print_word(&group->elements[max_slope_index], buf));
			fprintf(stdout, "%.5f %.5f %.5f %f %s\n", mpq_get_d(t), mpq_get_d(m), s, max_slope, print_word(&group->elements[max_slope_index], buf));
#endif
		}
	}

	DEBUG("Clean up\n");
	for(int i = 0; i < nmax; i++) {
		mpq_clear(invariants[i].tr);
		mpq_clear(invariants[i].trinv);
	}
	free(invariants);
	free(distinct_invariants);
	for(int i = 0; i < nmax; i++)
		mat_clear(matrices[i]);
	free(matrices);
	coxeter_clear(group);
	mpq_clears(m, t, tmp, NULL);
	mps_context_free(solver);
}