/*
    Theseus - maximum likelihood superpositioning of macromolecular structures

    Copyright (C) 2004-2015 Douglas L. Theobald

    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.,
    59 Temple Place, Suite 330,
    Boston, MA  02111-1307  USA

    -/_|:|_|_\-
*/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "specfunc.h"
#define ITMAX 1000
#define EPS 1e-8


/* Calculates the "probabilists" Hermite polynomial */
double
Hermite(const int n, const double x)
{
    double a, b, result = 0.0;
    int i;

    // Prepare A and B
    a = 1.0;
    b = x;

    // Special cases: N=0 or N=1
    if (n == 0)
        return(a);
    else if (n == 1)
        return(b);
    else
    {
        // General case: n >= 2
        for(i = 1; i < n; ++i)
        {
            result = (x * b) - (i * a);
            a = b;
            b = result;
        }

        return(result);
    }
}


static double
Factorial(long unsigned int N)
{
    int             i;
    double          F;

    if (N == 1 || N == 0)
        return(1.0);

    F = 1.0;
    for (i = 1; i <= N; ++i)
        F *= (double) i;

    return (F);
}


/* http://mathworld.wolfram.com/ModifiedBesselFunctionoftheFirstKind.html */
/* Computes the 0th order modified Bessel function of the first kind
   NOT the Bessel function of the first kind. */
double
BesselI(const double nu, const double z)
{
    int         k, iters = 200;
    double      term, sum;

    sum = 0.0;
    for (k = 0; k < iters; ++k)
    {
        term = pow(0.25*z*z, k) / (Factorial(k) * tgamma(nu + k + 1));
        sum += term;

        if (fabs(term) < FLT_EPSILON)
            break;
    }

    return(pow(0.5 * z, nu) * sum);
}


double
BesselI0(const double z)
{
    int         k, iters = 200;
    double      term, sum;

    sum = 0.0;
    for (k = 0; k < iters; ++k)
    {
        term = pow(0.25*z*z, k) / (Factorial(k) * tgamma(k + 1));
        sum += term;

        if (fabs(term) < FLT_EPSILON)
            break;
    }

    return(sum);
}


double
BesselI1(const double z)
{
    int         k, iters = 200;
    double      term, sum;

    sum = 0.0;
    for (k = 0; k < iters; ++k)
    {
        term = pow(0.25*z*z, k) / (Factorial(k) * tgamma(k + 2));
        sum += term;

        if (fabs(term) < FLT_EPSILON)
            break;
    }

    return(0.5 * z * sum);
}


/* Izero() computes the 0th order modified bessel function of the first kind. */
#define IzeroEPSILON 1E-21 /* Max error acceptable in Izero */

double
Izero(const double x)
{
    double           sum, u, halfx, temp;
    long             n;

    sum = u = n = 1;
    halfx = 0.5*x;
    do
    {
        temp = halfx/(double)n;
        n += 1;
        temp *= temp;
        u *= temp;
        sum += u;
    }
    while (u >= IzeroEPSILON*sum);

    return(sum);
}
#undef IzeroEPSILON



#define ACC 40.0
#define BIGNO 1.0e10
#define BIGNI 1.0e-10
/* Evaluate modified Bessel function In(x) for n >= 0*/
double
bessi(const int n, const double x)
{
    int             j;
    double          bi, bim, bip, tox, ans;

    if (n < 0)
        return(0.0);

    if (n == 0)
      return(bessi0(x));
    if (n == 1)
      return(bessi1(x));

    if (x == 0.0)
        return (0.0);
    else
    {
        tox = 2.0/fabs(x);
        bip = ans = 0.0;
        bi = 1.0;
        for (j = 2*(n + (int)sqrt(ACC*n)); j > 0; j--)
        {
            bim = bip + j*tox*bi;
            bip = bi;
            bi = bim;

            if (fabs(bi) > BIGNO)
            {
                ans *= BIGNI;
                bi *= BIGNI;
                bip *= BIGNI;
            }

            if (j == n)
                ans = bip;
        }

        ans *= Izero(x)/bi;
        return (x < 0.0 && n%2 == 1 ? -ans : ans);
    }
}
#undef ACC
#undef BIGNO
#undef BIGNI


/* Evaluate modified Bessel function In(x) and n=0.
   M.Abramowitz and I.A.Stegun (1964)
   _Handbook of Mathematical Functions_
   Applied Mathematics Series vol. 55, Washington.
*/
double
bessi0(const double x)
{
    double           ax = fabs(x);
    double           ans, y;

    if (ax < 3.75)
    {
        y = x/3.75;
        y = y*y;
        ans = 1.0 + y*(3.5156229 + y*(3.0899424 + y*(1.2067492
              + y*(0.2659732 + y*(0.360768e-1 + y*0.45813e-2)))));
    }
    else
    {
        y = 3.75 / ax;
        ans = (exp(ax)/sqrt(ax))*(0.39894228 + y*(0.1328592e-1
              + y*(0.225319e-2 + y*(-0.157565e-2 + y*(0.916281e-2
              + y*(-0.2057706e-1 + y*(0.2635537e-1 + y*(-0.1647633e-1
              + y*0.392377e-2))))))));
    }

    /* printf("\nBesselI: % e   % e  % e", ans, Izero(x), ans-Izero(x)); */

    return(ans);
}


