#include <math.h>
#include <time.h>
#include <string.h>
#include <stdarg.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
// #include "libiomp/omp.h"
#include <omp.h>
#include "../include/type.h"
#include "../include/util.h"
#include "../include/global.h"

extern clock_t dur;
extern char *optarg;
extern int optind;

char c_getopt(int argc, char *argv[], char *pattern) {
    static size_t i = 0;
    int j;
    char ret;
    ++optind;

    if (optind > (int)strlen(pattern)) {
        return -1;
    }

    ret = pattern[i++];

    for (j = 0; j < argc; ++j) {
        if (argv[j][0] == '-' && argv[j][1] == ret) {
            optarg = argv[j+1];
        }
    }

    return ret;
}

/**
 * @brief initiator of odiff.
 *
 * @param inc1
 * @param inc2
 * @param skp1
 * @param skp2
 * @param inclu_len
 * @param skip_len
 * @param flag
 * @param id
 *
 * @return
 */
odiff* diff_alloc(gsl_vector* inc1, gsl_vector* inc2,
                  gsl_vector* skp1, gsl_vector* skp2,
                  int inclu_len, int skip_len, int flag, char* id) {
    odiff *diff = (odiff*)malloc(sizeof(odiff));
    diff->inc1 = inc1, diff->skp1 = skp1, diff->inc2 = inc2, diff->skp2 = skp2;
    diff->inclu_len = inclu_len, diff->skip_len = skip_len;
    diff->flag = flag, diff->id = (char*)malloc(sizeof(char)*(strlen(id)+1));
    strcpy(diff->id, id);
    return diff;
}

/**
 * @brief performing unary operation on gsl_vector.
 * @param vec
 * @param fun
 */
void unary_operate_vec(gsl_vector *vec, double (*fun) (double)) {
    size_t i;
    for (i = 0; i < vec->size; ++i) {
        gsl_vector_set(vec, i, fun(gsl_vector_get(vec, i)));
    }
    return;
}

/**
 * @brief performing binary operation on gsl_vector.
 *
 * @param vec
 * @param pa
 * @param fun
 */
void binary_operate_vec(gsl_vector *vec, double pa, double (*fun) (double, const double)) {
    size_t i;
    for (i = 0; i < vec->size; ++i) {
        gsl_vector_set(vec, i, fun(gsl_vector_get(vec, i), pa));
    }
    return;
}

/**
 * @brief performing element-wise operation on gsl_vector.
 *
 * @param vec
 * @param fun
 * @param argc
 * @param ...
 */
void element_wise_vec(gsl_vector *vec, double (*fun) (double, va_list), int argc, ...) {
    va_list argv;
    size_t i;
    for (i = 0; i < vec->size; ++i) {
        va_start(argv, argc);
        gsl_vector_set(vec, i, fun(gsl_vector_get(vec, i), argv));
        va_end(argv);
    }
    return;
}

/**
 * @brief cumulative summation of a gsl_vector.
 *
 * @param vec
 * @param fun
 * @param argc
 * @param ...
 *
 * @return
 */
double cuscumsum(gsl_vector *vec, double (*fun) (double, va_list), int argc, ...) {
    va_list argv, tmp;
    double sum = 0;
    size_t i;
    va_start(argv, argc);
    for (i = 0; i < vec->size; ++i) {
        va_copy(tmp, argv);
        sum += fun(gsl_vector_get(vec, i), tmp);
    }
    va_end(tmp);
    va_end(argv);
    return sum;
}

int parse_title(char* str, char** output) {
    int idx = 0;
    char *token = strtok(str, " \t\r\n\v\f");
    while(token) {
        output[idx] = (char*)malloc(sizeof(char)*TITLE_ELEMENT_LEN);
        strcpy(output[idx++], token);
        token = strtok(NULL, " \t\r\n\v\f");
    }

    // int base = 0;
    // do {
    //     base += idx? strlen(output[idx-1]) + LEN_OF_NT : 0;
    //     output[idx] = (char*)malloc(sizeof(char)*TITLE_ELEMENT_LEN);
    // } while(sscanf(str + base, "%s \t\n", output[idx++]) != EOF);

    return idx-1;
}

/**
 * @brief
 *
 * @param input a string in format '1,2,3,4,5,6,...'.
 * @param vec
 *
 * @return
 */
int str_to_vector(char* input, gsl_vector** vec) {
    int idx = 0, count = 1;
    size_t i = 0;
    // gsl_vector_free(*vec);

    for (i = 0; i < strlen(input); ++i) {
        if (input[i] == ',') {
            ++count;
        }
    }

    *vec = gsl_vector_alloc(count);
    char *str = strtok(input, ",");
    while(str) {
        if (strcmp(str, "NA") == 0 || strcmp(str, "NAN") == 0 ||
            strcmp(str, "na") == 0 || strcmp(str, "nan") == 0) {
            return -1;
        }
        gsl_vector_set(*vec, idx++, atof(str));
        str = strtok(NULL, ",");
    }

    return count;
}

int parse_line(char* str, char* id, gsl_vector** inc1, gsl_vector** skp1,
               gsl_vector** inc2, gsl_vector** skp2,
               int* inclu_len, int* skip_len) {
    int idx = 0;
    char output[7][COLUMN_LEN];

    char *token = strtok(str, " \t\r\n\v\f");
    while(token) {
        strcpy(output[idx++], token);
        token = strtok(NULL, " \t\r\n\v\f");
    }

    // int base = 0;
    // do {
    //     base += idx? strlen(output[idx-1]) + LEN_OF_NT : 0;
    // } while(sscanf(str + base, "%s \t\n", output[idx++]) != EOF);

    strcpy(id, output[0]);
    if (str_to_vector(output[1], inc1) == -1 ||
        str_to_vector(output[2], skp1) == -1 ||
        str_to_vector(output[3], inc2) == -1 ||
        str_to_vector(output[4], skp2) == -1) {
        printf("An error occured. rMATS cannot handle missing value. Sample Id: %s\n", id);
        printf("Exiting.\n");
        exit(0);

    }
    *inclu_len = atoi(output[5]);
    *skip_len = atoi(output[6]);

    return idx-1;
}

int parse_file(const char* filename, diff_list_node* list, char** title_element_list) {
    FILE *ifp;
    size_t olen = MAX_LINE;
    char *str_line = (char*)malloc(sizeof(char)*olen);
    char title[TITLE_LEN], id[MAX_LINE];
    int row_num=0, inclu_len, skip_len;
    gsl_vector *inc1 = NULL, *skp1 = NULL, *inc2 = NULL, *skp2 = NULL;

    if ((ifp = fopen(filename, "r")) == NULL) {
        printf("Fail to open!");
        return 0;
    }
    fgets(title, TITLE_LEN, ifp);
    parse_title(title, title_element_list);

    while (getline(&str_line, &olen, ifp) != -1) {
        ++row_num;
        parse_line(str_line, id, &inc1, &skp1, &inc2, &skp2, &inclu_len, &skip_len);
        if (inc1->size != skp1->size || inc2->size != skp2->size) {
            printf("An error occured. The length of the pair of vector should be equal. Sample Id: %s\n", id);
            printf("Size of vector: %ld, %ld, %ld, %ld\n", inc1->size, skp1->size, inc2->size, skp2->size);
            printf("Exiting\n");
            exit(0);
        }
        gsl_vector_add(inc1, skp1), gsl_vector_add(inc2, skp2);

        // TODO | or || ?
        // According to original python code 'if (vecprod(vecadd(inc1,skp1))==0) | (vecprod(vecadd(inc2,skp2))==0):',
        // it's a bit arithmetic '|'. However,  it should be a logical 'or' in such senario.
        if (cumprod(inc1) == 0 || cumprod(inc2) == 0) {
            gsl_vector_sub(inc1, skp1), gsl_vector_sub(inc2, skp2);
            diff_append(list, diff_alloc(inc1, inc2, skp1, skp2, inclu_len, skip_len, 0, id));
        } else {
            gsl_vector_sub(inc1, skp1), gsl_vector_sub(inc2, skp2);

            // TODO According to original python code, this function will not change anything.
            // original comment: add 1 in both inclusion and skipping counts for robustness in small sample size
            gsl_vector_add_constant(inc1, 0.0), gsl_vector_add_constant(skp1, 0.0);
            gsl_vector_add_constant(inc2, 0.0), gsl_vector_add_constant(skp2, 0.0);
            diff_append(list, diff_alloc(inc1, inc2, skp1, skp2, inclu_len, skip_len, 1, id));
        }
    }
    free(str_line);
    fclose(ifp);

    return row_num;
}

void display_dvector(const gsl_vector* vec) {
    size_t i;
    for (i = 0; i < vec->size; ++i) {
        printf("%.10f ", gsl_vector_get(vec, i));
    }
}

/**
 * @brief handy function.
 *
 * @param i
 * @param argv
 *
 * @return
 */
double logit(double i) {
    if (i < 0.01) {
        i = 0.01;
    } else if (i > 0.99) {
        i = 0.99;
    }
    return log(i/(1-i));
}

/*
double log_and_minus_x(const double i, va_list argv) {
    double pa = va_arg(argv, double);
    return logit(i) - logit(pa);
}

double rev_log_and_minus_x(const double i, va_list argv) {
    double pa = va_arg(argv, double);
    return pow(M_E, i + log(pa));
}
*/

/**
 * @brief cumulative production of a gsl_vector.
 *
 * @param vec
 *
 * @return
 */
double cumprod(const gsl_vector* vec) {
    double res = 1;
    size_t i;
    for (i = 0; i < vec->size; ++i) {
        res *= gsl_vector_get(vec, i);
    }
    return res;
}

// for multivar, 1, 2
double sum_for_multivar(const double i, va_list argv) {
    double pa = va_arg(argv, double);
    return pow(logit(i)-logit(pa), 2);
}

// for multivar_der, 1_der, 2_der
double sum_for_multivar_der(const double i, va_list argv) {
    double pa = va_arg(argv, double);
    // return -2 * (logit(i) - logit(pa))/(pa*(1-pa));
    return 2 * (logit(i) - logit(pa))/(pa*pa-pa);
}

double myfunc_marginal_2der(const double x, const double I, const double S,
                            const double beta, const double var,
                            const int inclu_len, const int skip_len) {
    double tmp1, tmp2, tmp3, one_x = 1-x;
    double powx = pow(x,2), pow1_x = pow(one_x,2), pow_len = pow(inclu_len*x + skip_len*one_x,2);

    tmp1 = (((2 * x - 1) * (logit(beta) - logit(x)) - 1)/var - 1)/(powx*pow1_x);
    tmp2 = I * skip_len * (2*inclu_len*x+skip_len)/(powx * pow_len);
    tmp3 = S * inclu_len * (inclu_len+2*skip_len*one_x)/(pow1_x * pow_len);

    return tmp1 - tmp2 - tmp3;
}

// for marginal
double sum_for_marginal(const double i, va_list argv) {
    int *idx = va_arg(argv, int*);
    double beta = va_arg(argv, double);
    double I_ = gsl_vector_get(va_arg(argv, gsl_vector*), *idx);
    double S_ = gsl_vector_get(va_arg(argv, gsl_vector*), *idx);
    double var = va_arg(argv, double), new_psi, f1, f1_2der;
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int);
    *idx += 1;

    new_psi = inclu_len * i/(inclu_len * i + skip_len * (1 - i));
    f1 = I_ * log(new_psi) + S_ * log(1 - new_psi) -
         pow(logit(i) - logit(beta),2)/(2*var) - log(i) - log(1-i);
    f1_2der = fabs(myfunc_marginal_2der(i, I_, S_, beta,
                                        var, inclu_len, skip_len));

    return 0.5 * log(fabs(f1_2der) + 0.00001) - f1;
}

