#include <math.h>
#include <stdio.h>
#include <stdarg.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_statistics_double.h>
#include "../include/type.h"
#include "../include/util.h"
#include "../include/myfunc.h"
#include "../include/global.h"
#include "../include/cthreadpool.h"

extern double rho;
extern double cutoff;
extern clock_t dur;

#define ITER_CUTOFF 0.01

/**
 * @brief invoke this function before return from
 *        MLE_marginal_iteration and MLE_marginal_iteration_constrain.
 *
 * @param mle
 * @param sum
 * @param psi1
 * @param psi2
 * @param beta0
 * @param beta1
 * @param var1
 * @param var2
 */
void mle_result_set(mle_result* mle, double sum, gsl_vector* psi1, gsl_vector* psi2,
                    double beta0, double beta1, double var1, double var2) {
    mle->sum = sum;
    mle->params.psi1 = psi1;
    mle->params.psi2 = psi2;
    mle->params.beta0 = beta0;
    mle->params.beta1 = beta1;
    mle->params.var1 = var1;
    mle->params.var2 = var2;
    return;
}

double myfunc_multivar(const double x[], va_list argv) {
    gsl_vector *psi1 = va_arg(argv, gsl_vector*);
    gsl_vector *psi2 = va_arg(argv, gsl_vector*);
    double var1 = va_arg(argv, double), var2 = va_arg(argv, double);
    double sum1, sum2;

    // TODO  memcpy replaced.
    sum1 = cuscumsum(psi1, sum_for_multivar, 1, x[0]);
    sum2 = cuscumsum(psi2, sum_for_multivar, 1, x[1]);
    sum1 = sum1/(var1*2), sum2 = sum2/(var2*2);

    return sum1 + sum2 + 0.05 * pow(rho, 2) / (1 - pow(rho, 2)) * \
        (pow(gsl_cdf_gaussian_Pinv(x[0], 1), 2) + pow(gsl_cdf_gaussian_Pinv(x[1], 1), 2) - \
         2 * rho * gsl_cdf_gaussian_Pinv(x[0], 1) * gsl_cdf_gaussian_Pinv(x[1], 1));
}

void myfunc_multivar_der(const double x[], double res[], va_list argv) {
    gsl_vector *psi1 = va_arg(argv, gsl_vector*);
    gsl_vector *psi2 = va_arg(argv, gsl_vector*);
    double var1 = va_arg(argv, double), var2 = va_arg(argv, double);
    double sum1, sum2, tmp = pow(rho,2)/(1-pow(rho,2));
    double ppfx0 = gsl_cdf_gaussian_Pinv(x[0], 1);
    double ppfx1 = gsl_cdf_gaussian_Pinv(x[1], 1);

    sum1 = cuscumsum(psi1, sum_for_multivar_der, 1, x[0]);
    sum2 = cuscumsum(psi2, sum_for_multivar_der, 1, x[1]);
    sum1 = sum1/(var1*2), sum2 = sum2/(var2*2);

    // TODO are there any better way to convert cdf to pdf?
    res[0] = sum1 + tmp * 0.1*(ppfx0-rho*ppfx1)/gsl_ran_gaussian_pdf(ppfx0, 1);
    res[1] = sum2 + tmp * 0.1*(ppfx1-rho*ppfx0)/gsl_ran_gaussian_pdf(ppfx1, 1);

    return;
}

double myfunc_1_2(const double x[], va_list argv) {
    int flag = va_arg(argv, int);
    gsl_vector *psi1 = va_arg(argv, gsl_vector*);
    gsl_vector *psi2 = va_arg(argv, gsl_vector*);
    double var1 = va_arg(argv, double), var2 = va_arg(argv, double);
    double sum1, sum2;

    if (flag == 1) {
        sum1 = cuscumsum(psi1, sum_for_multivar, 1, x[0]+cutoff);
        sum2 = cuscumsum(psi2, sum_for_multivar, 1, x[0]);
    } else {
        sum1 = cuscumsum(psi1, sum_for_multivar, 1, x[0]);
        sum2 = cuscumsum(psi2, sum_for_multivar, 1, x[0]+cutoff);
    }
    sum1 = sum1/(var1*2), sum2 = sum2/(var2*2);

    return sum1 + sum2 + 0.05 * pow(rho, 2) / (1 - pow(rho, 2)) * \
        (pow(gsl_cdf_gaussian_Pinv(x[0]+cutoff, 1), 2) + pow(gsl_cdf_gaussian_Pinv(x[0], 1), 2) - \
         2 * rho * gsl_cdf_gaussian_Pinv(x[0]+cutoff, 1) * gsl_cdf_gaussian_Pinv(x[0], 1));
}

void myfunc_der_1_2(const double x[], double res[], va_list argv) {
    int flag = va_arg(argv, int);
    gsl_vector *psi1 = va_arg(argv, gsl_vector*);
    gsl_vector *psi2 = va_arg(argv, gsl_vector*);
    double var1 = va_arg(argv, double), var2 = va_arg(argv, double);
    double sum1, sum2, tmp = pow(rho,2)/(1-pow(rho,2));
    double ppfx0 = gsl_cdf_gaussian_Pinv(x[0], 1);
    double ppfxc = gsl_cdf_gaussian_Pinv(x[0]+cutoff, 1);

    if (flag == 1) {
        sum1 = cuscumsum(psi1, sum_for_multivar_der, 1, x[0]+cutoff);
        sum2 = cuscumsum(psi2, sum_for_multivar_der, 1, x[0]);
    } else {
        sum1 = cuscumsum(psi1, sum_for_multivar_der, 1, x[0]);
        sum2 = cuscumsum(psi2, sum_for_multivar_der, 1, x[0]+cutoff);
    }
    sum1 = sum1/(var1*2), sum2 = sum2/(var2*2);

    // TODO are there any better way to convert cdf to pdf?
    res[0] = sum1 + tmp * 0.1*(ppfxc-rho*ppfx0)/gsl_ran_gaussian_pdf(ppfxc, 1);
    res[1] = sum2 + tmp * 0.1*(ppfx0-rho*ppfxc)/gsl_ran_gaussian_pdf(ppfx0, 1);
    res[0] += res[1];

    return;
}

double myfunc_marginal(const double x[], va_list argv) {
    gsl_vector* I = va_arg(argv, gsl_vector*), *S = va_arg(argv, gsl_vector*);
    gsl_vector* psi = va_arg(argv, gsl_vector*);
    double var = va_arg(argv, double), sum = 0;
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int), idx = 0;

    sum = cuscumsum(psi, sum_for_marginal, 7, &idx, x[0], I, S, var, inclu_len, skip_len);

    return sum;
}

void myfunc_marginal_der(const double x[], double res[], va_list argv) {
    gsl_vector* I = va_arg(argv, gsl_vector*), *S = va_arg(argv, gsl_vector*);
    gsl_vector* psi = va_arg(argv, gsl_vector*);
    double var = va_arg(argv, double), sum;
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int), idx = 0;

    sum = cuscumsum(psi, sum_for_marginal_der, 7, &idx, x[0], I, S, var, inclu_len, skip_len);
    res[0] = sum;

    return;
}

double myfunc_marginal_1_2(const double x[], va_list argv) {
    int flag = va_arg(argv, int);
    double beta1, beta2;
    if (flag == 1) {
        beta1 = x[0] + cutoff, beta2 = x[0];
    } else {
        beta1 = x[0], beta2 = x[0] + cutoff;
    }
    // TODO parameter ordering
    return myfunc_marginal(&beta1, argv) + myfunc_marginal(&beta2, argv);
}

void myfunc_marginal_1_2_der(const double x[], double res[], va_list argv) {
    int flag = va_arg(argv, int);
    double beta1, beta2;
    double tmp1, tmp2;
    if (flag == 1) {
        beta1 = x[0] + cutoff, beta2 = x[0];
    } else {
        beta1 = x[0], beta2 = x[0] + cutoff;
    }
    // TODO parameter ordering
    myfunc_marginal_der(&beta1, &tmp1, argv);
    myfunc_marginal_der(&beta2, &tmp2, argv);
    res[0] = tmp1 + tmp2;

    return;
}

double myfunc_individual(const double x[], va_list argv) {
    double I = va_arg(argv, double), S = va_arg(argv, double);
    double beta = va_arg(argv, double), var = va_arg(argv, double);
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int);
    double new_psi = inclu_len * x[0]/(inclu_len * x[0] + skip_len * (1 - x[0]));

    // TODO This change the result.
    return pow((logit(x[0]) - logit(beta)), 2)/(2 * var) -
           (I * log(new_psi) + S * log(1 - new_psi) - log(x[0]) - log(1-x[0]));
    // return -(I * log(new_psi) + S * log(1 - new_psi) -
    //         pow((logit(x[0]) - logit(beta)), 2)/(2 * var) + log(x[0] - x[0]*x[0]));
    // return -(I * log(new_psi) + S * log(1 - new_psi) -
    //         pow((logit(x[0]) - logit(beta)), 2)/(2 * var) + log(x[0]) + log(1-x[0]));
}

void myfunc_individual_der(const double x[], double res[], va_list argv) {
    double I = va_arg(argv, double), S = va_arg(argv, double);
    double beta = va_arg(argv, double), var = va_arg(argv, double);
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int);
    double new_psi = inclu_len * x[0]/(inclu_len * x[0] + skip_len * (1 - x[0]));
    double new_psi_der = inclu_len * skip_len/pow(inclu_len * x[0] + skip_len * (1 - x[0]), 2);

    res[0] = 1/x[0] + S/(1 - new_psi) * new_psi_der + \
             (logit(x[0]) - logit(beta))/(var * x[0] * (1 - x[0])) - (1/(1-x[0]) + I/new_psi * new_psi_der);

    return;
}

#define individual_for_loop() \
    for (idx = 0; idx < psi1->size; ++idx) { \
        x[0] = gsl_vector_get(psi1, idx); \
        cur_sum += l_bfgs_b_wrapper(n, m, x, l, u, nbd, \
                                    myfunc_individual, myfunc_individual_der, \
                                    factr, pgtol, iprint, 15000, 15000, \
                                    6, gsl_vector_get(i1, idx), gsl_vector_get(s1, idx), \
                                    beta0, var1, inclu_len, skip_len); \
        gsl_vector_set(psi1, idx, x[0]); \
    } \
    for (idx = 0; idx < psi2->size; ++idx) { \
        x[0] = gsl_vector_get(psi2, idx); \
        cur_sum += l_bfgs_b_wrapper(n, m, x, l, u, nbd, \
                                    myfunc_individual, myfunc_individual_der, \
                                    factr, pgtol, iprint, 15000, 15000, \
                                    6, gsl_vector_get(i2, idx), gsl_vector_get(s2, idx), \
                                    beta1, var2, inclu_len, skip_len); \
        gsl_vector_set(psi2, idx, x[0]); \
    } \

int MLE_marginal_iteration(gsl_vector* i1, gsl_vector* i2,
                           gsl_vector* s1, gsl_vector* s2,
                           const int inclu_len, const int skip_len,
                           mle_result* mle) {
    gsl_vector* psi1 = gsl_vector_alloc(i1->size); // TODO deallocation.
    gsl_vector* psi2 = gsl_vector_alloc(i2->size);
    vec2psi(psi1, i1, s1, inclu_len, skip_len);
    vec2psi(psi2, i2, s2, inclu_len, skip_len);
    int iter_max = 100, count = 0, i = 0;
    size_t idx = 0;
    double prev_sum = 0, cur_sum = 0;
    double var1 = 10 * gsl_stats_variance(psi1->data, 1, psi1->size) * (psi1->size-1)/(psi1->size);
    double var2 = 10 * gsl_stats_variance(psi2->data, 1, psi2->size) * (psi2->size-1)/(psi2->size);
    double beta0, beta1, iter_cutoff = 1;
    integer n, m = 10, nbd[nmax];
    doublereal x[nmax], l[nmax], u[nmax], factr=1.0e7, pgtol=1.0e-5, iprint=-1;

    if (var1 <= 0.01 || psi1->size == 1) {
        var1 = 0.01;
    }
    if (var2 <= 0.01 || psi2->size == 1) {
        var2 = 0.01;
    }

    // TODO & or && ?
    // According to original python code 'while((iter_cutoff>0.01)&(count<=iter_maxrun)):',
    // it's a bit arithmetic '&'. However,  it should be a logical 'and' in such senario.
    while((iter_cutoff > ITER_CUTOFF) && (count <= iter_max)) {
        ++count, n = 2;
        x[0] = gsl_stats_mean(psi1->data, 1, psi1->size);
        x[1] = gsl_stats_mean(psi2->data, 1, psi2->size);
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.01;
            u[i] = 0.99;
        }
        l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                         myfunc_multivar, myfunc_multivar_der, factr, pgtol,
                         iprint, 15000, 15000, 4, psi1, psi2, var1, var2);
        beta0 = x[0], beta1 = x[1];
        n = 1, cur_sum = 0;

        individual_for_loop();

        if (count > 1) {
            iter_cutoff = fabs(prev_sum - cur_sum);
        }
        prev_sum = cur_sum;
    }
    if (count > iter_max) {
        mle_result_set(mle, cur_sum, psi1, psi2, 0, 0, var1, var2);
        return 0;
    }

    iter_cutoff = 1, iter_max = 100, count = 0, prev_sum = 0;
    while((iter_cutoff > ITER_CUTOFF) && (count <= iter_max)) {
        ++count, n = 1;
        x[0] = beta0;
        l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                         myfunc_marginal, myfunc_marginal_der,
                         factr, pgtol, iprint, 15000, 15000,
                         6, i1, s1, psi1, var1, inclu_len, skip_len);
        beta0 = x[0];
        x[0] = beta1;
        l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                         myfunc_marginal, myfunc_marginal_der,
                         factr, pgtol, iprint, 15000, 15000,
                         6, i2, s2, psi2, var2, inclu_len, skip_len);
        beta1 = x[0];

        cur_sum = 0;

        individual_for_loop();

        if (count > 1) {
            iter_cutoff = fabs(prev_sum - cur_sum);
        }
        prev_sum = cur_sum;
    }

    gsl_vector_free(psi1);
    gsl_vector_free(psi2);

    if (count > iter_max) {
        mle_result_set(mle, cur_sum, psi1, psi2, 0, 0, var1, var2);
        return 0;
    }

    mle_result_set(mle, cur_sum, psi1, psi2, beta0, beta1, var1, var2);

    return 0;
}

