/* monte/miser.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Michael Booth
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* MISER.  Based on the algorithm described in the following article,

   W.H. Press, G.R. Farrar, "Recursive Stratified Sampling for
   Multidimensional Monte Carlo Integration", Computers in Physics,
   v4 (1990), pp190-195.

*/

/* Author: MJB */
/* Modified by Brian Gough 12/2000 */

#include <config.h>
#include <math.h>
#include <stdlib.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_miser.h>

static int
estimate_corrmc (gsl_monte_function * f,
		 const double xl[], const double xu[],
		 size_t dim, size_t calls,
		 gsl_rng * r,
		 gsl_monte_miser_state * state,
		 double *result, double *abserr,
		 const double xmid[], double sigma_l[], double sigma_r[]);

int
gsl_monte_miser_integrate (gsl_monte_function * f,
			   const double xl[], const double xu[],
			   size_t dim, size_t calls,
			   gsl_rng * r,
			   gsl_monte_miser_state * state,
			   double *result, double *abserr)
{
  size_t n, estimate_calls, calls_l, calls_r;
  const size_t min_calls = state->min_calls;
  size_t i;
  size_t i_bisect;
  int found_best;

  double res_est = 0, err_est = 0;
  double res_r = 0, err_r = 0, res_l = 0, err_l = 0;
  double xbi_l, xbi_m, xbi_r, s;

  double vol;
  double weight_l, weight_r;

  double *x = state->x;
  double *xmid = state->xmid;
  double *sigma_l = state->sigma_l, *sigma_r = state->sigma_r;

  if (dim != state->dim)
    {
      GSL_ERROR ("number of dimensions must match allocated size", GSL_EINVAL);
    }

  for (i = 0; i < dim; i++)
    {
      if (xu[i] <= xl[i])
	{
	  GSL_ERROR ("xu must be greater than xl", GSL_EINVAL);
	}

      if (xu[i] - xl[i] > GSL_DBL_MAX)
	{
	  GSL_ERROR ("Range of integration is too large, please rescale",
		     GSL_EINVAL);
	}
    }

  if (state->alpha < 0)
    {
      GSL_ERROR ("alpha must be non-negative", GSL_EINVAL);
    }

  /* Compute volume */

  vol = 1;

  for (i = 0; i < dim; i++)
    {
      vol *= xu[i] - xl[i];
    }

  if (calls < state->min_calls_per_bisection)
    {
      double m = 0.0, q = 0.0;

      if (calls < 2)
	{
	  GSL_ERROR ("insufficient calls for subvolume", GSL_EFAILED);
	}

      for (n = 0; n < calls; n++)
	{
	  /* Choose a random point in the integration region */

	  for (i = 0; i < dim; i++)
	    {
	      x[i] = xl[i] + gsl_rng_uniform_pos (r) * (xu[i] - xl[i]);
	    }

	  {
	    double fval = GSL_MONTE_FN_EVAL (f, x);

	    /* recurrence for mean and variance */

	    double d = fval - m;
	    m += d / (n + 1.0);
	    q += d * d * (n / (n + 1.0));
	  }
	}

      *result = vol * m;

      *abserr = vol * sqrt (q / (calls * (calls - 1.0)));

      return GSL_SUCCESS;
    }

  estimate_calls = GSL_MAX (min_calls, calls * (state->estimate_frac));

  if (estimate_calls < 4 * dim)
    {
      GSL_ERROR ("insufficient calls to sample all halfspaces", GSL_ESANITY);
    }

  /* Flip coins to bisect the integration region with some fuzz */

  for (i = 0; i < dim; i++)
    {
      s = (gsl_rng_uniform (r) - 0.5) >= 0.0 ? state->dither : -state->dither;
      state->xmid[i] = (0.5 + s) * xl[i] + (0.5 - s) * xu[i];
    }

  /* The idea is to chose the direction to bisect based on which will
     give the smallest total variance.  We could (and may do so later)
     use MC to compute these variances.  But the NR guys simply estimate
     the variances by finding the min and max function values
     for each half-region for each bisection. */

  estimate_corrmc (f, xl, xu, dim, estimate_calls,
		   r, state, &res_est, &err_est, xmid, sigma_l, sigma_r);

  /* We have now used up some calls for the estimation */

  calls -= estimate_calls;

  /* Now find direction with the smallest total "variance" */

  {
    double best_var = GSL_DBL_MAX;
    double beta = 2.0 / (1.0 + state->alpha);
    found_best = 0;
    i_bisect = 0;
    weight_l = weight_r = 1.0;

    for (i = 0; i < dim; i++)
      {
	if (sigma_l[i] >= 0 && sigma_r[i] >= 0)
	  {
	    /* estimates are okay */
	    double var = pow (sigma_l[i], beta) + pow (sigma_r[i], beta);

	    if (var <= best_var)
	      {
		found_best = 1;
		best_var = var;
		i_bisect = i;
		weight_l = pow (sigma_l[i], beta);
		weight_r = pow (sigma_r[i], beta);
	      }
	  }
	else
	  {
	    if (sigma_l[i] < 0)
	      {
		GSL_ERROR ("no points in left-half space!", GSL_ESANITY);
	      }
	    if (sigma_r[i] < 0)
	      {
		GSL_ERROR ("no points in right-half space!", GSL_ESANITY);
	      }
	  }
      }
  }

  if (!found_best)
    {
      /* All estimates were the same, so chose a direction at random */

      i_bisect = gsl_rng_uniform_int (r, dim);
    }

  xbi_l = xl[i_bisect];
  xbi_m = xmid[i_bisect];
  xbi_r = xu[i_bisect];

  /* Get the actual fractional sizes of the two "halves", and
     distribute the remaining calls among them */

  {
    double fraction_l = fabs ((xbi_m - xbi_l) / (xbi_r - xbi_l));
    double fraction_r = 1 - fraction_l;

    double a = fraction_l * weight_l;
    double b = fraction_r * weight_r;

    calls_l = min_calls + (calls - 2 * min_calls) * a / (a + b);
    calls_r = min_calls + (calls - 2 * min_calls) * b / (a + b);
  }

  /* Compute the integral for the left hand side of the bisection */

  /* Due to the recursive nature of the algorithm we must allocate
     some new memory for each recursive call */

  {
    int status;

    double *xu_tmp = (double *) malloc (dim * sizeof (double));

    if (xu_tmp == 0)
      {
	GSL_ERROR_VAL ("out of memory for left workspace", GSL_ENOMEM, 0);
      }

    for (i = 0; i < dim; i++)
      {
	xu_tmp[i] = xu[i];
      }

    xu_tmp[i_bisect] = xbi_m;

    status = gsl_monte_miser_integrate (f, xl, xu_tmp,
					dim, calls_l, r, state,
					&res_l, &err_l);
    free (xu_tmp);

    if (status != GSL_SUCCESS)
      {
	return status;
      }
  }

  /* Compute the integral for the right hand side of the bisection */

  {
    int status;

    double *xl_tmp = (double *) malloc (dim * sizeof (double));

    if (xl_tmp == 0)
      {
	GSL_ERROR_VAL ("out of memory for right workspace", GSL_ENOMEM, 0);
      }

    for (i = 0; i < dim; i++)
      {
	xl_tmp[i] = xl[i];
      }

    xl_tmp[i_bisect] = xbi_m;

    status = gsl_monte_miser_integrate (f, xl_tmp, xu,
					dim, calls_r, r, state,
					&res_r, &err_r);
    free (xl_tmp);

    if (status != GSL_SUCCESS)
      {
	return status;
      }
  }

  *result = res_l + res_r;
  *abserr = sqrt (err_l * err_l + err_r * err_r);

  return GSL_SUCCESS;
}

gsl_monte_miser_state *
gsl_monte_miser_alloc (size_t dim)
{
  gsl_monte_miser_state *s =
    (gsl_monte_miser_state *) malloc (sizeof (gsl_monte_miser_state));

  if (s == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for miser state struct",
		     GSL_ENOMEM, 0);
    }

  s->x = (double *) malloc (dim * sizeof (double));

  if (s->x == 0)
    {
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for x", GSL_ENOMEM, 0);
    }

  s->xmid = (double *) malloc (dim * sizeof (double));

  if (s->xmid == 0)
    {
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for xmid", GSL_ENOMEM, 0);
    }

  s->sigma_l = (double *) malloc (dim * sizeof (double));

  if (s->sigma_l == 0)
    {
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for sigma_l", GSL_ENOMEM, 0);
    }

  s->sigma_r = (double *) malloc (dim * sizeof (double));

  if (s->sigma_r == 0)
    {
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for sigma_r", GSL_ENOMEM, 0);
    }

  s->fmax_l = (double *) malloc (dim * sizeof (double));

  if (s->fmax_l == 0)
    {
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fmax_l", GSL_ENOMEM, 0);
    }

  s->fmax_r = (double *) malloc (dim * sizeof (double));

  if (s->fmax_r == 0)
    {
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fmax_r", GSL_ENOMEM, 0);
    }

  s->fmin_l = (double *) malloc (dim * sizeof (double));

  if (s->fmin_l == 0)
    {
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fmin_l", GSL_ENOMEM, 0);
    }

  s->fmin_r = (double *) malloc (dim * sizeof (double));

  if (s->fmin_r == 0)
    {
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fmin_r", GSL_ENOMEM, 0);
    }

  s->fsum_l = (double *) malloc (dim * sizeof (double));

  if (s->fsum_l == 0)
    {
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum_l", GSL_ENOMEM, 0);
    }

  s->fsum_r = (double *) malloc (dim * sizeof (double));

  if (s->fsum_r == 0)
    {
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum_r", GSL_ENOMEM, 0);
    }

  s->fsum2_l = (double *) malloc (dim * sizeof (double));

  if (s->fsum2_l == 0)
    {
      free (s->fsum_r);
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum2_l", GSL_ENOMEM, 0);
    }

  s->fsum2_r = (double *) malloc (dim * sizeof (double));

  if (s->fsum2_r == 0)
    {
      free (s->fsum2_l);
      free (s->fsum_r);
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum2_r", GSL_ENOMEM, 0);
    }

  s->hits_r = (size_t *) malloc (dim * sizeof (size_t));

  if (s->hits_r == 0)
    {
      free (s->fsum2_r);
      free (s->fsum2_l);
      free (s->fsum_r);
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum2_r", GSL_ENOMEM, 0);
    }

  s->hits_l = (size_t *) malloc (dim * sizeof (size_t));

  if (s->hits_l == 0)
    {
      free (s->hits_r);
      free (s->fsum2_r);
      free (s->fsum2_l);
      free (s->fsum_r);
      free (s->fsum_l);
      free (s->fmin_r);
      free (s->fmin_l);
      free (s->fmax_r);
      free (s->fmax_l);
      free (s->sigma_r);
      free (s->sigma_l);
      free (s->xmid);
      free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for fsum2_r", GSL_ENOMEM, 0);
    }

  s->dim = dim;

  gsl_monte_miser_init (s);

  return s;
}

int
gsl_monte_miser_init (gsl_monte_miser_state * s)
{
  /* We use 8 points in each halfspace to estimate the variance. There are
     2*dim halfspaces. A variance estimate requires a minimum of 2 points. */
  s->min_calls = 16 * s->dim;
  s->min_calls_per_bisection = 32 * s->min_calls;
  s->estimate_frac = 0.1;
  s->alpha = 2.0;
  s->dither = 0.0;

  return GSL_SUCCESS;
}

