#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(NUMBER out, NUMBER in, int a, int b, int c, int d, int e)
{
	NUMBER tmp;
	INIT(tmp, GETTYPE(in));

	SET_INT(out, a);

	MUL(out, out, in);
	SET_INT(tmp, b);
	ADD(out, out, tmp);

	MUL(out, out, in);
	SET_INT(tmp, c);
	ADD(out, out, tmp);

	MUL(out, out, in);
	SET_INT(tmp, d);
	ADD(out, out, tmp);

	MUL(out, out, in);
	SET_INT(tmp, e);
	ADD(out, out, tmp);

	CLEAR(tmp);
}

// discriminant of the polynomial z^3 - x z^2 + y z - 1
// that is the function x^2 y^2 - 4 x^3 - 4 y^3 - 27 + 18 xy
void discriminant(NUMBER out, NUMBER x, NUMBER y)
{
	TYPE type = GETTYPE(out);
	NUMBER x2, x3, y2, y3, tmp;
	INIT(x2, type);INIT(x3, type);INIT(y2, type);INIT(y3, type);INIT(tmp, type);

	MUL(x2, x, x);
	MUL(x3, x2, x);
	MUL(y2, y, y);
	MUL(y3, y2, y);

	MUL(out, x2, y2);

	SET_INT(tmp, -4);
	MUL(tmp, tmp, x3);
	ADD(out, out, tmp);

	SET_INT(tmp, -4);
	MUL(tmp, tmp, y3);
	ADD(out, out, tmp);

	SET_INT(tmp, -27);
	ADD(out, out, tmp);

	SET_INT(tmp, 18);
	MUL(tmp, tmp, x);
	MUL(tmp, tmp, y);
	ADD(out, out, tmp);

	CLEAR(x2);CLEAR(x3);CLEAR(y2);CLEAR(y3);CLEAR(tmp);
}

void generators_triangle_rotation_generic(mat *gen, NUMBER rho1, NUMBER rho2, NUMBER rho3, mpq_t s, mpq_t q)
{
	mat_workspace *ws;
	mat r1,r2,r3;
	NUMBER b1,c1,a2,c2,a3,b3;
	TYPE type = GETTYPE(rho1);
	mpq_t sinv, qinv;

	ws = mat_workspace_init(3, type);
	INIT(b1, type);INIT(c1, type);INIT(a2, type);INIT(c2, type);INIT(a3, type);INIT(b3, type);
	mpq_init(sinv);
	mpq_init(qinv);
	mat_init(r1, 3, type);
	mat_init(r2, 3, type);
	mat_init(r3, 3, type);

	// sinv = s^{-1}
	mpq_inv(sinv, s);
	mpq_inv(qinv, q);

	// c1 = rho2 q, a2 = rho3 q, b3 = rho1 q, b1 = c2 = a3 = 1/q
	SET_Q(c1, q);
	SET_Q(a2, q);
	SET_Q(b3, q);
	MUL(c1, c1, rho2);
	MUL(a2, a2, rho3);
	MUL(b3, b3, rho1);
	SET_INT(b1, 1);
	SET_INT(c2, 1);
	SET_INT(a3, 1);
	SET_Q(b1, qinv);
	SET_Q(c2, qinv);
	SET_Q(a3, qinv);

	mat_zero(r1);
	mat_zero(r2);
	mat_zero(r3);
	SET_INT(*mat_ref(r1, 0, 0), -1);
	SET_INT(*mat_ref(r1, 1, 1), 1);
	SET_INT(*mat_ref(r1, 2, 2), 1);
	SET_INT(*mat_ref(r2, 0, 0), 1);
	SET_INT(*mat_ref(r2, 1, 1), -1);
	SET_INT(*mat_ref(r2, 2, 2), 1);
	SET_INT(*mat_ref(r3, 0, 0), 1);
	SET_INT(*mat_ref(r3, 1, 1), 1);
	SET_INT(*mat_ref(r3, 2, 2), -1);

	SET(*mat_ref(r1, 1, 0), b1);
	SET(*mat_ref(r1, 2, 0), c1);
	SET(*mat_ref(r2, 0, 1), a2);
	SET(*mat_ref(r2, 2, 1), c2);
	SET(*mat_ref(r3, 0, 2), a3);
	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)
	SET_INT(*mat_ref(gen[0], 0, 0), 1);
	SET_Q  (*mat_ref(gen[0], 1, 1), sinv);
	SET_Q  (*mat_ref(gen[0], 2, 2), s);

	// gen[1] = diag(s,1,s^{-1})
	SET_Q  (*mat_ref(gen[1], 0, 0), s);
	SET_INT(*mat_ref(gen[1], 1, 1), 1);
	SET_Q  (*mat_ref(gen[1], 2, 2), sinv);

	// gen[2] = diag(s^{-1},s,1)
	SET_Q  (*mat_ref(gen[2], 0, 0), sinv);
	SET_Q  (*mat_ref(gen[2], 1, 1), s);
	SET_INT(*mat_ref(gen[2], 2, 2), 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_workspace_clear(ws);
	CLEAR(b1);CLEAR(c1);CLEAR(a2);CLEAR(c2);CLEAR(a3);CLEAR(b3);
	mpq_clear(sinv);
	mpq_clear(qinv);
	mat_clear(r1);
	mat_clear(r2);
	mat_clear(r3);
}

#ifdef QEXT_TRIVIAL
// p1,p2,p3 are only allowed to be 2,3,4,6
void generators_triangle_rotation_2346_rational(mat *gen, int p1, int p2, int p3, mpq_t s, mpq_t q)
{
	mpq_t rho1, rho2, rho3;
	mpq_inits(rho1,rho2,rho3,NULL);

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

	generators_triangle_rotation_generic(gen, rho1, rho2, rho3, s, q);

	mpq_clears(rho1,rho2,rho3,NULL);
}
#endif

#ifdef QEXT
void generators_triangle_rotation_555_barbot(mat *gen, mpq_t s_, mpq_t q_)
{
	NUMBER rho, c, one, s;
	INIT(rho, QT_SQRT5);INIT(c, QT_SQRT5);INIT(one, QT_SQRT5);INIT(s, QT_SQRT5);

	// c = - (1+sqrt(5))/2
	mpq_set_si(c->a[0], -1, 2);
	mpq_set_si(c->a[1], -1, 2);
	SET_ONE(one);

	SET_Q(s, s_);

	// rho = s^2 + cs + 1
	SET(rho, one);
	MUL(rho, rho, s);
	ADD(rho, rho, c);
	MUL(rho, rho, s);
	ADD(rho, rho, one);

	generators_triangle_rotation_generic(gen, rho, rho, rho, s_, q_);

	CLEAR(rho);CLEAR(c);CLEAR(one);CLEAR(s);
}
#endif

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_triangle_rotation_subgroup(group_t *group, mat *gen, mat *matrices)
{
	mat_workspace *ws;
	mat tmp;
	char buf[100], buf2[100], buf3[100];

	// allocate stuff
	TYPE type = GETTYPE(gen[0]->x[0]);
	ws = mat_workspace_init(3, type);
	mat_init(tmp, 3, type);

//	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
	mat_clear(tmp);
	mat_workspace_clear(ws);
}