// $Id: mathx.cpp,v 1.55 2018/01/03 21:33:02 mkkuhner Exp $

/*
Copyright 2002-2005 Mary K. Kuhner, Peter Beerli, and Joseph Felsenstein

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

*/

/* Authors: Peter Beerli, Mary Kuhner, Jon Yamato and Joseph Felsenstein */


#include <cassert>
#include <cmath>
#include <iostream>
#include <numeric>
#include <stdio.h>

#include "constants.h" // for use of EXPMIN in UnderFlowExp()
#include "definitions.h"
#include "errhandling.h"
#include "mathx.h"
#include "registry.h"
#include "runreport.h"
#include "stringx.h"
#include "vectorx.h"

#ifdef DMALLOC_FUNC_CHECK
#include "/usr/local/include/dmalloc.h"
#endif

#ifdef HAVE_LGAMMA
#define LGAMMA lgamma
#else
#define LGAMMA mylgamma
#endif

using namespace std;

//------------------------------------------------------------------------------------

const double EigenCalculator::accuracy = 1e-15;
const double SMALL_v0 = 0.01; // erynes heuristic used by BesselK and DvBesselK
const double NUDGE_AMOUNT = 1.0e-09; // erynes heuristic used by BesselK and DvBesselK

//------------------------------------------------------------------------------------

double mylgamma (double z);

//------------------------------------------------------------------------------------
// Calculation of rate values following a gamma distribution for
// given probability values.

DoubleVec1d gamma_rates(double alpha, long int ratenum)
{
    vector<double> values;
    double x, low, mid, high, xlow, xhigh, freq, inc, mid0;
    long int i, j;

    values.reserve(ratenum);

    x    = 10.0;
    inc  = 1.0 / (double)ratenum;
    freq = -inc / 2.0;
    mid0 = incompletegamma(alpha, 10.0);

    for (i = 0; i < ratenum; i++)
    {
        low   = 0;
        mid   = mid0;
        high  = 1.0;
        freq += inc;

        if (freq < mid)
        {
            high  = mid;
            xlow  = 0;
            xhigh = 10.0;
            x     = 5.0;
        }

        else
        {
            low   = mid;
            xlow  = 10.0;
            xhigh = 1e10;
            x     = 1e5;
        }

        for (j = 0; j < 1000 && fabs(low - high) > 0.0001 && x > 0.000000001; j++)
        {
            mid = incompletegamma(alpha, x);
            if (freq < mid)
            {
                high  = mid;
                xhigh = x;
                x     = (x + xlow) / 2.0;
            }

            else
            {
                low  = mid;
                xlow = x;
                x    = (x + xhigh) / 2.0;
            }
        }

        if (x >= 10e10)
        {
            values.clear();
            break;
        }

        values.push_back(x / alpha);
    }

    return values;
}

//------------------------------------------------------------------------------------

#define MIN(a,b) (((a)<(b)) ? (b) : (a))

double my_gamma(double x)
{
    double result = log_gamma(x);
    if (result < EXPMAX)
        return exp(log_gamma(x));
    return EXP_OF_EXPMAX;
}

double
alnorm (double x, int up)
{
    /* Initialized data */
    /* *** machine dependent constants ????????????? */
    /*static */ double zero = 0.0;
    /*static */ double a1 = 5.75885480458;
    /*static */ double a2 = 2.62433121679;
    /*static */ double a3 = 5.92885724438;
    /*static */ double b1 = -29.8213557807;
    /*static */ double b2 = 48.6959930692;
    /*static */ double c1 = -3.8052e-8;
    /*static */ double c2 = 3.98064794e-4;
    /*static */ double c3 = -.151679116635;
    /*static */ double c4 = 4.8385912808;
    /*static */ double c5 = .742380924027;
    /*static */ double one = 1.0;
    /*static */ double c6 = 3.99019417011;
    /*static */ double d1 = 1.00000615302;
    /*static */ double d2 = 1.98615381364;
    /*static */ double d3 = 5.29330324926;
    /*static */ double d4 = -15.1508972451;
    /*static */ double d5 = 30.789933034;
    /*static */ double half = 0.5;
    /*static */ double ltone = 7.0;
    /*static */ double utzero = 18.66;
    /*static */ double con = 1.28;
    /*static */ double p = .398942280444;
    /*static */ double q = .39990348504;
    /*static */ double r = .398942280385;

    /*static */ double y, result;

    if (x < zero)
    {
        up = !up;
        x = -x;
    }
    if (x <= ltone || (up && x <= utzero))
    {
        y = half * x * x;
        if (x > con)
        {
            result =
                r * exp (-y) / (x + c1 +
                                d1 / (x + c2 +
                                      d2 / (x + c3 +
                                            d3 / (x + c4 +
                                                  d4 / (x + c5 +
                                                        d5 / (x + c6))))));
            return ((!up) ? one - result : result);
        }
        result =
            half - x * (p - q * y / (y + a1 + b1 / (y + a2 + b2 / (y + a3))));
        return ((!up) ? one - result : result);
    }
    else
    {
        return ((!up) ? 1.0 : 0.0);
    }
    /*fake */ return -99;
}                               // alnorm

// The "complementary error function."
double erfc(double x)
{
    if (x >= 0.0)
        return incompletegamma(0.5, x*x);
    return 2.0 - incompletegamma(0.5, x*x); // a mathematical fact
}

//  ALGORITHM AS239  APPL. STATIST. (1988) VOL. 37, NO. 3
//  Computation of the Incomplete Gamma Integral
//  Auxiliary functions required: LogG() = logarithm of the gamma function,
//  and Tail() = algorithm AS66
//  In Mathematica, this is GammaRegularized[a,x] == Gamma[a,x]/Gamma[a].
//  erynes note: The code below implements
//  incompletegamma(a,x) = (1/G(a)) times the integral of exp(-t)*t^(a-1)
//  from t = x to t = infinity for a > 0, where G(a) is the gamma function
//  (if "a" is a positive integer, then G(a) = (a-1)!).
//  erynes note number 2:
//  incompletegamma(1/2,x^2) == erfc(x) for x >= 0,
//  where erfc(x) is the complementary error function.
double incompletegamma (double alpha, double x)
{
    double gama, d_1, d_2, d_3;
    /*static */ double a, b, c, an, rn;
    /*static */ double pn1, pn2, pn3, pn4, pn5, pn6, arg;

    gama = 0.0;
    /*  Check that we have valid values for X and P */
    if (alpha <= 0.0 || x < 0.0)
    {
        logic_error e("Failure in incomplete-gamma calculation");
        throw e;
    }
    if (fabs (x) < DBL_EPSILON)
        return gama;

    /*  Use a normal approximation if P > PLIMIT */
    if (alpha > 1e3)
    {
        pn1 =
            sqrt (alpha) * 3.0 * (pow (x / alpha, (1.0 / 3.0)) + 1.0 / (alpha * 9.0) -
                                  1.0);
        gama = alnorm(pn1, false);
        return gama;
    }

    /*  If X is extremely large compared to P then set GAMMAD = 1 */
    if (x > 1e8)
    {
        gama = 1.0;
        return gama;
    }

    if (x <= 1.0 || x < alpha)
    {
        /*  Use Pearson's series expansion. */
        /*  (Note that P is not large enough to force overflow in lgamma()). */
        arg = alpha * log (x) - x - LGAMMA (alpha + 1.0);
        c = 1.0;
        gama = 1.0;
        a = alpha;
        while (c > 1e-14)
        {
            a += 1.0;
            c = c * x / a;
            gama += c;
        }
        arg += log (gama);
        gama = 0.0;
        if (arg >= -88.0)
        {
            gama = exp(arg);
        }

    }
    else
    {
        /*  Use a continued fraction expansion */
        arg = alpha * log (x) - x - LGAMMA (alpha);
        a = 1.0 - alpha;
        b = a + x + 1.0;
        c = 0.0;
        pn1 = 1.0;
        pn2 = x;
        pn3 = x + 1.0;
        pn4 = x * b;
        gama = pn3 / pn4;
        for (;;)
        {
            a += 1.0;
            b += 2.0;
            c += 1.0;
            an = a * c;
            pn5 = b * pn3 - an * pn1;
            pn6 = b * pn4 - an * pn2;
            if (fabs (pn6) > 0.0)
            {
                rn = pn5 / pn6;
                /* Computing MIN */
                d_2 = 1e-14, d_3 = rn * 1e-14;
                if ((d_1 = gama - rn, fabs (d_1)) <= MIN (d_2, d_3))
                {
                    arg += log (gama);
                    gama = 1.0;
                    if (arg >= -88.0)
                    {
                        gama = 1.0 - exp (arg);
                    }
                    return gama;
                }
                gama = rn;
            }
            pn1 = pn3;
            pn2 = pn4;
            pn3 = pn5;
            pn4 = pn6;
            if (fabs (pn5) >= 1e37)
            {
                /*  Re-scale terms in continued fraction if terms are large */
                pn1 /= 1e37;
                pn2 /= 1e37;
                pn3 /= 1e37;
                pn4 /= 1e37;
            }
        }
    }
    return gama;
}                               // incompletegamma()