int MLE_marginal_iteration_constrain(gsl_vector* i1, gsl_vector* i2,
                                     gsl_vector* s1, gsl_vector* s2,
                                     const int inclu_len, const int skip_len,
                                     mle_result* mle) {
    gsl_vector* psi1 = gsl_vector_alloc(i1->size); // TODO deallocation.
    gsl_vector* psi2 = gsl_vector_alloc(i2->size);
    vec2psi(psi1, i1, s1, inclu_len, skip_len);
    vec2psi(psi2, i2, s2, inclu_len, skip_len);
    int iter_max = 100, count = 0, i = 0;
    size_t idx = 0;
    double prev_sum = 0, cur_sum = 0, iter_cutoff = 1;
    double var1 = 10 * gsl_stats_variance(psi1->data, 1, psi1->size) * (psi1->size-1)/(psi1->size);
    double var2 = 10 * gsl_stats_variance(psi2->data, 1, psi2->size) * (psi2->size-1)/(psi2->size);
    double beta0, beta1;
    integer n = 1, m = 10, nbd[nmax];
    doublereal x[nmax], l[nmax], u[nmax], factr=1.0e7, pgtol=1.0e-5, iprint=-1;

    if (var1 <= 0.01 || psi1->size == 1) {
        var1 = 0.01;
    }
    if (var2 <= 0.01 || psi2->size == 1) {
        var2 = 0.01;
    }

    // TODO & or && ?
    // According to original python code 'while((iter_cutoff>0.01)&(count<=iter_maxrun)):',
    // it's a bit arithmetic '&'. However,  it should be a logical 'and' in such senario.
    while((iter_cutoff > ITER_CUTOFF) && (count <= iter_max)) {
        ++count;
        x[0] = gsl_stats_mean(psi1->data, 1, psi1->size);
        x[1] = gsl_stats_mean(psi2->data, 1, psi2->size);
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.001;
            u[i] = 0.999-cutoff;
        }
        if (x[0] > x[1]) {
            l_bfgs_b_wrapper(n, m, x+1, l, u, nbd,
                             myfunc_1_2, myfunc_der_1_2, factr, pgtol,
                             iprint, 15000, 15000, 5, 1, psi1, psi2, var1, var2);
            beta1 = fmax(fmin(x[1], 1-cutoff), 0);
            beta0 = beta1 + cutoff;
        } else {
            l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                             myfunc_1_2, myfunc_der_1_2, factr, pgtol,
                             iprint, 15000, 15000, 5, 2, psi1, psi2, var1, var2);
            beta0 = fmax(fmin(x[0], 1-cutoff), 0);
            beta1 = beta0 + cutoff;
        }
        cur_sum = 0;
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.01;
            u[i] = 0.99;
        }

        individual_for_loop();

        if (count > 1) {
            iter_cutoff = fabs(prev_sum - cur_sum);
        }
        prev_sum = cur_sum;
    }
    if (count > iter_max) {
        mle_result_set(mle, cur_sum, psi1, psi2, 0, 0, var1, var2);
        return 0;
    }

    iter_cutoff = 1, iter_max = 100, count = 0, prev_sum = 0;
    while((iter_cutoff > ITER_CUTOFF) && (count <= iter_max)) {
        ++count;
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.001;
            u[i] = 0.999-cutoff;
        }
        x[0] = beta0;
        x[1] = beta1;
        if (gsl_stats_mean(psi1->data, 1, psi1->size) > gsl_stats_mean(psi2->data, 1, psi2->size)) {
            l_bfgs_b_wrapper(n, m, x+1, l, u, nbd,
                             myfunc_marginal_1_2, myfunc_marginal_1_2_der, factr, pgtol,
                             iprint, 15000, 15000, 13, 1, i1, s1, psi1, var1, inclu_len, skip_len,
                             i2, s2, psi2, var2, inclu_len, skip_len);
            beta1 = fmax(fmin(x[1], 1-cutoff), 0);
            beta0 = beta1 + cutoff;
        } else {
            l_bfgs_b_wrapper(n, m, x, l, u, nbd,
                             myfunc_marginal_1_2, myfunc_marginal_1_2_der, factr, pgtol,
                             iprint, 15000, 15000, 13, 2, i1, s1, psi1, var1, inclu_len, skip_len,
                             i2, s2, psi2, var2, inclu_len, skip_len);
            beta0 = fmax(fmin(x[0], 1-cutoff), 0);
            beta1 = beta0 + cutoff;
        }
        cur_sum = 0;
        for (i = 0; i < n; ++i) {
            nbd[i] = 2;
            l[i] = 0.01;
            u[i] = 0.99;
        }

        individual_for_loop();

        if (count > 1) {
            iter_cutoff = fabs(prev_sum - cur_sum);
        }
        prev_sum = cur_sum;
        // printf("%.12f, %.12f, %.12f, %.12f\n", beta0, beta1, iter_cutoff, prev_sum);
    }

    gsl_vector_free(psi1);
    gsl_vector_free(psi2);

    if (count > iter_max) {
        mle_result_set(mle, cur_sum, psi1, psi2, 0, 0, var1, var2);
        return 0;
    }

    mle_result_set(mle, cur_sum, psi1, psi2, beta0, beta1, var1, var2);

    return 0;
}

void* thread_wrapper_for_LT(void* arg) {
    double *ret = (double*)malloc(sizeof(double));
    odiff *data = (odiff*)arg;
    *ret = likelihood_test(data->inc1, data->inc2,
                           data->skp1, data->skp2,
                           data->inclu_len, data->skip_len,
                           data->flag, data->id);
    return (void*)ret;
}

void* batch_wrapper_for_LT(void* arg) {
    batch_datum *args = (batch_datum*)arg;
    int batch_size = args->batch_size, i;
    double *ret = (double*)malloc(sizeof(double)*batch_size);
    odiff *datum = (odiff*)*args->datum;
    odiff data;
    for (i = 0; i < batch_size; ++i) {
        data = datum[i];
        ret[i] = likelihood_test(data.inc1, data.inc2,
                                 data.skp1, data.skp2,
                                 data.inclu_len, data.skip_len,
                                 data.flag, data.id);
    }
    return (void*)ret;
}

double likelihood_test(gsl_vector *i1, gsl_vector *i2, gsl_vector *s1, gsl_vector *s2,
                       int inclu_len, int skip_len, int flag, char* id) {
    printf("Testing %s\n", id);
    if (!flag) {
        printf("1 return from: %s\n", id);
        return 1;
    } else {
        mle_result mle;
        MLE_marginal_iteration(i1, i2, s1, s2, inclu_len, skip_len, &mle);
        if (fabs(mle.params.beta0 - mle.params.beta1) <= cutoff) {
            printf("2 return from: %s\n", id);
			return 1;
        } else {
            mle_result mle_constrain;
            MLE_marginal_iteration_constrain(i1, i2, s1, s2, inclu_len, skip_len, &mle_constrain);
            printf("3 return from: %s\n", id);
            return 1 - gsl_cdf_chisq_P(2 * (fabs(mle_constrain.sum - mle.sum)), 1);
        }
    }
}

int vec2psi(gsl_vector* psi, gsl_vector *inc, gsl_vector *skp,
            int inclu_len, int skip_len) {
    size_t idx;
    for(idx = 0; idx < inc->size; ++idx) {
        if (gsl_vector_get(inc, idx) + gsl_vector_get(skp, idx) == 0) {
            gsl_vector_set(psi, idx, 0.5);
        } else {
            gsl_vector_set(psi, idx, gsl_vector_get(inc, idx)/
                    (gsl_vector_get(inc, idx) + inclu_len * gsl_vector_get(skp, idx)/skip_len));
        }
    }
    return 0;
}


// lu2tlub.c
//
//  blocked LU decomposition library for column-major version
//
// Time-stamp: <11/05/13 11:35:56 makino>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#ifndef NOBLAS
#ifdef MKL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
#endif

#include <lu2lib.h>

typedef double v2df __attribute__((vector_size(16)));
typedef union {v2df v; double s[2];}v2u;

void timer_init();

#define MAXTHREADS 4

static int findpivot0(int n, double a[][n], int current)
{
    double amax = fabs(a[0][current]);
    int i;
    int p=current;
    BEGIN_TSC;
    for(i=current+1;i<n;i++){
	if (fabs(a[0][i]) > amax){
	    amax = fabs(a[0][i]);
	    p = i;
	}
    }
    END_TSC(t,7);
    return p;
}
static int findpivot(int n, double a[][n], int current)
{
    int p;
    int p2;
    BEGIN_TSC;
    p = cblas_idamax(n-current, a[0]+current, 1)+current;
    //    p2 = findpivot0( n, a, current);
    //    printf("n, current, p, p2 = %d %d %d %d\n",n, current,p, p2);
    END_TSC(t,7);
    return p;
}
static int findpivot_sequentical(int n, double a[][n], int current)
{
    double amax = fabs(a[0][current]);
    int i;
    int p=current;
    for(i=current+1;i<n;i++){
	if (fabs(a[0][i]) > amax){
	    amax = fabs(a[0][i]);
	    p = i;
	}
    }
    return p;
}
static int findpivot_omp(int n, double a[][n], int current)
// factor 2 slower than sequential code even for n=8k....
// on Core i7 920
{
    double amax[MAXTHREADS]={-1.0,-1.0,-1.0,-1.0};
    int p[MAXTHREADS];
    double am;
    int pm;
    int di= (n-current-1)/4;
    int k;
    BEGIN_TSC;
    if (di > 1024){
	//#pragma omp parallel for private(k)
	for (k=0;k<MAXTHREADS; k++){
	    int i;
	    int istart = current+1+k*di;
	    int iend = current+1+(k+1)*di;
	    if (iend > n) iend = n;
	    for(i=istart;i<iend;i++){
		if (fabs(a[0][i]) > amax[k]){
		    amax[k] = fabs(a[0][i]);
		    p[k] = i;
		}
	    }
	}
	pm =p[0];
	am = amax[0];
	for (k=1;k<MAXTHREADS; k++){
	    if(amax[k]>am){
		am = amax[k];
		pm=p[k];
	    }
	}
    }else{
	pm=findpivot_sequentical( n,  a, current);
    }
    END_TSC(t,7);
    return pm;
}

static void swaprows(int n, double a[][n], int row1, int row2,
		     int cstart, int cend)
{
    int j;
    if (row1 != row2){
	for(j=cstart;j<cend;j++){
	    double tmp = a[j][row1];
	    a[j][row1] = a[j][row2];
	    a[j][row2]=tmp;
	}
    }
}

static void scalerow( int n, double a[n+1][n], double scale,
	       int row, int cstart, int cend)
{
    int j;
    for(j=cstart;j<cend;j++) a[j][row]*= scale;
}

static void vsmulandsub(int n, double a[n+1][n], int cr, int cc,
		 int c0,  int r0,int r1)
{
    int j,k;
    double * ar = a[cr];
    k=c0;
    double s = a[c0][cc];
    double *al = a[c0];
    while (r0 & 7){
	al[r0] -= ar[r0]*s;
	r0++;
    }
    while (r1 & 7){
	al[r1-1] -= ar[r1-1]*s;
	r1--;
    }
    v2df * arv = (v2df*) (ar+r0);
    v2df * alv = (v2df*) (al+r0);
    v2df ss = (v2df){s,s};
    //    for(j=r0;j<r1;j++)
    //	al[j] -= ar[j]*s;
    for(j=0;j<(r1-r0)/2;j+=4){
	alv[j] -= arv[j]*ss;
	alv[j+1] -= arv[j+1]*ss;
	alv[j+2] -= arv[j+2]*ss;
	alv[j+3] -= arv[j+3]*ss;
	__builtin_prefetch(alv+j+32,1,3);
	__builtin_prefetch(arv+j+32,0,0);

    }
}

static void vvmulandsub(int n, double a[n+1][n], int cr, int cc,
		 int c0, int c1, int r0,int r1)
{
    int j,k;
    double * ar = a[cr];
#ifdef TIMETEST
    BEGIN_TSC;
#endif
    if (c1-c0 == 1){
	vsmulandsub(n,a, cr, cc,c0, r0,r1);
    }else{
	for (k=c0;k<c1;k++){
	    double s = a[k][cc];
	    double *al = a[k];
	    for(j=r0;j<r1;j++)
		al[j] -= ar[j]*s;
	}
    }
#ifdef TIMETEST
    END_TSC(t,6);
#endif
}
static void mmmulandsub_old(int n, double a[n+1][n], int m0, int m1,
		 int c0, int c1, int r0,int r1)
{
    int j,k,l;
    printf("Enter mmul\n");
#ifndef NOBLAS
    cblas_dgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
		 r1-r0, c1-c0, m1-m0, -1.0, &(a[m0][r0]), n,
		 &(a[c0][m0]), n, 1, &(a[c0][r0]), n );
    // example:
    // r0, m0 = i+m,i
    // m0, c0 = i, i+m
    // r0, c0 = i+m, i+m
    //r1-r0 = n-i-m
    // c1-c0 = iend-i-m
    // m1-m0 = m
