//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;
}


/*
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


//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);


/* rng/cmrg.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

/* This is a combined multiple recursive generator. The sequence is,

   z_n = (x_n - y_n) mod m1

   where the two underlying generators x and y are,

   x_n = (a_{1} x_{n-1} + a_{2} x_{n-2} + a_{3} x_{n-3}) mod m1
   y_n = (b_{1} y_{n-1} + b_{2} y_{n-2} + b_{3} y_{n-3}) mod m2

   with coefficients a11 ... a23,

   a_{1} = 0,     a_{2} = 63308, a_{3} = -183326
   b_{1} = 86098, b_{2} = 0,     b_{3} = -539608

   and moduli m1, m2,

   m1 = 2^31 - 1 = 2147483647
   m2 = 2^31 - 2000169 = 2145483479

   We initialize the generator with

   x_1 = s_1 MOD m1, x_2 = s_2 MOD m1, x_3 = s_3 MOD m1
   y_1 = s_4 MOD m2, y_2 = s_5 MOD m2, y_3 = s_6 MOD m2

   where s_n = (69069 * s_{n-1}) mod 2^32 and s_0 = s is the
   user-supplied seed.

   NOTE: According to the paper the initial values for x_n must lie in
   the range 0 <= x_n <= (m1 - 1) and the initial values for y_n must
   lie in the range 0 <= y_n <= (m2 - 1), with at least one non-zero
   value -- our seeding procedure satisfies these constraints.

   We then use 7 iterations of the generator to "warm up" the internal
   state.

   The theoretical value of z_{10008} is 719452880. The subscript 10008
   means (1) seed the generator with s=1, (2) do the seven warm-up
   iterations that are part of the seeding process, (3) then do 10000
   actual iterations.

   The period of this generator is about 2^205.

   From: P. L'Ecuyer, "Combined Multiple Recursive Random Number
   Generators," Operations Research, 44, 5 (1996), 816--822.

   This is available on the net from L'Ecuyer's home page,

   http://www.iro.umontreal.ca/~lecuyer/myftp/papers/combmrg.ps
   ftp://ftp.iro.umontreal.ca/pub/simulation/lecuyer/papers/combmrg.ps */

static inline unsigned long int cmrg_get (void *vstate);
static double cmrg_get_double (void *vstate);
static void cmrg_set (void *state, unsigned long int s);

static const long int m1 = 2147483647, m2 = 2145483479;

static const long int a2 = 63308, qa2 = 33921, ra2 = 12979;
static const long int a3 = -183326, qa3 = 11714, ra3 = 2883;
static const long int b1 = 86098, qb1 = 24919, rb1 = 7417;
static const long int b3 = -539608, qb3 = 3976, rb3 = 2071;

typedef struct
  {
    long int x1, x2, x3;	/* first component */
    long int y1, y2, y3;	/* second component */
  }
cmrg_state_t;

static inline unsigned long int
cmrg_get (void *vstate)
{
  cmrg_state_t *state = (cmrg_state_t *) vstate;

  /* Component 1 */

  {
    long int h3 = state->x3 / qa3;
    long int p3 = -a3 * (state->x3 - h3 * qa3) - h3 * ra3;

    long int h2 = state->x2 / qa2;
    long int p2 = a2 * (state->x2 - h2 * qa2) - h2 * ra2;

    if (p3 < 0)
      p3 += m1;
    if (p2 < 0)
      p2 += m1;

    state->x3 = state->x2;
    state->x2 = state->x1;
    state->x1 = p2 - p3;
    if (state->x1 < 0)
      state->x1 += m1;
  }

  /* Component 2 */

  {
    long int h3 = state->y3 / qb3;
    long int p3 = -b3 * (state->y3 - h3 * qb3) - h3 * rb3;

    long int h1 = state->y1 / qb1;
    long int p1 = b1 * (state->y1 - h1 * qb1) - h1 * rb1;

    if (p3 < 0)
      p3 += m2;
    if (p1 < 0)
      p1 += m2;

    state->y3 = state->y2;
    state->y2 = state->y1;
    state->y1 = p1 - p3;
    if (state->y1 < 0)
      state->y1 += m2;
  }

  if (state->x1 < state->y1)
    return (state->x1 - state->y1 + m1);
  else
    return (state->x1 - state->y1);
}