void
gsl_monte_miser_free (gsl_monte_miser_state * s)
{
  free (s->hits_r);
  free (s->hits_l);
  free (s->fsum2_r);
  free (s->fsum2_l);
  free (s->fsum_r);
  free (s->fsum_l);
  free (s->fmin_r);
  free (s->fmin_l);
  free (s->fmax_r);
  free (s->fmax_l);
  free (s->sigma_r);
  free (s->sigma_l);
  free (s->xmid);
  free (s->x);
  free (s);
}

static int
estimate_corrmc (gsl_monte_function * f,
		 const double xl[], const double xu[],
		 size_t dim, size_t calls,
		 gsl_rng * r,
		 gsl_monte_miser_state * state,
		 double *result, double *abserr,
		 const double xmid[], double sigma_l[], double sigma_r[])
{
  size_t i, n;

  double *x = state->x;
  double *fsum_l = state->fsum_l;
  double *fsum_r = state->fsum_r;
  double *fsum2_l = state->fsum2_l;
  double *fsum2_r = state->fsum2_r;
  size_t *hits_l = state->hits_l;
  size_t *hits_r = state->hits_r;

  double m = 0.0, q = 0.0;
  double vol = 1.0;

  for (i = 0; i < dim; i++)
    {
      vol *= xu[i] - xl[i];
      hits_l[i] = hits_r[i] = 0;
      fsum_l[i] = fsum_r[i] = 0.0;
      fsum2_l[i] = fsum2_r[i] = 0.0;
      sigma_l[i] = sigma_r[i] = -1;
    }

  for (n = 0; n < calls; n++)
    {
      double fval;

      unsigned int j = (n/2) % dim;
      unsigned int side = (n % 2);

      for (i = 0; i < dim; i++)
	{
          double z = gsl_rng_uniform_pos (r) ;

          if (i != j)
            {
              x[i] = xl[i] + z * (xu[i] - xl[i]);
            }
          else
            {
              if (side == 0)
                {
                  x[i] = xmid[i] + z * (xu[i] - xmid[i]);
                }
              else
                {
                  x[i] = xl[i] + z * (xmid[i] - xl[i]);
                }
            }
	}

      fval = GSL_MONTE_FN_EVAL (f, x);

      /* recurrence for mean and variance */
      {
	double d = fval - m;
	m += d / (n + 1.0);
	q += d * d * (n / (n + 1.0));
      }

      /* compute the variances on each side of the bisection */
      for (i = 0; i < dim; i++)
	{
	  if (x[i] <= xmid[i])
	    {
	      fsum_l[i] += fval;
	      fsum2_l[i] += fval * fval;
	      hits_l[i]++;
	    }
	  else
	    {
	      fsum_r[i] += fval;
	      fsum2_r[i] += fval * fval;
	      hits_r[i]++;
	    }
	}
    }

  for (i = 0; i < dim; i++)
    {
      double fraction_l = (xmid[i] - xl[i]) / (xu[i] - xl[i]);

      if (hits_l[i] > 0)
	{
	  fsum_l[i] /= hits_l[i];
	  sigma_l[i] = sqrt (fsum2_l[i] - fsum_l[i] * fsum_l[i] / hits_l[i]);
	  sigma_l[i] *= fraction_l * vol / hits_l[i];
	}

      if (hits_r[i] > 0)
	{
	  fsum_r[i] /= hits_r[i];
	  sigma_r[i] = sqrt (fsum2_r[i] - fsum_r[i] * fsum_r[i] / hits_r[i]);
	  sigma_r[i] *= (1 - fraction_l) * vol / hits_r[i];
	}
    }

  *result = vol * m;

  if (calls < 2)
    {
      *abserr = GSL_POSINF;
    }
  else
    {
      *abserr = vol * sqrt (q / (calls * (calls - 1.0)));
    }

  return GSL_SUCCESS;
}


/* specfunc/bessel_Y1.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_trig.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"

#include "bessel.h"
#include "bessel_amp_phase.h"
#include "cheb_eval.c"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* based on SLATEC besy1, 1977 version, w. fullerton */

/* chebyshev expansions

 series for by1        on the interval  0.          to  1.60000d+01
                                        with weighted error   1.87e-18
                                         log weighted error  17.73
                               significant figures required  17.83
                                    decimal places required  18.30
*/

static double by1_data[14] = {
  0.03208047100611908629,
  1.262707897433500450,
  0.00649996189992317500,
 -0.08936164528860504117,
  0.01325088122175709545,
 -0.00089790591196483523,
  0.00003647361487958306,
 -0.00000100137438166600,
  0.00000001994539657390,
 -0.00000000030230656018,
  0.00000000000360987815,
 -0.00000000000003487488,
  0.00000000000000027838,
 -0.00000000000000000186
};
static cheb_series by1_cs = {
  by1_data,
  13,
  -1, 1,
  10
};

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

int gsl_sf_bessel_Y1_e(const double x, gsl_sf_result * result)
{
  const double two_over_pi = 2.0/M_PI;
  const double xmin = 1.571*GSL_DBL_MIN; /*exp ( amax1(alog(r1mach(1)), -alog(r1mach(2)))+.01) */
  const double x_small = 2.0 * GSL_SQRT_DBL_EPSILON;
  const double xmax    = 1.0/GSL_DBL_EPSILON;

  /* CHECK_POINTER(result) */

  if(x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < xmin) {
    OVERFLOW_ERROR(result);
  }
  else if(x < x_small) {
    const double lnterm = log(0.5*x);
    gsl_sf_result J1;
    gsl_sf_result c;
    int status = gsl_sf_bessel_J1_e(x, &J1);
    cheb_eval_e(&by1_cs, -1.0, &c);
    result->val = two_over_pi * lnterm * J1.val + (0.5 + c.val)/x;
    result->err = fabs(lnterm) * (fabs(GSL_DBL_EPSILON * J1.val) + J1.err) + c.err/x;
    return status;
  }
  else if(x < 4.0) {
    const double lnterm = log(0.5*x);
    int status;
    gsl_sf_result J1;
    gsl_sf_result c;
    cheb_eval_e(&by1_cs, 0.125*x*x-1.0, &c);
    status = gsl_sf_bessel_J1_e(x, &J1);
    result->val = two_over_pi * lnterm * J1.val + (0.5 + c.val)/x;
    result->err = fabs(lnterm) * (fabs(GSL_DBL_EPSILON * J1.val) + J1.err) + c.err/x;
    return status;
  }
  else if(x < xmax) {
    const double z = 32.0/(x*x) - 1.0;
    gsl_sf_result ca;
    gsl_sf_result ct;
    gsl_sf_result cp;
    const int stat_ca = cheb_eval_e(&_gsl_sf_bessel_amp_phase_bm1_cs,  z, &ca);
    const int stat_ct = cheb_eval_e(&_gsl_sf_bessel_amp_phase_bth1_cs, z, &ct);
    const int stat_cp = gsl_sf_bessel_cos_pi4_e(x, ct.val/x, &cp);
    const double sqrtx = sqrt(x);
    const double ampl  = (0.75 + ca.val) / sqrtx;
    result->val  = -ampl * cp.val;
    result->err  = fabs(cp.val) * ca.err/sqrtx + fabs(ampl) * cp.err;
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_3(stat_ca, stat_ct, stat_cp);
  }
  else {
    UNDERFLOW_ERROR(result);
  }
}

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

#include "eval.h"

double gsl_sf_bessel_Y1(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_Y1_e(x, &result));
}


#ifndef BLAS_WRAPPER_H
#define BLAS_WRAPPER_H

#include <cassert>
#include <cmath>
#include <cstring>
#include <vector>

// Useful dense kernels from BLAS, with readable, overloaded, cross-platform names and some simplified calling
//  dot    (dot-product of vectors)
//  nrm2   (2-norm of a vector)
//  asum   (1-norm of a vector)
//  amax   (index of maximum absolute value in a vector)
//  swap   (exchanging values in two vectors)
//  copy   (copying values from one vector to another)
//  axpy   (adding a scalar times a vector to another vector)
//  scal   (multiplying a vector by a scalar)
//  gemv   (multiplying a matrix times a vector, scaling, and adding result to another vector))
//  gemm   (multiplying two matrices, scaling, and adding result to another matrix)
// In addition:
//  set_zero  (zero out all entries in a vector)
//  abs_max   (return the infinity norm of a vector, i.e. the magnitude of its largest element)
// There are also version using std::vector for convenience.

// Matrices are always assumed to be in column-major format.

// You can #define one of:
//    USE_FORTRAN_BLAS (if your BLAS calls should look like dgemm_ with FORTRAN calling conventions, as in GOTO BLAS)
//    USE_AMD_BLAS     (if using the AMD Math Library)
//    USE_CBLAS        (if instead you have calls like cblas_dgemm, and have a file "cblas.h" available)
// or, if you're on the Mac, it will default to the vecLib CBLAS if none of these are specified.

namespace BLAS{

    template<class T>
    inline void set_zero(int n, T *x)
    { std::memset(x, 0, n*sizeof(T)); }

}

//============================================================================
#ifdef USE_FORTRAN_BLAS

extern "C" {
    double dsdot_(const int*, const float*, const int*, const float*, const int*);
    double sdot_(const int*, const float*, const int*, const float*, const int*);
    double ddot_(const int*, const double*, const int*, const double*, const int*);
    float snrm2_(const int*, const float*, const int*);
    double dnrm2_(const int*, const double*, const int*);
    float sasum_(const int*, const float*, const int*);
    double dasum_(const int*, const double*, const int*);
    int isamax_(const int*, const float*, const int*);
    int idamax_(const int*, const double*, const int*);
    void sswap_(const int*, float*, const int*, float*, const int*);
    void dswap_(const int*, double*, const int*, double*, const int*);
    void scopy_(const int*, const float*, const int*, float*, const int*);
    void dcopy_(const int*, const double*, const int*, double*, const int*);
    void saxpy_(const int*, const float*, const float*, const int*, float*, const int*);
    void daxpy_(const int*, const double*, const double*, const int*, double*, const int*);
    void sscal_(const int*, const float*, float*, const int*);
    void dscal_(const int*, const double*, double*, const int*);
    void sgemv_(const char*, const int*, const int*, const float*, const float*, const int*, const float*, const int*, const float*, float*, const int*);
    void dgemv_(const char*, const int*, const int*, const double*, const double*, const int*, const double*, const int*, const double*, double*, const int*);
    void sgemm_(const char*, const char*, const int*, const int*, const int*, const float*, const float*, const int*, const float*, const int*, const float*, float*, const int*);
    void dgemm_(const char*, const char*, const int*, const int*, const int*, const double*, const double*, const int*, const double*, const int*, const double*, double*, const int*);
}

namespace BLAS{

    enum Transpose {NoTrans='N', Trans='T'};
    enum UpperLower {Upper='U', Lower='L'};
    enum UnitDiag {NonUnit='N', Unit='U'};
    enum Side {Left='L', Right='R'};

    // dot products

    inline double dot(int n, const float *x, int incx, const float *y, int incy=1)
    { return dsdot_(&n, x, &incx, y, &incy); }

    inline double dot(int n, const float *x, const float *y, int incy=1)
    { const int one=1; return dsdot_(&n, x, &one, y, &incy); }

