#ifndef __INTEGRAL_H__
#define __INTEGRAL_H__
#include "integral.h"
#endif

#ifndef __GSLEXTRA_H__
#define __GSLEXTRA_H__
#include "../include/gslextra.h"
#endif

#ifndef __GSLPRINT_H__
#define __GSLPRINT_H__
#include "../include/gslprint.h"
#endif

#include <gsl/gsl_eigen.h>

// calculate the attraction energy matrix element (Z integral)
double nuclear_attraction_energy_matrix_element(orbital * a, orbital * b, atomic_orbital * atom_HEAD);

// calculate the single electron hamiltonian matrix (core hamiltonian matrix) element
double single_electron_hamiltonian_matrix_element(orbital * a, orbital * b, atomic_orbital * atom_HEAD);

// calculate the fock matrix element
double fock_matrix_element(gsl_quad_tensor * v, gsl_matrix * density_matrix, gsl_matrix * h_matrix, int i, int j, int length);

// calculate the Hartree-Fock energy of the system
double HF_energy(gsl_quad_tensor * v, gsl_matrix * density_matrix, gsl_matrix * h_matrix, int length);

// calculate the kinetic energy matrix
void kinetic_energy_matrix(gsl_matrix * dest, orbital * HEAD, int length);

// calculate the attraction energy matrix (Z integrals)
void nuclear_attraction_energy_matrix(gsl_matrix * dest, orbital * HEAD, atomic_orbital * atom_HEAD, int length);

// calculate the single electron hamiltonian matrix (core hamiltonian matrix)
void core_hamiltonian_matrix(gsl_matrix * dest, orbital * HEAD, atomic_orbital * atom_HEAD, int length);

//obtain the quad tensor of the two-electron Coulomb integrals
void two_electron_quad_tensor(gsl_quad_tensor * dest, orbital * HEAD, int length);

//obtain the fock matrix
void fock_matrix(gsl_matrix * dest, gsl_quad_tensor * v, gsl_matrix * density_matrix, gsl_matrix * h_matrix, int length);

// perform initial guess of the coefficient matrix by performing diagonalization of core hamiltonian matrix
void initial_guess(gsl_matrix * dest, gsl_matrix * core_hamiltonian, gsl_matrix * S, int length);

// calculate density matrix from coefficient matrix and the number of electrons
void density_matrix(gsl_matrix * dest, gsl_matrix * coef, int el_num, int length);

//perform RHF as well as printing information
int RHF_SCF_print(double * tot_energy, gsl_vector * energy, gsl_matrix * coef, orbital * HEAD, atomic_orbital * atom_HEAD, int length, int el_num, int iteration_max, double errmax, int countmax, double alpha, int mixing_type, int SCF_INITIAL_FLAG, int SCF_FOCK_FLAG, int SCF_COEF_FLAG, int FOCK_FLAG);

//calculate the nuclei repulsion energy
double nuclei_repulsion(atomic_orbital * atomlist_HEAD);


/**
 * Simple and fast kmeans-implementation making use of multithreading.
 *
 * Author: Anders Bennehag
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 Anders Bennehag
 *
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <pthread.h>
#include <assert.h>
#include <gsl/gsl_rng.h>
#include "fastkmeans/fastkmeans.h"

typedef enum {
    SUCCESS=0,
    ALLOCATION_ERROR,
} fkm_error;

typedef enum {
    WAIT,
    RELEASED,
    EXIT,
} barrier_status;

typedef struct {
    const gsl_matrix* points;
    gsl_matrix* clusters;
    const size_t num_points;
    const size_t dim;
    const size_t k;
    unsigned* owner_of;
} targs;

typedef struct {
    pthread_mutex_t lock;
    pthread_cond_t cond;
    int count;
    int goal;
    barrier_status status;
} tbarrier;

typedef struct {
    targs* args;
    size_t thread_id;
    pthread_t pthread_id;
    size_t num_threads;
    tbarrier* bar_start;
    tbarrier* bar_finish;
} thread_meta;

typedef struct {
    thread_meta* threads;
    const size_t num_threads;
    tbarrier bar_start;
    tbarrier bar_finish;
} thread_pool;

int fkm_clusters_assignment(const thread_pool* tpool);
double fkm_clusters_update(const targs* args);
double fkm_matrix_diff(gsl_matrix* a, gsl_matrix* b);
int fkm_threads_init(thread_pool* tpool);
int fkm_threads_join(thread_pool* tpool);
int barrier_finish(tbarrier* bar);

int fkm_kmeans(const gsl_matrix* points, gsl_matrix* clusters,
               size_t max_iter, int num_threads) {

    if (num_threads<0) {
        printf("Invalid number of threads\n");
        return 1;
    }

    size_t num_points = points->size1;
    unsigned owner_of[num_points];
    targs args = {
        .points=points,
        .clusters=clusters,
        .num_points=num_points,
        .dim=points->size2,
        .k=clusters->size1,
        .owner_of=owner_of,
    };

    thread_meta tmeta[num_threads];
    thread_pool tpool = {
        .num_threads=num_threads,
        .threads=tmeta,
        .bar_start={
            .lock=PTHREAD_MUTEX_INITIALIZER,
            .cond=PTHREAD_COND_INITIALIZER,
            .goal=num_threads+1,
        },
        .bar_finish={
            .lock=PTHREAD_MUTEX_INITIALIZER,
            .cond=PTHREAD_COND_INITIALIZER,
            .goal=num_threads+1,
        },
    };
    for (int i=0; i<num_threads; ++i) {
        tmeta[i].args = &args;
        tmeta[i].pthread_id = 0;
        tmeta[i].thread_id = i;
        tmeta[i].num_threads = num_threads;
        tmeta[i].bar_start=&tpool.bar_start;
        tmeta[i].bar_finish=&tpool.bar_finish;
    }
    fkm_threads_init(&tpool);
    gsl_rng* rng = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(rng, time(NULL));

    // Randomize which center each point belongs to
    for (size_t p=0; p < args.num_points; ++p) {
        // TODO: Make sure every cluster gets assigned
        owner_of[p] = gsl_rng_uniform_int(rng, args.k);
        /*FKM_DEBUGFMT("Made %u owner of %lu", owner_of[p], p);*/
    }

    // Initiate clusters to random point in input
    for (size_t m=0; m<args.k; m++) {
        size_t rand_p = gsl_rng_uniform_int(rng, args.num_points);
        for (size_t j=0; j<args.dim; ++j) {
            double val = gsl_matrix_get(points,
                                        rand_p,
                                        j);
            gsl_matrix_set(clusters, m, j, val);
        }
    }

    for (size_t it=0; it<max_iter; ++it) {
        FKM_DEBUGFMT("iter %lu num_points %lu dim %lu, k %lu num_threads %i",
                it, args.num_points, args.dim, args.k, num_threads);
        /*for (size_t p=0; p<num_points;++p) {*/
            /*printf("%lu:%u ", p, owner_of[p]);*/
        /*}*/
        /*printf("\n");*/
        fkm_clusters_assignment(&tpool);
        double diff = fkm_clusters_update(&args);
        FKM_DEBUGFMT("Diff: %f", diff);
        if (fabs(diff) < 1e-10) {
            FKM_DEBUG("Break since no progress");
            break;
        }
    }
    barrier_finish(&tpool.bar_start);
    int err = fkm_threads_join(&tpool);
    return 0;
}

int barrier_wait(tbarrier* bar) {
    pthread_mutex_lock(&bar->lock);
    FKM_DEBUGFMT("barrier_wait: count:%i goal:%i status:%i", bar->count,
                                                      bar->goal,
                                                      bar->status);
    assert(bar->goal > 0);
    bar->count += 1;
    if (bar->goal == bar->count) {
        bar->status = RELEASED;
        bar->count = 0;
        FKM_DEBUG("Goal reached in barrier, broadcasting release");
        pthread_cond_broadcast(&bar->cond);
        pthread_mutex_unlock(&bar->lock);
        return 0;
    }
    pthread_mutex_unlock(&bar->lock);
    for (;;) {
        pthread_cond_wait(&bar->cond, &bar->lock);
        if (bar->status == RELEASED) {
            FKM_DEBUG("Barrier released");
            pthread_mutex_unlock(&bar->lock);
            return 0;
        } else if (bar->status == EXIT) {
            FKM_DEBUG("Barrier signals FINISHED");
            pthread_mutex_unlock(&bar->lock);
            return 1;
        } else {
            // Spurious wakeup
            FKM_DEBUG("Spurious wakeup");
        }
    }
}

int barrier_finish(tbarrier* bar) {
    int err = pthread_mutex_lock(&bar->lock);
    if (err) {
        FKM_ERROR("Could not acquire mutex");
        return 1;
    }
    bar->status = EXIT;
    err = pthread_mutex_unlock(&bar->lock);
    if (err) {
        FKM_ERROR("Could not release mutex");
        return 1;
    }
    err = pthread_cond_broadcast(&bar->cond);
    if (err) {
        FKM_ERROR("Could not broadcast cond-variable");
        return 1;
    }
    return 0;
}
int fkm_clusters_assignment_single(targs* args,
                                     size_t offset,
                                     size_t stride);

void* fkm_thread_manage(void* data) {
    thread_meta* tmeta = data;
    targs* args = tmeta->args;

    int stop=0;
    for (;;) {
        stop = barrier_wait(tmeta->bar_start);
        if (stop) {
            return 0;
        }
        fkm_clusters_assignment_single(args,
                                       tmeta->thread_id,
                                       tmeta->num_threads);
        stop = barrier_wait(tmeta->bar_finish);
        if (stop) {
            return 0;
        }
    }
}

int fkm_clusters_assignment_single(targs* args,
                                     size_t offset,
                                     size_t stride) {
    for (size_t p=offset; p<args->num_points; p+=stride) {
        double min_err = 1e308;
        for (size_t m=0; m<args->k; ++m) {
            double err = 0;
            for (size_t j=0; j<args->dim; ++j) {
                double a = fabs(gsl_matrix_get(args->clusters, m, j)
                                - gsl_matrix_get(args->points, p, j));
                err += a*a;
            }
            if (err < min_err) {
                min_err = err;
                args->owner_of[p] = m;
            }
        }
    }
    return 0;
}

int fkm_threads_init(thread_pool* tpool) {

    for (size_t thr=0; thr<tpool->num_threads;++thr) {
        thread_meta* tdata = &tpool->threads[thr];
        int err = pthread_create(&tdata->pthread_id,
                                 NULL,
                                 &fkm_thread_manage,
                                 tdata);
        if (err) {
            FKM_ERRORFMT("pthread_create-error %i", err);
            return 1;
        }
    }
    return 0;
}
int fkm_threads_join(thread_pool* tpool) {
    for (size_t thr=0; thr<tpool->num_threads;++thr) {
        thread_meta* tdata = &tpool->threads[thr];
        int err = pthread_join(tdata->pthread_id, NULL);
        if (err) {
            FKM_ERRORFMT("pthread_join-error %i", err);
        }
        return 1;
    }
    return 0;
}

int fkm_clusters_assignment(const thread_pool* tpool) {
    tbarrier* bar_start = tpool->threads[0].bar_start;
    FKM_DEBUGFMT("bar_start: count:%i goal:%i status:%i", bar_start->count,
                                                      bar_start->goal,
                                                      bar_start->status);
    int err = barrier_wait(bar_start);
    if (err) {
        FKM_ERROR("barrier_wait failed");
        return 1;
    }
    tbarrier* bar_finish = tpool->threads[0].bar_finish;
    FKM_DEBUGFMT("bar_finish: count:%i goal:%i status:%i", bar_finish->count,
                                                      bar_finish->goal,
                                                      bar_finish->status);
    err = barrier_wait(tpool->threads[0].bar_finish);
    if (err) {
        FKM_ERROR("barrier_wait failed");
        return 1;
    }
    return 0;
}

/**
 * Update clusters.
 *
 *
 */
