/* movstat/mvacc.c
 *
 * Moving window mean/variance accumulator - based on a modification
 * to Welford's algorithm, discussed here:
 *
 * https://stackoverflow.com/a/6664212
 *
 * Copyright (C) 2018 Patrick Alken
 *
 * 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 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_movstat.h>

typedef double ringbuf_type_t;

#include "ringbuf.c"

typedef struct
{
  size_t n;      /* window size */
  size_t k;      /* number of samples currently in window */
  double mean;   /* current window mean */
  double M2;     /* current window M2 */
  ringbuf *rbuf; /* ring buffer storing current window */
} mvacc_state_t;

static size_t
mvacc_size(const size_t n)
{
  size_t size = 0;

  size += sizeof(mvacc_state_t);
  size += ringbuf_size(n);

  return size;
}

static int
mvacc_init(const size_t n, void * vstate)
{
  mvacc_state_t * state = (mvacc_state_t *) vstate;

  state->n = n;
  state->k = 0;
  state->mean = 0.0;
  state->M2 = 0.0;

  state->rbuf = (ringbuf *) ((unsigned char *) vstate + sizeof(mvacc_state_t));
  ringbuf_init(n, state->rbuf);

  return GSL_SUCCESS;
}

static int
mvacc_insert(const double x, void * vstate)
{
  mvacc_state_t * state = (mvacc_state_t *) vstate;

  if (ringbuf_is_full(state->rbuf))
    {
      /* remove oldest window element and add new one */
      double old = ringbuf_peek_back(state->rbuf);
      double prev_mean = state->mean;

      state->mean += (x - old) / (double) state->n;
      state->M2 += ((old - prev_mean) + (x - state->mean)) * (x - old);
    }
  else
    {
      double delta = x - state->mean;

      /*
       * Welford algorithm:
       *
       * mu_new = mu_old + (x - mu_old) / n
       * M2_new = M2_old + (x - mu_old) * (x - mu_new)
       */

      ++(state->k);
      state->mean += delta / (double) state->k;
      state->M2 += delta * (x - state->mean);
    }

  /* add new element to ring buffer */
  ringbuf_insert(x, state->rbuf);

  return GSL_SUCCESS;
}

static int
mvacc_delete(void * vstate)
{
  mvacc_state_t * state = (mvacc_state_t *) vstate;

  if (!ringbuf_is_empty(state->rbuf))
    {
      if (state->k > 1)
        {
          /*
           * mu_new = mu_old + (mu_old - x_old) / (n - 1)
           * M2_new = M2_old - (mu_old - x_old) * (mu_new - x_old)
           */

          double old = ringbuf_peek_back(state->rbuf);
          double prev_mean = state->mean;
          double delta = prev_mean - old;

          state->mean += delta / (state->k - 1.0);
          state->M2 -= delta * (state->mean - old);
        }
      else if (state->k == 1)
        {
          state->mean = 0.0;
          state->M2 = 0.0;
        }

      ringbuf_pop_back(state->rbuf);
      --(state->k);
    }

  return GSL_SUCCESS;
}

static int
mvacc_mean(void * params, double * result, const void * vstate)
{
  const mvacc_state_t * state = (const mvacc_state_t *) vstate;
  (void) params;
  *result = state->mean;
  return GSL_SUCCESS;
}

static int
mvacc_variance(void * params, double * result, const void * vstate)
{
  const mvacc_state_t * state = (const mvacc_state_t *) vstate;

  (void) params;

  if (state->k < 2)
    *result = 0.0;
  else
    *result = state->M2 / (state->k - 1.0);

  return GSL_SUCCESS;
}

static int
mvacc_sd(void * params, double * result, const void * vstate)
{
  double variance;
  int status = mvacc_variance(params, &variance, vstate);
  *result = sqrt(variance);
  return status;
}

static const gsl_movstat_accum mean_accum_type =
{
  mvacc_size,
  mvacc_init,
  mvacc_insert,
  mvacc_delete,
  mvacc_mean
};

const gsl_movstat_accum *gsl_movstat_accum_mean = &mean_accum_type;

static const gsl_movstat_accum variance_accum_type =
{
  mvacc_size,
  mvacc_init,
  mvacc_insert,
  mvacc_delete,
  mvacc_variance
};

const gsl_movstat_accum *gsl_movstat_accum_variance = &variance_accum_type;

static const gsl_movstat_accum sd_accum_type =
{
  mvacc_size,
  mvacc_init,
  mvacc_insert,
  mvacc_delete,
  mvacc_sd
};

const gsl_movstat_accum *gsl_movstat_accum_sd = &sd_accum_type;


//This computes "the" IDCT (inverse discrete cosine transformation) along dim of matrix X.
//This uses a CBLAS matrix multiplication by the DCT-III matrix.

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

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

#ifndef M_SQRT1_2
    #define M_SQRT1_2 0.707106781186547524401
#endif

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

int idct_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int idct_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int idct_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);
int idct_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc);

int idct_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idct_cblas_s: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in idct_cblas_s: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const float xsc = (sc) ? (float)(2.0*M_SQRT1_2) : 1.0f;
        const float ysc = (sc) ? 2.0f/sqrtf((float)(2u*ndct)) : 2.0f/(float)(2u*ndct);
        const float dcsc = 0.5f * xsc * ysc;

        //Initialize DCT-III matrix
        const size_t LN = Lx * ndct;
        const float P_N = (float)(M_PI/(double)ndct);
        float *DCT, x0;
        DCT = (float *)aligned_alloc(sizeof(float),LN*sizeof(float));
        if (!DCT) { fprintf(stderr,"error in idct_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<ndct; ++n)
        {
            *DCT++ = xsc * ysc;
            for (size_t l=1u; l<Lx; ++l, ++DCT)
            {
                *DCT = ysc * cosf(P_N*(0.5f+(float)n)*(float)l);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            x0 = dcsc * *X;
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,1,0.0f,Y,1);
            for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=V; v>0u; --v, X+=Lx)
                {
                    x0 = dcsc * *X;
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,1,0.0f,Y,1);
                    for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=B*(Lx-1u), Y+=B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X, Y-=K*ndct-1u)
                    {
                        x0 = dcsc * *X;
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,(int)K,0.0f,Y,(int)K);
                        for (size_t n=ndct; n>0u; --n, Y+=K) { *Y -= x0; }
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int idct_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idct_cblas_d: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in idct_cblas_d: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
     else
    {
        //Scaling
        const double xsc = (sc) ? 2.0*M_SQRT1_2 : 1.0;
        const double ysc = (sc) ? 2.0/sqrt((double)(2u*ndct)) : 2.0/(double)(2u*ndct);
        const double dcsc = 0.5 * xsc * ysc;

        //Initialize DCT-III matrix
        const size_t LN = Lx * ndct;
        const double P_N = M_PI/(double)ndct;
        double *DCT, x0;
        DCT = (double *)aligned_alloc(sizeof(double),LN*sizeof(double));
        if (!DCT) { fprintf(stderr,"error in idct_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<ndct; ++n)
        {
            *DCT++ = xsc * ysc;
            for (size_t l=1u; l<Lx; ++l, ++DCT)
            {
                *DCT = ysc * cos(P_N*(0.5+(double)n)*(double)l);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            x0 = dcsc * *X;
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,1,0.0,Y,1);
            for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=V; v>0u; --v, X+=Lx)
                {
                    x0 = dcsc * *X;
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,1,0.0,Y,1);
                    for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=B*(Lx-1u), Y+=B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, ++X, Y-=K*ndct-1u)
                    {
                        x0 = dcsc * *X;
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,(int)K,0.0,Y,(int)K);
                        for (size_t n=ndct; n>0u; --n, Y+=K) { *Y -= x0; }
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int idct_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idct_cblas_c: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in idct_cblas_c: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const float xsc = (sc) ? (float)(2.0*M_SQRT1_2) : 1.0f;
        const float ysc = (sc) ? 2.0f/sqrtf((float)(2u*ndct)) : 2.0f/(float)(2u*ndct);
        const float dcsc = 0.5f * xsc * ysc;

        //Initialize DCT-III matrix
        const size_t LN = Lx * ndct;
        const float P_N = (float)(M_PI/(double)ndct);
        float *DCT, x0r, x0i;
        DCT = (float *)aligned_alloc(sizeof(float),LN*sizeof(float));
        if (!DCT) { fprintf(stderr,"error in idct_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<ndct; ++n)
        {
            *DCT++ = xsc * ysc;
            for (size_t l=1u; l<Lx; ++l, ++DCT)
            {
                *DCT = ysc * cosf(P_N*(0.5f+(float)n)*(float)l);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            x0r = dcsc * *X; x0i = dcsc * *(X+1);
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2,0.0f,Y,2);
            cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X+1,2,0.0f,Y+1,2);
            for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0r; *++Y -= x0i; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, X+=2u*Lx)
                {
                    x0r = dcsc * *X; x0i = dcsc * *(X+1);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2,0.0f,Y,2);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X+1,2,0.0f,Y+1,2);
                    for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0r; *++Y -= x0i; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2u, Y-=2u*K*ndct-2u)
                    {
                        x0r = dcsc * *X; x0i = dcsc * *(X+1);
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X,2*(int)K,0.0f,Y,2*(int)K);
                        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0f,DCT,(int)Lx,X+1,2*(int)K,0.0f,Y+1,2*(int)K);
                        for (size_t n=ndct; n>0u; --n, Y+=2u*K) { *Y -= x0r; *(Y+1) -= x0i; }
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

int idct_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndct, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idct_cblas_z: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndct<Lx) { fprintf(stderr,"error in idct_cblas_z: ndct must be >= Lx (length of vecs in X)\n"); return 1; }

    if (ndct==0u || N==0u) {}
    else if (ndct==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Scaling
        const double xsc = (sc) ? 2.0*M_SQRT1_2 : 1.0;
        const double ysc = (sc) ? 2.0/sqrt((double)(2u*ndct)) : 2.0/(double)(2u*ndct);
        const double dcsc = 0.5 * xsc * ysc;

        //Initialize DCT-III matrix
        const size_t LN = Lx * ndct;
        const double P_N = M_PI/(double)ndct;
        double *DCT, x0r, x0i;
        DCT = (double *)aligned_alloc(sizeof(double),LN*sizeof(double));
        if (!DCT) { fprintf(stderr,"error in idct_cblas_z: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<ndct; ++n)
        {
            *DCT++ = xsc * ysc;
            for (size_t l=1u; l<Lx; ++l, ++DCT)
            {
                *DCT = ysc * cos(P_N*(0.5+(double)n)*(double)l);
            }
        }
        DCT -= LN;

        if (Lx==N)
        {
            x0r = dcsc * *X; x0i = dcsc * *(X+1);
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2,0.0,Y,2);
            cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X+1,2,0.0,Y+1,2);
            for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0r; *++Y -= x0i; }
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, X+=2u*Lx)
                {
                    x0r = dcsc * *X; x0i = dcsc * *(X+1);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2,0.0,Y,2);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X+1,2,0.0,Y+1,2);
                    for (size_t n=ndct; n>0u; --n, ++Y) { *Y -= x0r; *++Y -= x0i; }
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndct-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2u, Y-=2u*K*ndct-2u)
                    {
                        x0r = dcsc * *X; x0i = dcsc * *(X+1);
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X,2*(int)K,0.0,Y,2*(int)K);
                        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)ndct,(int)Lx,1.0,DCT,(int)Lx,X+1,2*(int)K,0.0,Y+1,2*(int)K);
                        for (size_t n=ndct; n>0u; --n, Y+=2u*K) { *Y -= x0r; *(Y+1) -= x0i; }
                    }
                }
            }
        }
        free(DCT);
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* specfunc/beta_inc.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */
/* Modified for cdfs by Brian Gough, June 2003 */

#include <gsl/gsl_sf_gamma.h>

