#include "misc.h"
#include <math.h>
#include <stdlib.h> /* for size_t */
#include <gsl/gsl_sf_gamma.h>
#include "vector.h"

static struct {
    int n;
    double f;
} ln_table[LN_MAX + 1] = {
    {0,-INFINITY},
    {1,0},
    {2,0.69314718055994529},
    {3,1.0986122886681098},
    {4,1.3862943611198906},
    {5,1.6094379124341003},
    {6,1.791759469228055},
    {7,1.9459101490553132},
    {8,2.0794415416798357},
    {9,2.1972245773362196},
    {10,2.3025850929940459},
    {11,2.3978952727983707},
    {12,2.4849066497880004},
    {13,2.5649493574615367},
    {14,2.6390573296152584},
    {15,2.7080502011022101},
    {16,2.7725887222397811},
    {17,2.8332133440562162},
    {18,2.8903717578961645},
    {19,2.9444389791664403},
    {20,2.9957322735539909},
    {21,3.044522437723423},
    {22,3.0910424533583161},
    {23,3.1354942159291497},
    {24,3.1780538303479458},
    {25,3.2188758248682006},
    {26,3.2580965380214821},
    {27,3.2958368660043291},
    {28,3.3322045101752038},
    {29,3.3672958299864741},
    {30,3.4011973816621555},
    {31,3.4339872044851463},
    {32,3.4657359027997265},
    {33,3.4965075614664802},
    {34,3.5263605246161616},
    {35,3.5553480614894135},
    {36,3.5835189384561099},
    {37,3.6109179126442243},
    {38,3.6375861597263857},
    {39,3.6635616461296463},
    {40,3.6888794541139363},
    {41,3.713572066704308},
    {42,3.7376696182833684},
    {43,3.7612001156935624},
    {44,3.784189633918261},
    {45,3.8066624897703196},
    {46,3.8286413964890951},
    {47,3.8501476017100584},
    {48,3.8712010109078911},
    {49,3.8918202981106265},
    {50,3.912023005428146},
    {51,3.9318256327243257},
    {52,3.9512437185814275},
    {53,3.970291913552122},
    {54,3.9889840465642745},
    {55,4.0073331852324712},
    {56,4.0253516907351496},
    {57,4.0430512678345503},
    {58,4.0604430105464191},
    {59,4.0775374439057197},
    {60,4.0943445622221004},
    {61,4.1108738641733114},
    {62,4.1271343850450917},
    {63,4.1431347263915326},
    {64,4.1588830833596715},
    {65,4.1743872698956368},
    {66,4.1896547420264252},
    {67,4.2046926193909657},
    {68,4.219507705176107},
    {69,4.2341065045972597},
    {70,4.2484952420493594},
    {71,4.2626798770413155},
    {72,4.2766661190160553},
    {73,4.290459441148391},
    {74,4.3040650932041702},
    {75,4.3174881135363101},
    {76,4.3307333402863311},
    {77,4.3438054218536841},
    {78,4.3567088266895917},
    {79,4.3694478524670215},
    {80,4.3820266346738812},
    {81,4.3944491546724391},
    {82,4.4067192472642533},
    {83,4.4188406077965983},
    {84,4.4308167988433134},
    {85,4.4426512564903167},
    {86,4.4543472962535073},
    {87,4.4659081186545837},
    {88,4.4773368144782069},
    {89,4.4886363697321396},
    {90,4.499809670330265},
    {91,4.5108595065168497},
    {92,4.5217885770490405},
    {93,4.5325994931532563},
    {94,4.5432947822700038},
    {95,4.5538768916005408},
    {96,4.5643481914678361},
    {97,4.5747109785033828},
    {98,4.5849674786705723},
    {99,4.5951198501345898},
    {100,4.6051701859880918},
};

