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

#ifndef AUGUR_MCMCLIB_H
#define AUGUR_MCMCLIB_H

#include "augur_util.h"
#include "augur_math.h"
#include "augur_vecop.h"
#include "augur_blkop.h"
#include "augur_blkstk.h"
#include "augur_dist.h"
#include <augur_iface.h>
#include <math.h>
#include <gsl/gsl_rng.h>

typedef struct mcmc {
  AugurMod_t curr;
  AugurMod_t prop;
  AugurAux_t aux;
  int accept;
  double currLL;
  double propLL;
  double eps;
  double L;
  double leapLen;
  double deltaMax;
  int nutsalloc;
} mcmc_t;

mcmc_t MCMC;
gsl_rng* h_rng;

void dump_state(mcmc_t state) {
  printf("currLL: %f\n", state.currLL);
  printf("propLL: %f\n", state.propLL);
  printf("eps: %f\n", state.eps);
  printf("leapLen: %f\n", state.leapLen);
  printf("L: %f\n", state.L);

}

// double modObjFn(AugurMod_t curr, AugurAux_t aux);
EXTERNC void mcmcStep();

/*
void acceptAlways() {
  MCMC.currLL = modObjFn(MCMC.curr, MCMC.aux);
}
*/

/**
 * Returns TRUE if we should accept, FALSE if we should reject.
 */
Bool_t augur_mcmc_ar( double auxll
		    , double (*objFn)(AugurAux_t, AugurMod_t)
		    ) {
  double currll = objFn(MCMC.aux, MCMC.curr);
  double propll = objFn(MCMC.aux, MCMC.prop);
  double u = gsl_rng_uniform(h_rng);

  if (augur_log(u) < propll - currll + auxll) {
    return TRUE;
  }
  else {
    return FALSE;
  }
}

/**
 * Returns TRUE if we should accept, FALSE if we should reject.
 */
__HOSTORDEV__ Bool_t augur_mcmc_ar_idx
       ( double auxll
       , AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
       , double (*objFn)(AugurAux_t, AugurMod_t)
       ) {
  double currll = objFn(aux, curr);
  double propll = objFn(aux, prop);
  // double u = gsl_rng_uniform(h_rng);
  double u = augur_std_uniform_sample(aux.rng);

  if (augur_log(u) < propll - currll + auxll) {
    return TRUE;
  }
  else {
    return FALSE;
  }
}

int acceptReject(double auxLL, double (*objFn)(AugurAux_t, AugurMod_t)) {
  MCMC.propLL = objFn(MCMC.aux, MCMC.prop);
  double u = gsl_rng_uniform(h_rng);
  // printf("currLL: %f  propLL: %f  auxLL: %f   ar: %d\n", MCMC.currLL, MCMC.propLL, auxLL, log(u) < MCMC.propLL - MCMC.currLL + auxLL);
  if (log(u) < MCMC.propLL - MCMC.currLL + auxLL) {
    MCMC.accept += 1;
    AugurMod_t tmp = MCMC.curr;
    MCMC.curr = MCMC.prop;
    MCMC.prop = tmp;
    // double tmp2 = MCMC.currLL;
    MCMC.currLL = MCMC.propLL;
    // MCMC.propLL = tmp2;
    return 1;
  }
  else {
    return 0;
  }
}

void swapCurrProp(double (*objFn)(AugurAux_t, AugurMod_t)) {
  MCMC.propLL = objFn(MCMC.aux, MCMC.prop);
  AugurMod_t tmp = MCMC.curr;
  MCMC.curr = MCMC.prop;
  MCMC.prop = tmp;
  double tmp2 = MCMC.currLL;
  MCMC.currLL = MCMC.propLL;
  MCMC.propLL = tmp2;
}

void initMcmc() {
  const gsl_rng_type * T;
  gsl_rng_env_setup();
  T = gsl_rng_default;
  h_rng = gsl_rng_alloc (T);

  // MCMC.curr = curr;
  // MCMC.prop = prop;
  // MCMC.aux = aux;
  MCMC.accept = 0;
  // MCMC.currLL = objFn(MCMC.aux, MCMC.curr);
  MCMC.propLL = MCMC.currLL;
  MCMC.eps = 1.0;
  MCMC.L = 20;
  MCMC.leapLen = 10.0;
  MCMC.deltaMax = 1000.0;
  MCMC.nutsalloc = 0;
  return;
}

EXTERNC void setHmcParams(double eps, double leapLen) {
  MCMC.eps = eps;
  MCMC.leapLen = leapLen;
}

EXTERNC void setHmcParams2(double eps, int L) {
  MCMC.eps = eps;
  MCMC.L = L;
}

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

double h_sample_uniform(gsl_rng* rng, double left, double right) {
  double u = gsl_rng_uniform(rng);
  return left + u * (right - left);
}

/**
 * Metropololis-within-gibbs Kernel. (Note it is host/dev code)
 */
__HOSTORDEV__ void augur_mcmc_mwg
      ( AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
      , double* llCorrect
      , void (*proposal)(AugurAux_t, AugurMod_t, AugurMod_t)
      , void (*swap)(AugurAux_t, AugurMod_t, AugurMod_t, int)
      , double (*objFn)(AugurAux_t, AugurMod_t) ) {
  proposal(aux, curr, prop);
  if (augur_mcmc_ar_idx(*llCorrect, aux, curr, prop, objFn)) {
    // Accept, so copy proposal theta into current theta (maintain invariant)
    swap(aux, curr, prop, 0);
  }
  else {
    // Reject, so copy current theta into proposal theta (maintain invariant)
    swap(aux, curr, prop, 1);
  }
}

/**
 * Gradient checking. (Call from host code)
 */
void h_augur_chk_grad
      ( AugurMemLoc_t loc, AugurBlk_t* pt, AugurBlk_t* grad
      , AugurAux_t aux, AugurMod_t curr
      , double (*objFn)(AugurAux_t, AugurMod_t) ) {
  double eps = 10e-8;
  printf("Evaluating gradient at point:\n");
  h_augur_blk_dump(loc, pt);

  for (uint_t i = 0; i < pt->num_blks; i++) {
    switch (pt->typs[i]) {
    case AUGUR_DBL: {
      // TODO
      break;
    }
    case AUGUR_VEC: {
      AugurFlatVec_t* fvec = (AugurFlatVec_t*) pt->blks[i];
      for (int j = 0; j < fvec->base_elems; j++) {
	h_augur_flat_vec_basis_add(loc, fvec, j, eps);
	double plus = objFn(aux, curr);
	h_augur_flat_vec_basis_add(loc, fvec, j, -2.0 * eps);
	double minus = objFn(aux, curr);
	double fin_diff = (plus - minus) / (2.0 * eps);
	double ad;
	h_augur_flat_vec_getd_idx(loc, &ad, (AugurFlatVec_t*) (grad->blks[i]), j);
	printf("plus: %f, minus: %f\n", plus, minus);
	printf("auto-diff: %f, finite-diff: %f, error: %f\n", ad, fin_diff, ad - fin_diff);
	h_augur_flat_vec_basis_add(loc, fvec, j, eps);
      }
      break;
    }
    case AUGUR_MAT: {
      AugurMat_t* mat = (AugurMat_t*) pt->blks[i];
      for (int j = 0; j < mat->row * mat->col; j++) {
	h_augur_mat_basis_add(loc, mat, j, eps);
	double plus = objFn(aux, curr);
	h_augur_mat_basis_add(loc, mat, j, -2.0 * eps);
	double minus = objFn(aux, curr);
	double fin_diff = (plus - minus) / (2.0 * eps);
	double ad;
	h_augur_mat_getd_idx(loc, &ad, (AugurMat_t*) (grad->blks[i]), j);
	printf("auto-diff: %f, finite-diff: %f, error: %f\n", ad, fin_diff, ad - fin_diff);
	h_augur_mat_basis_add(loc, mat, j, eps);
      }
      break;
    }
    default: {
      // TODO
      break;
    }
    }
  }
}

/**
 * Calls a gradient function. (Call from host code)
 *
 * The struct curr contains "the point" we are evaluating gradfn at.
 * The result of the gradient computation is in grad, which is part of
 * the struct aux.
 */
void h_augur_mcmc_call_grad
      ( AugurMemLoc_t loc
      , AugurAux_t aux, AugurMod_t curr
      , AugurBlk_t* grad
      , void (*gradfn)(AugurAux_t, AugurMod_t)
      ) {
  h_augur_blk_zero(loc, grad);
  gradfn(aux, curr);
}

/**
 * The struct curr contains pt, "the point" we are evaluating gradfn at.
 */
void h_augur_mcmc_chkcall_grad
      ( AugurMemLoc_t loc
      , AugurAux_t aux, AugurMod_t curr
      , AugurBlk_t* grad
      , void (*gradfn)(AugurAux_t, AugurMod_t)
      , AugurBlk_t* pt
      , double (*objfn) (AugurAux_t, AugurMod_t)
      , Bool_t f_chkgrad
      ) {
  h_augur_mcmc_call_grad(loc, aux, curr, grad, gradfn);
  if (f_chkgrad) {
    h_augur_chk_grad(loc, pt, grad, aux, curr, objfn);
  }
}

/**
 * Leapfrog simulation. (Call from host code)
 *
 * Leapfrog with (POSITIVE) gradient of log-likelihood of objfn.
 * (Signs are important!!)
 *
 * theta0 / thetaStar: initial / proposal point
 * grad: result of gradient
 * p0 / pStar: initial / proposal momentum
 *
 * mod.thetaStar = thetaStar
 * aux.grad = grad
 *
 * gradfn(aux, mod) evaluates gradient at thetaStar, putting result in grad
 *
 */
void h_augur_mcmc_leapfrog1
      ( AugurMemLoc_t loc
      , AugurAux_t aux, AugurMod_t mod
      , AugurBlk_t* grad, AugurBlk_t* thetaStar, AugurBlk_t* pStar
      , void (*gradfn)(AugurAux_t, AugurMod_t)
      , double stepsize
      ) {
  // Simulate Hamiltonian dynamics first half step of momentum
  // pStar = p0 + stepsize / 2.0 * d_like(x, thetaStar)
  h_augur_mcmc_call_grad(loc, aux, mod, grad, gradfn);
  h_augur_blk_scale_plus(loc, pStar, pStar, stepsize / 2.0, grad);

  // Full step for position
  // Position: thetaStar = thetaStar + stepsize * pStar
  h_augur_blk_scale_plus(loc, thetaStar, thetaStar, stepsize, pStar);

  // Simulate last half step of momentum
  // pStar = pStar + stepsize / 2.0 * d_like(x, thetaStar)
  h_augur_mcmc_call_grad(loc, aux, mod, grad, gradfn);
  h_augur_blk_scale_plus(loc, pStar, pStar, stepsize / 2.0, grad);
}

/**
 * Leapfrog simulation. (Call from host code)
 *
 * Leapfrog with (POSITIVE) gradient of log-likelihood of objfn.
 * (Signs are important!!)
 *
 * theta0 / thetaStar: initial / proposal point
 * grad: result of gradient
 * p0 / pStar: initial / proposal momentum
 *
 * curr.theta0 = theta0, prop.theta0 = thetaStar, aux.grad = grad
 *
 * gradfn(aux, curr) evaluates gradient at theta0, putting result in grad
 * gradfn(aux, prop) evaluates gradient at thetaStar, putting result in grad
 *
 */
void h_augur_mcmc_leapfrog
      ( AugurMemLoc_t loc
      , AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
      , AugurBlk_t* theta0, AugurBlk_t* thetaStar
      , AugurBlk_t* grad, AugurBlk_t* p0, AugurBlk_t* pStar
      , void (*gradfn)(AugurAux_t, AugurMod_t)
      , double (*objfn) (AugurAux_t, AugurMod_t)
      , double stepsize, int simsteps
      ) {
  // Simulate Hamiltonian dynamics first half step of momentum
  // pStar = p0 + stepsize / 2.0 * d_like(x, theta0)
  h_augur_mcmc_call_grad(loc, aux, curr, grad, gradfn);
  h_augur_blk_scale_plus(loc, pStar, p0, stepsize / 2.0, grad);

  // Simulate full steps
  for (uint_t l = 0; l < simsteps; l++) {
    // Full step for position
    // Position: thetaStar = thetaStar + stepsize * pStar
    h_augur_blk_scale_plus(loc, thetaStar, thetaStar, stepsize, pStar);

    // Move momentum except at last step
    if (l != simsteps-1) {
      // Momentum: pStar = pStar + stepsize * d_like(x, thetaStar)
      h_augur_mcmc_call_grad(loc, aux, prop, grad, gradfn);
      h_augur_blk_scale_plus(loc, pStar, pStar, stepsize, grad);
    }
  }

  // Simulate last half step of momentum
  // pStar = pStar + stepsize / 2.0 * d_like(x, thetaStar)
  h_augur_mcmc_call_grad(loc, aux, prop, grad, gradfn);
  h_augur_blk_scale_plus(loc, pStar, pStar, stepsize / 2.0, grad);
}