//------------------------------------------------------------------------------------
// Uses Lanczos-type approximation to ln(gamma) for z > 0.
//  Reference: Lanczos, C. 'A precision approximation of the gamma function',
//      J. SIAM Numer. Anal., B, 1, 86-96, 1964.
//  Accuracy: About 14 significant digits except for small regions
//      in the vicinity of 1 and 2.
//  Programmer: Alan Miller CSIRO Division of Mathematics & Statistics
//  Latest revision - 17 April 1988

double log_gamma(double z)
{
    if (z <= 0.0)
        return DBL_MAX; // This will kill the receiving calculation.

    long int i;
    double result, denom;

    double a[] = { 1.659470187408462e-7, 9.934937113930748e-6,
                   -0.1385710331296526,  12.50734324009056,
                   -176.6150291498386,    771.3234287757674,
                   -1259.139216722289,     676.5203681218835 };

    result = 0.0;
    denom  = z + 7.0;
    for (i = 0; i < 8; i++)
    {
        result += a[i] / denom;
        denom  -= 1.0;
    }

    result += 0.9999999999995183;
    result = log(result) - (z + 6.5) + (z - 0.5) * log(z + 6.5) + 0.9189385332046727;
    return result;
}

// The function for chi is:
//  f(x) = {1/[2^(df/2) * gamma(df/2)]} * x^{[(df/2)-1] * e^(-x/2)}

double
find_chi (long int df, double prob)
{
    double a, b, m;
    double xb = 200.0;
    double xa = 0.0;
    double xm = 5.0;
    a = probchi (df, xa);
    m = probchi (df, xm);
    b = probchi (df, xb);
    while (fabs (m - prob) > EPSILON)
    {
        if (m < prob)
        {
            b = m;
            xb = xm;
        }
        else
        {
            a = m;
            xa = xm;
        }
        xm = (-(b * xa) + prob * xa + a * xb - prob * xb) / (a - b);      //(xa + xb)/2.0;

        m = probchi (df, xm);
    }
    return xm;
}

double
probchi (long int df, double chi)
{
    double prob;
    double v = ((double) df) / 2.0;
    if (chi > DBL_EPSILON && v > DBL_EPSILON)
    {
        //lg = EXP (LGAMMA (v));
        prob = 1.0 - incompletegamma (v, chi / 2.0);
    }
    else
    {
        prob = 1.0;
        // printf("prob=%f v=%f chi=%f lg(v/2)=%f  ig(chi/2,v/2)=%f\n",
        // prob,v,chi,lg, incompletegamma(chi/2.0,v/2.0));
    }

    return prob;
}

//       Uses Lanczos-type approximation to ln(gamma) for z > 0.
//       Reference:
//            Lanczos, C. 'A precision approximation of the gamma
//                    function', J. SIAM Numer. Anal., B, 1, 86-96, 1964.
//       Accuracy: About 14 significant digits except for small regions
//                 in the vicinity of 1 and 2.
//       Programmer: Alan Miller
//                   CSIRO Division of Mathematics & Statistics
//       Latest revision - 17 April 1988
// translated and modified into C by Peter Beerli 1997

double
mylgamma (double z)
{
    double a[9] = { 0.9999999999995183, 676.5203681218835,
                    -1259.139216722289, 771.3234287757674, -176.6150291498386,
                    12.50734324009056, -0.1385710331296526, 9.934937113930748e-6,
                    1.659470187408462e-7
    };
    double lnsqrt2pi = 0.9189385332046727;
    double result;
    long int j;
    double tmp;
    if (z <= 0.0)
    {
        return DBL_MAX;           //this will kill the receiving calculation
    }
    result = 0.0;
    tmp = z + 7.0;
    for (j = 9; j >= 2; --j)
    {
        result += a[j - 1] / tmp;
        tmp -= 1.0;
    }
    result += a[0];
    result = log (result) + lnsqrt2pi - (z + 6.5) + (z - 0.5) * log (z + 6.5);
    return result;
}                               // lgamma

//------------------------------------------------------------------------------------

double SafeDivide(double num, double denom)
{
    if (denom)
    {
        return num/denom;
    }
    else
    {
        return num > 0.0 ? DBL_BIG : -DBL_BIG;
    }

} // SafeDivide

//------------------------------------------------------------------------------------

double logfac (long int n)
{
    /* log(n!) values were calculated with Mathematica
       with a precision of 30 digits */
    switch (n)
    {
        case 0:
            return 0.0;
        case 1:
            return 0.0;
        case 2:
            return 0.693147180559945309417232121458;
        case 3:
            return 1.791759469228055000812477358381;
        case 4:
            return 3.1780538303479456196469416013;
        case 5:
            return 4.78749174278204599424770093452;
        case 6:
            return 6.5792512120101009950601782929;
        case 7:
            return 8.52516136106541430016553103635;
        case 8:
            return 10.60460290274525022841722740072;
        case 9:
            return 12.80182748008146961120771787457;
        case 10:
            return 15.10441257307551529522570932925;
        case 11:
            return 17.50230784587388583928765290722;
        case 12:
            return 19.98721449566188614951736238706;
        case 13:
            return 22.5521638531234228855708498286;
        case 14:
            return 25.1912211827386815000934346935;
        case 15:
            return 27.8992713838408915660894392637;
        case 16:
            return 30.6718601060806728037583677495;
        case 17:
            return 33.5050734501368888840079023674;
        case 18:
            return 36.3954452080330535762156249627;
        case 19:
            return 39.3398841871994940362246523946;
        case 20:
            return 42.3356164607534850296598759707;
        case 21:
            return 45.3801388984769080261604739511;
        case 22:
            return 48.4711813518352238796396496505;
        case 23:
            return 51.6066755677643735704464024823;
        case 24:
            return 54.7847293981123191900933440836;
        case 25:
            return 58.0036052229805199392948627501;
        case 26:
            return 61.2617017610020019847655823131;
        case 27:
            return 64.5575386270063310589513180238;
        case 28:
            return 67.8897431371815349828911350102;
        case 29:
            return 71.2570389671680090100744070426;
        case 30:
            return 74.6582363488301643854876437342;
        default:
            return log(factorial(static_cast<double>(n)));
            //return LGAMMA (n + 1.0);
    }
} // logfac

