// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-

/*
 * Copyright (C) 2009 RobotCub Consortium
 * Author: Alessandro Scalzo alessandro.scalzo@iit.it
 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
 *
 */

#include <gsl/gsl_math.h>
#include <memory.h>

#include <stdio.h>

#ifndef __ALE_TOUCHSENSOR_H__
#define __ALE_TOUCHSENSOR_H__

#define MAX_TAXELS 128

class TouchSensor
{
    bool calibrated_skin;

public:

    void setColor(unsigned char r, unsigned char g, unsigned char b)
    {
        R_MAX = r;
        G_MAX = g;
        B_MAX = b;
    }

    void setCalibrationFlag (bool use_calibrated_skin)
    {
        calibrated_skin=use_calibrated_skin;
    }

    void resize(int width,int height,int margin)
    {
        if (3*margin>=width || 3*margin>=height) margin=0;

        double scaleX=double(width -2*margin)/(dXmax-dXmin);
        double scaleY=double(height-2*margin)/(dYmax-dYmin);
        double scale=scaleX<scaleY?scaleX:scaleY;

        m_Radius=m_RadiusOrig*scale;

        m_maxRangeLight=int(m_Radius);

        double dXmid=0.5*(dXmin+dXmax);
        double dYmid=0.5*(dYmin+dYmax);

        int w2=width /2;
        int h2=height/2;

        for (int i=0; i<nTaxels; ++i)
        {
            x[i]=w2+int(scale*(dX[i]-dXmid));
            y[i]=h2+int(scale*(dY[i]-dYmid));
        }

        for (int i=0; i<nVerts; ++i)
        {
            xv[i]=w2+int(scale*(dXv[i]-dXmid));
            yv[i]=h2+int(scale*(dYv[i]-dYmid));
        }

        double sigma=0.5*5.55*scale;
        int maxRange=int(2.5*sigma);

        if (maxRange!=m_maxRange)
        {
            m_maxRange=maxRange;

            delete [] Exponential;
            Exponential=new double[maxRange];

            double k=-0.5/(sigma*sigma);
            for (int x=0; x<maxRange; ++x)
            {
                Exponential[x]=exp(k*double(x*x));
            }
        }

        xMin=w2+int(scale*(dXc-dXmid-15.0))-maxRange;
        xMax=w2+int(scale*(dXc-dXmid+15.0))+maxRange;
        yMin=h2+int(scale*(dYc-dYmid-15.0))-maxRange;
        yMax=h2+int(scale*(dYc-dYmid+15.0))+maxRange;

        if (xMin<0)      xMin=0;
        if (xMax>width)  xMax=width;
        if (yMin<0)      yMin=0;
        if (yMax>height) yMax=height;

        m_Width=width;
        m_Height=height;
    }

    virtual ~TouchSensor()
    {
        if (Exponential)
        {
            delete [] Exponential;
            Exponential=0;
        }
    }

    int Abs(int x)
    {
        return x>=0?x:-x;
    }

    int get_nTaxels ()
    {
        return nTaxels;
    }

    void eval_light(unsigned char *image)
    {
        int act;
        int dx,dy;
        int Y0,Y1;
        int dya,dyb,dxa,dxb;

        double remapped_activation[MAX_TAXELS];
        switch (ilayoutNum)
        {
            case 0:
                for (int i=0; i<nTaxels; ++i)  remapped_activation[i]=activation[i];
                break;
            case 1:
                for (int i=0; i<nTaxels; ++i)  remapped_activation[nTaxels-1-i]=activation[i];
                break;
            default:
                for (int i=0; i<nTaxels; ++i)  remapped_activation[i]=activation[i];
                printf("WARN: unkwnown layout number.\n");
                break;
        }

        int maxRange2=m_maxRangeLight*m_maxRangeLight;

        for (int i=0; i<nTaxels; ++i) if (remapped_activation[i]>0.0)
        {
            act=int(dGain*remapped_activation[i]);
            Y0=(m_Height-y[i]-1)*m_Width+x[i];

            dya=(y[i]>=m_maxRangeLight)?-m_maxRangeLight:-y[i];
            dyb=(y[i]+m_maxRangeLight<m_Height)?m_maxRangeLight:m_Height-y[i]-1;

            dxa=(x[i]>=m_maxRangeLight)?-m_maxRangeLight:-x[i];
            dxb=(x[i]+m_maxRangeLight<m_Width)?m_maxRangeLight:m_Width-x[i]-1;

            for (dy=dya; dy<=dyb; ++dy)
            {
                Y1=Y0-dy*m_Width;

                for (dx=dxa; dx<=dxb; ++dx)
                {
                    if (dx*dx+dy*dy<=maxRange2)
                    {
                        image[(dx+Y1)*3]=act<255?act:255;
                    }
                }
            }
        }
    }

    void eval(unsigned char *image)
    {
        int act;
        int dx,dy;
        int Y0,Y1;
        int index;
        double k0,k1;
        int dya,dyb,dxa,dxb;
        double remapped_activation[MAX_TAXELS];
        switch (ilayoutNum)
        {
            case 0:
                for (int i=0; i<nTaxels; ++i)  remapped_activation[i]=activation[i];
                break;
            case 1:
                for (int i=0; i<nTaxels; ++i)  remapped_activation[nTaxels-1-i]=activation[i];
                break;
            default:
                for (int i=0; i<nTaxels; ++i)  remapped_activation[i]=activation[i];
                printf("WARN: unkwnown layout number.\n");
                break;
        }

        for (int i=0; i<nTaxels; ++i) if (remapped_activation[i]>0.0)
        {
            k0=dGain*remapped_activation[i];
            Y0=(m_Height-y[i]-1)*m_Width+x[i];

            dya=(y[i]>=m_maxRange)?-m_maxRange:-y[i];
            dyb=(y[i]+m_maxRange<m_Height)?m_maxRange:m_Height-y[i]-1;

            dxa=(x[i]>=m_maxRange)?-m_maxRange:-x[i];
            dxb=(x[i]+m_maxRange<m_Width)?m_maxRange:m_Width-x[i]-1;

            for (dy=dya; dy<=dyb; ++dy)
            {
                k1=k0*Exponential[Abs(dy)];
                Y1=Y0-dy*m_Width;

                for (dx=dxa; dx<=dxb; ++dx)
                {
                    index=(dx+Y1)*3;

                    if (image[index]<R_MAX || image[index+1]<G_MAX || image[index+2]<B_MAX)
                    {
                        act=int(k1*Exponential[Abs(dx)]);

                        int actR=image[index  ]+(act*R_MAX)/255;
                        int actG=image[index+1]+(act*G_MAX)/255;
                        int actB=image[index+2]+(act*B_MAX)/255;

                        image[index  ]=actR<R_MAX?actR:R_MAX;
                        image[index+1]=actG<G_MAX?actG:G_MAX;
                        image[index+2]=actB<B_MAX?actB:B_MAX;
                    }
                }
            }
        }
    }

    void setActivationFirst7(unsigned char* data)
    {
        for (int i=0; i<7; ++i)
        {
            activation[i]=data[i+1]<=244?double(244-data[i+1]):0.0;
        }
    }
    void setActivationLast5(unsigned char* data)
    {
        for (int i=1; i<=5; ++i)
        {
            activation[i+6]=data[i]<=244?double(244-data[i]):0.0;
        }
    }

    void setActivationFromPortData(double val, int id)
    {
        if (calibrated_skin)
        {
            if (val>244.0)
            {
                activation[id]=244.0;
            }
            else if (val<0.0)
            {
                activation[id]=0.0;
            }
            else
            {
                activation[id]=val;
            }
        }
        else
        {
            activation[id]=val<=244?double(244-val):0.0;
        }
    }

    virtual void draw(unsigned char *image)
    {
        for (int i=0; i<nVerts; ++i)
        {
            drawLine(image,xv[i],yv[i],xv[(i+1)%nVerts],yv[(i+1)%nVerts]);
        }

        for (int i=0; i<nTaxels; ++i)
        {
            drawCircle(image,x[i],y[i],m_Radius);
        }
    }

protected:
    void dither(int x,int y,unsigned char *image)
    {
        static const unsigned char R1=0x80,G1=0x50,B1=0x00;
        static const unsigned char R2=3*R1/4,G2=3*G1/4,B2=3*B1/4;
        static const unsigned char R4=3*R2/4,G4=3*G2/4,B4=3*B2/4;

        //y=m_Width-y-1;
        //int bytePos=(x+(y-1)*m_Width)*3;
        int bytePos=(x+(m_Height-y-2)*m_Width)*3;

        if (image[bytePos-3]<R4) image[bytePos-3]=R4;
        if (image[bytePos-2]<G4) image[bytePos-2]=G4;
        //if (image[bytePos-1]<B4) image[bytePos-1]=B4;

        if (image[bytePos  ]<R2) image[bytePos  ]=R2;
        if (image[bytePos+1]<G2) image[bytePos+1]=G2;
        //if (image[bytePos+2]<B2) image[bytePos+2]=B2;

        if (image[bytePos+3]<R4) image[bytePos+3]=R4;
        if (image[bytePos+4]<G4) image[bytePos+4]=G4;
        //if (image[bytePos+5]<B4) image[bytePos+5]=B4;

        bytePos+=m_Width*3;

        if (image[bytePos-3]<R2) image[bytePos-3]=R2;
        if (image[bytePos-2]<G2) image[bytePos-2]=G2;
        //if (image[bytePos-1]<B2) image[bytePos-1]=B2;

        if (image[bytePos  ]<R1) image[bytePos  ]=R1;
        if (image[bytePos+1]<G1) image[bytePos+1]=G1;
        //if (image[bytePos+2]<B1) image[bytePos+2]=B1;

        if (image[bytePos+3]<R2) image[bytePos+3]=R2;
        if (image[bytePos+4]<G2) image[bytePos+4]=G2;
        //if (image[bytePos+5]<B2) image[bytePos+5]=B2;

        bytePos+=m_Width*3;

        if (image[bytePos-3]<R4) image[bytePos-3]=R4;
        if (image[bytePos-2]<G4) image[bytePos-2]=G4;
        //if (image[bytePos-1]<B4) image[bytePos-1]=B4;

        if (image[bytePos  ]<R2) image[bytePos  ]=R2;
        if (image[bytePos+1]<G2) image[bytePos+1]=G2;
        //if (image[bytePos+2]<B2) image[bytePos+2]=B2;

        if (image[bytePos+3]<R4) image[bytePos+3]=R4;
        if (image[bytePos+4]<G4) image[bytePos+4]=G4;
        //if (image[bytePos+5]<B4) image[bytePos+5]=B4;
    }

    void drawLine(unsigned char *image,int x0,int y0,int x1,int y1)
    {
        if (x1==x0 && y1==y0) return;
        double Vx=double(x1-x0);
        double Vy=double(y1-y0);
        double dt=1.0/sqrt(Vx*Vx+Vy*Vy);

        for (double t=0.0; t<=1.0; t+=dt)
        {
            dither(x0+int(t*Vx),y0+int(t*Vy),image);
        }
    }

    void drawCircle(unsigned char *image,int cx,int cy,double radius)
    {
        double dt=1.0/(2*M_PI*radius);

        int dx,dy;

        double cs,sn;

        for (double t=0.0; t<=M_PI_4; t+=dt)
        {
            cs=cos(t);
            sn=sin(t);

            dx=int(radius*cs);
            dy=int(radius*sn);

            dither(cx+dx,cy+dy,image);
            dither(cx+dx,cy-dy,image);
            dither(cx-dx,cy-dy,image);
            dither(cx-dx,cy+dy,image);

            dither(cx+dy,cy+dx,image);
            dither(cx+dy,cy-dx,image);
            dither(cx-dy,cy-dx,image);
            dither(cx-dy,cy+dx,image);
        }
    }

    // original
    double dX[MAX_TAXELS],dY[MAX_TAXELS];
    static double dXmin,dXmax,dYmin,dYmax;
    double dXv[8],dYv[8];
    double dXc,dYc;
    double dGain;
    int    ilayoutNum;
    int    ilrMirror;

    double m_Radius,m_RadiusOrig;
    double activation[MAX_TAXELS];

    unsigned char R_MAX, G_MAX, B_MAX;

    int m_maxRangeLight;
    static int m_maxRange;
    static double *Exponential;

    // scaled
    int x[MAX_TAXELS],y[MAX_TAXELS];
    int xv[8],yv[8];

    int nVerts;
    int nTaxels;

    int xMin,xMax,yMin,yMax;

    int m_Width,m_Height;

    public:
    int min_tax;
    int max_tax;
};

#endif


/*
* -----------------------------------------------------------------
*  ode_lib.c
*  ODE Solver Library
*  Version: 2.0
*  Last Update: Oct 1, 2019
*
*  This is the implementation of a computational library with
*  numerical tools for Ordinary Differential Equations (ODE).
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2019 by Americo Barbosa da Cunha Junior
* -----------------------------------------------------------------
*/

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

#include <cvode/cvode.h>               /* prototypes for CVODE functions and const */
#include <nvector/nvector_serial.h>    /* access to serial NVector                 */
#include <sunmatrix/sunmatrix_dense.h> /* access to dense SUNMatrix                */
#include <sunlinsol/sunlinsol_dense.h> /* access to dense SUNLinearSolver          */
#include <sundials/sundials_types.h>   /* defs. of realtype, sunindextype          */

#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>

#include "../include/ode_lib.h"

/*
*------------------------------------------------------------
*   uround
*
*   This function computes the machine unit roundoff
*   defined as the smallest u such that 1.0 + u > 1.0
*
*   Output:
*   uround - machine unit roundoff
*
*   last update: May 20, 2009
*------------------------------------------------------------
*/

