#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_eigen.h>
#include "shared/libklp_matrix_header.h"
#include "shared/libtpl_header.h"
#include "population_constants.h"
#include "population_params.h"
#include "population_initializers.h"
#include "population_functions.h"

void population_from_row_ordered_transition_matrix(const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters, TRANSITION_MATRIX row_transition_matrix) {
  if (parameters.equilibrium) {
    equilibrium_from_row_ordered_transition_matrix(klp_params, parameters, row_transition_matrix);
  } else {
    population_proportion_from_row_ordered_transition_matrix(klp_params, parameters, row_transition_matrix);
  }
}

void equilibrium_from_row_ordered_transition_matrix(const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters, TRANSITION_MATRIX row_transition_matrix) {
  EIGENSYSTEM eigensystem;

  eigensystem = eigensystem_from_row_ordered_transition_matrix(row_transition_matrix);
  print_equilibrium(eigensystem, klp_params, parameters);

}

void population_proportion_from_row_ordered_transition_matrix(const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters, TRANSITION_MATRIX row_transition_matrix) {
  EIGENSYSTEM eigensystem;

  eigensystem = eigensystem_from_row_ordered_transition_matrix(row_transition_matrix);
  print_population_proportion(eigensystem, klp_params, parameters);
}

EIGENSYSTEM eigensystem_from_row_ordered_transition_matrix(TRANSITION_MATRIX row_transition_matrix) {
  TRANSITION_MATRIX column_transition_matrix;
  EIGENSYSTEM eigensystem;

  column_transition_matrix = transpose_matrix(row_transition_matrix);
  eigensystem              = convert_transition_matrix_to_eigenvectors(column_transition_matrix);
  invert_matrix(&eigensystem);

  return eigensystem;
}

TRANSITION_MATRIX convert_structures_to_transition_matrix(const SOLUTION* all_structures, int num_structures) {
  // This code to convert the list of structures to a transition matrix does not ensure that there are only single b.p. moves.
  int i, j;
  double col_sum;
  TRANSITION_MATRIX transition_matrix = init_transition_matrix(num_structures, 'R');

  for (i = 0; i < num_structures; ++i) {
    col_sum = 0;

    for (j = 0; j < num_structures; ++j) {
      if (i != j) {
        T_COL_ORDER(transition_matrix, i, j) = \
                                               MIN(1, exp(-((double)all_structures[j].energy - (double)all_structures[i].energy) / VIENNA_RT));

        col_sum += T_COL_ORDER(transition_matrix, i, j);
#ifdef INSANE_DEBUG
        printf("%d\t%d\t%.4e\n", i, j, T_COL_ORDER(transition_matrix, i, j));
#endif
      }
    }

#ifdef INSANE_DEBUG
    printf("%d col_sum:\t%.4e\n\n", i, col_sum);
#endif
    T_COL_ORDER(transition_matrix, i, i) = -col_sum;
  }

  return transition_matrix;
}

EIGENSYSTEM convert_transition_matrix_to_eigenvectors(TRANSITION_MATRIX transition_matrix) {
  // Generates right eigenvectors from a column-ordered transition rate matrix, and saves them in a one-dimensional column-ordered array (eigensystem.vectors).
  int i, j;
  EIGENSYSTEM eigensystem;
  eigensystem                             = init_eigensystem(transition_matrix.row_length);
  gsl_matrix_view matrix_view             = gsl_matrix_view_array(transition_matrix.matrix, eigensystem.length, eigensystem.length);
  gsl_vector_complex* eigenvalues         = gsl_vector_complex_alloc(eigensystem.length);
  gsl_matrix_complex* eigenvectors        = gsl_matrix_complex_alloc(eigensystem.length, eigensystem.length);
  gsl_eigen_nonsymmv_workspace* workspace = gsl_eigen_nonsymmv_alloc(eigensystem.length);
  gsl_eigen_nonsymmv_params(1, workspace);
  gsl_eigen_nonsymmv(&matrix_view.matrix, eigenvalues, eigenvectors, workspace);
  gsl_eigen_nonsymmv_free(workspace);

  for (i = 0; i < eigensystem.length; ++i) {
    eigensystem.values[i]               = GSL_REAL(gsl_vector_complex_get(eigenvalues, i));
    gsl_vector_complex_view eigenvector = gsl_matrix_complex_column(eigenvectors, i);

    for (j = 0; j < eigensystem.length; ++j) {
      E_COL_ORDER(eigensystem.vectors, i, j, eigensystem.length) = GSL_REAL(gsl_vector_complex_get(&eigenvector.vector, j));
    }
  }

  free_transition_matrix(transition_matrix);
  gsl_vector_complex_free(eigenvalues);
  gsl_matrix_complex_free(eigenvectors);
  return eigensystem;
}

void invert_matrix(EIGENSYSTEM* eigensystem) {
  int i, j, signum;
  gsl_matrix* matrix_to_invert = gsl_matrix_alloc(eigensystem->length, eigensystem->length);
  gsl_matrix* inversion_matrix = gsl_matrix_alloc(eigensystem->length, eigensystem->length);
  gsl_permutation* permutation = gsl_permutation_alloc(eigensystem->length);

  for (i = 0; i < eigensystem->length; ++i) {
    for (j = 0; j < eigensystem->length; ++j) {
      gsl_matrix_set(matrix_to_invert, i, j, E_ROW_ORDER(eigensystem->vectors, i, j, eigensystem->length));
    }
  }

  gsl_linalg_LU_decomp(matrix_to_invert, permutation, &signum);
  gsl_linalg_LU_invert(matrix_to_invert, permutation, inversion_matrix);

  for (i = 0; i < eigensystem->length; ++i) {
    for (j = 0; j < eigensystem->length; ++j) {
      E_ROW_ORDER(eigensystem->inverse_vectors, i, j, eigensystem->length) = gsl_matrix_get(inversion_matrix, i, j);
    }
  }

  gsl_matrix_free(matrix_to_invert);
  gsl_matrix_free(inversion_matrix);
  gsl_permutation_free(permutation);
}

double probability_at_logtime(const EIGENSYSTEM eigensystem, const KLP_PARAMS klp_params, double timepoint, int start_index, int target_index) {
  // This function is hard-wired to only consider the kinetics for folding from a distribution where p_{0}(start_index) == 1.
  int i;
  double cumulative_probability = 0;

  if (klp_params.start_state < 0 || klp_params.end_state < 0) {
    if (klp_params.start_state < 0) {
      fprintf(stderr, "Error: the starting structure could not be found. Usually this means that you did not specify an explicit starting structure so the empty structure was used, but the energy band was not wide enough for RNAsubopt to sample it.\n");
    }

    if (klp_params.end_state < 0) {
      fprintf(stderr, "Error: the ending structure could not be found. Usually this means that you did explicitly provided a suboptimal ending structure, but the energy band was not wide enough for RNAsubopt to sample it.\n");
    }

    abort();
  }

  for (i = 0; i < eigensystem.length; ++i) {
    cumulative_probability +=
      E_ROW_ORDER(eigensystem.vectors, target_index, i, eigensystem.length) *
      E_ROW_ORDER(eigensystem.inverse_vectors, i, start_index, eigensystem.length) *
      exp(eigensystem.values[i] * pow(10, timepoint));
  }

  return cumulative_probability;
}

void find_key_structure_indices_in_structure_list(KLP_PARAMS* klp_params, const POPULATION_PARAMS parameters, const SOLUTION* all_structures, int num_structures) {
  int i;

  for (i = 0; i < num_structures; ++i) {
    if (klp_params->start_state == -1) {
      if (!strcmp(parameters.start_structure, all_structures[i].structure)) {
        klp_params->start_state = i;
      }
    }

    if (klp_params->end_state == -1) {
      if (!strcmp(parameters.end_structure, all_structures[i].structure)) {
        klp_params->end_state = i;
      }
    }
  }
}

void serialize_eigensystem(const EIGENSYSTEM eigensystem, const POPULATION_PARAMS parameters) {
  tpl_node* tpl;
  int i;
  double value, vector, inverse_vector;
  tpl = tpl_map("iA(f)A(f)A(f)", &eigensystem.length, &value, &vector, &inverse_vector);
  tpl_pack(tpl, 0);

  for (i = 0; i < eigensystem.length * eigensystem.length; ++i) {
    if (i < eigensystem.length) {
      value = eigensystem.values[i];
      tpl_pack(tpl, 1);
    }

    vector         = eigensystem.vectors[i];
    inverse_vector = eigensystem.inverse_vectors[i];

    tpl_pack(tpl, 2);
    tpl_pack(tpl, 3);
  }

  tpl_dump(tpl, TPL_FILE, parameters.filename);
  tpl_free(tpl);
}

EIGENSYSTEM deserialize_eigensystem(const POPULATION_PARAMS parameters) {
  tpl_node* tpl;
  int i, length;
  double value, vector, inverse_vector;
  EIGENSYSTEM eigensystem;

  tpl = tpl_map("iA(f)A(f)A(f)", &length, &value, &vector, &inverse_vector);
  tpl_load(tpl, TPL_FILE, parameters.filename);
  tpl_unpack(tpl, 0);

  eigensystem = init_eigensystem(length);

  for (i = 0; i < eigensystem.length * eigensystem.length; ++i) {
    if (i < eigensystem.length) {
      tpl_unpack(tpl, 1);
      eigensystem.values[i] = value;
    }

    tpl_unpack(tpl, 2);
    tpl_unpack(tpl, 3);

    eigensystem.vectors[i]         = vector;
    eigensystem.inverse_vectors[i] = inverse_vector;
  }

  tpl_free(tpl);

  return eigensystem;
}

void ensure_key_structures_and_energies_assigned(POPULATION_PARAMS* parameters) {
  int i, seq_length = strlen(parameters->sequence);

  if (parameters->start_structure == NULL) {
    parameters->start_structure = malloc((seq_length + 1) * sizeof(char));

    for (i = 0; i < seq_length; ++i) {
      parameters->start_structure[i] = '.';
    }

    parameters->start_structure[i] = '\0';
  }

  if (parameters->end_structure == NULL) {
    set_end_structure(parameters);
  }
}

void set_end_structure(POPULATION_PARAMS* parameters) {
  paramT* vienna_params;

  vienna_params             = init_vienna_params(*parameters);
  parameters->end_structure = malloc((strlen(parameters->sequence) + 1) * sizeof(char));
  fold_par(parameters->sequence, parameters->end_structure, vienna_params, 0, 0);
}

SOLUTION* sample_structures(const POPULATION_PARAMS parameters) {
  double energy_cap;
  paramT* vienna_params;

  vienna_params = init_vienna_params(parameters);
  energy_cap    = parameters.energy_cap ? (int)round(parameters.energy_cap * 100) : INF;

  return subopt_par(parameters.sequence, parameters.start_structure, vienna_params, energy_cap, 0, 0, NULL);
}

double estimate_equilibrium(const EIGENSYSTEM eigensystem, const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters, int eigensystem_index) {
  int i, everything_in_equilibrium = 0;
  double equilibrium_time;

  equilibrium_time = soft_bound_for_population_proportion(eigensystem, klp_params, parameters, eigensystem_index, parameters.end_time, 1);

#ifdef INSANE_DEBUG
  printf("equilibrium_time: %f\n", equilibrium_time);
#endif

  while (equilibrium_time >= parameters.start_time && index_in_equilibrium_within_window(eigensystem, klp_params, parameters, eigensystem_index, equilibrium_time)) {
    equilibrium_time -= parameters.step_size;
  }

#ifdef INSANE_DEBUG
  printf("equilibrium_time after backtrack: %f\n", equilibrium_time);
#endif

  if (equilibrium_time < parameters.start_time) {
    return NEG_INF(parameters);
  } else {
    while (equilibrium_time <= parameters.end_time) {
      if (parameters.all_subpop_for_eq) {
        everything_in_equilibrium = 1;

        for (i = 0; i < eigensystem.length && everything_in_equilibrium; ++i) {
          everything_in_equilibrium = index_in_equilibrium_within_window(eigensystem, klp_params, parameters, i, equilibrium_time);
        }
      } else {
        everything_in_equilibrium = index_in_equilibrium_within_window(eigensystem, klp_params, parameters, eigensystem_index, equilibrium_time);
      }

      if (everything_in_equilibrium) {
        return equilibrium_time;
      } else {
        equilibrium_time += parameters.step_size;
      }
    }

    return POS_INF(parameters);
  }
}