double fkm_clusters_update(const targs* args) {
    /*printf("DEBUG: num_points %lu dim %lu, k %lu\n", num_points, dim, k);*/
    unsigned cluster_size[args->k];
    for (size_t m=0; m<args->k; ++m) {
        cluster_size[m] = 0;
    }
    gsl_matrix* mean_sums = gsl_matrix_calloc(args->k, args->dim);
    for (size_t p=0; p<args->num_points; ++p) {
        unsigned m = args->owner_of[p];
        assert(m < args->k);
        cluster_size[m] += 1;
        /*LOGFMT("p:%lu belongs to %u. Children %u", p, m, cluster_size[m]);*/
        for (size_t j=0; j<args->dim; ++j) {
            double val = gsl_matrix_get(mean_sums, m, j)
                + gsl_matrix_get(args->points, p, j);
            gsl_matrix_set(mean_sums, m, j, val);
        }
    }
    double diff = 0;
    for (size_t m=0; m<args->k; ++m) {
        /*printf("Updating cluster %lu to", m);*/
        for (size_t j=0; j<args->dim; ++j) {
            double val = gsl_matrix_get(mean_sums, m, j);
            val /= cluster_size[m];
            diff += val - gsl_matrix_get(args->clusters, m, j);
            gsl_matrix_set(args->clusters, m, j, val);
            /*printf(" %u %f", cluster_size[m], val);*/
        }
        /*printf("\n");*/
    }
    gsl_matrix_free(mean_sums);
    return diff;
}

typedef double (*elem_func)(double, double);

double fkm_matrix_elem_func(gsl_matrix* a, gsl_matrix* b, elem_func f) {
    double v = 0;
    for (size_t i=0; i<a->block->size; ++i) {
        v += f(a->block->data[i], b->block->data[i]);
    }
    return v;
}

double diff(double a, double b) {
    return fabs(a - b);
}

double fkm_matrix_diff(gsl_matrix* a, gsl_matrix* b) {
    return fkm_matrix_elem_func(a, b, diff);
}

void print_matrix(gsl_matrix* a) {
    for (size_t i=0; i<5 && i<a->block->size; ++i) {
        printf("%lu: %f\n", i, a->block->data[i]);
    }
}

gsl_matrix* fkm_matrix_load(FILE* fin) {
    size_t rows, cols;
    int err = fscanf(fin, "%lu %lu", &rows, &cols);
    if (err == EOF) {
        printf("ERROR: Could not parse matrix dimensions\n");
        return 0;
    }
    gsl_matrix* mat = gsl_matrix_alloc(rows, cols);
    err = gsl_matrix_fscanf(fin, mat);
    if (err) {
        printf("ERROR: Could not parse matrix data\n");
        return 0;
    }
    printf("Loaded %lu rows, %lu columns matrix\n", rows, cols);
    return mat;
}

int fkm_matrix_save(FILE* fout, gsl_matrix* mat) {
    int err = 0;
    fprintf(fout, "%zu %zu\n", mat->size1, mat->size2);
    for (size_t i=0; i<mat->block->size; ++i) {
        if (i && i % mat->size2 == 0) {
            err = fprintf(fout, "\n");
            if (err<0) {
                FKM_ERRORFMT("fprintf returned %i", err);
            }
        }
        int err = fprintf(fout, "%f ", mat->block->data[i]);
        if (err<0) {
            FKM_ERRORFMT("fprintf returned %i", err);
        }
    }
    return err;
}


#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include <Python.h>
#include <numpy/arrayobject.h>
#include <lapacke.h>

/* DGESVD prototype */
extern void LAPACK_dgesvd( char* jobu, char* jobvt, int* m, int* n, double* a,
                    int* lda, double* s, double* u, int* ldu, double* vt, int* ldvt,
                    double* work, int* lwork, int* info );

double** An(double *pi, double *x, int labs, int dims);
double** A(double **PI, double **X, int labs, int dims, int n_pool);
double** Fisher(double *pi, double *x, double sigma, int labs, int dims);

static char estVar_docstring[] =
    "Calculate the A and Fisher matrix.";

static PyObject *varRedu_estVar(PyObject *self, PyObject *args);

static PyMethodDef module_methods[] = {
    {"estVar", varRedu_estVar, METH_VARARGS, estVar_docstring},
    {NULL, NULL, 0, NULL}
};

static struct PyModuleDef moduledef = {
    PyModuleDef_HEAD_INIT,
    "_variance_reduction",  /* m_name */
    "This module provides calculate A and Fisher matrix using C.",  /* m_doc */
    -1,                  /* m_size */
    module_methods,      /* m_methods */
    NULL,                /* m_reload */
    NULL,                /* m_traverse */
    NULL,                /* m_clear */
    NULL,                /* m_free */
};

PyMODINIT_FUNC PyInit__variance_reduction(void){
    PyObject *m = PyModule_Create(&moduledef);
    if(m==NULL){
        return NULL;
    }

    /* Load 'numpy' */
    import_array();

    return m;
}

double* matrix_mul(double* a, double* b, int m1, int n1, int m2, int n2){
    double *ret = (double*) malloc(m1 * n2 * sizeof(double));
    if(n1 != m2){
        return NULL;
    }
    for(int i=0; i<m1; i++)
        for(int j=0; j<n2; j++){
            double temp = 0.0;
            for(int p=0; p<n1; p++)
                temp += a[i*n1 + p] * b[p*n2 + j];
            ret[i*n2 + j] = temp;
        }
    return ret;
}

void pinv(double** X, int labs, int dims){
    int m = labs*dims, n = labs*dims, lda = labs*dims, ldu = labs*dims,
        ldvt = labs*dims, lwork, info;
    double wkopt;
    double *work;
    double *s  = (double*) malloc(labs*dims * sizeof(double));
    double *u  = (double*) malloc(labs*dims * labs*dims * sizeof(double));
    double *vt = (double*) malloc(labs*dims * labs*dims * sizeof(double));
    double *a  = (double*) malloc(labs*dims * labs*dims * sizeof(double));

    for(int i=0; i<m; i++)
        for(int j=0; j<n; j++)
            a[i*labs*dims + j] = X[i][j];

     /* Query and allocate the optimal workspace */
    lwork = -1;
    LAPACK_dgesvd("All", "All", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork,
        &info);
    lwork = (int)wkopt;
    work = (double*)malloc( lwork*sizeof(double) );
    /* Compute SVD */
    LAPACK_dgesvd("All", "All", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork,
        &info);
    /* Check for convergence  */
    if(info > 0) {
        printf("The algorithm computing SVD failed to converge. %d\n", info);
    }
    if(info < 0) {
        printf("Has illegal value. %d\n", info);
    }

    int numSigular = 0;
    double *si  = (double*) malloc(labs*dims * labs*dims * sizeof(double));
    memset(si, 0, labs*dims * labs*dims * sizeof(double));

    for(int i=0; i<ldu; i++){
        if(s[i] > 1e-30){
            si[i*ldu + i] = 1.0 / s[i];
            numSigular += 1;
        }else{
            si[i+ldu + i] = 0.0;
        }
    }

    /* calculating transpose */
    double *ret = matrix_mul(vt, si, labs*dims, numSigular, numSigular, numSigular);
    double *ret_pinv = matrix_mul(ret, u, labs*dims, numSigular, numSigular, labs*dims);

    for(int i=0; i<m; i++)
        for(int j=0; j<n; j++)
            X[i][j] = ret_pinv[i*n + j];

    free(ret);
    free(ret_pinv);
    free(work);
    free(a);
    free(s);
    free(vt);
    free(u);
    free(si);

    return;
}