#else
    for(j=r0;j<r1;j++)
	for (k=c0;k<c1;k++)
	    for (l=m0; l<m1; l++)
		a[k][j] -= a[l][j]*a[k][l];
#endif
}

static void matmul_for_nk4_0(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;
    for(l=0;l<4;l+=2){
	for(k=0;k<4;k+=2){
	    for(j=0;j<n;j++){
		c[k][j] -= a[l][j]*b[k][l]+ a[l+1][j]*b[k][l+1];
		c[k+1][j] -= a[l][j]*b[k+1][l]+ a[l+1][j]*b[k+1][l+1];
	    }
	}
    }
}
static void matmul_for_nk8_0(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    double btmp[m][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)btmp[i][j]=b[i][j];
#pragma omp parallel for private(l,k,j)
    for(l=0;l<8;l+=2){
	for(k=0;k<8;k+=2){
	    for(j=0;j<n;j++){
		c[k][j] -= a[l][j]*btmp[k][l]+ a[l+1][j]*btmp[k][l+1];
		c[k+1][j] -= a[l][j]*btmp[k+1][l]+ a[l+1][j]*btmp[k+1][l+1];
	    }
	}
    }

}
static void matmul_for_nk8_2(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    double btmp[m][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)btmp[i][j]=b[i][j];
#pragma omp parallel for private(l,k,j)
    for(k=0;k<8;k+=2){
	for(l=0;l<8;l+=4){
	    for(j=0;j<n;j++){
		c[k][j] -= a[l][j]*btmp[k][l]
		    + a[l+1][j]*btmp[k][l+1]
		    + a[l+2][j]*btmp[k][l+2]
		    + a[l+3][j]*btmp[k][l+3];
		c[k+1][j] -= a[l][j]*btmp[k+1][l]
		    + a[l+1][j]*btmp[k+1][l+1]
		    + a[l+2][j]*btmp[k+1][l+2]
		    + a[l+3][j]*btmp[k+1][l+3];
		//		c[k][j] -= a[l][j]*btmp[k][l]+ a[l+1][j]*btmp[k][l+1];
		//		c[k+1][j] -= a[l][j]*btmp[k+1][l]+ a[l+1][j]*btmp[k+1][l+1];
	    }
	}
    }

}
static void matmul_for_nk8_3(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    const int mm = 32;
    double btmp[m][m];
    double atmp[m][mm];

    for(i=0;i<m;i++)for(j=0;j<m;j++)btmp[i][j]=b[i][j];
    for(j=0;j<n;j+=mm){
	int jj;
	for(i=0;i<m;i++){
	    for(k=0;k<mm;k++){
		atmp[i][k]=a[i][j+k];
	    }
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=2){
		c[i][j+jj] -= atmp[0][jj]*btmp[i][0]
		    +atmp[1][jj]*btmp[i][1]
		    +atmp[2][jj]*btmp[i][2]
		    +atmp[3][jj]*btmp[i][3]
		    +atmp[4][jj]*btmp[i][4]
		    +atmp[5][jj]*btmp[i][5]
		    +atmp[6][jj]*btmp[i][6]
		    +atmp[7][jj]*btmp[i][7];
		c[i][j+jj+1] -= atmp[0][jj+1]*btmp[i][0]
		    +atmp[1][jj+1]*btmp[i][1]
		    +atmp[2][jj+1]*btmp[i][2]
		    +atmp[3][jj+1]*btmp[i][3]
		    +atmp[4][jj+1]*btmp[i][4]
		    +atmp[5][jj+1]*btmp[i][5]
		    +atmp[6][jj+1]*btmp[i][6]
		    +atmp[7][jj+1]*btmp[i][7];
	    }
	}
    }

}
static void matmul_for_nk8_4(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    const int mm = 16;
    double btmp[m][m];
    v2df b2tmp[m][m];
    double atmp[m][mm];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
    //#pragma omp parallel for private(j,i,atmp)
    //use of OMP here does not speed things up
    for(j=0;j<n;j+=mm){
	int jj;
	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
	    dest[0]=src[0];
	    dest[ 1]=src[ 1];
	    dest[ 2]=src[ 2];
	    dest[ 3]=src[ 3];
	    dest[ 4]=src[ 4];
	    dest[ 5]=src[ 5];
	    dest[ 6]=src[ 6];
	    dest[ 7]=src[ 7];
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    __builtin_prefetch(c[i]+j+64,1,0);
	    __builtin_prefetch(c[i]+j+80,1,0);
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=4){
		v2df* cp = (v2df*)(&c[i][j+jj]);
		v2df* ap = (v2df*)(atmp[0]+jj);
		v2df* cpp = (v2df*)(&c[i][j+jj+2]);
		v2df* app = (v2df*)(atmp[0]+jj+2);
		*cp -= (*ap)*b2tmp[i][0]
		    +(*(ap+16))*b2tmp[i][1]
		    +(*(ap+32))*b2tmp[i][2]
		    +(*(ap+48))*b2tmp[i][3]
		    +(*(ap+64))*b2tmp[i][4]
		    +(*(ap+80))*b2tmp[i][5]
		    +(*(ap+96))*b2tmp[i][6]
		    +(*(ap+112))*b2tmp[i][7];
		*cpp -= (*app)*b2tmp[i][0]
		    +(*(app+16))*b2tmp[i][1]
		    +(*(app+32))*b2tmp[i][2]
		    +(*(app+48))*b2tmp[i][3]
		    +(*(app+64))*b2tmp[i][4]
		    +(*(app+80))*b2tmp[i][5]
		    +(*(app+96))*b2tmp[i][6]
		    +(*(app+112))*b2tmp[i][7];
	    }
	}
    }

}
static void matmul_for_nk8_5(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=8;
    const int mm = 32;
    double btmp[m][m];
    v2df b2tmp[m][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
    //#pragma omp parallel for private(j,i)
    //use of OMP here does not speed things up
    for(j=0;j<n;j+=mm){
	double atmp[m][mm];
	int jj;
	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
	    dest[0]=src[0];
	    dest[ 1]=src[ 1];
	    dest[ 2]=src[ 2];
	    dest[ 3]=src[ 3];
	    dest[ 4]=src[ 4];
	    dest[ 5]=src[ 5];
	    dest[ 6]=src[ 6];
	    dest[ 7]=src[ 7];
	    dest[ 8]=src[ 8];
	    dest[ 9]=src[ 9];
	    dest[ 10]=src[ 10];
	    dest[ 11]=src[ 11];
	    dest[ 12]=src[ 12];
	    dest[ 13]=src[ 13];
	    dest[ 14]=src[ 14];
	    dest[ 15]=src[ 15];
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    __builtin_prefetch(c[i]+j+64,1,0);
	    __builtin_prefetch(c[i]+j+80,1,0);
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=4){
		v2df* cp = (v2df*)(&c[i][j+jj]);
		v2df* ap = (v2df*)(atmp[0]+jj);
		v2df* cpp = (v2df*)(&c[i][j+jj+2]);
		v2df* app = (v2df*)(atmp[0]+jj+2);
		*cp -= (*ap)*b2tmp[i][0]
		    +(*(ap+16))*b2tmp[i][1]
		    +(*(ap+32))*b2tmp[i][2]
		    +(*(ap+48))*b2tmp[i][3]
		    +(*(ap+64))*b2tmp[i][4]
		    +(*(ap+80))*b2tmp[i][5]
		    +(*(ap+96))*b2tmp[i][6]
		    +(*(ap+112))*b2tmp[i][7];
		*cpp -= (*app)*b2tmp[i][0]
		    +(*(app+16))*b2tmp[i][1]
		    +(*(app+32))*b2tmp[i][2]
		    +(*(app+48))*b2tmp[i][3]
		    +(*(app+64))*b2tmp[i][4]
		    +(*(app+80))*b2tmp[i][5]
		    +(*(app+96))*b2tmp[i][6]
		    +(*(app+112))*b2tmp[i][7];
	    }
	}
    }

}

static void matmul_for_nk8(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int m0,m1,m2,m3;
    int s1, s2, s3;
    int ds = (n/64)*16;
    s1 = ds;
    s2 = ds*2;
    s3 = ds*3;
    m0 = ds;
    m1 = ds;
    m2 = ds;
    m3 = n-s3;
    //    fprintf(stderr,"n, s, m = %d %d %d %d %d %d %d %d\n",
    //    n,s1,s2,s3,m0,m1,m2,m3);

#pragma omp parallel
#pragma omp sections
  {
#pragma omp section
      matmul_for_nk8_5(n1,a,n2,b,n3,c,m0);
#pragma omp section
      matmul_for_nk8_5(n1,((double*)a)+s1 ,n2,b,n3,((double*)c)+s1,m1);
#pragma omp section
      matmul_for_nk8_5(n1,((double*)a)+s2 ,n2,b,n3,((double*)c)+s2,m2);
#pragma omp section
      matmul_for_nk8_5(n1,((double*)a)+s3 ,n2,b,n3,((double*)c)+s3,m3);
  }
}

static void matmul_for_nk4_3(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=4;
    const int mm = 16;
    double btmp[m][m];
    v2df b2tmp[m][m];
    double atmp[m][mm];
    v2df atmpt[mm/2][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
    //#pragma omp parallel for private(j,i,atmp)
    //use of OMP here does not speed things up
    for(j=0;j<n;j+=mm){
	int jj;

	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
	    atmpt[ 0][i]=src[0];
	    atmpt[ 1][i]=src[ 1];
	    atmpt[ 2][i]=src[ 2];
	    atmpt[ 3][i]=src[ 3];
	    atmpt[ 4][i]=src[ 4];
	    atmpt[ 5][i]=src[ 5];
	    atmpt[ 6][i]=src[ 6];
	    atmpt[ 7][i]=src[ 7];
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    if (jjend+j > n) jjend = n-j;
	    v2df* cp = (v2df*)(&c[i][j]);
	    v2df* ap = (v2df*)(atmpt[0]);
	    v2df* cpp = (v2df*)(&c[i][j+2]);
	    v2df* app = (v2df*)(atmpt[1]);
	    v2df* bp = b2tmp[i];
	    for(jj=0;jj<jjend;jj+=4){
		*cp -= ap[0]*bp[0]
		    +ap[1]*bp[1]
		    +ap[2]*bp[2]
		    +ap[3]*bp[3];
		*cpp -= app[0]*bp[0]
		    +app[1]*bp[1]
		    +app[2]*bp[2]
		    +app[3]*bp[3];
		cp += 2;
		cpp+= 2;
		ap += m*2;
		app+= m*2;
	    }
	}

    }

}

static void matmul_for_nk4_4(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=4;
    const int mm = 32;
    double btmp[m][m];
    v2df b2tmp[m][m];
    double atmp[m][mm];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
    //#pragma omp parallel for private(j,i,atmp)
    // use of OMP does not speed things up here...
    for(j=0;j<n;j+=mm){
	int jj;
	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
#if 0
	    for(k=0;k<mm;k++)atmp[i][k]=a[i][j+k];
#endif
#if 0
	    for(k=0;k<mm/2;k++)dest[k]=src[k];
#endif
	    for(k=0;k<mm/2;k+=2){
		dest[k]=src[k];
		dest[k+1]=src[k+1];
	    }
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    v2df * bp = b2tmp[i];
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=2){
		v2df* cp = (v2df*)(&c[i][j+jj]);
		v2df* ap = (v2df*)(atmp[0]+jj);
		v2df* cpp = (v2df*)(&c[i][j+jj+2]);
		v2df* app = (v2df*)(atmp[0]+jj+2);
		*cp -= (*ap)*bp[0]
		    +(*(ap+16))*bp[1]
		    +(*(ap+32))*bp[2]
		    +(*(ap+48))*bp[3];
#if 0
		*cpp -= (*app)*bp[0]
		    +(*(app+16))*bp[1]
		    +(*(app+32))*bp[2]
		    +(*(app+48))*bp[3];
#endif
	    }
	}
    }

}
static void matmul_for_nk4_1(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i,j,k,l;
    const int m=4;
    const int mm = 32;
    double btmp[m][m];
    v2df b2tmp[m][m];

    for(i=0;i<m;i++)for(j=0;j<m;j++)b2tmp[i][j]=(v2df){b[i][j],b[i][j]};
#pragma omp parallel for private(j,i)
    // use of OMP does not speed things up here...
    for(j=0;j<n;j+=mm){
	double atmp[m][mm];
	int jj;
	for(i=0;i<m;i++){
	    v2df * dest = (v2df*)(atmp[i]);
	    v2df * src = (v2df*)(a[i]+j);
#if 0
	    for(k=0;k<mm;k++)atmp[i][k]=a[i][j+k];
#endif
#if 0
	    for(k=0;k<mm/2;k++)dest[k]=src[k];
#endif
	    for(k=0;k<mm/2;k+=4){
		dest[k]=src[k];
		dest[k+1]=src[k+1];
		dest[k+2]=src[k+2];
		dest[k+3]=src[k+3];
	    }
	}
	for(i=0;i<m;i++){
	    int jjend = mm;
	    v2df * bp = b2tmp[i];
	    if (jjend+j > n) jjend = n-j;
	    for(jj=0;jj<jjend;jj+=4){
		v2df* cp = (v2df*)(&c[i][j+jj]);
		v2df* ap = (v2df*)(atmp[0]+jj);
		v2df* cpp = (v2df*)(&c[i][j+jj+2]);
		v2df* app = (v2df*)(atmp[0]+jj+2);
		*cp -= (*ap)*bp[0]
		    +(*(ap+16))*bp[1]
		    +(*(ap+32))*bp[2]
		    +(*(ap+48))*bp[3];
		*cpp -= (*app)*bp[0]
		    +(*(app+16))*bp[1]
		    +(*(app+32))*bp[2]
		    +(*(app+48))*bp[3];
	    }
	}
    }

}
static void matmul_for_nk2_0(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;
    for(j=0;j<n;j++){
	    c[0][j] -= a[0][j]*b[0][0]+a[1][j]*b[0][1];
	    c[1][j] -= a[0][j]*b[1][0]+a[1][j]*b[1][1];
    }
}
static void matmul_for_nk2_1(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;

    for(j=0;j<n;j+=2){
	    c[0][j] -= a[0][j]*b[0][0]+a[1][j]*b[0][1];
	    c[1][j] -= a[0][j]*b[1][0]+a[1][j]*b[1][1];
	    c[0][j+1] -= a[0][j+1]*b[0][0]+a[1][j+1]*b[0][1];
	    c[1][j+1] -= a[0][j+1]*b[1][0]+a[1][j+1]*b[1][1];
    }
}
static void matmul_for_nk2_2(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;

    register double b00 = b[0][0];
    register double b01 = b[0][1];
    register double b10 = b[1][0];
    register double b11 = b[1][1];
    for(j=0;j<n;j+=2){
	    c[0][j] -= a[0][j]*b00+a[1][j]*b01;
	    c[0][j+1] -= a[0][j+1]*b00+a[1][j+1]*b01;
	    c[1][j] -= a[0][j]*b10+a[1][j]*b11;
	    c[1][j+1] -= a[0][j+1]*b10+a[1][j+1]*b11;
    }

}
static void matmul_for_nk2(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int j,k,l;
    register v2df b00 = (v2df){b[0][0],b[0][0]};
    register v2df b01 = (v2df){b[0][1],b[0][1]};
    register v2df b10 = (v2df){b[1][0],b[1][0]};
    register v2df b11 = (v2df){b[1][1],b[1][1]};
    v2df * a0 = (v2df*) a[0];
    v2df * a1 = (v2df*) a[1];
    v2df * c0 = (v2df*) c[0];
    v2df * c1 = (v2df*) c[1];
    int nh = n>>1;
    if (nh & 1){
	j=nh-1;
	c0[j] -= a0[j]*b00+a1[j]*b01;
	c1[j] -= a0[j]*b10+a1[j]*b11;
	nh = nh-1;
    }
    for(j=0;j<nh;j+=2){
	c0[j] -= a0[j]*b00+a1[j]*b01;
	c0[j+1] -= a0[j+1]*b00+a1[j+1]*b01;
	c1[j] -= a0[j]*b10+a1[j]*b11;
	c1[j+1] -= a0[j+1]*b10+a1[j+1]*b11;
	//	__builtin_prefetch((double*)&a0[j+32],0);
	//	__builtin_prefetch((double*)&a1[j+32],0);
	//	__builtin_prefetch((double*)&c0[j+32],1);
	//	__builtin_prefetch((double*)&c1[j+32],1);
	//	asm("prefetcht2 %0"::"m"(a0[j+32]):"memory");
	//	asm("prefetcht2 %0"::"m"(a1[j+32]):"memory");
    }
}