double uround(void)
{
    double u = 1.0;
    double comp = 0.0;

    while(comp != 1.0)
    {
	u *= 0.5;
	comp = 1.0 + u;
    }

    return 2.0*u;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   wnorm
*
*   This function computes v1 and v2 vector weight norm
*   defined as
*   wnorm = sqrt( (1/n)*(sum (v1[i]*v2[i])^2) )
*
*   Input:
*   n     - vectors dimension
*   v     - vector 1
*   v     - vector 2
*
*   Output:
*   wnorm - weight norm
*
*   last update: May 20, 2009
*------------------------------------------------------------
*/

double wnorm(int n,double *v1,double *v2)
{
    int i;
    double wnorm = 0.0;

    for ( i = 0; i < n; i++ )
	wnorm += v1[i]*v1[i]*v2[i]*v2[i];

    return sqrt( wnorm / (double)n );
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   ewset
*
*   This function creates a error weight vector
*   defined as ewv[i] = rtol*abs(v[i]) + atol
*
*   Input:
*   n    - vector dimension
*   v    - vector
*   atol - absolute tolerance
*   rtol - relative tolerance
*
*   Output:
*   ewv  - error weight vector
*
*   last update: May 22, 2009
*------------------------------------------------------------
*/

void ewtset(int n,double *v,double atol,double rtol,double *ewt)
{
    int i;

    for ( i = 0; i < n; i++ )
        ewt[i] =  rtol*fabs(v[i]) + atol;

    return;
}
/*------------------------------------------------------------*/

/*------------------------------------------------------------
*   jacobian
*
*   This function computes the jacobian matrix of ydot = f(y,t)
*   defiend as  Jij = dfi/dyj
*
*   Input:
*   n      - vector dimension
*   f_data - pointer to external data
*   Fy     - reaction mapping
*   y      - composition vector
*   t      - time step
*   atol   - absolute tolerance
*   rtol   - relative tolerance
*
*   Output:
*   J       - jacobian matrix
*   success or error
*
*   last update: Oct 7, 2009
------------------------------------------------------------*/

int jacobian(CVRhsFn f,void *f_data,gsl_vector *Fy,gsl_vector *y,
                    double t,double atol,double rtol,gsl_matrix *J)
{
    unsigned int i, j, N1, N2;
    double roundoff, min_inc_mult;
    double fnorm, minInc, inc, inc_inv, yjsaved, srur;
    double *yd_data  = NULL;
    double *Fyd_data = NULL;
    N_Vector yd      = NULL;
    N_Vector Fyd     = NULL;
    gsl_vector *ewt  = NULL;

    min_inc_mult = 1.0e3;

    /* vectors dimensions */
    N1 = y->size;
    N2 = Fy->size;

    /* error weight vector */
    ewt = gsl_vector_calloc(N1);

    /* computing the unit roundoff */
    roundoff = uround();

    /* memory allocation and startup of yd and Fyd */
    yd  = N_VNew_Serial(N1);
    Fyd = N_VNew_Serial(N2);
    if ( yd == NULL || Fyd == NULL )
        return GSL_ENOMEM;

    /* obtaining yd and Fyd compoments  */
    yd_data  = NV_DATA_S(yd);
    Fyd_data = NV_DATA_S(Fyd);

    /* setting yd equal y vector */
    for ( i = 0; i < N1; i++ )
        yd_data[i] = y->data[i];

    /* setting Fyd equal the null vector */
    for ( i = 0; i < N2; i++ )
        Fyd_data[i] = 0.0;

    /* defing error weight vector */
    ewtset(N1,y->data,atol,rtol,ewt->data);

    /* computing weight norm */
    fnorm = wnorm(N2,Fy->data,ewt->data);

    /* square root of the machine roundoff */
    srur = sqrt(roundoff);

    /* computing disturbance parameter */
    minInc = (fnorm != 0.0) ?
	    (min_inc_mult*fabs(t)*roundoff*((double)N1)*fnorm) : 1.0;

    for ( j = 0; j < N1; j++ )
    {
        /* saving y[j] value */
        yjsaved = yd_data[j];

        /* disturbance */
        inc = GSL_MAX(srur*fabs(yjsaved),minInc/ewt->data[j]);

    	/* disturbing y */
        yd_data[j] += inc;

        /* computing Fy disturbed */
        f(t,yd,Fyd,f_data);

	/* restoring yd[j] original value */
        yd_data[j] = yjsaved;

	/* computing the step */
        inc_inv = 1.0/inc;

        /* computing jacobian matrix column j */
    	for ( i = 0; i < N2; i++ )
            J->data[i*J->tda+j] = inc_inv*(Fyd_data[i] - Fy->data[i]);
    }

    /* releasing allocated memory */
    N_VDestroy_Serial(yd);
    N_VDestroy_Serial(Fyd);
    gsl_vector_free(ewt);
    yd       = NULL;
    Fyd      = NULL;
    yd_data  = NULL;
    Fyd_data = NULL;
    ewt      = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*------------------------------------------------------------
*   gradient
*
*   This function computes the gradient matrix of R(phi)
*   defiend as: Aij = d R_i/d phi_j
*
*   Input:
*   cvode_mem - ODE solver workspace
*   t0        - initial time
*   delta_t   - time step
*   phi     - composition vector
*   Rphi    - reac tion mapping
*
*   Output:
*   A       - gradient matrix
*   success or error
*
*   last update: Oct 1, 2019
------------------------------------------------------------*/

int gradient(void *cvode_mem,double t0,double delta_t,
             gsl_vector *phi,gsl_vector *Rphi,gsl_matrix *A)
{
    unsigned int i, j, flag;
    double inc, inc_inv, phijsaved, srur;
    gsl_vector *Rphid = NULL;

    /* memory allocation for Rphid */
    Rphid = gsl_vector_calloc(Rphi->size);

    /* square root of 1.0e6 times the machine roundoff */
    srur = sqrt(1.0e6*DBL_EPSILON);

    for ( j = 0; j < phi->size; j++ )
    {
        /* save phi_j value */
        phijsaved = phi->data[j];

        /* disturbance */
        inc = phijsaved*srur + srur;

    	/* disturbe phi_j */
        phi->data[j] += inc;

        /* compute R(phi) disturbed */
	flag = odesolver_reinit(t0,phi,cvode_mem);
        if ( flag != GSL_SUCCESS )
            return flag;

	flag = odesolver(cvode_mem,delta_t,Rphid);
        if ( flag != GSL_SUCCESS )
            return flag;

	/* restore phi_j original value */
        phi->data[j] = phijsaved;

	/* compute the step */
        inc_inv = 1.0/inc;

        /* compute gradient matrix column j */
    	for ( i = 0; i < Rphi->size; i++ )
            A->data[i*A->tda+j] = inc_inv*(Rphid->data[i] - Rphi->data[i]);
    }

    /* release allocated memory */
    gsl_vector_free(Rphid);
    Rphid = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   linear_approx
*
*   This function computes the linear approximation
*   for a vector function F(x) near the point x0.
*
*   F(x) = F(x0) + DF(x0)*(x-x0)
*
*   Input:
*   x    - vector x
*   x0   - vector x0
*   Fx0  - function at x0
*   DFx0 - jacobian matrix at x0
*
*   Output:
*   Fx   - linear approximation for F(x)
*
*   last update: Jun 9, 2009
*------------------------------------------------------------
*/

void linear_approx(gsl_vector *x,gsl_vector *x0,
                gsl_vector *Fx0,gsl_matrix *DFx0,gsl_vector *Fx)
{
    gsl_vector *aux = NULL;

    /* memory allocation */
    aux = gsl_vector_alloc(x0->size);

    /* aux := x */
    gsl_vector_memcpy(aux,x);

    /* aux := x - x0 */
    gsl_vector_sub(aux,x0);

    /* Fx := F(x0) */
    gsl_vector_memcpy(Fx,Fx0);

    /* Fx := F(x0) + DF(x0)*(x-x0) */
    gsl_blas_dgemv(CblasNoTrans,1.0,DFx0,aux,1.0,Fx);

    /* releasing allocated memory */
    gsl_vector_free(aux);
    aux = NULL;

    return;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver_init
*
*   This function initiates the ODE solver workspace.
*
*   Input:
*   f         - right hand side function
*   f_data    - right hand side function data
*   t0        - initial time
*   mxsteps   - max # of solver iterations
*   atol      - absolute tolerance
*   rtol      - relative tolerance
*   x         - initial condition
*   cvode_mem - ODE solver workspace
*
*   Output:
*   success or error
*
*   last update: Oct 1, 2019
*------------------------------------------------------------
*/

int odesolver_init(CVRhsFn f,void *f_data,double t0,int mxsteps,
		   double atol,double rtol,gsl_vector *x,void *cvode_mem)
{
    int         maxnef = 100;           /* max. # of error test failures  */
    int           flag = CV_SUCCESS;    /* return value flag              */
    N_Vector        y0 = NULL;          /* initial condition vector       */
    SUNMatrix        A = NULL;          /* Matrix for linear solver use   */
    SUNLinearSolver LS = NULL;          /* dense linear solver object     */

    /* memory allocation for initial condition y0 */
    y0 = N_VMake_Serial(x->size,x->data);
    if ( y0 == NULL )
	return GSL_ENOMEM;

    /* memory allocation for CVODE workspace */
    flag = CVodeInit(cvode_mem,f,t0,y0);
    if( flag != CV_SUCCESS )
    	return GSL_ENOMEM;

    /* specifies scalar absolute and relative tolerances */
    flag = CVodeSStolerances(cvode_mem,rtol,atol);
    if( flag != CV_SUCCESS )
    	return flag;

    /* set user data for right hand side function */
    flag = CVodeSetUserData(cvode_mem,f_data);
    if( flag != CV_SUCCESS )
        return flag;

    /* Create dense SUNMatrix for use in linear solver */
    A = SUNDenseMatrix(x->size,x->size);
    if ( A == NULL )
	return GSL_ENOMEM;

    /* Create dense SUNLinearSolver object for use by CVode */
    LS = SUNLinSol_Dense(y0, A);
    if ( LS == NULL )
	return GSL_ENOMEM;

    /* Call CVodeSetLinearSolver to attach the matrix and linear solver to CVode */
    flag = CVodeSetLinearSolver(cvode_mem, LS, A);
    if( flag != CVLS_SUCCESS )
    	return flag;

    /* set solver max # of steps */
    flag = CVodeSetMaxNumSteps(cvode_mem,mxsteps);
    if( flag != CV_SUCCESS )
        return flag;

    /* set max # of error test failures permitted */
    flag = CVodeSetMaxErrTestFails(cvode_mem,maxnef);
    if( flag != CV_SUCCESS )
        return flag;

    /* release allocated memory for y0 */
    N_VDestroy_Serial(y0);
    y0 = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver_reinit
*
*   This function reinitiates the ODE solver workspace.
*
*   Input:
*   f         - right hand side function
*   f_data    - right hand side function data
*   t0        - initial time
*   x         - initial condition
*   cvode_mem - ODE solver workspace
*
*   Output:
*   success or error
*
*   last update: Oct 1, 2019
*------------------------------------------------------------
*/

int odesolver_reinit(double t0,gsl_vector *x,void *cvode_mem)
{
    int flag    = CV_SUCCESS;  /* return value flag              */
    N_Vector y0 = NULL;        /* initial condition vector       */

    /* memory allocation for y0 */
    y0 = N_VMake_Serial(x->size,x->data);
    if ( y0 == NULL )
	return GSL_ENOMEM;

    /* restart CVODE workspace */
    flag = CVodeReInit(cvode_mem,t0,y0);
    if( flag != CV_SUCCESS )
    	return flag;

    /* release allocated memory */
    N_VDestroy_Serial(y0);
    y0 = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver
*
*   This function performs the direct integration of the
*   governing equations.
*
*   Input:
*   cvode_mem - ODE solver workspace
*   tf   - final time
*   Fx   - solution vector
*
*   Output:
*   success or error
*
*   last update: Oct 31, 2009
*------------------------------------------------------------
*/

int odesolver(void *cvode_mem,double tf,gsl_vector *Fx)
{
    double t;
    int flag   = CV_SUCCESS;
    N_Vector y = NULL;

    /* memory allocation and startup of y */
    y = N_VMake_Serial(Fx->size,Fx->data);
    if ( y == NULL )
	return GSL_ENOMEM;

    /* calling CVode solver */
    flag = CVode(cvode_mem,tf,y,&t,CV_NORMAL);
    if( flag != CV_SUCCESS )
        return flag;

    /* releasing allocated memory */
    N_VDestroy_Serial(y);
    y = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/


// Copyright (c) 2005  Stanford University (USA).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public License as
// published by the Free Software Foundation; either version 3 of the License,
// or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
//
// Author(s)     : Daniel Russel <drussel@alumni.princeton.edu>

#ifndef CGAL_POLYNOMIAL_INTERNAL_GSL_NUMERIC_SOLVER_H
#define CGAL_POLYNOMIAL_INTERNAL_GSL_NUMERIC_SOLVER_H
#include <CGAL/Polynomial/basic.h>
#include <CGAL/Polynomial/internal/numeric_solvers_support.h>
#include <vector>

#ifdef CGAL_POLYNOMIAL_USE_GSL

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

namespace CGAL { namespace POLYNOMIAL { namespace internal {

/*template <bool CLEAN>
inline double gsl_max_error()
{
    if (CLEAN) return .0000005;
    else return 0;
    }*/

template <bool CLEAN>
inline void gsl_compute_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots_)
{

  //double max_error=gsl_max_error<CLEAN>();

    gsl_poly_complex_workspace *workspace;
    int degree= end-begin-1;
    workspace= gsl_poly_complex_workspace_alloc(degree+1);

//! I can't use auto_ptr because it is an array
    double *roots= new double[2*degree];

/*for ( int i=0; i< 2*fn.degree(); ++i){
  roots[i]=0;
  }*/

    int ret = gsl_poly_complex_solve(begin, degree+1, workspace,
        roots);
    roots_.reserve(ret);

    if (ret!=GSL_SUCCESS) {
        std::cerr << "GSL solver did not converge on ";
        std::copy(begin, end, std::ostream_iterator<double>(std::cerr, " "));
        std::cerr << std::endl;
    }

    double last= -std::numeric_limits<double>::infinity();
    for ( int i=degree-1; i>=0; --i) {
        double r= roots[2*i];
        double c= roots[2*i+1];
        if (root_is_good(r, c, lb, ub)) {
            roots_.push_back(r);
        } else if (CLEAN && root_is_good(r,c, last, ub)) {
	  last= r;
	}
    }

    std::sort(roots_.begin(), roots_.end(), std::greater<double>() );
    delete roots;

    gsl_poly_complex_workspace_free(workspace);

    if (CLEAN) filter_solver_roots(begin,end, lb, ub, last, roots_);
    return;
}

template <bool CLEAN>
inline void gsl_compute_cubic_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots_)
{
    CGAL_Polynomial_precondition(begin[3] != 0);
    //double max_error=gsl_max_error<CLEAN>();

    double r[3];
    int num_roots= gsl_poly_solve_cubic(begin[2]/begin[3],
        begin[1]/begin[3],
        begin[0]/begin[3], &r[0],&r[1],&r[2]);
    roots_.reserve(num_roots);
    double last= -std::numeric_limits<double>::infinity();
// I want reverse sorted roots
    for (int i=num_roots-1; i>=0; --i) {
        if (r[i]>  lb && r[i] < ub) roots_.push_back(r[i]);
	else if (CLEAN && r[i] <lb && r[i] > last){
	  last= r[i];
	}
    }
    if (CLEAN) filter_solver_roots(begin, end, lb, ub, last, roots_);
}

inline void gsl_polynomial_compute_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots)
{
    int degree= end-begin-1;
    switch( degree) {
        case -1:
        case 0:
            break;
        case 1:
            compute_linear_roots(begin,end, lb, ub, roots);
            break;
        case 2:
            compute_quadratic_roots(begin, end, lb, ub, roots);
            break;
        case 3:
            gsl_compute_cubic_roots<false>(begin, end, lb, ub, roots);
            break;
        default:
            gsl_compute_roots<false>(begin, end, lb, ub, roots);
    }
}

inline void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots)
{
    int degree= end-begin-1;
    switch( degree) {
        case -1:
        case 0:
            break;
        case 1:
            compute_linear_cleaned_roots(begin,end, lb, ub, roots);
            break;
        case 2:
            compute_quadratic_cleaned_roots(begin, end, lb, ub, roots);
            break;
        case 3:
            gsl_compute_cubic_roots<true>(begin, end, lb, ub, roots);
            break;
        default:
            gsl_compute_roots<true>(begin, end, lb, ub, roots);
    }
}

inline double gsl_evaluate_polynomial(const double *b, const double *e, double t)
{
    if (b==e) return 0;
/*double *d= new double[coefs.size()];
for (unsigned int i=0; i< coefs.size(); ++i){
  d[i]= coefs[i];
  }*/
    double v= gsl_poly_eval(b, e-b, t);
    return v;
}

/*

inline void gsl_polynomial_compute_roots(const double *begin, const double *end,
                  double lb, double ub,
                  std::vector<double> &roots){
 CGAL_assertion(0&& no_gsl_support_built);
}

inline void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
                  double lb, double ub,
                  std::vector<double> &roots){
CGAL_assertion(0&& no_gsl_support_built);
}

inline double gsl_evaluate_polynomial(const double *b, const double *e, double t) {
CGAL_assertion(0&& no_gsl_support_built);
return std::numeric_limits<double>::nan();
}
*/

/*// GSL
void gsl_polynomial_compute_roots(const double *begin, const double *end,
                  double lb, double ub, std::vector<double> &roots);

void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
double lb, double ub, std::vector<double> &roots);*/

struct GSL_numeric_solver
{
    void operator()(const double *begin, const double *end,
        double lb, double ub,
        std::vector<double> &roots) const
    {
        gsl_polynomial_compute_roots(begin, end, lb, ub, roots);
    }
};

struct GSL_cleaned_numeric_solver
{
    void operator()(const double *begin, const double *end,
        double lb, double ub,
        std::vector<double> &roots) const
    {
        gsl_polynomial_compute_cleaned_roots(begin, end, lb, ub, roots);
    }
};

} } } //namespace CGAL::POLYNOMIAL::internal

#endif // CGAL_POLYNOMIAL_USE_GSL

#endif


// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz, Conor Williams, Oliver Schonrock
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#pragma once

#include <units/bits/external/hacks.h>
#include <array>
#include <cassert>
#include <cmath>
#include <cstdint>
#include <numeric>
#include <tuple>
#include <type_traits>
#include <gsl/gsl-lite.hpp>

namespace units::detail {

template<typename T>
[[nodiscard]] constexpr T abs(T v) noexcept
{
  return v < 0 ? -v : v;
}

// the following functions enable gcd and related computations on ratios
// with exponents. They avoid overflow. Further information here:
// https://github.com/mpusz/units/issues/62#issuecomment-588152833

// Computes (a * b) mod m relies on unsigned integer arithmetic, should not
// overflow
[[nodiscard]] constexpr std::uint64_t mulmod(std::uint64_t a, std::uint64_t b, std::uint64_t m)
{
  std::uint64_t res = 0;

  if (b >= m) {
    if (m > UINT64_MAX / 2u) {
      b -= m;
    } else {
      b %= m;
    }
  }

  while (a != 0) {
    if (a & 1) {
      if (b >= m - res) {
        res -= m;
      }
      res += b;
    }
    a >>= 1;

    std::uint64_t temp_b = b;
    if (b >= m - b) {
      temp_b -= m;
    }
    b += temp_b;
  }

  return res;
}

// Calculates (a ^ e) mod m , should not overflow.
[[nodiscard]] constexpr std::uint64_t modpow(std::uint64_t a, std::uint64_t e, std::uint64_t m)
{
  a %= m;
  std::uint64_t result = 1;

  while (e > 0) {
    if (e & 1) {
      result = mulmod(result, a, m);
    }
    a = mulmod(a, a, m);
    e >>= 1;
  }
  return result;
}

// gcd(a * 10 ^ e, b), should not overflow
[[nodiscard]] constexpr std::intmax_t gcdpow(std::intmax_t a, std::intmax_t e, std::intmax_t b) noexcept
{
  assert(a > 0);
  assert(e >= 0);
  assert(b > 0);

  // gcd(i, j) = gcd(j, i mod j) for j != 0 Euclid;
  //
  // gcd(a 10^e, b) = gcd(b, a 10^e mod b)
  //
  // (a 10^e) mod b -> [ (a mod b) (10^e mod b) ] mod b

  return std::gcd(
      b, static_cast<std::intmax_t>(mulmod(static_cast<std::uint64_t>(a % b),
                                           modpow(10, static_cast<std::uint64_t>(e), static_cast<std::uint64_t>(b)),
                                           static_cast<std::uint64_t>(b))));
}

constexpr void cwap(std::intmax_t& lhs, std::intmax_t& rhs)
{
  std::intmax_t tmp = lhs;
  lhs = rhs;
  rhs = tmp;
}

// Computes the rational gcd of n1/d1 x 10^e1 and n2/d2 x 10^e2
[[nodiscard]] constexpr auto gcd_frac(std::intmax_t n1, std::intmax_t d1, std::intmax_t e1, std::intmax_t n2, std::intmax_t d2,
                                      std::intmax_t e2) noexcept
{
  // Short cut for equal ratios
  if (n1 == n2 && d1 == d2 && e1 == e2) {
    return std::array{n1, d1, e1};
  }

  if (e2 > e1) {
    detail::cwap(n1, n2);
    detail::cwap(d1, d2);
    detail::cwap(e1, e2);
  }

  std::intmax_t exp = e2;  // minimum

  // gcd(a/b,c/d) = gcd(a⋅d, c⋅b) / b⋅d

  assert(std::numeric_limits<std::intmax_t>::max() / n1 > d2);
  assert(std::numeric_limits<std::intmax_t>::max() / n2 > d1);

  std::intmax_t num = detail::gcdpow(n1 * d2, e1 - e2, n2 * d1);

  assert(std::numeric_limits<std::intmax_t>::max() / d1 > d2);

  std::intmax_t den = d1 * d2;

  std::intmax_t gcd = std::gcd(num, den);

  return std::array{num / gcd, den / gcd, exp};
}

constexpr void normalize(std::intmax_t& num, std::intmax_t& den, std::intmax_t& exp)
{
  if(num == 0) {
    den = 1;
    exp = 0;
    return;
  }

  std::intmax_t gcd = std::gcd(num, den);
  num = num * (den < 0 ? -1 : 1) / gcd;
  den = detail::abs(den) / gcd;

  while (num % 10 == 0) {
    num /= 10;
    ++exp;
  }
  while (den % 10 == 0) {
    den /= 10;
    --exp;
  }
}

[[nodiscard]] constexpr std::intmax_t safe_multiply(std::intmax_t lhs, std::intmax_t rhs)
{
  constexpr std::intmax_t c = std::uintmax_t(1) << (sizeof(std::intmax_t) * 4);

  const std::intmax_t a0 = detail::abs(lhs) % c;
  const std::intmax_t a1 = detail::abs(lhs) / c;
  const std::intmax_t b0 = detail::abs(rhs) % c;
  const std::intmax_t b1 = detail::abs(rhs) / c;

  gsl_Expects(a1 == 0 || b1 == 0);                               // overflow in multiplication
  gsl_Expects(a0 * b1 + b0 * a1 < (c >> 1));                     // overflow in multiplication
  gsl_Expects(b0 * a0 <= INTMAX_MAX);                            // overflow in multiplication
  gsl_Expects((a0 * b1 + b0 * a1) * c <= INTMAX_MAX - b0 * a0);  // overflow in multiplication

  return lhs * rhs;
}

}  // namespace units::detail


/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/*
 * File:   stdheader.h
 * Author: lars
 *
 * Created on May 19, 2020, 2:40 PM
 */

#ifndef STDHEADER_H
#define STDHEADER_H
#include <string>
#include <vector>
#include <random>
#include <chrono>
#include <limits>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <cstddef>
#include <regex>
#include <cmath>
#include <cstdlib>
#include <filesystem>
#include <tuple>
#include <map>
#include <utility>
#include <iomanip>
#include <type_traits>
#include <set>
#include <experimental/filesystem>
#include <filesystem>
#include <time.h>
#include <unordered_set>
#include <unordered_map>
//simple parallel
#include <parallel/algorithm>
//#include <experimental/algorithm>

#include <nlohmann/json.hpp>
#include <gsl/gsl_statistics_double.h>
#include "CLI11.hpp"
#include <essentia/algorithmfactory.h>
#include <essentia/streaming/algorithms/poolstorage.h>
#include <essentia/streaming/algorithms/fileoutput.h>
#include <essentia/pool.h>
#include <essentia/essentiamath.h>

#include "statInterface.h"
#include "utilities.h"
#include "featureSelektor.h"
#include "AudioFeature.h"
#include "GLCM.h"
#include "FACM.h"
#include "Haralick.h"
#include "AudioFile.h"
#include "distanceMetric.h"
#include "essentiaInterface.h"
#include "Json_Handler.h"
#include "printObjects.h"
#include "CLIHandler.h"
#include "src/hierarchCluster/fastcluster.h"

#include "simpleTests.h"
#endif /* STDHEADER_H */


/*
** smooth variance computations
**
** 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>

VImage variance_smoothing(VImage *src[], float width)
{
  size_t nslices, nrows, ncols;

  /* image dims */
  nslices = VImageNBands(src[0]);
  nrows  = VImageNRows(src[0]);
  ncols  = VImageNColumns(src[0]);

  VImage var = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  VFillImage(var,VAllBands,0);

  /*double *data,int n,double *a,double *v)*/

  int j;
  double ave,var,nx,s,u;

  nx = (double)n;
  ave = 0;
  for (j=0; j<n; j++) ave += data[j];
  ave /= nx;

  var = u = 0;
  for (j=0; j<n; j++) {
    s = data[j]-ave;
    u   += s;
    var += s*s;
  }
  var=(var-u*u/nx)/(nx-1);

  /* smooth variance */

  return var;
}


#ifndef libraries_h
#define libraries_h

#include <fstream>
#include <iostream>
#include <iomanip>
#include <ctime>
#include <math.h>
#include <vector>
#include <deque>
#include <queue>
#include <stack>
#include <array>
#include <algorithm>
#include <unordered_map>
#include <map>
#include <memory>
#include <utility>
#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_statistics.h>
#include "types.h"
//#include <omp.h>
//#include <tbb/tbb.h>

#endif


/* eigen/gsl_eigen.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, 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.
 */

#ifndef __GSL_EIGEN_H__
#define __GSL_EIGEN_H__

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

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

__BEGIN_DECLS

typedef struct {
  size_t size;
  double * d;
  double * sd;
} gsl_eigen_symm_workspace;

gsl_eigen_symm_workspace * gsl_eigen_symm_alloc (const size_t n);
void gsl_eigen_symm_free (gsl_eigen_symm_workspace * w);
int gsl_eigen_symm (gsl_matrix * A, gsl_vector * eval, gsl_eigen_symm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * gc;
  double * gs;
} gsl_eigen_symmv_workspace;

gsl_eigen_symmv_workspace * gsl_eigen_symmv_alloc (const size_t n);
void gsl_eigen_symmv_free (gsl_eigen_symmv_workspace * w);
int gsl_eigen_symmv (gsl_matrix * A, gsl_vector * eval, gsl_matrix * evec, gsl_eigen_symmv_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
} gsl_eigen_herm_workspace;

gsl_eigen_herm_workspace * gsl_eigen_herm_alloc (const size_t n);
void gsl_eigen_herm_free (gsl_eigen_herm_workspace * w);
int gsl_eigen_herm (gsl_matrix_complex * A, gsl_vector * eval,
                         gsl_eigen_herm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
  double * gc;
  double * gs;
} gsl_eigen_hermv_workspace;

gsl_eigen_hermv_workspace * gsl_eigen_hermv_alloc (const size_t n);
void gsl_eigen_hermv_free (gsl_eigen_hermv_workspace * w);
int gsl_eigen_hermv (gsl_matrix_complex * A, gsl_vector * eval,
                           gsl_matrix_complex * evec,
                           gsl_eigen_hermv_workspace * w);

typedef enum {
  GSL_EIGEN_SORT_VAL_ASC,
  GSL_EIGEN_SORT_VAL_DESC,
  GSL_EIGEN_SORT_ABS_ASC,
  GSL_EIGEN_SORT_ABS_DESC
}
gsl_eigen_sort_t;

/* Sort eigensystem results based on eigenvalues.
 * Sorts in order of increasing value or increasing
 * absolute value.
 *
 * exceptions: GSL_EBADLEN
 */

int gsl_eigen_symmv_sort(gsl_vector * eval, gsl_matrix * evec,
                         gsl_eigen_sort_t sort_type);

int gsl_eigen_hermv_sort(gsl_vector * eval, gsl_matrix_complex * evec,
                         gsl_eigen_sort_t sort_type);

/* The following functions are obsolete: */

/* Eigensolve by Jacobi Method
 *
 * The data in the matrix input is destroyed.
 *
 * exceptions:
 */
int
gsl_eigen_jacobi(gsl_matrix * matrix,
                      gsl_vector * eval,
                      gsl_matrix * evec,
                      unsigned int max_rot,
                      unsigned int * nrot);

/* Invert by Jacobi Method
 *
 * exceptions:
 */
int
gsl_eigen_invert_jacobi(const gsl_matrix * matrix,
                             gsl_matrix * ainv,
                             unsigned int max_rot);

__END_DECLS

#endif /* __GSL_EIGEN_H__ */


#include "lah.h"

#ifdef HAVE_LAPACK
#include <lapacke.h>

lah_Return lah_eigenValue(lah_mat *P, lah_value *S, lah_mat *Z)
{
    lapack_int res, nEig;
    lapack_int *isuppz;
    if (P == NULL || S == NULL || Z == NULL
        || P->nR != P->nC)
    {
        return lahReturnParameterError;
    }
    /* allocate space for the output parameters and workspace arrays */
    isuppz = malloc(2 * P->nR * sizeof(lapack_int));

    /* Data allocation */
    /* Sollte A-> data nur dreiecksmatrix enthalten ?*/
    res = SYEVR(LAH_LAPACK_LAYOUT, 'V', 'A', 'L', P->nR, P->data, P->nR,
                0, 0, 0, 0, LAMCH('S'), &nEig, S, Z->data, P->nR, isuppz);

    /* free support array */
    free(isuppz);
    if (res > 0)
        return lahReturnExternError;
    else if (res < 0)
        return lahReturnParameterError;
    else
        return lahReturnOk;
}
#endif


/***********************************
V1 IMPLEMENTATION USING RANDOM POINTS
************************************/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <cblas.h>
#include <string.h>
#include <mpi.h>

#include "../inc/knnring.h"

int main(int argc, char *argv[]){
  int p_rank;
  int p_size;
  int n;
  int d;
  int k;
  double *X;

  //INITIALIZE MPI COMMUNICATIONS
  MPI_Init(&argc,&argv);
  if(argc<4){
    printf("ERROR: GIVE CORRECT NUMBERS OF ARGUMENTS n,d,k\n");
    exit(1);
  }else{
    n = atoi(argv[1]);
    d = atoi(argv[2]);
    k = atoi(argv[3]);
  }
  MPI_Comm_size(MPI_COMM_WORLD, &p_size);
  MPI_Comm_rank(MPI_COMM_WORLD, &p_rank);
  MPI_Status status;                      // STATUS COMMUNICATION
  int tag = 1;                            //COMMUNICATION TAG

  struct timespec ts_start;
	struct timespec ts_end;
  double time;

  if(p_rank == 0){

    X = (double *)malloc(n*d*sizeof(double));

    for(int i=1;i<p_size;i++){
      create_points(X,n*d);
      MPI_Send(X,n*d,MPI_DOUBLE, i, tag, MPI_COMM_WORLD);
    }

    //compute points for myself
    create_points(X,n*d);

    clock_gettime(CLOCK_MONOTONIC, &ts_start);
    knnresult results = distrAllkNN(X,n,d,k);
    clock_gettime(CLOCK_MONOTONIC, &ts_end);
    time = 1000000*(double)(ts_end.tv_sec-ts_start.tv_sec)+(double)(ts_end.tv_nsec-ts_start.tv_nsec)/1000;
    printf("TOTAL V1 time: %lf\n",time);

    //WRITE RESULTS TO FILE
    char str[200];
    snprintf(str,sizeof(str),"results/dist_knn_%d_%d_%d.txt",n,d,k);
    //write_to_file(str,results.nidx,results.ndist,n*k,results.k);

    for(int p=1;p<p_size;p++){
      MPI_Recv(results.nidx, n*k,    MPI_INT,p,tag,MPI_COMM_WORLD, &status);
      MPI_Recv(results.ndist,n*k, MPI_DOUBLE,p,tag,MPI_COMM_WORLD, &status);
      //write_to_file(str,results.nidx,results.ndist,n*k,results.k);
    }

  }else{
    //receive points
    X = (double *)malloc(n*d*sizeof(double));
    MPI_Recv(X,n*d,MPI_DOUBLE, 0, tag, MPI_COMM_WORLD, &status);

    clock_gettime(CLOCK_MONOTONIC, &ts_start);
    knnresult results = distrAllkNN(X,n,d,k);
    clock_gettime(CLOCK_MONOTONIC, &ts_end);
    time = 1000000*(double)(ts_end.tv_sec-ts_start.tv_sec)+(double)(ts_end.tv_nsec-ts_start.tv_nsec)/1000;
    //printf("TOTAL V1 time: %lf from process %d\n",time,p_rank);

    MPI_Send(results.nidx, n*k, MPI_INT   ,0,tag,MPI_COMM_WORLD);
    MPI_Send(results.ndist,n*k, MPI_DOUBLE,0,tag,MPI_COMM_WORLD);
  }

  //execution is over
  MPI_Finalize();
  printf("DONE ....=>EXITING\n");
  return 0;
}


/*
 * C version of Diffusive Nested Sampling (DNest4) by Brendon J. Brewer
 *
 * Yan-Rong Li, liyanrong@mail.ihep.ac.cn
 * Jun 30, 2016
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include <mpi.h>
#include <gsl/gsl_rng.h>

#include "dnest.h"
#include "model3.h"

int which_level_update;
int num_params;
DNestFptrSet *fptrset_thismodel3;

void model3()
{
  int i, argc=0, narg=6;
  char **argv;

  argv = malloc(narg*sizeof(char *));
  for(i=0; i<narg; i++)
  {
    argv[i] = malloc(200*sizeof(char));
  }
  strcpy(argv[argc++], "dnest");
  strcpy(argv[argc++], "-s");
  strcpy(argv[argc++], "restart_dnest3.txt");
  strcpy(argv[argc++], "-l"); //level-dependnet sampling
  strcpy(argv[argc++], "-g"); // tag
  strcpy(argv[argc++], "3");

  /* setup szie of modeltype, which is used for dnest */
  num_params = 2;
  fptrset_thismodel3 = dnest_malloc_fptrset();

  /* setup functions used for dnest*/
  fptrset_thismodel3->from_prior = from_prior_thismodel3;
  fptrset_thismodel3->log_likelihoods_cal = log_likelihoods_cal_thismodel3;
  fptrset_thismodel3->log_likelihoods_cal_initial = log_likelihoods_cal_thismodel3;
  fptrset_thismodel3->log_likelihoods_cal_restart = log_likelihoods_cal_thismodel3;
  fptrset_thismodel3->perturb = perturb_thismodel3;
  fptrset_thismodel3->print_particle = print_particle_thismodel3;
  fptrset_thismodel3->restart_action = restart_action_model3;

  /* run dnest */
  dnest(argc, argv, fptrset_thismodel3, num_params, NULL, NULL, NULL, "./", "OPTIONS3", NULL, NULL);

  /* free memory */
  dnest_free_fptrset(fptrset_thismodel3);

  for(i=0; i<narg; i++)
    free(argv[i]);
  free(argv);

}

/*====================================================*/
/* users responsible for following struct definitions */

void from_prior_thismodel3(void *model)
{
  int i;
  double *params = (double *)model;
  for(i=0; i<num_params; i++)
  {
    params[i] = -6.0 + 12.0 * dnest_rand();
    dnest_wrap(&params[i], -6.0, 6.0);
  }
}

double log_likelihoods_cal_thismodel3(const void *model)
{
  double *params = (double *)model;
  double logL;
  double logl1;
  double logl2;

  logl1 = -0.5 * pow( (sqrt((params[0] - 3.0)* (params[0] - 3.0) + params[1]*params[1])- 2.0)/0.1,  2.0)
          - 0.5 * log(2.0*M_PI * 0.01);
  logl2 = -0.5 * pow( (sqrt((params[0] + 3.0)* (params[0] + 3.0) + params[1]*params[1])- 2.0)/0.1,  2.0)
          - 0.5 * log(2.0*M_PI * 0.01);

  double max = fmax(logl1, logl2);

  logL = log( exp(logl1-max) + exp(logl2-max) ) + max;

  return logL;
}

double perturb_thismodel3(void *model)
{
  double *params = (double *)model;
  double logH = 0.0, width, limit1, limit2;
  int which = dnest_rand_int(num_params), which_level;
  int size_levels;

  which_level_update = dnest_get_which_level_update();
  size_levels = dnest_get_size_levels();
  which_level = which_level_update > (size_levels - 20)?(size_levels-20):which_level_update;
  which_level = 0;
  if(which_level > 0 )
  {
    limit1 = limits[(which_level_update-1) * num_params *2 + which *2 ];
    limit2 = limits[(which_level_update-1) * num_params *2 + which *2 + 1];
  }
  else
  {
    limit1 = -6.0;
    limit2 = 6.0;
  }
  width = (limit2 - limit1);
  params[which] += width * dnest_randh();
  dnest_wrap(&params[which], -6.0, 6.0);
  return logH;
}
/*=======================================================*/

void print_particle_thismodel3(FILE *fp, const void *model)
{
  int i;
  double *params = (double *)model;
  for(i=0; i<num_params; i++)
  {
    fprintf(fp, "%f ", params[i]);
  }
  fprintf(fp, "\n");
}

/*========================================================*/

void restart_action_model3(int iflag)
{
  return;
}


/*
 *  gsl_headers.h
 *  Pi4U
 *
 *  Created by Panagiotis Hadjidoukas on 1/1/14.
 *  Copyright 2014 ETH Zurich. All rights reserved.
 *
 */

#ifndef _GSL_HEADERS_
#define _GSL_HEADERS_ 1

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

#include <gsl/gsl_roots.h>

/* NDL */
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_sort_vector.h>

#endif


#ifndef BASIS_H
#define BASIS_H

#include <algorithm>
#include <cstddef>
#include <cmath>
#include <map>
#include <memory>
#include <mutex>
#include <vector>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_spblas.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_wavelet.h>

#include "constants.h"
#include "vector.h"

using std::array;
using std::shared_ptr;
using std::unique_ptr;
using std::vector;
using std::map;
using std::pair;

using BasisMap = Vector[]; // length
using ConvMap = Vector[]; // length * length

void projForward(gsl_vector *f);
void projBackward(gsl_vector *u);

void projForward(gsl_vector *f, gsl_vector *u);
void projBackward(gsl_vector *u, gsl_vector *f);

const Vector& getBasis(size_t k);
const Vector& getConvoluted(size_t p, size_t f);

double convolutedBasis(size_t p, size_t f, size_t mu);

void normalize(gsl_vector *f);

extern unique_ptr<BasisMap> storedBases;
extern unique_ptr<ConvMap> storedConvs;

#endif // WAVELETS_H


/*
 * Copyright 2009-2019 The VOTCA-MPIP Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * author: Denis Andrienko
 */

#ifndef __VOTCA_CTP_EIGEN_H
#define __VOTCA_CTP_EIGEN_H

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/numeric/conversion/cast.hpp>

#include <votca/tools/eigen.h>
#include <Eigen/Eigenvalues>

/*
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>
*/

#ifdef DEBUG_LINALG
#include <chrono>
#endif

namespace boost { namespace numeric { namespace ublas {

// specialization for double precision
template<class F, class A>
inline matrix<double,F,A>    // prod( m1, m2 )
prod(const matrix<double,F,A> &m1, const matrix<double,F,A> &m2)
{

    #ifdef DEBUG_LINALG
    auto start = std::chrono::system_clock::now();
    #endif

    boost::numeric::ublas::matrix<double,F,A> AxB( m1.size1(), m2.size2() );

    // Note that a Map<Type> m(&data) gives a view into the data (can change it)
    // while assignments Matrix<Type> m = Map< Matrix<Type> > (&data)
    // lead to a data copy and lost connection between the reference and matrix
    Eigen::Map<const Eigen::MatrixXd> mA_( &(m1.data()[0]), m1.size2(), m1.size1() );
    Eigen::Map<const Eigen::MatrixXd> mB_( &(m2.data()[0]), m2.size2(), m2.size1() );
    Eigen::Map<Eigen::MatrixXd> AxB_( &(AxB.data()[0]), AxB.size2(), AxB.size1() );

    AxB_ = mB_*mA_;

    #ifdef DEBUG_LINALG
    auto end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    std::cout << "\n... ... ... \x1b[0;34mEigen "
              << "[" << m1.size1() << "x" << m1.size2() << "]"
              << "*[" << m2.size1() << "x" << m2.size2() << "]"
              << " time: " << elapsed_seconds.count()
              << "s\x1b[0;39m";
    #endif

    return AxB;

            }

// specialization for single precision
template<class F, class A>
inline matrix<float, F, A> // prod( m1, m2 )
prod(const matrix<float, F, A> &m1, const matrix<float, F, A> &m2) {

    #ifdef DEBUG_LINALG
    auto start = std::chrono::system_clock::now();
    #endif

    boost::numeric::ublas::matrix<float, F, A> AxB(m1.size1(), m2.size2());

    Eigen::Map<const Eigen::MatrixXf> mA_(&(m1.data()[0]), m1.size2(), m1.size1());
    Eigen::Map<const Eigen::MatrixXf> mB_(&(m2.data()[0]), m2.size2(), m2.size1());
    Eigen::Map<Eigen::MatrixXf> AxB_(&(AxB.data()[0]), AxB.size2(), AxB.size1());

    AxB_ = mB_*mA_;

    #ifdef DEBUG_LINALG
    auto end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    std::cout << "\n... ... ... \x1b[0;34mEigen "
            << "[" << m1.size1() << "x" << m1.size2() << "]"
            << "*[" << m2.size1() << "x" << m2.size2() << "]"
            << " time: " << elapsed_seconds.count()
            << "s\x1b[0;39m";
    #endif

    return AxB;
}

}}}

namespace votca { namespace ctp { namespace linalg {

inline void invert_symm( const ub::matrix<double> &A, ub::matrix<double> &V){
    throw "Inversion of a matrix, linalg_invert, is not implemented in eigen.h";
}

/**
 * \brief eigenvalues of a symmetric matrix A*x=E*x
 * @param E vector of eigenvalues
 * @param V input: matrix to diagonalize
 * @param V output: eigenvectors
 *
 * This function wraps Eigen::EigenSolver
 *
 */
inline void eigenvalues_symm(const ub::matrix<double> &A,
                                   ub::vector<double> &E,
                                   ub::matrix<double> &V)
{

    #ifdef DEBUG_LINALG
        auto start = std::chrono::system_clock::now();
    #endif

/*
        gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        V.resize(N, N, false);

        // gsl does not handle conversion of a symmetric_matrix
        ub::matrix<double> _A( N,N );
        //make copy of A as A is destroyed by GSL
        _A = A;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
        assert(status == 0);
	gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

        std::cout << "\nREFERENCE";
        std::cout << "\n A" << A;
        std::cout << "\n E" << E;
        std::cout << "\n V" << V;
        std::cout << "\nREFERENCE\n";

*/
    Eigen::Map<const Eigen::MatrixXd> A_( &(A.data()[0]), A.size2(), A.size1() );

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A_.transpose());
    /*
      std::cout << "\nEIGEN";
      std::cout << "\n E\n" << es.eigenvalues();
      std::cout << "\n V\n" << es.eigenvectors();
      std::cout << "\nEIGEN";
    */

    // copy back to UBLAS objects
    Eigen::Map<Eigen::VectorXd>( &E(0), A.size2() ) = es.eigenvalues();
    Eigen::Map<Eigen::MatrixXd>( &V(0,0), A.size2(), A.size1() ) = es.eigenvectors().transpose();

    #ifdef DEBUG_LINALG
        auto end = std::chrono::system_clock::now();

        std::chrono::duration<double> elapsed_seconds = end - start;

        std::cout << "\n... ... ... \x1b[0;34mEigen Eigenvalues"
                  << "[" << A.size1() << "x" << A.size2() << "]"
                  << " time: " << elapsed_seconds.count()
                  << "s\x1b[0;39m";
    #endif
}

}}}

#endif  // __VOTCA_CTP_EIGEN_H


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

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

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

    gsl_vector * vec = gsl_vector_calloc(3);

    gsl_vector_set(vec, 0, 10.0);
    gsl_vector_set(vec, 1, 20.0);
    gsl_vector_set(vec, 2, 30.0);

    int i = 0;
    for(i=0 ; i<3 ; i++)
        fprintf(stdout, "%f\n", gsl_vector_get(vec, i));

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

    gsl_vector_free(vec);

    exit(EXIT_SUCCESS);
}


