#include <time.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>

#include <uuid/uuid.h>

#include <openssl/sha.h>

#include <gsl/gsl_statistics_double.h>

#include "sig.h"
#include "pok.h"
#include "like.h"
#include "bn512.h"
#include "utils_like.h"

double print_trials_res(double * trials_res, int nb_trials)
{

    double mean = gsl_stats_mean(trials_res, 1, nb_trials);
    double stddev = gsl_stats_sd_m(trials_res, 1, nb_trials, mean);
    double confidence_lo = mean - ((1.96 * stddev) / sqrt(nb_trials));
    double confidence_hi = mean + ((1.96 * stddev) / sqrt(nb_trials));
    printf("    mean                   = %f\n", mean * 1000);
    printf("    stddev                 = %f\n", stddev * 1000);
    printf("    95%% confidence inteval = [%f, %f]\n\n", confidence_lo * 1000, confidence_hi * 1000);
    return mean * 1000;

}

void mesure_pairing(int nb_trials)
{

    printf("pairing : \n");

    mclBnG1 P;
    mclBnG2 Q;
    mclBnGT e;

    mclBn_init(MCL_BN462, MCLBN_COMPILED_TIME_VAR);

    mclBnG1_hashAndMapTo(&P, "abc", 3);
    mclBnG2_hashAndMapTo(&Q, "def", 3);

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        mclBn_pairing(&e, &P, &Q);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

}