// for marginal_der
double sum_for_marginal_der(const double i, va_list argv) {
    int *idx = va_arg(argv, int*);
    double beta = va_arg(argv, double);
    double I_ = gsl_vector_get(va_arg(argv, gsl_vector*), *idx);
    double S_ = gsl_vector_get(va_arg(argv, gsl_vector*), *idx);
    double var = va_arg(argv, double);
    int inclu_len = va_arg(argv, int), skip_len = va_arg(argv, int);
    double f1_1der, f1_2der, f1_3der, one_i = 1-i;
    *idx += 1;
    double powi = pow(i,2), pow1_i = pow(one_i,2);

    f1_3der = (2 * i - 1)/(powi * pow1_i*beta*(1-beta)*var);
    f1_2der = myfunc_marginal_2der(i, I_, S_, beta, var, inclu_len, skip_len);
    f1_1der = (logit(i) - logit(beta))/(beta * (1-beta) * var);

    return 0.5 * f1_3der/f1_2der - f1_1der;
}

// function used to manipulate our linked list.
int diff_append(diff_list_node* header, odiff* data) {
    diff_list_node *tmp = (diff_list_node*)malloc(sizeof(diff_list_node));
    tmp->data = data;
    tmp->end = tmp;
    tmp->next = NULL;
    header->end->next = tmp;
    header->end->end = tmp;
    header->end = tmp;

    return 0;
}

int diff_insert(diff_list_node* header, odiff* data, int idx) {

    return 6;
}

int diff_get_next(diff_list_node* header, odiff* data) {

    return 6;
}

int diff_get_at(diff_list_node* header, odiff* data, int idx) {
    int i;
    diff_list_node* tmp = header;
    for (i = 0; i <= idx; ++i) {
        tmp = tmp->next;
    }
    *data = *(tmp->data);
    return 0;
}

void split_data_list(int lenofl, int batch_size, diff_list_node* list, batch_datum *ret) {
    int i = 0, group = lenofl/batch_size, carry = lenofl % batch_size;
    diff_list_node *node = list;
    odiff **datum = (odiff**)malloc(sizeof(odiff*)*(group+1));
    for (i = 0; i < group+1; ++i) {
        datum[i] = (odiff*)malloc(sizeof(odiff)*batch_size);
    }

    for (i = 0; i < lenofl; ++i) {
        node = node->next;
        datum[i/batch_size][i%batch_size] = *(node->data);
    }
    for (i = 0; i < group; ++i) {
        ret[i].batch_size = batch_size;
        ret[i].datum = (void**)&datum[i];
    }
    ret[group].batch_size = carry;
    ret[group].datum = (void**)&datum[group];

    return;
}

void mp_threadpool(int nthread, int ntask, void* (*func)(void *), void** datum, void **ret) {
    int i;
    omp_set_num_threads(nthread);

    #pragma omp parallel for private(i)
    for (i = 0; i < ntask; ++i) {
        ret[i] = (*func)(datum[i]);
    }

    return;
}

/**
 * @brief C wrapper of Fortran l_bfgs_b routine.
 *
 * @param n
 * @param m
 * @param x[]
 * @param l[]
 * @param u[]
 * @param nbd[]
 * @param fp
 * @param gp
 * @param factr
 * @param pgtol
 * @param iprint
 * @param maxiter
 * @param argc
 * @param ...
 *
 * @return
********************************************************************
c     --------------------------------------------------------------
c             DESCRIPTION OF THE VARIABLES IN L-BFGS-B
c     --------------------------------------------------------------
c
c     n is an INTEGER variable that must be set by the user to the
c       number of variables.  It is not altered by the routine.
c
c     m is an INTEGER variable that must be set by the user to the
c       number of corrections used in the limited memory matrix.
c       It is not altered by the routine.  Values of m < 3  are
c       not recommended, and large values of m can result in excessive
c       computing time. The range  3 <= m <= 20 is recommended.
c
c     x is a DOUBLE PRECISION array of length n.  On initial entry
c       it must be set by the user to the values of the initial
c       estimate of the solution vector.  Upon successful exit, it
c       contains the values of the variables at the best point
c       found (usually an approximate solution).
c
c     l is a DOUBLE PRECISION array of length n that must be set by
c       the user to the values of the lower bounds on the variables. If
c       the i-th variable has no lower bound, l(i) need not be defined.
c
c     u is a DOUBLE PRECISION array of length n that must be set by
c       the user to the values of the upper bounds on the variables. If
c       the i-th variable has no upper bound, u(i) need not be defined.
c
c     nbd is an INTEGER array of dimension n that must be set by the
c       user to the type of bounds imposed on the variables:
c       nbd(i)=0 if x(i) is unbounded,
c              1 if x(i) has only a lower bound,
c              2 if x(i) has both lower and upper bounds,
c              3 if x(i) has only an upper bound.
c
c     f is a DOUBLE PRECISION variable.  If the routine setulb returns
c       with task(1:2)= 'FG', then f must be set by the user to
c       contain the value of the function at the point x.
c
c     g is a DOUBLE PRECISION array of length n.  If the routine setulb
c       returns with taskb(1:2)= 'FG', then g must be set by the user to
c       contain the components of the gradient at the point x.
c
c     factr is a DOUBLE PRECISION variable that must be set by the user.
c       It is a tolerance in the termination test for the algorithm.
c       The iteration will stop when
c
c        (f^k - f^{k+1})/max{|f^k|,|f^{k+1}|,1} <= factr*epsmch
c
c       where epsmch is the machine precision which is automatically
c       generated by the code. Typical values for factr on a computer
c       with 15 digits of accuracy in double precision are:
c       factr=1.d+12 for low accuracy;
c             1.d+7  for moderate accuracy;
c             1.d+1  for extremely high accuracy.
c       The user can suppress this termination test by setting factr=0.
c
c     pgtol is a double precision variable.
c       On entry pgtol >= 0 is specified by the user.  The iteration
c         will stop when
c
c                 max{|proj g_i | i = 1, ..., n} <= pgtol
c
c         where pg_i is the ith component of the projected gradient.
c       The user can suppress this termination test by setting pgtol=0.
c
c     wa is a DOUBLE PRECISION  array of length
c       (2mmax + 4)nmax + 12mmax^2 + 12mmax used as workspace.
c       This array must not be altered by the user.
c
c     iwa is an INTEGER  array of length 3nmax used as
c       workspace. This array must not be altered by the user.
c
c     task is a CHARACTER string of length 60.
c       On first entry, it must be set to 'START'.
c       On a return with task(1:2)='FG', the user must evaluate the
c         function f and gradient g at the returned value of x.
c       On a return with task(1:5)='NEW_X', an iteration of the
c         algorithm has concluded, and f and g contain f(x) and g(x)
c         respectively.  The user can decide whether to continue or stop
c         the iteration.
c       When
c         task(1:4)='CONV', the termination test in L-BFGS-B has been
c           satisfied;
c         task(1:4)='ABNO', the routine has terminated abnormally
c           without being able to satisfy the termination conditions,
c           x contains the best approximation found,
c           f and g contain f(x) and g(x) respectively;
c         task(1:5)='ERROR', the routine has detected an error in the
c           input parameters;
c       On exit with task = 'CONV', 'ABNO' or 'ERROR', the variable task
c         contains additional information that the user can print.
c       This array should not be altered unless the user wants to
c          stop the run for some reason.  See driver2 or driver3
c          for a detailed explanation on how to stop the run
c          by assigning task(1:4)='STOP' in the driver.
c
c     iprint is an INTEGER variable that must be set by the user.
c       It controls the frequency and type of output generated:
c        iprint<0    no output is generated;
c        iprint=0    print only one line at the last iteration;
c        0<iprint<99 print also f and |proj g| every iprint iterations;
c        iprint=99   print details of every iteration except n-vectors;
c        iprint=100  print also the changes of active set and final x;
c        iprint>100  print details of every iteration including x and g;
c       When iprint > 0, the file iterate.dat will be created to
c                        summarize the iteration.
c
c     csave  is a CHARACTER working array of length 60.
c
c     lsave is a LOGICAL working array of dimension 4.
c       On exit with task = 'NEW_X', the following information is
c         available:
c       lsave(1) = .true.  the initial x did not satisfy the bounds;
c       lsave(2) = .true.  the problem contains bounds;
c       lsave(3) = .true.  each variable has upper and lower bounds.
c
c     isave is an INTEGER working array of dimension 44.
c       On exit with task = 'NEW_X', it contains information that
c       the user may want to access:
c         isave(30) = the current iteration number;
c         isave(34) = the total number of function and gradient
c                         evaluations;
c         isave(36) = the number of function value or gradient
c                                  evaluations in the current iteration;
c         isave(38) = the number of free variables in the current
c                         iteration;
c         isave(39) = the number of active constraints at the current
c                         iteration;
c
c         see the subroutine setulb.f for a description of other
c         information contained in isave
c
c     dsave is a DOUBLE PRECISION working array of dimension 29.
c       On exit with task = 'NEW_X', it contains information that
c         the user may want to access:
c         dsave(2) = the value of f at the previous iteration;
c         dsave(5) = the machine precision epsmch generated by the code;
c         dsave(13) = the infinity norm of the projected gradient;
c
c         see the subroutine setulb.f for a description of other
c         information contained in dsave
c
c     --------------------------------------------------------------
c           END OF THE DESCRIPTION OF THE VARIABLES IN L-BFGS-B
c     --------------------------------------------------------------
c
c     << An example of subroutine 'timer' for AIX Version 3.2 >>
c
c     subroutine timer(ttime)
c     double precision ttime
c     integer itemp, integer mclock
c     itemp = mclock()
c     ttime = dble(itemp)*1.0d-2
c     return
c     end

*********************************************************************
 */
double l_bfgs_b_wrapper(integer n, integer m, doublereal x[], doublereal l[],
                        doublereal u[], integer nbd[],
                        double (*fp) (const double x[], va_list argv),
                        void (*gp) (const double x[], double res[], va_list argv),
                        doublereal factr, doublereal pgtol, integer iprint,
                        int maxfun, int maxiter, int argc, ...) {
    va_list argv, tmp;
    char task[SIXTY], csave[SIXTY];
    logical lsave[4];
    integer iwa[3*n], isave[44];
    doublereal dsave[29], wa[2*m*n+5*n+11*m*m+8*m];
    va_start(argv, argc);

    strcpy(task,"START");
    int i, count = 0, funcount = 0, maxls = 20;
    for(i=5;i<SIXTY;i++) {
        task[i]=' ';
    }

    doublereal f = 0, g[n];
    for (i = 0; i < n; ++i) {
        g[i] = 0;
    }

    // TODO # The minimization routine wants f and g at the current x.
    //      # Note that interruptions due to maxfun are postponed
    //      # until the completion of the current minimization iteration.
    //      # Overwrite f and g:
    do {
        setulb_(&n,&m,x,l,u,nbd,&f,g,&factr,&pgtol,wa,iwa,task,&iprint,
                csave,lsave,isave,dsave, &maxls);

        if (strncmp(task,"FG",2)==0) {

            va_copy(tmp, argv);
            f = fp(x, tmp);
            va_copy(tmp, argv);
            gp(x, g, tmp);
            ++funcount;

        } else if(strncmp(task,"NEW_X",5)==0){
            ++count;
        } else {
            break;
        }
    } while(count < maxiter && funcount < maxfun);

    f = fp(x, argv);
    va_end(argv);
    va_end(tmp);

    return f;
}


/*
 * BRAINS
 * (B)LR (R)everberation-mapping (A)nalysis (I)n AGNs with (N)ested (S)ampling
 * Yan-Rong Li, liyanrong@ihep.ac.cn
 * Thu, Aug 4, 2016
 */