static PyObject *varRedu_estVar(PyObject *self, PyObject *args)
{
    int dims, n_pool, labs, sigma;
    PyObject *PI_obj, *X_obj, *ePI_obj, *eX_obj;

    if (!PyArg_ParseTuple(args, "dOOOO", &sigma, &PI_obj, &X_obj, &ePI_obj, &eX_obj))
        return NULL;

    PyArrayObject *PI_array  =  (PyArrayObject*)PyArray_FROM_OTF(PI_obj,  NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    PyArrayObject *X_array   =  (PyArrayObject*)PyArray_FROM_OTF(X_obj,   NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    PyArrayObject *ePI_array =  (PyArrayObject*)PyArray_FROM_OTF(ePI_obj, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    PyArrayObject *eX_array  =  (PyArrayObject*)PyArray_FROM_OTF(eX_obj,  NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);

    if (PI_array == NULL || X_array == NULL || ePI_array == NULL || eX_array == NULL) {
        Py_XDECREF(PI_array);
        Py_XDECREF(X_array);
        Py_XDECREF(ePI_array);
        Py_XDECREF(eX_array);
        return NULL;
    }

    labs   = (int)PyArray_DIM(PI_array, 1);
    n_pool = (int)PyArray_DIM(X_array, 0);
    dims   = (int)PyArray_DIM(X_array, 1);

    double **PI  =  (double**) malloc(n_pool * sizeof(double*));
    double **X   =  (double**) malloc(n_pool * sizeof(double*));
    for(int i=0; i<n_pool; i++){
        PI[i] = (double*) malloc(labs * sizeof(double));
        X[i]  = (double*) malloc(dims * sizeof(double));
    }
    for(int i=0; i<n_pool; i++){
        for(int j=0; j<labs; j++){
            PI[i][j] = *(double*)PyArray_GETPTR2(PI_array, i, j);
        }
        for(int j=0; j<dims; j++){
            X[i][j] = *(double*)PyArray_GETPTR2(X_array, i, j);
        }
    }
    double *ePI =  (double*) PyArray_DATA(ePI_array);
    double *eX  =  (double*) PyArray_DATA(eX_array);

    double **retF = Fisher(ePI, eX, sigma, labs, dims);
    double **retA = A(PI, X, labs, dims, n_pool);

    pinv(retF, labs, dims);

    /* calculates the trace of the multiply of pinv(F) and A  */
    double score = 0.0;
    for(int i=0; i<dims*labs; i++){
        for(int k=0; k<dims*labs; k++){
            score += retA[i][k] * retF[k][i];
        }
    }

    Py_DECREF(PI_array);
    Py_DECREF(X_array);
    Py_DECREF(ePI_array);
    Py_DECREF(eX_array);

    PyObject* ret = Py_BuildValue("d", score);

    for(int i=0; i<n_pool; i++){
        free(PI[i]);
        free(X[i]);
    }
    free(PI);
    free(X);

    for(int i=0; i<labs*dims; i++){
        free(retF[i]);
        free(retA[i]);
    }
    free(retF);
    free(retA);

    return ret;
}

double** An(double *pi, double *x, int labs, int dims){
    double **g = (double**) malloc(labs*dims * sizeof(double*));
    for(int i=0; i<labs*dims; i++){
        g[i] = (double*) malloc(labs * sizeof(double));
        memset(g[i], 0, labs * sizeof(double));
    }

    for(int p=0; p<labs; p++)
        for(int i=0; i<dims; i++){
            for(int c=0; c<labs; c++)
                if(p == c) g[p*dims + i][c] = pi[p] * (1.0-pi[p]) * x[i];
                else g[p*dims + i][c] = -1.0 * pi[p] * pi[c] * x[i];
        }

    double **an = (double**) malloc(labs*dims * sizeof(double*));
    for(int i=0; i<labs*dims; i++){
        an[i] = (double*) malloc(labs*dims * sizeof(double));
        memset(an[i], 0, labs*dims * sizeof(double));
    }

    for(int p=0; p<labs; p++)
        for(int i=0; i<dims; i++)
            for(int q=0; q<labs; q++)
                for(int j=0; j<dims; j++){
                    /* inner product */
                    double tmp = 0.0;
                    for(int k=0; k<labs; k++){
                        tmp += g[p*dims + i][k] * g[q*dims + j][k];
                    }
                    an[p*dims + i][q*dims + j] = tmp;
                }

    for(int i=0; i<labs*dims; i++)
        free(g[i]);
    free(g);

    return an;
}

double** A(double **PI, double **X, int labs, int dims, int n_pool){
    double **ret = (double**) malloc(labs*dims * sizeof(double*));
    for(int i=0; i<labs*dims; i++){
        ret[i] = (double*) malloc(labs*dims * sizeof(double));
        memset(ret[i], 0, labs*dims * sizeof(double));
    }

    for(int n=0; n<n_pool; n++){
        double **an = An(PI[n], X[n], labs, dims);

        for(int p=0; p<labs; p++)
            for(int i=0; i<dims; i++)
                for(int q=0; q<labs; q++)
                    for(int j=0; j<dims; j++)
                        ret[p*dims + i][q*dims + j] += an[p*dims + i][q*dims + j];
        for(int i=0; i<labs*dims; i++)
            free(an[i]);
        free(an);
    }
    return ret;
}

double** Fisher(double *pi, double *x, double sigma, int labs, int dims){
    double **ret = (double**) malloc(labs*dims * sizeof(double*));
    for(int i=0; i<labs*dims; i++){
        ret[i] = (double*) malloc(labs*dims * sizeof(double*));
        memset(ret[i], 0, labs*dims * sizeof(double));
    }

    for(int p=0; p<labs; p++)
        for(int i=0; i<dims; i++)
            for(int q=0; q<labs; q++)
                for(int j=0; j<dims; j++){
                    if(p == q && i == j)
                        ret[p*dims + i][q*dims + j] = x[i]*x[i]*pi[p]*(1.0-pi[p]) + 1.0/sigma*sigma;
                    else if(p == q && i != j)
                        ret[p*dims + i][q*dims + j] = x[i]*x[j]*pi[p]*(1.0-pi[p]);
                    else
                        ret[p*dims + i][q*dims + j] = x[i]*x[j]*pi[p]*pi[q];
                }
    return ret;
}


/* bclsqr.c
   $Revision: 273 $ $Date: 2006-09-04 15:59:04 -0700 (Mon, 04 Sep 2006) $

   ----------------------------------------------------------------------
   This file is part of BCLS (Bound-Constrained Least Squares).

   Copyright (C) 2006 Michael P. Friedlander, Department of Computer
   Science, University of British Columbia, Canada. All rights
   reserved. E-mail: <mpf@cs.ubc.ca>.

   BCLS is free software; you can redistribute it and/or modify it
   under the terms of the GNU Lesser General Public License as
   published by the Free Software Foundation; either version 2.1 of the
   License, or (at your option) any later version.

   BCLS is distributed in the hope that it will be useful, but WITHOUT
   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
   or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General
   Public License for more details.

   You should have received a copy of the GNU Lesser General Public
   License along with BCLS; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
   USA
   ----------------------------------------------------------------------
*/
/*!
   \file
   Interface to LSQR routine.  Used to compute a Newton step.
*/

#include <cblas.h>
#include <string.h>
#include <stdio.h>

#include "bcls.h"
#include "bclib.h"
#include "bclsqr.h"
#include "lsqr.h"

/*!

  \brief Mat-vec routine called by LSQR.

  LSQR calls this routine, which in turn calls bcls_aprod: This
  routine is declared "static" so that it won't be confused with the
  user's own Aprod routine.

  - If mode = 1,
    - y(1:m)  <-  y(1:m)  +  A(:,ix) * dxFree.
    - y(m+1:) <-  y(m+1:) +     damp * dxFree

  - If mode = 2,
    - dxFree <- dxFree + A'* y(1:m) + damp y(m+1:).

  Note that mSubProb is the number of rows in the subproblem.  This
  may or may not be equal to m, which is the number of rows in the
  original problem.  If the subproblem is damped (because the user has
  either provided a linear term or an explicit damping parameter),
  then mSubProb = m + nFree.  Otherwise, mSubProb = m.

  \param[in]      mode      Determines which producte with A is required.

  \param[in]      mSubProb  Number of rows in the matrix seen by LSQR.  Also:
                            - length of y.
  \param[in]      nFree     Number of columns in A(:,ix).  Also:
                            - length of ix
                            - length of dxFree.
  \param[in,out]  dxFree    Primal variables
  \param[in,out]  y         Dual variables.
  \param[in,out]  UsrWrk    Transit pointer to the BCLS problem context.

*/
static void
aprod_free_lsqr( const int mode, const int mSubProb, const int nFree,
                 double dxFree[], double y[], void *UsrWrk)
{
    int j;
    BCLS *ls = (BCLS *)UsrWrk;  // Reclaim access to the BCLS workspace.
    const int    m      = ls->m;
    const int    preconditioned = ls->Usolve != NULL;
    const int    damped = ls->damp_actual > 0.0;
    const double damp   = ls->damp_actual;
    int          *ix    = ls->ix;
    double       *dx    = ls->dx;    // Used as workspace.
    double       *dy    = ls->wrk_u; // ...

    if (mode == 1) {

        // Solve U dy = dxFree.  U is nFree-by-nFree, and so the
        // relevant part of dy has length nFree.
        if (preconditioned)
            bcls_usolve( ls, BCLS_PRECON_U, nFree, ix, dy, dxFree );
        else
            cblas_dcopy( nFree, dxFree, 1, dy, 1 );

        // y2 <- y2 + damp * dy(1:nFree).
        if (damped)
            cblas_daxpy( nFree, damp, dy, 1, &y[m], 1 );

        // Scatter dy into dx: dx(ix) <- dy(1:nFree).
        for (j = 0; j < nFree; j++)  dx[ ix[j] ] = dy[j];

        // dy <- A(:,ix) * dx(ix).
        bcls_aprod( ls, BCLS_PROD_A, nFree, ix, dx, dy );

        // y1 <- y1 + dy.
        cblas_daxpy( m, 1.0, dy, 1, y, 1 );

    }
    else { // mode == 2

        // dx <- A' * y1.
        bcls_aprod( ls, BCLS_PROD_At, nFree, ix, dx, y );

        // Gather dx(ix) into dy: dy(1:nFree) <- dx(ix).
        for (j = 0; j < nFree; j++) dy[j] = dx[ ix[j] ];

        // dy <- dy + damp * y2.
        if (damped)
            cblas_daxpy( nFree, damp, &y[m], 1, dy, 1 );

        // Solve U' dx = dy.
        if (preconditioned)
            bcls_usolve( ls, BCLS_PRECON_Ut, nFree, ix, dy, dx );
        else
            cblas_dcopy( nFree, dy, 1, dx, 1 );

        // dxFree <- dxFree + dx.
        cblas_daxpy( nFree, 1.0, dx, 1, dxFree, 1 );

    }
    return;
}

/*!

  \brief Compute a Newton step using LSQR.

  \see  bcls_newton_step_cgls.

  \param[in,out] ls      BCLS problem context.
  \param[in]     m       Number of rows in A.
  \param[in]     nFree   Number of columns in A(:,ix).  Also:
                         - length of ix and dxFree
  \param[in]     ix      Index of free variables.
  \param[in]     damp    Regularization parameter.
  \param[in]     itnLim  Iteration limit on current LSQR call.
  \param[in]     tol     LSQR's atol and btol.
  \param[in,out] dxFree  Search direction on free variables.
  \param[in,out] x       Current point.
  \param[in]     c       Linear term.
  \param[in,out] r       Residual. Used as RHS for LSQR.
  \param[out]    itns    Number of LSQR iterations on current subproblem.
  \param[out]    opt     Optimality achieved by LSQR on current subproblem.

  \return
  - 0: Required accurace was achieved.
  - 1: The iteration limit (itnLim) was reached.
  - 2: A(:,ix) is excessively ill-conditioned.

*/
int
bcls_newton_step_lsqr( BCLS *ls, int m, int nFree, int ix[], double damp,
		       int itnLim, double tol, double dxFree[], double x[],
		       double c[], double r[], int *itns, double *opt )
{
    int j, k;                             // Misc. counters.
    int mpn;                              // No. of rows in [ N; damp I ].
    int          unscale_dxFree   = 0;
    const int    rescaling_method = 1;
    const int    linear   = c != NULL;
    const int    damped   = damp  > 0.0;
    const double damp_min = ls->damp_min;
    const double damp2    = damp * damp;
    const double zero     = 0.0;
    double damp_actual;
    double beta1, beta2;

    // LSQR outputs
    int    istop;      // Termination flag.
    double anorm;      // Estimate of Frobenious norm of Abar.
    double acond;      // Estimate of condition no. of Abar.
    double rnorm;      // Estimate of the final value of norm(rbar).
    double xnorm;      // Estimate of the norm of the final solution dx.

    // Set r(m+1:) <- - 1/beta c + damp^2/beta x, where
    //     beta = max(min_damp, damp).
    // Note that r is declared length (m+n), so there is always enough
    // space.  The chosen damping parameter is stored in
    // ls->damp_actual so that it can be used in bcls_aprod_free.
    if (!damped  &&  !linear) {
	mpn = m;
	ls->damp_actual = 0.0;
    }
    else {

	mpn = m + nFree;

	//--------------------------------------------------------------
	// Rescale the RHS.  Will need to unscale LSQR's solution later.
	//--------------------------------------------------------------
	if (rescaling_method) {

	    if (linear) {

		unscale_dxFree  = 1;
		damp_actual     = fmax( damp, damp_min );
		ls->damp_actual = damp_actual;

		// r(1:m) = damp_actual * r(1:m)
		cblas_dscal( m, damp_actual, r, 1 );

		// r(m+1:) = - c
		for (j = 0; j < nFree; j++ )
		    r[m+j] = - c[ ix[j] ];

		// r(m+1:) = - c - damp^2 * x
		if (damped)
		    for (j = 0; j < nFree; j++)
			r[m+j] -= damp2 * x[ ix[j] ];
	    }
	    else {

	        ls->damp_actual = damp;
		for (j = 0; j < nFree; j++)
		    r[m+j] = - damp * x[ ix[j] ];
	    }
	}
	//--------------------------------------------------------------
	// No rescaling.
	//--------------------------------------------------------------
	else {
	    if (damped  &&  linear) {

		damp_actual     =  fmax( damp, damp_min );
		beta1           =  -1.0 / damp_actual;
		beta2           =  -damp * damp / damp_actual;
		ls->damp_actual =  damp_actual;

		for (j = 0; j < nFree; j++) {
		    k = ix[j];
		    r[m+j] = beta1 * c[k] + beta2 * x[k];
		}
	    }
	    else if ( damped  &&  !linear ) {

		beta2 = - damp;
		ls->damp_actual = damp;
		for (j = 0; j < nFree; j++)
		    r[m+j] = beta2 * x[ ix[j] ];
	    }
	    else if (!damped  &&   linear ) {

		beta1 = - 1.0 / damp_min;
		ls->damp_actual = damp_min;
		for (j = 0; j < nFree; j++)
		    r[m+j] = beta1 * c[ ix[j] ];
	    }
	}
    }

    // -----------------------------------------------------------------
    // Solve the subproblem with LSQR.
    // -----------------------------------------------------------------
    bcls_timer( &(ls->stopwatch[BCLS_TIMER_LSQR]), BCLS_TIMER_START );

    lsqr( mpn, nFree, aprod_free_lsqr, zero, (void *)ls,
          r, ls->wrk_v, ls->wrk_w, dxFree, NULL,
          tol, tol, ls->conlim, itnLim, ls->minor_file,
          &istop, itns, &anorm, &acond,
          &rnorm, opt, &xnorm );

    bcls_timer( &(ls->stopwatch[BCLS_TIMER_LSQR]), BCLS_TIMER_STOP );

    // -----------------------------------------------------------------
    // Cleanup and exit.  First unscale dxFree if needed.
    // -----------------------------------------------------------------
    if (rescaling_method  &&  unscale_dxFree)
	cblas_dscal( nFree, 1.0/damp_actual, dxFree, 1 );

    if (istop <= 3)
	return 0;
    else if (istop == 5)
	return 1;
    else
	return 2;
}


#include <math.h>
#include <stdlib.h>
#if !defined(__APPLE__)
#include <malloc.h>
#endif
#include <stdio.h>
#include <assert.h>
#include <time.h>
#include <string.h>

#include <fftw3.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_erf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_legendre.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

#include "../cosmolike_core/theory/basics.c"
#include "../cosmolike_core/theory/structs.c"
#include "../cosmolike_core/theory/parameters.c"
#include "../cosmolike_core/emu17/P_cb/emu.c"
#include "../cosmolike_core/theory/recompute.c"
#include "../cosmolike_core/theory/cosmo3D.c"
#include "../cosmolike_core/theory/redshift_spline.c"
#include "../cosmolike_core/theory/halo.c"
#include "../cosmolike_core/theory/HOD.c"
#include "../cosmolike_core/theory/pt.c"
#include "../cosmolike_core/theory/cosmo2D_fourier.c"
#include "../cosmolike_core/theory/IA.c"
#include "../cosmolike_core/theory/cluster.c"
#include "../cosmolike_core/theory/BAO.c"
#include "../cosmolike_core/theory/external_prior.c"
#include "../cosmolike_core/theory/init_baryon.c"
#include "init_emu.c"

double C_shear_tomo_sys(double ell,int z1,int z2);
double C_cgl_tomo_sys(double ell_Cluster,int zl,int nN, int zs);
double C_gl_tomo_sys(double ell,int zl,int zs);
void set_data_shear(int Ncl, double *ell, double *data, int start);
void set_data_ggl(int Ncl, double *ell, double *data, int start);
void set_data_clustering(int Ncl, double *ell, double *data, int start);
void set_data_cluster_N(double *data, int start);
void set_data_cgl(double *ell_Cluster, double *data, int start);
void compute_data_vector(char *details, double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q,double mass_obs_norm, double mass_obs_slope, double mass_z_slope, double mass_obs_scatter_norm, double mass_obs_scatter_mass_slope, double mass_obs_scatter_z_slope, double Q1, double Q2, double Q3);
double log_multi_like(double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q,double mass_obs_norm, double mass_obs_slope, double mass_z_slope, double mass_obs_scatter_norm, double mass_obs_scatter_mass_slope, double mass_obs_scatter_z_slope, double Q1, double Q2, double Q3);
void write_vector_wrapper(char *details, input_cosmo_params ic, input_nuisance_params in);
double log_like_wrapper(input_cosmo_params ic, input_nuisance_params in);
int get_N_tomo_shear(void);
int get_N_tomo_clustering(void);
int get_N_ggl(void);
int get_N_ell(void);

int get_N_tomo_shear(void){
  return tomo.shear_Nbin;
}
int get_N_tomo_clustering(void){
  return tomo.clustering_Nbin;
}
int get_N_ggl(void){
  return tomo.ggl_Npowerspectra;
}
int get_N_ell(void){
  return like.Ncl;
}

double C_shear_tomo_sys(double ell, int z1, int z2)
{
  double C;
  // C= C_shear_tomo_nointerp(ell,z1,z2);
  // if(like.IA==1) C+=C_II_nointerp(ell,z1,z2)+C_GI_nointerp(ell,z1,z2);

  if(like.IA!=1) C= C_shear_tomo_nointerp(ell,z1,z2);
  //if(like.IA==1) C= C_shear_shear_IA(ell,z1,z2);
  if(like.IA==1) C = C_shear_tomo_nointerp(ell,z1,z2)+C_II_nointerp(ell,z1,z2)+C_GI_nointerp(ell,z1,z2);
  if(like.IA==2) C += C_II_lin_nointerp(ell,z1,z2)+C_GI_lin_nointerp(ell,z1,z2);
  if(like.shearcalib==1) C *=(1.0+nuisance.shear_calibration_m[z1])*(1.0+nuisance.shear_calibration_m[z2]);
  //printf("%le %d %d %le\n",ell,z1,z2,C_shear_tomo_nointerp(ell,z1,z2)+C_II_JB_nointerp(ell,z1,z2)+C_GI_JB_nointerp(ell,z1,z2));
return C;
}

double C_gl_tomo_sys(double ell,int zl,int zs)
{
  double C;
  // C=C_gl_tomo_nointerp(ell,zl,zs);
  // if(like.IA==1) C += C_gI_nointerp(ell,zl,zs);

  if(like.IA!=1) C=C_gl_tomo_nointerp(ell,zl,zs);
  if(like.IA==1) C = C_ggl_IA(ell,zl,zs);
  if(like.IA==2) C += C_gI_lin_nointerp(ell,zl,zs);
  if(like.shearcalib==1) C *=(1.0+nuisance.shear_calibration_m[zs]);
return C;
}

double C_cgl_tomo_sys(double ell_Cluster, int zl,int nN, int zs)
{
  double C;
  C=C_cgl_tomo_nointerp(ell_Cluster,zl,nN,zs);
  //if(like.IA!=0) C +=
  if(like.shearcalib==1) C *=(1.0+nuisance.shear_calibration_m[zs]);
return C;
}

void set_data_shear(int Ncl, double *ell, double *data, int start)
{
  int i,z1,z2,nz;
  double a;
  for (nz = 0; nz < tomo.shear_Npowerspectra; nz++){
    z1 = Z1(nz); z2 = Z2(nz);
    for (i = 0; i < Ncl; i++){
      if (ell[i] < like.lmax_shear){ data[Ncl*nz+i] = C_shear_tomo_sys(ell[i],z1,z2);}
      else {data[Ncl*nz+i] = 0.;}
    }
  }
}

void set_data_ggl(int Ncl, double *ell, double *data, int start)
{
  int i, zl,zs,nz;
  for (nz = 0; nz < tomo.ggl_Npowerspectra; nz++){
    zl = ZL(nz); zs = ZS(nz);
    for (i = 0; i < Ncl; i++){
      if (test_kmax(ell[i],zl)){
        data[start+(Ncl*nz)+i] = C_gl_tomo_sys(ell[i],zl,zs);
      }
      else{
        data[start+(Ncl*nz)+i] = 0.;
      }
    }
  }
}

void set_data_clustering(int Ncl, double *ell, double *data, int start){
  int i, nz;
  for (nz = 0; nz < tomo.clustering_Npowerspectra; nz++){
    //printf("%d %e %e\n",nz, gbias.b[nz][1],pf_photoz(gbias.b[nz][1],nz));
    for (i = 0; i < Ncl; i++){
      if (test_kmax(ell[i],nz)){data[start+(Ncl*nz)+i] = C_cl_tomo_nointerp(ell[i],nz,nz);}
      else{data[start+(Ncl*nz)+i] = 0.;}
      //printf("%d %d %le %le\n",nz,nz,ell[i],data[Ncl*(tomo.shear_Npowerspectra+tomo.ggl_Npowerspectra + nz)+i]);
    }
  }
}

void set_data_cluster_N(double *data, int start){
  int nN, nz;
  for (nz = 0; nz < tomo.cluster_Nbin; nz++){
    for (nN = 0; nN < Cluster.N200_Nbin; nN++){
      data[start+Cluster.N200_Nbin*nz+nN] = N_N200(nz, nN);
    }
  }
}

void set_data_cgl(double *ell_Cluster, double *data, int start)
{
  int zl,zs,nN,nz,i,j;
  for(nN = 0; nN < Cluster.N200_Nbin; nN++){
    for (nz = 0; nz < tomo.cgl_Npowerspectra; nz++){
      zl = ZC(nz); zs = ZSC(nz);
      for (i = 0; i < Cluster.lbin; i++){
        j = start;
        j += (nz*Cluster.N200_Nbin+nN)*Cluster.lbin +i;
        data[j] = C_cgl_tomo_sys(ell_Cluster[i],zl,nN,zs);
      }
    }
  }
}

int set_cosmology_params(double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu)
{
  cosmology.Omega_m=OMM;
  cosmology.Omega_v= 1.0-cosmology.Omega_m;
  cosmology.sigma_8=S8;
  cosmology.n_spec= NS;
  cosmology.w0=W0;
  cosmology.wa=WA;
  cosmology.omb=OMB;
  cosmology.h0=H0;
  cosmology.MGSigma=MGSigma;
  cosmology.MGmu=MGmu;

  if (cosmology.Omega_m < 0.04 || cosmology.Omega_m > 0.7) return 0;
  if (cosmology.omb < 0.04 || cosmology.omb > 0.055) return 0;
  if (cosmology.sigma_8 < 0.4 || cosmology.sigma_8 > 1.2) return 0;
  if (cosmology.n_spec < 0.84 || cosmology.n_spec > 1.06) return 0;
  if (cosmology.w0 < -2.1 || cosmology.w0 > -0.0) return 0;
  if (cosmology.wa < -2.6 || cosmology.wa > 2.6) return 0;
  if (cosmology.h0 < 0.4 || cosmology.h0 > 0.9) return 0;

  return 1;
}

void set_nuisance_shear_calib(double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10)
{
  nuisance.shear_calibration_m[0] = M1;
  nuisance.shear_calibration_m[1] = M2;
  nuisance.shear_calibration_m[2] = M3;
  nuisance.shear_calibration_m[3] = M4;
  nuisance.shear_calibration_m[4] = M5;
  nuisance.shear_calibration_m[5] = M6;
  nuisance.shear_calibration_m[6] = M7;
  nuisance.shear_calibration_m[7] = M8;
  nuisance.shear_calibration_m[8] = M9;
  nuisance.shear_calibration_m[9] = M10;
}

int set_nuisance_shear_photoz(double SP1,double SP2,double SP3,double SP4,double SP5,double SP6,double SP7,double SP8,double SP9,double SP10,double SPS1)
{
  int i;
  nuisance.bias_zphot_shear[0]=SP1;
  nuisance.bias_zphot_shear[1]=SP2;
  nuisance.bias_zphot_shear[2]=SP3;
  nuisance.bias_zphot_shear[3]=SP4;
  nuisance.bias_zphot_shear[4]=SP5;
  nuisance.bias_zphot_shear[5]=SP6;
  nuisance.bias_zphot_shear[6]=SP7;
  nuisance.bias_zphot_shear[7]=SP8;
  nuisance.bias_zphot_shear[8]=SP9;
  nuisance.bias_zphot_shear[9]=SP10;

  for (i=0;i<tomo.shear_Nbin; i++){
    nuisance.sigma_zphot_shear[i]=SPS1;
    if (nuisance.sigma_zphot_shear[i]<0.000001) return 0;
  }
  return 1;
}

int set_nuisance_clustering_photoz(double CP1,double CP2,double CP3,double CP4,double CP5,double CP6,double CP7,double CP8,double CP9,double CP10,double CPS1)
{
  int i;
  nuisance.bias_zphot_clustering[0]=CP1;
  nuisance.bias_zphot_clustering[1]=CP2;
  nuisance.bias_zphot_clustering[2]=CP3;
  nuisance.bias_zphot_clustering[3]=CP4;
  nuisance.bias_zphot_clustering[4]=CP5;
  nuisance.bias_zphot_clustering[5]=CP6;
  nuisance.bias_zphot_clustering[6]=CP7;
  nuisance.bias_zphot_clustering[7]=CP8;
  nuisance.bias_zphot_clustering[8]=CP9;
  nuisance.bias_zphot_clustering[9]=CP10;

  for (i=0;i<tomo.clustering_Nbin; i++){
    nuisance.sigma_zphot_clustering[i]=CPS1;
    if (nuisance.sigma_zphot_clustering[i]<0.0001) return 0;
  }
  return 1;
}

int set_nuisance_ia(double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q)
{
  nuisance.A_ia=A_ia;
  nuisance.beta_ia=beta_ia;
  nuisance.eta_ia=eta_ia;
  nuisance.eta_ia_highz=eta_ia_highz;
  nuisance.LF_alpha=LF_alpha;
  nuisance.LF_P=LF_P;
  nuisance.LF_Q=LF_Q;
  nuisance.LF_red_alpha=LF_red_alpha;
  nuisance.LF_red_P=LF_red_P;
  nuisance.LF_red_Q=LF_red_Q;
  if (nuisance.A_ia < 0.0 || nuisance.A_ia > 10.0) return 0;
  if (nuisance.beta_ia < -1.0 || nuisance.beta_ia > 3.0) return 0;
  if (nuisance.eta_ia < -3.0 || nuisance.eta_ia> 3.0) return 0;
  if (nuisance.eta_ia_highz < -1.0 || nuisance.eta_ia_highz> 1.0) return 0;
  // if(like.IA!=0){
  //  if (check_LF()) return 0;
  // }
return 1;
}

int set_nuisance_cluster_Mobs(double cluster_Mobs_lgN0,  double cluster_Mobs_alpha, double cluster_Mobs_beta, double cluster_Mobs_sigma0, double cluster_Mobs_sigma_qm, double cluster_Mobs_sigma_qz)
{
  //  nuisance.cluster_Mobs_lgM0 = mass_obs_norm;  //fiducial : 1.72+log(1.e+14*0.7); could use e.g. sigma = 0.2 Gaussian prior
  //  nuisance.cluster_Mobs_alpha = mass_obs_slope; //fiducial: 1.08; e.g. sigma = 0.1 Gaussian prior
  //  nuisance.cluster_Mobs_beta = mass_z_slope; //fiducial: 0.0; e.g. sigma = 0.1 Gaussian prior
  //  nuisance.cluster_Mobs_sigma = mass_obs_scatter; //fiducial 0.25; e.g. sigma = 0.05 Gaussian prior

  // fiducial values and priors from Murata et al. (2018) except for redshift-related parameters
  nuisance.cluster_Mobs_lgN0 = cluster_Mobs_lgN0; //fiducial: 3.207, flat prior [0.5, 5.0]
  nuisance.cluster_Mobs_alpha = cluster_Mobs_alpha; //fiducial: 0.993, flat prior [0.0, 2.0]
  nuisance.cluster_Mobs_beta = cluster_Mobs_beta; //fiducial: 0.0, flat prior [-1.5, 1.5]
  nuisance.cluster_Mobs_sigma0 = cluster_Mobs_sigma0; //fiducial: 0.456, flat prior [0.0, 1.5]
  nuisance.cluster_Mobs_sigma_qm = cluster_Mobs_sigma_qm; //fiducial: -0.169, flat prior [-1.5, 1.5]
  nuisance.cluster_Mobs_sigma_qz = cluster_Mobs_sigma_qz; //fiducial: 0.0, flat prior [-1.5, 1.5]

  if (nuisance.cluster_Mobs_lgN0 < 0.5 || nuisance.cluster_Mobs_lgN0 > 5.0) return 0;
  if (nuisance.cluster_Mobs_alpha < 0.0 || nuisance.cluster_Mobs_alpha > 2.0) return 0;
  if (nuisance.cluster_Mobs_beta < -1.5 || nuisance.cluster_Mobs_beta > 1.5) return 0;
  if (nuisance.cluster_Mobs_sigma0 < 0.0|| nuisance.cluster_Mobs_sigma0 > 1.5) return 0;
  if (nuisance.cluster_Mobs_sigma_qm < -1.5 && nuisance.cluster_Mobs_sigma_qm > 1.5) return 0;
  if (nuisance.cluster_Mobs_sigma_qz < -1.5 && nuisance.cluster_Mobs_sigma_qz > 1.5)return 0;

return 1;
}

int set_nuisance_gbias(double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8,double B9, double B10)
{
  int i;
  gbias.b[0] = B1;
  gbias.b[1] = B2;
  gbias.b[2] = B3;
  gbias.b[3] = B4;
  gbias.b[4] = B5;
  gbias.b[5] = B6;
  gbias.b[6] = B7;
  gbias.b[7] = B8;
  gbias.b[8] = B9;
  gbias.b[9] = B10;
  if(like.bias==1){
    for (i = 0; i < 10; i++){
      if (gbias.b[i] < 0.8 || gbias.b[i] > 3.0) return 0;
    }
  }
  return 1;
}

double log_multi_like(double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5, double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q,double mass_obs_norm, double mass_obs_slope, double mass_z_slope, double mass_obs_scatter_norm, double mass_obs_scatter_mass_slope, double mass_obs_scatter_z_slope, double Q1, double Q2, double Q3)
{
  int i,j,k,m=0,l;
  static double *pred;
  static double *ell;
  static double *ell_Cluster;
  static double darg;
  double chisqr,a,log_L_prior=0.0, log_L=0.0;

  if(ell==0){
    pred= create_double_vector(0, like.Ndata-1);
    ell= create_double_vector(0, like.Ncl-1);
    darg=(log(like.lmax)-log(like.lmin))/like.Ncl;
    for (l=0;l<like.Ncl;l++){
      ell[l]=exp(log(like.lmin)+(l+0.5)*darg);
    }
    ell_Cluster= create_double_vector(0, Cluster.lbin-1);
    darg=(log(Cluster.l_max)-log(Cluster.l_min))/Cluster.lbin;
    for (l=0;l<Cluster.lbin;l++){
      ell_Cluster[l]=exp(log(Cluster.l_min)+(l+0.5)*darg);
    }
  }
  if (set_cosmology_params(OMM,S8,NS,W0,WA,OMB,H0,MGSigma,MGmu)==0){
    printf("Cosmology out of bounds\n");
    return -1.0e15;
  }
  set_nuisance_shear_calib(M1,M2,M3,M4,M5,M6,M7,M8,M9,M10);
  if (set_nuisance_shear_photoz(SP1,SP2,SP3,SP4,SP5,SP6,SP7,SP8,SP9,SP10,SPS1)==0){
    printf("Shear photo-z sigma too small\n");
    return -1.0e15;
  }
  if (set_nuisance_clustering_photoz(CP1,CP2,CP3,CP4,CP5,CP6,CP7,CP8,CP9,CP10,CPS1)==0){
    printf("Clustering photo-z sigma too small\n");
    return -1.0e15;
  }
  if (set_nuisance_ia(A_ia,beta_ia,eta_ia,eta_ia_highz,LF_alpha,LF_P,LF_Q,LF_red_alpha,LF_red_P,LF_red_Q)==0){
    printf("IA parameters out of bounds\n");
    return -1.0e15;
  }
  if (set_nuisance_gbias(B1,B2,B3,B4,B5,B6,B7,B8,B9,B10)==0){
    printf("Bias out of bounds\n");
    return -1.0e15;
  }

  //printf("like %le %le %le %le %le %le %le %le\n",cosmology.Omega_m, cosmology.Omega_v,cosmology.sigma_8,cosmology.n_spec,cosmology.w0,cosmology.wa,cosmology.omb,cosmology.h0);
  // printf("like %le %le %le %le\n",gbias.b[0][0], gbias.b[1][0], gbias.b[2][0], gbias.b[3][0]);
  // for (i=0; i<10; i++){
  //   printf("nuisance %le %le %le\n",nuisance.shear_calibration_m[i],nuisance.bias_zphot_shear[i],nuisance.sigma_zphot_shear[i]);
  // }

  log_L_prior=0.0;
  // if(like.Aubourg_Planck_BAO_SN==1) log_L_prior+=log_L_Planck_BAO_SN();
  // if(like.SN==1) log_L_prior+=log_L_SN();
  //if(like.BAO==1) log_L_prior+=log_L_BAO();
  // if(like.Planck==1) log_L_prior+=log_L_Planck();
  // if(like.Planck15_BAO_w0wa==1) log_L_prior+=log_L_Planck15_BAO_w0wa();//CH
  //if(like.Planck15_BAO_H070p6_JLA_w0wa==1) log_L_prior+=log_L_Planck15_BAO_H070p6_JLA_w0wa();//CH
  // if(like.IA!=0) log_L_prior+=log_L_ia();
  // if(like.IA!=0) log_L_prior+=log_like_f_red();
  if(like.wlphotoz!=0) log_L_prior+=log_L_wlphotoz();
  if(like.clphotoz!=0) log_L_prior+=log_L_clphotoz();
  if(like.shearcalib==1) log_L_prior+=log_L_shear_calib();
  if(like.IA!=0) {
    log_L = 0.0;
    log_L -= pow((nuisance.A_ia - prior.A_ia[0])/prior.A_ia[1],2.0);
    log_L -= pow((nuisance.beta_ia - prior.beta_ia[0])/prior.beta_ia[1],2.0);
    log_L -= pow((nuisance.eta_ia - prior.eta_ia[0])/prior.eta_ia[1],2.0);
    log_L -= pow((nuisance.eta_ia_highz - prior.eta_ia_highz[0])/prior.eta_ia_highz[1],2.0);
    log_L_prior+=0.5*log_L;
  }
  if(like.baryons==1){
    log_L = 0.0;
    log_L -= pow((Q1 - prior.bary_Q1[0])/prior.bary_Q1[1],2.0);
    log_L -= pow((Q2 - prior.bary_Q2[0])/prior.bary_Q2[1],2.0);
    log_L -= pow((Q3 - prior.bary_Q3[0])/prior.bary_Q3[1],2.0);
    log_L_prior+=0.5*log_L;
  }

  // if(like.clusterMobs==1) log_L_prior+=log_L_clusterMobs();

  // printf("%d %d %d %d\n",like.BAO,like.wlphotoz,like.clphotoz,like.shearcalib);
  // printf("logl %le %le %le %le\n",log_L_shear_calib(),log_L_wlphotoz(),log_L_clphotoz(),log_L_clusterMobs());
  int start=0;

  if(like.shear_shear==1) {
    set_data_shear(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.shear_Npowerspectra;
  }
  if(like.shear_pos==1){
    set_data_ggl(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.ggl_Npowerspectra;
  }
  if(like.pos_pos==1){
    set_data_clustering(like.Ncl,ell,pred, start);
    start=start+like.Ncl*tomo.clustering_Npowerspectra;
  }

  chisqr=0.0;
  for (i=0; i<like.Ndata; i++){
    for (j=0; j<like.Ndata; j++){
      a=(pred[i]-data_read(1,i)+Q1*bary_read(1,0,i)+Q2*bary_read(1,1,i)+Q3*bary_read(1,2,i))*invcov_read(1,i,j)*(pred[j]-data_read(1,j)+Q1*bary_read(1,0,j)+Q2*bary_read(1,1,j)+Q3*bary_read(1,2,j));
      //a=(pred[i]-data_read(1,i))*invcov_read(1,i,j)*(pred[j]-data_read(1,j));
      chisqr=chisqr+a;
    }
    // if (fabs(data_read(1,i)) < 1.e-25){
    //    printf("%d %le %le %le\n",i,data_read(1,i),pred[i],invcov_read(1,i,i));
    // }
  }
  if (chisqr<0.0){
    printf("error: chisqr = %le\n",chisqr);
    //exit(EXIT_FAILURE);
  }
//  printf("%le\n",chisqr);
  return -0.5*chisqr+log_L_prior;
}

void compute_data_vector(char *details, double OMM, double S8, double NS, double W0,double WA, double OMB, double H0, double MGSigma, double MGmu, double B1, double B2, double B3, double B4,double B5, double B6, double B7, double B8, double B9, double B10, double SP1, double SP2, double SP3, double SP4, double SP5,double SP6, double SP7, double SP8, double SP9, double SP10, double SPS1, double CP1, double CP2, double CP3, double CP4, double CP5, double CP6, double CP7, double CP8, double CP9, double CP10, double CPS1, double M1, double M2, double M3, double M4, double M5, double M6, double M7, double M8, double M9, double M10, double A_ia, double beta_ia, double eta_ia, double eta_ia_highz, double LF_alpha, double LF_P, double LF_Q, double LF_red_alpha, double LF_red_P, double LF_red_Q, double mass_obs_norm, double mass_obs_slope, double mass_z_slope, double mass_obs_scatter_norm, double mass_obs_scatter_mass_slope, double mass_obs_scatter_z_slope, double Q1, double Q2, double Q3)
{

  int i,j,k,m=0,l;
  static double *pred;
  static double *ell;
  static double *ell_Cluster;
  static double darg;
  double chisqr,a,log_L_prior=0.0;

  if(ell==0){
    pred= create_double_vector(0, like.Ndata-1);
    ell= create_double_vector(0, like.Ncl-1);
    darg=(log(like.lmax)-log(like.lmin))/like.Ncl;
    for (l=0;l<like.Ncl;l++){
      ell[l]=exp(log(like.lmin)+(l+0.5)*darg);
    }
    ell_Cluster= create_double_vector(0, Cluster.lbin-1);
    darg=(log(Cluster.l_max)-log(Cluster.l_min))/Cluster.lbin;
    for (l=0;l<Cluster.lbin;l++){
      ell_Cluster[l]=exp(log(Cluster.l_min)+(l+0.5)*darg);
    }
  }
// for (l=0;l<like.Ncl;l++){
//   printf("%d %le\n",i,ell[l]);
// }

  set_cosmology_params(OMM,S8,NS,W0,WA,OMB,H0,MGSigma,MGmu);
  set_nuisance_shear_calib(M1,M2,M3,M4,M5,M6,M7,M8,M9,M10);
  set_nuisance_shear_photoz(SP1,SP2,SP3,SP4,SP5,SP6,SP7,SP8,SP9,SP10,SPS1);
  set_nuisance_clustering_photoz(CP1,CP2,CP3,CP4,CP5,CP6,CP7,CP8,CP9,CP10,CPS1);
  set_nuisance_ia(A_ia,beta_ia,eta_ia,eta_ia_highz,LF_alpha,LF_P,LF_Q,LF_red_alpha,LF_red_P,LF_red_Q);
  set_nuisance_gbias(B1,B2,B3,B4,B5,B6,B7,B8,B9,B10);

  int start=0;
  if(like.shear_shear==1) {
    set_data_shear(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.shear_Npowerspectra;
  }
  if(like.shear_pos==1){
    //printf("ggl\n");
    set_data_ggl(like.Ncl, ell, pred, start);
    start=start+like.Ncl*tomo.ggl_Npowerspectra;
  }
  if(like.pos_pos==1){
    //printf("clustering\n");
    set_data_clustering(like.Ncl,ell,pred, start);
    start=start+like.Ncl*tomo.clustering_Npowerspectra;
  }

  FILE *F;
  char filename[300];
  if (strstr(details,"FM") != NULL){
    sprintf(filename,"%s",details);
  }
  else {sprintf(filename,"datav/%s_%s",like.probes,details);}
  F=fopen(filename,"w");
  for (i=0;i<like.Ndata; i++){
    fprintf(F,"%d %le\n",i,pred[i]);
    //printf("%d %le\n",i,pred[i]);
  }
  fclose(F);
  // printf("&gbias.b1_function %p\n",&gbias.b1_function);
  // printf("gbias.b1_function  %p\n",gbias.b1_function);
  // printf("bgal_z   %p\n",bgal_z);
  // printf("&bgal_z  %p\n",&bgal_z);
  // printf("b1_per_bin   %p\n",b1_per_bin);
  // printf("&b1_per_bin  %p\n",&b1_per_bin);

}

void write_datavector_wrapper(char *details, input_cosmo_params ic, input_nuisance_params in)
{
  compute_data_vector(details, ic.omega_m, ic.sigma_8, ic.n_s, ic.w0, ic.wa, ic.omega_b, ic.h0, ic.MGSigma, ic.MGmu,
    in.bias[0], in.bias[1], in.bias[2], in.bias[3],in.bias[4], in.bias[5], in.bias[6], in.bias[7],in.bias[8], in.bias[9],
    in.source_z_bias[0], in.source_z_bias[1], in.source_z_bias[2], in.source_z_bias[3], in.source_z_bias[4],
    in.source_z_bias[5], in.source_z_bias[6], in.source_z_bias[7], in.source_z_bias[8], in.source_z_bias[9],
    in.source_z_s,
    in.lens_z_bias[0], in.lens_z_bias[1], in.lens_z_bias[2], in.lens_z_bias[3], in.lens_z_bias[4],
    in.lens_z_bias[5], in.lens_z_bias[6], in.lens_z_bias[7], in.lens_z_bias[8], in.lens_z_bias[9],
    in.lens_z_s,
    in.shear_m[0], in.shear_m[1], in.shear_m[2], in.shear_m[3], in.shear_m[4],
    in.shear_m[5], in.shear_m[6], in.shear_m[7], in.shear_m[8], in.shear_m[9],
    in.A_ia, in.beta_ia, in.eta_ia, in.eta_ia_highz,
    in.lf[0], in.lf[1], in.lf[2], in.lf[3], in.lf[4], in.lf[5],
    in.m_lambda[0], in.m_lambda[1], in.m_lambda[2], in.m_lambda[3],
    in.m_lambda[4], in.m_lambda[5],in.bary[0], in.bary[1], in.bary[2]);
}

double log_like_wrapper(input_cosmo_params ic, input_nuisance_params in)
{
  double like = log_multi_like(ic.omega_m, ic.sigma_8, ic.n_s, ic.w0, ic.wa, ic.omega_b, ic.h0, ic.MGSigma, ic.MGmu,
    in.bias[0], in.bias[1], in.bias[2], in.bias[3],in.bias[4], in.bias[5], in.bias[6], in.bias[7],in.bias[8], in.bias[9],
    in.source_z_bias[0], in.source_z_bias[1], in.source_z_bias[2], in.source_z_bias[3], in.source_z_bias[4],
    in.source_z_bias[5], in.source_z_bias[6], in.source_z_bias[7], in.source_z_bias[8], in.source_z_bias[9],
    in.source_z_s,
    in.lens_z_bias[0], in.lens_z_bias[1], in.lens_z_bias[2], in.lens_z_bias[3], in.lens_z_bias[4],
    in.lens_z_bias[5], in.lens_z_bias[6], in.lens_z_bias[7], in.lens_z_bias[8], in.lens_z_bias[9],
    in.lens_z_s,
    in.shear_m[0], in.shear_m[1], in.shear_m[2], in.shear_m[3], in.shear_m[4],
    in.shear_m[5], in.shear_m[6], in.shear_m[7], in.shear_m[8], in.shear_m[9],
    in.A_ia, in.beta_ia, in.eta_ia, in.eta_ia_highz,
    in.lf[0], in.lf[1], in.lf[2], in.lf[3], in.lf[4], in.lf[5],
    in.m_lambda[0], in.m_lambda[1], in.m_lambda[2], in.m_lambda[3],
    in.m_lambda[4], in.m_lambda[5],in.bary[0], in.bary[1], in.bary[2]);

  return like;
}

void save_zdistr_sources(int zs){
  double z,dz =(redshift.shear_zdistrpar_zmax-redshift.shear_zdistrpar_zmin)/300.0;
  printf("Printing redshift distribution n(z) for source redshift bin %d\n",zs);

   FILE *F1;
   char filename[300];
   sprintf(filename,"zdistris/zdist_sources_bin%d.txt",zs);
   F1 = fopen(filename,"w");
   for (z =redshift.shear_zdistrpar_zmin; z< redshift.shear_zdistrpar_zmax; z+= dz){
      fprintf(F1,"%e %e\n", z, zdistr_photoz(z,zs));
   }
}

void save_zdistr_lenses(int zl){
   double z,dz =(redshift.clustering_zdistrpar_zmax-redshift.clustering_zdistrpar_zmin)/300.0;
  printf("Printing redshift distribution n(z) and bias b(z) for lens redshift bin %d\n",zl);

   FILE *F1;
   char filename[300];
   sprintf(filename,"zdistris/zdist_lenses_bin%d.txt", zl);
   F1 = fopen(filename,"w");
   for (z =redshift.clustering_zdistrpar_zmin; z< redshift.clustering_zdistrpar_zmax; z+= dz){
      fprintf(F1,"%e %e\n", z, pf_photoz(z,zl));
   }
}

 int main(int argc, char** argv)
{
  clock_t begin, end;
  double time_spent;
  int i;
  char arg1[400],arg2[400],arg3[400];
/* here, do your time-consuming job */
  int sce=atoi(argv[1]);

  int N_scenarios=36;

  double area_table[36]={7623.22,14786.3,9931.47,8585.43,17681.8,15126.9,9747.99,8335.08,9533.42,18331.3,12867.8,17418.9,19783.1,12538.8,15260.0,16540.7,19636.8,11112.7,10385.5,16140.2,18920.1,17976.2,11352.0,9214.77,16910.7,11995.6,16199.8,14395.1,8133.86,13510.5,19122.3,15684.5,12014.8,14059.7,10919.3,13212.7};

  double nsource_table[36]={13.991,33.3975,17.069,28.4875,35.3643,10.3802,11.1105,29.5904,31.4608,15.7463,13.3066,9.07218,9.58719,16.3234,25.8327,10.7415,38.0412,32.8821,19.8893,27.0369,15.1711,14.2418,19.1851,26.926,22.0012,12.6553,18.6304,11.9787,36.8728,22.5265,17.4381,12.3424,10.0095,23.5979,20.8771,24.8956};

  double nlens_table[36]={22.9726 ,61.5948 ,28.7809 ,51.4354 ,65.7226 ,16.3777 ,17.6899 ,53.6984 ,57.562 ,26.266 ,21.703 ,14.0587 ,14.9667 ,27.36 ,46.0364 ,17.0253 ,71.39 ,60.5184 ,34.2283 ,48.4767 ,25.1812 ,23.44 ,32.8578 ,48.2513 ,38.3766 ,20.5029 ,31.783 ,19.2647 ,68.9094 ,39.4168 ,29.4874 ,19.9292 ,15.7162 ,41.5487 ,36.1616 ,44.148};

  char survey_designation[1][200]={"LSST"};
  char tomo_binning_source[1][200]={"source_std"};
  char tomo_binning_lens[1][200]={"LSST_gold"};

  char source_zfile[36][400]={"wl_redshift_model0_WLz01.880307e-01_WLalpha8.485694e-01.txt", "wl_redshift_model1_WLz01.731166e-01_WLalpha7.662434e-01.txt", "wl_redshift_model2_WLz01.846221e-01_WLalpha8.297540e-01.txt", "wl_redshift_model3_WLz01.758423e-01_WLalpha7.812893e-01.txt", "wl_redshift_model4_WLz01.721357e-01_WLalpha7.608290e-01.txt", "wl_redshift_model5_WLz01.931476e-01_WLalpha8.768147e-01.txt", "wl_redshift_model6_WLz01.919821e-01_WLalpha8.703814e-01.txt", "wl_redshift_model7_WLz01.751912e-01_WLalpha7.776952e-01.txt", "wl_redshift_model8_WLz01.741405e-01_WLalpha7.718958e-01.txt", "wl_redshift_model9_WLz01.860048e-01_WLalpha8.373865e-01.txt", "wl_redshift_model10_WLz01.888904e-01_WLalpha8.533151e-01.txt", "wl_redshift_model11_WLz01.954564e-01_WLalpha8.895592e-01.txt", "wl_redshift_model12_WLz01.945099e-01_WLalpha8.843347e-01.txt", "wl_redshift_model13_WLz01.853877e-01_WLalpha8.339802e-01.txt", "wl_redshift_model14_WLz01.775192e-01_WLalpha7.905458e-01.txt", "wl_redshift_model15_WLz01.925611e-01_WLalpha8.735775e-01.txt", "wl_redshift_model16_WLz01.708849e-01_WLalpha7.539247e-01.txt", "wl_redshift_model17_WLz01.733832e-01_WLalpha7.677150e-01.txt", "wl_redshift_model18_WLz01.820009e-01_WLalpha8.152851e-01.txt", "wl_redshift_model19_WLz01.767381e-01_WLalpha7.862345e-01.txt", "wl_redshift_model20_WLz01.866426e-01_WLalpha8.409073e-01.txt", "wl_redshift_model21_WLz01.877261e-01_WLalpha8.468882e-01.txt", "wl_redshift_model22_WLz01.826188e-01_WLalpha8.186960e-01.txt", "wl_redshift_model23_WLz01.768086e-01_WLalpha7.866234e-01.txt", "wl_redshift_model24_WLz01.802711e-01_WLalpha8.057362e-01.txt", "wl_redshift_model25_WLz01.897506e-01_WLalpha8.580632e-01.txt", "wl_redshift_model26_WLz01.831217e-01_WLalpha8.214720e-01.txt", "wl_redshift_model27_WLz01.906926e-01_WLalpha8.632630e-01.txt", "wl_redshift_model28_WLz01.714197e-01_WLalpha7.568766e-01.txt", "wl_redshift_model29_WLz01.798667e-01_WLalpha8.035040e-01.txt", "wl_redshift_model30_WLz01.842554e-01_WLalpha8.277299e-01.txt", "wl_redshift_model31_WLz01.901797e-01_WLalpha8.604322e-01.txt", "wl_redshift_model32_WLz01.937710e-01_WLalpha8.802561e-01.txt", "wl_redshift_model33_WLz01.790701e-01_WLalpha7.991072e-01.txt", "wl_redshift_model34_WLz01.811701e-01_WLalpha8.106987e-01.txt", "wl_redshift_model35_WLz01.781525e-01_WLalpha7.940419e-01.txt"};

  char lens_zfile[36][400]={"LSS_redshift_model0_LSSz02.629496e-01_LSSalpha9.285983e-01.txt","LSS_redshift_model1_LSSz02.852923e-01_LSSalpha8.985943e-01.txt","LSS_redshift_model2_LSSz02.664822e-01_LSSalpha9.186036e-01.txt","LSS_redshift_model3_LSSz02.798758e-01_LSSalpha9.014201e-01.txt","LSS_redshift_model4_LSSz02.873873e-01_LSSalpha8.978683e-01.txt","LSS_redshift_model5_LSSz02.593970e-01_LSSalpha9.470921e-01.txt","LSS_redshift_model6_LSSz02.600213e-01_LSSalpha9.425114e-01.txt","LSS_redshift_model7_LSSz02.811155e-01_LSSalpha9.006370e-01.txt","LSS_redshift_model8_LSSz02.831875e-01_LSSalpha8.995165e-01.txt","LSS_redshift_model9_LSSz02.649368e-01_LSSalpha9.224338e-01.txt","LSS_redshift_model10_LSSz02.622058e-01_LSSalpha9.314128e-01.txt","LSS_redshift_model11_LSSz02.584820e-01_LSSalpha9.568082e-01.txt","LSS_redshift_model12_LSSz02.588053e-01_LSSalpha9.527220e-01.txt","LSS_redshift_model13_LSSz02.656075e-01_LSSalpha9.206866e-01.txt","LSS_redshift_model14_LSSz02.768397e-01_LSSalpha9.037492e-01.txt","LSS_redshift_model15_LSSz02.596975e-01_LSSalpha9.447600e-01.txt","LSS_redshift_model16_LSSz02.901709e-01_LSSalpha8.971658e-01.txt","LSS_redshift_model17_LSSz02.847362e-01_LSSalpha8.988183e-01.txt","LSS_redshift_model18_LSSz02.698330e-01_LSSalpha9.121821e-01.txt","LSS_redshift_model19_LSSz02.782257e-01_LSSalpha9.026084e-01.txt","LSS_redshift_model20_LSSz02.642756e-01_LSSalpha9.243038e-01.txt","LSS_redshift_model21_LSSz02.632273e-01_LSSalpha9.276296e-01.txt","LSS_redshift_model22_LSSz02.689934e-01_LSSalpha9.135968e-01.txt","LSS_redshift_model23_LSSz02.780987e-01_LSSalpha9.027073e-01.txt","LSS_redshift_model24_LSSz02.723464e-01_LSSalpha9.085463e-01.txt","LSS_redshift_model25_LSSz02.615210e-01_LSSalpha9.343470e-01.txt","LSS_redshift_model26_LSSz02.683327e-01_LSSalpha9.147934e-01.txt","LSS_redshift_model27_LSSz02.608392e-01_LSSalpha9.376962e-01.txt","LSS_redshift_model28_LSSz02.889655e-01_LSSalpha8.974355e-01.txt","LSS_redshift_model29_LSSz02.729686e-01_LSSalpha9.077654e-01.txt","LSS_redshift_model30_LSSz02.669178e-01_LSSalpha9.176391e-01.txt","LSS_redshift_model31_LSSz02.612016e-01_LSSalpha9.358553e-01.txt","LSS_redshift_model32_LSSz02.591077e-01_LSSalpha9.496317e-01.txt","LSS_redshift_model33_LSSz02.742326e-01_LSSalpha9.063038e-01.txt","LSS_redshift_model34_LSSz02.710103e-01_LSSalpha9.103760e-01.txt","LSS_redshift_model35_LSSz02.757517e-01_LSSalpha9.047459e-01.txt"};

double shear_prior[36]={0.00891915,0.0104498 ,0.0145972 ,0.0191916 ,0.00450246 ,0.00567828 ,0.00294841 ,0.00530922 ,0.0118632 ,0.0151849 ,0.00410151 ,0.0170622 ,0.0197331 ,0.0106615 ,0.0124445 ,0.00994507 ,0.0136251 ,0.0143491 ,0.0164314 ,0.016962 ,0.0186608 ,0.00945903 ,0.0113246 ,0.0155225 ,0.00800846 ,0.00732104 ,0.00649453 ,0.00243976 ,0.0125932 ,0.0182587 ,0.00335859 ,0.00682287 ,0.0177269 ,0.0035219 ,0.00773304 ,0.0134886};

double delta_z_prior[36]={0.0032537,0.00135316,0.00168787,0.00215043,0.00406031,0.00222358,0.00334993,0.00255186,0.00266499,0.00159226,0.00183664,0.00384965,0.00427765,0.00314377,0.00456113,0.00347868,0.00487938,0.00418152,0.00469911,0.00367598,0.0028009,0.00234161,0.00194964,0.00200982,0.00122739,0.00310886,0.00275168,0.00492736,0.00437241,0.00113931,0.00104864,0.00292328,0.00452082,0.00394114,0.00150756,0.003613};

double sigma_z[36]={0.0849973 ,0.0986032 ,0.0875521 ,0.0968222 ,0.0225239 ,0.0718278 ,0.0733675 ,0.0385274 ,0.0425549 ,0.0605867 ,0.0178555 ,0.0853407 ,0.0124119 ,0.0531027 ,0.0304032 ,0.0503145 ,0.0132213 ,0.0941765 ,0.0416444 ,0.0668198 ,0.063227 ,0.0291332 ,0.0481633 ,0.0595606 ,0.0818742 ,0.0472518 ,0.0270185 ,0.0767401 ,0.0219945 ,0.0902663 ,0.0779705 ,0.0337666 ,0.0362358 ,0.0692429 ,0.0558841 ,0.0150457};

double sigma_z_prior[36]={0.00331909,0.00529541,0.00478151,0.00437497,0.00443062,0.00486333,0.00467423,0.0036723,0.00426963,0.00515357,0.0054553,0.00310132,0.00305971,0.00406327,0.00594293,0.00348709,0.00562526,0.00396025,0.00540537,0.00500447,0.00318595,0.00460592,0.00412137,0.00336418,0.00524988,0.00390092,0.00498349,0.0056667,0.0036384,0.00455861,0.00554822,0.00381061,0.0057615,0.00357705,0.00590572,0.00422393};

  init_cosmo_runmode("halofit");
  init_bary("dmo");
  init_binning_fourier(15,20.0,3000.0,3000.0,21.0,10,10);
  init_priors(shear_prior[sce],sigma_z[sce],delta_z_prior[sce],sigma_z_prior[sce],sigma_z[sce]*0.6,delta_z_prior[sce],sigma_z_prior[sce],3.0,1.2,3.8,2.0,16.0,5.0,0.8);
  init_survey(survey_designation[0],nsource_table[sce],nlens_table[sce],area_table[sce]);
  sprintf(arg1,"zdistris/%s",source_zfile[sce]);
  sprintf(arg2,"zdistris/%s",lens_zfile[sce]);
  init_galaxies(arg1,arg2,"gaussian","gaussian",tomo_binning_source[0],tomo_binning_lens[0]);
  init_IA("NLA_HF","GAMA");
  init_probes("3x2pt");

  //init_Pdelta("emu",0.8,0.35);
  // init_Pdelta("linear",0.8,0.35);

  // for (i =0; i< 10; i++){
  //   save_zdistr_sources(i);
  // }
  // for (i =0; i< 10; i++){
  //   save_zdistr_lenses(i);
  // }
//   double Omega;
//   char filename[300];
// for (i=0;i<200; i++){
//   Omega=0.05+i*0.002;
//   sprintf(filename,"test_fid_%d", i);
//compute_data_vector(filename,Omega,0.831,0.9645,-1.,0.,0.0491685,0.6727,0.,0.,1.3,1.4,1.5,1.6,1.7,1.8,1.9,2.0,2.1,2.2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,5.92,1.1,-0.47,0.0,0.0,0.0,0.0,0.0,0.0,0.0,3.207,0.993,0.0,0.456,0.0,0.0);
// }

sprintf(arg3,"Y10_area%le",area_table[sce]);
compute_data_vector(arg3,0.3156,0.831,0.9645,-1.,0.,0.0491685,0.6727,0.,0.,gbias.b[0],gbias.b[1],gbias.b[2],gbias.b[3],gbias.b[4],gbias.b[5],gbias.b[6],gbias.b[7],gbias.b[8],gbias.b[9],0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,sigma_z[sce],0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,sigma_z[sce]*0.6,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,5.92,1.1,-0.47,0.0,0.0,0.0,0.0,0.0,0.0,0.0,3.207,0.993,0.0,0.456,0.0,0.0,0.0,0.0,0.0);

// compute_data_vector("mu1_Sigma0",0.3156,0.831,0.9645,-1.,0.,0.0491685,0.6727,0.,1.,1.35,1.5,1.65,1.8,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.01,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,5.92,1.1,-0.47,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.72+log(1.e+14*0.7),1.08,0.0,0.25,0.9,0.9,0.9,0.9);
  // compute_data_vector("mu1_Sigma1",0.3156,0.831,0.9645,-1.,0.,0.0491685,0.6727,1.,1.,1.35,1.5,1.65,1.8,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.01,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,5.92,1.1,-0.47,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.72+log(1.e+14*0.7),1.08,0.0,0.25,0.9,0.9,0.9,0.9);

  // init_data_inv("cov/WFIRST_3x2pt_clusterN_clusterWL_inv","datav/WFIRST_all_2pt_clusterN_clusterWL_fid");

  // begin = clock();
  // log_multi_like(0.3156,0.831,0.9645,-1.,0.,0.0491685,0.6727,0.,0.,1.3,1.4,1.5,1.6,1.7,1.8,1.9,2.0,2.1,2.2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.05,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,5.92,1.1,-0.47,0.0,0.0,0.0,0.0,0.0,0.0,0.0,3.207,0.993,0.0,0.456,0.0,0.0);
  // // printf("knonlin %le\n",nonlinear_scale_computation(1.0));
  // // printf("knonlin %le\n",nonlinear_scale_computation(0.5));
  // end = clock();
  // time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
  // printf("timespent %le\n",time_spent);

  return 0;
}


#ifndef PDCD_SMSVM_H
#define PDCD_SMSVM_H

#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <math.h>

//This class implements the method SMART_CD.

/*
The optimization problem to solve is of the form f(x)+g(x)+h(Mx) ;
 where f(x)=sum_{j=1}^m lambda_f[j] phi_j(<A_j,x>) and g(x)=sum_{i=1}^n g_i(x_i), h(x)= |x|_1. We all assume that each phi_j is 1-smooth.
*/

template<typename L, typename D>
class PDCD_SMSVM
{
private:

  std::vector<D> Au;
  std::vector<D> Az;

  std::vector<D> Mu;
  std::vector<D> Mz;

  std::vector<D> Mx;
  std::vector<D> Ax;
  std::vector<D> lambda_f;

protected:

  // parameters

  D mu_f;

  D mu_psi;

  D mu_g;

  L n;   // x\in \R^n

  L tau;  //number of threads on each node/computer

  D sumofLi;

  // variables
  std::vector<D> u;

  std::vector<D> z;

  std::vector<D> x;

  std::vector<D> v;

  std::vector<D> L_M;

  std::vector<D> L_f;

  std::vector<D> t;

  D gamma;

  D theta;

  D theta0;

  L m_1;

  L m_2;

  // sampling variables

  std::vector<D> proba_vector;

  std::vector<D> S;

  std::vector<D> all_n;

  std::vector<D> sampled;

  D max_p;

  D min_p;

  // auxiliary variables

  L max_nb_loops;

  L evaluation;

  ofstream samp;

  L nb_iters;

  L nb_of_iters_per_loop;

  L print_every_N;

  D running_time;

  Matrix<L,D> data_A;

  Matrix<L, D> data_M;

  std::vector<D> lambda;

  std::vector<D> M_tlambda;

  D beta_s;

  D function_value;

  D residual1;

  D residual2;

public:

  gsl_rng * rng;

  virtual inline D gradient_of_phi_j(D, L){return D(NULL);}

  virtual inline D value_of_g_i(D, L){return D(NULL);}
  virtual inline D value_of_phi_j(D, L){return D(NULL);}

  virtual inline D prox_of_g_i(D,D,D, L){return D(NULL);}
  virtual inline D value_of_phistar_i(D,L) {return D(NULL);}

  virtual inline void set_matrix_M(){}

  virtual inline void set_matrix_A(){}
  PDCD_SMSVM()
  {

  }

  void set_rng()
  {
    gsl_rng_env_setup();
    const gsl_rng_type * T;
    T = gsl_rng_default;
    rng = gsl_rng_alloc(T);
    gsl_rng_set(rng,time(NULL));
    //gsl_rng_set(rng, 27432042);

  }

// sample i with probability pi=proba_vector[i]
  L sampling()
  {
    //L i=(floor)(gsl_rng_uniform(rng)*n);
    L i=gsl_rng_uniform_int(rng, n);
    if(tau==1)
    {
      D y=gsl_rng_uniform(rng);
      while(y*max_p>proba_vector[i])
      {
        i=(floor)(gsl_rng_uniform(rng)*n);
        y=gsl_rng_uniform(rng);
      }
    }
    return i;
  }

// sample S
  void batch_sampling()
  {
    if(tau<n)
    {
      L i=sampling();
      for(L k=0;k<tau;k++)
      {
        while(sampled[i]==1)
        {
          i=sampling();
        }
        sampled[i]=1;
        S[k]=i;
      }
      for(L k=0;k<tau;k++)
      {
        sampled[S[k]]=0;
      }
    }
    else {
      S=all_n;
      //cout<<"s=all_n"<<endl;
    }
  }

  void compute_x()
  {
    for(L i=0;i<n;i++){
    x[i]=gamma*u[i]+z[i];
	}
	for(L j=0;j<m_2;j++){
        Mx[j]=gamma*Mu[j]+Mz[j];
    }
    for(L j=0;j<m_1;j++){
        Ax[j]=gamma*Au[j]+Az[j];
    }
  }

  inline void compute_function_value() {
     D res=0;
     D res2= 0;
     D tmp1= 0;
     for(L i=0;i<this->n;i++){
       res+=value_of_g_i(x[i],i);
     }
     for(L j=0;j<m_1;j++){
       res+=lambda_f[j]*value_of_phi_j(Ax[j],j);
     }
     for(L j=0;j<m_2;j++){
     	if (1- Mx[j]> tmp1){
     		tmp1= 1- Mx[j];
		 }
        res2+=max(0.0,1- Mx[j]);
     }
     residual1= tmp1;
     residual2= res2/m_2;
     function_value=res+ m_2*residual2;;
   }

  void initialize(D beta_0, vector<D> & x0, vector<D> & y0, L val_tau, D p_N, D val_lambda_f)
  {
  	set_matrix_M();
    set_matrix_A();

    this->tau=val_tau;
    m_1=data_A.get_n();
    m_2=data_M.get_n();
    cout<<"m_1="<<m_1<<endl;
    cout<<"m_2="<<m_2<<endl;
    lambda_f.resize(m_1,val_lambda_f);
    n=data_A.nfeatures;
    tau=val_tau;
    all_n.resize(n,0);
    for(L i=0;i<n;i++)
    all_n[i]=i;
    print_every_N= p_N;
    nb_of_iters_per_loop=floor(max(1.,n/(tau+0.0)));
    lambda.resize(m_2,0);
    for(L j=0;j<m_2;j++)
           lambda[j]=y0[j];

    u.clear();
    u.resize(n,0);
    z.clear();
    z.resize(n,0);
    x.clear();
    x.resize(n,0);
    for(L i=0;i<n;i++)
    {
      z[i]=x0[i];
      x[i]=x0[i];
    }
    sampled.clear();
    sampled.resize(n,0);
    S.clear();
    S.resize(tau,0);
    t.clear();
    t.resize(n,0);
    M_tlambda.resize(n,0);
    compute_Mty();

    Au.clear();
    Au.resize(m_1,0);
    Az.clear();
    Az.resize(m_1,0);
    Ax.clear();
    Ax.resize(m_1,0);
    Mu.clear();
    Mu.resize(m_2,0);
    Mz.clear();
    Mz.resize(m_2,0);
    Mx.clear();
    Mx.resize(m_2,0);
    compute_Az(x0);
    compute_Mz(x0);
    compute_Ax(x0);
    compute_Mx(x0);
    beta_s= beta_0;
    set_v();
    set_p();
    set_rng();

    theta0= min_p;
    theta= theta0;
    cout<< "theta0= "<< theta0<< endl;
    gamma= 1- theta0;

    cout<<"finished SMART_CD initializing"<<endl;
  }

  inline void compute_and_record_res(){
  	    if (nb_iters%print_every_N== 0){
  	    cout<< "gamma= "<< gamma<< " theta= "<< theta<< " beta_s= "<< beta_s<< endl;
        //compute_KKT_residual();
        compute_function_value();
        cout<<setprecision(9)<<"Iteration: "<<nb_iters<<"; time="<<running_time<<"; infeasibility= "<< residual1<<" "<< residual2<<"; function value="<<function_value<< endl;
        samp<<setprecision(9)<< nb_iters<<" "<<running_time<<" "<<residual1<< " "<< residual2<< " "<< function_value<<" "<<endl;
        }
   }

  inline D partial_i_of_f(L i)
  {
  	D res=0;
      for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
      {
        L j=data_A.col_idx[k];
        D tmp=lambda_f[j]*gradient_of_phi_j(gamma*Au[j]+Az[j], j);
        res+=data_A.A_t[k]*tmp;
      }
      for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
      {
        L j=data_M.col_idx[k];
        D tmp=(gamma*Mu[j]+Mz[j]- 1)/beta_s+lambda[j];
        if (tmp> 0){
        	tmp= 0;
		}
		else if( tmp< -1){
			tmp= -1;
		}
        res+=data_M.A_t[k]*tmp;
      }
      return res;
  }

  inline void set_v()
   {
     v.resize(n,0);
     L_f.resize(n,0);
     L_M.resize(n,0);
     D maxv=0;
     D minv=std::numeric_limits<double>::max();
     D sumv=0;
     D sumvi1=0;
     L sumw=0;
     L maxw=0;
     L minw=n;
     sumofLi=0;
     for(L j=0;j<m_1;j++)
     {
       sumw+=data_A.w_t[j];
       maxw=max(maxw,data_A.w_t[j]);
       minw=min(minw,data_A.w_t[j]);
     }
     cout<<"sumw="<<sumw<<";  maxw="<<maxw<<"; minw="<<minw<<endl;
     for(L i=0;i<n;i++)
     {
       D lfi=0;
       D lmi=0;
       D lfi1= 0;
       D lmi1= 0;
       for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
       {
         L j=data_A.col_idx[k];
         lfi+=(1.+(data_A.w_t[j]-1.)*(tau-1.)/max(n-1.,1.))*data_A.A_t[k]*data_A.A_t[k]*lambda_f[j];
         lfi1+=data_A.A_t[k]*data_A.A_t[k]*lambda_f[j];
       }
       for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
       {
         L j=data_M.col_idx[k];
         lmi+=(1.+(data_M.w_t[j]-1.)*(tau-1.)/max(n-1.,1.))*data_M.A_t[k]*data_M.A_t[k];
         lmi1+=data_M.A_t[k]*data_M.A_t[k];
       }
       L_f[i]=lfi;
       L_M[i]= lmi;
       sumv+=lfi;
       sumvi1+=lfi1;
       if(maxv<lfi) maxv=lfi;
       if(minv>lfi) minv=lfi;
     }
     if(tau==n){
          for(L i=0;i<n;i++)
              L_f[i]=sumvi1;
     }
     sumofLi=sumvi1;
     cout<<"  max of v: "<<maxv<<" ;  min of v: "<<minv<<" ;  sumofv: "<<sumv<<" sumofLi="<<sumofLi<<endl;
   }

   inline void set_p(){
     proba_vector.resize(n,0.0);
     D res= 0;
     D tmp= 1;
     D tmp2= 0;
     for (L i=0; i< n; i++){
     	res+= sqrt(L_f[i]+ L_M[i]/beta_s);
	 }
	 for (L i= 0; i< n; i++){
	 	proba_vector[i]= sqrt(L_f[i]+ L_M[i]/beta_s)/res;
	 	if (proba_vector[i]< tmp){
	 		tmp= proba_vector[i];
		 }
		if (proba_vector[i]> tmp2){
	 		tmp2= proba_vector[i];
		 }
	 }
	 if (tmp== 0){
	 	for (L i= 0; i< n; i++){
	 	proba_vector[i]= 1.0/n;
	    }
	    min_p= 1.0/n;
	    max_p= 1.0/n;
	 }
	 else{
	    min_p= tmp;
	    max_p= tmp2;
     }
	 cout<< "max_p= "<< max_p<< "; min_p= "<< min_p<< endl;
   }

  inline void update_z_coordinate( L i, D dz){
     z[i]+=dz;
     for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
     {
       L j=data_A.col_idx[k];
       Az[j]+=dz*data_A.A_t[k];
     }
     for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
     {
       L j=data_M.col_idx[k];
       Mz[j]+=dz*data_M.A_t[k];
       //if(j==2869) cout<<"gamma="<<gamma<<"; "<<dz<<"; "<<Mz[j]<<endl;
     }
   }

    inline void update_x_coordinate( L i, D dx){
     x[i]+=dx;
     x[i]=x[i];
     L j;
     for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
     {
       j=data_M.col_idx[k];
       Mx[j]+=dx*data_M.A_t[k];
     }
      for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
     {
       j=data_A.col_idx[k];
       Ax[j]+=dx*data_A.A_t[k];
     }
   }

   inline void update_u_coordinate( L i, D du){
     u[i]+=du;
     for (L k = data_A.ptr_t[i]; k < data_A.ptr_t[i + 1];k++)
     {
       L j=data_A.col_idx[k];
       Au[j]+=du*data_A.A_t[k];
     }
     for (L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++)
     {
       L j=data_M.col_idx[k];
       Mu[j]+=du*data_M.A_t[k];
     }
   }

   void compute_Au(){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Au[j]+=u[kj]*data_A.A[k];
       }
   }

   void compute_Mu(){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mu[j]+=u[kj]*data_M.A[k];
       }
   }

   void compute_Az(){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Az[j]+=z[kj]*data_A.A[k];
       }
   }
   void compute_Mz(){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mz[j]+=z[kj]*data_M.A[k];
       }
   }

   void compute_Au(vector<D> & x0){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Au[j]+=x0[kj]*data_A.A[k];
       }

   }

   void compute_Az(vector<D> & x0){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Az[j]+=x0[kj]*data_A.A[k];
       }
   }
   void compute_Mu(vector<D> & x0){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mu[j]+=x0[kj]*data_M.A[k];
       }

   }

   void compute_Mz(vector<D> & x0){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mz[j]+=x0[kj]*data_M.A[k];
       }
   }

     void compute_Mx(vector<D> & x0){
     for(L j=0;j<m_2;j++)
       for(L k = data_M.ptr[j]; k < data_M.ptr[j + 1];k++){
         L kj=data_M.row_idx[k];
         Mx[j]+=x0[kj]*data_M.A[k];
       }
   }

   void compute_Ax(vector<D> & x0){
     for(L j=0;j<m_1;j++)
       for(L k = data_A.ptr[j]; k < data_A.ptr[j + 1];k++){
         L kj=data_A.row_idx[k];
         Ax[j]+=x0[kj]*data_A.A[k];
       }
   }

  void compute_Mty(){
        M_tlambda.clear();
        M_tlambda.resize(this->n,0);
        for(L i=0;i<this->n;i++)
        {
          for(L k = data_M.ptr_t[i]; k < data_M.ptr_t[i + 1];k++){
            L j=data_M.col_idx[k];
            M_tlambda[i]+=lambda[j]*data_M.A_t[k];
           }
        }

    }

    void update_theta()
  {

	 theta= (sqrt(theta*theta*theta*theta+ 4*theta*theta)- theta*theta)/2;

  }

  void reset(){
  	beta_s= beta_s/(1+ theta);
  }

  void PDCD_SMSVM_solver(D beta_0, vector<D> & x0,vector<D> & y0, L val_tau, L max_nb_epoch, L p_N, D val_lambda_f,string filename1, D time2)
  {
    initialize(beta_0, x0, y0, val_tau,  p_N, val_lambda_f);
    cout<<"running SMART_CD"<<" ; "<<filename1<<" max_nb_epoch "<<max_nb_epoch<<endl;
    nb_iters=0;
    filename1="results/PDCD_SMSVM_"+filename1;
    samp.open(filename1.c_str());
    //srand48(27432042);
    srand(time(NULL));
    running_time= 0;
    print_every_N=p_N;
    compute_and_record_res();
    D start;
    start = std::clock();
    while(nb_iters<max_nb_epoch)
    {
      start = std::clock();
      D ti=0;
      //D si=0;
      D gi=0;
      D Li=0;
      L i=0;
      for(L it=0;it<nb_of_iters_per_loop;it++)
      {
        gamma*=(1-theta);
        if(theta==1) gamma=1;
        batch_sampling();
        for(L it_S=0;it_S<tau;it_S++)
        {
          i=S[it_S];
          gi=partial_i_of_f(i);
          Li=(L_f[i]+ L_M[i]/beta_s)*theta/theta0;
          t[i]=prox_of_g_i(gi, Li, z[i],i);
        }
        for(L it_S=0;it_S<tau;it_S++)
        {
          i=S[it_S];
          ti=t[i];
          update_z_coordinate(i, ti);
          update_u_coordinate(i, -(1- theta/theta0)/gamma*ti);
        }
        update_theta();
        reset();
      }
      running_time+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
      nb_iters++;
      compute_x();
      compute_and_record_res();
      if (running_time> time2){
      	break;
	  }
    }

  }

};

#endif