#include <R.h>
#include <Rmath.h>
#include <string.h>
#include <omp.h>
#include "lossdistrib.h"

#include <cblas.h>
#include <lapacke.h>

#ifdef BUILD_TESTS
#include <gsl/gsl_rng.h>
#endif

#define MIN(x, y) (((x) < (y)) ? (x) : (y))

void GHquad(const int* n, double* Z, double* w) {
    // Setup for eigenvalue computations
    char JOBZ   = 'v'; // Compute eigenvalues & vectors

    // Initialize array for eigenvectors
    double * V      = malloc(sizeof(double)*(*n)*(*n));

    for(int i = 0; i<(*n)-1; i++){
      w[i] = sqrt((i+1.)/2);
    }

    // Run eigen decomposition
    int INFO = LAPACKE_dstev(LAPACK_COL_MAJOR, JOBZ, *n, Z, w, V, *n);

    for(int i = 0; i<(*n); i++) {
      w[i] = V[i*(*n)] * V[i*(*n)];
      Z[i] *= sqrt(2);
    }

    // Deallocate temporary arrays
    free(V);
}

void lossdistrib(const double *p, const int *np, const double *w,
                 const double *S, const int *N, const int* T,
                 const int *defaultflag, double *q) {
    /* recursive algorithm with first order correction for computing
       the loss distribution.
       p vector of default probabilities
       np length of p
       w issuer weights
       S vector of severities (should be same length as p)
       N number of ticks in the grid
       T where we truncate the distribution.
       defaultflag if true compute the default distribution
       q the loss distribution */
    openblas_set_num_threads(1);
    int d1, d2, bound;
    double d, p1, p2, pbar;
    double *qtemp = calloc(*T, sizeof(double));

    double lu = 1./(*N-1);
    q[0] = 1;
    int M = 1;
    for(int i=0; i<(*np); i++){
        d = (*defaultflag)? w[i]/lu : S[i] * w[i]/ lu;
        d1 = floor(d);
        d2 = ceil(d);
        p1 = p[i] * (d2-d);
        p2 = p[i] - p1;
        memcpy(qtemp, q, MIN(M, *T) * sizeof(double));
        pbar = 1-p[i];
        bound = MIN(M, *T);
#ifdef USE_BLAS
        cblas_dscal(bound, pbar, q, 1);
#else
        for(int j=0; j < bound; j++){
            q[j] *= pbar;
        }
#endif
        bound = MIN(M, *T-d2);
#ifdef USE_BLAS
        cblas_daxpy(bound, p1, qtemp, 1, q+d1, 1);
        cblas_daxpy(bound, p2, qtemp, 1, q+d2, 1);
#else
        for(int j=0; j < bound; j++){
            q[d1+j] += p1 * qtemp[j];
            q[d2+j] += p2 * qtemp[j];
        }
#endif
        M += d2;
    }

    /* correction for weight loss */
    if(M > *N && *T==*N){
        double sum = 0;
        for(int j=0; j < MIN(M, *N); j++) {
            sum += q[j];
        }
        q[*N-1] += 1-sum;
    }
    free(qtemp);
}

void lossdistrib_Z(const double *p, const int *np, const double *w, const double *S, const int *N,
                   const int *defaultflag, const double *rho, const double *Z, const int *nZ, double *q) {
    double* pshocked = malloc(sizeof(double) * (*np) * (*nZ));

    #pragma omp parallel for
    for(int i = 0; i < *nZ; i++){
        for(int j = 0; j < *np; j++){
            pshocked[j + (*np) * i] = shockprob(p[j], rho[j], Z[i], 0);
        }
        lossdistrib(pshocked + (*np) * i, np, w, S + (*np) * i, N, N,
                    defaultflag, q + (*N) * i);
    }
    free(pshocked);
}

static inline void posK(int T, double K, double lu, double* val) {
    for(int i = 0; i < T; i++) {
        val[i] = K-lu*i;
    }
}

void exp_trunc(const double *p, const int *np, const double *w, const double *S,
               const int *N, const double *K, double *r) {

    double lu = 1./(*N+1);
    int T = (int) floor((*K) * (*N))+1;
    const int flag = 0;
    double* qtemp = calloc( T, sizeof(double));
    lossdistrib(p, np, w, S, N, &T, &flag, qtemp);
    double* val = malloc(T * sizeof(double));
    posK(T, *K, lu, val);
    *r = cblas_ddot(T, val, 1, qtemp, 1);
    free(qtemp);
}