/*---------------------------------------------------*/
/* Evaluate modified Bessel function In(x) and n=1.  */
/*---------------------------------------------------*/
double
bessi1(const double x)
{
    double           ax, ans, y;

    if ((ax=fabs(x)) < 3.75) {
      y=x/3.75,y=y*y;
      ans=ax*(0.5+y*(0.87890594+y*(0.51498869+y*(0.15084934
         +y*(0.2658733e-1+y*(0.301532e-2+y*0.32411e-3))))));
    } else {
      y=3.75/ax;
      ans=0.2282967e-1+y*(-0.2895312e-1+y*(0.1787654e-1
         -y*0.420059e-2));
      ans=0.39894228+y*(-0.3988024e-1+y*(-0.362018e-2
         +y*(0.163801e-2+y*(-0.1031555e-1+y*ans))));
      ans *= (exp(ax)/sqrt(ax));
    }

    return (x < 0.0 ? -ans : ans);
}


/* This is what MathWorld calls the \Gamma[a,z]
   http://mathworld.wolfram.com/IncompleteGammaFunction.html
   upper_incomplete_gamma(a, x) =  gammq(a, x)*gamma(a)
*/
double
UpperIncompleteGamma(const double a, const double x)
{
    return(gammq(a, x) * tgamma(a));
}


/* This what MathWorld calls the Regularized Gamma Function Q[a,z]
   http://mathworld.wolfram.com/RegularizedGammaFunction.html
   upper_incomplete_Gamma(a, x) / Gamma(a) = gammq(a, x)
*/
double
gammq(const double a, const double x)
{
/*     double          amax = 1e8; */
    if (x < 0.0 || a < 0.0)
    {
        fprintf(stderr, "\n ERROR13: Invalid arguments in gammq(): a:%e x:%e. \n", a, x);
        /* exit(EXIT_FAILURE); */
        return(0.0);
    }

    /* DLT -- my own approximation for small a & x */
/*     if (a < DBL_EPSILON) */
/*         return(0.0); */
/*     else if (x < DBL_EPSILON) */
/*         return(1.0); */

    /* DLT -- my own approximation for large a & x */
/*     if (a > amax) */
/*     { */
/*         if (x < a) */
/*             return(1.0); */
/*         else if (x > a) */
/*             return(0.0); */
/*         else if (x == a) */
/*             return 0.5; */
/*     } */

    if (x < (a + 1.0))
        return(1.0 - gser(a, x));
    else
        return(gcf(a, x));
}


/* This what MathWorld calls the Regularized Gamma Function P[a,z]
   http://mathworld.wolfram.com/RegularizedGammaFunction.html
   lower_incomplete_gamma(a, x) / Gamma(a) = gammp(a, x) */
double
gammp(const double a, const double x)
{
    double          amax = 1e7;
    if (x < 0.0 || a < 0.0)
    {
        fprintf(stderr, "\n ERROR12: Invalid arguments in gammp(). \n");
        /* exit(EXIT_FAILURE); */
        return(0.0);
    }

    /* DLT -- my own approximation for small a & x */
    if (a < DBL_EPSILON)
        return(1.0);
    else if (x < DBL_EPSILON)
        return(0.0);

    /* DLT -- my own approximation for large a & x */
    if (a > amax)
    {
        if (x < a)
            return(0.0);
        else if (x > a)
            return(1.0);
        else if (x == a)
            return 0.5;
    }

    if (x < (a + 1.0))
        return(gser(a, x));
    else
        return(1.0 - gcf(a, x));
}


double
gcf(double a, double x)
{
    extern double   lgamma(double xx);
    int             i;
    double          an, b, c, d, del, h;

    b = x + 1.0 - a;
    c = 1.0 / DBL_EPSILON;
    d = 1.0 / b;
    h = d;

    for (i = 1; i <= ITMAX; i++)
    {
        an = -i * (i - a);
        b += 2.0;
        d = an * d + b;

        if (fabs(d) < DBL_EPSILON)
            d = DBL_EPSILON;

        c = b + an / c;

        if (fabs(c) < DBL_EPSILON)
            c = DBL_EPSILON;

        d = 1.0 / d;
        del = d * c;
        h *= del;

        if (fabs(del - 1.0) < EPS)
            break;
    }

    if (i > ITMAX)
    {
        fprintf(stderr, "\n ERROR14: \'a\' param too large, ITMAX too small in gcf(). \n\n");
    }

    return(exp(-x + a * log(x) - lgamma(a)) * h);
}


