#ifndef UTIL_H
#define UTIL_H

#include <iostream>
#include <stdio.h>
#include <vector>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/Householder>
#include <Eigen/LU>
#include <assert.h>
#include <time.h>
#include <sys/time.h>
#include <random>
#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif

#include "spaND.h"

namespace spaND {

bool are_connected(Eigen::VectorXi &a, Eigen::VectorXi &b, SpMat &A);
bool should_be_disconnected(int lvl1, int lvl2, int sep1, int sep2);
double elapsed(timeval& start, timeval& end);
void swap2perm(Eigen::VectorXi* swap, Eigen::VectorXi* perm);
bool isperm(const Eigen::VectorXi* perm);
Eigen::VectorXi invperm(const Eigen::VectorXi& perm);
SpMat symmetric_graph(SpMat& A);

typedef timeval timer;
timer wctime();

/**
 * C <- alpha A   * B   + beta C
 * C <- alpha A^T * B   + beta C
 * C <- alpha A   * B^T + beta C
 * C <- alpha A^T * B^T + beta C
 * Gemm
 */
void gemm(Eigen::MatrixXd* A, Eigen::MatrixXd* B, Eigen::MatrixXd* C, CBLAS_TRANSPOSE tA, CBLAS_TRANSPOSE tB, double alpha, double beta);

/** Return a new
 * C <- alpha A^(/T) * B^(/T)
 **/
Eigen::MatrixXd* gemm_new(Eigen::MatrixXd* A, Eigen::MatrixXd* B, CBLAS_TRANSPOSE tA, CBLAS_TRANSPOSE tB, double alpha);

/**
 * C <- alpha A * A^T + beta C (or A^T * A)
 */
void syrk(Eigen::MatrixXd* A, Eigen::MatrixXd* C, CBLAS_TRANSPOSE tA, double alpha, double beta);

/**
 * A <- L, L L^T = A
 * Return != 0 if potf failed (not spd)
 */
int potf(Eigen::MatrixXd* A);

int ldlt(Eigen::MatrixXd* A, Eigen::MatrixXd* L, Eigen::VectorXd* d, Eigen::VectorXi* p, double* rcond);

/**
 * A <- [L\U] (lower and upper)
 * p <- swap (NOT a permutation)
 * A[p] = L*U
 * L is unit diagonal
 * U is not
 * Return != 0 if getf failed (singular)
 */
int getf(Eigen::MatrixXd* A, Eigen::VectorXi* swap);

/**
 * A = P L U Q
 * A <- [L\U] (lower and upper)
 * L is unit diagonal
 * U is not
 */
void fpgetf(Eigen::MatrixXd* A, Eigen::VectorXi* p, Eigen::VectorXi* q);

/**
 * A = L U
 * breaks down into L & U, and split the diagonal between the two
 * A = L * (D + U') = L * D (I + D^-1 U') = { L * |D|^1/2 } { |D|^1/2 sign(D) (I + D^-1 U') }
 */
void split_LU(Eigen::MatrixXd* A, Eigen::MatrixXd* L, Eigen::MatrixXd* U);

/**
 * Compute an estimated 1-norm condition number of A using its LU or Cholesky factorization
 */
double rcond_1_getf(Eigen::MatrixXd* A_LU, double A_1_norm);
double rcond_1_potf(Eigen::MatrixXd* A_LLT, double A_1_norm);
double rcond_1_trcon(Eigen::MatrixXd* LU, char uplo, char diag);

/**
 * B <- B * L^(-1)
 * B <- B * L^(-T)
 * B <- B * U^(-1)
 * B <- B * U^(-T)
 */
void trsm_right(Eigen::MatrixXd* L, Eigen::MatrixXd* B, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans, CBLAS_DIAG diag);

/**
 * B <- L^(-1) * B
 * B <- L^(-T) * B
 * B <- U^(-1) * B
 * B <- U^(-T) * B
 */
void trsm_left(Eigen::MatrixXd* L, Eigen::MatrixXd* B, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans, CBLAS_DIAG diag);

/**
 * x <- L^(-1) * x
 * x <- L^(-T) * x
 * x <- U^(-1) * x
 * x <- U^(-T) * x
 */
void trsv(Eigen::MatrixXd* L, Segment* x, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans, CBLAS_DIAG diag);

/**
 * x <- L^T * x
 */
void trmv_trans(Eigen::MatrixXd* L, Segment* x);

/**
 * A <- L^T * A
 */
void trmm_trans(Eigen::MatrixXd* L, Eigen::MatrixXd* A);

/**
 * x2 <- x2 - A21 * x1
 */
void gemv_notrans(Eigen::MatrixXd* A21, Segment* x1, Segment* x2);

/**
 * x2 <- x2 - A12^T * x1
 */
void gemv_trans(Eigen::MatrixXd* A12, Segment* x1, Segment* x2);

/**
 * AP = QR
 */
void geqp3(Eigen::MatrixXd* A, Eigen::VectorXi* jpvt, Eigen::VectorXd* tau);

/**
 * A = U S VT
 * Compute the full SVD, where if A is mxn, U is mxm, V is nxn, and S is min(M,N)
 * VT is V^T, *not* V.
 */
void gesvd(Eigen::MatrixXd* A, Eigen::MatrixXd* U, Eigen::VectorXd* S, Eigen::MatrixXd* VT);

/**
 * A = U S U^T
 * A <- U
 * Full Symmetric EVD
 */
void syev(Eigen::MatrixXd* A, Eigen::VectorXd* S);

/**
 * x <- Q * x
 */
void ormqr_notrans(Eigen::MatrixXd* v, Eigen::VectorXd* h, Segment* x);

/**
 * x <- Q^T * x
 * A <- Q^T * A
 */
void ormqr_trans(Eigen::MatrixXd* v, Eigen::VectorXd* h, Segment* x);

/**
 * A <- A   * Q
 * A <- A   * Q^T
 * A <- Q   * A
 * A <- Q^T * A
 */
void ormqr(Eigen::MatrixXd* v, Eigen::VectorXd* h, Eigen::MatrixXd* A, char side, char trans);

/**
 * Create the thin Q
 */
void orgqr(Eigen::MatrixXd* v, Eigen::VectorXd* h);

/**
 * Create a square Q
 */
// void orgqr_fat(Eigen::MatrixXd* v, Eigen::VectorXd* h);

/**
 * A = QR
 */
void geqrf(Eigen::MatrixXd* A, Eigen::VectorXd* tau);

int choose_rank(Eigen::VectorXd& s, double tol);

std::size_t hashv(std::vector<size_t> vals);

// Hash function for Eigen matrix and vector.
// The code is from `hash_combine` function of the Boost library. See
// http://www.boost.org/doc/libs/1_55_0/doc/html/hash/reference.html#boost.hash_combine .
template<typename T>
struct matrix_hash : std::unary_function<T, size_t> {
  std::size_t operator()(T const& matrix) const {
    // Note that it is oblivious to the storage order of Eigen matrix (column- or
    // row-major). It will give you the same hash value for two different matrices if they
    // are the transpose of each other in different storage order.
    size_t seed = 0;
    for (size_t i = 0; i < matrix.size(); ++i) {
      auto elem = *(matrix.data() + i);
      seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
    }
    return seed;
  }
};

void block2dense(Eigen::VectorXi &rowval, Eigen::VectorXi &colptr, Eigen::VectorXd &nnzval, int i, int j, int li, int lj, Eigen::MatrixXd *dst, bool transpose);

Eigen::MatrixXd linspace_nd(int n, int dim);

// Returns A[p,p]
SpMat symm_perm(SpMat &A, Eigen::VectorXi &p);

// Random vector with seed
Eigen::VectorXd random(int size, int seed);

Eigen::MatrixXd random(int rows, int cols, int seed);

// Set Eigen::MatrixXd to zero using memset
void setZero(Eigen::MatrixXd* A);

// Print vector
template<typename T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& v) {
    for(auto v_ : v) {
        os << v_ << " " ;
    }
    os << std::endl;
    return os;
}

// Range
template<typename T>
struct ItRange
{
    public:
        ItRange(const T& begin, const T& end) : begin_(begin), end_(end) {}
        const T& begin() const {
            return this->begin_;
        }
        const T& end() {
            return this->end_;
        }
    private:
        T begin_;
        T end_;
};

/**
 * Statistics and Logging
 **/

struct Profile {
    public:
        double elim;
        double scale;
        double spars;
        double merge;

        double elim_pivot;
        double elim_panel;
        double elim_schur;

        double scale_pivot;
        double scale_panel;

        double spars_assmb;
        double spars_spars;
        double spars_scatt;

        double merge_alloc;
        double merge_copy;

        double potf;
        double ldlt;
        double trsm;
        double gemm;
        double geqp3;
        double geqrf;
        double syev;
        double gesvd;
        double getf;

        double buildq;
        double scattq;
        double perma;
        double scatta;
        double assmb;
        double phi;

        Profile() :
            elim(0), scale(0), spars(0), merge(0),
            elim_pivot(0), elim_panel(0), elim_schur(0),
            scale_pivot(0), scale_panel(0),
            spars_assmb(0), spars_spars(0), spars_scatt(0),
            merge_alloc(0), merge_copy(0),
            potf(0), ldlt(0), trsm(0), gemm(0), geqp3(0), geqrf(0), syev(0), gesvd(0), getf(0),
            buildq(0), scattq(0), perma(0), scatta(0), assmb(0), phi(0)
            {}
};

struct PivotFlops {
    long rows;
    double time;
};

struct PanelFlops {
    long rows;
    long cols;
    double time;
};

struct GemmFlops {
    long rows;
    long cols;
    long inner;
    double time;
};

struct RRQRFlops {
    long rows;
    long cols;
    double time;
};

struct ProfileFlops {
    public:
        std::vector<PivotFlops> pivot;
        std::vector<PanelFlops> panel;
        std::vector<GemmFlops>  gemm;
        std::vector<RRQRFlops>  rrqr;
        ProfileFlops() {};
};

template<typename T>
struct Stats {
    private:
        T min;
        T max;
        T sum;
        int count;
    public:
        Stats(): min(std::numeric_limits<T>::max()), max(std::numeric_limits<T>::lowest()), sum(0), count(0) {};
        void addData(T value) {
            this->min = (this->min < value ? this->min : value);
            this->max = (this->max > value ? this->max : value);
            this->sum += value;
            this->count += 1;
        }
        T      getMin()   const { return this->count == 0 ? 0 : this->min; }
        T      getMax()   const { return this->count == 0 ? 0 : this->max; }
        double getMean()  const { return ((double)this->sum)/((double)this->count); }
        int    getCount() const { return this->count; }
        T      getSum()   const { return this->sum; }
};

struct Log {
    public:
        int dofs_nd;
        int dofs_left_nd;
        int dofs_left_elim;
        int dofs_left_spars;
        long long int fact_nnz;
        Stats<int> rank_before;
        Stats<int> rank_after;
        int ignored;
        Stats<int> nbrs;

        Stats<double> cond_diag_elim;
        Stats<double> norm_diag_elim;
        Stats<double> cond_U_elim;
        Stats<double> cond_L_elim;

        Stats<double> cond_diag_scal;
        Stats<double> norm_diag_scal;
        Stats<double> cond_U_scal;
        Stats<double> cond_L_scal;

        double Asym_before_scaling;
        double Asym_after_scaling;
        Stats<double> Anorm_before_lower;
        Stats<double> Anorm_before_upper;
        Stats<double> Anorm_after_lower;
        Stats<double> Anorm_after_upper;

        Log() :
            dofs_nd(0),
            dofs_left_nd(0), dofs_left_elim(0), dofs_left_spars(0),
            fact_nnz(0), ignored(0),
            Asym_before_scaling(0), Asym_after_scaling(0)
            {}
};

}

#endif