/*!
 *  \file init.c
 *  \brief initialize the program.
 */

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_interp.h>

#include "brains.h"

/*!
 * This function initialize the program.
 */
void init()
{
  int i, j;

  nq = 1 + parset.flag_trend;
  num_params_trend = nq;
  num_params_drw = 3; /* include systematic error */
  if(parset.flag_dim == 0)
  {
    num_params_resp = 0; /* no A and Ag for 0D */
  }
  else
  {
    num_params_resp = 2; /* response A and Ag */
  }

  if(parset.flag_trend_diff > 0)
  {
    num_params_difftrend = parset.flag_trend_diff; /* differences of the trends between continuum and line */
  }
  else
  {
    num_params_difftrend = 0;
  }

  num_params_var = num_params_drw + num_params_trend + num_params_resp + num_params_difftrend;

  /* number of parameters for narrow line, only valid for 2d RM. */
  num_params_nlr = 0;
  if(parset.flag_narrowline >= 2)
    num_params_nlr = 3;

  /* number of parameters for spectral broadening, only valid for 2d RM */
  num_params_res = 1;
  if(parset.flag_InstRes > 0)
    num_params_res = n_line_data;

  /* number of parameters for line center, only valid for 2d RM */
  num_params_linecenter = 0;
  if(parset.flag_linecenter >  0)
    num_params_linecenter = 1;
  else if(parset.flag_linecenter <  0)
    num_params_linecenter = n_line_data;

  switch(parset.flag_blrmodel)
  {
    case -1:  /* user defined analytical transfer function */
      BLRmodel_size = num_params_MyTransfun2d * sizeof(double);
      set_blr_range_model = set_par_range_mytransfun;
      break;
    case 0:
      BLRmodel_size = num_params_MyBLRmodel2d * sizeof(double);
      set_blr_range_model = set_blr_range_mymodel;
      break;
    case 1:
      BLRmodel_size = sizeof(BLRmodel1);
      set_blr_range_model = set_blr_range_model1;
      break;
    case 2:
      BLRmodel_size = sizeof(BLRmodel2);
      set_blr_range_model = set_blr_range_model2;
      break;
    case 3:
      BLRmodel_size = sizeof(BLRmodel3);
      set_blr_range_model = set_blr_range_model3;
      break;
    case 4:
      BLRmodel_size = sizeof(BLRmodel4);
      set_blr_range_model = set_blr_range_model4;
      break;
    case 5:
      BLRmodel_size = sizeof(BLRmodel5);
      set_blr_range_model = set_blr_range_model5;
      break;
    case 6:
      BLRmodel_size = sizeof(BLRmodel6);
      set_blr_range_model = set_blr_range_model6;
      break;
    case 7:
      BLRmodel_size = sizeof(BLRmodel7);
      set_blr_range_model = set_blr_range_model7;
      break;
    case 8:
      BLRmodel_size = sizeof(BLRmodel8);
      set_blr_range_model = set_blr_range_model8;
      break;
    case 9:
      BLRmodel_size = sizeof(BLRmodel9);
      set_blr_range_model = set_blr_range_model9;
      break;
    default:
      BLRmodel_size = sizeof(BLRmodel1);
      set_blr_range_model = set_blr_range_model1;
      break;
  }

#ifdef SA
  if(parset.flag_dim > 2 || parset.flag_dim < 0)
  {
    num_params_sa_extpar = sizeof(SAExtPar)/sizeof(double);
    switch(parset.flag_sa_blrmodel)
    {
      case 0:
        SABLRmodel_size = num_params_MyBLRmodel2d * sizeof(double);
        set_sa_blr_range_model = set_sa_blr_range_mymodel;
        break;
      case 1:
        SABLRmodel_size = sizeof(SABLRmodel1);
        set_sa_blr_range_model = set_sa_blr_range_model1;
        break;
      case 2:
        SABLRmodel_size = sizeof(SABLRmodel2);
        set_sa_blr_range_model = set_sa_blr_range_model2;
        break;
      case 3:
        SABLRmodel_size = sizeof(SABLRmodel3);
        set_sa_blr_range_model = set_sa_blr_range_model3;
        break;
      case 4:
        SABLRmodel_size = sizeof(SABLRmodel4);
        set_sa_blr_range_model = set_sa_blr_range_model4;
        break;
      case 5:
        SABLRmodel_size = sizeof(SABLRmodel5);
        set_sa_blr_range_model = set_sa_blr_range_model5;
        break;
      case 6:
        SABLRmodel_size = sizeof(SABLRmodel6);
        set_sa_blr_range_model = set_sa_blr_range_model6;
        break;
      case 7:
        SABLRmodel_size = sizeof(SABLRmodel7);
        set_sa_blr_range_model = set_sa_blr_range_model7;
        break;
      case 8:
        SABLRmodel_size = sizeof(SABLRmodel8);
        set_sa_blr_range_model = set_sa_blr_range_model8;
        break;
      case 9:
        SABLRmodel_size = sizeof(SABLRmodel9);
        set_sa_blr_range_model = set_sa_blr_range_model9;
        break;
      default:
        SABLRmodel_size = sizeof(SABLRmodel1);
        set_sa_blr_range_model = set_sa_blr_range_model1;
        break;
    }
  }
#endif

  if(parset.flag_dim != 3)
  {
    /* set maximum continuum point */
    n_con_max = parset.n_con_recon;
    if(parset.flag_dim >=-1)
    {
      if(n_con_data > n_con_max)
        n_con_max = n_con_data;
    }
  }

  allocate_memory();

  /* initialize GSL */
  gsl_T = gsl_rng_default;
  gsl_r = gsl_rng_alloc (gsl_T);

#ifndef Debug
  if(parset.flag_rng_seed != 1)
  {
    gsl_rng_set(gsl_r, time(NULL)+thistask+1350);
  }
  else
  {
    gsl_rng_set(gsl_r, parset.rng_seed+thistask+1350);
  }
#else
  if(parset.flag_rng_seed != 1)
  {
    gsl_rng_set(gsl_r, 6666+thistask+1350);
    printf("# debugging, task %d brains random seed %d.\n", thistask, 6666+thistask+1350);
  }
  else
  {
    gsl_rng_set(gsl_r, parset.rng_seed+thistask+1350);
  }
#endif

  /* default BH mass range */
  mass_range[0] = 1.0;
  mass_range[1] = 1.0e4;

  /* default rcloud_max_set */
  rcloud_max_set = 1.0e6;

  /* not RM */
  if(parset.flag_dim != 3)
  {
    gsl_acc = gsl_interp_accel_alloc();
    gsl_linear = gsl_interp_alloc(gsl_interp_linear, parset.n_con_recon);

    if(parset.flag_dim >=-1)
    {
      /* set Larr_data */
      for(i=0;i<n_con_data;i++)
      {
        Larr_data[i*nq + 0]=1.0;
        for(j=1; j<nq; j++)
          Larr_data[i*nq + j] = pow(Tcon_data[i], j);
      }
    }

    Tcad_data = 1.0;
    Tspan_data = 1.0e4;
    if(parset.flag_dim == 0)
    {
      /* set cadence and time span of data */
      Tspan_data = (Tcon_data[n_con_data -1] - Tcon_data[0]);
      Tspan_data_con = Tspan_data;
      if(Tspan_data < 0.0)
      {
        if(thistask == roottask)
        {
          printf("# Incorrect epochs in continuum, please check the input data.\n");
          exit(0);
        }
      }
      Tcad_data = Tspan_data;
      for(i=1; i< n_con_data; i++)
      {
        if(Tcad_data > Tcon_data[i] - Tcon_data[i-1])
          Tcad_data = Tcon_data[i] - Tcon_data[i-1];
      }
    }

    if(parset.flag_dim > 0 || parset.flag_dim == -1)
    {
      /* set cadence and time span of data */
      Tspan_data_con = (Tcon_data[n_con_data -1] - Tcon_data[0]);
      Tspan_data = (Tline_data[n_line_data -1] - Tcon_data[0]);
      if(Tspan_data < 0.0)
      {
        if(thistask == roottask)
        {
          printf("# Incorrect epochs in continuum and line, please check the input data.\n");
          exit(0);
        }
      }
      Tcad_data = Tspan_data;
      for(i=1; i< n_con_data; i++)
      {
        if(Tcad_data > Tcon_data[i] - Tcon_data[i-1])
          Tcad_data = Tcon_data[i] - Tcon_data[i-1];
      }
      for(i=1; i< n_line_data; i++)
      {
        if(Tcad_data > Tline_data[i] - Tline_data[i-1])
          Tcad_data = Tline_data[i] - Tline_data[i-1];
      }

      /* set mediate time of continuum data */
      Tmed_data = 0.5*(Tcon_data[0] + Tcon_data[n_con_data-1]);

      for(i=0; i<num_params_difftrend; i++)
      {
        pow_Tcon_data[i] = (pow(Tcon_data[n_con_data-1]-Tmed_data, i+2)
                          - pow(Tcon_data[0]-Tmed_data, i+2)) / (i+2) / Tspan_data_con;
      }

      /* set time back for continuum reconstruction */
      time_back_set = Tspan_data_con + (Tcon_data[0] - Tline_data[0]);
      time_back_set = fmax(2.0*Tcad_data, time_back_set);

      /* make rcloud_max and time_back consistent with each other, rcloud_max has a higher priority */
      double DT=Tcon_data[0] - time_back_set;
      if(parset.rcloud_max > 0.0)
      {
        DT = fmax(DT, Tline_data[0] - parset.rcloud_max*2.0);
      }
      else if(parset.time_back > 0.0) /* neglect when parset.rcloud_max is set */
      {
        DT = fmax(DT, Tcon_data[0] - parset.time_back);
      }
      time_back_set = Tcon_data[0] - DT;

      /* set the range of cloud radial distribution */
      rcloud_min_set = 0.0;
      rcloud_max_set = Tspan_data/2.0;

      /* rcloud_max should smaller than  (Tl0 - Tc0)/2 */
      rcloud_max_set = fmin( rcloud_max_set,  (Tline_data[0] - Tcon_data[0] + time_back_set)/2.0 );

      if(parset.rcloud_max > 0.0)
        rcloud_max_set = fmin(rcloud_max_set, parset.rcloud_max);

      /* check whether n_con_recon is appropriate */
      double med_cad, med_cad_recon;
      //med_cad = get_mediate_cad(Tcon_data, n_con_data);
      med_cad = (Tcon_data[n_con_data-1] - Tcon_data[0])/(n_con_data -1);
      med_cad_recon = (Tcon_data[n_con_data-1] - Tcon_data[0] + time_back_set)/(parset.n_con_recon-1);
      if(med_cad_recon > 1.05*med_cad)
      {
        if(thistask == roottask)
        {
          printf("# Too small NConRecon.\n"
               "# Better to change it to %d or set RCloudMax/TimeBack to smaller than %f/%f.\n",
               (int)(parset.n_con_recon * med_cad_recon/med_cad),
               (Tline_data[0] - Tcon_data[0] + time_back_set)/2.0 ,time_back_set);
          printf("\e[1;35m" "# Use '-f' option in command line if want to ignore this check.\n" "\e[0m");
        }
        if(parset.flag_force_run != 1)
        {
          exit(0);
        }
      }
    }

    if(thistask == roottask && parset.flag_dim >=-1)
    {
      printf("rcloud_min_max_set: %f %f\n", rcloud_min_set, rcloud_max_set);
    }

    // response range A and Ag
    resp_range[0][0] = log(0.1);
    resp_range[0][1] = log(10.0);

    resp_range[1][0] = -1.0;
    resp_range[1][1] =  3.0;

    /* set the range of continuum variation  */
    var_range_model[0][0] = log(1.0); /* systematic error in continuum */
    var_range_model[0][1] = log(1.0+10.0);

    var_range_model[1][0] = -15.0; /* log(sigma) */
    var_range_model[1][1] = -1.0;

    var_range_model[2][0] = log(Tcad_data); /* log(tau) */
    var_range_model[2][1] = log(Tspan_data);

    var_range_model[3][0] = -10.0; /* mean value or trend parameter values */
    var_range_model[3][1] =  10.0;

    for(i=0; i<num_params_difftrend; i++)
    {
      var_range_model[4+i][0] = -1.0/pow(Tspan_data, i+1); /* slope of the trend in the differences between contiuum and line */
      var_range_model[4+i][1] =  1.0/pow(Tspan_data, i+1);
    }

    var_range_model[4+num_params_difftrend][0] = -10.0; /* light curve values */
    var_range_model[4+num_params_difftrend][1] =  10.0;

    if(num_params_nlr > 1)
    {
      if(parset.flag_narrowline == 2) /* Gaussian prior */
      {
        nlr_range_model[0][0] = -10.0;
        nlr_range_model[0][1] =  10.0;

        nlr_prior_model[0] = GAUSSIAN;
      }
      else
      {
        nlr_range_model[0][0] = log(1.0e-1); /* logrithmic prior */
        nlr_range_model[0][1] = log(1.0e4);

        nlr_prior_model[0] = UNIFORM;
      }

      nlr_range_model[1][0] = -10.0;
      nlr_range_model[1][1] =  10.0;

      nlr_prior_model[1] = GAUSSIAN;

      nlr_range_model[2][0] = -10.0;
      nlr_range_model[2][1] =  10.0;

      nlr_prior_model[2] = GAUSSIAN;
    }

    // range for systematic error
    sys_err_line_range[0] = log(1.0);
    sys_err_line_range[1] = log(1.0+10.0);

    set_blr_range_model();

  }

#ifdef SA
  /* SA */
  if(parset.flag_dim > 2)
  {
    set_sa_blr_range_model();

    sa_extpar_range[0][0] = log(100.0);  /* DA */
    sa_extpar_range[0][1] = log(1000.0);

    sa_extpar_range[1][0] = 0.0;         /* PA */
    sa_extpar_range[1][1] = 180.0;

    sa_extpar_range[2][0] = log(0.5);    /* FA  */
    sa_extpar_range[2][1] = log(2.0);

    sa_extpar_range[3][0] = -(wave_sa_data[1]-wave_sa_data[0]);    /* CO  */
    sa_extpar_range[3][1] =  (wave_sa_data[1]-wave_sa_data[0]);
  }
#endif
}