double
gser(double a, double x)
{
    extern double   lgamma(double xx);
    int             n;
    double          sum, del, ap;

    if (x < 0.0)
    {
        fprintf(stderr, "\n ERROR15: x < 0.0 in routine gser(). \n\n");
        exit(EXIT_FAILURE);
    }
    else if (x == 0.0)
    {
        return(0.0);
    }
    else
    {
        ap = a;
        del = sum = 1.0 / a;

        for (n = 1; n <= ITMAX; ++n)
        {
            ++ap;
            del *= x / ap;
            sum += del;

            if (fabs(del) < fabs(sum) * EPS)
                return(sum * exp(-x + a * log(x) - lgamma(a)));
        }
    }

    fprintf(stderr, "\n ERROR16: \'a\' param too large, ITMAX too small in gser(). \n\n");
    return(0.0);
}
#undef ITMAX
#undef EPS


#define MAXIT 200
#define EPS 1e12

double
betai(double a, double b, double x)
{
    double          bt;

    if (x < 0.0 || x > 1.0)
    {
        fprintf(stderr, "\n\n ERROR: 0.0 < \'x\' = %12.3e > 1.0 in routine betai \n", x);
        exit(EXIT_FAILURE);
    }

    if (x == 0.0 || x == 1.0)
        bt = 0.0;
    else
        bt = exp(lgamma(a + b) - lgamma(a) - lgamma(b) + (a * log(x)) + (b * log(1.0 - x)));

    if (x < (a + 1.0) / (a + b + 2.0))
        return(bt * betacf(a, b, x) / a);
    else
        return(1.0 - bt * betacf(b, a, 1.0 - x) / b);
}


double
betacf(double a, double b, double x)
{
    int             m, m2;
    double          aa, c, d, del, h, qab, qam, qap;

    qab = a + b;
    qap = a + 1.0;
    qam = a - 1.0;
    c = 1.0;
    d = 1.0 - qab * x / qap;

    if (fabs(d) < DBL_EPSILON)
        d = DBL_EPSILON;

    d = 1.0 / d;
    h = d;

    for (m = 1; m <= MAXIT; m++)
    {
        m2 = 2 * m;
        aa = m * (b - m) * x / ((qam + m2) * (a + m2));
        d = 1.0 + aa * d;

        if (fabs(d) < DBL_EPSILON)
            d = DBL_EPSILON;

        c = 1.0 + aa / c;

        if (fabs(c) < DBL_EPSILON)
            c = DBL_EPSILON;

        d = 1.0 / d;
        h *= d * c;
        aa = -(a + m) * (qab + m) * x / ((a + m2) * (qap + m2));
        d = 1.0 + aa * d;

        if (fabs(d) < DBL_EPSILON)
            d = DBL_EPSILON;

        c = 1.0 + aa / c;

        if (fabs(c) < DBL_EPSILON)
            c = DBL_EPSILON;

        d = 1.0 / d;
        del = d * c;
        h *= del;

        if (fabs(del - 1.0) < EPS)
            break;
    }

    if (m > MAXIT)
    {
        fprintf(stderr, "\n\n ERROR: a or b too big, or MAXIT too small in betacf \n");
        exit(EXIT_FAILURE);
    }

    return (h);
}


double
beta(double z, double w)
{
    return(exp(lgamma(z) + lgamma(w) - lgamma(z + w)));
}

#undef MAXIT
#undef EPS

int
findmin(const double *vec, const int len)
{
    int             i, mini = 0;
    double          min = DBL_MAX;

    for (i = 0; i < len; ++i)
    {
        if (min > vec[i])
        {
            min = vec[i];
            mini = i;
        }
    }

    return(mini);
}


int
findmax(const double *vec, const int len)
{
    int             i, maxi = 0;
    double          max = -DBL_MAX;

    for (i = 0; i < len; ++i)
    {
        if (max < vec[i])
        {
            max = vec[i];
            maxi = i;
        }
    }

    return(maxi);
}


double
mymaxdbl(const double x, const double y)
{
    if (x > y)
        return(x);
    else
        return(y);
}


double
mymindbl(const double x, const double y)
{
    if (x < y)
        return(x);
    else
        return(y);
}


int
mymaxint(const int x, const int y)
{
    if (x > y)
        return(x);
    else
        return(y);
}


int
myminint(const int x, const int y)
{
    if (x < y)
        return(x);
    else
        return(y);
}


int
myround(const double num)
{
    if (num - (int) num >= 0.5)
        return(ceil(num));
    else
        return(floor(num));
}


double
mysquare(const double val)
{
    return(val * val);
}


double
mycube(const double val)
{
    return(val * val * val);
}


double
mypow4(double val)
{
    val *= val;

    return(val * val);
}