static double
beta_cont_frac (const double a, const double b, const double x,
                const double epsabs)
{
  const unsigned int max_iter = 512;    /* control iterations      */
  const double cutoff = 2.0 * GSL_DBL_MIN;      /* control the zero cutoff */
  unsigned int iter_count = 0;
  double cf;

  /* standard initialization for continued fraction */
  double num_term = 1.0;
  double den_term = 1.0 - (a + b) * x / (a + 1.0);

  if (fabs (den_term) < cutoff)
    den_term = GSL_NAN;

  den_term = 1.0 / den_term;
  cf = den_term;

  while (iter_count < max_iter)
    {
      const int k = iter_count + 1;
      double coeff = k * (b - k) * x / (((a - 1.0) + 2 * k) * (a + 2 * k));
      double delta_frac;

      /* first step */
      den_term = 1.0 + coeff * den_term;
      num_term = 1.0 + coeff / num_term;

      if (fabs (den_term) < cutoff)
        den_term = GSL_NAN;

      if (fabs (num_term) < cutoff)
        num_term = GSL_NAN;

      den_term = 1.0 / den_term;

      delta_frac = den_term * num_term;
      cf *= delta_frac;

      coeff = -(a + k) * (a + b + k) * x / ((a + 2 * k) * (a + 2 * k + 1.0));

      /* second step */
      den_term = 1.0 + coeff * den_term;
      num_term = 1.0 + coeff / num_term;

      if (fabs (den_term) < cutoff)
        den_term = GSL_NAN;

      if (fabs (num_term) < cutoff)
        num_term = GSL_NAN;

      den_term = 1.0 / den_term;

      delta_frac = den_term * num_term;
      cf *= delta_frac;

      if (fabs (delta_frac - 1.0) < 2.0 * GSL_DBL_EPSILON)
        break;

      if (cf * fabs (delta_frac - 1.0) < epsabs)
        break;

      ++iter_count;
    }

  if (iter_count >= max_iter)
    return GSL_NAN;

  return cf;
}

/* The function beta_inc_AXPY(A,Y,a,b,x) computes A * beta_inc(a,b,x)
   + Y taking account of possible cancellations when using the
   hypergeometric transformation beta_inc(a,b,x)=1-beta_inc(b,a,1-x).

   It also adjusts the accuracy of beta_inc() to fit the overall
   absolute error when A*beta_inc is added to Y. (e.g. if Y >>
   A*beta_inc then the accuracy of beta_inc can be reduced) */

static double
beta_inc_AXPY (const double A, const double Y,
               const double a, const double b, const double x)
{
  if (x == 0.0)
    {
      return A * 0 + Y;
    }
  else if (x == 1.0)
    {
      return A * 1 + Y;
    }
  else if (a > 1e5 && b < 10 && x > a / (a + b))
    {
      /* Handle asymptotic regime, large a, small b, x > peak [AS 26.5.17] */
      double N = a + (b - 1.0) / 2.0;
      return A * gsl_sf_gamma_inc_Q (b, -N * log (x)) + Y;
    }
  else if (b > 1e5 && a < 10 && x < b / (a + b))
    {
      /* Handle asymptotic regime, small a, large b, x < peak [AS 26.5.17] */
      double N = b + (a - 1.0) / 2.0;
      return A * gsl_sf_gamma_inc_P (a, -N * log1p (-x)) + Y;
    }
  else
    {
      double ln_beta = gsl_sf_lnbeta (a, b);
      double ln_pre = -ln_beta + a * log (x) + b * log1p (-x);

      double prefactor = exp (ln_pre);

      if (x < (a + 1.0) / (a + b + 2.0))
        {
          /* Apply continued fraction directly. */
          double epsabs = fabs (Y / (A * prefactor / a)) * GSL_DBL_EPSILON;

          double cf = beta_cont_frac (a, b, x, epsabs);

          return A * (prefactor * cf / a) + Y;
        }
      else
        {
          /* Apply continued fraction after hypergeometric transformation. */
          double epsabs =
            fabs ((A + Y) / (A * prefactor / b)) * GSL_DBL_EPSILON;
          double cf = beta_cont_frac (b, a, 1.0 - x, epsabs);
          double term = prefactor * cf / b;

          if (A == -Y)
            {
              return -A * term;
            }
          else
            {
              return A * (1 - term) + Y;
            }
        }
    }
}

/* Direct series evaluation for testing purposes only */

#if 0
static double
beta_series (const double a, const double b, const double x,
             const double epsabs)
{
  double f = x / (1 - x);
  double c = (b - 1) / (a + 1) * f;
  double s = 1;
  double n = 0;

  s += c;

  do
    {
      n++;
      c *= -f * (2 + n - b) / (2 + n + a);
      s += c;
    }
  while (n < 512 && fabs (c) > GSL_DBL_EPSILON * fabs (s) + epsabs);

  s /= (1 - x);

  return s;
}
#endif


/* specfunc/bessel_i.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_bessel.h>

#include "error.h"

#include "bessel.h"

/* i_{l+1}/i_l
 */