/**
 * Hamiltonian Monte Carlo Kernel. (Call from host code)
 *
 * Suppose this kernel moves blk = [p_1, .., p_n].
 * Then we have the following equalities on the inputs:
 *
 * curr.blk = theta0
 * prop.blk = thetaStar
 *
 * For compositionality, we maintain the invariant that:
 * curr.blk = prop.blk
 */
void h_augur_mcmc_hmc
     ( AugurMemLoc_t loc
     , AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
     , AugurBlk_t* theta0, AugurBlk_t* thetaStar
     , AugurBlk_t* grad, AugurBlk_t* p0, AugurBlk_t* pStar
     , double simlen, double eps
     , void (*computeGrad)(AugurAux_t, AugurMod_t)
     , double (*objFn)(AugurAux_t, AugurMod_t)
     ) {
  // Initialize momentum
  h_augur_blk_randn(aux.rng, p0);

  // Randomize simsteps
  double scale_eps = h_augur_uniform_sample(h_rng, 0.8, 1.2);
  double stepsize = scale_eps * eps;
  double s = h_augur_uniform_sample(h_rng, 0.9, 1.1);
  uint_t simsteps = augur_max(1, round(s * simlen / stepsize));

  // Simulate Hamiltonian dynamics with Leapfrog method
  h_augur_mcmc_leapfrog(loc, aux, curr, prop, theta0, thetaStar, grad, p0, pStar, computeGrad, objFn, stepsize, simsteps);

  // Compute correction
  double initMomLL = h_augur_blk_dot(loc, p0, p0) / 2.0;
  double propMomLL = h_augur_blk_dot(loc, pStar, pStar) / 2.0;
  double auxll = initMomLL - propMomLL;

  // After trajectory, compute whether we should accept or reject
  if (augur_mcmc_ar(auxll, objFn)) {
    // Accept, so copy proposal theta into current theta (for invariant)
    h_augur_blk_cpy(loc, theta0, thetaStar);
  }
  else {
    // Reject, so copy current theta into proposal theta (for invariant)
    h_augur_blk_cpy(loc, thetaStar, theta0);
  }
}

/**
 * Elliptical Slice Sampling Kernel. (Note it is host/dev code)
 *
 * Suppose this kernel moves param = currTheta.
 * Then we have the following equalities on the inputs:
 *
 * curr.param = currTheta
 * prop.param = propTheta
 *
 * For compositionality, we maintain the invariant that:
 * curr.param = prop.param
 */
__HOSTORDEV__ void augur_mcmc_eslice
      ( AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
      , AugurVec_t* curr_theta, AugurVec_t* prop_theta
      , AugurVec_t* mean, AugurMat_t* cov
      , AugurMat_t* L, AugurVec_t* nu
      , double (*objfn)(AugurAux_t, AugurMod_t) ) {
  augur_mvnormal_sample(aux.rng, nu, mean, cov, L, nu);
  double u = augur_std_uniform_sample(aux.rng);
  double log_y = objfn(aux, curr) + augur_log(u);

  double theta = augur_uniform_sample(aux.rng, 0.0, 2.0 * AUGUR_PI);
  double theta_min = theta - 2.0 * AUGUR_PI;
  double theta_max = theta;

  while (TRUE) {
    double diff = theta_max - theta_min;
    if (diff < 1e-8) {
      // Maintain invariant that curr == prop
      augur_vec_cpy(prop_theta, curr_theta);
      break;
    }

    // propSt = (currSt - mean) * cos(theta) + (nu - mean) * sin(theta) + mean;
    for (uint_t i = 0; i < prop_theta->elems; i++) {
      double mean_i = AUGUR_VEC_GETD(mean, i);
      double a = (AUGUR_VEC_GETD(curr_theta, i) - mean_i) * augur_cos(theta);
      double b = (AUGUR_VEC_GETD(nu, i) - mean_i) * augur_sin(theta);
      AUGUR_VEC_SETD(prop_theta, i, a + b + mean_i);
    }

    double prop_ll = objfn(aux, prop);
    if (prop_ll > log_y) {
      // Maintain invariant that curr == prop
      augur_vec_cpy(curr_theta, prop_theta);
      break;
    }

    // Shrink bracket
    if (theta < 0) {
      theta_min = theta;
    }
    else {
      theta_max = theta;
    }
    theta = augur_uniform_sample(aux.rng, theta_min, theta_max);
  }
}

/**
 * Reflective Slice Kernel. (Call from host code)
 *
 * Suppose this kernel moves blk = [p_1, .., p_n].
 * Then we have the following equalities on the inputs:
 *
 * curr.blk = theta0
 * prop.blk = thetaStar
 *
 * For compositionality, we maintain the invariant that:
 * curr.blk = prop.blk
 */
void h_augur_mcmc_refl_slice
     ( AugurMemLoc_t loc
     , AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
     , AugurBlk_t* theta0, AugurBlk_t* thetaProp
     , AugurBlk_t* grad, AugurBlk_t* p0
     , double simlen, double eps
     , void (*computeGrad)(AugurAux_t, AugurMod_t)
     , double (*objFn)(AugurAux_t, AugurMod_t)
     ) {
  // Initialize momentum
  h_augur_blk_randn(aux.rng, p0);

  // Compute slice level
  double z = objFn(aux, curr) - gsl_ran_exponential(h_rng, 1.0);
  int cond = 0;
  double stepsize = eps;
  uint_t simsteps = augur_max(1, round(simlen / stepsize));

  for (int i = 0 ; i < simsteps; i++) {
    // Take scaled-step in direction of momentum
    h_augur_blk_scale_plus(loc, thetaProp, thetaProp, stepsize, p0);

    cond = objFn(aux, prop) < z;
    if (cond) {
      // Reflect (outside version) if we leave slice

      // Compute grad at outside point
      h_augur_blk_zero(loc, grad);
      computeGrad(aux, prop);

      // Compute reflection and update direction
      double angle1 = h_augur_blk_dot(loc, p0, grad);
      double angle2 = h_augur_blk_dot(loc, grad, grad);
      double scale = angle1 / angle2;
      h_augur_blk_scale_plus(loc, p0, p0, -2.0 * scale, grad);

    }
  }

  if (!cond) {
    // Copy if we are in slice
    h_augur_blk_cpy(loc, theta0, thetaProp);
  }
}

/* NUTS */

typedef enum NutsDir {
  NUTS_MINUS = 0,
  NUTS_PLUS = 1,
} NutsDir_t;

typedef struct NutsRet {
  int n;
  int s;
} NutsRet_t;

typedef struct NutsRT {
  AugurBlk_t* work;
  AugurBlkStk_t* theta_prop_stk;
  AugurBlkStk_t* theta_stk;
  AugurBlkStk_t* mom_stk;
} NutsRT_t;

typedef struct NutsInfo {
  AugurAux_t aux;
  AugurMod_t mod;
  AugurBlk_t* grad;
  double (*modObjFn)(AugurAux_t, AugurMod_t);
  void (*computeGrad)(AugurAux_t, AugurMod_t);
  double stepsize;
  double logu;
} NutsInfo_t;

int h_augur_nuts_update_s(AugurMemLoc_t loc, int s, AugurBlk_t* work, AugurBlk_t* theta_plus, AugurBlk_t* mom_plus, AugurBlk_t* theta_minus, AugurBlk_t* mom_minus) {
  if (s == 1) {
    h_augur_blk_minus(loc, work, theta_plus, theta_minus);
    int ut1 = augur_indicator(h_augur_blk_dot(loc, work, mom_plus) >= 0.0);
    int ut2 = augur_indicator(h_augur_blk_dot(loc, work, mom_minus) >= 0.0);
    if (ut1 && ut2) {
      return 1;
    }
  }
  return 0;
}

/**
 * info.mod.theta = theta (gradient is always evaluated at latest value of theta)
 *
 * Invariants:
 * Suppose dir = PLUS, then:
 *
 * theta: latest value of theta^+
 * mom: latest value of mom^+
 */
NutsRet_t h_augur_nuts_build_tree
      ( AugurMemLoc_t loc, NutsInfo_t* info , NutsRT_t* rt
      , AugurBlk_t* shape
      , AugurBlk_t* theta, AugurBlk_t* mom, int j, NutsDir_t dir
      , int tab) {
  if (j == 0) {
    // Simulate (updates theta, mom)
    h_augur_mcmc_leapfrog1(loc, info->aux, info->mod,
			   info->grad, theta, mom,
			   info->computeGrad,
			   info->stepsize);

    // Update return value
    augur_blk_stk_push(loc, rt->theta_prop_stk, shape, theta);

    // Check stopping condition
    NutsRet_t ret;
    double ll = info->modObjFn(info->aux, info->mod) - 0.5 * h_augur_blk_dot(loc, mom, mom);
    ret.n = augur_indicator(info->logu <= ll);
    ret.s = augur_indicator(info->logu < MCMC.deltaMax + ll);

    return ret;
  }
  else {
    // Build left subtree
    NutsRet_t left = h_augur_nuts_build_tree(loc, info, rt, shape, theta, mom, j - 1, dir, tab+2);

    // Build right subtree (if we should keep going)
    if (left.s == 1) {
      // Save "left"-most
      if (j == 1) {
	augur_blk_stk_push(loc, rt->theta_stk, shape, theta);
	augur_blk_stk_push(loc, rt->mom_stk, shape, mom);
      }

      NutsRet_t right = h_augur_nuts_build_tree(loc, info, rt, shape, theta, mom, j - 1, dir, tab+2);

      // Propogate proposal with probability proportional to size of subtrees
      AugurBlk_t* prop1 = augur_blk_stk_pop(rt->theta_prop_stk);
      AugurBlk_t* prop2 = augur_blk_stk_pop(rt->theta_prop_stk);
      double bias = ((double) right.n) / (left.n + right.n);
      if (h_augur_bernoulli_sample(h_rng, bias) == 0) {
	augur_blk_stk_push(loc, rt->theta_prop_stk, shape, prop2);
      }
      else {
	augur_blk_stk_push(loc, rt->theta_prop_stk, shape, prop1);
      }
      augur_blk_stk_dump(loc, rt->theta_prop_stk);

      // Propogate tree size
      left.n += right.n;

      // Check u-turn
      AugurBlk_t* theta_minus; AugurBlk_t* mom_minus;
      AugurBlk_t* theta_plus; AugurBlk_t* mom_plus;
      switch (dir) {
      case NUTS_MINUS: {
	theta_plus = augur_blk_stk_peek(rt->theta_stk);
	mom_plus = augur_blk_stk_peek(rt->mom_stk);
	theta_minus = theta;
	mom_minus = mom;
	break;
      }
      case NUTS_PLUS: {
	theta_minus = augur_blk_stk_peek(rt->theta_stk);
	mom_minus = augur_blk_stk_peek(rt->mom_stk);
	theta_plus = theta;
	mom_plus = mom;
	break;
      }
      }
      left.s = h_augur_nuts_update_s(loc, right.s, rt->work, theta_plus, mom_plus, theta_minus, mom_minus);

      // Pop "left"-most
      if (j > 1) {
	augur_blk_stk_pop(rt->theta_stk);
	augur_blk_stk_pop(rt->mom_stk);
      }
    }

    return left;
  }
}

/**
 * No-U-Turn Kernel. (Call from host code)
 *
 * Suppose this kernel moves blk = [p_1, .., p_n].
 * Then we have the following equalities on the inputs:
 *
 * curr.blk = theta0
 * prop.blk = thetaStar
 *
 * For compositionality, we maintain the invariant that:
 * curr.blk = prop.blk
 */