void mesure_arithmetic_EC(int nb_trials)
{

    mclBnG1 P, xP;
    mclBnG2 Q, xQ;
    mclBnFr x;

    mclBn_init(MCL_BN462, MCLBN_COMPILED_TIME_VAR);

    mclBnFr_setByCSPRNG(&x);
    mclBnG1_hashAndMapTo(&P, "abc", 3);
    mclBnG2_hashAndMapTo(&Q, "def", 3);

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    printf("Scalar mult in G1 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        mclBnG1_mul(&xP, &P, &x);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }
    print_trials_res(trials_res, nb_trials);

    printf("Scalar mult in G2 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        mclBnG2_mul(&xQ, &Q, &x);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }
    print_trials_res(trials_res, nb_trials);

    printf("Add point in G1 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        mclBnG1_add(&xP, &P, &xP);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }
    print_trials_res(trials_res, nb_trials);

    printf("Add point in G2 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        mclBnG2_add(&xQ, &Q, &xQ);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }
    print_trials_res(trials_res, nb_trials);

}

void mesure_edd25519(int nb_trials)
{
    struct stat st = {0};
    size_t sig_len = ED25519_SIG_LENGTH;
    unsigned char * sig = (unsigned char *) malloc(sig_len * sizeof(unsigned char));;
    unsigned char msg[] = "Test sig";

    if (stat("./keys", &st) == -1) {
        mkdir("./keys", 0700);
    }

    sgen_ed25519("keys/pub.pem", "keys/priv.pem");

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    printf("sig ed25519 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ssig_ed25519("keys/priv.pem", msg, sizeof(msg), &sig, &sig_len);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

    printf("verif ed25519 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        sver_ed25519("keys/pub.pem", msg, sizeof(msg), sig, sig_len);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

    free(sig);

}

void mesure_sok(int nb_trials)
{

    const char * G2_basePoint_hexstr = "1 0257ccc85b58dda0dfb38e3a8cbdc5482e0337e7c1cd96ed61c913820408208f9ad2699bad92e0032ae1f0aa6a8b48807695468e3d934ae1e4df 1d2e4343e8599102af8edca849566ba3c98e2a354730cbed9176884058b18134dd86bae555b783718f50af8b59bf7e850e9b73108ba6aa8cd283 0a0650439da22c1979517427a20809eca035634706e23c3fa7a6bb42fe810f1399a1f41c9ddae32e03695a140e7b11d7c3376e5b68df0db7154e 073ef0cbd438cbe0172c8ae37306324d44d5e6b0c69ac57b393f1ab370fd725cc647692444a04ef87387aa68d53743493b9eba14cc552ca2a93a";

    char msg[] = "test";
    mclBnFr x, d;
    mclBnG2 Q, xQ, Rho;

    mclBnFr_setByCSPRNG(&x);
    mclBnG2_setStr(&Q, G2_basePoint_hexstr, strlen(G2_basePoint_hexstr), 16);
    mclBnG2_mul(&xQ, &Q, &x);

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    printf("sok_G2 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        sok_G2(&Q, &x, &xQ, (unsigned char *)msg, strlen(msg), &Rho, &d);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

    printf("sokver_G2 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        sokver_G2(&Q, &xQ, &Rho, &d, (unsigned char *)msg, strlen(msg));
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

}

void mesure_nipok(int nb_trials)
{

    const char * G1_basePoint_hexstr = "1 21a6d67ef250191fadba34a0a30160b9ac9264b6f95f63b3edbec3cf4b2e689db1bbb4e69a416a0b1e79239c0372e5cd70113c98d91f36b6980d 0118ea0460f7f7abb82b33676a7432a490eeda842cccfa7d788c659650426e6af77df11b8ae40eb80f475432c66600622ecaa8a5734d36fb03de";

    mclBnFr x, d;
    mclBnG1 P, xP, Rho;

    mclBnFr_setByCSPRNG(&x);
    mclBnG1_setStr(&P, G1_basePoint_hexstr, strlen(G1_basePoint_hexstr), 16);

    mclBnG1_mul(&xP, &P, &x);

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    printf("nipok_G1 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        nipok_G1(&P, &x, &xP, &Rho, &d);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

    printf("nipokver_G1 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        nipokver_G1(&P, &xP, &Rho, &d);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

}

void mesure_eqnipok(int nb_trials)
{
    const char * G1_basePoint_hexstr = "1 21a6d67ef250191fadba34a0a30160b9ac9264b6f95f63b3edbec3cf4b2e689db1bbb4e69a416a0b1e79239c0372e5cd70113c98d91f36b6980d 0118ea0460f7f7abb82b33676a7432a490eeda842cccfa7d788c659650426e6af77df11b8ae40eb80f475432c66600622ecaa8a5734d36fb03de";
    const char * G2_basePoint_hexstr = "1 0257ccc85b58dda0dfb38e3a8cbdc5482e0337e7c1cd96ed61c913820408208f9ad2699bad92e0032ae1f0aa6a8b48807695468e3d934ae1e4df 1d2e4343e8599102af8edca849566ba3c98e2a354730cbed9176884058b18134dd86bae555b783718f50af8b59bf7e850e9b73108ba6aa8cd283 0a0650439da22c1979517427a20809eca035634706e23c3fa7a6bb42fe810f1399a1f41c9ddae32e03695a140e7b11d7c3376e5b68df0db7154e 073ef0cbd438cbe0172c8ae37306324d44d5e6b0c69ac57b393f1ab370fd725cc647692444a04ef87387aa68d53743493b9eba14cc552ca2a93a";

    mclBnFr x, d;
    mclBnG1 P, xP, Rho;
    mclBnG2 Q, xQ, Sigma;

    mclBnFr_setByCSPRNG(&x);
    mclBnG1_setStr(&P, G1_basePoint_hexstr, strlen(G1_basePoint_hexstr), 16);
    mclBnG2_setStr(&Q, G2_basePoint_hexstr, strlen(G2_basePoint_hexstr), 16);

    mclBnG1_mul(&xP, &P, &x);

    mclBnG2_mul(&xQ, &Q, &x);

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    printf("eq_nipok_G1_G2 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        eq_nipok_G1_G2(&P, &xP, &Q, &xQ, &x, &Rho, &Sigma, &d);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

    printf("eq_nipokver_G1_G2 : \n");
    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        eq_nipokver_G1_G2(&P, &xP, &Q, &xQ, &Rho, &Sigma, &d);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    print_trials_res(trials_res, nb_trials);

}

double mesure_ake_a_get_mx(mclBnG1 * P, mclBnG2 * Q, mclBnFr * x, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ake_a_get_mx(P, Q, x, omega, omega_len, xP, xQ, x_ni);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_verify_mx(mclBnG1 * P, mclBnG1 * xP, mclBnG2 * Q, mclBnG2 * xQ, unsigned char * omega, size_t omega_len, XY_ni * x_ni, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        verify_mx(P, xP, Q, xQ, omega, omega_len, x_ni);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_ake_b_get_my(mclBnG2 * Q, mclBnFr * y, unsigned char * omega, size_t omega_len, mclBnG2 * yQ, XY_ni * y_ni, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ake_b_get_my(Q, y, omega, omega_len, yQ, y_ni);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_ake_b_get_sigma_Y_1(char * priv_key_path, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, mclBnG2 * yQ, XY_ni * y_ni, unsigned char * sigma_Y_1, size_t sig_len, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ake_b_get_sigma_Y_1(priv_key_path, omega, omega_len, xP, xQ, x_ni, yQ, y_ni, sigma_Y_1, sig_len);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_verify_my(mclBnG2 * Q, mclBnG2 * yQ, unsigned char * omega, size_t omega_len, XY_ni * y_ni, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        verify_my(Q, yQ, omega, omega_len, y_ni);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_verify_sigma_Y_1(char * pub_key_path, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, mclBnG2 * yQ, XY_ni * y_ni, unsigned char * sigma_Y_1, size_t sig_len, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        verify_sigma_Y_1(pub_key_path, omega, omega_len, xP, xQ, x_ni, yQ, y_ni, sigma_Y_1, sig_len);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_ake_a_get_sigma_X(char * priv_key_path, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, mclBnG2 * yQ, XY_ni * y_ni, unsigned char * sigma_Y_1 , unsigned char * sigma_X, size_t sig_len, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ake_a_get_sigma_X(priv_key_path, omega, omega_len, xP, xQ, x_ni, yQ, y_ni, sigma_Y_1, sigma_X, sig_len);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_verify_sigma_X(char * pub_key_path, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, mclBnG2 * yQ, XY_ni * y_ni, unsigned char * sigma_Y_1 , unsigned char * sigma_X, size_t sig_len, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        verify_sigma_X(pub_key_path, omega, omega_len, xP, xQ, x_ni, yQ, y_ni, sigma_Y_1, sigma_X, sig_len);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_ake_b_get_sigma_Y_2(char * priv_key_path, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, mclBnG2 * yQ, XY_ni * y_ni, unsigned char * sigma_Y_1 , unsigned char * sigma_X, unsigned char * sigma_Y_2, size_t sig_len, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ake_b_get_sigma_Y_2(priv_key_path, omega, omega_len, xP, xQ, x_ni, yQ, y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_verify_sigma_Y_2(char * pub_key_path, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, mclBnG2 * yQ, XY_ni * y_ni, unsigned char * sigma_Y_1 , unsigned char * sigma_X, unsigned char * sigma_Y_2, size_t sig_len, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        verify_sigma_Y_2(pub_key_path, omega, omega_len, xP, xQ, x_ni, yQ, y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_ake_a_get_shared_key(mclBnG1 * L_pk, mclBnG2 * yQ, mclBnFr * x, mclBnGT * ka, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ake_a_get_shared_key(L_pk, yQ, x, ka);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_ake_b_get_shared_key(mclBnG1 * L_pk, mclBnG2 * xQ, mclBnFr * y, mclBnGT * kb, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ake_b_get_shared_key(L_pk, xQ, y, kb);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_ake_O_get_sst(char * priv_key_path, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, mclBnG2 * yQ, XY_ni * y_ni, unsigned char * sigma_Y_1 , unsigned char * sigma_X, unsigned char * sigma_Y_2, size_t sig_len, SST * sst, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        ake_O_get_sst(priv_key_path, omega, omega_len, xP, xQ, x_ni, yQ, y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len, sst);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
        free(sst->m);
        free(sst->sigma_O);
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_verify_sst(char * pub_key_path, unsigned char * omega, size_t omega_len, mclBnG1 * xP, mclBnG2 * xQ, XY_ni * x_ni, mclBnG2 * yQ, XY_ni * y_ni, unsigned char * sigma_Y_1 , unsigned char * sigma_X, unsigned char * sigma_Y_2, size_t sig_len, SST * sst, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        verify_sst(pub_key_path, omega, omega_len, xP, xQ, x_ni, yQ, y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len, sst);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_verify_L_ni_for_2L(mclBnG1 * P, mclBnG1 * L1_pk, Lambda_ni * l1_ni, mclBnG1 * L2_pk, Lambda_ni * l2_ni, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        verify_L_ni(P, 4, L1_pk, l1_ni, L2_pk, l2_ni);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_tdgen_get_li_T1_for_2L(mclBnG1 * xP, mclBnG2 * yQ, mclBnFr * l1_sk, mclBnGT * l1_T1, mclBnFr * l2_sk, mclBnGT * l2_T1, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        tdgen_get_li_T1(xP, yQ, 4, l1_sk, l1_T1, l2_sk, l2_T1);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_tdgen_get_li_T2_for_2L(mclBnG1 * P, mclBnG1 * xP, mclBnG2 * yQ, mclBnG1 * L1_pk, mclBnFr * l1_sk, mclBnGT * l1_T1, Lambda_eq_ni * l1_T2, mclBnG1 * L2_pk, mclBnFr * l2_sk, mclBnGT * l2_T1, Lambda_eq_ni * l2_T2, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        tdgen_get_li_T2(P, xP, yQ, 8, L1_pk, l1_sk, l1_T1, l1_T2, L2_pk, l2_sk, l2_T1, l2_T2);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_verify_li_T2_for_2L(mclBnG1 * P, mclBnG1 * xP, mclBnG2 * yQ, mclBnG1 * L1_pk, mclBnGT * l1_T1, Lambda_eq_ni * l1_T2, mclBnG1 * L2_pk, mclBnGT * l2_T1, Lambda_eq_ni * l2_T2, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        verify_li_T2(P, xP, yQ, 6, L1_pk, l1_T1, l1_T2, L2_pk, l2_T1, l2_T2);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

double mesure_open_get_shared_key_for_2(mclBnGT * k, mclBnGT * l1_T1, mclBnGT * l2_T1, int nb_trials)
{

    double trials_res[nb_trials];
    clock_t begin;
    clock_t end;

    for(int i = 0; i < nb_trials; i++)
    {
        begin = clock();
        open_get_shared_key(k, 2, l1_T1, l2_T1);
        end = clock();
        trials_res[i] = (double)(end - begin) / CLOCKS_PER_SEC;
    }

    return print_trials_res(trials_res, nb_trials);

}

int main(int argc, char *argv[])
{

    printf("Run multiple times differents functions of the like potocol. All mesures are in ms.\n\n");

    // Mesure perf parameter
    int nb_trials = 100;
    if( argc == 2 )
        nb_trials = atoi(argv[1]);
    printf("Number of trials : %d\n\n", nb_trials);
    double total_time_A = 0.0;
    double total_time_B = 0.0;
    double total_time_O = 0.0;
    double total_time_Ver = 0.0;
    double total_time_TDGen = 0.0;
    double total_time_Open = 0.0;

    /*******************************************************************
     *                                                                 *
     *                      INDEPENDANT MESURES                        *
     *                                                                 *
     * *****************************************************************/

    mesure_pairing(nb_trials);
    mesure_arithmetic_EC(nb_trials);
    mesure_edd25519(nb_trials);
    mesure_sok(nb_trials);
    mesure_nipok(nb_trials);
    mesure_eqnipok(nb_trials);

    /*******************************************************************
     *                                                                 *
     *                          LIKE MESURES                           *
     *                                                                 *
     * *****************************************************************/

    // General Param
    mclBnG1 P, xP;
    mclBnG2 Q, xQ, yQ;
    mclBnFr x, y;
    XY_ni x_ni, y_ni;
    mclBnGT ka, kb, k, l1_T1, l2_T1;
    SST sst;
    Lambda_eq_ni l1_T2, l2_T2;

    // Id
    size_t id_len = sizeof(uuid_t);
    uuid_t id_A, id_B, id_L1, id_L2;
    uuid_generate_random(id_A);
    uuid_generate_random(id_B);
    uuid_generate_random(id_L1);
    uuid_generate_random(id_L2);
    size_t omega_len = 4 * id_len;
    unsigned char omega[omega_len];

    // Keys
    char pub_key_A[] = "keys/a_pub_key.pem";
    char priv_key_A[] = "keys/a_priv_key.pem";
    char pub_key_B[] = "keys/b_pub_key.pem";
    char priv_key_B[] = "keys/b_priv_key.pem";
    char pub_key_O[] = "keys/o_pub_key.pem";
    char priv_key_O[] = "keys/o_priv_key.pem";
    mclBnG1 L1_pk, L2_pk, L_pk;
    mclBnFr l1_sk, l2_sk;
    Lambda_ni l1_ni, l2_ni;

    // Sig buffer
    size_t sig_len = ED25519_SIG_LENGTH;
    unsigned char * sigma_Y_1 = (unsigned char *) malloc(sig_len * sizeof(unsigned char));
    unsigned char * sigma_X = (unsigned char *) malloc(sig_len * sizeof(unsigned char));
    unsigned char * sigma_Y_2 = (unsigned char *) malloc(sig_len * sizeof(unsigned char));

    // Setup
    setup(&P, &Q);

    //UKeyGen A
    u_o_key_gen(pub_key_A, priv_key_A);

    // UKeyGen B
    u_o_key_gen(pub_key_B, priv_key_B);

    // UKeyGen O
    u_o_key_gen(pub_key_O, priv_key_O);

    // LambdaKeyGen L1
    a_key_gen(&P, &l1_sk, &L1_pk, &l1_ni);

    // LambdaKeyGen L2
    a_key_gen(&P, &l2_sk, &L2_pk, &l2_ni);

    // AKE Precal
    verify_L_ni(&P, 4, &L1_pk, &l1_ni, &L2_pk, &l2_ni);
    ake_precalc_add_lipk(&L_pk, 2, &L1_pk, &L2_pk);
    ake_precalc_get_omega(omega, 8, id_A, id_len, id_B, id_len, id_L1, id_len, id_L2, id_len);

    // Mesure ake_a_get_mx
    printf("ake_a_get_mx : \n");
    double time_ake_a_get_mx = mesure_ake_a_get_mx(&P, &Q, &x, omega, omega_len, &xP, &xQ, &x_ni, nb_trials);

    // A : ake_a_get_mx
    ake_a_get_mx(&P, &Q, &x, omega, omega_len, &xP, &xQ, &x_ni);
    total_time_A += time_ake_a_get_mx;

    // Mesure verify_mx
    printf("verify_mx : \n");
    double time_verify_mx = mesure_verify_mx(&P, &xP, &Q, &xQ, omega, omega_len, &x_ni, nb_trials);

    // O : verify_mx
    verify_mx(&P, &xP, &Q, &xQ, omega, omega_len, &x_ni);
    total_time_O += time_verify_mx;

    // Mesure ake_b_get_my
    printf("ake_b_get_my : \n");
    double time_ake_b_get_my = mesure_ake_b_get_my(&Q, &y, omega, omega_len, &yQ, &y_ni, nb_trials);

    // Mesure ake_b_get_sigma_Y_1
    printf("ake_b_get_sigma_Y_1 : \n");
    double time_ake_b_get_sigma_Y_1 = mesure_ake_b_get_sigma_Y_1(priv_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sig_len, nb_trials);

    // B : ake_b_get_my, verify_mx, ake_b_get_sigma_Y_1
    verify_mx(&P, &xP, &Q, &xQ, omega, omega_len, &x_ni);
    ake_b_get_my(&Q, &y, omega, omega_len, &yQ, &y_ni);
    ake_b_get_sigma_Y_1(priv_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sig_len);
    total_time_B += time_verify_mx + time_ake_b_get_my + time_ake_b_get_sigma_Y_1;

    // Mesure verify_my
    printf("verify_my : \n");
    double time_verify_my = mesure_verify_my(&Q, &yQ, omega, omega_len, &y_ni, nb_trials);

    // Mesure verify_sigma_Y_1
    printf("verify_sigma_Y_1 : \n");
    double time_verify_sigma_Y_1 = mesure_verify_sigma_Y_1(pub_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sig_len, nb_trials);

    // O : verify_my, verify_sigma_Y_1
    verify_my(&Q, &yQ, omega, omega_len, &y_ni);
    verify_sigma_Y_1(pub_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sig_len);
    total_time_O += time_verify_my + time_verify_sigma_Y_1;

    // Mesure ake_a_get_sigma_X
    printf("ake_a_get_sigma_X : \n");
    double time_ake_a_get_sigma_X = mesure_ake_a_get_sigma_X(priv_key_A, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sig_len, nb_trials);

    // A : verify_my, verify_sigma_Y_1, ake_a_get_sigma_X
    verify_my(&Q, &yQ, omega, omega_len, &y_ni);
    verify_sigma_Y_1(pub_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sig_len);
    ake_a_get_sigma_X(priv_key_A, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sig_len);
    total_time_A += time_verify_my + time_verify_sigma_Y_1 + time_ake_a_get_sigma_X;

    // Mesure verify_sigma_X
    printf("verify_sigma_X : \n");
    double time_verify_sigma_X = mesure_verify_sigma_X(pub_key_A, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sig_len, nb_trials);

    // O : verify_sigma_X
    verify_sigma_X(pub_key_A, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sig_len);
    total_time_O += time_verify_sigma_X;

    // Mesure ake_b_get_sigma_Y_2
    printf("ake_b_get_sigma_Y_2 : \n");
    double time_ake_b_get_sigma_Y_2 = mesure_ake_b_get_sigma_Y_2(priv_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len, nb_trials);

    // B : verify_sigma_X, ake_b_get_sigma_Y_2
    verify_sigma_X(pub_key_A, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sig_len);
    ake_b_get_sigma_Y_2(priv_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len);
    total_time_B += time_verify_sigma_X + time_ake_b_get_sigma_Y_2;

    // Mesure ake_a_get_shared_key
    printf("ake_a_get_shared_key : \n");
    double time_ake_a_get_shared_key = mesure_ake_a_get_shared_key(&L_pk, &yQ, &x, &ka, nb_trials);

    // A : ake_a_get_shared_key
    ake_a_get_shared_key(&L_pk, &yQ, &x, &ka);
    total_time_A += time_ake_a_get_shared_key;

    // Mesure ake_b_get_shared_key
    printf("ake_b_get_shared_key : \n");
    double time_ake_b_get_shared_key = mesure_ake_b_get_shared_key(&L_pk, &xQ, &y, &kb, nb_trials);

    // B : ake_b_get_shared_key
    ake_b_get_shared_key(&L_pk, &xQ, &y, &kb);
    total_time_B += time_ake_b_get_shared_key;

    // Mesure ake_b_get_sigma_Y_2
    printf("verify_sigma_Y_2 : \n");
    double time_verify_sigma_Y_2 = mesure_verify_sigma_Y_2(pub_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len, nb_trials);

    // Mesure ake_O_get_sst
    printf("ake_O_get_sst : \n");
    double time_ake_O_get_sst = mesure_ake_O_get_sst(priv_key_O, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len, &sst, nb_trials);

    // O : verify_sigma_Y_2, ake_O_get_sst
    verify_sigma_Y_2(pub_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len);
    ake_O_get_sst(priv_key_O, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len, &sst);
    total_time_O += time_verify_sigma_Y_2 + time_ake_O_get_sst;

    // Mesure verify_sst
    printf("verify_sst : \n");
    double time_verify_sst = mesure_verify_sst(pub_key_O, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len, &sst, nb_trials);

    // Mesure verify_L_ni
    printf("verify_L_ni (2 Authorities) : \n");
    double time_verify_L_ni = mesure_verify_L_ni_for_2L(&P, &L1_pk, &l1_ni, &L2_pk, &l2_ni, nb_trials);

    // Verification
    verify_L_ni(&P, 4, &L1_pk, &l1_ni, &L2_pk, &l2_ni);
    verify_mx(&P, &xP, &Q, &xQ, omega, omega_len, &x_ni);
    verify_my(&Q, &yQ, omega, omega_len, &y_ni);
    verify_sigma_Y_1(pub_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sig_len);
    verify_sigma_X(pub_key_A, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sig_len);
    verify_sigma_Y_2(pub_key_B, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len);
    verify_sst(pub_key_O, omega, omega_len, &xP, &xQ, &x_ni, &yQ, &y_ni, sigma_Y_1, sigma_X, sigma_Y_2, sig_len, &sst);
    total_time_Ver += time_verify_L_ni + time_verify_mx + time_verify_my + time_verify_sigma_Y_1 + time_verify_sigma_X + time_verify_sigma_Y_2 + time_verify_sst;

    // Mesure tdgen_get_li_T1
    printf("tdgen_get_li_T1 (2 Authorities) : \n");
    double time_tdgen_get_li_T1_for_2L = mesure_tdgen_get_li_T1_for_2L(&xP, &yQ, &l1_sk, &l1_T1, &l2_sk, &l2_T1, nb_trials);

    // Mesure tdgen_get_li_T2
    printf("tdgen_get_li_T2 (2 Authorities) : \n");
    double time_tdgen_get_li_T2_for_2L = mesure_tdgen_get_li_T2_for_2L(&P, &xP, &yQ, &L1_pk, &l1_sk, &l1_T1, &l1_T2, &L2_pk, &l2_sk, &l2_T1, &l2_T2, nb_trials);

    // TDGen
    tdgen_get_li_T1(&xP, &yQ, 4, &l1_sk, &l1_T1, &l2_sk, &l2_T1);
    tdgen_get_li_T2(&P, &xP, &yQ, 8, &L1_pk, &l1_sk, &l1_T1, &l1_T2, &L2_pk, &l2_sk, &l2_T1, &l2_T2);
    total_time_TDGen += time_tdgen_get_li_T1_for_2L + time_tdgen_get_li_T2_for_2L;

    // Mesure verify_li_T2
    printf("verify_li_T2 (2 Authorities) : \n");
    double time_verify_li_T2_for_2 = mesure_verify_li_T2_for_2L(&P, &xP, &yQ, &L1_pk, &l1_T1, &l1_T2, &L2_pk, &l2_T1, &l2_T2, nb_trials);

    // Mesure open_get_shared_key
    printf("open_get_shared_key (2 Authorities) : \n");
    double time_open_get_shared_key_for_2 = mesure_open_get_shared_key_for_2(&k, &l1_T1, &l2_T1, nb_trials);

    // Open
    verify_li_T2(&P, &xP, &yQ, 6, &L1_pk, &l1_T1, &l1_T2, &L2_pk, &l2_T1, &l2_T2);
    open_get_shared_key(&k, 2, &l1_T1, &l2_T1);
    total_time_Open += time_verify_li_T2_for_2 + time_open_get_shared_key_for_2;

    // Check key
    int res = mclBnGT_isEqual(&ka, &kb);
    if(res != 1)
    {
        printf("Keys not equal\n");
    }
    res = mclBnGT_isEqual(&ka, &k);
    if(res != 1)
    {
        printf("Keys not equal\n");
    }

    printf("Total CPU running time for A : %f\n", total_time_A);
    printf("Total CPU running time for B : %f\n", total_time_B);
    printf("Total CPU running time for O : %f\n", total_time_O);
    printf("Total CPU running time for Verification : %f\n", total_time_Ver);
    printf("Total CPU running time for TDGen (2 Authorities) : %f\n", total_time_TDGen);
    printf("Total CPU running time for Open (2 Authorities) : %f\n", total_time_Open);

    free(sigma_Y_1);
    free(sigma_X);
    free(sigma_Y_2);
    free(sst.m);
    free(sst.sigma_O);

    return 0;

}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <omp.h>
#include <fftw3.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include "fftlog.h"

#define check_memory(p, name) if(p == NULL){printf("Problems to alloc %s.\n", name); return 0;}
#define Lc_MAX	1.0e+2
#define Mc_MIN	1.0e+5
#define M_max 	6.0e+15

/*Structure for the peaks in the density field*/
typedef struct Halos_centers {
	int x[3];		/*Index of the halo center*/
	float den;		/*Density of the halo's central cell*/
} PEAKS;

/*Structure for the final halos*/
typedef struct Halos {
	int x[3];		/*Index of central cell of teh halo*/
	int cont;		/*Number of cells in the halo*/
} HALOS;

/*Barrier used for the halo definition*/
float Barrier(float S, float dc, char barrier, float a, float b, float alpha){
	float resp;

	/*The Press-Schechter barrier*/
	if(barrier == 0)
		resp = dc;

	/*The Sheth-Tormen barrier*/
	else if(barrier == 1)
		resp = sqrt(a)*dc*(1.0 + b*pow(S/(a*dc*dc), alpha));

	return resp;
}

/*Partition function for the quicksort*/
long int partition_peaks( PEAKS a[], long l, long r) {
   	long i, j, k;
	PEAKS pivot, t;
  	pivot.den = a[l].den;
	for(k=0;k<3;k++) pivot.x[k] = a[l].x[k];
   	i = l; j = r+1;

   	while( 1){
   	do ++i; while( a[i].den >= pivot.den && i < r );
   	do --j; while( a[j].den < pivot.den );
   	if( i >= j ) break;
   	t.den = a[i].den; a[i].den = a[j].den; a[j].den = t.den;
	for(k=0;k<3;k++){ t.x[k] = a[i].x[k]; a[i].x[k] = a[j].x[k]; a[j].x[k] = t.x[k];}
   	}
   	t.den = a[l].den; a[l].den = a[j].den; a[j].den= t.den;
	for(k=0;k<3;k++){ t.x[k] = a[l].x[k]; a[l].x[k] = a[j].x[k]; a[j].x[k] = t.x[k];}
   	return j;
}

/*The quicksort algorithm to sort the peaks list*/
void quickSort_peaks( PEAKS a[], long l, long r){
	long j;

   	if( l < r ){
   	// divide and conquer
        j = partition_peaks( a, l, r);
       	quickSort_peaks( a, l, j-1);
      	quickSort_peaks( a, j+1, r);
   	}
}

/*Define the distance between two cells*/
long int dist2(long int i, long int j, long int k){
	long int resp;

	resp = i*i + j*j + k*k;

	return resp;
}

/*Define the cyclic sum for floats*/
float cysumf(float x, float y, float L){
	float resp;

	resp = x + y;
	if(resp>=L)	resp -= L;
	if(resp<0)	resp += L;

	return resp;
}

/*Define the cyclic sum*/
int cysum(int i, int j, int nd){
	int resp;

	resp = i+j;
	if(resp>=nd)	resp -= nd;
	if(resp<0)	resp += nd;

	return resp;
}

/*Window function in the Fourier space*/
double W(double k, double R){
	double resp;

	resp = 3.0/(pow(k*R,2))*(sin(k*R)/(k*R) - cos(k*R));
	return resp;
}

/*Evaluate the square root of matter variance*/
double calc_sigma(double *k, double *P, int Nk, double R){
	int i;
	double resp;

	resp = 0.0;
	for(i=0;i<Nk-1;i++)
		resp += (k[i+1] - k[i])/2.0*(P[i]*pow(k[i]*W(k[i],R), 2) + P[i+1]*pow(k[i+1]*W(k[i+1],R), 2));

	return resp/(2.0*M_PI*M_PI);
}

/*Evaluate the mass function for a given sigma*/
double fh(double sigma, int model, double dc){
	double resp, nu;
	double B, d, e, f, g;

	//Press-Schechter
	if(model == 0){
		nu = dc/sigma;
		resp = sqrt(2.0/M_PI)*nu*exp(-nu*nu/2.0);
	}

	//Tinker Delta = 300
	else if(model == 1){
		B = 0.466;
		d = 2.06;
		e = 0.99;
		f = 0.48;
		g = 1.310;

		resp = B*(pow(sigma/e, -d) + pow(sigma, -f))*exp(-g/(sigma*sigma));
	}

	return resp;
}

/*Find the index of the next sphere*/
int Next_Count(int *spheres, int Ncells, int count){
	int i, resp;

	for(i=0;i<Ncells;i++)
		if(spheres[i] == count){
			resp = i + 1;
			break;
		}

	return resp;
}

/*Halo concentration*/
float f_c(float Mv, float Mstar, float z){
	float resp;

	resp = 9.0/(1.0 + z)*pow(Mv/Mstar, -0.13);

	return resp;
}

/*Generate a random number from 0 to Rv following the NFW profile*/
float Generate_NFW(float rv, float c, float A, int seed){
	float Int, rs, r, rtmp;

	gsl_rng *rng_ptr;
	rng_ptr = gsl_rng_alloc (gsl_rng_taus);
	gsl_rng_set(rng_ptr, seed);

	Int = gsl_rng_uniform(rng_ptr);
	rs = rv/c;

	r = gsl_rng_uniform(rng_ptr);
	rtmp = r + 1.0;
	while(fabs(r-rtmp) > 0.001){
		rtmp = r;
		r = r - ((log(1.0 + r*c) - r*c/(1.0 + r*c) - A*Int)*pow(1.0 + r*c, 2))/(c*(2.0*r*c + r*r*c*c));
	}
    gsl_rng_free(rng_ptr);

	return r*rv;
}

/*Mean value of central galaxies*/
float Ncentral(float M, float logMmin, float siglogM){
	float resp;

	resp = 0.5*(1.0 + erf((log10(M) - logMmin)/siglogM));

	return resp;
}

/*Mean value of satellite galaxies*/
float Nsatellite(float M, float logM0, float logM1, float alpha){
	float resp;

	resp = pow((M - pow(10.0, logM0))/pow(10.0, logM1), alpha);

	return resp;
}

int main(int argc,char *argv[])
{
FILE *power, *den_grid, *halo_cat, *disp_cat, *light_cat, *collapse_cat;
char powerfile[100], denfile[100], halofile[100], dispfile[100], lightfile[100], collapsefile[100];
char DO_2LPT, BARRIER, out_inter, out_halos, out_collapse, DEN_GRID, DISP_CAT, DO_HOD;
int i, j, k, nx, ny, nz, nz2, Nmin, N_cores, Nk, Nr, Ncells, seed, cont_tmp, grows, grows_tmp, tmp, nmin, m, m_tmp, nsnap;
long np, cont, nh, l;
float Lc, Om0, redshift, Growth, dc, EB_a, EB_b, EB_alpha, rhoc, Hz, Omz, rhomz, Mtot, Mcell, Lx, Ly, Lz, klx, kly, klz, Normx, Normk, Dv, kx, ky, kz, kmod, *Sig_grid, sigtmp, std, den, den_tmp, factx, facty, factz, fact, phixx, phixy, phixz, phiyy, phiyz, phizz, Pobs[3], LoS[3], dist_min, dist_max, theta_min, cos_min;
double *K, *P, *R, *M, *Sig, Rmin, Rmax, *R_xi, *Xi;
fftwf_plan p1, p2;
fftwf_complex *deltak, *deltak_tmp;
float *delta;
int *flag, *sphere;
PEAKS *peaks, *tmpp;
HALOS *halos;

if (argc != 35){
	printf("\nWrong number of arguments.\n");
	printf("arg1: Name of the power spectrum file.\n");
	printf("arg2: Size (in Mpc/h) or mass (in M_odot/h) of each cell.\n");
	printf("arg3-5: Number of cells along each direction.\n");
	printf("arg6: Some seed for the random number generator.\n");
	printf("arg7: Use the 2LPT to move the halos? Yes (1) or No (0).\n");
	printf("arg8: The Value of Omega_m today.\n");
	printf("arg9: The readshift z.\n");
	printf("arg10: The rate between the growth function at the final resdshit and at the redshift of the input power spectrum.\n");
	printf("arg11: The value of critical density delta _{c}. Put 0 to use the fit.\n");
	printf("arg12: The minimum number of partiles in a halo of the final catalogue.\n");
	printf("arg13: The number of cores to use in the parallel parts.\n");
	printf("arg14: Prefix for the outputs.\n");
	printf("arg15: Which barrier would you like to use to find the halos?\n\tThe statical barrier (SB) (0);\n\tThe ellipsoidal barrier (EB) (1).\n");
	printf("arg16: Which intermediate results would you like to save?:\n\tNo one (0);\n\tThe gaussian density grid (1);\n\tThe particles displaced with LPT (2);\n\tBoth (3).\n");
	printf("arg17: How do you want the final halo catalogue?\n\tNo halo catalogue (0);\n\tThe positions and velocities in the real space (1);\n\tThe positions and velocities in the real space light cone (2);\n\tThe positions and velocities in redshift space light cone(3).\n");
	printf("arg18-20: The three parameters for the ellipsoidal barrier: a, b and alpha.\n");
	printf("arg21: Read the density grid (0) or compute it (1)?\n");
	printf("arg22: Read the displacement field (0) or compute it (1)?\n");
	printf("arg23-25: Position of the observer in units of the box size.\n");
	printf("arg26-28: Direction of the line of sight.\n");
	printf("arg29-30: Minimum and maximum comoving distance of the halos in this snapshot in the light cone.\n");
	printf("arg31: Angular aperture of the light cone in units of pi.\n");
	printf("arg32: Save the information about the collapsed particles in the light cone? Yes (1) or No (0).\n");
   	printf("arg33: Populate the halos with a HOD?\n\tNo (0);\n\tYes, with a single type of galaxy (1)\n\tYes, with multiple types of galaxies(2).\n");
	printf("arg34: Number of this snapshot.\n");

	exit(0);
}

/*Get the name of all files*/
sprintf(powerfile, "%s", argv[1]);
sprintf(denfile, "%s_den.dat", argv[14]);
sprintf(halofile, "%s_halos.dat", argv[14]);
sprintf(dispfile, "%s_disp.dat", argv[14]);

/*Parameters with specifications of the box and options for this simulation*/
Lc = atof(argv[2]);			//Size or mass of each cell
nx = atoi(argv[3]);			//Number of cells along the x-direction
ny = atoi(argv[4]);			//Number of cells along the y-direction
nz = atoi(argv[5]);			//Number of cells along the z-direction
seed = atoi(argv[6]);			//Seed for the random generator (same seed gives the same final catalogue)
DO_2LPT = (char)atoi(argv[7]);		//Parameter with the information about the use (or not) of second order lagrangian perturbation theory
Nmin = atoi(argv[12]);			//Number of particles in the smaller final halo
N_cores = atoi(argv[13]);		//Number of cores used by openmp in the parallel parts
BARRIER = (char)atoi(argv[15]);		//Parameter with the information about the utilization (or not) of the EB
out_inter = (char)atoi(argv[16]);	//Parameter with the information about which intermediate results must be output
out_halos = (char)atoi(argv[17]);	//Parameter with the information about what to save in the final halo catalogue
out_collapse = (char)atoi(argv[32]);	//Parameter with the information about the collapsed particles in the light cone
DEN_GRID = (char)atoi(argv[21]);	//Compute a new density field (1) or just read it from a file (0)?
DISP_CAT = (char)atoi(argv[22]);	//Compute the displacement field (1) or just read it from a file (0)?
DO_HOD = (char)atoi(argv[33]);      //Populate the halos with no galaxies (0), one type of galaxy (1) or multiple types (2)?

/*Some physical parametrs used in this simulation*/
Om0 = atof(argv[8]);			//Omega_m value today (z=0)
redshift = atof(argv[9]);		//Redshift of the final catalogues
Growth = atof(argv[10]);		//Ratio between the growth function at the final redshift and the redshift of the inpur power spectrum
dc = atof(argv[11]);			//Value of the critical density for the halo formation linearly extrapoleted using linear theory to the redshift of the final catalogues

/*Parameters for the EB*/
EB_a = atof(argv[18]);			//Parameter a of the EB
EB_b = atof(argv[19]);			//Parameter b of the EB
EB_alpha = atof(argv[20]);		//Parameter alpha of the EB

/*Parameters for the construction of the light cone*/
Pobs[0] = atof(argv[23]);		//Position x of the observer in units of the box size
Pobs[1] = atof(argv[24]);		//Position y of the observer in units of the box size
Pobs[2] = atof(argv[25]);		//Position z of the observer in units of the box size
LoS[0] = atof(argv[26]);		//Component x of the direction of the line of sight
LoS[1] = atof(argv[27]);		//Component y of the direction of the line of sight
LoS[2] = atof(argv[28]);		//Component z of the direction of the line of sight
/*Normalize the LoS vector*/
kmod = 0.0;
for(i=0;i<3;i++)
	kmod += LoS[i]*LoS[i];
for(i=0;i<3;i++)
	LoS[i] = LoS[i]/sqrt(kmod);
dist_min = atof(argv[29]);		//Minimum comoving distance of this slice
dist_max = atof(argv[30]);		//Maximum comoving distance of this slice
theta_min = atof(argv[31])*M_PI;	//Minimum angle theta
cos_min = cos(theta_min);		//Cossine of the minimum angle theta
nsnap = atoi(argv[34]);			//Number of this snapshot
sprintf(lightfile, "%s_%d_LightCone.dat", argv[14], nsnap);
sprintf(collapsefile, "%s_%d_Collapse.dat", argv[14], nsnap);

/*Some derived parameters used in this simulation*/
rhoc = 2.775e+11;			//Critical density in unitis of M_odot/Mpc*h^2
Hz = 100.0*sqrt(Om0*pow(1.0 + redshift, 3.0) + (1.0 - Om0));			//Hubble constant at the final redshift
Omz = Om0*pow(1.0 + redshift, 3.0)/(Om0*pow(1.0 + redshift, 3.0) + (1.0 - Om0));//Matter contrast density at the final redshift
rhomz = Om0*rhoc;			//Matter density at the final redshift
Dv = (18*M_PI*M_PI + 82.0*(Omz - 1.0) - 39.0*pow(Omz - 1.0, 2.0))/Omz;		//Overdensity used to put galaxies in the halos
if(Lc < Lc_MAX)				//If the size of each cell was given compute the mass of each cell
	Mcell = rhomz*pow(Lc, 3.0);
else if(Lc > Mc_MIN){			//If the mass of each cell was given compute the size of each cell
	Mcell = Lc;
	Lc = pow(Mcell/rhomz, 1.0/3.0);
}
else{					//Notify an unexpected behavior and exit
	printf("A cell larger than %f [Mpc/h] or with a mass smaller than %e [M_odot/h] is not expected. Please, change this value or change the definition of Lc_MAX and Mc_MIN in the code.\n", Lc_MAX, Mc_MIN);
	exit(0);
}
Lx = Lc*nx;				//Compute the size of the box along the x-direction
Ly = Lc*ny;				//Compute the size of the box along the y-direction
Lz = Lc*nz;				//Compute the size of the box along the z-direction
Mtot = rhomz*Lx*Ly*Lz;			//Compute the total mass in the box
klx = 2.0*M_PI/Lx;			//Compute the fundamental frequency in the x-direction
kly = 2.0*M_PI/Ly;			//Compute the fundamental frequency in the y-direction
klz = 2.0*M_PI/Lz;			//Compute the fundamental frequency in the z-direction
Normx = 1.0/sqrt(Lx*Ly*Lz);		//Compute the normalization needed when aplyed the FFTW3 from k to x space
Normk = sqrt(Lx*Ly*Lz)/(nx*ny*nz);	//Compute the normalization needed when aplyed the FFTW3 from x to k space
nz2 = nz/2 + 1;				//Quantity used to alloc the complex arrays used in the FFTW3
nmin = nx;				//Determine the smaller direction
if(nmin > ny)	nmin = ny;
if(nmin > nz)	nmin = nz;

/*Compute the number of repetitions of this box to construct the light cone*/
float Pos[3], dist, cost, vr, Mass;
int Nrep_x, Nrep_y, Nrep_z;
if(out_halos == 2 || out_halos == 3){
	Nrep_x = floor(dist_max/Lx) + 1;
	Nrep_y = floor(dist_max/Ly) + 1;
	Nrep_z = floor(dist_max/Lz) + 1;
}

/*Parameters of the HOD model*/
int Ngals, Ncen, Nsat;
float r,  phi, theta, Rv, C, A;
float logMmin, siglogM, logM0, logM1, alpha;
logMmin = 12.44005264;
siglogM = 0.79560376;
logM0 = 11.98154109;
logM1 = 12.99600074;
alpha = 1.13717828;

/*Check some inputs before to start*/
if(out_inter == 0 && out_halos == 0){
	printf("You need to choose something to output! arg16, arg17 and/or arg18 must be >0!\n");
	exit(0);
}

if(nx<0 || ny<0 || nz<0){
	printf("You are trying to use n = (%d, %d, %d) and it is not possible!\n", nx, ny, nz);
	exit(0);
}

if(DO_2LPT < 0 || DO_2LPT >1){
	printf("You are trying to use DO_2LPT = %d and it is not possible! Setting DO_2LPT = 0.\n", DO_2LPT);
	DO_2LPT = 0;
}

if(Growth <= 0.0){
	printf("You gave a value of the ratio between the growths of %f and it is not physical!\n", Growth);
	exit(0);
}

if(Nmin < 0){
	printf("You gave a negative number for the number of particles in the smaller halo (%d). Settin it in 1.\n", Nmin);
	Nmin = 1;
}

if(N_cores < 0){
	printf("You gave a negative number for the number of cores (%d). Settin it in 1.\n", N_cores);
	N_cores = 1;
}

if(BARRIER != 0 && BARRIER != 1){
	printf("You need to chose a valid barrier for the void detection! Your choice were %d.\n", BARRIER);
	exit(0);
}

if(Om0>1.0 || Om0<0.0){
	printf("Your Omega _{m} = %f! Put some valid value between 0.0 and 1.0.\n", Om0);
	exit(0);
}

if(dc < 0.0){
	printf("Your delta_{c} = %f < 0. Using the fit.\n", dc);
	dc = 1.686*pow(Omz, 0.0055);
}

if(dc == 0.0)
	dc = 1.686*pow(Omz, 0.0055);

if(out_halos > 1 && theta_min > 1.0){
	printf("Theta min must be equal or smaller than 1! Setting it to 1.\n");
	theta_min = 1.0;
	cos_min = -1.0;
}

if(out_halos > 1 && LoS[0] == 0.0 && LoS[1] == 0.0 && LoS[2] == 0.0){
	printf("You must give a non vanishing vector for the direction of the line of sight!\n");
	exit(0);
}

if(out_collapse == 1 && out_halos < 2){
	printf("It is not possible to save the information about the collapsed particles without the creation of a light cone. Ignoring this parameter.\n");
	out_collapse = 0;
}

printf("\nRunning the ExSHalos!\n\
Omega_m = %.3f, z = %.3f, Growth = %.3f, H = %.2f, d_c = %.3f and Delta_virial = %.1f\n\
L = (%.5f, %.5f, %.5f), N_cells = (%d, %d, %d), M_tot = %.5e, M_cell = %.5e and seed = %d.\n", Omz, redshift, Growth, Hz, dc, Dv, Lx, Ly, Lz, nx, ny, nz, Mtot, Mcell, seed);

omp_set_num_threads(N_cores);	//Set the number of cores used by the openmp

/**************************************/
/*   Constructing the density grids   */
/**************************************/
printf("\nConstructing the density grid in real and fourier space!\n");

/*Opennning the power spectrum file*/
power = fopen(powerfile, "r");
if (power == NULL) {
	printf("Unable to open %s\n", powerfile);
	exit(0);
}

/*Measuring the number of k's*/
Nk = -1;
while(!feof(power)){
	fscanf(power, "%f %f", &kx, &ky);
	Nk ++;
}
rewind(power);

/*Reading the power spectrum*/
K = (double *)malloc(Nk*sizeof(double));
check_memory(K, "K")
P = (double *)malloc(Nk*sizeof(double));
check_memory(P, "P")
for(i=0;i<Nk;i++){
	fscanf(power, "%lf %lf", &K[i], &P[i]);
	P[i] = pow((double)Growth, 2.0)*P[i];
}
fclose(power);

/*Evaluating the Sigma(R)*/
Nr = Nk;
R = (double *)malloc(Nr*sizeof(double));
check_memory(R, "R")
M = (double *)malloc(Nr*sizeof(double));
check_memory(M, "M")
Sig = (double *)malloc(Nr*sizeof(double));
check_memory(Sig, "Sig")
Rmin = (double)pow(Mcell*0.9*3.0/(4.0*M_PI*rhomz), 1.0/3.0);
Rmax = (double)pow(M_max*3.0/(4.0*M_PI*rhomz), 1.0/3.0);
for(i=0;i<Nr;i++){
	R[i] = pow(10, log10(Rmin) + i*(log10(Rmax) - log10(Rmin))/(Nr-1));
	M[i] = 4.0/3.0*M_PI*(double)rhomz*pow(R[i], 3);
}
for(i=0;i<Nr;i++)
	Sig[i] = sqrt(calc_sigma(K, P, Nk, R[i]));

/*Interpolating the Sigma(M)*/
gsl_interp_accel *acc = gsl_interp_accel_alloc();
gsl_spline *spline = gsl_spline_alloc(gsl_interp_cspline, Nr);
gsl_spline_init(spline, M, Sig, Nr);

/*Evaluate the integral of the mass function*/
double *Int;
Int = (double *)malloc(Nr*sizeof(double));
check_memory(Int, "Int")
Int[0] = 0.0;

for(i=1;i<Nr;i++)
	Int[i] = Int[i-1] - (log(Sig[i]) - log(Sig[i-1]))/2.0*(fh(Sig[i], 1, (double) dc)/pow(R[i], -3.0) + fh(Sig[i-1], 1, (double) dc)/pow(R[i-1], -3.0));

/*Interpolate the integral of the mass function as function of mass and its inverse*/
gsl_interp_accel *acc_I = gsl_interp_accel_alloc();
gsl_interp_accel *acc_InvI = gsl_interp_accel_alloc();
gsl_spline *spline_I = gsl_spline_alloc(gsl_interp_cspline, Nr);
gsl_spline *spline_InvI = gsl_spline_alloc(gsl_interp_cspline, Nr);
gsl_spline_init(spline_I, M, Int, Nr);
gsl_spline_init(spline_InvI, Int, M, Nr);
free(Int);

/*Compute the Sigma as function of the number of cells in the halo*/
Ncells = floor(M_max/Mcell);
Sig_grid = (float *)malloc(Ncells*sizeof(float));
check_memory(Sig_grid, "Sig_grid")
Sig_grid[0] = 1e+30;
for(i=1;i<Ncells;i++)
	Sig_grid[i] = pow(gsl_spline_eval(spline, i*Mcell, acc), 2.0);
gsl_spline_free(spline);
gsl_interp_accel_free(acc);
free(R);
free(M);
free(Sig);

/*Read the density grid*/
if(DEN_GRID == 0){
		delta = (float*)fftwf_malloc((size_t)nx*(size_t)ny*(size_t)nz*sizeof(float));
		check_memory(delta, "delta")

		printf("Reading the density grid\n");
		den_grid = fopen(denfile, "rb");
		if (den_grid == NULL) {
			printf("Unable to open %s\n", denfile);
			exit(0);
		}

		fread(&nx, sizeof(int), 1, den_grid);
		fread(&ny, sizeof(int), 1, den_grid);
		fread(&nz, sizeof(int), 1, den_grid);
		fread(&Lc, sizeof(float), 1, den_grid);
		for(i=0;i<nx;i++)
			for(j=0;j<ny;j++)
				for(k=0;k<nz;k++){
					size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

					fread(&delta[ind], sizeof(float), 1, den_grid);
					delta[ind] = Growth*delta[ind];
				}
		fclose(den_grid);
}

/*Construct the density grid*/
if(DEN_GRID == 1){

	/*Compute the Power spectrum in the box*/
	R_xi = (double *)malloc(Nk*sizeof(double));
	check_memory(R_xi, "R_xi")
	Xi = (double *)malloc(Nk*sizeof(double));
	check_memory(Xi, "Xi")
	pk2xi(Nk, K, P, R_xi, Xi);
	for(i=0;i<Nk;i++)
		if(R_xi[i] > (double)pow(Lx*Ly*Lz, 1.0/3.0)/2.0)
			Xi[i] = 0.0;
	xi2pk(Nk, R_xi, Xi, K, P);
	free(R_xi);
	free(Xi);

	/*Interpolate the power spectrum*/
	acc = gsl_interp_accel_alloc();
	spline = gsl_spline_alloc(gsl_interp_cspline, Nk);
	gsl_spline_init(spline, K, P, Nk);
	free(K);
	free(P);

	/*Allocating the density grids*/
	delta = (float*)fftwf_malloc((size_t)nx*(size_t)ny*(size_t)nz*sizeof(float));
	check_memory(delta, "delta")
	deltak_tmp = (fftwf_complex *) fftwf_malloc((size_t)nx*(size_t)ny*(size_t)nz2*sizeof(fftwf_complex));
	check_memory(deltak_tmp, "deltak_tmp")
	deltak = (fftwf_complex *) fftwf_malloc((size_t)nx*(size_t)ny*(size_t)nz2*sizeof(fftwf_complex));
	check_memory(deltak, "deltak")

	/*Alloc the needed quantities for the random generator*/
	gsl_rng *rng_ptr;
	rng_ptr = gsl_rng_alloc (gsl_rng_taus);
	gsl_rng_set(rng_ptr, seed);

	/*Constructing the Fourier space density grid*/
	#pragma omp parallel for private(i, j, k, kx, ky, kz, kmod, std)
	for(i=0;i<nx;i++){
		if(2*i<nx) kx = (float)i*klx;
		else kx = (float)(i-nx)*klx;

		for(j=0;j<ny;j++){
			if(2*j<ny) ky = (float)j*kly;
			else ky = (float)(j-ny)*kly;

			for(k=0;k<nz2;k++){
				kz = (float)k*klz;
				if(k == nz/2)	kz = -(float)nz/2.0*klz;

				size_t ind = (size_t)(i*ny + j)*(size_t)nz2 + (size_t)k;
				kmod = sqrt(kx*kx + ky*ky + kz*kz);

				if(kmod == 0.0)	kmod = pow(klx*kly*klz, 1.0/3.0)/4.0;
				std = sqrt(gsl_spline_eval(spline, kmod, acc)/2.0);

				/*Generate Gaussian random number with std*/
				deltak[ind][0] = (float)gsl_ran_gaussian(rng_ptr, std);
				deltak[ind][1] = (float)gsl_ran_gaussian(rng_ptr, std);
				deltak_tmp[ind][0] = deltak[ind][0];
				deltak_tmp[ind][1] = deltak[ind][1];

				if(isnan(deltak_tmp[ind][0]))	printf("Problem with deltak_tmp[%ld][0]\n", ind);
				if(isnan(deltak_tmp[ind][1]))	printf("Problem with deltak_tmp[%ld][1]\n", ind);
			}
		}
	}
	gsl_spline_free(spline);
	gsl_interp_accel_free(acc);
	gsl_rng_free (rng_ptr);

	/*Execute the FFTW3 to compute the density grid in real space*/
	p1 = fftwf_plan_dft_c2r_3d(nx, ny, nz, deltak_tmp, delta, FFTW_ESTIMATE);
	fftwf_execute(p1);
	fftwf_free(deltak_tmp);

	/*Save the density grid*/
	if(out_inter == 1 || out_inter == 3){
		printf("Saving the density grid\n");
		den_grid = fopen(denfile, "wb");
		if (den_grid == NULL) {
			printf("Unable to open %s\n", denfile);
			exit(0);
		}

		fwrite(&nx, sizeof(int), 1, den_grid);
		fwrite(&ny, sizeof(int), 1, den_grid);
		fwrite(&nz, sizeof(int), 1, den_grid);
		fwrite(&Lc, sizeof(float), 1, den_grid);
		for(i=0;i<nx;i++)
			for(j=0;j<ny;j++)
				for(k=0;k<nz;k++){
					size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

					fwrite(&delta[ind], sizeof(float), 1, den_grid);
				}
		fclose(den_grid);
	}
}

/*Compute the mean and std of the linear density field*/
kx = 0.0;
ky = 0.0;
for(i=0;i<nx;i++)
	for(j=0;j<ny;j++)
		for(k=0;k<nz;k++){
			size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

			delta[ind] = delta[ind]*Normx;
			kx += delta[ind]*delta[ind];
			ky += delta[ind];
		}
kx = kx/((float)nx*(float)ny*(float)nz);
ky = ky/((float)nx*(float)ny*(float)nz);
printf("Mean = %f and Sigma = %f\n", ky, sqrt(kx - ky*ky));

/*************************/
/*   Finding the halos   */
/*************************/
if(out_halos != 0){
printf("\nFinding the spherical halos!\n");

/*Alloc the flag array*/
flag = (int *)malloc((size_t)nx*(size_t)ny*(size_t)nz*sizeof(int));
check_memory(flag, "flag")

/*Initialize the flag array*/
for(i=0;i<nx;i++)
	for(j=0;j<ny;j++)
		for(k=0;k<nz;k++){
			size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

			flag[ind] = -1;
		}

/*Counting the number of peaks*/
np = 0;
for(i=0;i<nx;i++)
	for(j=0;j<ny;j++)
		for(k=0;k<nz;k++){
			size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;
			den = delta[ind];

			if(den > delta[(size_t)(cysum(i, 1, nx)*ny + j)*(size_t)nz + (size_t)k] && den > delta[(size_t)(cysum(i, -1, nx)*ny + j)*(size_t)nz + (size_t)k] && den > delta[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] && den > delta[(size_t)(i*ny + cysum(j, -1, ny))*(size_t)nz + (size_t)k] && den > delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] && den > delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -1, nz)])
				np++;
		}

/*Alloc the array with the peaks and final halos*/
peaks = (PEAKS *)malloc(np*sizeof(PEAKS));
halos = (HALOS *)malloc(np*sizeof(HALOS));
cont = 0;

/*Save the position and density of each peak*/
for(i=0;i<nx;i++)
	for(j=0;j<ny;j++)
		for(k=0;k<nz;k++){
			size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;
			den = delta[ind];

			if(den > delta[(size_t)(cysum(i, 1, nx)*ny + j)*(size_t)nz + (size_t)k] && den > delta[(size_t)(cysum(i, -1, nx)*ny + j)*(size_t)nz + (size_t)k] && den > delta[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] && den > delta[(size_t)(i*ny + cysum(j, -1, ny))*(size_t)nz + (size_t)k] && den > delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] && den > delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -1, nz)]){
				peaks[cont].x[0] = i;
				peaks[cont].x[1] = j;
				peaks[cont].x[2] = k;
				peaks[cont].den = den;
				cont ++;
			}
		}

/*Check the new number of peaks and elements in the peaks array*/
if(cont != np){
	printf("The number of peaks does not match. %ld != %ld!\n", np, cont);
	exit(0);
}

/*Sort the peaks*/
quickSort_peaks(peaks, 0, np-1);

/*Grow the spherical halos around the density peaks*/
nh = 0;
printf("We have %ld peaks\n", np);
for(l=0;l<np;l++){

	/*If this peak is already in a halo jump to teh next one*/
	if(flag[(size_t)(peaks[l].x[0]*ny + peaks[l].x[1])*(size_t)nz + (size_t)peaks[l].x[2]] != -1)
		continue;

	/*Check if this peak is near to the slice used to construct the light cone*/
	if(out_halos == 2 || out_halos == 3){
		m = 1;
		for(i=-Nrep_x;i<=Nrep_x;i++)
			for(j=-Nrep_y;j<=Nrep_y;j++)
				for(k=-Nrep_z;k<=Nrep_z;k++){

					/*Compute the distance for this replic*/
					Pos[0] = (peaks[l].x[0] + 0.5)*Lc + Lx*i - Pobs[0];
					Pos[1] = (peaks[l].x[1] + 0.5)*Lc + Ly*j - Pobs[1];
					Pos[2] = (peaks[l].x[2] + 0.5)*Lc + Lz*k - Pobs[2];
					dist = 0.0;
					for(m=0;m<3;m++)
						dist += Pos[m]*Pos[m];
					dist = sqrt(dist);

					if(dist <= dist_min - Rmax || dist > dist_max + Rmax)	m = 0;

					/*Compute the angle theta*/
					cost = 0.0;
					for(m=0;m<3;m++)
						cost += Pos[m]*LoS[m];
					cost = cost/dist;

					if(theta_min + Rmax/dist < M_PI && cost < cos(theta_min + Rmax/dist))	m = 0;
				}
		if(m == 0)
			continue;
	}

	den = peaks[l].den;
	den_tmp = peaks[l].den;
	cont = 0;
	cont_tmp = 1;
	grows_tmp = 0;

	/*Grows the shells up to the minimum of the barrier*/
	while(den_tmp >= Barrier(Sig_grid[Ncells - 1], dc, BARRIER, EB_a, EB_b, EB_alpha)){
		if(cont < cont_tmp)	grows = grows_tmp;
		grows_tmp ++;
		den = den_tmp;
		cont = cont_tmp;
		den_tmp = den*(float)cont;
		tmp = floor(sqrt((double) grows_tmp));
		if(tmp > nmin/2)	tmp = nmin/2;

		for(i=-tmp;i<=tmp;i++)
			for(j=-tmp;j<=tmp;j++)
				for(k=-tmp;k<=tmp;k++)
					if(dist2(i, j, k) == grows_tmp){
						size_t ind = (size_t)(cysum(peaks[l].x[0], i, nx)*ny + cysum(peaks[l].x[1], j, ny))*(size_t)nz + (size_t)cysum(peaks[l].x[2], k, nz);

						if(flag[ind] != -1)
							den_tmp += -Mtot;
						else
							den_tmp += delta[ind];
						cont_tmp ++;
					}

		den_tmp = den_tmp/(float)cont_tmp;
	}

	/*Decrease the shells up to the correct value of the barrier*/
	while(den < Barrier(Sig_grid[cont], dc, BARRIER, EB_a, EB_b, EB_alpha) && cont > 0){
		den_tmp = den;
		cont_tmp = cont;
		den = den*(float)cont;
		tmp = floor(sqrt((double) grows));
		if(tmp > nmin/2)	tmp = nmin/2;

		for(i=-tmp;i<=tmp;i++)
			for(j=-tmp;j<=tmp;j++)
				for(k=-tmp;k<=tmp;k++)
					if(dist2(i, j, k) == grows){
						size_t ind = (size_t)(cysum(peaks[l].x[0], i, nx)*ny + cysum(peaks[l].x[1], j, ny))*(size_t)nz + (size_t)cysum(peaks[l].x[2], k, nz);

						den -= delta[ind];
						cont --;
					}

		if(cont > 0)	den = den/(float)cont;
		if(cont < cont_tmp)	grows_tmp = grows;

		grows --;
	}

	if(cont == 0)
		continue;

	/*Put the correct flags to the cells*/
	tmp = floor(sqrt((double) grows_tmp));
	for(i=-tmp;i<=tmp;i++)
		for(j=-tmp;j<=tmp;j++)
			for(k=-tmp;k<=tmp;k++)
				if(dist2(i, j, k) < grows_tmp){
					size_t ind = (size_t)(cysum(peaks[l].x[0], i, nx)*ny + cysum(peaks[l].x[1], j, ny))*(size_t)nz + (size_t)cysum(peaks[l].x[2], k, nz);

					if(flag[ind] != -1)
						printf("(1): This flag != -1! Flag = %d and the new one is %ld\n", flag[ind], nh);

					flag[ind] = nh;
				}

	/*Save the halo information*/
	if(cont >= Nmin){
		halos[nh].cont = cont;
		for(i=0;i<3;i++)
			halos[nh].x[i] = peaks[l].x[i];
		nh ++;
	}
	else{
		for(i=-tmp;i<=tmp;i++)
			for(j=-tmp;j<=tmp;j++)
				for(k=-tmp;k<=tmp;k++)
					if(dist2(i, j, k) < grows_tmp){
						size_t ind = (size_t)(cysum(peaks[l].x[0], i, nx)*ny + cysum(peaks[l].x[1], j, ny))*(size_t)nz + (size_t)cysum(peaks[l].x[2], k, nz);
						flag[ind] = -2;
					}
	}
}
free(peaks);
free(Sig_grid);

/*Find the possible number of particles in a halo
sphere = (int *)malloc(Ncells*sizeof(int));
m = 0;
for(l=0;l<10000;l++){
	if(l%100 == 0)
		printf("l = %ld\n", l);

	tmp = floor(sqrt((float) l));
	cont = 0;

	for(i=-tmp;i<=tmp;i++)
		for(j=-tmp;j<=tmp;j++)
			for(k=-tmp;k<=tmp;k++)
				if(dist2(i, j, k) == l)
					cont ++;

	if(cont > 0){
		if(m > 0)	sphere[m] = sphere[m-1] + cont;
		else		sphere[m] = cont;

		m ++;
	}
}

/*Save this information
den_grid = fopen("Spheres.dat", "wb");
	if (den_grid == NULL) {
		printf("Unable to open spheres.dat\n");
		exit(0);
	}

fwrite(&m, sizeof(int), 1, den_grid);
for(i=0;i<m;i++)
	fwrite(&sphere[i], sizeof(int), 1, den_grid);
fclose(den_grid);*/

/*Read the data with the number of cells in each sphere*/
den_grid = fopen("Spheres.dat", "rb");
if (den_grid == NULL) {
	printf("Unable to open spheres.dat\n");
	exit(0);
}

fread(&m, sizeof(int), 1, den_grid);
sphere = (int *)malloc(m*sizeof(int));
for(i=0;i<m;i++)
	fread(&sphere[i], sizeof(int), 1, den_grid);
fclose(den_grid);

printf("We have %ld halos\n", nh);
}
/********************************/
/*   Displacing the particles   */
/********************************/
printf("\nDisplacing the particles using 1LPT!\n");

/*Define the arrays to store the final position, velocity and mass of each halo*/
float **velh, **posh, *Massh;
if(out_halos != 0){
	gsl_rng *rng_ptr;
	rng_ptr = gsl_rng_alloc (gsl_rng_taus);
	gsl_rng_set(rng_ptr, seed);

    Massh = (float *)malloc(nh*sizeof(float));
	velh = (float **)malloc(nh*sizeof(float *));
	posh = (float **)malloc(nh*sizeof(float *));
	for(i=0;i<nh;i++){
		velh[i] = (float *)malloc(3*sizeof(float));
		posh[i] = (float *)malloc(3*sizeof(float));
		for(j=0;j<3;j++){
			posh[i][j] = 0.0;
			velh[i][j] = 0.0;
		}

        cont = Next_Count(sphere, Ncells, halos[i].cont);

		den_tmp = gsl_spline_eval(spline_I, halos[i].cont*Mcell, acc_I) + (gsl_spline_eval(spline_I, sphere[cont]*Mcell, acc_I) - gsl_spline_eval(spline_I, halos[i].cont*Mcell, acc_I))*gsl_rng_uniform(rng_ptr);
		Massh[i] = gsl_spline_eval(spline_InvI, den_tmp, acc_InvI);
	}
    free(sphere);
}

/*Read the displacement field*/
if(DISP_CAT == 0){
	/*Open the output file for the displacement field*/
	printf("Reading the displacement field\n");
	disp_cat = fopen(dispfile, "rb");

	if (disp_cat == NULL) {
		printf("Unable to open %s\n", dispfile);
		exit(0);
	}

	fread(&nx, sizeof(int), 1, disp_cat);
	fread(&ny, sizeof(int), 1, disp_cat);
	fread(&nz, sizeof(int), 1, disp_cat);
	fread(&Lc, sizeof(float), 1, disp_cat);

	/*Read the displacement and add to each halo*/
	for(i=0;i<nx;i++)
		for(j=0;j<ny;j++)
			for(k=0;k<nz;k++){
				size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

				if(DO_2LPT == 0){
					fread(&kx, sizeof(float), 1, disp_cat);
					fread(&ky, sizeof(float), 1, disp_cat);
					fread(&kz, sizeof(float), 1, disp_cat);

					if(out_halos != 0){
						tmp = flag[ind];
						if(tmp < 0)	continue;

						posh[tmp][0] += Growth*kx;
						posh[tmp][1] += Growth*ky;
						posh[tmp][2] += Growth*kz;

						velh[tmp][0] += Growth*pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*kx;
						velh[tmp][1] += Growth*pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*ky;
						velh[tmp][2] += Growth*pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*kz;
					}
				}
				else{
					fread(&kx, sizeof(float), 1, disp_cat);
					fread(&ky, sizeof(float), 1, disp_cat);
					fread(&kz, sizeof(float), 1, disp_cat);

					fread(&factx, sizeof(float), 1, disp_cat);
					fread(&facty, sizeof(float), 1, disp_cat);
					fread(&factz, sizeof(float), 1, disp_cat);

					if(out_halos != 0){

						tmp = flag[ind];
						if(tmp < 0)	continue;

						posh[tmp][0] += Growth*(kx - Growth*3.0/7.0*pow(Omz, -1.0/143)*factx);
						posh[tmp][1] += Growth*(ky - Growth*3.0/7.0*pow(Omz, -1.0/143)*facty);
						posh[tmp][2] += Growth*(kz - Growth*3.0/7.0*pow(Omz, -1.0/143)*factz);

						velh[tmp][0] += Growth*(pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*kx); //- Growth*3.0/7.0*pow(Omz, -1.0/143)*2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*factx);
						velh[tmp][1] += Growth*(pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*ky); //- Growth*3.0/7.0*pow(Omz, -1.0/143)*2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*facty);
						velh[tmp][2] += Growth*(pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*kz); //- Growth*3.0/7.0*pow(Omz, -1.0/143)*2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*factz);
					}
				}
			}
	fclose(disp_cat);
}

/*Compute the displacement field*/
if(DISP_CAT == 1){

	/*Define the arrays with the displacement field used in 2LPT*/
	float *S1, *S2, *S3;
	if(DO_2LPT == 1){
		S1 = (float *)malloc((size_t)nx*(size_t)ny*(size_t)nz*sizeof(float));
		S2 = (float *)malloc((size_t)nx*(size_t)ny*(size_t)nz*sizeof(float));
		S3 = (float *)malloc((size_t)nx*(size_t)ny*(size_t)nz*sizeof(float));
	}

	/*Alloc deltak*/
	if(DEN_GRID == 0){
		deltak = (fftwf_complex *) fftwf_malloc((size_t)nx*(size_t)ny*(size_t)nz2*sizeof(fftwf_complex));
		check_memory(deltak, "deltak")
	}

	/*Redefine the FFTW3 plan to compute the displacements*/
	fftwf_destroy_plan(p1);
	p1 = NULL;

	p1 = fftwf_plan_dft_c2r_3d(nx, ny, nz, deltak, delta, FFTW_ESTIMATE);

	/*Divide the fourier space density by the green's function*/
	#pragma omp parallel for private(i, j, k, kx, ky, kz, factx, facty, factz, fact)
	for(i=0;i<nx;i++){
		if(2*i<nx) kx = i*klx;
		else kx = (i-nx)*klx;

		factx = 1.0/90.0*(2.0*cos(3.0*kx*Lc) - 27.0*cos(2.0*kx*Lc) + 270.0*cos(kx*Lc) - 245.0)/(Lc*Lc);

		for(j=0;j<ny;j++){
			if(2*j<ny) ky = j*kly;
			else ky = (j-ny)*kly;

			facty = 1.0/90.0*(2.0*cos(3.0*ky*Lc) - 27.0*cos(2.0*ky*Lc) + 270.0*cos(ky*Lc) - 245.0)/(Lc*Lc);

			for(k=0;k<nz2;k++){
				kz = k*klz;
				if(k == nz/2) kz = -(float)nz/2.0*klz;

				factz = 1.0/90.0*(2.0*cos(3.0*kz*Lc) - 27.0*cos(2.0*kz*Lc) + 270.0*cos(kz*Lc) - 245.0)/(Lc*Lc);

				size_t ind = (size_t)(i*ny + j)*(size_t)nz2 + (size_t)k;
				if(kx != 0.0 || ky != 0.0 || kz != 0.0){
					fact = factx + facty + factz;

					deltak[ind][0] = deltak[ind][0]/fact;
					deltak[ind][1] = deltak[ind][1]/fact;
				}
				else{
					deltak[ind][0] = 0.0;
					deltak[ind][1] = 0.0;
				}
			}
		}
	}

	/*Compute the potential at first order*/
	fftwf_execute(p1);

	/*Compute the first order displacements and update the position and velocity of each halo*/
	if(DO_2LPT == 1){
	#pragma omp parallel for private(i, j, k, tmp)
	for(i=0;i<nx;i++)
		for(j=0;j<ny;j++)
			for(k=0;k<nz;k++){
				size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

				S1[ind] = -(1.0*delta[(size_t)(cysum(i, 3, nx)*ny + j)*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(cysum(i, 2, nx)*ny + j)*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(cysum(i, 1, nx)*ny + j)*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(cysum(i, -1, nx)*ny + j)*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(cysum(i, -2, nx)*ny + j)*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(cysum(i, -3, nx)*ny + j)*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
				S2[ind] = -(1.0*delta[(size_t)(i*ny + cysum(j, 3, ny))*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(i*ny + cysum(j, 2, ny))*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(i*nx + cysum(j, -1, ny))*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(i*ny + cysum(j, -2, ny))*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(i*ny + cysum(j, -3, ny))*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
				S3[ind] = -(1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 3, nz)] - 9.0*delta[(size_t)(i*ny + j)*nz + (size_t)cysum(k, 2, nz)] + 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] - 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + cysum(k, -1, nz)] + 9.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -2, nz)] - 1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -3, nz)])*Normx/(60.0*Lc);

				if(out_halos != 0){
					tmp = flag[ind];
					if(tmp < 0)	continue;

					posh[tmp][0] += S1[ind];
					posh[tmp][1] += S2[ind];
					posh[tmp][2] += S3[ind];

					velh[tmp][0] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*S1[ind];
					velh[tmp][1] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*S2[ind];
					velh[tmp][2] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*S3[ind];
				}
			}
	}

	else{
	/*Open the output file for the displacement field*/
	if(out_inter == 2 || out_inter == 3){
		printf("Saving the displaced particles\n");
		disp_cat = fopen(dispfile, "wb");

		if (disp_cat == NULL) {
			printf("Unable to open %s\n", dispfile);
			exit(0);
		}

		fwrite(&nx, sizeof(int), 1, disp_cat);
		fwrite(&ny, sizeof(int), 1, disp_cat);
		fwrite(&nz, sizeof(int), 1, disp_cat);
		fwrite(&Lc, sizeof(float), 1, disp_cat);
	}

	#pragma omp parallel for private(i, j, k, tmp, kx, ky, kz)
	for(i=0;i<nx;i++)
		for(j=0;j<ny;j++)
			for(k=0;k<nz;k++){
				size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

				/*save the displacement field*/
				if(out_inter == 2 || out_inter == 3){

					kx = -(1.0*delta[(size_t)(cysum(i, 3, nx)*ny + j)*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(cysum(i, 2, nx)*ny + j)*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(cysum(i, 1, nx)*ny + j)*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(cysum(i, -1, nx)*ny + j)*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(cysum(i, -2, nx)*ny + j)*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(cysum(i, -3, nx)*ny + j)*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
					ky = -(1.0*delta[(size_t)(i*ny + cysum(j, 3, ny))*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(i*ny + cysum(j, 2, ny))*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(i*nx + cysum(j, -1, ny))*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(i*ny + cysum(j, -2, ny))*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(i*ny + cysum(j, -3, ny))*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
					kz = -(1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 3, nz)] - 9.0*delta[(size_t)(i*ny + j)*nz + (size_t)cysum(k, 2, nz)] + 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] - 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + cysum(k, -1, nz)] + 9.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -2, nz)] - 1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -3, nz)])*Normx/(60.0*Lc);

					fwrite(&kx, sizeof(float), 1, disp_cat);
					fwrite(&ky, sizeof(float), 1, disp_cat);
					fwrite(&kz, sizeof(float), 1, disp_cat);

					if(out_halos != 0){
						tmp = flag[ind];
						if(tmp < 0)	continue;

						posh[tmp][0] += kx;
						posh[tmp][1] += ky;
						posh[tmp][2] += kz;

						velh[tmp][0] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*kx;
						velh[tmp][1] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*ky;
						velh[tmp][2] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*kz;
					}
				}

				/*Do not save the displacements*/
				else if(out_halos != 0){
					tmp = flag[ind];
					if(tmp < 0)	continue;

					kx = -(1.0*delta[(size_t)(cysum(i, 3, nx)*ny + j)*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(cysum(i, 2, nx)*ny + j)*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(cysum(i, 1, nx)*ny + j)*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(cysum(i, -1, nx)*ny + j)*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(cysum(i, -2, nx)*ny + j)*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(cysum(i, -3, nx)*ny + j)*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
					ky = -(1.0*delta[(size_t)(i*ny + cysum(j, 3, ny))*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(i*ny + cysum(j, 2, ny))*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(i*nx + cysum(j, -1, ny))*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(i*ny + cysum(j, -2, ny))*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(i*ny + cysum(j, -3, ny))*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
					kz = -(1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 3, nz)] - 9.0*delta[(size_t)(i*ny + j)*nz + (size_t)cysum(k, 2, nz)] + 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] - 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + cysum(k, -1, nz)] + 9.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -2, nz)] - 1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -3, nz)])*Normx/(60.0*Lc);

					posh[tmp][0] += kx;
					posh[tmp][1] += ky;
					posh[tmp][2] += kz;

					velh[tmp][0] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*kx;
					velh[tmp][1] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*ky;
					velh[tmp][2] += pow(Omz, 5.0/9.0)*Hz/(1.0 + redshift)*kz;
				}
			}
	if(out_inter == 2 || out_inter == 3)
		fclose(disp_cat);
	}

	if(DO_2LPT == 1){
	printf("Displacing the particles using 2LPT!\n");

	/*Evaluating the second order contribution*/
	p2 = fftwf_plan_dft_r2c_3d(nx, ny, nz, delta, deltak, FFTW_ESTIMATE);

	/*Compute the second order "density"*/
	#pragma omp parallel for private(i, j, k)
	for(i=0;i<nx;i++)
	       	for(j=0;j<ny;j++)
			for(k=0;k<nz;k++){
				phixx = (1.0*S1[(size_t)(cysum(i, 3, nx)*ny + j)*(size_t)nz + (size_t)k] - 9.0*S1[(size_t)(cysum(i, 2, nx)*ny + j)*(size_t)nz + (size_t)k] + 45.0*S1[(size_t)(cysum(i, 1, nx)*ny + j)*(size_t)nz + (size_t)k] - 45.0*S1[(size_t)(cysum(i, -1, nx)*ny + j)*(size_t)nz + (size_t)k] + 9.0*S1[(size_t)(cysum(i, -2, nx)*ny + j)*(size_t)nz + (size_t)k] - 1.0*S1[(size_t)(cysum(i, -3, nx)*ny + j)*(size_t)nz + (size_t)k])/(60.0*Lc);
				phixy = (1.0*S1[(size_t)(i*ny + cysum(j, 3, ny))*(size_t)nz + (size_t)k] - 9.0*S1[(size_t)(i*ny + cysum(j, 2, ny))*(size_t)nz + (size_t)k] + 45.0*S1[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] - 45.0*S1[(size_t)(i*nx + cysum(j, -1, ny))*(size_t)nz + (size_t)k] + 9.0*S1[(size_t)(i*ny + cysum(j, -2, ny))*(size_t)nz + (size_t)k] - 1.0*S1[(size_t)(i*ny + cysum(j, -3, ny))*(size_t)nz + (size_t)k])/(60.0*Lc);
				phixz = (1.0*S1[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 3, nz)] - 9.0*S1[(size_t)(i*ny + j)*nz + (size_t)cysum(k, 2, nz)] + 45.0*S1[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] - 45.0*S1[(size_t)(i*ny + j)*(size_t)nz + cysum(k, -1, nz)] + 9.0*S1[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -2, nz)] - 1.0*S1[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -3, nz)])/(60.0*Lc);
				phiyy = (1.0*S2[(size_t)(i*ny + cysum(j, 3, ny))*(size_t)nz + (size_t)k] - 9.0*S2[(size_t)(i*ny + cysum(j, 2, ny))*(size_t)nz + (size_t)k] + 45.0*S2[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] - 45.0*S2[(size_t)(i*nx + cysum(j, -1, ny))*(size_t)nz + (size_t)k] + 9.0*S2[(size_t)(i*ny + cysum(j, -2, ny))*(size_t)nz + (size_t)k] - 1.0*S2[(size_t)(i*ny + cysum(j, -3, ny))*(size_t)nz + (size_t)k])/(60.0*Lc);
				phiyz = (1.0*S2[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 3, nz)] - 9.0*S2[(size_t)(i*ny + j)*nz + (size_t)cysum(k, 2, nz)] + 45.0*S2[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] - 45.0*S2[(size_t)(i*ny + j)*(size_t)nz + cysum(k, -1, nz)] + 9.0*S2[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -2, nz)] - 1.0*S2[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -3, nz)])/(60.0*Lc);
				phizz = (1.0*S3[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 3, nz)] - 9.0*S3[(size_t)(i*ny + j)*nz + (size_t)cysum(k, 2, nz)] + 45.0*S3[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] - 45.0*S3[(size_t)(i*ny + j)*(size_t)nz + cysum(k, -1, nz)] + 9.0*S3[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -2, nz)] - 1.0*S3[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -3, nz)])/(60.0*Lc);

				delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)k] = 1.0*(phixx*phiyy + phixx*phizz + phiyy*phizz - pow(phixy, 2.0) - pow(phixz, 2.0) - pow(phiyz, 2.0));
			}

	/*Go to fourier space to solve the posson equation*/
	fftwf_execute(p2);

	/*Divide the fourier space density by the green's function*/
	#pragma omp parallel for private(i, j, k, kx, ky, kz, fact, factx, facty, factz)
	for(i=0;i<nx;i++){
		if(2*i<nx) kx = i*klx;
		else kx = (i-nx)*klx;

		factx = 1.0/90.0*(2.0*cos(3.0*kx*Lc) - 27.0*cos(2.0*kx*Lc) + 270.0*cos(kx*Lc) - 245.0)/(Lc*Lc);

		for(j=0;j<ny;j++){
			if(2*j<ny) ky = j*kly;
			else ky = (j-ny)*kly;

			facty = 1.0/90.0*(2.0*cos(3.0*ky*Lc) - 27.0*cos(2.0*ky*Lc) + 270.0*cos(ky*Lc) - 245.0)/(Lc*Lc);

			for(k=0;k<nz2;k++){
				kz = k*klz;
				if(k == nz/2) kz = -(float)nz/2.0*klz;

				factz = 1.0/90.0*(2.0*cos(3.0*kz*Lc) - 27.0*cos(2.0*kz*Lc) + 270.0*cos(kz*Lc) - 245.0)/(Lc*Lc);

				size_t ind = (size_t)(i*ny + j)*(size_t)nz2 + (size_t)k;
				if(kx != 0.0 || ky != 0.0 || kz != 0.0){
					fact = factx + facty + factz;

					deltak[ind][0] = deltak[ind][0]/fact*Normk;
					deltak[ind][1] = deltak[ind][1]/fact*Normk;
				}
				else{
					deltak[ind][0] = 0.0;
					deltak[ind][1] = 0.0;
				}
			}
		}
	}

	/*Come back to real space*/
	fftwf_execute(p1);

	/*Open the output file for the displacement field*/
	if(out_inter == 2 || out_inter == 3){
		printf("Saving the displaced particles\n");
		disp_cat = fopen(dispfile, "wb");

		if (disp_cat == NULL) {
			printf("Unable to open %s\n", dispfile);
			exit(0);
		}

		fwrite(&nx, sizeof(int), 1, disp_cat);
		fwrite(&ny, sizeof(int), 1, disp_cat);
		fwrite(&nz, sizeof(int), 1, disp_cat);
		fwrite(&Lc, sizeof(float), 1, disp_cat);
	}

	/*Compute the second order displacements and velocities*/
	#pragma omp parallel for private(i, j, k, kx, ky, kz, tmp)
	for(i=0;i<nx;i++)
		for(j=0;j<ny;j++)
			for(k=0;k<nz;k++){
				size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

				/*save the displacement field*/
				if(out_inter == 2 || out_inter == 3){

					kx = (1.0*delta[(size_t)(cysum(i, 3, nx)*ny + j)*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(cysum(i, 2, nx)*ny + j)*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(cysum(i, 1, nx)*ny + j)*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(cysum(i, -1, nx)*ny + j)*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(cysum(i, -2, nx)*ny + j)*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(cysum(i, -3, nx)*ny + j)*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
					ky = (1.0*delta[(size_t)(i*ny + cysum(j, 3, ny))*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(i*ny + cysum(j, 2, ny))*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(i*nx + cysum(j, -1, ny))*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(i*ny + cysum(j, -2, ny))*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(i*ny + cysum(j, -3, ny))*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
					kz = (1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 3, nz)] - 9.0*delta[(size_t)(i*ny + j)*nz + (size_t)cysum(k, 2, nz)] + 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] - 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + cysum(k, -1, nz)] + 9.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -2, nz)] - 1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -3, nz)])*Normx/(60.0*Lc);

					fwrite(&S1[ind], sizeof(float), 1, disp_cat);
					fwrite(&S2[ind], sizeof(float), 1, disp_cat);
					fwrite(&S3[ind], sizeof(float), 1, disp_cat);

					fwrite(&kx, sizeof(float), 1, disp_cat);
					fwrite(&ky, sizeof(float), 1, disp_cat);
					fwrite(&kz, sizeof(float), 1, disp_cat);

					if(out_halos != 0){
						tmp = flag[ind];
						if(tmp < 0)	continue;

						kx = -3.0/7.0*pow(Omz, -1.0/143)*kx;
						ky = -3.0/7.0*pow(Omz, -1.0/143)*ky;
						kz = -3.0/7.0*pow(Omz, -1.0/143)*kz;

						posh[tmp][0] += kx;
						posh[tmp][1] += ky;
						posh[tmp][2] += kz;

						//velh[tmp][0] += 2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*kx;
						//velh[tmp][1] += 2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*ky;
						//velh[tmp][2] += 2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*kz;
					}
				}

				/*Do not save the displacements*/
				else if(out_halos != 0){
					tmp = flag[ind];
					if(tmp < 0)	continue;

					kx = (1.0*delta[(size_t)(cysum(i, 3, nx)*ny + j)*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(cysum(i, 2, nx)*ny + j)*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(cysum(i, 1, nx)*ny + j)*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(cysum(i, -1, nx)*ny + j)*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(cysum(i, -2, nx)*ny + j)*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(cysum(i, -3, nx)*ny + j)*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
					ky = (1.0*delta[(size_t)(i*ny + cysum(j, 3, ny))*(size_t)nz + (size_t)k] - 9.0*delta[(size_t)(i*ny + cysum(j, 2, ny))*(size_t)nz + (size_t)k] + 45.0*delta[(size_t)(i*ny + cysum(j, 1, ny))*(size_t)nz + (size_t)k] - 45.0*delta[(size_t)(i*nx + cysum(j, -1, ny))*(size_t)nz + (size_t)k] + 9.0*delta[(size_t)(i*ny + cysum(j, -2, ny))*(size_t)nz + (size_t)k] - 1.0*delta[(size_t)(i*ny + cysum(j, -3, ny))*(size_t)nz + (size_t)k])*Normx/(60.0*Lc);
					kz = (1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 3, nz)] - 9.0*delta[(size_t)(i*ny + j)*nz + (size_t)cysum(k, 2, nz)] + 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, 1, nz)] - 45.0*delta[(size_t)(i*ny + j)*(size_t)nz + cysum(k, -1, nz)] + 9.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -2, nz)] - 1.0*delta[(size_t)(i*ny + j)*(size_t)nz + (size_t)cysum(k, -3, nz)])*Normx/(60.0*Lc);

					kx = -3.0/7.0*pow(Omz, -1.0/143)*kx;
					ky = -3.0/7.0*pow(Omz, -1.0/143)*ky;
					kz = -3.0/7.0*pow(Omz, -1.0/143)*kz;

					posh[tmp][0] += kx;
					posh[tmp][1] += ky;
					posh[tmp][2] += kz;

					//velh[tmp][0] += 2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*kx;
					//velh[tmp][1] += 2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*ky;
					//velh[tmp][2] += 2.0*pow(Omz, 6.0/11.0)*Hz/(1.0 + redshift)*kz;
				}
			}

	if(out_inter == 2 || out_inter == 3)
		fclose(disp_cat);

	/*Free the FFTW memory*/
	fftwf_destroy_plan(p2);
	free(S1);
	free(S2);
	free(S3);
	}
	fftwf_destroy_plan(p1);
	fftwf_free(deltak);
}
fftwf_free(delta);
if(out_collapse == 0 && out_halos != 0)
	free(flag);