void lossdistrib_joint(const double *p, const double* pp, const int *np,
                       const double *w, const double *S, const int *N,
                       const int *defaultflag, double *q) {
    /* recursive algorithm with first order correction
       computes jointly the loss and recovery distribution
       p vector of default probabilities
       np length of p
       w vector of issuer weights (length np)
       S vector of severities (should be same length as p)
       N number of ticks in the grid
       defaultflag if true computes the default distribution
       q the joint probability distribution */

    int i, j1, j2, bound;
    double x, y1, y2;
    double alpha1, alpha2, beta1, beta2;
    double w1, w2, w3, w4;
    double ppw1, ppw2, ppw3;

    double lu = 1./(*N-1);
    double pbar;
#ifndef USE_BLAS
    double temp;
#endif
    double* begin;
    double* qtemp = calloc( (*N) * (*N), sizeof(double));
    q[0] = 1;

    int Mx = 1, My = 1;
    for(int k = 0; k < (*np); k++) {
        y1 = (1-S[k]) * w[k] / lu;
        y2 = w[k]/lu;
        x = (*defaultflag)? y2 : y2 - y1;
        i = floor(x);
        j1 = floor(y1);
        j2 = floor(y2);
        alpha1 = i + 1 - x;
        alpha2 = 1 - alpha1;
        beta1 = j1 + 1 - y1;
        beta2 = 1 - beta1;
        w1 = alpha1 * beta1 * p[k];
        w2 = alpha1 * beta2 * p[k];
        w3 = alpha2 * beta2 * p[k];
        w4 = alpha2 * beta1 * p[k];

        bound = MIN(Mx, *N);
        pbar = 1-p[k];
        if(pp) {
            pbar -= pp[k];
            if(defaultflag) {
                ppw1 = alpha1 * alpha1 * pp[k];
                ppw2 = alpha1 * alpha2 * pp[k];
                ppw3 = alpha2 * alpha2 * pp[k];
            } else {
                ppw1 = pp[k] * (j2+1-y2);
                ppw2 = pp[k] * (y2-j2);
            }
        }

        for(int n = 0; n < MIN(My, *N); n++) {
            memcpy(qtemp+n*(*N), q+n*(*N), MIN(Mx, *N) * sizeof(double));
#ifdef USE_BLAS
            cblas_dscal(bound, pbar, q+(*N)*n, 1);
#else
            for(int m=0; m < bound; m++) {
                q[m+(*N)*n] *= pbar;
            }
#endif
        }
        bound = MIN(Mx, *N-i-1);
        for(int n=0; n < MIN(My, *N-j2-1); n++) {
            begin = qtemp+(*N)*n;
#ifdef USE_BLAS
            cblas_daxpy(bound, w1, begin, 1, q+i+(*N)*(j1+n), 1);
            cblas_daxpy(bound, w2, begin, 1, q+i+(*N)*(j1+1+n), 1);
            cblas_daxpy(bound, w3, begin, 1, q+i+1+(*N)*(j1+1+n), 1);
            cblas_daxpy(bound, w4, begin, 1, q+i+1+(*N)*(j1+n), 1);

            if(pp) {
                if(defaultflag) {
                    cblas_daxpy(bound, ppw1, begin, 1, q+i+(*N)*(j2+n), 1);
                    cblas_daxpy(bound, ppw2, begin, 1, q+i+(*N)*(j2+1+n), 1);
                    cblas_daxpy(bound, ppw3, begin, 1, q+i+1+(*N)*(j2+1+n), 1);
                    cblas_daxpy(bound, ppw2, begin, 1, q+i+1+(*N)*(j2+n), 1);
                } else {
                    cblas_daxpy(bound, ppw1, begin, 1, q+(*N)*(j2+n), 1);
                    cblas_daxpy(bound, ppw2, begin, 1, q+(*N)*(j2+1+n), 1);
                }
            }
#else
            for(int m = 0; m < bound; m++) {
                temp = *(begin + m);
                q[i+m+(*N)*(j1+n)] += w1 * temp;
                q[i+m+(*N)*(j1+1+n)] += w2 * temp;
                q[i+1+m+(*N)*(j1+1+n)] += w3 * temp;
                q[i+1+m+(*N)*(j1+n)] += w4 * temp;
                if(pp) {
                    if(defaultflag) {
                        q[i+m+(*N)*(j2+n)] += ppw1 * temp;
                        q[i+1+m+(*N)*(j2+n)] += ppw2 * temp;
                        q[i+m+(*N)*(j2+1+n)] += ppw2 * temp;
                        q[i+1+m+(*N)*(j2+1+n)] += ppw3 * temp;
                    } else{
                        q[m+(*N)*(j2+n)] +=  ppw1 * temp;
                        q[m+(*N)*(j2+1+n)] +=  ppw2 * temp;
                    }
                }
            }
#endif
        }
        Mx += i+1;
        if(pp) {
            My += j2+1;
        } else {
            My += j1+1;
        }
    }
    /* correction for weight loss */
    if(Mx>*N || My>*N){
        double sum = 0;
        for(int m=0; m < MIN(Mx, *N); m++){
            for(int n=0; n < MIN(My, *N); n++){
                sum += q[m+n*(*N)];
            }
        }
        q[MIN(*N, Mx)*MIN(My,*N)-1] += 1 - sum;
    }
    free(qtemp);
}

