#include "coxeter.h"
#include "queue.h"

#include <memory.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>

static void applyGenerator(int i, double *x, double *y, int rank, double **schlaefliMatrix)
{
  memcpy(y, x, rank*sizeof(double));
  for(int j = 0; j < rank; j++)
    y[i] -= schlaefliMatrix[i][j]*x[j];
}

static int check_equal_sector(gsl_matrix *x1, gsl_matrix *x2, gsl_matrix *yinv, gsl_matrix *schlaefli, gsl_vector *starting, gsl_vector *i1, gsl_vector *i2)
{
  double scalarProduct;
  int rank = x1->size1;

  // calculate <y^{-1} * x1 * x2 * s, e_i> for all i, or identically the components of schlaefli*y^{-1}*x*s
  gsl_blas_dgemv(CblasNoTrans, 1.0, x2, starting, 0.0, i1);
  gsl_blas_dgemv(CblasNoTrans, 1.0, x1, i1, 0.0, i2);
  gsl_blas_dgemv(CblasNoTrans, 1.0, yinv, i2, 0.0, i1);
  gsl_blas_dgemv(CblasNoTrans, 1.0, schlaefli, i1, 0.0, i2);
  for(int i = 0; i < rank; i++)
    if(gsl_vector_get(i2, i) < 0)
      return 0;
  return 1;
}

static void invert(gsl_matrix *x, gsl_matrix *xinv)
{
  int rank = x->size1;
  gsl_matrix *xLU = gsl_matrix_alloc(rank, rank);
  gsl_permutation *p = gsl_permutation_alloc(rank);
  int s;
  gsl_matrix_memcpy(xLU, x);
  gsl_linalg_LU_decomp(xLU, p, &s);
  gsl_linalg_LU_invert(xLU, p, xinv);
  gsl_permutation_free(p);
  gsl_matrix_free(xLU);
}

static void generate_schlaefli_matrix(semisimple_type_t type, gsl_matrix *mat)
{
  gsl_matrix_set_zero(mat);
  int offset = 0;

  for(int k = 0; k < type.n; k++) {
    if(type.factors[k].series == 'A') {
      for(int i = 0; i < type.factors[k].rank; i++)
	for(int j = 0; j < type.factors[k].rank; j++)
	  if(i == j)
	    gsl_matrix_set(mat, offset + i, offset + j, 2.0);
	  else if(i - j == 1 || i - j == -1)
	    gsl_matrix_set(mat, offset + i, offset + j, -1.0);
    } else if(type.factors[k].series == 'B' || type.factors[k].series == 'C') {
      for(int i = 0; i < type.factors[k].rank; i++)
	for(int j = 0; j < type.factors[k].rank; j++)
	  if(i == j)
	    gsl_matrix_set(mat, offset + i, offset + j, 2.0);
	  else if(i == 0 && j == 1 || i == 1 && j == 0)
	    gsl_matrix_set(mat, offset + i, offset + j, -sqrt(2.0));
	  else if(i - j == 1 || i - j == -1)
	    gsl_matrix_set(mat, offset + i, offset + j, -1.0);
    } else if(type.factors[k].series == 'D') {
      for(int i = 0; i < type.factors[k].rank; i++)
	for(int j = 0; j < type.factors[k].rank; j++)
	  if(i == j)
	    gsl_matrix_set(mat, offset + i, offset + j, 2.0);
	  else if(i > 0 && j > 0 && (i - j == 1 || i - j == -1) || i == 0 && j == 2 || i == 2 && j == 0)
	    gsl_matrix_set(mat, offset + i, offset + j, -1.0);
    } else if(type.factors[k].series == 'F') {
      ERROR(type.factors[k].rank != 4, "A Coxeter group of type %c%d does not exist or is not implemented!\n", type.factors[k].series, type.factors[k].rank);
      for(int i = 0; i < 4; i++)
	gsl_matrix_set(mat, offset + i, offset + i, 2.0);
      for(int i = 0; i < 3; i++) {
	gsl_matrix_set(mat, offset + i, offset + i + 1, i == 1 ? -sqrt(2.0) : -1.0);
	gsl_matrix_set(mat, offset + i + 1, offset + i, i == 1 ? -sqrt(2.0) : -1.0);
      }
    } else if(type.factors[k].series == 'G') {
      ERROR(type.factors[k].rank != 2, "A Coxeter group of type %c%d does not exist or is not implemented!\n", type.factors[k].series, type.factors[k].rank);
      gsl_matrix_set(mat, offset + 0, offset + 0, 2.0);
      gsl_matrix_set(mat, offset + 0, offset + 1, -sqrt(3.0));
      gsl_matrix_set(mat, offset + 1, offset + 0, -sqrt(3.0));
      gsl_matrix_set(mat, offset + 1, offset + 1, 2.0);
    } else {
      ERROR(1, "A Coxeter group of type %c%d does not exist or is not implemented!\n", type.factors[k].series, type.factors[k].rank);
    }

    offset += type.factors[k].rank;
  }
}