void h_augur_mcmc_nuts
      ( AugurMemLoc_t loc, AugurAux_t aux, AugurMod_t curr, AugurMod_t prop
      , AugurBlk_t* theta0, AugurBlk_t* theta_prop
      , AugurBlk_t* grad, AugurBlk_t* work
      , AugurBlk_t* theta_minus, AugurBlk_t* mom_minus
      , AugurBlk_t* theta_plus, AugurBlk_t* mom_plus
      , AugurBlk_t* shape
      , double stepsize
      , void (*computeGrad)(AugurAux_t, AugurMod_t)
      , double (*modObjFn)(AugurAux_t, AugurMod_t)
      ) {
  // Initialize simulation values
  h_augur_blk_cpy(loc, theta_minus, theta0);
  h_augur_blk_randn(aux.rng, mom_minus);
  h_augur_blk_cpy(loc, theta_plus, theta0);
  h_augur_blk_cpy(loc, mom_plus, mom_minus);

  // Initialize info
  NutsInfo_t info;
  info.aux = aux;
  info.mod = prop;
  info.grad = grad;
  info.modObjFn = modObjFn;
  info.computeGrad = computeGrad;
  info.logu = modObjFn(aux, curr) - 0.5 * h_augur_blk_dot(loc, mom_minus, mom_minus) - gsl_ran_exponential(h_rng, 1.0);

  // Initialize auxilliary nuts runtime values
  NutsRT_t rt;
  rt.work = work;
  AugurBlkStk_t theta_prop_stk = augur_blk_stk_stk_alloc();
  AugurBlkStk_t theta_stk = augur_blk_stk_stk_alloc();
  AugurBlkStk_t mom_stk = augur_blk_stk_stk_alloc();
  rt.theta_prop_stk = &theta_prop_stk;
  rt.theta_stk = &theta_stk;
  rt.mom_stk = &mom_stk;

  int n = 1;
  int j = 0;
  int s = 1;

  while (TRUE) {
    NutsRet_t nuts_ret;
    augur_blk_stk_reset(rt.theta_stk);
    augur_blk_stk_reset(rt.mom_stk);
    if (h_augur_bernoulli_sample(h_rng, 0.5)) {
      info.stepsize = -stepsize;
      h_augur_blk_cpy(loc, theta_prop, theta_minus);
      nuts_ret = h_augur_nuts_build_tree(loc, &info, &rt, shape, theta_prop, mom_minus, j, NUTS_MINUS, 0);
      h_augur_blk_cpy(loc, theta_minus, theta_prop);
    }
    else {
      info.stepsize = stepsize;
      h_augur_blk_cpy(loc, theta_prop, theta_plus);
      nuts_ret = h_augur_nuts_build_tree(loc, &info, &rt, shape, theta_prop, mom_plus, j, NUTS_PLUS, 0);
      h_augur_blk_cpy(loc, theta_plus, theta_prop);
    }

    AugurBlk_t* theta_prop_p = augur_blk_stk_pop(rt.theta_prop_stk);
    Bool_t f_cpy = FALSE;
    if (nuts_ret.s == 1) {
      double bias = augur_min(1.0, ((double) nuts_ret.n) / n);
      if (h_augur_bernoulli_sample(h_rng, bias) == 1) {
	// Update proposal
	h_augur_blk_cpy(loc, theta_prop, theta_prop_p);
	f_cpy = TRUE;
      }
    }

    n += nuts_ret.n;
    s = h_augur_nuts_update_s(loc, nuts_ret.s, rt.work, theta_plus, mom_plus, theta_minus, mom_minus);
    j += 1;

    if (s != 1) {
      // Maintain invariant
      if (f_cpy) {
	h_augur_blk_cpy(loc, theta0, theta_prop);
      }
      else {
	h_augur_blk_cpy(loc, theta_prop, theta0);
      }
      break;
    }
  }
}

#endif


#ifdef HAVE_LIBGSL
/* interface_gsl.h
 * Easel's interfaces to the GNU Scientific Library
 *
 * SRE, Tue Jul 13 15:36:48 2004
 * SVN $Id: interface_gsl.h 664 2011-02-27 17:08:36Z eddys $
 * SVN $URL: https://svn.janelia.org/eddylab/eddys/easel/branches/hmmer/3.1/interface_gsl.h $
 */
#ifndef eslINTERFACE_GSL_INCLUDED
#define eslINTERFACE_GSL_INCLUDED

#include <stdlib.h>
#include <easel/easel.h>
#include <easel/dmatrix.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_eigen.h>

extern int esl_GSL_MatrixInversion(ESL_DMATRIX *A, ESL_DMATRIX **ret_Ai);

#endif /*eslINTERFACE_GSL_INCLUDED*/
#endif /*HAVE_LIBGSL*/


/*
 *  aeif_cond_beta_multisynapse.h
 *
 *  This file is part of NEST.
 *
 *  Copyright (C) 2004 The NEST Initiative
 *
 *  NEST 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.
 *
 *  NEST 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 NEST.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#ifndef AEIF_COND_BETA_MULTISYNAPSE_H
#define AEIF_COND_BETA_MULTISYNAPSE_H

// Generated includes:
#include "config.h"

#ifdef HAVE_GSL

// External includes:
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>

// Includes from nestkernel:
#include "archiving_node.h"
#include "connection.h"
#include "event.h"
#include "nest_types.h"
#include "ring_buffer.h"
#include "universal_data_logger.h"

/* BeginDocumentation
 Name: aeif_cond_beta_multisynapse - Conductance based adaptive exponential
                                      integrate-and-fire neuron model according
                                      to Brette and Gerstner (2005) with
                                      multiple synaptic rise time and decay
                                      time constants, and synaptic conductance
                                      modeled by a beta function.

 Description:

 aeif_cond_beta_multisynapse is a conductance-based adaptive exponential
 integrate-and-fire neuron model. It allows an arbitrary number of synaptic
 rise time and decay time constants. Synaptic conductance is modeled by a
 beta function, as described by A. Roth and M.C.W. van Rossum
 in Computational Modeling Methods for Neuroscientists, MIT Press 2013,
 Chapter 6.

 The time constants are supplied by two arrays, "tau_rise" and "tau_decay" for
 the synaptic rise time and decay time, respectively. The synaptic
 reversal potentials are supplied by the array "E_rev". The port numbers
 are automatically assigned in the range from 1 to n_receptors.
 During connection, the ports are selected with the property "receptor_type".

 The membrane potential is given by the following differential equation:
 C dV/dt = -g_L(V-E_L) + g_L*Delta_T*exp((V-V_T)/Delta_T) + I_syn_tot(V, t)
           - w + I_e

 where:
 I_syn_tot(V,t) = \sum_i g_i(t) (V - E_{rev,i}) ,

 the synapse i is excitatory or inhibitory depending on the value of E_{rev,i}
 and the differential equation for the spike-adaptation current w is:

 tau_w * dw/dt = a(V - E_L) - w

 When the neuron fires a spike, the adaptation current w <- w + b.

Parameters:
The following parameters can be set in the status dictionary.

Dynamic state variables:
  V_m        double - Membrane potential in mV
  w          double - Spike-adaptation current in pA.

Membrane Parameters:
  C_m        double - Capacity of the membrane in pF
  t_ref      double - Duration of refractory period in ms.
  V_reset    double - Reset value for V_m after a spike. In mV.
  E_L        double - Leak reversal potential in mV.
  g_L        double - Leak conductance in nS.
  I_e        double - Constant external input current in pA.
  Delta_T    double - Slope factor in mV
  V_th       double - Spike initiation threshold in mV
  V_peak     double - Spike detection threshold in mV.

Adaptation parameters:
  a          double - Subthreshold adaptation in nS.
  b          double - Spike-triggered adaptation in pA.
  tau_w      double - Adaptation time constant in ms

Synaptic parameters
  E_rev      double vector - Reversal potential in mV.
  tau_rise  double vector - Rise time of synaptic conductance in ms (beta
                      function).
  tau_decay double vector - Decay time of synaptic conductance in ms (beta
                      function).

Integration parameters
  gsl_error_tol  double - This parameter controls the admissible error of the
                          GSL integrator. Reduce it if NEST complains about
                          numerical instabilities.

 Examples:

 import nest
 import numpy as np

 neuron = nest.Create('aeif_cond_beta_multisynapse')
 nest.SetStatus(neuron, {"V_peak": 0.0, "a": 4.0, "b":80.5})
 nest.SetStatus(neuron, {'E_rev':[0.0,0.0,0.0,-85.0],
                         'tau_decay':[50.0,20.0,20.0,20.0],
                         'tau_rise':[10.0,10.0,1.0,1.0]})

 spike = nest.Create('spike_generator', params = {'spike_times':
                                                 np.array([10.0])})

 voltmeter = nest.Create('voltmeter', 1, {'withgid': True})

 delays=[1.0, 300.0, 500.0, 700.0]
 w=[1.0, 1.0, 1.0, 1.0]
 for syn in range(4):
     nest.Connect(spike, neuron, syn_spec={'model': 'static_synapse',
                                           'receptor_type': 1 + syn,
                                           'weight': w[syn],
                                           'delay': delays[syn]})

 nest.Connect(voltmeter, neuron)

 nest.Simulate(1000.0)
 dmm = nest.GetStatus(voltmeter)[0]
 Vms = dmm["events"]["V_m"]
 ts = dmm["events"]["times"]
 import pylab
 pylab.figure(2)
 pylab.plot(ts, Vms)
 pylab.show()

 Sends: SpikeEvent

 Receives: SpikeEvent, CurrentEvent, DataLoggingRequest

 author: Bruno Golosio 07/10/2016
 SeeAlso: aeif_cond_alpha_multisynapse
 */

namespace nest
{
/**
 * Function computing right-hand side of ODE for GSL solver.
 * @note Must be declared here so we can befriend it in class.
 * @note Must have C-linkage for passing to GSL. Internally, it is
 *       a first-class C++ function, but cannot be a member function
 *       because of the C-linkage.
 * @note No point in declaring it inline, since it is called
 *       through a function pointer.
 * @param void* Pointer to model neuron instance.
 */
extern "C" int
aeif_cond_beta_multisynapse_dynamics( double, const double*, double*, void* );

/**
 * Conductance based exponential integrate-and-fire neuron model according to
 * Brette and Gerstner
 * (2005) with multiple ports.
 */
class aeif_cond_beta_multisynapse : public Archiving_Node
{

public:
  aeif_cond_beta_multisynapse();
  aeif_cond_beta_multisynapse( const aeif_cond_beta_multisynapse& );
  virtual ~aeif_cond_beta_multisynapse();

  friend int
  aeif_cond_beta_multisynapse_dynamics( double, const double*, double*, void* );

  /**
   * Import sets of overloaded virtual functions.
   * @see Technical Issues / Virtual Functions: Overriding, Overloading, and
   * Hiding
   */
  using Node::handle;
  using Node::handles_test_event;

  port send_test_event( Node&, rport, synindex, bool );

  void handle( SpikeEvent& );
  void handle( CurrentEvent& );
  void handle( DataLoggingRequest& );

  port handles_test_event( SpikeEvent&, rport );
  port handles_test_event( CurrentEvent&, rport );
  port handles_test_event( DataLoggingRequest&, rport );

  void get_status( DictionaryDatum& ) const;
  void set_status( const DictionaryDatum& );

private:
  void init_state_( const Node& proto );
  void init_buffers_();
  void calibrate();
  void update( Time const&, const long, const long );

  // The next two classes need to be friends to access the State_ class/member
  friend class RecordablesMap< aeif_cond_beta_multisynapse >;
  friend class UniversalDataLogger< aeif_cond_beta_multisynapse >;

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

  /**
   * Independent parameters of the model.
   */
  struct Parameters_
  {
    double V_peak_;  //!< Spike detection threshold in mV
    double V_reset_; //!< Reset Potential in mV
    double t_ref_;   //!< Refractory period in ms

