// lasp_math_raw.c
//
// last-edit-by: J.A. de Jong
//
// Description:
// Operations working on raw arrays of floating point numbers
//////////////////////////////////////////////////////////////////////
#define TRACERPLUS (-5)
#include "lasp_math_raw.h"
#if LASP_USE_BLAS
#include <cblas.h>
#endif

void d_elem_prod_d(d res[],
                   const d arr1[],
                   const d arr2[],
                   const us size) {

    #if LASP_USE_BLAS == 1

    #if LASP_DEBUG

    if(arr1 == arr2) {
        DBGWARN("d_elem_prod_d: Array 1 and array 2 point to the same"
                " memory. This results in pointer aliasing, for which"
                " testing is still to be done. Results might be"
                " unrealiable.");
    }

    #endif

    #if LASP_DOUBLE_PRECISION
    #define elem_prod_fun cblas_dsbmv
    #else
    #define elem_prod_fun cblas_ssbmv
    #endif
    /* These parameters do not matter for this specific case */
    const CBLAS_ORDER  mat_order= CblasColMajor;
    const CBLAS_UPLO   uplo = CblasLower;

    /* Extra multiplication factor */
    const d alpha = 1.0;

    /* void cblas_dsbmv(OPENBLAS_CONST enum CBLAS_ORDER order, */
    /*                  OPENBLAS_CONST enum CBLAS_UPLO Uplo, */
    /*                  OPENBLAS_CONST blasint N, */
    /*                  OPENBLAS_CONST blasint K, */
    /*                  OPENBLAS_CONST double alpha, */
    /*                  OPENBLAS_CONST double *A, */
    /*                  OPENBLAS_CONST blasint lda, */
    /*                  OPENBLAS_CONST double *X, */
    /*                  OPENBLAS_CONST blasint incX, */
    /*                  OPENBLAS_CONST double beta, */
    /*                  double *Y, */
    /*                  OPENBLAS_CONST blasint incY); */

    elem_prod_fun(mat_order,
                  uplo,
                  (blasint) size,
                  0,             // Just the diagonal; 0 super-diagonal bands
                  alpha,        /* Multiplication factor alpha */
                  arr1,
                  1,            /* LDA */
                  arr2,         /* x */
                  1, /* incX = 1 */
                  0.0,          /* Beta */
                  res,    /* The Y matrix to write to */
                  1); /* incY */
    #undef elem_prod_fun

    #else  /* No blas routines, routine is very simple, but here we
            * go! */
    DBGWARN("Performing slow non-blas vector-vector multiplication");
    for(us i=0;i<size;i++) {
        res[i] = arr1[i]*arr2[i];
    }
    #endif
}

void c_hadamard(c res[],
                const c arr1[],
                const c arr2[],
                const us size) {

    fsTRACE(15);
    uVARTRACE(15,size);
    dbgassert(arr1 && arr2 && res,NULLPTRDEREF);

    #if LASP_USE_BLAS == 1

    #if LASP_DEBUG

    if(arr1 == arr2) {
        DBGWARN("c_elem_prod_c: Array 1 and array 2 point to the same"
                " memory. This results in pointer aliasing, for which"
                " testing is still to be done. Results might be"
                " unrealiable.");
    }

    #endif  /* LASP_DEBUG */

    #if LASP_DOUBLE_PRECISION
    #define elem_prod_fun cblas_zgbmv
    #else
    #define elem_prod_fun cblas_cgbmv
    #endif

    c alpha = 1.0;
    c beta = 0.0;

    TRACE(15,"Calling " annestr(elem_prod_fun));
    uVARTRACE(15,size);
    elem_prod_fun(CblasColMajor,
                  CblasNoTrans,
                  (blasint) size, /* M: Number of rows */
                  (blasint) size, /* B: Number of columns */
                  0,              /* KL: Number of sub-diagonals */
                  0,              /* KU: Number of super-diagonals */
                  (d*) &alpha,    /* Multiplication factor */
                  (d*) arr2,      /* A */
                  1,              /* LDA */
                  (d*) arr1,      /* x */
                  1,              /* incX = 1 */
                  (d*) &beta,
                  (d*) res,    /* The Y matrix to write to */
                  1);          /* incY (increment in res) */

    #undef elem_prod_fun

    #else  /* No blas routines, routine is very simple, but here we
            * go! */
    DBGWARN("Performing slower non-blas vector-vector multiplication");
    for(us i=0;i<size;i++) {
        res[i] = arr1[i]*arr2[i];
    }
    #endif
    feTRACE(15);
}

//////////////////////////////////////////////////////////////////////


/**
 *
 * @file pdpltmg_house.c
 *
 *  PLASMA auxiliary routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mathieu Faverge
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:45:13 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *  Parallel tile Householder matrix generation
 *  sequential call to either static or dynamic scheduling
 *
 * See http://www.mathworks.fr/fr/help/matlab/ref/gallery.html#f84-999993
 *
 * Householder matrix
 *
 * Generates a random column vector of size M, and returns the housholder matrix
 * H = eye(n,n) - beta*v*v' that satisfies the relationship
 *
 * H*x = -sign(x(1))*norm(x)*e1
 *
 * where e1 is the first column of eye(n,n). Note that if x is complex, then
 * sign(x) exp(i*arg(x)) (which equals x./abs(x) when x is nonzero).
 *
 */
void plasma_dpltmg_house( PLASMA_desc A, unsigned long long int seed,
                          PLASMA_sequence *sequence, PLASMA_request *request )
{
    plasma_context_t *plasma;
    Quark_Task_Flags task_flags = Quark_Task_Flags_Initializer;
    PLASMA_desc descV;
    double *V;
    double tau;

    plasma = plasma_context_self();
    if (sequence->status != PLASMA_SUCCESS)
        return;
    QUARK_Task_Flag_Set(&task_flags, TASK_SEQUENCE, (intptr_t)sequence->quark_sequence);

    /*
     * Submit A initialization, such that other thread have work while main
     * thread is initializing V
     */
    plasma_dynamic_call_6(plasma_pdlaset,
                          PLASMA_enum,        PlasmaUpperLower,
                          double, 0.,
                          double, 1.,
                          PLASMA_desc,        A,
                          PLASMA_sequence*,   sequence,
                          PLASMA_request*,    request);

    /*
     * Allocate the descriptor of the Householder reflector.
     * Exploit the fact that for one column, the PlasmaCM and PlasmaCCRB storage
     * are identical
     */
    V = (double*) plasma_shared_alloc( plasma, A.m, PlasmaRealDouble );
    descV = plasma_desc_init(
        PlasmaRealDouble, A.mb, A.nb, A.mb*A.nb,
        A.m, 1, 0, 0, A.m, 1 );
    descV.mat = V;

    /* Initialize Householder vector */
    {
        CORE_dplrnt( A.m, 1, V, A.m, A.m, 0, 0, seed );
        LAPACKE_dlarfg_work( A.m, V, V+1, 1, &tau );
        V[0] = 1.;
    }

    /* Computes A = A - tau * V * V' (ger == gemm) */
    plasma_parallel_call_9(plasma_pdgemm,
                           PLASMA_enum,        PlasmaNoTrans,
                           PLASMA_enum,        PlasmaTrans,
                           double, (-tau),
                           PLASMA_desc,        descV,
                           PLASMA_desc,        descV,
                           double, 1.,
                           PLASMA_desc,        A,
                           PLASMA_sequence*,   sequence,
                           PLASMA_request*,    request);

    plasma_dynamic_sync();
    plasma_shared_free( plasma, V );
}


/**
 * Last modified: Tue Aug 30 16:29:05 CST 2016
 * @author: Lie Yan
 * @email: robin.lie.yan@outlook.com
 *
 */

#pragma once

#include <gsl.h>
#include <iostream>

namespace grund {

template<int TypeTag, typename ValueType>
class Unit {
public:
  using intrinsic_type = ValueType;

  Unit() = default;
  Unit(const Unit &rhs) = default;
  Unit(Unit &&rhs) = default;
  Unit &operator=(const Unit &rhs) = default;
  Unit &operator=(Unit &&rhs)      = default;

  constexpr explicit Unit(const ValueType &v) : _value(v) {}
  constexpr explicit operator ValueType() const { return _value; }
  constexpr ValueType value() const { return _value; }

  // unary minus
  constexpr Unit operator-() const { return Unit(-_value); }

  constexpr Unit &operator+=(const Unit &rhs) {
    _value += rhs._value;
    return *this;
  }

  constexpr Unit operator+(const Unit &rhs) const {
    Unit ret(*this);
    return ret += rhs;
  }

  constexpr Unit &operator-=(const Unit &rhs) {
    _value -= rhs._value;
    return *this;
  }

  constexpr Unit operator-(const Unit &rhs) const {
    Unit ret(*this);
    return ret -= rhs;
  }

  constexpr bool operator==(const Unit &rhs) const {
    return _value == rhs._value;
  }

  // derived operator
  constexpr bool operator!=(const Unit &rhs) const {
    return not(*this == rhs);
  }

  constexpr bool operator<(const Unit &rhs) const {
    return _value < rhs._value;
  }

  // derived operator
  constexpr bool operator<=(const Unit &rhs) const {
    return not(rhs < *this);
  }
  // derived operator
  constexpr bool operator>(const Unit &rhs) const { return rhs < *this; }
  // derived operator
  constexpr bool operator>=(const Unit &rhs) const {
    return not(*this < rhs);
  }

  friend std::ostream &operator<<(std::ostream &out, const Unit &m) {
    out << ValueType(m);
    return out;
  }

  template<typename T>
  friend constexpr Unit operator*(const Unit &u, const T &v) {
    return Unit{u.value() * v};
  }

  // derived operator
  template<typename T>
  friend constexpr Unit operator*(const T &v, const Unit &u) {
    return operator*(u, v);
  }

  template<typename T>
  friend constexpr Unit operator/(const Unit &u, const T &v) {
    return Unit{u.value() / v};
  }

  template <typename U>
  U to() const { return U(_value); }

private:
  ValueType _value;
};
}


/* tz_fcmp.c
 *
 * 19-Nov-2007 Initial write: Ting Zhao
 */

#include <gsl/gsl_math.h>
#include "tz_mexutils.h"

void check_argin(int nrhs, const mxArray *prhs[])
{
  if (nrhs > 3  || nrhs < 2)
    mxErrMsgTxt("TZ_FCMP takes 2~3 arguments.");

  if (!(mxIsDouble(prhs[0]) && mxIsDouble(prhs[1])))
    mxErrMsgTxt("The first two arguments must be both double arrays.");

  if (!tz_mxSameSize(prhs[0], prhs[1]) && !tz_mxIsScalar(prhs[0]) &&
      !tz_mxIsScalar(prhs[1]))
    mxErrMsgTxt("The first two arguments must have the same size if neither of them is a scalar.");

  if (nrhs == 3) {
    if (!tz_mxIsDoubleScalar(prhs[2]))
      mxErrMsgTxt("The third arguments must be a double scalar.");
  }
}