static double
cmrg_get_double (void *vstate)
{
  return cmrg_get (vstate) / 2147483647.0 ;
}

static void
cmrg_set (void *vstate, unsigned long int s)
{
  /* An entirely adhoc way of seeding! This does **not** come from
     L'Ecuyer et al */

  cmrg_state_t *state = (cmrg_state_t *) vstate;

  if (s == 0)
    s = 1;	/* default seed is 1 */

#define LCG(n) ((69069 * n) & 0xffffffffUL)
  s = LCG (s);
  state->x1 = s % m1;
  s = LCG (s);
  state->x2 = s % m1;
  s = LCG (s);
  state->x3 = s % m1;

  s = LCG (s);
  state->y1 = s % m2;
  s = LCG (s);
  state->y2 = s % m2;
  s = LCG (s);
  state->y3 = s % m2;

  /* "warm it up" */
  cmrg_get (state);
  cmrg_get (state);
  cmrg_get (state);
  cmrg_get (state);
  cmrg_get (state);
  cmrg_get (state);
  cmrg_get (state);
}

static const gsl_rng_type cmrg_type =
{"cmrg",			/* name */
 2147483646,			/* RAND_MAX */
 0,			        /* RAND_MIN */
 sizeof (cmrg_state_t),
 &cmrg_set,
 &cmrg_get,
 &cmrg_get_double};

const gsl_rng_type *gsl_rng_cmrg = &cmrg_type;


/*
** random swap functions
**
** M. Kuhlmann, MPI-KYB, Sept 2015
**/

#include <viaio/Vlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_combination.h>

/* binomial coefficient, n choose k */
double binom (int n,int k)
{
  double prod,ix,nx,kx;
  int i;
  nx = (double)n;
  kx = (double)k;

  prod = ix = 1;
  for (i=1; i<=k; i++) {
    prod *= (nx-kx+ix)/ix;
    ix++;
  }
  return prod;
}

double factorial(int n) {
  int i;
  double ix = 1;
  double prod = 1;
  for (i=1; i<=n; i++) {
    prod *= ix;
    ix++;
  }
  return prod;
}

char **enumerate_swaps(int n)
{
  size_t i,j,num,b;

  long maxiter = (size_t) gsl_pow_int(2.0,(int)n);

  char **swaps = (char **)VCalloc(maxiter,sizeof(char *));
  for(i = 0; i < maxiter; i++)
    swaps[i] = (char *)VCalloc(n,sizeof(char));

  /* generate swap table */
  for (i=0; i<maxiter; i++) {
    /* decimal to binary */
    j = 0;
    num = i;
    while(num > 0)
    {
      b=num%2;
      swaps[i][j] = b;
      num=num/2;
      j++;
    }
  }

  return swaps;
}

char **randomSwaps(long maxiter,int n,unsigned long int seed)
{
  size_t i,j;

  char **swaps = (char **)VCalloc(maxiter,sizeof(char *));
  for(i = 0; i < maxiter; i++)
    swaps[i] = (char *)VCalloc(n,sizeof(char));

  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);

  for(i = 0; i < maxiter; i++)
    for(j = 0; j < n; j++)
      swaps[i][j] = gsl_ran_bernoulli(rx, 0.5);

  return swaps;
}

char **randomSwaps_noduplicates(long maxiter,int n,unsigned long int seed)
{
  size_t iter,i,j;

  /* if number of maxiter exceeds number of all possible combinations */
  if(maxiter > (size_t) gsl_pow_int(2.0,(int)n))
    VError("Error: Number of iterations larger than possible permutations!");

  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);

  char **swaps = (char **)VCalloc(maxiter,sizeof(char *));
  for(i = 0; i < maxiter; i++)
    swaps[i] = (char *)VCalloc(n,sizeof(char));

  /* generate swap table */
  iter = 0;
  while (iter < maxiter) {
    for (j=0; j<n; j++)
      swaps[iter][j] = gsl_ran_bernoulli(rx,0.5);
    /* check if already there */
    /* decrementing iter will result in overwriting the swaps */
    /* since there are no duplicates in can only be decremented once */
    for (j=0; j<iter; j++)
      if (memcmp(swaps[iter],swaps[j],n*sizeof(char)) == 0)
	iter--;
    iter++;
  }

  return swaps;
}

/*
 * shuffle blocks; needed for permutation_test
 */