    double g_L;     //!< Leak Conductance in nS
    double C_m;     //!< Membrane Capacitance in pF
    double E_L;     //!< Leak reversal Potential (aka resting potential) in mV
    double Delta_T; //!< Slope faktor in ms.
    double tau_w;   //!< adaptation time-constant in ms.
    double a;       //!< Subthreshold adaptation in nS.
    double b;       //!< Spike-triggered adaptation in pA
    double V_th;    //!< Spike threshold in mV.

    std::vector< double > tau_rise;  //!< Rise time of synaptic conductance
                                     //!< in ms.
    std::vector< double > tau_decay; //!< Decay time of synaptic conductance
                                     //!< in ms.
    std::vector< double > E_rev;     //!< reversal potentials in mV

    double I_e; //!< Intrinsic current in pA.

    double gsl_error_tol; //!< error bound for GSL integrator

    // boolean flag which indicates whether the neuron has connections
    bool has_connections_;

    Parameters_(); //!< Sets default parameter values

    void get( DictionaryDatum& ) const; //!< Store current values in dictionary
    void set( const DictionaryDatum& ); //!< Set values from dictionary

    //! Return the number of receptor ports
    inline size_t
    n_receptors() const
    {
      return E_rev.size();
    }
  };

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

  /**
   * State variables of the model.
   * @note Copy constructor and assignment operator required because
   *       of C-style arrays.
   */
  struct State_
  {

    /**
     * Enumeration identifying elements in state vector State_::y_.
     * This enum identifies the elements of the vector. It must be public to be
     * accessible from the iteration function. The last two elements of this
     * enum (DG, G) will be repeated
     * n times at the end of the state vector State_::y with n being the number
     * of synapses.
     */
    enum StateVecElems
    {
      V_M = 0,
      W,  // 1
      DG, // 2
      G,  // 3
      STATE_VECTOR_MIN_SIZE
    };

    static const size_t NUMBER_OF_FIXED_STATES_ELEMENTS = 2;        // V_M, W
    static const size_t NUMBER_OF_STATES_ELEMENTS_PER_RECEPTOR = 2; // DG, G

    std::vector< double > y_; //!< neuron state
    int r_;                   //!< number of refractory steps remaining

    State_( const Parameters_& ); //!< Default initialization
    State_( const State_& );
    State_& operator=( const State_& );

    void get( DictionaryDatum& ) const;
    void set( const DictionaryDatum& );

  }; // State_

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

  /**
   * Buffers of the model.
   */
  struct Buffers_
  {
    Buffers_( aeif_cond_beta_multisynapse& );
    Buffers_( const Buffers_&, aeif_cond_beta_multisynapse& );

    //! Logger for all analog data
    UniversalDataLogger< aeif_cond_beta_multisynapse > logger_;

    /** buffers and sums up incoming spikes/currents */
    std::vector< RingBuffer > spikes_;
    RingBuffer currents_;

    /** GSL ODE stuff */
    gsl_odeiv_step* s_;    //!< stepping function
    gsl_odeiv_control* c_; //!< adaptive stepsize control function
    gsl_odeiv_evolve* e_;  //!< evolution function
    gsl_odeiv_system sys_; //!< struct describing system

    // IntergrationStep_ should be reset with the neuron on ResetNetwork,
    // but remain unchanged during calibration. Since it is initialized with
    // step_, and the resolution cannot change after nodes have been created,
    // it is safe to place both here.
    double step_;            //!< simulation step size in ms
    double IntegrationStep_; //!< current integration time step,
                             //!< updated by solver

    /**
     * Input current injected by CurrentEvent.
     * This variable is used to transport the current applied into the
     * _dynamics function computing the derivative of the state vector.
     * It must be a part of Buffers_, since it is initialized once before
     * the first simulation, but not modified before later Simulate calls.
     */
    double I_stim_;
  };

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

  /**
   * Internal variables of the model.
   */
  struct Variables_
  {

    /** initial value to normalise synaptic conductance */
    std::vector< double > g0_;

    /**
     * Threshold detection for spike events: P.V_peak if Delta_T > 0.,
     * P.V_th if Delta_T == 0.
     */
    double V_peak;

    unsigned int refractory_counts_;
  };

  // Access functions for UniversalDataLogger -------------------------------

  //! Read out state vector elements, used by UniversalDataLogger
  template < State_::StateVecElems elem >
  double
  get_y_elem_() const
  {
    return S_.y_[ elem ];
  }

  // Data members -----------------------------------------------------------

  /**
   * @defgroup aeif_cond_beta_multisynapse
   * Instances of private data structures for the different types
   * of data pertaining to the model.
   * @note The order of definitions is important for speed.
   * @{
   */
  Parameters_ P_;
  State_ S_;
  Variables_ V_;
  Buffers_ B_;
  /** @} */

  //! Mapping of recordables names to access functions
  static RecordablesMap< aeif_cond_beta_multisynapse > recordablesMap_;
};

inline port
aeif_cond_beta_multisynapse::send_test_event( Node& target,
  rport receptor_type,
  synindex,
  bool )
{
  SpikeEvent e;
  e.set_sender( *this );

  return target.handles_test_event( e, receptor_type );
}

inline port
aeif_cond_beta_multisynapse::handles_test_event( CurrentEvent&,
  rport receptor_type )
{
  if ( receptor_type != 0 )
  {
    throw UnknownReceptorType( receptor_type, get_name() );
  }
  return 0;
}

inline port
aeif_cond_beta_multisynapse::handles_test_event( DataLoggingRequest& dlr,
  rport receptor_type )
{
  if ( receptor_type != 0 )
  {
    throw UnknownReceptorType( receptor_type, get_name() );
  }
  return B_.logger_.connect_logging_device( dlr, recordablesMap_ );
}

inline void
aeif_cond_beta_multisynapse::get_status( DictionaryDatum& d ) const
{
  P_.get( d );
  S_.get( d );
  Archiving_Node::get_status( d );

  ( *d )[ names::recordables ] = recordablesMap_.get_list();
}

inline void
aeif_cond_beta_multisynapse::set_status( const DictionaryDatum& d )
{
  Parameters_ ptmp = P_; // temporary copy in case of errors
  ptmp.set( d );         // throws if BadProperty
  State_ stmp = S_;      // temporary copy in case of errors
  stmp.set( d );         // throws if BadProperty

  // We now know that (ptmp, stmp) are consistent. We do not
  // write them back to (P_, S_) before we are also sure that
  // the properties to be set in the parent class are internally
  // consistent.
  Archiving_Node::set_status( d );

  // if we get here, temporaries contain consistent set of properties
  P_ = ptmp;
  S_ = stmp;
}

} // namespace

#endif // HAVE_GSL
#endif // AEIF_COND_BETA_MULTISYNAPSE_H //


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. Redistributions in binary form must reproduce the above copyright notice,
		this list of conditions and the following disclaimer in the
		documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL Javier Burguete Tolosa OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/**
 * \file rk_6_2.c
 * \brief Source file to optimize Runge-Kutta 6 steps 2nd order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_6_2.h"

#define DEBUG_RK_6_2 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 6 steps 2nd order Runge-Kutta
 * method.
 */
int
rk_tb_6_2 (Optimize * optimize) ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_tb_6_2: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t6 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  b41 (tb) = r[7];
  b42 (tb) = r[8];
  b43 (tb) = r[9];
  t5 (tb) = r[10];
  b51 (tb) = r[11];
  b52 (tb) = r[12];
  b53 (tb) = r[13];
  b54 (tb) = r[14];
  b62 (tb) = r[15];
  b63 (tb) = r[16];
  b64 (tb) = r[17];
  b65 (tb) = r[18];
  b61 (tb) = (0.5L - b62 (tb) * t2 (tb) - b63 (tb) * t3 (tb)
              - b64 (tb) * t4 (tb) - b65 (tb) * t5 (tb)) / t1 (tb);
  if (isnan (b61 (tb)))
    return 0;
  rk_b_6 (tb);
