#include <u.h>
#include <base.h>

// ----------------------------------------------------------------------------
// Internal structure

uint64 
rol64(uint64 x, int k)
{
	return (x << k) | (x >> (64 - k));
}

typedef struct Rng {
	uint64 s[4];
} Rng;

uint64
xoshiro256ss(Rng *state)
{
	uint64 *s     = state->s;
	uint64 result = rol64(s[1] * 5, 7) * 9;
	uint64 t      = s[1] << 17;

	s[2] ^= s[0];
	s[3] ^= s[1];
	s[1] ^= s[2];
	s[0] ^= s[3];

	s[2] ^= t;
	s[3] = rol64(s[3], 45);

	return result;
}

typedef struct Mix
{
	uint64 s;
} Mix;

uint64 
splitmix64(struct Mix *state) {
	uint64 result = state->s;

	state->s = result + 0x9E3779B97f4A7C15;
	result = (result ^ (result >> 30)) * 0xBF58476D1CE4E5B9;
	result = (result ^ (result >> 27)) * 0x94D049BB133111EB;
	return result ^ (result >> 31);
}

static Rng RNG;

// ----------------------------------------------------------------------------
// Exported functions

/* Initializes the global RNG */

error
rng·init(uint64 seed)
{
	Mix smstate = {seed};

    for (int i=0; i < 4; i++) 
        RNG.s[i] = splitmix64(&smstate);

    return 0;
}

/* Returns a random float64 between 0 and 1 */
double
rng·random(void)
{
    uint64 r = xoshiro256ss(&RNG); 
    return (double)r / (double)UINT64_MAX;
}

double
rng·exponential(double lambda)
{
    double f;

    f = rng·random();
    return -log(1 - f)/lambda;
}

static inline
double
erfinv(double x)
{
    /* useful constants */
    static double
      a0 = 1.1975323115670912564578e0, a1 = 4.7072688112383978012285e1,
      a2 = 6.9706266534389598238465e2, a3 = 4.8548868893843886794648e3,
      a4 = 1.6235862515167575384252e4, a5 = 2.3782041382114385731252e4,
      a6 = 1.1819493347062294404278e4, a7 = 8.8709406962545514830200e2,

      b0 = 1.0000000000000000000e0, b1 = 4.2313330701600911252e1,
      b2 = 6.8718700749205790830e2, b3 = 5.3941960214247511077e3,
      b4 = 2.1213794301586595867e4, b5 = 3.9307895800092710610e4,
      b6 = 2.8729085735721942674e4, b7 = 5.2264952788528545610e3,

      c0 = 1.42343711074968357734e0,  c1 = 4.63033784615654529590e0,
      c2 = 5.76949722146069140550e0,  c3 = 3.64784832476320460504e0,
      c4 = 1.27045825245236838258e0,  c5 = 2.41780725177450611770e-1,
      c6 = 2.27238449892691845833e-2, c7 = 7.74545014278341407640e-4,

      d0 = 1.4142135623730950488016887e0,  d1 = 2.9036514445419946173133295e0,
      d2 = 2.3707661626024532365971225e0,  d3 = 9.7547832001787427186894837e-1,
      d4 = 2.0945065210512749128288442e-1, d5 = 2.1494160384252876777097297e-2,
      d6 = 7.7441459065157709165577218e-4, d7 = 1.4859850019840355905497876e-9,

      e0 = 6.65790464350110377720e0,  e1 = 5.46378491116411436990e0,
      e2 = 1.78482653991729133580e0,  e3 = 2.96560571828504891230e-1,
      e4 = 2.65321895265761230930e-2, e5 = 1.24266094738807843860e-3,
      e6 = 2.71155556874348757815e-5, e7 = 2.01033439929228813265e-7,

      f0 = 1.414213562373095048801689e0,  f1 = 8.482908416595164588112026e-1,
      f2 = 1.936480946950659106176712e-1, f3 = 2.103693768272068968719679e-2,
      f4 = 1.112800997078859844711555e-3, f5 = 2.611088405080593625138020e-5,
      f6 = 2.010321207683943062279931e-7, f7 = 2.891024605872965461538222e-15,

      Ln2 = 0.693147180559945309417232121458176568075500134360255254120680009;

    int s;
    double r, z1, z2;

    if(x < 0) {
        s = -1;
        x = -x;
    } else {
        s = +1;
    }

    if(x <= 0.85) {
        r = 0.180625 - 0.25*x*x;
        z1 = ((((((a7*r+a6)*r+a5)*r+a4)*r+a3)*r+a2)*r+a1)*r + a0;
        z2 = ((((((b7*r+b6)*r+b5)*r+b4)*r+b3)*r+b2)*r+b1)*r + b0;
        return s*(x*z1) / z2;
    }
    r = sqrt(Ln2 - log(1.0-x));
    if(r <= 5.0) {
        r -= 1.6;
        z1 = ((((((c7*r+c6)*r+c5)*r+c4)*r+c3)*r+c2)*r+c1)*r + c0;
        z2 = ((((((d7*r+d6)*r+d5)*r+d4)*r+d3)*r+d2)*r+d1)*r + d0;
    } else {
        r -= 5.0;
        z1 = ((((((e7*r+e6)*r+e5)*r+e4)*r+e3)*r+e2)*r+e1)*r + e0;
        z2 = ((((((f7*r+f6)*r+f5)*r+f4)*r+f3)*r+f2)*r+f1)*r + f0;
    }

    return s*z1/z2;
}