static struct {
    int n;
    double f;
} ln_fact_table[LNFACT_MAX + 1] = {
    {0,0},
    {1,0},
    {2,0.69314718055994529},
    {3,1.791759469228055},
    {4,3.1780538303479458},
    {5,4.7874917427820458},
    {6,6.5792512120101012},
    {7,8.5251613610654147},
    {8,10.604602902745251},
    {9,12.801827480081469},
    {10,15.104412573075516},
    {11,17.502307845873887},
    {12,19.987214495661885},
    {13,22.552163853123425},
    {14,25.19122118273868},
    {15,27.89927138384089},
    {16,30.671860106080672},
    {17,33.505073450136891},
    {18,36.395445208033053},
    {19,39.339884187199495},
    {20,42.335616460753485},
    {21,45.380138898476908},
    {22,48.471181351835227},
    {23,51.606675567764377},
    {24,54.784729398112319},
    {25,58.003605222980518},
    {26,61.261701761002001},
    {27,64.557538627006338},
    {28,67.88974313718154},
    {29,71.257038967168015},
    {30,74.658236348830158},
    {31,78.092223553315307},
    {32,81.557959456115043},
    {33,85.054467017581516},
    {34,88.580827542197682},
    {35,92.136175603687093},
    {36,95.719694542143202},
    {37,99.330612454787428},
    {38,102.96819861451381},
    {39,106.63176026064346},
    {40,110.32063971475739},
    {41,114.03421178146171},
    {42,117.77188139974507},
    {43,121.53308151543864},
    {44,125.3172711493569},
    {45,129.12393363912722},
    {46,132.95257503561632},
    {47,136.80272263732635},
    {48,140.67392364823425},
    {49,144.5657439463449},
    {50,148.47776695177302},
    {51,152.40959258449735},
    {52,156.3608363030788},
    {53,160.3311282166309},
    {54,164.32011226319517},
    {55,168.32744544842765},
    {56,172.35279713916279},
    {57,176.39584840699735},
    {58,180.45629141754378},
    {59,184.53382886144948},
    {60,188.6281734236716},
    {61,192.7390472878449},
    {62,196.86618167289001},
    {63,201.00931639928152},
    {64,205.1681994826412},
    {65,209.34258675253685},
    {66,213.53224149456327},
    {67,217.73693411395422},
    {68,221.95644181913033},
    {69,226.1905483237276},
    {70,230.43904356577696},
    {71,234.70172344281826},
    {72,238.97838956183432},
    {73,243.26884900298271},
    {74,247.57291409618688},
    {75,251.89040220972319},
    {76,256.22113555000954},
    {77,260.56494097186322},
    {78,264.92164979855278},
    {79,269.29109765101981},
    {80,273.67312428569369},
    {81,278.06757344036612},
    {82,282.4742926876304},
    {83,286.89313329542699},
    {84,291.32395009427029},
    {85,295.76660135076065},
    {86,300.22094864701415},
    {87,304.68685676566872},
    {88,309.1641935801469},
    {89,313.65282994987905},
    {90,318.1526396202093},
    {91,322.66349912672615},
    {92,327.1852877037752},
    {93,331.71788719692847},
    {94,336.26118197919845},
    {95,340.81505887079902},
    {96,345.37940706226686},
    {97,349.95411804077025},
    {98,354.53908551944079},
    {99,359.1342053695754},
    {100,363.73937555556347},
};

void get_path_length(double *pos1, double *pos2, double R, double *l1, double *l2)
{
    /* Returns the path length inside and outside a circle of radius `R` for a
     * ray starting at position `pos1` and ending at position `pos2`.
     *
     * The path length inside the sphere is stored in `l1` and the path length
     * outside the sphere is stored in `l2`. */
    double dir[3], l, b, c, d1, d2;

    /* Calculate the vector from `pos1` to `pos2`. */
    SUB(dir,pos2,pos1);

    l = NORM(dir);

    normalize(dir);

    b = 2*DOT(dir,pos1);
    c = DOT(pos1,pos1) - R*R;

    if (b*b - 4*c <= 0) {
        /* Ray doesn't intersect the sphere. */
        *l1 = 0.0;
        *l2 = l;
        return;
    }

    d1 = (-b + sqrt(b*b - 4*c))/2;
    d2 = (-b - sqrt(b*b - 4*c))/2;

    if (d1 < 0) {
        /* Ray also doesn't intersect sphere. */
        *l1 = 0.0;
        *l2 = l;
    } else if (d1 >= l && d2 < 0) {
        /* Ray also doesn't intersect sphere. */
        *l1 = l;
        *l2 = 0.0;
    } else if (d2 < 0) {
        /* Ray intersects sphere once. */
        *l1 = d1;
        *l2 = l-d1;
    } else if (d1 >= l && d2 >= l) {
        /* Ray doesn't intersect the sphere. */
        *l1 = 0.0;
        *l2 = l;
    } else if (d1 >= l && d2 < l) {
        /* Ray intersects the sphere once. */
        *l2 = d1;
        *l1 = l-d1;
    } else if (d1 < l && d2 < l) {
        /* Ray intersects the sphere twice. */
        *l1 = d1-d2;
        *l2 = l-(d1-d2);
    }
}