/* tz_fcmp(x1, x2) compare two double float numbers.
 * tz_fcmp(x1, x2, ep) compare the double float numbers at the accuracy ep,
 * which is 1e-5 by default.
 */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  check_argin(nrhs, prhs);

  double epsilon = 1e-5;

  if (nrhs == 3) {
    epsilon = *mxGetPr(prhs[2]);
  }

  mwSize length;
  mwSize length1 = tz_mxGetL(prhs[0]);
  mwSize length2 = tz_mxGetL(prhs[1]);
  const mxArray *array = NULL;

  if (mxIsScalar(prhs[0]) && mxIsScalar(prhs[1])) {
    plhs[0] = mxCreateNumericMatrix(1, 1, mxINT8_CLASS, mxREAL);
    length = 1;
  } else if (!mxIsScalar(prhs[0]) && !mxIsScalar(prhs[1])) {
    plhs[0] = mxCreateNumericArray(mxGetNumberOfDimensions(prhs[0]),
				   mxGetDimensions(prhs[0]), mxINT8_CLASS,
				   mxREAL);
    length = length1;
  } else {
    if (length1 == 1) {
      array = prhs[1];
      length = length2;
    } else {
      array = prhs[0];
      length = length1;
    }

    plhs[0] = mxCreateNumericArray(mxGetNumberOfDimensions(array),
				   mxGetDimensions(array), mxINT8_CLASS,
				   mxREAL);
  }

  INT8_T *y = (INT8_T *) mxGetPr(plhs[0]);
  double *x1 = mxGetPr(prhs[0]);
  double *x2 = mxGetPr(prhs[1]);

  mwSize i;
  if (length1 == length2) {
    for (i = 0; i < length; i++) {
      y[i] = (INT8_T) gsl_fcmp(x1[i], x2[i], epsilon);
    }
  } else if (length1 > length2) {
    for (i = 0; i < length; i++) {
      y[i] = (INT8_T) gsl_fcmp(x1[i], x2[0], epsilon);
    }
  } else {
    for (i = 0; i < length; i++) {
      y[i] = (INT8_T) gsl_fcmp(x1[0], x2[i], epsilon);
    }
  }
}


#ifndef ALE_INCLUDE_ALE_H
#define ALE_INCLUDE_ALE_H

#include <nlohmann/json.hpp>
#include <string>
#include <vector>

#include <gsl/gsl_interp.h>

#include <nlohmann/json.hpp>
using json = nlohmann::json;

namespace ale {

  /// Interpolation enum for defining different methods of interpolation
  enum interpolation {
    /// Interpolate using linear interpolation
    linear,
    /// Interpolate using spline interpolation
    spline
  };

  /**
   *@brief Get the position of the spacecraft at a given time based on a set of coordinates, and their associated times
   *@param coords A vector of double vectors of coordinates
   *@param times A double vector of times
   *@param time Time to observe the spacecraft's position at
   *@param interp Interpolation type
   *@return A vector double of the spacecrafts position
   */
  std::vector<double> getPosition(std::vector<std::vector<double>> coords,
                                  std::vector<double> times,
                                  double time, interpolation interp);
  /**
   *@brief Get the velocity of the spacecraft at a given time based on a set of coordinates, and their associated times
   *@param coords A vector of double vectors of coordinates
   *@param times A double vector of times
   *@param time Time to observe the spacecraft's velocity at
   *@param interp Interpolation type
   *@return A vector double of the spacecrafts velocity
   */
  std::vector<double> getVelocity(std::vector<std::vector<double>> coords,
                                  std::vector<double> times,
                                  double time, const interpolation interp);
  /**
   *@brief Get the position of the spacecraft at a given time based on a derived function from a set of coeffcients
   *@param coeffs A vector of double vector of coeffcients
   *@param time Time to observe the spacecraft's position at
   *@return A vector double of the spacecrafts position
   */
  std::vector<double> getPosition(std::vector<std::vector<double>> coeffs, double time);

  /**
   *@brief Get the velocity of the spacecraft at a given time based on a derived function from a set of coeffcients
   *@param coeffs A vector of double vector of coeffcients
   *@param time Time to observe the spacecraft's velocity at
   *@return A vector double of the spacecrafts velocity
   */
  std::vector<double> getVelocity(std::vector<std::vector<double>> coeffs, double time);

  /**
   *@brief Get the rotation of the spacecraft at a given time based on a set of rotations, and their associated times
   *@param rotations A vector of double vector of rotations
   *@param times A double vector of times
   *@param time Time to observe the spacecraft's rotation at
   *@param interp Interpolation type
   *@return A vector double of the spacecrafts rotation
   */
  std::vector<double> getRotation(std::vector<std::vector<double>> rotations,
                                  std::vector<double> times,
                                  double time, interpolation interp);

  /**
   *@brief Get the angular velocity of the spacecraft at a given time based on a set of rotations, and their associated times
   *@param rotations A vector of double vector of rotations
   *@param times A double vector of times
   *@param time Time to observe the spacecraft's angular velocity at
   *@param interp Interpolation type
   *@return A vector double of the spacecrafts angular velocity
   */
  std::vector<double> getAngularVelocity(std::vector<std::vector<double>> rotations,
                                         std::vector<double> times,
                                         double time, interpolation interp);

   /**
    *@brief Get the rotation of the spacecraft at a given time based on a derived function from a set of coeffcients
    *@param coeffs A vector of double vector of coeffcients
    *@param time Time to observe the spacecraft's rotation at
    *@return A vector double of the spacecrafts rotation
    */
  std::vector<double> getRotation(std::vector<std::vector<double>> coeffs, double time);

  /**
   *@brief Get the angular velocity of the spacecraft at a given time based on a derived function from a set of coeffcients
   *@param coeffs A vector of double vector of coeffcients
   *@param time Time to observe the spacecraft's angular velocity at
   *@return A vector double of the spacecrafts angular velocity
   */
  std::vector<double> getAngularVelocity(std::vector<std::vector<double>> coeffs, double time);

  /**
   *@brief Generates a derivatives in respect to time from a polynomial constructed using the given coeffcients, time, and derivation number
   *@param coeffs A double vector of coefficients can be any number of coefficients
   *@param time Time to use when deriving
   *@param d The order of the derivative to generate (Currently supports 0, 1, and 2)
   *@return Evalutation of the given polynomial as a double
   */
  double evaluatePolynomial(std::vector<double> coeffs, double time, int d);

  /**
   *@brief Interpolates the spacecrafts position along a path generated from a set of points,
                times, and a time of observation
   *@param points A double vector of points
   *@param times A double vector of times
   *@param time A double to use as the time of observation
   *@param interp An interpolation enum dictating what type of interpolation to use
   *@param d The order of the derivative to generate when interpolating
                      (Currently supports 0, 1, and 2)
   *@return
   */
  double interpolate(std::vector<double> points, std::vector<double> times, double time, interpolation interp, int d);
  std::string loads(std::string filename, std::string props="", std::string formatter="usgscsm", bool verbose=true);

  json load(std::string filename, std::string props="", std::string formatter="usgscsm", bool verbose=true);

}

#endif // ALE_H


#pragma once

#include "tino/backends/backends.h"
#include "tino/backends/tensor_wrappers.h"
#include "tino/utils/macros.h"

#ifdef TINO_OPENBLAS_READY
#include <cblas.h>
#endif

namespace tino {
  namespace core {
    class context;
  }
  namespace backends {
    namespace blas {
      enum class layout_t {
#ifdef TINO_OPENBLAS_READY
        RowMajor    = CblasRowMajor,
        ColumnMajor = CblasColMajor
#else
        RowMajor,
        ColumnMajor
#endif
      };
      enum class trans_t {
#ifdef TINO_OPENBLAS_READY
        NoTrans = CblasNoTrans,
        Trans   = CblasTrans
#else
        NoTrans,
        Trans
#endif
      };

      template <typename T>
      struct blasOpts {
        T alpha         = (T)0;
        T beta          = (T)0;
        layout_t layout = layout_t::RowMajor;
        trans_t trans_a = trans_t::NoTrans;
        trans_t trans_b = trans_t::NoTrans;
      };

      template <typename T>
      tensor2d<T>& blas_gemm(core::context& ctx, const blasOpts<T>& blas_opts, tensor2d<T>& A, tensor2d<T>& B, tensor2d<T>& C) {}

      template <>
      tensor2d<float>& blas_gemm(core::context& ctx, const blasOpts<float>& blas_opts, tensor2d<float>& A, tensor2d<float>& B, tensor2d<float>& C) {
#ifdef TINO_OPENBLAS_READY
        const int M   = blas_opts.trans_a == trans_t::NoTrans ? A.template shape<1>() : A.template shape<0>();
        const int N   = blas_opts.trans_b == trans_t::NoTrans ? B.template shape<0>() : B.template shape<1>();
        const int K   = blas_opts.trans_a == trans_t::NoTrans ? A.template shape<0>() : A.template shape<1>();
        const int lda = blas_opts.trans_a == trans_t::NoTrans ? K : M;
        const int ldb = blas_opts.trans_b == trans_t::NoTrans ? N : K;
        const int ldc = N;

        cblas_sgemm(static_cast<CBLAS_LAYOUT>(blas_opts.layout),
                    static_cast<CBLAS_TRANSPOSE>(blas_opts.trans_a),
                    static_cast<CBLAS_TRANSPOSE>(blas_opts.trans_b),
                    M,
                    N,
                    K,
                    blas_opts.alpha,
                    A.data(),
                    lda,
                    B.data(),
                    ldb,
                    blas_opts.beta,
                    C.data(),
                    ldc);
#else
        std::cerr << "invalid backend: OpenBLAS" << std::endl;
        std::exit(1);
#endif

        TINO_MAYBE_UNUSED(ctx);
        TINO_MAYBE_UNUSED(blas_opts);
        TINO_MAYBE_UNUSED(A);
        TINO_MAYBE_UNUSED(B);
        TINO_MAYBE_UNUSED(C);

        return C;
      }

    } // namespace blas
  }   // namespace backends
} // namespace tino


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

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

#include "ccl.h"

/*----- ROUTINE: dc_NakamuraSuto -----
INPUT: cosmology, scale factor
TASK: Computes the peak threshold: delta_c(z) assuming LCDM.
Cosmology dependence of the critical linear density according to the spherical-collapse model.
Fitting function from Nakamura & Suto (1997; arXiv:astro-ph/9710107).
*/
double dc_NakamuraSuto(ccl_cosmology *cosmo, double a, int *status){

  double Om_mz = ccl_omega_x(cosmo, a, ccl_species_m_label, status);
  double dc0 = (3./20.)*pow(12.*M_PI,2./3.);
  double dc = dc0*(1.+0.012299*log10(Om_mz));

  return dc;

}

/*----- ROUTINE: Dv_BryanNorman -----
INPUT: cosmology, scale factor
TASK: Computes the virial collapse density contrast with respect to the matter density assuming LCDM.
Cosmology dependence of the virial collapse density according to the spherical-collapse model
Fitting function from Bryan & Norman (1998; arXiv:astro-ph/9710107)
*/
double Dv_BryanNorman(ccl_cosmology *cosmo, double a, int *status){

  double Om_mz = ccl_omega_x(cosmo, a, ccl_species_m_label, status);
  double x = Om_mz-1.;
  double Dv0 = 18.*pow(M_PI,2);
  double Dv = (Dv0+82.*x-39.*pow(x,2))/Om_mz;

  return Dv;
}

static double sigmaM_m2r(ccl_cosmology *cosmo, double halomass, int *status)
{
  double rho_m, smooth_radius;

  // Comoving matter density
  //rho_m = ccl_constants.RHO_CRITICAL*cosmo->params.Omega_m*cosmo->params.h*cosmo->params.h;
  rho_m = ccl_rho_x(cosmo, 1., ccl_species_m_label, 1, status);

  smooth_radius = pow((3.0*halomass) / (4*M_PI*rho_m), (1.0/3.0));

  return smooth_radius;
}

