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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "fresnel.h"

#define acc 1e-8

/* Used for previous version of branching between cases 1a, 1b and 2 in ComputeInt */
/* static const double betathreshold = 1./10; */
/* static const double invbetathreshold = 10.; */
/* static const double lambdathreshold = 0.01; */

/* Used for previous version of branching between cases 1a, 1b and 2 in ComputeInt */
/* static const double factorbranchingcondition = 0.2154434690031884; */
/* static const double case1athreshold = 0.1; */
/* Used for case 0 - hoping to cover previous instability */
/* static const double case0threshold = 0.05; */

/* Used for branching between cases 1a, 1b, 2, 3 and 4 in ComputeInt */
/* FIXME: another round of refinement would be welcome, case 3 has some cases with 1e-4 error */
static const double p2threshold = 0.02;
static const double p1threshold1 = 0.1;
static const double p1threshold2 = 100.;
static const double p2p1slopethreshold = 0.2;

static const double coeffdeltaC[13] = {-0.1,-0.0462962962963,-0.0230769230769,-0.0136554621849,-0.00899470899471,-0.00636363636364,-0.00473664266768,-0.00366161616162,-0.00291467938527,-0.00237483953787,-0.0019721019721,-0.00166370896185,-0.00142235123367};
static const double coeffdeltaS[13] = {-0.0714285714286,-0.0318181818182,-0.0174603174603,-0.0109649122807,-0.00750988142292,-0.00546058879392,-0.00414746543779,-0.00325630252101,-0.00262408157145,-0.00215946843854,-0.00180809015222,-0.00153594771242,-0.0013209013209};

static const double fcoeff[12] = {0.3989422275318915, 1.839999368141345e-7, -0.2992429124371606, 0.0029447692036381, 2.48395059327971, 3.897790465813753, -140.2709114924737, 952.371347498219, -3660.703989590287, 8649.34061360465, -11740.93693284928, 7032.8915074629};
static const double gcoeff[12] = {0, 0.1994718004197629, -0.000125952146250442, -0.7386823670326993, -0.353224701618227, 19.45013452321292, -97.8846433217499, 221.1470704754588, -32.29876222016862, -1102.56036216157, 2554.184335427029, -1962.422566478277};

static double invn[12] = {1., 0.5, 0.3333333333333333, 0.25, 0.2, 0.1666666666666667, 0.1428571428571428, 0.125, 0.1111111111111111, 0.1, 0.0909090909090909, 0.0833333333333333};

/* Use for Chebyshev, case 3 */
static double ChebyshevNodes[6] = {0.965925826289068,0.7071067811865475,0.2588190451025207,-0.2588190451025207,-0.7071067811865475,-0.965925826289068};
static double ChebyshevWeights[6][6] = {{0.3333333333333333,0.3333333333333333,0.3333333333333333,0.3333333333333333,0.3333333333333333,0.3333333333333333},{0.3219752754296894,0.2357022603955158,0.0862730150341736,-0.0862730150341736,-0.2357022603955158,-0.3219752754296894},{0.2886751345948129,0.,-0.2886751345948129,-0.2886751345948129,0.,0.2886751345948129},{0.2357022603955158,-0.2357022603955158,-0.2357022603955158,0.2357022603955158,0.2357022603955158,-0.2357022603955158},{0.1666666666666667,-0.3333333333333333,0.1666666666666667,0.1666666666666667,-0.3333333333333333,0.1666666666666667},{0.0862730150341736,-0.2357022603955158,0.3219752754296894,-0.3219752754296894,0.2357022603955158,-0.0862730150341736}};

/* Function computing cexp(I*x)-1 without loss of accuracy*/
static double complex cexpm1i(double x){
  double rr;//rr=cos(x)-1=2*sin(x/2)**2
  double ri=sin(x);
  double sinhalf=sin(0.5*x);
  rr=-2.0*sinhalf*sinhalf;
  return rr+I*ri;
};

/* Function computing the Fresnel integral for any real (positive) number, with 1e-8 accuracy */
/* Reference used: Computation of Fresnel Integrals. II by K. Mielenz, [J. Res. Natl. Inst. Stand. Technol. 105, 589 (2000)] */
static double complex FresnelE(const double x) {
  double complex res;

  /* Check argument */
  if(x<0) return -FresnelE(-x);

  /* If x<2, Taylor expansion */
  else if(x<2) {
    double C = 0;
    double S = 0;
    double x3 = x*x*x;
    double x4 = x3*x;
    int n = 0;
    double deltaC = x;
    double deltaS = x3/3.;
    while(true) {
      C += deltaC;
      S += deltaS;
      //deltaC = -(4*n+1.)/(4*n+5.)/(2*n+1.)/(2*n+2.) * x4 * deltaC;
      //deltaS = -(4*n+3.)/(4*n+7.)/(2*n+2.)/(2*n+3.) * x4 * deltaS;
      deltaC = coeffdeltaC[n] * x4 * deltaC;
      deltaS = coeffdeltaS[n] * x4 * deltaS;
      n++;
      if( (fabs(deltaC)<acc && fabs(deltaS)<acc) || n>12) break;
    }
    res = C + I*S;
  }

  /* If x>2, Mielenz-Boersma formula using tabulated coeffs */
  else {
    double xinv = 1./x;
    double xinv2 = xinv*xinv;
    double y[12] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
    y[0] = xinv;
    for(int i=1; i<12; i++) y[i] = y[i-1]*xinv2;
    double f = 0;
    double g = 0;
    for(int i=0; i<12; i++) {
      f += fcoeff[i] * y[i];
      g += gcoeff[i] * y[i];
    }
    res = sqrt(PI/2) * ( (1+I)/2 - (g+I*f) * cexp(I*x*x));
  }

  return res;
}

/* In fact equivalent to Case 2 */
//static double complex ComputeIntCase0(
//   gsl_vector* coeffsAreal,         /* */
//   gsl_vector* coeffsAimag,         /* */
//   double p0,                       /* */
//   double p1,                       /* */
//   double p2,                       /* */
//   double eps)                      /* */
// {
//   /* Get poly coeffs and rescale by epsilon */
//   double complex coeffs[4] = {0.,0.,0.,0.};
//   double epspow[4] = {0.,0.,0.,0.};
//   epspow[0] = 1.; epspow[1] = eps; epspow[2] = eps*eps; epspow[3] = eps * epspow[2];
//   for(int i=0; i<=3; i++) {
//     coeffs[i] = epspow[i] * (gsl_vector_get(coeffsAreal, i+1) + I*gsl_vector_get(coeffsAimag, i+1));
//   }
//   double p0r = p0;
//   double p1r = p1*epspow[1];
//   double p2r = p2*epspow[2];
//
//   /* In case p2<0, conjugation */
//   int conjug = 0;
//   if(p2r<0) {
//     conjug=1;
//     p2r = -p2r;
//     p1r = -p1r;
//     p0r = -p0r;
//     for(int i=0; i<=3; i++) coeffs[i] = conj(coeffs[i]) ;
//   }
//
//   //
//   //printf("case0\n");
//
//   /* Prefactor and bounds after change of variable */
//   double sqrtp2r = sqrt(p2r);
//   double complex factor = 1./sqrtp2r*cexp(I*p0r);
//   double a = p1r/(2*sqrtp2r);
//   double b = p1r/(2*sqrtp2r) + sqrtp2r;
//   /* Coeffs in polynomial changed by shift */
//   double alpha = 1./sqrtp2r;
//   double beta = -p1r/(2*p2r);
//   double complex a0 = coeffs[0]; double complex a1 = coeffs[1]; double complex a2 = coeffs[2]; double complex a3 = coeffs[3];
//   double alpha2 = alpha*alpha; double beta2 = beta*beta;
//   double complex a0p = a0 + a1*beta + a2*beta2 + a3*beta2*beta;
//   double complex a1p = a1*alpha + 2*a2*alpha*beta + 3*a3*alpha*beta2;
//   double complex a2p = a2*alpha2 + 3*a3*alpha2*beta;
//   double complex a3p = a3*alpha2*alpha;
//
//   /* Result */
//   double complex res = 0.;
//   res = factor*((a0p + I/2*a2p) * (FresnelE(b) - FresnelE(a)) + (1./2*(a3p - I*a1p) - I/2*a2p*b - I/2*a3p*b*b) * cexp(I*b*b) - (1./2*(a3p - I*a1p) - I/2*a2p*a - I/2*a3p*a*a) * cexp(I*a*a));
//   //
//   //printf("%g, %g, %g \n", creal(factor), creal((a0p + I/2*a2p) * (FresnelE(b) - FresnelE(a))), creal(+ (1./2*(a3p - I*a1p) - I/2*a2p*b - I/2*a3p*b*b) * cexp(I*b*b) - (1./2*(a3p - I*a1p) - I/2*a2p*a - I/2*a3p*a*a) * cexp(I*a*a)));
//   //exit(0);
//
//   /* Remembering conjugation */
//   if(conjug) res = conj(res);
//   return res;
// }

double complex ComputeIntCase1a(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2,                       /* */
  const double scale)                    /* */
{
  /* Setting up the coefficients and the required accuracy */
  double complex coeffs[12] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
  for(int i=0; i<=3; i++) coeffs[i] = coeffsA[i];
  int maxj0 = 3;
  double acctol = 1.e-5/scale;

  /* Polynomial given by the expansion of cexp(I*p1*x) */
  double complex poly1[8] = {0.,0.,0.,0.,0.,0.,0.,0.};
  double p1abs = fabs(p1);
  double complex term1 = 1.; double term1absmax = 1.;
  int maxj1 = 0;
  for(int i=0; i<8; i++) {
    poly1[i] = term1;
    term1absmax = term1absmax * p1abs * invn[i]; /* Note: invn[i] = 1/(i+1) */
    if(term1absmax<acctol) break;
    term1 = term1 * I*p1 * invn[i];
    maxj1++;
  }

  /* Polynomial given by the expansion of cexp(I*p2*x2) */
  double complex poly2[11] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
  double p2abs = fabs(p2);
  double complex term2 = 1.; double term2absmax = 1.;
  int maxj2 = 0;
  for(int i=0; i<4; i++) {
    poly2[2*i] = term2;
    term2absmax = term2absmax * p2abs * invn[i]; /* Note: invn[i] = 1/(i+1) */
    if(term2absmax<acctol) break;
    term2 = term2 * I*p2 * invn[i];
    maxj2 += 2;
  }

  /* Multiplying in place the polynomials - we cut according to the fact that we allow for only 12 coeffs up to x^11 (assuming the rest would be negligible anyway), and we know the constant in poly1 and poly2 is always 1. */
  for(int i=min(11, maxj0+maxj1); i>=0; i--) {
    for(int j=max(1, i-maxj0); j<=min(i, maxj1); j++) {
      coeffs[i] += coeffs[i-j] * poly1[j];
    }
  }
  maxj0 = min(11, maxj0 + maxj1);
  for(int i=min(11, maxj0+maxj2); i>=0; i--) {
    for(int j=max(1, i-maxj0); j<=min(i, maxj2); j++) {
      coeffs[i] += coeffs[i-j] * poly2[j];
    }
  }
  maxj0 = min(11, maxj0 + maxj2);

  /* Computing the integral itself */
  double complex res = 0.;
  for(int i=0; i<=maxj0; i++) {
    res += coeffs[i] * invn[i]; /* Note: invn[i] = 1/(i+1) */
  }
  return res;
}

double complex ComputeIntCase1b(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2,                       /* */
  const double scale)                    /* */
{
  /* Setting up the coefficients and the required accuracy */
  double complex coeffs[12] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
  for(int i=0; i<=3; i++) coeffs[i] = coeffsA[i];
  int maxj0 = 3;
  double acctol = 1.e-5/scale;

  /* Polynomial given by the expansion of cexp(I*p2*x2) */
  double complex poly2[11] = {0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.};
  double p2abs = fabs(p2);
  double complex term2 = 1.; double term2absmax = 1.;
  int maxj2 = 0;
  for(int i=0; i<4; i++) {
    poly2[2*i] = term2;
    term2absmax = term2absmax * p2abs * invn[i]; /* Note: invn[i] = 1/(i+1) */
    if(term2absmax<acctol) break;
    term2 = term2 * I*p2 * invn[i];
    maxj2 += 2;
  }

  /* Multiplying in place the polynomials - we cut according to the fact that we allow for only 12 coeffs up to x^11 (assuming the rest would be negligible anyway), and we know the constant in poly2 is always 1. */
  for(int i=min(11, maxj0+maxj2); i>=0; i--) {
    for(int j=max(1, i-maxj0); j<=min(i, maxj2); j++) {
      coeffs[i] += coeffs[i-j] * poly2[j];
    }
  }
  maxj0 = min(11, maxj0 + maxj2);

  /* Go down the recursion relation */
  double invp1 = 1./p1;
  double complex coeffE1 = 0.; double complex coeffE1minus1 = 0;
  for(int i=maxj0; i>=1; i--) {
    coeffE1 += -I*invp1 * coeffs[i];
    coeffs[i-1] += I*invp1 * i * coeffs[i];
  }
  coeffE1minus1 = -I*invp1 * coeffs[0];
  /* Result */
  double complex cexpm1ip1 = cexpm1i(p1);
  double complex res = (coeffE1*(cexpm1ip1+1.) + coeffE1minus1*cexpm1ip1);
  return res;
}