#ifndef WILCOXONPARALLELTESTS_H
#define WILCOXONPARALLELTESTS_H
#include <fstream>
#include <cmath>
#include <iostream>
#include <sstream>
#include <cstdlib>
#include <vector>
#include <cstring>
#include <gsl/gsl_cdf.h>

using namespace std;

struct approximatePosition {
    int x;
    double y;
};

class WilcoxonTest {
public:
    WilcoxonTest(float * _data, int _dataXsize, int _dataYsize, string _testIndexes, string _controlIndexes);
    WilcoxonTest(float * _data, int _dataXsize, int _dataYsize, vector<int> * _testIndexes, vector<int> * _controlIndexes);
	vector<double> * test();
private:

    //Variables
    int dataYsize;
    int dataXsize;
    float * data;
    vector<double> * pValues;
    string netCdfFileName;
    vector<int> * testIndexes;
    vector<int> * controlIndexes;
    std::vector<std::vector<approximatePosition> * > * approximatePTable;

    //Set up
    void readNetCdfFile(string dataName, string xDimension, string yDimension);
    std::vector<approximatePosition> * getPositions(string positionsLine);
    std::vector<string> * splitLine(string inputString, char lineSplit = ';');
    void readApproximatePtable();
    vector<int> * parseIntString(string input);