void joint_default_averagerecov_distrib(const double *p,const int *np,
                                        const double *S, const int *N,
                                        double *q) {
    /* recursive algorithm with first order correction
       computes jointly the loss and recovery distribution
       p vector of default probabilities
       np length of p
       S vector of severities (should be same length as p)
       N number of ticks in the grid
       q the joint probability distribution */

    double lu = 1./(*N-1);
    q[0] = 1;

    double* newrecov = malloc(*N * sizeof(double));
    unsigned int* index = malloc(*N * sizeof(unsigned int));
    double* weights = malloc(*N * sizeof(double));
    q[0] = 1;
    double temp;
    for(int i = 0; i < (*np); i++) {
        for(int j = i + 1; j > 0; j--) {
            for(int n = 0; n < *N; n++) {
                newrecov[n] = ( (j-1) * n * lu + (1-S[i])) / (double)j;
                weights[n] = modf(newrecov[n] * (*N-1), &temp);
                index[n] = (unsigned int) temp;
            }
            if(j < i+1) {
                cblas_dscal(*N, 1-p[i], q+(*N)*j, 1);
            }
            for(int k = 0; k < *N; k++) {
                q[j*(*N)+index[k]+1] += weights[k] * p[i] * q[(j-1)*(*N)+k];
                q[j*(*N)+index[k]] += (1-weights[k]) * p[i] * q[(j-1)*(*N)+k];
            }
        }
        cblas_dscal(*N, 1-p[i], q, 1);
    }
    free(newrecov);
    free(index);
}


void recovdist(const double *dp, const double *pp, const int *n, const double *w,
               const double *S, const int *N, double *q) {
    /* recursive algorithm with first order correction for computing
       the recovery distribution in case of prepayment.
       dp vector of default probabilities
       pp vector of prepay probabilities
       n length of p
       S vector of severities (should be same length as p)
       w vector of weights
       N number of ticks in the grid
       q the loss distribution */

    int d1l, d1u, d2l, d2u;
    double d1, d2, dp1, dp2, pp1, pp2;

    double lu = 1./(*N - 1);
    double* qtemp = calloc( (*N), sizeof(double));
    q[0] = 1;
    int M = 1;
    for(int i = 0; i < (*n); i++){
        d1 = w[i] * (1-S[i]) /lu;
        d2 = w[i]/lu;
        d1l = floor(d1);
        d1u = d1l + 1;
        d2l = floor(d2);
        d2u = d2l + 1;
        dp1 = dp[i] * (d1u - d1);
        dp2 = dp[i] - dp1;
        pp1 = pp[i] * (d2u - d2);
        pp2 = pp[i] - pp1;
        memcpy(qtemp, q, MIN(M, *N) * sizeof(double));
        for(int j = 0; j< MIN(M, *N); j++){
            q[j] = (1-dp[i]-pp[i]) * q[j];
        }
        for(int j=0; j < MIN(M, *N-d2u); j++){
            q[d1l+j] += dp1 * qtemp[j];
            q[d1u+j] += dp2 * qtemp[j];
            q[d2l+j] += pp1 * qtemp[j];
            q[d2u+j] += pp2 * qtemp[j];
        };
        M += d2u;
    }
    /* correction for weight loss */
    if(M > *N){
        double sum = 0;
        for(int j=0; j<MIN(M, *N); j++){
            sum += q[j];
        }
        q[*N-1] += 1-sum;
    }
    free(qtemp);
}