double factorial(double number)
{
    double temp;

    if(number <= 1.0) return 1.0;

    temp = number * factorial(number - 1.0);
    return temp;
}

//------------------------------------------------------------------------------------

double UnderFlowExp(double pow)
{
    return ((pow < EXPMIN) ? 0.0 : exp(pow));
} // UnderFlowExp

//------------------------------------------------------------------------------------

bool IsEven(long int n)
{
    // an even number divided by 2 is not truncated
    return ((n / 2L) * 2L == n);
} // IsEven

//------------------------------------------------------------------------------------

void ScaleLargestToZero(DoubleVec1d& unscaled)
{
    double big(*max_element(unscaled.begin(),unscaled.end()));
    for (unsigned long int wnum = 0; wnum < unscaled.size(); wnum++)
    {
        if (big <= EXPMIN)
        {
            assert(false); //Why is this?  We probably have an error somewhere.
            unscaled[wnum] = 0.0;
        }
        else if (unscaled[wnum] <= EXPMIN)
        {
            unscaled[wnum] = EXPMIN;
        }
        else
        {
            unscaled[wnum] -= big;
        }
    }
}

void ScaleToSumToOne(DoubleVec1d& vec)
{
    double sum = accumulate(vec.begin(), vec.end(), 0.0);
    transform(vec.begin(),
              vec.end(),
              vec.begin(),
              bind2nd(divides<double>(),sum));
}

// AddValsOfLogs takes vectors that are logs, scales and converts them to
//  normal values, adds them, takes their logs again, and re-scales them back.
DoubleVec1d AddValsOfLogs(DoubleVec1d vec1, DoubleVec1d vec2)
{
    assert(vec1.size() == vec2.size());
    double big(*max_element(vec1.begin(), vec1.end()));
    big = max(big, *max_element(vec2.begin(), vec2.end()));
    for (unsigned long int wnum=0; wnum<vec1.size(); wnum++)
    {
        if (big <= EXPMIN)
        {
            assert(false); //Why is this?  We probably have an error somewhere.
            vec1[wnum] = EXPMIN;
            vec2[wnum] = EXPMIN;
        }
        else
        {
            if (vec1[wnum] <= EXPMIN)
            {
                vec1[wnum] = EXPMIN;
            }
            else
            {
                vec1[wnum] -= big;
            }
            if (vec2[wnum] <= EXPMIN)
            {
                vec2[wnum] = EXPMIN;
            }
            else
            {
                vec2[wnum] -= big;
            }
        }
    }
    vec1 = SafeExp(vec1);
    vec2 = SafeExp(vec2);
    DoubleVec1d retvec = vec1;
    transform(vec2.begin(),
              vec2.end(),
              retvec.begin(),
              retvec.begin(),
              plus<double>());
    retvec = SafeLog(retvec);
    for (unsigned long int wnum = 0; wnum<retvec.size(); wnum++)
    {
        if ((retvec[wnum] <= EXPMIN) || (retvec[wnum] + big <=EXPMIN))
        {
            retvec[wnum] = EXPMIN;
        }
        else if ((retvec[wnum] >= EXPMAX) || (retvec[wnum] + big >= EXPMAX))
        {
            retvec[wnum] = EXPMAX;
        }
        else
        {
            retvec[wnum] += big;
        }
    }
    return retvec;
}

pair<double, double> EigenCalculator::Coeffs(double x, double y)
// cosine and sine of angle between the origin and (x,y)
// pair.first is cosine, pair.second is sine
{
    double root = sqrt(pow(x,2.0)+pow(y,2.0));
    pair<double, double> cs;
    if (root < accuracy)
    {
        cs.first = 1.0;
        cs.second = 0.0;
    }
    else
    {
        cs.first = x/root;
        cs.second = y/root;
    }
    return cs;
} // Coeffs

//------------------------------------------------------------------------------------

void EigenCalculator::Givens(DoubleVec2d& a, long int x, long int y, long int size,
                             const pair<double, double> cs, bool userow)
// Givens method.  Modifies input matrix.
{
    long int k;
    for (k = 0; k < size; ++k)
    {
        if (userow)
        {
            double d = cs.first * a[x][k] + cs.second * a[y][k];
            a[y][k] = cs.first * a[y][k] - cs.second * a[x][k];
            a[x][k] = d;
        }
        else
        {
            double d = cs.first * a[k][x] + cs.second * a[k][y];
            a[k][y] = cs.first * a[k][y] - cs.second * a[k][x];
            a[k][x] = d;
        }
    }
    // cases:  we pass array.size()=4
    // Pascal:  k from 1 to 4 (4 times)
    // C: k from 0 to 3 (4 times) (fine)
    // case:  we pass i (at its top value)
    // Pascal:  k from 1 to 4 (4 times)
    // C: k from 0 to 2 (3 times) (not fine)
} // Givens

//------------------------------------------------------------------------------------

void EigenCalculator::Tridiag(DoubleVec2d& a, DoubleVec2d& eigvecs)
{
    unsigned long int i, j;
    pair<double, double> cs;
    for (i = 1; i < a.size()-1; ++i)
    {
        for (j = i+1; j < a.size(); ++j)
        {
            cs = Coeffs(a[i-1][i], a[i-1][j]);
            Givens(a, i, j, a.size(), cs, true);
            Givens(a, i, j, a.size(), cs, false);
            Givens(eigvecs, i, j, eigvecs.size(), cs, true);
        }
    }
} // Tridiag

//------------------------------------------------------------------------------------

void EigenCalculator::Shiftqr(DoubleVec2d& a, DoubleVec2d& eigvecs)
{
    long int i;
    for (i = a.size()-1; i > 0; --i)
    {
        do {
            double d = sqrt(pow(a[i-1][i-1] - a[i][i], 2.0) +
                            pow(a[i][i-1], 2.0));
            double approx = a[i-1][i-1] + a[i][i];
            if (a[i][i] < a[i-1][i-1])
                approx = (approx-d)/2.0;
            else
                approx = (approx+d)/2.0;
            long int j;
            for (j = 0; j <= i; ++j)
                a[j][j] = a[j][j] - approx;
            for (j = 0; j <= i-1; ++j)
            {
                pair<double, double> cs = Coeffs(a[j][j], a[j+1][j]);
                // in the following two calls, Pascal's i has been
                // converted to i+1 because it is standing in for a
                // size.
                Givens(a, j, j+1, i+1, cs, true);
                Givens(a, j, j+1, i+1, cs, false);
                Givens(eigvecs, j, j+1, eigvecs.size(), cs, true);
            }
            for (j = 0; j <= i; ++j)
                a[j][j] += approx;
        } while (fabs(a[i][i-1]) > accuracy);
    }
} // Shiftqr

//------------------------------------------------------------------------------------

pair<DoubleVec1d, DoubleVec2d> EigenCalculator::Eigen(DoubleVec2d a)
// return a pair containing eigenvalues and eigenvectors
{
    unsigned long int i;
    DoubleVec2d eigvecs;
    for (i = 0; i < a.size(); ++i)
    {
        // ones along the diagonal, zeroes elsewhere
        DoubleVec1d row(a.size(), 0.0);
        row[i] = 1.0;
        eigvecs.push_back(row);
    }

    Tridiag(a, eigvecs);
    Shiftqr(a, eigvecs);
    DoubleVec1d eigvals(a.size(), 0.0);
    for (i = 0; i < a.size(); ++i)
    {
        eigvals[i] = a[i][i];
    }
    // changed 2018.1.3 on recommendation of Debian distro team
    return make_pair(eigvals, eigvecs);
} // Eigen

//------------------------------------------------------------------------------------