    //Wilcoxon Test main methods
    float calculateWValue(int yIndex, vector<float> * absoluteValues, vector<float> * signs);
    double calculatePValue(float w, int numberOfZeroes);
    float calculateZValue(float w, int Nr);

    //Wilcoxon Test helper methods
    double getApproximatePValue(float w, float z);
    double approximateP(float w, float z, approximatePosition beginningPos, approximatePosition endPos);
    double * rankThePairs(int yIndex, vector<float> * absoluteValues);
    int getSign(float value);
    int getNumberOfZeroes(vector<float> * absoluteValues);
    void trim(string& str);

    //sorting
    void quicksort(int m, int n, vector<float> * absoluteValues, vector<float> * signs);
    void swap(float * x, float * y);
    int choose_pivot(int i, int j );
};

#endif


// Copyright 2018 Jeremy Mason
//
// Licensed under the Apache License, Version 2.0, <LICENSE-APACHE or
// http://apache.org/licenses/LICENSE-2.0> or the MIT license <LICENSE-MIT or
// http://opensource.org/licenses/MIT>, at your option. This file may not be
// copied, modified, or distributed except according to those terms.

//! \file sb_desc.c
//! Contains functions that actually calculate the spherical Bessel descriptors.

#include <cblas.h>      // dscal
#include <math.h>       // pow
#include <stdint.h>     // uint32_t
#include <stdlib.h>     // abort
#include "sbessel.h"    // _sbessel
#include "sb_desc.h"
#include "sb_matrix.h"  // sb_mat_malloc
#include "sb_structs.h" // sb_vec
#include "sb_utility.h" // SB_CHK_ERR
#include "sb_vector.h"  // sb_vec_calloc
#include "safety.h"

