#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, ...) fprintf(stderr, msg, ##__VA_ARGS__)
#define DEBUG(msg, ...)

#define OUTPUT_POINTS
//#define OUTPUT_POINTS

struct result {
	int id;
	int count;
	mpq_t tr;
	mpq_t trinv;
	double x;
	double y;
};

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;
}

static int compare_result_with_id(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);
		if(d == 0) {
			d = (*b)->id - (*a)->id;
		}
	}

	return d;
}

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

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

	double slopea = (*a)->x / (*a)->y;
	double slopeb = (*b)->x / (*b)->y;

	return slopea > slopeb ? -1 : slopea < slopeb ? 1 : 0;
}


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);
}

void initialize_triangle_generators(mat_workspace *ws, mat *gen, mpq_t s, mpq_t q)
{
	mat r1,r2,r3;
	mpq_t rho1, rho2, rho3;
	mpq_t b1,c1,a2,c2,a3,b3;
	mpq_t sinv;

	mpq_inits(sinv,rho1,rho2,rho3,b1,c1,a2,c2,a3,b3,NULL);
	mat_init(r1, 3);
	mat_init(r2, 3);
	mat_init(r3, 3);

	mpq_set_ui(sinv, 1, 1);
	mpq_div(sinv, sinv, s);

	quartic(rho1, s, 0, 0, 1, -1, 1);
	quartic(rho2, s, 0, 0, 1, -1, 1);
	quartic(rho3, s, 0, 0, 1,  0, 1);

	mpq_mul(c1, rho2, q);
	mpq_mul(a2, rho3, q);
	mpq_mul(b3, rho1, q);
	mpq_set_ui(b1, 1, 1);
	mpq_set_ui(c2, 1, 1);
	mpq_set_ui(a3, 1, 1);
	mpq_div(b1, b1, q);
	mpq_div(c2, c2, q);
	mpq_div(a3, a3, q);

	// actually, we want minus everything
	mat_zero(r1);
	mat_zero(r2);
	mat_zero(r3);
	mpq_set_si(*mat_ref(r1, 0, 0), -1, 1);
	mpq_set_si(*mat_ref(r1, 1, 1), 1, 1);
	mpq_set_si(*mat_ref(r1, 2, 2), 1, 1);
	mpq_set_si(*mat_ref(r2, 0, 0), 1, 1);
	mpq_set_si(*mat_ref(r2, 1, 1), -1, 1);
	mpq_set_si(*mat_ref(r2, 2, 2), 1, 1);
	mpq_set_si(*mat_ref(r3, 0, 0), 1, 1);
	mpq_set_si(*mat_ref(r3, 1, 1), 1, 1);
	mpq_set_si(*mat_ref(r3, 2, 2), -1, 1);

	mpq_set(*mat_ref(r1, 1, 0), b1);
	mpq_set(*mat_ref(r1, 2, 0), c1);
	mpq_set(*mat_ref(r2, 0, 1), a2);
	mpq_set(*mat_ref(r2, 2, 1), c2);
	mpq_set(*mat_ref(r3, 0, 2), a3);
	mpq_set(*mat_ref(r3, 1, 2), b3);

	mat_zero(gen[0]);
	mat_zero(gen[1]);
	mat_zero(gen[2]);

	mpq_set_ui(*mat_ref(gen[0], 0, 0), 1, 1);
	mat_set(gen[0], 1, 1, sinv);
	mat_set(gen[0], 2, 2, s);

	mat_set(gen[1], 0, 0, s);
	mpq_set_ui(*mat_ref(gen[1], 1, 1), 1, 1);
	mat_set(gen[1], 2, 2, sinv);

	mat_set(gen[2], 0, 0, sinv);
	mat_set(gen[2], 1, 1, s);
	mpq_set_ui(*mat_ref(gen[2], 2, 2), 1, 1);

	mat_multiply(ws, gen[0], r2, gen[0]);
	mat_multiply(ws, gen[0], gen[0], r3);
	mat_multiply(ws, gen[1], r3, gen[1]);
	mat_multiply(ws, gen[1], gen[1], r1);
	mat_multiply(ws, gen[2], r1, gen[2]);
	mat_multiply(ws, gen[2], gen[2], r2);

	mat_pseudoinverse(ws, gen[3], gen[0]);
	mat_pseudoinverse(ws, gen[4], gen[1]);
	mat_pseudoinverse(ws, gen[5], gen[2]);

	/*
	mat_print(r1);
	mat_print(r2);
	mat_print(r3);
	mat_print(gen[0]);
	mat_print(gen[1]);
	mat_print(gen[2]);
	mat_print(gen[3]);
	mat_print(gen[4]);
	mat_print(gen[5]);
	*/

	mpq_clears(sinv,rho1,rho2,rho3,b1,c1,a2,c2,a3,b3,NULL);
	mat_clear(r1);
	mat_clear(r2);
	mat_clear(r3);
}

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 s, 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, s, 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 s, q, t, tmp;
	double sapprox, tapprox, qapprox, tqfactor;
	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;

	if(argc < 4) {
		fprintf(stderr, "Usage: %s <N> <s> <t>\n", argv[0]);
		exit(1);
	}

	nmax = atoi(argv[1]);

	DEBUG("Allocate\n");

	mpq_inits(s, q, 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, 3, 3, nmax);

	DEBUG("Compute matrices\n");
	enumerate(group, matrices, s, q);

	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;

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

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

	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];
			distinct_invariants[nuniq]->count = 1;
			nuniq++;
		} else {
			distinct_invariants[nuniq-1]->count++;
			int oldlength = group->elements[distinct_invariants[nuniq-1]->id].length;
			int newlength = group->elements[distinct_invariants[i]->id].length;
			if(newlength < oldlength)
				distinct_invariants[nuniq-1]->id = distinct_invariants[i]->id;
		}