void EigenCalculator::DotProduct(const DoubleVec2d& first, const DoubleVec2d& second,
                                 DoubleVec2d& answer)
// dot product of first and second put into PRE-EXISTING answer!
// second has been PRE-TRANSPOSED!!
{
    // should be square
    assert(first.size() == first[0].size());
    // and all the same size!
    assert(first.size() == second.size());
    assert(first.size() == answer.size());
    double initial = 0.0;

    long int i, j, n = first.size();
    for (i = 0; i < n; ++i)
    {
        for (j = 0; j < n; ++j)
        {
            answer[i][j] = inner_product(first[i].begin(), first[i].end(), second[j].begin(), initial);
        }
    }

} // DotProduct

//------------------------------------------------------------------------------------

DoubleVec2d Invert(const DoubleVec2d& src)
// Invert square matrix via Gauss-Jordan reduction
{
    // copy the target vector
    DoubleVec2d a(src);

    // invert it in place (that's what I have code for!)
    unsigned long int i, j, k;
    unsigned long int n = a.size();
    double temp;

    for (i = 0; i < n; ++i)
    {
        // DEBUG:  need to do something if matrix is singular!
        temp = 1.0 / a[i][i];
        a[i][i] = 1.0;
        for (j = 0; j < n; ++j)
        {
            a[i][j] *= temp;
        }
        for (j = 0; j < n; ++j)
        {
            if (j != i)
            {
                temp = a[j][i];
                a[j][i] = 0.0;
                for (k = 0; k < n; ++k)
                {
                    a[j][k] -= temp * a[i][k];
                }
            }
        }
    }
    return a;
} // Invert

//------------------------------------------------------------------------------------

DoubleVec2d Transpose(const DoubleVec2d& src)
// Transpose a square matrix
{
    DoubleVec2d a(src);
    unsigned long int i, j, n=a.size();
    for (i = 0; i < n; ++i)
    {
        for (j = 0; j < n; ++j)
        {
            a[i][j] = src[j][i];
        }
    }
    return a;
} // Transpose

//------------------------------------------------------------------------------------

// This is a free function for approximate comparison of doubles.
// It is used to help find the correct profile modifier value in
// the face of rounding error.

bool CloseEnough(double a, double b)
{
    if (a == 0)
    {
        if (b == 0) return true;
        else return false;
    }
    if (a<0)
    {
        if (b<0)
        {
            a = -a;
            b = -b;
        }
        else return false;
    }
    double la = log(a);
    double lb = log(b);
    double test = fabs(la - lb);
    if (test < EPSILON) return true;
    else return false;

} // CloseEnough

double SafeExpAndSum(const DoubleVec1d& src)
{
    assert(!src.empty());  // don't call me on an empty vector!
    double sum(0.0), biggest(*(max_element(src.begin(),src.end())));
    DoubleVec1d::const_iterator lns;
    for(lns = src.begin(); lns != src.end(); ++lns)
    {
        if ((*lns) - biggest > EXPMIN)
            sum += exp((*lns) - biggest);
    }

    return SafeLog(sum) + biggest;

} // SafeExpAndSum

DoubleVec1d SafeExp(const DoubleVec1d& src)
{
    assert(!src.empty());  // don't call me on an empty vector!
    DoubleVec1d retvec;
    DoubleVec1d::const_iterator lns;
    for(lns = src.begin(); lns != src.end(); ++lns)
    {
        retvec.push_back(SafeExp(*lns));
    }

    return retvec;

} // SafeSumOfLogsToBeExp

double SafeExp(double src)
{
    if (src <= EXPMIN)
    {
        return 0.0;
    }
    else if (src >= EXPMAX)
    {
        return DBL_BIG;
    }
    return exp(src);

} // SafeExp

// Technique to avoid overflow and underflow of a*exp(x).
// In the description that follows, we set a = -a for a < 0,
// and we use "M" to represent the maximum feasible number.
// This means that numbers > M overflow, and numbers < 1/M underflow.
//
// First we consider the case of overflow.
// a*exp(x) will not overflow if a*exp(x) <= M, meaning exp(x) <= M/a.
// Defining EXPMAX = log(M), we see a*exp(x) will not overflow if
//     x <= EXPMAX - log(a).
// For the case in which exp(x) overflows but a*exp(x) does not, we compute
// a*exp(x) == (a*exp(EXPMAX)) * exp(x - EXPMAX), so that neither of the two
// terms on the right-hand side of this equation overflows.
// (We use the pre-computed EXP_OF_EXPMAX instead of computing exp(EXPMAX).)
// In practical terms, if EXPMAX = 200 and x = 212 so that exp(212) overflows,
// we can compute a*exp(x) if fabs(a) < 6.14e-6.
//
// Underflow is analogous to overflow.  In this case, we have
// fabs(a*exp(x)) < 1.0/M becoming identically 0 for sufficiently large M.
// If "a" is sufficiently greater than 1.0, then the product will not underflow.
// (Again, we set a = -a for a > 0 to avoid writing fabs(a).)
// For a*exp(x) to avoid underflow, we have the condition
// a*exp(x) >= 1.0/M, meaning exp(x) >= 1.0/(M*a), or x > log(1.0/(M*a)), so
// a*exp(x) will not underflow if
//    x >= EXPMIN - log(a).
// For the case in which exp(x) underflows but a*exp(x) does not, we compute
// a*exp(x) == (a*exp(EXPMIN)) * exp(x - EXPMIN), so that neither of the two
// terms on the right-hand side of this equation underflows.
// (We use the pre-computed EXP_OF_EXPMIN instead of computing exp(EXPMIN).)
//
// erynes 2003/11/21 -- devised and implemented this function
double SafeProductWithExp(double a, double x)
{
    if (0.0 == a)  // must reject this at the outset to avoid log(0) and 1/0
        return 0.0; // keep going if 0 < a < DBL_EPSILON, in case x is large
    bool a_is_negative = a < 0 ? true : false;
    if (a_is_negative)
        a *= -1.0;

    if (x > 0.0)
    {
        if (x <= EXPMAX - log(a))
        {
            // computable
            if (x <= EXPMAX)
                return a_is_negative ? -a*exp(x) : a*exp(x);
            // else 0.0 < a < 1.0
            double a_exp_x1 = EXP_OF_EXPMAX / (1.0/a);
            return a_is_negative ? -a_exp_x1*exp(x - EXPMAX)
                :  a_exp_x1*exp(x - EXPMAX);
        }
        return OverflowOfProductWithExp(a_is_negative ? -a : a, x);
    }

    // else x < 0.
    if (x >= EXPMIN - log(a))
    {
        // computable
        if (x >= EXPMIN)
            return a_is_negative ? -a*exp(x) : a*exp(x);
        // else a > 1.0
        double a_exp_x1 = a * EXP_OF_EXPMIN;
        return a_is_negative ? -a_exp_x1*exp(x - EXPMIN)
            :  a_exp_x1*exp(x - EXPMIN);
    }
    return UnderflowOfProductWithExp(a_is_negative ? -a : a, x);

} // double SafeProductWithExp(double a, double x)