static
int
bessel_il_CF1(const int l, const double x, const double threshold, double * ratio)
{
  const int kmax = 2000;
  double tk   = 1.0;
  double sum  = 1.0;
  double rhok = 0.0;
  int k;

  for(k=1; k<=kmax; k++) {
    double ak = (x/(2.0*l+1.0+2.0*k)) * (x/(2.0*l+3.0+2.0*k));
    rhok = -ak*(1.0 + rhok)/(1.0 + ak*(1.0 + rhok));
    tk  *= rhok;
    sum += tk;
    if(fabs(tk/sum) < threshold) break;
  }

  *ratio = x/(2.0*l+3.0) * sum;

  if(k == kmax)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

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

int gsl_sf_bessel_i0_scaled_e(const double x, gsl_sf_result * result)
{
  double ax = fabs(x);

  /* CHECK_POINTER(result) */

  if(x == 0.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(ax < 0.2) {
    const double eax = exp(-ax);
    const double y = ax*ax;
    const double c1 = 1.0/6.0;
    const double c2 = 1.0/120.0;
    const double c3 = 1.0/5040.0;
    const double c4 = 1.0/362880.0;
    const double c5 = 1.0/39916800.0;
    const double sum = 1.0 + y*(c1 + y*(c2 + y*(c3 + y*(c4 + y*c5))));
    result->val = eax * sum;
    result->err = 2.0 * GSL_DBL_EPSILON * result->val;
  }
  else if(ax < -0.5*GSL_LOG_DBL_EPSILON) {
    result->val = (1.0 - exp(-2.0*ax))/(2.0*ax);
    result->err = 2.0 * GSL_DBL_EPSILON * result->val;
  }
  else {
    result->val = 1.0/(2.0*ax);
    result->err = 2.0 * GSL_DBL_EPSILON * result->val;
  }
  return GSL_SUCCESS;
}

int gsl_sf_bessel_i1_scaled_e(const double x, gsl_sf_result * result)
{
  double ax = fabs(x);

  /* CHECK_POINTER(result) */

  if(x == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(ax < 3.0*GSL_DBL_MIN) {
    UNDERFLOW_ERROR(result);
  }
  else if(ax < 0.25) {
    const double eax = exp(-ax);
    const double y  = x*x;
    const double c1 = 1.0/10.0;
    const double c2 = 1.0/280.0;
    const double c3 = 1.0/15120.0;
    const double c4 = 1.0/1330560.0;
    const double c5 = 1.0/172972800.0;
    const double sum = 1.0 + y*(c1 + y*(c2 + y*(c3 + y*(c4 + y*c5))));
    result->val = eax * x/3.0 * sum;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    double ex = exp(-2.0*ax);
    result->val = 0.5 * (ax*(1.0+ex) - (1.0-ex)) / (ax*ax);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    if(x < 0.0) result->val = -result->val;
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_i2_scaled_e(const double x, gsl_sf_result * result)
{
  double ax = fabs(x);

  /* CHECK_POINTER(result) */

  if(x == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(ax < 4.0*GSL_SQRT_DBL_MIN) {
    UNDERFLOW_ERROR(result);
  }
  else if(ax < 0.25) {
    const double y = x*x;
    const double c1 = 1.0/14.0;
    const double c2 = 1.0/504.0;
    const double c3 = 1.0/33264.0;
    const double c4 = 1.0/3459456.0;
    const double c5 = 1.0/518918400.0;
    const double sum = 1.0 + y*(c1 + y*(c2 + y*(c3 + y*(c4 + y*c5))));
    const double pre = exp(-ax) * x*x/15.0;
    result->val = pre * sum;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    double ex = exp(-2.0*ax);
    double x2 = x*x;
    result->val = 0.5 * ((3.0+x2)*(1.0-ex) - 3.0*ax*(1.0+ex))/(ax*ax*ax);
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int gsl_sf_bessel_il_scaled_e(const int l, double x, gsl_sf_result * result)
{
  double sgn = 1.0;
  double ax  = fabs(x);

  if(x < 0.0) {
    /* i_l(-x) = (-1)^l i_l(x) */
    sgn = ( GSL_IS_ODD(l) ? -1.0 : 1.0 );
    x = -x;
  }

  if(l < 0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    result->val = ( l == 0 ? 1.0 : 0.0 );
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(l == 0) {
    gsl_sf_result il;
    int stat_il = gsl_sf_bessel_i0_scaled_e(x, &il);
    result->val = sgn * il.val;
    result->err = il.err;
    return stat_il;
  }
  else if(l == 1) {
    gsl_sf_result il;
    int stat_il = gsl_sf_bessel_i1_scaled_e(x, &il);
    result->val = sgn * il.val;
    result->err = il.err;
    return stat_il;
  }
  else if(l == 2) {
    gsl_sf_result il;
    int stat_il = gsl_sf_bessel_i2_scaled_e(x, &il);
    result->val = sgn * il.val;
    result->err = il.err;
    return stat_il;
  }
  else if(x*x < 10.0*(l+1.5)/M_E) {
    gsl_sf_result b;
    int stat = gsl_sf_bessel_IJ_taylor_e(l+0.5, x, 1, 50, GSL_DBL_EPSILON, &b);
    double pre   = exp(-ax) * sqrt((0.5*M_PI)/x);
    result->val  = sgn * pre * b.val;
    result->err  = pre * b.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat;
  }
  else if(l < 150) {
    gsl_sf_result i0_scaled;
    int stat_i0  = gsl_sf_bessel_i0_scaled_e(ax, &i0_scaled);
    double rat;
    int stat_CF1 = bessel_il_CF1(l, ax, GSL_DBL_EPSILON, &rat);
    double iellp1 = rat * GSL_SQRT_DBL_MIN;
    double iell   = GSL_SQRT_DBL_MIN;
    double iellm1;
    int ell;
    for(ell = l; ell >= 1; ell--) {
      iellm1 = iellp1 + (2*ell + 1)/x * iell;
      iellp1 = iell;
      iell   = iellm1;
    }
    result->val  = sgn * i0_scaled.val * (GSL_SQRT_DBL_MIN / iell);
    result->err  = i0_scaled.err * (GSL_SQRT_DBL_MIN / iell);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_i0, stat_CF1);
  }
  else if(GSL_MIN(0.29/(l*l+1.0), 0.5/(l*l+1.0+x*x)) < 0.5*GSL_ROOT3_DBL_EPSILON) {
    int status = gsl_sf_bessel_Inu_scaled_asymp_unif_e(l + 0.5, x, result);
    double pre = sqrt((0.5*M_PI)/x);
    result->val *= sgn * pre;
    result->err *= pre;
    return status;
  }
  else {
    /* recurse down from safe values */
    double rt_term = sqrt((0.5*M_PI)/x);
    const int LMAX = 2 + (int) (1.2 / GSL_ROOT6_DBL_EPSILON);
    gsl_sf_result r_iellp1;
    gsl_sf_result r_iell;
    int stat_a1 = gsl_sf_bessel_Inu_scaled_asymp_unif_e(LMAX + 1 + 0.5, x, &r_iellp1);
    int stat_a2 = gsl_sf_bessel_Inu_scaled_asymp_unif_e(LMAX     + 0.5, x, &r_iell);
    double iellp1 = r_iellp1.val;
    double iell   = r_iell.val;
    double iellm1 = 0.0;
    int ell;
    iellp1 *= rt_term;
    iell   *= rt_term;
    for(ell = LMAX; ell >= l+1; ell--) {
      iellm1 = iellp1 + (2*ell + 1)/x * iell;
      iellp1 = iell;
      iell   = iellm1;
    }
    result->val  = sgn * iellm1;
    result->err  = fabs(result->val)*(GSL_DBL_EPSILON + fabs(r_iellp1.err/r_iellp1.val) + fabs(r_iell.err/r_iell.val));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_2(stat_a1, stat_a2);
  }
}

int gsl_sf_bessel_il_scaled_array(const int lmax, const double x, double * result_array)
{
  if(x == 0.0) {
    int ell;
    result_array[0] = 1.0;
    for (ell = lmax; ell >= 1; ell--) {
      result_array[ell] = 0.0;
    };
    return GSL_SUCCESS;
  } else {
    int ell;
    gsl_sf_result r_iellp1;
    gsl_sf_result r_iell;
    int stat_0 = gsl_sf_bessel_il_scaled_e(lmax+1, x, &r_iellp1);
    int stat_1 = gsl_sf_bessel_il_scaled_e(lmax,   x, &r_iell);
    double iellp1 = r_iellp1.val;
    double iell   = r_iell.val;
    double iellm1;
    result_array[lmax] = iell;
    for(ell = lmax; ell >= 1; ell--) {
      iellm1 = iellp1 + (2*ell + 1)/x * iell;
      iellp1 = iell;
      iell   = iellm1;
      result_array[ell-1] = iellm1;
    }
    return GSL_ERROR_SELECT_2(stat_0, stat_1);
  }
}

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

#include "eval.h"

double gsl_sf_bessel_i0_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_i0_scaled_e(x, &result));
}

double gsl_sf_bessel_i1_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_i1_scaled_e(x, &result));
}

double gsl_sf_bessel_i2_scaled(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_i2_scaled_e(x, &result));
}

double gsl_sf_bessel_il_scaled(const int l, const double x)
{
  EVAL_RESULT(gsl_sf_bessel_il_scaled_e(l, x, &result));
}


// sammlung verwendeter Funktionen
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <omp.h>

#ifndef HEADER_FILE_UTIL
#define HEADER_FILE_UTIL

double Norm_Diff(double *vec1,double *vec2, int dim, int p); //berechnet norm von || vec1-vec2 ||_p
void Lattice_Setup(double **positions,int  numbertoinf,int dim, double latticespacing); // Legt Koordianten auf mxn Gittermatrix der dimension numbertoinf**dim, dim
int ipow(int base, int exp);
int Kahan_Sum(int N, double input[], double *ans);
int Sign_Sum(int N, double input[], double *ans);
int Neumaier_Sum(int N, double input[N], double *ans);
void deleteSpaces(char src[], char dst[]);

void vec_zero(double *v,int  N);
void vec_zero_i(int *v,int  N);

int Update_Lattice_Position(int * pos, const double *vec, const double lattice_spacing);

// begin stepper with different external forces -------//

void VVerlet_Step(int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t,
					void (*deriv) (double *y, double *ans, double t,int N));

void VVerlet_Step_Target_Square(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t,
					void (*deriv) (double *y, double *ans, double t,int N));

void VVerlet_Step_deriv(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t, //mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
					void (*derivmethod) (double *y, double *ans, double t,int N));

void VVerlet_Step_Yukawa(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t, //mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
					void (*derivmethod) (double *y, double *ans, double t,int N));

void VVerlet_Step_Pore_Rectangle(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t, // wie Verlet step, aber Teilchen reflektiert stehen falls in Target (Quadrat)
					void (*derivmethod) (double *y, double *ans, double t,int N));

void VVerlet_Step_Pore_Yukawa(const int N,  double  x[N/2] , double v[N/2], double a[N/2], double *t, //mach einzelnen update Schritt nach Velocity-Verlet ohne Hinderniss
					void (*derivmethod) (double *y, double *ans, double t,int N));

// end stepper with different external forces -------//

void VVerlet(int N, double *y, double **ans, double *t,
			void (*deriv) (double *y, double *ans, double t, int N));

void VVerlet_parallel(const int N, const double *y, double **ans, double *t,
				void (*derivmethod) (double *y, double *ans, double t, int N));

void VVerlet_parallel_burn(const int N, const  double *y, double **ans, double *t,							// Velocity Verlet für Start y, Ausgabe ans[LengthT][ORDER] zu Zeiten T
				void (*derivmethod) (double *y, double *ans, double t, int N));

void Bath_Setup(double *y, char *label, gsl_rng * r);

void Ommega_Setup(gsl_rng * r);

void Gamma_Setup(double * gamma);

void deriv(double *y, double *ans, double t, int N);
void deriv_parallel(const double *yin, double *ans, double t, int N);

void deriv_Yukawa(const double *y, double *ans, double t, int N);

#endif


/***************************************************************************
    File                 : muParserScripting.h
    Project              : QtiPlot
    --------------------------------------------------------------------

    Copyright            : (C) 2006 by Ion Vasilief, Knut Franke
    Email (use @ for *)  : ion_vasilief*yahoo.fr, knut.franke*gmx.de
    Description          : Evaluate mathematical expressions using muParser

 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *  This program is free software; you can redistribute it and/or modify   *
 *  it under the terms of the GNU General Public License as published by   *
 *  the Free Software Foundation; either version 2 of the License, or      *
 *  (at your option) any later version.                                    *
 *                                                                         *
 *  This program is distributed in the hope that it will be useful,        *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of         *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          *
 *  GNU General Public License for more details.                           *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the Free Software           *
 *   Foundation, Inc., 51 Franklin Street, Fifth Floor,                    *
 *   Boston, MA  02110-1301  USA                                           *
 *                                                                         *
 ***************************************************************************/
#ifndef MUPARSER_SCRIPTING_H
#define MUPARSER_SCRIPTING_H

#include "ScriptingEnv.h"
#include "Script.h"
#include "muParserScript.h"

#include <muParser.h>
#include "math.h"
#include <gsl/gsl_sf.h>
#include <gsl/gsl_cdf.h>
#include <q3asciidict.h>

//! TODO
class muParserScripting: public ScriptingEnv
{
  Q_OBJECT

  public:
    static const char *langName;
    muParserScripting(ApplicationWindow *parent) : ScriptingEnv(parent, langName) { d_initialized=true; }
    static ScriptingEnv *constructor(ApplicationWindow *parent) { return new muParserScripting(parent); }

    bool isRunning() const { return true; }
    Script *newScript(const QString &code, QObject *context, const QString &name="<input>")
    {
      return new muParserScript(this, code, context, name);
    }

    // we do not support global variables
    bool setQObject(QObject*, const char*) { return false; }
    bool setInt(int, const char*) { return false; }
    bool setDouble(double, const char*) { return false; }

    const QStringList mathFunctions() const;
    const QString mathFunctionDoc (const QString &name) const;

    struct mathFunction
    {
      char *name;
      int numargs;
      double (*fun1)(double);
      double (*fun2)(double,double);
      double (*fun3)(double,double,double);
      char *description;
    };
    static const mathFunction math_functions[];

  private:
    static double mod(double x, double y)
      { return fmod(x,y); }
    static double bessel_J0(double x)
      { return gsl_sf_bessel_J0 (x); }
    static double bessel_J1(double x)
      { return gsl_sf_bessel_J1 (x); }
    static double bessel_Jn(double x, double n)
      { return gsl_sf_bessel_Jn ((int)n, x); }
    static double bessel_Yn(double x, double n)
      { return gsl_sf_bessel_Yn ((int)n, x); }
    static double bessel_Jn_zero(double n, double s)
      { return gsl_sf_bessel_zero_Jnu(n, (unsigned int) s); }
    static double bessel_Y0(double x)
      { return gsl_sf_bessel_Y0 (x); }
    static double bessel_Y1(double x)
      { return gsl_sf_bessel_Y1 (x); }
    static double beta(double a, double b)
      { return gsl_sf_beta (a,b); }
    static double erf(double x)
      { return gsl_sf_erf (x); }
    static double erfc(double x)
      { return gsl_sf_erfc (x); }
    static double erf_Z(double x)
      { return gsl_sf_erf_Z (x); }
    static double erf_Q(double x)
      { return gsl_sf_erf_Q (x); }
    static double gamma(double x)
      { return gsl_sf_gamma (x); }
    static double lngamma(double x)
      { return gsl_sf_lngamma (x); }
    static double hazard(double x)
      { return gsl_sf_hazard (x); }
	static double lambert_W0(double x)
	   { return gsl_sf_lambert_W0(x); }
	static double lambert_Wm1(double x)
	   { return gsl_sf_lambert_Wm1(x); }
	static double ttable(double x, double n)
	   { return gsl_cdf_tdist_Pinv(x, n); }
};

class EmptySourceError : public mu::ParserError
{
	public:
		EmptySourceError() {}
};

#endif


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

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

#include "ccl.h"

// Analytic FT of NFW profile, from Cooray & Sheth (2002; Section 3 of https://arxiv.org/abs/astro-ph/0206508)
// Normalised such that U(k=0)=1
static double u_nfw_c(ccl_cosmology *cosmo, double rv, double c, double k, int *status) {

  double rs, ks;
  double f1, f2, f3, fc;

  // Special case to prevent numerical problems if k=0,
  // the result should be unity here because of the normalisation
  if (k==0.) {
    return 1.;
  }

  // The general k case
  else{

    // Scale radius for NFW (rs=rv/c)
    rs = rv/c;

    // Dimensionless wave-number variable
    ks = k*rs;

    // Various bits for summing together to get final result
    f1 = sin(ks)*(gsl_sf_Si(ks*(1.+c))-gsl_sf_Si(ks));
    f2 = cos(ks)*(gsl_sf_Ci(ks*(1.+c))-gsl_sf_Ci(ks));
    f3 = sin(c*ks)/(ks*(1.+c));
    fc = log(1.+c)-c/(1.+c);

    return (f1+f2-f3)/fc;

  }
}

/*----- ROUTINE: ccl_halo_concentration -----
INPUT: cosmology, a halo mass [Msun], scale factor, halo definition, concentration model label
TASK: Computes halo concentration; the ratio of virial raidus to scale radius for an NFW halo.
*/
double ccl_halo_concentration(ccl_cosmology *cosmo, double halomass,
                              double a, double odelta, int *status) {

  double gz, g0, nu, delta_c, a_form;
  double Mpiv, A, B, C;

  switch(cosmo->config.halo_concentration_method){

    // Bhattacharya et al. (2011; 1005.2239; Delta = 200rho_m; Table 2)
  case ccl_bhattacharya2011:

    if (odelta != 200.) {
      *status = CCL_ERROR_CONC_DV;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halomod.c: halo_concentration(): Bhattacharya (2011) concentration "
        "relation only valid for Delta_v = 200");
      return NAN;
    }

    gz = ccl_growth_factor(cosmo,a,status);
    g0 = ccl_growth_factor(cosmo,1.0,status);
    delta_c = 1.686;
    nu = delta_c/ccl_sigmaM(cosmo, log10(halomass), a, status);
    return 9.*pow(nu,-0.29)*pow(gz/g0,1.15);

    // Duffy et al. (2008; 0804.2486; Table 1)
  case ccl_duffy2008:

    Mpiv = 2e12/cosmo->params.h; // Pivot mass in Msun (note in the paper units are Msun/h)

    if (odelta == Dv_BryanNorman(cosmo, a, status)) {

      // Duffy et al. (2008) for virial density haloes (second section in Table 1)
      A = 7.85;
      B = -0.081;
      C = -0.71;
      return A*pow(halomass/Mpiv,B)*pow(a,-C);

    } else if (odelta == 200.) {

      // Duffy et al. (2008) for x200 mean-matter-density haloes (third section in Table 1)
      A = 10.14;
      B = -0.081;
      C = -1.01;
      return A*pow(halomass/Mpiv,B)*pow(a,-C);

    } else {

      *status = CCL_ERROR_CONC_DV;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_halomod.c: halo_concentration(): Duffy (2008) virial "
        "concentration only valid for virial Delta_v or 200\n");
      return NAN;

    }

    // Constant concentration (good for tests)
  case ccl_constant_concentration:

    return 4.;

    // Something went wrong
  default:

    *status = CCL_ERROR_HALOCONC;
    ccl_raise_gsl_warning(*status, "ccl_halomod.c: concentration-mass relation specified incorrectly");
    return NAN;

  }
}

// Fourier Transforms of halo profiles
static double window_function(ccl_cosmology *cosmo, double m, double k,
                              double a, double odelta, ccl_win_label label,
                              int *status) {
  double rho_matter, c, rv;

  switch(label){

  case ccl_nfw:

    // The mean background matter density in Msun/Mpc^3
    rho_matter = ccl_rho_x(cosmo, 1., ccl_species_m_label, 1, status);

    // The halo virial radius
    rv = r_delta(cosmo, m, a, odelta, status);

    // The halo concentration for this halo mass and at this scale factor
    c = ccl_halo_concentration(cosmo, m, a, odelta, status);

    // The function U is normalised to 1 for k<<1 so multiplying by M/rho turns units to overdensity
    return m*u_nfw_c(cosmo, rv, c, k, status)/rho_matter;

    // Something went wrong
  default:

    *status = CCL_ERROR_HALOWIN;
    ccl_raise_warning(*status, "ccl_halomod.c: Window function specified incorrectly");
    return NAN;

  }

}

// Parameters structure for the one-halo integrand
typedef struct{
  ccl_cosmology *cosmo;
  double k, a;
  int *status;
} Int_one_halo_Par;

// Integrand for the one-halo integral
static double one_halo_integrand(double log10mass, void *params){

  Int_one_halo_Par *p = (Int_one_halo_Par *)params;;
  double halomass = pow(10,log10mass);
  double odelta = Dv_BryanNorman(p->cosmo, p->a, p->status); // Virial density for haloes

  // The normalised Fourier Transform of a halo density profile
  double wk = window_function(p->cosmo,halomass, p->k, p->a, odelta, ccl_nfw, p->status);

  // Fairly sure that there should be no ln(10) factor should be here since the integration is being specified in log10 range
  double dn_dlogM = ccl_massfunc(p->cosmo, halomass, p->a, odelta, p->status);

  return dn_dlogM*pow(wk,2);
}

// The one-halo term integral using gsl
static double one_halo_integral(ccl_cosmology *cosmo, double k, double a, int *status){

  int one_halo_integral_status = 0, qagstatus;
  double result = 0, eresult;
  double log10mmin = log10(cosmo->gsl_params.HM_MMIN);
  double log10mmax = log10(cosmo->gsl_params.HM_MMAX);
  Int_one_halo_Par ipar;
  gsl_function F;
  gsl_integration_workspace *w = NULL;

  w = gsl_integration_workspace_alloc(cosmo->gsl_params.HM_LIMIT);
  if (w == NULL) {
    *status = CCL_ERROR_MEMORY;
  }

  if (*status == 0) {
    // Structure required for the gsl integration
    ipar.cosmo = cosmo;
    ipar.k = k;
    ipar.a = a;
    ipar.status = &one_halo_integral_status;
    F.function = &one_halo_integrand;
    F.params = &ipar;

    // Actually does the integration
    qagstatus = gsl_integration_qag(
      &F, log10mmin, log10mmax,
      cosmo->gsl_params.HM_EPSABS,
      cosmo->gsl_params.HM_EPSREL,
      cosmo->gsl_params.HM_LIMIT,
      cosmo->gsl_params.HM_INT_METHOD, w,
      &result, &eresult);

    // Check for errors
    if (qagstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(qagstatus, "ccl_halomod.c: one_halo_integral():");
      *status = CCL_ERROR_ONE_HALO_INT;
      ccl_cosmology_set_status_message(cosmo, "ccl_halomod.c: one_halo_integral(): Integration failure\n");
      result = NAN;
    }
  }

  // Clean up
  gsl_integration_workspace_free(w);

  return result;
}

// Parameters structure for the two-halo integrand
typedef struct{
  ccl_cosmology *cosmo;
  double k, a;
  int *status;
} Int_two_halo_Par;

// Integrand for the two-halo integral
static double two_halo_integrand(double log10mass, void *params){

  Int_two_halo_Par *p = (Int_two_halo_Par *)params;
  double halomass = pow(10,log10mass);
  double odelta = Dv_BryanNorman(p->cosmo, p->a, p->status); // Virial density for haloes

  // The normalised Fourier Transform of a halo density profile
  double wk = window_function(p->cosmo, halomass, p->k, p->a, odelta, ccl_nfw, p->status);

  // Fairly sure that there should be no ln(10) factor should be here since the integration is being specified in log10 range
  double dn_dlogM = ccl_massfunc(p->cosmo, halomass, p->a, odelta, p->status);

  // Halo bias
  double b = ccl_halo_bias(p->cosmo, halomass, p->a, odelta, p->status);

  return b*dn_dlogM*wk;
}

// The two-halo term integral using gsl
static double two_halo_integral(ccl_cosmology *cosmo, double k, double a, int *status){

  int two_halo_integral_status = 0, qagstatus;
  double result = 0, eresult;
  double log10mmin = log10(cosmo->gsl_params.HM_MMIN);
  double log10mmax = log10(cosmo->gsl_params.HM_MMAX);
  Int_two_halo_Par ipar;
  gsl_function F;
  gsl_integration_workspace *w = NULL;

  w = gsl_integration_workspace_alloc(cosmo->gsl_params.HM_LIMIT);
  if (w == NULL) {
    *status = CCL_ERROR_MEMORY;
  }

  if (*status == 0) {
    // Structure required for the gsl integration
    ipar.cosmo = cosmo;
    ipar.k = k;
    ipar.a = a;
    ipar.status = &two_halo_integral_status;
    F.function = &two_halo_integrand;
    F.params = &ipar;

    // Actually does the integration
    qagstatus = gsl_integration_qag(
      &F, log10mmin, log10mmax,
      cosmo->gsl_params.HM_EPSABS,
      cosmo->gsl_params.HM_EPSREL,
      cosmo->gsl_params.HM_LIMIT,
      cosmo->gsl_params.HM_INT_METHOD, w,
      &result, &eresult);

    // Check for errors
    if (qagstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(qagstatus, "ccl_halomod.c: two_halo_integral():");
      *status = CCL_ERROR_TWO_HALO_INT;
      ccl_cosmology_set_status_message(cosmo, "ccl_halomod.c: two_halo_integral(): Integration failure\n");
      result = NAN;
    }
  }

  // Clean up
  gsl_integration_workspace_free(w);

  return result;
}

/*----- ROUTINE: ccl_twohalo_matter_power -----
INPUT: cosmology, wavenumber [Mpc^-1], scale factor
TASK: Computes the two-halo power spectrum term in the halo model assuming NFW haloes
*/
double ccl_twohalo_matter_power(ccl_cosmology *cosmo, double k, double a, int *status){

  // Get the integral
  double I2h = two_halo_integral(cosmo, k, a, status);

  // The addative correction is the missing part of the integral below the lower-mass limit
  double A = 1.-two_halo_integral(cosmo, 0., a, status);

  // Virial overdensity for haloes
  double odelta = Dv_BryanNorman(cosmo, a, status);

  // ...multiplied by the ratio of window functions
  double W1 = window_function(cosmo, cosmo->gsl_params.HM_MMIN, k,  a, odelta, ccl_nfw, status);
  double W2 = window_function(cosmo, cosmo->gsl_params.HM_MMIN, 0., a, odelta, ccl_nfw, status);
  A = A*W1/W2;

  // Add the additive correction to the calculated integral
  I2h = I2h+A;

  return ccl_linear_matter_power(cosmo, k, a, status)*I2h*I2h;

}

/*----- ROUTINE: ccl_onehalo_matter_power -----
INPUT: cosmology, wavenumber [Mpc^-1], scale factor
TASK: Computes the one-halo power spectrum term in the halo model assuming NFW haloes
*/
double ccl_onehalo_matter_power(ccl_cosmology *cosmo, double k, double a, int *status){

  return one_halo_integral(cosmo, k, a, status);

}

/*----- ROUTINE: ccl_onehalo_matter_power -----
INPUT: cosmology, wavenumber [Mpc^-1], scale factor
TASK: Computes the halo model power spectrum by summing the two- and one-halo terms
*/
double ccl_halomodel_matter_power(ccl_cosmology *cosmo, double k, double a, int *status){

  // This matter power spectrum method doesn't work if mu / Sigma parameterisation of modified gravity
  // Is turned on:
  if (fabs(cosmo->params.mu_0)>1e-14 || fabs(cosmo->params.sigma_0)>1e-14){
	  *status = CCL_ERROR_NOT_IMPLEMENTED;
	  strcpy(cosmo->status_message,
           "ccl_halofmod.c: ccl_halomodel_matter_power(): The halo model power spectrum "
           "is not implemented the mu / Sigma modified gravity parameterisation.\n");
	  return NAN;
  }

  // Standard sum of two- and one-halo terms
  return ccl_twohalo_matter_power(cosmo, k, a, status)+ccl_onehalo_matter_power(cosmo, k, a, status);

}


//IN method.
//Linear transformation (weights only, no biases) of Ni inputs to No outputs.
//This version uses CBLAS

//Input X has Ni neurons and output Y has No neurons.

//The vecs of length Ni are always contiguous in memory, such that:

//If col-major: Y[:,l] = W' * X[:,l]
//where:
//X has size Ni x L
//Y has size No x L
//W has size Ni x No

//If row-major: Y[l,:] = X[l,:] * W'
//X has size L x Ni
//Y has size L x No
//W has size No x Ni

//For a different set-up that allows linear transformation of vecs in
//any orientation, use the linear function from math.

//I retain the for loop through L for compatibility with real-time streaming.

#include <cblas.h>

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

int linear_cblas_s (float *Y, const float *X, const float *W, const size_t Ni, const size_t No, const size_t L);
int linear_cblas_d (double *Y, const double *X, const double *W, const size_t Ni, const size_t No, const size_t L);
int linear_cblas_c (float *Y, const float *X, const float *W, const size_t Ni, const size_t No, const size_t L);
int linear_cblas_z (double *Y, const double *X, const double *W, const size_t Ni, const size_t No, const size_t L);

int linear_cblas_s (float *Y, const float *X, const float *W, const size_t Ni, const size_t No, const size_t L)
{
    for (size_t l=L; l>0u; --l, X+=Ni, Y+=No)
    {
        cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)No,(int)Ni,1.0f,W,(int)Ni,X,1,0.0f,Y,1);
    }

    return 0;
}