double complex ComputeIntCase2(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2)                       /* */
{
  /* Setting up the coefficients */
  double complex coeffs[4] = {0.,0.,0.,0.};
  for(int i=0; i<=3; i++) coeffs[i] = coeffsA[i];
  double complex coeffE2 = 0; double complex coefffresnel = 0; double complex constant = 0;
  /* Conjugate if required */
  int conjug = 0; double p1b = p1; double p2b = p2;
  if(p2<0) { conjug = 1; p1b = -p1; p2b = -p2; for(int i=0; i<=3; i++) coeffs[i] = conj(coeffs[i]); }

  /* n=3 and n=2 */
  double inv2p2 = 1. / (2.*p2b);
  for(int i=3; i>=2; i--) {
    coeffE2 += -I*inv2p2 * coeffs[i];
    coeffs[i-2] += I*inv2p2 * (i-1) * coeffs[i];
    coeffs[i-1] += -p1b * inv2p2 * coeffs[i];
  }
  /* n=1 */
  coeffE2 += -I*inv2p2 * coeffs[1];
  constant = I*inv2p2 * coeffs[1];
  coefffresnel += -p1b * inv2p2 * coeffs[1];
  /* n=0 */
  coefffresnel += coeffs[0];

  /* Fresnel integral and complex exponential */
  double complex E2 = cexp(I * (p1b + p2b));
  double sqrtp2 = sqrt(p2b);
  double invsqrtp2 = 1./sqrtp2;
  double halfratio = 0.5 * p1b * invsqrtp2;
  double complex fresnel = cexp(-I*halfratio*halfratio) * invsqrtp2 * (FresnelE(halfratio + sqrtp2) - FresnelE(halfratio));

  /* Result */
  double complex res = (constant + coeffE2*E2 + coefffresnel*fresnel);
  if(conjug) res = conj(res); /* Remember possible conjugation */
  return res;
}

double complex ComputeIntCase3(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2)                       /* */
{
  /* Prepare change of variables */
  double r = p2/p1;

  /* Compute integrand F on nodes */
  double Fvalues[6] = {0.,0.,0.,0.,0.,0.};
  double xk; double y; double sqrtterm; double factor; double x;
  for(int i=0; i<6; i++) {
    xk = ChebyshevNodes[i];
    y = (1.+xk)/2.;
    sqrtterm = sqrt(1. + 4.*r*(1.+r)*y);
    factor = (1.+r)/sqrtterm;
    x = (2.*(1.+r)*y)/(1. + sqrtterm);
    Fvalues[i] = coeffsA[3];
    for(int j=2; j>=0; j--) Fvalues[i] = coeffsA[j] + x*Fvalues[i];
    Fvalues[i] = factor * Fvalues[i];
  }

  /* Compute Chebyshev coefficients ci */
  double complex c[6] = {0.,0.,0.,0.,0.,0.};
  for(int i=0; i<6; i++) {
    for(int j=0; j<6; j++) c[i] += ChebyshevWeights[i][j] * Fvalues[j];
  }

  /* Coefficients of polynomial in 1/(I*p), p=p1*(1+r) */
  double p = p1*(1.+r);
  double complex coeffszcos[6] = {0.,0.,0.,0.,0.,0.};
  double complex coeffszsin[6] = {0.,0.,0.,0.,0.,0.};
  coeffszcos[0] = c[1] + c[3] + c[5];
  coeffszcos[1] = -8.*c[2] - 32.*c[4];
  coeffszcos[2] = 96.*c[3] + 800.*c[5];
  coeffszcos[3] = -1536.*c[4];
  coeffszcos[4] = 30720.*c[5];
  coeffszcos[5] = 0.;
  coeffszsin[0] = 0.5*c[0] + c[2] + c[4];
  coeffszsin[1] = -2.*c[1] - 18.*c[3] - 50.*c[5];
  coeffszsin[2] = 16.*c[2] + 320.*c[4];
  coeffszsin[3] = -192.*c[3] -6720.*c[5];
  coeffszsin[4] = 3072.*c[4];
  coeffszsin[5] = -61440.*c[5];

  /* Coefficients of cos(p/2) and sin(p/2) */
  double complex z = 1./(I*p);
  double complex coeffcos = coeffszcos[5];
  for(int i=4; i>=0; i--) coeffcos = coeffszcos[i] + z*coeffcos;
  double complex coeffsin = coeffszsin[5];
  for(int i=4; i>=0; i--) coeffsin = coeffszsin[i] + z*coeffsin;
  coeffcos = z * coeffcos;
  coeffsin = z * coeffsin;

  /* Result */
  double pov2 = p/2.;
  return cexp(I*pov2) * (2*cos(pov2)*coeffcos + 2*I*sin(pov2)*coeffsin);
}

double complex ComputeIntCase4(
  const double complex* coeffsA,         /* */
  const double p1,                       /* */
  const double p2)                       /* */
{
  double complex a0 = coeffsA[0];
  double complex a1 = coeffsA[1];
  double complex a2 = coeffsA[2];
  double complex a3 = coeffsA[3];
  double r = 2.*p2/p1;
  double inv1plusr = 1./(1.+r); double inv1plusr2 = inv1plusr*inv1plusr;
  double inv1plusr4 = inv1plusr2*inv1plusr2;
  double invp1 = 1./p1; double invp12 = invp1*invp1; double invp13 = invp12*invp1;

  double complex term1 = cexp(I*(p1 + p2))*(-I*invp1*(a0 + a1 + a2 + a3)*inv1plusr + invp12*(a1 + 2.*a2 + 3.*a3)*inv1plusr2 + I*invp13*(2.*a2 + 6.*a3 + r*(3.*a3 - a1))*inv1plusr4);
  double complex term2 = - (-I*invp1*a0 + invp12*a1 + I*invp13*(2.*a2 - a1*r));
  return term1 + term2;
}

double complex ComputeInt(
  gsl_matrix* splinecoeffsAreal,         /*  */
  gsl_matrix* splinecoeffsAimag,         /*  */
  gsl_matrix* quadsplinecoeffsphase)     /*  */
{
  double complex res = 0.;
  double complex resint = 0.;
  /* Number of points - i.e. nb of intervals + 1 */
  /* Assumes that the dimensions match and that the frequency vectors are the same between the different splinecoeffs */
  int nbpts = (int) quadsplinecoeffsphase->size1;

  //clock_t tbegtotal = clock();
  for(int j=0; j<nbpts-1; j++) {
    double eps = gsl_matrix_get(quadsplinecoeffsphase, j+1, 0) - gsl_matrix_get(quadsplinecoeffsphase, j, 0);
    double epspow[4];
    epspow[0] = 1.;
    epspow[1] = eps;
    epspow[2] = eps*eps;
    epspow[3] = eps*epspow[2];
    double p0 = gsl_matrix_get(quadsplinecoeffsphase, j, 1);

    /* Rescale the interval to [0,1] and scale out the constant amplitude term */
    double p1 = gsl_matrix_get(quadsplinecoeffsphase, j, 2) * eps;
    double p2 = gsl_matrix_get(quadsplinecoeffsphase, j, 3) * epspow[2];
    double complex A0 = gsl_matrix_get(splinecoeffsAreal, j, 1) + I*gsl_matrix_get(splinecoeffsAimag, j, 1);
    double complex A0inv = 1./A0;
    double A0abs = cabs(A0); /* Used for accuracy requirement of adaptive Taylor expansions Ia and Ib */
    double complex coeffsA[4] = {0.,0.,0.,0.};
    coeffsA[0] = 1.;
    for(int i=1; i<=3; i++) coeffsA[i] = epspow[i] * A0inv * (gsl_matrix_get(splinecoeffsAreal, j, i+1) + I*gsl_matrix_get(splinecoeffsAimag, j, i+1));

    /* Factor scaled out */
    double complex factor = eps * A0 * cexp(I*p0);

    double absp1 = fabs(p1); double absp2 = fabs(p2);
    if(absp1<p1threshold1 && absp2<p2threshold) {
      resint = factor * ComputeIntCase1a(coeffsA, p1, p2, A0abs);
    }
    else if(p1threshold1<=absp1 && absp1<p1threshold2 && absp2<p2threshold) {
      resint = factor * ComputeIntCase1b(coeffsA, p1, p2, A0abs);
    }
    else if(absp2>=p2p1slopethreshold*absp1 && absp2>=p2threshold) {
      resint = factor * ComputeIntCase2(coeffsA, p1, p2);
    }
    else if(absp2<p2p1slopethreshold*absp1 && absp2>=p2threshold && absp1<p1threshold2) {
      resint = factor * ComputeIntCase3(coeffsA, p1, p2);
    }
    else {
      resint = factor * ComputeIntCase4(coeffsA, p1, p2);
    }
    res += resint;
  }

  return res;
}


/* ode-initval/rk8pd.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.
 */

/* Runge-Kutta 8(9), Prince-Dormand
 *
 * High Order Embedded Runge-Kutta Formulae
 * P.J. Prince and J.R. Dormand
 * J. Comp. Appl. Math.,7, pp. 67-75, 1981
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

/* Prince-Dormand constants */

static const double Abar[] = {
  14005451.0 / 335480064.0,
  0.0,
  0.0,
  0.0,
  0.0,
  -59238493.0 / 1068277825.0,
  181606767.0 / 758867731.0,
  561292985.0 / 797845732.0,
  -1041891430.0 / 1371343529.0,
  760417239.0 / 1151165299.0,
  118820643.0 / 751138087.0,
  -528747749.0 / 2220607170.0,
  1.0 / 4.0
};

static const double A[] = {
  13451932.0 / 455176623.0,
  0.0,
  0.0,
  0.0,
  0.0,
  -808719846.0 / 976000145.0,
  1757004468.0 / 5645159321.0,
  656045339.0 / 265891186.0,
  -3867574721.0 / 1518517206.0,
  465885868.0 / 322736535.0,
  53011238.0 / 667516719.0,
  2.0 / 45.0
};

static const double ah[] = {
  1.0 / 18.0,
  1.0 / 12.0,
  1.0 / 8.0,
  5.0 / 16.0,
  3.0 / 8.0,
  59.0 / 400.0,
  93.0 / 200.0,
  5490023248.0 / 9719169821.0,
  13.0 / 20.0,
  1201146811.0 / 1299019798.0
};

static const double b21 = 1.0 / 18.0;
static const double b3[] = { 1.0 / 48.0, 1.0 / 16.0 };
static const double b4[] = { 1.0 / 32.0, 0.0, 3.0 / 32.0 };
static const double b5[] = { 5.0 / 16.0, 0.0, -75.0 / 64.0, 75.0 / 64.0 };
static const double b6[] = { 3.0 / 80.0, 0.0, 0.0, 3.0 / 16.0, 3.0 / 20.0 };
static const double b7[] = {
  29443841.0 / 614563906.0,
  0.0,
  0.0,
  77736538.0 / 692538347.0,
  -28693883.0 / 1125000000.0,
  23124283.0 / 1800000000.0
};
static const double b8[] = {
  16016141.0 / 946692911.0,
  0.0,
  0.0,
  61564180.0 / 158732637.0,
  22789713.0 / 633445777.0,
  545815736.0 / 2771057229.0,
  -180193667.0 / 1043307555.0
};
static const double b9[] = {
  39632708.0 / 573591083.0,
  0.0,
  0.0,
  -433636366.0 / 683701615.0,
  -421739975.0 / 2616292301.0,
  100302831.0 / 723423059.0,
  790204164.0 / 839813087.0,
  800635310.0 / 3783071287.0
};
static const double b10[] = {
  246121993.0 / 1340847787.0,
  0.0,
  0.0,
  -37695042795.0 / 15268766246.0,
  -309121744.0 / 1061227803.0,
  -12992083.0 / 490766935.0,
  6005943493.0 / 2108947869.0,
  393006217.0 / 1396673457.0,
  123872331.0 / 1001029789.0
};
static const double b11[] = {
  -1028468189.0 / 846180014.0,
  0.0,
  0.0,
  8478235783.0 / 508512852.0,
  1311729495.0 / 1432422823.0,
  -10304129995.0 / 1701304382.0,
  -48777925059.0 / 3047939560.0,
  15336726248.0 / 1032824649.0,
  -45442868181.0 / 3398467696.0,
  3065993473.0 / 597172653.0
};
static const double b12[] = {
  185892177.0 / 718116043.0,
  0.0,
  0.0,
  -3185094517.0 / 667107341.0,
  -477755414.0 / 1098053517.0,
  -703635378.0 / 230739211.0,
  5731566787.0 / 1027545527.0,
  5232866602.0 / 850066563.0,
  -4093664535.0 / 808688257.0,
  3962137247.0 / 1805957418.0,
  65686358.0 / 487910083.0
};
static const double b13[] = {
  403863854.0 / 491063109.0,
  0.0,
  0.0,
  -5068492393.0 / 434740067.0,
  -411421997.0 / 543043805.0,
  652783627.0 / 914296604.0,
  11173962825.0 / 925320556.0,
  -13158990841.0 / 6184727034.0,
  3936647629.0 / 1978049680.0,
  -160528059.0 / 685178525.0,
  248638103.0 / 1413531060.0,
  0.0
};

typedef struct
{
  double *k[13];
  double *ytmp;
  double *y0;
}
rk8pd_state_t;