    inline float dotf(int n, const float *x, int incx, const float *y, int incy=1)
    { return (float)sdot_(&n, x, &incx, y, &incy); }

    inline float dotf(int n, const float *x, const float *y, int incy=1)
    { const int one=1; return (float)sdot_(&n, x, &one, y, &incy); }

    inline double dot(int n, const double *x, int incx, const double *y, int incy=1)
    { return ddot_(&n, x, &incx, y, &incy); }

    inline double dot(int n, const double *x, const double *y, int incy=1)
    { const int one=1; return ddot_(&n, x, &one, y, &incy); }

    // 2-norm

    inline float norm2(int n, const float *x, int incx=1)
    { return snrm2_(&n, x, &incx); }

    inline double norm2(int n, const double *x, int incx=1)
    { return dnrm2_(&n, x, &incx); }

    // 1-norm (sum of absolute values)

    inline float abs_sum(int n, const float *x, int incx=1)
    { return sasum_(&n, x, &incx); }

    inline double abs_sum(int n, const double *x, int incx=1)
    { return dasum_(&n, x, &incx); }

    // inf-norm (maximum absolute value: index of max returned)

    inline int index_abs_max(int n, const float *x, int incx=1)
    { return isamax_(&n, x, &incx)-1; }

    inline int index_abs_max(int n, const double *x, int incx=1)
    { return idamax_(&n, x, &incx)-1; }

    inline float abs_max(int n, const float *x, int incx=1)
    { return std::fabs(x[isamax_(&n, x, &incx)-1]); }

    inline double abs_max(int n, const double *x, int incx=1)
    { return std::fabs(x[idamax_(&n, x, &incx)-1]); }

    // swap (actual data exchanged, not just pointers)

    inline void swap(int n, float *x, int incx, float *y, int incy=1)
    { sswap_(&n, x, &incx, y, &incy); }

    inline void swap(int n, float *x, float *y, int incy=1)
    { const int one=1; sswap_(&n, x, &one, y, &incy); }

    inline void swap(int n, double *x, int incx, double *y, int incy=1)
    { dswap_(&n, x, &incx, y, &incy); }

    inline void swap(int n, double *x, double *y, int incy=1)
    { const int one=1; dswap_(&n, x, &one, y, &incy); }

    // copy (y=x)

    inline void copy(int n, const float *x, int incx, float *y, int incy=1)
    { scopy_(&n, x, &incx, y, &incy); }

    inline void copy(int n, const float *x, float *y, int incy=1)
    { const int one=1; scopy_(&n, x, &one, y, &incy); }

    inline void copy(int n, const double *x, int incx, double *y, int incy=1)
    { dcopy_(&n, x, &incx, y, &incy); }

    inline void copy(int n, const double *x, double *y, int incy=1)
    { const int one=1; dcopy_(&n, x, &one, y, &incy); }

    // saxpy (y=alpha*x+y)

    inline void add_scaled(int n, float alpha, const float *x, int incx, float *y, int incy=1)
    { saxpy_(&n, &alpha, x, &incx, y, &incy); }

    inline void add_scaled(int n, float alpha, const float *x, float *y, int incy=1)
    { const int one=1; saxpy_(&n, &alpha, x, &one, y, &incy); }

    inline void add_scaled(int n, double alpha, const double *x, int incx, double *y, int incy=1)
    { daxpy_(&n, &alpha, x, &incx, y, &incy); }

    inline void add_scaled(int n, double alpha, const double *x, double *y, int incy=1)
    { const int one=1; daxpy_(&n, &alpha, x, &one, y, &incy); }

    // scale (x=alpha*x)

    inline void scale(int n, float alpha, float *x, int incx=1)
    { sscal_(&n, &alpha, x, &incx); }

    inline void scale(int n, double alpha, double *x, int incx=1)
    { dscal_(&n, &alpha, x, &incx); }

    // gemv (y=alpha*A*x+beta*y, or using A^T)
    // The matrix is always m*n; the size of x and y depend on if A is transposed.

    inline void multiply_matrix_vector(Transpose transpose,
                                       int m, int n, float alpha, const float *A, int lda,
                                       const float *x, int incx, float beta, float *y, int incy=1)
    { sgemv_((const char*)&transpose, &m, &n, &alpha, A, &lda, x, &incx, &beta, y, &incy); }

    inline void multiply_matrix_vector(int m, int n, const float *A, const float *x, float *y, int incy=1) // y=A*x
    { const int onei=1; const float zero=0, onef=1; sgemv_("N", &m, &n, &onef, A, &m, x, &onei, &zero, y, &incy); }

    inline void multiply_matrix_vector(Transpose transpose,
                                       int m, int n, double alpha, const double *A, int lda,
                                       const double *x, int incx, double beta, double *y, int incy=1)
    { dgemv_((const char*)&transpose, &m, &n, &alpha, A, &lda, x, &incx, &beta, y, &incy); }

    inline void multiply_matrix_vector(int m, int n, const double *A, const double *x, double *y, int incy=1) // y=A*x
    { const int onei=1; const double zero=0, onef=1; dgemv_("N", &m, &n, &onef, A, &m, x, &onei, &zero, y, &incy); }

    // gemm (C=alpha*A*B+beta*C)

    inline void multiply_matrix_matrix(Transpose transA, Transpose transB,
                                       int m, int n, int k, float alpha, const float *A, int lda,
                                       const float *B, int ldb, float beta, float *C, int ldc)
    { sgemm_((const char*)&transA, (const char*)&transB, &m, &n, &k, &alpha, A, &lda, B, &ldb, &beta, C, &ldc); }

    inline void multiply_matrix_matrix(int m, int n, int k, const float *A, const float *B, float *C)
    { const float zero=0, one=1; sgemm_("N", "N", &m, &n, &k, &one, A, &m, B, &k, &zero, C, &m); } // C=A*B

    inline void multiply_matrix_matrix(Transpose transA, Transpose transB,
                                       int m, int n, int k, double alpha, const double *A, int lda,
                                       const double *B, int ldb, double beta, double *C, int ldc)
    { dgemm_((const char*)&transA, (const char*)&transB, &m, &n, &k, &alpha, A, &lda, B, &ldb, &beta, C, &ldc); }

    inline void multiply_matrix_matrix(int m, int n, int k, const double *A, const double *B, double *C)
    { const double zero=0, one=1; dgemm_("N", "N", &m, &n, &k, &one, A, &m, B, &k, &zero, C, &m); } // C=A*B

};

//============================================================================
#elif defined USE_AMD_BLAS

#include <acml.h>

namespace BLAS{

    enum Transpose {NoTrans='N', Trans='T'};
    enum UpperLower {Upper='U', Lower='L'};
    enum UnitDiag {NonUnit='N', Unit='U'};
    enum Side {Left='L', Right='R'};

    // dot products

    inline double dot(int n, const float *x, int incx, const float *y, int incy=1)
    { return dsdot(n, (float*)x, incx, (float*)y, incy); }

    inline double dot(int n, const float *x, const float *y, int incy=1)
    { return dsdot(n, (float*)x, 1, (float*)y, incy); }

    inline float dotf(int n, const float *x, int incx, const float *y, int incy=1)
    { return sdot(n, (float*)x, incx, (float*)y, incy); }

    inline float dotf(int n, const float *x, const float *y, int incy=1)
    { return sdot(n, (float*)x, 1, (float*)y, incy); }

    inline double dot(int n, const double *x, int incx, const double *y, int incy=1)
    { return ddot(n, (double *)x, incx, (double *)y, incy); }

    inline double dot(int n, const double *x, const double *y, int incy=1)
    { return ddot(n, (double *)x, 1, (double *)y, incy); }

    // 2-norm

    inline float norm2(int n, const float *x, int incx=1)
    { return snrm2(n, (float*)x, incx); }

    inline double norm2(int n, const double *x, int incx=1)
    { return dnrm2(n, (double*)x, incx); }

    // 1-norm (sum of absolute values)

    inline float abs_sum(int n, const float *x, int incx=1)
    { return sasum(n, (float*)x, incx); }

    inline double abs_sum(int n, const double *x, int incx=1)
    { return dasum(n, (double*)x, incx); }

    // inf-norm (maximum absolute value: index of max returned)

    inline int index_abs_max(int n, const float *x, int incx=1)
    { return isamax(n, (float*)x, incx)-1; }

    inline int index_abs_max(int n, const double *x, int incx=1)
    { return idamax(n, (double*)x, incx)-1; }

    inline float abs_max(int n, const float *x, int incx=1)
    { return std::fabs(x[isamax(n, (float*)x, incx)]-1); }

    inline double abs_max(int n, const double *x, int incx=1)
    { return std::fabs(x[idamax(n, (double*)x, incx)]-1); }

    // swap (actual data exchanged, not just pointers)

    inline void swap(int n, float *x, int incx, float *y, int incy=1)
    { sswap(n, x, incx, y, incy); }

    inline void swap(int n, float *x, float *y, int incy=1)
    { sswap(n, x, 1, y, incy); }

    inline void swap(int n, double *x, int incx, double *y, int incy=1)
    { dswap(n, x, incx, y, incy); }

    inline void swap(int n, double *x, double *y, int incy=1)
    { dswap(n, x, 1, y, incy); }

    // copy (y=x)

    inline void copy(int n, const float *x, int incx, float *y, int incy=1)
    { scopy(n, (float*)x, incx, y, incy); }

    inline void copy(int n, const float *x, float *y, int incy=1)
    { scopy(n, (float*)x, 1, y, incy); }

    inline void copy(int n, const double *x, int incx, double *y, int incy=1)
    { dcopy(n, (double *)x, incx, y, incy); }

    inline void copy(int n, const double *x, double *y, int incy=1)
    { dcopy(n, (double *)x, 1, y, incy); }

    // saxpy (y=alpha*x+y)

    inline void add_scaled(int n, float alpha, const float *x, int incx, float *y, int incy=1)
    { saxpy(n, alpha, (float*)x, incx, y, incy); }

    inline void add_scaled(int n, float alpha, const float *x, float *y, int incy=1)
    { saxpy(n, alpha, (float*)x, 1, y, incy); }

    inline void add_scaled(int n, double alpha, const double *x, int incx, double *y, int incy=1)
    { daxpy(n, alpha, (double*)x, incx, y, incy); }

    inline void add_scaled(int n, double alpha, const double *x, double *y, int incy=1)
    { daxpy(n, alpha, (double*)x, 1, y, incy); }

    // scale (x=alpha*x)

    inline void scale(int n, float alpha, float *x, int incx=1)
    { sscal(n, alpha, x, incx); }

    inline void scale(int n, double alpha, double *x, int incx=1)
    { dscal(n, alpha, x, incx); }

    // gemv (y=alpha*A*x+beta*y, or using A^T)
    // The matrix is always m*n; the size of x and y depend on if A is transposed.

    inline void multiply_matrix_vector(Transpose transpose,
                                       int m, int n, float alpha, const float *A, int lda,
                                       const float *x, int incx, float beta, float *y, int incy=1)
    { sgemv(transpose, m, n, alpha, (float*)A, lda, (float*)x, incx, beta, y, incy); }

    inline void multiply_matrix_vector(int m, int n, const float *A, const float *x, float *y, int incy=1) // y=A*x
    { sgemv(NoTrans, m, n, 1.f, (float*)A, m, (float*)x, 1, 0.f, y, incy); }

