#include <Accelerate/Accelerate.h>

#pragma mark Define directives

#define BATCH_SIZE 1024
#define HALF_BATCH  512
#define VEC_LEN (1<<24)
#define TRIALS 6

#define SCALE_D 0x1p-53
#define RAND_53 ((random() << 22) | (random() >> 9))
#define RAND_D ((double)RAND_53)
#define EXP_D (-log(SCALE_D * (double)(RAND_53 + 1L)))

#define SCALE_F 0x1p-24
#define RAND_24 (random() >> 7)
#define RAND_F ((float)RAND_24)
#define EXP_F (-logf(SCALE_F * (float)(RAND_24 + 1L)))

#pragma mark Function declarations

double randNormD(double * const x);
void loopNormD(double * const v, const int n);
void vecNormD(double * const v, const int n);
void vecNormD2(double * const v, const int n);
void vecNormHelpD(double * const v);
void vecNormHelpD2(double * const v);

float randNormF(float * const x);
void loopNormF(float * const v, const int n);
void vecNormF(float * const v, const int n);
void vecNormF2(float * const v, const int n);
void vecNormHelpF(float * const v);
void vecNormHelpF2(float * const v);

#pragma mark Main routine

int main(int argc, const char * argv[])
{
    double *v = malloc(VEC_LEN * sizeof(double));
    float *w = malloc(VEC_LEN * sizeof(float));
    time_t t[TRIALS+1];
    double s[TRIALS];
    
    srandomdev();
    vecNormD(v, VEC_LEN);
    vecNormF(w, VEC_LEN);
    
    t[0] = clock();
    loopNormF(w, VEC_LEN);
    t[1] = clock();
    vecNormF(w, VEC_LEN);
    t[2] = clock();
    vecNormF2(w, VEC_LEN);
    t[3] = clock();
    loopNormD(v, VEC_LEN);
    t[4] = clock();
    vecNormD(v, VEC_LEN);
    t[5] = clock();
    vecNormD2(v, VEC_LEN);
    t[6] = clock();
    
    for (int i = 0; i < TRIALS; i++) {
        s[i] = (double)(t[i+1] - t[i]) / CLOCKS_PER_SEC;
    }
    printf("Float\nLooped: % .6f\nVector: % .6f\nHybrid: % .6f\n", s[0], s[1], s[2]);
    printf("\nDouble\nLooped: % .6f\nVector: % .6f\nHybrid: % .6f\n", s[3], s[4], s[5]);
    
    return 0;
}

#pragma mark Double

double randNormD(double * const x) {
    // Box-Muller transform
    const double r = sqrt(2.0 * EXP_D);
    const double t = (2.0 * (double)M_PI * SCALE_D) * RAND_D;
    *x = r * cos(t);
    return r * sin(t);
}

void loopNormD(double * const v, const int n) {
    for (int i = 0; i < n-1; i += 2) v[i] = randNormD(&v[i+1]);
    if ((n % 2) && (n > 0)) v[n-1] = randNormD(&v[n-1]);
}

void vecNormD(double * v, const int n) {
    for (int i = n/BATCH_SIZE; i-->0; v += BATCH_SIZE) vecNormHelpD(v);
    loopNormD(v, n%BATCH_SIZE);
}

void vecNormD2(double * v, const int n) {
    for (int i = n/BATCH_SIZE; i-->0; v += BATCH_SIZE) vecNormHelpD2(v);
    loopNormD(v, n%BATCH_SIZE);
}

void vecNormHelpD(double * const v) {
    const int h = HALF_BATCH;
    const double s = SCALE_D;
    const double p = 2.0 * M_PI * SCALE_D;
    const double negtwo = -2.0;
    double temp[h];
    
    for (int i = 0; i < h; i++) temp[i] = RAND_D;
    vDSP_vsmsaD(temp, 1, &s, &s, temp, 1, h);
    vvlog(temp, temp, &h);
    vDSP_vsmulD(temp, 1, &negtwo, temp, 1, h);
    vvsqrt(temp, temp, &h);
    
    for (int i = 0; i < h; i++) v[i] = RAND_D;
    vDSP_vsmulD(v, 1, &p, v, 1, h);
    vvsincos(v, v, v+h, &h);
    vDSP_vmulD(v, 1, temp, 1, v, 1, h);
    vDSP_vmulD(v+h, 1, temp, 1, v+h, 1, h);
}

