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

static double beta_inc_unnormalized (double a, double b, double x);
static double incomplete_wallis_integral (double theta, unsigned int m);

void random_direction_vector (const gsl_rng* r, gsl_vector* x)
{
  gsl_ran_dir_nd(r, x->size, x->data);
}

static void rotate_from_nth_canonical (gsl_vector* x, const gsl_vector* orient)
{
  const size_t n = x->size;
  double xn = gsl_vector_get(x, n - 1);
  double mun = gsl_vector_get(orient, n - 1);
  gsl_vector_const_view orient_sub = gsl_vector_const_subvector(orient, 0, n - 1);
  double b = gsl_blas_dnrm2(&orient_sub.vector);
  double a = (dot_product(orient, x) - xn*mun) / b;
  double c = mun, s = sqrt(1 - gsl_pow_2(c));
  gsl_blas_daxpy((xn*s + a*(c - 1))/b, orient, x);
  gsl_vector_set(x, n - 1, gsl_vector_get(x, n - 1)
                 + xn*(c - 1) - a*s
                 - mun*(xn*s + a*(c - 1))/b);
}

/* TODO: There is an edge case when mean is the (n-1)th canonical
   basis vector. Fix it.
*/
void hollow_cone_random_vector (const gsl_rng* r, const gsl_vector* mean, double theta_min, double theta_max, gsl_vector* x)
{
  unsigned int n = x->size;
  gsl_ran_dir_nd(r, n - 1, x->data);

  // Generate random vector around the nth canonical basis vector
  double omega_min = planar_angle_to_solid_angle(theta_min, n);
  double omega_max = planar_angle_to_solid_angle(theta_max, n);
  gsl_vector_scale(x, cos(theta_max) * tan(solid_angle_to_planar_angle(gsl_ran_flat(r, omega_min, omega_max), n)));
  gsl_vector_set(x, n - 1, cos(theta_max));
  gsl_vector_scale(x, 1.0/gsl_blas_dnrm2(x));

  // Rotate to arbitrary basis
  rotate_from_nth_canonical(x, mean);
}

void subsampling_random_vector (const gsl_rng* r, const gsl_vector* mean, double theta_max, gsl_vector* x)
{
  hollow_cone_random_vector(r, mean, 0, theta_max, x);
}

static double beta_inc_unnormalized (double a, double b, double x)
{
  return gsl_sf_beta_inc(a, b, x) * gsl_sf_beta(a, b);
}

static double incomplete_wallis_integral (double theta, unsigned int m)
{
  /**
     @param theta 0 < theta < pi
     @param m
     @return \int_0^\theta \sin^m x dx
   **/
  if ((theta < 0) || (theta > M_PI))
    GSL_ERROR("Incomplete Wallis integral only allows theta in [0,pi]", GSL_EDOM);
  if (theta < M_PI_2)
    return 0.5 * beta_inc_unnormalized((m+1)/2.0, 0.5, gsl_pow_2(sin(theta)));
  else
    return 0.5 * (gsl_sf_beta((m+1)/2.0, 0.5) + beta_inc_unnormalized(0.5, (m+1)/2.0, gsl_pow_2(cos(theta))));
}

double planar_angle_to_solid_angle (double planar_angle, unsigned int dimension)
{
  return 2 * pow(M_PI, (dimension - 1)/2.0)
    * incomplete_wallis_integral(planar_angle, dimension - 2)
    / gsl_sf_gamma((dimension - 1)/2.0);
}

double solid_angle_to_planar_angle (double solid_angle, unsigned int dimension)
{
  double f (double planar_angle, void* params)
  {
    return planar_angle_to_solid_angle(planar_angle, dimension) - solid_angle;
  }

  gsl_function gsl_f = {&f};
  /* if (fabs(GSL_FN_EVAL(&gsl_f, 0))/surface_area_of_ball(dimension) < BISECTION_FUNCTION_EPSABS) */
  /*   return 0; */
  /* else if (fabs(GSL_FN_EVAL(&gsl_f, M_PI))/surface_area_of_ball(dimension) < BISECTION_FUNCTION_EPSABS) */
  /*   return M_PI; */
  /* else return bisection(&gsl_f, 0, M_PI); */
  if (solid_angle == 0)
    return 0;
  else if (solid_angle == surface_area_of_ball(dimension))
    return M_PI;
  else return bisection(&gsl_f, 0, M_PI);
}