/*!
 * This function allocates memory for variables used throughout the code.
 */
void allocate_memory()
{
  int i;

  if(parset.flag_dim != 3)
  {
    Tcon = malloc(parset.n_con_recon * sizeof(double));
    Fcerrs = malloc(parset.n_con_recon * sizeof(double));

    PSmat = malloc(parset.n_con_recon * parset.n_con_recon * sizeof(double));
    //PNmat = malloc(parset.n_con_recon * parset.n_con_recon * sizeof(double));
    USmat = malloc(parset.n_con_recon * n_con_data * sizeof(double));
    PSmat_data = malloc(n_con_data * n_con_data * sizeof(double));
    //PNmat_data = malloc(n_con_data * n_con_data * sizeof(double));
    PCmat_data = malloc(n_con_data * n_con_data * sizeof(double));
    IPCmat_data = malloc(n_con_data * n_con_data * sizeof(double));
    PQmat = malloc(parset.n_con_recon * parset.n_con_recon * sizeof(double));
    PEmat1 = malloc(parset.n_con_recon * n_con_data * sizeof(double));
    PEmat2 = malloc(parset.n_con_recon * parset.n_con_recon * sizeof(double));

    blr_range_model = malloc(BLRmodel_size/sizeof(double) * sizeof(double *));
    for(i=0; i<BLRmodel_size/sizeof(double); i++)
    {
      blr_range_model[i] = malloc(2*sizeof(double));
    }

    workspace = malloc((n_con_data*nq + 7*n_con_max + nq*nq + nq)*sizeof(double));
    workspace_uv = malloc(2*parset.n_con_recon*sizeof(double));
    Larr_data = malloc(n_con_data*nq*sizeof(double));
    Larr_rec = malloc(parset.n_con_recon*nq*sizeof(double));
    pow_Tcon_data = malloc(num_params_difftrend*sizeof(double));

    var_param = malloc(num_params_var * sizeof(double));
    var_param_std = malloc(num_params_var * sizeof(double));

    for(i=0; i<num_params_var; i++)
    {
      var_param[i] = var_param_std[i] = 0.0;
    }
  }

#ifdef SA
  if(parset.flag_dim > 2 || parset.flag_dim < 0)
  {
    sa_extpar_range = malloc(num_params_sa_extpar * sizeof(double *));
    for(i=0; i<num_params_sa_extpar; i++)
    {
      sa_extpar_range[i]=malloc(2*sizeof(double));
    }

    sa_blr_range_model = malloc(SABLRmodel_size/sizeof(double) * sizeof(double *));
    for(i=0; i<SABLRmodel_size/sizeof(double); i++)
    {
      sa_blr_range_model[i] = malloc(2*sizeof(double));
    }

    if(parset.flag_sa_par_mutual != 0)
    {
      idx_sa_par_mutual = malloc(2*sizeof(int));
      idx_rm_par_mutual = malloc(2*sizeof(int));
    }
  }
#endif

  return;
}

/*!
 * This function free memory.
 */
void free_memory()
{
  int i;

  if(parset.flag_dim != 3)
  {
    free(Tcon);
    free(Fcerrs);

    free(PSmat);
    //(PNmat);
    free(USmat);
    free(PSmat_data);
    //free(PNmat_data);
    free(PCmat_data);
    free(IPCmat_data);
    free(PQmat);
    free(PEmat1);
    free(PEmat2);

    for(i=0; i<BLRmodel_size/sizeof(double); i++)
    {
      free(blr_range_model[i]);
    }
    free(blr_range_model);

    free(workspace);
    free(workspace_uv);
    free(Larr_data);
    free(Larr_rec);
    free(pow_Tcon_data);

    free(var_param);
    free(var_param_std);
  }

#ifdef SA
  if(parset.flag_dim > 2 || parset.flag_dim < 0)
  {
    for(i=0; i<num_params_sa_extpar; i++)
    {
      free(sa_extpar_range[i]);
    }
    free(sa_extpar_range);

    for(i=0; i<SABLRmodel_size/sizeof(double); i++)
    {
      free(sa_blr_range_model[i]);
    }
    free(sa_blr_range_model);

    if(parset.flag_sa_par_mutual != 0)
    {
      free(idx_sa_par_mutual);
      free(idx_rm_par_mutual);
    }
  }
#endif
  return;
}

/*!
 * This function normalise the light curves to a scale of unity.
 */
void scale_con_line()
{
  int i, j;
  double ave_con, ave_line;

  con_scale = 1.0;
  line_scale = 1.0;

  ave_con = 0.0;
  ave_line = 0.0;
  for(i=0; i<n_con_data; i++)
  {
    ave_con += Fcon_data[i];
  }

  ave_con /= n_con_data;
  con_scale = 1.0/ave_con;

  for(i=0; i<n_con_data; i++)
  {
    Fcon_data[i] *=con_scale;
    Fcerrs_data[i] *=con_scale;
  }

  if(thistask == roottask)
    printf("con scale: %e\t%e\n", con_scale, ave_con);

  con_error_mean *= con_scale;

  if(parset.flag_dim == 0)
  {
    return;
  }

  for(i=0; i<n_line_data; i++)
  {
    ave_line += Fline_data[i];
  }
  ave_line /=n_line_data;

  line_scale = 1.0/ave_line;

  if(thistask == roottask)
    printf("line scale: %e\t%e\n", line_scale, ave_line);

  for(i=0; i<n_line_data; i++)
  {
    // note mask with error < 0.0
    if(Flerrs_data[i] > 0.0)
    {
      Fline_data[i] *= line_scale;
      Flerrs_data[i] *= line_scale;
    }

    if(parset.flag_dim==2 || parset.flag_dim == -1 || parset.flag_dim == 5)
      for(j=0; j<n_vel_data; j++)
      {
        // note mask with error < 0.0
        if(Flerrs2d_data[i*n_vel_data + j] > 0.0)
        {
          Fline2d_data[i*n_vel_data + j] *= line_scale;
          Flerrs2d_data[i*n_vel_data + j] *= line_scale;
        }
      }
  }
  line_error_mean *= line_scale;

  if( (parset.flag_dim==2 || parset.flag_dim == -1 || parset.flag_dim == 5) && parset.flag_narrowline!=0)
  {
    parset.flux_narrowline *= line_scale;
    parset.flux_narrowline_err *= line_scale;
  }
  return;
}

/*!
 * This function copes with parameter fixing.\n
 * Only fix BLR model parameters.
 */
void set_par_fix_blrmodel()
{
  int i;
  char *pstr;

  npar_fix = 0;

  if(thistask == roottask)
  {
    pstr = parset.str_par_fix_val;
    // set the default value if not provided.
    for(i=strlen(parset.str_par_fix); i<num_params_blr_model; i++)
      parset.str_par_fix[i] = '0';

    for(i=0; i<num_params_blr_model; i++)
    {
      if(parset.str_par_fix[i] == '0')
      {
        par_fix[i] = 0;
        par_fix_val[i] = -DBL_MAX;  /* set to be the smallest value */
      }
      else if(parset.str_par_fix[i] == '1')
      {
        if(pstr == NULL)
        {
          printf("# %d-th BLR parameter value is not provided (counting from 0).\n", i);
          exit(0);
        }
        par_fix[i] = 1;
        sscanf(pstr, "%lf", &par_fix_val[i]);
        npar_fix++;
        printf("# %d-th BLR parameter fixed, value= %f.\n", i, par_fix_val[i]);
        pstr = strchr(pstr, ':'); /* values are separated by ":" */
        if(pstr!=NULL)
        {
          pstr++;
        }
      }
      else   // default value
      {
        par_fix[i] = 0;
        par_fix_val[i] = -DBL_MAX;
      }
    }
  }

  MPI_Bcast(par_fix, num_params_blr_model, MPI_INT, roottask, MPI_COMM_WORLD);
  MPI_Bcast(par_fix_val, num_params_blr_model, MPI_DOUBLE, roottask, MPI_COMM_WORLD);

  return;
}

/*
 * setup BLR model parameter range.
 */

// model 1
void set_blr_range_model1()
{
  int i;

  i = 0;
  //mu
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //beta
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 2.0;
  //F
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 0.999;
  //inc
  blr_range_model[i][0] = 0.0;  // in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;  // in rad
  blr_range_model[i++][1] = 90.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);
  //lambda
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.5;
  //q
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;

  //blr_range_model[2][1] = fmin(blr_range_model[2][1], log(rcloud_max_set));
  return;
}

// model 2
void set_blr_range_model2()
{
  int i;

  i = 0;
  //mu
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //beta
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 2.0;
  //F
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 0.999;
  //inc
  blr_range_model[i][0] = 0.0;  // in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;  // in rad
  blr_range_model[i++][1] = 90.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);
  //sigr
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //sigtheta
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;

  return;
}

// model 3
void set_blr_range_model3()
{
  int i;

  i = 0;
  //Rin
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //F
  blr_range_model[i][0] = log(1.0);
  blr_range_model[i++][1] = log(1.0e2);
  //alpha
  blr_range_model[i][0] = -3.0;
  blr_range_model[i++][1] = 3.0;
  //inc
  blr_range_model[i][0] = 0.0;  // in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;   // in rad
  blr_range_model[i++][1] = 90.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);
  //xi
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //q
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;

  //rcloud_max_set = fmax(rcloud_max_set, exp(blr_range_model[3][1] + blr_range_model[4][1]));
  return;
}