int **randomBlocks(long int maxiter, int nimages, int block_size, unsigned long int seed)
{
  int nblocks = nimages/block_size;
  int i,j,iter;

  /* ini with real order */
  int **table = (int **)VCalloc(maxiter,sizeof(int*));
  for(i = 0; i < maxiter; i++)
  {
    table[i] = (int *)VCalloc(2*nimages,sizeof(int));
    for(j = 0; j < nimages; j++)
    {
      table[i][2*j] = j;
      table[i][2*j+1] = j+nimages;
    }
  }

  /* each row of table has order: 0, nimages, 1, nimages+1, ... */
  /* shuffle blocks of size 2*block_size */
  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);

  for (iter=0; iter<maxiter; iter++)
    for (j=0; j<nblocks; j++)
      gsl_ran_shuffle(rx, (void *)&table[iter][j*2*block_size], 2*block_size, sizeof(int));
  return table;
}
/*
 * shuffle blocks; needed for permutation_test
 * maxiter is the number of random permutations
 * nimages is the number of images in one condition (2*nimages in total)
 * block_size is the size of the permutation blocks
 */
int **randomBlocks_noduplicates(long int maxiter, int nimages, int block_size, unsigned long int seed)
{
  int nblocks = nimages/block_size;
  int i,j,iter;

  /* ini with real order */
  int **table = (int **)VCalloc(maxiter,sizeof(int*));
  for(i = 0; i < maxiter; i++)
  {
    table[i] = (int *)VCalloc(2*nimages,sizeof(int));
    for(j = 0; j < nimages; j++)
    {
      table[i][2*j] = j;
      table[i][2*j+1] = j+nimages;
    }
  }

  /* each row of table has order: 0, nimages, 1, nimages+1, ... */
  /* shuffle blocks of size 2*block_size */
  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);

  iter = 0;
  while (iter < maxiter) {
    /* shuffle */
    for (j = 0; j<nblocks; j++)
      gsl_ran_shuffle(rx, (void *)&table[iter][j*2*block_size], 2*block_size, sizeof(int));

    /* sort for one side */
    int k, l, m;
    int index = 0;
    int temp = 0;
    for (m = 0; m < nblocks; m++) {
      for (k = 0; k < block_size-1; k++) {
	index = k;
	for (l = k+1; l < block_size; l++) {
	  if (table[iter][m*2*block_size+2*index] < table[iter][m*2*block_size+2*l])
	    index = l;
	}
	/* swap pairs 'k' and 'index' */
	temp = table[iter][m*2*block_size+2*k];
	table[iter][m*2*block_size+2*k] = table[iter][m*2*block_size+2*index];
	table[iter][m*2*block_size+2*index] = temp;
	temp = table[iter][m*2*block_size+2*k+1];
	table[iter][m*2*block_size+2*k+1] = table[iter][m*2*block_size+2*index+1];
	table[iter][m*2*block_size+2*index+1] = temp;
      }
    }

    /*
     * decrementing iter will result in overwriting the last swaps row
     * since there are no duplicates it can only be decremented once
     */
    for(j=0; j<iter; j++)
      if (memcmp(table[iter],table[j],2*nimages*sizeof(int)) == 0)
	iter--;
    iter++;
  }
  return table;
}


// demo3d.c

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <time.h>
#include <string.h>
#include <assert.h>
#include <stdbool.h>
#include <math.h>
#include <gsl/gsl_math.h>

#define SDL_MAIN_HANDLED
#include <SDL2/SDL.h>