/*Compute the final position and velocity of the halos*/
if(out_halos != 0){
	for(i=0;i<nh;i++){
		posh[i][0] = cysumf(halos[i].x[0]*Lc + Lc/2.0, posh[i][0]/halos[i].cont, Lx);
		posh[i][1] = cysumf(halos[i].x[1]*Lc + Lc/2.0, posh[i][1]/halos[i].cont, Ly);
		posh[i][2] = cysumf(halos[i].x[2]*Lc + Lc/2.0, posh[i][2]/halos[i].cont, Lz);

		velh[i][0] = velh[i][0]/halos[i].cont;
		velh[i][1] = velh[i][1]/halos[i].cont;
		velh[i][2] = velh[i][2]/halos[i].cont;
	}
}

/*Saving the positions and velocities in real space*/
if(out_halos == 1){
	printf("Saving the halos\n");
	halo_cat = fopen(halofile, "w");

	if (halo_cat == NULL) {
		printf("Unable to open %s\n", halofile);
		exit(0);
	}

	fprintf(halo_cat, "%ld\n", nh);
	for(i=0;i<nh;i++){
		fprintf(halo_cat, "%f %f %f %f %f %f %e %d\n", posh[i][0], posh[i][1], posh[i][2], velh[i][0], velh[i][1], velh[i][2], Massh[i], halos[i].cont);
	}
	fclose(halo_cat);
}