double shockprob(double p, double rho, double Z, int give_log){
    if(rho==1){
        return((double)(Z<=qnorm(p, 0, 1, 1, 0)));
    }else{
        return( pnorm( (qnorm(p, 0, 1, 1, 0) - sqrt(rho) * Z)/sqrt(1 - rho), 0, 1, 1, give_log));
    }
}

double dqnorm(double x){
    return 1/dnorm(qnorm(x, 0, 1, 1, 0), 0, 1, 0);
}

double dshockprob(double p, double rho, double Z){
    return( dnorm((qnorm(p, 0, 1, 1, 0) - sqrt(rho) * Z)/sqrt(1-rho), 0, 1, 0) * dqnorm(p)/sqrt(1-rho) );
}

void shockprobvec2(const double p, const double rho, const double* Z, const int nZ, double *q){
    /* return a two column vectors with shockprob in the first column
       and dshockprob in the second column*/
    #pragma omp parallel for
    for(int i = 0; i < nZ; i++){
        q[i] = shockprob(p, rho, Z[i], 0);
        q[i + nZ] = dshockprob(p, rho, Z[i]);
    }
}

double shockseverity(double S, double Z, double rho, double p){
    if(p==0){
        return 0;
    }else{
        return( exp(shockprob(S * p, rho, Z, 1) - shockprob(p, rho, Z, 1)) );
    }
}

double quantile(const double* Z, const double* w, const int nZ, const double p0){
    double cumw;
    int i;
    cumw = 0;
    for(i=0; i<nZ; i++){
        cumw += w[i];
        if(cumw > p0){
            break;
        }
    }
    return( Z[i] );
}

void fitprob(const double* Z, const double* w, const int* nZ, const double* rho,
             const double* p0, double* result){
    double eps = 1e-12;
    double *q = malloc( 2 * (*nZ) * sizeof(double));
    double dp, p, phi;

    if(*p0 == 0){
        *result = 0;
    }else if(*rho == 1){
        *result = pnorm(quantile(Z, w, *nZ, *p0), 0, 1, 1, 0);
    }else{
        shockprobvec2(*p0, *rho, Z, *nZ, q);
        dp = (cblas_ddot(*nZ, q, 1, w, 1) - *p0)/cblas_ddot(*nZ, q + *nZ, 1, w, 1);
        p = *p0;
        while(fabs(dp) > eps){
            phi = 1;
            while( (p - phi * dp) < 0 || (p - phi * dp) > 1){
                phi *= 0.8;
            }
            p -= phi * dp;
            shockprobvec2(p, *rho, Z, *nZ, q);
            dp = (cblas_ddot(*nZ, q, 1, w, 1) - *p0)/cblas_ddot(*nZ, q + *nZ, 1, w, 1);
        }
        *result = p;
    }
    free(q);
}

void stochasticrecov(double* R, double* Rtilde, double* Z, double* w, int* nZ,
                     double* rho, double* porig, double* pmod, double* q){
    double ptemp, ptilde;

    if(*porig==0){
        for(int i = 0; i < *nZ; i++){
            q[i] = *R;
        }
    }else{
        ptemp = (1 - *R) / (1 - *Rtilde) * *porig;
        fitprob(Z, w, nZ, rho, &ptemp, &ptilde);
        #pragma omp parallel for
        for(int i = 0; i < *nZ; i++){
            q[i] = fabs(1 - (1 - *Rtilde) * exp( shockprob(ptilde, *rho, Z[i], 1) -
                                                 shockprob(*pmod, *rho, Z[i], 1)));
        }
    }
}

void lossdistrib_joint_Z(const double *dp, const double *pp, const int *ndp,
                         const double *w, const double *S, const int *N,
                         const int *defaultflag, const double *rho,
                         const double *Z, const double *wZ, const int *nZ,
                         double *q) {

    int N2 = (*N) * (*N);
    double* qmat = calloc(N2 * (*nZ), sizeof(double));

    const double alpha = 1;
    const double beta = 0;
    #pragma omp parallel for
    for(int i = 0; i < *nZ; i++){
        double* dpshocked = malloc(sizeof(double) * (*ndp));
        double* ppshocked = NULL;
        if(pp) {
            ppshocked = malloc(sizeof(double) * (*ndp));
        }
        for(int j = 0; j < *ndp; j++){
            dpshocked[j] = shockprob(dp[j], rho[j], Z[i], 0);
            if(pp) {
                ppshocked[j] = shockprob(pp[j], rho[j], -Z[i], 0);
            }
        }

        lossdistrib_joint(dpshocked, ppshocked, ndp, w, S + (*ndp) * i,
                          N, defaultflag, qmat + N2 * i);
        free(dpshocked);
        if(pp) {
            free(ppshocked);
        }
    }

    cblas_dgemv(CblasColMajor, CblasNoTrans, N2, *nZ, alpha, qmat, N2, wZ, 1, beta, q, 1);

    free(qmat);
}