//		gmp_printf("%d %d %s\n", i, nuniq-1, print_word(&group->elements[i], buf));
	}

	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]));

		distinct_invariants[i]->x = x;
		distinct_invariants[i]->y = y;

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

	qsort(distinct_invariants, nuniq, sizeof(struct result*), compare_result_by_slope);

//	printf("- 0 0 - - - - 0.5\n");
	int cumulative = 0;
	double slope;
	for(int i = 0; i < nuniq; i++) {
		slope = distinct_invariants[i]->y/distinct_invariants[i]->x;

		mpq_set_si(tmp, 1, 1);
		if(mpq_cmp(distinct_invariants[i]->tr, tmp) == 0 && mpq_cmp(distinct_invariants[i]->trinv, tmp) == 0) {
			continue;
		}
		mpq_set_si(tmp, 0, 1);
		if(mpq_cmp(distinct_invariants[i]->tr, tmp) == 0 && mpq_cmp(distinct_invariants[i]->trinv, tmp) == 0) {
			continue;
		}
		mpq_set_si(tmp, -1, 1);
		if(mpq_cmp(distinct_invariants[i]->tr, tmp) == 0 && mpq_cmp(distinct_invariants[i]->trinv, tmp) == 0) {
			continue;
		}
		mpq_set_si(tmp, 3, 1);
		if(mpq_cmp(distinct_invariants[i]->tr, tmp) == 0 && mpq_cmp(distinct_invariants[i]->trinv, tmp) == 0) {
			continue;
		}

		cumulative += distinct_invariants[i]->count;
		gmp_printf("%d %d %d %Qd %Qd %f %f %f %f %f %s\n",
		           distinct_invariants[i]->id, distinct_invariants[i]->count, cumulative,
		           distinct_invariants[i]->tr, distinct_invariants[i]->trinv,
		           log(fabs(mpq_get_d(distinct_invariants[i]->tr))), log(fabs(mpq_get_d(distinct_invariants[i]->trinv))),
		           distinct_invariants[i]->x, distinct_invariants[i]->y, slope,
		           print_word(&group->elements[distinct_invariants[i]->id], buf)
			);
	}
//	printf("- 0 %d - - - - 2.0\n", cumulative);

#ifdef OUTPUT_SUMMARY
	fprintf(stdout, "%.3f %.3f %f %s\n", mpq_get_d(s), mpq_get_d(q)*tqfactor, max_slope, print_word(&group->elements[max_slope_index], buf));
#endif

//	output_invariants(group, matrices, s, q, solver);

//	for(int i = 0; i < 10; i++) {
//		mpq_set_ui(t,100+i,100);
//		mpq_canonicalize(t);

		//printf("%f %f\n", mpq_get_d(t), max_slope(group, matrices, s, t, &index));
//	}

	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(s, q, t, tmp, NULL);
	mps_context_free(solver);
}