/*Putting galaxies in the halos*/
if(DO_HOD == 1){
	printf("Saving the galaxies\n");
	sprintf(halofile, "%s_gals.dat", argv[14]);
	halo_cat = fopen(halofile, "w");

	if (halo_cat == NULL) {
		printf("Unable to open %s\n", halofile);
		exit(0);
	}

	cont = 0;
	for(i=0;i<nh;i++){
		/*Compute the number of central and satellite galaxies*/
		if(Ncentral(Massh[i], logMmin, siglogM) >= gsl_rng_uniform(rng_ptr))
			Ncen = 1;
		else
			Ncen = 0;
		Nsat = gsl_ran_poisson(rng_ptr, (double) Nsatellite(Massh[i], logM0, logM1, alpha));
		Ngals = Ncen + Nsat;
		if(Ngals == 0) continue;

		/*Save the central galaxy*/
		if(Ncen == 1){
			fprintf(halo_cat, "%f %f %f %f %f %f %d\n", posh[i][0], posh[i][1], posh[i][2], velh[i][0], velh[i][1], velh[i][2], i);
			cont ++;
		}

		/*Put the satellite galaxies following the NFW profile*/
		if(Nsat > 0){
			Rv = pow(3.0*Massh[i]/(4.0*M_PI*Dv*rhom), 1.0/3.0);
			C = f_c(Massh[i], (float) Mstar, z);
			A = log(1.0 + C) - C/(1.0 + C);
		}

		for(j=0;j<Nsat;j++){
			phi = 2.0*M_PI*gsl_rng_uniform(rng_ptr);
			theta = M_PI*gsl_rng_uniform(rng_ptr);

			r = Generate_NFW(Rv, C, A, seed);

			kx = cysumf(posh[i][0], r*sin(theta)*cos(phi), Lx);
			ky = cysumf(posh[i][1], r*sin(theta)*sin(phi), Ly);
			kz = cysumf(posh[i][2], r*cos(theta), Lz);
			fprintf(halo_cat, "%f %f %f ", kx, ky, kz);

			kx = velh[i][0];
			ky = velh[i][1];
			kz = velh[i][2];
			fprintf(halo_cat, "%f %f %f %d\n", kx, ky, kz, i);

			cont ++;
		}
	}
	fclose(halo_cat);

	n_bar = cont/(Lx*Ly*Lz);
	printf("n_bar = %f\n", n_bar);
}

