/* specfunc/legendre_Qn.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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_bessel.h"
#include "gsl_sf_elementary.h"
#include "gsl_sf_exp.h"
#include "gsl_sf_pow_int.h"
#include "gsl_sf_legendre.h"

#include "error.h"

/* Evaluate f_{ell+1}/f_ell
 * f_ell := Q^{b}_{a+ell}(x)
 * x > 1
 */
static
int
legendreQ_CF1_xgt1(int ell, double a, double b, double x, double * result)
{
  const double RECUR_BIG = GSL_SQRT_DBL_MAX;
  const int maxiter = 5000;
  int n = 1;
  double Anm2 = 1.0;
  double Bnm2 = 0.0;
  double Anm1 = 0.0;
  double Bnm1 = 1.0;
  double a1 = ell + 1.0 + a + b;
  double b1 = (2.0*(ell+1.0+a) + 1.0) * x;
  double An = b1*Anm1 + a1*Anm2;
  double Bn = b1*Bnm1 + a1*Bnm2;
  double an, bn;
  double fn = An/Bn;

  while(n < maxiter) {
    double old_fn;
    double del;
    double lna;
    n++;
    Anm2 = Anm1;
    Bnm2 = Bnm1;
    Anm1 = An;
    Bnm1 = Bn;
    lna = ell + n + a;
    an = b*b - lna*lna;
    bn = (2.0*lna + 1.0) * x;
    An = bn*Anm1 + an*Anm2;
    Bn = bn*Bnm1 + an*Bnm2;

    if(fabs(An) > RECUR_BIG || fabs(Bn) > RECUR_BIG) {
      An /= RECUR_BIG;
      Bn /= RECUR_BIG;
      Anm1 /= RECUR_BIG;
      Bnm1 /= RECUR_BIG;
      Anm2 /= RECUR_BIG;
      Bnm2 /= RECUR_BIG;
    }

    old_fn = fn;
    fn = An/Bn;
    del = old_fn/fn;

    if(fabs(del - 1.0) < 4.0*GSL_DBL_EPSILON) break;
  }

  *result = fn;

  if(n == maxiter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* Uniform asymptotic for Q_l(x).
 * Assumes x > -1.0 and x != 1.0.
 * Discards second order and higher terms.
 */
static
int
legendre_Ql_asymp_unif(const double ell, const double x, gsl_sf_result * result)
{
  if(x < 1.0) {
    double u   = ell + 0.5;
    double th  = acos(x);
    gsl_sf_result Y0, Y1;
    int stat_Y0, stat_Y1;
    int stat_m;
    double pre;
    double B00;
    double sum;

    /* B00 = 1/8 (1 - th cot(th) / th^2
     * pre = sqrt(th/sin(th))
     */
    if(th < GSL_ROOT4_DBL_EPSILON) {
      B00 = (1.0 + th*th/15.0)/24.0;
      pre = 1.0 + th*th/12.0;
    }
    else {
      double sin_th = sqrt(1.0 - x*x);
      double cot_th = x / sin_th;
      B00 = 1.0/8.0 * (1.0 - th * cot_th) / (th*th);
      pre = sqrt(th/sin_th);
    }

    stat_Y0 = gsl_sf_bessel_Y0_e(u*th, &Y0);
    stat_Y1 = gsl_sf_bessel_Y1_e(u*th, &Y1);

    sum = -0.5*M_PI * (Y0.val + th/u * Y1.val * B00);

    stat_m = gsl_sf_multiply_e(pre, sum, result);
    result->err += 0.5*M_PI * fabs(pre) * (Y0.err + fabs(th/u*B00)*Y1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_3(stat_m, stat_Y0, stat_Y1);
  }
  else {
    double u   = ell + 0.5;
    double xi  = acosh(x);
    gsl_sf_result K0_scaled, K1_scaled;
    int stat_K0, stat_K1;
    int stat_e;
    double pre;
    double B00;
    double sum;

    /* B00 = -1/8 (1 - xi coth(xi) / xi^2
     * pre = sqrt(xi/sinh(xi))
     */
    if(xi < GSL_ROOT4_DBL_EPSILON) {
      B00 = (1.0-xi*xi/15.0)/24.0;
      pre = 1.0 - xi*xi/12.0;
    }
    else {
      double sinh_xi = sqrt(x*x - 1.0);
      double coth_xi = x / sinh_xi;
      B00 = -1.0/8.0 * (1.0 - xi * coth_xi) / (xi*xi);
      pre = sqrt(xi/sinh_xi);
    }

    stat_K0 = gsl_sf_bessel_K0_scaled_e(u*xi, &K0_scaled);
    stat_K1 = gsl_sf_bessel_K1_scaled_e(u*xi, &K1_scaled);

    sum = K0_scaled.val - xi/u * K1_scaled.val * B00;

    stat_e = gsl_sf_exp_mult_e(-u*xi, pre * sum, result);
    result->err  = GSL_DBL_EPSILON * fabs(result->val) * fabs(u*xi);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_3(stat_e, stat_K0, stat_K1);
  }
}

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

int
gsl_sf_legendre_Q0_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0 || x == 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(x*x < GSL_ROOT6_DBL_EPSILON) { /* |x| <~ 0.05 */
    const double c3 = 1.0/3.0;
    const double c5 = 1.0/5.0;
    const double c7 = 1.0/7.0;
    const double c9 = 1.0/9.0;
    const double c11 = 1.0/11.0;
    const double y = x * x;
    const double series = 1.0 + y*(c3 + y*(c5 + y*(c7 + y*(c9 + y*c11))));
    result->val = x * series;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(x);
    return GSL_SUCCESS;
  }
  else if(x < 1.0) {
    result->val = 0.5 * log((1.0+x)/(1.0-x));
    result->err  = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < 10.0) {
    result->val = 0.5 * log((x+1.0)/(x-1.0));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x*GSL_DBL_MIN < 2.0) {
    const double y = 1.0/(x*x);
    const double c1 = 1.0/3.0;
    const double c2 = 1.0/5.0;
    const double c3 = 1.0/7.0;
    const double c4 = 1.0/9.0;
    const double c5 = 1.0/11.0;
    const double c6 = 1.0/13.0;
    const double c7 = 1.0/15.0;
    result->val = (1.0/x) * (1.0 + y*(c1 + y*(c2 + y*(c3 + y*(c4 + y*(c5 + y*(c6 + y*c7)))))));
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    UNDERFLOW_ERROR(result);
  }
}

int
gsl_sf_legendre_Q1_e(const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0 || x == 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(x*x < GSL_ROOT6_DBL_EPSILON) { /* |x| <~ 0.05 */
    const double c3 = 1.0/3.0;
    const double c5 = 1.0/5.0;
    const double c7 = 1.0/7.0;
    const double c9 = 1.0/9.0;
    const double c11 = 1.0/11.0;
    const double y = x * x;
    const double series = 1.0 + y*(c3 + y*(c5 + y*(c7 + y*(c9 + y*c11))));
    result->val = x * x * series - 1.0;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < 1.0){
    result->val = 0.5 * x * (log((1.0+x)/(1.0-x))) - 1.0;
    result->err  = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < 6.0) {
    result->val = 0.5 * x * log((x+1.0)/(x-1.0)) - 1.0;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x*GSL_SQRT_DBL_MIN < 0.99/M_SQRT3) {
    const double y = 1/(x*x);
    const double c1 = 3.0/5.0;
    const double c2 = 3.0/7.0;
    const double c3 = 3.0/9.0;
    const double c4 = 3.0/11.0;
    const double c5 = 3.0/13.0;
    const double c6 = 3.0/15.0;
    const double c7 = 3.0/17.0;
    const double c8 = 3.0/19.0;
    const double sum = 1.0 + y*(c1 + y*(c2 + y*(c3 + y*(c4 + y*(c5 + y*(c6 + y*(c7 + y*c8)))))));
    result->val = sum / (3.0*x*x);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    UNDERFLOW_ERROR(result);
  }
}

int
gsl_sf_legendre_Ql_e(const int l, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0 || x == 1.0 || l < 0) {
    DOMAIN_ERROR(result);
  }
  else if(l == 0) {
    return gsl_sf_legendre_Q0_e(x, result);
  }
  else if(l == 1) {
    return gsl_sf_legendre_Q1_e(x, result);
  }
  else if(l > 100000) {
    return legendre_Ql_asymp_unif(l, x, result);
  }
  else if(x < 1.0){
    /* Forward recurrence.
     */
    gsl_sf_result Q0, Q1;
    int stat_Q0 = gsl_sf_legendre_Q0_e(x, &Q0);
    int stat_Q1 = gsl_sf_legendre_Q1_e(x, &Q1);
    double Qellm1 = Q0.val;
    double Qell   = Q1.val;
    double Qellp1;
    int ell;
    for(ell=1; ell<l; ell++) {
      Qellp1 = (x*(2.0*ell + 1.0) * Qell - ell * Qellm1) / (ell + 1.0);
      Qellm1 = Qell;
      Qell   = Qellp1;
    }
    result->val = Qell;
    result->err = GSL_DBL_EPSILON * l * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_Q0, stat_Q1);
  }
  else {
    /* x > 1.0 */

    double rat;
    int stat_CF1  = legendreQ_CF1_xgt1(l, 0.0, 0.0, x, &rat);
    int stat_Q;
    double Qellp1 = rat * GSL_SQRT_DBL_MIN;
    double Qell   = GSL_SQRT_DBL_MIN;
    double Qellm1;
    int ell;
    for(ell=l; ell>0; ell--) {
      Qellm1 = (x * (2.0*ell + 1.0) * Qell - (ell+1.0) * Qellp1) / ell;
      Qellp1 = Qell;
      Qell   = Qellm1;
    }

    if(fabs(Qell) > fabs(Qellp1)) {
      gsl_sf_result Q0;
      stat_Q = gsl_sf_legendre_Q0_e(x, &Q0);
      result->val = GSL_SQRT_DBL_MIN * Q0.val / Qell;
      result->err = l * GSL_DBL_EPSILON * fabs(result->val);
    }
    else {
      gsl_sf_result Q1;
      stat_Q = gsl_sf_legendre_Q1_e(x, &Q1);
      result->val = GSL_SQRT_DBL_MIN * Q1.val / Qellp1;
      result->err = l * GSL_DBL_EPSILON * fabs(result->val);
    }

    return GSL_ERROR_SELECT_2(stat_Q, stat_CF1);
  }
}

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

#include "eval.h"

double gsl_sf_legendre_Q0(const double x)
{
  EVAL_RESULT(gsl_sf_legendre_Q0_e(x, &result));
}

double gsl_sf_legendre_Q1(const double x)
{
  EVAL_RESULT(gsl_sf_legendre_Q1_e(x, &result));
}

double gsl_sf_legendre_Ql(const int l, const double x)
{
  EVAL_RESULT(gsl_sf_legendre_Ql_e(l, x, &result));
}


/*
 * Gsl_vector: test gsl_vector
 *
 * Copyright (c) 2012 Jérémie Decock
 *
 * Required: GSL library (libgsl0-dev)
 * Usage: gcc gsl_vector.c -lgsl -lgslcblas -lm
 *    or: gcc gsl_vector.c $(pkg-config --libs gsl)
 */

#include <stdio.h>
#include <gsl/gsl_vector.h>

static double v2;

int main(int argc, char * argv[]) {

    gsl_vector * v = gsl_vector_calloc(3);

    gsl_vector_set_all(v, 84.0);

    gsl_vector_fprintf(stdout, v, "%f");

    gsl_vector_free(v);

    return 0;
}


#ifndef SDCA_MATH_BLAS_H
#define SDCA_MATH_BLAS_H

#if defined(BLAS_MKL)
  //#include <mkl.h> // produces warnings with -pedantic
  #include <mkl_cblas.h>
#elif defined(BLAS_ACCELERATE)
  #include <Accelerate/Accelerate.h>
#elif defined(BLAS_MATLAB)
  #include <cstddef>
  typedef std::ptrdiff_t ptrdiff_t; // Matlab's blas.h needs ptrdiff_t
  #include <blas.h>
#elif defined(BLAS_DEFAULT)
  #include <cblas.h>
#elif defined(BLAS_DEFAULT_LOCAL_HEADER)
  #include "cblas.h"
#endif

#if defined(BLAS_MATLAB)
  enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113};
#endif