/* multifit/multiwlinear.c
 *
 * Copyright (C) 2015 Patrick Alken
 *
 * 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 <gsl/gsl_errno.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

#include "linear_common.c"

int
gsl_multifit_wlinear (const gsl_matrix * X,
                      const gsl_vector * w,
                      const gsl_vector * y,
                      gsl_vector * c,
                      gsl_matrix * cov,
                      double *chisq, gsl_multifit_linear_workspace * work)
{
  size_t rank;
  int status = gsl_multifit_wlinear_tsvd(X, w, y, GSL_DBL_EPSILON, c, cov, chisq, &rank, work);

  return status;
}

int
gsl_multifit_wlinear_tsvd (const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           const double tol,
                           gsl_vector * c,
                           gsl_matrix * cov,
                           double * chisq,
                           size_t * rank,
                           gsl_multifit_linear_workspace * work)
{
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (y->size != n)
    {
      GSL_ERROR("number of observations in y does not match matrix",
                GSL_EBADLEN);
    }
  else if (w->size != n)
    {
      GSL_ERROR("number of weights in w does not match matrix",
                GSL_EBADLEN);
    }
  else if (p != c->size)
    {
      GSL_ERROR ("number of parameters c does not match matrix",
                 GSL_EBADLEN);
    }
  else if (tol <= 0)
    {
      GSL_ERROR ("tolerance must be positive", GSL_EINVAL);
    }
  else
    {
      int status;
      double rnorm, snorm;
      gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
      gsl_vector_view b = gsl_vector_subvector(work->t, 0, n);

      /* compute A = sqrt(W) X, b = sqrt(W) y */
      status = gsl_multifit_linear_applyW(X, w, y, &A.matrix, &b.vector);
      if (status)
        return status;

      /* compute SVD of A */
      status = gsl_multifit_linear_bsvd(&A.matrix, work);
      if (status)
        return status;

      status = multifit_linear_solve(X, &b.vector, tol, 0.0, rank,
                                     c, &rnorm, &snorm, work);
      if (status)
        return status;

      *chisq = rnorm * rnorm;

      /* variance-covariance matrix cov = s2 * (Q S^-1) (Q S^-1)^T */
      {
        const size_t p = X->size2;
        size_t i, j;
        gsl_matrix_view QSI = gsl_matrix_submatrix(work->QSI, 0, 0, p, p);
        gsl_vector_view D = gsl_vector_subvector(work->D, 0, p);

        for (i = 0; i < p; i++)
          {
            gsl_vector_view row_i = gsl_matrix_row (&QSI.matrix, i);
            double d_i = gsl_vector_get (&D.vector, i);

            for (j = i; j < p; j++)
              {
                gsl_vector_view row_j = gsl_matrix_row (&QSI.matrix, j);
                double d_j = gsl_vector_get (&D.vector, j);
                double s;

                gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);

                gsl_matrix_set (cov, i, j, s / (d_i * d_j));
                gsl_matrix_set (cov, j, i, s / (d_i * d_j));
              }
          }
      }
    }

  return GSL_SUCCESS;
}

int
gsl_multifit_wlinear_svd (const gsl_matrix * X,
                          const gsl_vector * w,
                          const gsl_vector * y,
                          double tol,
                          size_t * rank,
                          gsl_vector * c,
                          gsl_matrix * cov,
                          double *chisq, gsl_multifit_linear_workspace * work)
{
  int status = gsl_multifit_wlinear_tsvd(X, w, y, tol, c, cov, chisq, rank, work);
  return status;
}

int
gsl_multifit_wlinear_usvd (const gsl_matrix * X,
                           const gsl_vector * w,
                           const gsl_vector * y,
                           double tol,
                           size_t * rank,
                           gsl_vector * c,
                           gsl_matrix * cov,
                           double *chisq, gsl_multifit_linear_workspace * work)
{
  /* FIXME: this call does actually perform balancing, but this function is deprecated anyway */
  int status = gsl_multifit_wlinear_tsvd(X, w, y, tol, c, cov, chisq, rank, work);
  return status;
}


#include "gemm.h"
#if defined(USE_BLAS)
#include <cblas.h>
#else
// see https://github.com/pjreddie/darknet/blob/master/src/gemm.c

static void gemm_nn(int M, int N, int K, float ALPHA,
                    float *A, int lda,
                    float *B, int ldb,
                    float *C, int ldc)
{
    int i;
    #pragma omp parallel for
    for (i = 0; i < M; ++i)
    {
        int j, k;
        for (k = 0; k < K; ++k)
        {
            float A_PART = ALPHA * A[i * lda + k];
            for (j = 0; j < N; ++j)
            {
                C[i * ldc + j] += A_PART * B[k * ldb + j];
            }
        }
    }
}

static void gemm_nt(int M, int N, int K, float ALPHA,
                    float *A, int lda,
                    float *B, int ldb,
                    float *C, int ldc)
{
    int i;
    #pragma omp parallel for
    for (i = 0; i < M; ++i)
    {
        int j, k;
        for (j = 0; j < N; ++j)
        {
            float sum = 0;
            for (k = 0; k < K; ++k)
            {
                sum += ALPHA * A[i * lda + k] * B[j * ldb + k];
            }
            C[i * ldc + j] += sum;
        }
    }
}

static void gemm_tn(int M, int N, int K, float ALPHA,
                    float *A, int lda,
                    float *B, int ldb,
                    float *C, int ldc)
{
    int i;
    #pragma omp parallel for
    for (i = 0; i < M; ++i)
    {
        int j, k;
        for (k = 0; k < K; ++k)
        {
            float A_PART = ALPHA * A[k * lda + i];
            for (j = 0; j < N; ++j)
            {
                C[i * ldc + j] += A_PART * B[k * ldb + j];
            }
        }
    }
}

static void gemm_tt(int M, int N, int K, float ALPHA,
                    float *A, int lda,
                    float *B, int ldb,
                    float *C, int ldc)
{
    int i;
    #pragma omp parallel for
    for (i = 0; i < M; ++i)
    {
        int j, k;
        for (j = 0; j < N; ++j)
        {
            float sum = 0;
            for (k = 0; k < K; ++k)
            {
                sum += ALPHA * A[i + k * lda] * B[k + j * ldb];
            }
            C[i * ldc + j] += sum;
        }
    }
}
#endif

void gemm(int TA, int TB, int M, int N, int K, float ALPHA,
          data_val_t **A, int offa, int lda,
          data_val_t **B, int offb, int ldb,
          float BETA,
          data_val_t **C, int offc, int ldc)
{
#if defined(USE_BLAS)
    cblas_sgemm(
        CblasRowMajor,
        TA ? CblasTrans : CblasNoTrans,
        TB ? CblasTrans : CblasNoTrans,
        M, N, K, ALPHA, *A + offa, lda, *B + offb, ldb, BETA, *C + offc, ldc);
#else
    int i, j;
    for (i = 0; i < M; ++i)
    {
        for (j = 0; j < N; ++j)
        {
            (*C + offc)[i * ldc + j] *= BETA;
        }
    }
    if (!TA && !TB)
        gemm_nn(M, N, K, ALPHA, *A + offa, lda, *B + offb, ldb, *C + offc, ldc);
    else if (TA && !TB)
        gemm_tn(M, N, K, ALPHA, *A + offa, lda, *B + offb, ldb, *C + offc, ldc);
    else if (!TA && TB)
        gemm_nt(M, N, K, ALPHA, *A + offa, lda, *B + offb, ldb, *C + offc, ldc);
    else
        gemm_tt(M, N, K, ALPHA, *A + offa, lda, *B + offb, ldb, *C + offc, ldc);
#endif
}


/* ----------------------------------------------------------------------------
  JAM_AXI_VEL_WMMT

    Calculates weighted first moments.

    INPUTS
      xp    : projected x' [pc]
      yp    : projected y' [pc]
      nxy   : number of x' and y' values given
      incl  : inclination [radians]
      lum   : projected luminous MGE
      pot   : projected potential MGE
      beta  : velocity anisotropy (1 - vz^2 / vr^2)
      kappa : rotation parameter

    NOTES
      * Based on janis1_weighted_first_moment IDL code by Michele Cappellari.

  Laura L Watkins [lauralwatkins@gmail.com]

  This code is released under a BSD 2-clause license.
  If you use this code for your research, please cite:
  Watkins et al. 2013, MNRAS, 436, 2598
  "Discrete dynamical models of omega Centauri"
  http://adsabs.harvard.edu/abs/2013MNRAS.436.2598W
---------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include "jam.h"
#include "../mge/mge.h"
#include "../tools/tools.h"

double** jam_axi_vel_wmmt( double *xp, double *yp, int nxy, double incl, \
        struct multigaussexp *lum, struct multigaussexp *pot, \
        double *beta, double *kappa, int *gslFlag_vel ) {

    struct params_losint lp;
    struct multigaussexp ilum, ipot;
    double *bani, *s2l, *q2l, *s2q2l, *s2p, *e2p;
    double *iz0, *iz1;
    double lim, result, error, si, ci, trpig, **sb_mu1;
    int i;
    size_t neval;
    int status = 0, *gslFlag;
    int TadejaVar = 0; gslFlag = &TadejaVar;

    // ---------------------------------

    // MGE parameters for inner integrand function

    // convert from projected MGEs to intrinsic MGEs
    ilum = mge_deproject( lum, incl );
    ipot = mge_deproject( pot, incl );

    // calculate useful quantities
    bani = (double *) malloc( ilum.ntotal * sizeof( double ) );
    s2l = (double *) malloc( ilum.ntotal * sizeof( double ) );
    q2l = (double *) malloc( ilum.ntotal * sizeof( double ) );
    s2q2l = (double *) malloc( ilum.ntotal * sizeof( double ) );
    s2p = (double *) malloc( ipot.ntotal * sizeof( double ) );
    e2p = (double *) malloc( ipot.ntotal * sizeof( double ) );

    for ( i = 0; i < ilum.ntotal; i++ ) {
        bani[i] = 1. / ( 1. - beta[i] );
        s2l[i] = pow( ilum.sigma[i], 2 );
        q2l[i] = pow( ilum.q[i], 2 );
        s2q2l[i] = s2l[i] * q2l[i];
    }

    for ( i = 0; i < ipot.ntotal; i++ ) {
        s2p[i] = pow( ipot.sigma[i], 2 );
        e2p[i] = 1. - pow( ipot.q[i], 2 );
    }

    // parameters for integrand function
    lp.incl = incl;
    lp.lum = &ilum;
    lp.pot = &ipot;
    lp.bani = bani;
    lp.s2l = s2l;
    lp.q2l = q2l;
    lp.s2q2l = s2q2l;
    lp.s2p = s2p;
    lp.e2p = e2p;
    lp.kappa = kappa;
    lp.gslFlag_losint = gslFlag;

    // ---------------------------------

    // set up integration
    gsl_integration_workspace *w = gsl_integration_workspace_alloc( 1000 );
    gsl_integration_cquad_workspace *ww = gsl_integration_cquad_workspace_alloc(1000);
    gsl_function F;
    F.function = &jam_axi_vel_losint;

    // trig angles
    si = sin( incl );
    ci = cos( incl );

    // outer limit of integration
    lim = 4. * maximum( ilum.sigma, ilum.ntotal );

    iz0 = (double *) malloc( nxy * sizeof( double ) );
    iz1 = (double *) malloc( nxy * sizeof( double ) );
    for ( i = 0; i < nxy; i++ ) {

        // parameters for integrand function
        lp.xp = xp[i];
        lp.yp = yp[i];

        // do z^0 integral
        lp.zpow = 0.;
        F.function = &jam_axi_vel_losint;
        F.params = &lp;
        gsl_integration_cquad(&F, -lim, lim, 0., 1e-4, ww, &result,
            &error, &neval);
        iz0[i] = result;

        // do z^1 integral
        lp.zpow = 1.;
        F.function = &jam_axi_vel_losint;
        F.params = &lp;
        gsl_set_error_handler_off();
        status += gsl_integration_qag( &F, -lim, lim, 1., 1., 1000, 2, w,
            &result, &error );
         /* uncomment these two lines for testing
        int test = 1;// Tadeja
        status += test; // Tadeja
        */
        if (status > 0 && *gslFlag<20) { // the limiting value is very arbitrary
            *gslFlag_vel += 1;   }

        if (status > 0 && *gslFlag>=20) { // the limiting value is very arbitrary so that the values do not exceed allowed size for integers
            *gslFlag_vel -= 1;   }
        *gslFlag_vel += *gslFlag; // if there was an integration error in losint

        if ( fabs( result ) > 1e-6 ) {
            gsl_integration_cquad(&F, -lim, lim, 0., 1e-3, ww, &result,
                &error, &neval);
        }
        iz1[i] = result;
    }

    // tidy up
    gsl_integration_workspace_free( w );
    gsl_integration_cquad_workspace_free(ww);

    // ---------------------------------

    sb_mu1 = (double **) malloc( nxy * sizeof( double* ) );
    for ( i = 0; i < nxy; i++ ) \
        sb_mu1[i] = (double *) malloc( 3 * sizeof( double ) );

    // calculate for each velocity component
    trpig = 2. * sqrt( M_PI * G );
    for ( i = 0; i < nxy; i++ ) {
        sb_mu1[i][0] = trpig * ( yp[i] * ci * iz0[i] - si * iz1[i] );
        sb_mu1[i][1] = -trpig * xp[i] * ci * iz0[i];
        sb_mu1[i][2] = trpig * xp[i] * si * iz0[i];
    }

    // ---------------------------------

    free( bani );
    free( s2l );
    free( q2l );
    free( s2q2l );
    free( s2p );
    free( e2p );

    free( iz0 );
    free( iz1 );

    return sb_mu1;

}


#ifndef __CNN_FC_H__
#define __CNN_FC_H__

#include <cblas.h>

#include "cnn_types.h"

#ifdef CNN_WITH_CUDA
#include <cublas_v2.h>
#include <cuda_runtime.h>

#include "cnn_init.h"
#endif

static inline void cnn_forward_fc(union CNN_LAYER* layerRef,
                                  struct CNN_CONFIG* cfgRef, int layerIndex)
{
#ifdef CNN_WITH_CUDA
    float alpha = 1.0;
    float beta = 0.0;

    struct CNN_LAYER_FC* layerPtr = &layerRef[layerIndex].fc;