int coxeter_rank(semisimple_type_t type)
{
  int rank = 0;
  for(int i = 0; i < type.n; i++)
    rank += type.factors[i].rank;
  return rank;
}

int coxeter_order(semisimple_type_t type)
{
  int order = 1;
  for(int i = 0; i < type.n; i++) {
    if(type.factors[i].series == 'A') { // (rank+1)!
      for(int j = 1; j <= type.factors[i].rank + 1; j++)
	order *= j;
    } else if(type.factors[i].series == 'B' || type.factors[i].series == 'C') { // 2^rank * rank!
      for(int j = 1; j <= type.factors[i].rank; j++)
	order *= 2*j;
    } else if(type.factors[i].series == 'D') { // 2^(rank-1) * rank!
      for(int j = 2; j <= type.factors[i].rank; j++)
	order *= 2*j;
    } else if(type.factors[i].series == 'E') {
      if(type.factors[i].rank == 6)
	order *= 51840;
      else if(type.factors[i].rank == 7)
	order *= 2903040;
      else if(type.factors[i].rank == 8)
	order *= 696729600;
      else
	ERROR(1, "A Coxeter group of type %c%d does not exist or is not implemented!\n", type.factors[i].series, type.factors[i].rank);
    } else if(type.factors[i].series == 'F') {
      ERROR(type.factors[i].rank != 4, "A Coxeter group of type %c%d does not exist or is not implemented!\n", type.factors[i].series, type.factors[i].rank);
      order *= 1152;
    } else if(type.factors[i].series == 'G') {
      ERROR(type.factors[i].rank != 2, "A Coxeter group of type %c%d does not exist or is not implemented!\n", type.factors[i].series, type.factors[i].rank);
      order *= 12;
    } else if(type.factors[i].series == 'H') {
      if(type.factors[i].rank == 2)
	order *= 10;
      else if(type.factors[i].rank == 3)
	order *= 120;
      else if(type.factors[i].rank == 4)
	order *= 14400;
      else
	ERROR(1, "A Coxeter group of type %c%d does not exist or is not implemented!\n", type.factors[i].series, type.factors[i].rank);
    } else {
      ERROR(1, "A Coxeter group of type %c%d does not exist or is not implemented!\n", type.factors[i].series, type.factors[i].rank);
    }
  }
  return order;
}

static void generate_starting_vector(int rank, gsl_matrix *schlaefli, gsl_vector *result)
{
  gsl_matrix *schlaefliLU = gsl_matrix_alloc(rank, rank);
  gsl_vector *diagonal = gsl_vector_alloc(rank);
  gsl_permutation *p = gsl_permutation_alloc(rank);
  int s;
  for(int i = 0; i < rank; i++)
    gsl_vector_set(diagonal, i, 1.0);
  gsl_matrix_memcpy(schlaefliLU, schlaefli);
  gsl_linalg_LU_decomp(schlaefliLU, p, &s);
  gsl_linalg_LU_solve(schlaefliLU, p, diagonal, result);
  gsl_permutation_free(p);
  gsl_vector_free(diagonal);
  gsl_matrix_free(schlaefliLU);
}