#if DEBUG_RK_6_2
  rk_print_tb (optimize, "rk_tb_6_2", stderr);
  fprintf (stderr, "rk_tb_6_2: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 2nd order, 3rd order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_6_2t (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_tb_6_2t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t6 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  b41 (tb) = r[7];
  b42 (tb) = r[8];
  b43 (tb) = r[9];
  t5 (tb) = r[10];
  b51 (tb) = r[11];
  b52 (tb) = r[12];
  b53 (tb) = r[13];
  b54 (tb) = r[14];
  b61 (tb) = r[15];
  b62 (tb) = r[16];
  b63 (tb) = r[17];
  b64 (tb) = (1.L / 3.L - 0.5L * t5 (tb)
              - b61 (tb) * t1 (tb) * (t1 (tb) - t5 (tb))
              - b62 (tb) * t2 (tb) * (t2 (tb) - t5 (tb))
              - b63 (tb) * t3 (tb) * (t3 (tb) - t5 (tb)))
    / (t4 (tb) * (t4 (tb) - t5 (tb)));
  if (isnan (b64 (tb)))
    return 0;
  b65 (tb) = (0.5L - b61 (tb) * t1 (tb) - b62 (tb) * t2 (tb)
              - b63 (tb) * t3 (tb) - b64 (tb) * t4 (tb)) / t5 (tb);
  if (isnan (b65 (tb)))
    return 0;
  rk_b_6 (tb);
#if DEBUG_RK_6_2
  rk_print_tb (optimize, "rk_tb_6_2t", stderr);
  fprintf (stderr, "rk_tb_6_2t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 1st-2nd order Runge-Kutta
 * pair.
 */
int
rk_tb_6_2p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_tb_6_2p: start\n");
#endif
  if (!rk_tb_6_2 (optimize))
    return 0;
  tb = optimize->coefficient;
  e61 (tb) = e62 (tb) = e63 (tb) = e64 (tb) = 0.L;
  rk_e_6 (tb);
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_tb_6_2p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 1st-2nd order, 1st-3rd order
 * in equations depending only in time, Runge-Kutta pair.
 */
int
rk_tb_6_2tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_tb_6_2tp: start\n");
#endif
  if (!rk_tb_6_2t (optimize))
    return 0;
  tb = optimize->coefficient;
  e61 (tb) = e62 (tb) = e63 (tb) = e64 (tb) = 0.L;
  rk_e_6 (tb);
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_tb_6_2tp: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 6 steps 2nd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_2 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_objective_tb_6_2: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_objective_tb_6_2: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_2: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 6 steps 2nd order, third
 * order in equations depending only on time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_2t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_objective_tb_6_2t: start\n");
#endif
  tb = rk->tb->coefficient;
#if DEBUG_RK_6_2
  rk_print_tb (optimize, "rk_objective_tb_6_2t", stderr);
#endif
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (b65 (tb) < 0.L)
    o += b65 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_2
  fprintf (stderr, "rk_objective_tb_6_2t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_2t: end\n");
#endif
  return o;
}


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

#ifndef EpiGenMCMC_parameter_h
#define EpiGenMCMC_parameter_h

#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <algorithm>
#include <cmath>
#include <limits>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

class Parameter {
    std::vector <double> parameter_values;
    std::vector <std::string> parameter_names;
    std::vector <bool> estimate;
    std::vector <std::string> transform;
    std::vector <std::string> prior;
    std::vector <double> prior_par_1;
    std::vector <double> prior_par_2;
    std::vector <double> prior_par_3;
    std::vector <std::string> proposal;
    std::vector <double> proposal_sd;
    std::vector <double> proposal_lower;
    std::vector <double> proposal_upper;
    std::vector <double> accepted;
    std::vector <double> rejected;
    std::vector <double> acceptance_rate;
    std::vector <int> params_to_estim;
    int total_params;
    int curr_param_to_estimate;
    double old_param_value;
    double optimal_acceptance;
    double lower_acceptance;
    double upper_acceptance;
    int adapt_every;
    int max_adapt_times;
    bool stop_adapting;
    double get_prior(double, int) const;
    double get_transform(double, std::string, bool) const;
public:
    Parameter();
    Parameter(const Parameter &);
    Parameter (std::string);
    bool is_estim(int) const;
    int get_curr_estim() const;
    void set_next_param();
    int get_total_params() const;
    int get_total_params_to_estim() const;
    int get_estim_index(int) const;
    double get(std::string) const;
    double get(int) const;
    double get_lower(int) const;
    double get_upper(int) const;
    void set(int, double);
    std::vector<double> get_values_vector() const;
    std::vector <std::string> get_names_vector() const;
    std::string getname(int) const;
    void reset();
    void accept();
    void reject();
    double get_acceptance() const;
    void stop_adapt();
    void start_adapt();
    void adapt();
    double propose(gsl_rng *);
    double get_prior_ratio() const;
    double get_prior_all() const;
    bool param_exists(std::string) const;
    void transform_param(int, bool);
    double get_lognormal_sd (double, double) const;
    double get_lognormal_mean (double, double) const;
};

struct MCMCoptions {
    int particles;
    int iterations;
    int log_every;
    int pfilter_every;
    double pfilter_threshold;
    int which_likelihood;
    int num_trees;
    std::string log_filename;
    std::string traj_filename;
    std::string model;
    int total_dt;
    double sim_dt;
    int num_groups;
    int seed;
    bool verbose;
    bool save_traj;
    bool use_lhs;
    int lhs_divides;
    int lhs_iterations;
    int num_threads;
    double heat_factor;
    int heat_length;
    double cool_rate;
    gsl_rng ** rng;
    MCMCoptions();
    MCMCoptions(const MCMCoptions&);
    MCMCoptions(std::string);

};

#endif


#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) > (b) ? (a) : (b))

#define DefaultWorkSize 1e9 // 1GB
#define NumSplitDefaultMax 20

#define OUTPUT stdout // stderr

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <typeinfo>

#define __STDC_FORMAT_MACROS
#include <inttypes.h>
#include <sys/time.h>
#include <float.h>

#if defined (MKL)
#include <mkl_cblas.h>
#include <mkl_trans.h>
#include <mkl_spblas.h>
#else
#include <cblas.h>
#endif

#include <omp.h>

#if defined (FLOAT128)
#if defined (ARM)
#define __float128 long double
#define FLT128_MAX LDBL_MAX
#define FLT128_MIN LDBL_MIN
#else
#include <quadmath.h>
#endif
//#ifdef __INTEL_COMPILER
//__float128 std::abs (const __float128 x);
//#endif
#include <mplapack/mpblas__Float128.h>
#endif

#if defined (CUBLAS)
#include <cuda.h>
#include <cuda_runtime_api.h>
#include <cublas_v2.h>
#endif

#include "../../include/ozblas.h"
#include "ozblas_internal.h"

// -------------------------------------
// constexpr functions
// -------------------------------------

// -------------------------------------
// getEmin
// -------------------------------------
template <typename TYPE>
constexpr int32_t getEmin () {
	fprintf (OUTPUT, "OzBLAS error: TYPE is not specified in getEmin.\n");
	exit (1);
	return 0;
}
template <>
constexpr int32_t getEmin <float> () {
	return -126;
}
template <>
constexpr int32_t getEmin <double> () {
	return -1022;
}
#if defined (FLOAT128)
template <>
constexpr int32_t getEmin <__float128> () {
	return -16382;
}
#endif

// -------------------------------------
// getEpse
// -------------------------------------
template <typename TYPE>
constexpr int32_t getEpse () {
	fprintf (OUTPUT, "OzBLAS error: TYPE is not specified in getEpse.\n");
	exit (1);
	return 0;
}
template <>
constexpr int32_t getEpse <float> () {
	return 24;
}
template <>
constexpr int32_t getEpse <double> () {
	return 53;
}
#if defined (FLOAT128)
template <>
constexpr int32_t getEpse <__float128> () {
	return 113;
}
#endif

// -------------------------------------
// getTypeMax
// -------------------------------------
template <typename TYPE>
constexpr TYPE getTypeMax () {
	fprintf (OUTPUT, "OzBLAS error: TYPE is not specified in getTypeMax.\n");
	exit (1);
	return 0;
}
template <>
constexpr float getTypeMax <float> () {
	return FLT_MAX;
}
template <>
constexpr double getTypeMax <double> () {
	return DBL_MAX;
}
#if defined (FLOAT128)
template <>
constexpr __float128 getTypeMax <__float128> () {
	return FLT128_MAX;
}
#endif

// -------------------------------------
// getTypeMin
// -------------------------------------
template <typename TYPE>
constexpr TYPE getTypeMin () {
	fprintf (OUTPUT, "OzBLAS error: TYPE is not specified in getTypeMin.\n");
	exit (1);
	return 0;
}
template <>
constexpr float getTypeMin <float> () {
	return FLT_MIN;
}
template <>
constexpr double getTypeMin <double> () {
	return DBL_MIN;
}
#if defined (FLOAT128)
template <>
constexpr __float128 getTypeMin <__float128> () {
	return FLT128_MIN;
}
#endif

/*
#define SCALBN scalbnl
#define LOG2 log2l
#define LOG logl
#define CEIL ceill
#define FABS fabsl
*/


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gbpInterpolate.h>

// params is not used at the moment but we must allow for
//   it to meet the required ADaPS function definition
void free_interpolate(void **interp, void *params) {
    if((*interp) != NULL) {
        SID_free(SID_FARG((interp_info *)(*interp))->x);
        SID_free(SID_FARG((interp_info *)(*interp))->y);
        gsl_interp_free(((interp_info *)(*interp))->interp);
        gsl_interp_accel_free(((interp_info *)(*interp))->accel);
        SID_free(SID_FARG * interp);
    }
}


#ifndef TNet_Types_h
#define TNet_Types_h

#ifdef HAVE_ATLAS
extern "C"{
  #include <cblas.h>
  #include <clapack.h>
}
#endif

namespace TNet
{
  // TYPEDEFS ..................................................................
#if DOUBLEPRECISION
  typedef double  BaseFloat;
#else
  typedef float   BaseFloat;
#endif

#ifndef UINT_16
  typedef unsigned short  UINT_16   ;
  typedef unsigned        UINT_32   ;
  typedef short           INT_16    ;
  typedef int             INT_32    ;
  typedef float           FLOAT_32  ;
  typedef double          DOUBLE_64 ;
#endif

  // ...........................................................................
  // The following declaration assumes that SSE instructions are enabled
  // and that we are using GNU C/C++ compiler, which defines the __attribute__
  // notation.
  //
  // ENABLE_SSE is defined in <config.h>. Its value depends on options given
  // in the configure phase of builidng the library
#if defined(__GNUC__ )
  // vector of four single floats
  typedef float  v4sf __attribute__((vector_size(16)));
  // vector of two single doubles
  typedef double v2sd __attribute__((vector_size(16)));

  typedef BaseFloat BaseFloat16Aligned __attribute__((aligned(16))) ;

  typedef union
  {
    v4sf    v;
    float   f[4];
  } f4vector;

  typedef union
  {
    v2sd    v;
    double  f[2];
  } d2vector;
#endif // ENABLE_SSE && defined(__GNUC__ )

  typedef enum
  {
#ifdef HAVE_ATLAS
    TRANS    = CblasTrans,
    NO_TRANS = CblasNoTrans
#else
    TRANS    = 'T',
    NO_TRANS = 'N'
#endif
  } MatrixTrasposeType;

} // namespace TNet

#endif // #ifndef TNet_Types_h


#include <linux/types.h>
#include "sweeny_uf.h"
#include "extract_args.h"
#include "timeseries.h"
#include <stdio.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_rng.h> //Verwendung von Mersenne-Twister Pseudozufallszahlengenerator

#define TS_FILE_D "../time_series/uf/simulation_l%u_q%.4f_b%.4f_c%.4f_s%u.hdf5"
#define BUF_LEN 512
char fn[BUF_LEN];
char impl_title[] = "Union-Find implementation";

char verbose=0;
__u64 *four_cs_moment ;
__u64 *sec_cs_moment ;
__u32 *size_giant;
__u32 *num_bonds;
__u32 *num_cluster;
__u32 DX;
__u32 N;
__u32 seed=123456;
double q=2;
double coupling;
double beta;
double v;
double K;
__u32 steps;
__u32 cutoff;
int main(int argc, char *argv[])
{

    if(!extractArgs(argc, argv,impl_title))
      return EXIT_FAILURE;
    snprintf(fn,BUF_LEN,TS_FILE_D,DX,q,beta,coupling,seed);
    if(!init_observables())
        return EXIT_FAILURE;
    if(!init_sweeny_uf( q,DX,beta,coupling,cutoff,steps,seed,num_bonds,num_cluster,size_giant,sec_cs_moment,four_cs_moment))
        return EXIT_FAILURE;
    if(!simulate_sweeny_uf())
        return EXIT_FAILURE;
    destroy_sweeny_uf();
    save_timeseries();
    destroy_observables();
    return EXIT_SUCCESS;
}
/******************************************************************************
 *****************************************************************************/


/**
 * \file IIRFilter.h
 */

#ifndef ATK_EQ_IIRFILTER_H
#define ATK_EQ_IIRFILTER_H

#include <algorithm>
#include <cassert>
#include <vector>

#include <gsl/gsl>

#include <ATK/config.h>
#include <ATK/Core/TypeTraits.h>
#include <ATK/EQ/config.h>

namespace ATK
{
  /// IIR filter template class (Direct Form I)
  template<class Coefficients >
  class IIRFilter final : public Coefficients
  {
  protected:
    /// Simplify parent calls
    using Parent = Coefficients;
    using typename Parent::DataType;
    using typename Parent::AlignedScalarVector;
    using Parent::converted_inputs;
    using Parent::outputs;
    using Parent::coefficients_in;
    using Parent::coefficients_out;
    using Parent::input_sampling_rate;
    using Parent::output_sampling_rate;
    using Parent::nb_input_ports;
    using Parent::nb_output_ports;

    using Parent::in_order;
    using Parent::out_order;
    using Parent::input_delay;
    using Parent::output_delay;
    using Parent::setup;

  public:
    /*!
     * @brief Constructor
     * @param nb_channels is the number of input and output channels
     */
    explicit IIRFilter(gsl::index nb_channels = 1)
      :Parent(nb_channels)
    {
    }

    /// Move constructor
    IIRFilter(IIRFilter&& other)
    :Parent(std::move(other))
    {
    }

    void setup() final
    {
      Parent::setup();
      input_delay = in_order;
      output_delay = out_order;

      if (out_order > 0)
      {
        coefficients_out_2.resize(out_order, 0);
        for (unsigned int i = 1; i < out_order; ++i)
        {
          coefficients_out_2[i] = coefficients_out[out_order - 1] * coefficients_out[i] + coefficients_out[i - 1];
        }
        coefficients_out_2[0] = coefficients_out[out_order - 1] * coefficients_out[0];
      }
      if (out_order > 1)
      {
        coefficients_out_3.resize(out_order, 0);
        for (unsigned int i = 0; i < 2; ++i)
        {
          coefficients_out_3[i] = coefficients_out[out_order - 2]  * coefficients_out[i] + coefficients_out[out_order - 1] * coefficients_out_2[i];
        }
        for (unsigned int i = 2; i < out_order; ++i)
        {
          coefficients_out_3[i] = coefficients_out[out_order - 2]  * coefficients_out[i] + coefficients_out[out_order - 1] * coefficients_out_2[i] + coefficients_out[i - 2];
        }
        if (out_order > 2)
        {
          coefficients_out_4.resize(out_order, 0);
          for (unsigned int i = 0; i < 3; ++i)
          {
            coefficients_out_4[i] = coefficients_out[out_order - 3]  * coefficients_out[i] + coefficients_out[out_order - 2] * coefficients_out_2[i] + coefficients_out[out_order - 1] * coefficients_out_3[i];
          }
          for (unsigned int i = 3; i < out_order; ++i)
          {
            coefficients_out_4[i] = coefficients_out[out_order - 3]  * coefficients_out[i] + coefficients_out[out_order - 2] * coefficients_out_2[i] + coefficients_out[out_order - 1] * coefficients_out_3[i] + coefficients_out[i - 3];
          }
        }
        else // out_order = 2
        {
          coefficients_out_4.resize(out_order, 0);
          for (unsigned int i = 0; i < 2; ++i)
          {
            coefficients_out_4[i] = coefficients_out[out_order - 2] * coefficients_out_2[i] + coefficients_out[out_order - 1] * coefficients_out_3[i];
          }
        }
      }
    }

    template<typename T>
    void handle_recursive_iir(const T* ATK_RESTRICT coefficients_out_ptr, const T* ATK_RESTRICT coefficients_out_2_ptr, const T* ATK_RESTRICT coefficients_out_3_ptr, const T* ATK_RESTRICT coefficients_out_4_ptr, DataType* ATK_RESTRICT output, gsl::index size) const
    {
      gsl::index i = 0;
      if (out_order > 2)
      {
        for (i = 0; i < std::min(size - 3, size); i += 4)
        {
          DataType tempout = output[i];
          DataType tempout2 = output[i] * coefficients_out_ptr[out_order - 1] + output[i + 1];
          DataType tempout3 = output[i] * coefficients_out_ptr[out_order - 2] + tempout2 * coefficients_out_ptr[out_order - 1] + output[i + 2];
          DataType tempout4 = output[i] * coefficients_out_ptr[out_order - 3] + tempout2 * coefficients_out_ptr[out_order - 2] + tempout3 * coefficients_out_ptr[out_order - 1] + output[i + 3];

          ATK_VECTORIZE_REMAINDER for (unsigned int j = 0; j < out_order; ++j)
          {
            tempout += coefficients_out_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
            tempout2 += coefficients_out_2_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
            tempout3 += coefficients_out_3_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
            tempout4 += coefficients_out_4_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
          }
          output[i] = tempout;
          output[i + 1] = tempout2;
          output[i + 2] = tempout3;
          output[i + 3] = tempout4;
        }
      }
      else if(out_order == 2)
      {
        for (i = 0; i < std::min(size - 3, size); i += 4)
        {
          DataType tempout = output[i];
          DataType tempout2 = output[i] * coefficients_out_ptr[out_order - 1] + output[i + 1];
          DataType tempout3 = output[i] * coefficients_out_ptr[out_order - 2] + tempout2 * coefficients_out_ptr[out_order - 1] + output[i + 2];
          DataType tempout4 = tempout2 * coefficients_out_ptr[out_order - 2] + tempout3 * coefficients_out_ptr[out_order - 1] + output[i + 3];

          ATK_VECTORIZE_REMAINDER for (unsigned int j = 0; j < out_order; ++j)
          {
            tempout += coefficients_out_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
            tempout2 += coefficients_out_2_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
            tempout3 += coefficients_out_3_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
            tempout4 += coefficients_out_4_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
          }
          output[i] = tempout;
          output[i + 1] = tempout2;
          output[i + 2] = tempout3;
          output[i + 3] = tempout4;
        }
      }
      for (; i < size; ++i)
      {
        DataType tempout = output[i];
        for (unsigned int j = 0; j < out_order; ++j)
        {
          tempout += coefficients_out_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
        }
        output[i] = tempout;
      }
    }

    void process_impl(gsl::index size) const final
    {
      assert(input_sampling_rate == output_sampling_rate);
      assert(nb_input_ports == nb_output_ports);
      assert(coefficients_in.data());
      assert(out_order == 0 || coefficients_out.data() != nullptr);

      const auto* ATK_RESTRICT coefficients_in_ptr = coefficients_in.data();
      const auto* ATK_RESTRICT coefficients_out_ptr = coefficients_out.data();
      const auto* ATK_RESTRICT coefficients_out_2_ptr = coefficients_out_2.data();
      const auto* ATK_RESTRICT coefficients_out_3_ptr = coefficients_out_3.data();
      const auto* ATK_RESTRICT coefficients_out_4_ptr = coefficients_out_4.data();

      for(gsl::index channel = 0; channel < nb_input_ports; ++channel)
      {
        const DataType* ATK_RESTRICT input = converted_inputs[channel] - static_cast<int64_t>(in_order);
        DataType* ATK_RESTRICT output = outputs[channel];

        for(gsl::index i = 0; i < size; ++i)
        {
          output[i] = 0;
        }

        for (gsl::index j = 0; j < in_order + 1; ++j)
        {
          for (gsl::index i = 0; i < size; ++i)
          {
            output[i] += coefficients_in_ptr[j] * input[i + j];
          }
        }

        handle_recursive_iir(coefficients_out_ptr, coefficients_out_2_ptr, coefficients_out_3_ptr, coefficients_out_4_ptr, output, size);
      }
    }

    /// Returns the vector of internal coefficients for the MA section
    const AlignedScalarVector& get_coefficients_in() const
    {
      return coefficients_in;
    }

    /// Returns the vector of internal coefficients for the AR section, without degree 0 implicitely set to -1
    const AlignedScalarVector& get_coefficients_out() const
    {
      return coefficients_out;
    }

  protected:
    AlignedScalarVector coefficients_out_2;
    AlignedScalarVector coefficients_out_3;
    AlignedScalarVector coefficients_out_4;
  };

  /// IIR filter template class. Transposed Direct Form II implementation
  template<class Coefficients >
  class IIRTDF2Filter final : public Coefficients
  {
  public:
    /// Simplify parent calls
    using Parent = Coefficients;
    using typename Parent::DataType;
    using typename Parent::AlignedScalarVector;
    using Parent::converted_inputs;
    using Parent::outputs;
    using Parent::coefficients_in;
    using Parent::coefficients_out;
    using Parent::input_sampling_rate;
    using Parent::output_sampling_rate;
    using Parent::nb_input_ports;
    using Parent::nb_output_ports;

    using Parent::in_order;
    using Parent::out_order;
    using Parent::input_delay;
    using Parent::output_delay;
    using Parent::setup;
  protected:
    mutable typename Parent::AlignedVector state;
  public:
    explicit IIRTDF2Filter(gsl::index nb_channels = 1)
      :Parent(nb_channels)
    {
    }

    /// Move constructor
    IIRTDF2Filter(IIRTDF2Filter&& other)
      :Parent(std::move(other))
    {
    }

    void setup() final
    {
      Parent::setup();
      input_delay = in_order;
      output_delay = out_order;
      state.assign(nb_input_ports * (std::max(input_delay, output_delay) + 1), TypeTraits<DataType>::Zero());
    }

    void process_impl(gsl::index size) const final
    {
      assert(input_sampling_rate == output_sampling_rate);

      for(gsl::index channel = 0; channel < nb_input_ports; ++channel)
      {
        const DataType* ATK_RESTRICT input = converted_inputs[channel];
        DataType* ATK_RESTRICT output = outputs[channel];
        DataType* ATK_RESTRICT current_state = &state[channel * (std::max(input_delay, output_delay) + 1)];

        for(gsl::index i = 0; i < size; ++i)
        {
          output[i] = coefficients_in[in_order] * input[i] + current_state[0];
          auto min_order = std::min(input_delay, output_delay);

          for(gsl::index j = 0; j < min_order; ++j)
          {
            current_state[j] = current_state[j + 1] + input[i] * coefficients_in[in_order - static_cast<int64_t>(j) - 1] + output[i] * coefficients_out[out_order - static_cast<int64_t>(j) - 1];
          }
          for(gsl::index j = min_order; j < input_delay; ++j)
          {
            current_state[j] = current_state[j + 1] + input[i] * coefficients_in[in_order - static_cast<int64_t>(j) - 1];
          }
          for(gsl::index j = min_order; j < output_delay; ++j)
          {
            current_state[j] = current_state[j + 1] + output[i] * coefficients_out[out_order - static_cast<int64_t>(j) - 1];
          }
        }
      }
    }

    /// Returns the vector of internal coefficients for the MA section
    const AlignedScalarVector& get_coefficients_in() const
    {
      return coefficients_in;
    }

    /// Returns the vector of internal coefficients for the AR section, without degree 0 implicitely set to -1
    const AlignedScalarVector& get_coefficients_out() const
    {
      return coefficients_out;
    }
  };

}

#endif


/* specfunc/elementary.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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_elementary.h"

#include "error.h"
#include "check.h"

int
gsl_sf_multiply_e(const double x, const double y, gsl_sf_result * result)
{
  const double ax = fabs(x);
  const double ay = fabs(y);

  if(x == 0.0 || y == 0.0) {
    /* It is necessary to eliminate this immediately.
     */
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if((ax <= 1.0 && ay >= 1.0) || (ay <= 1.0 && ax >= 1.0)) {
    /* Straddling 1.0 is always safe.
     */
    result->val = x*y;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double f = 1.0 - 2.0 * GSL_DBL_EPSILON;
    const double min = GSL_MIN_DBL(fabs(x), fabs(y));
    const double max = GSL_MAX_DBL(fabs(x), fabs(y));
    if(max < 0.9 * GSL_SQRT_DBL_MAX || min < (f * DBL_MAX)/max) {
      result->val = GSL_COERCE_DBL(x*y);
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      CHECK_UNDERFLOW(result);
      return GSL_SUCCESS;
    }
    else {
      OVERFLOW_ERROR(result);
    }
  }
}