static void matmul_for_nk4_5(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{

    matmul_for_nk2(n1, a, n2, b, n3,c, n);
    matmul_for_nk2(n1, (double(*)[])  (a[2]), n2,
		   (double(*)[])  &(b[0][2]), n3,c, n);
    matmul_for_nk2(n1, a, n2, (double(*)[])  b[2], n3,
		   (double(*)[])  c[2], n);
    matmul_for_nk2(n1, (double(*)[])  &(a[2]), n2,
		   (double(*)[])  &(b[2][2]), n3,
		   (double(*)[])  c[2], n);

}

static void matmul_for_nk4_6(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    register v2df b00 = (v2df){b[0][0],b[0][0]};
    register v2df b01 = (v2df){b[0][1],b[0][1]};
    register v2df b02 = (v2df){b[0][2],b[0][2]};
    register v2df b03 = (v2df){b[0][3],b[0][3]};
    register v2df b10 = (v2df){b[1][0],b[1][0]};
    register v2df b11 = (v2df){b[1][1],b[1][1]};
    register v2df b12 = (v2df){b[1][2],b[1][2]};
    register v2df b13 = (v2df){b[1][3],b[1][3]};
    register v2df b20 = (v2df){b[2][0],b[2][0]};
    register v2df b21 = (v2df){b[2][1],b[2][1]};
    register v2df b22 = (v2df){b[2][2],b[2][2]};
    register v2df b23 = (v2df){b[2][3],b[2][3]};
    register v2df b30 = (v2df){b[3][0],b[3][0]};
    register v2df b31 = (v2df){b[3][1],b[3][1]};
    register v2df b32 = (v2df){b[3][2],b[3][2]};
    register v2df b33 = (v2df){b[3][3],b[3][3]};

    v2df * a0 = (v2df*) a[0];
    v2df * a1 = (v2df*) a[1];
    v2df * a2 = (v2df*) a[2];
    v2df * a3 = (v2df*) a[3];
    v2df * c0 = (v2df*) c[0];
    v2df * c1 = (v2df*) c[1];
    v2df * c2 = (v2df*) c[2];
    v2df * c3 = (v2df*) c[3];
    int nh = n>>1;
    //#pragma omp parallel
    {
	//#pragma omp section
	{
	    int j;
	    //#pragma omp for private (j)
	    for(j=0;j<nh;j++){
		c0[j] -= a0[j]*b00+a1[j]*b01+a2[j]*b02+a3[j]*b03;
		c1[j] -= a0[j]*b10+a1[j]*b11+a2[j]*b12+a3[j]*b13;
	    }
	}
	//#pragma omp section
	{
	    int j;
	    //#pragma omp for private (j)
	    for(j=0;j<nh;j++){
		c2[j] -= a0[j]*b20+a1[j]*b21+a2[j]*b22+a3[j]*b23;
		c3[j] -= a0[j]*b30+a1[j]*b31+a2[j]*b32+a3[j]*b33;
	    }
	}
    }
}
static void matmul_for_nk4_7(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    v2df * a0 = (v2df*) a[0];
    v2df * a1 = (v2df*) a[1];
    v2df * a2 = (v2df*) a[2];
    v2df * a3 = (v2df*) a[3];
    int j;
    {
	int nh = n>>1;
	v2df * c0 = (v2df*) c[0];
	v2df * c1 = (v2df*) c[1];
	 v2df b00 = (v2df){b[0][0],b[0][0]};
	 v2df b01 = (v2df){b[0][1],b[0][1]};
	 v2df b02 = (v2df){b[0][2],b[0][2]};
	 v2df b03 = (v2df){b[0][3],b[0][3]};
	 v2df b10 = (v2df){b[1][0],b[1][0]};
	 v2df b11 = (v2df){b[1][1],b[1][1]};
	 v2df b12 = (v2df){b[1][2],b[1][2]};
	 v2df b13 = (v2df){b[1][3],b[1][3]};
	if (nh & 1){
	    j=nh-1;
	    c0[j] -= a0[j]*b00+a1[j]*b01+a2[j]*b02+a3[j]*b03;
	    c1[j] -= a0[j]*b10+a1[j]*b11+a2[j]*b12+a3[j]*b13;
	    nh--;
	}
	for(j=0;j<nh;j+=2){
	    c0[j] -= a0[j]*b00+a1[j]*b01+a2[j]*b02+a3[j]*b03;
	    c1[j] -= a0[j]*b10+a1[j]*b11+a2[j]*b12+a3[j]*b13;
	    c0[j+1] -= a0[j+1]*b00+a1[j+1]*b01+a2[j+1]*b02+a3[j+1]*b03;
	    c1[j+1] -= a0[j+1]*b10+a1[j+1]*b11+a2[j+1]*b12+a3[j+1]*b13;
	}
    }
    {
	int nh = n>>1;
	v2df * c2 = (v2df*) c[2];
	v2df * c3 = (v2df*) c[3];
	 v2df b20 = (v2df){b[2][0],b[2][0]};
	 v2df b21 = (v2df){b[2][1],b[2][1]};
	 v2df b22 = (v2df){b[2][2],b[2][2]};
	 v2df b23 = (v2df){b[2][3],b[2][3]};
	register v2df b30 = (v2df){b[3][0],b[3][0]};
	register v2df b31 = (v2df){b[3][1],b[3][1]};
	register v2df b32 = (v2df){b[3][2],b[3][2]};
	register v2df b33 = (v2df){b[3][3],b[3][3]};

	for(j=0;j<nh;j++){
	    c2[j] -= a0[j]*b20+a1[j]*b21+a2[j]*b22+a3[j]*b23;
	    c3[j] -= a0[j]*b30+a1[j]*b31+a2[j]*b32+a3[j]*b33;
	}
    }
}

static void matmul_for_nk4_8(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    v2df * a0 = (v2df*) a[0];
    v2df * a1 = (v2df*) a[1];
    v2df * a2 = (v2df*) a[2];
    v2df * a3 = (v2df*) a[3];
    int nh = n>>1;
    int j;

    int k;
    v2df * cv;
    v2df * cvv;
    register v2df b0;
    register v2df b1;
    register v2df b2;
    register v2df b3;
    register v2df b4;
    register v2df b5;
    register v2df b6;
    register v2df b7;
    for(k=0;k<4;k+=2){
	cv = (v2df*) c[k];
	cvv = (v2df*) c[k+1];
	b0 = (v2df){b[k][0],b[k][0]};
	b1 = (v2df){b[k][1],b[k][1]};
	b2 = (v2df){b[k][2],b[k][2]};
	b3 = (v2df){b[k][3],b[k][3]};
	b4 = (v2df){b[k+1][0],b[k+1][0]};
	b5 = (v2df){b[k+1][1],b[k+1][1]};
	b6 = (v2df){b[k+1][2],b[k+1][2]};
	b7 = (v2df){b[k+1][3],b[k+1][3]};
	for(j=0;j<nh;j++){
	    register v2df aa0 = a0[j];
	    register v2df aa1 = a1[j];
	    register v2df aa2 = a2[j];
	    register v2df aa3 = a3[j];
	    register v2df x = aa0*b0;
	    x+= aa1*b1;
	    x+= aa2*b2;
	    x+= aa3*b3;
	    cv[j]-=x;
	    x = aa0*b4;
	    x+= aa1*b5;
	    x+= aa2*b6;
	    x+= aa3*b7;
	    cvv[j] -= x;
	}
    }
}

static void matmul_for_nk4(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int m0,m1,m2,m3;
    int s1, s2, s3;
    int ds = (n/32)*8;
    s1 = ds;
    s2 = ds*2;
    s3 = ds*3;
    m0 = ds;
    m1 = ds;
    m2 = ds;
    m3 = n-s3;
    //    fprintf(stderr,"n, s, m = %d %d %d %d %d %d %d %d\n",
    //    n,s1,s2,s3,m0,m1,m2,m3);

#pragma omp parallel
#pragma omp sections
  {
#pragma omp section
      matmul_for_nk4_7(n1,a,n2,b,n3,c,m0);
#pragma omp section
      matmul_for_nk4_7(n1,((double*)a)+s1 ,n2,b,n3,((double*)c)+s1,m1);
#pragma omp section
      matmul_for_nk4_7(n1,((double*)a)+s2 ,n2,b,n3,((double*)c)+s2,m2);
#pragma omp section
      matmul_for_nk4_7(n1,((double*)a)+s3 ,n2,b,n3,((double*)c)+s3,m3);
  }
}
static void matmul_for_nk8_9(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    matmul_for_nk4(n1, a, n2, b, n3,c, n);
    matmul_for_nk4(n1, (double(*)[])  (a[4]), n2,
		   (double(*)[])  &(b[0][4]), n3,c, n);
    matmul_for_nk4(n1, a, n2, (double(*)[])  b[4], n3,
		   (double(*)[])  c[4], n);
    matmul_for_nk4(n1, (double(*)[])  &(a[4]), n2,
		   (double(*)[])  &(b[4][4]), n3,
		   (double(*)[])  c[4], n);

}
#if 0
static void matmul_for_nk8(int n1, double a[][n1],
			   int n2, double b[][n2],
			   int n3, double c[][n3],
			   int n)
{
    int i;
    int nb=384;
    for (i=0;i<n;i+=nb){
	int iend = i+nb;
	if (iend > n) iend = n;
	matmul_for_nk8_worker(n1, (double(*)[]) (a[0]+i),
			      n2,  (double(*)[]) (b[0]+i),
			      n3, (double(*)[]) (c[0]+i),
			      iend-i);
    }
}
#endif

static void matmul_for_small_nk_local(int n1, double a[][n1],
			 int n2, double b[][n2],
			 int n3, double c[][n3],
			 int m,
			 int kk,
			 int n)
{
    // simplest version
    int j,k,l;
    BEGIN_TSC;
    if (kk == 2){
	matmul_for_nk2(n1, a, n2, b, n3,c, n);
	END_TSC(t,16);
	return;
    }
    if (kk == 4){
	matmul_for_nk4(n1, a, n2, b, n3,c, n);
	END_TSC(t,13);
	return;
    }
    if (kk == 8){
	matmul_for_nk8(n1, a, n2, b, n3,c, n);
	END_TSC(t,12);
	return;
    }

    for(j=0;j<n;j++)
	for(k=0;k<m;k++)
	    for(l=0;l<kk;l++)
		c[k][j] -= a[l][j]*b[k][l];
}

static void mmmulandsub(int n, double a[n+1][n], int rshift, int m0, int m1,
		 int c0, int c1, int r0,int r1)
{
    int j,k,l;
#ifdef TIMETEST
    BEGIN_TSC;
#endif
#ifndef NOBLAS
    if ((m1-m0)<=8){
	// =4 is slighly faster than =8 on Ci7
	matmul_for_small_nk_local(n, (double(*)[])  &(a[m0-rshift][r0]),
			      n, (double(*)[])  &(a[c0-rshift][m0]),
			      n, (double(*)[])  &(a[c0-rshift][r0]),
			      c1-c0,m1-m0,r1-r0);
    }else{
	cblas_dgemm( CblasColMajor, CblasNoTrans, CblasNoTrans,
		     r1-r0, c1-c0, m1-m0, -1.0, &(a[m0-rshift][r0]), n,
		     &(a[c0-rshift][m0]), n, 1, &(a[c0-rshift][r0]), n );
    }
    // example:
    // r0, m0 = i+m,i
    // m0, c0 = i, i+m
    // r0, c0 = i+m, i+m
    //r1-r0 = n-i-m
    // c1-c0 = iend-i-m
    // m1-m0 = m
#else
    for(j=r0;j<r1;j++)
	for (k=c0;k<c1;k++)
	    for (l=m0; l<m1; l++)
		a[k-rshift][j] -= a[l-rshift][j]*a[k-rshift][l];
#endif
#ifdef TIMETEST
    END_TSC(t,4);
#endif
}

static int nswap;
static void column_decomposition(int n, double a[][n],  int m, int pv[], int i)
{
    // shift a so that partial array is okay
    int  j, k;
    int ip,ii;
    double ainv;
#ifdef TIMETEST
    BEGIN_TSC;
#endif
    for(ip=0;ip<m;ip++){
	ii=i+ip;
	int p = findpivot(n,a,ii);
	pv[ip]=p;
	swaprows(n,a,p,ii,0,m);
	nswap++;
	// normalize row ii
	ainv = 1.0/a[ip][ii];
	scalerow(n,a,ainv,ii,0,ip);
	scalerow(n,a,ainv,ii,ip+1,m);
	// subtract row ii from all lower rows
	vvmulandsub(n,  a, ip,ii, ip+1, m, ii+1, n);
    }
#ifdef TIMETEST
    END_TSC(t,5);
#endif
}

static void process_right_part(int n,
			double a[n+1][n],
			int m,
			int pv[],
			int i,
			int iend)
{
    int ii;
    // exchange rows
    for(ii=i;ii<i+m;ii++){
	swaprows(n,a,pv[ii-i],ii,m,iend-i);
    }

    // normalize rows
    for(ii=i;ii<i+m;ii++){
	scalerow(n,a,1.0/a[ii-i][ii] ,ii,m,iend-i);
    }
    // subtract rows (within i-i+m-1)
    for(ii=i;ii<i+m;ii++){
	vvmulandsub(n,  a, ii-i,ii,  m, iend-i, ii+1, i+m);
    }

    // subtract rows i-i+m-1 from all lower rows
    mmmulandsub(n, a, i, i,i+m, i+m, iend, i+m, n);
    //    fprintf(stderr,"process_r, end\n");
    //    usleep(1);
}
static void column_decomposition_recursive(int n,
				    double a[n+1][n],
				    int m,
				    int pv[],
				    int i)
{
    int  j, k;
    int ip,ii;
    double ainv;
    //    fprintf(stderr,"enter t column recursive %d %d\n", i, m);
    if (m <= 2){
	// perform non-recursive direct decomposition
	column_decomposition(n, a, m, pv,i);
    }else{
	// process the left half by recursion
	//	fprintf(stderr,"call column recursive %d %d\n", i, m);
	column_decomposition_recursive(n, a, m/2, pv,i);
	// process the right half
	//	fprintf(stderr,"call right part %d %d\n", i, m);
	process_right_part(n,a,m/2,pv,i,i+m);
	//	fprintf(stderr,"call right recursive %d %d\n", i, m);
	column_decomposition_recursive(n, a+m/2, m/2, pv+m/2,i+m/2);
	// process the swap of rows for the left half
	//	fprintf(stderr,"call swaprowse %d %d\n", i, m);
	for(ii=i+m/2;ii<i+m;ii++){
	    swaprows(n,a,pv[ii-i],ii,0,m/2);
	}
	//	fprintf(stderr,"call scalerows %d %d\n", i, m);
	// normalize rows
	for(ii=i+m/2;ii<i+m;ii++){
	    scalerow(n,a,1.0/a[ii-i][ii] ,ii,0,m/2);
	}
    }
}

void cm_column_decomposition_recursive(int n,
				    double a[n+1][n],
				    int m,
				    int pv[],
				    int i)
{
    column_decomposition_recursive( n, a, m, pv, i);
}
void cm_column_decomposition(int n,
				    double a[n+1][n],
				    int m,
				    int pv[],
				    int i)
{
    column_decomposition( n, a, m, pv, i);
}
void cm_process_right_part(int n,
			   double a[n+1][n],
			   int m,
			   int pv[],
			   int i,
			   int iend)
{
    process_right_part(n, a, m, pv, i,iend);
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <gsl/gsl_math.h>

#include "cosmocalc.h"
#include "haloprofs.h"
#include "weaklens.h"

void test_cosmo(char *path);

int main(int argc, char **argv)
{
  cosmoData.useSmoothTransFunc = 0;
  cosmoData.delta = 200.0;
  char path[4096];

  //init cosmology 2
  cosmoData.cosmoNum = 2;
  cosmoData.OmegaM = 0.28;
  cosmoData.OmegaL = 0.72;
  cosmoData.OmegaB = 0.05;
  cosmoData.OmegaNu = 0.0;
  cosmoData.OmegaK = 0.0;
  cosmoData.h = 0.7;
  cosmoData.w0 = -1.0;
  cosmoData.wa = 0.0;
  cosmoData.SpectralIndex = 0.96;
  cosmoData.Sigma8 = 0.8;

  sprintf(path,"./cosmo2");
  test_cosmo(path);

  //init cosmology 1
  cosmoData.cosmoNum = 1;
  cosmoData.OmegaM = 0.3;
  cosmoData.OmegaL = 0.68;
  cosmoData.OmegaB = 0.05;
  cosmoData.OmegaNu = 0.0;
  cosmoData.OmegaK = 1.0 - cosmoData.OmegaM - cosmoData.OmegaL - cosmoData.OmegaNu;
  cosmoData.h = 0.7;
  cosmoData.w0 = -0.8;
  cosmoData.wa = 0.2;
  cosmoData.SpectralIndex = 0.96;
  cosmoData.Sigma8 = 0.8;

  sprintf(path,"./cosmo1");
  test_cosmo(path);

  return 0;
}

void test_cosmo(char *path)
{
  char fname[4096];
  char fnameo[4096];
  char str[4096];
  FILE *fp;
  FILE *fpout;
  double z;

  sprintf(fname,"%s/codist.dat",path);
  sprintf(fnameo,"%s/codist.dat.matt",path);
  fp = fopen(fname,"r");
  if( fp != NULL)
    {
      fpout = fopen(fnameo,"w");
      while(fgets(str,4096,fp) != NULL)
	{
	  if(str[0] == '#')
	    continue;
	  sscanf(str,"%le %*e\n",&z);

	  fprintf(fpout,"%e %e\n",z,comvdist(1.0/(1.0+z)));
	}
      fclose(fp);
      fclose(fpout);
    }

  sprintf(fname,"%s/Evol_z.dat",path);
  sprintf(fnameo,"%s/Evol_z.dat.matt",path);
  fp = fopen(fname,"r");
  if(fp != NULL)
    {
      fpout = fopen(fnameo,"w");
      while(fgets(str,4096,fp) != NULL)
	{
	  if(str[0] == '#')
	    continue;
	  sscanf(str,"%le %*e\n",&z);

	  //fprintf(stderr,"weff(%f) = %f, h(a) = %f\n",1.0/(1.0+z),weff(1.0/(1.0+z)),hubble_noscale(1.0/(1.0+z)));
	  fprintf(fpout,"%e %e\n",z,hubble_noscale(1.0/(1.0+z)));
	}
      fclose(fp);
      fclose(fpout);
    }
}


/* hka chi-square stat estimation
based on original Hudson, Kreitman, Aguade 1987 implementation.

The parameters of the model are calculated by solving the system of equations numerically.

A. D. Kern  6/2005
*/

#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#include "string.h"
#include "hka.h"
#include "ctype.h"

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multiroots.h>

/* currently the maximum number of loci is at 10^7, and the maximum locus name is 80 characters long */
struct hkaData data[MAXLOCI];
int locusNumber = 0;
int lineageNumber = 1;

int main(int argc, char *argv[]){
  int i = 0;
  gsl_vector  *chiSquares, *expectedPolyA, *expectedD, *devs;
  gsl_vector  *params; //0-locusNumber-1 = 4Nu's, locusNumber = tHat, locusNumber + 1 = fHat
  double x;

  getData(argc, argv);
  if(lineageNumber > 1){
    params = gsl_vector_alloc(locusNumber + 2);
  }
  else{
    params = gsl_vector_alloc(locusNumber + 1);
  }

  //solve the system of equations
  solveSystem(params);

  //calculate chiSquare Stats
  chiSquares =gsl_vector_alloc(locusNumber);
  expectedPolyA = gsl_vector_alloc(locusNumber);
  expectedD = gsl_vector_alloc(locusNumber);
  devs =  gsl_vector_alloc(locusNumber);

  x =  calculateHKA(params, chiSquares, expectedPolyA, expectedD);

  //print some stuff
  if(lineageNumber > 1){
    printf("chiSquared = %f\nt_hat = %f\nf_hat = %f\n",x, gsl_vector_get(params,locusNumber), gsl_vector_get(params,locusNumber + 1));
  }
  else{
    printf("chiSquared = %f\nt_hat = %f\nf_hat = 1.0\n",x, gsl_vector_get(params,locusNumber));
  }
    for(i = 0; i < locusNumber; i++){
      printf("locus %s \ttheta:\t %f\texpSA:\t %f\tobsSA:\t %f\texpD:\t %f\tobsD:\t %f\t chiSquared:\t%f\n",data[i].name,   gsl_vector_get(params,i),gsl_vector_get(expectedPolyA,i), data[i].sa, gsl_vector_get(expectedD,i), data[i].d, gsl_vector_get(chiSquares,i));
  }

  gsl_vector_free(params);
  return(0);
}

void usage(){
  printf("usage:\nhka infile\n");
  exit(1);
}

/* parses the data file and reads in options (currently no options boss) */
void getData(int argc, char *argv[]){
  FILE *infile;
  int  l, na, nb, sa, sb, args, flag, i,lineCount;
  double d;
  char string[81];  //here's the character length limit
  char *sArray[30], line[500];

  if (argc < 2){
    usage();
  }
  else{
    infile = fopen(argv[1],"r");
    if (infile == NULL){
      fprintf(stderr,"Error opening infile!!!\n");
      exit(1);
    }
    flag = 0;
    args = 2;
    while(args < argc){
      switch(argv[args][1]){
      case 'm' : //missing data mode
	flag = 1;
      }
      args++;
    }
    if (flag){
      //missing data mode expect- na, d,lArray, sArray
      while(fgets(line, 100, infile)){
	lineCount = chopByWhite(line,sArray,30);
	//	data[locusNumber].name = (char) sArray[0];
	data[locusNumber].na = atoi(sArray[1]);
	data[locusNumber].d = (double) atof(sArray[2]);
	for(i = 0; i < data[locusNumber].na - 2; i++){
	  data[locusNumber].lArray[i] = atoi(sArray[i+3]);
	  data[locusNumber].sArray[i] = atoi(sArray[i+data[locusNumber].na - 1 + 3]);
	}

	printf("%d\n",data[locusNumber].sArray[2]);
      }
    }
    else{

      /* collect hka data from infile- expect length, na, nb, sa, sb, d */
      while(fscanf(infile, "%80s %d %d %d %d %d %lf", string,  &l, &na, &nb, &sa, &sb, &d) != EOF){
	strcpy(data[locusNumber].name, string);
	data[locusNumber].length = l;
	data[locusNumber].na = na;
	data[locusNumber].nb = nb;
	if (nb > 1){
	  lineageNumber = 2;
	}
	data[locusNumber].sa = (double) sa;
	data[locusNumber].sb = (double) sb;
	data[locusNumber].d = (double) d;
	data[locusNumber].lArray[0] = 666;
	locusNumber++;
      }
    }
    fclose(infile);
  }
}

/* harmSum returns the denominator portion of Watterson's estimator */
double harmSum(int n){
  int i;
  double sum = 0.0;

  for(i = 1; i < n; i++){
    sum += 1.0 /  i;
  }
  return(sum);
}

/* harmSumSquare is for calculating the variance of Watterson's estimator */
double harmSumSquare(int n){
  int i;
  double sum = 0.0;

  for(i = 1; i < n; i++){
    sum += 1.0 / ( i * i);
  }
  return(sum);
}

/*setFunction- this sets up the multiroot function for solving */
int setHKAFunction(const gsl_vector *x, void *p, gsl_vector *f){
  double sumS, trueS, sumD, trueD, yi, trueYi;
  double sumSb, trueSb;
  int i;

  /*set up all the equations. the first are the sum of segSites and sum of divergence respectively */
  if (lineageNumber == 1){
    sumS = yi = sumD = trueS = trueD =  0.0;
    for(i = 0; i < locusNumber; i++){
      sumS += data[i].length * harmSum(data[i].na) * gsl_vector_get(x,i);
      sumD += data[i].length * (gsl_vector_get(x,locusNumber) + 1) * gsl_vector_get(x,i);
      trueS += data[i].sa;
      trueD += data[i].d;
    }
    gsl_vector_set(f,0,fabs(trueS - sumS));
    gsl_vector_set(f,1,fabs(trueD - sumD));
    //Sa+D eqns
    for(i = 0; i < locusNumber - 1; i++){
      trueYi = data[i].sa + data[i].d;
      yi = gsl_vector_get(x,i) *						\
	((data[i].length * gsl_vector_get(x,locusNumber) +		\
	  data[i].length + (data[i].length * harmSum(data[i].na))));
      gsl_vector_set(f,i+2,fabs(trueYi - yi));
    }
  }
  else{
    sumS = sumSb = yi = sumD = trueS = trueSb = trueD =  0.0;
    for(i = 0; i < locusNumber; i++){
      sumS += data[i].length * harmSum(data[i].na) * gsl_vector_get(x,i);
      sumSb += data[i].length * harmSum(data[i].nb) * gsl_vector_get(x,i) * gsl_vector_get(x, locusNumber + 1);
      sumD += data[i].length * (gsl_vector_get(x,locusNumber) + ((1.0 + gsl_vector_get(x,locusNumber+1)) / 2.0)) * gsl_vector_get(x,i);
      trueS += data[i].sa;
      trueSb += data[i].sb;
      trueD += data[i].d;
    }
    gsl_vector_set(f,0,fabs(trueS - sumS));
    gsl_vector_set(f,1,fabs(trueSb - sumSb));
    gsl_vector_set(f,2,fabs(trueD - sumD));
    //Sa+Sb+D eqns
    for(i = 0; i < locusNumber - 1; i++){
      trueYi = data[i].sa + data[i].sb + data[i].d;
      yi = (data[i].length * (gsl_vector_get(x,locusNumber) + ((1.0 + gsl_vector_get(x,locusNumber+1)) / 2.0)) * gsl_vector_get(x,i)) \
	+ ( data[i].length * harmSum(data[i].na) * gsl_vector_get(x,i)) + \
	(data[i].length * harmSum(data[i].nb) * gsl_vector_get(x,i) * gsl_vector_get(x, locusNumber + 1));
      gsl_vector_set(f,i+3,fabs(trueYi - yi));
    }
  }
  return GSL_SUCCESS;
}

int solveSystem(gsl_vector *params){
  const gsl_multiroot_fsolver_type *T;
  gsl_multiroot_fsolver *s;
  gsl_vector *x;
  int status;
  size_t  iter = 0;
  void *point=NULL;
  size_t n;

  if (lineageNumber == 1){
    n = locusNumber + 1;
    x = gsl_vector_alloc(locusNumber + 1);
  }
  else{
    n = locusNumber + 2;
    x = gsl_vector_alloc(locusNumber + 2);
  }

  gsl_multiroot_function f = {&setHKAFunction, n, point};
  gsl_vector_set_all(x, 1);
  T = gsl_multiroot_fsolver_hybrids;
  s = gsl_multiroot_fsolver_alloc(T, n);
  gsl_multiroot_fsolver_set(s, &f, x);

  do{
    iter++;
    status = gsl_multiroot_fsolver_iterate(s);
    if(status){
      break;
    }
    status = gsl_multiroot_test_residual(s->f, 1e-7);
  }
  while (status == GSL_CONTINUE && iter < 100);

  gsl_vector_memcpy(params, s->x);
  gsl_vector_free(x);
  gsl_multiroot_fsolver_free (s);

  return 0;
}

/* calculateHKA- this is the main beast that calculates estimates of t_hat, f(the ratio of popn sizes),
 and theta's for each locus. it returns the chi-square statistic, and takes pointers to the other
 relavent quantities (t_hat, etc.). */
double calculateHKA(gsl_vector *params,gsl_vector *chiSquares, gsl_vector *expectedPolyA, gsl_vector *expectedD){
  double  chiSquared, chiSquared_i;
  double expSA, expSB, varSA, varSB, expD, varD;
  double hsA, hsB;
  double devA, devB, devD,fHat;
  int i;

  if(lineageNumber > 1){
    fHat = gsl_vector_get(params,locusNumber+1);
  }
  else{
    fHat = 1.0;
  }
  chiSquared = 0.0;
  for(i=0; i < locusNumber; i++){
    hsA = harmSum(data[i].na);
    hsB = harmSum(data[i].nb);
    //calculate expectations and variances at the ith locus
    expSA = gsl_vector_get(params,i) * data[i].length * hsA;
    varSA = expSA + ((gsl_vector_get(params,i) * data[i].length) * (gsl_vector_get(params,i) * data[i].length) * harmSumSquare(data[i].na));
    expSB =  gsl_vector_get(params,i) * fHat * data[i].length * hsB;
    varSB = expSB + ((gsl_vector_get(params,i) * fHat * data[i].length) * (gsl_vector_get(params,i) * fHat * data[i].length) * harmSumSquare(data[i].nb)); // equals zero if no poly data from speciesB
    expD = (gsl_vector_get(params,i) * data[i].length) * (gsl_vector_get(params,locusNumber) + (0.5 * (1.0 + fHat)));
    varD = expD + (((gsl_vector_get(params,i) * data[i].length) * 0.5 * (1.0 + fHat)) * ((gsl_vector_get(params,i) *  data[i].length) * 0.5 * (1.0 + fHat)));

    //calculate deviations for species_a_theta, species_b_theta, and divergence.
    // then add to chi-square stat
    devA = ((data[i].sa - expSA) *  (data[i].sa - expSA)) / varSA;
    devB = ((data[i].sb - expSB) *  (data[i].sb - expSB)) / varSB;  //careful here will not be number if no poly data for speciesB
    devD = ((data[i].d - expD) * (data[i].d - expD)) / varD;

    //is deviation for species B finite?
    if (isnan(devB)){
      chiSquared_i = devA + devD;
    }
    else{
      chiSquared_i = devA + devB + devD;
    }
    chiSquared += chiSquared_i;
    gsl_vector_set(chiSquares,i,chiSquared_i);
    gsl_vector_set(expectedPolyA, i, expSA);
    gsl_vector_set(expectedD, i, expD);
  }

  return(chiSquared);
}

/* took this out of Jim Kent's tree */
int chopByWhite(char *in, char *outArray[], int outSize)
/* Like chopString, but specialized for white space separators. */
{
int recordCount = 0;
char c;
for (;;)
    {
    if (outArray != NULL && recordCount >= outSize)
	break;

    /* Skip initial separators. */
    while (isspace(*in)) ++in;
    if (*in == 0)
	break;

    /* Store start of word and look for end of word. */
    if (outArray != NULL)
	outArray[recordCount] = in;
    recordCount += 1;
    for (;;)
        {
        if ((c = *in) == 0)
            break;
        if (isspace(c))
            break;
        ++in;
        }
    if (*in == 0)
	break;

    /* Tag end of word with zero. */
    if (outArray != NULL)
	*in = 0;
    /* And skip over the zero. */
    in += 1;
    }
return recordCount;
}


/*
Copyright (C) 2019-2020 JingWeiZhangHuai <jingweizhanghuai@163.com>
Licensed under the Apache License, Version 2.0; you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
*/

#include <cblas.h>
#include "morn_tensor.h"

void _GetRowData0(float *tdata,float *data,int height,int width,int y_locate,int x_locate)
{
    if(y_locate<0)       {memset(data,0,width*sizeof(float)); return;}
    if(y_locate>=height) {memset(data,0,width*sizeof(float)); return;}

    tdata = tdata+y_locate*width;
    if(x_locate<0)
    {
        memset(data,0,(0-x_locate)*sizeof(float));
        memcpy(data-x_locate,tdata,(width+x_locate)*sizeof(float));
        return;
    }
    if(x_locate>0)
    {
        memcpy(data,tdata+x_locate,(width-x_locate)*sizeof(float));
        memset(data+width-x_locate,0,x_locate*sizeof(float));
        return;
    }
    memcpy(data,tdata,width*sizeof(float));
}
void _GetRowData(float *tdata,float *data,int height,int width,int y_locate,int x_locate,int stride)
{
    if(y_locate<0)       {memset(data,0,width/stride*sizeof(float)); return;}
    if(y_locate>=height) {memset(data,0,width/stride*sizeof(float)); return;}

    tdata = tdata+y_locate*width;

    int i=x_locate;int n=0;
    for(;i<0                        ;i+=stride) {data[n]=0;       n++;}
    for(;i<MIN(width,width+x_locate);i+=stride) {data[n]=tdata[i];n++;}
    for(;i<width+x_locate           ;i+=stride) {data[n]=0;       n++;}
}

void _SetRowData(float *tdata,float *data,int height,int width,int y_locate,int x_locate,int stride)
{
    if(y_locate<0)       return;
    if(y_locate>=height) return;

    tdata = tdata+y_locate*width;

    int i=x_locate;int n=0;
    for(;i<0                        ;i+=stride) {                  n++;}
    for(;i<MIN(width,width+x_locate);i+=stride) {tdata[i]+=data[n];n++;}
  //for(;i<width+x_locate           ;i+=stride) {                  n++;}
}

void ConvTensorToMatData0(MTensor *tns,int bc,float *mdata,int knl_height,int knl_width)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    // int mheight= knl_height*knl_width*channel+1;
    int mwidth = width*height;

    int x1=0-(knl_width /2);int x2=x1+knl_width -1;
    int y1=0-(knl_height/2);int y2=y1+knl_height-1;

    float *tdata = tns->data[bc];
    for(int c=0;c<channel;c++)
    {
        int k=0;
        {
            for(int i=x1;i<=x2;i++)
            {
                float *data = mdata + k*mwidth;k++;
                for(int n=y1;n<y1+height;n++)
                {
                    _GetRowData0(tdata,data,height,width,n,i);
                    data+=width;
                }
            }
        }
        for(int j=y1+1+height-1;j<=y2+height-1;j++)
        {
            for(int i=x1;i<=x2;i++)
            {
                float *data = mdata + k*mwidth;k++;
                memcpy(data,data-knl_width*mwidth+width,(mwidth-width)*sizeof(float));
                _GetRowData0(tdata,data+(mwidth-width),height,width,j,i);
            }
        }
        tdata+=width*height;
        mdata+=mwidth*knl_height*knl_width;
    }
    for(int i=0;i<mwidth;i++) mdata[i]=1.0;
}

void ConvTensorToMatData(MTensor *tns,int bc,float *mdata,int knl_height,int knl_width,int y_stride,int x_stride)
{
    if((x_stride==1)&&(y_stride==1)) {ConvTensorToMatData0(tns,bc,mdata,knl_height,knl_width);return;}

    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width = width/x_stride;
    int out_height=height/y_stride;
    // int mheight= knl_height*knl_width*channel+1;
    int mwidth = out_width*out_height;

    int x1=0-(knl_width /2);int x2=x1+knl_width -1;
    int y1=0-(knl_height/2);int y2=y1+knl_height-1;

    float *tdata = tns->data[bc];
    for(int c=0;c<channel;c++)
    {
        int k=0;
        for(int j=y1;j<=y2;j++)for(int i=x1;i<=x2;i++)
        {
            float *data = mdata + k*mwidth;k++;
            for(int n=j;n<j+height;n+=y_stride)
            {
                _GetRowData(tdata,data,height,width,n,i,x_stride);
                data+=out_width;
            }
        }
        tdata+=width*height;
        mdata+=mwidth*knl_height*knl_width;
    }
    for(int i=0;i<mwidth;i++) mdata[i]=1.0;
}

void ConvMatDataToTensor(float *mdata,MTensor *tns,int bc,int knl_height,int knl_width,int y_stride,int x_stride)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width = width/x_stride;
    int out_height=height/y_stride;
    // int mheight= knl_height*knl_width*channel+1;
    int mwidth = out_height*out_width;

    // printf("height=%d,width=%d,channel=%d,out_height=%d,out_width=%d\n",height,width,channel,out_height,out_width);

    int x1=0-(knl_width /2);int x2=x1+knl_width -1;
    int y1=0-(knl_height/2);int y2=y1+knl_height-1;

    float *tdata = tns->data[bc];

    for(int c=0;c<channel;c++)
    {
        int k=0;
        for(int j=y1;j<=y2;j++)for(int i=x1;i<=x2;i++)
        {
            float *data = mdata + k*mwidth;k++;
            for(int n=j;n<j+height;n+=y_stride)
            {
                _SetRowData(tdata,data,height,width,n,i,x_stride);
                data+=out_width;
            }
        }
        tdata+=width*height;
        mdata+=mwidth*knl_height*knl_width;
    }
}