double
rng·normal(void)
{
    double f;
    f = rng·random();

    return sqrt(2)*erfinv(2*f-1);
}

/* Returns true or false on success of trial */
bool
rng·bernoulli(double f)
{
    return rng·random() < f;
}

/* Returns a random integer between 0 and max
 * TODO: Modulo throws off uniformity 
 */
uint64
rng·randi(int max)
{
    uint64 r = xoshiro256ss(&RNG); 
    return r % max;
}

/*
 * Ahrens, J. H., & Dieter, U. (1982).
 * Computer Generation of Poisson Deviates from Modified Normal Distributions.
 */
static double factorial[10] = {1., 1., 2., 6., 24., 120., 720., 5040., 40320., 362880.};
static double coeffs[9] = {
    -.500000000, +.333333333, -.249999856,
    +.200011780, -.166684875, +.142187833,
    -.124196313, +.125005956, -.114265030,
};

static inline
double
log1pmx(double x, double off)
{
    int i;
    double r, t;

    if(-0.25 < x && x < 0.25) {
        r = 0;
        t = 1;
        for(i=0;i<arrlen(coeffs);i++) {
            r += coeffs[i]*t;
            t *= x;
        }

        return x*x*r;
    }
    return log(1+x) - off;
}

static inline
double
procf(double mu, double s, int64 K, double *px, double *py, double *fx, double *fy)
{
    double d, V, X;
    double w, b1, b2, c1, c2, c3, c0, c;

    w  = 0.3989422804014327/s;
    b1 = 0.041666666666666664/mu;
    b2 = 0.3*b1*b1;
    c3 = 0.14285714285714285*b1*b2;
    c2 = b2 - 15.*c3;
    c1 = b1 - 6.*b2 + 45.*c3;
    c0 = 1 - b1 + 3.*b2 - 15.*c3;
    c  = .1069/mu;

    if(K < 10) {
        *px = -mu;
        *py = pow(mu,K) / factorial[K];
    }else{
        d = 0.08333333333333333/K;
        d = d - 4.8*d*d*d;
        V = (mu - K) / K;

        *px = K*log1pmx(V,mu-K) - d;
        *py = 0.3989422804014327/sqrt(K);
    }

    X = (K - mu + 0.5)/s;
    *fx = -0.5*X*X;
    *fy = w*(((c3*X*X + c2)*X*X + c1)*X*X + c0);

    return c;
}

static inline
uint64
bigpoisson(double mu)
{
    int64  L,K;
    double G,s,d,U,E,T;
    double px,py,fx,fy,c;

    s = sqrt(mu);
    d = 6*mu*mu;
    L = floor(mu - 1.1484);

stepN:
    G = mu + s*rng·normal();
    K = floor(G);
    if(K<0)
        goto stepP;
stepI:
    if(K>=L)
        return K;
stepS:
    U = rng·random();
    if(d*U >= (mu-K)*(mu-K)*(mu-K))
        return K;
stepP:
    if(G < 0)
        goto stepE;
stepQ:
    c = procf(mu, s, K, &px, &py, &fx, &fy);
stepE:
    E = rng·exponential(1.0);
    U = rng·random();
    U = U + U - 1;
    T = 1.8 + copysign(E,U);
    if(T < 0.6744)
        goto stepE;
    K = floor(mu + s*T);
    c = procf(mu, s, K, &px, &py, &fx, &fy);
stepH:
    if(c*fabs(U) > (py*exp(px + E) - fy*exp(fx + E)))
        goto stepE;
    return K;
}

uint64
rng·poisson(double mean)
{
    int64  n;
    double z;

    if(mean<10.0) {
        for(n=0, z=rng·exponential(1.0); z<mean; ++n, z+=rng·exponential(1.0))
            ;
        return n;
    }

    return bigpoisson(mean);
}