double soft_bound_for_population_proportion(const EIGENSYSTEM eigensystem, const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters, int eigensystem_index, double final_time, int sign) {
  double current_probability, final_probability, current_time, temp_time, last_time = final_time;

  if (parameters.soft_bounds) {
    last_time           = final_time;
    current_time        = parameters.start_time + (parameters.end_time - parameters.start_time) / 2.0;
    current_probability = probability_at_logtime(eigensystem, klp_params, current_time, klp_params.start_state, eigensystem_index);
    final_probability   = probability_at_logtime(eigensystem, klp_params, final_time, klp_params.start_state, eigensystem_index);

    while (fabs(current_time - last_time) > parameters.step_size) {
      current_probability = probability_at_logtime(eigensystem, klp_params, current_time, klp_params.start_state, eigensystem_index);

#ifdef INSANE_DEBUG
      printf("current_time: %+.4f\tlast_time: %+.4f\tp(%d, current): %.4f\tp(%d, target): %.4f\n", current_time, last_time, eigensystem_index, current_probability, eigensystem_index, final_probability);
#endif

      temp_time = current_time;

      if (fabs(current_probability - final_probability) > parameters.delta) {
        current_time += sign * fabs(last_time - current_time) / 2.0;
      } else {
        current_time -= sign * fabs(last_time - current_time) / 2.0;
      }

      last_time = temp_time;
    }

#ifdef INSANE_DEBUG
    printf("Bounded at %+f within %f of requested delta, %f\n\n", current_time, fabs(current_probability - final_probability), parameters.delta);
#endif

    return round(current_time / parameters.step_size) * parameters.step_size;
  } else {
    return final_time;
  }
}

int index_in_equilibrium_within_window(const EIGENSYSTEM eigensystem, const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters, int eigensystem_index, double logtime) {
  int i;
  double current_proportion, future_proportion;

  current_proportion = probability_at_logtime(
                         eigensystem,
                         klp_params,
                         logtime,
                         klp_params.start_state,
                         eigensystem_index
                       );

  for (i = 1; i < parameters.window_size; ++i) {
    future_proportion = probability_at_logtime(
                          eigensystem,
                          klp_params,
                          logtime + parameters.step_size * i,
                          klp_params.start_state,
                          eigensystem_index
                        );

#ifdef INSANE_DEBUG
    printf(
      "current_time: %f, window_time: %f, index: %d, abs(p(window) - p(current)): %e, within epsilon?: %s\n",
      logtime,
      logtime + parameters.step_size * i,
      eigensystem_index,
      fabs(current_proportion - future_proportion),
      fabs(current_proportion - future_proportion) < parameters.epsilon ? "yes" : "no"
    );
#endif

    if (fabs(current_proportion - future_proportion) > parameters.epsilon) {
      return 0;
    }
  }

  return 1;
}

static int compare_tuple(const void* pa, const void* pb) {
  const double* a = *(const double**)pa;
  const double* b = *(const double**)pb;

  if (a[0] > b[0]) {
    return 1;
  } else if (a[0] < b[0]) {
    return -1;
  } else {
    return 0;
  }
}

int* array_of_indices_to_output(const EIGENSYSTEM eigensystem, const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters, double logtime) {
  int i, output_size;
  int* id_list;
  double** final_proportions;

  switch (parameters.num_subpop_to_show) {
    case -1:
      output_size = 2;
      break;

    case 0:
      output_size = eigensystem.length;
      break;

    default:
      output_size = parameters.num_subpop_to_show;
  }

#ifdef INSANE_DEBUG
  printf("number of subpopulations to output: %d\n", output_size);
#endif

  id_list    = malloc((output_size + 1) * sizeof(int));
  id_list[0] = output_size;

  if (parameters.num_subpop_to_show == -1) {
    id_list[1] = klp_params.end_state;
    id_list[2] = klp_params.start_state;
  } else {
    final_proportions = malloc(eigensystem.length * sizeof(double*));

    for (i = 0; i < eigensystem.length; ++i) {
      final_proportions[i] = malloc(2 * sizeof(double));
    }

    for (i = 0; i < eigensystem.length; ++i) {
      final_proportions[i][0] = probability_at_logtime(eigensystem, klp_params, logtime, klp_params.start_state, i);
      final_proportions[i][1] = i;
    }

#ifdef INSANE_DEBUG
    printf("\nBefore calling qsort:\n\n");

    for (i = 0; i < eigensystem.length; ++i) {
      printf("final_proportions[%d]\t%.8f\n", (int)final_proportions[i][1], final_proportions[i][0]);
    }

#endif

    qsort(final_proportions, eigensystem.length, sizeof(final_proportions[0]), compare_tuple);

#ifdef INSANE_DEBUG
    printf("\nAfter calling qsort:\n\n");

    for (i = 0; i < eigensystem.length; ++i) {
      printf("final_proportions[%d]\t%.8f\n", (int)final_proportions[i][1], final_proportions[i][0]);
    }

    printf("\nIndices to keep:\n\n");
#endif

    for (i = 1; i <= id_list[0]; ++i) {
      id_list[i] = (int)final_proportions[eigensystem.length - i][1];

#ifdef INSANE_DEBUG
      printf("final_proportions[%d]\t%.8f\n", id_list[i], final_proportions[eigensystem.length - i][0]);
#endif
    }
  }

  return id_list;
}

void print_equilibrium(const EIGENSYSTEM eigensystem, const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters) {
  double soft_right_bound, equilibrium_time;
  int i, list_length;
  int* id_list;

  soft_right_bound = soft_bound_for_population_proportion(eigensystem, klp_params, parameters, klp_params.end_state, parameters.end_time, 1);
  id_list          = array_of_indices_to_output(eigensystem, klp_params, parameters, soft_right_bound);
  list_length      = id_list++[0];

  printf("index\tlogtime\n");

  for (i = 0; i < list_length; ++i) {
    equilibrium_time = estimate_equilibrium(eigensystem, klp_params, parameters, id_list[i]);

    printf("%d\t", id_list[i]);

    if (equilibrium_time == NEG_INF(parameters)) {
      printf("-Infinity\n");
    } else if (equilibrium_time == POS_INF(parameters)) {
      printf("Infinity\n");
    } else {
      printf("%+.8f\n", equilibrium_time);
    }
  }
}

void print_population_proportion(const EIGENSYSTEM eigensystem, const KLP_PARAMS klp_params, const POPULATION_PARAMS parameters) {
  double current_time, soft_left_bound, soft_right_bound;
  int i, list_length;
  int* id_list;

  soft_left_bound  = soft_bound_for_population_proportion(eigensystem, klp_params, parameters, klp_params.start_state, parameters.start_time, -1);
  soft_right_bound = soft_bound_for_population_proportion(eigensystem, klp_params, parameters, klp_params.end_state, parameters.end_time, 1);
  id_list          = array_of_indices_to_output(eigensystem, klp_params, parameters, soft_right_bound);
  list_length      = id_list++[0];

  printf("logtime\t");

  for (i = 0; i < list_length; ++i) {
    printf("%d\t", id_list[i]);
  }

  printf("\n");

  for (current_time = soft_left_bound; current_time <= soft_right_bound; current_time += parameters.step_size) {
    printf("%+.8f\t", current_time);

    for (i = 0; i < list_length - 1; ++i) {
      printf("%+.8f\t", probability_at_logtime(eigensystem, klp_params, current_time, klp_params.start_state, id_list[i]));
    }

    printf("%+.8f", probability_at_logtime(eigensystem, klp_params, current_time, klp_params.start_state, id_list[i]));

    printf("\n");
  }
}

void print_array(char* title, double* matrix, int length) {
  int i;
  printf("%s\n", title);

  for (i = 0; i < length; ++i) {
    printf("%+.4f\n", matrix[i]);
  }

  printf("\n");
}

void print_square_matrix(char* title, double* matrix, int length) {
  int i, j;
  printf("%s\n", title);

  for (i = 0; i < length; ++i) {
    for (j = 0; j < length; ++j) {
      printf("%+.2e ", E_ROW_ORDER(matrix, i, j, length));
    }

    printf("\n");
  }

  printf("\n");
}

void print_eigenvalues(const EIGENSYSTEM eigensystem) {
  int i;

  for (i = 0; i < eigensystem.length; ++i) {
    printf("%+.8f\n", eigensystem.values[i]);
  }
}


/* specfunc/coupling.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001, 2002 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_coupling.h>

#include "error.h"

inline
static
int locMax3(const int a, const int b, const int c)
{
  int d = GSL_MAX(a, b);
  return GSL_MAX(d, c);
}

inline
static
int locMin3(const int a, const int b, const int c)
{
  int d = GSL_MIN(a, b);
  return GSL_MIN(d, c);
}

inline
static
int locMin5(const int a, const int b, const int c, const int d, const int e)
{
  int f = GSL_MIN(a, b);
  int g = GSL_MIN(c, d);
  int h = GSL_MIN(f, g);
  return GSL_MIN(e, h);
}

/* See: [Thompson, Atlas for Computing Mathematical Functions] */

static
int
delta(int ta, int tb, int tc, gsl_sf_result * d)
{
  gsl_sf_result f1, f2, f3, f4;
  int status = 0;
  status += gsl_sf_fact_e((ta + tb - tc)/2, &f1);
  status += gsl_sf_fact_e((ta + tc - tb)/2, &f2);
  status += gsl_sf_fact_e((tb + tc - ta)/2, &f3);
  status += gsl_sf_fact_e((ta + tb + tc)/2 + 1, &f4);
  if(status != 0) {
    OVERFLOW_ERROR(d);
  }
  d->val = f1.val * f2.val * f3.val / f4.val;
  d->err = 4.0 * GSL_DBL_EPSILON * fabs(d->val);
  return GSL_SUCCESS;
}

static
int
triangle_selection_fails(int two_ja, int two_jb, int two_jc)
{
  return ((two_jb < abs(two_ja - two_jc)) || (two_jb > two_ja + two_jc));
}

static
int
m_selection_fails(int two_ja, int two_jb, int two_jc,
                  int two_ma, int two_mb, int two_mc)
{
  return (
         abs(two_ma) > two_ja
      || abs(two_mb) > two_jb
      || abs(two_mc) > two_jc
      || GSL_IS_ODD(two_ja + two_ma)
      || GSL_IS_ODD(two_jb + two_mb)
      || GSL_IS_ODD(two_jc + two_mc)
      || (two_ma + two_mb + two_mc) != 0
          );
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_coupling_3j_e (int two_ja, int two_jb, int two_jc,
                      int two_ma, int two_mb, int two_mc,
                      gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(two_ja < 0 || two_jb < 0 || two_jc < 0) {
    DOMAIN_ERROR(result);
  }
  else if (   triangle_selection_fails(two_ja, two_jb, two_jc)
           || m_selection_fails(two_ja, two_jb, two_jc, two_ma, two_mb, two_mc)
     ) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    int jca  = (-two_ja + two_jb + two_jc) / 2,
        jcb  = ( two_ja - two_jb + two_jc) / 2,
        jcc  = ( two_ja + two_jb - two_jc) / 2,
        jmma = ( two_ja - two_ma) / 2,
        jmmb = ( two_jb - two_mb) / 2,
        jmmc = ( two_jc - two_mc) / 2,
        jpma = ( two_ja + two_ma) / 2,
        jpmb = ( two_jb + two_mb) / 2,
        jpmc = ( two_jc + two_mc) / 2,
        jsum = ( two_ja + two_jb + two_jc) / 2,
        kmin = locMax3 (0, jpmb - jmmc, jmma - jpmc),
        kmax = locMin3 (jcc, jmma, jpmb),
        k, sign = GSL_IS_ODD (kmin - jpma + jmmb) ? -1 : 1,
        status = 0;
    double sum_pos = 0.0, sum_neg = 0.0, norm, term;
    gsl_sf_result bc1, bc2, bc3, bcn1, bcn2, bcd1, bcd2, bcd3, bcd4;

    status += gsl_sf_choose_e (two_ja, jcc , &bcn1);
    status += gsl_sf_choose_e (two_jb, jcc , &bcn2);
    status += gsl_sf_choose_e (jsum+1, jcc , &bcd1);
    status += gsl_sf_choose_e (two_ja, jmma, &bcd2);
    status += gsl_sf_choose_e (two_jb, jmmb, &bcd3);
    status += gsl_sf_choose_e (two_jc, jpmc, &bcd4);

    if (status != 0) {
      OVERFLOW_ERROR (result);
    }

    norm = sqrt (bcn1.val * bcn2.val)
           / sqrt (bcd1.val * bcd2.val * bcd3.val * bcd4.val * ((double) two_jc + 1.0));

    for (k = kmin; k <= kmax; k++) {
      status += gsl_sf_choose_e (jcc, k, &bc1);
      status += gsl_sf_choose_e (jcb, jmma - k, &bc2);
      status += gsl_sf_choose_e (jca, jpmb - k, &bc3);

      if (status != 0) {
        OVERFLOW_ERROR (result);
      }

      term = bc1.val * bc2.val * bc3.val;

      if (sign < 0) {
        sum_neg += norm * term;
      } else {
        sum_pos += norm * term;
      }

      sign = -sign;
    }

    result->val  = sum_pos - sum_neg;
    result->err  = 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
    result->err += 2.0 * GSL_DBL_EPSILON * (kmax - kmin) * fabs(result->val);

    return GSL_SUCCESS;
  }
}