    inline void multiply_matrix_vector(Transpose transpose,
                                       int m, int n, double alpha, const double *A, int lda,
                                       const double *x, int incx, double beta, double *y, int incy=1)
    { dgemv(transpose, m, n, alpha, (double*)A, lda, (double*)x, incx, beta, y, incy); }

    inline void multiply_matrix_vector(int m, int n, const double *A, const double *x, double *y, int incy=1) // y=A*x
    { dgemv(NoTrans, m, n, 1., (double*)A, m, (double*)x, 1, 0., y, incy); }

    // gemm (C=alpha*A*B+beta*C)

    inline void multiply_matrix_matrix(Transpose transA, Transpose transB,
                                       int m, int n, int k, float alpha, const float *A, int lda,
                                       const float *B, int ldb, float beta, float *C, int ldc)
    { sgemm(transA, transB, m, n, k, alpha, (float*)A, lda, (float*)B, ldb, beta, C, ldc); }

    inline void multiply_matrix_matrix(int m, int n, int k, const float *A, const float *B, float *C)
    { sgemm(NoTrans, NoTrans, m, n, k, 1.f, (float*)A, m, (float*)B, k, 0.f, C, m); } // C=A*B

    inline void multiply_matrix_matrix(Transpose transA, Transpose transB,
                                       int m, int n, int k, double alpha, const double *A, int lda,
                                       const double *B, int ldb, double beta, double *C, int ldc)
    { dgemm(transA, transB, m, n, k, alpha, (double*)A, lda, (double*)B, ldb, beta, C, ldc); }

    inline void multiply_matrix_matrix(int m, int n, int k, const double *A, const double *B, double *C)
    { dgemm(NoTrans, NoTrans, m, n, k, 1., (double*)A, m, (double*)B, k, 0., C, m); } // C=A*B

};

//============================================================================
#elif defined USE_CBLAS || defined __APPLE__

#ifdef USE_CBLAS
#include <cblas.h>
#elif defined __APPLE__
#include <Accelerate/Accelerate.h>
#endif

namespace BLAS{

    enum Transpose {NoTrans=CblasNoTrans, Trans=CblasTrans};
    enum UpperLower {Upper=CblasUpper, Lower=CblasLower};
    enum UnitDiag {NonUnit=CblasNonUnit, Unit=CblasUnit};
    enum Side {Left=CblasLeft, Right=CblasRight};

    // dot products

    inline float dotf(int n, const float *x, int incx, const float *y, int incy=1)
    { return cblas_sdot(n, x, incx, y, incy); }

    inline float dotf(int n, const float *x, const float *y, int incy=1)
    { return cblas_sdot(n, x, 1, y, incy); }

    inline double dot(int n, const float *x, int incx, const float *y, int incy=1)
    { return cblas_dsdot(n, x, incx, y, incy); }

    inline double dot(int n, const float *x, const float *y, int incy=1)
    { return cblas_dsdot(n, x, 1, y, incy); }

    inline double dot(int n, const double *x, int incx, const double *y, int incy=1)
    { return cblas_ddot(n, x, incx, y, incy); }

    inline double dot(int n, const double *x, const double *y, int incy=1)
    { return cblas_ddot(n, x, 1, y, incy); }

    // 2-norm

    inline float norm2(int n, const float *x, int incx=1)
    { return cblas_snrm2(n, x, incx); }

    inline double norm2(int n, const double *x, int incx=1)
    { return cblas_dnrm2(n, x, incx); }

    // 1-norm (sum of absolute values)

    inline float abs_sum(int n, const float *x, int incx=1)
    { return cblas_sasum(n, x, incx); }

    inline double abs_sum(int n, const double *x, int incx=1)
    { return cblas_dasum(n, x, incx); }

    // inf-norm (maximum absolute value)

    inline int index_abs_max(int n, const float *x, int incx=1)
    { return cblas_isamax(n, x, incx); }

    inline int index_abs_max(int n, const double *x, int incx=1)
    { return cblas_idamax(n, x, incx); }

    inline float abs_max(int n, const float *x, int incx=1)
    { return std::fabs(x[cblas_isamax(n, x, incx)]); }

    inline double abs_max(int n, const double *x, int incx=1)
    { return std::fabs(x[cblas_idamax(n, x, incx)]); }

    // swap (actual data exchanged, not just pointers)

    inline void swap(int n, float *x, int incx, float *y, int incy=1)
    { cblas_sswap(n, x, incx, y, incy); }

    inline void swap(int n, float *x, float *y, int incy=1)
    { cblas_sswap(n, x, 1, y, incy); }

    inline void swap(int n, double *x, int incx, double *y, int incy=1)
    { cblas_dswap(n, x, incx, y, incy); }

    inline void swap(int n, double *x, double *y, int incy=1)
    { cblas_dswap(n, x, 1, y, incy); }

    // copy (y=x)

    inline void copy(int n, const float *x, int incx, float *y, int incy=1)
    { cblas_scopy(n, x, incx, y, incy); }

    inline void copy(int n, const float *x, float *y, int incy=1)
    { cblas_scopy(n, x, 1, y, incy); }

    inline void copy(int n, const double *x, int incx, double *y, int incy=1)
    { cblas_dcopy(n, x, incx, y, incy); }

    inline void copy(int n, const double *x, double *y, int incy=1)
    { cblas_dcopy(n, x, 1, y, incy); }

    // saxpy (y=alpha*x+y)

    inline void add_scaled(int n, float alpha, const float *x, int incx, float *y, int incy=1)
    { cblas_saxpy(n, alpha, x, incx, y, incy); }

    inline void add_scaled(int n, float alpha, const float *x, float *y, int incy=1)
    { cblas_saxpy(n, alpha, x, 1, y, incy); }

    inline void add_scaled(int n, double alpha, const double *x, int incx, double *y, int incy=1)
    { cblas_daxpy(n, alpha, x, incx, y, incy); }

    inline void add_scaled(int n, double alpha, const double *x, double *y, int incy=1)
    { cblas_daxpy(n, alpha, x, 1, y, incy); }

    // scale (x=alpha*x)

    inline void scale(int n, float alpha, float *x, int incx=1)
    { cblas_sscal(n, alpha, x, incx); }

    inline void scale(int n, double alpha, double *x, int incx=1)
    { cblas_dscal(n, alpha, x, incx); }

    // gemv (y=alpha*A*x+beta*y, or using A^T)
    // The matrix is always m*n; the size of x and y depend on if A is transposed.

    inline void multiply_matrix_vector(Transpose transpose,
                                       int m, int n, float alpha, const float *A, int lda,
                                       const float *x, int incx,
                                       float beta, float *y, int incy=1)
    { cblas_sgemv(CblasColMajor, (CBLAS_TRANSPOSE)transpose, m, n, alpha, A, lda, x, incx, beta, y, incy); }

    inline void multiply_matrix_vector(int m, int n, const float *A, const float *x, float *y, int incy=1) // y=A*x
    { cblas_sgemv(CblasColMajor, CblasNoTrans, m, n, 1.f, A, m, x, 1, 0.f, y, incy); }

    inline void multiply_matrix_vector(Transpose transpose,
                                       int m, int n, double alpha, const double *A, int lda,
                                       const double *x, int incx, double beta, double *y, int incy=1)
    { cblas_dgemv(CblasColMajor, (CBLAS_TRANSPOSE)transpose, m, n, alpha, A, lda, x, incx, beta, y, incy); }

    inline void multiply_matrix_vector(int m, int n, const double *A, const double *x, double *y, int incy=1) // y=A*x
    { cblas_dgemv(CblasColMajor, CblasNoTrans, m, n, 1., A, m, x, 1, 0., y, incy); }

    // gemm (C=alpha*A*B+beta*C)

    inline void multiply_matrix_matrix(Transpose transA, Transpose transB,
                                       int m, int n, int k, float alpha, const float *A, int lda,
                                       const float *B, int ldb, float beta, float *C, int ldc)
    { cblas_sgemm(CblasColMajor, (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB, m, n, k, alpha, A, lda, B, ldb, beta, C, ldc); }

    inline void multiply_matrix_matrix(int m, int n, int k, const float *A, const float *B, float *C)
    { cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1.f, A, m, B, k, 0.f, C, m); } // C=A*B

    inline void multiply_matrix_matrix(Transpose transA, Transpose transB,
                                       int m, int n, int k, double alpha, const double *A, int lda,
                                       const double *B, int ldb, double beta, double *C, int ldc)
    { cblas_dgemm(CblasColMajor, (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB, m, n, k, alpha, A, lda, B, ldb, beta, C, ldc); }

    inline void multiply_matrix_matrix(int m, int n, int k, const double *A, const double *B, double *C)
    { cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1., A, m, B, k, 0., C, m); } // C=A*B

    inline void rank_one_update( int m, int n, double alpha, const double* x, const double* y, double* A )
    {
        cblas_dger( CblasColMajor, m, n, alpha, x, 1, y, 1, A, m );
    }

}  // namespace BLAS

#endif

// std::vector calls =========================================================
namespace BLAS{

    template<class T>
    inline void set_zero(std::vector<T> &x)
    { set_zero((int)x.size(), &x[0]); }

    inline float dotf(const std::vector<float> &x, const std::vector<float> &y)
    { assert(x.size()==y.size()); return dotf((int)x.size(), &x[0], &y[0]); }

    inline double dot(const std::vector<float> &x, const std::vector<float> &y)
    { assert(x.size()==y.size()); return dot((int)x.size(), &x[0], &y[0]); }

    inline double dot(const std::vector<double> &x, const std::vector<double> &y)
    { assert(x.size()==y.size()); return dot((int)x.size(), &x[0], &y[0]); }

    inline float norm2(const std::vector<float> &x)
    { return norm2((int)x.size(), &x[0]); }

    inline double norm2(const std::vector<double> &x)
    { return norm2((int)x.size(), &x[0]); }

    inline float abs_sum(const std::vector<float> &x)
    { return abs_sum((int)x.size(), &x[0]); }

    inline double abs_sum(const std::vector<double> &x)
    { return abs_sum((int)x.size(), &x[0]); }

    inline int index_abs_max(const std::vector<float> &x)
    { return index_abs_max((int)x.size(), &x[0]); }

    inline int index_abs_max(const std::vector<double> &x)
    { return index_abs_max((int)x.size(), &x[0]); }

    inline float abs_max(const std::vector<float> &x)
    { return abs_max((int)x.size(), &x[0]); }

    inline double abs_max(const std::vector<double> &x)
    { return abs_max((int)x.size(), &x[0]); }

    inline void swap(std::vector<float> &x, std::vector<float> &y)
    { assert(x.size()==y.size()); swap((int)x.size(), &x[0], &y[0]); }

    inline void swap(std::vector<double> &x, std::vector<double> &y)
    { assert(x.size()==y.size()); swap((int)x.size(), &x[0], &y[0]); }

    inline void copy(const std::vector<float> &x, std::vector<float> &y)
    { assert(x.size()==y.size()); copy((int)x.size(), &x[0], &y[0]); }

    inline void copy(const std::vector<double> &x, std::vector<double> &y)
    { assert(x.size()==y.size()); copy((int)x.size(), &x[0], &y[0]); }