void BCloss_recov_dist(const double *defaultprob, const int *dim1, const int *dim2,
                       const double *issuerweights, const double *recov,
                       const double *Z, const double *w, const int *n,
                       const double *rho, const int *N, const int *defaultflag,
                       double *L, double *R) {
    /*
      computes the loss and recovery distribution over time with a flat gaussian
      correlation
      inputs:
      defaultprob: matrix of size dim1 x dim2. dim1 is the number of issuers
      and dim2 number of time steps
      issuerweights: vector of issuer weights (length dim1)
      recov: vector of recoveries (length dim1)
      Z: vector of factor values (length n)
      w: vector of factor weights (length n)
      rho: correlation beta vector (length dim1)
      N: number of ticks in the grid
      defaultflag: if true, computes the default distribution
      outputs:
      L: matrix of size (N, dim2)
      R: matrix of size (N, dim2)
    */
    double *Lw = malloc((*N) * (*n) * sizeof(double));
    double *Rw = malloc((*N) * (*n) * sizeof(double));
    const double alpha = 1;
    const double beta = 0;

    for(int t = 0; t < (*dim2); t++) {
        memset(Lw, 0, (*N) * (*n) * sizeof(double));
        memset(Rw, 0, (*N) * (*n) * sizeof(double));
        #pragma omp parallel for
        for(int i = 0; i < *n; i++) {
            double* gshocked = malloc((*dim1) * sizeof(double));
            double* Sshocked = malloc((*dim1) * sizeof(double));
            double* Rshocked = malloc((*dim1) * sizeof(double));
            for(int j=0; j < (*dim1); j++) {
                double g = defaultprob[j + (*dim1) * t];
                gshocked[j] = shockprob(g, rho[j], Z[i], 0);
                Sshocked[j] = shockseverity(1-recov[j], Z[i], rho[j], g);
                Rshocked[j] = 1 - Sshocked[j];
            }
            lossdistrib(gshocked, dim1, issuerweights, Sshocked, N, N, defaultflag,
                        Lw + i * (*N));
            lossdistrib(gshocked, dim1, issuerweights, Rshocked , N, N, defaultflag,
                        Rw + i * (*N));
            free(gshocked);
            free(Sshocked);
            free(Rshocked);
        }
        cblas_dgemv(CblasColMajor, CblasNoTrans, *N, *n, alpha, Lw, *N, w, 1, beta, L + t * (*N), 1);
        cblas_dgemv(CblasColMajor, CblasNoTrans, *N, *n, alpha, Rw, *N, w, 1, beta, R + t * (*N), 1);
    }
    free(Lw);
    free(Rw);
}