int
gsl_sf_coupling_6j_INCORRECT_e(int two_ja, int two_jb, int two_jc,
                               int two_jd, int two_je, int two_jf,
                               gsl_sf_result * result)
{
  return gsl_sf_coupling_6j_e(two_ja, two_jb, two_je, two_jd, two_jc, two_jf, result);
}

int
gsl_sf_coupling_6j_e(int two_ja, int two_jb, int two_jc,
                     int two_jd, int two_je, int two_jf,
                     gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(   two_ja < 0 || two_jb < 0 || two_jc < 0
     || two_jd < 0 || two_je < 0 || two_jf < 0
     ) {
    DOMAIN_ERROR(result);
  }
  else if(   triangle_selection_fails(two_ja, two_jb, two_jc)
          || triangle_selection_fails(two_ja, two_je, two_jf)
          || triangle_selection_fails(two_jb, two_jd, two_jf)
          || triangle_selection_fails(two_je, two_jd, two_jc)
     ) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result n1;
    gsl_sf_result d1, d2, d3, d4, d5, d6;
    double norm;
    int tk, tkmin, tkmax;
    double phase;
    double sum_pos = 0.0;
    double sum_neg = 0.0;
    double sumsq_err = 0.0;
    int status = 0;
    status += delta(two_ja, two_jb, two_jc, &d1);
    status += delta(two_ja, two_je, two_jf, &d2);
    status += delta(two_jb, two_jd, two_jf, &d3);
    status += delta(two_je, two_jd, two_jc, &d4);
    if(status != GSL_SUCCESS) {
      OVERFLOW_ERROR(result);
    }
    norm = sqrt(d1.val) * sqrt(d2.val) * sqrt(d3.val) * sqrt(d4.val);

    tkmin = locMax3(0,
                   two_ja + two_jd - two_jc - two_jf,
                   two_jb + two_je - two_jc - two_jf);

    tkmax = locMin5(two_ja + two_jb + two_je + two_jd + 2,
                    two_ja + two_jb - two_jc,
                    two_je + two_jd - two_jc,
                    two_ja + two_je - two_jf,
                    two_jb + two_jd - two_jf);

    phase = GSL_IS_ODD((two_ja + two_jb + two_je + two_jd + tkmin)/2)
            ? -1.0
            :  1.0;

    for(tk=tkmin; tk<=tkmax; tk += 2) {
      double term;
      double term_err;
      gsl_sf_result den_1, den_2;
      gsl_sf_result d1_a, d1_b;
      status = 0;

      status += gsl_sf_fact_e((two_ja + two_jb + two_je + two_jd - tk)/2 + 1, &n1);
      status += gsl_sf_fact_e(tk/2, &d1_a);
      status += gsl_sf_fact_e((two_jc + two_jf - two_ja - two_jd + tk)/2, &d1_b);
      status += gsl_sf_fact_e((two_jc + two_jf - two_jb - two_je + tk)/2, &d2);
      status += gsl_sf_fact_e((two_ja + two_jb - two_jc - tk)/2, &d3);
      status += gsl_sf_fact_e((two_je + two_jd - two_jc - tk)/2, &d4);
      status += gsl_sf_fact_e((two_ja + two_je - two_jf - tk)/2, &d5);
      status += gsl_sf_fact_e((two_jb + two_jd - two_jf - tk)/2, &d6);

      if(status != GSL_SUCCESS) {
        OVERFLOW_ERROR(result);
      }

      d1.val = d1_a.val * d1_b.val;
      d1.err = d1_a.err * fabs(d1_b.val) + fabs(d1_a.val) * d1_b.err;

      den_1.val  = d1.val*d2.val*d3.val;
      den_1.err  = d1.err * fabs(d2.val*d3.val);
      den_1.err += d2.err * fabs(d1.val*d3.val);
      den_1.err += d3.err * fabs(d1.val*d2.val);

      den_2.val  = d4.val*d5.val*d6.val;
      den_2.err  = d4.err * fabs(d5.val*d6.val);
      den_2.err += d5.err * fabs(d4.val*d6.val);
      den_2.err += d6.err * fabs(d4.val*d5.val);

      term  = phase * n1.val / den_1.val / den_2.val;
      phase = -phase;
      term_err  = n1.err / fabs(den_1.val) / fabs(den_2.val);
      term_err += fabs(term / den_1.val) * den_1.err;
      term_err += fabs(term / den_2.val) * den_2.err;

      if(term >= 0.0) {
        sum_pos += norm*term;
      }
      else {
        sum_neg -= norm*term;
      }

      sumsq_err += norm*norm * term_err*term_err;
    }

    result->val  = sum_pos - sum_neg;
    result->err  = 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
    result->err += sqrt(sumsq_err / (0.5*(tkmax-tkmin)+1.0));
    result->err += 2.0 * GSL_DBL_EPSILON * (tkmax - tkmin + 2.0) * fabs(result->val);

    return GSL_SUCCESS;
  }
}

int
gsl_sf_coupling_RacahW_e(int two_ja, int two_jb, int two_jc,
                         int two_jd, int two_je, int two_jf,
                         gsl_sf_result * result)
{
  int status = gsl_sf_coupling_6j_e(two_ja, two_jb, two_je, two_jd, two_jc, two_jf, result);
  int phase_sum = (two_ja + two_jb + two_jc + two_jd)/2;
  result->val *= ( GSL_IS_ODD(phase_sum) ? -1.0 : 1.0 );
  return status;
}