static void *
rk8pd_alloc (size_t dim)
{
  rk8pd_state_t *state = (rk8pd_state_t *) malloc (sizeof (rk8pd_state_t));
  int i, j;

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk8pd_state", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->ytmp);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  for (i = 0; i < 13; i++)
    {
      state->k[i] = (double *) malloc (dim * sizeof (double));

      if (state->k[i] == 0)
        {
          for (j = 0; j < i; j++)
            {
              free (state->k[j]);
            }
          free (state->y0);
          free (state->ytmp);
          free (state);
          GSL_ERROR_NULL ("failed to allocate space for k's", GSL_ENOMEM);
        }
    }

  return state;
}

static int
rk8pd_apply (void *vstate,
             size_t dim,
             double t,
             double h,
             double y[],
             double yerr[],
             const double dydt_in[],
             double dydt_out[], const gsl_odeiv_system * sys)
{
  rk8pd_state_t *state = (rk8pd_state_t *) vstate;

  size_t i;

  double *const ytmp = state->ytmp;
  double *const y0 = state->y0;
  /* Note that k1 is stored in state->k[0] due to zero-based indexing */
  double *const k1 = state->k[0];
  double *const k2 = state->k[1];
  double *const k3 = state->k[2];
  double *const k4 = state->k[3];
  double *const k5 = state->k[4];
  double *const k6 = state->k[5];
  double *const k7 = state->k[6];
  double *const k8 = state->k[7];
  double *const k9 = state->k[8];
  double *const k10 = state->k[9];
  double *const k11 = state->k[10];
  double *const k12 = state->k[11];
  double *const k13 = state->k[12];

  DBL_MEMCPY (y0, y, dim);

  /* k1 step */
  if (dydt_in != NULL)
    {
      DBL_MEMCPY (k1, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);

      if (s != GSL_SUCCESS)
	{
	  return s;
	}
    }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + b21 * h * k1[i];

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[0] * h, ytmp, k2);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b3[0] * k1[i] + b3[1] * k2[i]);

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[1] * h, ytmp, k3);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b4[0] * k1[i] + b4[2] * k3[i]);

  /* k4 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[2] * h, ytmp, k4);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b5[0] * k1[i] + b5[2] * k3[i] + b5[3] * k4[i]);

  /* k5 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[3] * h, ytmp, k5);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b6[0] * k1[i] + b6[3] * k4[i] + b6[4] * k5[i]);

  /* k6 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[4] * h, ytmp, k6);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b7[0] * k1[i] + b7[3] * k4[i] + b7[4] * k5[i] +
                  b7[5] * k6[i]);

  /* k7 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[5] * h, ytmp, k7);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b8[0] * k1[i] + b8[3] * k4[i] + b8[4] * k5[i] +
                  b8[5] * k6[i] + b8[6] * k7[i]);

  /* k8 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[6] * h, ytmp, k8);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b9[0] * k1[i] + b9[3] * k4[i] + b9[4] * k5[i] +
                  b9[5] * k6[i] + b9[6] * k7[i] + b9[7] * k8[i]);

  /* k9 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[7] * h, ytmp, k9);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b10[0] * k1[i] + b10[3] * k4[i] + b10[4] * k5[i] +
                  b10[5] * k6[i] + b10[6] * k7[i] + b10[7] * k8[i] +
                  b10[8] * k9[i]);

  /* k10 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[8] * h, ytmp, k10);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b11[0] * k1[i] + b11[3] * k4[i] + b11[4] * k5[i] +
                  b11[5] * k6[i] + b11[6] * k7[i] + b11[7] * k8[i] +
                  b11[8] * k9[i] + b11[9] * k10[i]);

  /* k11 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[9] * h, ytmp, k11);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b12[0] * k1[i] + b12[3] * k4[i] + b12[4] * k5[i] +
                  b12[5] * k6[i] + b12[6] * k7[i] + b12[7] * k8[i] +
                  b12[8] * k9[i] + b12[9] * k10[i] + b12[10] * k11[i]);

  /* k12 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k12);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b13[0] * k1[i] + b13[3] * k4[i] + b13[4] * k5[i] +
                  b13[5] * k6[i] + b13[6] * k7[i] + b13[7] * k8[i] +
                  b13[8] * k9[i] + b13[9] * k10[i] + b13[10] * k11[i] +
                  b13[11] * k12[i]);

  /* k13 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp, k13);

    if (s != GSL_SUCCESS)
      {
	return s;
      }
  }

  /* final sum  */
  for (i = 0; i < dim; i++)
    {
      const double ksum8 =
        Abar[0] * k1[i] + Abar[5] * k6[i] + Abar[6] * k7[i] +
        Abar[7] * k8[i] + Abar[8] * k9[i] + Abar[9] * k10[i] +
        Abar[10] * k11[i] + Abar[11] * k12[i] + Abar[12] * k13[i];
      y[i] += h * ksum8;
    }

  /* Evaluate dydt_out[]. */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
	{
	  /* Restore initial values */
	  DBL_MEMCPY (y, y0, dim);
	  return s;
	}
    }

  /* error estimate */
  for (i = 0; i < dim; i++)
    {
      const double ksum8 =
        Abar[0] * k1[i] + Abar[5] * k6[i] + Abar[6] * k7[i] +
        Abar[7] * k8[i] + Abar[8] * k9[i] + Abar[9] * k10[i] +
        Abar[10] * k11[i] + Abar[11] * k12[i] + Abar[12] * k13[i];
      const double ksum7 =
        A[0] * k1[i] + A[5] * k6[i] + A[6] * k7[i] + A[7] * k8[i] +
        A[8] * k9[i] + A[9] * k10[i] + A[10] * k11[i] + A[11] * k12[i];
      yerr[i] = h * (ksum7 - ksum8);
    }

  return GSL_SUCCESS;
}

static int
rk8pd_reset (void *vstate, size_t dim)
{
  rk8pd_state_t *state = (rk8pd_state_t *) vstate;

  int i;

  for (i = 0; i < 13; i++)
    {
      DBL_ZERO_MEMSET (state->k[i], dim);
    }

  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk8pd_order (void *vstate)
{
  rk8pd_state_t *state = (rk8pd_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 8;
}

static void
rk8pd_free (void *vstate)
{
  rk8pd_state_t *state = (rk8pd_state_t *) vstate;
  int i;

  for (i = 0; i < 13; i++)
    {
      free (state->k[i]);
    }
  free (state->y0);
  free (state->ytmp);
  free (state);
}

static const gsl_odeiv_step_type rk8pd_type = { "rk8pd",        /* name */
  1,                            /* can use dydt_in */
  1,                            /* gives exact dydt_out */
  &rk8pd_alloc,
  &rk8pd_apply,
  &rk8pd_reset,
  &rk8pd_order,
  &rk8pd_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk8pd = &rk8pd_type;


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

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

#include "dnest.h"
#include "model1.h"

int which_level_update;
int num_params;

DNestFptrSet *fptrset_thismodel;

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

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

  /* setup szie of modeltype, which is used for dnest */
  num_params = 20;

  /* allocate memory */
  fptrset_thismodel = dnest_malloc_fptrset();

  /* setup functions used for dnest*/
  fptrset_thismodel->from_prior = from_prior_thismodel;
  fptrset_thismodel->log_likelihoods_cal = log_likelihoods_cal_thismodel;
  fptrset_thismodel->log_likelihoods_cal_initial = log_likelihoods_cal_thismodel;
  fptrset_thismodel->log_likelihoods_cal_restart = log_likelihoods_cal_thismodel;
  fptrset_thismodel->perturb = perturb_thismodel;
  fptrset_thismodel->print_particle = print_particle_thismodel;
  fptrset_thismodel->restart_action = restart_action_model1;

  /* run dnest */
  dnest(argc, argv, fptrset_thismodel, num_params, NULL, NULL, NULL, "./", "OPTIONS1", NULL, NULL);

  /* free memory */
  dnest_free_fptrset(fptrset_thismodel);

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

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

void from_prior_thismodel(void *model)
{
  int i;
  double *params = (double *)model;
  for(i=0; i<num_params; i++)
  {
    params[i] = -0.5 + dnest_rand();
  }
}

double log_likelihoods_cal_thismodel(const void *model)
{
  double *params = (double *)model;
  double logL;
  const double u = 0.01;
	const double v = 0.1;
	const double C = log(1.0/sqrt(2*M_PI));

	double logl1 = num_params*(C - log(u));
	double logl2 = num_params*(C - log(v));

  int i;
	for(i=0; i<num_params; i++)
	{
		logl1 += -0.5*pow(( params[i] - 0.031)/u, 2);
		logl2 += -0.5*pow(  params[i]/v, 2);
	}
	logl1 += log(100.);

  double max = fmax(logl1, logl2);

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

  return logL;
}

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

  which_level_update = dnest_get_which_level_update();
  size_levels = dnest_get_size_levels();
  which_level = which_level_update > (size_levels - 20)?(size_levels-20):which_level_update;
  if(which_level > 0)
  {
    limit1 = limits[(which_level-1) * num_params *2 + which *2 ];
    limit2 = limits[(which_level-1) * num_params *2 + which *2 + 1];
  }
  else
  {
    limit1 = -0.5;
    limit2 = 0.5;
  }

  width = (limit2 - limit1);
	params[which] += width * dnest_randh();
	dnest_wrap(&params[which], limit1, limit2);
  return logH;
}

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

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

void restart_action_model1(int iflag)
{
  return;
}


/**
 *
 * @file core_sgetrf_rectil.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Piotr Luszczek
 * @date 2009-11-15
 *
 * @generated s Tue Jan  7 11:44:48 2014
 *
 **/

#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include "common.h"

#define A(m, n) BLKADDR(A, float, m, n)

void
CORE_sgetrf_rectil_init(void);

static inline void
CORE_sgetrf_rectil_rec(const PLASMA_desc A, int *IPIV, int *info,
                       float *pivot,
                       int thidx,  int thcnt,
                       int column, int width,
                       int ft,     int lt);
static inline void
CORE_sgetrf_rectil_update(const PLASMA_desc A, int *IPIV,
                          int column, int n1,     int n2,
                          int thidx,  int thcnt,
                          int ft,     int lt);

/***************************************************************************//**
 *
 * @ingroup CORE_float
 *
 *  CORE_sgetrf_rectil computes a LU factorization of a general M-by-N
 *  matrix A stored in CCRB layout using partial pivoting with row
 *  interchanges.
 *
 *  The factorization has the form
 *
 *    A = P * L * U
 *
 *  where P is a permutation matrix, L is lower triangular with unit
 *  diagonal elements (lower trapezoidal if m > n), and U is upper
 *  triangular (upper trapezoidal if m < n).
 *
 *  This is the recursive version of the algorithm applied on tile layout.
 *
 *  WARNINGS:
 *     - The function CORE_sgetrf_rectil_init has to be called prior
 *     to any call to this function.
 *     - You cannot call this kernel on different matrices at the same
 *     time.
 *     - The matrix A cannot be more than one tile wide.
 *     - The number of threads calling this function has to be excatly
 *     the number defined by info[2] with each one of them a different
 *     index between 0 included and info[2] excluded.
 *
 *******************************************************************************
 *
 *  @param[in,out] A
 *          PLASMA descriptor of the matrix A to be factorized.
 *          On entry, the M-by-N matrix to be factorized.
 *          On exit, the factors L and U from the factorization
 *          A = P*L*U; the unit diagonal elements of L are not stored.
 *
 *  @param[out] IPIV
 *          The pivot indices; for 0 <= i < min(M,N) stored in Fortran
 *          mode (starting at 1), row i of the matrix was interchanged
 *          with row IPIV(i).
 *          On exit, each value IPIV[i] for 0 <= i < min(M,N) is
 *          increased by A.i, which means A.i < IPIV[i] <= A.i+M.
 *
 *  @param[in,out] info
 *          Array of 3 integers.
 *          - info[0], see returned value
 *          - info[1], is the thread index 0 <= info[0] < info[2]
 *          - info[2], on entry is the number of threads trying to
 *                     participate to the factorization,
 *                     on exit is the real number of threads used to
 *                     perform the factorization.
 *    Info[2] threads, and exactly info[2], have to call this function
 *    to avoid dead lock.
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval -k, the k-th argument had an illegal value
 *          \retval k if U(k,k) is exactly zero. The factorization
 *                  has been completed, but the factor U is exactly
 *                  singular, and division by zero will occur if it is used
 *                  to solve a system of equations.
 *
 */
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgetrf_rectil = PCORE_sgetrf_rectil
#define CORE_sgetrf_rectil PCORE_sgetrf_rectil
#endif
int CORE_sgetrf_rectil(const PLASMA_desc A, int *IPIV, int *info)
{
    int ft, lt;
    int thidx = info[1];
    int thcnt = min( info[2], A.mt );
    int minMN = min( A.m, A.n );
    float pivot;

    info[0] = 0;
    info[2] = thcnt;

    if ( A.nt > 1 ) {
        coreblas_error(1, "Illegal value of A.nt");
        info[0] = -1;
        return -1;
    }

    if ( thidx >= thcnt )
      return 0;

    int q = A.mt / thcnt;
    int r = A.mt % thcnt;

    if (thidx < r) {
        q++;
        ft = thidx * q;
        lt = ft + q;
    } else {
        ft = r * (q + 1) + (thidx - r) * q;
        lt = ft + q;
        lt = min( lt, A.mt );
    }

    CORE_sgetrf_rectil_rec( A, IPIV, info, &pivot,
                            thidx, thcnt, 0, minMN, ft, lt);

    if ( A.n > minMN ) {
        CORE_sgetrf_rectil_update( A, IPIV,
                                   0, minMN, A.n-minMN,
                                   thidx, thcnt,
                                   ft, lt);
    }

    return info[0];
}

/*******************************************************************
 *   Additional routines
 */
#define AMAX1BUF_SIZE (48 << 1)

/* 48 threads should be enough for everybody */
static volatile float CORE_samax1buf[AMAX1BUF_SIZE];
static float sfmin;

void
CORE_sgetrf_rectil_init(void)
{
    int i;
    for (i = 0; i < AMAX1BUF_SIZE; ++i) CORE_samax1buf[i] = -1.0;
    sfmin =  LAPACKE_slamch_work('S');
}

static void
CORE_samax1_thread(float localamx,
                   int thidx, int thcnt, int *thwinner,
                   float *diagvalue,
                   float *globalamx,
                   int pividx, int *ipiv)
{
    if (thidx == 0) {
        int i, j = 0;
        float curval = localamx, tmp;
        float curamx = fabsf(localamx);

        /* make sure everybody filled in their value */
        for (i = 1; i < thcnt; ++i) {
            while (CORE_samax1buf[i << 1] == -1.0) { /* wait for thread i to store its value */
            }
        }

        /* better not fuse the loop above and below to make sure data is sync'd */

        for (i = 1; i < thcnt; ++i) {
            tmp = CORE_samax1buf[ (i << 1) + 1];
            if (fabsf(tmp) > curamx) {
                curamx = fabsf(tmp);
                curval = tmp;
                j = i;
            }
        }

        if (0 == j)
            ipiv[0] = pividx;

        /* make sure everybody knows the amax value */
        for (i = 1; i < thcnt; ++i)
            CORE_samax1buf[ (i << 1) + 1] = curval;

        CORE_samax1buf[0] = -j - 2.0; /* set the index of the winning thread */
        CORE_samax1buf[1] = *diagvalue; /* set the index of the winning thread */

        *thwinner = j;
        *globalamx = curval;

        for (i = 1; i < thcnt; ++i)
            CORE_samax1buf[i << 1] = -3.0;

        /* make sure everybody read the max value */
        for (i = 1; i < thcnt; ++i) {
            while (CORE_samax1buf[i << 1] != -1.0) {
            }
        }

        CORE_samax1buf[0] = -1.0;
    } else {
        CORE_samax1buf[(thidx << 1) + 1] = localamx;
        CORE_samax1buf[thidx << 1] = -2.0;  /* announce to thread 0 that local amax was stored */
        while (CORE_samax1buf[0] == -1.0) { /* wait for thread 0 to finish calculating the global amax */
        }
        while (CORE_samax1buf[thidx << 1] != -3.0) { /* wait for thread 0 to store amax */
        }
        *thwinner  = -CORE_samax1buf[0] - 2.0;
        *diagvalue = CORE_samax1buf[1];
        *globalamx = CORE_samax1buf[(thidx << 1) + 1]; /* read the amax from the location adjacent to the one in the above loop */
        CORE_samax1buf[thidx << 1] = -1.0;  /* signal thread 0 that this thread is done reading */

        if (thidx == *thwinner)
            ipiv[0] = pividx;

        while (CORE_samax1buf[0] != -1.0) { /* wait for thread 0 to finish */
        }
    }
}

static void
CORE_sbarrier_thread(int thidx,
                     int thcnt)
{
    int idum1, idum2;
    float ddum1 = 0.;
    float ddum2 = 0.;
    /* it's probably faster to implement a dedicated barrier */
    CORE_samax1_thread( 1.0, thidx, thcnt, &idum1, &ddum1, &ddum2, 0, &idum2 );
}

static inline void
CORE_sgetrf_rectil_update(const PLASMA_desc A, int *IPIV,
                          int column, int n1,     int n2,
                          int thidx,  int thcnt,
                          int ft,     int lt)
{
    int ld, lm, tmpM;
    int ip, j, it, i, ldft;
    float zone  = 1.0;
    float mzone = -1.0;
    float *Atop, *Atop2, *U, *L;
    int offset = A.i;

    ldft = BLKLDD(A, 0);
    Atop  = A(0, 0) + column * ldft;
    Atop2 = Atop    + n1     * ldft;

    if (thidx == 0)
    {
        /* Swap to the right */
        int *lipiv = IPIV+column;
        int idxMax = column+n1;
        for (j = column; j < idxMax; ++j, ++lipiv) {
            ip = (*lipiv) - offset - 1;
            if ( ip != j )
            {
                it = ip / A.mb;
                i  = ip % A.mb;
                ld = BLKLDD(A, it);
                cblas_sswap(n2, Atop2                     + j, ldft,
                            A(it, 0) + (column+n1)*ld + i, ld   );
            }
        }

        /* Trsm on the upper part */
        U = Atop2 + column;
        cblas_strsm( CblasColMajor, CblasLeft, CblasLower,
                     CblasNoTrans, CblasUnit,
                     n1, n2, (zone),
                     Atop  + column, ldft,
                     U,              ldft );

        /* Signal to other threads that they can start update */
        CORE_sbarrier_thread( thidx, thcnt );

        /* First tile */
        L = Atop + column + n1;
        tmpM = min(ldft, A.m) - column - n1;

        /* Apply the GEMM */
        cblas_sgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
                     tmpM, n2, n1,
                     (mzone), L,      ldft,
                     U,      ldft,
                     (zone),  U + n1, ldft );

    }
    else
    {
        ld = BLKLDD( A, ft );
        L  = A( ft, 0 ) + column * ld;
        lm = ft == A.mt-1 ? A.m - ft * A.mb : A.mb;

        U = Atop2 + column;

        /* Wait for pivoting and triangular solve to be finished
         * before to really start the update */
        CORE_sbarrier_thread( thidx, thcnt );

        /* First tile */
        /* Apply the GEMM */
        cblas_sgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
                     lm, n2, n1,
                     (mzone), L,         ld,
                                         U,         ldft,
                     (zone),  L + n1*ld, ld );
    }

    /* Update the other blocks */
    for( it = ft+1; it < lt; it++)
    {
        ld = BLKLDD( A, it );
        L  = A( it, 0 ) + column * ld;
        lm = it == A.mt-1 ? A.m - it * A.mb : A.mb;

        /* Apply the GEMM */
        cblas_sgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
                     lm, n2, n1,
                     (mzone), L,          ld,
                                         U,          ldft,
                     (zone),  L + n1*ld,  ld );
    }
}