int
gsl_sf_multiply_err_e(const double x, const double dx,
                         const double y, const double dy,
                         gsl_sf_result * result)
{
  int status = gsl_sf_multiply_e(x, y, result);
  result->err += fabs(dx*y) + fabs(dy*x);
  return status;
}

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

#include "eval.h"

double gsl_sf_multiply(const double x, const double y)
{
  EVAL_RESULT(gsl_sf_multiply_e(x, y, &result));
}


#include <viaio/VImage.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include "gsl_utils.h"

#define ABS(x) ((x) >= 0) ? (x) : -(x)
#define MIN(a,b) ((a) < (b)) ? (a) : (b)
#define MAX(a,b) ((a) > (b)) ? (a) : (b)
#define ROUND(x) (int)(((x) >= 0) ? ((x) + 0.5) : ((x) - 0.5))

void whitecov2(VImage effect_image,VImage rho_vol,
	       gsl_matrix_float* Y, gsl_matrix_float* X, gsl_matrix_float* con,
	       VFloat* Dfs, int numlags, int slice)
{
  int numcon = 1;
  int n = Y->size1;
  VFloat Df = Dfs[numcon];

  int i=0,j=0,k=0;

  /* some checks and warnings */
  if(X->size2 != n)
    VError("whitecov2: Warning! sizes of design and data matrix do not match, n=%d, %ld",n,X->size2);

  if(X->size1 != con->size2)
    VError("whitecov2: Warning! contrast and design do not match.");

  gsl_vector_float* irho = gsl_vector_float_alloc(VImageNBands(rho_vol));
  if(numlags == 1) {
    float* p = irho->data;
    const float drho = 0.01;
    for(i=0;i<irho->size;i++) {
      float tmp = ROUND((VPixel(rho_vol, i, slice, 0, VFloat) / drho)) * drho;
      tmp = MIN(tmp, (1-drho));
      tmp = MAX(tmp, (-1+drho));
      *p++ = tmp;
    }
  }
  else {
    float* p = irho->data;
    for(i=1;i<=irho->size;i++)
      *p++ = i;
  }

  /* : X=X';  */
  gsl_matrix_float* transX = gsl_matrix_float_alloc(X->size2, X->size1);
  gsl_matrix_float_transpose_memcpy(transX, X);

  /* : Xstar=X; */
  gsl_matrix_float* XStar = gsl_matrix_float_alloc(transX->size1, transX->size2);
  gsl_matrix_float_memcpy(XStar, transX);

  /* END allocate memory buffer */

  /****************************** order == 1 *****************************/
  if (numlags == 1) {

    /* allocate memory buffers */
    gsl_vector_float* pixBig = gsl_vector_float_alloc(irho->size);
    gsl_matrix_float* pinvXStar = gsl_matrix_float_alloc(XStar->size2, XStar->size1);
    gsl_matrix_float* V = gsl_matrix_float_alloc(pinvXStar->size1, pinvXStar->size1);
    gsl_matrix_float* buffer_01 = gsl_matrix_float_alloc(con->size1,V->size2);
    gsl_matrix_float* buffer_02 = gsl_matrix_float_alloc(con->size1,con->size1);
    gsl_matrix_float* cVcinv = gsl_matrix_float_alloc(con->size1, con->size1);

    gsl_vector_float* u = funique(irho);
    float* rho = u->data;
    for(i=0;i<u->size;i++) {

      /* :  pix=int16(find(irho==rho)); */
      float* p = irho->data;
      float* ppB = pixBig->data;
      int counter = 0;
      for(j=0;j<irho->size;j++) {
	if(*p++ == *rho) {
	  *ppB++ = (float)j;
	  counter++;
	}
      }
      gsl_vector_float* pix = gsl_vector_float_alloc(counter);
      ppB = pixBig->data;
      float* pp = pix->data;
      for(j=0;j<counter;j++) *pp++ = *ppB++;

      /* : Ystar=Y(:,pix); */
      gsl_matrix_float* YStar = fmat_subcols(Y,pix);

      /* : factor=1./sqrt(1-rho^2); */
      float factor = pow(1 - *rho * *rho, -0.5);

      /* : Ystar(k1,:)=(Y(k1,pix)-rho*Y(k1-1,pix))*factor; */
      /* point to last element */
      float* pStar = YStar->data+(YStar->size1*YStar->size2)-1;
      /* traverse matrix in reverse order */
      for (j = 0; j < (YStar->size1-1)*YStar->size2; ++j) {
	*pStar = (*pStar - *rho * *(pStar - YStar->size2)) * factor;
	pStar--;
      }

      /* : Xstar(k1,:)=(X(k1,:)-rho*X(k1-1,:))*factor; */
      pStar = XStar->data+XStar->size2;
      float* pX = transX->data+transX->size2;
      for (j = 0; j < (XStar->size1-1)*XStar->size2; ++j) {
	*pStar++ = (*pX - *rho * *(pX - transX->size2)) * factor;
	pX++;
      }

      /* : pinvXstar=pinv(Xstar); */
      fmat_PseudoInv(XStar,pinvXStar);

      /* : betahat=pinvXstar*Ystar; */
      gsl_matrix_float* betahat = fmat_x_mat(pinvXStar, YStar,NULL);

      /* : resid=Ystar-Xstar*betahat; */
      gsl_matrix_float* buffer = fmat_x_mat(XStar, betahat, NULL);
      gsl_matrix_float_sub(YStar, buffer);
      gsl_matrix_float* resid =  YStar;
      gsl_matrix_float_free(buffer);

      /* : SSE=sum(resid.^2,1); */
      buffer = gsl_matrix_float_alloc(resid->size1, resid->size2);
      gsl_matrix_float_memcpy (buffer, resid);
      gsl_matrix_float_mul_elements (buffer, buffer);
      gsl_vector_float* sse = fsum(buffer, 1,NULL);
      gsl_matrix_float_free(buffer);

      /* : sd=sqrt(SSE/Df); */
      gsl_vector_float* sd = gsl_vector_float_alloc(sse->size);
      float* pSSE = sse->data;
      float* pSD = sd->data;
      for (j = 0; j < sse->size; ++j) {
	*pSD++ = (float)sqrt(*pSSE++/Df);
      }

      /* :   V=pinvXstar*pinvXstar'; */
      fmat_x_matT(pinvXStar, pinvXStar, V);

      /* : mag_ef=contrast*betahat; */
      gsl_matrix_float* mag_ef = fmat_x_mat(con, betahat, NULL);

      /* : mag_sd=sqrt(diag(contrast*V*contrast'))*sd; */
      fmat_x_mat(con,V,buffer_01);
      fmat_x_matT(buffer_01, con, buffer_02);

      gsl_vector_float_view diag = gsl_matrix_float_diagonal(buffer_02);
      gsl_matrix_float* mag_sd = gsl_matrix_float_alloc(diag.vector.size, sd->size);
      float* pMag = mag_sd->data;
      for (j = 0; j < diag.vector.size; ++j) {
	float s = sqrt(gsl_vector_float_get(&diag.vector,j));
	pSD = sd->data;
	for (k = 0; k < sd->size; ++k) {
	  *pMag++ = s * (*pSD++);
	}
      }

      /* effect_slice(pix,1)= (mag_ef./(mag_sd+(mag_sd<=0)).*(mag_sd>0))'; */
      VFloat* peff = VPixelPtr(effect_image,slice,0,0);
      float* pmeff = mag_ef->data;
      float* pmsd = mag_sd->data;
      float* ppix = pix->data;
      for (j=0; j<pix->size; ++j) {
	*(peff+(int)*ppix++) = *pmeff / (*pmsd+(*pmsd<=0)) * (*pmsd>0);
	pmeff++; pmsd++;
      }

      /* free some memory */
      gsl_matrix_float_free(betahat);
      gsl_matrix_float_free(YStar);
      gsl_matrix_float_free(mag_ef);
      gsl_matrix_float_free(mag_sd);
      gsl_vector_float_free(pix);
      gsl_vector_float_free(sse);
      gsl_vector_float_free(sd);

      rho++;
    } /* END for(i=0;i<u->size;i++)  */

    /* free some memory */
    gsl_vector_float_free(pixBig);
    gsl_matrix_float_free(pinvXStar);
    gsl_matrix_float_free(V);
    gsl_matrix_float_free(buffer_01);
    gsl_matrix_float_free(buffer_02);
    gsl_matrix_float_free(cVcinv);

  } /******************* END order == 1 **********************************/

/*************************** order > 1 *********************************/
    else{

      /* allocate memory buffer */
      gsl_vector_float* Coradj_pix = gsl_vector_float_alloc(VImageNColumns(rho_vol)+1);
      gsl_matrix_float* Ainv = gsl_matrix_float_alloc(Coradj_pix->size, Coradj_pix->size);
      gsl_matrix* dbuff = gsl_matrix_alloc(Ainv->size1,Ainv->size2);
      gsl_matrix_float* A = gsl_matrix_float_alloc(Ainv->size1,Ainv->size2);
      int nl = Ainv->size2;
      gsl_matrix_float* buffer_01 = gsl_matrix_float_alloc(n-nl,1);
      gsl_matrix_float* buffer_02 = gsl_matrix_float_alloc(A->size1,1);
      gsl_matrix_float* buffer_03 = gsl_matrix_float_alloc(A->size1,transX->size2);
      gsl_matrix_float* B = gsl_matrix_float_alloc(buffer_01->size1, A->size2);
      gsl_matrix_float* Vmhalf = gsl_matrix_float_alloc(n-nl,n);
      gsl_matrix_float* buffer_04 = gsl_matrix_float_alloc(Vmhalf->size1, transX->size2);
      gsl_matrix_float* YStar = gsl_matrix_float_alloc(n,1);
      gsl_matrix_float* pinvXStar = gsl_matrix_float_alloc(XStar->size2, XStar->size1);
      gsl_matrix_float* betahat = gsl_matrix_float_alloc(pinvXStar->size1, YStar->size2);
      gsl_matrix_float* buffer_05 = gsl_matrix_float_alloc(XStar->size1, betahat->size2);
      gsl_vector_float* sse = gsl_vector_float_alloc(1);
      gsl_vector_float* sd = gsl_vector_float_alloc(YStar->size2);
      gsl_matrix_float* V = gsl_matrix_float_alloc(pinvXStar->size1, pinvXStar->size1);
      gsl_matrix_float* mag_ef = gsl_matrix_float_alloc(con->size1, betahat->size2);
      gsl_matrix_float* buffer_06 = gsl_matrix_float_alloc(con->size1, V->size2);
      gsl_matrix_float* buffer_07 = gsl_matrix_float_alloc(con->size1, con->size1);
      gsl_matrix_float* mag_sd = gsl_matrix_float_alloc(con->size1, sd->size);
      gsl_matrix_float* buffer_08 = gsl_matrix_float_alloc(mag_ef->size1, mag_ef->size2);
      gsl_vector_float* sst = gsl_vector_float_alloc(1);
      /* END allocate memory buffer */

     /* we know that irho only contains a list of indices for every voxel so we can skip
        the unique call and iterate over irho itself */
      float* rho = irho->data;
      for(i=0;i<irho->size;i++) {

        /* : Coradj_pix=squeeze(rho_vol(pix,slice,:)); */
        float* pCoradj = Coradj_pix->data+1;
        for(k=0;k<Coradj_pix->size-1;k++) {
          *(pCoradj++) = VPixel(rho_vol,
                                i,
                                slice,
                                k,
                                VFloat);
        }

        /* : [Ainvt posdef]=chol(toeplitz([1 Coradj_pix'])); */

        /* at first the toeplitz matrix */
        Coradj_pix->data[0] = 1;
        fmat_toeplitz(Coradj_pix, Ainv);
        /* the cholesky decomposition */
        /* double buffer */
        for(j=0;j<Ainv->size1;j++) {
          for(k=0;k<Ainv->size2;k++) {
            gsl_matrix_set(dbuff,j,k,(double)gsl_matrix_float_get(Ainv,j,k));
          }
        }

        if(gsl_linalg_cholesky_decomp(dbuff) == GSL_EDOM)
          VError("Calculation error, cholesky decomposition failed for pix=%d",i);

        /* note: the gsl cholesky factorization returns the inverse
         * of Ainvt in the lower triangular part of it's output matrix.
         * Since we will use the inverse matrix of Ainvt in the next steps we
         * will work with the lower triangular part in contrast to the matlab algorithm */

        /* convert back to float and remove upper triangular matrix */
        for(j=0;j<Ainv->size1;j++) {
          for(k=0;k<Ainv->size2;k++) {
            if(j<k) {
              gsl_matrix_float_set(Ainv,j,k,0);
            }
            else {
              gsl_matrix_float_set(Ainv,j,k,(float)gsl_matrix_get(dbuff,j,k));
            }
          }
        }

        /* :  A=inv(Ainvt'); */
        fInv(Ainv, A);

        /* : B=ones(n-nl,1)*A(nl,:); */
	gsl_matrix_float_set_all(buffer_01,1);
        gsl_matrix_float_view subm = gsl_matrix_float_submatrix(A,nl-1,0,1,A->size2);
        fmat_x_mat(buffer_01,&subm.matrix,B);

        /* : Vmhalf=spdiags(double(B),double(1:nl),double(n-nl),double(n)); */
        gsl_matrix_float_set_zero(Vmhalf);
        for(j=0;j<B->size1;j++) {
          for(k=0;k<B->size2;k++) {
            gsl_matrix_float_set(Vmhalf,j,k+1+j,gsl_matrix_float_get(B,j,k));
          }
        }

        /* : Ystar=single(zeros(n,1)); */
        gsl_matrix_float_set_zero(YStar);

        /* : Ystar(1:nl)=A*Y(1:nl,pix); */
        subm = gsl_matrix_float_submatrix(Y,0,i,nl,1);
        fmat_x_mat(A,&subm.matrix,buffer_02);
        for(j=0;j<nl;j++) {
          gsl_matrix_float_set(YStar,j,0,gsl_matrix_float_get(buffer_02,j,0));
        }

	/* : Ystar((nl+1):n)=single(Vmhalf*double(Y(:,pix))); */
	subm = gsl_matrix_float_submatrix(Y,0,i,Y->size1,1);
	fmat_x_mat(Vmhalf,&subm.matrix,buffer_01);
	for(j=nl;j<n;j++) {
	  gsl_matrix_float_set(YStar,j,0,gsl_matrix_float_get(buffer_01,j-nl,0));
	}

        /* : Xstar(1:nl,:)=A*X(1:nl,:); */
        subm = gsl_matrix_float_submatrix(transX,0,0,nl,transX->size2);
        fmat_x_mat(A, &subm.matrix, buffer_03);
        for(j=0;j<buffer_03->size1;j++) {

          for(k=0;k<buffer_03->size2;k++) {
            gsl_matrix_float_set(XStar,j,k,gsl_matrix_float_get(buffer_03,j,k));
          }
        }

        /* : Xstar((nl+1):n,:)=single(Vmhalf*double(X)); */
        fmat_x_mat(Vmhalf,transX,buffer_04);
        for(j=nl;j<n;j++) {
          for(k=0;k<transX->size2;k++) {
            gsl_matrix_float_set(XStar,j,k,gsl_matrix_float_get(buffer_04,j-nl,k));
          }
        }

        /* : pinvXstar=pinv(Xstar); */
        fmat_PseudoInv(XStar,pinvXStar);

        /* : betahat=pinvXstar*Ystar; */
        fmat_x_mat(pinvXStar, YStar,betahat);

        /* : resid=Ystar-Xstar*betahat; */
        fmat_x_mat(XStar, betahat, buffer_05);
        gsl_matrix_float_sub(YStar, buffer_05);
        gsl_matrix_float* resid =  YStar;

        /* : SSE=sum(resid.^2,1); */
        gsl_matrix_float_mul_elements (resid, resid);
        fsum(resid, 1,sse);

        /* : sd=sqrt(SSE/Df); */
        float* pSSE = sse->data;
        float* pSD = sd->data;
        for (j = 0; j < sse->size; ++j) {
          *pSD++ = (float)sqrt(*pSSE++/Df);
        }

        /* :   V=pinvXstar*pinvXstar'; */
        fmat_x_matT(pinvXStar, pinvXStar, V);

        /* : mag_ef=contrast*betahat; */
        fmat_x_mat(con, betahat, mag_ef);

        /* : mag_sd=sqrt(diag(contrast*V*contrast'))*sd; */
        fmat_x_mat(con,V,buffer_06);
        fmat_x_matT(buffer_06, con, buffer_07);

        gsl_vector_float_view diag = gsl_matrix_float_diagonal(buffer_07);
        float* pMag = mag_sd->data;
        for (j = 0; j < diag.vector.size; ++j) {
          float s = sqrt(gsl_vector_float_get(&diag.vector,j));
          pSD = sd->data;
          for (k = 0; k < sd->size; ++k) {
            *pMag++ = s * *pSD++;
          }
        }

	/* effect_slice(pix,1)= (mag_ef./(mag_sd+(mag_sd<=0)).*(mag_sd>0))'; */
	VFloat* peff = VPixelPtr(effect_image, slice, 0, 0);
	float* pmeff = mag_ef->data;
	float* pmsd = mag_sd->data;
	*(peff+i) = *pmeff / (*pmsd+(*pmsd<=0)) * (*pmsd>0);

        /* next value */
        rho++;
      } /* END for(i=0;i<irho->size;i++) */

      /* free some memory */
      gsl_vector_float_free(Coradj_pix);
      gsl_matrix_float_free(Ainv);
      gsl_matrix_free(dbuff);
      gsl_matrix_float_free(A);
      gsl_matrix_float_free(buffer_01);
      gsl_matrix_float_free(buffer_02);
      gsl_matrix_float_free(buffer_03);
      gsl_matrix_float_free(buffer_04);
      gsl_matrix_float_free(B);
      gsl_matrix_float_free(Vmhalf);
      gsl_matrix_float_free(YStar);
      gsl_matrix_float_free(pinvXStar);
      gsl_matrix_float_free(buffer_05);
      gsl_vector_float_free(sd);
      gsl_matrix_float_free(V);
      gsl_matrix_float_free(mag_ef);
      gsl_matrix_float_free(buffer_06);
      gsl_matrix_float_free(buffer_07);
      gsl_matrix_float_free(mag_sd);
      gsl_matrix_float_free(buffer_08);
      gsl_vector_float_free(sse);
      gsl_vector_float_free(sst);

    }/************************************** END order > 1 ***************************************/

  /* :
   * minus = ones(size(effect_slice),'single');
   * minus(find(effect_slice<0))=single(-1.0);
   * effect_slice=single(sqrt(Df .* log(1 + effect_slice.^2 ./ Df) .* (1 - 0.5 ./ Df)));
   * effect_slice=effect_slice .* minus;
   */
  double u=0;
  VFloat* peff = VPixelPtr(effect_image, slice,0,0);
  for (i = 0; i < VImageNColumns(effect_image) * VImageNRows(effect_image); ++i) {
    VFloat oldVal = *peff;
    Df = Dfs[0];
    u = (double)(*peff);
    u = sqrt(Df * log(1.0 + pow(u,2.0) / Df) * (1 - 0.5 / Df));
    u = (oldVal < 0) ? u * (-1.0) : u;
    if (gsl_isnan(u) || gsl_isinf(u)) u = 0;
    (*peff) = (float)u;
    peff++;
  }

  /* free allocated memory */
  gsl_matrix_float_free(XStar);
  gsl_matrix_float_free(transX);
  /* END free allocated memory */
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     mpepc_pose_follower.h
* \author   Jong Jin Park
*
* Declaration of MPEPCPoseFollower.
*/

#ifndef MPEPC_POSE_FOLLOWER_H
#define MPEPC_POSE_FOLLOWER_H

#include <mpepc/trajectory/params.h>
#include <mpepc/trajectory/robot_trajectory_info.h>
#include <core/pose.h>
#include <mpepc/control/control_law_coordinates.h>
#include <nlopt.h>
#include <vector>
#include <memory>

namespace vulcan
{

namespace mpepc
{

class  RobotSimulator;
class  TrajectoryEvaluator;
class  TaskManifold;
struct trajectory_planner_debug_info_t;

/**
* MPEPCPoseFollower optimizes velocity gain given a pose to converge to.
*
* The optimizer handles the simulate/evaluate loop internally and finds the optimal
* target along with the intermediate trajectories generated by the optimization
* algorithm. NLopt package is used for the numerical optimzation, and a number of
* different algorithms can be implemented.
*/
class MPEPCPoseFollower
{

public:

    /**
    * Constructor for MPEPCPoseFollower.
    *
    * \param    params          Parameters for the optimization
    */
    MPEPCPoseFollower(const mpepc_optimizer_params_t& params);

    /**
    * Destructor for MPEPCPoseFollower.
    */
    ~MPEPCPoseFollower(void);

    /**
    * Setup sets up the optmizer with the robot simulator and trajectory evaluator to be used.
    *
    * \param    simulator       RobotSimulator to use for the trajectory generation.
    * \param    evaluator       TrajectoryEvaluator for calculating the cost of a trajectory.
    */
    void setup(RobotSimulator& simulator, TrajectoryEvaluator& evaluator); // NOTE: This feels very unsafe. A better way?

    /**
    * setInitialGuesses adds a goal pose (if navigation task) and the previous optimum
    * to the initial guesses for local optimizer.
    *
    * \param    task                 TaskManifold that contains the goal.
    * \param    previousMotionTarget Previous output of the metric planner, i.e. the previous optimum.
    */
    void setInitialGuesses(const TaskManifold&    task,
                           const motion_target_t& previousMotionTarget);

    /**
    * run runs the optimizer and finds the best motion target by evaluating the expected
    * costs of an estimated trajectory generated from a candidate motion target.
    *
    * It returns the optimal target found, while the intermediate information including
    * all trajectories considered are stored in the provided debug info instance.
    *
    * \param    debugInfo        Place to store relevant information
    * \return   Optimal target found
    */
    motion_target_t runOptimizer(trajectory_planner_debug_info_t& debugInfo);

    /**
    * evaluateCost calculates the cost of a fixed-length trajectory represented by the given optimizer_target_t.
    * The length of the trajectory is specified in the given instance of the planner_data_t.
    *
    * \param    coords          Optimization variable, currently encoding a target pose in the robot frame and a velocity gain for the kinematic control law.
    * \return   Cost of the trajectory.
    */
    double evaluateCost(const double x[]);

    /**
    * evaluateGradient calculates the gradient around a particular point, by a simple forward differencing.
    *
    * \param    coords          Optimization variable, currently encoding a target pose in the robot frame and a velocity gain for the kinematic control law.
    * \param    cost            Evaluated cost at the given coordinate.
    * \param    gradient        Calculated gradient for each dimension (r, theta, delta, velocity) [output]
    */
    void evaluateGradient(const double x[], double cost, double gradient[]);

    bool haveFoundSolution(void) const { return haveFoundSolution_; };

//     /**
//     * evaluateSingleMotionTarget is a method which resets the data and info struct and evaluate a single optimizer target.
//     * The optimization_info_t will hold a single trajectory considered. TODO: Do i need this?
//     *
//     * \param candidateMotionTarget
//     * \param data
//     * \param info
//     */
//     double evaluateSingleMotionTarget(const motion_target_t& candidateMotionTarget, const planner_data_t& data, optimization_info_t& info);

private:

    // setting up the optimizer
    void setupNLOpt(void);

    // optimizers
    nlopt_opt globalOptimizer;
    nlopt_opt localOptimizer;

    // simulator and evaluator
    RobotSimulator*      simulator_;
    TrajectoryEvaluator* evaluator_;

    robot_trajectory_info_t trajectory;

    // debug info
    trajectory_planner_debug_info_t* optimizerInfo_;

    // inital robot state
    pose_t robotPose_;

    // target pose to converge to
    pose_t   targetPose_;
    motion_target_t candidateMotionTarget_;

    // indicator for finding a right solution
    bool haveFoundSolution_;

    // parameters
    mpepc_optimizer_params_t params_;
};

} // mpepc
} // vulcan

#endif // MPEPC_POSE_FOLLOWER_H


#ifndef BIO_MATRIX_DEPENDENCIES_H_
#define BIO_MATRIX_DEPENDENCIES_H_

#include "bio/defs.h"
#include "bio/matrix.h"
#include "bio/contingency_homogeneity.h"

#include <boost/array.hpp>

#include <gsl/gsl_math.h>

#include <vector>
#include <numeric>
#include <set>

BIO_NS_START

/** Indexes a pair of bases for comparison. */
struct BasePair
{
	/** The index of the base we wish to analyse. */
	unsigned observed_index;

	/** The index of the base we condition on. */
	unsigned conditioned_index;

	BasePair(unsigned observed_index, unsigned conditioned_index);

	bool operator<(const BasePair & rhs) const;
};

std::ostream &
operator<<(std::ostream & os, const BasePair & base_pair);

/** A contingency table to compare a pair of bases. */
typedef ContingencyTable<4, 4> base_pair_contingency_table_t;

/** The contingency tables for all base pairs in a pssm. */
typedef std::map<BasePair, base_pair_contingency_table_t> pssm_contingency_tables_t;

/** The results of analysing the dependencies between a pair of bases. */
struct BasePairDependencyResults
{
	double homogeneity_bayes_factor;
};

/** The results of the dependencies between all base pairs in a pssm. */
typedef std::map< BasePair, BasePairDependencyResults > pssm_dependency_results_t;

/** An iterator pointing to a result. */
typedef pssm_dependency_results_t::const_iterator pssm_dependency_result_it;

/** A less than operator that lets us sort by scores. */
struct HomogeneityLessThen
{
	bool operator()(pssm_dependency_result_it lhs, pssm_dependency_result_it rhs) const;
};

/** An ordered set of results. */
typedef std::set< pssm_dependency_result_it, HomogeneityLessThen > pssm_dependency_result_set;

/** Contains all the sequences that were used to compose a Pssm. Indexed first by position then by sequence number. */
typedef std::vector< std::vector<char> > pssm_source_matrix_t;
typedef boost::shared_ptr< pssm_source_matrix_t > pssm_source_matrix_ptr_t;
typedef std::map< TableLink, pssm_source_matrix_ptr_t > pssm_source_map_t;

std::ostream &
operator<<(std::ostream & os, pssm_source_map_t::value_type value);

void
build_all_pssm_sources(pssm_source_map_t & pssm_source_map);

/** Take a Transfac matrix and generate a matrix from the sequences that defined it. */
void
build_pssm_source(
	const Matrix * matrix,
	pssm_source_matrix_t & pssm_source);

/** Adds a sequence to a pssm source. */
void
add_sequence_to_pssm_source(
	const seq_t & sequence,
	pssm_source_matrix_t & pssm_source);

/** Count the conditional frequencies in a matrix and generate contingency tables. */
void
count_frequencies(
	const pssm_source_matrix_t & pssm_source,
	pssm_contingency_tables_t & contingency_tables);

/** Calculate the dependencies in all the contingency tables. */
void
calculate_dependencies(
	pssm_contingency_tables_t & contingency_tables,
	pssm_dependency_results_t & results);

/** Build an ordered set of results. */
void
build_result_set(
	pssm_dependency_results_t & results,
	pssm_dependency_result_set & result_set);

struct MatrixDependencies
{
	pssm_source_matrix_t pssm_source;
	pssm_contingency_tables_t contingency_tables;
	pssm_dependency_results_t results;
	pssm_dependency_result_set result_set;

	MatrixDependencies(const pssm_source_matrix_t & pssm_source);
	MatrixDependencies(const Matrix * matrix);
	MatrixDependencies();
};

typedef std::map<const Matrix *, MatrixDependencies> matrix_dependencies_map_t;

BIO_NS_END

#endif //BIO_MATRIX_DEPENDENCIES_H_