    inline void add_scaled(float alpha, const std::vector<float> &x, std::vector<float> &y)
    { assert(x.size()==y.size()); add_scaled((int)x.size(), alpha, &x[0], &y[0]); }

    inline void add_scaled(double alpha, const std::vector<double> &x, std::vector<double> &y)
    { assert(x.size()==y.size()); add_scaled((int)x.size(), alpha, &x[0], &y[0]); }

    inline void scale(float alpha, std::vector<float> &x)
    { scale((int)x.size(), alpha, &x[0]); }

    inline void scale(float alpha, std::vector<double> &x)
    { scale((int)x.size(), alpha, &x[0]); }

    // I'm not sure if it makes sense to include level 2 or level 3 std::vector versions,
    // since there isn't an STL matrix type...

}  // namespace BLAS

#endif


// Copyright (c) 2013-2017 Anton Kozhevnikov, Thomas Schulthess
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that
// the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the
//    following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions
//    and the following disclaimer in the documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

/** \file sbessel.h
 *
 *  \brief Contains implementation of sirius::Spherical_Bessel_functions and sirius::sbessel_approx classes.
 */

#ifndef __SBESSEL_PW_H__
#define __SBESSEL_PW_H__

#include <gsl/gsl_sf_bessel.h>
#include "eigenproblem.h"
#include "Unit_cell/unit_cell.h"

namespace sirius
{

/// Spherical Bessel functions \f$ j_{\ell}(q x) \f$ up to lmax.
class Spherical_Bessel_functions
{
    private:
        int lmax_{-1};
        double q_{0};
        Radial_grid<double> const* rgrid_{nullptr};

        std::vector<Spline<double>> sbessel_;

    public:

        Spherical_Bessel_functions()
        {
        }

        Spherical_Bessel_functions(int lmax__, Radial_grid<double> const& rgrid__, double q__)
            : lmax_(lmax__)
            , q_(q__)
            , rgrid_(&rgrid__)
        {
            assert(q_ >= 0);

            sbessel_ = std::vector<Spline<double>>(lmax__ + 2);
            for (int l = 0; l <= lmax__ + 1; l++) {
                sbessel_[l] = Spline<double>(rgrid__);
            }

            std::vector<double> jl(lmax__ + 2);
            for (int ir = 0; ir < rgrid__.num_points(); ir++) {
                double t = rgrid__[ir] * q__;
                gsl_sf_bessel_jl_array(lmax__ + 1, t, &jl[0]);
                for (int l = 0; l <= lmax__ + 1; l++) {
                    sbessel_[l](ir) = jl[l];
                }
            }

            for (int l = 0; l <= lmax__ + 1; l++) {
                sbessel_[l].interpolate();
            }
        }

        static void sbessel(int lmax__, double t__, double* jl__)
        {
            gsl_sf_bessel_jl_array(lmax__, t__, jl__);
        }

        static void sbessel_deriv_q(int lmax__, double q__, double x__, double* jl_dq__)
        {
            std::vector<double> jl(lmax__ + 2);
            sbessel(lmax__ + 1, x__ * q__, &jl[0]);

            for (int l = 0; l <= lmax__; l++) {
                if (q__ != 0) {
                    jl_dq__[l] = (l / q__) * jl[l] - x__ * jl[l + 1];
                } else {
                    if (l == 1) {
                        jl_dq__[l] = x__ / 3;
                    } else {
                        jl_dq__[l] = 0;
                    }
                }
            }
        }

        Spline<double> const& operator[](int l__) const
        {
            assert(l__ <= lmax_);
            return sbessel_[l__];
        }

        /// Derivative of Bessel function with respect to q.
        /** \f[
         *    \frac{\partial j_{\ell}(q x)}{\partial q} = \frac{\ell}{q} j_{\ell}(q x) - x j_{\ell+1}(q x)
         *  \f]
         */
        Spline<double> deriv_q(int l__)
        {
            assert(l__ <= lmax_);
            assert(q_ >= 0);
            Spline<double> s(*rgrid_);
            if (q_ != 0) {
                for (int ir = 0; ir < rgrid_->num_points(); ir++) {
                    s(ir) = (l__ / q_) * sbessel_[l__](ir) - (*rgrid_)[ir] * sbessel_[l__ + 1](ir);
                }
            } else {
                if (l__ == 1) {
                    for (int ir = 0; ir < rgrid_->num_points(); ir++) {
                        s(ir) = (*rgrid_)[ir] / 3;
                    }
                }
            }
            s.interpolate();
            return std::move(s);
        }
};

class sbessel_approx
{
    private:

        Unit_cell const& unit_cell_;

        int lmax_;

        mdarray<std::vector<double>, 2> qnu_;
        mdarray<double, 4> coeffs_;

        int nqnu_max_;

        static double sbessel_l2norm(double nu, int l, double R)
        {
            if (std::abs(nu) < 1e-10) TERMINATE_NOT_IMPLEMENTED;

            if (l == 0)
            {
                return (nu * R * 2 - std::sin(nu * R * 2)) / 4 / std::pow(nu, 3);
            }
            else
            {
                double jl[l + 2];
                gsl_sf_bessel_jl_array(l + 1, R * nu, &jl[0]);
                return std::pow(R, 3) * (jl[l] * jl[l] - jl[l + 1] * jl[l - 1]) / 2;
            }
        }

    public:

        sbessel_approx(Unit_cell const& unit_cell__,
                       int lmax__,
                       double const qmin__,
                       double const qmax__,
                       double const eps__)
            : unit_cell_(unit_cell__),
              lmax_(lmax__)
        {
            PROFILE("sirius::sbessel_approx");

            qnu_ = mdarray<std::vector<double>, 2>(lmax_ + 1, unit_cell_.num_atom_types());

            for (int l = 0; l <= lmax_; l++) {
                for (int iat = 0; iat < unit_cell_.num_atom_types(); iat++) {
                    qnu_(l, iat) = build_approx_freq(qmin__, qmax__, l, unit_cell_.atom_type(iat).mt_radius(), eps__);
                }
            }

            nqnu_max_ = 0;
            for (int l = 0; l <= lmax_; l++) {
                for (int iat = 0; iat < unit_cell_.num_atom_types(); iat++) {
                    nqnu_max_ = std::max(nqnu_max_, static_cast<int>(qnu_(l, iat).size()));
                }
            }
        }

        void approximate(std::vector<double> const& q__)
        {
            PROFILE("sirius::sbessel_approx::approximate");

            coeffs_ = mdarray<double, 4>(nqnu_max_, q__.size(), lmax_ + 1, unit_cell_.num_atom_types());

            #pragma omp parallel for
            for (int l = 0; l <= lmax_; l++)
            {
                for (int iat = 0; iat < unit_cell_.num_atom_types(); iat++)
                {
                    int n = nqnu(l, iat);

                    mdarray<double, 2> A(n, n);
                    for (int iq = 0; iq < n; iq++)
                    {
                        for (int jq = 0; jq <= iq; jq++)
                        {
                            A(jq, iq) = A(iq, jq) = overlap(qnu_(l, iat)[jq], qnu_(l, iat)[iq], l,
                                                            unit_cell_.atom_type(iat).mt_radius());
                        }

                        for (int j = 0; j < (int)q__.size(); j++)
                        {
                            if (std::abs(q__[j]) < 1e-12)
                            {
                                coeffs_(iq, j, l, iat) = 0;
                            }
                            else
                            {
                                coeffs_(iq, j, l, iat) = overlap(qnu_(l, iat)[iq], q__[j], l,
                                                                 unit_cell_.atom_type(iat).mt_radius());
                            }
                        }
                    }
                    linalg<CPU>::gesv(n, (int)q__.size(), A.at<CPU>(), A.ld(), &coeffs_(0, 0, l, iat), coeffs_.ld());
                }
            }
        }

        inline double qnu(int const iq, int const l, int const iat)
        {
            return qnu_(l, iat)[iq];
        }

        inline int nqnu(int const l, int const iat)
        {
            return static_cast<int>(qnu_(l, iat).size());
        }

        inline int nqnu_max()
        {
            return nqnu_max_;
        }

        inline double coeff(int const iq, int const j, int const l, int const iat)
        {
            return coeffs_(iq, j, l, iat);
        }

        // \int_0^{R} j(nu1 * r) * j(nu2 * r) * r^2 dr
        // this integral can be computed analytically
        static double overlap(double nu1__, double nu2__, int l__, double R__)
        {
            if (std::abs(nu1__) < 1e-10 || std::abs(nu2__) < 1e-10) TERMINATE_NOT_IMPLEMENTED;

            if (std::abs(nu1__ - nu2__) < 1e-12)
            {
                if (l__ == 0)
                {
                    return (nu2__ * R__ * 2 - std::sin(nu2__ * R__ * 2)) / 4 / std::pow(nu2__, 3);
                }
                else
                {
                    double jl[l__ + 2];
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu2__, &jl[0]);
                    return std::pow(R__, 3) * (jl[l__] * jl[l__] - jl[l__ + 1] * jl[l__ - 1]) / 2;
                }
            }
            else
            {
                if (l__ == 0)
                {
                    return (nu2__ * std::cos(nu2__ * R__) * std::sin(nu1__ * R__) - nu1__ * std::cos(nu1__ * R__) * std::sin(nu2__ * R__)) /
                           (std::pow(nu1__, 3) * nu2__ - nu1__ * std::pow(nu2__, 3));
                }
                else
                {
                    double j1[l__ + 2];
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu1__, &j1[0]);

                    double j2[l__ + 2];
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu2__, &j2[0]);

                    return std::pow(R__, 2) * (nu2__ * j2[l__ - 1] * j1[l__] - nu1__ * j1[l__ - 1] * j2[l__]) / (std::pow(nu1__, 2) - std::pow(nu2__, 2));
                }
            }
        }

        std::vector<double> build_approx_freq(double const qmin__,
                                              double const qmax__,
                                              int const l__,
                                              double const R__,
                                              double const eps__)
        {
            std::vector<double> qnu;

            double min_val;
            int n = 2;

            do
            {
                n++;
                qnu.resize(n);
                for (int i = 0; i < n; i++) qnu[i] = qmin__ + (qmax__ - qmin__) * i / (n - 1);

                dmatrix<double_complex> ovlp(n, n);
                for (int i = 0; i < n; i++)
                {
                    for (int j = 0; j <= i; j++)
                    {
                        double o = overlap(qnu[j], qnu[i], l__, R__);
                        ovlp(j, i) = o / sbessel_l2norm(qnu[i], l__, R__) / sbessel_l2norm(qnu[j], l__, R__);
                    }
                }

                std::vector<double> eval(n);
                dmatrix<double_complex> z(n, n);

                Eigensolver_lapack<double_complex> solver;
                solver.solve(n, ovlp, &eval[0], z);
                min_val = eval[0];

            } while (min_val > eps__);

            return qnu;
        }
};

class Spherical_Bessel_approximant
{
    private:

        int lmax_;

        double R_;

        /// List of Bessel function scaling factors for each angular momentum.
        std::vector< std::vector<double> > qnu_;

        //mdarray<double, 4> coeffs_;

        int nqnu_max_;