// This function, when possible, computes exp(x1) - exp(x2)
// without overflow or underflow, although exp(x1) and/or exp(x2)
// may overflow or underflow individually.
// The technique is as follows:  Define "result" = exp(x1) - exp(x2).
// Then
//     result == exp(x2) * ( exp(x1)/exp(x2) - 1.0 )
//            == exp(x2) * ( exp(x1 - x2) - 1.0 ),
// and
//     log(result) == x2 + log(exp(x1 - x2) - 1.0),
// so that the right-hand side of this equation is computable.
// If EXPMIN < log(result) < EXPMAX, then we can return
//     result = exp(x2 + log(exp(x1 - x2) - 1.0).
// Note that, in practice, for x1 > EXPMAX, x2 must be extremely
// close to x1 in order for this function to be useful.
// For example, for EXPMAX = 200 and x2 = 212 with exp(212) overflowing,
// SafeExpDiff(x1, x2) will overflow and return EXP_OF_EXPMAX
// unless 212.000006 >= x1 >= 211.999994.
//
// erynes 2003/11/21 -- implemented and half-devised this function
double SafeExpDiff(double x1, double x2)
{
    if (x1 >= EXPMIN && x2 >= EXPMIN &&
        x1 <= EXPMAX && x2 <= EXPMAX)
        return exp(x1) - exp(x2);

    bool result_is_negative = x2 > x1 ? true : false;
    double x_larger  = result_is_negative ? x2 : x1,
        x_smaller = result_is_negative ? x1 : x2,
        arg_diff = x_larger - x_smaller;

    if (arg_diff < DBL_EPSILON)
        return 0.0;

    if (arg_diff > EXPMAX)
    {
        if (x_smaller < 0.0 && x_larger < EXPMAX)
            return result_is_negative ? -exp(x_larger) : exp(x_larger);
        return OverflowOfSafeExpDiff(x1, x2);
    }

    if (arg_diff < EXPMIN)
        return UnderflowOfSafeExpDiff(x1, x2); // recall x_smaller < x_larger

    double log_of_result = x_smaller + log(exp(arg_diff) - 1);

    if (log_of_result > EXPMAX)
        return OverflowOfSafeExpDiff(x1, x2);

    if (log_of_result < EXPMIN)
        return UnderflowOfSafeExpDiff(x1, x2);

    return result_is_negative ? -exp(log_of_result) : exp(log_of_result);
}

// This function is called when SafeExpDiff(x1, x2) overflows,
// which means exp(x1) - exp(x2) overflows.
// It can easily be modified to also track how often this overflows.
// 2003/11/21 added by erynes
double OverflowOfSafeExpDiff(double x1, double x2)
{
    RunReport& runreport = registry.GetRunReport();
    string msg="Overflow error:  Attempted to compute exp(";
    msg += ToString(x1) + ") - exp(" + ToString(x2) + ").  Returning "
        + ToString(x1 < x2 ? -EXP_OF_EXPMAX : EXP_OF_EXPMAX)
        + " = " + (x1 < x2 ? "-" : "") + "EXP_OF_EXPMAX.  "
        + "(Further overflow errors of this type will not be reported.)\n";
    runreport.ReportOnce(msg, oncekey_OverflowOfSafeExpDiff, true);

    return x1 < x2 ? -EXP_OF_EXPMAX : EXP_OF_EXPMAX;
}

// This function is called when SafeExpDiff(x1, x2) underflows,
// which means exp(x1) - exp(x2) underflows.
// It can easily be modified to track how often this underflows.
// 2003/11/21 added by erynes
double UnderflowOfSafeExpDiff(double x1, double x2)
{
    RunReport& runreport = registry.GetRunReport();
    string msg = "Underflow error:  Attempted to compute exp(";
    msg += ToString(x1) + ") - exp(" + ToString(x2) + ").  Returning 0.  "
        + "(Further underflow errors of this type will not be reported.)\n";
    runreport.ReportOnce(msg, oncekey_UnderflowOfSafeExpDiff, false);

    return 0.0;
}

// This function is called when SafeProductWithExp(a, x) overflows,
// which means a*exp(x) overflows.
// It can easily be modified to also track how often a*exp(x) overflows.
// 2003/11/21 added by erynes
double OverflowOfProductWithExp(double a, double x)
{
    RunReport& runreport = registry.GetRunReport();
    string msg = "Overflow error:  Attempted to compute ";
    msg += ToString(a) + " * exp(" + ToString(x) + ").  Returning "
        + ToString(a < 0 ? -EXP_OF_EXPMAX : EXP_OF_EXPMAX)
        + " = " + (a < 0 ? "-" : "") + "EXP_OF_EXPMAX.  "
        + "(Further overflow errors of this type will not be reported.)\n";
    runreport.ReportOnce(msg, oncekey_OverflowOfProductWithExp, true);

    return a < 0 ? -EXP_OF_EXPMAX : EXP_OF_EXPMAX;
}

// This function is called when SafeProductWithExp(a, x) underflows,
// which means a*exp(x) underflows.
// It can easily be modified to track how often a*exp(x) underflows.
// 2003/11/21 added by erynes
double UnderflowOfProductWithExp(double a, double x)
{
    RunReport& runreport = registry.GetRunReport();
    string msg = "Underflow error:  Attempted to compute ";
    msg += ToString(a) + " * exp(" + ToString(x) + ").  Returning 0.  "
        + "(Further underflow errors of this type will not be reported.)\n";
    runreport.ReportOnce(msg, oncekey_UnderflowOfProductWithExp, false);

    return 0;
}

double LogZero(const double x)
{
    return ((x > 0.0) ? log(x) : 0.0);
}

double SafeLog(const double x)
{
    return ((x > 0.0) ? log(x) : -DBL_BIG);
}

DoubleVec1d SafeLog(const DoubleVec1d src)
{
    DoubleVec1d retvec;
    for (DoubleVec1d::const_iterator i=src.begin(); i != src.end(); ++i)
    {
        retvec.push_back(SafeLog(*i));
    }
    return retvec;
}

long int ChooseRandomFromWeights(DoubleVec1d& weights)
{
    double sumweights = accumulate(weights.begin(),weights.end(),0.0);
    transform(weights.begin(),weights.end(),weights.begin(),
              bind2nd(divides<double>(),sumweights));

    long int chosen = -1;
    double chance = registry.GetRandom().Float();
    while (chance > 0 && chosen < static_cast<long int>(weights.size()-1))
    {
        chance -= weights[++chosen];
    }
    return chosen;
}

// ExpE1(x) -- added 2004/10/06 by erynes to calculate the expectation value
//                                           of "endtime" with positive growth
//
// Receives a positive real number x, and returns exp(x)*E1(x), where E1(x) is
// the exponential integral function En(x) for n = 1.  The function returns the
// product of exp(x) and E1(x), rather than E1(x) or En(x) alone, because at
// present we only use the product of exp(x) and E1(x), and since the most
// common form of E1(x) includes an internal factor of exp(-x), we can avoid two
// calls to exp() by allowing exp(x) and exp(-x) to cancel one another first.
//
double ExpE1(const double& x)
{
    if (x <= 0.0)
    {
        string msg = " Nonpositive number (" + ToString(x)
            + ") received by math function ExpE1(); this is illegal.";
        implementation_error e(msg);
        throw e;
    }

    const double epsilon = 1.0e-07; // convergence criterion
    const unsigned long int max_iterations = 15UL; // surrender criterion
    double result;

    if (x >= 1.0)
    {
        // Continued fraction representation:  Lentz's algorithm.
        double a, b, c, d, h, delta;
        unsigned long int nIter(0UL);
        b = x + 1.0;
        c = 1.0/DBL_EPSILON;
        d = 1.0/b;
        h = d;
        for (unsigned long int i = 1; i <= max_iterations; i++)
        {
            a = -1.0*i*i;
            b += 2.0;
            d = 1.0/(a*d + b);
            c = b + a/c;
            delta = c*d;
            h *= delta;
            nIter++;
            if (fabs(delta - 1.0) < epsilon)
                return h;
        }
        // Failed to converge after nIter iterations.
        // Apparently this can only happen when x is very close to 1.
        // In that case, our result will be close enough.
        return h;
    }

    else
    {
        //series representation
        double factor, term;
        unsigned long int nIter(0UL);
        result = -log(x) - EULERS_CONSTANT;
        factor = 1.0;
        for (unsigned long int i = 1; i <= max_iterations; i++)
        {
            factor *= -x/i;
            term = -factor/i;
            result += term;
            nIter++;
            if (fabs(term) < fabs(result) * epsilon)
                return SafeProductWithExp(result, x);
        }
        // Failed to converge after nIter iterations.
        // Apparently this can only happen when x is very close to 1.
        // In that case, our result will be close enough.
        return SafeProductWithExp(result, x);
    }
}

