/*
Copyright 2002-2008 Greg Kochanski. 
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
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 Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

This program may be downloaded from http://www.sourceforge.org from the "speechresearch" project.
*/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "gpk_std.h"
#include "gpklib/assert.h"
#include "gpklib/numerics.h"
#include "gpklib/romberg.h"
#include "gpklib/ieee754.h"
#include "gpklib/err.h"

/* Romberg integration on a closed or open interval. */

#define JMAX 15
C int KMAX  = 4;
C double BESTACC  = 1e-10;
C int MAX_ZEROSIZE  = 4;

#define D if(0)

static double max(double a, double b) {
 return a > b ? a : b;
}

static double max(double a, double b, double c) {
 return max(a, max(b, c));
}

/* Define this if you want to use routines from Numerical Recipes in C. */
#ifdef NumericalRecipes
#include "nr.h"
double ratintG(C double *xa, C double *ya, int n, double x, double *pdy)
{
 double y, dy, *dxa, *dya;
 int i;
 dxa = (double*)malloc(n*sizeof(double));
 dya = (double*)malloc(n*sizeof(double));
 assert(fxa && fya);
 for(i=0; i<n; i++) {
 	dxa[i] = xa[i];
	dya[i] = ya[i];
	}
 ratint(dxa, dya, n, x, &y, &dy);	/* From Numerical Recipes in C, 2nd Edition, Press et al. */
 free(dxa);
 free(dya);
 if(pdy)
 	*pdy = dy
 return y;
}

double polintG(C double *xa, C double *ya, int n, double x, double *pdy)
{
 double y, dy, *dxa, *dya;
 int i;
 dxa = (double*)malloc(n*sizeof(double));
 dya = (double*)malloc(n*sizeof(double));
 assert(fxa && fya);
 for(i=0; i<n; i++) {
 	dxa[i] = xa[i];
	dya[i] = ya[i];
	}
 polint(dxa, dya, n, x, &y, &dy);	/* From Numerical Recipes in C, 2nd Edition, Press et al. */
 free(dxa);
 free(dya);
 if(pdy)
 	*pdy = dy
 return y;
}
#else
# define ratintG ratint
# define polintG polint
#endif