int
gsl_sf_coupling_9j_e(int two_ja, int two_jb, int two_jc,
                     int two_jd, int two_je, int two_jf,
                     int two_jg, int two_jh, int two_ji,
                     gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(   two_ja < 0 || two_jb < 0 || two_jc < 0
     || two_jd < 0 || two_je < 0 || two_jf < 0
     || two_jg < 0 || two_jh < 0 || two_ji < 0
     ) {
    DOMAIN_ERROR(result);
  }
  else if(   triangle_selection_fails(two_ja, two_jb, two_jc)
          || triangle_selection_fails(two_jd, two_je, two_jf)
          || triangle_selection_fails(two_jg, two_jh, two_ji)
          || triangle_selection_fails(two_ja, two_jd, two_jg)
          || triangle_selection_fails(two_jb, two_je, two_jh)
          || triangle_selection_fails(two_jc, two_jf, two_ji)
     ) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    int tk;
    int tkmin = locMax3(abs(two_ja-two_ji), abs(two_jh-two_jd), abs(two_jb-two_jf));
    int tkmax = locMin3(two_ja + two_ji, two_jh + two_jd, two_jb + two_jf);
    double sum_pos = 0.0;
    double sum_neg = 0.0;
    double sumsq_err = 0.0;
    double phase;
    for(tk=tkmin; tk<=tkmax; tk += 2) {
      gsl_sf_result s1, s2, s3;
      double term;
      double term_err;
      int status = 0;

      status += gsl_sf_coupling_6j_e(two_ja, two_ji, tk,  two_jh, two_jd, two_jg,  &s1);
      status += gsl_sf_coupling_6j_e(two_jb, two_jf, tk,  two_jd, two_jh, two_je,  &s2);
      status += gsl_sf_coupling_6j_e(two_ja, two_ji, tk,  two_jf, two_jb, two_jc,  &s3);

      if(status != GSL_SUCCESS) {
        OVERFLOW_ERROR(result);
      }
      term = s1.val * s2.val * s3.val;
      term_err  = s1.err * fabs(s2.val*s3.val);
      term_err += s2.err * fabs(s1.val*s3.val);
      term_err += s3.err * fabs(s1.val*s2.val);

      if(term >= 0.0) {
        sum_pos += (tk + 1) * term;
      }
      else {
        sum_neg -= (tk + 1) * term;
      }

      sumsq_err += ((tk+1) * term_err) * ((tk+1) * term_err);
    }

    phase = GSL_IS_ODD(tkmin) ? -1.0 : 1.0;

    result->val  = phase * (sum_pos - sum_neg);
    result->err  = 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
    result->err += sqrt(sumsq_err / (0.5*(tkmax-tkmin)+1.0));
    result->err += 2.0 * GSL_DBL_EPSILON * (tkmax-tkmin + 2.0) * fabs(result->val);

    return GSL_SUCCESS;
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_coupling_3j(int two_ja, int two_jb, int two_jc,
                          int two_ma, int two_mb, int two_mc)
{
  EVAL_RESULT(gsl_sf_coupling_3j_e(two_ja, two_jb, two_jc,
                                   two_ma, two_mb, two_mc,
                                   &result));
}

double gsl_sf_coupling_6j_INCORRECT(int two_ja, int two_jb, int two_jc,
                                    int two_jd, int two_je, int two_jf)
{
  EVAL_RESULT(gsl_sf_coupling_6j_INCORRECT_e(two_ja, two_jb, two_jc,
                                             two_jd, two_je, two_jf,
                                             &result));
}

double gsl_sf_coupling_6j(int two_ja, int two_jb, int two_jc,
                          int two_jd, int two_je, int two_jf)
{
  EVAL_RESULT(gsl_sf_coupling_6j_e(two_ja, two_jb, two_jc,
                                   two_jd, two_je, two_jf,
                                   &result));
}

double gsl_sf_coupling_RacahW(int two_ja, int two_jb, int two_jc,
                          int two_jd, int two_je, int two_jf)
{
  EVAL_RESULT(gsl_sf_coupling_RacahW_e(two_ja, two_jb, two_jc,
                                      two_jd, two_je, two_jf,
                                      &result));
}

double gsl_sf_coupling_9j(int two_ja, int two_jb, int two_jc,
                          int two_jd, int two_je, int two_jf,
                          int two_jg, int two_jh, int two_ji)
{
  EVAL_RESULT(gsl_sf_coupling_9j_e(two_ja, two_jb, two_jc,
                                   two_jd, two_je, two_jf,
                                   two_jg, two_jh, two_ji,
                                   &result));
}


#include <gsl/gsl_linalg.h>

int
main (void)
{
  double a_data[] = { 0.18, 0.60, 0.57, 0.96,
                      0.41, 0.24, 0.99, 0.58,
                      0.14, 0.30, 0.97, 0.66,
                      0.51, 0.13, 0.19, 0.85 } ;

  double b_data[] = { 1.0, 2.0, 3.0, 4.0 };

  gsl_matrix_view m
    = gsl_matrix_view_array(a_data, 4, 4);

  gsl_vector_view b
    = gsl_vector_view_array(b_data, 4);

  gsl_vector *x = gsl_vector_alloc (4);

  int s;

  gsl_permutation * p = gsl_permutation_alloc (4);

  gsl_linalg_LU_decomp (&m.matrix, p, &s);

  gsl_linalg_LU_solve (&m.matrix, p, &b.vector, x);

  gsl_vector_fprintf(stdout, x, "%g");

  gsl_permutation_free (p);
}


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_randist.h>

#include "allvars.h"
#include "proto.h"

void mcmc_con_run()
{
  char fname_mcmc[100];
  strcpy(fname_mcmc, "data/mcmc_con.txt");

  printf("*******con mcmc\n");
  mcmc_con_init();
  if(flag_mcmc==1)
  {
    mcmc_sampling(fname_mcmc, &probability_con);
    mcmc_stats(fname_mcmc);
  }
  else
  {
    nc = nc_lim_low;
    printf("reading par.txt\n");
    read_input();
  }

  reconstruct_con();

  memcpy(theta_best_con, theta_best, ntheta*sizeof(double));
  memcpy(theta_best_var_con, theta_best_var, ntheta*2*sizeof(double));
}

void mcmc_con_init()
{
  int i;

  ntheta = 2;

  i=0;
  theta_range[i][0] = log(1.0e-6);
  theta_range[i++][1] = log(10.0);
  theta_range[i][0] = log(1.0e-2);
  theta_range[i++][1] = log(1.0e5);

  i = 0;
  theta_input[i++] = log(0.1);
  theta_input[i++] = log(100.0);

  i = 0;
  sigma_input[i++] = 0.1;
  sigma_input[i++] = 0.5;

  i = 0;
  theta_fixed[i++] = 0;
  theta_fixed[i++] = 0;

}
/*
 * probability for continuum mcmc.
 */
double probability_con(double *theta)//double sigmahat, double taud, double alpha)
{
  double sigmahat, taud, alpha;
  double prob, prior, lndet, lndet_ICq, sigma;
  double *ybuf, *Larr, *Cq, *ICq, *ave, *yave, *ysub;
  int i, nq, info;

  nq = (flag_detrend + 1);
  ybuf = workspace;
  Larr = workspace+ncon_data;
  Cq = Larr + nq*ncon_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;
  yave = ave + nq;
  ysub = yave + ncon_data;

  alpha = 1.0;
  sigmahat = exp(theta[0]);
  taud = exp(theta[1]);

  sigma = sigmahat * sqrt(taud / 2.0);

  set_covar_mat_con(sigma, taud, alpha);

  for(i=0;i<ncon_data*ncon_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }
  memcpy(ICmat, Cmat, ncon_data*ncon_data*sizeof(double));
  inverse_mat(ICmat, ncon_data, &info);

  for(i=0;i<ncon_data;i++)
  {
    if(flag_detrend==0)
    {
      Larr[i]=1.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
    }
  }
  //multiply_matvec(ICmat, Larr, ncon_data, ybuf);
  multiply_mat_MN(ICmat, Larr, Tmat1, ncon_data, nq, ncon_data);
  multiply_mat_MN_transposeA(Larr, Tmat1, ICq, nq, nq, ncon_data);
  memcpy(Cq, ICq, nq*nq*sizeof(double));
  inverse_mat(Cq, nq, &info);

  multiply_mat_MN_transposeA(Larr, ICmat, Tmat1, nq, ncon_data, ncon_data);
  multiply_mat_MN(Cq, Tmat1, Tmat2, nq, ncon_data, nq);
  multiply_mat_MN(Tmat2, Fcon_data, ave, nq, 1, ncon_data);
  multiply_mat_MN(Larr, ave, yave, ncon_data, 1, nq);

//  lambda = cblas_ddot(ncon_data, Larr, 1, ybuf, 1);
//  multiply_matvec(ICmat, Fcon_data, ncon_data, ybuf);
//  ave_con = cblas_ddot(ncon_data, Larr, 1, ybuf, 1);
//  ave_con /=lambda;

  for(i=0;i<ncon_data;i++)ysub[i] = Fcon_data[i] - yave[i];
  multiply_matvec(ICmat, ysub, ncon_data, ybuf);
  prob = -0.5 * cblas_ddot(ncon_data, ysub, 1, ybuf, 1);

  lndet = lndet_mat(Cmat, ncon_data, &info);
  lndet_ICq = lndet_mat(ICq, nq, &info);
  prob = prob - 0.5*lndet - 0.5*lndet_ICq;

/* penalize on larger tau than the length of continuum */
  prior = 0.0;
  prior += - theta[0];
  if(theta[1] > log(cad_con) )
  {
    prior = ( log(cad_con) - theta[1]);
  }
  else
  {
    prior +=  - (log(cad_con) - theta[1]);
  }
  return prob + prior;
}

/* calculate covariance matrix */
void set_covar_mat_con(double sigma, double tau, double alpha)
{
  double t1, t2, nerr;
  int i, j;

  for(i=0; i<ncon_data; i++)
  {
    t1 = Tcon_data[i];
    for(j=0; j<=i; j++)
    {

      t2 = Tcon_data[j];
      Smat[i*ncon_data+j] = sigma*sigma * exp (- pow (fabs(t1-t2) / tau, alpha));
      Smat[j*ncon_data+i] = Smat[i*ncon_data+j];

      Nmat[i*ncon_data+j] = Nmat[j*ncon_data+i] = 0.0;
    }
    nerr = Fcerrs_data[i];
    Nmat[i*ncon_data+i] = nerr*nerr;
  }
  return;
}

void set_covar_Umat_con(double sigma, double tau, double alpha)
{
  double t1, t2;
  int i, j;

  for(i=0; i<ncon; i++)
  {
    t1 = Tcon[i];
    for(j=0; j<ncon_data; j++)
    {
      t2 = Tcon_data[j];
      USmat[i*ncon_data+j] = sigma*sigma * exp (- pow (fabs(t1-t2) / tau, alpha) );
    }
  }
  return;
}

/* reconstruction */
void reconstruct_con()
{
  double sigma, tau, alpha, sigmahat;
  double *ybuf, *Larr, *Larr_rec, *yave, *yave_rec, *Cq, *ICq, *ysub, *ave;
  double *Tmp1, *Tmp2, *Tmp3, *Tmp4;

  int i, nq, info, nmax;

  nq = 1+flag_detrend;

  yave = workspace;
  ysub = yave + ncon_data;
  Larr_rec = ysub + ncon_data;    //Larr_rec is a nq*ncon matrix
  yave_rec = Larr_rec + nq*ncon;  //yave_rec is ncon matrix
  ybuf = yave_rec + ncon;
  Larr = ybuf + ncon_data;   // Larr is a nq*n matrix
  Cq = Larr + nq*ncon_data;
  ICq = Cq + nq*nq;
  ave = ICq + nq*nq;

  nmax = max(ncon, ncon_data);
  Tmp1 = array_malloc(nmax*nmax);     // temporary matrixes
  Tmp2 = array_malloc(nmax*nmax);
  Tmp3 = array_malloc(nmax*nmax);
  Tmp4 = array_malloc(nmax*nmax);

  sigmahat = exp(theta_best[0]);
  tau = exp(theta_best[1]);
  sigma = sigmahat * sqrt(tau/2.0);
  alpha = 1.0;

  set_covar_mat_con(sigma, tau, alpha);
  set_covar_Umat_con(sigma, tau, alpha);

  for(i=0;i<ncon_data*ncon_data; i++)
  {
    Cmat[i] = Smat[i] + Nmat[i];
  }
  memcpy(ICmat, Cmat, ncon_data*ncon_data*sizeof(double));
  inverse_mat(ICmat, ncon_data, &info);

  /* the best estimate for average */
  for(i=0;i<ncon_data;i++)
  {
    if(flag_detrend==0)
    {
      Larr[i]=1.0;
    }
    else
    {
      Larr[i*nq + 0] = 1.0;
      Larr[i*nq + 1] = Tcon_data[i];
    }
  }

  for(i=0;i<ncon;i++)
  {
    if(flag_detrend==0)
    {
      Larr_rec[i]=1.0;
    }
    else
    {
      Larr_rec[i*nq + 0] = 1.0;
      Larr_rec[i*nq + 1] = Tcon[i];
    }
  }

// calculate the covariance matrix Cq for q.
// Cq = (L^T x C^-1 x L)^-1
  multiply_mat_MN(ICmat, Larr, Tmat1, ncon_data, nq, ncon_data);
  multiply_mat_MN_transposeA(Larr, Tmat1, ICq, nq, nq, ncon_data);
  memcpy(Cq, ICq, nq*nq*sizeof(double));
  inverse_mat(Cq, nq, &info);
// calculate the best estimate of q.
// q = Cq x L^T x C^-1 x y
  multiply_mat_MN_transposeA(Larr, ICmat, Tmat1, nq, ncon_data, ncon_data);
  multiply_mat_MN(Cq, Tmat1, Tmat2, nq, ncon_data, nq);
  multiply_mat_MN(Tmat2, Fcon_data, ave, nq, 1, ncon_data);
// subtract the linear trend
  multiply_mat_MN(Larr, ave, yave, ncon_data, 1, nq);
  for(i=0;i<ncon_data;i++)ysub[i] = Fcon_data[i] - yave[i];
// calculate the best estimate for s.
// s = S x C^-1 x (y - Lxq)
  multiply_matvec(ICmat, ysub, ncon_data, ybuf);
  multiply_matvec_MN(USmat, ncon, ncon_data, ybuf, Fcon);
  multiply_mat_MN(Larr_rec, ave, yave_rec, ncon, 1, nq);

  for(i=0; i<ncon; i++)Fcon[i] = Fcon[i] + yave_rec[i];

  multiply_mat_MN(USmat, ICmat, Tmp1, ncon, ncon_data, ncon_data);
  multiply_mat_MN_transposeB(Tmp1, USmat, Tmp2, ncon, ncon, ncon_data);
  multiply_mat_MN(Tmp1, Larr, Tmp3, ncon, nq, ncon_data);
  for(i=0; i<ncon*nq; i++)Tmp3[i] -= Larr_rec[i];
  multiply_mat_MN(Tmp3, Cq, Tmp1, ncon, nq, nq);
  multiply_mat_MN_transposeB(Tmp1, Tmp3, Tmp4, ncon, ncon, nq);

  for(i=0; i<ncon; i++)
  {
    Fcerrs[i] = sqrt(sigma*sigma - Tmp2[i*ncon+i] + Tmp4[i*ncon+i]);
  }

  FILE *fp;
  fp = fopen("data/scon.txt", "w");
  for(i=0; i<ncon; i++)
  {
    fprintf(fp, "%f %f %f\n", Tcon[i], Fcon[i]*scale_con, Fcerrs[i]*scale_con);
  }
  fclose(fp);
  free(Tmp1);
  free(Tmp2);
  free(Tmp3);
  free(Tmp4);
}


#ifndef ESMCI_Mat_H
#define ESMCI_Mat_H

#include "ESMCI_Macros.h"
#include <stdexcept>
#include <iostream>
#include <vector>
#include <algorithm>
#include <functional>
#include <numeric>
#include <initializer_list>
#include <complex.h>
#include <cblas.h>
#include <lapacke.h>

namespace ESMCI{
  namespace MapperUtil{

    /* A Matrix class
     * The matrix class is used to create an n-dimensional matrix of values
     */
    template<typename T>
    class Matrix{
      public:
        Matrix(const std::vector<int> &dims, const std::vector<T> &data);
        Matrix(std::initializer_list<int> dims, std::initializer_list<T> data);
        Matrix(const std::vector<int> &dims, const T &val);

        /* Get the matrix dimensions */
        std::vector<int> get_dims(void ) const;
        /* Get a reference to the data strored in the matrix */
        const T *get_data_by_ref(void ) const;
        T *get_data_by_ref(void );
        /* Get the data stored in the matrix */
        std::vector<T> get_data(void ) const;

        /* Get the transpose of the matrix */
        Matrix<T> transpose(void ) const;
        /* Get the inverse of the matrix */
        Matrix<T> inv(void ) const;
        /* Get the pseudo inverse of the matrix */
        Matrix<T> pinv(void ) const;

        /* Returns true if the matrix, m, is equal to this matrix,
         * false otherwise. This function uses a tolerance, tol, to
         * compare the values stored in the matrix (unlike == that
         * does not provide a tolerance)
         */
        bool equals(const Matrix<T> &m, double tol) const;

        /* Compare if two matrices are equal */
        template<typename U>
        friend bool operator==(const Matrix<U> &lhs, const Matrix<U> &rhs);
        /* Compare if all values in the matrix is equal to val */
        template<typename U>
        friend bool operator==(const Matrix<U> &m, const U &val);
        template<typename U>
        friend bool operator==(const U &val, const Matrix<U> &m);
        /* Operator to add/subtract/multiply two matrices */
        template<typename U>
        friend Matrix<U> operator+(const Matrix<U> &lhs, const Matrix<U> &rhs);
        template<typename U>
        friend Matrix<U> operator-(const Matrix<U> &lhs, const Matrix<U> &rhs);
        template<typename U>
        friend Matrix<U> operator*(const Matrix<U> &lhs, const Matrix<U> &rhs);
        /* Operator to scale a matrix by a scalar */
        template<typename U, typename V>
        friend Matrix<U> operator*(const Matrix<U> &lhs, const V &rhs);
        template<typename U, typename V>
        friend Matrix<V> operator*(const U &lhs, const Matrix<V> &rhs);
        template<typename U>
        friend std::ostream& operator<<(std::ostream &ostr, const Matrix<U> &m);
      private:
        std::vector<int> dims_;
        std::vector<T> data_;
    };

    template<typename T>
    Matrix<T>::Matrix(const std::vector<int> &dims, const std::vector<T> &data):dims_(dims),data_(data)
    {}

    template<typename T>
    Matrix<T>::Matrix(std::initializer_list<int> dims, std::initializer_list<T> data):dims_(dims.begin(),dims.end()),data_(data.begin(),data.end())
    {}

    template<typename T>
    Matrix<T>::Matrix(const std::vector<int> &dims, const T& val):dims_(dims)
    {
      assert(dims.size() > 0);
      int data_sz = std::accumulate(dims.begin(), dims.end(), 1, std::multiplies<int>());
      assert(data_sz > 0);
      data_.resize(data_sz, val);
    }

    template<typename T>
    std::vector<int> Matrix<T>::get_dims(void ) const
    {
      return dims_;
    }

    template<typename T>
    const T *Matrix<T>::get_data_by_ref(void ) const
    {
      return ((data_.size() > 0 ) ? (&(data_[0])) : NULL);
    }

    template<typename T>
    T *Matrix<T>::get_data_by_ref(void )
    {
      return ((data_.size() > 0 ) ? (&(data_[0])) : NULL);
    }

    template<typename T>
    std::vector<T> Matrix<T>::get_data(void ) const
    {
      return data_;
    }

    template<typename T>
    Matrix<T> Matrix<T>::transpose(void ) const
    {
      assert(dims_.size() <= 2);
      int nelems = static_cast<int>(data_.size());
      if(dims_.size() == 1){
        return Matrix<T>(dims_, data_);
      }
      else{
        std::vector<int> dims(dims_.rbegin(), dims_.rend());
        if((dims_[0] == 1) || dims_[1] == 1){
          return Matrix<T>(dims, data_);
        }
        else{
          // FIXME: Use inplace transpose
          std::vector<T> data(data_.size(), static_cast<T>(0));
          int nrows = dims_[0];
          int ncols = dims_[1];
          int tnrows = ncols;
          int tncols = nrows;
          for(int idx = 0; idx < nelems; idx++){
            int ridx = idx/ncols;
            int cidx = idx - ridx * ncols;
            int tridx = cidx;
            int tcidx = ridx;
            int tidx = tridx * tncols + tcidx;
            data[tidx] = data_[idx];
          }
          return Matrix<T>(dims, data);
        }
      }
    }

    /* Calculate the inverse of the matrix using LAPACK */
    inline int LAPACK_Minv(int m, float *A)
    {
      lapack_int n = static_cast<lapack_int>(m);
      lapack_int *ipiv = (lapack_int *)calloc(n+1, sizeof(lapack_int));
      lapack_int info;
      /* Calculate LU decomposition of the matrix */
      info = LAPACKE_sgetrf(LAPACK_ROW_MAJOR, n, n, A, n, ipiv);
      /* info == 0 => success */
      if(info < 0){
        std::cout << "LAPACKE_sgetrf failed, the "
          << -info << "th arg in input array, A[" << -info << "] = "
          << A[-info] << " is invalid\n";
        return ESMF_FAILURE;
      }
      else if(info > 0){
        std::cout << "LAPACKE_sgetrf failed, the U["
          << info << "," << info << "] = 0, U is singular and div by zero can occur"
          << " if used to solve a system of equations\n";
        return ESMF_FAILURE;
      }
      /* Calculate the inverse of the matrix */
      info = LAPACKE_sgetri(LAPACK_ROW_MAJOR, n, A, n, ipiv);
      /* info == 0 => success */
      if(info < 0){
        std::cout << "LAPACKE_sgetri failed, the "
          << -info << "th arg in input array, A[" << -info << "] = "
          << A[-info] << " is invalid\n";
        return ESMF_FAILURE;
      }
      else if(info > 0){
        std::cout << "LAPACKE_sgetri failed, the U["
          << info << "," << info << "] = 0, the matrix is singular and its inverse "
          << "cannot be computed\n";
        return ESMF_FAILURE;
      }
      free(ipiv);

      return ESMF_SUCCESS;
    }

    /* Calculate the inverse of the matrix using LAPACK */
    inline int LAPACK_Minv(int m, double *A)
    {
      lapack_int n = static_cast<lapack_int>(m);
      lapack_int *ipiv = (lapack_int *)calloc(n+1, sizeof(lapack_int));
      lapack_int info;
      /* Calculate LU decomposition of the matrix */
      info = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, A, n, ipiv);
      /* info == 0 => success */
      if(info < 0){
        std::cout << "LAPACKE_dgetrf failed, the "
          << -info << "th arg in input array, A[" << -info << "] = "
          << A[-info] << " is invalid\n";
        return ESMF_FAILURE;
      }
      else if(info > 0){
        std::cout << "LAPACKE_dgetrf failed, the U["
          << info << "," << info << "] = 0, U is singular and div by zero can occur"
          << " if used to solve a system of equations\n";
        return ESMF_FAILURE;
      }
      /* Calculate the inverse of the matrix */
      info = LAPACKE_dgetri(LAPACK_ROW_MAJOR, n, A, n, ipiv);
      /* info == 0 => success */
      if(info < 0){
        std::cout << "LAPACKE_dgetri failed, the "
          << -info << "th arg in input array, A[" << -info << "] = "
          << A[-info] << " is invalid\n";
        return ESMF_FAILURE;
      }
      else if(info > 0){
        std::cout << "LAPACKE_dgetri failed, the U["
          << info << "," << info << "] = 0, the matrix is singular and its inverse "
          << "cannot be computed\n";
        return ESMF_FAILURE;
      }
      free(ipiv);

      return ESMF_SUCCESS;
    }

    /* Calculate the inverse of the matrix */
    template<typename T>
    Matrix<T> Matrix<T>::inv(void ) const
    {
      Matrix<T> res = *this;
      if(dims_.size() == 1){
        return res;
      }

      // We are only concerned about 2d matrices for now
      assert(dims_.size() == 2);
      assert(std::adjacent_find(dims_.cbegin(), dims_.cend(), std::not_equal_to<T>()) == dims_.cend());

      int ret = LAPACK_Minv(res.dims_[0], res.get_data_by_ref());
      //assert(ret == ESMF_SUCCESS);
      if(ret != ESMF_SUCCESS){
        std::cerr << "Finding matrix inv failed for : \n" << *this << "\n";
        std::string err_msg("LAPACK routine to find inv failed");
        throw std::runtime_error(err_msg);
      }

      return res;
    }

    /* Compute pseudo inverse = Inverse(A_transpose * A) * A_transpose */
    template<typename T>
    Matrix<T> Matrix<T>::pinv(void ) const
    {
      Matrix<T> trans = transpose();
      try{
        // Left inverse
        return ((trans * (*this)).inv() * trans);
      }
      catch(...){
        // Right inverse
        return (trans * ((*this) * trans).inv());
      }
    }

    template<typename T>
    bool Matrix<T>::equals(const Matrix<T> &m, double tol) const
    {
      std::vector<int> m_dims = m.get_dims();
      std::vector<T> m_data = m.get_data();

      if(m_dims != dims_){
        return false;
      }

      if(m_data.size() != data_.size()){
        return false;
      }

      for(typename std::vector<T>::const_iterator citer1 = m_data.cbegin(),
          citer2 = data_.cbegin();
          (citer1 != m_data.cend()) && (citer2 != data_.cend());
          ++citer1, ++citer2){
        if(fabs(*citer1 - *citer2) > tol){
          return false;
        }
      }

      return true;
    }

    template<typename T>
    bool operator==(const Matrix<T> &lhs, const Matrix<T> &rhs)
    {
      if(lhs.dims_ != rhs.dims_){
        return false;
      }
      if(lhs.data_ != rhs.data_){
        return false;
      }
      return true;
    }

    template<typename T>
    bool operator==(const Matrix<T> &m, const T& val)
    {
      for(typename std::vector<T>::const_iterator citer = m.data_.cbegin();
          citer != m.data_.cend(); ++citer){
        if(*citer != val){
          return false;
        }
      }
      return true;
    }

    template<typename T>
    bool operator==(const T &val, const Matrix<T> &m)
    {
      return (m == val);
    }

    template<typename T>
    Matrix<T> operator+(const Matrix<T> &lhs, const Matrix<T> &rhs)
    {
      assert(lhs.dims_.size() == rhs.dims_.size());
      assert(std::equal(lhs.dims_.begin(), lhs.dims_.end(), rhs.dims_.begin()));
      Matrix<T> res = lhs;
      std::transform(res.data_.begin(), res.data_.end(), rhs.data_.begin(),
                      res.data_.begin(), std::plus<T>());
      return res;
    }

    template<typename U, typename V>
    Matrix<U> operator+(const Matrix<U> &lhs, const V &rhs)
    {
      Matrix<U> res = lhs;
      std::transform(res.data_.begin(), res.data_.end(),
                      res.data_.begin(),
                      std::bind1st(std::plus<U>(), static_cast<U>(rhs)));
      return res;
    }

    template<typename U, typename V>
    Matrix<V> operator+(const U &lhs, const Matrix<V> &rhs)
    {
      return rhs + lhs;
    }

    template<typename T>
    Matrix<T> operator-(const Matrix<T> &lhs, const Matrix<T> &rhs)
    {
      assert(lhs.dims_.size() == rhs.dims_.size());
      assert(std::equal(lhs.dims_.begin(), lhs.dims_.end(), rhs.dims_.begin()));
      Matrix<T> res = lhs;
      std::transform(res.data_.begin(), res.data_.end(), rhs.data_.begin(),
                      res.data_.begin(), std::minus<T>());
      return res;
    }

    template<typename U, typename V>
    Matrix<U> operator-(const Matrix<U> &lhs, const V &rhs)
    {
      Matrix<U> res = lhs;
      std::transform(res.data_.begin(), res.data_.end(),
                      res.data_.begin(),
                      std::bind1st(std::minus<U>(), static_cast<U>(rhs)));
      return res;
    }

    /* Multiply two matrices using BLAS
     */
    inline int BLAS_Mmult(int m, int n, int k, int alpha,
                          const float *A, int lda, const float *B, int ldb,
                          int beta, float *C, int ldc)
    {
      /* cblas_sgemm has no return code ! */
      cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                  m, n, k, alpha, A, lda, B, ldb, beta, C, ldc);
      return ESMF_SUCCESS;
    }

    /* Multiply two matrices using BLAS
     */
    inline int BLAS_Mmult(int m, int n, int k, int alpha,
                          const double *A, int lda, const double *B, int ldb,
                          int beta, double *C, int ldc)
    {
      /* cblas_sgemm has no return code ! */
      cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                  m, n, k, alpha, A, lda, B, ldb, beta, C, ldc);
      return ESMF_SUCCESS;
    }

    /* Multiply two matrices, currently uses BLAS routines for matrix
     * multiplication
     */
    template<typename T>
    Matrix<T> operator*(const Matrix<T> &lhs, const Matrix<T> &rhs)
    {
      assert((lhs.dims_.size() > 0) && (rhs.dims_.size() > 0));
      // No tensor products for now
      assert((lhs.dims_.size() <= 2) && (rhs.dims_.size() <= 2));
      assert(lhs.dims_[lhs.dims_.size()-1] == rhs.dims_[0]);
      /* Determine inputs to the BLAS routine to perform
       * matrix multiplication
       */
      int m, n, k, alpha, lda, ldb, beta, ldc;
      alpha = 1;
      beta = 1;

      std::vector<int> res_dims;
      if(lhs.dims_.size() == 1){
        res_dims.push_back(lhs.dims_[0]);
        m = 1;
        k = 1;
        n = 1;
        lda = 1;
        ldb = 1;
        ldc = 1;
      }
      else{
        assert(lhs.dims_.size() == 2);
        res_dims.push_back(lhs.dims_[0]);
        res_dims.push_back(rhs.dims_[rhs.dims_.size()-1]);
        m = lhs.dims_[0];
        k = lhs.dims_[lhs.dims_.size() - 1];
        n = rhs.dims_[rhs.dims_.size() - 1];
        lda = k;
        ldb = n;
        ldc = n;
      }

      Matrix<T> res(res_dims, 0);
      const T *A = lhs.get_data_by_ref();
      const T *B = rhs.get_data_by_ref();
      T *C = res.get_data_by_ref();

      /* Use BLAS for multiplying the two matrices */
      int ret = BLAS_Mmult(m, n, k, alpha, A, lda, B, ldb, beta, C, ldc);
      assert(ret == ESMF_SUCCESS);

      return res;
    }

    template<typename U, typename V>
    Matrix<U> operator*(const Matrix<U> &lhs, const V &rhs)
    {
      Matrix<U> res = lhs;
      std::transform(res.data_.begin(), res.data_.end(),
                      res.data_.begin(),
                      std::bind1st(std::multiplies<U>(), static_cast<U>(rhs)));
      return res;
    }
    template<typename U, typename V>
    Matrix<V> operator*(const U &lhs, const Matrix<V> &rhs)
    {
      return rhs * lhs;
    }

    template<typename T>
    std::ostream& operator<<(std::ostream &ostr, const Matrix<T> &m)
    {
      std::vector<int> dim_wgts(m.dims_);
      //std::vector<int> dim_wgts(m.dims_.size(), 0);
      //std::partial_sum(m.dims_.rbegin(), m.dims_.rend(), dim_wgts.rbegin(), std::multiplies<int>());
      std::vector<int> dim_wgts_counter(dim_wgts.size(), 0);
      assert(dim_wgts.size() > 0);
      std::size_t dim_wgts_counter_idx = dim_wgts.size() - 1;
      for(typename std::vector<T>::const_iterator citer = m.data_.cbegin();
          citer != m.data_.cend(); ++citer){
        ostr << *citer << ", ";
        dim_wgts_counter[dim_wgts_counter_idx]++;
        if(dim_wgts_counter[dim_wgts_counter_idx] >=
            dim_wgts[dim_wgts_counter_idx]){
          while(dim_wgts_counter[dim_wgts_counter_idx]>=dim_wgts[dim_wgts_counter_idx]){
            dim_wgts_counter[dim_wgts_counter_idx] = 0;
            ostr << "\n";
            if(dim_wgts_counter_idx > 0){
              dim_wgts_counter[dim_wgts_counter_idx - 1]++;
              dim_wgts_counter_idx--;
            }
            else{
              break;
            }
          }

          dim_wgts_counter_idx = dim_wgts.size() - 1;
        }
      }
      return ostr;
    }

  } // namespace MapperUtil
} // namespace ESMCI