// Lookup tables used in get_radial_basis. Built using function in `tables.c`.
static const double _u_data[10152] = {
  #include "unl.tbl"
};
static const size_t _u_n_max = 140;

static const double _c1_data[10011] = {
  #include "c1.tbl"
};
static const size_t _c1_n_max = 140;

static const double _c2_data[10011] = {
  #include "c2.tbl"
};
static const size_t _c2_n_max = 140;

/// Calculates the radial basis functions for the spherical Bessel descriptors.
/// Numerical efficiency depends heavily on the lookup tables defined above,
/// allowing the function to be reduced to evaluating the recursion relations.
///
/// # Parameters
/// - `gnl`: pointer to a matrix to hold the result
/// - `r_data`: radial coordinates of the atoms
/// - `n_max`: defines the number of descriptors calculated
/// - `l`: order of the spherical Bessel functions
/// - `n_atom`: number of atoms in the environment
/// - `rc`: cutoff radius for the environment
///
/// # Returns
/// A copy of `gnl`
static sb_mat * get_radial_basis(
    sb_mat * gnl,
    double * r_data,
    uint32_t l,
    uint32_t n_atom,
    double rc) {
  // access lookup tables directly
  const double * u_data  = _u_data  + l * (2 * _u_n_max  - l + 5) / 2;
  const double * c1_data = _c1_data + l * (2 * _c1_n_max - l + 3) / 2;
  const double * c2_data = _c2_data + l * (2 * _c2_n_max - l + 3) / 2;

  // gnl->n_cols
  const size_t n_cols = gnl->n_cols;

  // forward declaration of variables without initializations
  size_t n, a;
  double u0, u1, u2, d0, d1, e;
  double * g_data;

  // fnl built in gnl
  g_data = gnl->data;
  for (n = 0; n < n_cols; ++n) {
    for (a = 0; a < n_atom; ++a) {
      g_data[a] = c1_data[n] * _sbessel(l, r_data[a] * u_data[n])
        - c2_data[n] * _sbessel(l, r_data[a] * u_data[n + 1]);
    }
    g_data += n_atom;
  }
  sb_mat_smul(gnl, pow(rc, -1.5));

  // initialize quantities used for recursion
  u1 = SB_SQR(u_data[0]);
  u2 = SB_SQR(u_data[1]);
  d1 = 1.;

  // convert to gnl
  g_data = gnl->data;
  for (n = 1; n < n_cols; ++n) {
    u0 = u1;
    u1 = u2;
    u2 = SB_SQR(u_data[n + 1]);

    e = (u0 * u2) / ((u0 + u1) * (u1 + u2));
    d0 = d1;
    d1 = 1. - e / d0;

    g_data += n_atom;
    cblas_dscal(n_atom, 1. / sqrt(d1), g_data, 1);
    cblas_daxpy(n_atom, sqrt(e / (d1 * d0)), g_data - n_atom, 1, g_data, 1);
  }

  return gnl;
}

/// Calculates spherical Bessel descriptors for the given atomic environment.
/// `desc` should contain space for the descriptors, labelled by (n, l) and
/// ordered lexicographically. `disp` should contain the relative Cartesian
/// coordinates of the surrouding atoms in the format `[x_1, y_1, z_1, ...]`.
/// `weights` should contain the weights used in the construction of the
/// neighbor density function (e.g., `[1., ...]`). `restrict` is not used to
/// help with portability.
///
/// # Parameters
/// - `desc`: pointer to an array to hold the result
/// - `disp`: pointer to an array of the relative displacements
/// - `weights`: pointer to an array of the atomic weights
/// - `rc`: cutoff radius for the environment
/// - `n_atom`: number of atoms in the environment
/// - `n_max`: defines the number of descriptors calculated
///
/// # Returns
/// A copy of `desc`
///
/// # Performance
/// The following preprocessor definitions (usually in `safety.h`) enable
/// various safety checks:
/// - `SAFE_MEMORY`: `desc`, `disp`, and `weights` are not `NULL`
/// - `SAFE_FINITE`: `rc` is nonnegative
///
/// # Warning
/// You are reponsible for ensuring that enough memory is allocated for the
/// relevant arrays, and should expect undefined behavior otherwise. The
/// lengths should be:
/// - `desc`: `(n_max + 1) * (n_max + 2) / 2`
/// - `disp`: at least `3 * n_atom`
/// - `weights`: at least `n_atom`
///
/// # Examples
/// ```
/// #include <stddef.h>     // size_t
/// #include <stdio.h>      // printf
/// #include <stdint.h>     // uint32_t
/// #include "sb_desc.h"    // sb_descriptors
///
/// /// An example where `sb_descriptors()` is used to calculate the spherical
/// /// Bessel descriptors for an atomic environment containing four atoms; the
/// /// first and second descriptors should be `0.031870` and `0.138078`.
/// int main(void) {
///   // Sets the number of descriptors returned.
///   uint32_t n_max = 4;
///
///   // Allocate memory for the result.
///   double desc[15] = { 0. };
///
///   // Number of atoms in the environment.
///   uint32_t n_atom = 4;
///
///   // Displacements to the surrounding atoms in Angstroms.
///   // [x_1, y_1, z_1, ...]
///   double disp[12] = {
///      1.3681827, -1.3103517, -1.3131874,
///     -1.5151760,  1.3360077, -1.3477119,
///     -1.3989598, -1.2973683,  1.3679189,
///      1.2279369,  1.3400378,  1.4797429
///   };
///
///   // Weights for the surrounding atoms.
///   double weights[4] = { 1., 1., 1., 1. };
///
///   // Cutoff radius in Angstroms.
///   double rc = 3.7711;
///
///   sb_descriptors(desc, disp, weights, rc, n_atom, n_max);
///
///   // Output the result
///   // Labelled by (n, l), ordered lexicographically
///   printf("SB descriptors:\n");
///   for (size_t a = 0; a < (n_max + 1) * (n_max + 2) / 2; ++a)
///     printf("%.6f\n", desc[a]);
///
///   printf("Completed successfully!\n");
/// }
/// ```
double * sb_descriptors(
    double * desc_arr,
    double * disp_arr,
    const double * weights_arr,
    const double rc,
    const uint32_t n_atom,
    const uint32_t n_max) {
#ifdef SAFE_MEMORY
  SB_CHK_ERR(!desc_arr, abort(), "sb_descriptors: desc cannot be NULL");
  SB_CHK_ERR(!disp_arr, abort(), "sb_descriptors: disp cannot be NULL");
  SB_CHK_ERR(!weights_arr, abort(), "sb_descriptors: weights cannot be NULL");
#endif
#ifdef SAFE_FINITE
  SB_CHK_ERR(rc < 0., abort(), "sb_descriptors: rc cannot be negative");
#endif
  // Check that n_max is within limit defined by tables
  SB_CHK_ERR(n_max > _u_n_max || n_max > _c1_n_max || n_max > _c2_n_max,
      abort(), "sb_descriptors: n_max above limit defined by lookup tables");

  // Convert raw pointers to sb_vec and sb_mat
  sb_mat * disp = malloc(sizeof(sb_mat));
  SB_CHK_ERR(!disp, abort(), "sb_descriptors: failed to allocate disp");

  disp->n_rows = 3;
  disp->n_cols = n_atom;
  disp->n_elem = 3 * n_atom;
  disp->data   = disp_arr;

  double * data1, * data2, * data3;
  size_t a, b;

  // Calculate radial coordinates
  sb_vec * radius = sb_vec_calloc(n_atom, 'r');
  data1 = disp->data;
  data2 = radius->data;
  for (a = 0; a < n_atom; ++a) {
    for (b = 0; b < 3; ++b) {
      *data2 += SB_SQR(data1[b]);
    }
    data1 += 3;
    data2 += 1;
  }
  sb_vec_sqrt(radius);

  // Normalize displacement vectors
  sb_mat_vdiv(disp, radius, 'c');
  sb_vec_smul(radius, 1. / rc);

  // Calculate angle cosines
  sb_mat * gamma = sb_mat_malloc(n_atom, n_atom);
  sb_mat_mm_mul(gamma, disp, disp, "tn");

  // Legendre polynomials
  sb_mat * * lp = malloc((n_max + 1) * sizeof(sb_mat *));
  SB_CHK_ERR(!lp, abort(), "sb_descriptors: failed to allocate lp");
  for (a = 0; a <= n_max; ++a) {
    lp[a] = sb_mat_malloc(n_atom, n_atom);
  }

  sb_mat_set_all(lp[0], 1.);
  sb_mat_memcpy(lp[1], gamma);
  for (a = 2; a <= n_max; ++a) { // l = a
    sb_mat_memcpy(lp[a], gamma);
    sb_mat_pmul(lp[a], lp[a - 1]);
    sb_mat_smul(lp[a], (2. * a - 1.) / (a - 1.));
    sb_mat_psub(lp[a], lp[a - 2]);
    sb_mat_smul(lp[a], (a - 1.) / a);
  }

  // Include multiplier here to simplify calculation below
  for (a = 0; a <= n_max; ++a) { // l = a
    sb_mat_smul(lp[a], (2. * a + 1.) / 12.566370614359172);
  }

  // Radial basis functions
  sb_mat * * gnl = malloc((n_max + 1) * sizeof(sb_mat *));
  SB_CHK_ERR(!gnl, abort(), "sb_descriptors: failed to allocate gnl");
  for (a = 0; a <= n_max; ++a) { // l = a
    gnl[a] = sb_mat_malloc(n_atom, n_max - a + 1);
    get_radial_basis(gnl[a], radius->data, a, n_atom, rc);
    // Scale by the weights
    for (b = 0; b < n_atom; ++b) {
      cblas_dscal(gnl[a]->n_cols, weights_arr[b], gnl[a]->data + b, n_atom);
    }
  }

  // radius can be used for workspace
  data1 = radius->data;
  for (a = 0; a <= n_max; ++a) { // l = a
    data2 = lp[a]->data;
    data3 = gnl[a]->data;
    for (b = a; b <= n_max; ++b) { // n = b
      cblas_dgemv(CblasColMajor, CblasNoTrans, n_atom, n_atom,
          1., data2, n_atom, data3 + (b - a) * n_atom, 1, 0., data1, 1);
      desc_arr[b * (b + 1) / 2 + a] = cblas_ddot(n_atom, data3 + (b - a) * n_atom, 1, data1, 1);
    }
  }

  // Free memory
  for (a = 0; a <= n_max; ++a) {
    SB_MAT_FREE_ALL(lp[a], gnl[a]);
  }
  SB_FREE_ALL(disp, lp, gnl);
  SB_VEC_FREE_ALL(radius);
  SB_MAT_FREE_ALL(gamma);

  return desc_arr;
}


