#include <gsl/gsl_blas.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>
#include "nd-random.h"
#include "utils.h"

#define INTEGRATION_EPSABS 1e-3
#define INTEGRATION_EPSREL 1e-3

unsigned int shifted_gaussian_random_vector
(const gsl_rng* r, const gsl_vector* mean, double theta_max, double standard_deviation, gsl_vector* x)
{
  for (int i=0; i<x->size; i++)
    gsl_vector_set(x, i, gsl_ran_gaussian(r, standard_deviation));
  gsl_vector_add(x, mean);
  gsl_blas_dscal(1/gsl_blas_dnrm2(x), x);
  if (angle_between_vectors(x, mean) > theta_max)
    return 1 + shifted_gaussian_random_vector(r, mean, theta_max, standard_deviation, x);
  else return 1;
}

double shifted_gaussian_pdf (double theta, double mean, double theta_max,
                             double standard_deviation, unsigned int dimension, gsl_integration_workspace* w)
{
  // NOTE: Due to the rejection sampling, the returned result is only
  // proportional to the actual PDF.
  if (theta > theta_max) return 0;
  
  double projection = mean*cos(theta);
  double integrand (double r, void* params)
  {
    return gsl_pow_uint(r, dimension - 1)
      * gaussian_pdf((r - projection) / standard_deviation);
  }
  
  gsl_function gsl_integrand = {&integrand};
  double result, error;
  /* gsl_integration_qagiu(&gsl_integrand, 0, INTEGRATION_EPSABS, INTEGRATION_EPSREL, */
  /*                       w->limit, w, &result, &error); */
  double rmax = 0.5 * (projection + sqrt(gsl_pow_2(projection) + 4*(dimension-1)*gsl_pow_2(standard_deviation)));
  double halfwidth = 3 * standard_deviation;
  gsl_integration_qag(&gsl_integrand,
                      rmax - halfwidth < 0 ? 0 : rmax - halfwidth,
                      rmax + halfwidth,
                      INTEGRATION_EPSABS, INTEGRATION_EPSREL,
                      w->limit, GSL_INTEG_GAUSS41, w, &result, &error);
  return result
    * gaussian_pdf(sqrt(gsl_pow_2(mean) - gsl_pow_2(projection)) / standard_deviation)
    / gsl_pow_uint(M_SQRT2*sqrt(M_PI), dimension - 2)
    / gsl_pow_uint(standard_deviation, dimension);
}