        static double sbessel_l2norm(double nu, int l, double R)
        {
            if (std::abs(nu) < 1e-10)
            {
                if (l == 0) return std::pow(R, 3) / 3.0;
                return 0;
            }

            if (l == 0)
            {
                return (nu * R * 2 - std::sin(nu * R * 2)) / 4 / std::pow(nu, 3);
            }
            else
            {
                double jl[l + 2];
                gsl_sf_bessel_jl_array(l + 1, R * nu, &jl[0]);
                return std::pow(R, 3) * (jl[l] * jl[l] - jl[l + 1] * jl[l - 1]) / 2;
            }
        }

    public:

        Spherical_Bessel_approximant(int lmax__,
                                     double R__,
                                     double const qmin__,
                                     double const qmax__,
                                     double const eps__)
            : lmax_(lmax__),
              R_(R__)
        {
            PROFILE("sirius::Spherical_Bessel_approximant");

            qnu_ = std::vector< std::vector<double> >(lmax_ + 1);

            #pragma omp parallel for
            for (int l = 0; l <= lmax_; l++)
                qnu_[l] = build_approx_freq(qmin__, qmax__, l, R_, eps__);

            nqnu_max_ = 0;
            for (int l = 0; l <= lmax_; l++)
                nqnu_max_ = std::max(nqnu_max_, nqnu(l));
        }

        std::vector<double> approximate(int l__, double nu__)
        {
            int n = nqnu(l__);
            std::vector<double> x(n);
            matrix<double> A(n, n);

            for (int iq = 0; iq < n; iq++)
            {
                for (int jq = 0; jq <= iq; jq++)
                {
                    A(jq, iq) = A(iq, jq) = overlap(qnu(jq, l__), qnu(iq, l__), l__, R_);
                }
                x[iq] = overlap(qnu(iq, l__), nu__, l__, R_);
            }
            linalg<CPU>::gesv(n, 1, A.at<CPU>(), A.ld(), &x[0], n);
            return x;
        }

        void approximate(std::vector<double> const& q__)
        {
            //runtime::Timer t("sirius::sbessel_approx::approximate");

            //coeffs_ = mdarray<double, 4>(nqnu_max_, q__.size(), lmax_ + 1, unit_cell_.num_atom_types());
            //
            //#pragma omp parallel for
            //for (int l = 0; l <= lmax_; l++)
            //{
            //    for (int iat = 0; iat < unit_cell_.num_atom_types(); iat++)
            //    {
            //        int n = nqnu(l, iat);

            //        mdarray<double, 2> A(n, n);
            //        for (int iq = 0; iq < n; iq++)
            //        {
            //            for (int jq = 0; jq <= iq; jq++)
            //            {
            //                A(jq, iq) = A(iq, jq) = overlap(qnu_(l, iat)[jq], qnu_(l, iat)[iq], l,
            //                                                unit_cell_.atom_type(iat).mt_radius());
            //            }

            //            for (int j = 0; j < (int)q__.size(); j++)
            //            {
            //                if (std::abs(q__[j]) < 1e-12)
            //                {
            //                    coeffs_(iq, j, l, iat) = 0;
            //                }
            //                else
            //                {
            //                    coeffs_(iq, j, l, iat) = overlap(qnu_(l, iat)[iq], q__[j], l,
            //                                                     unit_cell_.atom_type(iat).mt_radius());
            //                }
            //            }
            //        }
            //        linalg<CPU>::gesv(n, (int)q__.size(), A.at<CPU>(), A.ld(), &coeffs_(0, 0, l, iat), coeffs_.ld());
            //    }
            //}
        }

        inline double qnu(int const iq, int const l) const
        {
            return qnu_[l][iq];
        }

        inline int nqnu(int const l) const
        {
            return static_cast<int>(qnu_[l].size());
        }

        inline int nqnu_max() const
        {
            return nqnu_max_;
        }

        //inline double coeff(int const iq, int const j, int const l, int const iat)
        //{
        //    return coeffs_(iq, j, l, iat);
        //}
        //

        // \int_0^{R} j(nu1 * r) * j(nu2 * r) * r^2 dr
        // this integral can be computed analytically
        static double overlap(double nu1__, double nu2__, int l__, double R__)
        {
            if (std::abs(nu1__) < 1e-10 && std::abs(nu2__) < 1e-10 && l__ == 0) return std::pow(R__, 3) / 3.0;

            if ((std::abs(nu1__) < 1e-10 || std::abs(nu2__) < 1e-10) && l__ > 0) return 0;

            if ((std::abs(nu1__) < 1e-10 || std::abs(nu2__) < 1e-10) && l__ == 0)
            {
                double nu = std::max(nu1__, nu2__);
                double nuR = nu * R__;
                return (std::sin(nuR) - nuR * std::cos(nuR)) / std::pow(nu, 3);
            }

            if (std::abs(nu1__ - nu2__) < 1e-12)
            {
                if (l__ == 0)
                {
                    return (nu2__ * R__ * 2 - std::sin(nu2__ * R__ * 2)) / 4 / std::pow(nu2__, 3);
                }
                else
                {
                    double jl[l__ + 2];
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu2__, &jl[0]);
                    return std::pow(R__, 3) * (jl[l__] * jl[l__] - jl[l__ + 1] * jl[l__ - 1]) / 2;
                }
            }
            else
            {
                if (l__ == 0)
                {
                    return (nu2__ * std::cos(nu2__ * R__) * std::sin(nu1__ * R__) - nu1__ * std::cos(nu1__ * R__) * std::sin(nu2__ * R__)) /
                           (std::pow(nu1__, 3) * nu2__ - nu1__ * std::pow(nu2__, 3));
                }
                else
                {
                    double j1[l__ + 2];
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu1__, &j1[0]);

                    double j2[l__ + 2];
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu2__, &j2[0]);

                    return std::pow(R__, 2) * (nu2__ * j2[l__ - 1] * j1[l__] - nu1__ * j1[l__ - 1] * j2[l__]) / (std::pow(nu1__, 2) - std::pow(nu2__, 2));
                }
            }
            TERMINATE("this is wrong");
            return -1;
        }

        std::vector<double> build_approx_freq(double const qmin__,
                                              double const qmax__,
                                              int const l__,
                                              double const R__,
                                              double const eps__)
        {
            std::vector<double> qnu;

            double min_val;
            int n = 2;

            do
            {
                n++;
                qnu.resize(n);
                for (int i = 0; i < n; i++) qnu[i] = qmin__ + (qmax__ - qmin__) * i / (n - 1);

                dmatrix<double> ovlp(n, n);
                for (int i = 0; i < n; i++)
                {
                    for (int j = 0; j <= i; j++)
                    {
                        double o = overlap(qnu[j], qnu[i], l__, R__);
                        ovlp(j, i) = o / sbessel_l2norm(qnu[i], l__, R__) / sbessel_l2norm(qnu[j], l__, R__);
                    }
                }

                std::vector<double> eval(n);
                dmatrix<double> z(n, n);

                Eigensolver_lapack<double> solver;
                solver.solve(n, ovlp, &eval[0], z);
                min_val = eval[0];

            } while (min_val > eps__);

            return qnu;
        }
};

class Spherical_Bessel_approximant2
{
    private:

        int lmax_;

        double R_;

        /// List of Bessel function scaling factors for each angular momentum.
        std::vector<double> qnu_;

        static double sbessel_l2norm(double nu, int l, double R)
        {
            if (std::abs(nu) < 1e-10)
            {
                if (l == 0) return std::pow(R, 3) / 3.0;
                return 0;
            }

            if (l == 0)
            {
                return (nu * R * 2 - std::sin(nu * R * 2)) / 4 / std::pow(nu, 3);
            }
            else
            {
                double jl[l + 2];
                gsl_sf_bessel_jl_array(l + 1, R * nu, &jl[0]);
                return std::pow(R, 3) * (jl[l] * jl[l] - jl[l + 1] * jl[l - 1]) / 2;
            }
        }

    public:

        Spherical_Bessel_approximant2(int lmax__,
                                     double R__,
                                     double const qmin__,
                                     double const qmax__,
                                     int nq__)
            : lmax_(lmax__),
              R_(R__)
        {
            PROFILE("sirius::Spherical_Bessel_approximant");

            int nq = nfreq(qmin__, qmax__, 0, R__, 1e-12);
            qnu_.resize(nq);
            for (int i = 0; i < nq; i++) qnu_[i] = qmin__ + (qmax__ - qmin__) * i / (nq - 1);

        }

        std::vector<double> approximate(int l__, double nu__)
        {
            int n = nqnu();
            std::vector<double> x(n);
            matrix<double> A(n, n);

            for (int iq = 0; iq < n; iq++)
            {
                for (int jq = 0; jq <= iq; jq++)
                {
                    A(jq, iq) = A(iq, jq) = overlap(qnu(jq), qnu(iq), l__, R_);
                }
                x[iq] = overlap(qnu(iq), nu__, l__, R_);
            }
            linalg<CPU>::gesv(n, 1, A.at<CPU>(), A.ld(), &x[0], n);
            return x;
        }

        inline double qnu(int const iq) const
        {
            return qnu_[iq];
        }

        inline int nqnu() const
        {
            return static_cast<int>(qnu_.size());
        }

        // \int_0^{R} j(nu1 * r) * j(nu2 * r) * r^2 dr
        // this integral can be computed analytically
        static double overlap(double nu1__, double nu2__, int l__, double R__)
        {
            if (std::abs(nu1__) < 1e-10 && std::abs(nu2__) < 1e-10 && l__ == 0) return std::pow(R__, 3) / 3.0;

            if ((std::abs(nu1__) < 1e-10 || std::abs(nu2__) < 1e-10) && l__ > 0) return 0;

            if ((std::abs(nu1__) < 1e-10 || std::abs(nu2__) < 1e-10) && l__ == 0)
            {
                double nu = std::max(nu1__, nu2__);
                double nuR = nu * R__;
                return (std::sin(nuR) - nuR * std::cos(nuR)) / std::pow(nu, 3);
            }

            if (std::abs(nu1__ - nu2__) < 1e-12)
            {
                if (l__ == 0)
                {
                    return (nu2__ * R__ * 2 - std::sin(nu2__ * R__ * 2)) / 4 / std::pow(nu2__, 3);
                }
                else
                {
                    std::vector<double> jl(l__ + 2);
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu2__, &jl[0]);
                    return std::pow(R__, 3) * (jl[l__] * jl[l__] - jl[l__ + 1] * jl[l__ - 1]) / 2;
                }
            }
            else
            {
                if (l__ == 0)
                {
                    return (nu2__ * std::cos(nu2__ * R__) * std::sin(nu1__ * R__) - nu1__ * std::cos(nu1__ * R__) * std::sin(nu2__ * R__)) /
                           (std::pow(nu1__, 3) * nu2__ - nu1__ * std::pow(nu2__, 3));
                }
                else
                {
                    std::vector<double> j1(l__ + 2);
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu1__, &j1[0]);

                    std::vector<double> j2(l__ + 2);
                    gsl_sf_bessel_jl_array(l__ + 1, R__ * nu2__, &j2[0]);

                    return std::pow(R__, 2) * (nu2__ * j2[l__ - 1] * j1[l__] - nu1__ * j1[l__ - 1] * j2[l__]) / (std::pow(nu1__, 2) - std::pow(nu2__, 2));
                }
            }
            TERMINATE("this is wrong");
            return -1;
        }

        int nfreq(double const qmin__,
                  double const qmax__,
                  int const l__,
                  double const R__,
                  double const eps__)
        {
            std::vector<double> qnu;

            double min_val;
            int n = 2;

            do
            {
                n++;
                qnu.resize(n);
                for (int i = 0; i < n; i++) qnu[i] = qmin__ + (qmax__ - qmin__) * i / (n - 1);

                dmatrix<double> ovlp(n, n);
                for (int i = 0; i < n; i++)
                {
                    for (int j = 0; j <= i; j++)
                    {
                        double o = overlap(qnu[j], qnu[i], l__, R__);
                        ovlp(j, i) = o / sbessel_l2norm(qnu[i], l__, R__) / sbessel_l2norm(qnu[j], l__, R__);
                    }
                }

                std::vector<double> eval(n);
                dmatrix<double> z(n, n);

                Eigensolver_lapack<double> solver;
                solver.solve(n, ovlp, &eval[0], z);
                min_val = eval[0];

                if (n > 100) return 100;

            } while (min_val > eps__);

            return n;
        }
};

};