#include "libvecdisp.h"

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

	// Init
	ret = vecdisp_init();
	assert(ret == VECDISP_SUCCESS);
	ret = vecdisp_out_init();
	assert(ret == VECDISP_SUCCESS);

	SDL_GameController * controller_p0 = NULL, * controller_p1 = NULL;
	int controller_cnt = 0;

	SDL_Init( SDL_INIT_GAMECONTROLLER );
	SDL_GameControllerEventState(SDL_ENABLE);

	vecdisp_shape_t * shape_cube3d = vecdisp_shape_create(VECDISP_SHAPE_LINES, NULL, 0 );

	// format: {x, y, z}, x = horizontal, y = vertical, z = depth
	double cube3d[][3] = {
		{ -1, -1, -1 },
		{ 1, -1, -1 },
		{ 1, -1, -1 },
		{ 1, 1, -1 },
		{ 1, 1, -1 },
		{ -1, 1, -1 },
		{ -1, 1, -1 },
		{ -1, -1, -1 },
		{ -1, -1, -1 },
		{ -1, -1, 1 },
		{ -1, 1, -1 },
		{ -1, 1, 1 },
		{ 1, 1, -1 },
		{ 1, 1, 1 },
		{ 1, -1, -1 },
		{ 1, -1, 1 },
		{ -1, -1, 1 },
		{ 1, -1, 1 },
		{ 1, -1, 1 },
		{ 1, 1, 1 },
		{ 1, 1, 1 },
		{ -1, 1, 1 },
		{ -1, 1, 1 },
		{ -1, -1, 1 },
	 };
	for(int i = 0; i < 24; i++) {
		uint16_t data[2] = {0,0};
		vecdisp_shape_data_add(shape_cube3d, &data, 1 );

	}
	printf("%i\n", shape_cube3d->data_len);

	double rotx = 0, roty = 0, rotz = 0;
	double sqrt_3 = sqrt(3);
	double sqrt_6 = sqrt(6);

	// main loop
	SDL_Event event;
	bool quit = false;
	while( quit == false ) {
		while( SDL_PollEvent(&event) != 0 ) {
			switch (event.type) {
			case SDL_QUIT:
				quit = true;
				break;
			case SDL_CONTROLLERDEVICEADDED:
				if(controller_p0 == NULL) {
					controller_p0 = SDL_GameControllerOpen(event.cdevice.which);
					printf("adding Controller p0\n");
					controller_cnt++;
				}
				else if(controller_p1 == NULL) {
					controller_p1 = SDL_GameControllerOpen(event.cdevice.which);
					printf("adding Controller p1\n");
					controller_cnt++;
				}
				else {
					printf("Only two controllers allowed.\n");
				}
				break;
			case SDL_CONTROLLERDEVICEREMOVED:
				if (event.cdevice.which == SDL_JoystickInstanceID(SDL_GameControllerGetJoystick(controller_p0) ) )  {
					SDL_GameControllerClose(controller_p0);
					printf("freeing controller_p0\n");
					controller_p0 = NULL;
					controller_cnt--;
				}
				else if (event.cdevice.which == SDL_JoystickInstanceID(SDL_GameControllerGetJoystick(controller_p1) ) )  {
					SDL_GameControllerClose(controller_p1);
					printf("freeing controller_p1\n");
					controller_p1 = NULL;
					controller_cnt--;
				}
				break;
			case SDL_CONTROLLERBUTTONDOWN:
				switch (event.cbutton.button) {
				case SDL_CONTROLLER_BUTTON_BACK:
					if(event.cbutton.state == 1) {
						SDL_Event user_quit_event;
						user_quit_event.type = SDL_QUIT;
						SDL_PushEvent( &user_quit_event );
					}
					break;
				}
				break;
			case SDL_KEYDOWN:
				switch (event.key.keysym.sym) {
				case SDLK_q: {
					SDL_Event user_quit_event;
					user_quit_event.type = SDL_QUIT;
					SDL_PushEvent( &user_quit_event );
					break;
					}
				}
				break;
			}
		}

		rotx += 0.00004;
		roty += 0.00002;
		rotz += 0.00001;
		if( rotx >= M_PI ) rotx -= M_PI;
		if( roty >= M_PI ) roty -= M_PI;
		if( rotz >= M_PI ) rotz -= M_PI;

		for(int i = 0; i < 24; i++) {
			double c[3];
			cube3d[i][0] = cos(rotx) * cube3d[i][0] - sin(rotx) * cube3d[i][1]; // Rotation around X Axis
			cube3d[i][1] = sin(rotx) * cube3d[i][0] + cos(rotx) * cube3d[i][1];
			cube3d[i][2] = 1 * cube3d[i][2];

			cube3d[i][0] = cos(roty) * cube3d[i][0] - sin(roty) * cube3d[i][2]; // Rotation around Y Axis
			cube3d[i][1] = 1 * cube3d[i][1];
			cube3d[i][2] = cos(roty) * cube3d[i][2] + sin(roty) * cube3d[i][0];

			cube3d[i][0] = 1 * cube3d[i][0]; // Rotation around Z Axis
			cube3d[i][1] = cos(rotz) * cube3d[i][1] - sin(rotz) * cube3d[i][2]; // Rotation around Z Axis
			cube3d[i][2] = cos(rotz) * cube3d[i][2] + sin(rotz) * cube3d[i][1];

			c[0] = ( sqrt(3) * cube3d[i][0] + (-1) * sqrt_3 * cube3d[i][2] ) / sqrt_6;
			c[1] = ( 1 * cube3d[i][0] + 2 * cube3d[i][1] + 1 * cube3d[i][2] ) / sqrt_6;

			shape_cube3d->data[i][0] =  lround((DRAW_RES / 4) * c[0] + (DRAW_RES / 2));
			shape_cube3d->data[i][1] = lround((DRAW_RES / 4) * c[1] + (DRAW_RES / 2));
		}

		vecdisp_draw_shape( shape_cube3d, 0, 0, DRAW_RES - 1, DRAW_RES - 1, DRAW_BRTNS_BRIGHT);
		vecdisp_dbg_showfps();
		vecdisp_draw_update();
	}

	// cleaning up
	vecdisp_shape_destroy(shape_cube3d);
	SDL_QuitSubSystem( SDL_INIT_EVERYTHING );
	vecdisp_out_end();
	vecdisp_end();
	return 0;
}