// Function to compute the modified Bessel function of the second kind,
// of order v, at argument x.  It is also sometimes called the MacDonald
// function.  It is denoted in the literature as K(v,x), where "v" is
// written as a subscript, rather than a parameter.  Mathematica denotes
// this function BesselK, so we'll do the same.
//
// Because K(v,x) is calculated iteratively, and because
// dK(v,x)/dx is a simple function of both K(v,x) and K(v+1,x),
// this function returns both of these results, since the latter
// can be obtained essentially for free once the former is computed.
// The return value of this function is K(v,x).  K(v+1,x) is returned
// by reference in argument "resultFor_vPlusOne."
//
// Implemented by erynes in July and October 2005, adapted from:
// Shanjie Zhang and Jianming Jin, _Computation of Special Functions._
// New York: Wiley-Interscience (1996).
//
// Contains an original approximation for x < 9 when v is very close
// to an integer.  No special approximation appears to be needed
// for any other case, including when v is equal to an integer.
//
double BesselK(double v, double x, double& resultFor_vPlusOne)
{
    if (x <= 0.0)
        throw implementation_error("BesselK() received illegal argument:  "
                                   + ToString(x) + ".");

    bool orderIsNegative(false), mustUseTwo_v0s(false),
        first_v0_calculation(true);
    double result(0.0); // used ONLY if -1 < v < 0

    if (v < 0.0)
    {
        if (v > -1.0)
        {
            // Generally, we calculate K(v0,x) and K(v0+1,x),
            // where v0 is the "fractional part" of v, and then
            // we calculate K(v,x) and K(v+1,x) iteratively.
            // If v <= -1, we use the property K(-v,x) = K(v,x),
            // and swap the results at the end.  But if -1 < v < 0,
            // then v < 0 and v+1 > 0, so we have to compute K(v0,x)
            // twice--once for v (say, -0.7), and a separate time
            // for v+1 (say, 0.3).
            v = -v;
            mustUseTwo_v0s = true;
        }
        else
        {
            // K(-v, x) == K(v, x) and K(-v + 1, x) == K(v - 1, x).
            v = -v - 1.0;
        }
        orderIsNegative = true; // later, we'll swap K(v+1,x) and K(v,x)
    }

    // For tiny x, K(v,x) = 0.5*gamma(v)*pow(0.5*x, -v).
    double gamma_vPlus1__DividedBy__DBL_BIG = my_gamma(v+1.0)/DBL_BIG;
    if (x <= 2.0*pow(gamma_vPlus1__DividedBy__DBL_BIG*0.5, 1.0/(v+1.0)))
    {
        // K(v+1,x) overflows, and hence K(v,x) overflows too.
        resultFor_vPlusOne = DBL_BIG;
        return DBL_BIG;
    }

    // For huge x, K(v,x) = sqrt(PI/(2x))*exp(-x).
    if (x > -EXPMIN || 0.0 == exp(-x)*sqrt(0.5*PI/x))
    {
        // K(v+1,x) underflows, for all v.
        resultFor_vPlusOne = 0.0;
        return 0.0;
    }

    const double EPSILON_ZJ = 1.0e-15; // Zhang & Jin's convergence criterion
    unsigned long int n = static_cast<unsigned long int>(floor(v)); // closest integral order
    double v0 = v - n; // the "fractional part" of the order
    const unsigned long int MAX_ITERATIONS = 50; // Zhang & Jin's heuristic cutoff

    double half_x_squared = 0.25*x*x; // avoid repeated recalculations
    double Kv0 = 0.0;      // == K(v0, x)
    double Kv0plus1 = 0.0; // == K(v0+1, x)

    // First, calculate K(v0, x) and K(v0+1, x).

  Calculate_Kv0:
    if (x <= 9.0)
    {
        // Small argument:  Use the series expansion.
        if (0.0 == v0)
        {
            // K(0,x) = -(ln(x/2) + EULERS_CONSTANT)*I(0,x)
            //          + sum(coeff_i*term_i, from i=1 to i=large),
            //
            // where I(0,x) = modified Bessel fn. of the 1st kind of order 0
            //              = sum(term_i, from i = 0 to i = large),
            // with
            //
            // term_i = ((x/2)^(2*i)) / (i!)^2
            //
            // and
            //
            // coeff_i = sum(1/j, from j=1 to j=i), or 1 if i=0.
            //
            // K(1,x) = (1/x) + (ln(x/2) + EULERS_CONSTANT)*I(1,x)
            //                - (x/2)*sum(coeff_i_2*term_i_2, from i=1 to i=large),
            // where I(1,x) = modified Bessel fn. of the 1st kind of order 1
            //              = (x/2)*sum(term_i_2, from i=1 to i=large),
            //
            // with
            //
            // term_i_2 = ((x/2)^(2*i)) / (i!*(i+1)!)
            //
            // and
            //
            // coeff_i_2 = sum(1/j, from j=1 to j=i) + (1/2)*(1/(i+1)), or 1/2 if i=0.

            double prevKv0 = DBL_BIG, prevKv0plus1 = DBL_BIG;
            double term_A = log(0.5*x) + EULERS_CONSTANT;
            double term_i = 1.0, term_i_2 = 1.0;
            double coeff_i = 0.0, coeff_i_2 = 0.0;
            Kv0 = -term_A; // i=0 term
            Kv0plus1 = 1.0/x + term_A*0.5*x - 0.25*x; // i=0 term

            for (unsigned long int i = 1; i <= MAX_ITERATIONS; i++)
            {
                term_i *= half_x_squared/(i*i);
                term_i_2 *= half_x_squared/(i*(i+1));
                coeff_i += 1.0/i;
                coeff_i_2 = coeff_i + 1.0/(2.0*(i+1.0));
                Kv0 += (-term_A + coeff_i)*term_i;
                Kv0plus1 += (term_A - coeff_i_2)*0.5*x*term_i_2;
                if (fabs((prevKv0 - Kv0)/Kv0) < EPSILON_ZJ &&
                    fabs((prevKv0plus1 - Kv0plus1)/Kv0plus1) < EPSILON_ZJ)
                    break; // convergence achieved
                prevKv0 = Kv0;
                prevKv0plus1 = Kv0plus1;
            }
        }

        else // v0 > 0
        {
            // K(v0, x) = (PI/2)*(I(-v0, x) - I(v0, x))/sin(v0*PI),
            // where
            // I(v0,x) = modified Bessel fn. of the 1st kind of order v0
            //         = sum(((x/2)^(2i+v0))/(i!*gamma(i+v0+1)),
            //                from i=0 to i=large)
            //
            // and gamma(x+1) = x*gamma(x).
            //
            // For v0 close to 0 or 1, the above formula for K(v0,x) approaches 0/0,
            // and behaves quasi-randomly.  An excellent example is K(1.999999,7).
            // Increasing the number of iterations and increasing the strictness
            // of the convergence criterion (i.e., decreasing EPSILON_ZJ)
            // does not seem to help.  We thus employ the following approximation,
            // derived by erynes in autumn 2005.

            if (v0 <= SMALL_v0 || v0 >= 1.0 - SMALL_v0)
            {
                double K0(0.0), K1(0.0); // K(0,x) and K(1,x)
                K0 = BesselK(0.0, x, K1);
                if (v0 >= 1.0 - SMALL_v0)
                {
                    v0 = -(1.0 - v0); // calculate K(-eps, x), K(1 - eps, x), etc.
                    n += 1; // An example:  Redefining 2 + .95 to be 3 - .05, n=2 --> n=3.
                }

                Kv0 = K0 * (v0*v0/(2.0*x + 1.0) + 1.0);
                Kv0plus1 = ((2.0*x-1.0)*(4.0*x+1.0)*K1/(2.0*(x+1.0)*(x+1.0)))*v0*v0 + (K0/x)*v0 + K1;

                if (0 == n)
                {
                    resultFor_vPlusOne = Kv0plus1;
                    if (Kv0 >= DBL_BIG)
                    {
                        // Try to avoid returning "inf" in either variable.
                        resultFor_vPlusOne = DBL_BIG;
                        return DBL_BIG;
                    }
                    if (resultFor_vPlusOne >= DBL_BIG)
                        resultFor_vPlusOne = DBL_BIG;
                    return Kv0;
                }
                if (1 == n)
                {
                    resultFor_vPlusOne = Kv0 + (2.0*(1.0 + v0)/x)*Kv0plus1;
                    if (Kv0plus1 >= DBL_BIG)
                    {
                        // Try to avoid returning "inf" in either variable.
                        resultFor_vPlusOne = DBL_BIG;
                        return DBL_BIG;
                    }
                    if (resultFor_vPlusOne >= DBL_BIG)
                        resultFor_vPlusOne = DBL_BIG;
                    return Kv0plus1;
                }
            }

            else // the size of v0 makes K(v0, x) = (PI/2)*(I(-v0, x) - I(v0, x))/sin(v0*PI) stable
            {
                // Variables for computing K(v0,x).
                double gamma_1_plus_v0 = my_gamma(1.0+v0),
                    gamma_1_minus_v0 = my_gamma(1.0-v0);
                double factor_neg = 1.0/(gamma_1_minus_v0*pow(0.5*x, v0));
                double factor_pos = 1.0/(factor_neg*gamma_1_minus_v0*gamma_1_plus_v0);
                double neg_term_i = 1.0, pos_term_i = 1.0;
                double sum = factor_neg - factor_pos; // i=0 terms
                double prev_sum = DBL_BIG;
                // Variables for computing K(v0+1,x).
                double factor_neg_2 = -v0*factor_neg/(0.5*x);
                double factor_pos_2 = factor_pos*0.5*x/(1.0+v0);
                double neg_term_i_2 = 1.0, pos_term_i_2 = 1.0;
                double sum_2 = factor_neg_2 - factor_pos_2; // i=0 terms
                double prev_sum_2 = DBL_BIG;

                for (unsigned long int i = 1; i < 2.4*MAX_ITERATIONS; i++) // Zhang & Jin's heuristic cutoff
                {
                    // Compute the sum for K(v0,x).
                    neg_term_i *= half_x_squared/(i*(i - v0));
                    pos_term_i *= half_x_squared/(i*(i + v0));
                    sum += factor_neg*neg_term_i - factor_pos*pos_term_i;

                    // Compute the sum for K(v0+1,x).
                    neg_term_i_2 *= half_x_squared/(i*(i - (v0+1.0)));
                    pos_term_i_2 *= half_x_squared/(i*(i + (v0+1.0)));
                    sum_2 += factor_neg_2*neg_term_i_2 - factor_pos_2*pos_term_i_2;

                    if (fabs((prev_sum - sum)/sum) < EPSILON_ZJ &&
                        fabs((prev_sum_2 - sum_2)/sum_2) < EPSILON_ZJ)
                        break; // convergence achieved
                    prev_sum = sum;
                    prev_sum_2 = sum_2;
                }

                Kv0 = (0.5*PI)*sum/sin(v0*PI);
                Kv0plus1 = (0.5*PI)*sum_2/sin((v0+1.0)*PI);
            }
        }
    }
    else // x > 9.
    {
        // Large argument:  Use the asymptotic expansion.
        //
        // K(v0,x) = sqrt(PI/(2x))*exp(-x)*(1 + sum(product[(u - (2k-1)^2)/(8k*x),
        //                                                  from u=1 to u=m],
        //                                          from m=1 to m=kmax)),
        // where u = 4*v0*v0
        // and kmax is an integer determined heuristically by Zhang & Jin (1996).
        //

        double sum(0.0), product(1.0), sum_2(0.0), product_2(1.0);
        double four_v0_squared = 4.0*v0*v0; // avoid repeated recalculations
        double four_v0plus1_squared = 4.0*(v0+1.0)*(v0+1.0); // ditto
        unsigned long int kmax = 14; // kmax is Zhang & Jin's heuristic cutoff

        if (x >= 50.0)
            kmax = 8;  // Zhang & Jin heuristics
        else if (x >= 35.0)
            kmax = 10; // Zhang & Jin heuristics

        for (unsigned long int k = 1; k <= kmax; k++)
        {
            product *= (four_v0_squared - (2.0*k - 1.0)*(2.0*k - 1.0))/(8.0*k*x);
            sum += product;
            product_2 *= (four_v0plus1_squared - (2.0*k - 1.0)*(2.0*k - 1.0))/(8.0*k*x);
            sum_2 += product_2;
        }
        Kv0 = sqrt(PI/(2.0*x))*exp(-x)*(1.0 + sum);
        Kv0plus1 = sqrt(PI/(2.0*x))*exp(-x)*(1.0 + sum_2);
    }

    if (mustUseTwo_v0s)
    {
        if (first_v0_calculation)
        {
            result = Kv0;
            first_v0_calculation = false;
            v0 = -v + 1.0;
            goto Calculate_Kv0;
        }
        else
            resultFor_vPlusOne = Kv0;
        if (result >= DBL_BIG)
        {
            // Try to avoid returning "inf" in either variable.
            result = resultFor_vPlusOne = DBL_BIG;
        }
        else if (resultFor_vPlusOne >= DBL_BIG)
            resultFor_vPlusOne = DBL_BIG;
        return result;
    }

    // Now, use K(v0,x) and K(v0+1,x) to calculate the desired K(v,x) == K(v0+n,x),
    // using the recurrence relationship:
    //
    // K(v+1,x) = K(v-1,x) + (2v/x)K(v,x).

    double A = 0.0, B = Kv0, C = Kv0plus1;
    for (unsigned long int i = 1; i <= n; i++)
    {
        A = B;
        B = C;
        C = A + (2.0*(v0 + i)/x)*B;
    }

    if (orderIsNegative)
        swap(B,C);
    if (B >= DBL_BIG)
        B = C = DBL_BIG;
    else if (C >= DBL_BIG)
        C = DBL_BIG;
    resultFor_vPlusOne = C;
    return B;
}