    struct CNN_MAT* wPtr = &layerPtr->weight;
    struct CNN_MAT* outData = &layerPtr->outMat.data;
    struct CNN_MAT* preOutData = &layerRef[layerIndex - 1].outMat.data;

    // Weight matrix multiplication
    cnn_assert_cublas(cublasSgemm(                       //
        cnnInit.blasHandle, CUBLAS_OP_N, CUBLAS_OP_N,    //
        outData->cols, cfgRef->batch, preOutData->cols,  //
        &alpha,                                          //
        wPtr->mat, wPtr->cols,                           //
        preOutData->mat, preOutData->cols,               //
        &beta,                                           //
        outData->mat, outData->cols));

    // Add bias
    beta = 1.0;
    cnn_assert_cudnn(cudnnAddTensor(cnnInit.cudnnHandle,                    //
                                    &alpha,                                 //
                                    layerPtr->biasTen, layerPtr->bias.mat,  //
                                    &beta,                                  //
                                    layerPtr->dstTen, outData->mat));
#else
    // Weight matrix multiplication
    cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                layerRef[layerIndex - 1].outMat.data.rows,
                layerRef[layerIndex].outMat.data.cols,
                layerRef[layerIndex - 1].outMat.data.cols, 1.0,
                layerRef[layerIndex - 1].outMat.data.mat,
                layerRef[layerIndex - 1].outMat.data.cols,
                layerRef[layerIndex].fc.weight.mat,
                layerRef[layerIndex].fc.weight.cols, 0.0,
                layerRef[layerIndex].outMat.data.mat,
                layerRef[layerIndex].outMat.data.cols);

    // Add bias
    for (int j = 0; j < cfgRef->batch; j++)
    {
        int dstIndex = j * layerRef[layerIndex].outMat.data.cols;
        cblas_saxpy(layerRef[layerIndex].fc.bias.cols, 1.0,
                    layerRef[layerIndex].fc.bias.mat, 1,
                    &layerRef[layerIndex].outMat.data.mat[dstIndex], 1);
    }
#endif
}

static inline void cnn_backward_fc(union CNN_LAYER* layerRef,
                                   struct CNN_CONFIG* cfgRef, int layerIndex)
{
#ifdef CNN_WITH_CUDA
    float alpha = 1.0;
    float beta = 1.0;

    struct CNN_LAYER_FC* layerPtr = &layerRef[layerIndex].fc;

    struct CNN_MAT* wPtr = &layerPtr->weight;
    struct CNN_MAT* outData = &layerPtr->outMat.data;
    struct CNN_MAT* preOutData = &layerRef[layerIndex - 1].outMat.data;

    // Sum weight gradient matrix
    cnn_assert_cublas(                                             //
        cublasSgemm(cnnInit.blasHandle, CUBLAS_OP_N, CUBLAS_OP_T,  //
                    wPtr->cols, wPtr->rows, cfgRef->batch,         //
                    &alpha,                                        //
                    outData->grad, outData->cols,                  //
                    preOutData->mat, preOutData->cols,             //
                    &beta,                                         //
                    wPtr->grad, wPtr->cols));

    // Sum bias gradient matrix
    if (cfgRef->batch > 1)
    {
        cnn_assert_cudnn(
            cudnnReduceTensor(cnnInit.cudnnHandle, layerPtr->reduDesc,
                              layerPtr->indData, layerPtr->indSize,  //
                              cnnInit.wsData, cnnInit.wsSize,        //
                              &alpha,                                //
                              layerPtr->dstTen, outData->grad,       //
                              &beta,                                 //
                              layerPtr->biasTen, layerPtr->bias.grad));
    }
    else
    {
        cnn_assert_cublas(                                        //
            cublasSaxpy(cnnInit.blasHandle, layerPtr->bias.cols,  //
                        &alpha,                                   //
                        outData->grad, 1,                         //
                        layerPtr->bias.grad, 1));
    }
#else
    // Sum weight gradient matrix
    cblas_sgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
                layerRef[layerIndex].fc.weight.rows,
                layerRef[layerIndex].fc.weight.cols, cfgRef->batch, 1.0,
                layerRef[layerIndex - 1].outMat.data.mat,
                layerRef[layerIndex - 1].outMat.data.cols,
                layerRef[layerIndex].outMat.data.grad,
                layerRef[layerIndex].outMat.data.cols, 1.0,
                layerRef[layerIndex].fc.weight.grad,
                layerRef[layerIndex].fc.weight.cols);

    // Sum bias gradient matrix
    for (int j = 0; j < cfgRef->batch; j++)
    {
        int srcShift = j * layerRef[layerIndex].outMat.data.cols;
        cblas_saxpy(layerRef[layerIndex].fc.bias.cols, 1.0,
                    &layerRef[layerIndex].outMat.data.grad[srcShift], 1,
                    layerRef[layerIndex].fc.bias.grad, 1);
    }
#endif

    // Find layer gradient
    if (layerIndex > 1)
    {
#ifdef CNN_WITH_CUDA
        beta = 0.0;
        cnn_assert_cublas(                                             //
            cublasSgemm(cnnInit.blasHandle, CUBLAS_OP_T, CUBLAS_OP_N,  //
                        wPtr->rows, cfgRef->batch, wPtr->cols,         //
                        &alpha,                                        //
                        wPtr->mat, wPtr->cols,                         //
                        outData->grad, outData->cols,                  //
                        &beta,                                         //
                        preOutData->grad, preOutData->cols));
#else
        cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
                    layerRef[layerIndex - 1].outMat.data.rows,
                    layerRef[layerIndex - 1].outMat.data.cols,
                    layerRef[layerIndex].outMat.data.cols, 1.0,
                    layerRef[layerIndex].outMat.data.grad,
                    layerRef[layerIndex].outMat.data.cols,
                    layerRef[layerIndex].fc.weight.mat,
                    layerRef[layerIndex].fc.weight.cols, 0.0,
                    layerRef[layerIndex - 1].outMat.data.grad,
                    layerRef[layerIndex - 1].outMat.data.cols);
#endif
    }
}

#endif


//This involves multiplication of two vectors.

#include <stdio.h>
#include <gsl/gsl_vector.h>

 void vector_mul(double* a, double *b, double *c, size_t size)
{
	int i;
  	gsl_vector * a_vec = gsl_vector_alloc (size);
	gsl_vector * b_vec = gsl_vector_alloc (size);
	gsl_vector * c_vec = gsl_vector_alloc (size);
  	for (i = 0; i < size; i++)
    {
      	gsl_vector_set (a_vec, i, a[i]);
		gsl_vector_set (b_vec, i, b[i]);
    }
	gsl_vector_mul(a_vec,b_vec);
	gsl_vector_memcpy(c_vec, a_vec);
  	for (i = 0; i < size; i++) /* OUT OF RANGE ERROR */
    {
		c[i] = gsl_vector_get (c_vec, i);
      	printf ("v_%d = %g\n", i, gsl_vector_get (c_vec, i));
    }

  	gsl_vector_free (a_vec);
	gsl_vector_free (b_vec);
	gsl_vector_free (c_vec);
}

/*
//The main function has been provided solely for the sake of testing the function. It will be removed when the problem set is formally passed on.
int main (void)
{
	double a[5] = {1.0, 2.0, 3.0, 4.0, 5.0};
	double b[5] = {0.1, 0.2, 0.3, 0.4, 0.5};
	double *c;
	vector_mul(a, b, c, 5);
	return 0;
}*/


/* spdgemm.c
 *
 * Copyright (C) 2014 Patrick Alken
 *
 * 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 <math.h>
#include <gsl/gsl_spmatrix.h>
#include <gsl/gsl_spblas.h>
#include <gsl/gsl_errno.h>

/*
gsl_spblas_dgemm()
  Multiply two sparse matrices

Inputs: alpha - scalar factor
        A     - sparse matrix
        B     - sparse matrix
        C     - (output) C = alpha * A * B

Return: success or error

Notes:
1) based on CSparse routine cs_multiply
*/

int
gsl_spblas_dgemm(const double alpha, const gsl_spmatrix *A,
                 const gsl_spmatrix *B, gsl_spmatrix *C)
{
  if (A->size2 != B->size1 || A->size1 != C->size1 || B->size2 != C->size2)
    {
      GSL_ERROR("matrix dimensions do not match", GSL_EBADLEN);
    }
  else if (A->sptype != B->sptype || A->sptype != C->sptype)
    {
      GSL_ERROR("matrix storage formats do not match", GSL_EINVAL);
    }
  else if (!GSL_SPMATRIX_ISCCS(A))
    {
      GSL_ERROR("compressed column format required", GSL_EINVAL);
    }
  else
    {
      int status = GSL_SUCCESS;
      const size_t M = A->size1;
      const size_t N = B->size2;
      size_t *Bi = B->i;
      size_t *Bp = B->p;
      double *Bd = B->data;
      size_t *w = (size_t *) A->work; /* workspace of length M */
      double *x = (double *) C->work; /* workspace of length M */
      size_t *Cp, *Ci;
      double *Cd;
      size_t j, p;
      size_t nz = 0;

      if (C->nzmax < A->nz + B->nz)
        {
          status = gsl_spmatrix_realloc(A->nz + B->nz, C);
          if (status)
            {
              GSL_ERROR("unable to realloc matrix C", status);
            }
        }

      /* initialize workspace to 0 */
      for (j = 0; j < M; ++j)
        w[j] = 0;

      Cp = C->p;
      Ci = C->i;
      Cd = C->data;

      for (j = 0; j < N; ++j)
        {
          if (nz + M > C->nzmax)
            {
              status = gsl_spmatrix_realloc(2 * C->nzmax + M, C);
              if (status)
                {
                  GSL_ERROR("unable to realloc matrix C", status);
                }

              /* these pointers could have changed due to reallocation */
              Ci = C->i;
              Cd = C->data;
            }

          Cp[j] = nz; /* column j of C starts here */

          for (p = Bp[j]; p < Bp[j + 1]; ++p)
            {
              nz = gsl_spblas_scatter(A, Bi[p], Bd[p], w, x, j + 1, C, nz);
            }

          for (p = Cp[j]; p < nz; ++p)
            Cd[p] = x[Ci[p]];
        }

      Cp[N] = nz;
      C->nz = nz;

      /* scale by alpha */
      gsl_spmatrix_scale(C, alpha);

      return status;
    }
} /* gsl_spblas_dgemm() */

/*
gsl_spblas_scatter()

  Keep a running total x -> x + alpha*A(:,j) for adding matrices together in CCS,
which will eventually be stored in C(:,j)

  When a new non-zero element with row index i is found, update C->i with
the row index. C->data is updated only by the calling function after all
matrices have been added via this function.

Inputs: A     - sparse matrix m-by-n
        j     - column index
        alpha - scalar factor
        w     - keeps track which rows of column j have been added to C;
                initialize to 0 prior to first call
        x     - column vector of length m
        mark  -
        C     - output matrix whose jth column will be added to A(:,j)
        nz    - (input/output) number of non-zeros in matrix C

Notes:
1) This function is designed to be called successively when adding multiple
matrices together. Column j of C is stored contiguously as per CCS but not
necessarily in order - ie: the row indices C->i may not be in ascending order.

2) based on CSparse routine cs_scatter
*/

size_t
gsl_spblas_scatter(const gsl_spmatrix *A, const size_t j, const double alpha,
                   size_t *w, double *x, const size_t mark, gsl_spmatrix *C,
                   size_t nz)
{
  size_t p;
  size_t *Ai = A->i;
  size_t *Ap = A->p;
  double *Ad = A->data;
  size_t *Ci = C->i;

  for (p = Ap[j]; p < Ap[j + 1]; ++p)
    {
      size_t i = Ai[p];          /* A(i,j) is nonzero */

      if (w[i] < mark)           /* check if row i has been stored in column j yet */
        {
          w[i] = mark;           /* i is new entry in column j */
          Ci[nz++] = i;          /* add i to pattern of C(:,j) */
          x[i] = alpha * Ad[p];  /* x(i) = alpha * A(i,j) */
        }
      else                       /* this (i,j) exists in C from a previous call */
        {
          x[i] += alpha * Ad[p]; /* add alpha*A(i,j) to C(i,j) */
        }
    }

  return (nz) ;
} /* gsl_spblas_scatter() */


#include <gsl/gsl_vector.h>
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include "verif.h"
#include "data.h"
#include "prob_functions.h"