struct TensorConvPara
{
    MLayer *prev;

    int knl_num;
    int knl_height;
    int knl_width;

    int x_stride;
    int y_stride;

    int res_valid;

    float rate;
    float decay;
    float momentum;
};
void *mTensorConvPara(MFile *ini,char *name)
{
    struct TensorConvPara *para = (struct TensorConvPara *)mMalloc(sizeof(struct TensorConvPara));

    para->prev = mNetworkLayer(ini,mINIRead(ini,name,"prev"));
    mException((para->prev == NULL),EXIT,"invalid prev");
    para->res_valid = (strcmp("Input",mLayerType(para->prev))!=0);

    para->knl_num   = 1; mINIRead(ini,name,"knl_num"   ,"%d",&(para->knl_num   ));
    para->knl_height= 1; mINIRead(ini,name,"knl_height","%d",&(para->knl_height));
    para->knl_width = 1; mINIRead(ini,name,"knl_width" ,"%d",&(para->knl_width ));
    para->x_stride  = 1; mINIRead(ini,name,"x_stride"  ,"%d",&(para->x_stride  ));
    para->y_stride  = 1; mINIRead(ini,name,"y_stride"  ,"%d",&(para->y_stride  ));

    para->rate    =0.001;if(mINIRead(ini,name,"rate"    ,"%f",&(para->rate    ))==NULL) mINIRead(ini,"para","rate"    ,"%f",&(para->rate    ));
    para->momentum=0.9  ;if(mINIRead(ini,name,"momentum","%f",&(para->momentum))==NULL) mINIRead(ini,"para","momentum","%f",&(para->momentum));
    para->decay   =0.01 ;if(mINIRead(ini,name,"decay"   ,"%f",&(para->decay   ))==NULL) mINIRead(ini,"para","decay"   ,"%f",&(para->decay   ));
    mException((para->decay<0.0f)||(para->decay>=1.0f),EXIT,"invalid para decay");

    return para;
}