/*************************************************************************************************************
Calculate shear Gaussian + non-Gaussian covariance using CosmoLike.
Gaussian, super-sample and connected non-Gaussian contributions can each be switched on and off independently.
This loops over all combinations of z bins, saving each to disk separately.

Provide two command line arguments:
  1. Path to ini file with all the configuration (see the example_input/example.ini).
  2. Index of the first power spectrum, spec1_idx. This will then iterate over all spec2_idx <= spec1_idx.
**************************************************************************************************************/

#include <string.h>

#include <gsl/gsl_deriv.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sf_expint.h>

// CosmoLike includes (std & GSL includes must be above here)
#include <cosmolike_core/theory/structs.c>
#include <cosmolike_core/theory/basics.c>
#include <cosmolike_core/theory/recompute.c>
#include <cosmolike_core/theory/cosmo3D.c>
#include <cosmolike_core/theory/redshift_spline.c>
#include <cosmolike_core/theory/cosmo2D_fourier.c>
#include <cosmolike_core/theory/halo.c>
#include <cosmolike_core/theory/covariances_3D.c>
#include <cosmolike_core/theory/covariances_fourier.c>
#include <covs/init.c>

// Cl covariance params
typedef struct {
    int lmin;
    int lmax;
    char ell_str[200];
    char output_dir[200];
    int do_g;
    int do_ss;
    int do_cng;
} clcov_par;
clcov_par clcov_params = {.lmin = 2, .lmax = -1, .ell_str = "NULL", .do_g = 0, .do_ss = 0, .do_cng = 0};

// Set Cl covariance params from ini file
void set_clcov_parameters(char *paramfile, int output)
{
  char line[256];
  int iline=0;

  FILE* input = fopen(paramfile, "r");
  while(fgets(line, 256, input) != NULL) {
    char name[128], val[128];
    iline++;
    if(line[0] == '#') continue;
    sscanf(line, "%128s : %128s", name, val);

    if(strcmp(name, "lmin") == 0) {
      sscanf(val, "%d", &clcov_params.lmin);
      if(output == 1) printf("lmin %d \n", clcov_params.lmin);
    } else if(strcmp(name, "lmax") == 0) {
      sscanf(val, "%d", &clcov_params.lmax);
      if(output == 1) printf("lmax %d \n", clcov_params.lmax);
    } else if(strcmp(name, "output_dir") == 0) {
      sprintf(clcov_params.output_dir, "%s", val);
      if(output == 1) printf("output_dir %s \n", clcov_params.output_dir);
    } else if(strcmp(name, "c_footprint_file") == 0) {
      sprintf(covparams.C_FOOTPRINT_FILE, "%s", val);
      if(output == 1) printf("c_footprint_file %s \n", covparams.C_FOOTPRINT_FILE);
    } else if(strcmp(name, "ell") == 0) {
      sprintf(clcov_params.ell_str, "%s", val);
      if(output == 1) printf("ell_str %s \n", clcov_params.ell_str);
    } else if(strcmp(name, "do_g") == 0) {
      sscanf(val, "%d", &clcov_params.do_g);
      if(output == 1) printf("do_g %d \n", clcov_params.do_g);
    } else if(strcmp(name, "do_ss") == 0) {
      sscanf(val, "%d", &clcov_params.do_ss);
      if(output == 1) printf("do_ss %d \n", clcov_params.do_ss);
    } else if(strcmp(name, "do_cng") == 0) {
      sscanf(val, "%d", &clcov_params.do_cng);
      if(output == 1) printf("do_cng %d \n", clcov_params.do_cng);
    }
  }
}