static void
CORE_sgetrf_rectil_rec(const PLASMA_desc A, int *IPIV, int *info,
                       float *pivot,
                       int thidx,  int thcnt,
                       int column, int width,
                       int ft,     int lt)
{
    int ld, jp, n1, n2, lm, tmpM, piv_sf;
    int ip, j, it, i, ldft;
    int max_i, max_it, thwin;
    float zone  = 1.0;
    float mzone = -1.0;
    float tmp1;
    float tmp2 = 0.;
    float pivval;
    float *Atop, *Atop2, *U, *L;
    float             abstmp1;
    int offset = A.i;

    ldft = BLKLDD(A, 0);
    Atop = A(0, 0) + column * ldft;

    if ( width > 1 ) {
        /* Assumption: N = min( M, N ); */
        n1 = width / 2;
        n2 = width - n1;

        Atop2 = Atop + n1 * ldft;

        CORE_sgetrf_rectil_rec( A, IPIV, info, pivot,
                                thidx, thcnt, column, n1, ft, lt );

        if ( *info != 0 )
            return;

        if (thidx == 0)
        {
            /* Swap to the right */
            int *lipiv = IPIV+column;
            int idxMax = column+n1;
            for (j = column; j < idxMax; ++j, ++lipiv) {
                ip = (*lipiv) - offset - 1;
                if ( ip != j )
                {
                    it = ip / A.mb;
                    i  = ip % A.mb;
                    ld = BLKLDD(A, it);
                    cblas_sswap(n2, Atop2                     + j, ldft,
                                    A(it, 0) + (column+n1)*ld + i, ld   );
                }
            }
            /* Trsm on the upper part */
            U = Atop2 + column;
            cblas_strsm( CblasColMajor, CblasLeft, CblasLower,
                         CblasNoTrans, CblasUnit,
                         n1, n2, (zone),
                         Atop  + column, ldft,
                         U,              ldft );

            /* SIgnal to other threads that they can start update */
            CORE_sbarrier_thread( thidx, thcnt );
            pivval = *pivot;
            if ( pivval == 0.0 ) {
                *info = column+n1;
                return;
            } else {
                if ( fabsf(pivval) >= sfmin ) {
                    piv_sf = 1;
                    pivval = 1.0 / pivval;
                } else {
                    piv_sf = 0;
                }
            }

            /* First tile */
            {
                L = Atop + column + n1;
                tmpM = min(ldft, A.m) - column - n1;

                /* Scale last column of L */
                if ( piv_sf ) {
                    cblas_sscal( tmpM, (pivval), L+(n1-1)*ldft, 1 );
                } else {
                    int i;
                    Atop2 = L+(n1-1)*ldft;
                    for( i=0; i < tmpM; i++, Atop2++)
                        *Atop2 = *Atop2 / pivval;
                }

                /* Apply the GEMM */
                cblas_sgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
                             tmpM, n2, n1,
                             (mzone), L,      ldft,
                                                 U,      ldft,
                             (zone),  U + n1, ldft );

                /* Search Max in first column of U+n1 */
                tmp2    = U[n1];
                max_it  = ft;
                max_i   = cblas_isamax( tmpM, U+n1, 1 ) + n1;
                tmp1    = U[max_i];
                abstmp1 = fabsf(tmp1);
                max_i   += column;
            }
        }
        else
        {
            pivval = *pivot;
            if ( pivval == 0.0 ) {
                *info = column+n1;
                return;
            } else {
                if ( fabsf(pivval) >= sfmin ) {
                    piv_sf = 1;
                    pivval = 1.0 / pivval;
                } else {
                    piv_sf = 0;
                }
            }

            ld = BLKLDD( A, ft );
            L  = A( ft, 0 ) + column * ld;
            lm = ft == A.mt-1 ? A.m - ft * A.mb : A.mb;

            U = Atop2 + column;

            /* First tile */
            /* Scale last column of L */
            if ( piv_sf ) {
                cblas_sscal( lm, (pivval), L+(n1-1)*ld, 1 );
            } else {
                int i;
                Atop2 = L+(n1-1)*ld;
                for( i=0; i < lm; i++, Atop2++)
                    *Atop2 = *Atop2 / pivval;
            }

            /* Wait for pivoting and triangular solve to be finished
             * before to really start the update */
            CORE_sbarrier_thread( thidx, thcnt );

            /* Apply the GEMM */
            cblas_sgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
                         lm, n2, n1,
                         (mzone), L,         ld,
                                             U,         ldft,
                         (zone),  L + n1*ld, ld );

            /* Search Max in first column of L+n1*ld */
            max_it  = ft;
            max_i   = cblas_isamax( lm, L+n1*ld, 1 );
            tmp1    = L[n1*ld+max_i];
            abstmp1 = fabsf(tmp1);
        }

        /* Update the other blocks */
        for( it = ft+1; it < lt; it++)
        {
            ld = BLKLDD( A, it );
            L  = A( it, 0 ) + column * ld;
            lm = it == A.mt-1 ? A.m - it * A.mb : A.mb;

            /* Scale last column of L */
            if ( piv_sf ) {
                cblas_sscal( lm, (pivval), L+(n1-1)*ld, 1 );
            } else {
                int i;
                Atop2 = L+(n1-1)*ld;
                for( i=0; i < lm; i++, Atop2++)
                    *Atop2 = *Atop2 / pivval;
            }

            /* Apply the GEMM */
            cblas_sgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
                         lm, n2, n1,
                         (mzone), L,          ld,
                                             U,          ldft,
                         (zone),  L + n1*ld,  ld );

            /* Search the max on the first column of L+n1*ld */
            jp = cblas_isamax( lm, L+n1*ld, 1 );
            if ( fabsf( L[n1*ld+jp] ) > abstmp1 ) {
                tmp1 = L[n1*ld+jp];
                abstmp1 = fabsf(tmp1);
                max_i  = jp;
                max_it = it;
            }
        }

        jp = offset + max_it*A.mb + max_i;
        CORE_samax1_thread( tmp1, thidx, thcnt, &thwin,
                            &tmp2, pivot, jp + 1, IPIV + column + n1 );

        if ( thidx == 0 ) {
            U[n1] = *pivot; /* all threads have the pivot element: no need for synchronization */
        }
        if (thwin == thidx) { /* the thread that owns the best pivot */
            if ( jp-offset != column+n1 ) /* if there is a need to exchange the pivot */
            {
                ld = BLKLDD(A, max_it);
                Atop2 = A( max_it, 0 ) + (column + n1 )* ld + max_i;
                *Atop2 = tmp2;
            }
        }

        CORE_sgetrf_rectil_rec( A, IPIV, info, pivot,
                                thidx, thcnt, column+n1, n2, ft, lt );
        if ( *info != 0 )
            return;

        if ( thidx == 0 )
        {
            /* Swap to the left */
            int *lipiv = IPIV+column+n1;
            int idxMax = column+width;
            for (j = column+n1; j < idxMax; ++j, ++lipiv) {
                ip = (*lipiv) - offset - 1;
                if ( ip != j )
                {
                    it = ip / A.mb;
                    i  = ip % A.mb;
                    ld = BLKLDD(A, it);
                    cblas_sswap(n1, Atop + j,                 ldft,
                                    A(it, 0) + column*ld + i, ld  );
                }
            }
        }

    } else if ( width == 1 ) {

        /* Search maximum for column 0 */
        if ( column == 0 )
        {
            if ( thidx == 0 )
              tmp2 = Atop[column];

            /* First tmp1 */
            ld = BLKLDD(A, ft);
            Atop2   = A( ft, 0 );
            lm      = ft == A.mt-1 ? A.m - ft * A.mb : A.mb;
            max_it  = ft;
            max_i   = cblas_isamax( lm, Atop2, 1 );
            tmp1    = Atop2[max_i];
            abstmp1 = fabsf(tmp1);

            /* Update */
            for( it = ft+1; it < lt; it++)
            {
                Atop2= A( it, 0 );
                lm   = it == A.mt-1 ? A.m - it * A.mb : A.mb;
                jp   = cblas_isamax( lm, Atop2, 1 );
                if (  fabsf(Atop2[jp]) > abstmp1 ) {
                    tmp1 = Atop2[jp];
                    abstmp1 = fabsf(tmp1);
                    max_i  = jp;
                    max_it = it;
                }
            }

            jp = offset + max_it*A.mb + max_i;
            CORE_samax1_thread( tmp1, thidx, thcnt, &thwin,
                                &tmp2, pivot, jp + 1, IPIV + column );

            if ( thidx == 0 ) {
                Atop[0] = *pivot; /* all threads have the pivot element: no need for synchronization */
            }
            if (thwin == thidx) { /* the thread that owns the best pivot */
                if ( jp-offset != 0 ) /* if there is a need to exchange the pivot */
                {
                    Atop2 = A( max_it, 0 ) + max_i;
                    *Atop2 = tmp2;
                }
            }
        }

        CORE_sbarrier_thread( thidx, thcnt );

        /* If it is the last column, we just scale */
        if ( column == (min(A.m, A.n))-1 ) {

            pivval = *pivot;
            if ( pivval != 0.0 ) {
                if ( thidx == 0 ) {
                    if ( fabsf(pivval) >= sfmin ) {
                        pivval = 1.0 / pivval;

                        /*
                         * We guess than we never enter the function with m == A.mt-1
                         * because it means that there is only one thread
                         */
                        lm = ft == A.mt-1 ? A.m - ft * A.mb : A.mb;
                        cblas_sscal( lm - column - 1, (pivval), Atop+column+1, 1 );

                        for( it = ft+1; it < lt; it++)
                        {
                            ld = BLKLDD(A, it);
                            Atop2 = A( it, 0 ) + column * ld;
                            lm = it == A.mt-1 ? A.m - it * A.mb : A.mb;
                            cblas_sscal( lm, (pivval), Atop2, 1 );
                        }
                    } else {
                        /*
                         * We guess than we never enter the function with m == A.mt-1
                         * because it means that there is only one thread
                         */
                        int i;
                        Atop2 = Atop + column + 1;
                        lm = ft == A.mt-1 ? A.m - ft * A.mb : A.mb;

                        for( i=0; i < lm-column-1; i++, Atop2++)
                            *Atop2 = *Atop2 / pivval;

                        for( it = ft+1; it < lt; it++)
                            {
                                ld = BLKLDD(A, it);
                                Atop2 = A( it, 0 ) + column * ld;
                                lm = it == A.mt-1 ? A.m - it * A.mb : A.mb;

                                for( i=0; i < lm; i++, Atop2++)
                                    *Atop2 = *Atop2 / pivval;
                            }
                    }
                }
                else
                {
                    if ( fabsf(pivval) >= sfmin ) {
                        pivval = 1.0 / pivval;

                        for( it = ft; it < lt; it++)
                        {
                            ld = BLKLDD(A, it);
                            Atop2 = A( it, 0 ) + column * ld;
                            lm = it == A.mt-1 ? A.m - it * A.mb : A.mb;
                            cblas_sscal( lm, (pivval), Atop2, 1 );
                        }
                    } else {
                        /*
                         * We guess than we never enter the function with m == A.mt-1
                         * because it means that there is only one thread
                         */
                        int i;
                        for( it = ft; it < lt; it++)
                        {
                            ld = BLKLDD(A, it);
                            Atop2 = A( it, 0 ) + column * ld;
                            lm = it == A.mt-1 ? A.m - it * A.mb : A.mb;

                            for( i=0; i < lm; i++, Atop2++)
                                *Atop2 = *Atop2 / pivval;
                        }
                    }
                }
            }
            else {
                *info = column + 1;
                return;
            }
        }
    }
}