struct HandleTensorConv
{
    float *mat;
    float *kernel;
    float *update;
};
void endTensorConv(void *info)
{
    struct HandleTensorConv *handle = (struct HandleTensorConv *)info;
    if(handle->mat   != NULL) mFree(handle->mat);
    if(handle->kernel!= NULL) mFree(handle->kernel);
    if(handle->update!= NULL) mFree(handle->update);
}
#define HASH_TensorConv 0x9087d39c
void TensorConvSet(MLayer *layer)
{
    if(layer->state != DFLT) return;
    // mException(strcmp("Conv",mLayerType(layer)),EXIT,"invalid layer type");
    struct TensorConvPara *para = (struct TensorConvPara *)(layer->para);
    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out= layer->tns;

    MHandle *hdl=mHandle(out,TensorConv);
    struct HandleTensorConv *handle = (struct HandleTensorConv *)(hdl->handle);

    int out_height= in->height/para->y_stride;
    int out_width = in->width /para->x_stride;
    int mwidth = (out_height*out_width);
    int mheight= para->knl_height*para->knl_width*in->channel+1;
    int data_size = para->knl_num*mheight;

    mTensorRedefine(out,in->batch,para->knl_num,out_height,out_width,NULL);
    if(morn_network_flag == MORN_TRAIN)
    {
        if(INVALID_TENSOR(res)) mTensorRedefine(res,in->batch,in->channel,in->height,in->width,in->data);
        else
        {
            // printf("llllllllllllllllllllllllll layer->name=%s\n",layer->name);
            mTensorRedefine(res,in->batch,in->channel,in->height,in->width,NULL);
        }

        if(morn_network_flag==MORN_TRAIN)
        {
            if(handle->update != NULL) mFree(handle->update);
            handle->update =(float *)mMalloc(data_size*sizeof(float));
            memset(handle->update,0,data_size*sizeof(float));
        }
    }

    if(handle->kernel !=NULL) mFree(handle->kernel);
    handle->kernel = (float *)mMalloc(data_size*sizeof(float));

    if(morn_network_parafile==NULL)
    {
        float scale = sqrt(2.0f/mheight);
        for(int i=0;i<data_size;i++)
            handle->kernel[i] = mNormalRand(0.0f,1.0f)*scale;
    }
    else
    {
        mNetworkParaRead(layer,"kernel",handle->kernel,data_size*sizeof(float));
    }

    if(handle->mat!=NULL) mFree(handle->mat);
    handle->mat = (float *)mMalloc(mheight*mwidth*sizeof(float));

    hdl->valid = 1;
}