namespace sdca {

#if defined(MKL_INT)
  typedef MKL_INT blas_int;
#elif defined(BLAS_MATLAB)
  typedef ptrdiff_t blas_int;
#else
  typedef int blas_int;
#endif

inline void
sdca_blas_scal(
    const blas_int n,
    const float alpha,
    float* X
  ) {
#if defined(sscal)
  blas_int inc(1);
  sscal(const_cast<blas_int*>(&n), const_cast<float*>(&alpha), X, &inc);
#else
  cblas_sscal(n, alpha, X, 1);
#endif
}

inline void
sdca_blas_scal(
    const blas_int n,
    const double alpha,
    double* X
  ) {
#if defined(dscal)
  blas_int inc(1);
  dscal(const_cast<blas_int*>(&n), const_cast<double*>(&alpha), X, &inc);
#else
  cblas_dscal(n, alpha, X, 1);
#endif
}

inline void
sdca_blas_copy(
    const blas_int n,
    const float* X,
    float* Y
  ) {
#if defined(scopy)
  blas_int inc(1);
  scopy(const_cast<blas_int*>(&n), const_cast<float*>(X), &inc, Y, &inc);
#else
  cblas_scopy(n, X, 1, Y, 1);
#endif
}

inline void
sdca_blas_copy(
    const blas_int n,
    const double* X,
    double* Y
  ) {
#if defined(dcopy)
  blas_int inc(1);
  dcopy(const_cast<blas_int*>(&n), const_cast<double*>(X), &inc, Y, &inc);
#else
  cblas_dcopy(n, X, 1, Y, 1);
#endif
}

inline void
sdca_blas_swap(
    const blas_int n,
    float* X,
    float* Y
  ) {
#if defined(scopy)
  blas_int inc(1);
  sswap(const_cast<blas_int*>(&n), X, &inc, Y, &inc);
#else
  cblas_sswap(n, X, 1, Y, 1);
#endif
}

inline void
sdca_blas_swap(
    const blas_int n,
    double* X,
    double* Y
  ) {
#if defined(dcopy)
  blas_int inc(1);
  dswap(const_cast<blas_int*>(&n), X, &inc, Y, &inc);
#else
  cblas_dswap(n, X, 1, Y, 1);
#endif
}

inline void
sdca_blas_axpy(
    const blas_int n,
    const float alpha,
    const float* X,
    float* Y
  ) {
#if defined(saxpy)
  blas_int inc(1);
  saxpy(const_cast<blas_int*>(&n), const_cast<float*>(&alpha),
        const_cast<float*>(X), &inc, Y, &inc);
#else
  cblas_saxpy(n, alpha, X, 1, Y, 1);
#endif
}

inline void
sdca_blas_axpy(
    const blas_int n,
    const double alpha,
    const double* X,
    double* Y
  ) {
#if defined(daxpy)
  blas_int inc(1);
  daxpy(const_cast<blas_int*>(&n), const_cast<double*>(&alpha),
        const_cast<double*>(X), &inc, Y, &inc);
#else
  cblas_daxpy(n, alpha, X, 1, Y, 1);
#endif
}

inline void
sdca_blas_axpby(
    const blas_int n,
    const float alpha,
    const float* X,
    const float beta,
    float* Y
  ) {
#if defined(cblas_saxpby)
  cblas_saxpby(n, alpha, X, 1, beta, Y, 1);
#elif defined(sscal)
  blas_int inc(1);
  sscal(const_cast<blas_int*>(&n), const_cast<float*>(&beta), Y, &inc);
  saxpy(const_cast<blas_int*>(&n), const_cast<float*>(&alpha),
        const_cast<float*>(X), &inc, Y, &inc);
#else
  cblas_sscal(n, beta, Y, 1);
  cblas_saxpy(n, alpha, X, 1, Y, 1);
#endif
}

inline void
sdca_blas_axpby(
    const blas_int n,
    const double alpha,
    const double* X,
    const double beta,
    double* Y
  ) {
#if defined(cblas_daxpby)
  cblas_daxpby(n, alpha, X, 1, beta, Y, 1);
#elif defined(dscal)
  blas_int inc(1);
  dscal(const_cast<blas_int*>(&n), const_cast<double*>(&beta), Y, &inc);
  daxpy(const_cast<blas_int*>(&n), const_cast<double*>(&alpha),
        const_cast<double*>(X), &inc, Y, &inc);
#else
  cblas_dscal(n, beta, Y, 1);
  cblas_daxpy(n, alpha, X, 1, Y, 1);
#endif
}

inline float
sdca_blas_dot(
    const blas_int n,
    const float* X,
    const float* Y
  ) {
#if defined(sdot)
  blas_int inc(1);
  return sdot(const_cast<blas_int*>(&n), const_cast<float*>(X), &inc,
              const_cast<float*>(Y), &inc);
#else
  return cblas_sdot(n, X, 1, Y, 1);
#endif
}

inline double
sdca_blas_dot(
    const blas_int n,
    const double* X,
    const double* Y
  ) {
#if defined(ddot)
  blas_int inc(1);
  return ddot(const_cast<blas_int*>(&n), const_cast<double*>(X), &inc,
              const_cast<double*>(Y), &inc);
#else
  return cblas_ddot(n, X, 1, Y, 1);
#endif
}

inline float
sdca_blas_asum(
    const blas_int n,
    const float* X
  ) {
#if defined(sasum)
  blas_int inc(1);
  return sasum(const_cast<blas_int*>(&n), const_cast<float*>(X), &inc);
#else
  return cblas_sasum(n, X, 1);
#endif
}

inline double
sdca_blas_asum(
    const blas_int n,
    const double* X
  ) {
#if defined(dasum)
  blas_int inc(1);
  return dasum(const_cast<blas_int*>(&n), const_cast<double*>(X), &inc);
#else
  return cblas_dasum(n, X, 1);
#endif
}

inline float
sdca_blas_nrm2(
    const blas_int n,
    const float* X
  ) {
#if defined(snrm2)
  blas_int inc(1);
  return snrm2(const_cast<blas_int*>(&n), const_cast<float*>(X), &inc);
#else
  return cblas_snrm2(n, X, 1);
#endif
}

inline double
sdca_blas_nrm2(
    const blas_int n,
    const double* X
  ) {
#if defined(dnrm2)
  blas_int inc(1);
  return dnrm2(const_cast<blas_int*>(&n), const_cast<double*>(X), &inc);
#else
  return cblas_dnrm2(n, X, 1);
#endif
}

inline void
sdca_blas_gemv(
    const blas_int m,
    const blas_int n,
    const float* A,
    const float* X,
    float* Y,
    const CBLAS_TRANSPOSE transA = CblasNoTrans,
    const float alpha = 1,
    const float beta = 0
    ) {
#if defined(sgemv)
  blas_int inc(1);
  char trans((transA == CblasNoTrans) ? 'N' : 'T');
  sgemv(&trans, const_cast<blas_int*>(&m), const_cast<blas_int*>(&n),
        const_cast<float*>(&alpha),
        const_cast<float*>(A), const_cast<blas_int*>(&m),
        const_cast<float*>(X), &inc, const_cast<float*>(&beta), Y, &inc);
#else
  cblas_sgemv(CblasColMajor, transA, m, n, alpha, A, m,
              X, 1, beta, Y, 1);
#endif
}

inline void
sdca_blas_gemv(
    const blas_int m,
    const blas_int n,
    const double* A,
    const double* X,
    double* Y,
    const CBLAS_TRANSPOSE transA = CblasNoTrans,
    const double alpha = 1,
    const double beta = 0
    ) {
#if defined(dgemv)
  blas_int inc(1);
  char trans((transA == CblasNoTrans) ? 'N' : 'T');
  dgemv(&trans, const_cast<blas_int*>(&m), const_cast<blas_int*>(&n),
        const_cast<double*>(&alpha),
        const_cast<double*>(A), const_cast<blas_int*>(&m),
        const_cast<double*>(X), &inc, const_cast<double*>(&beta), Y, &inc);
#else
  cblas_dgemv(CblasColMajor, transA, m, n, alpha, A, m,
              X, 1, beta, Y, 1);
#endif
}

inline void
sdca_blas_ger(
    const blas_int m,
    const blas_int n,
    const float alpha,
    const float* X,
    const float* Y,
    float* A
    ) {
#if defined(sger)
  blas_int inc(1);
  sger(const_cast<blas_int*>(&m), const_cast<blas_int*>(&n),
       const_cast<float*>(&alpha), const_cast<float*>(X), &inc,
       const_cast<float*>(Y), &inc, A, const_cast<blas_int*>(&m));
#else
  cblas_sger(CblasColMajor, m, n, alpha, X, 1, Y, 1, A, m);
#endif
}

inline void
sdca_blas_ger(
    const blas_int m,
    const blas_int n,
    const double alpha,
    const double* X,
    const double* Y,
    double* A
    ) {
#if defined(dger)
  blas_int inc(1);
  dger(const_cast<blas_int*>(&m), const_cast<blas_int*>(&n),
       const_cast<double*>(&alpha), const_cast<double*>(X), &inc,
       const_cast<double*>(Y), &inc, A, const_cast<blas_int*>(&m));
#else
  cblas_dger(CblasColMajor, m, n, alpha, X, 1, Y, 1, A, m);
#endif
}

inline void
sdca_blas_gemm(
    const blas_int m,
    const blas_int n,
    const blas_int k,
    const float* A,
    const blas_int lda,
    const float* B,
    const blas_int ldb,
    float* C,
    const CBLAS_TRANSPOSE transA = CblasNoTrans,
    const CBLAS_TRANSPOSE transB = CblasNoTrans,
    const float alpha = 1,
    const float beta = 0
    ) {
#if defined(sgemm)
  char transa((transA == CblasNoTrans) ? 'N' : 'T');
  char transb((transB == CblasNoTrans) ? 'N' : 'T');
  sgemm(&transa, &transb, const_cast<blas_int*>(&m), const_cast<blas_int*>(&n),
        const_cast<blas_int*>(&k), const_cast<float*>(&alpha),
        const_cast<float*>(A), const_cast<blas_int*>(&lda),
        const_cast<float*>(B), const_cast<blas_int*>(&ldb),
        const_cast<float*>(&beta), C, const_cast<blas_int*>(&m));
#else
  cblas_sgemm(CblasColMajor, transA, transB, m, n, k,
              alpha, A, lda, B, ldb, beta, C, m);
#endif
}

inline void
sdca_blas_gemm(
    const blas_int m,
    const blas_int n,
    const blas_int k,
    const double* A,
    const blas_int lda,
    const double* B,
    const blas_int ldb,
    double* C,
    const CBLAS_TRANSPOSE transA = CblasNoTrans,
    const CBLAS_TRANSPOSE transB = CblasNoTrans,
    const double alpha = 1,
    const double beta = 0
    ) {
#if defined(dgemm)
  char transa((transA == CblasNoTrans) ? 'N' : 'T');
  char transb((transB == CblasNoTrans) ? 'N' : 'T');
  dgemm(&transa, &transb, const_cast<blas_int*>(&m), const_cast<blas_int*>(&n),
        const_cast<blas_int*>(&k), const_cast<double*>(&alpha),
        const_cast<double*>(A), const_cast<blas_int*>(&lda),
        const_cast<double*>(B), const_cast<blas_int*>(&ldb),
        const_cast<double*>(&beta), C, const_cast<blas_int*>(&m));
#else
  cblas_dgemm(CblasColMajor, transA, transB, m, n, k,
              alpha, A, lda, B, ldb, beta, C, m);
#endif
}

}

#endif


//Gets line spectral frequencies (LSFs).
//from the linear prediction (LP) polynomial coefficients along cols or rows of X.

#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include "cmp_ascend.c"

#ifdef __cplusplus
namespace ov {
extern "C" {
#endif

int poly2lsf_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim);
int poly2lsf_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim);
int poly2lsf_c (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim);
int poly2lsf_z (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim);