/*
 * BRAINS
 * (B)LR (R)everberation-mapping (A)nalysis (I)n AGNs with (N)ested (S)ampling
 * Yan-Rong Li, liyanrong@ihep.ac.cn
 * Thu, Aug 4, 2016
 */

/*!
 *  \file allvars.h
 *  \brief header file of allvars.c.
 */

#ifndef _ALLVARS_H
#define _ALLVARS_H

#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <mpi.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_interp.h>

/*! \def GRAVITY
 *  \brief Gravitational constant.

 *  \def SOLAR_MASS
 *  \brief Solar mass.

 *  \def C
 *  \brief speed of light.

 *  \def SEC_PER_YEAR
 *  \brief seconds per year.

 *  \def CM_PER_LD
 *  \brief light day in centimeter.

 *  \def PI
 *  \brief Pi.

 *  \def BRAINS_MAX_STR_LENGTH
 *  \brief maximum string length.
 */
#define GRAVITY       6.672e-8
#define SOLAR_MASS    1.989e33
#define C             2.9979e10
#define SEC_PER_YEAR  3.155e7
#define CM_PER_LD    (C*8.64e4)
#define CM_PER_PC    (3.08567758e+18)
#define CM_PER_MPC   (3.08567758e+24)

#define PI            M_PI
#define BRAINS_MAX_STR_LENGTH  (256)

#define EPS (DBL_MIN)  /* epsilon of the machine as in Matlab */

/* for PARDICT */
#define MAXTAGS 300
#define DOUBLE 1
#define STRING 2
#define INT 3

enum PRIOR_TYPE {GAUSSIAN=1, UNIFORM=2};

/* variables for MPICH */
extern int thistask, totaltask, namelen;
extern int roottask;
extern char proc_name[MPI_MAX_PROCESSOR_NAME];

/*! \struct PARSET
 *  \brief the configuration parameters.
 */
typedef struct
{
  char param_file[BRAINS_MAX_STR_LENGTH];
  char continuum_file[BRAINS_MAX_STR_LENGTH],
       line_file[BRAINS_MAX_STR_LENGTH],
       line2d_file[BRAINS_MAX_STR_LENGTH],
       pcon_out_file[BRAINS_MAX_STR_LENGTH],
       pline_out_file[BRAINS_MAX_STR_LENGTH],
       pline2d_out_file[BRAINS_MAX_STR_LENGTH],
       pline2d_data_out_file[BRAINS_MAX_STR_LENGTH],
       cloud_out_file[BRAINS_MAX_STR_LENGTH],
       tran_out_file[BRAINS_MAX_STR_LENGTH],
       tran2d_out_file[BRAINS_MAX_STR_LENGTH],
       tran2d_data_out_file[BRAINS_MAX_STR_LENGTH];

  char file_dir[BRAINS_MAX_STR_LENGTH];

  int flag_dim;

  int flag_trend, flag_trend_diff;

  int n_con_recon, n_line_recon, n_vel_recon;

  int n_tau;
  double rcloud_max, time_back;

  int flag_blrmodel;

  int n_cloud_per_task, n_vel_per_cloud;

  int flag_save_clouds;

  int flag_InstRes;
  double InstRes, InstRes_err;
  char file_instres[BRAINS_MAX_STR_LENGTH];

  int num_particles;

  char str_par_fix[BRAINS_MAX_STR_LENGTH], str_par_fix_val[BRAINS_MAX_STR_LENGTH];

  int flag_narrowline;
  double flux_narrowline, width_narrowline, shift_narrowline;
  double flux_narrowline_err, width_narrowline_err, shift_narrowline_err;

  int flag_linecenter;
  double linecenter_err;

  int flag_postprc;
  int flag_restart;
  int flag_sample_info;
  int flag_temp;
  int flag_exam_prior;
  int flag_rng_seed, rng_seed;
  int flag_fixvar;
  int flag_nonlinear;

  int flag_force_update;
  int flag_force_run;

  int flag_con_sys_err, flag_line_sys_err;

  double temperature;

  int flag_help, flag_end;
  int flag_para_name;

  double redshift;
  double linecenter;
#ifdef SA
  char sa_file[BRAINS_MAX_STR_LENGTH];
  int flag_sa_blrmodel, flag_sa_par_mutual;
  double sa_linecenter;
  double sa_InstRes;
  char sa_str_par_fix[BRAINS_MAX_STR_LENGTH], sa_str_par_fix_val[BRAINS_MAX_STR_LENGTH];
  int n_sa_vel_recon, n_sa_base_recon;
#endif
}PARSET;
extern PARSET parset;

typedef struct
{
  int id;
  void *addr;
  char tag[50];
  int isset;
}PARDICT;
extern PARDICT *pardict;
extern int num_pardict;

extern double VelUnit, C_Unit;

extern int n_con_data, n_line_data, n_vel_data, n_vel_data_incr, n_vel_data_ext, n_con_max;
extern double *Tcon_data, *Fcon_data,  *Fcerrs_data;
extern double *Tline_data, *Fline_data, *Flerrs_data;
extern double *Vline_data, *Fline2d_data, *Flerrs2d_data, *Wline_data;
extern double *Vline_data_ext, *Wline_data_ext;
extern double con_scale, line_scale;

extern double con_error_mean, line_error_mean, line_error_mean_sq;

extern char dnest_options_file[BRAINS_MAX_STR_LENGTH];

extern int which_parameter_update, which_particle_update;  // which parameter and particle to be updated
extern int which_level_update;
extern double *limits;  // external from dnest

/* continuum reconstruction */
extern int nq;
extern double *Tcon, *Fcon, *Fcerrs, *Fcon_rm;
extern double Tcon_min, Tcon_max;
extern double *PSmat, *USmat, *PSmat_data;
extern double *PCmat_data, *IPCmat_data, *PQmat, *PEmat1, *PEmat2;
extern double *workspace, *workspace_uv;
extern double *var_param, *var_param_std;
extern double logz_con;
extern double *Larr_data, *Larr_rec;
extern double *pow_Tcon_data;

extern double Tspan_data, Tspan_data_con, Tcad_data, Tmed_data;

/* line reconstruction */
extern double *Fline_at_data;
extern double *Tline, *Fline, *Flerrs;
extern double Tline_min, Tline_max;
extern double logz_line;

/* line 2d reconstruction */
extern double *Fline2d_at_data;
extern double *Fline2d, *Flerrs2d;
extern double logz_line2d;

extern int BLRmodel_size;
extern int *par_fix, npar_fix;
extern double *par_fix_val;

extern int num_params_blr_tot;
extern int num_params, num_params_blr, num_params_blr_model, num_params_var, num_params_difftrend, num_params_nlr, num_params_res;
extern int num_params_drw, num_params_trend, num_params_resp;
extern int num_params_linecenter;

extern double **blr_range_model, **par_range_model;
extern int *par_prior_model;  /* prior type for model parameters */
extern double **par_prior_gaussian; /* center and std of Gaussian priors */
extern double var_range_model[15][2];
extern double nlr_range_model[3][2];
extern int nlr_prior_model[3];
extern double mass_range[2];
extern double sys_err_line_range[2]; /* range for systematic error of line */
extern double resp_range[2][2];
extern int idx_resp, idx_difftrend, idx_linecenter;

extern double *instres_epoch, *instres_err_epoch;

/* transfer function / velocity-delay map */
extern double *TransTau, *TransV, *TransW, *Trans1D, *Trans2D_at_veldata, *Trans2D;
extern double rcloud_min_set, rcloud_max_set, time_back_set;

extern double **Fcon_rm_particles, **Fcon_rm_particles_perturb;
extern double *prob_con_particles, *prob_con_particles_perturb;

extern int force_update;
extern double **TransTau_particles, **TransTau_particles_perturb;
extern double **Trans1D_particles, **Trans1D_particles_perturb;
extern double **Trans2D_at_veldata_particles, **Trans2D_at_veldata_particles_perturb;
extern double **Fline_at_data_particles, **Fline_at_data_particles_perturb;

extern double *clouds_tau, *clouds_weight, *clouds_vel;

extern FILE *fcloud_out;
extern int icr_cloud_save;

/* GSL */
extern const gsl_rng_type * gsl_T;
extern gsl_rng * gsl_r;

extern gsl_interp_accel *gsl_acc;
extern gsl_interp  *gsl_linear;

#ifdef SA
extern double PhaseFactor;