double ln(unsigned int n)
{
    /* Returns the logarithm of n.
     *
     * Uses a lookup table to return results for n < 100. */

    if (n <= LN_MAX)
        return ln_table[n].f;

    return log(n);
}

double lnfact(unsigned int n)
{
    /* Returns the logarithm of n!.
     *
     * Uses a lookup table to return results for n < 100. */

    if (n <= LNFACT_MAX)
        return ln_fact_table[n].f;

    return gsl_sf_lnfact(n);
}

double kahan_sum(double *x, size_t n)
{
    /* Returns the sum of the elements of `x` using the Kahan summation algorithm.
     *
     * See https://en.wikipedia.org/wiki/Kahan_summation_algorithm. */
    size_t i;
    double sum, c, y, t;

    sum = 0.0;
    c = 0.0;
    for (i = 0; i < n; i++) {
        y = x[i] - c;
        t = sum + y;
        c = (t - sum) - y;
        sum = t;
    }

    return sum;
}

double interp1d(double x, double *xp, double *yp, size_t n)
{
    /* A fast interpolation routine which assumes that the values in `xp` are
     * evenly spaced.
     *
     * If x < xp[0] returns yp[0] and if x > xp[n-1] returns yp[n-1].  */
    size_t i;

    if (x <= xp[0]) return yp[0];

    i = (x-xp[0])/(xp[1]-xp[0]);

    if (i > n-2) return yp[n-1];

    return yp[i] + (yp[i+1]-yp[i])*(x-xp[i])/(xp[i+1]-xp[i]);
}

int isclose(double a, double b, double rel_tol, double abs_tol)
{
    /* Returns 1 if a and b are "close". This algorithm is taken from Python's
     * math.isclose() function.
     *
     * See https://www.python.org/dev/peps/pep-0485/. */
    return fabs(a-b) <= fmax(rel_tol*fmax(fabs(a),fabs(b)),abs_tol);
}

int allclose(double *a, double *b, size_t n, double rel_tol, double abs_tol)
{
    /* Returns 1 if all the elements of a and b are "close". This algorithm is
     * taken from Python's math.isclose() function.
     *
     * See https://www.python.org/dev/peps/pep-0485/. */
    size_t i;

    for (i = 0; i < n; i++) {
        if (!isclose(a[i],b[i],rel_tol,abs_tol)) return 0;
    }

    return 1;
}

double logsumexp(double *a, size_t n)
{
    /* Returns the log of the sum of the exponentials of the array `a`.
     *
     * This function is designed to reduce underflow when the exponentials of
     * `a` are very small, for example when computing probabilities. */
    size_t i;
    double amax, sum;

    amax = a[0];
    for (i = 0; i < n; i++) {
        if (a[i] > amax) amax = a[i];
    }

    sum = 0.0;

    for (i = 0; i < n; i++) {
        sum += exp(a[i]-amax);
    }

    sum = log(sum);

    return amax + sum;
}

double norm(double x, double mu, double sigma)
{
    /* Returns the PDF for a gaussian random variable with mean `mu` and
     * standard deviation `sigma`. */
    return exp(-pow(x-mu,2)/(2*pow(sigma,2)))/(sqrt(2*M_PI)*sigma);
}

double norm_cdf(double x, double mu, double sigma)
{
    /* Returns the CDF for a gaussian random variable with mean `mu` and
     * standard deviation `sigma`. */
    return erfc(-(x-mu)/(sqrt(2)*sigma))/2.0;
}