/******************************************************************************
CosmoLike Configuration Space Covariances for Projected Galaxy 2-Point Statistics
https://github.com/CosmoLike/CosmoCov
by CosmoLike developers
******************************************************************************/

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
// #include "../class/include/class.h"

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

//void omega_a(double aa,double *om_m,double *om_v);
double omv_vareos(double a);
static inline double hoverh0(double a);
double growfac(double a);
//int func_for_growfac(double a,const double y[],double f[],void *params);
double Tsqr_EH_wiggle(double khoverMPC);
//double int_for_sigma_r_sqr(double k, void * args);
double sigma_r_sqr();
//double Delta_L_wiggle(double k);
//double Delta_lin_wiggle(double k,double a);
double p_lin(double k,double a);
//double int_sig_R_knl(double logk, void *args);
//double int_neff(double lnk, void *args);
//double int_cur(double lnk, void *args);
//void nonlin_scale(double amp, double *R_NL, double *neff, double *Curv);
//double Halofit(double k, double amp, double omm, double omv,double w_z, double R_NL, double neff,double Curv, double P_delta_Lin);
//void Delta_halofit(double **table_P_NL,double logkmin, double logkmax, double dk, double da);
//double Delta_NL_Halofit(double k_NL, double a); //k in h/Mpc
double Pdelta(double k_NL,double a); //k in coverH0 units

//double int_for_chi(double a,void * args);
double f_K(double chi);
double chi(double a);
double a_chi(double chi1);
// extern void emu(double *xstar, double *ystar, double *kstar);

//c%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//variable Omega_v
//c%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double omv_vareos(double a)
{
  return(cosmology.Omega_v*exp(-3.*((cosmology.w0+cosmology.wa+1.)*log(a)+cosmology.wa*(1.-a))));
}

//c%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//c evolution of omega matter and omega lamda with expansion factor

void omega_a(double aa,double *om_m,double *om_v)
{
  double a2,omega_curv;
  a2=aa*aa;
  omega_curv=1.0-cosmology.Omega_m- cosmology.Omega_v;
  *om_m=cosmology.Omega_m /(cosmology.Omega_m +aa*(omv_vareos(aa) *a2 +omega_curv));
  *om_v=omv_vareos(aa)*a2*aa/(cosmology.Omega_m+aa*(a2*omv_vareos(aa) +omega_curv));
}
//c%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//growth factor including Dark energy parameters w0, wa
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

//function for growfac (DGL)
int func_for_growfac(double a,const double y[],double f[],void *params)
{
  //double *p=(double *)params;
  if (a == 0) {
    printf("a=0 in function 'func_for_growfac'!\n");
    exit(1);
  }
  double aa=a*a;
  double omegam=cosmology.Omega_m/(aa*a);
  double omegav=omv_vareos(a);
  double hub = hoverh0(a);
  double one_plus_mg_mu = 1.;
  hub = hub*hub;
  f[0]=y[1];
  if(cosmology.MGmu != 0){
    one_plus_mg_mu += cosmology.MGmu*omegav/hub/cosmology.Omega_v;
  }
  f[1]=y[0]*3.*cosmology.Omega_m/(2.*hub*aa*aa*a)*one_plus_mg_mu-y[1]/a*(2.-(omegam+(3.*(cosmology.w0+cosmology.wa*(1.-a))+1)*omegav)/(2.*hub));
  return GSL_SUCCESS;
}

static inline double hoverh0(double a){
  return sqrt(cosmology.Omega_m /(a*a*a) + (1.-cosmology.Omega_m -cosmology.Omega_v )/(a*a) + omv_vareos(a) );
}

double growfac(double a)
{
  const double MINA=1.e-8;
  static cosmopara C;
  static double *ai;
  static double *table;
  double res;

  gsl_interp *intf=gsl_interp_alloc(gsl_interp_linear,Ntable.N_a);
  gsl_interp_accel *acc=gsl_interp_accel_alloc();

  if (recompute_expansion(C))
  {

    if(table!=0) free_double_vector(table,0, Ntable.N_a-1);
    if(ai!=0) free_double_vector(ai,0, Ntable.N_a-1);
    ai=create_double_vector(0, Ntable.N_a-1);
    table=create_double_vector(0, Ntable.N_a-1);

    int i;

    const gsl_odeiv_step_type *T=gsl_odeiv_step_rkf45;
    gsl_odeiv_step *s=gsl_odeiv_step_alloc(T,2);
    gsl_odeiv_control *c=gsl_odeiv_control_y_new(1.e-6,0.0);
    gsl_odeiv_evolve *e=gsl_odeiv_evolve_alloc(2);

    double t=MINA;            //start a
    double t1=1.1;                //final a
    double h=1.e-6;              //initial step size
    double y[2]={MINA,MINA};   //initial conditions
    double norm;
    double par[0]={};
    gsl_odeiv_system sys={func_for_growfac,NULL,2,&par};

    for (i=1;i<=Ntable.N_a;i++) {
      ai[i-1]=i*t1/(1.*Ntable.N_a);
      while(t<ai[i-1])
        gsl_odeiv_evolve_apply(e,c,s,&sys,&t,ai[i-1],&h,y);
      if (i==1) norm=y[0]/ai[i-1];
      table[i-1]=y[0]/norm;
    }

    gsl_odeiv_evolve_free(e);
    gsl_odeiv_control_free(c);
    gsl_odeiv_step_free(s);
    update_cosmopara(&C);

  }
  gsl_interp_init(intf,ai,table,Ntable.N_a);
  res=gsl_interp_eval(intf,ai,table,a,acc);
  gsl_interp_accel_free(acc);
  gsl_interp_free(intf);
  return(res);
}

// ---------------------------- Transfer Function from EH98 ----------------------
//Input: k -- Wavenumber at which to calculate transfer function, in Mpc^-1. Output: Returns the value of the full transfer function fitting formula. This is the form given in Section 3 of Eisenstein & Hu (1997). Notes: Units are Mpc, not h^-1 Mpc.

double Tsqr_EH_wiggle(double khoverMPC)
{
  static double omhh=-123.;
  static double obhh=-123.;
  static double OMEGA_V = -123.;
  static double f_baryon;

  static double k_equality;
  static double sound_horizon;
  static double beta_c;
  static double alpha_c;
  static double beta_node;
  static double alpha_b;
  static double beta_b;
  static double k_silk;

  //if (omhh != cosmology.Omega_m*cosmology.h0*cosmology.h0 || obhh != cosmology.omb*cosmology.h0*cosmology.h0|| OMEGA_V != cosmology.Omega_v){
  double theta_cmb,z_equality,z_drag,R_drag,R_equality;
  double z_drag_b1, z_drag_b2;
  double alpha_c_a1, alpha_c_a2, beta_c_b1, beta_c_b2, alpha_b_G, y;

  omhh = cosmology.Omega_m*cosmology.h0*cosmology.h0;
  obhh = cosmology.omb*cosmology.h0*cosmology.h0;
  OMEGA_V = cosmology.Omega_v;
  f_baryon = obhh/omhh;
    //printf("%le\n",f_baryon);

    theta_cmb=2.728/2.7;// Tcmb in units of 2.7 K
    z_equality= 2.50e4*omhh/POW4(theta_cmb);//Redshift of matter-radiation equality, really 1+z

    k_equality=0.0746*omhh/SQR(theta_cmb);//Scale of equality, in Mpc^-1
    z_drag_b1 = 0.313*pow(omhh,-0.419)*(1+0.607*pow(omhh,0.674));
    z_drag_b2 = 0.238*pow(omhh,0.223);
    z_drag=1291*pow(omhh,0.251)/(1+0.659*pow(omhh,0.828))*(1+z_drag_b1*pow(obhh,z_drag_b2));//Redshift of drag epoch
    R_drag=31.5*obhh/POW4(theta_cmb)*(1000/(1+z_drag));//Photon-baryon ratio at drag epoch

    R_equality= 31.5*obhh/POW4(theta_cmb)*(1000/z_equality);//Photon-baryon ratio at equality epoch
    sound_horizon=2./3./k_equality*sqrt(6./R_equality)*log((sqrt(1+R_drag)+sqrt(R_drag+R_equality))/(1+sqrt(R_equality)));//Sound horizon at drag epoch, in Mpc
    k_silk= 1.6*pow(obhh,0.52)*pow(omhh,0.73)*(1+pow(10.4*omhh,-0.95));//Silk damping scale, in Mpc^-1
    alpha_c_a1 = pow(46.9*omhh,0.670)*(1+pow(32.1*omhh,-0.532));
    alpha_c_a2 = pow(12.0*omhh,0.424)*(1+pow(45.0*omhh,-0.582));
    alpha_c=pow(alpha_c_a1,-f_baryon)*pow(alpha_c_a2,-CUBE(f_baryon)); //CDM suppression

    beta_c_b1 = 0.944/(1+pow(458*omhh,-0.708));
    beta_c_b2 = pow(0.395*omhh, -0.0266);
    beta_c=1.0/(1+beta_c_b1*(pow(1-f_baryon, beta_c_b2)-1));//CDM log shift

    y = z_equality/(1+z_drag);
    alpha_b_G = y*(-6.*sqrt(1+y)+(2.+3.*y)*log((sqrt(1+y)+1)/(sqrt(1+y)-1)));
    alpha_b=2.07*k_equality*sound_horizon*pow(1+R_drag,-0.75)*alpha_b_G;//Baryon suppression
    beta_node = 8.41*pow(omhh, 0.435);//Sound horizon shift
    beta_b= 0.5+f_baryon+(3.-2.*f_baryon)*sqrt(pow(17.2*omhh,2.0)+1);//Baryon envelope shift
  //}
  // Start of TFfit_onek from the original tf_fit.c at http://background.uchicago.edu/~whu/transfer/transferpage.html
    double T_c_ln_beta,T_c_ln_nobeta , T_c_C_alpha, T_c_C_noalpha;
    double q,qsqr, xx, xx_tilde;
    double T_c_f, T_c, s_tilde, T_b_T0, T_b;

  double k=khoverMPC*cosmology.h0; //internally this routine uses Mpc^-1 not h/Mpc
  q = k/13.41/k_equality;
  qsqr =SQR(q);
  xx = k*sound_horizon;

  T_c_ln_beta = log(2.718282+1.8*beta_c*q);
  T_c_ln_nobeta = log(2.718282+1.8*q);

  T_c_ln_beta = log(2.718282+1.8*beta_c*q);
  T_c_C_alpha = 14.2/alpha_c + 386.0/(1+69.9*pow(q,1.08));
  T_c_C_noalpha = 14.2 + 386.0/(1+69.9*pow(q,1.08));

  T_c_f = 1.0/(1.0+POW4(xx/5.4));
  T_c = T_c_f*T_c_ln_beta/(T_c_ln_beta+T_c_C_noalpha*qsqr) +(1-T_c_f)*T_c_ln_beta/(T_c_ln_beta+T_c_C_alpha*qsqr);

  s_tilde = sound_horizon*pow(1+CUBE(beta_node/xx),-1./3.);
  xx_tilde = k*s_tilde;

  T_b_T0 = T_c_ln_nobeta/(T_c_ln_nobeta+T_c_C_noalpha*qsqr);

  T_b = sin(xx_tilde)/(xx_tilde)*(T_b_T0/(1+SQR(xx/5.2))+alpha_b/(1+CUBE(beta_b/xx))*exp(-pow(k/k_silk,1.4)));

  return SQR(f_baryon*T_b + (1-f_baryon)*T_c);
}