// model 4
void set_blr_range_model4()
{
  int i;

  i = 0;
  //Rin
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //F
  blr_range_model[i][0] = log(1.0);
  blr_range_model[i++][1] = log(1.0e2);
  //alpha
  blr_range_model[i][0] = -3.0;
  blr_range_model[i++][1] = 3.0;
  //inc
  blr_range_model[i][0] = 0.0; // in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;  // in rad
  blr_range_model[i++][1] = 90.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);
  //xi
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = sqrt(2.0)/2.0;
  //q
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;

  //rcloud_max_set = fmax(rcloud_max_set, exp(blr_range_model[3][1] + blr_range_model[4][1]));
  return;
}

// model 5
void set_blr_range_model5()
{
  int i;

  i = 0;
  //mu
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //Fin
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //Fout
  blr_range_model[i][0] = log(1.0);
  blr_range_model[i++][1] = log(10.0);
  //alpha
  blr_range_model[i][0] = 1.0;
  blr_range_model[i++][1] = 3.0;
  //inc
  blr_range_model[i][0] = 0.0; // in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;  // in degree
  blr_range_model[i++][1] = 90.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //beta
  blr_range_model[i][0] = 1.0;
  blr_range_model[i++][1] = 5.0;
  //xi
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);
  //fellip
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //fflow
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //sigr_circ
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);
  //sigthe_circ
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(1.0);
  //sigr_rad
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);
  //sigthe_rad
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(1.0);
  //theta_rot
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 90.0;
  //sig_turb
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);

  //rcloud_max_set = fmax(rcloud_max_set, exp(blr_range_model[2][1] + blr_range_model[4][1]));

  return;
}

// model 6
void set_blr_range_model6()
{
  int i;

  i = 0;
  //mu
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //beta
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 2.0;
  //F
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //inc
  blr_range_model[i][0] = 0.0; //in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;  // in rad
  blr_range_model[i++][1] = 90.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //gamma
  blr_range_model[i][0] = 1.0;
  blr_range_model[i++][1] = 5.0;
  //xi
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);
  //fellip
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //fflow
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //sigr_circ
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);
  //sigthe_circ
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(1.0);
  //sigr_rad
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);
  //sigthe_rad
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(1.0);
  //theta_rot
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 90.0;
  //sig_turb
  blr_range_model[i][0] = log(0.0001);
  blr_range_model[i++][1] = log(0.1);

  //rcloud_max_set = 44.85;
  //blr_range_model[2][1] = fmin(blr_range_model[2][1], log(rcloud_max_set));
  return;
}

// model 7
void set_blr_range_model7()
{
  int i;

  i = 0;
  //mu
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //beta
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 2.0;
  //F
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //inc
  blr_range_model[i][0] = 0.0;  // in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;  // in rad
  blr_range_model[i++][1] = 90.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //gamma
  blr_range_model[i][0] = 1.0;
  blr_range_model[i++][1] = 5.0;
  //xi
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;

  //fsh
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //mu_un
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //beta_un
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 2.0;
  //F_un
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //opn_un
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 90.0;

  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);
  //fellip
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //fflow
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //sigr_circ
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);
  //sigthe_circ
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(1.0);
  //sigr_rad
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);
  //sigthe_rad
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(1.0);
  //theta_rot
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 90.0;

  //fellip_un
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //fflow_un
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;

  //sig_turb
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);

  return;
}

// model 8
void set_blr_range_model8()
{
  int i;

  i = 0;
  //theta_min
  blr_range_model[i][0] = 20.0;     // in degree
  blr_range_model[i++][1] = 90.0;
  //dtheta_max
  blr_range_model[i][0] = 0.0;      // in degree
  blr_range_model[i++][1] = 90.0;
  //r_min
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.25);
  //fr_max
  blr_range_model[i][0] = 1.0;  //
  blr_range_model[i++][1] = 10.0;
  //gamma
  blr_range_model[i][0] = 0.0;  //
  blr_range_model[i++][1] = 3.0;
  //alpha
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 3.0;
  //lambda
  blr_range_model[i][0] = -3.0;
  blr_range_model[i++][1] = 0.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //xi
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //Rv
  blr_range_model[i][0] = log(10.0);
  blr_range_model[i++][1] = log(50.0);
  //Rblr
  blr_range_model[i][0] = log(10.0);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //inc
  blr_range_model[i][0] = 0.0;   // cos(inc)
  blr_range_model[i++][1] = 1.0;
  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);

  return;
}

// model 9
void set_blr_range_model9()
{
  int i;

  i = 0;
  //mu
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //beta
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 2.0;
  //F
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 0.999;
  //inc
  blr_range_model[i][0] = 0.0;  // in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;  // in rad
  blr_range_model[i++][1] = 90.0;
  //mbh
  blr_range_model[i][0] = log(mass_range[0]);
  blr_range_model[i++][1] = log(mass_range[1]);

  return;
}

/*
 * set 1D BLR model and functions.
 */
void set_blr_model1d()
{
  switch(parset.flag_blrmodel)
  {
    case -1:
      num_params_blr_model = num_params_MyTransfun1d;
      transfun_1d_cal = transfun_1d_cal_mytransfun;
      break;

    case 0:
      num_params_blr_model = num_params_MyBLRmodel1d;
      gen_cloud_sample = gen_cloud_sample_mymodel;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 1:
      num_params_blr_model = 6;
      gen_cloud_sample = gen_cloud_sample_model1;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 2:
      num_params_blr_model = 6;
      gen_cloud_sample = gen_cloud_sample_model1;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 3:
      num_params_blr_model = 6;
      gen_cloud_sample = gen_cloud_sample_model3;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 4:
      num_params_blr_model = 6;
      gen_cloud_sample = gen_cloud_sample_model3;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 5:
      num_params_blr_model = 9;
      gen_cloud_sample = gen_cloud_sample_model5;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 6:
      num_params_blr_model = 8;
      gen_cloud_sample = gen_cloud_sample_model6;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 7:
      num_params_blr_model = 13;
      gen_cloud_sample = gen_cloud_sample_model7;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 8:
      num_params_blr_model = 13;
      gen_cloud_sample = gen_cloud_sample_model8;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    case 9:
      num_params_blr_model = 5;
      gen_cloud_sample = gen_cloud_sample_model9;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;

    default:
      num_params_blr_model = 6;
      gen_cloud_sample = gen_cloud_sample_model1;
      transfun_1d_cal = transfun_1d_cal_cloud;
      break;
  }
  return;
}

/*
 * set 2D BLR model and functions.
 */
void set_blr_model2d()
{
  switch(parset.flag_blrmodel)
  {
    case -1:
      num_params_blr_model = num_params_MyTransfun2d;
      transfun_2d_cal = transfun_2d_cal_mytransfun;
      break;

    case 0:
      num_params_blr_model = num_params_MyBLRmodel2d;
      gen_cloud_sample = gen_cloud_sample_mymodel;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 1:
      num_params_blr_model = sizeof(BLRmodel1)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model1;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 2:
      num_params_blr_model = sizeof(BLRmodel2)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model2;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 3:
      num_params_blr_model = sizeof(BLRmodel3)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model3;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 4:
      num_params_blr_model = sizeof(BLRmodel4)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model4;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 5:
      num_params_blr_model = sizeof(BLRmodel5)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model5;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 6:
      num_params_blr_model = sizeof(BLRmodel6)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model6;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 7:
      num_params_blr_model = sizeof(BLRmodel7)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model7;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 8:
      num_params_blr_model = sizeof(BLRmodel8)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model8;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    case 9:
      num_params_blr_model = sizeof(BLRmodel9)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model9;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;

    default:
      num_params_blr_model = sizeof(BLRmodel1)/sizeof(double);
      gen_cloud_sample = gen_cloud_sample_model1;
      transfun_2d_cal = transfun_2d_cal_cloud;
      break;
  }
  return;
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include "parmt_config.h"
#include "compearth.h"
#ifdef PARMT_USE_INTEL
#include <mkl.h>
#include <mkl_lapacke.h>
#include <mkl_cblas.h>
#include <ipps.h>
#else
#include <lapacke.h>
#include <cblas.h>
#endif
#include "iscl/array/array.h"
#include "iscl/linalg/linalg.h"
#include "iscl/memory/memory.h"
#include "iscl/statistics/statistics.h"
#include "iscl/sorting/sorting.h"

static double median_sorted_array(int n, const double *__restrict x);
static int setG64f(const int npts,
                   const double *__restrict__ Gxx,
                   const double *__restrict__ Gyy,
                   const double *__restrict__ Gzz,
                   const double *__restrict__ Gxy,
                   const double *__restrict__ Gxz,
                   const double *__restrict__ Gyz,
                   double *__restrict__ G);

#define LDG 8

int parmt_computeL1Magnitude64f(const int ldm,
                                const int nobs,
                                const int nmtSolve,
                                const int maxit, const double eps,
                                const double tol,
                                const int *__restrict__ signalPtr,
                                const int *__restrict__ lags,
                                const int *__restrict__ mtPtr,
                                const double *__restrict__ Gxx,
                                const double *__restrict__ Gyy,
                                const double *__restrict__ Gzz,
                                const double *__restrict__ Gxy,
                                const double *__restrict__ Gxz,
                                const double *__restrict__ Gyz,
                                const double *__restrict__ mts,
                                const double *__restrict__ d,
                                double *__restrict__ mags)
{
    const char *fcnm = "parmt_computeL1Magnitude64f\0";
    double *G, *est, *obs, *wts, xmag;
    double m6[8] __attribute__((aligned(64)));
    int i, i1, i2, ierr, imt, jmt, k, maxlag, nptsAll, nptsPad;
    bool luseLag;
    const double p = 1.0;
    const double one = 1.0;
    const double zero = 0.0;
    nptsAll = signalPtr[nobs];
    if (nobs < 1)
    {
        printf("%s: Error no observations\n", fcnm);
        return -1;
    }
    // Get the max number of lags
    maxlag = 0;
    luseLag = false;
    if (lags != NULL)
    {
        luseLag = true;
        maxlag = lags[array_absArgmax32i(nobs, lags, &ierr)];
    }
    if (maxlag == 0){luseLag = false;}
    nptsPad = nptsAll + nobs*maxlag;
    G = memory_calloc64f(nptsPad*LDG);
    obs = memory_calloc64f(nptsPad);
    est = memory_calloc64f(nptsPad);
    wts = memory_calloc64f(nptsPad);
    // Insert the Green's functions and data into the moment tensor matrix
    if (!luseLag)
    {
        for (k=0; k<nobs; k++)
        {
            i1 = signalPtr[k];
            i2 = signalPtr[k+1];
            for (i=i1; i<i2; i++)
            {
                G[i*LDG+0] = Gxx[i];
                G[i*LDG+1] = Gyy[i];
                G[i*LDG+2] = Gzz[i];
                G[i*LDG+3] = Gxy[i];
                G[i*LDG+4] = Gxz[i];
                G[i*LDG+5] = Gyz[i];
                obs[i] = d[i];
            }
        }
    }
    else
    {

    }
    for (imt=0; imt<nmtSolve; imt++)
    {
        // Extract the moment tensor
        jmt = mtPtr[imt];
        for (i=0; i<6; i++)
        {
            m6[0] = mts[ldm*jmt+0];
        }
        // Remove the scalar moment
        compearth_CMT2m0(1, 1, m6, &xmag);
        cblas_dscal(6, 1.0/xmag, m6, 1);
        // Create the linear model [G*m]*M0 = [est]*{M0} = {obs}
        cblas_dgemv(CblasRowMajor, CblasNoTrans,
                    nptsAll, 6, one, G, LDG, m6, 1, zero, est, 1);
        // Solve for magnitude in the L1 norm
        ierr = linalg_irls64f_work(nptsAll, 1,
                                   maxit, p,
                                   eps, tol,
                                   est, obs, &xmag, wts);
        mags[imt] = xmag;
 double xmagMw;
compearth_m02mw(1, CE_KANAMORI_1978, &xmag, &xmagMw);
printf("%f\n", xmagMw);
    }
    memory_free64f(&wts);
    memory_free64f(&G);
    memory_free64f(&obs);
    memory_free64f(&est);
    return 0;
}

int parmt_computeL1StationMagnitude64f(const int ldm, const int nmt,
                                       const int npts, const int nblock,
                                       const double *__restrict__ Gxx,
                                       const double *__restrict__ Gyy,
                                       const double *__restrict__ Gzz,
                                       const double *__restrict__ Gxy,
                                       const double *__restrict__ Gxz,
                                       const double *__restrict__ Gyz,
                                       const double *__restrict__ mts,
                                       const double *__restrict__ d,
                                       double *__restrict__ mags)
{
    const char *fcnm = "parmt_computeL1StationMagnitude64f\0";
    double *G, *M, *R, *Dmat, *res, *resSort, xmag, xopt;
    const double one = 1.0;
    const double zero = 0.0;
    const double negOne =-one;
    int *perm;
    bool lsorted;
    int i, ic, idx, ierr, imt, jdx, jmt, mblock, Mrows, Ncols;
    const int Kcols = 6; // Number of columns of G
    Mrows = npts;
    mblock = nblock;// + computePadding6f(npts);
    G = memory_calloc64f(LDG*npts);
    R = memory_calloc64f(mblock*npts);
    M = memory_calloc64f(mblock*8);
    Dmat = memory_calloc64f(mblock*npts);
    res = memory_calloc64f(npts);
    resSort = memory_calloc64f(npts);
    perm = memory_calloc32i(npts);
    for (i=0; i<npts; i++){perm[i] = i;}
        // Set the observations
        for (i=0; i<npts; i++)
        {
            for (ic=0; ic<nblock; ic++)
            {
                Dmat[mblock*i+ic] = d[i];
            }
        }
#ifdef __INTEL_COMPILER
    __assume_aligned(G, 64);
#endif
    ierr = setG64f(npts, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz, G);
    if (ierr != 0)
    {
        printf("%s: Failed to set G\n", fcnm);
        return -1;
    }
int nsorted = 0;
    for (jmt=0; jmt<nmt; jmt=jmt+nblock)
    {
        Ncols = MIN(nblock, nmt - jmt); // Number of columns of M
        // Set the observations
        cblas_dcopy(npts*mblock, Dmat, 1, R, 1);
        for (i=0; i<6; i++)
        {
            for (ic=0; ic<Ncols; ic++)
            {
                imt = jmt + ic;
                idx = ldm*imt + i;
                jdx = mblock*i;
                M[jdx+ic] = mts[idx];///xmag;
            }
        }
        // Compute R = GM - D = GM - R
        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                    Mrows, Ncols, Kcols, one, G, LDG,
                    M, mblock, negOne, R, mblock);
        //lsorted = false;
        //for (i=0; i<npts; i++){perm[i] = i;}
        // Sort the columns of the residual matrix
        for (ic=0; ic<Ncols; ic++)
        {
            imt = jmt + ic;
            // Compute the absolute value of the residual
            for (i=0; i<npts; i++)
            {
                res[i] = fabs(R[ic+i*mblock]);
            }
            //cblas_dcopy(npts, &R[ic], mblock, res, 1);
            // Apply the old permutation
            sorting_applyPermutation64f_work(npts, perm, res, resSort);
            lsorted = sorting_issorted64f(npts, resSort, SORT_ASCENDING, &ierr);
            if (!lsorted)
            {
                sorting_argsort64f_work(npts, res, SORT_ASCENDING, perm);
                sorting_applyPermutation64f_work(npts, perm, res, resSort);
            }
            else
            {
                nsorted = nsorted + 1;
            }
            // Compute the median
            xopt = median_sorted_array(npts, resSort);
            //compearth_CMT2mw(1, 1, &mts[8*imt], &xmag);
            compearth_CMT2m0(1, 1, &mts[8*imt], &xmag);
            xopt = xopt*xmag;
            compearth_m02mw(1, CE_KANAMORI_1978, &xopt, &mags[imt]);
        }
    }
printf("%d %d\n", nsorted, nmt);
    memory_free32i(&perm);
    memory_free64f(&G);
    memory_free64f(&R);
    memory_free64f(&M);
    memory_free64f(&Dmat);
    memory_free64f(&res);
    memory_free64f(&resSort);
    return 0;
}