// Function to compute a numerical estimate of the analytically
// intractable partial derivative of K(v,x) with repect to v,
// where K(v,x) is the modified Bessel function of the second kind
// of order v evaluated at x, sometimes called the MacDonald function.
// [Note:  The partial derivative of K(v,x) with repect to x
// is simply (v/x)*K(v,x) - K(v+1,x).]
//
// Arguments:  v is the order, x is the argument,
// Kvx is the already-computed value of K(v,x).
//
// WARNING:  This simple algorithm may be insufficiently precise!
//
// Contains an original approximation derived by erynes for when
// v is close to an integer.
//
double DvBesselK(double v, double x, double Kvx)
{
    if (x <= 0.0)
        throw implementation_error("DvBesselK() received illegal argument:  "
                                   + ToString(x) + ".");
    if (0.0 == v)
        return 0.0; // this is independent of x, for nonzero x

    // K(-v,x) == K(v,x), so the sign of the value we return
    // must equal the sign of v.  Work with v > 0 for convenience.
    bool vIsNegative(false);
    if (v < 0.0)
    {
        vIsNegative = true;
        v = -v;
    }

    // For tiny x, K(v,x) = 0.5*gamma(v)*pow(0.5*x, -v).
    double gamma_v__DividedBy__DBL_BIG = my_gamma(v)/DBL_BIG;
    if (x <= 2.0*pow(0.5*gamma_v__DividedBy__DBL_BIG, 1.0/v))
        return (vIsNegative ? -DBL_BIG : DBL_BIG);
    else if (DBL_BIG == Kvx)
        return (vIsNegative ? -DBL_BIG : DBL_BIG);

    unsigned long int n = static_cast<unsigned long int>(floor(v));
    double v0 = v - 1.0*n;
    const double h = 1.0e-06; // used to compute the derivative from its definition
    double dummy; // dummy variable needed for storage; value not used

    if (v0 <= SMALL_v0 && v0+h >= SMALL_v0)
    {
        // need to nudge backwards
        v0 = (SMALL_v0 - NUDGE_AMOUNT) - h;
        v = n + v0;
        Kvx = BesselK(v, x, dummy);
    }
    else if (v0 < 1.0 - SMALL_v0 && v0+h >= 1.0 - SMALL_v0)
    {
        // need to nudge backwards
        v0 = (1.0 - SMALL_v0 - NUDGE_AMOUNT) - h;
        v = n + v0;
        Kvx = BesselK(v, x, dummy);
    }
    else if (v0 > 1.0 - SMALL_v0 && v0+h == 1.0)
    {
        // need to nudge backwards
        v0 = (1.0 - NUDGE_AMOUNT) - h;
        v = n + v0;
        Kvx = BesselK(v, x, dummy);
    }
    // Note that v0 > 1.0 - SMALL_v0 && v0+h > 1.0 is safe,
    // because h < SMALL_v0, hence v0+h < 1.0 + SMALL_v0.
    // Also is v0 == 1.0 - SMALL_v0 is safe, because in that case
    // K(v0,x) and K(v0+h,x) will both be calculated using the
    // derived approximation.

    double K_v_plus_h__x(0.0), result;

    K_v_plus_h__x = BesselK(v+h, x, dummy);
    result = (vIsNegative ? -(K_v_plus_h__x - Kvx)/h :
              (K_v_plus_h__x - Kvx)/h);
    if (result <= -DBL_BIG)
        result = -DBL_BIG;
    else if (result >= DBL_BIG)
        result = DBL_BIG;
    return result;

#if 0
    // This is old code by erynes that computes the derivative of the approximation
    // that's conditionally employed in BesselK().  For at least the moment (early
    // 2007), we're taking it out, and always computing the derivative using an
    // approximation to the definition of the derivative (i.e., slope computed
    // at points x and x+h where h is small).

    if (v0 > 1.0 - SMALL_v0)
    {
        n += 1;
        v0 = v - 1.0*n; // Thus, v0 < 0.0  Example:  2.0 + 0.95 --> 3.0 - 0.05.
    }

    double K1x(0.0);
    double K0x = BesselK(0.0, x, K1x);
    double A = 0.0;
    // erynes determined that, for small "e", e > 0 or e < 0,
    // K(e,x) = K(0,x)*(1 +  e^2/(2x + 1)),
    // K(1+e,x) = K(1,x) + (K(0,x)/x)*e + ((2x-1)(4x+1)K(1,x)/(2*(x+1)^2))*e^2,
    // where the "smallness" of e can vary with x, but is often near 0.01.
    // "e" in these equations corresponds to "v0" in the code.
    double B = (2.0 * K0x/(2.0 * x + 1.0)) * v0;
    if (0 == n)
        return (vIsNegative ? -B : B);
    double C = K0x/x + ((2.0*x-1.0)*(4.0*x+1.0)*K1x/((x+1.0)*(x+1.0)))*v0; // recall x > 0
    Kvx = K1x + (K0x/x)*v0 + ((2.0*x-1.0)*(4.0*x+1.0)*K1x/(2.0*(x+1.0)*(x+1.0)))*v0*v0;

    double Kv1x = K0x*(v0*v0/(2.0*x + 1.0) + 1.0) + (2.0*(1.0+v0)/x)*Kvx; // == K(2+v0,x)

    for (unsigned long int i = 1; i < n; i++)
    {
        A = B;
        B = C;
        if (0 == i % 2)
        {
            C = A + (2.0/x)*(Kv1x + (i + v0)*B);
            if (i != n - 1) // avoid unnecessary computation
                Kvx = BesselK(1.0+i+v0, x, Kv1x);
        }
        else
            C = A + (2.0/x)*(Kvx + (i + v0)*B);
    }

    return (vIsNegative ? -C : C);
#endif // 0
}