#endif //ESMCI_Mat_H


// CONSTANTIN MIHAI - 336CA
// TEMA 2 ASC

#include <cblas.h>

#include "utils.h"

double* copy(int N, double *A) {

	int i, j;

	double *B = malloc(N * N * sizeof(double));
	for (i = 0; i < N; i++) {
		for (j = 0; j < N; j++) {
			B[i * N + j] = A[i * N + j];
		}
	}

	return B;
}

double* add_matrix(int N, double *A, double *B) {

	int i, j;

	double *C = calloc(N * N, sizeof(double));
	for (i = 0; i < N; i++) {
		for (j = 0; j < N; j++) {
			C[i * N + j] = A[i * N + j] + B[i * N + j];
		}
	}
	return C;
}

double* my_solver(int N, double *A, double *B) {

	/*
		A - matrice superior triunghiulara
		B - matrice normala
		Vrem C = B * A^t + A^2 * B
	*/

	/*
		CblasRowMajor - matrice stocata pe o singura linie
		CblasRight - facem B * A^t
		CblasUpper - matrice superior triunghiulara
		CblasTrans - folosim A^t
		CblasNoTrans - folosim A, nu A^t
		CblasNonUnit - A nu e matrice unitara
	*/

	// B2 <- B2 * A^t
	double *B2 = copy(N, B);
	cblas_dtrmm(CblasRowMajor, CblasRight, CblasUpper, CblasTrans, CblasNonUnit, N, N, 1.0, A, N, B2, N);

	// A2 <- A2 * A
	double *A2 = copy(N, A);
	cblas_dtrmm(CblasRowMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, N, N, 1.0, A, N, A2, N);

	// B <- A2 * B
	cblas_dtrmm(CblasRowMajor, CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, N, N, 1.0, A2, N, B, N);

	// result <- B2 + B
	double *result = add_matrix(N, B2, B);

	return result;
}