// 2D version of numpy savetxt. Use header = "0" for no header
int savetxt_2d(char *path, double **array, int n_row, int n_col, char *header) {

    FILE *file = fopen(path, "w");

    if (strncmp(header, "0", 2) != 0) fprintf(file, "# %s\n", header);

    for (int i = 0; i < n_row; i++) {
      printf("Saving %d / %d ... \r", i + 1, n_row);
      for (int j = 0; j < n_col; j++) {
        fprintf(file, "%.10e", array[i][j]);
        if (j < n_col - 1) {
          fprintf(file, " ");
        }
      }
      fprintf(file, "\n");
    }

    fclose(file);
    return 0;
}

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

  if (argc != 3){
    fprintf(stderr, "Syntax: %s config_file spec1_idx\n", argv[0]);
    exit(1);
  }

  setbuf(stdout, NULL);

  // Set this to 1 to output details about inputs for diagnostics
  int output = 1;

  // Set cosmo, survey and Cl covariance params from ini file
  char *inifile = argv[1];
  set_cosmological_parameters(inifile, output);
  set_survey_parameters(inifile, output);
  covparams.full_tomo = 1;
  set_clcov_parameters(inifile, output);
  init_source_sample(redshift.shear_REDSHIFT_FILE, tomo.shear_Nbin);
  init_lens_sample(redshift.clustering_REDSHIFT_FILE, tomo.clustering_Nbin);

  // Determine which contributions to include
  char cov_str[9] = "";
  printf("Including Gaussian contribution: ");
  if (clcov_params.do_g) {
    printf("yes \n");
    sprintf(cov_str, "%sg_", cov_str);
  } else printf("no \n");
  printf("Including super-sample contribution: ");
  if (clcov_params.do_ss) {
    printf("yes \n");
    sprintf(cov_str, "%sss_", cov_str);
  } else printf("no \n");
  printf("Including connected non-Gaussian contribution: ");
  if (clcov_params.do_cng) {
    printf("yes \n");
    sprintf(cov_str, "%scng_", cov_str);
  } else printf("no \n");
  covparams.ssc = clcov_params.do_ss;
  covparams.cng = clcov_params.do_cng;

  // Obtain the number of z bins from config
  int n_zbin;
  if (tomo.shear_Nbin != tomo.clustering_Nbin) error("tomo.shear_Nbin != tomo.clustering_Nbin");
  else n_zbin = tomo.shear_Nbin;

  // Determine ells, either from the list provided in the config or the provided lmin and lmax
  // Do this in 2 steps: first determine the number of ells, then the ells themselves
  int n_ell;
  if (strcmp(clcov_params.ell_str, "NULL") == 0) {
    if (clcov_params.lmax < clcov_params.lmin) error("lmax < lmin, make sure lmax is provided");
    n_ell = clcov_params.lmax - clcov_params.lmin + 1;
  }
  else { // Count the number of commas + 1
    n_ell = 1;
    const char *remaining_str = clcov_params.ell_str;
    while ((remaining_str = strstr(remaining_str, ",")))
    {
      n_ell++;
      remaining_str++;
    }
  }
  int ell[n_ell];
  if (strcmp(clcov_params.ell_str, "NULL") == 0) {
    for (int l = clcov_params.lmin; l <= clcov_params.lmax; l++) {
      ell[l - clcov_params.lmin] = l;
    }
  }
  else {
    char* rem_str = calloc(strlen(clcov_params.ell_str) + 1, sizeof(char));
    strcpy(rem_str, clcov_params.ell_str);
    int l_idx = 0;
    char *token = strtok(rem_str, ",");
    while (token != NULL) {
      ell[l_idx] = strtol(token, NULL, 10);
      token = strtok(NULL, ",");
      l_idx++;
    }
  }

  // Prepare a string about the ells to go in the header of the output text files
  char ell_header_str[200];
  if (strcmp(clcov_params.ell_str, "NULL") == 0) {
    sprintf(ell_header_str, "lmin %d, lmax %d", clcov_params.lmin, clcov_params.lmax);
  } else {
    sprintf(ell_header_str, "ell: %s", clcov_params.ell_str);
  }
  printf("%s \n", ell_header_str);

  // Generate list of power spectra within the shear data vector
  // Rows are the different spectra, cols are: z1, z2
  int n_spectra_sametype = n_zbin * (n_zbin + 1) / 2;
  int spectra_sametype[n_spectra_sametype][2];
  int spec_idx = 0;
  for (int diag = 0; diag < n_zbin; diag++) {
    for (int row = 0; row < n_zbin - diag; row++) {
      spectra_sametype[spec_idx][0] = row;
      spectra_sametype[spec_idx][1] = row + diag;
      spec_idx++;
    }
  }

  // Read in the provided spec1_idx and check it is allowed
  int spec1_idx = atoi(argv[2]);
  if (spec1_idx < 0 || spec1_idx >= n_spectra_sametype) {
    error("spec1_idx provided is not valid");
  } else {
    printf("Requested spec1_idx is %d \n", spec1_idx);
  }

  // Common variables for the covariance blocks
  int z1, z2, z3, z4, l1, l2;
  double cov;
  char save_path[250];
  char header[250];

  // Shear-shear covariance (symmetric so only do spec2_idx <= spec1_idx)
  for (int spec2_idx = 0; spec2_idx <= spec1_idx; spec2_idx++) {

    // Allocate memory for the block and fill with nans
    double** cov_shear_shear = (double**) malloc(n_ell * sizeof(double*));
    for (int i = 0; i < n_ell; i++) cov_shear_shear[i] = (double*) malloc(n_ell * sizeof(double));
    for (int l1_idx = 0; l1_idx < n_ell; l1_idx++) {
      for (int l2_idx = 0; l2_idx < n_ell; l2_idx++) {
        cov_shear_shear[l1_idx][l2_idx] = NAN;
      }
    }

    // Loop over all combinations of (l1, l2), unless the two spectra are the same in which case only need l2 <= l1
    for (int l1_idx = 0; l1_idx < n_ell; l1_idx++) {
      printf("spec2_idx %d / %d, l1_idx %d / %d \n", spec2_idx, spec1_idx, l1_idx, n_ell);
      int l2_idx_max;
      if (spec2_idx == spec1_idx) {
        l2_idx_max = l1_idx;
      } else {
        l2_idx_max = n_ell - 1;
      }
      for (int l2_idx = 0; l2_idx <= l2_idx_max; l2_idx++) {
        l1 = ell[l1_idx];
        l2 = ell[l2_idx];
        z1 = spectra_sametype[spec1_idx][0];
        z2 = spectra_sametype[spec1_idx][1];
        z3 = spectra_sametype[spec2_idx][0];
        z4 = spectra_sametype[spec2_idx][1];
        cov = 0.0;
        if (clcov_params.do_g) cov += cov_G_shear_shear_tomo(l1, l2, z1, z2, z3, z4);
        if (clcov_params.do_ss || clcov_params.do_cng) cov += cov_NG_shear_shear_tomo(l1, l2, z1, z2, z3, z4);
        cov_shear_shear[l1_idx][l2_idx] = cov;
        if (spec2_idx == spec1_idx) {
          cov_shear_shear[l2_idx][l1_idx] = cov;
        }
      }
    }

    // Save block to disk and free memory
    sprintf(save_path, "%s/cov_%sspec1_%d_spec2_%d.txt", clcov_params.output_dir, cov_str, spec1_idx, spec2_idx);
    sprintf(header, "cov_shear_shear for spec_idx %d and %d with do_g=%d, do_ss=%d, do_cng=%d, %s",
            spec1_idx, spec2_idx, clcov_params.do_g, clcov_params.do_ss, clcov_params.do_cng, ell_header_str);
    if (savetxt_2d(save_path, cov_shear_shear, n_ell, n_ell, header) == 0) printf("\nSaved %s\n", save_path);
    for (int i = 0; i < n_ell; i++) free(cov_shear_shear[i]);
    free(cov_shear_shear);

  } // next spec2_idx for this fixed spec1_idx

  printf("Done \n");
  return 0;
}