#include "enumerate_triangle_group.h"
#include "linalg.h"

int solve_characteristic_polynomial(mps_context *solv, mps_monomial_poly *poly, mpq_t tr, mpq_t trinv, double *eigenvalues)
{
	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_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);
}

// p1,p2,p3 are only allowed to be 2,3,4,6
void initialize_triangle_generators(mat_workspace *ws, mat *gen, int p1, int p2, int p3, 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);

	// sinv = s^{-1}
	mpq_set_ui(sinv, 1, 1);
	mpq_div(sinv, sinv, s);

	// rho_i = s^2 + 2s cos(2 pi / p_i) + 1
	// coefficient 2 is the value for p=infinity, not sure if that would even work
	quartic(rho1, s, 0, 0, 1, p1 == 2 ? -2 : p1 == 3 ? -1 : p1 == 4 ? 0 : p1 == 6 ? 1 : 2, 1);
	quartic(rho2, s, 0, 0, 1, p2 == 2 ? -2 : p2 == 3 ? -1 : p2 == 4 ? 0 : p2 == 6 ? 1 : 2, 1);
	quartic(rho3, s, 0, 0, 1, p3 == 2 ? -2 : p3 == 3 ? -1 : p3 == 4 ? 0 : p3 == 6 ? 1 : 2, 1);

	// c1 = rho2 q, a2 = rho3 q, b3 = rho1 q, b1 = c2 = a3 = 1/q
	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);

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

	// gen[0] = diag(1,s^{-1},s)
	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);

	// gen[1] = diag(s,1,s^{-1})
	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);

	// gen[3] = diag(s^{-1},s,1)
	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);

	// gen[0] = r2 * gen[0] * r3
	// gen[1] = r3 * gen[1] * r1
	// gen[2] = r1 * gen[2] * r2
	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);

	// gen[3] = gen[0]^{-1}
	// gen[4] = gen[1]^{-1}
	// gen[5] = gen[2]^{-1}
	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, int p1, int p2, int p3, mpq_t s, mpq_t q)
{
	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, p1, p2, p3, s, q);

	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;
		if(!group->elements[i].need_to_compute)
			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);
}