int poly2lsf_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim)
{
    const float z = 0.0f, o = 1.0f;
    const int P = (dim==0) ? R-1 : C-1;
    const int job = 'E', compz = 'N';  //eigenvalues only
    const lapack_int ldh = P+1, n = P+1, ldz = 1;
    const lapack_int ilo = 1, ihi = n;  //avoids balancing
    lapack_int info;
    float *poly, *prev, *omegas, *compan, *wr, *wi, zz[1];
    int r, c;

    //Checks
    if (R<1) { fprintf(stderr,"error in poly2lsf_s: nrows X must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in poly2lsf_s: ncols X must be positive\n"); return 1; }
    if (P<1) { fprintf(stderr,"error in poly2lsf_s: P (length of polynomial coeffs including a0=1) must be positive\n"); return 1; }

    //Allocate
    if (!(poly=(float *)malloc((size_t)(n+1)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(prev=(float *)malloc((size_t)(n+1)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(omegas=(float *)malloc((size_t)(2*n)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(wr=(float *)malloc((size_t)(n)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(wi=(float *)malloc((size_t)(n)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(compan=(float *)malloc((size_t)(n*n)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_s: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0)
    {
        for (c=0; c<C; c++)
        {
            if (iscolmajor) { cblas_scopy(n,&X[c*R],1,poly,1); }
            else { cblas_scopy(n,&X[c],C,poly,1); }
            cblas_sscal(n,-1.0f/poly[0],poly,1); poly[n] = z;
            for (r=1; r<=n; r++) { prev[r] = poly[n-r]; }
            for (r=1; r<=n; r++) { poly[r] -= prev[r]; }
            cblas_scopy(n*n,&z,0,compan,1); cblas_scopy(n-1,&o,0,&compan[1],n+1); cblas_scopy(n,&poly[1],1,compan,n);
            //fprintf(stderr,"compan = \n"); for (r=0; r<n; r++) { for (c2=0; c2<n; c2++) { fprintf(stderr,"%f ",compan[r+c2*n]); } fprintf(stderr,"\n"); }
            info = LAPACKE_shseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,compan,ldh,wr,wi,zz,ldz);  //eig
            if (info) { fprintf(stderr,"error in poly2lsf_s: lapacke decomposition failed\n"); return 1; }
            for (r=0; r<n; r++) { omegas[r] = (wi[r]>10.0f*FLT_EPSILON) ? atan2f(wi[r],wr[r]) : 0.0f; }
            for (r=1; r<=n; r++) { poly[r] += prev[r] + prev[r]; }
            cblas_scopy(n*n,&z,0,compan,1); cblas_scopy(n-1,&o,0,&compan[1],n+1); cblas_scopy(n,&poly[1],1,compan,n);
            //fprintf(stderr,"compan = \n"); for (r=0; r<n; r++) { for (c2=0; c2<n; c2++) { fprintf(stderr,"%f ",compan[r+c2*n]); } fprintf(stderr,"\n"); }
            info = LAPACKE_shseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,compan,ldh,wr,wi,zz,ldz);  //eig
            if (info) { fprintf(stderr,"error in poly2lsf_s: lapacke decomposition failed\n"); return 1; }
            for (r=0; r<n; r++) { omegas[n+r] = (wi[r]>10.0f*FLT_EPSILON) ? atan2f(wi[r],wr[r]) : 0.0f; }
            qsort(omegas,(size_t)(2*n),sizeof(float),cmp_ascend_s);
            if (iscolmajor) { cblas_scopy(n-1,&omegas[n+1],1,&Y[c*(n-1)],1); }  //cblas_scopy(2*n,omegas,1,&Y[2*c*n],1);
            else { cblas_scopy(n-1,&omegas[n+1],1,&Y[c],C); }  //cblas_scopy(2*n,omegas,1,&Y[c],C);
        }
    }
    else if (dim==1)
    {
        for (r=0; r<R; r++)
        {
            if (iscolmajor) { cblas_scopy(n,&X[r],R,poly,1); }
            else { cblas_scopy(n,&X[r*C],1,poly,1); }
            cblas_sscal(n,-1.0f/poly[0],poly,1); poly[n] = z;
            for (c=1; c<=n; c++) { prev[c] = poly[n-c]; }
            for (c=1; c<=n; c++) { poly[c] -= prev[c]; }
            cblas_scopy(n*n,&z,0,compan,1); cblas_scopy(n-1,&o,0,&compan[1],n+1); cblas_scopy(n,&poly[1],1,compan,n);
            info = LAPACKE_shseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,compan,ldh,wr,wi,zz,ldz);  //eig
            if (info) { fprintf(stderr,"error in poly2lsf_s: lapacke decomposition failed\n"); return 1; }
            for (c=0; c<n; c++) { omegas[c] = (wi[c]>10.0f*FLT_EPSILON) ? atan2f(wi[c],wr[c]) : 0.0f; }
            for (c=1; c<=n; c++) { poly[c] += prev[c] + prev[c]; }
            cblas_scopy(n*n,&z,0,compan,1); cblas_scopy(n-1,&o,0,&compan[1],n+1); cblas_scopy(n,&poly[1],1,compan,n);
            info = LAPACKE_shseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,compan,ldh,wr,wi,zz,ldz);  //eig
            if (info) { fprintf(stderr,"error in poly2lsf_s: lapacke decomposition failed\n"); return 1; }
            for (c=0; c<n; c++) { omegas[n+c] = (wi[c]>10.0f*FLT_EPSILON) ? atan2f(wi[c],wr[c]) : 0.0f; }
            qsort(omegas,(size_t)(2*n),sizeof(float),cmp_ascend_s);
            if (iscolmajor) { cblas_scopy(n-1,&omegas[n+1],1,&Y[r],R); }  //cblas_scopy(2*n,omegas,1,&Y[r],R);
            else { cblas_scopy(n-1,&omegas[n+1],1,&Y[r*(n-1)],1); }  //cblas_scopy(2*n,omegas,1,&Y[2*r*n],1);
        }
    }
    else
    {
        fprintf(stderr,"error in poly2lsf_s: dim must be 0 or 1.\n"); return 1;
    }

    //Exit
    free(compan); free(wr); free(wi); free(poly); free(prev); free(omegas);
    return 0;
}

int poly2lsf_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim)
{
    const double z = 0.0, o = 1.0;
    const int P = (dim==0) ? R-1 : C-1;
    const int job = 'E', compz = 'N';  //eigenvalues only
    const lapack_int ldh = P+1, n = P+1, ldz = 1;
    const lapack_int ilo = 1, ihi = n;  //avoids balancing
    lapack_int info;
    double *poly, *prev, *omegas, *compan, *wr, *wi, zz[1];
    int r, c;

    //Checks
    if (R<1) { fprintf(stderr,"error in poly2lsf_d: nrows X must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in poly2lsf_d: ncols X must be positive\n"); return 1; }
    if (P<1) { fprintf(stderr,"error in poly2lsf_d: P (length of polynomial coeffs including a0=1) must be positive\n"); return 1; }

    //Allocate
    if (!(poly=(double *)malloc((size_t)(n+1)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(prev=(double *)malloc((size_t)(n+1)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(omegas=(double *)malloc((size_t)(2*n)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(wr=(double *)malloc((size_t)(n)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(wi=(double *)malloc((size_t)(n)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(compan=(double *)malloc((size_t)(n*n)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_d: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0)
    {
        for (c=0; c<C; c++)
        {
            if (iscolmajor) { cblas_dcopy(n,&X[c*R],1,poly,1); }
            else { cblas_dcopy(n,&X[c],C,poly,1); }
            cblas_dscal(n,-1.0/poly[0],poly,1); poly[n] = z;
            for (r=1; r<=n; r++) { prev[r] = poly[n-r]; }
            for (r=1; r<=n; r++) { poly[r] -= prev[r]; }
            cblas_dcopy(n*n,&z,0,compan,1); cblas_dcopy(n-1,&o,0,&compan[1],n+1); cblas_dcopy(n,&poly[1],1,compan,n);
            info = LAPACKE_dhseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,compan,ldh,wr,wi,zz,ldz);  //eig
            if (info) { fprintf(stderr,"error in poly2lsf_d: lapacke decomposition failed\n"); return 1; }
            for (r=0; r<n; r++) { omegas[r] = (wi[r]>10.0*DBL_EPSILON) ? atan2(wi[r],wr[r]) : 0.0; }
            for (r=1; r<=n; r++) { poly[r] += prev[r] + prev[r]; }
            cblas_dcopy(n*n,&z,0,compan,1); cblas_dcopy(n-1,&o,0,&compan[1],n+1); cblas_dcopy(n,&poly[1],1,compan,n);
            info = LAPACKE_dhseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,compan,ldh,wr,wi,zz,ldz);  //eig
            if (info) { fprintf(stderr,"error in poly2lsf_d: lapacke decomposition failed\n"); return 1; }
            for (r=0; r<n; r++) { omegas[n+r] = (wi[r]>10.0*DBL_EPSILON) ? atan2(wi[r],wr[r]) : 0.0; }
            qsort(omegas,(size_t)(2*n),sizeof(double),cmp_ascend_d);
            if (iscolmajor) { cblas_dcopy(n-1,&omegas[n+1],1,&Y[c*(n-1)],1); }  //cblas_dcopy(2*n,omegas,1,&Y[2*c*n],1);
            else { cblas_dcopy(n-1,&omegas[n+1],1,&Y[c],C); }  //cblas_dcopy(2*n,omegas,1,&Y[c],C);
        }
    }
    else if (dim==1)
    {
        for (r=0; r<R; r++)
        {
            if (iscolmajor) { cblas_dcopy(n,&X[r],R,poly,1); }
            else { cblas_dcopy(n,&X[r*C],1,poly,1); }
            cblas_dscal(n,-1.0/poly[0],poly,1); poly[n] = z;
            for (c=1; c<=n; c++) { prev[c] = poly[n-c]; }
            for (c=1; c<=n; c++) { poly[c] -= prev[c]; }
            cblas_dcopy(n*n,&z,0,compan,1); cblas_dcopy(n-1,&o,0,&compan[1],n+1); cblas_dcopy(n,&poly[1],1,compan,n);
            info = LAPACKE_dhseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,compan,ldh,wr,wi,zz,ldz);  //eig
            if (info) { fprintf(stderr,"error in poly2lsf_d: lapacke decomposition failed\n"); return 1; }
            for (c=0; c<n; c++) { omegas[c] = (wi[c]>10.0*DBL_EPSILON) ? atan2(wi[c],wr[c]) : 0.0; }
            for (c=1; c<=n; c++) { poly[c] += prev[c] + prev[c]; }
            cblas_dcopy(n*n,&z,0,compan,1); cblas_dcopy(n-1,&o,0,&compan[1],n+1); cblas_dcopy(n,&poly[1],1,compan,n);
            info = LAPACKE_dhseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,compan,ldh,wr,wi,zz,ldz);  //eig
            if (info) { fprintf(stderr,"error in poly2lsf_d: lapacke decomposition failed\n"); return 1; }
            for (c=0; c<n; c++) { omegas[n+c] = (wi[c]>10.0*DBL_EPSILON) ? atan2(wi[c],wr[c]) : 0.0; }
            qsort(omegas,(size_t)(2*n),sizeof(double),cmp_ascend_d);
            if (iscolmajor) { cblas_dcopy(n-1,&omegas[n+1],1,&Y[r],R); }  //cblas_dcopy(2*n,omegas,1,&Y[r],R);
            else { cblas_dcopy(n-1,&omegas[n+1],1,&Y[r*(n-1)],1); }
        }
    }
    else
    {
        fprintf(stderr,"error in poly2lsf_d: dim must be 0 or 1.\n"); return 1;
    }

    //Exit
    free(compan); free(wr); free(wi); free(poly); free(prev); free(omegas);
    return 0;
}

int poly2lsf_c (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim)
{
    const float z[2] =  {0.0f,0.0f}, o[2] =  {1.0f,0.0f};
    const int P = (dim==0) ? R-1 : C-1;
    const int job = 'E', compz = 'N';  //eigenvalues only
    const lapack_int ldh = P+1, n = P+1, ldz = 1;
    const lapack_int ilo = 1, ihi = n;  //avoids balancing
    lapack_int info;
    float *poly, *prev, *omegas, *compan, *wc, zz[2], sc[2];
    int r, c;

    //Checks
    if (R<1) { fprintf(stderr,"error in poly2lsf_c: nrows X must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in poly2lsf_c: ncols X must be positive\n"); return 1; }
    if (P<1) { fprintf(stderr,"error in poly2lsf_c: P (length of polynomial coeffs including a0=1) must be positive\n"); return 1; }

    //Allocate
    if (!(poly=(float *)malloc((size_t)(2*n+2)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_c: problem with malloc. "); perror("malloc"); return 1; }
    if (!(prev=(float *)malloc((size_t)(2*n+2)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_c: problem with malloc. "); perror("malloc"); return 1; }
    if (!(wc=(float *)malloc((size_t)(2*n)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_c: problem with malloc. "); perror("malloc"); return 1; }
    if (!(omegas=(float *)malloc((size_t)(2*n)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_c: problem with malloc. "); perror("malloc"); return 1; }
    if (!(compan=(float *)malloc((size_t)(4*n*n)*sizeof(float)))) { fprintf(stderr,"error in poly2lsf_c: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0)
    {
        for (c=0; c<C; c++)
        {
            if (iscolmajor) { cblas_ccopy(n,&X[2*c*R],1,poly,1); }
            else { cblas_ccopy(n,&X[2*c],C,poly,1); }
            sc[0] = 1.0f/(poly[0]*poly[0]+poly[1]*poly[1]); sc[1] = poly[1]*sc[0]; sc[0] *= -poly[0];
            cblas_cscal(n,sc,poly,1); poly[2*n] = poly[2*n+1] = 0.0f;
            for (r=1; r<=n; r++) { prev[2*r] = poly[2*(n-r)]; prev[2*r+1] = poly[2*(n-r)+1]; }
            for (r=1; r<=n; r++) { poly[2*r] -= prev[2*r]; poly[2*r+1] -= prev[2*r+1]; }
            cblas_ccopy(n*n,z,0,compan,1); cblas_ccopy(n-1,o,0,&compan[2],n+1); cblas_ccopy(n,&poly[2],1,compan,n);
            info = LAPACKE_chseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,(lapack_complex_float *)compan,ldh,(lapack_complex_float *)wc,(lapack_complex_float *)zz,ldz);
            if (info) { fprintf(stderr,"error in poly2lsf_c: lapacke decomposition failed\n"); return 1; }
            for (r=0; r<n; r++) { omegas[r] = atan2f(wc[2*r+1],wc[2*r]); }
            //for (r=0; r<n; r++) { omegas[r] = (wc[2*r+1]>10.0f*FLT_EPSILON) ? atan2f(wc[2*r+1],wc[2*r]) : 0.0f; }
            for (r=1; r<=n; r++) { poly[2*r] += prev[2*r] + prev[2*r]; poly[2*r+1] += prev[2*r+1] + prev[2*r+1]; }
            cblas_ccopy(n*n,z,0,compan,1); cblas_ccopy(n-1,o,0,&compan[2],n+1); cblas_ccopy(n,&poly[2],1,compan,n);
            info = LAPACKE_chseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,(lapack_complex_float *)compan,ldh,(lapack_complex_float *)wc,(lapack_complex_float *)zz,ldz);
            if (info) { fprintf(stderr,"error in poly2lsf_c: lapacke decomposition failed\n"); return 1; }
            for (r=0; r<n; r++) { omegas[n+r] = atan2f(wc[2*r+1],wc[2*r]); }
            //for (r=0; r<n; r++) { omegas[n+r] = (wc[2*r+1]>10.0f*FLT_EPSILON) ? atan2f(wc[2*r+1],wc[2*r]) : 0.0f; }
            qsort(omegas,(size_t)(2*n),sizeof(float),cmp_ascend_s);
            if (iscolmajor) { cblas_scopy(2*n,omegas,1,&Y[2*c*n],1); }
            else { cblas_scopy(2*n,omegas,1,&Y[c],C); }
        }
    }
    else if (dim==1)
    {
        for (r=0; r<R; r++)
        {
            if (iscolmajor) { cblas_ccopy(n,&X[2*r],R,poly,1); }
            else { cblas_ccopy(n,&X[2*r*C],1,poly,1); }
            sc[0] = 1.0f/(poly[0]*poly[0]+poly[1]*poly[1]); sc[1] = poly[1]*sc[0]; sc[0] *= -poly[0];
            cblas_cscal(n,sc,poly,1); poly[2*n] = poly[2*n+1] = 0.0f;
            for (c=1; c<=n; c++) { prev[2*c] = poly[2*(n-c)]; prev[2*c+1] = poly[2*(n-c)+1]; }
            for (c=1; c<=n; c++) { poly[2*c] -= prev[2*c]; poly[2*c+1] -= prev[2*c+1]; }
            cblas_ccopy(n*n,z,0,compan,1); cblas_ccopy(n-1,o,0,&compan[2],n+1); cblas_ccopy(n,&poly[2],1,compan,n);
            info = LAPACKE_chseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,(lapack_complex_float *)compan,ldh,(lapack_complex_float *)wc,(lapack_complex_float *)zz,ldz);
            if (info) { fprintf(stderr,"error in poly2lsf_c: lapacke decomposition failed\n"); return 1; }
            for (c=0; c<n; c++) { omegas[c] = atan2f(wc[2*c+1],wc[2*c]); }
            //for (c=0; c<n; c++) { omegas[c] = (wc[2*r+1]>10.0f*FLT_EPSILON) ? atan2f(wc[2*r+1],wc[2*r]) : 0.0f; }
            for (c=1; c<=n; c++) { poly[2*c] += prev[2*c] + prev[2*c]; poly[2*c+1] += prev[2*c+1] + prev[2*c+1]; }
            cblas_ccopy(n*n,z,0,compan,1); cblas_ccopy(n-1,o,0,&compan[2],n+1); cblas_ccopy(n,&poly[2],1,compan,n);
            info = LAPACKE_chseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,(lapack_complex_float *)compan,ldh,(lapack_complex_float *)wc,(lapack_complex_float *)zz,ldz);
            if (info) { fprintf(stderr,"error in poly2lsf_c: lapacke decomposition failed\n"); return 1; }
            for (c=0; c<n; c++) { omegas[n+c] = atan2f(wc[2*c+1],wc[2*c]); }
            //for (c=0; c<n; c++) { omegas[n+c] = (wc[2*r+1]>10.0f*FLT_EPSILON) ? atan2f(wc[2*r+1],wc[2*r]) : 0.0f; }
            qsort(omegas,(size_t)(2*n),sizeof(float),cmp_ascend_s);
            if (iscolmajor) { cblas_scopy(2*n,omegas,1,&Y[r],R); }
            else { cblas_scopy(2*n,omegas,1,&Y[2*r*n],1); }
        }
    }
    else
    {
        fprintf(stderr,"error in poly2lsf_c: dim must be 0 or 1.\n"); return 1;
    }

    //Exit
    free(compan); free(wc); free(poly); free(prev); free(omegas);
    return 0;
}

int poly2lsf_z (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim)
{
    const double z[2] =  {0.0,0.0}, o[2] =  {1.0,0.0};
    const int P = (dim==0) ? R-1 : C-1;
    const int job = 'E', compz = 'N';  //eigenvalues only
    const lapack_int ldh = P+1, n = P+1, ldz = 1;
    const lapack_int ilo = 1, ihi = n;  //avoids balancing
    lapack_int info;
    double *poly, *prev, *omegas, *compan, *wc, zz[2], sc[2];
    int r, c;

    //Checks
    if (R<1) { fprintf(stderr,"error in poly2lsf_z: nrows X must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in poly2lsf_z: ncols X must be positive\n"); return 1; }
    if (P<1) { fprintf(stderr,"error in poly2lsf_z: P (length of polynomial coeffs including a0=1) must be positive\n"); return 1; }

    //Allocate
    if (!(poly=(double *)malloc((size_t)(2*n+2)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_z: problem with malloc. "); perror("malloc"); return 1; }
    if (!(prev=(double *)malloc((size_t)(2*n+2)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_z: problem with malloc. "); perror("malloc"); return 1; }
    if (!(wc=(double *)malloc((size_t)(2*n)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_z: problem with malloc. "); perror("malloc"); return 1; }
    if (!(omegas=(double *)malloc((size_t)(2*n)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_z: problem with malloc. "); perror("malloc"); return 1; }
    if (!(compan=(double *)malloc((size_t)(4*n*n)*sizeof(double)))) { fprintf(stderr,"error in poly2lsf_z: problem with malloc. "); perror("malloc"); return 1; }

    if (dim==0)
    {
        for (c=0; c<C; c++)
        {
            if (iscolmajor) { cblas_zcopy(n,&X[2*c*R],1,poly,1); }
            else { cblas_zcopy(n,&X[2*c],C,poly,1); }
            sc[0] = 1.0/(poly[0]*poly[0]+poly[1]*poly[1]); sc[1] = poly[1]*sc[0]; sc[0] *= -poly[0];
            cblas_zscal(n,sc,poly,1); poly[2*n] = poly[2*n+1] = 0.0;
            for (r=1; r<=n; r++) { prev[2*r] = poly[2*(n-r)]; prev[2*r+1] = poly[2*(n-r)+1]; }
            for (r=1; r<=n; r++) { poly[2*r] -= prev[2*r]; poly[2*r+1] -= prev[2*r+1]; }
            cblas_zcopy(n*n,z,0,compan,1); cblas_zcopy(n-1,o,0,&compan[2],n+1); cblas_zcopy(n,&poly[2],1,compan,n);
            info = LAPACKE_zhseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,(lapack_complex_double *)compan,ldh,(lapack_complex_double *)wc,(lapack_complex_double *)zz,ldz);
            if (info) { fprintf(stderr,"error in poly2lsf_z: lapacke decomposition failed\n"); return 1; }
            for (r=0; r<n; r++) { omegas[r] = atan2(wc[2*r+1],wc[2*r]); }
            //for (r=0; r<n; r++) { omegas[r] = (wc[2*r+1]>10.0*DBL_EPSILON) ? atan2(wc[2*r+1],wc[2*r]) : 0.0; }
            for (r=1; r<=n; r++) { poly[2*r] += prev[2*r] + prev[2*r]; poly[2*r+1] += prev[2*r+1] + prev[2*r+1]; }
            cblas_zcopy(n*n,z,0,compan,1); cblas_zcopy(n-1,o,0,&compan[2],n+1); cblas_zcopy(n,&poly[2],1,compan,n);
            info = LAPACKE_zhseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,(lapack_complex_double *)compan,ldh,(lapack_complex_double *)wc,(lapack_complex_double *)zz,ldz);
            if (info) { fprintf(stderr,"error in poly2lsf_z: lapacke decomposition failed\n"); return 1; }
            for (r=0; r<n; r++) { omegas[n+r] = atan2(wc[2*r+1],wc[2*r]); }
            qsort(omegas,(size_t)(2*n),sizeof(double),cmp_ascend_d);
            if (iscolmajor) { cblas_dcopy(2*n,omegas,1,&Y[2*c*n],1); }
            else { cblas_dcopy(2*n,omegas,1,&Y[c],C); }
        }
    }
    else if (dim==1)
    {
        for (r=0; r<R; r++)
        {
            if (iscolmajor) { cblas_zcopy(n,&X[2*r],R,poly,1); }
            else { cblas_zcopy(n,&X[2*r*C],1,poly,1); }
            sc[0] = 1.0/(poly[0]*poly[0]+poly[1]*poly[1]); sc[1] = poly[1]*sc[0]; sc[0] *= -poly[0];
            cblas_zscal(n,sc,poly,1); poly[2*n] = poly[2*n+1] = 0.0;
            for (c=1; c<=n; c++) { prev[2*c] = poly[2*(n-c)]; prev[2*c+1] = poly[2*(n-c)+1]; }
            for (c=1; c<=n; c++) { poly[2*c] -= prev[2*c]; poly[2*c+1] -= prev[2*c+1]; }
            cblas_zcopy(n*n,z,0,compan,1); cblas_zcopy(n-1,o,0,&compan[2],n+1); cblas_zcopy(n,&poly[2],1,compan,n);
            info = LAPACKE_zhseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,(lapack_complex_double *)compan,ldh,(lapack_complex_double *)wc,(lapack_complex_double *)zz,ldz);
            if (info) { fprintf(stderr,"error in poly2lsf_z: lapacke decomposition failed\n"); return 1; }
            for (c=0; c<n; c++) { omegas[c] = atan2(wc[2*c+1],wc[2*c]); }
            //for (c=0; c<n; c++) { omegas[n+c] = (wc[2*r+1]>10.0*DBL_EPSILON) ? atan2(wc[2*r+1],wc[2*r]) : 0.0; }
            for (c=1; c<=n; c++) { poly[2*c] += prev[2*c] + prev[2*c]; poly[2*c+1] += prev[2*c+1] + prev[2*c+1]; }
            cblas_zcopy(n*n,z,0,compan,1); cblas_zcopy(n-1,o,0,&compan[2],n+1); cblas_zcopy(n,&poly[2],1,compan,n);
            info = LAPACKE_zhseqr(LAPACK_COL_MAJOR,job,compz,n,ilo,ihi,(lapack_complex_double *)compan,ldh,(lapack_complex_double *)wc,(lapack_complex_double *)zz,ldz);
            if (info) { fprintf(stderr,"error in poly2lsf_z: lapacke decomposition failed\n"); return 1; }
            for (c=0; c<n; c++) { omegas[n+c] = atan2(wc[2*c+1],wc[2*c]); }
            //for (c=0; c<n; c++) { omegas[n+c] = (wc[2*r+1]>10.0*DBL_EPSILON) ? atan2(wc[2*r+1],wc[2*r]) : 0.0; }
            qsort(omegas,(size_t)(2*n),sizeof(double),cmp_ascend_d);
            if (iscolmajor) { cblas_dcopy(2*n,omegas,1,&Y[r],R); }
            else { cblas_dcopy(2*n,omegas,1,&Y[2*r*n],1); }
        }
    }
    else
    {
        fprintf(stderr,"error in poly2lsf_z: dim must be 0 or 1.\n"); return 1;
    }

    //Exit
    free(compan); free(wc); free(poly); free(prev); free(omegas);
    return 0;
}

#ifdef __cplusplus
}
}
#endif


#include <math.h>
#include <galpy_potentials.h>
#include <stdio.h>
#include <gsl/gsl_sf_gegenbauer.h>
#include <gsl/gsl_sf_legendre.h>

#ifndef GSL_MAJOR_VERSION
#define GSL_MAJOR_VERSION 1
#endif
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
//SCF Disk potential
//4 arguments: amp, Acos, Asin, a

const int FORCE =1;
const int DERIV =2;

//Useful Functions

//Converts from cylindrical coordinates to spherical
static inline void cyl_to_spher(double R, double Z,double *r, double *theta)
{
    *r = sqrt(R*R + Z*Z);
    *theta = atan2(R, Z);
}

//Integer power
double power(double x, int i)
{
    if (i==0)
        return 1;
    return x*power(x,i - 1);
}

//Calculates xi
static inline void calculateXi(double r, double a, double *xi)
{
    *xi = (r - a)/(r + a);
}

//Potentials, forces, and derivative functions
//LCOV_EXCL_START
double computePhi(double Acos_val, double Asin_val, double mCos, double mSin, double P, double phiTilde, int m)
{
    return (Acos_val*mCos + Asin_val*mSin)*P*phiTilde;
}
//LCOV_EXCL_STOP
double computeAxiPhi(double Acos_val, double P, double phiTilde)
{
    return Acos_val*P*phiTilde;
}

double computeF_r(double Acos_val, double Asin_val, double mCos, double mSin, double P, double dphiTilde, int m)
{
    return -(Acos_val*mCos + Asin_val*mSin)*P*dphiTilde;
}
double computeAxiF_r(double Acos_val, double P, double dphiTilde)
{
    return -Acos_val*P*dphiTilde;
}

double computeF_theta(double Acos_val, double Asin_val, double mCos, double mSin, double dP, double phiTilde, int m)
{
    return -(Acos_val*mCos + Asin_val*mSin)*dP*phiTilde;
}
double computeAxiF_theta(double Acos_val, double dP, double phiTilde)
{
    return -Acos_val*dP*phiTilde;
}

double computeF_phi(double Acos_val, double Asin_val, double mCos, double mSin, double P, double phiTilde, int m)
{
    return m*(Acos_val*mSin - Asin_val*mCos)*P*phiTilde;
}
double computeAxiF_phi(double Acos_val, double P, double phiTilde)
{
    return 0.;
}

double computeF_rr(double Acos_val, double Asin_val, double mCos, double mSin, double P, double d2phiTilde, int m)
{
    return -(Acos_val*mCos + Asin_val*mSin)*P*d2phiTilde;
}
double computeAxiF_rr(double Acos_val, double P, double d2phiTilde)
{
    return -Acos_val*P*d2phiTilde;
}

double computeF_rphi(double Acos_val, double Asin_val, double mCos, double mSin, double P, double dphiTilde, int m)
{
    return m*(Acos_val*mSin - Asin_val*mCos)*P*dphiTilde;
}
double computeAxiF_rphi(double Acos_val, double P, double d2phiTilde)
{
    return 0.;
}

double computeF_phiphi(double Acos_val, double Asin_val, double mCos, double mSin, double P, double phiTilde, int m)
{
    return m*m*(Acos_val*mCos + Asin_val*mSin)*P*phiTilde;
}
double computeAxiF_phiphi(double Acos_val, double P, double d2phiTilde)
{
    return 0.;
}

//Calculates the Gegenbauer polynomials
void compute_C(double xi, int N, int L, double * C_array)
{
    int l;
    for (l = 0; l < L; l++)
    {
        gsl_sf_gegenpoly_array(N - 1, 3./2 + 2*l, xi, C_array + l*N);
    }
}

//Calculates the derivative of the Gegenbauer polynomials
void compute_dC(double xi, int N, int L, double * dC_array)
{
    int n,l;
    for (l = 0; l < L; l++)
    {
        *(dC_array +l*N) = 0;
        if (N != 1)
            gsl_sf_gegenpoly_array(N - 2, 5./2 + 2*l, xi, dC_array + l*N + 1);
        for (n = 0; n<N; n++)
        {
            *(dC_array +l*N + n) *= 2*(2*l + 3./2);
        }
    }

}

//Calculates the second derivative of the Gegenbauer polynomials
void compute_d2C(double xi, int N, int L, double * d2C_array)
{
    int n,l;
    for (l = 0; l < L; l++)
    {
        *(d2C_array +l*N) = 0;
        if (N >1)
            *(d2C_array +l*N + 1) = 0;
        if (N > 2)
            gsl_sf_gegenpoly_array(N - 3, 7./2 + 2*l, xi, d2C_array + l*N + 2);
        for (n = 0; n<N; n++)
        {
            *(d2C_array +l*N + n) *= 4*(2*l + 3./2)*(2*l + 5./2);
        }
    }

}

//Compute phi_Tilde
void compute_phiTilde(double r, double a, int N, int L, double* C, double * phiTilde)
{
    double xi;
    calculateXi(r, a, &xi);
    double rterms = -1./(r + a);
    int n,l;
    for (l = 0; l < L; l++)
    {

        if (l != 0)
            rterms *= r*a/((a + r)*(a + r));

        for (n = 0; n < N; n++)
        {
            *(phiTilde + l*N + n)  = rterms*(*(C + n + l*N));
        }
    }

}

//Computes the derivative of phiTilde with respect to r
void compute_dphiTilde(double r, double a, int N, int L, double * C, double * dC, double * dphiTilde)
{
    double xi;
    calculateXi(r, a, &xi);
    double rterm = 1./(r*power(a + r, 3));
    int n,l;
    for (l = 0; l < L; l++)
    {
        if (l != 0)
        {
            rterm *= (a*r)/power(a + r, 2) ;
        }
        for (n = 0; n < N; n++)
        {
            *( dphiTilde + l*N + n) = rterm *(((2*l + 1)*r*(a + r) - l*power(a + r,2))*(*(C + l*N + n)) -
                                              2*a*r*(*(dC + l*N + n)));

        }
    }
}

//Computes the second derivative of phiTilde with respect to r
void compute_d2phiTilde(double r, double a, int N, int L, double * C, double * dC,double * d2C, double * d2phiTilde)
{
    double xi;
    calculateXi(r, a, &xi);
    double rterm = 1./(r*r) / power(a + r,5);
    int n,l;
    for (l = 0; l < L; l++)
    {

        if (l != 0)
        {
            rterm *= (a*r)/power(a + r, 2);
        }
        for (n = 0; n < N; n++)
        {

            double C_val = *(C + l*N + n);
            double dC_val = *(dC + l*N + n);
            double d2C_val = *(d2C + l*N + n);
            *( d2phiTilde + l*N + n) = rterm*(C_val*(l*(1 - l)*power(a + r, 4) - (4*l*l + 6*l + 2.)*r*r*power(a + r,2) +
                                              l*(4*l + 2)*r*power(a + r,3))
                                              + a*r*((4*r*r + 4*a*r + (8*l + 4)*r*(a + r)-
                                                      4*l*power(a + r,2))*dC_val - 4*a*r*d2C_val));

        }
    }
}

//Computes the associated Legendre polynomials
void compute_P(double x, int L, int M, double * P_array)
{
    if (M == 1){
        gsl_sf_legendre_Pl_array (L - 1, x, P_array);
    } else {
        #if GSL_MAJOR_VERSION == 2
            gsl_sf_legendre_array_e(GSL_SF_LEGENDRE_NONE,L - 1, x, -1, P_array);
        #else
            int m;
            for (m = 0; m < M; m++)
            {
                gsl_sf_legendre_Plm_array(L - 1, m, x, P_array);
                P_array += L - m;
            }
        #endif
    }

}

//Computes the associated Legendre polynomials and its derivative
void compute_P_dP(double x, int L, int M, double * P_array, double *dP_array)
{
    if (M == 1){
        gsl_sf_legendre_Pl_deriv_array (L - 1, x, P_array, dP_array);
    } else {
        #if GSL_MAJOR_VERSION == 2
            gsl_sf_legendre_deriv_array_e(GSL_SF_LEGENDRE_NONE, L - 1, x, -1,P_array, dP_array);

        #else
            int m;
            for (m = 0; m < M; m++)
            {
                gsl_sf_legendre_Plm_deriv_array(L - 1, m, x, P_array, dP_array);
                P_array += L - m;
                dP_array += L - m;
            }
        #endif
    }
}

typedef struct equations equations;
struct equations
{
    double ((**Eq)(double, double, double, double, double, double, int));
    double *(*phiTilde);
    double *(*P);
    double *Constant;
};

typedef struct axi_equations axi_equations;
struct axi_equations
{
    double ((**Eq)(double, double, double));
    double *(*phiTilde);
    double *(*P);
    double *Constant;
};

//Compute axi symmetric potentials.
void compute(double a, int N, int L, int M,
	     double r, double theta, double phi,
	     double *Acos, int eq_size,
	     axi_equations e,
	     double *F)
{
    int i,n,l;
    for (i = 0; i < eq_size; i++)
    {
        *(F + i) =0; //Initialize each F
    }

    for (l = 0; l < L; l++)
    {

        for (n = 0; n < N; n++)
        {

            double Acos_val = *(Acos + M*l + M*L*n);
            for (i = 0; i < eq_size; i++)
            {
                double (*Eq)(double, double, double) = *(e.Eq + i);
                double *P = *(e.P + i);
                double *phiTilde = *(e.phiTilde + i);
                *(F + i) += (*Eq)(Acos_val, P[l], phiTilde[l*N + n]);
            }

        }
    }

    //Multiply F by constants
    for (i = 0; i < eq_size; i++)
    {
        double constant = *(e.Constant + i);
        *(F + i) *= constant*sqrt(4*M_PI);
    }

}

//Compute Non Axi symmetric Potentials
void computeNonAxi(double a, int N, int L, int M,
		   double r, double theta, double phi,
		   double *Acos, double *Asin, int eq_size,
		   equations e,
		   double *F)
{
    int i,n,l,m;
    for (i = 0; i < eq_size; i++)
    {
        *(F + i) =0; //Initialize each F
    }

    int v1counter=0;
    int v2counter = 0;
    int Pindex = 0;
    for (l = 0; l < L; l++)
    {
        v1counter = 0;
        for (m = 0; m<=l; m++)
        {
            #if GSL_MAJOR_VERSION == 2
                Pindex = v2counter;
            #else
                Pindex = v1counter + l;
            #endif

            double mCos = cos(m*phi);
            double mSin = sin(m*phi);
            for (n = 0; n < N; n++)
            {
                double Acos_val = *(Acos +m + M*l + M*L*n);
                double Asin_val = *(Asin +m + M*l + M*L*n);
                for (i = 0; i < eq_size; i++)
                {
                    double (*Eq)(double, double, double, double, double, double, int) = *(e.Eq + i);
                    double *P = *(e.P + i);
                    double *phiTilde = *(e.phiTilde + i);
                    *(F + i) += (*Eq)(Acos_val, Asin_val, mCos, mSin, P[Pindex], phiTilde[l*N + n], m);
                }

            }

                v2counter++;
                v1counter+=M-m - 1;

        }

    }
    //Multiply F by constants
    for (i = 0; i < eq_size; i++)
    {
        double constant = *(e.Constant + i);
        *(F + i) *= constant*sqrt(4*M_PI);
    }

}

//Compute the Forces
void computeForce(double R,double Z, double phi,
		  double t,
		  struct potentialArg * potentialArgs, double * F)
{
    double * args= potentialArgs->args;
    //Get args
    double a = *args++;
    int isNonAxi = (int)*args++;
    int N = (int)*args++;
    int L = (int)*args++;
    int M = (int)*args++;

    double* Acos = args;

    double* caching_i = (args + (isNonAxi + 1)*N*L*M);
    double *Asin;
    if (isNonAxi == 1)
    {
        Asin = args + N*L*M;
    }
    double *cached_type = caching_i;
    double * cached_coords = (caching_i+ 1);
    double * cached_values = (caching_i + 4);
    if ((int)*cached_type==FORCE)
    {
        if (*cached_coords == R && *(cached_coords + 1) == Z && *(cached_coords + 2) == phi)
        {
            *F = *cached_values;
            *(F + 1) = *(cached_values + 1);
            *(F + 2) = *(cached_values + 2);
            return;
        }
    }
    double r;
    double theta;
    cyl_to_spher(R, Z, &r, &theta);

    double xi;
    calculateXi(r, a, &xi);

//Compute the gegenbauer polynomials and its derivative.
    double *C= (double *) malloc ( N*L * sizeof(double) );
    double *dC= (double *) malloc ( N*L * sizeof(double) );
    double *phiTilde= (double *) malloc ( N*L * sizeof(double) );
    double *dphiTilde= (double *) malloc ( N*L * sizeof(double) );

    compute_C(xi, N, L, C);
    compute_dC(xi, N, L, dC);

//Compute phiTilde and its derivative
    compute_phiTilde(r, a, N, L, C, phiTilde);

    compute_dphiTilde(r, a, N, L, C, dC, dphiTilde);

//Compute Associated Legendre Polynomials
    int M_eff = M;
    int size = 0;

    if (isNonAxi==0)
    {
    M_eff = 1;
    size = L;
    } else{
    size = L*L - L*(L-1)/2;
    }

    double *P= (double *) malloc ( size * sizeof(double) );
    double *dP= (double *) malloc ( size * sizeof(double) );
    compute_P_dP(cos(theta), L, M_eff, P, dP);

    double (*PhiTilde_Pointer[3]) = {dphiTilde,phiTilde,phiTilde};
    double (*P_Pointer[3]) = {P, dP, P};

    double Constant[3] = {1., -sin(theta), 1.};

    if (isNonAxi == 1)
    {
        double (*Eq[3])(double, double, double, double, double, double, int) = {&computeF_r, &computeF_theta, &computeF_phi};
        equations e = {Eq,&PhiTilde_Pointer[0], &P_Pointer[0], &Constant[0]};
        computeNonAxi(a, N, L, M,r, theta, phi, Acos, Asin, 3, e, F);
    }
    else
    {
        double (*Eq[3])(double, double, double) = {&computeAxiF_r, &computeAxiF_theta, &computeAxiF_phi};
        axi_equations e = {Eq,&PhiTilde_Pointer[0], &P_Pointer[0], &Constant[0]};
        compute(a, N, L, M,r, theta, phi, Acos, 3, e, F);
    }

    //Caching

    *cached_type = (double)FORCE;

    * cached_coords = R;
    * (cached_coords + 1) = Z;
    * (cached_coords + 2) = phi;
    * (cached_values) = *F;
    * (cached_values + 1) = *(F + 1);
    * (cached_values + 2) = *(F + 2);

    // Free memory
    free(C);
    free(dC);
    free(phiTilde);
    free(dphiTilde);
    free(P);
    free(dP);

}

//Compute the Derivatives
void computeDeriv(double R,double Z, double phi,
		  double t,
		  struct potentialArg * potentialArgs, double * F)
{
    double * args= potentialArgs->args;
    //Get args
    double a = *args++;
    int isNonAxi = (int)*args++;
    int N = (int) *args++;
    int L = (int) *args++;
    int M = (int) *args++;
    double* Acos = args;

    double * caching_i = (args + (isNonAxi + 1)*N*L*M);
    double *Asin;
    if (isNonAxi == 1)
    {
        Asin = args + N*L*M;
    }

    double *cached_type = caching_i;
    double * cached_coords = (caching_i+ 1);
    double * cached_values = (caching_i + 4);
    if ((int)*cached_type==DERIV)
    {
        if (*cached_coords == R && *(cached_coords + 1) == Z && *(cached_coords + 2) == phi)
        {
            *F = *cached_values;
            *(F + 1) = *(cached_values + 1);
            *(F + 2) = *(cached_values + 2);
            return;
        }
    }

    double r;
    double theta;
    cyl_to_spher(R, Z, &r, &theta);

    double xi;
    calculateXi(r, a, &xi);

//Compute the gegenbauer polynomials and its derivative.
    double *C= (double *) malloc ( N*L * sizeof(double) );
    double *dC= (double *) malloc ( N*L * sizeof(double) );
    double *d2C= (double *) malloc ( N*L * sizeof(double) );
    double *phiTilde= (double *) malloc ( N*L * sizeof(double) );
    double *dphiTilde= (double *) malloc ( N*L * sizeof(double) );
    double *d2phiTilde= (double *) malloc ( N*L * sizeof(double) );

    compute_C(xi, N, L, C);
    compute_dC(xi, N, L, dC);
    compute_d2C(xi, N, L, d2C);

//Compute phiTilde and its derivative
    compute_phiTilde(r, a, N, L, C, phiTilde);
    compute_dphiTilde(r, a, N, L, C, dC, dphiTilde);
    compute_d2phiTilde(r, a, N, L, C, dC, d2C, d2phiTilde);

//Compute Associated Legendre Polynomials
    int M_eff = M;
    int size = 0;

    if (isNonAxi==0)
    {
    M_eff = 1;
    size = L;
    } else{
    size = L*L - L*(L-1)/2;
    }
    double *P= (double *) malloc ( size * sizeof(double) );

    compute_P(cos(theta), L,M_eff, P);

    double (*PhiTilde_Pointer[3])= {d2phiTilde,phiTilde,dphiTilde};
    double (*P_Pointer[3]) = {P, P, P};

    double Constant[3] = {1., 1., 1.};

    if (isNonAxi==1)
    {
        double (*Eq[3])(double, double, double, double, double, double, int) = {&computeF_rr, &computeF_phiphi, &computeF_rphi};
        equations e = {Eq,&PhiTilde_Pointer[0],&P_Pointer[0],&Constant[0]};
        computeNonAxi(a, N, L, M,r, theta, phi, Acos, Asin, 3, e, F);
    }
    else
    {
        double (*Eq[3])(double, double, double) = {&computeAxiF_rr, &computeAxiF_phiphi, &computeAxiF_rphi};
        axi_equations e = {Eq,&PhiTilde_Pointer[0],&P_Pointer[0],&Constant[0]};
        compute(a, N, L, M,r, theta, phi, Acos, 3, e, F);
    }

    //Caching

    *cached_type = (double)DERIV;

    * cached_coords = R;
    * (cached_coords + 1) = Z;
    * (cached_coords + 2) = phi;
    * (cached_values) = *F;
    * (cached_values + 1) = *(F + 1);
    * (cached_values + 2) = *(F + 2);

    //Free memory
    free(C);
    free(dC);
    free(d2C);
    free(phiTilde);
    free(dphiTilde);
    free(d2phiTilde);
    free(P);
}

//Compute the Potential
double SCFPotentialEval(double R,double Z, double phi,
                        double t,
                        struct potentialArg * potentialArgs)
{
    double * args= potentialArgs->args;
    //Get args
    double a = *args++;
    int isNonAxi = (int)*args++;
    int N = (int) *args++;
    int L = (int) *args++;
    int M = (int) *args++;
    double* Acos = args;
    double* Asin;
    if (isNonAxi==1) //LCOV_EXCL_START
    {
        Asin = args + N*L*M;
    } //LCOV_EXCL_STOP
    //convert R,Z to r, theta
    double r;
    double theta;
    cyl_to_spher(R, Z,&r, &theta);
    double xi;
    calculateXi(r, a, &xi);

    //Compute the gegenbauer polynomials and its derivative.
    double *C= (double *) malloc ( N*L * sizeof(double) );
    double *phiTilde= (double *) malloc ( N*L * sizeof(double) );

    compute_C(xi, N, L, C);

    //Compute phiTilde and its derivative
    compute_phiTilde(r, a, N, L, C, phiTilde);
    //Compute Associated Legendre Polynomials

    int M_eff = M;
    int size = 0;

    if (isNonAxi==0)
    {
    M_eff = 1;
    size = L;
    } else{ //LCOV_EXCL_START
    size = L*L - L*(L-1)/2;
    } //LCOV_EXCL_STOP

    double *P= (double *) malloc ( size * sizeof(double) );

    compute_P(cos(theta), L,M_eff, P);

    double potential;

    double (*PhiTilde_Pointer[1]) = {phiTilde};
    double (*P_Pointer[1]) = {P};

    double Constant[1] = {1.};

    if (isNonAxi==1) //LCOV_EXCL_START
    {
        double (*Eq[1])(double, double, double, double, double, double, int) = {&computePhi};
        equations e = {Eq,&PhiTilde_Pointer[0],&P_Pointer[0],&Constant[0]};
        computeNonAxi(a, N, L, M,r, theta, phi, Acos, Asin, 1, e, &potential);
    } //LCOV_EXCL_STOP
    else
    {
        double (*Eq[1])(double, double, double) = {&computeAxiPhi};
        axi_equations e = {Eq,&PhiTilde_Pointer[0],&P_Pointer[0],&Constant[0]};
        compute(a, N, L, M,r, theta, phi, Acos, 1, e, &potential);
    }

    //Free memory
    free(C);
    free(phiTilde);
    free(P);

    return potential;

}

//Compute the force in the R direction
double SCFPotentialRforce(double R,double Z, double phi,
                          double t,
                          struct potentialArg * potentialArgs)
{
    double r;
    double theta;
    cyl_to_spher(R, Z, &r, &theta);
    // The derivatives
    double dr_dR = R/r;
    double dtheta_dR = Z/(r*r);
    double dphi_dR = 0;

    double F[3];
    computeForce(R, Z, phi, t,potentialArgs, &F[0]) ;

    return *(F + 0)*dr_dR + *(F + 1)*dtheta_dR + *(F + 2)*dphi_dR;

}

//Compute the force in the z direction
double SCFPotentialzforce(double R,double Z, double phi,
                          double t,
                          struct potentialArg * potentialArgs)
{
    double r;
    double theta;
    cyl_to_spher(R, Z,&r, &theta);

    double dr_dz = Z/r;
    double dtheta_dz = -R/(r*r);
    double dphi_dz = 0;

    double F[3];
    computeForce(R, Z, phi, t,potentialArgs, &F[0]) ;
    return *(F + 0)*dr_dz + *(F + 1)*dtheta_dz + *(F + 2)*dphi_dz;
}

//Compute the force in the phi direction
double SCFPotentialphiforce(double R,double Z, double phi,
                            double t,
                            struct potentialArg * potentialArgs)
{

    double r;
    double theta;
    cyl_to_spher(R, Z, &r, &theta);

    double dr_dphi = 0;
    double dtheta_dphi = 0;
    double dphi_dphi = 1;

    double F[3];
    computeForce(R, Z, phi, t,potentialArgs, &F[0]) ;

    return *(F + 0)*dr_dphi + *(F + 1)*dtheta_dphi + *(F + 2)*dphi_dphi;
}

//Compute the planar force in the R direction
double SCFPotentialPlanarRforce(double R,double phi,
                                double t,
                                struct potentialArg * potentialArgs)
{
    return SCFPotentialRforce(R,0., phi,t,potentialArgs);

}

//Compute the planar force in the phi direction
double SCFPotentialPlanarphiforce(double R,double phi,
                                  double t,
                                  struct potentialArg * potentialArgs)
{
    return SCFPotentialphiforce(R,0., phi,t,potentialArgs);
}

//Compute the planar double derivative of the potential with respect to R
double SCFPotentialPlanarR2deriv(double R, double phi,
                                 double t,
                                 struct potentialArg * potentialArgs)
{
    double Farray[3];
    computeDeriv(R, 0, phi, t,potentialArgs, &Farray[0]) ;
    return *Farray;
}

//Compute the planar double derivative of the potential with respect to phi
double SCFPotentialPlanarphi2deriv(double R, double phi,
                                   double t,
                                   struct potentialArg * potentialArgs)
{
    double Farray[3];
    computeDeriv(R, 0, phi, t,potentialArgs, &Farray[0]) ;
    return *(Farray + 1);
}

//Compute the planar double derivative of the potential with respect to R, Phi
double SCFPotentialPlanarRphideriv(double R, double phi,
                                   double t,
                                   struct potentialArg * potentialArgs)
{
    double Farray[3];
    computeDeriv(R, 0, phi, t,potentialArgs, &Farray[0]) ;
    return *(Farray + 2);
}


/*
 * mcmc_lotkavolterra.c
 *
 *  Created on: 15/11/2015
 *      Author: Laura Ferrer
 */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#define USAGE "./mcmc_lotkavolterra.x n_steps n_burn"

void leer(double *tiempo, double *presa, double *depredador, int n_puntos);
double* iniciar(int n_puntos);

int main (int argc, char **argv)
{
	int i=0;
	int lineas = 0;
	int iteraciones = atof(argv[2])+atof(argv[1]);
	int corte = atof(argv[2]);

	double a = 1;
	double b = 1;
	double c = 1;
	double d = 1;

	double a2;
	double b2;
	double c2;
	double d2;

	FILE *file;
	file = fopen("lotka_volterra_obs.dat", "r");

	while(!feof(file)) //Cuenta la cantidad de datos provistos.
	{
  		char ch = fgetc(file);
  		if(ch == '\n')
  		{
    			lineas++;
  		}
	}

	fclose(file);

	lineas --;// menos la primera línea

	double *x = iniciar(lineas);
	double *y= iniciar(lineas);
	double *x_real=	iniciar(lineas);
	double *y_real = iniciar(lineas);
	double *t = iniciar(lineas);
	double *lay = iniciar(lineas);
	double *lax = iniciar(lineas);

	leer(t,x_real,y_real,lineas);
	lay[0]=y_real[0];
	lax[0]=x_real[0];

	double parecido (double modo)
	{
		i = 0;
		double suma=0;
		if (modo == 1)
		{

			for (i = 0; i < lineas; i += 1)
			{
				suma += pow(x[i]-y_real[i],2.0);
				suma += pow(y[i]-x_real[i],2.0);
			}

		}

		if (modo == 2)
		{

			for (i = 0; i < lineas; i += 1)
			{
				suma += pow(lax[i]-y_real[i],2.0);
				suma += pow(lay[i]-x_real[i],2.0);
			}
		}

		return (1.0/2.0)*suma;
	}

	double dx (double x, double y, double la, double lb)
	{
		double dx = x*(la-lb*y);
		return dx;
	}

	double dy (double x, double y, double lc, double ld)
	{
		double dy = -y*(lc-ld*x);
		return dy;
	}

	void kutta4 (double la, double lb, double lc, double ld)
	{

		double h = t[1]-t[0];
		i = 1;

		for (i = 1; i < lineas; i += 1)
		{
			//Inicio
			double kx1 = dx(lax[i-1],lay[i-1],la,lb);
			double ky1 = dy(lax[i-1],lay[i-1],lc,ld);

			//paso 1
			double x1 = lax[i-1] + (h/2.0)*kx1;
			double y1 = lay[i-1] + (h/2.0)*ky1;

			double kx2 = dx(x1,y1,la,lb);
			double ky2 = dy(x1,y1,lc,ld);

			//paso 2
			double x2 = lax[i-1] + (h/2.0)*kx2;
			double y2 = lay[i-1] + (h/2.0)*ky2;

			double kx3 = dx(x2,y2,la,lb);
			double ky3 = dy(x2,y2,lc,ld);

			//paso 3
			double x3 = lax[i-1] + h*kx3;
			double y3 = lay[i-1] + h*ky3;

			double kx4 = dx(x3,y3,la,lb);
			double ky4 = dy(x3,y3,lc,ld);

			//paso 4

			double mx = (1.0/6.0)*(kx1 + 2.0*kx2+2.0*kx3+kx4);
			double my = (1.0/6.0)*(ky1 + 2.0*ky2+2.0*ky3+ky4);

			lax[i]= lax[i-1] + h*mx;
			lay[i]= lay[i-1] + h*my;

		}
	}

	void mcmc()
	{
		int k = 1;
		for (k = 1; k < iteraciones; k++)
		{
    			const gsl_rng_type * T;
    			gsl_rng * r;
   			gsl_rng_env_setup();
    			T = gsl_rng_default;
   			r = gsl_rng_alloc (T);

			a2 = a + (2*gsl_ran_gaussian(r,drand48())+1);
			srand((unsigned)time(NULL));
			b2 = b + (2*gsl_ran_gaussian(r,drand48())+1);
			srand((unsigned)time(NULL));
			c2 = c + (2*gsl_ran_gaussian(r,drand48())+1);
			srand((unsigned)time(NULL));
			d2 = d + (2*gsl_ran_gaussian(r,drand48())+1);
			srand((unsigned)time(NULL));

			kutta4(a,b,c,d);

			int j =0;

			for (j = 0; j < lineas; j++) // Copia las listas
			{
     				x[j] = lax[j];
				y[j] = lay[j];
   			}

			kutta4(a2,b2,c2,d2);

			// Calculo de los errores y similitudes
			double difvieja = parecido(1);
			double difnueva = parecido(2);

			if (difnueva < difvieja)
			{
				a=a2;
        	    		b=b2;
        	   		c=c2;
        	    		d=d2;
				if (k>corte) {printf ("%g,%g,%g,%g\n",a,b,c,d);}
			}
			else
			{

				srand((unsigned)time(NULL));
				double alfa = difvieja/difnueva;
				double beta = drand48();

				if (beta >= alfa)
				{
					a=a2;
        	    			b=b2;
        	   			c=c2;
        	    			d=d2;
					if (k>corte) {printf ("%g,%g,%g,%g\n",a,b,c,d);}
				}
				else if (k>corte)
				{
					printf ("%g,%g,%g,%g\n",a,b,c,d);
				}
			}
		}
	}

	mcmc();
	return 0;
}

void leer(double *tiempo, double *presa, double *depredador, int n_puntos)
{
	FILE *file;
	int j;
	file = fopen("lotka_volterra_obs.dat", "r");
	rewind(file);
	fscanf(file,"%*[^\n]\n"); //Lee cualquier cosa en la primera línea
	for(j=0;j<n_puntos;j++)
	{
		fscanf(file, "%lf %lf %lf\n", &tiempo[j], &presa[j], &depredador[j]);
	}
	fclose(file);
}

double* iniciar(int n_puntos)
{
	double *array;
	int i;
	if(!(array = malloc(n_puntos * sizeof(double))))
	{
    		printf("Problema en reserva\n");
    		exit(1);
  	}
  	for(i=0;i<n_puntos;i++)
	{
  	array[i] = 0.0;
  	}
  return array;
}


/* statistics/gsl_statistics_ushort.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Jim Davies, 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.
 */

#ifndef __GSL_STATISTICS_USHORT_H__
#define __GSL_STATISTICS_USHORT_H__

#include <stddef.h>
#include <gsl/gsl_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

GSL_EXPORT double gsl_stats_ushort_mean (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ushort_variance (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ushort_sd (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ushort_variance_with_fixed_mean (const unsigned short data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ushort_sd_with_fixed_mean (const unsigned short data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ushort_absdev (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ushort_skew (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ushort_kurtosis (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ushort_lag1_autocorrelation (const unsigned short data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_ushort_covariance (const unsigned short data1[], const size_t stride1,const unsigned short data2[], const size_t stride2, const size_t n);

GSL_EXPORT double gsl_stats_ushort_variance_m (const unsigned short data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ushort_sd_m (const unsigned short data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ushort_absdev_m (const unsigned short data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ushort_skew_m_sd (const unsigned short data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_ushort_kurtosis_m_sd (const unsigned short data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_ushort_lag1_autocorrelation_m (const unsigned short data[], const size_t stride, const size_t n, const double mean);

GSL_EXPORT double gsl_stats_ushort_covariance_m (const unsigned short data1[], const size_t stride1,const unsigned short data2[], const size_t stride2, const size_t n, const double mean1, const double mean2);

GSL_EXPORT double gsl_stats_ushort_pvariance (const unsigned short data1[], const size_t stride1, const size_t n1, const unsigned short data2[], const size_t stride2, const size_t n2);
GSL_EXPORT double gsl_stats_ushort_ttest (const unsigned short data1[], const size_t stride1, const size_t n1, const unsigned short data2[], const size_t stride2, const size_t n2);

GSL_EXPORT unsigned short gsl_stats_ushort_max (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT unsigned short gsl_stats_ushort_min (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_ushort_minmax (unsigned short * min, unsigned short * max, const unsigned short data[], const size_t stride, const size_t n);

GSL_EXPORT size_t gsl_stats_ushort_max_index (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT size_t gsl_stats_ushort_min_index (const unsigned short data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_ushort_minmax_index (size_t * min_index, size_t * max_index, const unsigned short data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_ushort_median_from_sorted_data (const unsigned short sorted_data[], const size_t stride, const size_t n) ;
GSL_EXPORT double gsl_stats_ushort_quantile_from_sorted_data (const unsigned short sorted_data[], const size_t stride, const size_t n, const double f) ;

__END_DECLS

#endif /* __GSL_STATISTICS_USHORT_H__ */


/*
Performing approximate Bayesian computation sequential Monte Carlo (Toni et al.
2009) on a beta-binomial model.

Synthetic data is generated by `ground_truth_and_analysis.ipynb`.

This script writes particles.csv, where each row corresponds to a particle and
each column corresponds to a round of SMC.

Defining #DEBUG_MODE will silence all writing to stdout. One may then add
printf statements in the code, and perhaps write the output to file as:
`./run.sh > output.txt`

TODO: Export all functions which are specific to the beta-binomial model into
smc.h, and perhaps rename smc.h as beta_binomial.h

Author: Juvid Aryaman
*/

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

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_statistics.h>

#define N_DATA 50
#define N_TRUTH 10

#define PRIOR_ALPHA 0.5
#define PRIOR_BETA 0.5

#define N_PARTICLES 5000
#define N_ROUNDS_SMC 50
#define KERNEL_SD 0.05
#define QUANTILE_ACCEPT_DISTANCE 0.8

#define RND gsl_rng_uniform(r)
#define SEED 1
#define DISTANCE_THRESHOLD_INIT 10

#define OUTFILE_NAME "particles.csv"

//#define DEBUG_MODE

#include "smc.h"

int main(int argc, char *argv[]) {

/* set up GSL RNG */
gsl_rng *r = gsl_rng_alloc(gsl_rng_mt19937);
/* end of GSL setup */

gsl_rng_set(r, SEED);

/////////////////////////
/*Read data*/
/////////////////////////

FILE *data_pointer;

data_pointer = fopen("binom_data.csv", "r");

int data[N_DATA];
int i, j, read_error_status;
for (i=0; i < N_DATA; i++){
	read_error_status = fscanf(data_pointer, "%d\n", &data[i]);
}
if (read_error_status != 1){printf("Error reading data\n"); return 0;}

/////////////////////////
/*Initialise variables*/
/////////////////////////

/*Make a (N_ROUNDS_SMC X N_PARTICLES) array to store all particles at all rounds
of SMC*/
double** theta_particle;
theta_particle = (double**) malloc(N_ROUNDS_SMC * sizeof(double*));
for (i = 0; i < N_ROUNDS_SMC; i++){
	theta_particle[i] = (double*) malloc(N_PARTICLES * sizeof(double));
}

double distance_threshold = DISTANCE_THRESHOLD_INIT;
int *simulated_data = (int*) malloc(N_DATA * sizeof(int));
double *distance = malloc(N_PARTICLES * sizeof(double));
double *weight = malloc(N_PARTICLES * sizeof(double));
double weight_normalizer = 0.0;

int time_smc=0; // an index of each round of SMC
int param_index_chosen;

/////////////////////////
/*Perform ABC SMC*/
/////////////////////////

/*For every round of SMC*/
for (time_smc = 0; time_smc < N_ROUNDS_SMC; time_smc++) {
	#ifndef DEBUG_MODE
		printf("Round %d of SMC\n", time_smc);
	#endif

	/*Draw or perturb a particle and compute distance*/
	for (i = 0; i < N_PARTICLES; i++) {
		distance[i] = distance_threshold + 1.0; // reset distance of particle i
		while (distance[i] > distance_threshold) {
			if (time_smc == 0) {
				// Sample from the prior
				theta_particle[0][i] = gsl_ran_beta(r, PRIOR_ALPHA, PRIOR_BETA);
			}
			else{
				/*Sample from the old weights and perturb*/
				param_index_chosen = weighted_choice(r, weight);
				if ((param_index_chosen < 0)||(param_index_chosen >= N_PARTICLES)) {
					printf("Error in param_index_chosen\n"); return -1;
			}
				theta_particle[time_smc][i] =
					theta_particle[time_smc-1][param_index_chosen] +
					gsl_ran_gaussian(r, KERNEL_SD);

				// check if bounds of prior exceeded
				if((theta_particle[time_smc][i]<0) || (theta_particle[time_smc][i] > 1)) continue;
			}

			// Simulate a candidate dataset
			for (j = 0; j < N_DATA; j++) {
				simulated_data[j] = gsl_ran_binomial(r, theta_particle[time_smc][i], N_TRUTH);
			}

			// Compute distance between data and simulation
			distance[i] = distance_metric(data, simulated_data);

		}
	}
	#ifndef DEBUG_MODE
		printf("Particles sampled.\n");
	#endif

	/*Compute weights*/
	if (time_smc==0){ for (i = 0; i < N_PARTICLES; i++) weight[i] = 1.0;}
	else{
		weight_normalizer = 0.0;
		for (i = 0; i < N_PARTICLES; i++) {
			weight_normalizer += weight[i]*kernel_pdf(theta_particle[time_smc-1][i],
				theta_particle[time_smc][i]);
		}

		for (i = 0; i < N_PARTICLES; i++) {
			weight[i] = prior_pdf(theta_particle[time_smc][i])/weight_normalizer;
		}

	}

	/*Normalise weights*/
	weight_normalizer = 0.0;
	for (i = 0; i < N_PARTICLES; i++)	weight_normalizer += weight[i];
	for (i = 0; i < N_PARTICLES; i++)	weight[i] = weight[i]/weight_normalizer;

	/* Resample weights*/
	distance_threshold = update_distance_threshold(distance);

}

#ifndef DEBUG_MODE
	printf("Writing particles to file\n");
#endif
	write_particles_to_csv(theta_particle);
#ifndef DEBUG_MODE
	printf("Done!\n");
#endif

return 0; //return from main
} //close main


#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_qrng.h>
#include <gsl/gsl_errno.h>

/* Setup */
gsl_qrng *mgsl_qrng_alloc(int type, unsigned int d)
{
  const gsl_qrng_type *types[] = { gsl_qrng_niederreiter_2, gsl_qrng_sobol, gsl_qrng_halton, gsl_qrng_reversehalton };
  const gsl_qrng_type *T;
  gsl_qrng *q;
  T = types[type];
  q = gsl_qrng_alloc(T, d);
  return q;
}


//This takes univariate X, windows X with W, applies the 1D FFT to each frame,
//takes power, transforms with T mat to B cfs, and then compresses power.
//The output Y is power (real-valued), i.e. the FFT squared.
//The size of Y is BxC if dim==0, and RxB if dim==1.

//This is equivalent to stft -> apply_spectrogram_T_mat -> pow_compress.

#include <stdio.h>
#include <float.h>
#include <math.h>
#include <cblas.h>
#include <fftw3.h>

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int spectrogram_s (float *Y, const float *X1, const float *X2, const float *X3, const size_t N, const size_t L, const size_t B, const size_t F, const int c0, const float stp, const int mn0, const float p, const float preg);

int spectrogram_s (float *Y, const int iscolmajor, const int R, const int C, const float *X, const int N, const float *W, const int L, const float *H, const int dim, const int c0, const float stp, const int mn0, const int nfft, const float p, const float preg);
int spectrogram_d (double *Y, const int iscolmajor, const int R, const int C, const double *X, const int N, const double *W, const int L, const double *H, const int dim, const int c0, const double stp, const int mn0, const int nfft, const double p, const double preg);

int spectrogram_s (float *Y, const int iscolmajor, const int R, const int C, const float *X, const int N, const float *W, const int L, const float *H, const int dim, const int c0, const float stp, const int mn0, const int nfft, const float p, const float preg)
{
    const float z = 0.0f, o = 1.0f;
    const int F = nfft/2 + 1;  //num non-negative FFT freqs
    const int Lpre = L/2;      //nsamps before center samp
    int ss = c0 - Lpre;        //start samp of current frame
    int r, c, f, n = 0;
    float *X1, *Y1;
    fftwf_plan plan;
    //struct timespec tic, toc;

    //Checks
    if (N<1) { fprintf(stderr,"error in spectrogram_s: N (length X) must be positive\n"); return 1; }
    if (R<1) { fprintf(stderr,"error in spectrogram_s: R (nrows Y) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in spectrogram_s: C (ncols Y) must be positive\n"); return 1; }
    if (L<1) { fprintf(stderr,"error in spectrogram_s: L (winlength) must be positive\n"); return 1; }
    if (c0<0) { fprintf(stderr,"error in spectrogram_s: c0 (center samp of 1st frame) must be nonnegative\n"); return 1; }
    if (c0>N-1) { fprintf(stderr,"error in spectrogram_s: c0 (center samp of 1st frame) must be < N (length X)\n"); return 1; }
    if (L>=N) { fprintf(stderr,"error in spectrogram_s: L (winlength) must be < N (length X)\n"); return 1; }
    if (stp<=0.0f) { fprintf(stderr,"error in spectrogram_s: stp (step size) must be positive\n"); return 1; }
    if (nfft<L) { fprintf(stderr,"error in spectrogram_s: nfft must be >= L (winlength)\n"); return 1; }
    if (p!=p || p<0.0f || p>1.0f) { fprintf(stderr,"error in spectrogram_s: p must be in [0.0 1.0]\n"); return 1; }
    if (preg!=preg || preg<0.0f) { fprintf(stderr,"error in spectrogram_s: preg must be nonnegative\n"); return 1; }

    //Initialize fftwf
    X1 = fftwf_alloc_real((size_t)nfft);
    Y1 = fftwf_alloc_real((size_t)nfft);
    plan = fftwf_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!plan) { fprintf(stderr,"error in spectrogram_s: problem creating fftw plan\n"); return 1; }
    cblas_scopy(nfft,&z,0,&X1[0],1); //zero-pad

    //clock_gettime(CLOCK_REALTIME,&tic);

    //Initialize Y to preg
    cblas_scopy(R*C,&preg,0,&Y[0],1);

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(roundf(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasColMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasColMajor,CblasNoTrans,R,F,1.0f,&H[0],R,&Y1[0],1,1.0f,&Y[c*R],1);
                //for (b=0; b<R; b++) { Y[b+c*R] += cblas_sdot(f2s[b],&H[b+f1s[b]*R],R,&Y1[f1s[b]],1); }
            }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(roundf(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasRowMajor,CblasNoTrans,R,F,1.0f,&H[0],F,&Y1[0],1,1.0f,&Y[c],C);
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(roundf(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasColMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasColMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasColMajor,CblasNoTrans,C,F,1.0f,&H[0],C,&Y1[0],1,1.0f,&Y[r],R);
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(roundf(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_scopy(-ss,&z,0,&X1[0],1);
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0f,&W[-ss],1,&X[0],1,0.0f,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,L,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_ssbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0f,&W[0],1,&X[ss],1,0.0f,&X1[0],1);
                    cblas_scopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_scopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_saxpy(L,-cblas_sdot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftwf_execute(plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_sgemv(CblasRowMajor,CblasNoTrans,C,F,1.0f,&H[0],F,&Y1[0],1,1.0f,&Y[r*C],1);
            }
        }
    }
    else
    {
        fprintf(stderr,"error in spectrogram_s: dim must be 0 or 1.\n"); return 1;
    }
    //clock_gettime(CLOCK_REALTIME,&toc); fprintf(stderr,"elapsed time = %f ms\n",(toc.tv_sec-tic.tv_sec)*1e3+(toc.tv_nsec-tic.tv_nsec)/1e6);

    //Power compress
    if (1.0f-p>FLT_EPSILON)
    {
        if (p>FLT_EPSILON)
        {
            if (fabsf(p-0.5f)<=FLT_EPSILON) { for (n=0; n<R*C; n++) { Y[n] = sqrtf(Y[n]+preg); } }
            else if (fabsf(p-1.0f/3.0f)<=FLT_EPSILON) { for (n=0; n<R*C; n++) { Y[n] = cbrtf(Y[n]+preg); } }
            else { for (n=0; n<R*C; n++) { Y[n] = powf(Y[n]+preg,p); } }
        }
        else { for (n=0; n<R*C; n++) { Y[n] = logf(Y[n]+preg); } }
    }
    else { for (n=0; n<R*C; n++) { Y[n] += preg; } }

    //Exit
    fftwf_destroy_plan(plan); fftwf_free(X1); fftwf_free(Y1);
    return 0;
}

int spectrogram_d (double *Y, const int iscolmajor, const int R, const int C, const double *X, const int N, const double *W, const int L, const double *H, const int dim, const int c0, const double stp, const int mn0, const int nfft, const double p, const double preg)
{
    const double z = 0.0, o = 1.0;
    const int F = nfft/2 + 1;  //num non-negative FFT freqs
    const int Lpre = L/2;      //nsamps before center samp
    int ss = c0 - Lpre;        //start samp of current frame
    int r, c, f, n = 0;
    double *X1, *Y1;
    fftw_plan plan;

    //Checks
    if (N<1) { fprintf(stderr,"error in spectrogram_d: N (length X) must be positive\n"); return 1; }
    if (R<1) { fprintf(stderr,"error in spectrogram_d: R (nrows Y) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in spectrogram_d: C (ncols Y) must be positive\n"); return 1; }
    if (L<1) { fprintf(stderr,"error in spectrogram_d: L (winlength) must be positive\n"); return 1; }
    if (c0<0) { fprintf(stderr,"error in spectrogram_d: c0 (center samp of 1st frame) must be nonnegative\n"); return 1; }
    if (c0>N-1) { fprintf(stderr,"error in spectrogram_d: c0 (center samp of 1st frame) must be < N (length X)\n"); return 1; }
    if (L>=N) { fprintf(stderr,"error in spectrogram_d: L (winlength) must be < N (length X)\n"); return 1; }
    if (stp<=0.0) { fprintf(stderr,"error in spectrogram_d: stp (step size) must be positive\n"); return 1; }
    if (nfft<L) { fprintf(stderr,"error in spectrogram_d: nfft must be >= L (winlength)\n"); return 1; }
    if (p!=p || p<0.0 || p>1.0) { fprintf(stderr,"error in spectrogram_d: p must be in [0.0 1.0]\n"); return 1; }
    if (preg!=preg || preg<0.0) { fprintf(stderr,"error in spectrogram_d: preg must be nonnegative\n"); return 1; }

    //Initialize fftw
    X1 = fftw_alloc_real((size_t)nfft);
    Y1 = fftw_alloc_real((size_t)nfft);
    plan = fftw_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!plan) { fprintf(stderr,"error in spectrogram_d: problem creating fftw plan\n"); return 1; }
    cblas_dcopy(nfft,&z,0,&X1[0],1); //zero-pad

    //Initialize Y to preg
    cblas_dcopy(R*C,&preg,0,&Y[0],1);

    if (dim==0)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(round(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasColMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasColMajor,CblasNoTrans,R,F,1.0,&H[0],R,&Y1[0],1,1.0,&Y[c*R],1);
            }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                //Window
                ss = (int)(round(c*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasRowMajor,CblasNoTrans,R,F,1.0,&H[0],F,&Y1[0],1,1.0,&Y[c],C);
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(round(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasColMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasColMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasColMajor,CblasNoTrans,C,F,1.0,&H[0],C,&Y1[0],1,1.0,&Y[r],R);
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                //Window
                ss = (int)(round(r*stp)) + c0 - Lpre;
                if (ss<0)
                {
                    cblas_dcopy(-ss,&z,0,&X1[0],1);
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L+ss,0,1.0,&W[-ss],1,&X[0],1,0.0,&X1[-ss],1);
                }
                else if (ss+L<=N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,L,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                }
                else if (ss<N)
                {
                    cblas_dsbmv(CblasRowMajor,CblasUpper,N-ss,0,1.0,&W[0],1,&X[ss],1,0.0,&X1[0],1);
                    cblas_dcopy(L-N+ss,&z,0,&X1[N-ss],1);
                }
                else
                {
                    cblas_dcopy(L,&z,0,&X1[0],1);
                }

                //Subtract mean
                if (mn0) { cblas_daxpy(L,-cblas_ddot(L,&X1[0],1,&o,0)/L,&o,0,&X1[0],1); }

                //FFT
                fftw_execute(plan);

                //Power
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }

                //Transform freq scale
                cblas_dgemv(CblasRowMajor,CblasNoTrans,C,F,1.0,&H[0],F,&Y1[0],1,1.0,&Y[r*C],1);
            }
        }
    }
    else
    {
        fprintf(stderr,"error in spectrogram_d: dim must be 0 or 1.\n"); return 1;
    }

    //Power compress
    if (1.0-p>DBL_EPSILON)
    {
        if (p>DBL_EPSILON)
        {
            if (fabs(p-0.5)<=DBL_EPSILON) { for (n=0; n<R*C; n++) { Y[n] = sqrt(Y[n]+preg); } }
            else if (fabs(p-1.0/3.0)<=DBL_EPSILON) { for (n=0; n<R*C; n++) { Y[n] = cbrt(Y[n]+preg); } }
            else { for (n=0; n<R*C; n++) { Y[n] = pow(Y[n]+preg,p); } }
        }
        else { for (n=0; n<R*C; n++) { Y[n] = log(Y[n]+preg); } }
    }
    else { for (n=0; n<R*C; n++) { Y[n] += preg; } }

    //Exit
    fftw_destroy_plan(plan); fftw_free(X1); fftw_free(Y1);
    return 0;
}

#ifdef __cplusplus
}
}
#endif

// //Get Hb (banded H) //this worked to get banded matrix storage (but was slower!):
// if (!(Hb=(float *)calloc((size_t)((kl+ku+1)*B),sizeof(float)))) { fprintf(stderr,"error in spectrogram_s: problem with calloc for Hb. "); perror("calloc"); return 1; }
// if (iscolmajor)
// {
//     for (r=0; r<kl+1+ku; r++)
//     {
//         if (r<ku) { b = 0; f = ku - r; }
//         else { b = r - ku; f = 0; }
//         while (b<B && f<F) { Hb[r+f*(kl+1+ku)] = H[b+f*B]; b++; f++; }
//     }
// }
// else
// {
//     for (c=0; c<kl+1+ku; c++)
//     {
//         if (c<kl) { b = kl - c; f = 0; }
//         else { b = 0; f = c - kl; }
//         while (b<B && f<F) { Hb[c+b*(kl+1+ku)] = H[f+b*F]; b++; f++; }
//     }
// }
// ...
// cblas_sgbmv(CblasRowMajor,CblasNoTrans,B,F,kl,ku,1.0f,&Hb[0],kl+ku+1,&Y1[0],1,1.0f,&Y[c],C);