void generate_coxeter_graph(semisimple_type_t type, int *result)
{
  int rank = coxeter_rank(type);
  int order = coxeter_order(type);
  int element_count;
  gsl_matrix *schlaefliMatrix = gsl_matrix_alloc(rank, rank);
  gsl_vector *startingVector = gsl_vector_alloc(rank);
  gsl_matrix *generators = (gsl_matrix*)malloc(rank*sizeof(gsl_matrix));
  double *generators_data = (double*)malloc(rank*rank*rank*sizeof(double));
  gsl_matrix *elements = (gsl_matrix*)malloc(order*sizeof(gsl_matrix));
  double *elements_data = (double*)malloc(rank*rank*order*sizeof(double));
  gsl_matrix *inverses = (gsl_matrix*)malloc(order*sizeof(gsl_matrix));
  double *inverses_data = (double*)malloc(rank*rank*order*sizeof(double));
  gsl_vector *i1 = gsl_vector_alloc(rank);
  gsl_vector *i2 = gsl_vector_alloc(rank);
  gsl_matrix *current_element = gsl_matrix_alloc(rank, rank);
  queue_t queue;
  int current;


  for(int i = 0; i < rank; i++)
    generators[i] = gsl_matrix_view_array(generators_data + i*rank*rank, rank, rank).matrix;
  for(int i = 0; i < order; i++)
    elements[i] = gsl_matrix_view_array(elements_data + i*rank*rank, rank, rank).matrix;
  for(int i = 0; i < order; i++)
    inverses[i] = gsl_matrix_view_array(inverses_data + i*rank*rank, rank, rank).matrix;

  generate_schlaefli_matrix(type, schlaefliMatrix);
  generate_starting_vector(rank, schlaefliMatrix, startingVector);

  for(int i = 0; i < rank; i++) {
    gsl_matrix_set_identity(&generators[i]);
    for(int j = 0; j < rank; j++)
      if(i == j)
	gsl_matrix_set(&generators[i], i, j, -1.0);
      else
	gsl_matrix_set(&generators[i], i, j, -gsl_matrix_get(schlaefliMatrix, i, j));
    //    gsl_matrix_fprintf(stdout, &generators[i], "%f");
    //    printf("\n");
  }

  queue_init(&queue);
  queue_put(&queue, 0);
  element_count = 1;
  gsl_matrix_set_identity(&elements[0]);
  gsl_matrix_set_identity(&inverses[0]);
  while((current = queue_get(&queue)) != -1) {
    for(int i = 0; i < rank; i++) {
      int j;
      for(j = 0; j < element_count; j++) {
	if(check_equal_sector(&generators[i], &elements[current], &inverses[j], schlaefliMatrix, startingVector, i1, i2)) { // generators[i] * elements[current] = elements[j]
	  result[rank*current + i] = j;
	  break;
	}
      }
      // if no existing element equals generators[i] * elements[current], create one
      if(j == element_count) {
	ERROR(element_count >= order, "Got more elements than the order of the group should be!\n");
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &generators[i], &elements[current], 0.0, &elements[element_count]);
	invert(&elements[element_count], &inverses[element_count]);
	result[rank*current + i] = element_count;
	queue_put(&queue, element_count);
	element_count++;
      }
    }
  }

  ERROR(element_count != order, "Somethink went wrong building the Coxeter group. Found %d elements, %d expected\n", element_count, order);

  /*
  for(int i = 0; i < order; i++) {
    printf("%d: ", i);
    for(int j = 0; j < rank; j++)
      printf("%d ", result[rank*i + j]);
    printf("\n");
  }
  */

  gsl_vector_free(startingVector);
  gsl_vector_free(i1);
  gsl_vector_free(i2);
  gsl_matrix_free(schlaefliMatrix);
  gsl_matrix_free(current_element);
  free(generators);
  free(elements);
  free(generators_data);
  free(elements_data);
}