static double median_sorted_array(int n, const double *__restrict x)
{
    double xmed;
    int n2;
    const double half = 0.5;
    n2 = n/2;
    // Even -> average middle two elements
    if (fmod(n, 2) == 0)
    {
        xmed = half*(x[n2-1] + x[n2]);
    }
    // Median is middle element
    else
    {
        xmed = x[n2];
    }
    return xmed;
}

static int setG64f(const int npts,
                   const double *__restrict__ Gxx,
                   const double *__restrict__ Gyy,
                   const double *__restrict__ Gzz,
                   const double *__restrict__ Gxy,
                   const double *__restrict__ Gxz,
                   const double *__restrict__ Gyz,
                   double *__restrict__ G)
{
    const char *fcnm = "setG64f\0";
    int i;
    bool lalign;
    if (Gxx == NULL || Gyy == NULL || Gzz == NULL || Gxy == NULL ||
        Gxz == NULL || G == NULL || npts < 1)

    {
        if (Gxx == NULL){printf("%s: Error Gxx is NULL\n", fcnm);}
        if (Gyy == NULL){printf("%s: Error Gyy is NULL\n", fcnm);}
        if (Gzz == NULL){printf("%s: Error Gzz is NULL\n", fcnm);}
        if (Gxy == NULL){printf("%s: Error Gxy is NULL\n", fcnm);}
        if (Gxz == NULL){printf("%s: Error Gxz is NULL\n", fcnm);}
        if (Gyz == NULL){printf("%s: ERror Gyz is NULL\n", fcnm);}
        if (npts < 1)
        {
            printf("%s: No points in Green's functions\n", fcnm);
        }
        return -1;
    }
    lalign = true;
    if (memory_isAligned(Gxx, 64) != 1 || memory_isAligned(Gyy, 64) != 1 ||
        memory_isAligned(Gzz, 64) != 1 || memory_isAligned(Gxy, 64) != 1 ||
        memory_isAligned(Gxz, 64) != 1 || memory_isAligned(Gyz, 64) != 1 ||
        memory_isAligned(G, 64) != 1)
    {
        lalign = false;
    }
    if (lalign)
    {
        #pragma omp simd aligned(G, Gxx, Gyy, Gzz, Gxy, Gxz, Gyz: 64)
        for (i=0; i<npts; i++)
        {
            G[LDG*i+0] = Gxx[i];
            G[LDG*i+1] = Gyy[i];
            G[LDG*i+2] = Gzz[i];
            G[LDG*i+3] = Gxy[i];
            G[LDG*i+4] = Gxz[i];
            G[LDG*i+5] = Gyz[i];
        }
    }
    else
    {
        #pragma omp simd
        for (i=0; i<npts; i++)
        {
            G[LDG*i+0] = Gxx[i];
            G[LDG*i+1] = Gyy[i];
            G[LDG*i+2] = Gzz[i];
            G[LDG*i+3] = Gxy[i];
            G[LDG*i+4] = Gxz[i];
            G[LDG*i+5] = Gyz[i];
        }
    }
    return 0;
}


/* multiroots/fdfsolver.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program 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
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_multiroots.h>

gsl_multiroot_fdfsolver *
gsl_multiroot_fdfsolver_alloc (const gsl_multiroot_fdfsolver_type * T,
                                       size_t n)
{
  int status;

  gsl_multiroot_fdfsolver * s;

  s = (gsl_multiroot_fdfsolver *) malloc (sizeof (gsl_multiroot_fdfsolver));

  if (s == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for multiroot solver struct",
                        GSL_ENOMEM, 0);
    }

  s->x = gsl_vector_calloc (n);

  if (s->x == 0)
    {
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for x", GSL_ENOMEM, 0);
    }

  s->f = gsl_vector_calloc (n);

  if (s->f == 0)
    {
      gsl_vector_free (s->x);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for f", GSL_ENOMEM, 0);
    }

  s->J = gsl_matrix_calloc (n,n);

  if (s->J == 0)
    {
      gsl_vector_free (s->x);
      gsl_vector_free (s->f);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for g", GSL_ENOMEM, 0);
    }

  s->dx = gsl_vector_calloc (n);

  if (s->dx == 0)
    {
      gsl_matrix_free (s->J);
      gsl_vector_free (s->x);
      gsl_vector_free (s->f);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for dx", GSL_ENOMEM, 0);
    }

  s->state = malloc (T->size);

  if (s->state == 0)
    {
      gsl_vector_free (s->dx);
      gsl_vector_free (s->x);
      gsl_vector_free (s->f);
      gsl_matrix_free (s->J);
      free (s);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to allocate space for multiroot solver state",
                        GSL_ENOMEM, 0);
    }

  s->type = T ;

  status = (s->type->alloc)(s->state, n);

  if (status != GSL_SUCCESS)
    {
      free (s->state);
      gsl_vector_free (s->dx);
      gsl_vector_free (s->x);
      gsl_vector_free (s->f);
      gsl_matrix_free (s->J);
      free (s);         /* exception in constructor, avoid memory leak */

      GSL_ERROR_VAL ("failed to set solver", status, 0);
    }

  s->fdf = NULL;

  return s;
}

int
gsl_multiroot_fdfsolver_set (gsl_multiroot_fdfsolver * s,
                             gsl_multiroot_function_fdf * f,
                             const gsl_vector * x)
{
  if (s->x->size != f->n)
    {
      GSL_ERROR ("function incompatible with solver size", GSL_EBADLEN);
    }

  if (x->size != f->n)
    {
      GSL_ERROR ("vector length not compatible with function", GSL_EBADLEN);
    }

  s->fdf = f;
  gsl_vector_memcpy(s->x,x);

  return (s->type->set) (s->state, s->fdf, s->x, s->f, s->J, s->dx);
}

int
gsl_multiroot_fdfsolver_iterate (gsl_multiroot_fdfsolver * s)
{
  return (s->type->iterate) (s->state, s->fdf, s->x, s->f, s->J, s->dx);
}

void
gsl_multiroot_fdfsolver_free (gsl_multiroot_fdfsolver * s)
{
  RETURN_IF_NULL (s);
  (s->type->free) (s->state);
  free (s->state);
  gsl_vector_free (s->dx);
  gsl_vector_free (s->x);
  gsl_vector_free (s->f);
  gsl_matrix_free (s->J);
  free (s);
}

const char *
gsl_multiroot_fdfsolver_name (const gsl_multiroot_fdfsolver * s)
{
  return s->type->name;
}

gsl_vector *
gsl_multiroot_fdfsolver_root (const gsl_multiroot_fdfsolver * s)
{
  return s->x;
}

gsl_vector *
gsl_multiroot_fdfsolver_dx (const gsl_multiroot_fdfsolver * s)
{
  return s->dx;
}

gsl_vector *
gsl_multiroot_fdfsolver_f (const gsl_multiroot_fdfsolver * s)
{
  return s->f;
}


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

/*
--------------------------------------------------------------------|
            ------------------------------                          |
            |          LAPACKE           |                          |
            ------------------------------                          |
--------------------------------------------------------------------|
*/