void EM (Dataset *data)
{
	int i, j;
	const double THRESHOLD = 1E-10;
	double Q, lastQ;

	srand(time(NULL));

	/* Initialize parameters to starting values */
	for (i = 0; i < data->numLabelers * data->numWorkflows; i++) {
		data->alpha[i] = data->priorAlpha[i];
		/*data->alpha[i] = (double) rand() / RAND_MAX * 4 - 2;*/
	}
	for (j = 0; j < data->numImages * data->numWorkflows; j++) {
		data->beta[j] = data->priorBeta[j];
		/*data->beta[j] = (double) rand() / RAND_MAX * 3;*/
	}

	Q = 0;
	EStep(data);
	Q = computeQ(data);
	/*printf("Q = %f\n", Q);*/
  int iter = 0;
	do {
  printf("Iteration %d ", iter++);
		lastQ = Q;
  printf("old Q: %f", lastQ);

		/* Re-estimate P(Z|L,alpha,beta) */
		EStep(data);
		Q = computeQ(data);
		/*printf("Q = %f; L = %f\n", Q, 0.0 computeLikelihood(data));*/
  printf("\tafter E: %f", Q);

		/*outputResults(data);*/
		MStep(data);

		Q = computeQ(data);
  printf("\tafter M: %f\n", Q);
		/*printf("Q = %f; L = %f\n", Q, 0.0 computeLikelihood(data));*/
	} while (fabs((Q - lastQ)/lastQ) > THRESHOLD);
	 outputResults(data);
}

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

    Dataset data;

	if (argc < 2) {
		fprintf(stdout, "Usage: em <data>\n");
		fprintf(stdout, "where the specified data file is formatted as described in the README file.\n");
		exit(1);
	}

	//These used to be 1
	//double prioralpha[2] = {1.0, 0.5};
	int NUMBER_OF_WORKER_POOLS;

	FILE *fi = fopen("../simulation.info", "r");
    fscanf(fi, "%d\n", &NUMBER_OF_WORKER_POOLS);

	double *prioralpha = (double *) malloc(sizeof(double) * NUMBER_OF_WORKER_POOLS);

    double temp1,temp2,temp3;
    fscanf(fi,"%lf\n",&temp1);
    fscanf(fi,"%lf,%lf\n",&temp2,&temp3);
    fscanf(fi,"%lf,%lf\n",&prioralpha[0],&prioralpha[1]);
    fclose(fi);

    prioralpha[0] = 0.5;
    prioralpha[1] = 0.5;

	double priorbeta = 0.5;
	//double prioralpha = 1.0;
	//double priorbeta = 1.0;

   //This code will now break, if extra arguments are passed, since prioralpha is
    // now an array.
  /*
  if ( argc > 2 )
    prioralpha = atof(argv[2]);
  if ( argc > 3 )
    priorbeta = atof(argv[3]);*/

  //printf("BLAH\n");
	readData(argv[1], &data, prioralpha, priorbeta);
	//	printf("LOADING TRUTH\n");
	//Load_truth(&data);
  //printf("STARTING EM\n");
	EM(&data);
	/*
  Accuracy(&data);
  int size;
  int s, samples = 50000;
  for ( size = 3; size <= 11; size+=2 ) {
    double acc = 0;
    for ( s = 0; s < samples; ++s ) {
      Randomizer( size);
      acc += Posterior( &data, size );
    }
    acc /= samples;
    printf("%d %f\n", size, acc);
  }

	free(data.priorAlpha);
	free(data.priorBeta);
	free(data.labels);
	free(data.alpha);
	free(data.beta);
	free(data.probZ1);
	free(data.probZ0);
	*/
}


//
//  likelihood.h
//  EpiGenMCMC
//
//  Created by Lucy Li on 13/04/2016.
//  Copyright (c) 2016 Lucy Li, Imperial College London. All rights reserved.
//

#ifndef __EpiGenMCMC__likelihood__
#define __EpiGenMCMC__likelihood__

#include <vector>
#include <stdio.h>
#include <limits>
#include <gsl/gsl_cdf.h>
#include "parameter.h"

class Likelihood {
public:
    Likelihood();
    double binomial_lik(double, double, int, int, int, int, bool);
    double binomial_lik(double, double, std::vector<double>::iterator, int, int, int, int, int, bool);
    double coalescent_lik(std::vector<double>::iterator, std::vector<double>::iterator, std::vector<double>::iterator, std::vector<double>::iterator, std::vector<double>::iterator, int, int, int, bool);
};

#endif /* defined(__EpiGenMCMC__likelihood__) */


#ifndef structs_h
#define structs_h

#include <ceed.h>
#include <petsc.h>

// -----------------------------------------------------------------------------
// PETSc Operator Structs
// -----------------------------------------------------------------------------

// Data for PETSc Matshell
typedef struct UserO_ *UserO;
struct UserO_ {
  MPI_Comm comm;
  DM dm;
  Vec X_loc, Y_loc, diag;
  CeedVector x_ceed, y_ceed;
  CeedOperator op;
  Ceed ceed;
};

// Data for PETSc Prolong/Restrict Matshells
typedef struct UserProlongRestr_ *UserProlongRestr;
struct UserProlongRestr_ {
  MPI_Comm comm;
  DM dmc, dmf;
  Vec loc_vec_c, loc_vec_f, mult_vec;
  CeedVector ceed_vec_c, ceed_vec_f;
  CeedOperator op_prolong, op_restrict;
  Ceed ceed;
};

// -----------------------------------------------------------------------------
// libCEED Data Structs
// -----------------------------------------------------------------------------

// libCEED data struct for level
typedef struct CeedData_ *CeedData;
struct CeedData_ {
  Ceed ceed;
  CeedBasis basis_x, basis_u, basis_c_to_f;
  CeedElemRestriction elem_restr_x, elem_restr_u, elem_restr_u_i, elem_restr_qd_i;
  CeedQFunction qf_apply;
  CeedOperator op_apply, op_restrict, op_prolong;
  CeedVector q_data, x_ceed, y_ceed;
};

// BP specific data
typedef struct {
  CeedInt num_comp_x, num_comp_u, topo_dim, q_data_size, q_extra;
  CeedQFunctionUser setup_geo, setup_rhs, apply, error;
  const char *setup_geo_loc, *setup_rhs_loc, *apply_loc, *error_loc;
  CeedEvalMode in_mode, out_mode;
  CeedQuadMode q_mode;
  PetscBool enforce_bc;
  PetscErrorCode (*bc_func)(PetscInt, PetscReal, const PetscReal *,
                            PetscInt, PetscScalar *, void *);
} BPData;

#endif // structs_h


#define _XOPEN_SOURCE 600

#include <stdlib.h>
#include <stdio.h>
#include <mpi.h>
#include <stdint.h>
#include <time.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <errno.h>
#include "fileio.h"
#include "likelihood.h"
#include "master_tasks.h"
#include "mutation.h"
#include "radix.h"
#include "randomisation.h"
#include "resampling.h"
#include "typedefs.h"

static const int ALG = 6;
static const int MASTER = 0;

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

  struct Process process;
  struct Estimates estimates;
  int pair_rank, src;
  int do_resampling;
  long *inds;
  double *w, *x, *x_resamp;
  double total_w = 1;
  double my_normal_weight;
  double w_pair[2];
  double rsamp_time=0;
  double rsamp_start=0;
  double comm_time=0;
  double comm_start;
  double comm_time2=0;
  double *output;
  double weight_time = 0;
  double mutation_time = 0.0;
  double init_time = 0.0;
   unsigned int cnts[2];
  short counts[2];
  short n_resamp;

  const gsl_rng_type * T;
  double timer_start;

  // Parse commandline arguments
  int run_idx = 0;
  if ( argc > 1 )
    run_idx = atoi( argv[ 1 ] );

  long N = 1000;
  if ( argc > 2 )
    N = atol( argv[ 2 ] );

  int state_dim;
  if ( argc > 3 )
    state_dim = atoi( argv[ 3 ] );
  else {
    perror("State dimesion must be provided");
    return -1;
  }

  double epc_threshold;
  if (argc > 4 ) {
    epc_threshold = atof( argv[ 4 ] );
  } else {
    perror("Reampling threshold must be provided");
    return -1;
  }

  // Initialize the MPI environment
  MPI_Init(NULL, NULL);

  gsl_rng_env_setup();
  T = gsl_rng_default;
  gsl_rng *rgen;
  rgen = gsl_rng_alloc( T );

  // Get the number of processes
  int world_size;
  MPI_Comm_size(MPI_COMM_WORLD, &world_size);

  // Get the rank of the process
  int world_rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &world_rank);

  // Start the timer
  struct timespec start_time;
  clock_gettime(CLOCK_REALTIME, &start_time);

  MPI_Barrier( MPI_COMM_WORLD );
  timer_start = MPI_Wtime();

  long data_length;
  process = readProcessFile( state_dim );
  data_length = (int)process.n;

  // Construct the communication hypercube
  MPI_Comm nthCube;
  int hc_rank;
  int nDim;
  constructHypercube( world_size, &nthCube, &hc_rank, &nDim );
  int S = nDim;

  //---------------
  // Initialisation
  //---------------
  x = malloc( sizeof( double ) * N * state_dim );
  w = malloc( sizeof( double ) * N );

  inds = malloc( sizeof( long ) * N );
  x_resamp = malloc( sizeof( double ) * N * state_dim );

  // Initial distribution setup
  setupInitialSeed( world_rank, rgen );

  // Create initial sample
  createInitialSample( x, N, rgen, state_dim );

  // Setup the randomisation
  set_resamplingseed( world_rank );

  int output_dim = 2 + state_dim + 2 * world_size + world_size * state_dim;
  if ( world_rank == MASTER )
    output = malloc( sizeof( double ) * output_dim * data_length );

  my_normal_weight = 1.0 / world_size;

  MPI_Barrier( MPI_COMM_WORLD );
  init_time += MPI_Wtime() - timer_start;

  // main loop
  for( long n = 0; n < data_length; n++ ) {

    // ------------------------------------------------------------------------------
    MPI_Barrier( MPI_COMM_WORLD );
    timer_start = MPI_Wtime();

    evaluateLikelihoods( x, process, state_dim, data_length, N, n, w,
			 my_normal_weight );

    MPI_Barrier( MPI_COMM_WORLD );
    weight_time += MPI_Wtime() - timer_start ;

    // Compute the relevant estimates for THIS filter only
    estimates = estimateOutput( w, x, world_rank, N, run_idx, state_dim );

    // Combine the results of all filters
    combineEstimates( world_rank, state_dim, estimates, output, n,
		      output_dim, world_size, &total_w, &my_normal_weight,
		      &do_resampling, epc_threshold);

    // -----------------------------
    // RESAMPLING
    // -----------------------------

    // Resampling within processors

    MPI_Barrier( MPI_COMM_WORLD );
    rsamp_start = MPI_Wtime();

    serial_multinomial_resample_sng_noutk( N, w, inds, N );

    for ( long i = 0; i < N; i++)
      for ( int d = 0; d < state_dim; d++ )
	x_resamp[ state_dim * i + d ] = x[ state_dim * inds[ i ] + d ];

    for ( long i = 0; i < N; i++)
      for ( int d = 0; d < state_dim ; d++ )
	x[ state_dim * i + d ] = x_resamp[ state_dim * i + d ];

    // -----------------------------------------------------------------------------
    MPI_Barrier( MPI_COMM_WORLD );
    rsamp_time += MPI_Wtime() - rsamp_start;
    comm_start = MPI_Wtime();

    if ( do_resampling ) {

      // ---------------------------
      // START THE RADIX INTERACTION
      // ---------------------------

      for ( int s = 0; s < S ; s++ ) {

	// Determine the process you need to communicate with
	MPI_Cart_shift( nthCube, s, 1, &src, &pair_rank );

	// The lower ranking process in the pair gets the filter weights and
	// does the island level resampling
	if ( hc_rank < pair_rank ) {

	  MPI_Recv( w_pair + 1, 1, MPI_DOUBLE, pair_rank, 0, nthCube,
		    MPI_STATUS_IGNORE );

	  // The lower ranking filter must send its weight to the paired filter
	  // so that the paired filter can update its weight
	  MPI_Send( &my_normal_weight, 1, MPI_DOUBLE, pair_rank, 1, nthCube );

	  w_pair[ 0 ] = my_normal_weight;

	  // Generate a binomial random variate indicating how many samples
	  // are drawn from each of the paired processes.
	  gsl_ran_multinomial( rgen , ( size_t ) 2, ( unsigned int ) ( 2 ), w_pair,
			       cnts );

	  counts[ 0 ] = ( short ) cnts[ 0 ];
	  counts[ 1 ] = ( short ) cnts[ 1 ];

	  // Send the resampling counts to the paired process
	  MPI_Send( counts + 1 , 1, MPI_SHORT, pair_rank, 3, nthCube );

	  n_resamp = counts[ 0 ];

	} else {

	  // Send my weight to the lower ranking filter in the pair
	  MPI_Send(&my_normal_weight, 1, MPI_DOUBLE, pair_rank, 0, nthCube );

	  // Receive the weight of the paired filter
	  MPI_Recv( w_pair, 1, MPI_DOUBLE, pair_rank, 1, nthCube, MPI_STATUS_IGNORE );

	  // Receive the duplicate count ( 0 or 1 )
	  MPI_Recv( &n_resamp, 1, MPI_SHORT, pair_rank, 3, nthCube, MPI_STATUS_IGNORE );

	  w_pair[ 1 ] = my_normal_weight;

	}

	// -----------------------------------------------------------------------------
	MPI_Barrier( MPI_COMM_WORLD );
	comm_time2 += MPI_Wtime() - comm_start;
	comm_start = MPI_Wtime();

	// Balance the resampled particle counts
	if ( n_resamp > 1 ) {

	  // Send the surplus particles, i.e. the last n_resamp - N of the
	  // resampled particles
	  MPI_Send( x, N * state_dim , MPI_DOUBLE, pair_rank, 2, nthCube );

	} else if ( n_resamp < 1 ) {

	  // Receive compensation for shortage
	  MPI_Recv( x,  N * state_dim, MPI_DOUBLE, pair_rank, 2, nthCube,
		    MPI_STATUS_IGNORE );

	}

	my_normal_weight = ( w_pair[ 0 ] + w_pair[ 1 ] ) / (double ) 2;
      }

    }

    // -----------------------------------------------------------------------------
    MPI_Barrier( MPI_COMM_WORLD );
    comm_time += MPI_Wtime() - comm_start;
    timer_start = MPI_Wtime();

    mutation(N, x, state_dim, rgen);

    // -----------------------------------------------------------------------------
    MPI_Barrier( MPI_COMM_WORLD );
    mutation_time += MPI_Wtime() - timer_start ;

  }

  struct timespec end_time;
  clock_gettime(CLOCK_REALTIME, &end_time);

  createOutput( ALG, world_rank, output, data_length, world_size,
		run_idx, N, state_dim, start_time, end_time, comm_time, rsamp_time,
		comm_time2, weight_time,  mutation_time, init_time,
		(int)( atof( argv[ 4 ] ) * 100 ) );

  // Finalize the MPI environment.
  MPI_Finalize();

  return 0;
}