void ccl_cosmology_compute_sigma(ccl_cosmology *cosmo, int *status)
{
  if(cosmo->computed_sigma)
    return;

  // create linearly-spaced values of the mass.
  int nm = cosmo->spline_params.LOGM_SPLINE_NM;
  double *m = NULL;
  double *y = NULL;
  double smooth_radius;
  double na, nb;

  m = ccl_linear_spacing(cosmo->spline_params.LOGM_SPLINE_MIN, cosmo->spline_params.LOGM_SPLINE_MAX, nm);
  if (m == NULL ||
      (fabs(m[0]-cosmo->spline_params.LOGM_SPLINE_MIN)>1e-5) ||
      (fabs(m[nm-1]-cosmo->spline_params.LOGM_SPLINE_MAX)>1e-5) ||
      (m[nm-1]>10E17)) {
    *status = CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo,"ccl_cosmology_compute_sigmas(): Error creating linear spacing in m\n");
  }

  if (*status == 0) {
    // create space for y, to be filled with sigma and dlnsigma_dlogm
    y = malloc(sizeof(double)*nm);
    if (y == NULL) {
      *status = CCL_ERROR_MEMORY;
    }
  }

  // start up of GSL pointers
  int gslstatus = 0;
  gsl_spline *logsigma = NULL;
  gsl_spline *dlnsigma_dlogm = NULL;

  // fill in sigma, if no errors have been triggered at this time.
  if (*status == 0) {
    for (int i=0; i<nm; i++) {
      smooth_radius = sigmaM_m2r(cosmo, pow(10,m[i]), status);
      y[i] = log(ccl_sigmaR(cosmo, smooth_radius, 1., status));
    }
    logsigma = gsl_spline_alloc(cosmo->spline_params.M_SPLINE_TYPE, nm);
    if (logsigma == NULL) {
      *status = CCL_ERROR_MEMORY;
    }
  }

  if (*status == 0) {
    gslstatus = gsl_spline_init(logsigma, m, y, nm);
    if (gslstatus != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE ;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_massfunc.c: ccl_cosmology_compute_sigma(): Error creating sigma(M) spline\n");
    }
  }

  // again, making splines assuming nothing bad has happened to this point
  if (*status == 0 ) {
    for (int i=0; i<nm; i++) {
      if(i==0) {
        gslstatus |= gsl_spline_eval_e(logsigma, m[i], NULL,&na);
        gslstatus |= gsl_spline_eval_e(logsigma, m[i]+cosmo->spline_params.LOGM_SPLINE_DELTA/2., NULL,&nb);
        y[i] = 2.*(na-nb)*y[i] / cosmo->spline_params.LOGM_SPLINE_DELTA;
      }
      else if (i==nm-1) {
        gslstatus |= gsl_spline_eval_e(logsigma, m[i]-cosmo->spline_params.LOGM_SPLINE_DELTA/2., NULL,&na);
        gslstatus |= gsl_spline_eval_e(logsigma, m[i], NULL,&nb);
        y[i] = 2.*(na-nb)*y[i] / cosmo->spline_params.LOGM_SPLINE_DELTA;
      }
      else {
        gslstatus |= gsl_spline_eval_e(logsigma, m[i]-cosmo->spline_params.LOGM_SPLINE_DELTA/2., NULL,&na);
        gslstatus |= gsl_spline_eval_e(logsigma, m[i]+cosmo->spline_params.LOGM_SPLINE_DELTA/2., NULL,&nb);
        y[i] = (na-nb) / cosmo->spline_params.LOGM_SPLINE_DELTA;
      }
    }

    if(gslstatus != GSL_SUCCESS ) {
      ccl_raise_gsl_warning(
        gslstatus, "ccl_massfunc.c: ccl_cosmology_compute_sigma():");
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_massfunc.c: ccl_cosmology_compute_sigma(): "
        "Error evaluating grid points for dlnsigma/dlogM spline\n");
    }
  }

  if (*status == 0) {
    dlnsigma_dlogm = gsl_spline_alloc(cosmo->spline_params.M_SPLINE_TYPE, nm);
    if (dlnsigma_dlogm == NULL) {
      *status = CCL_ERROR_MEMORY;
    }
  }

  if (*status == 0) {
    gslstatus = gsl_spline_init(dlnsigma_dlogm, m, y, nm);
    if (gslstatus != GSL_SUCCESS) {
      *status = CCL_ERROR_SPLINE ;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_massfunc.c: ccl_cosmology_compute_sigma(): Error creating dlnsigma/dlogM spline\n");
    }
  }

  if (*status == 0) {
    cosmo->data.logsigma = logsigma;
    cosmo->data.dlnsigma_dlogm = dlnsigma_dlogm;
    cosmo->computed_sigma = true;
  } else {
    gsl_spline_free(logsigma);
    gsl_spline_free(dlnsigma_dlogm);
  }

  free(m);
  free(y);
}

/*----- ROUTINE: ccl_sigma_M -----
INPUT: ccl_cosmology * cosmo, double halo mass in units of Msun, double scale factor
TASK: returns sigma from the sigmaM interpolation. Also computes the sigma interpolation if
necessary.
*/
double ccl_sigmaM(ccl_cosmology *cosmo, double log_halomass, double a, int *status)
{
  double sigmaM;
  // Check if sigma has already been calculated
  if (!cosmo->computed_sigma) {
    *status = CCL_ERROR_SIGMA_INIT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_massfunc.c: ccl_sigmaM(): linear power spctrum has not been computed!");
    return NAN;
  }

  double lgsigmaM;

  int gslstatus = gsl_spline_eval_e(cosmo->data.logsigma, log_halomass, NULL, &lgsigmaM);

  if(gslstatus != GSL_SUCCESS) {
    ccl_raise_gsl_warning(gslstatus, "ccl_massfunc.c: ccl_sigmaM():");
    *status |= gslstatus;
  }

  // Interpolate to get sigma
  sigmaM = exp(lgsigmaM)*ccl_growth_factor(cosmo, a, status);
  return sigmaM;
}

/*----- ROUTINE: ccl_dlnsigM_dlogM -----
INPUT: ccl_cosmology *cosmo, double halo mass in units of Msun
TASK: returns the value of the derivative of ln(sigma^-1) with respect to log10 in halo mass.
*/
double ccl_dlnsigM_dlogM(ccl_cosmology *cosmo, double log_halomass, int *status)
{
  // Check if sigma has already been calculated
  if (!cosmo->computed_sigma) {
    *status = CCL_ERROR_SIGMA_INIT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_massfunc.c: ccl_sigmaM(): linear power spctrum has not been computed!");
    return NAN;
  }

  double dlsdlgm;
  int gslstatus = gsl_spline_eval_e(cosmo->data.dlnsigma_dlogm,
				    log_halomass, NULL, &dlsdlgm);
  if(gslstatus) {
    ccl_raise_gsl_warning(gslstatus, "ccl_massfunc.c: ccl_dlnsigM_dlogM():");
    *status |= gslstatus;
  }
  return dlsdlgm;
}


/*  Standard C Libraries  */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "interpolation.h"
#include "float.h"

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

typedef struct {
    int start_index;
    int end_index;
    int n;
    double* x;
    double* y;
} segment;

typedef struct {
    int n;
    segment * seg;
} segments;

void free_segments(segments segs)
{
    for(int i=0; i<segs.n; i++)
    {
        free(segs.seg[i].x);
        free(segs.seg[i].y);
    }
//    free(segs.seg);
}

/***
 * What is segment:
 * mask                    | 00000000000000000111111111111100000000000000000 |
 *                         |       signal    |  no signal  |   signal        |
 * signal data             | ~~|~~~~~~~|~~~~~|_____________|~~~~~|~~~~~~~|~~ |
 * params                  |   | edges | cut |             | cut | edges |   |
 * stored for intrpolation |   |-------|                         |-------|   |
 * will interpolate        |           |-------------------------|           |
 * start_index             |                 X                               |
 * end_index               |                               X                 |
 * start_data              |   X                                             |
 * start_data_to           |           X                                     |
 * end_data                |                                     X           |
 ***/

segments add_new_segment(segments segs, int start_index, int end_index, int edges, int cut, vector data)
{
    int start_data = start_index - edges - cut;
    if(start_data < 0) start_data = 0;

    int start_data_to = start_index - cut;
    if(start_data_to < 0) start_data_to = 0;

    int end_data = end_index + cut;
    if(end_data > data.x) end_data = data.x - 1;

    int new_size = segs.n + 1;
    segs.seg = realloc(segs.seg, sizeof(segment)*new_size);

    int data_size = edges * 2;

    segs.seg[segs.n].start_index = start_data_to;
    segs.seg[segs.n].end_index = end_data;
    segs.seg[segs.n].n = data_size;
    segs.seg[segs.n].x = calloc(data_size, sizeof(double));
    segs.seg[segs.n].y = calloc(data_size, sizeof(double));

    for(int i=0; i<edges; i++)
    {
        int x = i + start_data;
        segs.seg[segs.n].x[i] = x;
        segs.seg[segs.n].y[i] = getv(data, x);

        x = edges + i + end_data;
        segs.seg[segs.n].x[i+edges] = x;
        segs.seg[segs.n].y[i+edges] = getv(data, x);
    }

    segs.n = new_size;
    return segs;
}

segments vector_split_by_mask(vector data, vector mask, int edges, int cut)
{
    segments segs;
    segs.n = 0;
    segs.seg = calloc(1, sizeof(segment));

    int start_index = -1;
    int end_index = -1;

    for(int i=0; i<data.x; i++)
    {
        if(start_index == -1 && getv(mask, i) == 1.0)
        {
            start_index = i;
            continue;
        }
        if(start_index != -1 && getv(mask, i) != 1.0)
        {
            end_index = i;
            segs = add_new_segment(segs, start_index, end_index, edges, cut, data);

            start_index = -1;
            end_index = -1;
            continue;
        }
    }

//    if(start_index != -1)
//    {
//        end_index = data.x - 1;
//        segs = add_new_segment(segs, start_index, end_index, edges, 0, data);
//    }

    return segs;
}

vector vector_interpolate(vector data, vector mask, int cut, const gsl_interp_type * T)
{
    int edges = gsl_interp_type_min_size(T);
    segments segs = vector_split_by_mask(data, mask, edges, cut);

    vector result = copyv(data);

    for(int i=0; i<segs.n; i++)
    {
        gsl_interp_accel *acc = gsl_interp_accel_alloc ();
        gsl_spline *liner = gsl_spline_alloc(T, segs.seg[i].n);
        gsl_spline_init (liner, segs.seg[i].x, segs.seg[i].y, segs.seg[i].n);

        for(int j=segs.seg[i].start_index; j<segs.seg[i].end_index; j++)
        {
            setv(result, j, gsl_spline_eval (liner, j, acc));
        }

        gsl_spline_free (liner);
        gsl_interp_accel_free (acc);
    }

    free_segments(segs);

    return result;
}

vector vector_interpolate_by_mask(vector data, vector mask, int cut, int type)
{
    vector result;
    switch (type) {
    case 1: // Linear interpolation
        result = vector_interpolate(data, mask, cut, gsl_interp_linear);
        break;
    case 2: // Cubic spline (тatural boundary conditions)
        result = vector_interpolate(data, mask, cut, gsl_interp_cspline);
        break;
    case 3: // Cubic spline (periodic boundary conditions)
        result = vector_interpolate(data, mask, cut, gsl_interp_cspline_periodic);
        break;
    case 4: // Non-rounded Akima spline (natural boundary conditions)
        result = vector_interpolate(data, mask, cut, gsl_interp_akima);
        break;
    case 5: // Non-rounded Akima spline (periodic boundary conditions)
        result = vector_interpolate(data, mask, cut, gsl_interp_akima_periodic);
        break;
    case 6: // Steffen’s method
        result = vector_interpolate(data, mask, cut, gsl_interp_steffen);
        break;
    default:
        result = copyv(data);
        break;
    }
    return result;
}

