#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_sf_gamma.h>
#include "utils.h"

// This function exists so that guile code can access the value of
// M_PI as provided by <gsl_math.h>.
double pi ()
{
  /**
   @return Value of pi
  **/
  return M_PI;
}

double volume_of_ball (unsigned int dimension)
{
  /**
     @param dimension Dimension of the ball (2 for a circle, 3 for a sphere, etc.)
     @return Volume of the unit ball
   **/
  return exp(ln_volume_of_ball(dimension));
}

double ln_volume_of_ball (unsigned int dimension)
{
  /**
     @param dimension Dimension of the ball (2 for a circle, 3 for a sphere, etc.)
     @return Natural logarithm of the volume of the unit ball
   **/
  return (dimension/2.0)*M_LNPI - gsl_sf_lngamma(1 + dimension/2.0);
}

double surface_area_of_ball (unsigned int dimension)
{
  /**
     @param dimension Dimension of the ball (2 for a circle, 3 for a sphere, etc.)
     @return Surface area of the unit ball
   **/
  return dimension * volume_of_ball(dimension);
}

double ln_surface_area_of_ball (unsigned int dimension)
{
  /**
     @param dimension Dimension of the ball (2 for a circle, 3 for a sphere, etc.)
     @return Natural logarithm of the surface area of the unit ball
   **/
  return log(dimension) + ln_volume_of_ball(dimension);
}

double lower_incomplete_gamma (double s, double x)
{
  return gsl_sf_gamma(s) * gsl_sf_gamma_inc_P(s, x);
}

double angle_between_vectors (const gsl_vector* x, const gsl_vector* y)
{
  /**
     @param x Vector 1
     @param y Vector 2
     @return Angle between the two vectors in the range [0, pi]
  **/
  double dot_product;
  gsl_blas_ddot(x, y, &dot_product);
  // TODO: Is this a valid floating point comparison?
  if (dot_product != 0)
    return acos(dot_product / gsl_blas_dnrm2(x) / gsl_blas_dnrm2(y));
  else return 0;
}

double dot_product (const gsl_vector* x, const gsl_vector* y)
{
  /**
     @param x Vector 1
     @param y Vector 2
     @return Dot product between the two vectors
  **/
  double result;
  gsl_blas_ddot(x, y, &result);
  return result;
}

double gaussian_pdf (double x)
{
  /**
     @param x
     @return exp(-x^2/2) / sqrt(2*pi)
   **/
  return gsl_ran_gaussian_pdf(x, 1.0);
}

double gaussian_cdf (double x)
{
  /**
     @param x
     @return int_{-inf}^x gaussian_pdf(t) dt
   **/
  return 0.5*(1 + gsl_sf_erf(x/M_SQRT2));
}

double rerror (double approx, double exact)
{
  /**
     @param approx Approximate value
     @param exact Exact value
     @return Relative error
  **/
  return fabs(1 - approx/exact);
}

#define BISECTION_EPSABS 0
#define BISECTION_EPSREL 1e-6
#define BISECTION_MAX_ITERATIONS 1000
#define BISECTION_FUNCTION_EPSABS 1e-9
double bisection (gsl_function* f, double a, double b)
{
  const gsl_root_fsolver_type* T = gsl_root_fsolver_bisection;
  gsl_root_fsolver* s = gsl_root_fsolver_alloc(T);
  double solution;
  int status;

  void error_handler (const char* reason, const char* file, int line, int gsl_errno)
  {
    fprintf(stderr, "Bisection error handler invoked.\n");
    fprintf(stderr, "f(%g) = %g\n", a, GSL_FN_EVAL(f, a));
    fprintf(stderr, "f(%g) = %g\n", b, GSL_FN_EVAL(f, b));
    fprintf(stderr, "gsl: %s:%d: ERROR: %s\n", file, line, reason);
    abort();
  }

  gsl_error_handler_t* old_handler = gsl_set_error_handler(error_handler);
  gsl_root_fsolver_set(s, f, a, b);
  do {
    status = gsl_root_fsolver_iterate (s);
    solution = gsl_root_fsolver_root (s);
    a = gsl_root_fsolver_x_lower (s);
    b = gsl_root_fsolver_x_upper (s);
    status = gsl_root_test_interval (a, b, BISECTION_EPSABS, BISECTION_EPSREL);
  } while (status == GSL_CONTINUE);
  gsl_root_fsolver_free(s);
  gsl_set_error_handler(old_handler);
  return solution;
}

double bisection_rlimit (gsl_function* f, double a, double b)
{
  int sign = SIGNUM(GSL_FN_EVAL(f, a));
  for (int i=0; i<BISECTION_MAX_ITERATIONS; i++)
    if (sign*GSL_FN_EVAL(f, b) > 0) b *= 2;
    else return b;
  fprintf(stderr, "Bisection bracketing failed\n");
  exit(1);
}
#undef BISECTION_EPSABS
#undef BISECTION_EPSREL
#undef BISECTION_MAX_ITERATIONS
#undef BISECTION_FUNCTION_EPSABS