/********************************/
/*Put the halos in the lightcone*/
/********************************/
if(out_halos == 2 || out_halos == 3){
printf("\nPutting the halos in the light cone!\n");

printf("The code is using (%d, %d, %d) replicas to construct the light cone.\n", Nrep_x, Nrep_y, Nrep_z);
printf("This snapshot is in the range %f - %f [Mpc/h] with theta_min = %f.\n", dist_min, dist_max, theta_min);

/*Open the light cone file*/
light_cat = fopen(lightfile, "wb");

if (light_cat == NULL) {
	printf("Unable to open %s\n", lightfile);
	exit(0);
}
cont = 0;
fwrite(&cont, sizeof(long), 1, light_cat);

/*Run over all the halos and save then in the light cone file*/
for(l=0;l<nh;l++){
	for(i=-Nrep_x;i<=Nrep_x;i++)
		for(j=-Nrep_y;j<=Nrep_y;j++)
			for(k=-Nrep_z;k<=Nrep_z;k++){

				/*Compute the distance for this replic*/
				Pos[0] = posh[l][0] + Lx*i - Pobs[0];
				Pos[1] = posh[l][1] + Ly*j - Pobs[1];
				Pos[2] = posh[l][2] + Lz*k - Pobs[2];
				dist = 0.0;
				for(m=0;m<3;m++)
					dist += Pos[m]*Pos[m];
				dist = sqrt(dist);

				if(out_halos == 3){
					/*Compute the distance in redshift space*/
					vr = 0.0;
					for(m=0;m<3;m++)
						vr += velh[l][m]*Pos[m];
					vr = vr/dist;

					for(m=0;m<3;m++)
						Pos[m] = Pos[m] + vr/Hz*(1.0 + redshift)*Pos[m]/dist;
					dist = dist + vr/Hz*(1.0 + redshift);
				}

				if(dist <= dist_min || dist > dist_max)	continue;

				/*Compute the angle theta*/
				cost = 0.0;
				for(m=0;m<3;m++)
					cost += Pos[m]*LoS[m];
				cost = cost/dist;

				if(cost < cos_min)	continue;

				/*Save the information about this halo*/
				fwrite(&Pos[0], sizeof(float), 1, light_cat);
				fwrite(&Pos[1], sizeof(float), 1, light_cat);
				fwrite(&Pos[2], sizeof(float), 1, light_cat);
				fwrite(&velh[l][0], sizeof(float), 1, light_cat);
				fwrite(&velh[l][1], sizeof(float), 1, light_cat);
				fwrite(&velh[l][2], sizeof(float), 1, light_cat);
				fwrite(&Massh[l], sizeof(float), 1, light_cat);
				cont ++;

				/*Put galaxies in this halo (one type)*/
				if(DO_HOD == 1){
					/*Compute the number of central and satellite galaxies*/
					if(Ncentral(Massh[l], logMmin, siglogM) >= gsl_rng_uniform(rng_ptr))
						Ncen = 1;
					else
						Ncen = 0;
					Nsat = gsl_ran_poisson(rng_ptr, (double) Nsatellite(Massh[l], logM0, logM1, alpha));
					Ngals = Ncen + Nsat;

					/*Save the total number of galaxies*/
					fwrite(&Ngals, sizeof(int), 1, light_cat);

					/*Save the central galaxy*/
					fwrite(&Pos[0], sizeof(float), 1, light_cat);
					fwrite(&Pos[1], sizeof(float), 1, light_cat);
					fwrite(&Pos[2], sizeof(float), 1, light_cat);
					fwrite(&velh[l][0], sizeof(float), 1, light_cat);
					fwrite(&velh[l][1], sizeof(float), 1, light_cat);
					fwrite(&velh[l][2], sizeof(float), 1, light_cat);

					/*Put the satellite galaxies following the NFW profile*/
					if(Nsat > 0){
						Rv = pow(3.0*Massh[l]/(4.0*M_PI*Dv*rhom), 1.0/3.0);
						C = f_c(Massh[l], (float) Mstar, z);
						A = log(1.0 + C) - C/(1.0 + C);
					}

					for(m=0;m<Nsat;m++){
						phi = 2.0*M_PI*gsl_rng_uniform(rng_ptr);
						theta = M_PI*gsl_rng_uniform(rng_ptr);

						r = Generate_NFW(Rv, C, A, seed);

						kx = cysumf(Pos[0], r*sin(theta)*cos(phi), Lx);
						ky = cysumf(Pos[1], r*sin(theta)*sin(phi), Ly);
						kz = cysumf(Pos[2], r*cos(theta), Lz);
						fwrite(&kx, sizeof(float), 1, light_cat);
						fwrite(&ky, sizeof(float), 1, light_cat);
						fwrite(&kz, sizeof(float), 1, light_cat);

						kx = velh[i][0];
						ky = velh[i][1];
						kz = velh[i][2];
						fwrite(&kx, sizeof(float), 1, light_cat);
						fwrite(&ky, sizeof(float), 1, light_cat);
						fwrite(&kz, sizeof(float), 1, light_cat);
					}
				}
			}
}
rewind(light_cat);
fwrite(&cont, sizeof(long), 1, light_cat);
fclose(light_cat);

if(out_collapse == 1){

/*Open the file to save the information about the collapsed particles*/
collapse_cat = fopen(collapsefile, "wb");

if (collapse_cat == NULL) {
	printf("Unable to open %s\n", collapsefile);
	exit(0);
}

/*Save the information about the colapsed particles*/
int a, b, c;

cont = 0;
fwrite(&cont, sizeof(long), 1, collapse_cat);

#pragma omp parallel for private(i, j, k, a, b, c, Pos, dist, cost, tmp)
for(i=0;i<nx;i++)
	for(j=0;j<ny;j++)
		for(k=0;k<nz;k++){
			size_t ind = (size_t)(i*ny + j)*(size_t)nz + (size_t)k;

			for(a=-Nrep_x;a<=Nrep_x;a++)
				for(b=-Nrep_y;b<=Nrep_y;b++)
					for(c=-Nrep_z;c<=Nrep_z;c++){

						/*Compute the distance for this replic*/
						Pos[0] = i*Lc + Lc/2.0 + Lx*a;
						Pos[1] = j*Lc + Lc/2.0 + Ly*b;
						Pos[2] = k*Lc + Lc/2.0 + Lz*c;
						dist = 0.0;
						for(m=0;m<3;m++)
							dist += Pos[m]*Pos[m];
						dist = sqrt(dist);

						if(dist <= dist_min || dist > dist_max)	continue;

						/*Compute the angle theta*/
						cost = 0.0;
						for(m=0;m<3;m++)
							cost += Pos[m]*LoS[m];
						cost = cost/dist;

						if(cost < cos_min)	continue;

						tmp = flag[ind];
						cont ++;

						fwrite(&ind, sizeof(size_t), 1, collapse_cat);
						fwrite(&tmp, sizeof(int), 1, collapse_cat);
						fwrite(&redshift, sizeof(float), 1, collapse_cat);
					}
		}
rewind(collapse_cat);
fwrite(&cont, sizeof(long), 1, collapse_cat);
fclose(collapse_cat);

free(flag);
}
}