void dgetrf_(int* M, int *N, double* A, int* lda, int* IPIV, int* INFO);
void dgetri_(int* N, double* A, int* lda, int* IPIV, double* WORK, int* lwork, int* INFO);

/*
--------------------------------------------------------------------|
            ------------------------------                          |
            |           INIT FC          |                          |
            ------------------------------                          |
--------------------------------------------------------------------|
*/

ERL_NIF_TERM atom_nok;
ERL_NIF_TERM atom_true;
ERL_NIF_TERM atom_false;
ERL_NIF_TERM atom_matrix;

ErlNifResourceType *MULT_YIELDING_ARG = NULL;

int load(ErlNifEnv* env, void** priv_data, ERL_NIF_TERM load_info){
    atom_nok = enif_make_atom(env, "nok\0");
    atom_true = enif_make_atom(env, "true\0");
    atom_false = enif_make_atom(env, "false\0");
    atom_matrix = enif_make_atom(env, "matrix\0");
    return 0;
}

int upgrade(ErlNifEnv* caller_env, void** priv_data, void** old_priv_data, ERL_NIF_TERM load_info){
    return 0;
}

//Gives easier access to an ErlangBinary containing a matrix.
typedef struct{
    int n_rows;
    int n_cols;

    //Content of the matrix, in row major format.
    double* content;

    //Erlang binary containing the matrix.
    ErlNifBinary bin;
} Matrix;

//Access asked coordinates of matrix
double* matrix_at(int col, int row, Matrix m){
    return &m.content[row*m.n_cols + col];
}

//Allocates memory space of for matrix of dimensions n_rows, n_cols.
//The matrix_content can be modified, until a call to array_to_erl.
//Matrix content is stored in row major format.
Matrix matrix_alloc(int n_rows, int n_cols){
    ErlNifBinary bin;

    enif_alloc_binary(sizeof(double)*n_rows*n_cols, &bin);

    Matrix matrix;
    matrix.n_cols = n_cols;
    matrix.n_rows = n_rows;
    matrix.bin = bin;
    matrix.content = (double*) bin.data;

    return matrix;
}

//Creates a duplicate of a matrix.
//This duplicate can be modified until uploaded.
Matrix matrix_dup(Matrix m){
    Matrix d = matrix_alloc(m.n_rows, m.n_cols);
    memcpy(d.content, m.content, d.bin.size);
    return d;
}

//Free an allocated matrix that was not sent back to Erlang.
void matrix_free(Matrix m){
    enif_release_binary(&m.bin);
}

//Constructs a matrix erlang term.
//No modifications can be made afterwards to the matrix.
ERL_NIF_TERM matrix_to_erl(ErlNifEnv* env, Matrix m){
    ERL_NIF_TERM term = enif_make_binary(env, &m.bin);
    return enif_make_tuple4(env, atom_matrix, enif_make_int(env,m.n_rows), enif_make_int(env,m.n_cols), term);
}

int enif_is_matrix(ErlNifEnv* env, ERL_NIF_TERM term){
    int arity;
    const ERL_NIF_TERM* content;

    if(!enif_is_tuple(env, term))
        return 0;

    enif_get_tuple(env, term, &arity, &content);
    if(arity != 4)
        return 0;

    if(content[0] != atom_matrix
        || !enif_is_number(env, content[1])
        || !enif_is_number(env, content[2])
        || !enif_is_binary(env, content[3]))

            return 0;
    return 1;
}

//Reads an erlang term as a matrix.
//As such, no modifications can be made to the red matrix.
//Returns true if it was possible to read a matrix, false otherwise
int enif_get_matrix(ErlNifEnv* env, ERL_NIF_TERM term, Matrix *dest){

    int arity;
    const ERL_NIF_TERM* content;

    if(!enif_is_tuple(env, term))
        return 0;

    enif_get_tuple(env, term, &arity, &content);
    if(arity != 4)
        return 0;

    if(content[0] != atom_matrix
        || !enif_get_int(env, content[1], &dest->n_rows)
        || !enif_get_int(env, content[2], &dest->n_cols)
        || !enif_inspect_binary(env, content[3], &dest->bin))
    {
        return 0;
    }

    dest->content = (double*) (dest->bin.data);
    return 1;
}

//Used to translate at once a number of ERL_NIF_TERM.
//Data types are inferred via content of format string:
//  n: number (int or double) translated to double.
//  m: matrix
//  i: int
int enif_get(ErlNifEnv* env, const ERL_NIF_TERM* erl_terms, const char* format, ...){
    va_list valist;
    va_start(valist, format);
    int valid = 1;

    while(valid && *format != '\0'){
        switch(*format++){
            case 'n':
                //Read a number as a double.
                ;
                double *d = va_arg(valist, double*);
                int i;
                if(!enif_get_double(env, *erl_terms, d))
                    if(enif_get_int(env, *erl_terms, &i))
                        *d = (double) i;
                    else valid = 0;
                break;

            case 'm':
                //Reads a matrix.
                valid = enif_get_matrix(env, *erl_terms, va_arg(valist, Matrix*));
                break;

            case 'i':
                //Reads an int.
                valid = enif_get_int(env, *erl_terms, va_arg(valist, int*));
                break;

            default:
                //Unknown type... give an error.
                valid = 0;
                break;
        }
        erl_terms ++;
    }

    va_end(valist);
    return valid;
}

//----------------------------------------------------------------------------------------------------|
//                        ------------------------------------------                                  |
//                        |                   NIFS                 |                                  |
//                        ------------------------------------------                                  |
//----------------------------------------------------------------------------------------------------|

//@arg 0: List of Lists of numbers.
//@return: Matrix of dimension
ERL_NIF_TERM nif_matrix(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    unsigned n_rows, line_length, dest = 0, n_cols = -1;
    ERL_NIF_TERM list = argv[0], row, elem;
    Matrix m;

    //Reading incoming matrix.
    if(!enif_get_list_length(env, list, &n_rows) && n_rows > 0)
        return enif_make_badarg(env);

    for(int i = 0; enif_get_list_cell(env, list, &row, &list); i++){
        if(!enif_get_list_length(env, row, &line_length))
            return enif_make_badarg(env);

        if(n_cols == -1){
            //Allocate binary, make matrix accessor.
            n_cols = line_length;
            m = matrix_alloc(n_rows,n_cols);
        }

        if(n_cols != line_length)
            return enif_make_badarg(env);

        for(int j = 0; enif_get_list_cell(env, row, &elem, &row); j++){
            if(!enif_get_double(env, elem, &m.content[dest])){
                int i;
                if(enif_get_int(env, elem, &i)){
                    m.content[dest] = (double) i;
                }
                else{
                    return enif_make_badarg(env);
                }
            }
            dest++;
        }
    }

    return matrix_to_erl(env, m);
}

#define PRECISION 10

//Used for debug purpose.
void debug_write(char info[]){
    FILE* fp = fopen("debug.txt", "a");
    fprintf(fp, info);
    fprintf(fp, "\n");
    fclose(fp);
}

void debug_write_matrix(Matrix m){
    char *content = enif_alloc(sizeof(char)*((2*m.n_cols-1)*m.n_rows*PRECISION + m.n_rows*2 + 3));
    content[0] = '[';
    content[1] = '\0';
    char converted[PRECISION];

    for(int i=0; i<m.n_rows; i++){
        strcat(content, "[");
        for(int j = 0; j<m.n_cols; j++){
            snprintf(converted, PRECISION-1, "%.5lf", m.content[i*m.n_cols+j]);
            strcat(content, converted);
            if(j != m.n_cols-1)
                strcat(content, " ");
        }
        strcat(content, "]");
    }
    strcat(content, "]");
    debug_write(content);
    enif_free(content);

}

//@arg 0: matrix.
//@arg 1: int, coord m: row
//@arg 2: int, coord n: col
//@return: double, at cord Matrix(m,n).
ERL_NIF_TERM nif_get(ErlNifEnv * env, int argc, const ERL_NIF_TERM argv[]){
    int m,n;
    Matrix matrix;

    if(!enif_get(env, argv, "iim", &m, &n, &matrix))
        return enif_make_badarg(env);
    m--, n--;

    if(m < 0 || m >= matrix.n_rows || n < 0 || n >= matrix.n_cols)
        return enif_make_badarg(env);

    int index = m*matrix.n_cols+n;
    return enif_make_double(env, matrix.content[index]);
}

ERL_NIF_TERM nif_at(ErlNifEnv * env, int argc, const ERL_NIF_TERM argv[]){
    int n;
    Matrix matrix;

    if(!enif_get(env, argv, "mi", &matrix, &n))
        return enif_make_badarg(env);
    n--;

    if( n < 0 || n >= matrix.n_cols * matrix.n_rows)
        return enif_make_badarg(env);

    return enif_make_double(env, matrix.content[n]);
}

//Matrix to flattened list of ints
ERL_NIF_TERM nif_mtfli(ErlNifEnv * env, int argc, const ERL_NIF_TERM argv[]){
    Matrix M;
    if(!enif_get(env, argv, "m", &M)){
        return enif_make_badarg(env);
    }

    int n_elems = M.n_cols * M.n_rows;
    ERL_NIF_TERM *arr = enif_alloc(sizeof(ERL_NIF_TERM)*n_elems);
    for(int i = 0; i<n_elems; i++){
        arr[i] = enif_make_int(env, (int)M.content[i]);
    }

    ERL_NIF_TERM result = enif_make_list_from_array(env, arr, n_elems);
    enif_free(arr);
    return result;
}

//Matrix to flattened list of ints
ERL_NIF_TERM nif_mtfl(ErlNifEnv * env, int argc, const ERL_NIF_TERM argv[]){
    Matrix M;
    if(!enif_get(env, argv, "m", &M)){
        return enif_make_badarg(env);
    }

    int n_elems = M.n_cols * M.n_rows;
    ERL_NIF_TERM *arr = enif_alloc(sizeof(ERL_NIF_TERM)*n_elems);
    for(int i = 0; i<n_elems; i++){
        arr[i] = enif_make_double(env, M.content[i]);
    }

    ERL_NIF_TERM result = enif_make_list_from_array(env, arr, n_elems);
    enif_free(arr);
    return result;
}

//Equal all doubles
//Compares whether all doubles are approximately the same.
int equal_ad(double* a, double* b, int size){
    for(int i = 0; i<size; i++){
        if(fabs(a[i] - b[i])> 1e-6)
            return 0;
    }
    return 1;
}

//@arg 0: Array.
//@arg 1: Array.
//@return: true if arrays share content, false if they have different content || size..
ERL_NIF_TERM nif_equals(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    Matrix a,b;

    if(!enif_get(env, argv, "mm", &a, &b))
        return atom_false;

    //Compare number of columns and rows
    if((a.n_cols != b.n_cols || a.n_rows != b.n_rows))
        return atom_false;

    //Compare content of arrays
    if(!equal_ad(a.content, b.content, a.n_cols*a.n_rows))
        return atom_false;

    return atom_true;
}

//@arg 0: int.
//@arg 1: Array.
//@return: returns an array, containing requested row.
ERL_NIF_TERM nif_row(ErlNifEnv * env, int argc, const ERL_NIF_TERM argv[]){
    int row_req;
    Matrix matrix;
    if(!enif_get(env, argv, "im", &row_req, &matrix))
        return enif_make_badarg(env);

    row_req--;
    if(row_req<0 || row_req >= matrix.n_rows)
        return enif_make_badarg(env);

    Matrix row = matrix_alloc(1, matrix.n_cols);
    memcpy(row.content, matrix.content + (row_req * matrix.n_cols), matrix.n_cols*sizeof(double));

    return matrix_to_erl(env, row);
}