void interpolate_part(vector * data, int start, int end, const gsl_interp_type * T)
{
    int edges = gsl_interp_type_min_size(T);

    int start_index = start - edges;
    if (start_index < 0) start_index = 0;
    int end_index = end + edges;
    if (end_index >= data->x) end_index = data->x;

    int n = (start - start_index) + (end_index - end) + 1;
    int index = 0;
    vector x = zerov(n);
    vector y = zerov(n);

    for(int i=start_index; i<=start; i++)
    {
//        printf("i %i\n", i);
//        fprintf(stdout, "val %f\n", data->v[i]);
        setv(y, index, data->v[i]);
        setv(x, index, i);
        index++;
    }

    for(int i=end; i<end_index; i++)
    {
//        printf("i %i\n", i);
//        fprintf(stdout, "val %f\n", data->v[i]);
        setv(y, index, getv(*data, i));
        setv(x, index, i);
        index++;
    }

//    for (int i=start_index; i<end_index; i++)
//    {
//        if((i<=start_index || i>=end_index)&&(index<n)&&(data->v[i]!=0||i==start_index||i==end_index))
//        {
//            setv(y, index, getv(data, i));
//            setv(x, index, i);
//            index++;
//        }
//    }

    n = index;

    gsl_interp_accel *acc = gsl_interp_accel_alloc ();
    gsl_spline *liner = gsl_spline_alloc(T, n);
    gsl_spline_init (liner, x.v, y.v, n);

    for(int j=(start); j<end; j++)
    {
        data->v[j] = gsl_spline_eval (liner, j, acc);
    }

    gsl_spline_free (liner);
    gsl_interp_accel_free (acc);

    freev(x);
    freev(y);
}

void vector_interpolate_part(vector * data, int start, int end, int type)
{
    if(end - start > 1)
        switch (type) {
        case 1: // Linear interpolation
            interpolate_part(data, start, end, gsl_interp_linear);
            break;
        case 2: // Cubic spline (тatural boundary conditions)
            interpolate_part(data, start, end, gsl_interp_cspline);
            break;
        case 3: // Cubic spline (periodic boundary conditions)
            interpolate_part(data, start, end, gsl_interp_cspline_periodic);
            break;
        case 4: // Non-rounded Akima spline (natural boundary conditions)
            interpolate_part(data, start, end, gsl_interp_akima);
            break;
        case 5: // Non-rounded Akima spline (periodic boundary conditions)
            interpolate_part(data, start, end, gsl_interp_akima_periodic);
            break;
        case 6: // Steffen’s method
             interpolate_part(data, start, end, gsl_interp_steffen);
            break;
        }
}


#include <gsl/gsl_odeiv2.h>
#include "func.h"
#include "make_grid.h"

int
make_grid (struct param params, double r, double r1, double y[])
{
  int status;
  int *fake_jac;
  /* The integrator RK8PD (Runge-Kutta Prince-Dormand) doesn't need the
   * jacobian (some more sophisticated integrators do) so make this a
   * null pointer.  The linker will complain that the pointer type is
   * incompatable since it doesn't point to a function with a bunch of
   * arguments, but the pointer won't be used anyway so it doesn't
   * matter. */
  fake_jac = 0;

  gsl_odeiv2_system sys = { func, fake_jac, 2, &params };
  gsl_odeiv2_driver *d = gsl_odeiv2_driver_alloc_y_new
    (&sys, gsl_odeiv2_step_rk8pd,
     1.0e+0, 1.0e-8, 1.0e-8);

  status = gsl_odeiv2_driver_apply (d, &r, r1, y);

  gsl_odeiv2_driver_free (d);
  return 0;
}


#ifndef INCLUDED_TELEPATH_LAPACK_DRIVERS_LLS_H
#define INCLUDED_TELEPATH_LAPACK_DRIVERS_LLS_H

#include <lapacke.h>

namespace telepath{

    template< typename T >
    auto posv( int matrix_order, char uplo, std::size_t n, std::size_t nrhs,
        T* a, std::size_t lda, T* b, std::size_t ldb );
    template<>
    inline auto posv<float>( int matrix_order, char uplo, std::size_t n,
        std::size_t nrhs, float* a, std::size_t lda, float* b,
        std::size_t ldb ){
        return LAPACKE_sposv( matrix_order, uplo, n, nrhs, a, lda, b, ldb );
    }
    template<>
    inline auto posv<double>( int matrix_order, char uplo, std::size_t n,
        std::size_t nrhs, double* a, std::size_t lda, double* b,
        std::size_t ldb ){
        return LAPACKE_dposv( matrix_order, uplo, n, nrhs, a, lda, b, ldb );
    }
    template<>
    inline auto posv<std::complex<float> >( int matrix_order, char uplo,
        std::size_t n, std::size_t nrhs, std::complex<float>* a,
        std::size_t lda, std::complex<float>* b, std::size_t ldb ){
        return LAPACKE_cposv( matrix_order, uplo, n, nrhs, a, lda, b, ldb );
    }
    template<>
    inline auto posv<std::complex<double> >( int matrix_order, char uplo,
        std::size_t n, std::size_t nrhs, std::complex<double>* a,
        std::size_t lda, std::complex<double>* b, std::size_t ldb ){
        return LAPACKE_zposv( matrix_order, uplo, n, nrhs, a, lda, b, ldb );
    }
    inline auto posv( int matrix_order, char uplo, std::size_t n,
        std::size_t nrhs, double* a, std::size_t lda, double* b,
        std::size_t ldb, double* x, std::size_t ldx, lapack_int* iter ){
        return LAPACKE_dsposv( matrix_order, uplo, n, nrhs, a, lda, b, ldb,
            x, ldx, iter );
    }
    inline auto posv( int matrix_order, char uplo, std::size_t n,
        std::size_t nrhs, std::complex<double>* a, std::size_t lda,
        std::complex<double>* b, std::size_t ldb, std::complex<double>* x,
        std::size_t ldx, lapack_int* iter ){
        return LAPACKE_zcposv( matrix_order, uplo, n, nrhs, a, lda, b, ldb,
            x, ldx, iter );
    }

    template< typename T >
    auto gelsd( int matrix_order, std::size_t m, std::size_t n,
        std::size_t nrhs, T* a, std::size_t lda, T* b, std::size_t ldb,
        T* s, T rcond, lapack_int* rank );
    template<>
    inline auto gelsd<double>( int matrix_order, std::size_t m, std::size_t n,
        std::size_t nrhs, double* a, std::size_t lda, double* b,
        std::size_t ldb,double* s, double rcond, lapack_int* rank ){
        return LAPACKE_dgelsd( matrix_order, m, n, nrhs, a, lda, b, ldb,
            s, rcond, rank );
    }
    template<>
    inline auto gelsd<float>( int matrix_order, std::size_t m, std::size_t n,
        std::size_t nrhs, float* a, std::size_t lda, float* b,
        std::size_t ldb,float* s, float rcond, lapack_int* rank ){
        return LAPACKE_sgelsd( matrix_order, m, n, nrhs, a, lda, b, ldb,
            s, rcond, rank );
    }
    template<>
    inline auto gelsd<std::complex<double> >( int matrix_order, std::size_t m,
        std::size_t n, std::size_t nrhs, std::complex<double>* a,
        std::size_t lda, std::complex<double>* b, std::size_t ldb,
        std::complex<double>* s, std::complex<double> rcond, lapack_int* rank ){
        return LAPACKE_zgelsd( matrix_order, m, n, nrhs, a, lda, b, ldb,
            s, rcond, rank );
    }
    template<>
    inline auto gelsd<std::complex<float> >( int matrix_order, std::size_t m,
        std::size_t n, std::size_t nrhs, std::complex<float>* a,
        std::size_t lda, std::complex<float>* b, std::size_t ldb,
        std::complex<float>* s, std::complex<float> rcond, lapack_int* rank ){
        return LAPACKE_cgelsd( matrix_order, m, n, nrhs, a, lda, b, ldb,
            s, rcond, rank );
    }

} //namespace telepath

#endif


#include "stdio.h"
#include "gsl/gsl_integration.h"
#include "gsl/gsl_errno.h"
#include "non_limber.h"
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_monte_vegas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_sf_bessel.h>

// This is a workspace size for the gsl integrator
#define LIMBER_FIXED_TABLE_SIZE 4096

// data that is passed into the integrator
// This is everything we need to compute the
// integrand
struct ClIntegrandData {
	double ell;
	double chimin;
	double chimax;
	gsl_spline * WbX;
	gsl_spline * WbY;
    gsl_spline * D_chi;
	gsl_spline2d * P;
	gsl_interp_accel * accelerator_wx;
	gsl_interp_accel * accelerator_wy;
	gsl_interp_accel * accelerator_px;
	gsl_interp_accel * accelerator_py;
	gsl_interp_accel * accelerator_growth;
};

double cl_integrand(double *x, size_t dim, void *data_void)
{
	struct ClIntegrandData * data = (struct ClIntegrandData*) data_void;

	double chi1 = x[0];
	double log_delta = x[1];
	double log_nu = x[2];
	double nu = exp(log_nu);
	double delta = exp(log_delta);
	double chi2_p = chi1 * ( 1 + delta);
	double chi2_m = chi1 * (1 - delta);
	double logk = log_nu / chi1;

	if(chi2_m < data->chimin || chi2_p > data->chimax) return 0.0;

	double FX = gsl_spline_eval(data->WbX, chi1, data->accelerator_wx);
	double FY_p = gsl_spline_eval(data->WbY, chi2_p, data->accelerator_wy);
	double FY_m = gsl_spline_eval(data->WbY, chi2_m, data->accelerator_wy);

	double growth_1 = gsl_spline_eval(data->D_chi, chi1, data->accelerator_growth);
	double growth_2_p = gsl_spline_eval(data->D_chi, chi2_p, data->accelerator_growth);
	double growth_2_m = gsl_spline_eval(data->D_chi, chi2_m, data->accelerator_growth);

	// Get P(k,chi)
	// Deactivate error handling
	gsl_error_handler_t * old_handler = gsl_set_error_handler_off();

	double p;
	int status = gsl_spline2d_eval_e(data->P, chi1, logk, data->accelerator_px, data->accelerator_py, &p);

	// Restore the old error handler
	gsl_set_error_handler(old_handler);
	if (status)
	{
		//printf(stderr,'spline failed, for now, assume this is because k was out of range and return 0...should probably be more careful here though.');
		return 0.0;
	}
	double j1 = gsl_sf_bessel_jl(data->ell, nu);
	double j2_p = gsl_sf_bessel_jl(data->ell, nu*(1+delta));
	double j2_m = gsl_sf_bessel_jl(data->ell, nu*(1-delta));
	double prefac = 2 * delta * 2 * nu * nu * nu * p / growth_1 / M_PI / chi1 / chi1 ;
	return prefac * ( FY_p * growth_2_p * j2_p + FY_m * growth_2_m * j2_m );
}

static double inline gsl_spline_min_x(gsl_spline * s)
{
	return s->x[0];
}
static double inline gsl_spline_max_x(gsl_spline * s)
{
	return s->x[s->size-1];
}