//Calculate Normalization see Cosmology Notes 8.105
double int_for_sigma_r_sqr(double k, void * args)
{
  double kR, res, x;
  kR = k*8.; // r=8 Mpc/h
  x = (sin(kR) - kR*cos(kR))/(kR*kR*kR);
  res = pow(k,2.+cosmology.n_spec+ 0.5*cosmology.alpha_s*log(k/0.05))*Tsqr_EH_wiggle(k)*x*x;
  return res;
}

double sigma_r_sqr()
{
  static double res = -123.;
  static cosmopara C;
  double integral,array[1];

  if (recompute_Delta(C)) //strictly speaking, this is recomputed unnecessarily if only sigma_8 changes
  {
    integral = int_gsl_integrate_medium_precision(int_for_sigma_r_sqr,(void*)array,1e-4,1e6,NULL,512);
    res = 9.0*integral;   //see Peackock97, eq. 29
    update_cosmopara(&C);

  }
  if (!(res>0.0)){
    fprintf(stderr,"failed with sigma_r_sqr = %le\n", res);
  }
  assert(res>0.0);
  return res;
}

double Delta_L_wiggle(double k)
{
  static cosmopara C;

  static double *table_P;
  static double dk = .0, logkmin = .0, logkmax = .0;

  double klog,f1,norm;
  int i;

  if (k < limits.k_min_mpc || k > limits.k_max_mpc){
    norm=cosmology.sigma_8*cosmology.sigma_8/sigma_r_sqr();

    return norm*pow(k,cosmology.n_spec+ 0.5*cosmology.alpha_s*log(k/0.05)+3.0)*Tsqr_EH_wiggle(k);
    //printf("outside Delta_L_tab\n");
  }
  else{
    if (recompute_Delta(C))
    {
      if (cosmology.M_nu > 0){
        printf("Implementation of EH transfer function does not support massive neutrinos\n EXIT\n");
      }
      update_cosmopara(&C);
      norm=cosmology.sigma_8*cosmology.sigma_8/sigma_r_sqr();

      if(table_P!=0) free_double_vector(table_P,0, Ntable.N_k_lin-1);
      table_P=create_double_vector(0, Ntable.N_k_lin-1);

      logkmin = log(limits.k_min_mpc);
      logkmax = log(limits.k_max_mpc);
      dk = (logkmax - logkmin)/(Ntable.N_k_lin-1.);
      klog = logkmin;

      for (i=0; i<Ntable.N_k_lin; i++, klog += dk) {
       table_P[i]=log(norm*pow(exp(klog),cosmology.n_spec+ 0.5*cosmology.alpha_s*log(k/0.05)+3.0)*Tsqr_EH_wiggle(exp(klog)));
     }
      //printf("finished Delta_L_wiggle\n");
   }
 }
 klog=log(k);
 f1=interpol(table_P, Ntable.N_k_lin, logkmin, logkmax, dk,klog, 1.0,1.0 );
 return exp(f1);
}

// linear power spectrum routine with k in units H_0/c; used in covariances.c for beat coupling and in halo.c
double p_lin(double k,double a)
{
  static cosmopara C;
  static double **table_P_Lz = 0;
  static double logkmin = 0., logkmax = 0., dk = 0., da = 0.;
  int status;
  // if (strcmp(pdeltaparams.runmode,"CLASS")==0 || strcmp(pdeltaparams.runmode,"class")==0) return p_class(k,a,0, &status);

  double amp,ampsqr,grow0,aa,klog,val;

  int i,j;
  if (a >= 0.99999){a =0.99999;}
  if (recompute_cosmo3D(C)){
    update_cosmopara(&C);
    if (table_P_Lz!=0) free_double_matrix(table_P_Lz,0, Ntable.N_a-1, 0, Ntable.N_k_lin-1);
    table_P_Lz = create_double_matrix(0, Ntable.N_a-1, 0, Ntable.N_k_lin-1);
    grow0=growfac(1.);
    da = (1. - limits.a_min)/(Ntable.N_a-1.);
    aa = limits.a_min;
    for (i=0; i<Ntable.N_a; i++, aa +=da) {
      if(aa>1.0) aa=1.0;
      amp=growfac(aa)/grow0;
      ampsqr=amp*amp;

      logkmin = log(limits.k_min_mpc);
      logkmax = log(limits.k_max_mpc);
      dk = (logkmax - logkmin)/(Ntable.N_k_lin-1.);
      klog = logkmin;
      for (j=0; j<Ntable.N_k_lin; j++, klog += dk) {
        table_P_Lz[i][j] = log(ampsqr*Delta_L_wiggle(exp(klog)));
        //printf("%le %le",exp(klog),Delta_L_wiggle(exp(klog));
      }
    }
  }
  if (k/cosmology.coverH0 > exp(logkmax)) return 0.0;
  klog = log(k/cosmology.coverH0);
  val = interpol2d(table_P_Lz, Ntable.N_a, limits.a_min, 1., da, a, Ntable.N_k_lin, logkmin, logkmax, dk, klog, 3.0+cosmology.n_spec, 0.0);
  if(isnan(val) || (k==0)) return 0.0;
  return 2.0*constants.pi_sqr*exp(val)/k/k/k;
}

double int_sig_R_knl(double lnk, void *args) // tak12 A4
{
  double krsqr;

  double *params= (double *) args;
  double Rscale=params[0];
  //printf("Rscale %le k %le\n",Rscale,exp(lnk));
  krsqr= SQR(exp(lnk)*Rscale);
  return Delta_L_wiggle(exp(lnk))*exp(-krsqr);
}

double int_neff(double lnk, void *args) //tak12 A5
{
  double krsqr;
  double *params= (double *) args;
  double Rscale=params[0];
  krsqr= SQR(exp(lnk)*Rscale);
  return Delta_L_wiggle(exp(lnk))*2.0*krsqr*exp(-krsqr); //see S03 eq. 59
}

double int_cur(double lnk, void *args) //tak12 A5
{
  double krsqr;
  double *params= (double *) args;
  double Rscale=params[0];
  krsqr= SQR(exp(lnk)*Rscale);
  return Delta_L_wiggle(exp(lnk))*4.0*krsqr*(1.0-krsqr)*exp(-krsqr); // S03 eq.60
}

//iterative calculation of the nonlinear scale as defined in tak12 A4
void nonlin_scale(double amp, double *R_NL, double *neff, double *Curv)
{
  double sig_R,kmax,logkmax,sig_R_noamp,neffplus3;
  int iterstep;
  const int itermax  = 40;
  int converged=0;
  double array[1];
  double logRmin = -3.0;
  double logRmax =  4.0;

  iterstep=0;
  while(converged==0)
  {
    array[0]=pow(10.,(logRmin+logRmax)/2.0);

    //flexible upper limit of integration depending on drop-off of filter function
    kmax  = sqrt(5.*log(10.))/array[0];
    if (kmax<8000.0) logkmax = log(8000.0);
    sig_R_noamp=sqrt(int_gsl_integrate_medium_precision(int_sig_R_knl,(void*)array,-4.5,logkmax,NULL,512)); //integral goes over ln k exponent correspond to k_min~0.011

    sig_R=amp*sig_R_noamp;
    if (sig_R>1.0)  logRmin=log10(array[0]);
    if (sig_R<1.0)  logRmax=log10(array[0]);
    iterstep=iterstep+1;
    if(fabs(sig_R-1.0) < 0.0001 || iterstep>itermax) converged=1;
  }
  *R_NL=array[0]; //R where sig_R==1
  neffplus3=int_gsl_integrate_medium_precision(int_neff,(void*)array,-4.5,logkmax,NULL,512)/sig_R_noamp/sig_R_noamp;
  *neff= neffplus3 - 3.0;
  *Curv= int_gsl_integrate_medium_precision(int_cur,(void*)array,-4.5,logkmax,NULL,512)/sig_R_noamp/sig_R_noamp + SQR(neffplus3);

  //printf("%d %le\n",iterstep,amp);
}

double Halofit(double k, double amp, double omm, double omv,double w_z, double R_NL, double neff,double Curv, double P_delta_Lin)
{
  double y_scale,n2eff,n3eff,n4eff;
  double a_n,b_n,c_n,gamma_n,alpha_n,beta_n,nu_n,f1,f2,f3;

  double Delta_H,Delta_H_Prime,Delta_Q;
  //determine nonlinear scale, neff and curvature, see tak12 A4, A5
  y_scale=k*R_NL;

  n2eff=neff*neff;
  n3eff=n2eff*neff;
  n4eff=n2eff*n2eff;

  //calculate coefficients
  a_n = pow(10.,1.5222+2.8553*neff + 2.3706*n2eff+0.9903*n3eff+0.2250*n4eff-0.6038*Curv+0.1749*omv*(1.0+w_z));
  b_n = pow(10., -0.5642+0.5864*neff + 0.5716*n2eff-1.5474*Curv +0.2279*omv*(1.0+w_z));
  c_n = pow(10., 0.3698+ 2.0404*neff + 0.8161*n2eff+0.5869*Curv);
  gamma_n = 0.1971-0.0843*neff + 0.8460*Curv;
  alpha_n = fabs(6.0835 + 1.3373*neff - 0.1959*n2eff - 5.5274*Curv);
  beta_n = 2.0379 - 0.7354*neff + 0.3157*n2eff + 1.2490*n3eff + 0.3980*n4eff - 0.1682*Curv;
  nu_n = pow(10,5.2105+3.6902*neff);

  f1 = pow(omm,(-0.0307));
  f2 = pow(omm,(-0.0585));
  f3 = pow(omm,(0.0743));

  //TwoHaloTerm
  Delta_Q=P_delta_Lin*(pow((1.0+P_delta_Lin),beta_n)/(1.0+alpha_n*P_delta_Lin))*exp(-(y_scale/4.0+y_scale*y_scale/8.0));
  //OneHaloterm
  Delta_H_Prime=(a_n*pow(y_scale,3.0*f1))/(1.0+b_n*pow(y_scale,f2)+pow(c_n*f3*y_scale,3.0-gamma_n));
  Delta_H=Delta_H_Prime/(1.0+nu_n*pow(y_scale,-2.0)); // using mu=0.0 Tak A12
  //printf("Delta_Q %le Delta_H %le\n",Delta_Q,Delta_H);
  return Delta_H+Delta_Q;
}

void Delta_halofit(double **table_P_NL,double logkmin, double logkmax, double dk, double da)
{
  double rk,omm,omv,w_z,amp,grow0,aa,klog;
  double R_NL,Curv,neff,P_delta,P_delta_Lin;
  int i,j;

  grow0=growfac(1.);
  aa = limits.a_min;
  //binning in k and a must be the same as in emu
  for (i=0; i<Ntable.N_a; i++, aa +=da) {
    if(aa>1.0) aa=1.0;
    omega_a(aa,&omm,&omv);
    w_z=cosmology.w0+cosmology.wa*(1.-aa);
    amp=growfac(aa)/grow0;
    nonlin_scale(amp, &R_NL, &neff, &Curv);
    //printf("%le %le %le %le\n",aa,R_NL,neff,Curv);
    klog = logkmin;
    for (j=0; j<Ntable.N_k_nlin; j++, klog += dk) {
      rk=exp(klog);
      P_delta_Lin=amp*amp*Delta_L_wiggle(rk);
      P_delta=Halofit(rk, amp, omm, omv, w_z, R_NL, neff, Curv, P_delta_Lin);
      table_P_NL[i][j]=log(P_delta);
    }
  }
}

double Delta_NL_Halofit(double k_NL, double a)
{
  static cosmopara C;
  static double logkmin = 0., logkmax = 0., dk = 0., da = 0.;

  static double **table_P_NL=0;
  double klog,val;

  if (recompute_cosmo3D(C)){
    update_cosmopara(&C);
    if (table_P_NL!=0) free_double_matrix(table_P_NL,0, Ntable.N_a-1, 0, Ntable.N_k_nlin-1);
    table_P_NL = create_double_matrix(0, Ntable.N_a-1, 0, Ntable.N_k_nlin-1);

    da = (1. - limits.a_min)/(Ntable.N_a-1.);
    logkmin = log(limits.k_min_mpc);
    logkmax = log(limits.k_max_mpc);
    dk = (logkmax - logkmin)/(Ntable.N_k_nlin-1.);

    Delta_halofit(table_P_NL,logkmin, logkmax, dk, da);
  }
  klog = log(k_NL);
  val = interpol2d(table_P_NL, Ntable.N_a, limits.a_min, 1., da, a, Ntable.N_k_nlin, logkmin, logkmax, dk, klog, cosmology.n_spec, 0.0);
  return exp(val);
  // returns the dimensionless power spectrum as a function of scale factor a and k
}