/* SO3blas.c */

// SO(3) Lie group implemented with GNU GSL
// cf. wikipedia, "Rotation_group_SO(3)#Infinitesimal_rotations"

// EY note : 20160519 Bizarrely, matrix multiplication isn't implemented
// in a straightforward manner with the gsl_matrix class;
// see blas (which I don't know what it is yet) and dgemm for the libraries

/*
  Compiling advice:
  gcc matrixio.c -lgsl -lgslcblas

 */

#include <stdio.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

// Function prototype
/* int commute(gsl_matrix_view, gsl_matrix_view); */

void commute1(void);

// global variables

double l_x[] = {  0,  0,  0,
		    0,  0, -1,
		    0,  1,  0};

double l_y[] = {  0,  0,  1,
		    0,  0,  0,
		   -1,  0, 0};

double l_z[] = {  0, -1,  0,
		    1,  0,  0,
		    0,  0,  0};
// cannot put gsl_matrix view commands in global; I need to check why:
// error: initializer element is not constant

int main(void)
{
  gsl_matrix_view L_x = gsl_matrix_view_array(l_x,3,3);
  gsl_matrix_view L_y = gsl_matrix_view_array(l_y,3,3);
  gsl_matrix_view L_z = gsl_matrix_view_array(l_z,3,3);

  printf("[ %g, %g, %g]\n", l_x[0], l_x[1], l_x[2] );
  printf("[ %g, %g, %g]\n", l_x[3], l_x[4], l_x[5] );
  printf("[ %g, %g, %g]\n\n", l_x[6], l_x[7], l_x[8] );

  printf("[ %g, %g, %g]\n", l_y[0], l_y[1], l_y[2] );
  printf("[ %g, %g, %g]\n", l_y[3], l_y[4], l_y[5] );
  printf("[ %g, %g, %g]\n\n", l_y[6], l_y[7], l_y[8] );

  printf("[ %g, %g, %g]\n", l_z[0], l_z[1], l_z[2] );
  printf("[ %g, %g, %g]\n", l_z[3], l_z[4], l_z[5] );
  printf("[ %g, %g, %g]\n\n", l_z[6], l_z[7], l_z[8] );

  double xy[] = { 0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00 };

  gsl_matrix_view XY = gsl_matrix_view_array(xy,3,3);

  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,
		 &L_x.matrix,&L_z.matrix,
		 0.0, &XY.matrix);

  printf("[ %g, %g, %g ]\n", xy[0], xy[1], xy[2]);
  printf("[ %g, %g, %g ]\n", xy[3], xy[4], xy[5]);
  printf("[ %g, %g, %g ]\n\n", xy[6], xy[7], xy[8]);

  commute1();

  return 0;
}

void commute1(void) {

  gsl_matrix_view L_x = gsl_matrix_view_array(l_x,3,3);
  gsl_matrix_view L_y = gsl_matrix_view_array(l_y,3,3);
  gsl_matrix_view L_z = gsl_matrix_view_array(l_z,3,3);

  double xy[] = { 0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00 };

  gsl_matrix_view XY = gsl_matrix_view_array(xy,3,3);

  double yx[] = { 0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00 };

  gsl_matrix_view YX = gsl_matrix_view_array(yx,3,3);

  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,
		 &L_x.matrix,&L_y.matrix,
		 0.0, &XY.matrix);

  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,
		 &L_y.matrix,&L_x.matrix,
		 0.0, &YX.matrix);

   printf("[ %g, %g, %g ]\n", yx[0], yx[1], yx[2]);
   printf("[ %g, %g, %g ]\n", yx[3], yx[4], yx[5]);
   printf("[ %g, %g, %g ]\n", yx[6], yx[7], yx[8]);

}

/*

int commute(gsl_matrix_view X, gsl_matrix_view Y)
{
  double xy[] = { 0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00 };

  double yx[] = { 0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00,
		  0.00, 0.00, 0.00 };

  double bracket[] = { 0.00, 0.00, 0.00,
		       0.00, 0.00, 0.00,
		       0.00, 0.00, 0.00 };

  gsl_matrix_view XY = gsl_matrix_view_array(xy,3,3);
  gsl_matrix_view YX = gsl_matrix_view_array(yx,3,3);
  gsl_matrix_view Bracket = gsl_matrix_view_array(bracket,3,3);

  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,1.0,
		 &X.matrix,&Y.matrix,
		 0.0, &XY.matrix);
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans,1.0,
		 &X.matrix,&Y.matrix,
		 0.0, &XY.matrix);
  gls_blas_dgemm(CblasNoTrans, CblasNoTrans,1.0,
		 &XY.matrix, &YX.matrix,
		 0.0, &Bracket.matrix);

  return 1;
}

*/


/* specfunc/trig.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_sf_trig.h>

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"

/* sinh(x) series
 * double-precision for |x| < 1.0
 */
inline
static
int
sinh_series(const double x, double * result)
{
  const double y = x*x;
  const double c0 = 1.0/6.0;
  const double c1 = 1.0/120.0;
  const double c2 = 1.0/5040.0;
  const double c3 = 1.0/362880.0;
  const double c4 = 1.0/39916800.0;
  const double c5 = 1.0/6227020800.0;
  const double c6 = 1.0/1307674368000.0;
  const double c7 = 1.0/355687428096000.0;
  *result = x*(1.0 + y*(c0+y*(c1+y*(c2+y*(c3+y*(c4+y*(c5+y*(c6+y*c7))))))));
  return GSL_SUCCESS;
}

/* cosh(x)-1 series
 * double-precision for |x| < 1.0
 */
inline
static
int
cosh_m1_series(const double x, double * result)
{
  const double y = x*x;
  const double c0 = 0.5;
  const double c1 = 1.0/24.0;
  const double c2 = 1.0/720.0;
  const double c3 = 1.0/40320.0;
  const double c4 = 1.0/3628800.0;
  const double c5 = 1.0/479001600.0;
  const double c6 = 1.0/87178291200.0;
  const double c7 = 1.0/20922789888000.0;
  const double c8 = 1.0/6402373705728000.0;
  *result = y*(c0+y*(c1+y*(c2+y*(c3+y*(c4+y*(c5+y*(c6+y*(c7+y*c8))))))));
  return GSL_SUCCESS;
}

/* Chebyshev expansion for f(t) = sinc((t+1)/2), -1 < t < 1
 */
static double sinc_data[17] = {
  1.133648177811747875422,
 -0.532677564732557348781,
 -0.068293048346633177859,
  0.033403684226353715020,
  0.001485679893925747818,
 -0.000734421305768455295,
 -0.000016837282388837229,
  0.000008359950146618018,
  0.000000117382095601192,
 -0.000000058413665922724,
 -0.000000000554763755743,
  0.000000000276434190426,
  0.000000000001895374892,
 -0.000000000000945237101,
 -0.000000000000004900690,
  0.000000000000002445383,
  0.000000000000000009925
};
static cheb_series sinc_cs = {
  sinc_data,
  16,
  -1, 1,
  10
};

/* Chebyshev expansion for f(t) = g((t+1)Pi/8), -1<t<1
 * g(x) = (sin(x)/x - 1)/(x*x)
 */
static double sin_data[12] = {
  -0.3295190160663511504173,
   0.0025374284671667991990,
   0.0006261928782647355874,
  -4.6495547521854042157541e-06,
  -5.6917531549379706526677e-07,
   3.7283335140973803627866e-09,
   3.0267376484747473727186e-10,
  -1.7400875016436622322022e-12,
  -1.0554678305790849834462e-13,
   5.3701981409132410797062e-16,
   2.5984137983099020336115e-17,
  -1.1821555255364833468288e-19
};
static cheb_series sin_cs = {
  sin_data,
  11,
  -1, 1,
  11
};