extern int num_params_rm;
extern int num_params_sa, num_params_sa_blr_model, num_params_sa_extpar, num_params_sa_blr;
extern int n_epoch_sa_data, n_vel_sa_data, n_base_sa_data;
extern double *vel_sa_data, *base_sa_data, *Fline_sa_data, *Flerrs_sa_data, *phase_sa_data, *pherrs_sa_data;
extern double *wave_sa_data;

extern double sa_flux_norm;

extern double *phase_sa, *Fline_sa, *base_sa, *vel_sa, *wave_sa;

extern double *clouds_alpha, *clouds_beta;

extern double **sa_extpar_range, **sa_blr_range_model;

extern int SABLRmodel_size;

extern int *idx_sa_par_mutual, *idx_rm_par_mutual;

extern double *prob_sa_particles, *prob_sa_particles_perturb;

extern double sa_phase_error_mean, sa_line_error_mean;

extern double *workspace_phase;
#endif
#endif


#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_integration.h>

#include <fastpm/libfastpm.h>
#include <fastpm/logging.h>

#define LENGTH_FERMI_DIRAC_TABLE 4000 //sets the length of the table on which CDF
                                      //will be evaluated
#define MAX_FERMI_DIRAC          20.0 //Upper limit of F-D distribution in units of
                                      // p/T

#define NEXT(n, i)  (((n) + (i)/(n)) >> 1) // Needed for Healpix routine

//needed for healpix
unsigned int isqrt(int number) {
    unsigned int n  = 1;
    unsigned int n1 = NEXT(n, number);

    while(abs(n1 - n) > 1) {
        n  = n1;
        n1 = NEXT(n, number);
    }
    while(n1*n1 > number)
        n1--;
    return n1;
}

//Converts from pixel number to unit vector (needed for healpix)
void pix2vec (int pix, double *vec, int n_side)
{
    double z, phi;
    int nside_ = n_side;
    long ncap_=nside_*(nside_-1)*2;
    long npix_=12*nside_*nside_;
    double fact2_  = 4./npix_;
    if (pix<ncap_) /* North Polar cap */
    {
        int iring = (int)(0.5*(1+isqrt(1+2*pix))); /* counted from North pole */
        int iphi  = (pix+1) - 2*iring*(iring-1);

        z = 1.0 - (iring*iring)*fact2_;
        phi = (iphi-0.5) * 0.5*M_PI/iring;
    }
    else if (pix<(npix_-ncap_)) /* Equatorial region */
    {
        double fact1_  = (nside_<<1)*fact2_;
        int ip  = pix - ncap_;
        int iring = ip/(4*nside_) + nside_; /* counted from North pole */
        int iphi  = ip%(4*nside_) + 1;
        /* 1 if iring+nside is odd, 1/2 otherwise */
        double fodd = ((iring+nside_)&1) ? 1 : 0.5;

        int nl2 = 2*nside_;
        z = (nl2-iring)*fact1_;
        phi = (iphi-fodd) * M_PI/nl2;
    }
    else /* South Polar cap */
    {
        int ip = npix_ - pix;
        int iring = (int)(0.5*(1+isqrt(2*ip-1))); /* counted from South pole */
        int iphi  = 4*iring + 1 - (ip - 2*iring*(iring-1));

        z = -1.0 + (iring*iring)*fact2_;
        phi = (iphi-0.5) * 0.5*M_PI/iring;
    }

    double v[3];
    v[0] = sin(acos(z))*cos(phi);
    v[1] = sin(acos(z))*sin(phi);
    v[2] = z;

    /* rotate the vector to break degeneracy with grid axes */
    vec[0] = 0.5*v[0] -0.5*v[1] +0.70710678*v[2];
    vec[1] = 0.85355339*v[0] +0.14644661*v[1] -0.5*v[2];
    vec[2] = 0.14644661*v[0] +0.85355339*v[1] +0.5*v[2];
}

/* make params struct for functions we will integrate with gsl */
typedef struct kernel_params {
    double m[3];
    int n;
} kernel_params;

double fermi_dirac_kernel_vol_1(double x)
{
    /* Functional form of FD distribution for 1 particle
       over the full volume (i.e. no *x^2) */
    return 1. / (exp(x) + 1);
}

double fermi_dirac_kernel_vol(double x, void * p)
{
    /* FD kernel in the case of multiple particles
       over the full volume (i.e. no *x^2) */
    struct kernel_params * params = (struct kernel_params *)p;

    double result = fermi_dirac_kernel_vol_1(x);
    // only loop from i=1. i=0 term has been included in result already (r=1)
    for(int i = 1; i < params->n; i ++) {
        double r = params->m[i] / params->m[0];
        result += (r*r*r*r) * fermi_dirac_kernel_vol_1(x * r);
    }
    return result;
}

/* now define the 1D kernel and moments */
double fermi_dirac_kernel(double x, void * p)
{
    return x*x * fermi_dirac_kernel_vol(x, p);
}

/* Samples the low velocity end more effectively */
double low_vel_kernel(double x, void * p)
{
    return x * fermi_dirac_kernel_vol(x, p);
}

/* Needed for calculatin the velocity dispersion */
double fermi_dirac_dispersion(double x, void * p)
{
     return x*x*x*x * fermi_dirac_kernel_vol(x, p);
}

void
divide_fd(double *vel_table, double *mass,
          kernel_params * params,
          int n_shells,
          int lvk)
{
    double fermi_dirac_vel_ncdm[LENGTH_FERMI_DIRAC_TABLE];
    double fermi_dirac_cdf_ncdm[LENGTH_FERMI_DIRAC_TABLE]; //stores CDF

    int i,j;
    double v_bin,u,bin_average,bin_mass;
    double vel_edges[n_shells];
    double result,error;
    gsl_integration_workspace * w
        = gsl_integration_workspace_alloc (1000);

    gsl_function F,G,H;
    if(lvk)
        F.function = &low_vel_kernel;
    else
        F.function = &fermi_dirac_kernel;
    G.function = &fermi_dirac_kernel;
    H.function = &fermi_dirac_dispersion;

    F.params = (void*) params;
    G.params = (void*) params;
    H.params = (void*) params;

    // Initialize the CDF table
    for(i = 0; i < LENGTH_FERMI_DIRAC_TABLE; i++){
        fermi_dirac_vel_ncdm[i] = MAX_FERMI_DIRAC * i / (LENGTH_FERMI_DIRAC_TABLE - 1.0);
        gsl_integration_qags (&F, 0, fermi_dirac_vel_ncdm[i], 0,1e-7 , 1000,
			  w, &result, &error);
        fermi_dirac_cdf_ncdm[i] = result;
    }

    //Normalize to 1
    for(i = 0; i < LENGTH_FERMI_DIRAC_TABLE; i++)
        fermi_dirac_cdf_ncdm[i] /= fermi_dirac_cdf_ncdm[LENGTH_FERMI_DIRAC_TABLE - 1];

    //Define the edges of the velocity bins (lower edge held to 0)
    for(i=0;i<n_shells;i++){
        v_bin = (i+1)/(n_shells*1.0);
        j=0;
        while(j < LENGTH_FERMI_DIRAC_TABLE - 2)
            if(v_bin > fermi_dirac_cdf_ncdm[j + 1])
                j++;
            else
                break;

        u = (v_bin - fermi_dirac_cdf_ncdm[j]) / (fermi_dirac_cdf_ncdm[j + 1] - fermi_dirac_cdf_ncdm[j]);

        vel_edges[i] = fermi_dirac_vel_ncdm[j] * (1 - u) + fermi_dirac_vel_ncdm[j + 1] * u;
    }

    //Get the bin velocities and bin masses
    double total_mass;
    gsl_integration_qags (&G, 0, fermi_dirac_vel_ncdm[LENGTH_FERMI_DIRAC_TABLE-1], 0, 1e-7, 1000,
			  w, &result, &error);
    total_mass = result;
    for(i=0;i<n_shells;i++){
        //Special case for lowest bin - left edge has to be zero
        if (i==0){
            gsl_integration_qags (&H, 0,vel_edges[i], 0, 1e-7, 1000,
			  w, &result, &error);
            bin_average = result;
            gsl_integration_qags (&G, 0, vel_edges[i], 0, 1e-7, 1000,
			  w, &result, &error);
            bin_average /= result;

            bin_mass = result/total_mass;
        }
        else{
            gsl_integration_qags (&H, vel_edges[i-1], vel_edges[i], 0, 1e-7, 1000,
			  w, &result, &error);
            bin_average = result;
            gsl_integration_qags (&G, vel_edges[i-1], vel_edges[i], 0, 1e-7, 1000,
			  w, &result, &error);
            bin_average /= result;

            bin_mass = result/total_mass;
        }
        vel_table[i] = sqrt(bin_average);
        mass[i] = bin_mass;
    }

}

void divide_sphere_healpix(double *vec_table, int n_side)  //so is the direction totally fixed? could we rotate the divisions relative to north pole (i.e. relative to the box)?
{
    int i,k;
    double v_sq[3];
    for(k=0;k<3;k++)
        v_sq[k] = 0.;

    // Get unit vectors for all pixels
    for(i=0;i<12*n_side*n_side;i++){
        pix2vec(i,&vec_table[i*3],n_side);
        for(k=0;k<3;k++)
            v_sq[k] += vec_table[i*3+k]*vec_table[i*3+k];
    }
    // Isotropize the velocity dispersion - important for low n_side    [should we remove this if for high n_side???]
    for(k=0;k<3;k++){
        v_sq[k] /= 12*n_side*n_side;
        v_sq[k] /= 1./3.; // Set each direction to have 1/3 of total dispersion
    }
    for(i=0;i<12*n_side*n_side;i++)
        for(k=0;k<3;k++)
            vec_table[i*3+k] /= sqrt(v_sq[k]);
}

void divide_sphere_fibonacci(double *vec_table, int n_side)
{
    int n_fibonacci = n_side;
    double lat, lon;
    int i, j;
    //int N_tot = 2 * n_fibonacci + 1;

    for (i=-n_fibonacci;i<n_fibonacci+1;i++){
        lat = asin(2.0*i/(2.0*n_fibonacci+1));
        lon = 2.0*M_PI*i*2.0/(1.0+sqrt(5.0));
        j = i + n_fibonacci;
        vec_table[j*3+0] = cos(lat)*cos(lon);
        vec_table[j*3+1] = cos(lat)*sin(lon);
        vec_table[j*3+2] = sin(lat);
    }
}

void divide_sphere(FastPMncdmSphereScheme ncdm_sphere_scheme,
                   double *vec_table, int n_side)
{
    switch (ncdm_sphere_scheme){
        case FASTPM_NCDM_SPHERE_HEALPIX:
            divide_sphere_healpix(vec_table, n_side);
        break;
        case FASTPM_NCDM_SPHERE_FIBONACCI:
            divide_sphere_fibonacci(vec_table, n_side);
        break;
        default:
            fastpm_raise(-1, "Wrong ncdm sphere scheme.\n");
    }
}

static void _fastpm_ncdm_init_fill(FastPMncdmInitData* nid);

//create and fill the table
FastPMncdmInitData*
fastpm_ncdm_init_create(
    double BoxSize,
    FastPMCosmology * c,
    double z,
    int n_shells,
    int n_side,
    int lvk,
    FastPMncdmSphereScheme ncdm_sphere_scheme)
{
    FastPMncdmInitData* nid = malloc(sizeof(nid[0]));

    nid->BoxSize = BoxSize;
    nid->m_ncdm_sum = 0;
    for(int i = 0; i < c->N_ncdm; i ++) {
        nid->m_ncdm[i] = c->m_ncdm[i];
        nid->m_ncdm_sum += c->m_ncdm[i];
    }
    nid->n_ncdm = c->N_ncdm;
    nid->Omega_ncdm = c->Omega_ncdm;
    nid->z = z;
    fastpm_info("ncdm reference redshift = %g\n", z);
    nid->n_shells = n_shells;
    nid->n_side = n_side;
    nid->lvk = lvk;
    nid->ncdm_sphere_scheme = ncdm_sphere_scheme;

    /* this is the total number of velocity vectors produced */
    switch (ncdm_sphere_scheme){
        case FASTPM_NCDM_SPHERE_HEALPIX:
            nid->n_sphere = 12 * n_side * n_side;
        break;
        case FASTPM_NCDM_SPHERE_FIBONACCI:
            nid->n_sphere = 2 * n_side + 1;
        break;
        default:
            fastpm_raise(-1, "Wrong ncdm sphere scheme.\n");
    }

    nid->n_split = n_shells * nid->n_sphere;

    /* recall vel is a pointer to a 3 element array
    so lets make into multidim array of dim (n_split x 3) */
    //for now store all (3) neutrinos in the same nid table.
    nid->vel = calloc(nid->n_split, sizeof(double[3]));
    nid->mass = calloc(nid->n_split, sizeof(double));   //a 1d array to store all the masses

    _fastpm_ncdm_init_fill(nid);

    return nid;
}

//free the table
void
fastpm_ncdm_init_free(FastPMncdmInitData* nid)
{
    free(nid->vel);
    free(nid->mass);
    free(nid);
}