// Euler's psi function == d(log_gamma(x))/dx.
// Implemented by erynes in August 2005, adapted from:
// Shanjie Zhang and Jianming Jin, _Computation of Special Functions._
// New York: Wiley-Interscience (1996).
// (Their implementation is taken directly from Abramowitz and Stegun.)
//
double psi(double x)
{
    double result(0.0);
    double abs_x = x >= 0.0 ? x : -x;
    if (floor(x) == x)
    {
        // x is an integer.  Use the expansion for integers.
        if (x <= 0.0)
        {
            throw implementation_error("psi() received illegal argument:  "
                                       + ToString(x) + ".");
            return DBL_BIG; // psi(x) is undefined for 0 and negative integers
        }
        for (unsigned long int k = 1; k < static_cast<unsigned long int>(floor(x)); k++)
            result += 1.0/k;
        return -EULERS_CONSTANT + result;
    }

    if (floor(abs_x + 0.5) == abs_x + 0.5)
    {
        // x is a half-integer.  Use the expansion for half-integers.
        for (unsigned long int k = 1;
             k <= static_cast<unsigned long int>(floor(abs_x - 0.5)); k++)
            result += 2.0/(2.0*k - 1.0);
        result += -EULERS_CONSTANT - 2.0*LOG2;
    }
    else
    {
        // Not an integer or half-integer.
        if (abs_x < 10.0)
        {
            // Add an integer to abs_x to make abs_x + n > 10,
            // then use the asymptotic expansion for large x,
            // then use the recurrence relationship between psi(x+n) and psi(x).
            // First, compute sum[1/(x+k)], then later, subtract this
            // from psi(x+n) to obtain psi(x).
            unsigned long int n = static_cast<unsigned long int>(10.0 - floor(abs_x));
            for (unsigned long int k = 0; k < n; k++)
                result -= 1.0/(abs_x + k);
            abs_x += n;
        }
        // The asymptotic expansion.  These numbers are Bernoulli numbers;
        // the expansion actually comes from Abramowitz and Stegun's book.
        double x2 = 1.0/(abs_x*abs_x),
            a1 = -1.0/12.0,
            a2 = +1.0/120.0,
            a3 = -1.0/252.0,
            a4 = +1.0/240.0,
            a5 = -1.0/132.0,
            a6 = +691.0/32760.0,
            a7 = a1,
            a8 = +3617.0/8160.0;
        result += log(abs_x) - 0.5/abs_x +
            x2*(((((((a8*x2+a7)*x2+a6)*x2+a5)*x2+a4)*x2+a3)*x2+a2)*x2+a1);
    }

    if (x < 0.0)
        result += 1.0/abs_x + PI/tan(PI*abs_x); // psi(-x) = psi(x) + 1/x + PI*cot(PI*x)
    return result;
}

//____________________________________________________________________________________