/* Chebyshev expansion for f(t) = g((t+1)Pi/8), -1<t<1
 * g(x) = (2(cos(x) - 1)/(x^2) + 1) / x^2
 */
static double cos_data[11] = {
  0.165391825637921473505668118136,
 -0.00084852883845000173671196530195,
 -0.000210086507222940730213625768083,
  1.16582269619760204299639757584e-6,
  1.43319375856259870334412701165e-7,
 -7.4770883429007141617951330184e-10,
 -6.0969994944584252706997438007e-11,
  2.90748249201909353949854872638e-13,
  1.77126739876261435667156490461e-14,
 -7.6896421502815579078577263149e-17,
 -3.7363121133079412079201377318e-18
};
static cheb_series cos_cs = {
  cos_data,
  10,
  -1, 1,
  10
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

/* I would have prefered just using the library sin() function.
 * But after some experimentation I decided that there was
 * no good way to understand the error; library sin() is just a black box.
 * So we have to roll our own.
 */
int
gsl_sf_sin_e(double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  {
    const double P1 = 7.85398125648498535156e-1;
    const double P2 = 3.77489470793079817668e-8;
    const double P3 = 2.69515142907905952645e-15;

    const double sgn_x = GSL_SIGN(x);
    const double abs_x = fabs(x);

    if(abs_x < GSL_ROOT4_DBL_EPSILON) {
      const double x2 = x*x;
      result->val = x * (1.0 - x2/6.0);
      result->err = fabs(x*x2*x2 / 100.0);
      return GSL_SUCCESS;
    }
    else {
      double sgn_result = sgn_x;
      double y = floor(abs_x/(0.25*M_PI));
      int octant = y - ldexp(floor(ldexp(y,-3)),3);
      int stat_cs;
      double z;

      if(GSL_IS_ODD(octant)) {
        octant += 1;
        octant &= 07;
        y += 1.0;
      }

      if(octant > 3) {
        octant -= 4;
        sgn_result = -sgn_result;
      }

      z = ((abs_x - y * P1) - y * P2) - y * P3;

      if(octant == 0) {
        gsl_sf_result sin_cs_result;
        const double t = 8.0*fabs(z)/M_PI - 1.0;
        stat_cs = cheb_eval_e(&sin_cs, t, &sin_cs_result);
        result->val = z * (1.0 + z*z * sin_cs_result.val);
      }
      else { /* octant == 2 */
        gsl_sf_result cos_cs_result;
        const double t = 8.0*fabs(z)/M_PI - 1.0;
        stat_cs = cheb_eval_e(&cos_cs, t, &cos_cs_result);
        result->val = 1.0 - 0.5*z*z * (1.0 - z*z * cos_cs_result.val);
      }

      result->val *= sgn_result;

      if(abs_x > 1.0/GSL_DBL_EPSILON) {
        result->err = fabs(result->val);
      }
      else if(abs_x > 100.0/GSL_SQRT_DBL_EPSILON) {
        result->err = 2.0 * abs_x * GSL_DBL_EPSILON * fabs(result->val);
      }
      else if(abs_x > 0.1/GSL_SQRT_DBL_EPSILON) {
        result->err = 2.0 * GSL_SQRT_DBL_EPSILON * fabs(result->val);
      }
      else {
        result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      }

      return stat_cs;
    }
  }
}

int
gsl_sf_cos_e(double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  {
    const double P1 = 7.85398125648498535156e-1;
    const double P2 = 3.77489470793079817668e-8;
    const double P3 = 2.69515142907905952645e-15;

    const double abs_x = fabs(x);

    if(abs_x < GSL_ROOT4_DBL_EPSILON) {
      const double x2 = x*x;
      result->val = 1.0 - 0.5*x2;
      result->err = fabs(x2*x2/12.0);
      return GSL_SUCCESS;
    }
    else {
      double sgn_result = 1.0;
      double y = floor(abs_x/(0.25*M_PI));
      int octant = y - ldexp(floor(ldexp(y,-3)),3);
      int stat_cs;
      double z;

      if(GSL_IS_ODD(octant)) {
        octant += 1;
        octant &= 07;
        y += 1.0;
      }

      if(octant > 3) {
        octant -= 4;
        sgn_result = -sgn_result;
      }

      if(octant > 1) {
        sgn_result = -sgn_result;
      }

      z = ((abs_x - y * P1) - y * P2) - y * P3;

      if(octant == 0) {
        gsl_sf_result cos_cs_result;
        const double t = 8.0*fabs(z)/M_PI - 1.0;
        stat_cs = cheb_eval_e(&cos_cs, t, &cos_cs_result);
        result->val = 1.0 - 0.5*z*z * (1.0 - z*z * cos_cs_result.val);
      }
      else { /* octant == 2 */
        gsl_sf_result sin_cs_result;
        const double t = 8.0*fabs(z)/M_PI - 1.0;
        stat_cs = cheb_eval_e(&sin_cs, t, &sin_cs_result);
        result->val = z * (1.0 + z*z * sin_cs_result.val);
      }

      result->val *= sgn_result;

      if(abs_x > 1.0/GSL_DBL_EPSILON) {
        result->err = fabs(result->val);
      }
      else if(abs_x > 100.0/GSL_SQRT_DBL_EPSILON) {
        result->err = 2.0 * abs_x * GSL_DBL_EPSILON * fabs(result->val);
      }
      else if(abs_x > 0.1/GSL_SQRT_DBL_EPSILON) {
        result->err = 2.0 * GSL_SQRT_DBL_EPSILON * fabs(result->val);
      }
      else {
        result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      }

      return stat_cs;
    }
  }
}

int
gsl_sf_hypot_e(const double x, const double y, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x == 0.0 && y == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    const double a = fabs(x);
    const double b = fabs(y);
    const double min = GSL_MIN_DBL(a,b);
    const double max = GSL_MAX_DBL(a,b);
    const double rat = min/max;
    const double root_term = sqrt(1.0 + rat*rat);

    if(max < GSL_DBL_MAX/root_term) {
      result->val = max * root_term;
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_SUCCESS;
    }
    else {
      OVERFLOW_ERROR(result);
    }
  }
}

int
gsl_sf_complex_sin_e(const double zr, const double zi,
                        gsl_sf_result * szr, gsl_sf_result * szi)
{
  /* CHECK_POINTER(szr) */
  /* CHECK_POINTER(szi) */

  if(fabs(zi) < 1.0) {
    double ch_m1, sh;
    sinh_series(zi, &sh);
    cosh_m1_series(zi, &ch_m1);
    szr->val = sin(zr)*(ch_m1 + 1.0);
    szi->val = cos(zr)*sh;
    szr->err = 2.0 * GSL_DBL_EPSILON * fabs(szr->val);
    szi->err = 2.0 * GSL_DBL_EPSILON * fabs(szi->val);
    return GSL_SUCCESS;
  }
  else if(fabs(zi) < GSL_LOG_DBL_MAX) {
    double ex = exp(zi);
    double ch = 0.5*(ex+1.0/ex);
    double sh = 0.5*(ex-1.0/ex);
    szr->val = sin(zr)*ch;
    szi->val = cos(zr)*sh;
    szr->err = 2.0 * GSL_DBL_EPSILON * fabs(szr->val);
    szi->err = 2.0 * GSL_DBL_EPSILON * fabs(szi->val);
    return GSL_SUCCESS;
  }
  else {
    OVERFLOW_ERROR_2(szr, szi);
  }
}

int
gsl_sf_complex_cos_e(const double zr, const double zi,
                        gsl_sf_result * czr, gsl_sf_result * czi)
{
  /* CHECK_POINTER(czr) */
  /* CHECK_POINTER(czi) */

  if(fabs(zi) < 1.0) {
    double ch_m1, sh;
    sinh_series(zi, &sh);
    cosh_m1_series(zi, &ch_m1);
    czr->val =  cos(zr)*(ch_m1 + 1.0);
    czi->val = -sin(zr)*sh;
    czr->err = 2.0 * GSL_DBL_EPSILON * fabs(czr->val);
    czi->err = 2.0 * GSL_DBL_EPSILON * fabs(czi->val);
    return GSL_SUCCESS;
  }
  else if(fabs(zi) < GSL_LOG_DBL_MAX) {
    double ex = exp(zi);
    double ch = 0.5*(ex+1.0/ex);
    double sh = 0.5*(ex-1.0/ex);
    czr->val =  cos(zr)*ch;
    czi->val = -sin(zr)*sh;
    czr->err = 2.0 * GSL_DBL_EPSILON * fabs(czr->val);
    czi->err = 2.0 * GSL_DBL_EPSILON * fabs(czi->val);
    return GSL_SUCCESS;
  }
  else {
    OVERFLOW_ERROR_2(czr,czi);
  }
}

int
gsl_sf_complex_logsin_e(const double zr, const double zi,
                           gsl_sf_result * lszr, gsl_sf_result * lszi)
{
  /* CHECK_POINTER(lszr) */
  /* CHECK_POINTER(lszi) */

  if(zi > 60.0) {
    lszr->val = -M_LN2 + zi;
    lszi->val =  0.5*M_PI - zr;
    lszr->err = 2.0 * GSL_DBL_EPSILON * fabs(lszr->val);
    lszi->err = 2.0 * GSL_DBL_EPSILON * fabs(lszi->val);
  }
  else if(zi < -60.0) {
    lszr->val = -M_LN2 - zi;
    lszi->val = -0.5*M_PI + zr;
    lszr->err = 2.0 * GSL_DBL_EPSILON * fabs(lszr->val);
    lszi->err = 2.0 * GSL_DBL_EPSILON * fabs(lszi->val);
  }
  else {
    gsl_sf_result sin_r, sin_i;
    int status;
    gsl_sf_complex_sin_e(zr, zi, &sin_r, &sin_i); /* ok by construction */
    status = gsl_sf_complex_log_e(sin_r.val, sin_i.val, lszr, lszi);
    if(status == GSL_EDOM) {
      DOMAIN_ERROR_2(lszr, lszi);
    }
  }
  return gsl_sf_angle_restrict_symm_e(&(lszi->val));
}