int get_chi_mean_sigma(double chimin, double chimax, gsl_spline * WbX, gsl_spline * WbY,
				gsl_interp_accel * accelerator_wx,	gsl_interp_accel * accelerator_wy,
				double *chi_mean, double *chi_sigma)
{
    int nchi = 1000;
    double dchi = (chimax-chimin)/nchi;
    double chi = chimin;
    double wxwy_sum = 0.;
    double wxwy2_sum = 0.;
    double wx,wy;
    for (int i=0; i<nchi; i++){
    	wx = gsl_spline_eval(WbX, chi, accelerator_wx);
    	wy = gsl_spline_eval(WbY, chi, accelerator_wy);
    	wxwy_sum += wx*wy*chi;
    	wxwy2_sum += wx*wy*chi*chi;
    	chi += dchi;
    }
    *chi_mean = wxwy_sum*dchi;
    *chi_sigma = sqrt(wxwy2_sum*dchi - *chi_mean * *chi_mean);
    return 0;
}

int cl_integral(cl_config * config, gsl_spline * WbX,
			    gsl_spline * WbY, gsl_spline2d * P, gsl_spline * D_chi,
			    double * cl_out, double * cl_err_out)
{

    config->status = LIMBER_STATUS_ERROR;
    int any_parameter_error=0;
    if (WbX==NULL){
        fprintf(stderr, "NULL WX parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (WbY==NULL){
        fprintf(stderr, "NULL WY parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (P==NULL){
        fprintf(stderr, "NULL P parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (D_chi==NULL){
        fprintf(stderr, "NULL D_chi parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (config->n_ell<0){
        fprintf(stderr, "Negative n_ell parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (any_parameter_error){
        return 1;
    }

	config->status = LIMBER_STATUS_OK;

    static int n_ell_zero_warning = 0;
    if (config->n_ell==0){
        if (n_ell_zero_warning==0){
            fprintf(stderr, "Warning: n_ell=0 in Limber. Will not be treated as an error. Warning once only per process.\n");
        }
        n_ell_zero_warning = 1;
        return 1;
    }

	// Get the appropriate ranges over which to integrate
	// It is assumed that (at least one of) the kernel
	// splines should go to zero in some kind of reasonable
	// place, so we just use the range they specify
	struct ClIntegrandData data;
	double chimin_x = gsl_spline_min_x(WbX);
	double chimin_y = gsl_spline_min_x(WbY);
	double chimax_x = gsl_spline_max_x(WbX);
	double chimax_y = gsl_spline_max_x(WbY);
	double c_ell, c_ell_error;

	// Workspaces
    const gsl_rng_type *T;
    gsl_rng *r;
    gsl_rng_env_setup ();
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);
    size_t calls = 5000000;

	double reltol = config->relative_tolerance;
	double abstol = config->absolute_tolerance;

	// Take the smallest range since we want both the
	// splines to be valid there.
	// This range as well as all the data needed to compute
	// the integrand is put into a struct to be passed
	// through the integrator to the function above.
	double chimin = chimin_x>chimin_y ? chimin_x : chimin_y;
    double chimax = chimax_x<chimax_y ? chimax_x : chimax_y;
	data.chimin = chimin_y;
	data.chimax = chimax_y;
	data.WbX = WbX;
	data.WbY = WbY;
	data.P = P;
	data.accelerator_wx = gsl_interp_accel_alloc();
	data.accelerator_wy = gsl_interp_accel_alloc();
	data.accelerator_px = gsl_interp_accel_alloc();
	data.accelerator_py = gsl_interp_accel_alloc();
	data.accelerator_growth = gsl_interp_accel_alloc();
	data.D_chi = D_chi;

	double chi_mean, chi_sigma;
	get_chi_mean_sigma(chimin, chimax, WbX, WbY,
				data.accelerator_wx, data.accelerator_wy,
				&chi_mean, &chi_sigma);

	// save ell values for return
	double ell_vector[config->n_ell];
	double nu_0;

    gsl_monte_vegas_state *s = gsl_monte_vegas_alloc (3);

	// loop through ell values according to the input configuration
	for (int i_ell = 0; i_ell<config->n_ell; i_ell++){
		double ell = config->ell[i_ell];
		double log_nu_0 = log(ell+0.5);
		double log_nu_min = log_nu_0 - config->log_nu_range;
		double log_nu_max = log_nu_0 + config->log_nu_range;
		printf("delta_range_factor %f\n", config->delta_range_factor);
		double delta_min = 1. / ell / config->delta_range_factor;
		double delta_max = 1. * config->delta_range_factor / ell;

		double log_delta_min = log(delta_min);
		double log_delta_max = log(delta_max);
		printf("log_delta_min,max %f %f\n", delta_min, delta_max);
		double xl[3] = { chimin_x, log_delta_min, log_nu_min };
		double xu[3] = { chimax_x, log_delta_max, log_nu_max };
		data.ell=ell;
		gsl_monte_function G = {&cl_integrand, 3, &data};

        gsl_monte_vegas_integrate (&G, xl, xu, 3, 10000, r, s,
                               &c_ell, &c_ell_error);
        gsl_monte_vegas_integrate (&G, xl, xu, 3, calls/5, r, s,
                               &c_ell, &c_ell_error);
        gsl_monte_vegas_init (s);

		//Include the prefactor scaling
		c_ell *= config->prefactor;
		// Record the results into arrays
		cl_out[i_ell] = c_ell;
		cl_err_out[i_ell] = c_ell_error;
		ell_vector[i_ell] = ell;
	}
	gsl_monte_vegas_free (s);

	// Tidy up
	gsl_interp_accel_free(data.accelerator_wx);
	gsl_interp_accel_free(data.accelerator_wy);
	gsl_interp_accel_free(data.accelerator_px);
	gsl_interp_accel_free(data.accelerator_py);
	gsl_interp_accel_free(data.accelerator_growth);

	// These two are not deallocated because they are static and only initialized once.
	// gsl_integration_glfixed_table_free(table);
	// gsl_integration_workspace_free(W);

	// And that's it
	return 0;
}


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

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_math.h> // gsl_finite

#ifdef _OPENMP
#include <omp.h>
#endif

const size_t N=10000;
const size_t T=1000;
const size_t K=20;
const size_t L=2;
const double alpha_k = 0.5;

/*
 * 0: thompson sampling
 * 1: uniform subset
 *
 */
const unsigned int strategy = 0;

#include "helpers.h"
#include "model.h"
#include "set_counter.h"

#define to_string(arr, k) \
    for (size_t i = 0; i < k-1; i++) { \
        printf("%.16lf,", arr[i]); \
    } \
    printf("%.16lf", arr[k-1]);

unsigned int
move_gibbs(double *restrict random_numbers, double logth[K], size_t ngames,
           const size_t games[ngames][L], const size_t game_counts[ngames],
           const size_t win_counts[K])
{
    double ll, ll_p;
    double alpha;
    double logth_comp_old;
    double logth_Km1_old;
    unsigned int accepted = 0;

    for (size_t comp=0; comp<K-1; comp++) {
        assert(gsl_fcmp(log_sum_exp(logth, K), 1.0, 1e-15) == 0);
        ll = fullcond(comp, logth, ngames, games, game_counts, win_counts);

        /* sample a suitable value for the current component */
        logth_comp_old = logth[comp];
        logth_Km1_old = logth[K-1];

        if (logth_comp_old > logth[K-1]) {
            logth[comp] = log(*random_numbers++) + logth_comp_old + log1p(exp(logth[K-1] - logth_comp_old));
            logth[K-1] = logth_comp_old + log1p(exp(logth_Km1_old - logth_comp_old) - exp(logth[comp] - logth_comp_old));
        } else {
            logth[comp] = log(*random_numbers++) + logth_Km1_old + log1p(exp(logth_comp_old - logth_Km1_old));
            logth[K-1] = logth_Km1_old + log1p(exp(logth_comp_old - logth_Km1_old) - exp(logth[comp] - logth_Km1_old));
        }

        /* compute full conditional density at th_p */
        ll_p = fullcond(comp, logth, ngames, games, game_counts, win_counts);

        alpha = *random_numbers++;
        if (log(alpha) < ll_p - ll) {
            /* accept */
            accepted = 1;
        }
        else {
            /* reject */
            /* reset the proposed component back to its original value */
            /* th[K-1] += th[comp] - th_comp_old; */
            /* th[comp] = th_comp_old; */
            /* assert(th[K-1] >= 0); */
            logth[comp] = logth_comp_old;
            logth[K-1] = logth_Km1_old;
        }
    }

    return accepted;
}

void
resample_move(const gsl_rng *r, double logtheta[N][K], const double w[N],
              const struct set_counter *games_counter, const size_t wins[K])
{
    unsigned int cnt[N];
    size_t accepted = 0;

    gsl_ran_multinomial(r, N, N, w, cnt);

    /* populate particles */
    double (*logtheta_new)[K] = malloc(N * sizeof *logtheta_new);
    size_t n_new = 0;
    for (size_t n=0; n<N; n++) {
        for (size_t i=0; i < cnt[n]; i++) {
            memcpy(logtheta_new[n_new++], logtheta[n], sizeof *logtheta_new);
        }
    }

    /* pre-generate random numbers to avoid thread synchronization */
    double (*random_numbers)[2*(K-1)] = malloc(N * sizeof *random_numbers);
    for (size_t n=0; n<N; n++) {
        for (size_t k=0; k<2*(K-1); k++) {
            random_numbers[n][k] = gsl_rng_uniform_pos(r);
        }
    }

    /* extract game counts from the counter */
    size_t ngames = games_counter->size;
    size_t (*games)[L] = malloc(ngames * sizeof *games);
    size_t *game_counts = malloc(ngames * sizeof *game_counts);
    set_counter_keys(games_counter, games);
    set_counter_values(games_counter, game_counts);

#pragma omp parallel for reduction(+:accepted)
    for (size_t n=0; n<N; n++) {
        accepted += move_gibbs(random_numbers[n], logtheta_new[n], ngames, games, game_counts, wins);
    }

    printf("# to_move = %zu\n", N);
    printf("# accepted = %zu\n", accepted);
    printf("# acceptance ratio = %lf\n", (double) accepted / N);

    memcpy(logtheta, logtheta_new, N*K*sizeof(double));
    free(logtheta_new);
    free(random_numbers);
    free(games);
    free(game_counts);
}

void
sample_theta_star(const gsl_rng *r, double theta_star[K])
{
    double a[K];
    for (size_t k=0; k<K; k++) {
        a[k] = alpha_k;
    }

    gsl_ran_dirichlet(r, K, a, theta_star);
}

void
read_theta_star(const char *file_name, double theta_star[K])
{
    char buf[80];
    FILE *ts = fopen(file_name, "r");

    if (!ts) {
        fprintf(stderr, "error reading %s\n", file_name);
        exit(EXIT_FAILURE);
    }

    for (size_t k = 0; k<K; k++) {
        if (!fgets(buf, 80, ts)) {
            fprintf(stderr, "error reading %s\n", file_name);
            exit(EXIT_FAILURE);
        }
        theta_star[k] = atof(buf);
    };
    fclose(ts);
}

void
sim(const gsl_rng *r, const double theta_star[K])
{
    double (*logtheta)[K] = malloc(N * sizeof *logtheta);
    double *w = malloc(N * sizeof(double));
    double *logw = malloc(N * sizeof(double));

    ones(w, N);
    zeros(logw, N);

    size_t *wins = calloc(K, sizeof *wins);
    struct set_counter *games_counter = set_counter_alloc();

    /* general info */
    printf("# generator type: %s\n", gsl_rng_name(r));
    printf("# seed = %lu\n", gsl_rng_default_seed);
    printf("\n");

    /* sample N particles from the `uniform` prior */
    {
        double alpha[K];
        ones(alpha, K);
        double theta[K];
        for (size_t n = 0; n < N; n++) {
            gsl_ran_dirichlet(r, K, alpha, theta);
#pragma omp simd
            for (size_t k=0; k<K; k++)
                logtheta[n][k] = log(theta[k]);
        }
    }

    for(size_t t = 0; t < T; t++) {
        fprintf(stderr, "t = %zu\r", t); /* for progress monitoring */
        printf("# iteration = %zu\n", t);

        size_t players[L];
        if (strategy == 0) {
            /* presentation strategy: thompson sampling */
            printf("# strategy: thompson\n");
            /* sample a theta from the current posterior */
            gsl_ran_discrete_t *g = gsl_ran_discrete_preproc(N, w);
            size_t theta_sample_idx = gsl_ran_discrete(r, g);
            gsl_ran_discrete_free(g);

            printf("# sampled theta: ");
            to_string(logtheta[theta_sample_idx], K);
            printf("\n");

            /* pick L elements from current sample */
            gsl_sort_largest_index(players, L, logtheta[theta_sample_idx], 1, K);
        } else if (strategy == 1) {
            /* presentation strategy: uniform subset */
            printf("# strategy: uniform subset\n");
            size_t idx[K];
            for (size_t k=0; k<K; k++)
                idx[k] = k;

            gsl_ran_choose(r, players, L, idx, K, sizeof(size_t));
        }

        set_counter_add(games_counter, players);
        printf("# number of unique subsets so far: %zu\n", games_counter->size);

        double player_w[L];

        printf("# game: ");
        for (size_t l=0; l<L-1; l++) {
            printf("%zu,", players[l]);
            player_w[l] = theta_star[players[l]];
        }
        printf("%zu\n", players[L-1]);
        player_w[L-1] = theta_star[players[L-1]];

        printf("# player weights = ");
        to_string(player_w, L);
        printf("\n");

        /* determine outcome using theta_star */
        size_t winner;
        {
            gsl_ran_discrete_t *g = gsl_ran_discrete_preproc(L, player_w);
            size_t wn = gsl_ran_discrete(r, g);
            winner = players[wn];
            gsl_ran_discrete_free(g);
        }
        printf("# winner: %zu\n", winner);

        wins[winner]++;

        /* update weights */
#pragma omp parallel for
        for(size_t n = 0; n < N; n++) {
            double logtheta_winner = logtheta[n][winner];
            double lth_game[L];
            for (size_t l=0; l<L; l++) {
                lth_game[l] = logtheta[n][players[l]];
            }
            logw[n] += logtheta_winner - log_sum_exp(lth_game, L);
        }

        /* compute w from logw */
        {
            double lse = log_sum_exp(logw, N);
            for (size_t n=0; n<N; n++) {
                w[n] = exp(logw[n] - lse);
                assert(gsl_finite(w[n]) == 1);
            }
        }

        /* compute ess and perform resampling if necessary */
        {
            double two_logw[N];
            for (size_t n=0; n<N; n++)
                two_logw[n] = 2*logw[n];

            double ess = exp(2*log_sum_exp(logw, N) - log_sum_exp(two_logw, N));

            printf("# ess = %lf\n", ess);

            if (ess < .5*N) {
                printf("# resampling at iteration %zu\n", t);
                resample_move(r, logtheta, w, games_counter, wins);
                ones(w, N);
                zeros(logw, N);
            }
        }

        printf("\n");
    }
    fprintf(stderr, "\n");

    /* resample at the end  */
    printf("# resampling at iteration %zu\n", T);
    resample_move(r, logtheta, w, games_counter, wins);
    /* no need to reset the weights at this point but just to be safe... */
    ones(w, N);
    zeros(logw, N);

    for(size_t n = 0; n < N; n++) {
        to_string(logtheta[n], K);
        printf("\n");
    }

    /* cleanup */
    free(logtheta);
    free(w);
    free(logw);
    free(wins);
    set_counter_free(games_counter);
}

int
main(int argc, char *argv[])
{
    const gsl_rng_type *t;
    gsl_rng *r;

    gsl_rng_env_setup();

    t = gsl_rng_default;
    r = gsl_rng_alloc(t);

    gsl_set_error_handler_off();

    double *theta_star = malloc(K * sizeof(double));
    /* read theta_star from a file */
    if (argc==2)
        read_theta_star(argv[1], theta_star);
    else
        sample_theta_star(r, theta_star);

    printf("# K = %zu\n", K);
    printf("# N = %zu\n", N);
    printf("# T = %zu\n", T);
    printf("# L = %zu\n", L);
    printf("# theta_star = ");
    to_string(theta_star, K);
    printf("\n");

    // perform simulation
    sim(r, theta_star);

    // cleanup
    free(theta_star);
    gsl_rng_free(r);

    exit(EXIT_SUCCESS);
}


/* fft/test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * 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.
 */

#include <config.h>
#include <string.h>
#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>

#include <gsl/gsl_complex.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_dft_complex.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_fft_complex_float.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_real_float.h>
#include <gsl/gsl_fft_halfcomplex.h>
#include <gsl/gsl_fft_halfcomplex_float.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_test.h>

void my_error_handler (const char *reason, const char *file,
                       int line, int err);

#include "complex_internal.h"

/* Usage: test [n]
   Exercise the fft routines for length n. By default n runs from 1 to 100.
   The exit status indicates success or failure. */

#define BASE_DOUBLE
#include "templates_on.h"
#include "compare_source.c"
#include "bitreverse.c"
#include "test_complex_source.c"
#include "test_real_source.c"
#include "test_trap_source.c"
#include "templates_off.h"
#undef  BASE_DOUBLE

#define BASE_FLOAT
#include "templates_on.h"
#include "compare_source.c"
#include "bitreverse.c"
#include "test_complex_source.c"
#include "test_real_source.c"
#include "test_trap_source.c"
#include "templates_off.h"
#undef  BASE_FLOAT

int
main (int argc, char *argv[])
{
  size_t i;
  size_t start = 1, end = 99;
  size_t stride ;
  size_t n = 0;

  gsl_ieee_env_setup ();

  if (argc == 2)
    n = strtol (argv[1], NULL, 0);

  if (n)
    {
      start = n ;
      end = n ;
    }

  for (i = 1 ; i <= end ; i *= 2)
    {
      if (i >= start)
        {
          for (stride = 1 ; stride < 4 ; stride++)
            {
              test_complex_bitreverse_order (stride, i) ;
              test_complex_radix2 (stride, i) ;
              test_real_bitreverse_order (stride, i) ;
              test_real_radix2 (stride, i) ;
            }
        }
    }

  for (i = start ; i <= end ; i++)
    {
      for (stride = 1 ; stride < 4 ; stride++)
        {
          test_complex_func (stride, i) ;
          test_complex_float_func (stride, i) ;
          test_real_func (stride, i) ;
          test_real_float_func (stride, i) ;
        }
    }

  gsl_set_error_handler (&my_error_handler);
  test_trap () ;
  test_float_trap () ;

  exit (gsl_test_summary ());
}

void
my_error_handler (const char *reason, const char *file, int line, int err)
{
  if (0) printf ("(caught [%s:%d: %s (%d)])\n", file, line, reason, err) ;
}


// https://www.gnu.org/software/gsl/doc/html/fft.html
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>

int
main (void)
{
  int i, n = 100;
  double data[n];

  gsl_fft_real_wavetable * real;
  gsl_fft_halfcomplex_wavetable * hc;
  gsl_fft_real_workspace * work;

  for (i = 0; i < n; i++)
    {
      data[i] = 0.0;
    }

  for (i = n / 3; i < 2 * n / 3; i++)
    {
      data[i] = 1.0;
    }

  for (i = 0; i < n; i++)
    {
      printf ("%d: %e\n", i, data[i]);
    }
  printf ("\n");

  work = gsl_fft_real_workspace_alloc (n);
  real = gsl_fft_real_wavetable_alloc (n);

  gsl_fft_real_transform (data, 1, n,
                          real, work);

  gsl_fft_real_wavetable_free (real);

  for (i = 11; i < n; i++)
    {
      data[i] = 0;
    }

  hc = gsl_fft_halfcomplex_wavetable_alloc (n);

  gsl_fft_halfcomplex_inverse (data, 1, n,
                               hc, work);
  gsl_fft_halfcomplex_wavetable_free (hc);

  for (i = 0; i < n; i++)
    {
      printf ("%d: %e\n", i, data[i]);
    }

  gsl_fft_real_workspace_free (work);
  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

// convex hull is obtained as cvh(n_bin[0],...,n_bin[end])
// inputs are y = Ef(x). x:n_data by 4, y:n_data by 1
void obtain_convex_h_Nd(int n_data, double *ef2cvh, int *x, double *y, int Ndim, int *nCat_min, int *nCat_max, double *E0) {

    int i, j, k, l, nid, id1, id2, ctr;
    double tmp;
    int convex_h_ids[n_data];   //preallocated, not all filled
    double convex_h_x[n_data][Ndim];   //preallocated, not all filled
    double convex_h_y[n_data];  //preallocated, not all filled

    printf("Convex hull is created.\n");
    printf("Input data\n");
    printf("---------------------------\n");
    printf("  nL   nN   nM   nC   nV    Ef\n");
    for (i=0;i<n_data;i++) {
        for (j=0;j<Ndim;j++)
            printf("%4d ",x[Ndim*i+j]);
        printf("  % .4f\n",y[i]);
    }
    printf("---------------------------\n");

    // calculate the gradient and find Ndim base convex hull points
    double dEdx[n_data][Ndim];
    double dx[n_data][Ndim];
    for (i=0;i<n_data;i++) {
        printf("%d-data:%d,%d,%d,%d,%d, dE/dx=[",i,x[i*Ndim],x[i*Ndim+1],x[i*Ndim+2],x[i*Ndim+3],x[i*Ndim+4]);
        for (j=0;j<Ndim;j++) {
            dx[i][j] = (double) (nCat_max[j]-x[i*Ndim+j])*1.0/(nCat_max[j]-nCat_min[j]);
            if (dx[i][j] == 0.0)  // case of convex hull
                dEdx[i][j] = 0.0;
            else
                dEdx[i][j] = (E0[j]-y[i]) / dx[i][j];
            printf("% .2e ",dEdx[i][j]);
        }
        printf("]\n");
    }
    // check if the point is the convex hull
    int ctr_set[Ndim];
    nid = 0;
    for (i=0;i<n_data;i++) {
        if (dx[i][j] == 0.0) {
            convex_h_ids[nid] = i;
            for (j=0;j<Ndim;j++)
                convex_h_x[nid][j] = (double) x[i*Ndim+j]*1.0;
            convex_h_y[nid] = y[i];
            nid++;
            continue;
        }
        for (k=0;k<Ndim;k++) {
            ctr_set[k] = 1;
            for (j=0;j<n_data;j++) {
                if (j==i)
                    continue;
                if (dEdx[j][k] < dEdx[i][k]) {
                    ctr_set[k] = 0;
                    break;
                }
            }
        }
        ctr = 0;
        for (k=0;k<Ndim;k++)
            ctr += ctr_set[k];
        if (ctr > 0) {
            convex_h_ids[nid] = i;
            for (j=0;j<Ndim;j++)
                convex_h_x[nid][j] = (double) x[i*Ndim+j]*1.0;
            convex_h_y[nid] = y[i];
            nid++;
        }
    }
    printf("%d cvh points were identified.\n",nid);
    for (i=0;i<nid;i++) {
        printf("%d ",convex_h_ids[i]);
        for (j=0;j<Ndim;j++)
            printf("%f ",convex_h_x[i][j]);
        printf("%f\n",convex_h_y[i]);
    }

    // A*c = b
    gsl_matrix * A = gsl_matrix_alloc (Ndim,Ndim);
    gsl_matrix * V = gsl_matrix_alloc (Ndim,Ndim);
    gsl_vector * S = gsl_vector_alloc (Ndim);
    gsl_vector * work = gsl_vector_alloc (Ndim);
    gsl_vector * b = gsl_vector_alloc (Ndim);
    gsl_vector * c = gsl_vector_alloc (Ndim);

    double dij2[nid];
    int ids[nid];
    double ef_cvh;
    for (i=0;i<n_data;i++) {
        for (j=0;j<nid;j++) {
            dij2[j] = 0.0;
            for (k=0;k<Ndim;k++) {
                tmp = (double) x[i*Ndim+k]*1.0;
                dij2[j] += pow(tmp - convex_h_x[j][k],2);
            }
            ids[j] = j;
        }
        sort_array_ids(dij2,ids,nid);
        if (dij2[0]==0.0)
            ef2cvh[i] = 0.0;
        else {
            for (j=0;j<Ndim;j++) {
                for (k=0;k<Ndim;k++)
                    gsl_matrix_set (A,k,j,convex_h_x[ids[j]][k]);
                gsl_vector_set(b,j,x[i*Ndim+j]*1.0);
            }
            gsl_linalg_SV_decomp(A, V, S, work);
            gsl_linalg_SV_solve (A, V, S, b, c);
            ef_cvh = 0.0;
            for (j=0;j<Ndim;j++)
                ef_cvh += gsl_vector_get(c,j)*convex_h_y[ids[j]];
            ef2cvh[i] = y[i]-ef_cvh;
        }
    }

    gsl_matrix_free(A);
    gsl_matrix_free(V);
    gsl_vector_free(S);
    gsl_vector_free(work);
    gsl_vector_free(b);
    gsl_vector_free(c);

}


/**
 * @file modalbeamformer.h
 * @brief Beamforming in the spherical harmonics domain.
 * @author Kenichi Kumatani
 */
#ifndef MODALBEAMFORMER_H
#define MODALBEAMFORMER_H

#include <stdio.h>
#include <assert.h>
#include <float.h>

#include <gsl/gsl_block.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_fft_complex.h>
#include <common/refcount.h>
#include "common/jexception.h"

#include "stream/stream.h"
//#include "stream/pyStream.h"
#include "beamformer/spectralinfoarray.h"
#include "modulated/modulated.h"
#include "beamformer/beamformer.h"

// ----- definition for class `ModeAmplitudeCalculator' -----
//
gsl_complex modeAmplitude(int order, double ka);

class ModeAmplitudeCalculator {
 public:
  ModeAmplitudeCalculator(int order, float minKa=0.01, float maxKa=20, float wid=0.01);

  ~ModeAmplitudeCalculator();

  gsl_vector_complex *get() const { return mode_amplitude_; }

private:
  gsl_vector_complex *mode_amplitude_;
  float minKa_;
  float maxKa_;
  float wid_;
};

typedef refcount_ptr<ModeAmplitudeCalculator> ModeAmplitudeCalculatorPtr;

// ----- definition for class `EigenBeamformer' -----
//

/**
   @class EigenBeamformer
   @brief This beamformer is implemented based on Meyer and Elko's ICASSP paper.
          In Boaz Rafaely's paper, this method is referred to as the phase-mode beamformer
   @usage
   1) construct this object, bf = EigenBeamformer(...)
   2) set the radious of the spherical array with bf.set_eigenmike_geometry() or bf.set_array_geometry(..).
   3) set the look direction with bf.set_look_direction(..)
   4) process each block with bf.next() until it hits the end
*/
class EigenBeamformer : public  SubbandDS {
public:
  EigenBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "EigenBeamformer");
  ~EigenBeamformer();
  virtual const gsl_vector_complex* next(int frame_no = -5);
  virtual void reset();
  virtual unsigned dim() const { return dim_;}

  void set_sigma2(float sigma2){ sigma2_ = sigma2; }
  void set_weight_gain(float wgain){ wgain_ = wgain; }
  void set_eigenmike_geometry();
  void set_array_geometry(double a,  gsl_vector *theta_s, gsl_vector *phi_s);
  virtual void set_look_direction(double theta, double phi);

  const gsl_matrix_complex *mode_amplitudes();
  const gsl_vector *array_geometry(int type); // type==0 -> theta, type==1 -> phi
  virtual gsl_matrix *beampattern(unsigned fbinX, double theta = 0, double phi = 0,
                                  double minTheta=-M_PI, double maxTheta=M_PI,
                                  double minPhi=-M_PI, double maxPhi=M_PI,
                                  double widthTheta=0.1, double widthPhi=0.1 );
  /**
     @brief obtain the spherical transformation coefficients at each frame
     @return spherical harmonics transformation coefficients at the current frame
   */
  virtual SnapShotArrayPtr snapshot_array() const { return(st_snapshot_array_); }
  virtual SnapShotArrayPtr snapshot_array2() const { return(snapshot_array_); }

  const gsl_matrix_complex *blocking_matrix(unsigned fbinX, unsigned unitX=0 ) const {
    return (bfweight_vec_[unitX]->B())[fbinX];
  }

#ifdef ENABLE_LEGACY_BTK_API
  void setSigma2(float sigma2){ set_sigma2(sigma2); }
  void setWeightGain(float wgain){ set_weight_gain(wgain); }
  void setEigenMikeGeometry(){ set_eigenmike_geometry(); }
  void setArrayGeometry(double a,  gsl_vector *theta_s, gsl_vector *phi_s){ set_array_geometry(a, theta_s, phi_s); }
  virtual void setLookDirection(double theta, double phi){ set_look_direction(theta, phi); }
  const gsl_matrix_complex *getModeAmplitudes(){ return mode_amplitudes(); }
  const gsl_vector *getArrayGeometry(int type){ return array_geometry(type);}
  virtual gsl_matrix *getBeamPattern( unsigned fbinX, double theta = 0, double phi = 0,
                                      double minTheta=-M_PI, double maxTheta=M_PI,
                                      double minPhi=-M_PI, double maxPhi=M_PI,
                                      double widthTheta=0.1, double widthPhi=0.1 ){
    return beampattern(fbinX, theta, phi, minTheta, maxTheta, minPhi, maxPhi, widthTheta, widthPhi);
  }
  virtual SnapShotArrayPtr getSnapShotArray(){ return snapshot_array(); }
  virtual SnapShotArrayPtr getSnapShotArray2(){ return snapshot_array2(); }
  const gsl_matrix_complex *getBlockingMatrix(unsigned fbinX, unsigned unitX=0){
    return blocking_matrix(fbinX, unitX);
  }
#endif

 protected:
  virtual void calc_weights_( unsigned fbinX, gsl_vector_complex *weights );
  virtual bool calc_spherical_harmonics_at_each_position_( gsl_vector *theta_s, gsl_vector *phi_s ); // need to be tested!!
  virtual bool calc_steering_unit_(  int unitX=0, bool isGSC=false );
  virtual bool alloc_steering_unit_( int unitN=1 );
  void alloc_image_( bool flag=true );
  bool calc_mode_amplitudes_();

  unsigned samplerate_;
  unsigned NC_;
  unsigned maxOrder_;
  unsigned dim_; // the number of the spherical harmonics transformation coefficients
  bool weights_normalized_;
  gsl_matrix_complex *mode_mplitudes_; // [maxOrder_] the mode amplitudes.
  gsl_vector_complex *F_; // Spherical Transform coefficients [dim_]
  gsl_vector_complex **sh_s_; // Conjugate of spherical harmonics at each sensor position [dim_][nChan]: Y_n^{m*}
  SnapShotArrayPtr st_snapshot_array_; // for compatibility with a post-filtering object

  double theta_; // look direction
  double phi_;   // look direction

  double a_;     // the radius of the rigid sphere.
  gsl_vector *theta_s_; // sensor positions
  gsl_vector *phi_s_;   // sensor positions
  gsl_matrix *beampattern_;
  gsl_vector *WNG_; // white noise gain
  float wgain_; //
  float sigma2_; // dialog loading
};

typedef Inherit<EigenBeamformer,  SubbandDSPtr> EigenBeamformerPtr;

// ----- definition for class DOAEstimatorSRPEB' -----
//

/**
   @class DOAEstimatorSRPEB
   @brief estimate the direction of arrival based on the maximum steered response power
   @usage
   1) construct this object, doaEstimator = DOAEstimatorSRPEB(...)
   2) set the radious of the spherical array, doaEstimator.set_eigenmike_geometry() or doaEstimator.set_array_geometry(..).
   3) process each block, doaEstimator.next()
   4) get the N-best hypotheses at the current instantaneous frame through doaEstimator.nbest_doas()
   5) do doaEstimator.getFinalNBestHypotheses() after a static segment is processed.
      You can then obtain the averaged N-best hypotheses of the static segment with doaEstimator.nbest_doas().
*/
class DOAEstimatorSRPEB :
  public DOAEstimatorSRPBase, public EigenBeamformer {
  public:
  DOAEstimatorSRPEB( unsigned nBest, unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "DirectionEstimatorSRPBase");
  ~DOAEstimatorSRPEB();

  const gsl_vector_complex* next(int frame_no = -5);
  void reset();

protected:
  virtual void  calc_steering_unit_table_();
  virtual float calc_response_power_( unsigned uttX );
};

typedef Inherit<DOAEstimatorSRPEB, EigenBeamformerPtr> DOAEstimatorSRPEBPtr;

// ----- definition for class `SphericalDSBeamformer' -----
//

/**
   @class SphericalDSBeamformer
   @usage
   1) construct this object, mb = SphericalDSBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
   @note this implementation is based on Boaz Rafaely's letter,
   "Phase-Mode versus Delay-and-Sum Spherical Microphone Array", IEEE Signal Processing Letters, vol. 12, Oct. 2005.
*/
class SphericalDSBeamformer : public EigenBeamformer {
public:
  SphericalDSBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "SphericalDSBeamformer");
  ~SphericalDSBeamformer();
  virtual gsl_vector *calc_wng();

#ifdef ENABLE_LEGACY_BTK_API
  virtual gsl_vector *calcWNG(){ return calc_wng(); }
#endif

protected:
  virtual void calc_weights_( unsigned fbinX, gsl_vector_complex *weights );
  virtual bool calc_spherical_harmonics_at_each_position_( gsl_vector *theta_s, gsl_vector *phi_s );
};

typedef Inherit<SphericalDSBeamformer, EigenBeamformerPtr> SphericalDSBeamformerPtr;

// ----- definition for class `DualSphericalDSBeamformer' -----
//

/**
   @class DualSphericalDSBeamformer
   @usage
   1) construct this object, mb = SphericalDSBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
   @note In addition to  SphericalDSBeamformer, this class has an object of the *normal* D&S beamformer
*/
class DualSphericalDSBeamformer : public SphericalDSBeamformer {
public:
  DualSphericalDSBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "SphericalDSBeamformer");
  ~DualSphericalDSBeamformer();

  virtual SnapShotArrayPtr snapshot_array() const { return snapshot_array_; }
  virtual BeamformerWeights* beamformer_weight_object(unsigned srcX=0) const {
    return bfweight_vec2_[srcX];
  }

#ifdef ENABLE_LEGACY_BTK_API
  virtual SnapShotArrayPtr getSnapShotArray(){ return snapshot_array(); }
#endif

protected:
  virtual void calc_weights_( unsigned fbinX, gsl_vector_complex *weights );
  virtual bool alloc_steering_unit_( int unitN=1 );

  vector<BeamformerWeights *>                   bfweight_vec2_; // weights of a normal D&S beamformer.
};

typedef Inherit<DualSphericalDSBeamformer, SphericalDSBeamformerPtr> DualSphericalDSBeamformerPtr;

// ----- definition for class DOAEstimatorSRPPSphDSB' -----
//

class DOAEstimatorSRPSphDSB : public DOAEstimatorSRPBase, public SphericalDSBeamformer {
public:
  DOAEstimatorSRPSphDSB( unsigned nBest, unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "DOAEstimatorSRPPSphDSB" );
  ~DOAEstimatorSRPSphDSB();
  const gsl_vector_complex* next(int frame_no = -5);
  void reset();

protected:
  virtual void  calc_steering_unit_table_();
  virtual float calc_response_power_( unsigned uttX );
};

typedef Inherit<DOAEstimatorSRPSphDSB, SphericalDSBeamformerPtr> DOAEstimatorSRPSphDSBPtr;

// ----- definition for class `SphericalHWNCBeamformer' -----
//

/**
   @class SphericalHWNCBeamformer
   @usage
   1) construct this object, mb = SphericalDSBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
*/
class SphericalHWNCBeamformer : public EigenBeamformer {
public:
  SphericalHWNCBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, float ratio=1.0, const String& nm = "SphericalHWNCBeamformer");
  ~SphericalHWNCBeamformer();

  virtual gsl_vector *calc_wng();
  virtual void set_wng( double ratio){ ratio_=ratio; calc_wng();}

#ifdef ENABLE_LEGACY_BTK_API
  gsl_vector *calcWNG(){ return calc_wng(); }
  void setWNG( double ratio){ set_wng(ratio);}
#endif

protected:
  virtual void calc_weights_( unsigned fbinX, gsl_vector_complex *weights );

protected:
  float ratio_;
};

typedef Inherit<SphericalHWNCBeamformer, EigenBeamformerPtr> SphericalHWNCBeamformerPtr;

// ----- definition for class `SphericalGSCBeamformer' -----
//

/**
   @class SphericalGSCBeamformer
   @usage
   1) construct this object, mb = SphericalDSBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
   @note this implementation is based on Boaz Rafaely's letter,
   "Phase-Mode versus Delay-and-Sum Spherical Microphone Array", IEEE Signal Processing Letters, vol. 12, Oct. 2005.
*/
class SphericalGSCBeamformer : public SphericalDSBeamformer {
public:
  SphericalGSCBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "SphericalGSCBeamformer");
  ~SphericalGSCBeamformer();

  virtual const gsl_vector_complex* next(int frame_no = -5);
  virtual void reset();

  void set_look_direction(double theta, double phi);
  void set_active_weights_f(unsigned fbinX, const gsl_vector* packedWeight);

#ifdef ENABLE_LEGACY_BTK_API
  void setLookDirection(double theta, double phi){ set_look_direction(theta, phi); }
  void setActiveWeights_f( unsigned fbinX, const gsl_vector* packedWeight ){ set_active_weights_f(fbinX, packedWeight); }
#endif
};