// ********************  Functions for segSiteHMM

#include "stdio.h"
#include "math.h"
#include "segSites.h"
#include "../hmm/popGenTools.h"
#include "../hmm/numerical.h"
#include "assert.h"
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_sf.h>

//core function for probs
double my_h(double q, void * p){
  struct my_f_params * params = (struct my_f_params *) p;
  double beta = params->beta;
  int i = params->i;
  int n = params->n;

   return ((1.0 - exp(-2.0 * beta * (1.0 - q))) / (1.0 - exp(-2.0 * beta))) \
     * (2.0 / (q * (1.0 - q))) * gsl_ran_binomial_pdf(i, q, n);
}

/* integral of f over allele freq -- returns Prob(Segsite | mu, beta) */
double my_H(gsl_vector *pVector, void * p){
  struct my_F_params * params = (struct my_F_params *) p;
  struct my_f_params fParams;
  gsl_function f;
  gsl_integration_workspace * w;
  double result,error;

  if (gsl_vector_get(pVector, 0) == 0){
    return gsl_vector_get(pVector, 1) / params->i;
  }
  else{
    w = gsl_integration_workspace_alloc (100000);
    fParams.beta = gsl_vector_get(pVector, 0);
    fParams.i = params->i;
    fParams.n = params->n;
    f.function = &(my_h);
    f.params = &fParams;
    gsl_integration_qags(&f,0.0,1.0,1e-5,1e-5, 100000,w, &result,&error);
    gsl_integration_workspace_free(w);
    return result * gsl_vector_get(pVector,1);
  }
}

/* siteProbMatrix- fills up a matrix of log siteProbs. matrix is maxSampleSize+1 by maxSampleSize. rows represent
   variable sampleSizes (2 to max), columns represent freqs (0 to max -1), freq 0 is for monomorphic sites */
void siteProbMatrix(gsl_matrix *probs, gsl_vector *pVector,  int maxSampleSize, gsl_vector *sampleSizeVector){
  struct my_snpProb_params FParams;
  int i, j;
  double tot, prob;

  //check & init probs
  assert(probs->size1 == maxSampleSize + 1 && probs->size2 == maxSampleSize);
  gsl_matrix_set_zero(probs);

  //go through sampleSizes
  for(i = 2; i <= maxSampleSize; i++){
    //have sample size i?
    if (gsl_vector_get(sampleSizeVector, i)){
      //set tot to zero for p{monomorphic}
      tot = 0;
      //go through freqs
      for(j = 1; j < maxSampleSize; j++){
	//calc prob
	prob = 0;
	FParams.i = j;
	FParams.n = i;
	//need to impose constraint that p{SegSite = 0 | theta < 0}
	if(gsl_vector_get(pVector,1) < 0){
	  prob = 0;
	}
	else{
	  prob = my_H(pVector, &FParams);
	}
	gsl_matrix_set(probs, i, j, log(prob));
	tot += prob;
      }
      //set p{monomorphic}
      gsl_matrix_set(probs,i, 0, log(1.0 - tot));
    }
  }
}

double weightedLikLookSites(gsl_vector *pVector, void * p){
  struct my_jointLik_params * params = (struct my_jointLik_params *) p;
  gsl_matrix *probs;
  double lik;
  int i;

  //make prob matrix (log space)
  probs = gsl_matrix_alloc(params->maxSampleSize + 1, params->maxSampleSize);
  siteProbMatrix(probs, pVector,  params->maxSampleSize, params->sampleSizeVector);

  //tally likelihood
  lik = 0;
  for(i = 0; i < params->snpNumber; i++){
    lik += gsl_matrix_get(probs,params->data[i].n,params->data[i].i)  * gsl_vector_get(params->weights,i);
  }
  gsl_matrix_free(probs);
  return -lik;
}
//  weightedLikLookSites2State -- for use in 2 state HMM
double weightedLikLookSites2State(gsl_vector *pVector, void * p){
  struct my_jointLik_params * params = (struct my_jointLik_params *) p;
  gsl_matrix *probs, *probsN;
  gsl_vector *npVector;
  double lik;
  int i;

  //setup neutral params
  npVector = gsl_vector_alloc(2);
  gsl_vector_set(npVector,0,0);
  gsl_vector_set(npVector,1,gsl_vector_get(pVector,1));

  //alloc & make probs and probsN matrix (log space)
  probs = gsl_matrix_alloc(params->maxSampleSize + 1, params->maxSampleSize);
  siteProbMatrix(probs, pVector,  params->maxSampleSize, params->sampleSizeVector);
  probsN = gsl_matrix_alloc(params->maxSampleSize + 1, params->maxSampleSize);
  siteProbMatrix(probsN,npVector,  params->maxSampleSize, params->sampleSizeVector);

  //tally likelihood
  lik = 0;
  for(i = 0; i < params->snpNumber; i++){
    lik += gsl_matrix_get(probs,params->data[i].n,params->data[i].i)  * gsl_vector_get(params->weights,i);
    lik += gsl_matrix_get(probsN,params->data[i].n,params->data[i].i)  * (1.0 - gsl_vector_get(params->weights,i));
  }
  gsl_matrix_free(probs);
  gsl_matrix_free(probsN);
  gsl_vector_free(npVector);
  return -lik;
}

gsl_vector *jointMLEst(double *lik, void * p){
  size_t np = 2;
  struct my_jointLik_params * params = (struct my_jointLik_params *) p;
  const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
  gsl_multimin_fminimizer *s = NULL;
  gsl_multimin_function minex_func;
  gsl_vector *ss, *x, *results;

  size_t iter = 0, i;
  int status;
  double size;

  /* Initial vertex size vector */
  ss = gsl_vector_alloc (np);
  //initialize results vector
  results = gsl_vector_alloc (np);
  /* Set all step sizes to 1 */
  gsl_vector_set_all (ss, 0.01);

  /* Starting point */
  x = gsl_vector_alloc (np);
  gsl_vector_set (x, 0, gsl_vector_get(params->startingPoint,0));
  gsl_vector_set (x, 1, gsl_vector_get(params->startingPoint,1));

  /* Initialize method and iterate */
  minex_func.f = &weightedLikLookSites2State;
  minex_func.n = np;
  minex_func.params = params;

  s = gsl_multimin_fminimizer_alloc (T, np);
  gsl_multimin_fminimizer_set (s, &minex_func, x, ss);

  do
    {
      iter++;
      status = gsl_multimin_fminimizer_iterate(s);
      if (status)
	break;
      size = gsl_multimin_fminimizer_size (s);
      status = gsl_multimin_test_size (size, 1e-3);
    }
  while (status == GSL_CONTINUE && iter < 500);
  for (i = 0; i < np; i++){
    gsl_vector_set(results,i,gsl_vector_get (s->x, i));
  }
  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free (s);
  return(results);
}

//  weightedLikLookSitesNState -- for use in n state HMM
// pVector now consists of n parameters, not just two
// pVector = [theta_hat, beta_hat_1, beta_hat_2,
double weightedLikLookSitesNState(gsl_vector *pVector, void * p){
  struct my_jointLik_params * params = (struct my_jointLik_params *) p;
  gsl_matrix *probs;
  gsl_vector *currentVector;
  double lik;
  int i,j;

  //setup neutral params
  currentVector = gsl_vector_alloc(params->nstates);
  gsl_vector_set(currentVector,0,0);
  gsl_vector_set(currentVector,1,gsl_vector_get(pVector,0));

  //make prob matrix (log space)
  probs = gsl_matrix_alloc(params->maxSampleSize + 1, params->maxSampleSize);
  siteProbMatrix(probs, currentVector,  params->maxSampleSize, params->sampleSizeVector);

  //tally likelihood due to neutral
  lik = 0;
  for(i = 0; i < params->snpNumber; i++){
    lik += gsl_matrix_get(probs,params->data[i].n,params->data[i].i)  * gsl_matrix_get(params->posts,0,i);
  }
  //go through states tallying lik
  for (i = 1; i < params->nstates; i++){
    gsl_vector_set(currentVector,0,gsl_vector_get(pVector,i));
    gsl_vector_set(currentVector,1,gsl_vector_get(pVector,0));
    siteProbMatrix(probs,currentVector,  params->maxSampleSize, params->sampleSizeVector);
    for(j = 0; j < params->snpNumber; j++){
      lik += gsl_matrix_get(probs,params->data[j].n,params->data[j].i)  * gsl_matrix_get(params->posts,i,j);
    }
  }
  gsl_matrix_free(probs);
  gsl_vector_free(currentVector);
  return -lik;
}

//N state version
void jointMLEstNState(gsl_vector *results, double *lik, void * p){
  struct my_jointLik_params * params = (struct my_jointLik_params *) p;
  size_t np = params->nstates;
  const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
  gsl_multimin_fminimizer *s = NULL;
  gsl_multimin_function minex_func;
  gsl_vector *ss, *x;

  size_t iter = 0, i;
  int status,j;
  double size;

  //check results vector
  assert(results->size == np);

  /* Initial vertex size vector */
  ss = gsl_vector_alloc (np);

  /* Set all step sizes to 1 */
  gsl_vector_set_all (ss, 0.01);

  /* Starting point */
  x = gsl_vector_alloc (np);
  for(j = 0; j < np; j++){
    gsl_vector_set (x, j, gsl_vector_get(params->startingPoint,j));
  }

  /* Initialize method and iterate */
  minex_func.f = &weightedLikLookSitesNState;
  minex_func.n = np;
  minex_func.params = params;

  s = gsl_multimin_fminimizer_alloc (T, np);
  gsl_multimin_fminimizer_set (s, &minex_func, x, ss);

  do
    {
      iter++;
      status = gsl_multimin_fminimizer_iterate(s);
      if (status)
	break;
      size = gsl_multimin_fminimizer_size (s);
      status = gsl_multimin_test_size (size, 1e-5);
    }
  while (status == GSL_CONTINUE && iter < 500);
  for (i = 0; i < np; i++){
    gsl_vector_set(results,i,gsl_vector_get (s->x, i));
  }
  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free (s);
}


#pragma once

#include <initializer_list>
#include <list>
#include <vector>

#include <gsl/gsl_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_vector.h>

#include "../Utils/Types.h"
#include "Expression.h"

#define UNIQUE_GSL(M)                                           \
    typedef std::unique_ptr<M, decltype(&M##_free)> unique_##M; \
    inline unique_##M make_##M(size_t rows, size_t cols){       \
        return unique_##M(M##_alloc(rows, cols), M##_free);     \
    }                                                           \
    unique_##M to_##M(expression e);

UNIQUE_GSL(gsl_matrix)
UNIQUE_GSL(gsl_matrix_complex)

#undef UNIQUE_GSL

#define UNIQUE_GSL(M)                                           \
    typedef std::unique_ptr<M, decltype(&M##_free)> unique_##M; \
    inline unique_##M make_##M(size_t size){                    \
        return unique_##M(M##_alloc(size), M##_free);           \
    }                                                           \
    unique_##M to_##M(expression e);

UNIQUE_GSL(gsl_permutation)
UNIQUE_GSL(gsl_vector)
UNIQUE_GSL(gsl_vector_complex)

#undef UNIQUE_GSL

class MatrixExpression: public Expression {
    std::vector<expression> mat;
    size_t rows, cols;

    MatrixExpression();
    MatrixExpression(size_t numRows, size_t numCols);
    MatrixExpression(std::vector<expression>&& matrix, size_t numRows, size_t numCols);
    MatrixExpression(std::list<expression>& matrix, size_t numRows, size_t numCols);
    MatrixExpression(std::initializer_list<double> matrix, size_t numRows, size_t numCols);
    MatrixExpression(std::initializer_list<gsl_complex> matrix, size_t numRows, size_t numCols);
    MatrixExpression(unique_gsl_matrix& matrix);
    MatrixExpression(unique_gsl_matrix_complex& matrix);
    MatrixExpression(unique_gsl_permutation& permutation);
    MatrixExpression(unique_gsl_vector& vec);
    MatrixExpression(unique_gsl_vector_complex& vec);