//append the table
static void
_fastpm_ncdm_init_fill(FastPMncdmInitData* nid)
{
    int n_ncdm = nid->n_ncdm;
    int n_shells = nid->n_shells;
    int n_side = nid->n_side;
    int n_sphere = nid->n_sphere;
    int lvk = nid->lvk;

    double *vel_table, *vec_table, *masstab;
    vel_table = malloc(sizeof(double)*n_shells);
    /* masstab is the distribution integrated per shell, sums to 1 */
    masstab = calloc(n_shells,sizeof(double));
    vec_table = malloc(sizeof(double)*n_sphere*3);

    /* define the kernel params for dividing fd */
    kernel_params* params = malloc(sizeof(params[0]));
    for(int i = 0; i < n_ncdm; i ++) {
        params->m[i] = nid->m_ncdm[i];
    }
    params->n = n_ncdm;

    divide_fd(vel_table, masstab, params, n_shells, lvk);
    divide_sphere(nid->ncdm_sphere_scheme, vec_table, n_side);

    /* velocity_conversion_factor converts the dimless exponent
       in the FD distribution to velocity in FastPM units:
       conjugate momentum (a^2 xdot, where x is comoving dist).
       kTc = 50.3 eV/c^2 km/s */
    double m0 = nid->m_ncdm[0];
    double velocity_conversion_factor = 50.3 / m0 / HubbleConstant;

    int i, j, d;
    int s = 0;                          //row num  (s for split index)
    for(i = 0; i < n_sphere; i ++){
        for(j = 0; j < n_shells; j ++){
            /* define mass st sum over split gives 1 */
            nid->mass[s] = masstab[j] / n_sphere;
            for(d = 0; d < 3; d ++){
                nid->vel[s][d] = vel_table[j] * vec_table[i*3 + d] * velocity_conversion_factor;
            }
            s++;
        }
    }
    free(vel_table);
    free(masstab);
    free(vec_table);
}

void
fastpm_split_ncdm(FastPMncdmInitData * nid,
                  FastPMStore * src,
                  FastPMStore * dest,
                  MPI_Comm comm)
{
    /*
    Takes store src and splits according to the ncdm
    init data, and uses this to populate an dest store.
    src is fully populated after calling the setup_lpt
    func, but dest has only been init'd.
    */

    dest->np = src->np * nid->n_split;    // FIXME: ? Is proc imbalance an issue that needs to be accounted for?
    if(dest->np > dest->np_upper) {
        fastpm_raise(-1, "exceeding limit on the number limit of particles for the ncdm species \n");
    }

    size_t np_total = fastpm_store_get_np_total(src, comm);     //np_total is num of UNsplit ncdms

    /* avg mass of UNsplit ncdm site */
    double M0 = nid->Omega_ncdm * FASTPM_CRITICAL_DENSITY * pow(nid->BoxSize, 3) / np_total;
    double Mestate = M0 / nid->n_ncdm;
    fastpm_info("average mass of a ncdm particle is %g\n", Mestate);

    /* copy and amend meta-data */
    memmove(&dest->meta, &src->meta, sizeof(src->meta));

    dest->meta.M0 = 0.; /* will have a mass per particle */

    /* Compute the factor used to displace ncdm so that the expanded spheres almost touch.
       This assumes the largest speed particle is in the final entry of vel table */
    int n_shells = nid->n_shells;
    int n_sphere = nid->n_sphere;
    double vthm_max = 0;
    for (int d = 0; d < 3; d ++)
        vthm_max += nid->vel[n_sphere*n_shells-1][d]*nid->vel[n_sphere*n_shells-1][d];
    vthm_max = sqrt(vthm_max);
    double disp_factor = 0.5 * nid->BoxSize / nid->n_ncdm / vthm_max * (n_shells - 1) / n_shells;

    ptrdiff_t i, s, d;
    ptrdiff_t r = 0;    //row index of dest
    int c;
    double vthm;
    for(i = 0; i < src->np; i ++) {    //loop thru each src particle
        for(s = 0; s < nid->n_split; s ++) {  //loop thru split velocities
            /* copy all cols el by el */
            for(c = 0; c < 32; c ++) {
                if (!dest->columns[c] || !src->columns[c]) continue;

                size_t elsize = dest->_column_info[c].elsize;
                memcpy(dest->columns[c] + r * elsize,
                       src->columns[c] + i * elsize,
                       elsize);
            }

            /* give id, mass and add thm vel */
            dest->id[r] = s * src->meta._q_size + src->id[i];
            dest->mass[r] = nid->mass[s] * M0;    // ensures sum over split gives M0

            for(d = 0; d < 3; d ++){
                vthm = nid->vel[s][d];
                dest->v[r][d] = vthm;
                dest->x[r][d] += vthm * disp_factor;
                if (dest->q) dest->q[r][d] += vthm * disp_factor;
            }
            r ++;
        }
    }
}


/* ################################################################################## */
/* ###                                                                            ### */
/* ###                                 sigvel.c                                   ### */
/* ###                                                                            ### */
/* ###   Original: hydra.c (public version of Gadget 2)                           ### */
/* ###   Author: Volker Springel                                                  ### */
/* ###                                                                            ### */
/* ###   Modified: February 2011                                                  ### */
/* ###   Author: Fabrice Durier                                                   ### */
/* ###                                                                            ### */
/* ################################################################################## */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#ifndef NOMPI
#include <mpi.h>
#endif
#include <gsl/gsl_math.h>
#include "allvars.h"
#include "proto.h"

/*! \file sigvel.c
 *  Re-Computation of SPH acceleration and maximum signal velocity
 *  for 'feedback' particles only.
 *
 *  This file is a modified version of hydra.c, where the SPH forces are
 *  computed, and where the rate of change of entropy due to the shock heating
 *  (via artificial viscosity) is computed.
 */

#ifdef TIMESTEP_UPDATE

static double hubble_a, atime, hubble_a2, fac_mu, fac_vsic_fix, a3inv, fac_egy;

#ifdef PERIODIC
static double boxSize, boxHalf;

#ifdef LONG_X
static double boxSize_X, boxHalf_X;
#else
#define boxSize_X boxSize
#define boxHalf_X boxHalf
#endif
#ifdef LONG_Y
static double boxSize_Y, boxHalf_Y;
#else
#define boxSize_Y boxSize
#define boxHalf_Y boxHalf
#endif
#ifdef LONG_Z
static double boxSize_Z, boxHalf_Z;
#else
#define boxSize_Z boxSize
#define boxHalf_Z boxHalf
#endif
#endif

/*! This function is the driver routine for the update of the calculation
 *  of hydrodynamical force due to the feedback impact
 *  from either SNe or BHs on active gas particles.
 */
void get_sigvel(void)
{
  long long ntot, ntotleft;
  int i, j, k, n, ngrp, maxfill, source, ndone;
  int *nbuffer, *noffset, *nsend_local, *nsend, *numlist, *ndonelist;
  int level, sendTask, recvTask, nexport, place;
  double soundspeed_i;
  double tstart, tend, sumt, sumcomm;
  double timecomp = 0, timecommsumm = 0, timeimbalance = 0, sumimbalance;
#ifndef NOMPI
  MPI_Status status;
#endif

#ifdef PERIODIC
  boxSize = All.BoxSize;
  boxHalf = 0.5 * All.BoxSize;
#ifdef LONG_X
  boxHalf_X = boxHalf * LONG_X;
  boxSize_X = boxSize * LONG_X;
#endif
#ifdef LONG_Y
  boxHalf_Y = boxHalf * LONG_Y;
  boxSize_Y = boxSize * LONG_Y;
#endif
#ifdef LONG_Z
  boxHalf_Z = boxHalf * LONG_Z;
  boxSize_Z = boxSize * LONG_Z;
#endif
#endif

  if(All.ComovingIntegrationOn)
    {
      /* Factors for comoving integration of hydro */
      hubble_a = All.Omega0 / (All.Time * All.Time * All.Time)
        + (1 - All.Omega0 - All.OmegaLambda) / (All.Time * All.Time) + All.OmegaLambda;

      hubble_a = All.Hubble * sqrt(hubble_a);
      hubble_a2 = All.Time * All.Time * hubble_a;

      fac_mu = pow(All.Time, 3 * (GAMMA - 1) / 2) / All.Time;

      fac_egy = pow(All.Time, 3 * (GAMMA - 1));

      fac_vsic_fix = hubble_a * pow(All.Time, 3 * GAMMA_MINUS1);

      a3inv = 1 / (All.Time * All.Time * All.Time);
      atime = All.Time;
    }
  else
    hubble_a = hubble_a2 = atime = fac_mu = fac_vsic_fix = a3inv = fac_egy = 1.0;

  /* `NumSphUpdate' gives the number of feedback particles on this processor that want a force update */
  for(n = 0, NumSphUpdate = 0; n < N_gas; n++)
    {
      if(P[n].Ti_endstep == All.Ti_Current && SphP[n].FeedbackFlag > 1)
	NumSphUpdate++;
    }

  numlist = malloc(NTask * sizeof(int) * NTask);

#ifndef NOMPI
  MPI_Allgather(&NumSphUpdate, 1, MPI_INT, numlist, 1, MPI_INT, GADGET_WORLD);
#else
    numlist[0] = NumSphUpdate;
#endif
  for(i = 0, ntot = 0; i < NTask; i++)
    ntot += numlist[i];
  free(numlist);

  if(ThisTask == 0 && ntot)
    printf("\t --->  Number of feedback particles = %ld \n\n", ntot);

  noffset = malloc(sizeof(int) * NTask);	/* offsets of bunches in common list */
  nbuffer = malloc(sizeof(int) * NTask);
  nsend_local = malloc(sizeof(int) * NTask);
  nsend = malloc(sizeof(int) * NTask * NTask);
  ndonelist = malloc(sizeof(int) * NTask);

  i = 0;			/* first particle for this task */
  ntotleft = ntot;		/* particles left for all tasks together */

  while(ntotleft > 0)
    {
      for(j = 0; j < NTask; j++)
	nsend_local[j] = 0;

      /* do local particles and prepare export list */
      tstart = second();
      for(nexport = 0, ndone = 0; i < N_gas && nexport < All.BunchSizeHydro - NTask; i++)
	if(P[i].Ti_endstep == All.Ti_Current && SphP[i].FeedbackFlag > 1)
	  {
	    ndone++;

	    for(j = 0; j < NTask; j++)
	      Exportflag[j] = 0;

	    get_sigvel_evaluate(i, 0);

	    for(j = 0; j < NTask; j++)
	      {
		if(Exportflag[j])
		  {
		    for(k = 0; k < 3; k++)
		      {
			HydroDataIn[nexport].Pos[k] = P[i].Pos[k];
			HydroDataIn[nexport].Vel[k] = SphP[i].VelPred[k];
		      }
                    HydroDataIn[nexport].Hsml = SphP[i].Hsml;
                    HydroDataIn[nexport].Mass = P[i].Mass;
                    HydroDataIn[nexport].DhsmlDensityFactor = SphP[i].DhsmlDensityFactor;
                    HydroDataIn[nexport].Density = SphP[i].Density;
                    HydroDataIn[nexport].Pressure = SphP[i].Pressure;
                    HydroDataIn[nexport].Timestep = P[i].Ti_endstep - P[i].Ti_begstep;

                    /* calculation of F1 */
                    soundspeed_i = sqrt(GAMMA * SphP[i].Pressure / SphP[i].Density);
                    HydroDataIn[nexport].F1 = fabs(SphP[i].DivVel) /
		      (fabs(SphP[i].DivVel) + SphP[i].CurlVel +
                       0.0001 * soundspeed_i / SphP[i].Hsml / fac_mu);

		    HydroDataIn[nexport].Index = i;
		    HydroDataIn[nexport].Task = j;
		    nexport++;
		    nsend_local[j]++;
		  }
	      }
	  }
      tend = second();
      timecomp += timediff(tstart, tend);

      qsort(HydroDataIn, nexport, sizeof(struct hydrodata_in), hydro_compare_key);

      for(j = 1, noffset[0] = 0; j < NTask; j++)
	noffset[j] = noffset[j - 1] + nsend_local[j - 1];

      tstart = second();
#ifndef NOMPI
      MPI_Allgather(nsend_local, NTask, MPI_INT, nsend, NTask, MPI_INT, GADGET_WORLD);
#else
    nsend[0] = nsend_local[0];
#endif
      tend = second();
      timeimbalance += timediff(tstart, tend);

      /* now do the particles that need to be exported */

      for(level = 1; level < (1 << PTask); level++)
	{
	  tstart = second();
	  for(j = 0; j < NTask; j++)
	    nbuffer[j] = 0;
	  for(ngrp = level; ngrp < (1 << PTask); ngrp++)
	    {
	      maxfill = 0;
	      for(j = 0; j < NTask; j++)
		{
		  if((j ^ ngrp) < NTask)
		    if(maxfill < nbuffer[j] + nsend[(j ^ ngrp) * NTask + j])
		      maxfill = nbuffer[j] + nsend[(j ^ ngrp) * NTask + j];
		}
	      if(maxfill >= All.BunchSizeHydro)
		break;

	      sendTask = ThisTask;
	      recvTask = ThisTask ^ ngrp;
#ifndef NOMPI
	      if(recvTask < NTask)
		{
		  if(nsend[ThisTask * NTask + recvTask] > 0 || nsend[recvTask * NTask + ThisTask] > 0)
		    {
		      /* get the particles */
		      MPI_Sendrecv(&HydroDataIn[noffset[recvTask]],
				   nsend_local[recvTask] * sizeof(struct hydrodata_in), MPI_BYTE,
				   recvTask, TAG_HYDRO_A,
				   &HydroDataGet[nbuffer[ThisTask]],
				   nsend[recvTask * NTask + ThisTask] * sizeof(struct hydrodata_in), MPI_BYTE,
				   recvTask, TAG_HYDRO_A, GADGET_WORLD, &status);
		    }
		}
#endif
	      for(j = 0; j < NTask; j++)
		if((j ^ ngrp) < NTask)
		  nbuffer[j] += nsend[(j ^ ngrp) * NTask + j];
	    }
	  tend = second();
	  timecommsumm += timediff(tstart, tend);

	  /* now do the imported particles */
	  tstart = second();
	  for(j = 0; j < nbuffer[ThisTask]; j++)
	    get_sigvel_evaluate(j, 1);
	  tend = second();
	  timecomp += timediff(tstart, tend);

	  /* do a block to measure imbalance */
	  tstart = second();
#ifndef NOMPI
	      MPI_Barrier(GADGET_WORLD);
#endif
	  tend = second();
	  timeimbalance += timediff(tstart, tend);

	  /* get the result */
	  tstart = second();
	  for(j = 0; j < NTask; j++)
	    nbuffer[j] = 0;
	  for(ngrp = level; ngrp < (1 << PTask); ngrp++)
	    {
	      maxfill = 0;
	      for(j = 0; j < NTask; j++)
		{
		  if((j ^ ngrp) < NTask)
		    if(maxfill < nbuffer[j] + nsend[(j ^ ngrp) * NTask + j])
		      maxfill = nbuffer[j] + nsend[(j ^ ngrp) * NTask + j];
		}
	      if(maxfill >= All.BunchSizeHydro)
		break;

	      sendTask = ThisTask;
	      recvTask = ThisTask ^ ngrp;

#ifndef NOMPI
	      if(recvTask < NTask)
		{
		  if(nsend[ThisTask * NTask + recvTask] > 0 || nsend[recvTask * NTask + ThisTask] > 0)
		    {
		      /* send the results */
		      MPI_Sendrecv(&HydroDataResult[nbuffer[ThisTask]],
				   nsend[recvTask * NTask + ThisTask] * sizeof(struct hydrodata_out),
				   MPI_BYTE, recvTask, TAG_HYDRO_B,
				   &HydroDataPartialResult[noffset[recvTask]],
				   nsend_local[recvTask] * sizeof(struct hydrodata_out),
				   MPI_BYTE, recvTask, TAG_HYDRO_B, GADGET_WORLD, &status);

		      /* add the result to the particles */
		      for(j = 0; j < nsend_local[recvTask]; j++)
			{
			  source = j + noffset[recvTask];
			  place = HydroDataIn[source].Index;

			  for(k = 0; k < 3; k++)
			    SphP[place].FeedAccel[k] += HydroDataPartialResult[source].Acc[k];

			  if(SphP[place].MaxSignalVel < HydroDataPartialResult[source].MaxSignalVel)
			    SphP[place].MaxSignalVel = HydroDataPartialResult[source].MaxSignalVel;
			}
		    }
		}
#endif
	      for(j = 0; j < NTask; j++)
		if((j ^ ngrp) < NTask)
		  nbuffer[j] += nsend[(j ^ ngrp) * NTask + j];
	    }
	  tend = second();
	  timecommsumm += timediff(tstart, tend);

	  level = ngrp - 1;
	}

#ifndef NOMPI
      MPI_Allgather(&ndone, 1, MPI_INT, ndonelist, 1, MPI_INT, GADGET_WORLD);
#else
    ndonelist[0] = ndone;
#endif
      for(j = 0; j < NTask; j++)
	ntotleft -= ndonelist[j];
    }

  free(ndonelist);
  free(nsend);
  free(nsend_local);
  free(nbuffer);
  free(noffset);

  /* collect some timing information */
#ifndef NOMPI
  MPI_Reduce(&timecomp, &sumt, 1, MPI_DOUBLE, MPI_SUM, 0, GADGET_WORLD);
  MPI_Reduce(&timecommsumm, &sumcomm, 1, MPI_DOUBLE, MPI_SUM, 0, GADGET_WORLD);
  MPI_Reduce(&timeimbalance, &sumimbalance, 1, MPI_DOUBLE, MPI_SUM, 0, GADGET_WORLD);
#else
    sumt = timecomp;
    sumcomm = timecommsumm;
    sumimbalance = timeimbalance;
#endif
  if(ThisTask == 0)
    {
      All.CPU_HydCompWalk += sumt / NTask;
      All.CPU_HydCommSumm += sumcomm / NTask;
      All.CPU_HydImbalance += sumimbalance / NTask;
    }
}