/*******************/
/* Free the memory */
/*******************/
gsl_spline_free(spline_I);
gsl_spline_free(spline_InvI);
gsl_interp_accel_free(acc_I);
gsl_interp_accel_free(acc_InvI);

if(out_halos != 0){
	free(Massh);
	for(i=0;i<nh;i++){
		free(velh[i]);
		free(posh[i]);
	}

	/*Free the rest*/
	free(velh);
	free(posh);
	free(halos);
	gsl_rng_free(rng_ptr);
}

return 0;
}


#include <stdlib.h>            // malloc
#include <stdio.h>             // printf
#include <math.h>              // fabs, sqrt, etc.
#include <time.h>              // time
#include <unistd.h>            // getpid
#include <gsl/gsl_rng.h>       // GNU Scientific Library
#include <gsl/gsl_cdf.h>       // GNU Scientific Library
#include <gsl/gsl_randist.h>   // GNU Scientific Library

double dvonmises(double* x, double mu, double nu,
               double kappa1, double kappa2, double lambda) {
  double phi = x[0];
  double psi = x[1];
  double kernal = exp( kappa1*cos(phi - mu) + kappa2*cos(psi-nu) +
  lambda*sin(phi-mu)*sin(psi-nu));
  return kernal;
}

void rvonmises2(int* ptrn, double* ptrmu, double* ptrnu,
              double* ptrkappa1, double* ptrkappa2, double* ptrlambda,
              double* phiout, double* psiout)
{
  // Assign the variables
  int n = *ptrn;
  double mu = *ptrmu;
  double nu = *ptrnu;
  double kappa1 = *ptrkappa1;
  double kappa2 = *ptrkappa2;
  double lambda = *ptrlambda;
  double modeh;
  // If Unimodal...
   if(lambda*lambda<(kappa1*kappa2)) {
     double x[] = {mu, nu};
     modeh = dvonmises(x,mu,nu,kappa1,kappa2,lambda);
   }
   //If Bimodal...
   if(lambda*lambda>(kappa1*kappa2)) {
     double psi =
     acos(kappa2/fabs(lambda)*sqrt((lambda*lambda+kappa1*kappa1)/(lambda*lambda+kappa2*kappa2)));
     double phi =
     acos(kappa1/fabs(lambda)*sqrt((lambda*lambda+kappa2*kappa2)/(lambda*lambda+kappa1*kappa1)));
     //Caclulate the 4 possible maximum if lambda >0
     if(lambda>0.0){
       double x1[] = {mu+psi , nu+phi};
       double x2[] = {mu-psi , nu-phi};
       double mode1 = dvonmises(x1,mu,nu,kappa1,kappa2,lambda);
       double mode2 = dvonmises(x2,mu,nu,kappa1,kappa2,lambda);
       modeh = fmax(mode1,mode2);
     }
     if(lambda<0.0){
       double x1[] = {mu-phi , nu+psi};
       double x2[] = {mu+psi , nu-psi};
       double mode1 = dvonmises(x1,mu,nu,kappa1,kappa2,lambda);
       double mode2 = dvonmises(x2,mu,nu,kappa1,kappa2,lambda);
       modeh = fmax(mode1,mode2);
     }
   }
  //implement the rejection sampler now that we can use the mode to get alpha
  double pi = acos(-1);
  double height_of_sample_density = 1/(4*pi*pi);
  double alpha = height_of_sample_density/modeh;
  //printf("%s","mode height\n");
  //printf("%f\n",modeh);

  gsl_rng* r = gsl_rng_alloc (gsl_rng_taus);
  int seed = time(NULL)*getpid();
  gsl_rng_set(r,seed);

  int i = 0;
  while( i < n){
    //sample from the sample envelope: the uniform
    double two_uniforms[0];
    //evaluate the density at those uniforms
    two_uniforms[0] = gsl_ran_flat(r, -pi, pi);
    two_uniforms[1] = gsl_ran_flat(r, -pi, pi);
    double f_x = dvonmises(two_uniforms,mu,nu,kappa1,kappa2,lambda);
    double e_x = height_of_sample_density/alpha;

    // Test if U <= f_x/e_x and store the uniforms if they work
    double U = gsl_ran_flat(r,0,1);
    if( U <= f_x/e_x){
      phiout[i] = two_uniforms[0];
      psiout[i] = two_uniforms[1];
      i +=1;
    }
  }
}