int linear_cblas_d (double *Y, const double *X, const double *W, const size_t Ni, const size_t No, const size_t L)
{
    for (size_t l=L; l>0u; --l, X+=Ni, Y+=No)
    {
        cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)No,(int)Ni,1.0,W,(int)Ni,X,1,0.0,Y,1);
    }

    return 0;
}

int linear_cblas_c (float *Y, const float *X, const float *W, const size_t Ni, const size_t No, const size_t L)
{
    const float z[2] = {0.0f,0.0f}, o[2] = {1.0f,0.0f};

    for (size_t l=L; l>0u; --l, X+=2u*Ni, Y+=2u*No)
    {
        cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)No,(int)Ni,o,W,(int)Ni,X,1,z,Y,1);
    }

    return 0;
}

int linear_cblas_z (double *Y, const double *X, const double *W, const size_t Ni, const size_t No, const size_t L)
{
    const double z[2] = {0.0,0.0}, o[2] = {1.0,0.0};

    for (size_t l=L; l>0u; --l, X+=2u*Ni, Y+=2u*No)
    {
        cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)No,(int)Ni,o,W,(int)Ni,X,1,z,Y,1);
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Program fwhm_by_modsq
* to compute the Full Width at Half Maximum of an equivalent long exposure
* with the power spectrum (square modulus of the FFT)
* (by fitting a Kolmogorof law)
*
* JLP
* Version 11-05-99
-------------------------------------------------------------------*/
/*
#define DEBUG
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <math.h>
#include <fftw.h>
#include <jlp_ftoc.h>

static int fit_kolmo_center(float *in_image, int nx, int ny,
                        float *sigx, float *sigy);
static int aris_r0calc(float *modsq, int nx, int ny, float *r0);

main(argc,argv)
int argc;
char *argv[];
{
char in_name[61], comments[81];
float *in_image, *testi;
INT_PNTR pntr_image;
INT4 istatus, nx, ny;
float sigx, sigy;
register int i, j;

printf(" Program FWHM_BY_MODSQ to compute the Full Width at Half Maximum from the power spectrum)\n");
printf(" JLP Version 11-05-99 \n");

/* One parameters only is allowed to run the program: */
/* Carefull: 7 parameters always, using JLP "runs" */
if(argc == 7 && *argv[3]) argc = 4;
if(argc == 7 && *argv[2]) argc = 3;
if(argc == 7 && *argv[1]) argc = 2;
if(argc != 2 && argc != 1)
  {
  printf(" Syntax: fwhm_by_modsq filename\n");
  printf(" Fatal: Syntax error: argc=%d\n",argc);
  exit(-1);
  }