/*! This function is the 'core' of the SPH force update. A target
 *  particle is specified which may either be local, or reside in the
 *  communication buffer.
 *  Note that only the acceleration due to feedback and the change
 *  of signal velocity are updated, not the actual variation of Entropy rate.
 */
void get_sigvel_evaluate(int target, int mode)
{
  int j, k, n, timestep, startnode, numngb;
  FLOAT *pos, *vel;
  FLOAT mass, h_i, dhsmlDensityFactor, rho, pressure, f1, f2;
  double acc[3], dtEntropy, maxSignalVel;
  double dx, dy, dz, dvx, dvy, dvz;
  double h_i2, hinv, hinv4;
  double p_over_rho2_i, p_over_rho2_j, soundspeed_i, soundspeed_j;
  double hfc, dwk_i, vdotr, vdotr2, visc, mu_ij, rho_ij, vsig;
  double h_j, dwk_j, r, r2, u, hfc_visc;

#ifndef NOVISCOSITYLIMITER
  double dt;
#endif

  if(mode == 0)
    {
      pos = P[target].Pos;
      vel = SphP[target].VelPred;
      h_i = SphP[target].Hsml;
      mass = P[target].Mass;
      dhsmlDensityFactor = SphP[target].DhsmlDensityFactor;
      rho = SphP[target].Density;
      pressure = SphP[target].Pressure;
      timestep = P[target].Ti_endstep - P[target].Ti_begstep;
      soundspeed_i = sqrt(GAMMA * pressure / rho);
      f1 = fabs(SphP[target].DivVel) /
        (fabs(SphP[target].DivVel) + SphP[target].CurlVel +
         0.0001 * soundspeed_i / SphP[target].Hsml / fac_mu);
    }
  else
    {
      pos = HydroDataGet[target].Pos;
      vel = HydroDataGet[target].Vel;
      h_i = HydroDataGet[target].Hsml;
      mass = HydroDataGet[target].Mass;
      dhsmlDensityFactor = HydroDataGet[target].DhsmlDensityFactor;
      rho = HydroDataGet[target].Density;
      pressure = HydroDataGet[target].Pressure;
      timestep = HydroDataGet[target].Timestep;
      soundspeed_i = sqrt(GAMMA * pressure / rho);
      f1 = HydroDataGet[target].F1;
    }

  /* initialize variables before SPH loop is started */
  acc[0] = acc[1] = acc[2] = dtEntropy = 0;
  maxSignalVel = 0;

  p_over_rho2_i = pressure / (rho * rho) * dhsmlDensityFactor;
  h_i2 = h_i * h_i;

  /* Now start the actual SPH computation for this particle */
  startnode = All.MaxPart;
  do
    {
      numngb = ngb_treefind_pairs(&pos[0], h_i, &startnode);

      for(n = 0; n < numngb; n++)
	{
	  j = Ngblist[n];

	  dx = pos[0] - P[j].Pos[0];
	  dy = pos[1] - P[j].Pos[1];
	  dz = pos[2] - P[j].Pos[2];

#ifdef PERIODIC			/*  find the closest image in the given box size  */
	  if(dx > boxHalf_X)
	    dx -= boxSize_X;
	  if(dx < -boxHalf_X)
	    dx += boxSize_X;
	  if(dy > boxHalf_Y)
	    dy -= boxSize_Y;
	  if(dy < -boxHalf_Y)
	    dy += boxSize_Y;
	  if(dz > boxHalf_Z)
	    dz -= boxSize_Z;
	  if(dz < -boxHalf_Z)
	    dz += boxSize_Z;
#endif
	  r2 = dx * dx + dy * dy + dz * dz;
	  h_j = SphP[j].Hsml;
	  if(r2 < h_i2 || r2 < h_j * h_j)
	    {
	      r = sqrt(r2);
	      if(r > 0)
		{
		  p_over_rho2_j = SphP[j].Pressure / (SphP[j].Density * SphP[j].Density);
		  soundspeed_j = sqrt(GAMMA * p_over_rho2_j * SphP[j].Density);
		  dvx = vel[0] - SphP[j].VelPred[0];
		  dvy = vel[1] - SphP[j].VelPred[1];
		  dvz = vel[2] - SphP[j].VelPred[2];
		  vdotr = dx * dvx + dy * dvy + dz * dvz;

		  if(All.ComovingIntegrationOn)
		    vdotr2 = vdotr + hubble_a2 * r2;
		  else
		    vdotr2 = vdotr;

                  if(r2 < h_i2)
                    {
                      hinv = 1.0 / h_i;
#ifndef  TWODIMS
                      hinv4 = hinv * hinv * hinv * hinv;
#else
                      hinv4 = hinv * hinv * hinv / boxSize_Z;
#endif
                      u = r * hinv;
                      if(u < 0.5)
                        dwk_i = hinv4 * u * (KERNEL_COEFF_3 * u - KERNEL_COEFF_4);
                      else
                        dwk_i = hinv4 * KERNEL_COEFF_6 * (1.0 - u) * (1.0 - u);
                    }
                  else
                    {
                      dwk_i = 0;
                    }

                  if(r2 < h_j * h_j)
                    {
                      hinv = 1.0 / h_j;
#ifndef  TWODIMS
                      hinv4 = hinv * hinv * hinv * hinv;
#else
                      hinv4 = hinv * hinv * hinv / boxSize_Z;
#endif
                      u = r * hinv;
                      if(u < 0.5)
                        dwk_j = hinv4 * u * (KERNEL_COEFF_3 * u - KERNEL_COEFF_4);
                      else
                        dwk_j = hinv4 * KERNEL_COEFF_6 * (1.0 - u) * (1.0 - u);
                    }
                  else
                    {
                      dwk_j = 0;
                    }

                  if(soundspeed_i + soundspeed_j > maxSignalVel)
                    maxSignalVel = soundspeed_i + soundspeed_j;

                  if(vdotr2 < 0)        /* ... artificial viscosity */
                    {
                      mu_ij = fac_mu * vdotr2 / r;      /* note: this is negative! */

                      vsig = soundspeed_i + soundspeed_j - 3 * mu_ij;

                      if(vsig > maxSignalVel)
                        maxSignalVel = vsig;

		      if(P[j].Ti_endstep == All.Ti_Current)
			if(vsig > SphP[j].MaxSignalVel)
			  SphP[j].MaxSignalVel = vsig;

                      rho_ij = 0.5 * (rho + SphP[j].Density);
                      f2 =
                        fabs(SphP[j].DivVel) / (fabs(SphP[j].DivVel) + SphP[j].CurlVel +
                                                0.0001 * soundspeed_j / fac_mu / SphP[j].Hsml);

                      visc = 0.25 * All.ArtBulkViscConst * vsig * (-mu_ij) / rho_ij * (f1 + f2);

                      /* .... end artificial viscosity evaluation */
#ifndef NOVISCOSITYLIMITER
                      /* make sure that viscous acceleration is not too large */
                      dt = imax(timestep, (P[j].Ti_endstep - P[j].Ti_begstep)) * All.Timebase_interval;
                      if(dt > 0 && (dwk_i + dwk_j) < 0)
                        {
                          visc = dmin(visc, 0.5 * fac_vsic_fix * vdotr2 /
                                      (0.5 * (mass + P[j].Mass) * (dwk_i + dwk_j) * r * dt));
                        }
#endif
                    }
                  else
                    visc = 0;

                  p_over_rho2_j *= SphP[j].DhsmlDensityFactor;

                  hfc_visc = 0.5 * P[j].Mass * visc * (dwk_i + dwk_j) / r;

                  hfc = hfc_visc + P[j].Mass * (p_over_rho2_i * dwk_i + p_over_rho2_j * dwk_j) / r;

                  acc[0] -= hfc * dx;
                  acc[1] -= hfc * dy;
                  acc[2] -= hfc * dz;

		  if(P[j].Ti_endstep == All.Ti_Current)
		    {
		      if(SphP[j].FeedbackFlag == 0)
			SphP[j].FeedbackFlag = 1;

		      if(maxSignalVel > SphP[j].MaxSignalVel)
			SphP[j].MaxSignalVel = maxSignalVel;

		      SphP[j].FeedAccel[0] -= hfc * dx;
		      SphP[j].FeedAccel[1] -= hfc * dy;
		      SphP[j].FeedAccel[2] -= hfc * dz;
		    }

		}
	    }
	}
      fflush(stdout);
    }
  while(startnode >= 0);

  /* Now collect the result at the right place */
  if(mode == 0)
    {
      SphP[target].MaxSignalVel = maxSignalVel;

      for(k = 0; k < 3; k++)
        SphP[target].FeedAccel[k] = acc[k];
    }
  else
    {
      HydroDataResult[target].MaxSignalVel = maxSignalVel;

      for(k = 0; k < 3; k++)
        HydroDataResult[target].Acc[k] = acc[k];
    }
}

#endif