int main(int argc, char* argv[]) {
  if ( argc != 7 ) {
    fprintf(stderr,"usage: %s nreps mu nu kappa1 kappa2 lambda\n",argv[0]);

    return 1;
  }
  if (argv[7]=0){
    fprintf(stderr,"lambda cannot be 0");
    return 1;
  }
  // Simulation parameters
  int n = atoi(argv[1]);
  double mu = atof(argv[2]);
  double nu = atof(argv[3]);
  double kappa1 = atof(argv[4]);
  double kappa2 = atof(argv[5]);
  double lambda = atof(argv[6]);
//because our rvonmises funciton is void, we must allocate memor to psi and phi
//so that we can get results and call them later
  double* phiout = (double*) malloc(n*sizeof(double));
  double* psiout = (double*) malloc(n*sizeof(double));
  rvonmises2(&n,&mu,&nu,&kappa1,&kappa2,&lambda,phiout,psiout);
  printf("%s %s\n","phi","psi");
  for(int i = 0; i < n; i++){
    printf("%f %f\n",phiout[i],psiout[i]);
  }
  free(phiout);
  free(psiout);
  return 0;//to get it to work, comment this out
}


#include <stdlib.h>
#include <math.h>

#include <fftw3.h>

#include <gsl/gsl_sf_result.h>
#include <gsl/gsl_sf_gamma.h>