void BCloss_recov_trunc(const double *defaultprob, const int *dim1, const int *dim2,
                        const double *issuerweights, const double *recov,
                        const double *Z, const double *w, const int *n,
                        const double *rho, const int *N, const double * K,
                        const int *defaultflag,
                        double *ELt, double *ERt) {
    /*
      computes EL_t = E[(K-L_t)^+] and ER_t = E[(K-(1-R_t))^+]
       the the put options on loss and recovery over time
       with a flat gaussian correlation.
      inputs:
      defaultprob: matrix of size dim1 x dim2. dim1 is the number of issuers
      and dim2 number of time steps
      issuerweights: vector of issuer weights (length dim1)
      recov: vector of recoveries (length dim1)
      Z: vector of factor values (length n)
      w: vector of factor weights (length n)
      rho: correlation beta vector (length dim1)
      N: number of ticks in the grid
      K: the strike
      defaultflag: if true, computes the default distribution
      outputs:
      ELt: vector of length dim2
      ERt: vector of length dim2
    */
    int T = (int) floor((*K) * (*N))+1;
    double lu = 1./(*N+1);
    double* valL = malloc( T * sizeof(double));
    posK(T, *K, lu, valL);
    double* EL = malloc( (*n) * sizeof(double));
    double* ER = malloc( (*n) * sizeof(double));
    double* Lw = malloc(T * (*n) * sizeof(double));
    const double alpha = 1;
    const double beta = 0;

    for(int t=0; t < (*dim2); t++) {
        memset(Lw, 0, T * (*n) * sizeof(double));
        #pragma omp parallel
        {
            double g, Ktilde;
            int Ttilde;
            #pragma omp for
            for(int i=0; i < *n; i++){
                double* Rw = NULL;
                double* Rshocked = malloc((*dim1) * sizeof(double));
                double* Sshocked = malloc((*dim1) * sizeof(double));
                double* gshocked = malloc((*dim1) * sizeof(double));
                double* gshockedbar = malloc((*dim1) * sizeof(double));
                double* valR = NULL;
                for(int j=0; j < (*dim1); j++){
                    g = defaultprob[j + (*dim1) * t];
                    gshocked[j] = shockprob(g, rho[j], Z[i], 0);
                    Sshocked[j] = shockseverity(1-recov[j], Z[i], rho[j], g);
                    gshockedbar[j] = 1 - gshocked[j];
                    Rshocked[j] = 1 - Sshocked[j];
                }

                lossdistrib(gshocked, dim1, issuerweights, Sshocked,
                            N, &T, defaultflag, Lw + i * T);
                ER[i] = 0;
                Ktilde = *K - cblas_ddot(*dim1, issuerweights, 1, Sshocked, 1);
                if(Ktilde > 0){
                    Ttilde = (int) floor(Ktilde * (*N))+1;
                    Rw = calloc(Ttilde, sizeof(double));
                    lossdistrib(gshockedbar, dim1, issuerweights, Rshocked,
                                N, &Ttilde, defaultflag, Rw);
                    valR = malloc(Ttilde * sizeof(double));
                    posK(Ttilde, Ktilde, lu, valR);
                    ER[i] = cblas_ddot(Ttilde, Rw, 1, valR, 1);
                }
                if(Rw) {
                    free(Rw);
                }
                if(valR) {
                    free(valR);
                }
                free(Rshocked);
                free(Sshocked);
                free(gshocked);
                free(gshockedbar);
            }
        }
        cblas_dgemv(CblasColMajor, CblasTrans, T, *n, alpha, Lw, T, valL, 1, beta, EL, 1);
        ELt[t] = cblas_ddot(*n, EL, 1, w, 1);
        ERt[t] = cblas_ddot(*n, ER, 1, w, 1);
    }
    free(Lw);
    free(EL);
    free(ER);
    free(valL);
}

#ifdef BUILD_TESTS
int main() {
    const int nZ = 500;
    double* Z = malloc(nZ * sizeof(double));
    double* w = malloc(nZ * sizeof(double));

    GHquad(&nZ, Z, w);

    const int n = 1000;
    double* p = malloc(n * sizeof(double));
    double* weights = malloc(n * sizeof(double));
    double* rho = malloc(n * sizeof(double));
    gsl_rng* gen = gsl_rng_alloc(gsl_rng_mt19937);
    for(int i = 0; i<n; i++) {
        weights[i] = 1./n;
        rho[i] = 0.45;
        p[i] = gsl_rng_uniform(gen);
    }

    double* S = malloc(n * nZ * sizeof(double));
    for(int i = 0; i<nZ*n; i++) {
        S[i] = gsl_rng_uniform(gen);
     }

    const int N = 301;
    double* q = calloc(N * nZ, sizeof(double));

    int flag = 0;
    lossdistrib_Z(p, &n, weights, S, &N, &flag, rho, Z, &nZ, q);
    double *qvec = malloc( N * sizeof(double));
    double alpha=1, beta=0;
    cblas_dgemv(CblasColMajor, CblasNoTrans, N, nZ, alpha, q, N, w, 1, beta, qvec, 1);
    int T = 25;
    p = realloc(p, n * T * sizeof(double));
    for(int i = 0; i<n*T; i++) {
        p[i] = gsl_rng_uniform(gen);
    }
    double* L = malloc(N * T * sizeof(double));
    double* R = malloc(N * T * sizeof(double));
    BCloss_recov_dist(p, &n, &T, weights, S, Z, w, &nZ, rho, &N, &flag, L, R);
    free(p);
    free(S);
    free(weights);
    free(Z);
    free(w);
    free(q);
    free(rho);
    free(L);
    free(R);
    free(qvec);
    gsl_rng_free(gen);
    return 0;
}
#endif