typedef Inherit<SphericalGSCBeamformer, SphericalDSBeamformerPtr> SphericalGSCBeamformerPtr;

// ----- definition for class `SphericalHWNCGSCBeamformer' -----
//

/**
   @class SphericalHWNCGSCBeamformer
   @usage
   1) construct this object, mb = SphericalHWNCGSCBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
*/
class SphericalHWNCGSCBeamformer : public SphericalHWNCBeamformer {
public:
  SphericalHWNCGSCBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, float ratio=1.0, const String& nm = "SphericalHWNCGSCBeamformer");
  ~SphericalHWNCGSCBeamformer();

  virtual const gsl_vector_complex* next(int frame_no = -5);
  virtual void reset();

  void set_look_direction(double theta, double phi);
  void set_active_weights_f(unsigned fbinX, const gsl_vector* packedWeight);

#ifdef ENABLE_LEGACY_BTK_API
  void setLookDirection(double theta, double phi){ set_look_direction(theta, phi); }
  void setActiveWeights_f( unsigned fbinX, const gsl_vector* packedWeight ){ set_active_weights_f(fbinX, packedWeight); }
#endif
};

typedef Inherit<SphericalHWNCGSCBeamformer, SphericalHWNCBeamformerPtr> SphericalHWNCGSCBeamformerPtr;