    public:

        static expression construct();
        static expression construct(size_t numRows, size_t numCols);
        static expression construct(std::vector<expression>&& matrix, size_t numRows, size_t numCols);
        static expression construct(std::list<expression>& matrix, size_t numRows, size_t numCols);
        static expression construct(std::initializer_list<double> matrix, size_t numRows, size_t numCols);
        static expression construct(std::initializer_list<gsl_complex> matrix, size_t numRows, size_t numCols);
        static expression construct(unique_gsl_matrix& matrix);
        static expression construct(unique_gsl_matrix_complex& matrix);
        static expression construct(unique_gsl_permutation& matrix);
        static expression construct(unique_gsl_vector& vec);
        static expression construct(unique_gsl_vector_complex& vec);

        expression simplify() override;
        expression derivative(const std::string& var) override;
        expression integrate(const std::string& var) override;

        expression at(const int index) override;
        size_t shape(const int axis) const override;
        size_t size() const override;

        expression apply(TransformerFunction f) override;

        EXPRESSION_OVERRIDES
};


/* specfunc/hyperg_2F0.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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 2 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.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_hyperg.h>

#include "error.h"
#include "hyperg.h"

int
gsl_sf_hyperg_2F0_e(const double a, const double b, const double x, gsl_sf_result * result)
{
  if(x < 0.0) {
    /* Use "definition" 2F0(a,b,x) = (-1/x)^a U(a,1+a-b,-1/x).
     */
    gsl_sf_result U;
    double pre = pow(-1.0/x, a);
    int stat_U = gsl_sf_hyperg_U_e(a, 1.0+a-b, -1.0/x, &U);
    result->val = pre * U.val;
    result->err = GSL_DBL_EPSILON * fabs(result->val) + pre * U.err;
    return stat_U;
  }
  else if(x == 0.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    /* Use asymptotic series. ??
     */
    /* return hyperg_2F0_series(a, b, x, -1, result, &prec); */
    DOMAIN_ERROR(result);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_hyperg_2F0(const double a, const double b, const double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F0_e(a, b, x, &result));
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mersenne.h"
//#include <lapacke/lapacke.h>
#include <stdbool.h>
#include <lapacke.h>
#include <time.h>
#include <sys/time.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846264338327
#endif

/* Lattice size and dimensions */
#define ND 4
/* Size of the lattice dimensions */
static int Ldim[ND] = { 4, 4, 4, 4 };

#define NDIRS (2*ND)
int VOLUME;

// Choose update method
#define WORM_UPDATE
//#define LOCAL_UPDATE

//Choose spatial boundary conditions (temporal alway antiperiodic)
//#define PERIODIC
#define ANTIPERIODIC

//Uncomment to include site mass in the determinant
//(does not work with fluctuation determinant)
//#define MASS_IN_MATRIX

/* Flavors */
#define N_FLAVOR 2

/* Choose here the method for calculating the derivative */
//#define FULL_DETERMINANT
#define FLUCTUATION_DETERMINANT

/* Toggle direct measurement for site and link vev */
//#define MEASUREVEV

/* Save memory to fit large lattices
 * Reduces memory use significantly and
 * slightly increases compute times
 */
#define SAVE_MEMORY

/* field values */
#define UNOCCUPIED 0
#define OCCUPIED 1
#define NONE -100   //A meta value for sites that don't exist

/* In Staggered.c */
int site_vector_to_index( int vector[] );
void site_index_to_vector(int index, int vector[] );
void print_config();

/* In fermion_matrix.c */
void Dv( double * y, double * x );
void calc_Dinv( );
double determinant();
void * wrapmalloc( int size, size_t bitsize );

double det_add_monomers(int x1, int x2, int do_flavor[N_FLAVOR]);
double det_move_monomers(int x1, int x2, int do_flavor[N_FLAVOR]);
double det_remove_monomers(int x1, int x2, int do_flavor[N_FLAVOR]);
#ifdef MASS_IN_MATRIX
double det_add_monomers1(int x1, int do_flavor[N_FLAVOR]);
double det_remove_monomers1(int x1, int do_flavor[N_FLAVOR]);
#endif
void update_current_determinant( int do_flavor[N_FLAVOR] );
void update_background( int monomer );

void measure_vev( double * linkvev, double * sitevev );

/* update functions */
double worm_update( int *additions, int *removals, int *m_additions, int *m_removals, int *switches );
void local_update( int *additions, int *removals, int *moves,int *m_additions, int *m_removals, int *m_moves, int *switches );

/* Inversions */
void init_Dinv_cg();
void init_Dinv_deprecated();
void cg( double *source );


/*==========================================================
 * pca.c - Principal component analysis of diffraction data
 *
 * Calculates the two main components of the data
 * by Principal Component Analysis, as detailed in
 * Bernhardt et.al (2016), Biophys. J.
 *
 * The calling syntax is:
 *
 *		pca(data, mask, qx, qy, w, phi, m, n)
 *
 * Copyright 2017 Institute for X-ray Physics (University of Göttingen)
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
 * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 *========================================================*/

#include <math.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>

#define PI 3.14159265

/* The computational routine */
void symmetry(double *x, double *bgr, double sigma, double noise_level, int m, int n)
{
    int delta_theta = 20; /* hard coded theta window size */

    int col;
    int row;

    /* calculate snr */
    for (row=0; row<m; row++) {
        for (col=0; col<n; col++) {
            if (bgr[row*m + col] == 0){
                x[row*m + col] = 0;
            } else {
                x[row*m + col] = (x[row*m + col] - bgr[row*m + col]) / sqrt(bgr[row*m + col]);
        }
    }

    /* next we implement
% Gaussian filter (remove noise)
h = fspecial('gaussian', [10 10], sigma);
h = h./sum(h(:));
snr = conv2(snr,h,'same');

% binary image (snr threshold)
binary_image = (snr >= noise_level);
        */

    /* prepare fft */
    work = gsl_fft_real_workspace_alloc (n);
    real = gsl_fft_real_wavetable_alloc (n);

    /* fourier transform the rows */
    for (int row = 0; row < m; row++){
        double* data = x[row*m];
        gsl_fft_real_transform (data, 1, n, real, work);
        gsl_fft_real_wavetable_free (real);
        /* apply filter HERE */
        hc = gsl_fft_halfcomplex_wavetable_alloc (n);
        gsl_fft_halfcomplex_inverse (data, 1, n, hc, work);
        gsl_fft_halfcomplex_wavetable_free (hc);
        gsl_fft_real_workspace_free (work);
    }
    /* fourier transform the rows */

	/* apply gauss filter */

	/* fourier-backtransform the rows */
    /* fourier-backtransform the cols */

	/* apply threshold */
    for (row=0; row<m; row++) {
        for (col=0; col<n; col++) {
            if (x[row*m + col] > noise_level){
                x[row*m + col] = 1;
            } else {
                x[row*m + col] = 0;
        }
    }

}


/* file: del2mat.h */

#ifndef DEL2MAT_H
#define DEL2MAT_H

#include <petsc.h>
#include <petscvec.h>
#include <petscmat.h>

/* external Fortran 90 subroutine */
#define Del2Apply del2apply_
extern void Del2Apply(int*,double*,const double*,double*);

/* user data structure and routines
 * defining the matrix-free operator */

typedef struct {
  PetscInt    N;
  PetscScalar *F;
} Del2Mat;

#undef  __FUNCT__
#define __FUNCT__ "Del2Mat_mult"
/* y <- A * x */
PetscErrorCode Del2Mat_mult(Mat A, Vec x, Vec y)
{
  Del2Mat *ctx;
  const PetscScalar *xx;
  PetscScalar *yy;
  PetscErrorCode ierr;
  PetscFunctionBegin;
  ierr = MatShellGetContext(A,(void**)&ctx);CHKERRQ(ierr);
  /* get raw vector arrays */
  ierr = VecGetArrayRead(x,&xx);CHKERRQ(ierr);
  ierr = VecGetArray(y,&yy);CHKERRQ(ierr);
  /* call external Fortran subroutine */
  Del2Apply(&ctx->N,ctx->F,xx,yy);
  /* restore raw vector arrays */
  ierr = VecRestoreArrayRead(x,&xx);CHKERRQ(ierr);
  ierr = VecRestoreArray(y,&yy);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}
#undef  __FUNCT__
#define __FUNCT__ "Del2Mat_diag"
/*D_i <- A_ii */
PetscErrorCode Del2Mat_diag(Mat A, Vec D)
{
  PetscErrorCode ierr;
  PetscFunctionBegin;
  ierr = VecSet(D,6.0);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#endif


/*
** centrality maps using spectral coherence
**
** get cross-periodogram via multiplication of FFTs
**
** Ref: T. Subba Rao,
** On the Cross Periodogram of a Stationary Gaussian Vector Process,
** The Annals of Mathematical Statistics, Vol. 38, No. 2 (Apr., 1967),
**  pp. 593-597.  (2239172.pdf)
**
** Implementation follows SAS, see
**  http://www.tau.ac.il/cc/pages/docs/sas8/ets/chap17/
**
** G.Lohmann, September 2009
*/
#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>
#include <viaio/option.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_math.h>

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

#define PI 3.14159265

#define SQR(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

/* re-implementation of cblas_sspmv,  cblas_sspmv causes problems */
void my_sspmv(float *A,float *x,float *y,int n)
{
  size_t i,j,k,kk;
  float tmp1=0,tmp2=0;

  for (j=0; j<n; j++) y[j] = 0;
  kk = k = 0;
  for (j=0; j<n; j++) {
    tmp1 = x[j];
    tmp2 = 0;
    k = kk;
    for (i=0; i<j; i++) {
      y[i] += tmp1 * A[k];
      tmp2 += A[k] * x[i];
      k++;
    }
    y[j] += tmp1*A[kk+j] + tmp2;
    kk += (j+1);
  }
}

double Normalize(double *data,int n)
{
  int i;
  double sum,nx=(double)n;

  sum = 0;
  for (i=0; i<n; i++) sum += data[i];
  sum /= nx;

  for (i=0; i<n; i++) data[i] -= sum;
  return sum;
}

double Tukey(double x,double wx)
{
  if (ABS(x) >= wx) return 0;
  else return 0.5*(1.0 + cos((PI*x)/wx));
}

int GetFFtIndex(int n,double tr,double wavelength)
{
  int k0;
  double nx,cycle;

  nx = (double)n;
  cycle = wavelength / tr;
  k0 = (int)(nx/cycle + 0.5);
  return k0;
}

double *Weights(int n,int k0,int p)
{
  int k;
  double *w=NULL;
  double kx,wx,sum;

  w  = (double *) VCalloc(n,sizeof(double));
  wx = (double) p;
  kx = 0;
  for (k=0; k<p; k++) {
    w[k] = Tukey(kx,wx);
    kx++;
  }
  sum = 0;
  for (k=0; k<p; k++) sum += w[k];
  sum *= (2.0*PI);
  for (k=0; k<p; k++) w[k] /= sum;
  return w;
}

void Tables(gsl_matrix *tablecos,gsl_matrix *tablesin)
{
  int i,k,n;
  double kx,ix,nx,w;

  n = tablecos->size1;
  nx = (double)n;
  kx = 0;
  for (k=0; k<n; k++) {
    w = 2.0*PI*kx/nx;
    ix = 0;
    for (i=1; i<n; i++) {
      gsl_matrix_set(tablecos,k,i,cos(w*ix));
      gsl_matrix_set(tablesin,k,i,sin(w*ix));
      ix++;
    }
    kx++;
  }
}

void FFT(double *in,double *xcos,double *xsin,gsl_matrix *tablecos,gsl_matrix *tablesin)
{
  int i,k,n;
  double sum1,sum2,kx,ix,nx;

  n  = tablecos->size1;
  nx = 1;
  kx = 0;
  for (k=0; k<n; k++) {
    if (k >= tablecos->size1) VError(" k= %d",k);
    if (k >= tablesin->size1) VError(" k= %d",k);
    sum1 = sum2 = 0;
    /* w = 2.0*PI*kx/nx; */
    ix = 0;
    for (i=0; i<n; i++) {
      sum1 += in[i]*gsl_matrix_get(tablecos,k,i);
      sum2 += in[i]*gsl_matrix_get(tablesin,k,i);
      ix++;
    }
    xcos[k] = 2.0*sum1/nx;
    xsin[k] = 2.0*sum2/nx;
    kx++;
  }
}

void DegreeCentrality(float *A,float *ev,size_t n)
{
  size_t i,j,k;
  double sum=0,nx=(double)n;
  fprintf(stderr," degree centrality, n= %lu\n",n);

  for (i=0; i<n; i++) {
    sum=0;
    for (j=0; j<n; j++) {
      if (i==j) continue;
      if (j < i) k=j+i*(i+1)/2;
      else k=i+j*(j+1)/2;
      sum += A[k];
    }
    ev[i] = (float)(100.0*sum/nx);
  }
}

void EigenvectorCentrality(float *A,float *ev,size_t n)
{
  size_t i,iter,maxiter;
  float sum,d,nx;
  static float *y=NULL;

  fprintf(stderr," power iteration, n= %lu\n",n);
  if (y==NULL) y = (float *) VCalloc(n,sizeof(float));
  if (!y) VError(" err allocating vector ");
  nx = sqrt((double)n);
  for (i=0; i<n; i++) ev[i] = y[i] = 1.0/nx;

  maxiter=50;
  for (iter=0; iter < maxiter; iter++) {

    /* y = Ax,  A symmetric and lower-triangular */
    /* cblas_sspmv(CblasRowMajor,CblasLower,(int)n,1.0f,A,ev,1,1.0f,y,1); */
    my_sspmv(A,ev,y,n);

    sum = 0;
    for (i=0; i<n; i++) sum += y[i]*y[i];
    sum = sqrt(sum);

    d = 0;
    for (i=0; i<n; i++) {
      d += SQR(ev[i] - y[i]/sum);
      ev[i] = y[i]/sum;
    }
    fprintf(stderr," %5lu  %f\n",iter,d);
    if (d < 1.0e-6) break;
  }
  for (i=0; i<n; i++) ev[i] *= 100.0;
  fprintf(stderr," ecm done..\n");
}

VImage WriteOutput(VImage src,VImage map,int nslices,int nrows, int ncols, float *ev, size_t n)
{
  size_t i,b,r,c;

  VImage dest = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  VFillImage(dest,VAllBands,0);
  VCopyImageAttrs (src, dest);

  for (i=0; i<n; i++) {
    b = VPixel(map,0,0,i,VInteger);
    r = VPixel(map,0,1,i,VInteger);
    c = VPixel(map,0,2,i,VInteger);
    /* if (ev[i] > 0) fprintf(stderr," %6d    %3d %3d %3d  %f\n",i,c,r,b,ev[i]); */
    VPixel(dest,b,r,c,VFloat) = ev[i];
  }
  return dest;
}

VAttrList VSpectralECM(VAttrList list,VImage mask,VDouble wavelength,VShort nlags,
		       VShort first,VShort length,VShort type)
{
  int b,r,c,j,k,kk,k0,n,p;
  size_t i;
  size_t m,nvox;
  int last;
  gsl_matrix *matcos=NULL,*matsin=NULL;
  float *A=NULL,*ev=NULL;
  float tr=-1,xtr=-1;
  double *in,*xsin=NULL,*xcos=NULL,*w=NULL;
  double nx,u,sumx,freq=0;
  gsl_matrix *tablecos=NULL,*tablesin=NULL;
  double pi = 2.0*acos(0);

  /* get image dimensions */
  int nrows=0,ncols=0,nt=0;
  int nslices = VAttrListNumImages(list);
  VImage *src = VAttrListGetImages(list,nslices);
  VImageDimensions(src,nslices,&nt,&nrows,&ncols);
  size_t ntimesteps = nt;

  if (VImageNRows(mask) != nrows)
    VError(" inconsistent image dims, mask has %d rows, image has %d rows",VImageNRows(mask),nrows);
  if (VImageNColumns(mask) != ncols)
    VError(" inconsistent image dims, mask has %d columns, image has %d columns",VImageNColumns(mask),ncols);
  if (VImageNBands(mask) != nslices)
    VError(" inconsistent image dims, mask has %d slices, image has %d slices",VImageNBands(mask),nslices);

  /* get time steps to include */
  if (first < 0) first = 0;
  if (length <= 0) length = ntimesteps;
  last = first + length -1;
  if (last >= ntimesteps) last = ntimesteps-1;

  n = last - first + 1;
  if (n < 2) VError(" not enough timesteps, nt= %d",n);
  nx = (double)n;

  /*
  ** get freq range
  */
  if (VGetAttr (VImageAttrList (src[0]), "repetition_time", NULL,
		VFloatRepn, (VPointer) & xtr) == VAttrFound) {
    tr = (xtr/1000.0);
  }
  if (tr <= 0) VError(" illegal tr: %f",tr);

  k0   = GetFFtIndex((int)n,tr,wavelength);
  freq = 1.0/wavelength;
  fprintf(stderr," TR= %g secs,  wavelength= %g secs,  freq= %.3f Hz,  k0= %d\n",
	  tr,wavelength,freq,k0);
  if (k0 < 1 || k0 >= n/2) VError(" illegal freq, index must be in [1,%d]\n",n/2);

  /* weights */
  if (nlags < 0)
    p = n/15;   /* default */
  else
    p = (int)nlags;
  if (p < 3) VError(" nlags too small, %d",p);
  w = Weights((int)n,k0,p);

  /* count number of voxels */
  nvox = 0;
  for (b=0; b<nslices; b++) {
    if (VImageNRows(src[b]) < 2) continue;
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	if (fabs(VGetPixel(mask,b,r,c)) < TINY) continue;
	nvox++;
      }
    }
  }
  fprintf(stderr," nvoxels: %ld,  n= %d,  nlags= %d\n",(long)nvox, (int)n, (int)p);

  /*
  ** voxel maps
  */
  VImage map = VCreateImage(1,4,nvox,VIntegerRepn);
  if (map == NULL) VError(" error allocating addr map");
  VFillImage(map,VAllBands,0);
  VPixel(map,0,3,0,VInteger) = nslices;
  VPixel(map,0,3,1,VInteger) = nrows;
  VPixel(map,0,3,2,VInteger) = ncols;

  double *xmap = (double *)VCalloc(nvox,sizeof(double));

  i = 0;
  for (b=0; b<nslices; b++) {
    if (VImageNRows(src[b]) < 2) continue;
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	if (fabs(VGetPixel(mask,b,r,c)) < TINY) continue;
	VPixel(map,0,0,i,VInteger) = b;
        VPixel(map,0,1,i,VInteger) = r;
        VPixel(map,0,2,i,VInteger) = c;
	i++;
      }
    }
  }

  /* alloc memory */
  in   = (double *)VCalloc(n,sizeof(double));
  xsin = (double *)VCalloc(n,sizeof(double));
  xcos = (double *)VCalloc(n,sizeof(double));
  tablecos = gsl_matrix_calloc((int)n,(int)n);
  if (tablecos == NULL) VError(" err alloc cos");
  tablesin = gsl_matrix_calloc((int)n,(int)n);
  if (tablesin == NULL) VError(" err alloc sin");
  Tables(tablecos,tablesin);

  /*
  ** precompute FFT
  */
  matcos = gsl_matrix_calloc(nvox,(int)n);
  if (!matcos) VError(" err alloc matcos");
  matsin = gsl_matrix_calloc(nvox,(int)n);
  if (!matsin) VError(" err alloc matsin");
  Tables(tablecos,tablesin);

  for (i=0; i<nvox; i++) {

    b = VPixel(map,0,0,i,VInteger);
    r = VPixel(map,0,1,i,VInteger);
    c = VPixel(map,0,2,i,VInteger);

    j = 0;
    for (k=first; k<=last; k++) {
      in[j++] = (double) VGetPixel(src[b],k-first,r,c);
    }
    double q = Normalize(in,n);
    if (fabs(q) < TINY) continue;

    FFT(in,xcos,xsin,tablecos,tablesin);
    nx = (double)n;

    sumx = 0;
    for (k=-p; k<=p; k++) {
      kk = k+k0;
      if (kk < 0) kk = -k;
      if (kk >= n) kk = n-k;
      u = 2.0*(xcos[kk]*xcos[kk] + xsin[kk]*xsin[kk])/nx;
      sumx += w[ABS(k)]*u;
    }
    xmap[i] = sumx;

    for (k=0; k<n; k++) {
      gsl_matrix_set(matcos,i,k,xcos[k]);
      gsl_matrix_set(matsin,i,k,xsin[k]);
    }
  }

  /*
  ** compute spectral coherence matrix
  */
  m = (nvox*(nvox+1))/2;
  fprintf(stderr," matrix computation, n= %ld...\n",(long)nvox);
  A = (float *) VCalloc(m,sizeof(float));
  if (!A) VError(" err allocating matrix");
  memset(A,0,m*sizeof(float));
   size_t progress=0;

#pragma omp parallel for shared(progress) schedule(guided) firstprivate(A,xmap)
  for (i=0; i<nvox; i++) {
    if (i%1000 == 0) fprintf(stderr," %d000  of  %ld\r",(int)(++progress),(long)nvox);

    size_t j=0,kij=0;
    int k=0,kk=0;
    double v=0,x=0,y=0,cs=0,qs=0,c1,s1,c2,s2,re,im;

    x = xmap[i];
    for (j=0; j<i; j++) {

      y = xmap[j];

      cs = qs = 0;
      for (k=-p; k<=p; k++) {

	kk = k+k0;
	if (kk  < 0) kk = -k;
	if (kk >= n) kk = n-k;

	c1 = gsl_matrix_get(matcos,i,kk);
	s1 = gsl_matrix_get(matsin,i,kk);

	c2 = gsl_matrix_get(matcos,j,kk);
	s2 = gsl_matrix_get(matsin,j,kk);

	re = 2.0*(c1*c2 + s1*s2)/nx;
	im = 2.0*(c1*s2 - s1*c2)/nx;

	cs += w[ABS(k)] * re;
	qs += w[ABS(k)] * im;
      }

      if (type == 0) {  /* spectral coherence */
	if (x*y > TINY) {
	  v = (cs*cs + qs*qs) / (x*y);
	  v = sqrt(v);
	}
	else v = -1;
      }
      else {            /* phase sync */
	v = atan2(qs,cs)/pi;
	/* v = 1.0 - ABS(v); */
	v = cos(v);
      }
      if (v < TINY) v = TINY;  /* make matrix irreducible */

      kij=j+i*(i+1)/2;
      A[kij] = v;
    }
  }

  /* free space */
  gsl_matrix_free(matcos);
  gsl_matrix_free(matsin);

  /*
  ** eigenvector centrality
  */
  ev = (float *) VCalloc(nvox,sizeof(float));
  EigenvectorCentrality(A,ev,nvox);
  /* DegreeCentrality(A,ev,nvox); */
  VImage dest0 = WriteOutput(src[0],map,nslices,nrows,ncols,ev,nvox);
  VSetAttr(VImageAttrList(dest0),"name",NULL,VStringRepn,"eigenvector_centrality");
  VAttrList out_list = VCreateAttrList();
  VAppendAttr(out_list,"image",NULL,VImageRepn,dest0);
  return out_list;
}