void mTensorConvForward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    struct TensorConvPara *para = (struct TensorConvPara *)(layer->para);

    MTensor *in = para->prev->tns;
    MTensor *out=layer->tns;
    // printf("in->data[0][0]=%f\n",in->data[0][0]);

    TensorConvSet(layer);

    MHandle *hdl=mHandle(out,TensorConv);
    struct HandleTensorConv *handle = (struct HandleTensorConv *)(hdl->handle);

    int mwidth = (out->height*out->width);
    int mheight= para->knl_height*para->knl_width*in->channel+1;

    float *kernel_data= handle->kernel;
    float *in_data = handle->mat;

    for(int b=0;b<in->batch;b++)
    {
        // printf("ccccccccccccccc in->width=%d\n",in->width);
        ConvTensorToMatData(in,b,in_data,para->knl_height,para->knl_width,para->y_stride,para->x_stride);
        // printf("ccccccccccccccc in->width=%d\n",in->width);
        float *out_data = out->data[b];

        // printf("\nweight=\n");for(int ii=0;ii<100;ii++) printf("%f,",kernel_data[ii]);
        // printf("\ndata=\n");for(int ii=0;ii<100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata2352=\n");for(int ii=2352;ii<2352+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata6272=\n");for(int ii=6272;ii<6272+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata10192=\n");for(int ii=10192;ii<10192+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata=\n");for(int ii=784*5;ii<784*5+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata=\n");for(int ii=784*25;ii<784*25+100;ii++) printf("%f,",in_data[ii]*255);
        // printf("\ndata=\n");for(int ii=10000;ii<10000+100;ii++) printf("%f,",in_data[ii]*255);

        // float sum=0;
        // for(int ii=0;ii<75;ii++)
        // {
        //     sum+=(kernel_data[ii]*in_data[mwidth*ii]);
        //     printf("a[ii]=%f,b[n*ii]=%f,sum=%f,ii=%d,ii*mwidth=%d\n",kernel_data[ii],in_data[mwidth*ii]*255,sum,ii,ii*mwidth);
        // }
        // printf("\nsum=%f\n",sum);

        // printf("para->knl_num=%d,mwidth=%d,mheight=%d\n",para->knl_num,mwidth,mheight);
        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasNoTrans,
                    para->knl_num,mwidth,mheight,
                    1.0f,
                    kernel_data,mheight,
                        in_data,mwidth,
               0.0f,   out_data,mwidth);

        // printf("\nout=\n");for(int ii=00;ii<200;ii++) printf("%f,",out_data[ii]);
    }

    layer->state = MORN_FORWARD;
}

void mTensorConvBackward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    struct TensorConvPara *para = (struct TensorConvPara *)(layer->para);
    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out=layer->res;

    MHandle *hdl=mHandle(layer->tns,TensorConv);
    struct HandleTensorConv *handle = (struct HandleTensorConv *)(hdl->handle);
    mException((hdl->valid == 0),EXIT,"no forward operate");

    int mwidth = (out->height*out->width);
    int mheight= para->knl_height*para->knl_width*in->channel+1;

    float *kernel_data= handle->kernel;
    float *update_data= handle->update;
    float *    in_data= handle->mat;
    float *   res_data= handle->mat;

    mNetworkParaWrite(layer,"kernel",kernel_data,para->knl_num*mheight*sizeof(float));

    float beta = para->momentum;
    for(int b=0;b<out->batch;b++)
    {
        ConvTensorToMatData(in,b,in_data,para->knl_height,para->knl_width,para->y_stride,para->x_stride);
        float *out_data = out->data[b];

        // printf("m=%d,n=%d,k=%d\n",para->knl_num,mheight,mwidth);
        // printf("\ndelta=\n");for(int ii=00;ii<200;ii++) printf("%f,",0-2*out_data[ii]);
        // printf("\nin=\n");for(int ii=00;ii<200;ii++) printf("%f,",in_data[ii]);
        // printf("\nupdate0=\n");for(int ii=00;ii<75;ii++) printf("%f,",0-update_data[ii]*2*beta);
        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasTrans,
                    para->knl_num,mheight,mwidth,
                    1.0f,//1.0f/mwidth,
                       out_data,mwidth,
                        in_data,mwidth,
                    beta,
                    update_data,mheight);

        // printf("\nupdate1=\n");for(int ii=00;ii<75;ii++) printf("%f,",0-update_data[ii]*2);
        // printf("\nupdate=\n");for(int ii=760;ii<760+75;ii++) printf("%f,",update_data[ii]);
        // printf("\nbias_updates=\n");for(int ii=0;ii<32;ii++) printf("%f,",update_data[ii*76+75]);

        beta = 1.0;
    }
    // for(int i=0;i<28;i++)printf("%f,",in_data[i]);printf("\n");
    // for(int i=0;i<8;i++) printf("%f,",kernel_data[i]);
    // printf("aaaaaaaaaaa,para->knl_num=%d,mwidth=%d\n",para->knl_num,mwidth);

    if(para->res_valid)
    {
        // if(para->prev->state == MORN_FORWARD)
        // {
        //     printf("conv res set 0,conv res set 0,conv res set 0,conv res set 0,conv res set 0res->batch=%d\n",res->batch);
        //     for(int b=0;b<res->batch;b++)

        // }

        for(int b=0;b<in->batch;b++)
        {
            float *out_data = out->data[b];

            if(para->prev->state == MORN_FORWARD)
                memset(res->data[b],0,in->height*in->width*in->channel*sizeof(float));

            // printf("\nconvdelta=\n");for(int ii=200;ii<400;ii++) printf("%f,",out_data[ii]);
            // printf("\nconvkernel=\n");for(int ii=00;ii<200;ii++) printf("%f,",kernel_data[ii]);

            cblas_sgemm(CblasRowMajor,CblasTrans,CblasNoTrans,
                        mheight,mwidth,para->knl_num,
                        1.0f,
                        kernel_data,mheight,
                           out_data,mwidth,
                    0.0,   res_data,mwidth);

            // printf("\nconvdeltaout=\n");for(int ii=00;ii<200;ii++) printf("%f,",0-res_data[ii]*2);

            ConvMatDataToTensor(res_data,res,b,para->knl_height,para->knl_width,para->y_stride,para->x_stride);

            // printf("mheight=%d,mwidth=%d\n",mheight,mwidth);
            // printf("\nconvimd=\n");for(int ii=200;ii<400;ii++) printf("%f,",res->data[0][ii]);
        }

        para->prev->state = MORN_BACKWARD;
    }

    // printf("\nkernel_data0=\n");for(int ii=0;ii<200;ii++) printf("%f,",kernel_data[ii]);
    // printf("\nupdate_data=\n");for(int ii=0;ii<200;ii++) printf("%f,",update_data[ii]);
    // printf("%s:update_data=%f,kernel_data=%f,%f\n",layer->name,update_data[10],kernel_data[10],kernel_data[10]-(para->rate/(float)(in->batch))*update_data[10]);
    cblas_saxpby(para->knl_num*mheight,
                 (0.0f-(para->rate/(float)(in->batch))),update_data,1,
                 (1.0f-(para->decay*para->rate))       ,kernel_data,1);

    // printf("\nkernel_data1=\n");for(int ii=0;ii<200;ii++) printf("%f,",kernel_data[ii]);
}

//////////////////////////////////////////////////////////////////////////////////////////////////

void GroupConvTensorToMatData(MTensor *tns,int bc,float *mdata,int knl_channel,int knl_height,int knl_width,int c_stride,int y_stride,int x_stride)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width  =  width/x_stride;
    int out_height = height/y_stride;
    int out_channel=(channel-knl_channel/2+1)/c_stride;
    int mwidth = knl_height*knl_width*knl_channel+1;
    int mheight= out_height*out_width;

    int x0=(width -(out_width -1)*x_stride)/2;
    int y0=(height-(out_height-1)*y_stride)/2;

    float *tdata = tns->data[bc];
    int tsize = tns->height*tns->width;

    int k,i,j,c;
    for(k=0;k<out_channel;k++)
    {
        for(j=0;j<out_width;j++)
        {
            int n=y0           -knl_height/2;
            int m=x0+j*x_stride-knl_width /2;
            for(i=0;i<mwidth-1;i+=knl_channel)
            {
                int h= i/(knl_width*knl_channel)             +n;if(h<0)h=0;else if(h>=height)h=height-1;
                int w=(i%(knl_width*knl_channel))/knl_channel+m;if(w<0)w=0;else if(w>= width)w= width-1;
                for(c=0;c<knl_channel;c++)
                    mdata[(k*mheight+j)*mwidth+i+c]=tdata[MIN((c+k*c_stride),channel-1)*tsize+h*width+w];
            }
            mdata[(k*mheight+j)*mwidth+mwidth-1]=1.0f;
        }
        for(j=out_width;j<mheight;j++)
        {
            int num=MAX(0,(knl_height-y_stride))*knl_width*knl_channel;
            if(num>0) memcpy(mdata+(k*mheight+j)*mwidth,mdata+((k*mheight+j)-out_width)*mwidth+y_stride*knl_width*knl_channel,num*sizeof(float));

            int n=y0+j/out_width*y_stride-knl_height/2;
            int m=x0+j%out_width*x_stride-knl_width /2;
            for(i=num;i<mwidth-1;i+=knl_channel)
            {
                int h= i/(knl_width*knl_channel)             +n;if(h<0)h=0;else if(h>=height)h=height-1;
                int w=(i%(knl_width*knl_channel))/knl_channel+m;if(w<0)w=0;else if(w>= width)w= width-1;
                for(c=0;c<knl_channel;c++)
                    mdata[(k*mheight+j)*mwidth+i+c]=tdata[MIN((c+k*c_stride),channel-1)*tsize+h*width+w];
            }
            mdata[(k*mheight+j)*mwidth+mwidth-1]=1.0f;
        }
    }
}

void GroupConvMatDataToTensor(float *mdata,MTensor *tns,int bc,int knl_channel,int knl_height,int knl_width,int c_stride,int y_stride,int x_stride)
{
    int height = tns->height;
    int width  = tns->width;
    int channel= tns->channel;

    int out_width = width/x_stride;
    int out_height=height/y_stride;
    int out_channel=(channel-knl_channel/2+1)/c_stride;
    int mwidth = knl_height*knl_width*knl_channel+1;
    int mheight= out_height*out_width;

    int x0=(width -(out_width -1)*x_stride)/2;
    int y0=(height-(out_height-1)*y_stride)/2;

    float *tdata = tns->data[bc];
    int tsize = tns->height*tns->width;

    int k,i,j,c;
    for(k=0;k<out_channel;k++)for(j=0;j<mheight;j++)
    {
        int n=y0+j/out_width*y_stride+knl_height/2+1-knl_height;
        int m=x0+j%out_width*x_stride+knl_width /2+1-knl_width ;
        for(i=0;i<mwidth-1;i+=knl_channel)
        {
            int h= i/(knl_width*knl_channel)             +n;if(h<0)h=0;else if(h>=height)h=height-1;
            int w=(i%(knl_width*knl_channel))/knl_channel+m;if(w<0)w=0;else if(w>= width)w= width-1;
            for(c=0;c<knl_channel;c++)
                tdata[MIN((c+k*c_stride),channel-1)*tsize+h*width+w]+=mdata[(k*mheight+j)*mwidth+i+c];
        }
    }
}