// ----- definition for class `DualSphericalGSCBeamformer' -----
//

/**
   @class DualSphericalGSCBeamformer
   @usage
   1) construct this object, mb = DualSphericalGSCBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
   @note In addition to DualSphericalGSCBeamformer, this class has an object of the *normal* D&S beamformer
*/
class DualSphericalGSCBeamformer : public SphericalGSCBeamformer {
public:
  DualSphericalGSCBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "DualSphericalGSCBeamformer");
  ~DualSphericalGSCBeamformer();

  virtual SnapShotArrayPtr snapshot_array() const {return(snapshot_array_);}
  virtual BeamformerWeights* beamformer_weight_object(unsigned srcX=0) const {
    return bfweight_vec2_[srcX];
  }

#ifdef ENABLE_LEGACY_BTK_API
  virtual SnapShotArrayPtr getSnapShotArray(){return(snapshot_array_);}
#endif

protected:
  virtual void calc_weights_( unsigned fbinX, gsl_vector_complex *weights );
  virtual bool alloc_steering_unit_( int unitN=1 );

  vector<BeamformerWeights *>                   bfweight_vec2_; // weights of a normal D&S beamformer.
};

typedef Inherit<DualSphericalGSCBeamformer, SphericalGSCBeamformerPtr> DualSphericalGSCBeamformerPtr;