#endif


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "ccl.h"

typedef struct{
  double l;
  ccl_cosmology *cosmo;
  ccl_cl_tracer_collection_t *trc1;
  ccl_cl_tracer_collection_t *trc2;
  ccl_f2d_t *psp;
  int *status;
} integ_cl_par;

typedef struct{
  int chipow;
  double l1;
  double l2;
  ccl_cosmology *cosmo;
  ccl_cl_tracer_collection_t *trc1;
  ccl_cl_tracer_collection_t *trc2;
  ccl_cl_tracer_collection_t *trc3;
  ccl_cl_tracer_collection_t *trc4;
  ccl_f3d_t *tsp;
  ccl_f1d_t *ker_extra;
  ccl_a_finder *finda;
  int *status;
} integ_cov_par;

static void update_chi_limits(ccl_cl_tracer_collection_t *trc,
                              double *chimin, double *chimax,
                              int is_union)
{
  int itr;
  double chimin_h=1E15;
  double chimax_h=-1E15;
  for(itr=0; itr < trc->n_tracers; itr++) {
    if (trc->ts[itr]->chi_min < chimin_h)
      chimin_h = trc->ts[itr]->chi_min;
    if (trc->ts[itr]->chi_max > chimax_h)
      chimax_h = trc->ts[itr]->chi_max;
  }

  if(is_union) {
    if(chimin_h < *chimin)
      *chimin = chimin_h;
    if(chimax_h > *chimax)
      *chimax = chimax_h;
  }
  else {
    if(chimin_h > *chimin)
      *chimin = chimin_h;
    if(chimax_h < *chimax)
      *chimax = chimax_h;
  }
}

static void get_k_interval(ccl_cosmology *cosmo,
                           ccl_cl_tracer_collection_t *trc1,
                           ccl_cl_tracer_collection_t *trc2,
                           double l, double *lkmin, double *lkmax) {
  int itr;

  // Loop through all tracers and find distance bounds
  double chi_min1 = 1E15;
  double chi_max1 = -1E15;
  for (itr=0; itr < trc1->n_tracers; itr++) {
    if (trc1->ts[itr]->chi_min < chi_min1)
      chi_min1 = trc1->ts[itr]->chi_min;
    if (trc1->ts[itr]->chi_max > chi_max1)
      chi_max1 = trc1->ts[itr]->chi_max;
  }

  double chi_min2 = 1E15;
  double chi_max2 = -1E15;
  for (itr=0; itr < trc2->n_tracers; itr++) {
    if (trc2->ts[itr]->chi_min < chi_min2)
      chi_min2 = trc2->ts[itr]->chi_min;
    if (trc2->ts[itr]->chi_max > chi_max2)
      chi_max2 = trc2->ts[itr]->chi_max;
  }

  // Find maximum of minima and minimum of maxima
  // (i.e. edges where the product of both kernels will have support).
  double chi_min = fmax(chi_min1, chi_min2);
  double chi_max = fmin(chi_max1, chi_max2);

  if (chi_min <= 0)
    chi_min = 0.5*(l+0.5)/cosmo->spline_params.K_MAX;

  // Don't go beyond kmax
  *lkmax = log(fmin(cosmo->spline_params.K_MAX, 2*(l+0.5)/chi_min));
  *lkmin = log(fmax(cosmo->spline_params.K_MIN, (l+0.5)/chi_max));
}

static double transfer_limber_single(ccl_cl_tracer_t *tr, double l, double lk,
                                     double k, double chi_l, double a_l,
                                     ccl_cosmology *cosmo, ccl_f2d_t *psp,
                                     int ignore_jbes_deriv,
                                     int *status) {
  double dd = 0;

  // Kernel and transfer evaluated at chi_l
  double w = ccl_cl_tracer_t_get_kernel(tr, chi_l, status);
  double t = ccl_cl_tracer_t_get_transfer(tr, lk,a_l, status);
  double fl = ccl_cl_tracer_t_get_f_ell(tr, l, status);

  if (tr->der_bessel < 1) { //We don't need l+1
    dd = w*t;
    if (tr->der_bessel == -1) { //If we divide by (chi*k)^2
      double lp1h = l+0.5;
      dd /= (lp1h*lp1h);
    }
  }
  else { // We will need l+1
    if(ignore_jbes_deriv)
      dd = 0;
    else {
      // Compute chi_{l+1} and a_{l+1}
      double lp1h = l+0.5;
      double lp3h = l+1.5;
      double chi_lp = lp3h/k;
      double a_lp = ccl_scale_factor_of_chi(cosmo, chi_lp, status);

      // Compute power spectrum ratio there
      double pk_ratio = fabs(ccl_f2d_t_eval(psp, lk, a_lp, cosmo, status) /
                             ccl_f2d_t_eval(psp, lk, a_l, cosmo, status));

      // Compute kernel and trasfer at chi_{l+1}
      double w_p = ccl_cl_tracer_t_get_kernel(tr, chi_lp, status);
      double t_p = ccl_cl_tracer_t_get_transfer(tr, lk,a_lp, status);

      // sqrt(2l+1/2l+3)
      double sqell = sqrt(lp1h*pk_ratio/lp3h);
      if (tr->der_bessel == 1)
        dd = l*w*t/lp1h-sqell*w_p*t_p;
      else //we assume der_bessel=2 here to avoid extra if clause
        dd = sqell*2*w_p*t_p/lp3h - (0.25+2*l)*w*t/(lp1h*lp1h);
    }
  }
  return dd*fl;
}

static double transfer_limber_wrap(double l,double lk, double k, double chi,
                                   double a, ccl_cl_tracer_collection_t *trc,
                                   ccl_cosmology *cosmo,ccl_f2d_t *psp,
                                   int ignore_jbes_deriv, int *status) {
  int itr;
  double transfer = 0;

  for (itr=0; itr < trc->n_tracers; itr++) {
    transfer += transfer_limber_single(
      trc->ts[itr], l, lk, k, chi, a, cosmo, psp, ignore_jbes_deriv, status);
    if (*status != 0)
      return -1;
  }
  return transfer;
}

static double cl_integrand(double lk, void *params) {
  double d1, d2;
  integ_cl_par *p = (integ_cl_par *)params;
  double k = exp(lk);
  double chi = (p->l+0.5)/k;
  double a = ccl_scale_factor_of_chi(p->cosmo, chi, p->status);

  d1 = transfer_limber_wrap(p->l, lk, k, chi, a, p->trc1,
                            p->cosmo, p->psp, 0, p->status);
  if (d1 == 0)
    return 0;

  d2 = transfer_limber_wrap(p->l, lk, k, chi, a, p->trc2,
                            p->cosmo, p->psp, 0, p->status);

  if (d2 == 0)
    return 0;

  double pk = ccl_f2d_t_eval(p->psp, lk, a, p->cosmo, p->status);

  return k*pk*d1*d2;
}

static void integ_cls_limber_spline(ccl_cosmology *cosmo,
				    integ_cl_par *ipar,
				    double lkmin, double lkmax,
				    double *result, int *status) {
  int ik;
  int nk = (int)(fmax((lkmax - lkmin) / cosmo->spline_params.DLOGK_INTEGRATION + 0.5,
		      1))+1;
  double *fk_arr = NULL;
  double *lk_arr = NULL;
  lk_arr = ccl_linear_spacing(lkmin, lkmax, nk);
  if(lk_arr == NULL)
    *status = CCL_ERROR_LOGSPACE;

  if(*status == 0) {
    fk_arr = malloc(nk * sizeof(double));
    if(fk_arr == NULL)
      *status = CCL_ERROR_MEMORY;
  }

  if(*status == 0) {
    for(ik=0; ik<nk; ik++) {
      fk_arr[ik] = cl_integrand(lk_arr[ik], ipar);
      if(*(ipar->status)) {
	*status = *(ipar->status);
	break;
      }
    }
  }

  if(*status == 0) {
    ccl_integ_spline(1, nk, lk_arr, &fk_arr,
                     1, -1, result, gsl_interp_akima,
                     status);
  }
  free(fk_arr);
  free(lk_arr);
}

static void integ_cls_limber_qag_quad(ccl_cosmology *cosmo,
				      gsl_function *F,
				      double lkmin, double lkmax,
				      gsl_integration_workspace *w,
				      double *result, double *eresult,
				      int *status) {
  int gslstatus;
  size_t nevals;
  gsl_integration_cquad_workspace *w_cquad = NULL;
  // Integrate
  gslstatus = gsl_integration_qag(F, lkmin, lkmax, 0,
				  cosmo->gsl_params.INTEGRATION_LIMBER_EPSREL,
				  cosmo->gsl_params.N_ITERATION,
				  cosmo->gsl_params.INTEGRATION_LIMBER_GAUSS_KRONROD_POINTS,
				  w, result, eresult);

  // Test if a round-off error occured in the evaluation of the integral
  // If so, try another integration function, more robust but potentially slower
  if (gslstatus == GSL_EROUND) {
    ccl_raise_gsl_warning(gslstatus,
			  "ccl_cls.c: integ_cls_limber_qag_quad(): "
			  "Default GSL integration failure, attempting backup method.");
    w_cquad = gsl_integration_cquad_workspace_alloc(cosmo->gsl_params.N_ITERATION);
    if (w_cquad == NULL)
      *status = CCL_ERROR_MEMORY;

    if (*status == 0) {
      nevals = 0;
      gslstatus = gsl_integration_cquad(F, lkmin, lkmax, 0,
					cosmo->gsl_params.INTEGRATION_LIMBER_EPSREL,
					w_cquad, result, eresult, &nevals);
    }
  }
  gsl_integration_cquad_workspace_free(w_cquad);
  if(*status == 0)
    *status = gslstatus;
}

