#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, ...)

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

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

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

int main(int argc, char *argv[])
{
	mpq_t m, t, tmp;
	double s;
	mat_workspace *ws;
	mat gen[6];
	mps_context *solver;
	mat element, inverse;
	int letter1, letter2, letter;
	mpq_t tr, trinv;
	double x, y;
	int retval;
	double evs[3];
	char buf[100];

	DEBUG("Allocate\n");

	mpq_inits(m, t, tmp, tr, trinv, NULL);
	ws = mat_workspace_init(3);
	for(int i = 0; i < 6; i++)
		mat_init(gen[i], 3);
	mat_init(element, 3);
	mat_init(inverse, 3);

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

	for(int i = 1; i <= 99; i++) {
		for(int j = 1; j <= 100; j++) {
			mpq_set_ui(t, j, 100);
			mpq_set_ui(m, i, 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 matrix\n");
			initialize_triangle_generators(ws, gen, m, t);

			mat_identity(element);
			mat_identity(inverse);
			for(int i = 0; i < strlen(argv[1]); i+=2) {
				letter1 = argv[1][i] - 'a';
				letter2 = argv[1][i+1] - 'a';

				if(letter1 == 1 && letter2 == 2)
					letter = 0; // p = bc
				else if(letter1 == 2 && letter2 == 0)
					letter = 1; // q = ca
				else if(letter1 == 0 && letter2 == 1)
					letter = 2; // r = ab
				else if(letter1 == 2 && letter2 == 1)
					letter = 3; // p^{-1} = cb
				else if(letter1 == 0 && letter2 == 2)
					letter = 4; // q^{-1} = ac
				else if(letter1 == 1 && letter2 == 0)
					letter = 5; // r^{-1} = ba

				mat_multiply(ws, element, element, gen[letter]);
				mat_multiply(ws, inverse, gen[(letter+3)%6], inverse);
			}

			DEBUG("Compute traces\n");

			mat_trace(tr, element);
			mat_trace(trinv, inverse);

			DEBUG("Solve characteristic polynomials\n");
			retval = solve_characteristic_polynomial(solver, tr, trinv, evs);
			if(retval == 1) {
				fprintf(stderr, "Error! Could not solve polynomial.\n");
				return 1;
			}

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

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

//	gmp_printf("%Qd %Qd %f %f %f\n", tr, trinv, x, y, y/x);
			gmp_printf("%.5f %.5f %.5f %.5f\n", mpq_get_d(t), mpq_get_d(m), s, y/x);

		}
	}

	DEBUG("Clean up\n");
	mpq_clears(m, t, tmp, tr, trinv, NULL);
	mat_workspace_clear(ws);
	for(int i = 0; i < 6; i++)
		mat_clear(gen[i]);
	mat_clear(element);
	mat_clear(inverse);
	mps_context_free(solver);
}