/* Interactive input of parameters: */
if (argc == 2 )
 {
  strcpy(in_name,argv[1]);
 }
else
 {
  printf(" Input file := ");scanf("%s",in_name);
 }

/**********************************************************/
  JLP_BEGIN();
  JLP_INQUIFMT();
  istatus = JLP_VM_READIMAG1(&pntr_image,&nx,&ny,in_name,comments);
  if(istatus != 0) exit(-1);

  in_image = (float *) pntr_image;
/* Computing the power spectrum (for debug purpose)
  testi = (float *)malloc(nx * ny * sizeof(float));
  for(i = 0; i < nx * ny; i++) testi[i] = 0.;

  fftw_float(in_image,testi,nx,ny,1);
  for(i = 0; i < nx * ny; i++) in_image[i] = in_image[i] * in_image[i]
                                             + testi[i] * testi[i];
*/

  fit_kolmo_center(in_image, nx, ny, &sigx, &sigy);

JLP_END();
}
/*****************************************************************
* To fit a Kolmogorof law to f1(xx,yy)
*
* f1: array of values f1(xx[i],yy[i])
* xx, yy: arrays of coordinates x, y
* npts: number of points
*
* Parameters:
* sigx, sigy, xc, yc, rho
* error[sigx,sigy,xc,yc,rho]
*
* ifail = 0 if correct
*         -3: all values are negative or null!
*****************************************************************/
static int fit_kolmo_center(float *in_image, INT4 nx, INT4 ny,
                               float *sigx, float *sigy)
{
float *ff2, *gg;
INT4 istatus, npts, ifail, nx1, ny1, istart, jstart, iw, jw;
float aa, bb, r0, fwhm;
register int i, j;

nx1 = 20; ny1 = 20;
istart = nx/2 - nx1/2;
jstart = ny/2 - ny1/2;
ff2 = (float *) malloc(nx1 * ny1 * sizeof(float));
gg = (float *) malloc(nx1 * ny1 * sizeof(float));
if(ff2 == NULL || gg == NULL)
  {
  printf("FWHM/Fatal error alocating memory space \n");
  exit(-1);
  }
#ifdef DEBUG
printf(" istart=%d jstart=%d nx1=%d ny1=%d\n",istart,jstart,nx1,ny1);
#endif
for(j = 0; j < ny1; j++)
  for(i = 0; i < nx1; i++)
   {
   iw = i + istart;
   jw = j + jstart;
   ff2[i + j * nx1] = (iw - nx/2) * (iw - nx/2) + (jw - ny/2) * (jw - ny/2);
   gg[i + j * nx1] = in_image[iw + jw * nx];
   }
npts = nx1 * ny1;

jlp_fit_kolmogorof(ff2, gg, &npts, &aa, &bb, &ifail);

/* JLP99: I notice that the ratio between long exposure determination
* and this program is about 15: */
fwhm = pow((double)(bb/6.88),-0.6)/15.;
/* fwhm = lambda /r0 */
r0 = 1./fwhm;
#ifdef DEBUG
printf(" Full Kolmogorov fit: aa=%f bb=%f r0=%f fwhm~%f (10 mm)\n",
         aa,bb,r0,fwhm);
#else
printf(" Full Kolmogorov fit: r0=%f fwhm~%f (10 mm)\n",r0,fwhm);
#endif

aris_r0calc(in_image, nx, ny, &r0);
/* To make it compatible with the previous fit: */
r0 *= 4.;
printf(" Eric Aristidi approximation: r0=%f fwhm=%f\n",r0,1./r0);

return(0);
}

/***************************************************************
* fit_kolmogorof
* To fit a Kolmogorof law to Log_intensity(|x|^2) as a function of
* in x^2 + y^2
*
*   g(f) = rho * exp [ -6.88 * (lambda * f / r_0)^5/3 ]
*
*   g(f) = rho * exp [ - b * f^5/3 ]    with b = 6.88 * (lambda / r_0)^5/3
*
*  Log_Intensity(x,y)
*    G(f) = a - b f^(5/3)    with f^2 = x^2 + y^2
*
*  where:
*  The problem is to find the coefficients (a, b)
*  which minimize the sums:
*   SUM on all the selected disk (x, y) of ( Log_intensity - G(x,y) ) **2
*
* The normal equation can be written as:
*
*    n a - b SUM f^5/3 = SUM Log_g
*    a SUM f^5/3 - b SUM f^10/3 = SUM f^5/3 Log_g
*
* Hence:
*    a = 1/n * (SUM Log_g + b SUM f^5/3)
*    b = (SUM f^5/3 Log_g  - 1/n SUM Log_g SUM f^5/3)
*          / ( 1/n * SUM f^5/3 * SUM f^5/3 - SUM f^10/3)
*
*   where n is the number of points (SUM 1)
*
*
* gg: array of values gg(xx[i],yy[i])
*
* ff2: arrays of x^2 + y^2
* npts: number of points
*
* ifail = 0 if correct
*         -3: all values are negative or null!
*****************************************************************/
int jlp_fit_kolmogorof(float *ff2, float *gg, INT4 *npts,
                       float *aa, float *bb, INT4 *ifail)
{
/* gg measured intensities */
/* log_g log of measured intensities */
double *log_g, *ff;
double sum_g, sum_f1, sum_f2, sum_gf, w1;
register int i, j, k;

*ifail = 0;

 if((log_g = (double *) malloc(*npts * sizeof(double))) == NULL ||
     (ff = (double *) malloc(*npts * sizeof(double))) == NULL )
  {
  printf("jlp_fit_gauss/Error allocating memory for array (npts=%d)\n",*npts);
  *ifail = -1;
  return(-1);
  }

/* Transfer to double precision arrays and conversion of intensity to Log : */
i = 0;
for(k = 0; k < *npts; k++)
  {
       if(gg[k] > 0)
         {
          log_g[i] = log((double)gg[k]);
          ff[i] = ff2[k];
          i++;
         }
  }
*npts = i;

if(*npts == 0)
  {
  printf("jlp_fit_gauss/All input values are null!\n");
  *ifail = -1;
  return(-1);
  }

/* Compute the sums:  5/3 = 1.667 (but 5/6=0.833 since x^2+y^2)*/
/* Compute the sums:  10/3 = 0.833 (but 5/3=1.667 since x^2+y^2)*/
sum_f1 = 0.; sum_f2 = 0.;
sum_g = 0.; sum_gf = 0.;
for(k = 0; k < *npts; k++)
  {
  w1 = pow(ff[k],0.833);
  sum_f1 += w1;
  sum_f2 += pow(ff[k],1.667);
  sum_g  += log_g[k];
  sum_gf += w1 * log_g[k];
  }

#ifdef DEBUG
printf("sum_f1=%f sum_f2=%f sum_g=%f sum_gf=%f npts=%d\n",
        sum_f1,sum_f2,sum_g,sum_gf,*npts);
#endif
/* Result:
*    a = 1/n * (SUM Log_g + b SUM f^5/3)
*    b = (SUM f^5/3 Log_g  - 1/n SUM Log_g SUM f^5/3)
*          / ( 1/n * SUM f^5/3 * SUM f^5/3 - SUM f^10/3)
*/
  *bb = (sum_gf - (1. / *npts) * sum_g * sum_f1)
           / ( (1. / *npts) * sum_f1 * sum_f1 - sum_f2);
  *aa = (1. / *npts) * (sum_g + *bb * sum_f1);

return(0);
}
/**********************************************************/
/* Eric Aristidi's fit (approximation only) */
static int aris_r0calc(float *modsq, int nx, int ny, float *r0)
{
int     f12_1, f12_2, dim, ixc, iyc;
float   pix,lambda,x,y1,y2,r01,r02;
dim = nx;
ixc = nx/2;
iyc = ny/2;

 x=modsq[ixc + iyc*dim];

 f12_1=4;
 y1=modsq[(ixc+f12_1) + iyc * dim] + modsq[ixc + (iyc+f12_1) * dim];
 y1/=2.;

 f12_2=3;
 y2=modsq[(ixc+f12_2) + iyc * dim] + modsq[ixc + (iyc+f12_2) * dim];
 y2/=2.;

//  Compute r0
//  ---------
 pix=0.0119/206265.;lambda=0.00000065;
 r01=pow(3.44/log(x/y1),0.6)*lambda*(float)f12_1/pix/dim;
 r02=pow(3.44/log(x/y2),0.6)*lambda*(float)f12_2/pix/dim;
     *r0=(r02+r01)/2.;
}


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

#include "cnn_calc.h"
#include "cnn_init.h"
#include "cnn_private.h"

inline void cnn_restrict(float* mat, int size, float limit)
{
    for (int __i = 0; __i < size; __i++)
    {
        mat[__i] = fminf(mat[__i], limit);
    }
}

void cnn_mat_update(struct CNN_MAT* matPtr, float lRate, float limit)
{
    int size = matPtr->rows * matPtr->cols;

    // Limit gradient and update weight
#ifdef CNN_WITH_CUDA
    cnn_fminf_gpu(matPtr->grad, matPtr->grad, size, limit);
    cublasSaxpy(cnnInit.blasHandle, size, &lRate, matPtr->grad, 1, matPtr->mat,
                1);
#else
    cnn_restrict(matPtr->grad, size, limit);
    cblas_saxpy(size, lRate, matPtr->grad, 1, matPtr->mat, 1);
#endif

    // Clear gradient
#ifdef CNN_WITH_CUDA
    cudaMemset
#else
    memset
#endif
        (matPtr->grad, 0, size * sizeof(float));
}