struct TensorGroupConvPara
{
    MLayer *prev;

    int knl_num;
    int knl_height;
    int knl_width;

    int x_stride;
    int y_stride;

    int res_valid;

    float rate;
    float decay;
    float momentum;

    int knl_channel;
    int c_stride;
};
void *mTensorGroupConvPara(MFile *ini,char *name)
{
    struct TensorGroupConvPara *para = (struct TensorGroupConvPara *)mMalloc(sizeof(struct TensorConvPara));

    char *value = mINIRead(ini,name,"prev");
    para->prev = mNetworkLayer(ini,value);
    mException((para->prev == NULL),EXIT,"invalid prev");

    para->res_valid = (strcmp("Input",mLayerType(para->prev))!=0);

    value = mINIRead(ini,name,"knl_num");
    if(value != NULL) para->knl_num= atoi(value);else para->knl_num= 1;

    value = mINIRead(ini,name,"knl_channel");
    if(value != NULL) para->knl_channel= atoi(value);else para->knl_channel= DFLT;

    value = mINIRead(ini,name,"knl_height");
    if(value != NULL) para->knl_height= atoi(value);else para->knl_height= 1;

    value = mINIRead(ini,name,"knl_width");
    if(value != NULL) para->knl_width= atoi(value);else para->knl_width= 1;

    value = mINIRead(ini,name,"c_stride");
    if(value != NULL) para->c_stride= atoi(value);else para->c_stride= para->knl_channel;

    value = mINIRead(ini,name,"x_stride");
    if(value != NULL) para->x_stride= atoi(value);else para->x_stride= 1;

    value = mINIRead(ini,name,"y_stride");
    if(value != NULL) para->y_stride= atoi(value);else para->y_stride= 1;

    value = mINIRead(ini,name,"rate");
    if(value != NULL) para->rate = atof(value);
    else
    {
        value = mINIRead(ini,"para","rate");
        if(value != NULL) para->rate = atof(value);
        else              para->rate = 0.001;
    }

    value = mINIRead(ini,name,"decay");
    if(value != NULL) para->decay = atof(value);
    else
    {
        value = mINIRead(ini,"para","decay");
        if(value != NULL) para->decay = atof(value);
        else              para->decay = 0.01;
    }
    mException((para->decay<0.0f)||(para->decay>=1.0f),EXIT,"invalid para decay");

    value = mINIRead(ini,name,"momentum");
    if(value != NULL) para->momentum = atof(value);
    else
    {
        value = mINIRead(ini,"para","momentum");
        if(value != NULL) para->momentum = atof(value);
        else              para->momentum = 0.9;
    }

    return para;
}

struct HandleTensorGroupConv
{
    float *mat;
    float *kernel;
    float *update;
};
void endTensorGroupConv(void *info)
{
    struct HandleTensorGroupConv *handle = (struct HandleTensorGroupConv *)info;
    if(handle->mat   != NULL) mFree(handle->mat);
    if(handle->kernel!= NULL) mFree(handle->kernel);
    if(handle->update!= NULL) mFree(handle->update);
}
#define HASH_TensorGroupConv 0x82866393
void TensorGroupConvSet(MLayer *layer)
{
    if(layer->state != DFLT) return;

    struct TensorGroupConvPara *para = (struct TensorGroupConvPara *)(layer->para);
    if(para->knl_channel<=0)
    {
        layer->type_index=mTensorRegisterIndex("Conv");
        return;
    }

    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out= layer->tns;

    MHandle *hdl=mHandle(out,TensorGroupConv);
    struct HandleTensorGroupConv *handle = (struct HandleTensorGroupConv *)(hdl->handle);

    int out_height= in->height/para->y_stride;
    int out_width = in->width /para->x_stride;
    int out_channel=(in->channel-para->knl_channel/2+1)/para->c_stride;

    int mheight = (out_height*out_width*out_channel);
    int mwidth = para->knl_height*para->knl_width*para->knl_channel+1;
    int data_size = para->knl_num*mwidth;

    mTensorRedefine(out,in->batch,para->knl_num*out_channel,out_height,out_width,NULL);
    if(morn_network_flag == MORN_TRAIN)
    {
        if(INVALID_TENSOR(res)) mTensorRedefine(res,in->batch,in->channel,in->height,in->width,in->data);
        else                    mTensorRedefine(res,in->batch,in->channel,in->height,in->width,NULL);

        if(handle->update != NULL) mFree(handle->update);
        handle->update =(float *)mMalloc(data_size*sizeof(float));
        memset(handle->update,0,data_size*sizeof(float));
    }

    if(handle->kernel !=NULL) mFree(handle->kernel);
    handle->kernel = (float *)mMalloc(data_size*sizeof(float));

    if(morn_network_parafile==NULL)
    {
        float scale = sqrt(2.0f/mwidth);
        for(int i=0;i<data_size;i++)
            handle->kernel[i] = scale*mNormalRand(0.0f,1.0f);
    }
    else
    {
        mNetworkParaRead(layer,"kernel",handle->kernel,data_size*sizeof(float));
    }

    if(handle->mat!=NULL) mFree(handle->mat);
    handle->mat = (float *)mMalloc(mheight*mwidth*sizeof(float));

    hdl->valid = 1;
}

void mTensorGroupConvForward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    struct TensorGroupConvPara *para = (struct TensorGroupConvPara *)(layer->para);
    if(para->knl_channel<=0) return mTensorConvForward(layer);

    mException(strcmp("GroupConv",mLayerType(layer)),EXIT,"invalid layer type");

    MTensor *in = para->prev->tns;
    MTensor *out=layer->tns;

    TensorGroupConvSet(layer);

    MHandle *hdl=mHandle(out,TensorGroupConv);
    struct HandleTensorGroupConv *handle = (struct HandleTensorGroupConv *)(hdl->handle);

    int out_channel=(in->channel-para->knl_channel/2+1)/para->c_stride;
    int mheight = (out->height*out->width*out_channel);
    int mwidth = para->knl_height*para->knl_width*para->knl_channel+1;

    float *kernel_data= handle->kernel;
    float *in_data = handle->mat;

    for(int b=0;b<in->batch;b++)
    {
        GroupConvTensorToMatData(in,b,in_data,para->knl_channel,para->knl_height,para->knl_width,para->c_stride,para->y_stride,para->x_stride);
        float *out_data = out->data[b];

        in_data[mwidth-1]=1.0f;

        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasTrans,
                    para->knl_num,mheight,mwidth,
                    1.0f,
                    kernel_data,mwidth,
                        in_data,mwidth,
               0.0f,   out_data,mheight);
    }

    layer->state = MORN_FORWARD;
}

void mTensorGroupConvBackward(MLayer *layer)
{
    mException(INVALID_POINTER(layer),EXIT,"invalid input");
    struct TensorGroupConvPara *para = (struct TensorGroupConvPara *)(layer->para);
    if(para->knl_channel<=0) return mTensorConvBackward(layer);

    mException(strcmp("GroupConv",mLayerType(layer)),EXIT,"invalid layer type");

    MTensor *in = para->prev->tns;
    MTensor *res= para->prev->res;
    MTensor *out=layer->res;

    MHandle *hdl=mHandle(layer->tns,TensorGroupConv);
    struct HandleTensorGroupConv *handle = (struct HandleTensorGroupConv *)(hdl->handle);
    mException((hdl->valid == 0),EXIT,"no forward operate");

    int out_channel=(in->channel-para->knl_channel/2+1)/para->c_stride;
    int mheight = (out->height*out->width*out_channel);
    int mwidth = para->knl_height*para->knl_width*para->knl_channel+1;

    float *kernel_data= handle->kernel;
    float *update_data= handle->update;
    float *    in_data= handle->mat;
    float *   res_data= handle->mat;

    mNetworkParaWrite(layer,"kernel",kernel_data,para->knl_num*mwidth*sizeof(float));

    for(int b=0;b<in->batch;b++)
    {
        GroupConvTensorToMatData(in,b,in_data,para->knl_channel,para->knl_height,para->knl_width,para->c_stride,para->y_stride,para->x_stride);
        float *out_data = out->data[b];

        in_data[mwidth-1]=1.0f;

        cblas_sgemm(CblasRowMajor,CblasNoTrans,CblasNoTrans,
                    para->knl_num,mwidth,mheight,
                    1.0f,
                       out_data,mheight,
                        in_data,mwidth,
                    (b==0)?para->momentum:1.0f,
                    update_data,mwidth);
    }
    cblas_saxpby(para->knl_num*mwidth,
                 (0.0f-(para->rate/(float)(in->batch))),update_data,1,
                 (1.0f-(para->rate*para->decay))       ,kernel_data,1);

    if(para->res_valid==0) return;

    if(para->prev->state == MORN_FORWARD)
    {
        for(int b=0;b<res->batch;b++)
            memset(res->data[b],0,in->height*in->width*in->channel*sizeof(float));
        para->prev->state = MORN_BACKWARD;
    }

    for(int b=0;b<in->batch;b++)
    {
        float *out_data = out->data[b];

        cblas_sgemm(CblasRowMajor,CblasTrans,CblasNoTrans,
                    mheight,mwidth,para->knl_num,
                    1.0f,
                       out_data,mheight,
                    kernel_data,mwidth,
                0.0,   res_data,mwidth);

        GroupConvMatDataToTensor(res_data,res,b,para->knl_channel,para->knl_height,para->knl_width,para->c_stride,para->y_stride,para->x_stride);
    }
}


#ifndef SCOL_H
#define SCOL_H
#include "Common.h"
// SP_IVP.h
// Spherical Collapse IVP
//#include <omp.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <cvode/cvode.h>               /* prototypes for CVODE fcts., consts.  */
#include <nvector/nvector_serial.h>    /* access to serial N_Vector            */
#include <sunmatrix/sunmatrix_dense.h> /* access to dense SUNMatrix            */
#include <sunlinsol/sunlinsol_dense.h> /* access to dense SUNLinearSolver      */
#include <cvode/cvode_direct.h>        /* access to CVDls interface            */
#include <sundials/sundials_types.h>   /* defs. of realtype, sunindextype      */
#include <sundials/sundials_math.h>    /* contains the macros ABS, SUNSQR, EXP */

#define Ith(v,i)    NV_Ith_S(v,i-1)         /* Ith numbers components 1..NEQ */
#define IJth(A,i,j) SM_ELEMENT_D(A,i-1,j-1) /* IJth numbers rows,cols 1..NEQ */

#define NEQ     2                /* number of equations  */
#define Y1      RCONST(0.0)      /* initial y components */
#define RTOL    RCONST(1.0e-5)   /* scalar relative tolerance            */
#define ATOL1   RCONST(1.0e-11)   /* vector absolute tolerance components */
#define ATOL2   RCONST(1.0e-11)
#define T00      RCONST(-10.41431317630211772495840705232694745063781738281250)      /* initial time           */
#define NOUT    1024               /* number of output times */
#define NOUT_DC    10               /* number of output times */

#define HALF    RCONST(0.5)      /* 0.5                    */
#define ONE     RCONST(1.0)      /* 1.0                    */
#define TWO     RCONST(2.0)
#define THREE   RCONST(3.0)       /* 3.0                    */
#define DELTA1  RCONST(3e-5)   /* Delta_i1 */
#define DELTA2  RCONST(0.00012)   /* Delta_i2 */
#define EPSILON RCONST(1.0e-9)
#define ZERO    RCONST(0.0)
#define NINE    RCONST(9.0)
#define TEN     RCONST(10.0)
#define Gnewton RCONST(4.302e-09)
#define coef    RCONST(8987404.41) // 1/H0^2

// structures to store spherical collapse calculations
typedef struct arrays{
  int count;
  double xx[1002];
  double yy[1002]; } *arrays_T;

typedef struct arrays3D{
  int count;
  double xx[1002];
  double yy[1002];
  double zz[1002]; } *arrays_T3;

typedef struct usdat {
  realtype IC;
  realtype OM;
  realtype Rth;
  realtype T1;
  double par1;
  double par2;
  double par3;
  int mymg;
  double maxt;
  gsl_spline *spline;
  gsl_interp_accel *acc;
} *UserData;

extern int check_flagscol(void *flagvalue, const char *funcname, int opt); //

class SCOL {
public:

// used functions in example file
   double maxP_zeta(double sig2, double dsig2dR, double OM, double Z);
   double Delta_Lambda(double OM, double Z); //
   // solves for y_enviornment
   int yenv(double OM_REAL, double XF, double delta_envi, arrays_T xxyy); // gives the environmental dependence of spherical collapse
   // solves for y_halo
   int SphericalCollapse(double *dC, arrays_T3 xxyyzz, UserData data_vec, double TMULT_REAL, double delta_g); // spherical collapse solver
   // solves for a_virial
   double myscol(double myscolparams[], double acol, double omega0, double Rthp, double sig1, double sig2, double pars[], int mymg, int yenvf); // solves for virial quantities and stores them in array myscolparams

   void PrintOutput(realtype t, realtype y1, realtype y2);
   void PrintRootInfo(int root_f1); //

   double funcscol(double xi, void *user_data); //

};

#endif