#include "fftlog.h"

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

/* This code is FFTLog, which is described in arXiv:astro-ph/9905191 */

static double_complex lngamma_fftlog(double_complex z)
{
  gsl_sf_result lnr, phi;
  gsl_sf_lngamma_complex_e(creal(z), cimag(z), &lnr, &phi);
#ifdef _COMPLEX_STRUCT
  double_complex res = {lnr.val, phi.val};
  return res;
#else
  return lnr.val + I*phi.val;
#endif
}

static double_complex gamma_fftlog(double_complex z)
{
  return cexp(lngamma_fftlog(z));
}

static double_complex polar (double r, double phi)
{
#ifdef _COMPLEX_STRUCT
  double_complex res = {r*cos(phi) , r*sin(phi)};
  return res;
#else
  return (r*cos(phi) +I*(r*sin(phi)));
#endif
}

static void lngamma_4(double x, double y, double* lnr, double* arg)
{
#ifdef _COMPLEX_STRUCT
  double_complex z = {x,y};
  double_complex w = lngamma_fftlog(z);
#else
  double complex w = lngamma_fftlog(x+y*I);
#endif
  if(lnr) *lnr = creal(w);
  if(arg) *arg = cimag(w);
}

static double goodkr(int N, double mu, double q, double L, double kr)
{
  double xp = (mu+1+q)/2;
  double xm = (mu+1-q)/2;
  double y = M_PI*N/(2*L);
  double lnr, argm, argp;
  lngamma_4(xp, y, &lnr, &argp);
  lngamma_4(xm, y, &lnr, &argm);
  double arg = log(2/kr) * N/L + (argp + argm)/M_PI;
  double iarg = round(arg);
  if(arg != iarg)
    kr *= exp((arg - iarg)*L/N);
  return kr;
}

void compute_u_coefficients(int N, double mu, double q, double L, double kcrc, double_complex u[])
{
  double y = M_PI/L;
  double k0r0 = kcrc * exp(-L);
  double t = -2*y*log(k0r0/2);

  if(q == 0) {
    double x = (mu+1)/2;
    double lnr, phi;
    for(int m = 0; m <= N/2; m++) {
      lngamma_4(x, m*y, &lnr, &phi);
      u[m] = polar(1.0,m*t + 2*phi);
    }
  }
  else {
    double xp = (mu+1+q)/2;
    double xm = (mu+1-q)/2;
    double lnrp, phip, lnrm, phim;
    for(int m = 0; m <= N/2; m++) {
      lngamma_4(xp, m*y, &lnrp, &phip);
      lngamma_4(xm, m*y, &lnrm, &phim);
      u[m] = polar(exp(q*log(2) + lnrp - lnrm), m*t + phip - phim);
    }
  }

  for(int m = N/2+1; m < N; m++)
    u[m] = conj(u[N-m]);
  if((N % 2) == 0) {
#ifdef _COMPLEX_STRUCT
    double_complex z = {creal(u[N/2]) , 0.0};
    u[N/2] =  z;
#else
    u[N/2] = (creal(u[N/2]) + I*0.0);
#endif
   }
}

void fht(int N, const double r[], const double_complex a[], double k[], double_complex b[], double mu,
         double q, double kcrc, int noring, double_complex* u)
{
  double L = log(r[N-1]/r[0]) * N/(N-1.);
  double_complex* ulocal = NULL;
  if(u == NULL) {
    if(noring)
      kcrc = goodkr(N, mu, q, L, kcrc);
    ulocal = malloc (sizeof(double_complex)*N);
    compute_u_coefficients(N, mu, q, L, kcrc, ulocal);
    u = ulocal;
  }

  /* Compute the convolution b = a*u using FFTs */
  fftw_plan forward_plan = fftw_plan_dft_1d(N, (fftw_complex*) a, (fftw_complex*) b,  -1, FFTW_ESTIMATE);
  fftw_plan reverse_plan = fftw_plan_dft_1d(N, (fftw_complex*) b, (fftw_complex*) b, +1, FFTW_ESTIMATE);
  fftw_execute(forward_plan);
  for(int m = 0; m < N; m++)
#ifdef _COMPLEX_STRUCT
  { double_complex z = _Cmulcc(b[m],u[m]);
    z = _Cmulcr(z, 1/(double)(N));
    b[m] = z;
  }
#else
    b[m] *= u[m] / (double)(N);       // divide by N since FFTW doesn't normalize the inverse FFT
#endif
  fftw_execute(reverse_plan);
  fftw_destroy_plan(forward_plan);
  fftw_destroy_plan(reverse_plan);

  /* Reverse b array */
  double_complex tmp;
  for(int n = 0; n < N/2; n++) {
    tmp = b[n];
    b[n] = b[N-n-1];
    b[N-n-1] = tmp;
  }

  /* Compute k's corresponding to input r's */
  double k0r0 = kcrc * exp(-L);
  k[0] = k0r0/r[0];
  for(int n = 1; n < N; n++)
    k[n] = k[0] * exp(n*L/N);

  free(ulocal);
}

void fftlog_ComputeXi2D(double bessel_order,int N,const double l[],const double cl[],
			double th[], double xi[])
{
  double_complex* a = malloc(sizeof(double_complex)*N);
  double_complex* b = malloc(sizeof(double_complex)*N);

  for(int i=0;i<N;i++){
#ifdef _COMPLEX_STRUCT
    double_complex z =  {l[i]*cl[i],0.0};
    a[i]=z;
#else
	a[i]=l[i]*cl[i];
#endif
    }
  fht(N,l,a,th,b,bessel_order,0,1,1,NULL);
  for(int i=0;i<N;i++)
    xi[i]=creal(b[i])/(2*M_PI*th[i]);

  free(a);
  free(b);
}

void fftlog_ComputeXiLM(double l, double m, int N, const double k[], const double pk[],
			double r[], double xi[])
{
  double_complex* a = malloc(sizeof(double_complex)*N);
  double_complex* b = malloc(sizeof(double_complex)*N);

  for(int i = 0; i < N; i++){
#ifdef _COMPLEX_STRUCT
    double_complex z= { pow(k[i], m - 0.5) * pk[i],0};
    a[i] = z;
#else
	a[i] = pow(k[i], m - 0.5) * pk[i];
#endif
  }
  fht(N, k, a, r, b, l + 0.5, 0, 1, 1, NULL);
  for(int i = 0; i < N; i++)
    xi[i] = pow(2*M_PI*r[i], -(m-0.5)) * creal(b[i]);

  free(a);
  free(b);
}

void pk2xi(int N, const double k[], const double pk[], double r[], double xi[])
{
  fftlog_ComputeXiLM(0, 2, N, k, pk, r, xi);
}

void xi2pk(int N, const double r[], const double xi[], double k[], double pk[])
{
  static const double TwoPiCubed = 8*M_PI*M_PI*M_PI;
  fftlog_ComputeXiLM(0, 2, N, r, xi, k, pk);
  for(int j = 0; j < N; j++)
    pk[j] *= TwoPiCubed;
}