/**\brief Romberg integration.
	Qromo computes the integral of func over the region
	(a,b) or [a,b] (the boundaries are set by choose()).
	When it is called, func is called
	with two extra arguments, c,
	which is an arbitrary, uninterpreted pointer,
	and nc, which is an arbitrary ininterpreted integer.
	Choose selects the integration technique; it is a
	pointer to a method_fcn such as midpnt().
	Size is an estimate of the absolute value of the integral,
	used in deciding when to stop refinement.
	It is normally harmless to set this to zero,
	though it might result in extra CPU time if the integral
	contains cancelling positive and negative parts.
	Eps specifies the desired fractional accuracy of the integration,
	typically in the range 0 to 1.
	More precisely, the routine stops refining the value of the
	integral when the estimated error < (size+fabs(value))*eps.
	Note that there is no guarentee that high precisions (e.g. eps<1e-6) are attainable.
	\exception Exits if eps is too small.
	\return the value of the integral, or NaN if it could not be computed.

	NOTE: the function func may be evaluated in parallel.
	If func changes c for any reason, it MUST do its own locking.
	Otherwise, read-only access to c is assumed.
*/
double qromox(integrand_function func,	/** Function to be integrated. */
			 double a,	/** Start of region. */
			 double b,	/** End of region. */
			 method_fcn choose,	/** Midpoint or other. */
			 void *c,	/* Extra info passed to integrand. */
			 int nc,	/* Extra info passed to integrand. */
			 double eps,	/* Accuracy control. */
			 double size,	/* Size estimate, to control termination. */
			 int kmax,
			 BOOL rational_interpolate
			 )
{
 double sext = quiet_nan(0);
 double s[JMAX+1], h[JMAX+1];
 int j;
 assert(kmax>0 and kmax<=JMAX);
 assert(func && choose);
 assert(finite(size));
 assert(eps > 0.0);
 if(a == b)
 	return 0.0;
 if(eps < BESTACC)      {
	fprintf(stderr, "Eps (error limit) is too small in qromo = %g\n", eps);
	exit(4);
	}
 int nzerosize = 0;
 double sum = 0.0;
 double ss = 0.0;
 double selast = quiet_nan(0), dss = quiet_nan(0);
 for(j=0; j<kmax; j++)    {
	C method_fr hs = (*choose)(func, a, b, j, c, nc);
	assert(hs.h>0.0 && hs.h <= 1.0);
	if(isnan(hs.s))
		return hs.s;
	sum += hs.s;
	ss += hs.ss;
	s[j] = sum * hs.h;
	h[j] = hs.hext;
	D fprintf(stderr, "j=%d, h[j]=%g, s[j]=%g fcn=%p\n", j, h[j], s[j], choose);
	C double sz = hypot(s[j], size/(b-a));
	if(!(sz>0.0)) {
		nzerosize += 1;
		if(nzerosize > MAX_ZEROSIZE) {
			gpk_abort("Qromo: Zero tolerance in integration", 0);
			}
	}
	C int nuse = j+1<kmax ? j+1 : kmax;
	double dss1;
	if(rational_interpolate)
		sext = ratintG(&h[j-nuse+1], &s[j-nuse+1], nuse, 0.0, j>0 ? &dss1 : (double*)0);
	else
		sext = polintG(&h[j-nuse+1], &s[j-nuse+1], nuse, 0.0, j>0 ? &dss1 : (double*)0);
	D fprintf(stderr, "\t sext=%g, nuse=%d\n", sext, nuse);
	assert(!isnan(sext));
	/* For very oscillatory functions, be more conservative and attempt higher accuracy.
	 * Oscillatory functions are detected by having a large value of sum(func**2)/sum(func)**2.
	 * */
	C double osc_fcn_fac = pow((hs.ss*hs.h)/(sz*sz) + 1.0, 0.25);
	if(j==0 || osc_fcn_fac*fabs(s[j]-s[j-1]) > 0.1*sz)	{
		selast = sext;
		continue;
		}

	C double dss2  = sext-selast;
	dss = sqrt(fabs(dss1*dss2));
	D fprintf(stderr, "\t dss=%g from %g-%g\n", dss, sext, selast);
	selast = sext;

	assert(eps*sz >= 0.0);
	if(dss <= eps * sz)
		return sext*(b-a);
	}
 // Looks like we need smaller steps.    However, it'd be dumb to continue
 // to use a uniform step size across the whole integral.  Instead, we'll break
 // up the integral into three pieces, and let each set its own step
 // size.
 C double a1 = a + (b-a)/3;
 C double b1 = b - (b-a)/3;
 // We estimate the total integral.    This helps us terminate some of these partial integrations
 // early if the become insignificantly small compared to the total.  We divide the total by ten
 // so that it is a conservative underestimate, on the grounds that we'd rather do some extra
 // work than terminate an integral early and get an inaccurate answer.  (Note that we don't
 // trust the result of the above integration, so there is some chance it is wildly inaccurate.)
 C double size_est = fabs(sext * (b-a)) / 10.0;
 C double newacc = max(eps/2, BESTACC);
 C double sum1 = qromox(func, a1, b1, choose, c, nc,
			newacc, max(size, size_est), kmax, rational_interpolate);
 C double sum2 = qromox(func, a, a1, choose, c, nc,
			newacc, max(size, size_est, fabs(sum1)),
			kmax, rational_interpolate);
 C double sum3 = qromox(func, b1, b, choose, c, nc,
			newacc, max(size, size_est, fabs(sum1 + sum2)),
			kmax, rational_interpolate);
 D fprintf(stderr, "Qromo: recursing %g-%g: %g %g %g\n", a, b, sum2, sum1, sum3);
 return sum1 + sum2 + sum3;
}



double qromo(integrand_function func,	/** Function to be integrated. */
			 double a,	/** Start of region. */
			 double b,	/** End of region. */
			 method_fcn choose,	/** Midpoint or other. */
			 void *c,	/* Extra info passed to integrand. */
			 int nc,	/* Extra info passed to integrand. */
			 double eps,	/* Accuracy control. */
			 double size	/* Size estimate, to control termination. */
			 )
{
 return qromox(func, a, b, choose, c, nc, eps, size, KMAX, FALSE);
}