VDictEntry TYPDict[] = {
  { "freq", 0 },
  { "sync", 1 },
  { NULL }
};

int main (int argc,char *argv[])
{
  static VDouble  wavelength = 10;
  static VShort   nlags      = 10;
  static VShort   first      = 0;
  static VShort   length     = 0;
  static VShort   type       = 0;
  static VString  mask_filename   = "";
  static VShort   nproc  = 0;
  static VOptionDescRec  options[] = {
    {"wavelength", VDoubleRepn,1,(VPointer) &wavelength,VRequiredOpt,NULL,"wavelength in secs"},
    {"first",VShortRepn,1,(VPointer) &first,VOptionalOpt,NULL,"first timestep to use"},
    {"length",VShortRepn,1,(VPointer) &length,VOptionalOpt,NULL,"length of time series to use"},
    {"mask",VStringRepn,1,(VPointer) &mask_filename,VRequiredOpt,NULL,"mask"},
    {"nlags",VShortRepn,1,(VPointer) &nlags,VOptionalOpt,NULL,
     "num lags in cross-corr, use 0 to get default value"},
    {"type",VShortRepn,1,(VPointer) &type,VOptionalOpt,TYPDict,"frequency or phase coherence"},
    {"j",VShortRepn,1,(VPointer) &nproc,VOptionalOpt,NULL,"Number of processors to use, '0' to use all"}
  };
  FILE *in_file,*out_file;
  VString in_filename=NULL;
  char *prg = "vspectralecm";

  /* parse command line */
  VParseFilterCmdZ (VNumber (options),options,argc,argv,&in_file,&out_file,&in_filename);
  if (type > 2) VError(" illegal type");

  /* omp-stuff */
#ifdef _OPENMP
  int num_procs=omp_get_num_procs();
  if (nproc > 0 && nproc < num_procs) num_procs = nproc;
  fprintf(stderr,"using %d cores\n",(int)num_procs);
  omp_set_num_threads(num_procs);
#endif /* _OPENMP */

  /* read mask */
  VAttrList listm = VReadAttrList(mask_filename,0L,TRUE,FALSE);
  VImage mask = VReadImage(listm);
  if (mask == NULL) VError(" no ROI mask found");

  /* read functional data */
  VAttrList list = VReadAttrListZ(in_file,in_filename,0L,TRUE,FALSE);
  if (list == NULL) VError(" error reading input file %s",in_file);
  VAttrList geolist = VGetGeoInfo(list);

  /*
  ** process
  */
  VAttrList out_list = VSpectralECM(list,mask,wavelength,nlags,first,length,type);

  /* update geoinfo, 4D to 3D */
  if (geolist != NULL) {
    double *D = VGetGeoDim(geolist,NULL);
    D[0] = 3;  /* 3D */
    D[4] = 1;  /* just one timestep */
    VSetGeoDim(geolist,D);
    VSetGeoInfo(geolist,out_list);
  }

  VHistory(VNumber(options),options,prg,&list,&out_list);
  if (! VWriteFile (out_file, out_list)) exit (1);
  fprintf (stderr, "%s: done.\n", argv[0]);
  return 0;
}