int
gsl_sf_lnsinh_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(fabs(x) < 1.0) {
    double eps;
    sinh_series(x, &eps);
    result->val = log(eps);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < -0.5*GSL_LOG_DBL_EPSILON) {
    result->val = x + log(0.5*(1.0 - exp(-2.0*x)));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    result->val = -M_LN2 + x;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int gsl_sf_lncosh_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(fabs(x) < 1.0) {
    double eps;
    cosh_m1_series(x, &eps);
    return gsl_sf_log_1plusx_e(eps, result);
  }
  else if(x < -0.5*GSL_LOG_DBL_EPSILON) {
    result->val = x + log(0.5*(1.0 + exp(-2.0*x)));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    result->val = -M_LN2 + x;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

/*
inline int gsl_sf_sincos_e(const double theta, double * s, double * c)
{
  double tan_half = tan(0.5 * theta);
  double den = 1. + tan_half*tan_half;
  double cos_theta = (1.0 - tan_half*tan_half) / den;
  double sin_theta = 2.0 * tan_half / den;
}
*/

int
gsl_sf_polar_to_rect(const double r, const double theta,
                          gsl_sf_result * x, gsl_sf_result * y)
{
  double t   = theta;
  int status = gsl_sf_angle_restrict_symm_e(&t);
  double c = cos(t);
  double s = sin(t);
  x->val = r * cos(t);
  y->val = r * sin(t);
  x->err  = r * fabs(s * GSL_DBL_EPSILON * t);
  x->err += 2.0 * GSL_DBL_EPSILON * fabs(x->val);
  y->err  = r * fabs(c * GSL_DBL_EPSILON * t);
  y->err += 2.0 * GSL_DBL_EPSILON * fabs(y->val);
  return status;
}

int
gsl_sf_rect_to_polar(const double x, const double y,
                          gsl_sf_result * r, gsl_sf_result * theta)
{
  int stat_h = gsl_sf_hypot_e(x, y, r);
  if(r->val > 0.0) {
    theta->val = atan2(y, x);
    theta->err = 2.0 * GSL_DBL_EPSILON * fabs(theta->val);
    return stat_h;
  }
  else {
    DOMAIN_ERROR(theta);
  }
}

int gsl_sf_angle_restrict_symm_err_e(const double theta, gsl_sf_result * result)
{
  /* synthetic extended precision constants */
  const double P1 = 4 * 7.8539812564849853515625e-01;
  const double P2 = 4 * 3.7748947079307981766760e-08;
  const double P3 = 4 * 2.6951514290790594840552e-15;
  const double TwoPi = 2*(P1 + P2 + P3);

  const double y = GSL_SIGN(theta) * 2 * floor(fabs(theta)/TwoPi);
  double r = ((theta - y*P1) - y*P2) - y*P3;

  if(r >  M_PI) { r = (((r-2*P1)-2*P2)-2*P3); }  /* r-TwoPi */
  else if (r < -M_PI) r = (((r+2*P1)+2*P2)+2*P3); /* r+TwoPi */

  result->val = r;

  if(fabs(theta) > 0.0625/GSL_DBL_EPSILON) {
    result->val = GSL_NAN;
    result->err = GSL_NAN;
    GSL_ERROR ("error", GSL_ELOSS);
  }
  else if(fabs(theta) > 0.0625/GSL_SQRT_DBL_EPSILON) {
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val - theta);
    return GSL_SUCCESS;
  }
  else {
    double delta = fabs(result->val - theta);
    result->err = 2.0 * GSL_DBL_EPSILON * ((delta < M_PI) ? delta : M_PI);
    return GSL_SUCCESS;
  }
}

int gsl_sf_angle_restrict_pos_err_e(const double theta, gsl_sf_result * result)
{
  /* synthetic extended precision constants */
  const double P1 = 4 * 7.85398125648498535156e-01;
  const double P2 = 4 * 3.77489470793079817668e-08;
  const double P3 = 4 * 2.69515142907905952645e-15;
  const double TwoPi = 2*(P1 + P2 + P3);

  const double y = 2*floor(theta/TwoPi);

  double r = ((theta - y*P1) - y*P2) - y*P3;

  if(r > TwoPi) {r = (((r-2*P1)-2*P2)-2*P3); }  /* r-TwoPi */
  else if (r < 0) { /* may happen due to FP rounding */
    r = (((r+2*P1)+2*P2)+2*P3); /* r+TwoPi */
  }

  result->val = r;

  if(fabs(theta) > 0.0625/GSL_DBL_EPSILON) {
    result->val = GSL_NAN;
    result->err = fabs(result->val);
    GSL_ERROR ("error", GSL_ELOSS);
  }
  else if(fabs(theta) > 0.0625/GSL_SQRT_DBL_EPSILON) {
    result->err = GSL_DBL_EPSILON * fabs(result->val - theta);
    return GSL_SUCCESS;
  }
  else {
    double delta = fabs(result->val - theta);
    result->err = 2.0 * GSL_DBL_EPSILON * ((delta < M_PI) ? delta : M_PI);
    return GSL_SUCCESS;
  }
}

int gsl_sf_angle_restrict_symm_e(double * theta)
{
  gsl_sf_result r;
  int stat = gsl_sf_angle_restrict_symm_err_e(*theta, &r);
  *theta = r.val;
  return stat;
}

int gsl_sf_angle_restrict_pos_e(double * theta)
{
  gsl_sf_result r;
  int stat = gsl_sf_angle_restrict_pos_err_e(*theta, &r);
  *theta = r.val;
  return stat;
}

int gsl_sf_sin_err_e(const double x, const double dx, gsl_sf_result * result)
{
  int stat_s = gsl_sf_sin_e(x, result);
  result->err += fabs(cos(x) * dx);
  result->err += GSL_DBL_EPSILON * fabs(result->val);
  return stat_s;
}

int gsl_sf_cos_err_e(const double x, const double dx, gsl_sf_result * result)
{
  int stat_c = gsl_sf_cos_e(x, result);
  result->err += fabs(sin(x) * dx);
  result->err += GSL_DBL_EPSILON * fabs(result->val);
  return stat_c;
}

#if 0
int
gsl_sf_sin_pi_x_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(-100.0 < x && x < 100.0) {
    result->val = sin(M_PI * x) / (M_PI * x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double N = floor(x + 0.5);
    const double f = x - N;

    if(N < INT_MAX && N > INT_MIN) {
      /* Make it an integer if we can. Saves another
       * call to floor().
       */
      const int intN    = (int)N;
      const double sign = ( GSL_IS_ODD(intN) ? -1.0 : 1.0 );
      result->val = sign * sin(M_PI * f);
      result->err = GSL_DBL_EPSILON * fabs(result->val);
    }
    else if(N > 2.0/GSL_DBL_EPSILON || N < -2.0/GSL_DBL_EPSILON) {
      /* All integer-valued floating point numbers
       * bigger than 2/eps=2^53 are actually even.
       */
      result->val = 0.0;
      result->err = 0.0;
    }
    else {
      const double resN = N - 2.0*floor(0.5*N); /* 0 for even N, 1 for odd N */
      const double sign = ( fabs(resN) > 0.5 ? -1.0 : 1.0 );
      result->val = sign * sin(M_PI*f);
      result->err = GSL_DBL_EPSILON * fabs(result->val);
    }

    return GSL_SUCCESS;
  }
}
#endif

int gsl_sf_sinc_e(double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  {
    const double ax = fabs(x);

    if(ax < 0.8) {
      /* Do not go to the limit of the fit since
       * there is a zero there and the Chebyshev
       * accuracy will go to zero.
       */
      return cheb_eval_e(&sinc_cs, 2.0*ax-1.0, result);
    }
    else if(ax < 100.0) {
      /* Small arguments are no problem.
       * We trust the library sin() to
       * roughly machine precision.
       */
      result->val = sin(M_PI * ax)/(M_PI * ax);
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_SUCCESS;
    }
    else {
      /* Large arguments must be handled separately.
       */
      const double r = M_PI*ax;
      gsl_sf_result s;
      int stat_s = gsl_sf_sin_e(r, &s);
      result->val = s.val/r;
      result->err = s.err/r + 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return stat_s;
    }
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_sin(const double x)
{
  EVAL_RESULT(gsl_sf_sin_e(x, &result));
}

double gsl_sf_cos(const double x)
{
  EVAL_RESULT(gsl_sf_cos_e(x, &result));
}

double gsl_sf_hypot(const double x, const double y)
{
  EVAL_RESULT(gsl_sf_hypot_e(x, y, &result));
}

double gsl_sf_lnsinh(const double x)
{
  EVAL_RESULT(gsl_sf_lnsinh_e(x, &result));
}

double gsl_sf_lncosh(const double x)
{
  EVAL_RESULT(gsl_sf_lncosh_e(x, &result));
}

double gsl_sf_angle_restrict_symm(const double theta)
{
  double result = theta;
  EVAL_DOUBLE(gsl_sf_angle_restrict_symm_e(&result));
}

double gsl_sf_angle_restrict_pos(const double theta)
{
  double result = theta;
  EVAL_DOUBLE(gsl_sf_angle_restrict_pos_e(&result));
}

#if 0
double gsl_sf_sin_pi_x(const double x)
{
  EVAL_RESULT(gsl_sf_sin_pi_x_e(x, &result));
}
#endif

double gsl_sf_sinc(const double x)
{
  EVAL_RESULT(gsl_sf_sinc_e(x, &result));
}


/*
 * splinepdf.c: Routines to efficiently sample from a dimension of an
 *    N-dimensional spline surface. Uses a Metropolis-Hastings Markov Chain
 *    sampler.
 */

#include <math.h>
#include <gsl/gsl_rng.h>

#include "photospline/splinetable.h"
#include "photospline/bspline.h"
#include "photospline/splinepdf.h"

void logsplinepdf_n_sample(double *result, int results, int burnin,
    double *coords, int dim, struct splinetable *table, int derivatives,
    double (* proposal)(void*), double (* proposal_pdf)(double, double, void*),
    void* proposal_info, const gsl_rng *rng)
{
	int i;
	int centers[table->ndim];
	double val, lastval;
	double lastlogpdf = -INFINITY, logpdf;
	double lastproppdf, proppdf;
	double odds;
	double mint, maxt;
#ifdef DEBUG
	int accepted=0;
#endif

	/* Find the boundaries by choosing the first and last points
	 * with full support */
	mint = table->extents[dim][0];
	maxt = table->extents[dim][1];

	do {
		/*
		 * Get a starting point from the proposal distribution,
		 * making sure the PDF at the starting point is finite
		 * to avoid numerical problems.
		 */

		coords[dim] = lastval = (*proposal)(proposal_info);

		lastproppdf = (*proposal_pdf)(lastval,lastval,proposal_info);
		if (tablesearchcenters(table, coords, centers) != 0) {
			lastval = mint-1;
			continue;
		}
		lastlogpdf = ndsplineeval(table, coords, centers, 0);
		if (derivatives)
			lastlogpdf += log(ndsplineeval(table, coords, centers,
			    derivatives));
	} while (isnan(lastlogpdf) || lastval < mint || lastval > maxt);

	for (i = -burnin; i/burnin < results; i++) {
		coords[dim] = val = (*proposal)(proposal_info);

		/*
		 * If we ended up outside the table, reject the sample
		 * and try again
		 */
		if (val > maxt || val < mint ||
		    tablesearchcenters(table, coords, centers) != 0)
			goto reject;

		logpdf = ndsplineeval(table, coords, centers, 0);
		if (derivatives)
			logpdf += log(ndsplineeval(table, coords, centers,
			    derivatives));
		proppdf = (*proposal_pdf)(val,lastval,proposal_info);
		odds = exp(logpdf - lastlogpdf);
		odds *= lastproppdf/proppdf;

		if (odds > 1. || gsl_rng_uniform(rng) < odds) {
			/* Accept this value */
			lastval = val;
			lastlogpdf = logpdf;
			lastproppdf = proppdf;
		#ifdef DEBUG
			accepted++;
		#endif
		}

		/*
		 * Lastval has whatever we decided on now. If the burn-in
		 * period has elapsed, write it to output array.
		 *
		 * NB: This code is used in both the accept and reject
		 * cases.
		 */

	     reject:
		if (i >= 0 && (i % burnin) == 0)
			result[i/burnin] = lastval;
	}
#ifdef DEBUG
	printf("Efficiency: %e\n", (double)(accepted)/(double)(i));
#endif
}

void splinepdf_n_sample(double *result, int results, int burnin,
    double *coords, int dim, struct splinetable *table, int derivatives,
    double (* proposal)(void*), double (* proposal_pdf)(double, double, void*),
    void *proposal_info, const gsl_rng *rng)
{
	int i;
	int centers[table->ndim];
	double val, lastval;
	double lastpdf, pdf;
	double lastproppdf, proppdf;
	double odds;
	double mint, maxt;
#ifdef DEBUG
	int accepted=0;
#endif

	/* Find the boundaries by choosing the first and last points
	 * with full support */
	mint = table->extents[dim][0];
	maxt = table->extents[dim][1];

	/* Get a fully-supported starting point from the proposal distribution. */
	do {
		coords[dim] = lastval = (*proposal)(proposal_info);
	} while (lastval < mint || lastval > maxt ||
	    tablesearchcenters(table, coords, centers) != 0);

	lastproppdf = (*proposal_pdf)(lastval,lastval,proposal_info);
	lastpdf = ndsplineeval(table, coords, centers, derivatives);

	for (i = -burnin; i/burnin < results; i++) {
		coords[dim] = val = (*proposal)(proposal_info);

		/*
		 * If we ended up outside the table, reject the sample
		 * and try again
		 */
		if (val > maxt || val < mint ||
		    tablesearchcenters(table, coords, centers) != 0)
			goto reject;

		pdf = ndsplineeval(table, coords, centers, derivatives);
		proppdf = (*proposal_pdf)(val,lastval,proposal_info);
		odds = pdf/lastpdf;
		odds *= lastproppdf/proppdf;

		if (odds > 1. || gsl_rng_uniform(rng) < odds) {
			/* Accept this value */
			lastval = val;
			lastpdf = pdf;
			lastproppdf = proppdf;
			#ifdef DEBUG
				accepted++;
			#endif
		}

		/*
		 * Lastval has whatever we decided on now. If the burn-in
		 * period has elapsed, write it to output array.
		 *
		 * NB: This code is used in both the accept and reject
		 * cases.
		 */

	     reject:
		if (i >= 0 && (i % burnin) == 0)
			result[i / burnin] = lastval;
	}
	#ifdef DEBUG
		printf("Efficiency: %e\n", (double)(accepted)/(double)(i));
	#endif
}