void cnn_update(cnn_t cnn, float lRate, float gradLimit)
{
    int i;
    struct CNN_CONFIG* cfgRef;
    union CNN_LAYER* layerRef;

    // Set reference
    layerRef = cnn->layerList;
    cfgRef = &cnn->cfg;

    // Update network and clear gradient
    for (i = cfgRef->layers - 1; i > 0; i--)
    {
        // Clear layer gradient
#ifdef CNN_WITH_CUDA
        cudaMemset
#else
        memset
#endif
            (layerRef[i].outMat.data.grad, 0,
             sizeof(float) * layerRef[i].outMat.data.rows *
                 layerRef[i].outMat.data.cols);

        switch (cfgRef->layerCfg[i].type)
        {
            // Fully connected
            case CNN_LAYER_FC:
                cnn_mat_update(&layerRef[i].fc.weight, lRate, gradLimit);
                cnn_mat_update(&layerRef[i].fc.bias, lRate, gradLimit);
                break;

            // Convolution
            case CNN_LAYER_CONV:
                cnn_mat_update(&layerRef[i].conv.kernel, lRate, gradLimit);

#if defined(CNN_CONV_BIAS_FILTER) || defined(CNN_CONV_BIAS_LAYER)
                cnn_mat_update(&layerRef[i].conv.bias, lRate, gradLimit);
#endif
                break;

            // Batch normalization
            case CNN_LAYER_BN:
                cnn_mat_update(&layerRef[i].bn.bnScale, lRate, gradLimit);
                cnn_mat_update(&layerRef[i].bn.bnBias, lRate, gradLimit);
                break;

            case CNN_LAYER_INPUT:
            case CNN_LAYER_ACTIV:
            case CNN_LAYER_POOL:
            case CNN_LAYER_DROP:
                break;
        }
    }
}

static inline void cnn_backward_kernel(cnn_t cnn, float* errGrad)
{
    int i;

    struct CNN_CONFIG* cfgRef;
    union CNN_LAYER* layerRef;

    // Set reference
    layerRef = cnn->layerList;
    cfgRef = &cnn->cfg;

    // Backpropagation
    for (i = cfgRef->layers - 1; i > 0; i--)
    {
        switch (cfgRef->layerCfg[i].type)
        {
            // Fully connected
            case CNN_LAYER_FC:
                cnn_backward_fc(layerRef, cfgRef, i);
                break;

            // Activation function
            case CNN_LAYER_ACTIV:
                cnn_backward_activ(layerRef, cfgRef, i);
                break;

            // Convolution
            case CNN_LAYER_CONV:
                cnn_backward_conv(layerRef, cfgRef, i);
                break;

            // Pooling
            case CNN_LAYER_POOL:
                cnn_backward_pool(layerRef, cfgRef, i);
                break;

            // Dropout
            case CNN_LAYER_DROP:
                cnn_backward_drop(layerRef, cfgRef, i);
                break;

            // Batch normalization
            case CNN_LAYER_BN:
                cnn_backward_bn(layerRef, cfgRef, i);
                break;

            default:
                assert(!"Invalid layer type");
        }
    }
}

void cnn_backward(cnn_t cnn, float* errGrad)
{
    int size;

    struct CNN_CONFIG* cfgRef;
    union CNN_LAYER* layerRef;

    // Set reference
    layerRef = cnn->layerList;
    cfgRef = &cnn->cfg;

    // Copy gradient vector
    size = sizeof(float) * layerRef[cfgRef->layers - 1].outMat.data.rows *
           layerRef[cfgRef->layers - 1].outMat.data.cols;

#ifdef CNN_WITH_CUDA
    cudaMemcpy(layerRef[cfgRef->layers - 1].outMat.data.grad, errGrad, size,
               cudaMemcpyHostToDevice);
#else
    memcpy(layerRef[cfgRef->layers - 1].outMat.data.grad, errGrad, size);
#endif

    // Backpropagation
    cnn_backward_kernel(cnn, errGrad);
}

#ifdef CNN_WITH_CUDA
void cnn_backward_gpu(cnn_t cnn, float* errGrad)
{
    int size;

    struct CNN_CONFIG* cfgRef;
    union CNN_LAYER* layerRef;

    // Set reference
    layerRef = cnn->layerList;
    cfgRef = &cnn->cfg;

    // Copy gradient vector
    size = sizeof(float) * layerRef[cfgRef->layers - 1].outMat.data.rows *
           layerRef[cfgRef->layers - 1].outMat.data.cols;
    cudaMemcpy(layerRef[cfgRef->layers - 1].outMat.data.grad, errGrad, size,
               cudaMemcpyDeviceToDevice);

    // Backpropagation
    cnn_backward_kernel(cnn, errGrad);
}
#endif

static inline void cnn_forward_kernel(cnn_t cnn, float* inputMat,
                                      float* outputMat)
{
    int i;

    struct CNN_CONFIG* cfgRef;
    union CNN_LAYER* layerRef;

    // Set reference
    layerRef = cnn->layerList;
    cfgRef = &cnn->cfg;

    // Forward computation
    for (i = 1; i < cfgRef->layers; i++)
    {
        switch (cfgRef->layerCfg[i].type)
        {
            // Fully connected
            case CNN_LAYER_FC:
                cnn_forward_fc(layerRef, cfgRef, i);
                break;

            // Activation function
            case CNN_LAYER_ACTIV:
                cnn_forward_activ(layerRef, cfgRef, i);
                break;

            // Convolution
            case CNN_LAYER_CONV:
                cnn_forward_conv(layerRef, cfgRef, i);
                break;

            // Pooling
            case CNN_LAYER_POOL:
                cnn_forward_pool(layerRef, cfgRef, i);
                break;

            // Dropout
            case CNN_LAYER_DROP:
                if (cnn->opMode == CNN_OPMODE_TRAIN)
                {
                    cnn_forward_drop(layerRef, cfgRef, i);
                }
                else
                {
                    cnn_recall_drop(layerRef, cfgRef, i);
                }

                break;

            // Batch normalization
            case CNN_LAYER_BN:
                if (cnn->opMode == CNN_OPMODE_TRAIN)
                {
                    cnn_forward_bn(layerRef, cfgRef, i);
                }
                else
                {
                    cnn_recall_bn(layerRef, cfgRef, i);
                }

                break;

            default:
                assert(!"Invalid layer type");
        }
    }
}

void cnn_forward(cnn_t cnn, float* inputMat, float* outputMat)
{
    int size;

    struct CNN_CONFIG* cfgRef;
    union CNN_LAYER* layerRef;

    // Set reference
    layerRef = cnn->layerList;
    cfgRef = &cnn->cfg;

    // Copy input
    size = sizeof(float) * layerRef[0].outMat.data.rows *
           layerRef[0].outMat.data.cols;
#ifdef CNN_WITH_CUDA
    cudaMemcpy(layerRef[0].outMat.data.mat, inputMat, size,
               cudaMemcpyHostToDevice);
#else
    memcpy(layerRef[0].outMat.data.mat, inputMat, size);
#endif

    // Forward computation
    cnn_forward_kernel(cnn, inputMat, outputMat);

    // Copy output
    if (outputMat != NULL)
    {
        size = sizeof(float) * layerRef[cfgRef->layers - 1].outMat.data.rows *
               layerRef[cfgRef->layers - 1].outMat.data.cols;
#ifdef CNN_WITH_CUDA
        cudaMemcpy(outputMat, layerRef[cfgRef->layers - 1].outMat.data.mat,
                   size, cudaMemcpyDeviceToHost);
#else
        memcpy(outputMat, layerRef[cfgRef->layers - 1].outMat.data.mat, size);
#endif
    }
#ifdef CNN_WITH_CUDA
    else
    {
        cudaDeviceSynchronize();
    }
#endif
}

#ifdef CNN_WITH_CUDA
void cnn_forward_gpu(cnn_t cnn, float* inputMat, float* outputMat)
{
    int size;

    struct CNN_CONFIG* cfgRef;
    union CNN_LAYER* layerRef;

    // Set reference
    layerRef = cnn->layerList;
    cfgRef = &cnn->cfg;

    // Copy input
    size = sizeof(float) * layerRef[0].outMat.data.rows *
           layerRef[0].outMat.data.cols;
    cudaMemcpy(layerRef[0].outMat.data.mat, inputMat, size,
               cudaMemcpyDeviceToDevice);

    // Forward computation
    cnn_forward_kernel(cnn, inputMat, outputMat);

    // Copy output
    if (outputMat != NULL)
    {
        size = sizeof(float) * layerRef[cfgRef->layers - 1].outMat.data.rows *
               layerRef[cfgRef->layers - 1].outMat.data.cols;
        cudaMemcpy(outputMat, layerRef[cfgRef->layers - 1].outMat.data.mat,
                   size, cudaMemcpyDeviceToDevice);
    }
    else
    {
        cudaDeviceSynchronize();
    }
}
#endif


/* specfunc/gsl_sf_lambert.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#ifndef __GSL_SF_LAMBERT_H__
#define __GSL_SF_LAMBERT_H__

#include <gsl/gsl_sf_result.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

/* Lambert's Function W_0(x)
 *
 * W_0(x) is the principal branch of the
 * implicit function defined by W e^W = x.
 *
 * -1/E < x < \infty
 *
 * exceptions: GSL_EMAXITER;
 */
int     gsl_sf_lambert_W0_e(double x, gsl_sf_result * result);
double  gsl_sf_lambert_W0(double x);

/* Lambert's Function W_{-1}(x)
 *
 * W_{-1}(x) is the second real branch of the
 * implicit function defined by W e^W = x.
 * It agrees with W_0(x) when x >= 0.
 *
 * -1/E < x < \infty
 *
 * exceptions: GSL_MAXITER;
 */
int     gsl_sf_lambert_Wm1_e(double x, gsl_sf_result * result);
double  gsl_sf_lambert_Wm1(double x);

__END_DECLS

#endif /* __GSL_SF_LAMBERT_H__ */