//@arg 0: int.
//@arg 1: Array.
//@return: returns an array, containing requested col.
ERL_NIF_TERM nif_col(ErlNifEnv * env, int argc, const ERL_NIF_TERM argv[]){
    int col_req;
    Matrix matrix;
    if(!enif_get(env, argv, "im", &col_req, &matrix))
        return enif_make_badarg(env);

    col_req--;
    if(col_req<0 || col_req >= matrix.n_cols)
        return enif_make_badarg(env);

    Matrix col = matrix_alloc(matrix.n_rows, 1);

    for(int i = 0; i < matrix.n_rows; i++){
        col.content[i] = matrix.content[i * matrix.n_rows + col_req];
    }

    return matrix_to_erl(env, col);
}

//@arg 0: int.
//@arg 1: int.
//@return: empty Matrix of requested dimension..
ERL_NIF_TERM nif_zeros(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    int m,n;
    if(!enif_get(env, argv, "ii", &m, &n))
        return enif_make_badarg(env);

    Matrix a = matrix_alloc(m,n);
    memset(a.content, 0, sizeof(double)*m*n);
    return matrix_to_erl(env, a);
}

//@arg 0: int.
//@arg 1: int.
//@return: empty matrix of dimension [arg 0, arg 1]..
ERL_NIF_TERM nif_eye(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    int m;
    if(!enif_get_int(env, argv[0], &m))
        return enif_make_badarg(env);

    if(m <= 0)
        return enif_make_badarg(env);

    Matrix a = matrix_alloc(m,m);
    memset(a.content, 0, sizeof(double)*m*m);
    for(int i = 0; i<m; i++){
        a.content[i*m+i] = 1.0;
    }
    return matrix_to_erl(env, a);
}

//Either element-wise multiplication, or multiplication of a matrix by a number.
ERL_NIF_TERM nif_mult(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){

    Matrix a,b;
    double c;

    if(enif_get(env, argv, "mn", &a, &c)){
        Matrix d = matrix_alloc(a.n_rows, a.n_cols);

        for(int i = 0; i<a.n_cols*a.n_rows; i++){
            d.content[i] = a.content[i] * c;
        }
        return matrix_to_erl(env, d);
    }
    else
    if(enif_get(env, argv, "mm", &a, &b)
            && a.n_rows*a.n_cols == b.n_rows*b.n_cols){

        Matrix d = matrix_alloc(a.n_rows, a.n_cols);

        for(int i = 0; i < a.n_rows*a.n_cols; i++){
            d.content[i] = a.content[i] * b.content[i];
        }
        return matrix_to_erl(env, d);
    }

    return enif_make_badarg(env);
}

//Either element-wise addition, or addition of a matrix by a number.
ERL_NIF_TERM nif_add(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){

    Matrix a,b;
    double c;

    if(enif_get(env, argv, "mn", &a, &c)){
        Matrix d = matrix_alloc(a.n_rows, a.n_cols);

        for(int i = 0; i<a.n_cols*a.n_rows; i++){
            d.content[i] = a.content[i] + c;
        }
        return matrix_to_erl(env, d);
    }
    else
    if(enif_get(env, argv, "mm", &a, &b)
            && a.n_rows*a.n_cols == b.n_rows*b.n_cols){

        Matrix d = matrix_alloc(a.n_rows, a.n_cols);

        for(int i = 0; i < a.n_rows*a.n_cols; i++){
            d.content[i] = a.content[i] + b.content[i];
        }
        return matrix_to_erl(env, d);
    }

    return enif_make_badarg(env);
}

//Either element-wise subtraction, or subtraction of a matrix by a number.
ERL_NIF_TERM nif_sub(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){

    Matrix a,b;
    double c;

    if(enif_get(env, argv, "mn", &a, &c)){
        Matrix d = matrix_alloc(a.n_rows, a.n_cols);

        for(int i = 0; i<a.n_cols*a.n_rows; i++){
            d.content[i] = a.content[i] - c;
        }
        return matrix_to_erl(env, d);
    }
    else
    if(enif_get(env, argv, "mm", &a, &b)
            && a.n_rows*a.n_cols == b.n_rows*b.n_cols){

        Matrix d = matrix_alloc(a.n_rows, a.n_cols);

        for(int i = 0; i < a.n_rows*a.n_cols; i++){
            d.content[i] = a.content[i] - b.content[i];
        }
        return matrix_to_erl(env, d);
    }

    return enif_make_badarg(env);
}

//Either element-wise division, or division of a matrix by a number.
ERL_NIF_TERM nif_divide(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){

    Matrix a,b;
    double c;

    if(enif_get(env, argv, "mn", &a, &c)){

        if(c!=0.0){
            Matrix d = matrix_alloc(a.n_rows, a.n_cols);

            for(int i = 0; i<a.n_cols*a.n_rows; i++){
                d.content[i] = a.content[i] / c;
            }
            return matrix_to_erl(env, d);
        }
    }
    else if(enif_get(env, argv, "mm", &a, &b)
            && a.n_rows*a.n_cols == b.n_rows*b.n_cols){

        Matrix d = matrix_alloc(a.n_rows, a.n_cols);

        for(int i = 0; i < a.n_rows*a.n_cols; i++){
            if(b.content[i] == 0.0){
                return enif_make_badarg(env);
            }
            else{
                d.content[i] = a.content[i] / b.content[i];
            }
        }
        return matrix_to_erl(env, d);
    }

    return enif_make_badarg(env);
}

//Transpose of a matrix.
Matrix tr(Matrix a){
    Matrix result = matrix_alloc(a.n_cols ,a.n_rows);

    for(int j = 0; j < a.n_rows; j++){
        for(int i = 0; i < a.n_cols; i++){
            result.content[i*result.n_cols+j] = a.content[j*a.n_cols+i];
        }
    }
    return result;
}
ERL_NIF_TERM nif_transpose(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){

    Matrix a;
    if(!enif_get_matrix(env, argv[0], &a))
        return enif_make_badarg(env);

    return matrix_to_erl(env, tr(a));
}

//arg0: Matrix.
ERL_NIF_TERM nif_inv(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){

    Matrix a;
    if(!enif_get_matrix(env, argv[0], &a))
        return enif_make_badarg(env);

    Matrix inv = matrix_dup(a);

    int N = a.n_rows;
    int *IPIV = enif_alloc(sizeof(int)*N);
    int LWORK = N*N;
    double* WORK = enif_alloc(sizeof(int)*N*N);
    int INFO1, INFO2;

    dgetrf_(&N,&N,inv.content,&N,IPIV,&INFO1);
    dgetri_(&N,inv.content,&N,IPIV,WORK,&LWORK,&INFO2);

    enif_free(IPIV);
    enif_free(WORK);
    ERL_NIF_TERM result;

    if(INFO1 > 0 || INFO2 > 0){
        result = enif_raise_exception(env, enif_make_atom(env, "nif_inv: could not invert singular matrix."));
        matrix_free(inv);
    }
    else if(INFO1 < 0 || INFO2 < 0){
        result = enif_raise_exception(env, enif_make_atom(env, "nif_inv: LAPACK error."));
        matrix_free(inv);
    }
    else result = matrix_to_erl(env, inv);

    return result;
}

//----------------------------------------------------------------------------------------------------|
//                        ------------------------------------------                                  |
//                        |                   CBLAS                |                                  |
//                        ------------------------------------------                                  |
//----------------------------------------------------------------------------------------------------|
//Some CBLAS wrappers.

//Calculates the norm of input vector/matrix, aka the square root of the sum of its composants.
ERL_NIF_TERM nif_dnrm2(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    Matrix x;

    if(!enif_get(env, argv, "m", &x)){
        return enif_make_badarg(env);
    }

    double result = cblas_dnrm2(x.n_cols*x.n_rows, x.content, 1);

    return enif_make_double(env, result);
}

//Performs blas_ddot
//Input: two vectors / matrices
ERL_NIF_TERM nif_ddot(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    Matrix x,y;

    if(!enif_get(env, argv, "mm", &x, &y)){
        return enif_make_badarg(env);
    }

    int n = fmin(x.n_rows*x.n_cols, y.n_rows*y.n_cols);
    if(n <= 0){
        return enif_make_badarg(env);
    }

    double result = cblas_ddot(n, x.content, 1, y.content, 1);

    return enif_make_double(env, result);
}

//Performs blas_daxpy
//Input: a number, vectors X and Y
//Output: a vector of same dimension then Y, containing alpha X + Y
//------------------------------------------------------------------------
ERL_NIF_TERM nif_daxpy(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    Matrix x,y;
    int n;
    double alpha;

    if(!enif_get(env, argv, "inmm", &n, &alpha, &x, &y)){
        return enif_make_badarg(env);
    }

    if(fmin(x.n_rows, x.n_cols) * fmin(y.n_rows, y.n_cols) != 1){
        //We are not using vectors...
        return enif_make_badarg(env);
    }

    Matrix ny = matrix_dup(y);

    cblas_daxpy(n, alpha, x.content, 1, ny.content, 1);

    return matrix_to_erl(env, ny);
}

// Arguments: alpha, A, x, beta, y
// Performs alpha*A*x + beta*y.
// alpha, beta are numbers
// A, x, y are matrices (x and y being vectors)
// x and y are expected to have a length of A.n_cols
ERL_NIF_TERM nif_dgemv(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    Matrix A,x,y;
    double alpha, beta;

    if(!enif_get(env, argv, "nmmnm", &alpha, &A, &x, &beta, &y)){
        enif_make_badarg(env);
    }

    //Check dimensions compatibility
    int vec_length = fmin(fmax(x.n_cols, x.n_rows), fmax(y.n_cols, y.n_rows));
    if(vec_length < A.n_cols || fmin(x.n_cols, x.n_rows) != 1 || fmin(y.n_cols, y.n_rows) != 1){
        enif_make_badarg(env);
    }

    Matrix ny = matrix_dup(y);

    cblas_dgemv(CblasRowMajor, CblasNoTrans, A.n_rows, A.n_cols, alpha, A.content, A.n_rows, x.content, 1,  beta, ny.content, 1);

    return matrix_to_erl(env, ny);
}

//Arguments: double alpha, matrix A, matrix B, double beta, matrix C
ERL_NIF_TERM nif_dgemm(ErlNifEnv *env, int argc, const ERL_NIF_TERM argv[]){
    Matrix A, B;
    double alpha = 1.0;
    double beta = 0.0;

    if(!enif_get(env, argv, "mm", &A, &B)
            || A.n_cols != B.n_rows){

        return enif_make_badarg(env);
    }

    Matrix C = matrix_alloc(A.n_rows, B.n_cols);

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
    A.n_rows, B.n_cols, A.n_cols,
    alpha, A.content, A.n_cols, B.content, B.n_rows,
    1.0, C.content, C.n_cols);

    return matrix_to_erl(env, C);
}

ErlNifFunc nif_funcs[] = {
    {"matrix", 1, nif_matrix},
    {"get", 3, nif_get},
    {"at", 2, nif_at},
    {"mtfli", 1, nif_mtfli},
    {"mtfl", 1, nif_mtfl},
    {"equals", 2, nif_equals},
    {"row", 2, nif_row},
    {"col", 2, nif_col},
    {"zeros", 2, nif_zeros},
    {"eye", 1, nif_eye},
    {"mult", 2, nif_mult},
    {"add",  2, nif_add},
    {"sub",  2, nif_sub},
    {"divide", 2, nif_divide},
    {"transpose", 1, nif_transpose},
    {"inv", 1, nif_inv},

    //--- BLAS----------
    {"nrm2", 1, nif_dnrm2},
    {"vec_dot", 2, nif_ddot},
    {"dot", 2, nif_dgemm}
};

ERL_NIF_INIT(numerl, nif_funcs, load, NULL, upgrade, NULL)