/* movstat/madacc.c
 *
 * Copyright (C) 2018 Patrick Alken
 *
 * 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.
 */

/*
 * This module contains routines for computing the median absolute deviation (MAD)
 * of a moving fixed-sized window.
 */

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_statistics.h>

typedef double madacc_type_t;
typedef madacc_type_t ringbuf_type_t;

#include "ringbuf.c"

typedef struct
{
  size_t n;                        /* window size */
  const gsl_movstat_accum *medacc; /* median accumulator */
  void *median_state;              /* median accumulator workspace */
  ringbuf *rbuf;                   /* ring buffer storing current window, size n */
  madacc_type_t *work;             /* workspace, size n */
} madacc_state_t;

static size_t madacc_size(const size_t n);
static int madacc_init(const size_t n, void * vstate);
static int madacc_insert(const madacc_type_t x, void * vstate);
static int madacc_delete(void * vstate);
static int madacc_medmad(void * params, madacc_type_t * result, const void * vstate);

static size_t
madacc_size(const size_t n)
{
  size_t size = 0;
  const gsl_movstat_accum * acc = gsl_movstat_accum_median;

  size += sizeof(madacc_state_t);
  size += (acc->size)(n);            /* median accumulator */
  size += ringbuf_size(n);           /* rbuf */
  size += n * sizeof(madacc_type_t); /* work */

  return size;
}

static int
madacc_init(const size_t n, void * vstate)
{
  madacc_state_t * state = (madacc_state_t *) vstate;

  state->n = n;
  state->medacc = gsl_movstat_accum_median;

  state->median_state = (void *) ((unsigned char *) vstate + sizeof(madacc_state_t));
  state->rbuf = (ringbuf *) ((unsigned char *) state->median_state + (state->medacc->size)(n));
  state->work = (madacc_type_t *) ((unsigned char *) state->rbuf + ringbuf_size(n));

  (state->medacc->init)(n, state->median_state);
  ringbuf_init(n, state->rbuf);

  return GSL_SUCCESS;
}

static int
madacc_insert(const madacc_type_t x, void * vstate)
{
  madacc_state_t * state = (madacc_state_t *) vstate;

  /* insert element into median accumulator */
  (state->medacc->insert)(x, state->median_state);

  /* insert element into ring buffer */
  ringbuf_insert(x, state->rbuf);

  return GSL_SUCCESS;
}

static int
madacc_delete(void * vstate)
{
  madacc_state_t * state = (madacc_state_t *) vstate;

  if (!ringbuf_is_empty(state->rbuf))
    ringbuf_pop_back(state->rbuf);

  return GSL_SUCCESS;
}

static int
madacc_medmad(void * params, madacc_type_t * result, const void * vstate)
{
  const madacc_state_t * state = (const madacc_state_t *) vstate;

  if (ringbuf_is_empty(state->rbuf))
    {
      GSL_ERROR("no samples yet added to workspace", GSL_EINVAL);
    }
  else
    {
      int status;
      const double scale = *(double *) params;
      const int n = ringbuf_n(state->rbuf);
      int i;
      double median, mad;

      /* compute median of current window */
      status = (state->medacc->get)(NULL, &median, state->median_state);
      if (status)
        return status;

      /* compute current window absolute deviations from median */
      for (i = 0; i < n; ++i)
        {
          double xi = state->rbuf->array[(state->rbuf->head + i) % state->rbuf->size];
          state->work[i] = fabs(xi - median);
        }

      /* compute MAD of current window */
      mad = gsl_stats_median(state->work, 1, n);

      result[0] = median;
      result[1] = scale * mad;

      return GSL_SUCCESS;
    }
}

static const gsl_movstat_accum mad_accum_type =
{
  madacc_size,
  madacc_init,
  madacc_insert,
  NULL, /*XXX*/
  madacc_medmad
};

const gsl_movstat_accum *gsl_movstat_accum_mad = &mad_accum_type;


/* eigen/eigen_sort.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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 2 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.
 */

/* Author:  G. Jungman, Modified: B. Gough. */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>

/* The eigen_sort below is not very good, but it is simple and
 * self-contained. We can always implement an improved sort later.  */

int
gsl_eigen_symmv_sort (gsl_vector * eval, gsl_matrix * evec,
                      gsl_eigen_sort_t sort_type)
{
  if (evec->size1 != evec->size2)
    {
      GSL_ERROR ("eigenvector matrix must be square", GSL_ENOTSQR);
    }
  else if (eval->size != evec->size1)
    {
      GSL_ERROR ("eigenvalues must match eigenvector matrix", GSL_EBADLEN);
    }
  else
    {
      const size_t N = eval->size;
      size_t i;

      for (i = 0; i < N - 1; i++)
        {
          size_t j;
          size_t k = i;

          double ek = gsl_vector_get (eval, i);

          /* search for something to swap */
          for (j = i + 1; j < N; j++)
            {
              int test;
              const double ej = gsl_vector_get (eval, j);

              switch (sort_type)
                {
                case GSL_EIGEN_SORT_VAL_ASC:
                  test = (ej < ek);
                  break;
                case GSL_EIGEN_SORT_VAL_DESC:
                  test = (ej > ek);
                  break;
                case GSL_EIGEN_SORT_ABS_ASC:
                  test = (fabs (ej) < fabs (ek));
                  break;
                case GSL_EIGEN_SORT_ABS_DESC:
                  test = (fabs (ej) > fabs (ek));
                  break;
                default:
                  GSL_ERROR ("unrecognized sort type", GSL_EINVAL);
                }

              if (test)
                {
                  k = j;
                  ek = ej;
                }
            }

          if (k != i)
            {
              /* swap eigenvalues */
              gsl_vector_swap_elements (eval, i, k);

              /* swap eigenvectors */
              gsl_matrix_swap_columns (evec, i, k);
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_eigen_hermv_sort (gsl_vector * eval, gsl_matrix_complex * evec,
                      gsl_eigen_sort_t sort_type)
{
  if (evec->size1 != evec->size2)
    {
      GSL_ERROR ("eigenvector matrix must be square", GSL_ENOTSQR);
    }
  else if (eval->size != evec->size1)
    {
      GSL_ERROR ("eigenvalues must match eigenvector matrix", GSL_EBADLEN);
    }
  else
    {
      const size_t N = eval->size;
      size_t i;

      for (i = 0; i < N - 1; i++)
        {
          size_t j;
          size_t k = i;

          double ek = gsl_vector_get (eval, i);

          /* search for something to swap */
          for (j = i + 1; j < N; j++)
            {
              int test;
              const double ej = gsl_vector_get (eval, j);

              switch (sort_type)
                {
                case GSL_EIGEN_SORT_VAL_ASC:
                  test = (ej < ek);
                  break;
                case GSL_EIGEN_SORT_VAL_DESC:
                  test = (ej > ek);
                  break;
                case GSL_EIGEN_SORT_ABS_ASC:
                  test = (fabs (ej) < fabs (ek));
                  break;
                case GSL_EIGEN_SORT_ABS_DESC:
                  test = (fabs (ej) > fabs (ek));
                  break;
                default:
                  GSL_ERROR ("unrecognized sort type", GSL_EINVAL);
                }

              if (test)
                {
                  k = j;
                  ek = ej;
                }
            }

          if (k != i)
            {
              /* swap eigenvalues */
              gsl_vector_swap_elements (eval, i, k);

              /* swap eigenvectors */
              gsl_matrix_complex_swap_columns (evec, i, k);
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_eigen_nonsymmv_sort (gsl_vector_complex * eval,
                         gsl_matrix_complex * evec,
                         gsl_eigen_sort_t sort_type)
{
  if (evec->size1 != evec->size2)
    {
      GSL_ERROR ("eigenvector matrix must be square", GSL_ENOTSQR);
    }
  else if (eval->size != evec->size1)
    {
      GSL_ERROR ("eigenvalues must match eigenvector matrix", GSL_EBADLEN);
    }
  else
    {
      const size_t N = eval->size;
      size_t i;

      for (i = 0; i < N - 1; i++)
        {
          size_t j;
          size_t k = i;

          gsl_complex ek = gsl_vector_complex_get (eval, i);

          /* search for something to swap */
          for (j = i + 1; j < N; j++)
            {
              int test;
              const gsl_complex ej = gsl_vector_complex_get (eval, j);

              switch (sort_type)
                {
                case GSL_EIGEN_SORT_ABS_ASC:
                  test = (gsl_complex_abs (ej) < gsl_complex_abs (ek));
                  break;
                case GSL_EIGEN_SORT_ABS_DESC:
                  test = (gsl_complex_abs (ej) > gsl_complex_abs (ek));
                  break;
                case GSL_EIGEN_SORT_VAL_ASC:
                case GSL_EIGEN_SORT_VAL_DESC:
                default:
                  GSL_ERROR ("invalid sort type", GSL_EINVAL);
                }

              if (test)
                {
                  k = j;
                  ek = ej;
                }
            }

          if (k != i)
            {
              /* swap eigenvalues */
              gsl_vector_complex_swap_elements (eval, i, k);

              /* swap eigenvectors */
              gsl_matrix_complex_swap_columns (evec, i, k);
            }
        }

      return GSL_SUCCESS;
    }
}


/* multifit_nlinear/test.c
 *
 * Copyright (C) 2007, 2013, 2015, 2016 Brian Gough, Patrick Alken
 *
 * 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.
 */

/* These tests are based on the NIST Statistical Reference Datasets
   See http://www.nist.gov/itl/div898/strd/index.html for more
   information. */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_ieee_utils.h>

#include "test_fdf.c"

static void
test_proc(const gsl_multifit_nlinear_trs *trs,
          const gsl_multifit_nlinear_scale *scale,
          const gsl_multifit_nlinear_solver *solver,
          const int fdtype)
{
  gsl_multifit_nlinear_parameters fdf_params =
    gsl_multifit_nlinear_default_parameters();

  fdf_params.trs = trs;
  fdf_params.scale = scale;
  fdf_params.solver = solver;
  fdf_params.fdtype = fdtype;

  test_fdf_main(&fdf_params);
}

int
main (void)
{
  const gsl_multifit_nlinear_trs **nlinear_trs[6];
  const gsl_multifit_nlinear_solver **nlinear_solvers[5];
  const gsl_multifit_nlinear_scale **nlinear_scales[3];
  const gsl_multifit_nlinear_trs **trs;
  const gsl_multifit_nlinear_solver **solver;
  const gsl_multifit_nlinear_scale **scale;
  size_t i = 0;

  gsl_ieee_env_setup();

  /* initialize arrays */

  nlinear_trs[0] = &gsl_multifit_nlinear_trs_lm;
  nlinear_trs[1] = &gsl_multifit_nlinear_trs_lmaccel;
  nlinear_trs[2] = &gsl_multifit_nlinear_trs_dogleg;
  nlinear_trs[3] = &gsl_multifit_nlinear_trs_ddogleg;
  nlinear_trs[4] = &gsl_multifit_nlinear_trs_subspace2D;
  nlinear_trs[5] = NULL;

  nlinear_solvers[0] = &gsl_multifit_nlinear_solver_cholesky;
  nlinear_solvers[1] = &gsl_multifit_nlinear_solver_mcholesky;
  nlinear_solvers[2] = &gsl_multifit_nlinear_solver_qr;
  nlinear_solvers[3] = &gsl_multifit_nlinear_solver_svd;
  nlinear_solvers[4] = NULL;

  /* skip Marquardt scaling since it won't pass */
  nlinear_scales[0] = &gsl_multifit_nlinear_scale_levenberg;
  nlinear_scales[1] = &gsl_multifit_nlinear_scale_more;
  nlinear_scales[2] = NULL;

  /* run testsuite over all parameter combinations */

  for (trs = nlinear_trs[i]; trs != NULL; trs = nlinear_trs[++i])
    {
      size_t j = 0;

      fprintf(stderr, "trs = %s\n", (*trs)->name);

      for (solver = nlinear_solvers[j]; solver != NULL; solver = nlinear_solvers[++j])
        {
          size_t k = 0;

          /* don't use Cholesky solver with dogleg methods */
          if (i > 1 && *solver == gsl_multifit_nlinear_solver_cholesky)
            continue;

          fprintf(stderr, "solver = %s\n", (*solver)->name);
          for (scale = nlinear_scales[k]; scale != NULL; scale = nlinear_scales[++k])
            {
              test_proc(*trs, *scale, *solver, GSL_MULTIFIT_NLINEAR_FWDIFF);
              test_proc(*trs, *scale, *solver, GSL_MULTIFIT_NLINEAR_CTRDIFF);
            }
        }
    }

  exit (gsl_test_summary ());
}