/* specfunc/beta_inc.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_gamma.h>

#include "error.h"
#include "check.h"

static
int
beta_cont_frac(
  const double a,
  const double b,
  const double x,
  gsl_sf_result * result
  )
{
  const unsigned int max_iter = 512;        /* control iterations      */
  const double cutoff = 2.0 * GSL_DBL_MIN;  /* control the zero cutoff */
  unsigned int iter_count = 0;
  double cf;

  /* standard initialization for continued fraction */
  double num_term = 1.0;
  double den_term = 1.0 - (a+b)*x/(a+1.0);
  if (fabs(den_term) < cutoff) den_term = cutoff;
  den_term = 1.0/den_term;
  cf = den_term;

  while(iter_count < max_iter) {
    const int k  = iter_count + 1;
    double coeff = k*(b-k)*x/(((a-1.0)+2*k)*(a+2*k));
    double delta_frac;

    /* first step */
    den_term = 1.0 + coeff*den_term;
    num_term = 1.0 + coeff/num_term;
    if(fabs(den_term) < cutoff) den_term = cutoff;
    if(fabs(num_term) < cutoff) num_term = cutoff;
    den_term  = 1.0/den_term;

    delta_frac = den_term * num_term;
    cf *= delta_frac;

    coeff = -(a+k)*(a+b+k)*x/((a+2*k)*(a+2*k+1.0));

    /* second step */
    den_term = 1.0 + coeff*den_term;
    num_term = 1.0 + coeff/num_term;
    if(fabs(den_term) < cutoff) den_term = cutoff;
    if(fabs(num_term) < cutoff) num_term = cutoff;
    den_term = 1.0/den_term;

    delta_frac = den_term*num_term;
    cf *= delta_frac;

    if(fabs(delta_frac-1.0) < 2.0*GSL_DBL_EPSILON) break;

    ++iter_count;
  }

  result->val = cf;
  result->err = iter_count * 4.0 * GSL_DBL_EPSILON * fabs(cf);

  if(iter_count >= max_iter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

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

int
gsl_sf_beta_inc_e(
  const double a,
  const double b,
  const double x,
  gsl_sf_result * result
  )
{
  if(a <= 0.0 || b <= 0.0 || x < 0.0 || x > 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(x == 1.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result ln_beta;
    gsl_sf_result ln_x;
    gsl_sf_result ln_1mx;
    gsl_sf_result prefactor;
    const int stat_ln_beta = gsl_sf_lnbeta_e(a, b, &ln_beta);
    const int stat_ln_1mx = gsl_sf_log_1plusx_e(-x, &ln_1mx);
    const int stat_ln_x = gsl_sf_log_e(x, &ln_x);
    const int stat_ln = GSL_ERROR_SELECT_3(stat_ln_beta, stat_ln_1mx, stat_ln_x);

    const double ln_pre_val = -ln_beta.val + a * ln_x.val + b * ln_1mx.val;
    const double ln_pre_err =  ln_beta.err + fabs(a*ln_x.err) + fabs(b*ln_1mx.err);
    const int stat_exp = gsl_sf_exp_err_e(ln_pre_val, ln_pre_err, &prefactor);

    if(stat_ln != GSL_SUCCESS) {
      result->val = 0.0;
      result->err = 0.0;
      GSL_ERROR ("error", GSL_ESANITY);
    }

    if(x < (a + 1.0)/(a+b+2.0)) {
      /* Apply continued fraction directly. */
      gsl_sf_result cf;
      const int stat_cf = beta_cont_frac(a, b, x, &cf);
      int stat;
      result->val = prefactor.val * cf.val / a;
      result->err = (fabs(prefactor.err * cf.val) + fabs(prefactor.val * cf.err))/a;

      stat = GSL_ERROR_SELECT_2(stat_exp, stat_cf);
      if(stat == GSL_SUCCESS) {
        CHECK_UNDERFLOW(result);
      }
      return stat;
    }
    else {
      /* Apply continued fraction after hypergeometric transformation. */
      gsl_sf_result cf;
      const int stat_cf = beta_cont_frac(b, a, 1.0-x, &cf);
      int stat;
      const double term = prefactor.val * cf.val / b;
      result->val  = 1.0 - term;
      result->err  = fabs(prefactor.err * cf.val)/b;
      result->err += fabs(prefactor.val * cf.err)/b;
      result->err += 2.0 * GSL_DBL_EPSILON * (1.0 + fabs(term));
      stat = GSL_ERROR_SELECT_2(stat_exp, stat_cf);
      if(stat == GSL_SUCCESS) {
        CHECK_UNDERFLOW(result);
      }
      return stat;
    }
  }
}

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

#include "eval.h"

double gsl_sf_beta_inc(const double a, const double b, const double x)
{
  EVAL_RESULT(gsl_sf_beta_inc_e(a, b, x, &result));
}


/**
  * Copyright 2019 José Manuel Abuín Mosquera <josemanuel.abuin@usc.es>
  *
  * This file is part of Matrix Market Suite.
  *
  * Matrix Market Suite 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 3 of the License, or
  * (at your option) any later version.
  *
  * Matrix Market Suite 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 Matrix Market Suite. If not, see <http://www.gnu.org/licenses/>.
  */

#include <cblas.h>
#include "ConjugateGradientSolver.h"
#include <time.h>

/* Include redundant execution header. */
#include "../../../include/ourRMTlib.h"

int ipow(int base, int exp)
{
    int result = 1;
    for (;;)
    {
        if (exp & 1)
            result *= base;
        exp >>= 1;
        if (!exp)
            break;
        base *= base;
    }

    return result;
}

int ConjugateGradientSolver(unsigned long *II, unsigned long *J, double *A, unsigned long M, unsigned long N,
	unsigned long long nz, double *b, unsigned long M_Vector, unsigned long N_Vector, unsigned long long nz_vector,
	double *x, int iterationNumber, int F, int *ret_code) {

	//A*x=b

        double *Ap=(double *) malloc(nz_vector * sizeof(double));
        double *r=(double *) malloc(nz_vector * sizeof(double));
        double *p=(double *) malloc(nz_vector * sizeof(double));

	//double *x=(double *) calloc(nz_vector,sizeof(double));

	//r = b-A*x
	//If we take x=0 the init multiplication is avoided and r=b

	memcpy(r, b, N*sizeof(double));

	//p=r

	memcpy(p, r, N*sizeof(double));

	//rsold = r'*r
	double rsold = cblas_ddot(N,r,1,r,1);
	double rs_0 = cblas_ddot(N,r,1,r,1);

	int stop = 0;

	double alphaCG = 0.0;

	double rsnew = 0.0, rsnew2 = 0.0;
	unsigned long k = 0, selfCG_k=0;

	unsigned long maxIterations = M*2;

	if(iterationNumber != 0 ){
		maxIterations = iterationNumber;
	}

	int flag=0;

	while(!stop){

		//if( ((k+1) % F == 0) ){ //execute CG reliably
		//if(k == pow((double)F, (double)(selfCG_k+1))){
		if(k == ipow(F, (selfCG_k+1))){
				//fprintf(stderr, "F:%d k:%lu selfCG_k:%lu \n",F,k, selfCG_k);

				//Ap=A*p
				cblas_dgemv(CblasRowMajor, CblasNoTrans, M,N , 1.0, A, N, p, 1, 0.0, Ap, 1);

				//r=A*x
				cblas_dgemv(CblasRowMajor, CblasNoTrans, M,N , 1.0, A, N, x, 1, 0.0, r, 1);

				//r=b-r
				cblas_dscal(N, -1, r, 1); //r = -r
				cblas_daxpy(N, 1, b, 1, r, 1); //r = r + b

				//alphaCG=r'*p /(p'*Ap)
				alphaCG = cblas_ddot(N,r,1,p,1)/cblas_ddot(N,p,1,Ap,1);

				//x=x+alphaCG*p
				cblas_daxpy(N,alphaCG,p,1,x,1);

				//r=r-alphaCG*Ap
				cblas_daxpy(N,-alphaCG,Ap,1,r,1);

				//rsnew = r'*r
				rsnew = cblas_ddot(N,r,1,r,1);

				//fprintf(stderr, "k:%ld F_iteration:%ld alphaCG:%.10f error:%.10f\n",k,selfCG_k,alphaCG, sqrt(rsnew));

				// p=r+((-r'*Ap/(p'*Ap))*p)
				cblas_dscal(N, -(cblas_ddot(N,r,1,Ap,1)/cblas_ddot(N,p,1,Ap,1)), p, 1);
				cblas_daxpy(N,1.0,r,1,p,1);

				selfCG_k++;
				flag=0;

		}else{

				//Ap=A*p

				cblas_dgemv(CblasRowMajor, CblasNoTrans, M,N , 1.0, A, N, p, 1, 0.0, Ap, 1);

				//alphaCG=rsold/(p'*Ap)
				alphaCG = rsold/cblas_ddot(N,p,1,Ap,1);

				//x=x+alphaCG*p
				cblas_daxpy(N,alphaCG,p,1,x,1);

				//r=r-alphaCG*Ap
				cblas_daxpy(N,-alphaCG,Ap,1,r,1);

				//rsnew = r'*r
				rsnew = cblas_ddot(N,r,1,r,1);
				//fprintf(stderr, "k:%ld  alphaCG:%.10f error:%.10f\n",k,alphaCG, sqrt(rsnew));

				//p=r+rsnew/rsold*p
				cblas_dscal(N, rsnew/rsold, p, 1);
				cblas_daxpy(N,1.0,r,1,p,1);

				rsold = rsnew;

				if(isnan(alphaCG)){ //checking whether alphaCG is nan!
					fprintf(stderr, "!!! alphaCG:%.10f\n", alphaCG);
					exit(-1);
				}
		}
		k++;

		//if((sqrt(rsnew)<=EPSILON)||(k == maxIterations)){
		if(sqrt(rsnew)/sqrt(rs_0)<=EPSILON){
			stop = 1;
			fprintf(stderr, "STOPPED by CG\n");
		}

	}

	//memcpy(b, x, N*sizeof(double));

        free(Ap);
        free(r);
        free(p);
	//free(x);

	fprintf(stderr, "[%s] Number of total iterations: %lu Number of iterations in SS step:%lu \n",__func__,k, selfCG_k);

    *ret_code=1;
	return 1;
}


/*
 * Copyright 2014 Marc Normandin
 *
 * 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.
 */

/*
 * pso.h
 *
 *  Created on: Jun 30, 2013
 *      Author: marc
 */

#ifndef PSO_H_
#define PSO_H_

#include <iostream>
#include <vector>
#include <algorithm>
#include <functional>
#include <cmath>
#include <cstdlib>
#include <gsl/gsl_rng.h>
#include <limits>
#include <fstream>
#include <cassert>

#include "rng.h"
#include "dim.h"
#include "particle.h"

template<class FitnessFunction>
class PSO
{
public:
    // This routine is used by the PSO unit test
    PSO(const unsigned int numParticles, const std::vector<Dim>& dim,
        const gslseed_t seed, FitnessFunction& fitnessFunction, const unsigned int maxIterations)
        : mNumParticles(numParticles), mGBest(dim), mDim(dim), mRng(seed), mFitnessFunction(fitnessFunction),
        mMaxIterations(maxIterations)
    {
        mParticles.reserve(mNumParticles);
    }

    unsigned int getNumParticles() const
    {
        return mNumParticles;
    }

    const Particle& iterate()
    {
        createRandomParticles();

        unsigned int numIterations = 0;
        std::vector<double> particleFitnesses(mParticles.size());
        do
        {
            // Evaluate the fitness/objective function
        	mFitnessFunction(mParticles, &particleFitnesses);

            //For each particle
            for (unsigned int i = 0; i < mParticles.size(); i++)
            {
                // Calculate fitness value
                const prob_t fitness = particleFitnesses[i]; //

                // If the fitness value is better than the best fitness value (pBest) in history
                // set current value as the new pBest
                mParticles[i].updateFitness( fitness );
            }

            // Choose the particle with the best fitness value of all the particles as the gBest
            std::vector<Particle>::const_iterator best = std::max_element(mParticles.begin(), mParticles.end());
            mGBest = Particle( best->getBestPosition(), best->getBestFitness() );

            // Update the inertia weight
            const double inertiaWeight = computeInertiaWeight(numIterations, mMaxIterations);

            // For each particle
            for (unsigned int i = 0; i < mParticles.size(); i++)
            {
                mParticles[i].updatePosition( mGBest, mDim, mCognitiveWeight, mSocialWeight, mRng, inertiaWeight );
            }

            numIterations++;
        }
        while(numIterations < mMaxIterations);

        return mGBest;
    }

protected:
    PSO(const PSO&);
    void operator=(const PSO&);

    void createRandomParticles() {
        mParticles.clear(); // Remove previous particles in the container

        // For each particle
        for (unsigned int i = 0; i < mNumParticles; i++)
        {
            std::vector<dim_t> pos;

            // For each dimension
            for (unsigned int d = 0; d < mDim.size(); d++)
            {
                dim_t posd = mRng.uniform( mDim[d].min(), mDim[d].max() );
                pos.push_back(posd);
            }

            // Add the particle to the collection
            mParticles.push_back( Particle(pos) );
        }
    }

    // Compute inertia. This is based on equation 4.1 from:
    // http://www.hindawi.com/journals/ddns/2010/462145/
    double computeInertiaWeight(const unsigned int iteration, const unsigned int maxInterations) const
    {
        // Note. We add +1 because our iterations go from 0 to max-1.
        double inertiaWeight = (mOmega1 - mOmega2) * ( (maxInterations - (iteration+1.0)) / (1.0 * (iteration+1.0) ) ) + mOmega2;
        return inertiaWeight;
    }

private:
    unsigned int            mNumParticles;
    std::vector<Particle> 	mParticles; // Particle positions
    Particle 				mGBest;
    std::vector<Dim>		mDim;

    // These values control how random the particle velocities are
    static const double 	mCognitiveWeight;
    static const double 	mSocialWeight;

    // These values control the rate of convergence
    static const double     mOmega1;
    static const double     mOmega2;

    RandomNumberGenerator 	mRng;

    FitnessFunction&        mFitnessFunction;
    unsigned int            mMaxIterations;
};

#endif /* PSO_H_ */


/* randist/binomial_tpe.c
 *
 * Copyright (C) 1996, 2003, 2007 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_pow_int.h>
#include <gsl/gsl_sf_gamma.h>

/* The binomial distribution has the form,

   f(x) =  n!/(x!(n-x)!) * p^x (1-p)^(n-x) for integer 0 <= x <= n
        =  0                               otherwise

   This implementation follows the public domain ranlib function
   "ignbin", the bulk of which is the BTPE (Binomial Triangle
   Parallelogram Exponential) algorithm introduced in
   Kachitvichyanukul and Schmeiser[1].  It has been translated to use
   modern C coding standards.

   If n is small and/or p is near 0 or near 1 (specifically, if
   n*min(p,1-p) < SMALL_MEAN), then a different algorithm, called
   BINV, is used which has an average runtime that scales linearly
   with n*min(p,1-p).

   But for larger problems, the BTPE algorithm takes the form of two
   functions b(x) and t(x) -- "bottom" and "top" -- for which b(x) <
   f(x)/f(M) < t(x), with M = floor(n*p+p).  b(x) defines a triangular
   region, and t(x) includes a parallelogram and two tails.  Details
   (including a nice drawing) are in the paper.

   [1] Kachitvichyanukul, V. and Schmeiser, B. W.  Binomial Random
   Variate Generation.  Communications of the ACM, 31, 2 (February,
   1988) 216.

   Note, Bruce Schmeiser (personal communication) points out that if
   you want very fast binomial deviates, and you are happy with
   approximate results, and/or n and n*p are both large, then you can
   just use gaussian estimates: mean=n*p, variance=n*p*(1-p).

   This implementation by James Theiler, April 2003, after obtaining
   permission -- and some good advice -- from Drs. Kachitvichyanukul
   and Schmeiser to use their code as a starting point, and then doing
   a little bit of tweaking.

   Additional polishing for GSL coding standards by Brian Gough.  */

#define SMALL_MEAN 14           /* If n*p < SMALL_MEAN then use BINV
                                   algorithm. The ranlib
                                   implementation used cutoff=30; but
                                   on my computer 14 works better */

#define BINV_CUTOFF 110         /* In BINV, do not permit ix too large */

#define FAR_FROM_MEAN 20        /* If ix-n*p is larger than this, then
                                   use the "squeeze" algorithm.
                                   Ranlib used 20, and this seems to
                                   be the best choice on my machine as
                                   well */

#define LNFACT(x) gsl_sf_lnfact(x)

inline static double
Stirling (double y1)
{
  double y2 = y1 * y1;
  double s =
    (13860.0 -
     (462.0 - (132.0 - (99.0 - 140.0 / y2) / y2) / y2) / y2) / y1 / 166320.0;
  return s;
}

unsigned int
gsl_ran_binomial_tpe (const gsl_rng * rng, double p, unsigned int n)
{
  return gsl_ran_binomial (rng, p, n);
}

unsigned int
gsl_ran_binomial (const gsl_rng * rng, double p, unsigned int n)
{
  int ix;                       /* return value */
  int flipped = 0;
  double q, s, np;

  if (n == 0)
    return 0;

  if (p > 0.5)
    {
      p = 1.0 - p;              /* work with small p */
      flipped = 1;
    }

  q = 1 - p;
  s = p / q;
  np = n * p;

  /* Inverse cdf logic for small mean (BINV in K+S) */

  if (np < SMALL_MEAN)
    {
      double f0 = gsl_pow_uint (q, n);   /* f(x), starting with x=0 */

      while (1)
        {
          /* This while(1) loop will almost certainly only loop once; but
           * if u=1 to within a few epsilons of machine precision, then it
           * is possible for roundoff to prevent the main loop over ix to
           * achieve its proper value.  following the ranlib implementation,
           * we introduce a check for that situation, and when it occurs,
           * we just try again.
           */

          double f = f0;
          double u = gsl_rng_uniform (rng);

          for (ix = 0; ix <= BINV_CUTOFF; ++ix)
            {
              if (u < f)
                goto Finish;
              u -= f;
              /* Use recursion f(x+1) = f(x)*[(n-x)/(x+1)]*[p/(1-p)] */
              f *= s * (n - ix) / (ix + 1);
            }

          /* It should be the case that the 'goto Finish' was encountered
           * before this point was ever reached.  But if we have reached
           * this point, then roundoff has prevented u from decreasing
           * all the way to zero.  This can happen only if the initial u
           * was very nearly equal to 1, which is a rare situation.  In
           * that rare situation, we just try again.
           *
           * Note, following the ranlib implementation, we loop ix only to
           * a hardcoded value of SMALL_MEAN_LARGE_N=110; we could have
           * looped to n, and 99.99...% of the time it won't matter.  This
           * choice, I think is a little more robust against the rare
           * roundoff error.  If n>LARGE_N, then it is technically
           * possible for ix>LARGE_N, but it is astronomically rare, and
           * if ix is that large, it is more likely due to roundoff than
           * probability, so better to nip it at LARGE_N than to take a
           * chance that roundoff will somehow conspire to produce an even
           * larger (and more improbable) ix.  If n<LARGE_N, then once
           * ix=n, f=0, and the loop will continue until ix=LARGE_N.
           */
        }
    }
  else
    {
      /* For n >= SMALL_MEAN, we invoke the BTPE algorithm */

      int k;

      double ffm = np + p;      /* ffm = n*p+p             */
      int m = (int) ffm;        /* m = int floor[n*p+p]    */
      double fm = m;            /* fm = double m;          */
      double xm = fm + 0.5;     /* xm = half integer mean (tip of triangle)  */
      double npq = np * q;      /* npq = n*p*q            */

      /* Compute cumulative area of tri, para, exp tails */

      /* p1: radius of triangle region; since height=1, also: area of region */
      /* p2: p1 + area of parallelogram region */
      /* p3: p2 + area of left tail */
      /* p4: p3 + area of right tail */
      /* pi/p4: probability of i'th area (i=1,2,3,4) */

      /* Note: magic numbers 2.195, 4.6, 0.134, 20.5, 15.3 */
      /* These magic numbers are not adjustable...at least not easily! */

      double p1 = floor (2.195 * sqrt (npq) - 4.6 * q) + 0.5;

      /* xl, xr: left and right edges of triangle */
      double xl = xm - p1;
      double xr = xm + p1;

      /* Parameter of exponential tails */
      /* Left tail:  t(x) = c*exp(-lambda_l*[xl - (x+0.5)]) */
      /* Right tail: t(x) = c*exp(-lambda_r*[(x+0.5) - xr]) */

      double c = 0.134 + 20.5 / (15.3 + fm);
      double p2 = p1 * (1.0 + c + c);

      double al = (ffm - xl) / (ffm - xl * p);
      double lambda_l = al * (1.0 + 0.5 * al);
      double ar = (xr - ffm) / (xr * q);
      double lambda_r = ar * (1.0 + 0.5 * ar);
      double p3 = p2 + c / lambda_l;
      double p4 = p3 + c / lambda_r;

      double var, accept;
      double u, v;              /* random variates */

    TryAgain:

      /* generate random variates, u specifies which region: Tri, Par, Tail */
      u = gsl_rng_uniform (rng) * p4;
      v = gsl_rng_uniform (rng);

      if (u <= p1)
        {
          /* Triangular region */
          ix = (int) (xm - p1 * v + u);
          goto Finish;
        }
      else if (u <= p2)
        {
          /* Parallelogram region */
          double x = xl + (u - p1) / c;
          v = v * c + 1.0 - fabs (x - xm) / p1;
          if (v > 1.0 || v <= 0.0)
            goto TryAgain;
          ix = (int) x;
        }
      else if (u <= p3)
        {
          /* Left tail */
          ix = (int) (xl + log (v) / lambda_l);
          if (ix < 0)
            goto TryAgain;
          v *= ((u - p2) * lambda_l);
        }
      else
        {
          /* Right tail */
          ix = (int) (xr - log (v) / lambda_r);
          if (ix > (double) n)
            goto TryAgain;
          v *= ((u - p3) * lambda_r);
        }

      /* At this point, the goal is to test whether v <= f(x)/f(m)
       *
       *  v <= f(x)/f(m) = (m!(n-m)! / (x!(n-x)!)) * (p/q)^{x-m}
       *
       */

      /* Here is a direct test using logarithms.  It is a little
       * slower than the various "squeezing" computations below, but
       * if things are working, it should give exactly the same answer
       * (given the same random number seed).  */

#ifdef DIRECT
      var = log (v);

      accept =
        LNFACT (m) + LNFACT (n - m) - LNFACT (ix) - LNFACT (n - ix)
        + (ix - m) * log (p / q);

#else /* SQUEEZE METHOD */

      /* More efficient determination of whether v < f(x)/f(M) */

      k = abs (ix - m);

      if (k <= FAR_FROM_MEAN)
        {
          /*
           * If ix near m (ie, |ix-m|<FAR_FROM_MEAN), then do
           * explicit evaluation using recursion relation for f(x)
           */
          double g = (n + 1) * s;
          double f = 1.0;

          var = v;

          if (m < ix)
            {
              int i;
              for (i = m + 1; i <= ix; i++)
                {
                  f *= (g / i - s);
                }
            }
          else if (m > ix)
            {
              int i;
              for (i = ix + 1; i <= m; i++)
                {
                  f /= (g / i - s);
                }
            }

          accept = f;
        }
      else
        {
          /* If ix is far from the mean m: k=ABS(ix-m) large */

          var = log (v);

          if (k < npq / 2 - 1)
            {
              /* "Squeeze" using upper and lower bounds on
               * log(f(x)) The squeeze condition was derived
               * under the condition k < npq/2-1 */
              double amaxp =
                k / npq * ((k * (k / 3.0 + 0.625) + (1.0 / 6.0)) / npq + 0.5);
              double ynorm = -(k * k / (2.0 * npq));
              if (var < ynorm - amaxp)
                goto Finish;
              if (var > ynorm + amaxp)
                goto TryAgain;
            }

          /* Now, again: do the test log(v) vs. log f(x)/f(M) */

#if USE_EXACT
          /* This is equivalent to the above, but is a little (~20%) slower */
          /* There are five log's vs three above, maybe that's it? */

          accept = LNFACT (m) + LNFACT (n - m)
            - LNFACT (ix) - LNFACT (n - ix) + (ix - m) * log (p / q);

#else /* USE STIRLING */
          /* The "#define Stirling" above corresponds to the first five
           * terms in asymptoic formula for
           * log Gamma (y) - (y-0.5)log(y) + y - 0.5 log(2*pi);
           * See Abramowitz and Stegun, eq 6.1.40
           */

          /* Note below: two Stirling's are added, and two are
           * subtracted.  In both K+S, and in the ranlib
           * implementation, all four are added.  I (jt) believe that
           * is a mistake -- this has been confirmed by personal
           * correspondence w/ Dr. Kachitvichyanukul.  Note, however,
           * the corrections are so small, that I couldn't find an
           * example where it made a difference that could be
           * observed, let alone tested.  In fact, define'ing Stirling
           * to be zero gave identical results!!  In practice, alv is
           * O(1), ranging 0 to -10 or so, while the Stirling
           * correction is typically O(10^{-5}) ...setting the
           * correction to zero gives about a 2% performance boost;
           * might as well keep it just to be pendantic.  */

          {
            double x1 = ix + 1.0;
            double w1 = n - ix + 1.0;
            double f1 = fm + 1.0;
            double z1 = n + 1.0 - fm;

            accept = xm * log (f1 / x1) + (n - m + 0.5) * log (z1 / w1)
              + (ix - m) * log (w1 * p / (x1 * q))
              + Stirling (f1) + Stirling (z1) - Stirling (x1) - Stirling (w1);
          }
#endif
#endif
        }

      if (var <= accept)
        {
          goto Finish;
        }
      else
        {
          goto TryAgain;
        }
    }

Finish:

  return (flipped) ? (n - ix) : (unsigned int)ix;
}