void ccl_angular_cls_limber(ccl_cosmology *cosmo,
			    ccl_cl_tracer_collection_t *trc1,
			    ccl_cl_tracer_collection_t *trc2,
			    ccl_f2d_t *psp,
			    int nl_out, double *l_out, double *cl_out,
			    ccl_integration_t integration_method,
			    int *status) {

  // make sure to init core things for safety
  if (!cosmo->computed_distances) {
    *status = CCL_ERROR_DISTANCES_INIT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_cls.c: ccl_angular_cls_limber(): distance splines have not been precomputed!");
    return;
  }

  #pragma omp parallel shared(cosmo, trc1, trc2, l_out, cl_out, \
                              nl_out, status, psp, integration_method) \
                       default(none)
  {
    int clastatus, lind;
    integ_cl_par ipar;
    gsl_integration_workspace *w = NULL;
    int local_status = *status;
    gsl_function F;
    double lkmin, lkmax, l, result, eresult;

    if (local_status == 0) {
      // Set up integrating function parameters
      ipar.cosmo = cosmo;
      ipar.trc1 = trc1;
      ipar.trc2 = trc2;
      ipar.psp = psp;
      ipar.status = &clastatus;
    }

    if(integration_method == ccl_integration_qag_quad) {
      if (local_status == 0) {
	w = gsl_integration_workspace_alloc(cosmo->gsl_params.N_ITERATION);
	if (w == NULL) {
	  local_status = CCL_ERROR_MEMORY;
	}
      }

      if (local_status == 0) {
	// Set up integrating function
	F.function = &cl_integrand;
	F.params = &ipar;
      }
    }

    #pragma omp for schedule(dynamic)
    for (lind=0; lind < nl_out; ++lind) {
      if (local_status == 0) {
        l = l_out[lind];
        clastatus = 0;
        ipar.l = l;

        // Get integration limits
        get_k_interval(cosmo, trc1, trc2, l, &lkmin, &lkmax);

	// Integrate
	if(integration_method == ccl_integration_qag_quad) {
	  integ_cls_limber_qag_quad(cosmo, &F, lkmin, lkmax, w,
				    &result, &eresult, &local_status);
	}
	else if(integration_method == ccl_integration_spline) {
	  integ_cls_limber_spline(cosmo, &ipar, lkmin, lkmax,
				  &result, &local_status);
	}
	else
	  local_status = CCL_ERROR_NOT_IMPLEMENTED;

        if ((*ipar.status == 0) && (local_status == 0)) {
          cl_out[lind] = result / (l+0.5);
        }
        else {
          ccl_raise_gsl_warning(local_status, "ccl_cls.c: ccl_angular_cls_limber():");
          cl_out[lind] = NAN;
          local_status = CCL_ERROR_INTEG;
        }
      }
    }

    gsl_integration_workspace_free(w);

    if (local_status) {
      #pragma omp atomic write
      *status = local_status;
    }
  }

  if (*status) {
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_cls.c: ccl_angular_cls_limber(); integration error\n");
  }
}

void ccl_angular_cls_nonlimber(ccl_cosmology *cosmo,
                               ccl_cl_tracer_collection_t *trc1,
                               ccl_cl_tracer_collection_t *trc2,
                               ccl_f2d_t *psp,
                               int nl_out, int *l_out, double *cl_out,
                               int *status) {
  *status = CCL_ERROR_INCONSISTENT;
  ccl_cosmology_set_status_message(
    cosmo,
    "ccl_cls.c: ccl_angular_cls_nonlimber(); non-Limber integrator not implemented yet\n");
}

static double cov_integrand(double chi, void *params)
{
  double d1, d2, d3, d4, tkk, ker=1;
  integ_cov_par *p = (integ_cov_par *)params;
  double k1=(p->l1+0.5)/chi;
  double k2=(p->l2+0.5)/chi;
  double lk1=log(k1);
  double lk2=log(k2);
  double a = ccl_scale_factor_of_chi(p->cosmo, chi, p->status);

  d1 = transfer_limber_wrap(p->l1, lk1, k1, chi, a, p->trc1,
                            p->cosmo, NULL, 1, p->status);
  if (d1 == 0)
    return 0;
  d2 = transfer_limber_wrap(p->l1, lk1, k1, chi, a, p->trc2,
                            p->cosmo, NULL, 1, p->status);
  if (d2 == 0)
    return 0;
  d3 = transfer_limber_wrap(p->l2, lk2, k2, chi, a, p->trc3,
                            p->cosmo, NULL, 1, p->status);
  if (d3 == 0)
    return 0;
  d4 = transfer_limber_wrap(p->l2, lk2, k2, chi, a, p->trc4,
                            p->cosmo, NULL, 1, p->status);
  if (d4 == 0)
    return 0;

  tkk = ccl_f3d_t_eval(p->tsp, lk1, lk2, a,
                       p->finda, p->cosmo, p->status);
  if(p->ker_extra!=NULL)
    ker = ccl_f1d_t_eval(p->ker_extra, chi);

  return d1*d2*d3*d4*tkk*ker/pow(chi, p->chipow);
}

static void integ_cov_limber_spline(ccl_cosmology *cosmo,
				    integ_cov_par *ipar,
				    double chimin, double chimax,
				    double *result, int *status)
{
  int ichi;
  int nchi = (int)(fmax((chimax - chimin) / cosmo->spline_params.DCHI_INTEGRATION + 0.5,
		      1))+1;
  double *fchi_arr = NULL;
  double *chi_arr = NULL;
  chi_arr = ccl_linear_spacing(chimin, chimax, nchi);
  if(chi_arr == NULL)
    *status = CCL_ERROR_LOGSPACE;

  if(*status == 0) {
    fchi_arr = malloc(nchi * sizeof(double));
    if(fchi_arr == NULL)
      *status = CCL_ERROR_MEMORY;
  }

  if(*status == 0) {
    for(ichi=0; ichi<nchi; ichi++) {
      fchi_arr[ichi] = cov_integrand(chi_arr[ichi], ipar);
      if(*(ipar->status)) {
	*status = *(ipar->status);
	break;
      }
    }
  }

  if(*status == 0) {
    ccl_integ_spline(1, nchi, chi_arr, &fchi_arr,
                     1, -1, result, gsl_interp_akima,
                     status);
  }

  free(fchi_arr);
  free(chi_arr);
}

static void integ_cov_limber_qag_quad(ccl_cosmology *cosmo,
				      gsl_function *F,
				      double chimin, double chimax,
				      gsl_integration_workspace *w,
				      double *result, double *eresult,
				      int *status) {
  int gslstatus;
  size_t nevals;
  gsl_integration_cquad_workspace *w_cquad = NULL;
  // Integrate
  gslstatus = gsl_integration_qag(F, chimin, chimax, 0,
				  cosmo->gsl_params.INTEGRATION_LIMBER_EPSREL,
				  cosmo->gsl_params.N_ITERATION,
				  cosmo->gsl_params.INTEGRATION_LIMBER_GAUSS_KRONROD_POINTS,
				  w, result, eresult);

  // Test if a round-off error occured in the evaluation of the integral
  // If so, try another integration function, more robust but potentially slower
  if (gslstatus == GSL_EROUND) {
    ccl_raise_gsl_warning(gslstatus,
			  "ccl_cls.c: ccl_angular_cov_limber(): "
			  "Default GSL integration failure, attempting backup method.");
    w_cquad = gsl_integration_cquad_workspace_alloc(cosmo->gsl_params.N_ITERATION);
    if (w_cquad == NULL)
      *status = CCL_ERROR_MEMORY;

    if (*status == 0) {
      nevals = 0;
      gslstatus = gsl_integration_cquad(F, chimin, chimax, 0,
					cosmo->gsl_params.INTEGRATION_LIMBER_EPSREL,
					w_cquad, result, eresult, &nevals);
    }
  }
  gsl_integration_cquad_workspace_free(w_cquad);
  if(*status == 0)
    *status = gslstatus;
}

void ccl_angular_cl_covariance(ccl_cosmology *cosmo,
                               ccl_cl_tracer_collection_t *trc1,
                               ccl_cl_tracer_collection_t *trc2,
                               ccl_cl_tracer_collection_t *trc3,
                               ccl_cl_tracer_collection_t *trc4,
                               ccl_f3d_t *tsp,
                               int nl1_out, double *l1_out,
                               int nl2_out, double *l2_out,
                               double *cov_out,
                               ccl_integration_t integration_method,
                               int chi_exponent, ccl_f1d_t *kernel_extra,
                               double prefactor_extra, int *status)
{
  if(!cosmo->computed_distances) {
    *status = CCL_ERROR_DISTANCES_INIT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_cls.c: ccl_angular_cl_limber(): distance splines have not been precomputed!");
    return;
  }

  #pragma omp parallel shared(cosmo, trc1, trc2, trc3, trc4, tsp, \
                              nl1_out, l1_out, nl2_out, l2_out, cov_out, \
                              integration_method, chi_exponent, \
                              kernel_extra, prefactor_extra, status) \
                       default(none)
  {
    int clastatus, lind1,lind2;
    integ_cov_par ipar;
    gsl_integration_workspace *w = NULL;
    int local_status = *status;
    gsl_function F;
    double chimin, chimax;
    double l1, l2, result, eresult;
    ccl_a_finder *finda = ccl_a_finder_new_from_f3d(tsp);

    // Find integration limits
    chimin = 1E15;
    chimax = -1E15;
    update_chi_limits(trc1, &chimin, &chimax, 1);
    update_chi_limits(trc2, &chimin, &chimax, 0);
    update_chi_limits(trc3, &chimin, &chimax, 0);
    update_chi_limits(trc4, &chimin, &chimax, 0);

    if (local_status == 0) {
      // Set up integrating function parameters
      ipar.cosmo = cosmo;
      ipar.trc1 = trc1;
      ipar.trc2 = trc2;
      ipar.trc3 = trc3;
      ipar.trc4 = trc4;
      ipar.tsp = tsp;
      ipar.ker_extra = kernel_extra;
      ipar.finda = finda;
      ipar.status = &clastatus;
      ipar.chipow = chi_exponent;
    }

    if(integration_method == ccl_integration_qag_quad) {
      if (local_status == 0) {
	w = gsl_integration_workspace_alloc(cosmo->gsl_params.N_ITERATION);
	if (w == NULL) {
	  local_status = CCL_ERROR_MEMORY;
	}
      }

      if (local_status == 0) {
	// Set up integrating function
	F.function = &cov_integrand;
	F.params = &ipar;
      }
    }

    #pragma omp for schedule(dynamic)
    for (lind1=0; lind1 < nl1_out; ++lind1) {
      l1 = l1_out[lind1];
      ipar.l1 = l1;

      for (lind2=0; lind2 < nl2_out; ++lind2) {
        if (local_status == 0) {
          l2 = l2_out[lind2];
          clastatus = 0;
          ipar.l2 = l2;

          // Integrate
          if(integration_method == ccl_integration_qag_quad) {
            integ_cov_limber_qag_quad(cosmo, &F, chimin, chimax, w,
                                      &result, &eresult, &local_status);
          }
          else if(integration_method == ccl_integration_spline) {
            integ_cov_limber_spline(cosmo, &ipar, chimin, chimax,
                                    &result, &local_status);
          }
          else
            local_status = CCL_ERROR_NOT_IMPLEMENTED;

          if ((*ipar.status == 0) && (local_status == 0)) {
            cov_out[lind1+nl1_out*lind2] = result * prefactor_extra;
          }
          else {
            ccl_raise_gsl_warning(local_status, "ccl_cls.c: ccl_angular_cov_limber():");
            cov_out[lind1+nl1_out*lind2] = NAN;
            local_status = CCL_ERROR_INTEG;
          }
        }
      }
    }

    gsl_integration_workspace_free(w);

    if (local_status) {
      #pragma omp atomic write
        *status = local_status;
    }
    ccl_a_finder_free(finda);
  }

  if (*status) {
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_cls.c: ccl_angular_cov_limber(); integration error\n");
  }
}