double nonlinear_scale_computation(double a)
{
  static cosmopara C;
  static double da = 0.;
  static double *table=0;

  double omm,omv,amp,grow0,aa,res;
  double R_NL,Curv,neff;
  int i;

  if (recompute_cosmo3D(C)){
    update_cosmopara(&C);
    grow0=growfac(1.);
    da = (1.-limits.a_min)/(Ntable.N_a-1.);
    aa = limits.a_min;
    if (table!=0) free_double_vector(table, 0, Ntable.N_a-1);
    table   = create_double_vector(0, Ntable.N_a-1);
    for (i=0; i<Ntable.N_a; i++, aa+=da) {
      if(aa>1.0) aa=1.0;
      omega_a(aa,&omm,&omv);
      amp=growfac(aa)/grow0;
      nonlin_scale(amp, &R_NL, &neff, &Curv);
      table[i] = 1./R_NL;
    }
  }
  res = interpol(table, Ntable.N_a, limits.a_min, 1., da, a, 0.0, 0.0);
  return res;
}

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
//Pdelta is called with k in units H0/c since the comoving distance chi is in units c/H0. Upstream Pdelta all routines are in h/mpc
double Pdelta(double k_NL,double a)
{
  static int P_type = -1;
  if (P_type == -1){
    if (strcmp(pdeltaparams.runmode,"Halofit")==0) P_type = 0;
    if (strcmp(pdeltaparams.runmode,"halofit")==0) P_type = 0;
    // if (strcmp(pdeltaparams.runmode,"emu")==0) P_type = 1;
    // if (strcmp(pdeltaparams.runmode,"emu_only")==0) P_type = 2;
    if (strcmp(pdeltaparams.runmode,"linear")==0) P_type = 3;
    // if (strcmp(pdeltaparams.runmode,"CLASS")==0) P_type = 4;
    // if (strcmp(pdeltaparams.runmode,"class")==0) P_type = 4;
    if (strcmp(pdeltaparams.runmode,"cosmo_sim_test") ==0) P_type = 5;

  }

  //printf("%s set\n",pdeltaparams.runmode);
  double pdelta = 0.,kintern=k_NL/cosmology.coverH0,error,k_nonlin,res;
  int status;
  switch (P_type){
    case 0: pdelta=2.0*constants.pi_sqr*Delta_NL_Halofit(kintern,a)/k_NL/k_NL/k_NL; break;
    // case 1: pdelta=2.0*constants.pi_sqr*Delta_NL_emu(kintern,a)/k_NL/k_NL/k_NL; break;
    // case 2: pdelta=2.0*constants.pi_sqr*Delta_NL_emu_only(kintern,a)/k_NL/k_NL/k_NL; break;
    case 3: pdelta=p_lin(k_NL,a); break;
    // case 4: pdelta=p_class(k_NL,a,1, &status); break;
    case 5: k_nonlin=nonlinear_scale_computation(a);
    if (kintern<0.01) pdelta=2.0*constants.pi_sqr*Delta_NL_Halofit(kintern,a)/k_NL/k_NL/k_NL;
    else{
      error=0.01*pow((pdeltaparams.DIFF_A*kintern/k_nonlin),pdeltaparams.DIFF_n);
      pdelta=2.0*constants.pi_sqr*Delta_NL_Halofit(kintern,a)*(1.0+error)/k_NL/k_NL/k_NL;
    }
    break;
    default:
    printf("cosmo3D:Pdelta: %s Pdelta runmode not defined\n",pdeltaparams.runmode);
    printf("using Halofit (standard)\n");
    pdelta=2.0*constants.pi_sqr*Delta_NL_Halofit(kintern,a)/k_NL/k_NL/k_NL;
    break;
  }

//      double z = 1./a -1 ;
//	  if (z > 4.) {
//		 printf("	z:%lf    a:%lf   	k_Mpc:%lf    PkRatio:%lf \n",z,a,kintern,PkRatio_baryons(kintern, a));
//	  }

  // if (bary.isPkbary==1) pdelta = pdelta*PkRatio_baryons(kintern, a);
  return pdelta;
}

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/*============================================================
 *see BS 2.41 bzw Logbook for detailed calculation of chi from a.*/

double int_for_chi(double a, void * args){
  //double res,asqr;
  //asqr=a*a;
  //res= 1./sqrt(a*cosmology.Omega_m + asqr*(1.-cosmology.Omega_m -cosmology.Omega_v ) + asqr*asqr*omv_vareos(a));
  //return res;
  return 1./(a*a*hoverh0(a)); //changed to call of hoverh0 to be ready for other parametrizations
}

/*for the calculation of chi we have to integrate from a(z2)=a up to a(z1)=1, which means todays expansion factor*/
double chi(double a)
{
  static cosmopara C;
  static double *table;
  static double da = 0.;
  double aa,res;
  int i;
  double array[1];

  if (recompute_expansion(C)){
    update_cosmopara(&C);
    da = (1.-limits.a_min)/(Ntable.N_a-1.);
    aa = limits.a_min;
    if (table!=0) free_double_vector(table, 0, Ntable.N_a-1);
    table   = create_double_vector(0, Ntable.N_a-1);
    for (i=0; i<Ntable.N_a-1; i++, aa+=da) {
      table[i] = int_gsl_integrate_medium_precision(int_for_chi,(void*)array, aa, 1.,NULL,1000);
    }
    table[Ntable.N_a-1] =.0;
  }
  res = interpol(table, Ntable.N_a, limits.a_min, 1., da, a, 0.0, 0.0); // comoving distance in c/H_0
  if (res < 0){printf ("interpolation error in chi(%e)\n",a); res=0.01;}
  return res;
}
//auxilary function to look up a(chi)
double a_chi(double chi1){
  static gsl_spline * a_spline = NULL;
  static gsl_interp_accel * a_accel = NULL;
  static cosmopara C;
  static double chi_max =-1.;
  if (!a_spline){
    a_spline = gsl_spline_alloc(gsl_interp_cspline, Ntable.N_a);
    a_accel = gsl_interp_accel_alloc();
  }
  if (recompute_cosmo3D(C)){
    update_cosmopara(&C);
    double *table_a,*table_chi;
    table_a  = create_double_vector(0, Ntable.N_a-1);
    table_chi  = create_double_vector(0, Ntable.N_a-1);
    for (int i = 0; i < Ntable.N_a; i++){
      table_a[i] = 1.0 - (1.0 - 0.99*limits.a_min)/(Ntable.N_a-1.)*(double)i;
      table_chi[i] = int_gsl_integrate_medium_precision(int_for_chi,NULL, table_a[i], 1.,NULL,1000);
   //   printf("%d %e %e\n",i,table_a[i],table_chi[i]);
    }
    chi_max = int_gsl_integrate_medium_precision(int_for_chi,NULL, limits.a_min, 1.,NULL,1000);
    gsl_spline_init(a_spline, table_chi, table_a, Ntable.N_a);
    free_double_vector(table_a,0, Ntable.N_a-1);
    free_double_vector(table_chi,0, Ntable.N_a-1);
  }
  if (chi1 <=0.0){return 1.0;}
  if (chi1 > chi_max){printf("called a_chi(chi) with chi > chi(limits.a_min\nEXIT\n");exit(1);}
  return gsl_spline_eval(a_spline,chi1,a_accel);
}

/*===============================calculating the angular diameter distance f_K BS01 2.4, 2.30: f_K is a radial function that, depending on the curvature of the Universe, is a trigonometric, linear, or hyperbolic function of chi  */
double f_K(double chi)
{
  double K, K_h, f;
  K = (cosmology.Omega_m   + cosmology.Omega_v  - 1.);
  if (K > precision.medium) {           /* open */
    K_h = sqrt(K); // K in units H0/c see BS eq. 2.30
    f = 1./K_h*sin(K_h*chi);
    //printf("open\n");
  } else if (K < -precision.medium) {   /* closed */
    K_h = sqrt(-K);
    f = 1./K_h*sinh(K_h*chi);
    //printf("closed K=%le %le %le\n",K,cosmology.Omega_m,cosmology.Omega_v);
  } else {                     /* flat */
    f = chi;
    //printf("flatK=%le %le %le\n",K,cosmology.Omega_m,cosmology.Omega_v);
  }
  return f;
}


#ifndef _DRAGONCELLO_H
#define _DRAGONCELLO_H

#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <vector>

#include "constants.h"

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

using namespace std;
using namespace DRAGON;

struct diagEntry {
  double upper;
  double central;
  double lower;
};

struct norms {
    double max;
    double mean;
    double norm2;
};

class DRAGONCELLO {

public:

  DRAGONCELLO(int,int);
  ~DRAGONCELLO() { cout << "I am the destructor " << endl; }

  void buildXGrid();
  void buildYGrid();
  void buildZGrid();
  void buildPGrid();
  void initEmptyGrids();
  void initGrids();
  void initSource();
  void initBfield();
  void initAnisoSpatialDiffusion();
  void initDriftLikeVelocities();
  void initEloss();

  void dumpSpectra();
  void dumpSpectra_2(int counter);
  void dumpProfiles();
  void dumpProfiles2D();

  void compute_epsilon(int counter);

  void computeResidualLosses(int);
  void computeResidualDiffAniso(int);

  void computeSourceEnergyIntegral(int, int, int);
  void computeSolutionEnergyIntegral(int, int, int);

  void initCN2Daniso();

  void Run2D();

  void propagateInZaniso(double dt, double dtbar, int counter);
  void propagateInRaniso(double dt, double dtbar, int counter);
  void propagateMixedDerivatives(double dt, double dtbar, int counter);
  void propagateEloss(double dt, double dtbar);

  void printLocalFlux();
  void dumpSolution(int counter);

  inline long int index(int ix, int iy, int iz, int ip) {
    return  (((ix * ny + iy) * nz + iz) * np + ip);
  }

  inline long int indexSpatial(int ix, int iy, int iz) {
    return  (((ix * ny + iy) * nz + iz));
  }

  inline double computeVelocity(double momentum) {
	  return c_light*sqrt( 1. - mass*mass*c_light*c_light/(momentum*momentum)  );
  }

  inline vector<double> getP() {
	  return p_vec;
  }

  void solveTridiagonalSystem(vector<double>& a, vector<double>& b, vector<double>& c, vector<double>& r, vector<double>& u, int n);
  int gsl_linalg_solve_tridiag(const vector<double> & diag, const vector<double> & abovediag, const vector<double> & belowdiag,
                                const vector<double> & rhs, vector<double> & x);
  int solve_tridiag_nonsym(const vector<double> & diag, const vector<double> & abovediag, const vector<double> & belowdiag,
                            const vector<double> & rhs, vector<double> & x, size_t N);

private:

  int Z, A;
  int species_id;

  double deltax, deltay, deltaz, deltaLogp;
  vector<double> x_vec, y_vec, z_vec, p_vec;
  vector<double> deltaxUp, deltayUp, deltazUp;
  vector<double> deltaxDown, deltayDown, deltazDown;
  vector<double> deltaxCentral, deltayCentral, deltazCentral;

  unsigned int ixsun, iysun, izsun, ipLocal;

  vector<double> N;          //(r,z,p) or (x,y,z,p)
  vector<double> N_previous; //(r,z,p) or (x,y,z,p)

  double sourceEnergyIntegral;
  double totalInjectedEnergy;
  double solutionEnergyIntegral;
  double reaccelerationPower;
  double totalReaccelerationEnergy;

  vector<double> analyticalSolution;

  vector<double> sourceTerm;

  vector<double> b_r;  // (r,z) or (r,phi,z); b_r = B_r/|B| magnetic field unit vector projected along r
  vector<double> b_phi;  // (r,z) or (r,phi,z);
  vector<double> b_x;  // b_x = B_x/|B| magnetic field unit vector projected along x
  vector<double> b_y;  // b_y = B_y/|B|
  vector<double> b_z;  // same as before, but along z

  vector<double> diffusionCoefficient;      //(r,z,p) or (x,y,z,p)
  vector<double> diffusionCoefficient_xx;      //(r,z,p) or (x,y,z,p)
  vector<double> diffusionCoefficient_zz;      //(r,z,p) or (x,y,z,p)

  vector<double> Dpara;  //(r,z,p) or (x,y,z,p)
  vector<double> Dperp;  //(r,z,p) or (x,y,z,p)

  vector<double> D_rr;  //(r,z,p) or (r,phi,z,p)
  vector<double> D_rz;  //(r,z,p) or (r,phi,z,p)
  vector<double> D_xx;  //(x,z,p) or (x,y,z,p)
  vector<double> D_xz;  //(x,z,p) or (x,y,z,p)
  vector<double> D_zz;  //(r,z,p) or (x,z,p) or ...
  vector<double> u_r;  //(r,z,p) or (x,y,z,p)
  vector<double> u_x;  //(x,z,p) or (x,y,z,p)
  vector<double> u_z;  //(r,z,p) or (x,z,p) or ...

  vector<double> reaccelerationCoefficient;      //(r,z,p) or (x,y,z,p)
  vector<double> energyLossTerm;      //(r,z,p) or (x,y,z,p)

  vector<double> upperDiagonalZ;
  vector<double> lowerDiagonalZ;
  vector<double> centralDiagonalZ;

  vector<double> upperDiagonalX;
  vector<double> lowerDiagonalX;
  vector<double> centralDiagonalX;

  vector<double> upperDiagonalR;
  vector<double> lowerDiagonalR;
  vector<double> centralDiagonalR;

  vector<double> upperDiagonalDpp;
  vector<double> lowerDiagonalDpp;
  vector<double> centralDiagonalDpp;

  vector<double> upperDiagonalEloss;
  vector<double> lowerDiagonalEloss;
  vector<double> centralDiagonalEloss;
};

#endif