// ----- definition for class `SphericalMOENBeamformer' -----
//

/**
   @class SphericalGSCBeamformer
   @usage
   1) construct this object, mb = SphericalDSBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
   @note this implementation is based on Z. Li and R. Duraiswami's letter,
   "Flexible and Optimal Design of Spherical Microphone Arrays for Beamforming", IEEE Trans. SAP.
*/
class SphericalMOENBeamformer : public SphericalDSBeamformer {
public:
  SphericalMOENBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "SphericalMOENBeamformer");
  ~SphericalMOENBeamformer();

  virtual const gsl_vector_complex* next(int frame_no = -5);
  virtual void reset();
  void fix_terms(bool flag){ is_term_fixed_ = flag; }
  void set_diagonal_looading(unsigned fbinX, float diagonalWeight);
  virtual SnapShotArrayPtr snapshot_array() const { return snapshot_array_; }
  virtual gsl_matrix *beampattern(unsigned fbinX, double theta = 0, double phi = 0,
                                  double minTheta=-M_PI, double maxTheta=M_PI,
                                  double minPhi=-M_PI, double maxPhi=M_PI,
                                  double widthTheta=0.1, double widthPhi=0.1 );

#ifdef ENABLE_LEGACY_BTK_API
  void fixTerms( bool flag ){ fix_terms(flag); }
  void setLevelOfDiagonalLoading( unsigned fbinX, float diagonalWeight){ set_diagonal_looading(fbinX, diagonalWeight); }
  virtual gsl_matrix *getBeamPattern( unsigned fbinX, double theta = 0, double phi = 0,
                                      double minTheta=-M_PI, double maxTheta=M_PI,
                                      double minPhi=-M_PI, double maxPhi=M_PI,
                                      double widthTheta=0.1, double widthPhi=0.1 ){
    return beampattern(fbinX, theta, phi, minTheta, maxTheta, minPhi, maxPhi, widthTheta, widthPhi);
  }
#endif

protected:
  virtual bool alloc_steering_unit_( int unitN=1 );
  virtual void calc_weights_( unsigned fbinX, gsl_vector_complex *weights );
  bool calc_moen_weights_( unsigned fbinX, gsl_vector_complex *weights, double dThreshold = 1.0E-8, bool calcInverseMatrix = true, unsigned unitX=0 );

private:
  // maxOrder_ == Neff in the Li's paper.
  unsigned             bf_order_; // N in the Li's paper.
  float                CN_;
  gsl_matrix_complex** A_; /* A_[fftLen2+1][dim_][nChan]; Coeffcients of the spherical harmonics expansion; See Eq. (31) & (32) */
  gsl_matrix_complex** fixedW_; /* _fixedW[fftLen2+1][nChan][dim_]; [ A^H A + l^2 I ]^{-1} A^H */
  gsl_vector_complex** BN_;     // _BN[fftLen2+1][dim_]
  float*               diagonal_weights_;
  bool                 is_term_fixed_;
  float                dthreshold_;
};

typedef Inherit<SphericalMOENBeamformer, SphericalDSBeamformerPtr> SphericalMOENBeamformerPtr;

// ----- definition for class `SphericalSpatialDSBeamformer' -----
//

/**
   @class SphericalSpatialDSBeamformer
   @usage
   1) construct this object, mb = SphericalSpatialDSBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
   @note this implementation is based on Boaz Rafaely's letter,
   "Phase-Mode versus Delay-and-Sum Spherical Microphone Array", IEEE Signal Processing Letters, vol. 12, Oct. 2005.
*/
class SphericalSpatialDSBeamformer : public SphericalDSBeamformer {
public:
  SphericalSpatialDSBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, const String& nm = "SphericalSpatialDSBeamformer");
  ~SphericalSpatialDSBeamformer();
  virtual const gsl_vector_complex* next(int frame_no = -5);

protected:
  virtual void calc_weights_( unsigned fbinX, gsl_vector_complex *weights );
  virtual bool alloc_steering_unit_( int unitN = 1 );
  virtual bool calc_steering_unit_( int unitX = 0, bool isGSC = false );
};

typedef Inherit<SphericalSpatialDSBeamformer, SphericalDSBeamformerPtr> SphericalSpatialDSBeamformerPtr;

// ----- definition for class `SphericalSpatialHWNCBeamformer' -----
//

/**
   @class SphericalSpatialHWNCBeamformer
   @usage
   1) construct this object, mb = SphericalDSBeamformer(...)
   2) set the radious of the spherical array mb.set_array_geometry(..) or
   3) set the look direction mb.set_look_direction()
   4) process each block mb.next()
*/
class SphericalSpatialHWNCBeamformer : public SphericalHWNCBeamformer {
public:
  SphericalSpatialHWNCBeamformer( unsigned sampleRate, unsigned fftLen = 512, bool halfBandShift = false, unsigned NC=1, unsigned maxOrder=4, bool normalizeWeight=false, float ratio=1.0, const String& nm = "SphericalHWNCBeamformer");
  ~SphericalSpatialHWNCBeamformer();
  virtual const gsl_vector_complex* next(int frame_no = -5);

protected:
  virtual void calc_weights_( unsigned fbinX, gsl_vector_complex *weights );
  virtual bool alloc_steering_unit_( int unitN = 1 );
  virtual bool calc_steering_unit_( int unitX = 0, bool isGSC = false );

private:
  gsl_matrix_complex *calc_diffuse_noise_model_( unsigned fbinX );

  gsl_matrix_complex **SigmaSI_; // SigmaSI_[fftLen/2+1][chanN]
  double  dthreshold_;
};

typedef Inherit<SphericalSpatialHWNCBeamformer, SphericalHWNCBeamformerPtr> SphericalSpatialHWNCBeamformerPtr;

#endif