/**
  \file

  \author   Kevin P. Rauch
  \brief    Adaptive Gauss-Kronrod numerical integrator.

*/
#include <cmath>
#include <cstdio>

#include <algorithm>


namespace mutils {

/**
  Computes square of a number.

  \tparam R Floating-point type.
  \param[in]  x Argument.
  \return Square of argument.
*/
template<typename R>
R sqr(R x) { return x*x; }


#define MAXIT (sizeof(xptr)/sizeof(xptr[0])-1)

static double c_gl, (*func_gl)(double x);

static double hifn(double x)
{
  return((*func_gl)(x)/sqrt(x<c_gl ? c_gl-x : 1e-16*(c_gl ? fabs(c_gl) : 1)));
}

static double lofn(double x)
{
  return((*func_gl)(x)/sqrt(x>c_gl ? x-c_gl : 1e-16*(c_gl ? fabs(c_gl) : 1)));
}


typedef struct {
  double (*func)(double, void *);
  void   *fdata;
  double c;
} integ_data_t;

static double hifn_r(double x, void *vdata)
{
  integ_data_t *data = (integ_data_t *)vdata;
  double ac = data->c;
  return((*data->func)(x, data->fdata)/
         sqrt(x<ac ? ac-x : 1e-16*(ac ? fabs(ac) : 1)));
}

static double lofn_r(double x, void *vdata)
{
  integ_data_t *data = (integ_data_t *)vdata;
  double ac = data->c;
  return((*data->func)(x, data->fdata)/
         sqrt(x>ac ? x-ac : 1e-16*(ac ? fabs(ac) : 1)));
}


double kgauss_sub(double (*f)(double x), double (*f_r)(double x, void *),
  void *fdata, double a, double b, double c, double *acc,
  int root, int lev, double I0, int maxrecur, int paranoid)
{
  static double
x7[]=
 {0.9491079123427585245, 0.7415311855993944399, 0.40584515137739716691, 0},
w7[]=
 {0.12948496616886969327, 0.27970539148927666790, 0.38183005050511894495,
  0.20897959183673469388},
x15[]=
 {0.9914553711208126392, 0.8648644233597690728, 0.5860872354676911303,
  0.20778495500789846760},
w15[]=
 {0.06309209262997855329, 0.14065325971552591875, 0.19035057806478540991,
  0.10474107054236391401, 0.022935322010529224964, 0.10479001032225018384,
  0.16900472663926790283, 0.20443294007529889241},
x31[]=
 {0.9986871096784667298, 0.9753835882088933697, 0.9122048827832628784,
  0.8076889391724375091, 0.6673480981043001754, 0.49863678655283200429,
  0.30857924791058777890, 0.10452827381078071340},
w31[]=
 {0.031577706217045857274, 0.07033204641040065094, 0.09517802993183068012,
  0.052371606782402922364, 0.011319468444683435107, 0.052384370820982692472,
  0.08449876530124302120, 0.10221418000570274392, 0.0036349311950498838561,
  0.021039446258726795607, 0.042193500584546594485, 0.061821985645449856431,
  0.07787534711524599642, 0.09026180214655860231, 0.09919685766743291249,
  0.10409995547269735501},
x63[]=
 {0.9998092141980435177, 0.9960402386259685431, 0.9846371438756441798,
  0.9635649536133961699, 0.9319846573806651406, 0.8898093648749426400,
  0.8374568325601445865, 0.7756739083583348141, 0.7053824093748503091,
  0.6275454213822932614, 0.5430823509867011311, 0.45285563284960723138,
  0.35771483158603327041, 0.25855961875447247355, 0.15639264033608140153,
  0.052344665459830506663},
w63[]=
 {0.015788872779215423953, 0.035166023524553984272, 0.047589015038602680558,
  0.026185803412726870878, 0.005660867725095312756, 0.026192186880710567449,
  0.042249382781031758514, 0.051107090052427067322, 0.0018039393894459073286,
  0.010519600488254708543, 0.021096745715199243564, 0.030910992205938984344,
  0.038937673364353656898, 0.045130900978520531208, 0.049598428775219425281,
  0.052049977691713990513, 0.0005394072866580217702,
  0.0035577405571320363985, 0.008008877528118372922, 0.013129713474427210903,
  0.018455916099884639804, 0.023683152580752000206, 0.028605857490498295944,
  0.033099092907400232260, 0.037111404910397191759, 0.040648875788571024107,
  0.043742748418925043826, 0.046413730813032435148, 0.048652555041851185681,
  0.050419337829027882637, 0.051653256012700288788, 0.052290832457614024465},
x127[]=
 {0.9999732140537096663, 0.9994072045541133135, 0.9975832115407271425,
  0.9940109708349837137, 0.9883399710474278217, 0.9803243695495499628,
  0.9698006651097387898, 0.9566689345185500717, 0.9408797537558513210,
  0.9224249470755334487, 0.9013304843743343536, 0.8776505702242030085,
  0.8514623710548997088, 0.8228610497537872099, 0.7919549469554387927,
  0.7588609140247034710, 0.7236999634679475019, 0.6865935263842583902,
  0.6476606483346630945, 0.6070163823125118480, 0.5647714587971209075,
  0.5210330881098700049, 0.47590656926256125696, 0.42949731364743432232,
  0.38191294949982269268, 0.33326529310537285172, 0.28367206848397232384,
  0.23325827809314719522, 0.18215708913074090694, 0.13051006423363166234,
  0.07846658760948939210, 0.026182433405385318012},
w127[]=
 {0.007894436389622294500, 0.017583011762276993951, 0.023794507519301340354,
  0.013092901706363435451, 0.0028304340009942515258, 0.013096093440355333640,
  0.021124691390515879519, 0.025553545026213533694, 0.0009020326132240592195,
  0.005259800244982030301, 0.010548372857600215651, 0.015455496102969499733,
  0.019468836682176829061, 0.022565450489260265736, 0.024799214387609712688,
  0.026024988845856995283, 0.00026824492648199271332,
  0.0017788676370217658788, 0.004004438754554190525, 0.006564856737114416480,
  0.009227958049939655524, 0.011841576290375841473, 0.014302928745249129776,
  0.016549546453700112606, 0.018555702455198594863, 0.020324437894285511663,
  0.021871374209462521731, 0.023206865406516217476, 0.024326277520925592782,
  0.025209668914513941280, 0.025826628006350144365, 0.026145416228807012208,
  0.00007666028154662839408, 0.0005490365712772494279,
  0.0013149375828678979980, 0.0022863309701736851777,
  0.0034049581223715159493, 0.004624110998701176147, 0.005907814428674106763,
  0.007227959698738149350, 0.008561844248934732274, 0.009890749642424429307,
  0.011199152457074135131, 0.012474289971042915773, 0.013705938795913981808,
  0.014886315023128753955, 0.016010013553407559797, 0.017073903395086741336,
  0.018076904204550982752, 0.019019598918148049863, 0.019903688814155477260,
  0.020731356767667036739, 0.021504646454995216775, 0.022224966589876512971,
  0.022892785760346567564, 0.023507517384958226284, 0.024067543172072124703,
  0.024570314471893502741, 0.025012500880974706687, 0.025390194359438009696,
  0.025699193007816822839, 0.025935371167822469283, 0.026095105661905096599,
  0.026175694952196227010},
x255[]=
 {0.9999963067486951954, 0.9999152752559348126, 0.9996433483555746783,
  0.9990913137344203859, 0.9981867960827265115, 0.9968697356940136751,
  0.9950890556563352695, 0.9928012000738545143, 0.9899695008102539966,
  0.9865635049160346460, 0.9825582265834872996, 0.9779334496251760458,
  0.9726731369993381727, 0.9667649473643406394, 0.9601998389233076039,
  0.9529717393703213579, 0.9450772648209298621, 0.9365154750708113300,
  0.9272876559818071015, 0.9173971221917106181, 0.9068490349651485285,
  0.8956502311299054918, 0.8838090598842230091, 0.8713352249541538215,
  0.8582396302130598049, 0.8445342274991779505, 0.8302318660071731089,
  0.8153461432903685913, 0.7998912585787304708, 0.7838818697648637995,
  0.7673329559944715517, 0.7502596882683412608, 0.7326773107667189801,
  0.7146010356960037625, 0.6960459542984969734, 0.6770269662478566161,
  0.6575587289946255923, 0.6376556277780920590, 0.6173317660619912557,
  0.5966009751817463673, 0.5754768411167735517, 0.5539727456204857446,
  0.5321019185252632801, 0.5098774979234026434, 0.48731259509574956027,
  0.46442036146072008750, 0.44121405535670282430, 0.41770710704249008257,
  0.39391318079976768177, 0.36984623336963443023, 0.34552056811136913486,
  0.32095088424031443935, 0.29615232032684852625, 0.27114049099145203618,
  0.24593151549383166774, 0.22054203676240731795, 0.19498922939984828060,
  0.16929079535915298899, 0.14346494631303911128, 0.11753037221284078240,
  0.09150619611020609502, 0.06541191594510644914, 0.039267334634802255021,
  0.013092480382206823402},
w255[]=
 {0.003947218194811147250, 0.008791505881138496975, 0.011897253759650670177,
  0.006546450853181717726, 0.0014152170004971327031, 0.006548046720177666820,
  0.010562345695257939759, 0.012776772513106766847,
  0.00045101631047035672287, 0.0026299001224910151504,
  0.005274186428800107826, 0.007727748051484749866, 0.009734418341088414530,
  0.011282725244630132868, 0.012399607193804856344, 0.013012494422928497641,
  0.00013412814082582891278, 0.0008894338185147723354,
  0.0020022193772770952853, 0.0032824283685572082402,
  0.004613979024969827762, 0.005920788145187920736, 0.007151464372624564888,
  0.008274773226850056303, 0.009277851227599297431, 0.010162218947142755831,
  0.010935687104731260865, 0.011603432703258108738, 0.012163138760462796391,
  0.012604834457256970640, 0.012913314003175072183, 0.013072708114403506104,
  0.000038150115729678883900, 0.00027451814107948522455,
  0.0006574687913192254503, 0.0011431654850866902753,
  0.0017024790611857576058, 0.0023120554993505880716,
  0.0029539072143370533817, 0.0036139798493690746749,
  0.004280922124467366137, 0.004945374821212214654, 0.005599576228537067565,
  0.006237144985521457886, 0.006852969397956990904, 0.007443157511564376978,
  0.008005006776703779898, 0.008536951697543370668, 0.009038452102275491376,
  0.009509799459074024932, 0.009951844407077738630, 0.010365678383833518369,
  0.010752323227497608387, 0.011112483294938256485, 0.011446392880173283782,
  0.011753758692479113142, 0.012033771586036062351, 0.012285157235946751371,
  0.012506250440487353344, 0.012695097179719004848, 0.012849596503908411419,
  0.012967685583911234641, 0.013047552830952548299, 0.013087847476098113505,
  0.000010631773187689331551, 0.00007993324160416183366,
  0.00019934629225661752482, 0.00035868882139811536585,
  0.0005508028934055661185, 0.0007705041764177031044,
  0.0010138011699455911823, 0.0012771043628389088432,
  0.0015571262359162832193, 0.0018509459382531284054,
  0.0021560121638169668193, 0.0024700972126411342886,
  0.0027912405830786672989, 0.0031176998048792041352,
  0.0034479123342294940334, 0.0037804671803520797730,
  0.004114083913896834036, 0.004447597043058076436, 0.004779944312994714648,
  0.005110157949984851451, 0.005437358183979665448, 0.005760748571634470826,
  0.006079612745214887369, 0.006393312261301573719, 0.006701285238160699107,
  0.007003045467007902253, 0.007298181671581202210, 0.007586356581948386573,
  0.007867305490660966780, 0.008140833979476674666, 0.008406814548503037823,
  0.008665181949799686451, 0.008915927123675733374, 0.009159089753242067779,
  0.009394749581797212635, 0.009623016765016277986, 0.009844021640034099622,
  0.010057904370771690380, 0.010264804960494993925, 0.010464854101398603935,
  0.010658165257317194199, 0.010844828258217972251, 0.011024904540326784412,
  0.011198424015314858577, 0.011365383419231345237, 0.011525745897233022556,
  0.011679441536946909568, 0.011826368574785933586, 0.011966395058237419718,
  0.012099360836564130280, 0.012225079850850787912, 0.012343342780032364446,
  0.012453920155062457233, 0.012556566069048776249, 0.012651022586171432515,
  0.012737024893523940703, 0.012814307159984132831, 0.012882608979232163138,
  0.012941682193613858686, 0.012991297832248668971, 0.013031252857161437795,
  0.013061376397838128294, 0.013081535166722437529, 0.013091637782748817614},
    *xptr[]={x7, x15, x31, x63, x127, x255},
    *wptr[]={w7, w15, w31, w63, w127, w255};

  static int first=1;

  double acc1, sum_1=0, sum0, sum, *w, *x, epst, dx, xm, xr, xs1, xs2,
    fn[128], r, rc, eps, eps0;
  int i, num, iter, lm, looks_ok=1;

  if (a==b) return(0);
  xm=0.5*(a+b);
  if (root) {
    xs1=0.5*sqrt(c-a); xs2=0.5*sqrt(c-b); xr=2*(xs1-xs2);
  } else {
    xr=0.5*(b-a); xs1=xs2=0; /* not used */
  }
  acc1=std::max(1e-16, fabs(*acc));
  sum=0; iter=0; num=7; lm=(num+1)/2-1; x=xptr[0], w=wptr[0];
  if (root) {
    if (f_r) {
      for (i=0; i<lm; i++) {
        double z1=1+x[i], z2=1-x[i];

        fn[i]=(*f_r)(c+root*sqr(z1*xs1+z2*xs2), fdata)+
              (*f_r)(c+root*sqr(z1*xs2+z2*xs1), fdata);
        sum+=w[i]*fn[i];
      }
      fn[lm]=2*(*f_r)(c+root*sqr(xs1+xs2), fdata); sum+=w[lm]*fn[lm];
    } else {
      for (i=0; i<lm; i++) {
        double z1=1+x[i], z2=1-x[i];

        fn[i]=(*f)(c+root*sqr(z1*xs1+z2*xs2))+
              (*f)(c+root*sqr(z1*xs2+z2*xs1));
        sum+=w[i]*fn[i];
      }
      fn[lm]=2*(*f)(c+root*sqr(xs1+xs2)); sum+=w[lm]*fn[lm];
    }
  } else {
    if (f_r) {
      for (i=0; i<lm; i++) {
        dx=xr*x[i]; fn[i]=(*f_r)(xm-dx, fdata)+(*f_r)(xm+dx, fdata);
        sum+=w[i]*fn[i];
      }
      fn[lm]=2*(*f_r)(xm, fdata); sum+=w[lm]*fn[lm];
    } else {
      for (i=0; i<lm; i++) {
        dx=xr*x[i]; fn[i]=(*f)(xm-dx)+(*f)(xm+dx);
        sum+=w[i]*fn[i];
      }
      fn[lm]=2*(*f)(xm); sum+=w[lm]*fn[lm];
    }
  }

  lm++; rc=4*0.15; eps=0.05/(0.5*rc); /* Causes subdivision if first eps>0.05 */
  do {
    sum0=sum;
    iter++; x=xptr[iter]-lm; w=wptr[iter]; rc*=0.5;
    for (i=0, sum=0; i<lm; i++) sum+=w[i]*fn[i];
    if (root) {
      if (f_r) {
        for (i=lm; i<=num; i++) {
          double z1=1+x[i], z2=1-x[i];

          fn[i]=(*f_r)(c+root*sqr(z1*xs1+z2*xs2), fdata)+
                (*f_r)(c+root*sqr(z1*xs2+z2*xs1), fdata);
          sum+=w[i]*fn[i];
        }
      } else {
        for (i=lm; i<=num; i++) {
          double z1=1+x[i], z2=1-x[i];

          fn[i]=(*f)(c+root*sqr(z1*xs1+z2*xs2))+
                (*f)(c+root*sqr(z1*xs2+z2*xs1));
          sum+=w[i]*fn[i];
        }
      }
    } else {
      if (f_r) {
        for (i=lm; i<=num; i++) {
          dx=xr*x[i]; fn[i]=(*f_r)(xm-dx, fdata)+(*f_r)(xm+dx, fdata);
          sum+=w[i]*fn[i];
        }
      } else {
        for (i=lm; i<=num; i++) {
          dx=xr*x[i]; fn[i]=(*f)(xm-dx)+(*f)(xm+dx);
          sum+=w[i]*fn[i];
        }
      }
    }

    eps0=eps; lm=num+1; num=2*num+1;
    if (sum) eps=1e-16+fabs((sum0-sum)/sum); else eps=1;
    r=eps/eps0; if (I0) epst=fabs(xr*(sum-sum0)/I0); else epst=eps;

    /* Now estimate the true error from the local convergence rate. */
    if (iter>1) epst*=3*(r<1e-4 ? 1e-4 : r>1 ? 1 : r);

    /* If we're being ultraconservative, see if three successive values agree */
    if (paranoid) {
      if (I0)
        looks_ok=(fabs(xr*(sum_1-sum0))<=acc1*fabs(I0) &&
                    fabs(xr*(sum0-sum))<=acc1*fabs(I0) &&
                   fabs(xr*(sum_1-sum))<=acc1*fabs(I0));
      else
        looks_ok=(fabs(sum_1-sum0)<=acc1*fabs(sum0) &&
                    fabs(sum0-sum)<=acc1*fabs(sum) &&
                   fabs(sum_1-sum)<=acc1*fabs(sum));
      sum_1=sum0;
    }

    /* Subdivide if convergence too slow or final iteration before forced
       subdivision looks like it won't achieve accuracy goal. */
    if (r>rc || (iter==MAXIT-1 && epst*0.5*r>acc1)) {
      if (iter==1) epst*=3; /* First iter looks iffy, be more conservative. */
      break;
    }
  } while (((!looks_ok && epst<acc1) || (sum0!=sum && epst>acc1))
            && iter!=MAXIT);

  if (sum0==sum) epst=1e-16;
  if (!looks_ok || epst>acc1)
    if (lev<maxrecur && a!=xm) {
      if (I0==0 && epst<0.1) I0=xr*sum;
      /* main pass failed, so bias toward several small intervals with
         small maxiter, use estimate of integral I0 (if any good at all)
         to ensure each subinterval computed only as accurately as needed.
      */
      if (root) {
        integ_data_t data;
        data.func  = f_r;
        data.fdata = fdata;
        data.c     = c;

        if (root<0) {
          return(kgauss_sub(hifn, hifn_r, &data, a, xm, c,
                            acc, 0, lev+1, I0, maxrecur, paranoid)+
                 kgauss_sub(f, f_r, fdata, xm, b, c,
                            acc, -1, lev+1, I0, maxrecur, paranoid));
        } else {
          return(kgauss_sub(lofn, lofn_r, &data, 2*c-xm, 2*c-a, c,
                            acc, 0, lev+1, I0,maxrecur, paranoid)+
                 kgauss_sub(f, f_r, fdata, xm, b, c,
                            acc, 1, lev+1, I0, maxrecur, paranoid));
        }
      } else
        return(kgauss_sub(f, f_r, fdata, a, xm, c,
                          acc, 0, lev+1, I0, maxrecur, paranoid)+
               kgauss_sub(f, f_r, fdata, xm, b, c,
                          acc, 0, lev+1, I0, maxrecur, paranoid));
    } else {
      if (*acc>0 && first) {
        fprintf(stderr, "Precision not met in kgauss (%.1e/%.1e) in "
                "[%.12g,%.12g]\n", epst, acc1, xm-0.5*(b-a), xm+0.5*(b-a));
        first=0;
      }
      if (lev==0) *acc=(-epst);
    }
  else if (lev==0) *acc=epst;
  return(xr*sum);
}

double kgauss(double (*f)(double x), double a, double b, double err)
{
  return(kgauss_sub(f, NULL, NULL, a, b, 0.0, &err, 0, 0, 0., 100, 0));
}

double kgauss2(double (*f)(double x), double a, double b, double err)
{
  return(kgauss_sub(f, NULL, NULL, a, b, 0.0, &err, 0, 0, 0., 100, 1));
}

double ksqrtgauss(double (*f)(double x), double a, double b, double c,
  double err)
{
  /* Integrate f(x)/sqrt(x-c) or f(x)/sqrt(c-x) between arbitrary limits. */
  int sgn;

  func_gl=f; c_gl=c;
  if (a>b) { double tmp=a; a=b; b=tmp; sgn=-1; } else sgn=1;
  if (a>=c)
    return(sgn*kgauss_sub(f, NULL, NULL, 2*c-b, 2*c-a, c,
                          &err, 1, 0, 0., 100, 0));
  else if (b<=c)
         return(sgn*kgauss_sub(f, NULL, NULL, a, b, c,
                               &err, -1, 0, 0., 100, 0));
       else
         return(NAN);
}

double kgauss_r(double (*f_r)(double x, void *), void *fdata,
  double a, double b, double err)
{
  return(kgauss_sub(NULL, f_r, fdata, a, b, 0.0, &err, 0, 0, 0., 100, 0));
}

double kgauss2_r(double (*f_r)(double x, void *), void *fdata,
  double a, double b, double err)
{
  return(kgauss_sub(NULL, f_r, fdata, a, b, 0.0, &err, 0, 0, 0., 100, 1));
}

double ksqrtgauss_r(double (*f_r)(double x, void *), void *fdata,
  double a, double b, double c, double err)
{
  /* Integrate f(x)/sqrt(x-c) or f(x)/sqrt(c-x) between arbitrary limits. */
  int sgn;

  if (a>b) { double tmp=a; a=b; b=tmp; sgn=-1; } else sgn=1;
  if (a>=c)
    return(sgn*kgauss_sub(NULL, f_r, fdata, 2*c-b, 2*c-a, c,
                          &err, 1, 0, 0., 100, 0));
  else if (b<=c)
         return(sgn*kgauss_sub(NULL, f_r, fdata, a, b, c,
                               &err, -1, 0, 0., 100, 0));
       else
         return(NAN);
}

}  // namespace mutils