void vecNormHelpD2(double * const v) {
    const int h = HALF_BATCH;
    const double s = SCALE_D;
    const double p = 2.0 * (double)M_PI * SCALE_D;
    double * const w = v + h;
    double temp[h];
    
    for (int i = 0; i < h; i++) temp[i] = RAND_D;
    vDSP_vsmsaD(temp, 1, &s, &s, temp, 1, h);
    vvlog(temp, temp, &h);
    for (int i = 0; i < h; i++) temp[i] *= -2.0;
    vvsqrt(temp, temp, &h);
    
    for (int i = 0; i < h; i++) v[i] = p * RAND_D;
    vvsincos(v, v, v+h, &h);
    for (int i = 0; i < h; i++) {
        v[i] *= temp[i];
        w[i] *= temp[i];
    }
}

#pragma mark Float

float randNormF(float * const x) {
    // Box-Muller transform
    const float r = sqrtf(2.0f * EXP_F);
    const float t = (2.0f * (float)M_PI * SCALE_F) * RAND_F;
    *x = r * cosf(t);
    return r * sinf(t);
}

void loopNormF(float * const v, const int n) {
    for (int i = 0; i < n-1; i += 2) v[i] = randNormF(&v[i+1]);
    if ((n % 2) && (n > 0)) v[n-1] = randNormF(&v[n-1]);
}

void vecNormF(float * v, const int n) {
    for (int i = n/BATCH_SIZE; i-->0; v += BATCH_SIZE) vecNormHelpF(v);
    loopNormF(v, n%BATCH_SIZE);
}

void vecNormF2(float * v, const int n) {
    for (int i = n/BATCH_SIZE; i-->0; v += BATCH_SIZE) vecNormHelpF2(v);
    loopNormF(v, n%BATCH_SIZE);
}

void vecNormHelpF(float * const v) {
    const int h = HALF_BATCH;
    const float s = SCALE_F;
    const float p = 2.0f * (float)M_PI * SCALE_F;
    const float negtwo = -2.0f;
    float temp[h];
    
    for (int i = 0; i < h; i++) temp[i] = RAND_F;
    vDSP_vsmsa(temp, 1, &s, &s, temp, 1, h);
    vvlogf(temp, temp, &h);
    vDSP_vsmul(temp, 1, &negtwo, temp, 1, h);
    vvsqrtf(temp, temp, &h);
    
    for (int i = 0; i < h; i++) v[i] = RAND_F;
    vDSP_vsmul(v, 1, &p, v, 1, h);
    vvsincosf(v, v, v+h, &h);
    vDSP_vmul(v, 1, temp, 1, v, 1, h);
    vDSP_vmul(v+h, 1, temp, 1, v+h, 1, h);
}

void vecNormHelpF2(float * const v) {
    const int h = HALF_BATCH;
    const float s = SCALE_F;
    const float p = 2.0f * (float)M_PI * SCALE_F;
    float * const w = v + h;
    float temp[h];
    
    for (int i = 0; i < h; i++) temp[i] = RAND_F;
    vDSP_vsmsa(temp, 1, &s, &s, temp, 1, h);
    vvlogf(temp, temp, &h);
    for (int i = 0; i < h; i++) temp[i] *= -2.0f;
    vvsqrtf(temp, temp, &h);
    
    for (int i = 0; i < h; i++) v[i] = p * RAND_F;
    vvsincosf(v, v, v+h, &h);
    for (int i = 0; i < h; i++) {
        v[i] *= temp[i];
        w[i] *= temp[i];
    }
}