#if !defined(lowMachFlow_h)
#define lowMachFlow_h
#include <petsc.h>

// Define the test field number
typedef enum { VTEST, QTEST, WTEST } LowMachFlowTestFields;

typedef enum { STROUHAL, REYNOLDS, FROUDE, PECLET, HEATRELEASE, GAMMA, PTH, MU, K, CP, BETA, GRAVITY_DIRECTION, TOTAL_LOW_MACH_FLOW_PARAMETERS } LowMachFlowParametersTypes;
PETSC_EXTERN const char* lowMachFlowParametersTypeNames[TOTAL_LOW_MACH_FLOW_PARAMETERS + 1];

typedef enum {VEL, PRES, TEMP, TOTAL_LOW_MACH_FLOW_FIELDS} LowMachFlowFields;

typedef enum {MOM, MASS, ENERGY, TOTAL_LOW_MACH_SOURCE_FIELDS} LowMachSourceFields;

// Low Mach Kernels
PETSC_EXTERN void LowMachFlow_vIntegrandTestFunction(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, const PetscReal X[],
    PetscInt numConstants, const PetscScalar constants[], PetscScalar f0[]);

PETSC_EXTERN void LowMachFlow_vIntegrandTestGradientFunction(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[],
                                                             const PetscScalar u_x[], const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[],
                                                             PetscReal t, const PetscReal X[], PetscInt numConstants, const PetscScalar constants[], PetscScalar f1[]);

PETSC_EXTERN void LowMachFlow_wIntegrandTestFunction(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                                     const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, const PetscReal X[],
                                                     PetscInt numConstants, const PetscScalar constants[], PetscScalar f0[]);

PETSC_EXTERN void LowMachFlow_wIntegrandTestGradientFunction(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[],
                                                             const PetscScalar u_x[], const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[],
                                                             PetscReal t, const PetscReal X[], PetscInt numConstants, const PetscScalar constants[], PetscScalar f1[]);

PETSC_EXTERN void LowMachFlow_qIntegrandTestFunction(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                                     const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, const PetscReal X[],
                                                     PetscInt numConstants, const PetscScalar constants[], PetscScalar f0[]);

PETSC_EXTERN void LowMachFlow_g0_qu(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                  const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                  PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g1_qu(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g0_qT(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g0_qT(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g1_qT(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g0_vT(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g0_vu(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g1_vu(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g3_vu(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g2_vp(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g0_wu(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g0_wT(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g1_wT(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

PETSC_EXTERN void LowMachFlow_g3_wT(PetscInt dim, PetscInt Nf, PetscInt NfAux, const PetscInt uOff[], const PetscInt uOff_x[], const PetscScalar u[], const PetscScalar u_t[], const PetscScalar u_x[],
                                    const PetscInt aOff[], const PetscInt aOff_x[], const PetscScalar a[], const PetscScalar a_t[], const PetscScalar a_x[], PetscReal t, PetscReal u_tShift, const PetscReal x[],
                                    PetscInt numConstants, const PetscScalar constants[], PetscScalar g0[]);

#endif


/* multifit_nlinear/lm.c
 *
 * Copyright (C) 2014, 2015, 2016 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_permutation.h>

/*
 * This module contains an implementation of the Levenberg-Marquardt
 * algorithm for nonlinear optimization problems. This implementation
 * closely follows the following works:
 *
 * [1] H. B. Nielsen, K. Madsen, Introduction to Optimization and
 *     Data Fitting, Informatics and Mathematical Modeling,
 *     Technical University of Denmark (DTU), 2010.
 *
 * [2] J. J. More, The Levenberg-Marquardt Algorithm: Implementation
 *     and Theory, Lecture Notes in Mathematics, v630, 1978.
 */

typedef struct
{
  size_t n;           /* number of observations */
  size_t p;           /* number of parameters */
  gsl_vector *fvv;    /* D_v^2 f(x), size n */
  gsl_vector *vel;    /* geodesic velocity (standard LM step), size p */
  gsl_vector *acc;    /* geodesic acceleration, size p */
  gsl_vector *workp;  /* workspace, length p */
  gsl_vector *workn;  /* workspace, length n */

  int accel;          /* use geodesic acceleration? */

  /* tunable parameters */
  gsl_multifit_nlinear_parameters params;
} lm_state_t;

#include "common.c"

static void *lm_alloc (const int accel, const void * params, const size_t n, const size_t p);
static void *lm_alloc_noaccel (const void * params, const size_t n, const size_t p);
static void *lm_alloc_accel (const void * params, const size_t n, const size_t p);
static void lm_free(void *vstate);
static int lm_init(const void *vtrust_state, void *vstate);
static int lm_preloop(const void * vtrust_state, void * vstate);
static int lm_step(const void * vtrust_state, const double delta,
                   gsl_vector * dx, void * vstate);
static int lm_preduction(const void * vtrust_state, const gsl_vector * dx,
                         double * pred, void * vstate);

static void *
lm_alloc (const int accel, const void * params, const size_t n, const size_t p)
{
  const gsl_multifit_nlinear_parameters *mparams = (const gsl_multifit_nlinear_parameters *) params;
  lm_state_t *state;

  state = calloc(1, sizeof(lm_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate lm state", GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp", GSL_ENOMEM);
    }

  state->workn = gsl_vector_alloc(n);
  if (state->workn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workn", GSL_ENOMEM);
    }

  state->fvv = gsl_vector_alloc(n);
  if (state->fvv == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for fvv", GSL_ENOMEM);
    }

  state->vel = gsl_vector_alloc(p);
  if (state->vel == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for vel", GSL_ENOMEM);
    }

  state->acc = gsl_vector_alloc(p);
  if (state->acc == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for acc", GSL_ENOMEM);
    }

  state->n = n;
  state->p = p;
  state->params = *mparams;
  state->accel = accel;

  return state;
}

static void *
lm_alloc_noaccel (const void * params, const size_t n, const size_t p)
{
  return lm_alloc(0, params, n, p);
}

static void *
lm_alloc_accel (const void * params, const size_t n, const size_t p)
{
  return lm_alloc(1, params, n, p);
}

static void
lm_free(void *vstate)
{
  lm_state_t *state = (lm_state_t *) vstate;

  if (state->workp)
    gsl_vector_free(state->workp);

  if (state->workn)
    gsl_vector_free(state->workn);

  if (state->fvv)
    gsl_vector_free(state->fvv);

  if (state->vel)
    gsl_vector_free(state->vel);

  if (state->acc)
    gsl_vector_free(state->acc);

  free(state);
}

/*
lm_init()
  Initialize LM solver

Inputs: vtrust_state - trust state
        vstate       - workspace

Return: success/error
*/

static int
lm_init(const void *vtrust_state, void *vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  lm_state_t *state = (lm_state_t *) vstate;

  gsl_vector_set_zero(state->vel);
  gsl_vector_set_zero(state->acc);

  *(trust_state->avratio) = 0.0;

  return GSL_SUCCESS;
}

/*
lm_preloop()
  Initialize LM method for new Jacobian matrix
*/

static int
lm_preloop(const void * vtrust_state, void * vstate)
{
  int status;
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  const gsl_multifit_nlinear_parameters *params = trust_state->params;

  (void)vstate;

  /* initialize linear least squares solver */
  status = (params->solver->init)(trust_state, trust_state->solver_state);
  if (status)
    return status;

  return GSL_SUCCESS;
}

/*
lm_step()
  Calculate a new step vector by solving the linear
least squares system:

[      J     ] v = - [ f ]
[ sqrt(mu) D ]       [ 0 ]
*/

static int
lm_step(const void * vtrust_state, const double delta,
        gsl_vector * dx, void * vstate)
{
  int status;
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  lm_state_t *state = (lm_state_t *) vstate;
  const gsl_multifit_nlinear_parameters *params = trust_state->params;
  const double mu = *(trust_state->mu);

  (void)delta;

  /* prepare the linear solver with current LM parameter mu */
  status = (params->solver->presolve)(mu, trust_state, trust_state->solver_state);
  if (status)
    return status;

  /*
   * solve: [     J      ] v = - [ f ]
   *        [ sqrt(mu)*D ]       [ 0 ]
   */
  status = (params->solver->solve)(trust_state->f,
                                   state->vel,
                                   trust_state,
                                   trust_state->solver_state);
  if (status)
    return status;

  if (state->accel)
    {
      double anorm, vnorm;

      /* compute geodesic acceleration */
      status = gsl_multifit_nlinear_eval_fvv(params->h_fvv,
                                             trust_state->x,
                                             state->vel,
                                             trust_state->f,
                                             trust_state->J,
                                             trust_state->sqrt_wts,
                                             trust_state->fdf,
                                             state->fvv,
                                             state->workp);
      if (status)
        return status;

      /*
       * solve: [     J      ] a = - [ fvv ]
       *        [ sqrt(mu)*D ]       [  0  ]
       */
      status = (params->solver->solve)(state->fvv,
                                       state->acc,
                                       trust_state,
                                       trust_state->solver_state);
      if (status)
        return status;

      anorm = gsl_blas_dnrm2(state->acc);
      vnorm = gsl_blas_dnrm2(state->vel);

      /* store |a| / |v| */
      *(trust_state->avratio) = anorm / vnorm;
    }

  /* compute step dx = v + 1/2 a */
  scaled_addition(1.0, state->vel, 0.5, state->acc, dx);

  return GSL_SUCCESS;
}

/*
lm_preduction()
  Compute predicted reduction using Eq 4.4 of More 1978
*/

static int
lm_preduction(const void * vtrust_state, const gsl_vector * dx,
              double * pred, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  lm_state_t *state = (lm_state_t *) vstate;
  const gsl_vector *diag = trust_state->diag;
  const gsl_vector *p = state->vel;
  const double norm_Dp = scaled_enorm(diag, p);
  const double normf = gsl_blas_dnrm2(trust_state->f);
  const double mu = *(trust_state->mu);
  double norm_Jp;
  double u, v;

  (void)dx;

  /* compute work = J*p */
  gsl_blas_dgemv(CblasNoTrans, 1.0, trust_state->J, p, 0.0, state->workn);

  /* compute ||J*p|| */
  norm_Jp = gsl_blas_dnrm2(state->workn);

  u = norm_Jp / normf;
  v = norm_Dp / normf;

  *pred = u * u + 2.0 * mu * v * v;

  return GSL_SUCCESS;
}

static const gsl_multifit_nlinear_trs lm_type =
{
  "levenberg-marquardt",
  lm_alloc_noaccel,
  lm_init,
  lm_preloop,
  lm_step,
  lm_preduction,
  lm_free
};

const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_lm = &lm_type;

static const gsl_multifit_nlinear_trs lmaccel_type =
{
  "levenberg-marquardt+accel",
  lm_alloc_accel,
  lm_init,
  lm_preloop,
  lm_step,
  lm_preduction,
  lm_free
};

const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_lmaccel = &lmaccel_type;


/* movstat/apply.c
 *
 * Copyright (C) 2018 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

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

/*
gsl_movstat_apply_accum()
  Apply moving window statistic to input vector. This is a generalized
routine to handle window endpoints and apply a given accumulator to
the input vector.

Inputs: endtype      - end point handling criteria
        x            - input vector, size n
        accum        - accumulator to apply moving window statistic
        accum_params - parameters to pass to accumulator
        y            - output vector, size n
        z            - second output vector (i.e. minmax), size n; can be NULL
        w            - workspace

Notes:
1) It is allowed to have x = y for in-place moving statistics
*/

int
gsl_movstat_apply_accum(const gsl_movstat_end_t endtype,
                        const gsl_vector * x,
                        const gsl_movstat_accum * accum,
                        void * accum_params,
                        gsl_vector * y,
                        gsl_vector * z,
                        gsl_movstat_workspace * w)
{
  if (x->size != y->size)
    {
      GSL_ERROR("input and output vectors must have same length", GSL_EBADLEN);
    }
  else if (z != NULL && x->size != z->size)
    {
      GSL_ERROR("input and output vectors must have same length", GSL_EBADLEN);
    }
  else
    {
      const int n = (int) x->size;
      const int H = w->H; /* number of samples to left of current sample */
      const int J = w->J; /* number of samples to right of current sample */
      int i;
      double x1 = 0.0;    /* pad values for data edges */
      double xN = 0.0;
      double result[2];

      /* initialize accumulator */
      (accum->init)(w->K, w->state);

      /* pad initial window if necessary */
      if (endtype != GSL_MOVSTAT_END_TRUNCATE)
        {
          if (endtype == GSL_MOVSTAT_END_PADZERO)
            {
              x1 = 0.0;
              xN = 0.0;
            }
          else if (endtype == GSL_MOVSTAT_END_PADVALUE)
            {
              x1 = gsl_vector_get(x, 0);
              xN = gsl_vector_get(x, n - 1);
            }

          /* pad initial windows with H values */
          for (i = 0; i < H; ++i)
            (accum->insert)(x1, w->state);
        }
      else if (accum->delete_oldest == NULL) /* FIXME XXX */
        {
          /* save last K - 1 samples of x for later (needed for in-place input/output) */
          int idx1 = GSL_MAX(n - J - H, 0);
          int idx2 = n - 1;

          for (i = idx1; i <= idx2; ++i)
            w->work[i - idx1] = gsl_vector_get(x, i);
        }

      /* process input vector and fill y(0:n - J - 1) */
      for (i = 0; i < n; ++i)
        {
          double xi = gsl_vector_get(x, i);
          int idx = i - J;

          (accum->insert)(xi, w->state);

          if (idx >= 0)
            {
              (accum->get)(accum_params, result, w->state);
              gsl_vector_set(y, idx, result[0]);

              if (z != NULL)
                gsl_vector_set(z, idx, result[1]);
            }
        }

      if (endtype == GSL_MOVSTAT_END_TRUNCATE)
        {
          /* need to fill y(n-J:n-1) using shrinking windows */
          int idx1 = GSL_MAX(n - J, 0);
          int idx2 = n - 1;

          if (accum->delete_oldest == NULL)
            {
              int wsize = n - GSL_MAX(n - J - H, 0); /* size of work array */

              for (i = idx1; i <= idx2; ++i)
                {
                  int nsamp = n - GSL_MAX(i - H, 0); /* number of samples in this window */
                  int j;

                  (accum->init)(w->K, w->state);

                  for (j = wsize - nsamp; j < wsize; ++j)
                    (accum->insert)(w->work[j], w->state);

                  /* yi = acc_get [ work(i:K-2) ] */
                  (accum->get)(accum_params, result, w->state);
                  gsl_vector_set(y, i, result[0]);

                  if (z != NULL)
                    gsl_vector_set(z, i, result[1]);
                }
            }
          else
            {
              for (i = idx1; i <= idx2; ++i)
                {
                  if (i - H > 0)
                    {
                      /* delete oldest window sample as we move closer to edge */
                      (accum->delete_oldest)(w->state);
                    }

                  /* yi = acc_get [ work(i:K-2) ] */
                  (accum->get)(accum_params, result, w->state);
                  gsl_vector_set(y, i, result[0]);

                  if (z != NULL)
                    gsl_vector_set(z, i, result[1]);
                }
            }
        }
      else
        {
          /* pad final windows and fill y(n-J:n-1) */
          for (i = 0; i < J; ++i)
            {
              int idx = n - J + i;

              (accum->insert)(xN, w->state);

              if (idx >= 0)
                {
                  (accum->get)(accum_params, result, w->state);
                  gsl_vector_set(y, idx, result[0]);

                  if (z != NULL)
                    gsl_vector_set(z, idx, result[1]);
                }
            }
        }

      return GSL_SUCCESS;
    }
}

/*
gsl_movstat_apply()
  Apply user-defined moving window function to input vector

Inputs: endtype - end point handling criteria
        F       - user-defined function
        x       - input vector, size n
        y       - output vector, size n
        w       - workspace
*/

int
gsl_movstat_apply(const gsl_movstat_end_t endtype, const gsl_movstat_function * F,
                  const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w)
{
  int status = gsl_movstat_apply_accum(endtype, x, gsl_movstat_accum_userfunc, (void *) F, y, NULL, w);
  return status;
}


/*
 * Copyright (c) 2011-2018 The University of Tennessee and The University
 *                         of Tennessee Research Foundation.  All rights
 *                         reserved.
 *
 * @precisions normal z -> s d c
 *
 */
#include <math.h>
#include <stdlib.h>
#include <core_blas.h>
#include <cblas.h>
#include "dplasma.h"

/*
#define DEBUG_BUTTERFLY
*/

/* Forward declaration of kernels for the butterfly transformation */
void BFT_zQTL( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans, int is_diagonal );

void BFT_zQBL( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans, int is_diagonal );
void BFT_zQTR_trans( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans );
void BFT_zQTR( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans);
void BFT_zQBR( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans, int is_diagonal );

void RBMM_zTOP( int mb, int nb, int lda, int i_seg, int lvl, int N, int trans,
          PLASMA_Complex64_t *top, PLASMA_Complex64_t *btm,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_but_vec);
void RBMM_zBTM( int mb, int nb, int lda, int i_seg, int lvl, int N, int trans,
          PLASMA_Complex64_t *top, PLASMA_Complex64_t *btm,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_but_vec);

/* Bodies of kernels for the butterfly transformation */

void BFT_zQTL( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans, int is_diagonal )
{
    int i, j;

    (void)lvl;
    (void)N;

#if defined(DEBUG_BUTTERFLY)
    printf ("BFT_zQTL(mb:%d, nb:%d, lda:%d, i_seg:%d, j_seg:%d, lvl:%d, N:%d, bl_is_tr_trans:%d, is_diagonal:%d)\n",
            mb, nb, lda, i_seg, j_seg, lvl, N, bl_is_tr_trans, is_diagonal);
#endif

    if( bl_is_tr_trans ){
        for (j=0; j<nb; j++) {
            int start = is_diagonal ? j : 0;
            for (i=start; i<mb; i++) {
                PLASMA_Complex64_t ri = U_before[i_seg+i];
                PLASMA_Complex64_t rj = U_after[j_seg+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("HE A[%d][%d] = U_before[%d]*((tl[%d]+bl[%d]) + (tr[%d]+br[%d])) * U_after[%d]\n", i_seg+i, j_seg+j, i_seg+i, j*lda+i, j*lda+i, i*lda+j, j*lda+i, j_seg+j);
#endif
                C[j*lda+i] = ri * ((tl[j*lda+i]+bl[j*lda+i]) + (tr[i*lda+j]+br[j*lda+i])) * rj;
#if defined(DEBUG_BUTTERFLY)
                printf ("HE %lf %lf %lf %lf %lf %lf %lf\n", creal(C[j*lda+i]), creal(ri), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[i*lda+j]), creal(br[j*lda+i]), creal(rj));
#endif
            }
        }
    }else{
        for (j=0; j<nb; j++) {
            int start = is_diagonal ? j : 0;
            for (i=start; i<mb; i++) {
                PLASMA_Complex64_t ri = U_before[i_seg+i];
                PLASMA_Complex64_t rj = U_after[j_seg+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("GE A[%d][%d] = U_before[%d]*((tl[%d]+bl[%d]) + (tr[%d]+br[%d])) * U_after[%d]\n", i_seg+i, j_seg+j, i_seg+i, j*lda+i, j*lda+i, j*lda+i, j*lda+i, j_seg+j);
#endif
                C[j*lda+i] = ri * ((tl[j*lda+i]+bl[j*lda+i]) + (tr[j*lda+i]+br[j*lda+i])) * rj;
#if defined(DEBUG_BUTTERFLY)
                printf ("GE %lf %lf %lf %lf %lf %lf %lf\n", creal(C[j*lda+i]), creal(ri), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[j*lda+i]), creal(br[j*lda+i]), creal(rj));
#endif
            }
        }
    }
    return;
}

void BFT_zQBL( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans, int is_diagonal )
{
    int i, j;
    int r_to_s = N/(1<<(lvl+1));

#if defined(DEBUG_BUTTERFLY)
    if( is_diagonal )
        printf ("BFT_zQBL(diag)\n");
    else
        printf ("BFT_zQBL(lower)\n");
#endif

    if( bl_is_tr_trans ){
        for (j=0; j<nb; j++) {
            for (i=0; i<mb; i++) {
                PLASMA_Complex64_t si = U_before[i_seg+r_to_s+i];
                PLASMA_Complex64_t rj = U_after[j_seg+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("HE A[%d][%d] = U_before[%d]*((tl[%d]-bl[%d]) + (tr[%d]-br[%d])) * U_after[%d]\n", i_seg+i+r_to_s, j_seg+j, i_seg+r_to_s+i, j*lda+i, j*lda+i, i*lda+j, j*lda+i, j_seg+j);
#endif
                if( is_diagonal && j>i ){ /* If the tile of the TL quarter is on the diagonal, then we need to take the transpose element for the tiles that came from TL and BR */
                    C[j*lda+i] = si * ((tl[i*lda+j]-bl[j*lda+i]) + (tr[i*lda+j]-br[i*lda+j])) * rj;
                } else{
                    C[j*lda+i] = si * ((tl[j*lda+i]-bl[j*lda+i]) + (tr[i*lda+j]-br[j*lda+i])) * rj;
                }
#if defined(DEBUG_BUTTERFLY)
                printf ("HE %lf %lf %lf %lf %lf %lf %lf\n",creal(C[j*lda+i]), creal(si), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[i*lda+j]), creal(br[j*lda+i]), creal(rj));
#endif
            }
        }
    }else{
        for (j=0; j<nb; j++) {
            for (i=0; i<mb; i++) {
                PLASMA_Complex64_t si = U_before[i_seg+r_to_s+i];
                PLASMA_Complex64_t rj = U_after[j_seg+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("GE A[%d][%d] = U_before[%d]*((tl[%d]-bl[%d]) + (tr[%d]-br[%d])) * U_after[%d]\n", i_seg+i+r_to_s, j_seg+j, i_seg+r_to_s+i, j*lda+i, j*lda+i, j*lda+i, j*lda+i, j_seg+j);
#endif
                C[j*lda+i] = si * ((tl[j*lda+i]-bl[j*lda+i]) + (tr[j*lda+i]-br[j*lda+i])) * rj;
#if defined(DEBUG_BUTTERFLY)
                printf ("GE %lf %lf %lf %lf %lf %lf %lf\n", creal(C[j*lda+i]), creal(si), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[j*lda+i]), creal(br[j*lda+i]), creal(rj));
#endif
            }
        }
    }
    return;
}

/* This function writes into a transposed tile, so C is always transposed. */
void BFT_zQTR_trans( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans )
{
    int i,j;
    int r_to_s = N/(1<<(lvl+1));
#if defined(DEBUG_BUTTERFLY)
    printf ("BFT_zQTR_trans()\n");
#endif
    if( bl_is_tr_trans ){
        for (j=0; j<nb; j++) {
            for (i=0; i<mb; i++) {
                PLASMA_Complex64_t ri = U_before[i_seg+i];
                PLASMA_Complex64_t sj = U_after[j_seg+r_to_s+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("HE A[%d][%d] = U_before[%d]*((tl[%d]+bl[%d]) - (tr[%d]+br[%d])) * U_after[%d]\n", i_seg+j, j_seg+i+r_to_s, i_seg+i, j*lda+i, j*lda+i, i*lda+j, j*lda+i, j_seg+r_to_s+j);
#endif
                C[i*lda+j] = ri * ((tl[j*lda+i]+bl[j*lda+i]) - (tr[i*lda+j]+br[j*lda+i])) * sj;
#if defined(DEBUG_BUTTERFLY)
                printf ("HE %lf %lf %lf %lf %lf %lf %lf\n", creal(C[i*lda+j]), creal(ri), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[i*lda+j]), creal(br[j*lda+i]), creal(sj));
#endif
            }
        }
    }else{
        assert(0); // This should never happen.
    }
    return;
}

void BFT_zQTR( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans )
{
    int i, j;
    int r_to_s = N/(1<<(lvl+1));
#if defined(DEBUG_BUTTERFLY)
    printf ("BFT_zQTR()\n");
#endif
    if( bl_is_tr_trans ){
        for (j=0; j<nb; j++) {
            for (i=0; i<mb; i++) {
                PLASMA_Complex64_t ri = U_before[i_seg+i];
                PLASMA_Complex64_t sj = U_after[j_seg+r_to_s+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("HE A[%d][%d] = U_before[%d]*((tl[%d]+bl[%d]) - (tr[%d]+br[%d])) * U_after[%d]\n", i_seg+i, j_seg+j+r_to_s, i_seg+i, j*lda+i, j*lda+i, i*lda+j, j*lda+i, j_seg+r_to_s+j);
#endif
                C[j*lda+i] = ri * ((tl[j*lda+i]+bl[j*lda+i]) - (tr[i*lda+j]+br[j*lda+i])) * sj;
#if defined(DEBUG_BUTTERFLY)
                printf ("HE %lf %lf %lf %lf %lf %lf %lf\n", creal(C[j*lda+i]), creal(ri), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[i*lda+j]), creal(br[j*lda+i]), creal(sj));
#endif
            }
        }
    }else{
        for (j=0; j<nb; j++) {
            for (i=0; i<mb; i++) {
                PLASMA_Complex64_t ri = U_before[i_seg+i];
                PLASMA_Complex64_t sj = U_after[j_seg+r_to_s+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("GE A[%d][%d] = U_before[%d]*((tl[%d]+bl[%d]) - (tr[%d]+br[%d])) * U_after[%d]\n", i_seg+i, j_seg+j+r_to_s, i_seg+i, j*lda+i, j*lda+i, j*lda+i, j*lda+i, j_seg+r_to_s+j);
#endif
                C[j*lda+i] = ri * ((tl[j*lda+i]+bl[j*lda+i]) - (tr[j*lda+i]+br[j*lda+i])) * sj;
#if defined(DEBUG_BUTTERFLY)
                printf ("GE %lf %lf %lf %lf %lf %lf %lf\n", creal(C[j*lda+i]), creal(ri), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[j*lda+i]), creal(br[j*lda+i]), creal(sj));
#endif
            }
        }
    }
    return;
}

void BFT_zQBR( int mb, int nb, int lda, int i_seg, int j_seg, int lvl, int N,
          PLASMA_Complex64_t *tl, PLASMA_Complex64_t *bl,
          PLASMA_Complex64_t *tr, PLASMA_Complex64_t *br,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_before, PLASMA_Complex64_t *U_after, int bl_is_tr_trans, int is_diagonal )
{
    int i, j;
    int r_to_s = N/(1<<(lvl+1));

#if defined(DEBUG_BUTTERFLY)
    if( is_diagonal )
        printf ("BFT_zQBR(diag)\n");
    else
        printf ("BFT_zQBR(lower)\n");
#endif

    if( bl_is_tr_trans ){
        for (j=0; j<nb; j++) {
            int start = is_diagonal ? j : 0;
            for (i=start; i<mb; i++) {
                PLASMA_Complex64_t si = U_before[i_seg+r_to_s+i];
                PLASMA_Complex64_t sj = U_after[j_seg+r_to_s+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("HE A[%d][%d] = U_before[%d]*((tl[%d]-bl[%d]) - (tr[%d]-br[%d])) * U_after[%d]\n", i_seg+i+r_to_s, j_seg+j+r_to_s, i_seg+r_to_s+i, j*lda+i, j*lda+i, i*lda+j, j*lda+i, j_seg+r_to_s+j);
#endif
                C[j*lda+i] = si * ((tl[j*lda+i]-bl[j*lda+i]) - (tr[i*lda+j]-br[j*lda+i])) * sj;
#if defined(DEBUG_BUTTERFLY)
                printf ("HE %lf %lf %lf %lf %lf %lf %lf\n", creal(C[j*lda+i]), creal(si), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[i*lda+j]), creal(br[j*lda+i]), creal(sj));
#endif
            }
        }
    }else{
        for (j=0; j<nb; j++) {
            int start = is_diagonal ? j : 0;
            for (i=start; i<mb; i++) {
                PLASMA_Complex64_t si = U_before[i_seg+r_to_s+i];
                PLASMA_Complex64_t sj = U_after[j_seg+r_to_s+j];
#if defined(DEBUG_BUTTERFLY)
                printf ("GE A[%d][%d] = U_before[%d]*((tl[%d]-bl[%d]) - (tr[%d]-br[%d])) * U_after[%d]\n", i_seg+i+r_to_s, j_seg+j+r_to_s, i_seg+r_to_s+i, j*lda+i, j*lda+i, j*lda+i, j*lda+i, j_seg+r_to_s+j);
#endif
                C[j*lda+i] = si * ((tl[j*lda+i]-bl[j*lda+i]) - (tr[j*lda+i]-br[j*lda+i])) * sj;
#if defined(DEBUG_BUTTERFLY)
                printf ("GE %lf %lf %lf %lf %lf %lf %lf\n", creal(C[j*lda+i]), creal(si), creal(tl[j*lda+i]), creal(bl[j*lda+i]), creal(tr[j*lda+i]), creal(br[j*lda+i]), creal(sj));
#endif
            }
        }
    }
    return;
}

void RBMM_zTOP( int mb, int nb, int lda, int i_seg, int lvl, int N, int trans,
          PLASMA_Complex64_t *top, PLASMA_Complex64_t *btm,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_but_vec)
{
    int i, j;
    int r_to_s = N/(1<<(lvl+1));
#if defined(DEBUG_BUTTERFLY)
    printf ("RBMM_zTOP()\n");
#endif
    for (j=0; j<nb; j++) {
        for (i=0; i<mb; i++) {
            PLASMA_Complex64_t r = U_but_vec[i_seg+i];
            if( PlasmaConjTrans == trans ){
#if defined(DEBUG_BUTTERFLY)
                printf ("C[%d] = U_but_vec[%d]*(top[%d]+btm[%d])\n", j*lda+i, i_seg+i, j*lda+i, j*lda+i);
#endif
                C[j*lda+i] = r * (top[j*lda+i] + btm[j*lda+i]);
#if defined(DEBUG_BUTTERFLY)
                printf ("%lf %lf %lf %lf\n",creal(C[j*lda+i]), creal(r), creal(top[j*lda+i]), creal(btm[j*lda+i]));
#endif
            }else{
                PLASMA_Complex64_t s = U_but_vec[i_seg+r_to_s+i];
#if defined(DEBUG_BUTTERFLY)
                printf ("C[%d] = U_but_vec[%d]*top[%d] + U_but_vec[%d]*btm[%d]\n", j*lda+i, i_seg+i, j*lda+i, i_seg+r_to_s+i, j*lda+i);
#endif
                C[j*lda+i] =  r*top[j*lda+i] + s*btm[j*lda+i];
#if defined(DEBUG_BUTTERFLY)
                printf ("%lf %lf %lf %lf %lf\n",creal(C[j*lda+i]), creal(r), creal(top[j*lda+i]), creal(s), creal(btm[j*lda+i]));
#endif
            }
        }
    }
    return;
}

void RBMM_zBTM( int mb, int nb, int lda, int i_seg, int lvl, int N, int trans,
          PLASMA_Complex64_t *top, PLASMA_Complex64_t *btm,
          PLASMA_Complex64_t *C, PLASMA_Complex64_t *U_but_vec)
{
    int i, j;
    int r_to_s = N/(1<<(lvl+1));
#if defined(DEBUG_BUTTERFLY)
    printf ("RBMM_zBTM()\n");
#endif
    for (j=0; j<nb; j++) {
        for (i=0; i<mb; i++) {
            PLASMA_Complex64_t s = U_but_vec[i_seg+r_to_s+i];
            if( PlasmaConjTrans == trans ){
#if defined(DEBUG_BUTTERFLY)
                printf ("C[%d] = U_but_vec[%d]*(top[%d]-btm[%d])\n", j*lda+i, i_seg+r_to_s+i, j*lda+i, j*lda+i);
#endif
                C[j*lda+i] = s * (top[j*lda+i] - btm[j*lda+i]);
#if defined(DEBUG_BUTTERFLY)
                printf ("%lf %lf %lf %lf\n",creal(C[j*lda+i]), creal(s), creal(top[j*lda+i]), creal(btm[j*lda+i]));
#endif
            }else{
                PLASMA_Complex64_t r = U_but_vec[i_seg+i];
#if defined(DEBUG_BUTTERFLY)
                printf ("C[%d] = U_but_vec[%d]*top[%d] - U_but_vec[%d]*btm[%d]\n", j*lda+i, i_seg+i, j*lda+i, i_seg+r_to_s+i, j*lda+i);
#endif
                C[j*lda+i] = r*top[j*lda+i] - s*btm[j*lda+i];
#if defined(DEBUG_BUTTERFLY)
                printf ("%lf %lf %lf %lf %lf\n", creal(C[j*lda+i]), creal(r), creal(top[j*lda+i]), creal(s), creal(btm[j*lda+i]));
#endif
            }
        }
    }
    return;
}


/////////////////////////////////////////////////////////////////////
// = NMatrix
//
// A linear algebra library for scientific computation in Ruby.
// NMatrix is part of SciRuby.
//
// NMatrix was originally inspired by and derived from NArray, by
// Masahiro Tanaka: http://narray.rubyforge.org
//
// == Copyright Information
//
// SciRuby is Copyright (c) 2010 - 2014, Ruby Science Foundation
// NMatrix is Copyright (c) 2012 - 2014, John Woods and the Ruby Science Foundation
//
// Please see LICENSE.txt for additional copyright notices.
//
// == Contributing
//
// By contributing source code to SciRuby, you agree to be bound by
// our Contributor Agreement:
//
// * https://github.com/SciRuby/sciruby/wiki/Contributor-Agreement
//
// == gemv.h
//
// Header file for interface with ATLAS's CBLAS gemv functions and
// native templated version of LAPACK's gemv function.
//

#ifndef GEMV_H
# define GEMV_H

extern "C" { // These need to be in an extern "C" block or you'll get all kinds of undefined symbol errors.
#if defined HAVE_CBLAS_H
  #include <cblas.h>
#elif defined HAVE_ATLAS_CBLAS_H
  #include <atlas/cblas.h>
#endif
}

namespace nm { namespace math {

/*
 * GEneral Matrix-Vector multiplication: based on dgemv.f from Netlib.
 *
 * This is an extremely inefficient algorithm. Recommend using ATLAS' version instead.
 *
 * Template parameters: LT -- long version of type T. Type T is the matrix dtype.
 */
template <typename DType>
inline bool gemv(const enum CBLAS_TRANSPOSE Trans, const int M, const int N, const DType* alpha, const DType* A, const int lda,
          const DType* X, const int incX, const DType* beta, DType* Y, const int incY) {
  int lenX, lenY, i, j;
  int kx, ky, iy, jx, jy, ix;

  typename LongDType<DType>::type temp;

  // Test the input parameters
  if (Trans < 111 || Trans > 113) {
    rb_raise(rb_eArgError, "GEMV: TransA must be CblasNoTrans, CblasTrans, or CblasConjTrans");
    return false;
  } else if (lda < std::max(1, N)) {
    fprintf(stderr, "GEMV: N = %d; got lda=%d", N, lda);
    rb_raise(rb_eArgError, "GEMV: Expected lda >= max(1, N)");
    return false;
  } else if (incX == 0) {
    rb_raise(rb_eArgError, "GEMV: Expected incX != 0\n");
    return false;
  } else if (incY == 0) {
    rb_raise(rb_eArgError, "GEMV: Expected incY != 0\n");
    return false;
  }

  // Quick return if possible
  if (!M or !N or (*alpha == 0 and *beta == 1)) return true;

  if (Trans == CblasNoTrans) {
    lenX = N;
    lenY = M;
  } else {
    lenX = M;
    lenY = N;
  }

  if (incX > 0) kx = 0;
  else          kx = (lenX - 1) * -incX;

  if (incY > 0) ky = 0;
  else          ky =  (lenY - 1) * -incY;

  // Start the operations. In this version, the elements of A are accessed sequentially with one pass through A.
  if (*beta != 1) {
    if (incY == 1) {
      if (*beta == 0) {
        for (i = 0; i < lenY; ++i) {
          Y[i] = 0;
        }
      } else {
        for (i = 0; i < lenY; ++i) {
          Y[i] *= *beta;
        }
      }
    } else {
      iy = ky;
      if (*beta == 0) {
        for (i = 0; i < lenY; ++i) {
          Y[iy] = 0;
          iy += incY;
        }
      } else {
        for (i = 0; i < lenY; ++i) {
          Y[iy] *= *beta;
          iy += incY;
        }
      }
    }
  }

  if (*alpha == 0) return false;

  if (Trans == CblasNoTrans) {

    // Form  y := alpha*A*x + y.
    jx = kx;
    if (incY == 1) {
      for (j = 0; j < N; ++j) {
        if (X[jx] != 0) {
          temp = *alpha * X[jx];
          for (i = 0; i < M; ++i) {
            Y[i] += A[j+i*lda] * temp;
          }
        }
        jx += incX;
      }
    } else {
      for (j = 0; j < N; ++j) {
        if (X[jx] != 0) {
          temp = *alpha * X[jx];
          iy = ky;
          for (i = 0; i < M; ++i) {
            Y[iy] += A[j+i*lda] * temp;
            iy += incY;
          }
        }
        jx += incX;
      }
    }

  } else { // TODO: Check that indices are correct! They're switched for C.

    // Form  y := alpha*A**DType*x + y.
    jy = ky;

    if (incX == 1) {
      for (j = 0; j < N; ++j) {
        temp = 0;
        for (i = 0; i < M; ++i) {
          temp += A[j+i*lda]*X[j];
        }
        Y[jy] += *alpha * temp;
        jy += incY;
      }
    } else {
      for (j = 0; j < N; ++j) {
        temp = 0;
        ix = kx;
        for (i = 0; i < M; ++i) {
          temp += A[j+i*lda] * X[ix];
          ix += incX;
        }

        Y[jy] += *alpha * temp;
        jy += incY;
      }
    }
  }

  return true;
}  // end of GEMV

#if (defined(HAVE_CBLAS_H) || defined(HAVE_ATLAS_CBLAS_H))
template <>
inline bool gemv(const enum CBLAS_TRANSPOSE Trans, const int M, const int N, const float* alpha, const float* A, const int lda,
          const float* X, const int incX, const float* beta, float* Y, const int incY) {
  cblas_sgemv(CblasRowMajor, Trans, M, N, *alpha, A, lda, X, incX, *beta, Y, incY);
  return true;
}

template <>
inline bool gemv(const enum CBLAS_TRANSPOSE Trans, const int M, const int N, const double* alpha, const double* A, const int lda,
          const double* X, const int incX, const double* beta, double* Y, const int incY) {
  cblas_dgemv(CblasRowMajor, Trans, M, N, *alpha, A, lda, X, incX, *beta, Y, incY);
  return true;
}

template <>
inline bool gemv(const enum CBLAS_TRANSPOSE Trans, const int M, const int N, const Complex64* alpha, const Complex64* A, const int lda,
          const Complex64* X, const int incX, const Complex64* beta, Complex64* Y, const int incY) {
  cblas_cgemv(CblasRowMajor, Trans, M, N, alpha, A, lda, X, incX, beta, Y, incY);
  return true;
}

template <>
inline bool gemv(const enum CBLAS_TRANSPOSE Trans, const int M, const int N, const Complex128* alpha, const Complex128* A, const int lda,
          const Complex128* X, const int incX, const Complex128* beta, Complex128* Y, const int incY) {
  cblas_zgemv(CblasRowMajor, Trans, M, N, alpha, A, lda, X, incX, beta, Y, incY);
  return true;
}
#endif

}} // end of namespace nm::math

#endif // GEMM_H


#define _GNU_SOURCE
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

/* don't check ranges in Matrix operations and make Matrix call inline */
#define GSL_RANGE_CHECK_OFF
//#define HAVE_INLINE

#define FALSE 0
#define TRUE 1

#ifndef M_PI
#define M_PI            3.141592653589793238462643383280        /* pi */
#endif

#define LN2PI 1.837877066409345339

#define ODEdriver gsl_odeiv2_driver_alloc_y_new
#define rkf45 gsl_odeiv2_step_rkf45
#define rk8pd gsl_odeiv2_step_rk8pd
#define rkck  gsl_odeiv2_step_rkck
#define rk2   gsl_odeiv2_step_rk2

#define EOL -1

#define max(A, B) ((A) > (B) ? (A) : (B))
#define min(A, B) ((A) < (B) ? (A) : (B))
#define MIN(A, B) ((A) < (B) ? (A) : (B))
#define MAX(A, B) ((A) > (B) ? (A) : (B))
#define V(x) (x)*(x)

#define FIT_TIME_POINT(x) (x->mode == FIT)
#define RECORD_TIME_POINT(x, u) (x->mode == OUTPUT || (x->mode == FIT && u == x->t[x->i]))
#define NEXT_TIME_POINT(x) if (x->mode == FIT) x->i++
#define RESET_TIME_POINT(x) x->i = 0

enum {FIT, OUTPUT};

enum {SUCCESS=0, FAIL=1, ERROR1=1, ERROR2, ERROR3, ERROR4, ERROR5, ERROR6,
      ERROR7, ERROR8, ERROR9, ERROR10};

typedef uint boolean;

// for internal use
// type of parameter
enum {NOTUSED=0, DERIVED=21, FITTED=22, CONSTANT=23, AUXILLARY=24, INDIVIDUAL_CONSTANT=28};
// whether a fitted or auxillary parameter has a hyperparameter associated with it
enum {HYPER=26};
// prior of a fitted parameter that does not have a hyperparameters
enum {NOTAPPLICABLE=0, NORMAL=1, GAMMA=2, SICHI2=3, UNIFORM=4, PARETO=5,
      TRUNCNORMAL=6, EXPONENTIAL=7, BINOMIAL=8, POISSON=9, INVGAMMA=10,
      UNIFORM0=11, BETA=12, NORMALMEAN=13, CATEGORICAL=14, GEOMETRIC=15,
      USERDEFINED=16, AUXILLARY_HYPER=27, CIRCULAR=29};
// number type of the parameter
enum {REAL=17, INTEGER=18, CATEGORY=19, INT = INTEGER};
// type of data
enum {NOT_AVAILABLE=0, ABOVE_DETECTION=1, BELOW_DETECTION=2, MEASURED=3};

// for use in model files
// prior or type of the declared parameter
enum {Normal        =NORMAL,
      Gamma         =GAMMA,
      Sichi2        =SICHI2,
      Uniform       =UNIFORM,
      Circular      =CIRCULAR,
      Pareto        =PARETO,
      TruncNormal   =TRUNCNORMAL,
      Exponential   =EXPONENTIAL,
      Binomial      =BINOMIAL,
      Poisson       =POISSON,
      InvGamma      =INVGAMMA,
      Uniform0      =UNIFORM0,
      Beta          =BETA,
      NormalMean    =NORMALMEAN,
      Categorical   =CATEGORICAL,
      Geometric     =GEOMETRIC,
      UserDefined   =USERDEFINED,
      HyperP         =HYPER,
      Constant      =CONSTANT,
      Auxillary     =AUXILLARY,
      Auxillary_Hyper =AUXILLARY_HYPER,
      Derived       =DERIVED,
      Individual_Constant=INDIVIDUAL_CONSTANT};
enum {Nor           =NORMAL,
      Gam 	    =GAMMA,
      Sic 	    =SICHI2,
      Unif 	    =UNIFORM,
      Circ          =CIRCULAR,
      Pare 	    =PARETO,
      TNorm 	    =TRUNCNORMAL,
      Exp 	    =EXPONENTIAL,
      Bin 	    =BINOMIAL,
      Poi 	    =POISSON,
      IGam 	    =INVGAMMA,
      Unif0 	    =UNIFORM0,
      Be 	    =BETA,
      NorM 	    =NORMALMEAN,
      Cat 	    =CATEGORICAL,
      Geom 	    =GEOMETRIC,
      UD 	    =USERDEFINED,
      Hyp 	    =HYPER,
      Cst 	    =CONSTANT,
      Aux 	    =AUXILLARY,
      Aux_Hyp       =AUXILLARY_HYPER,
      Der	    =DERIVED,
      IndCst        =INDIVIDUAL_CONSTANT};

// number type of the parameter
enum {Real=REAL, Integer=INTEGER, Category=CATEGORY, Int=INTEGER};
// type of data
enum {NA=0, Above_Detection=1, Below_Detection=2, Measured=3};

typedef struct {
  uint mode, n, i;
  double *t;
} TimePoints;

struct line {
  int n;
  double *x;
  double *y;
  double *m;
  double *c;
};

struct data_individual {
  int n;
  int np;
  uint **value;
  double **Y;
  double saturated;
};

struct data_treatment {
  int nind;
  int *usevar;
  char levels[1000];
  struct data_individual *Individual;
};

struct data {
  int ntrt;
  int nvar;
  int maxnind;
  int maxnp;
  char filename[100];
  struct data_treatment *Treatment;
};

typedef struct treatmentlist {
  int n;
  int *treatment;
} Treatments;

typedef struct hyperblock {
  /* set in CreateBlock */
  int use;

  /* set in AddHyperToTreatment */
  int nhypers_used;
  int *hyper_used;
  int **nhypers_with_treatment;
  int ***hyper_with_treatment;
  int nassociated_pars;
  int *associated_par;
  int *ntreatments_with_parameter;
  int **treatment_with_parameter;

  /* set in Bayes */
  int hdistribution;
  int pdistribution;
  int *dependent_block;
} Hyperblock;

struct hyper {
  int index;
  int use;
  int parno;
  int hdist;
  int number;
  char name[100];
  double location;
  double scale;
  double shape;
};

typedef struct allhypers {
  int init;
  int ntrt;
  int *nind;
  int *trtused;
  int nhyper_parameters;
  struct hyper *H;
  int nblocks;
  Hyperblock *block;
  double *hyper;         //used for initialisation of thread hyper values
  double **hyper_store;
} Hyperparameters;

struct variable {
  int index;
  char name[100];
};

struct variables {
  int    variables;
  double scaletime;
  struct variable *W;
};

struct parameter {
  int index;
  int number;
  int distribution;
  int circular;
  int type;
  int hashyper;
  int hyper_location_block_index;
  int hyper_shape_block_index;
  int hyper_scale_block_index;
  int ncategories;
  double *p; // used for categorical parameters
  double location;
  double scale;
  double shape;
  char name[100];
  double (*(*link))(double);
};

struct blocks {
  int nblocks;
  int **pblock;
};

struct parameterset {
  int    pass;
  int    parameters;   // maximum parameter index
  int    npar;         // number of fitted parameters
  int    ninteger;
  int    *integer;
  int    nauxillary;
  int    *auxillary;
  struct parameter *P;
  struct blocks Blocks;
};

struct parameter_block {
  int categorical;
  int done;
  int npar;
  int *pblock;
  int covfinal;
  int covstart;
  int inrange;
  int storeno;
  int error;
  double accept;      /*mcmc acceptances*/
  double proposed;    /*mcmc iterations*/
  double covscale;    /*covariance matrix scaling factor*/
  //  double **L;         /*covariance matrix*/
  gsl_matrix *L;
  double **theta_store;
};

struct chain {
  int done;       /*if current individual is ready for next phase*/
  int loop;
  /* parameters for threads */
  double logmaxL;
  double logL;
  double *logP;
  double logpost;
  double *theta;       /*current parameter values*/
  double minlogL;
  double prop_logL;
  double prop_logP;
  double prop_logpost;
  int storeno;
  int laststoreno;
  double accept;      /*mcmc acceptances*/
  double proposed;    /*mcmc iterations*/
  double **theta_store;
  double *logLstore;
  double *logpoststore;
  double exchangeaccept;      /*mcmc acceptances*/
  double exchangeproposed;    /*mcmc iterations*/
  struct parameter_block *PB;
};

struct sim_individual {
  int done;
  int chains;
  int *chainno;
  double *temp;
  struct chain *Chain;
  struct parameterset Q;
};

typedef struct {
  int n;
  int m;
  int **a;
} Matrix;

struct sim_treatment {
  struct sim_individual *Individual;
};

struct thread {
  double *hyper;
  struct sim_treatment *Treatment;
  gsl_rng *stream;
};

typedef struct simulation {
  Hyperparameters HP;
  int *trtused;
  int **indused;
  int ntp;            // individual with most parameters
  int nrp;            // individual with most parameters
  int rdp;
  int sim;
  int maxc;
  int seed;
  int thin;
  int mcmc;
  int popd;
  int marg;
  int maxnp;
  int error;
  int range;
  int chains;
  int notime;
  int copyind;
  int threads;
  int verbose; /* 0: no output, 1: output chain 0+lowest exchangerate,
		  2: all chains*/
  int derived;
  int discard;
  int samples;
  int storeno;
  int gsl_error;
  int population;
  int covsamples;
  int growthcurves;
  int parsperblock;
  int phase1adjust;
  int phase2adjust;
  int mincovsamples;
  int ignorewarning;
  int usehypermeans;
  int sample_parameters;
  char expno[50];
  char outexpno[50];
  char *parsfile;
  char *tempfile;
  char logfile[1000];
  char factors[1000];
  double xrate;
  double timeend;
  double lowrate;
  double highrate;
  double minprior;
  double initscale;
  double covthresh;
  double timestart;
  double scalefactor;
  double saturatedlogL;
  double minexchangerate;
  struct sim_individual Sample;
  struct sim_treatment *Treatment;
  struct variables V;
} Simulation;

extern char expno[];

extern int* integervector( int );
extern uint* uintegervector( int );
extern uint** uintegermatrix( int, int );
extern double* doublevector( int );
extern double** doublematrix( int, int );
extern double*** doublematrix3( int, int, int );
extern double**** doublematrix4( int, int, int, int );
extern char* charvector( int );
extern char** charmatrix( int, int );
extern double factln( int );
extern double gammln( double );
extern void binorm( double*, double, double, double, double, double );
extern long int lrint(double);
extern double pow10(double);
extern double round(double);
extern double rtnewt(void(*)(double, double*, double*, double*),
		     double, double, double, double*);
extern double rtbis( double (*)(double, double*), double, double,
		     double, int*, double*);

extern void *malloc(size_t);
extern void exit(int);
extern void free(void*);
extern void Error(char*);
extern void ReadParam( char*, void*, char* ) ;

extern uint   binomialRNG(uint, double, const gsl_rng*);
extern void   multinomialRNG(uint, const double*, size_t, unsigned int*, const gsl_rng*);
extern int    poissonRNG(double, const gsl_rng*);
extern int    uniform_intRNG(int, int, const gsl_rng*);
extern double normalRNG(double, double, const gsl_rng*);
extern double betaRNG(double, double, const gsl_rng*);
extern double chisqRNG(double, const gsl_rng*);
extern double scaleinvchisqRNG(double, double, const gsl_rng*);
extern double invgammaRNG(double, double, const gsl_rng*);
extern double gammaRNG(double, double, const gsl_rng*);
extern double uniformRNG(double, double, const gsl_rng*);
extern double paretoRNG(double, double, const gsl_rng*);
extern void   dirichlet_multinomialRNG(uint, const double*, size_t, uint*, const gsl_rng*);
extern void   dirichletRNG(const double*, size_t, double*, const gsl_rng*);
extern uint   beta_binomialRNG(uint, double, double, const gsl_rng*);
extern uint   negative_binomialRNG(double, double, const gsl_rng*);
extern uint   beta_negative_binomialRNG(uint, double, double, const gsl_rng*);
/* Matrix *wishartRNG(int, int, Matrix*, Matrix*, const gsl_rng*); */
/* void   MVNRNG(int, Matrix*, Matrix*, Matrix*, const gsl_rng*); */

extern double poissonCMF(const double, const double, int);
extern double gamma_scalePDF(const double, const double, const double);
extern double betaPDF(const double, const double, const double);
extern double exponentialPDF(const double, const double);
extern double poissonPMF(const double, const double);
extern double geometricPDF(const double, const double);
extern double trnormalPDF(const double, const double, const double);
extern double normalCDF(const double, const double, const double, int);
extern double normalPDF(const double, const double, const double);
extern double binomialPMF(const double, const double, const double);
extern double multinomialPMF(const unsigned int, const double*, const double*);
extern double uniformPDF(const double, const double, const double);
extern double logisticPDF(const double, const double, const double);
extern double betabinomialPMF(const double, const double, const double, const double);
//extern double MVNPDF(Matrix*, Matrix*, Matrix*, const double, int);
extern double von_mises_cdf ( double x, double a, double b );
extern double von_mises_cdf_inv ( double cdf, double a, double b );

extern Matrix *MatrixInit(Matrix*, int, int, int*);
extern Matrix *MatrixDestroy(Matrix*);
extern double identity(double);
extern double logit(double);
extern double invlogit(double);

int    function(int, int, double*, double*, double*, TimePoints*);
int    Model(const uint, const uint, const uint, double*, double**,
	     TimePoints*);
//int    Model(int, int, int, double*, double**);
void   Block(struct parameterset*, int, ...);
void   Par(struct parameterset*, int, int, char*, int, ...);
void Create(Hyperparameters *H, int *h, int block, int par,
	    Matrix M, char *name, int type, double p1, double p2);
void   Hyper(Hyperparameters *HP, int index, int number, char *name, int distribution,
	     double shape_or_loc, double scale);
void   CreateHyperBlock(Hyperparameters *HP, int index);
void   AddHyperToATreatment(Hyperparameters *HP, int index, int h, int pindex,
			    int treatment);
void   AddHyperToTreatments(Hyperparameters *HP, int index, int h, int pindex, ...);
void   HyperParameters(Treatments, Hyperparameters*);
void   Var(struct variables*, int, char*);
void   GlobalParameters(void);
void   Variables(struct variables*);
void   OutputModel(int, int, double*, double*);
void   OutputData(int, int, double*, double*, double*, uint*);
void   DerivedHyperParameters(int trt, double *p);
void   DerivedParameters(int, int, int, double*, double**, TimePoints*);
void   Parameters(int, int, double*, struct parameterset*);
void   PredictData(int, int, double*, double*, double*, gsl_rng*);
void   SimulateData(int, int, double*, double*, double*, double*, uint*, gsl_rng*);
void   SaturatedModel(int, int, double*, double*, double*, uint*);
void   Residual(int, int, double*, double*, double*, double*, uint*);
void   Equations(double*, double*, int, double*, double*, int, double);
double timestep(void);
double logLikelihood(const uint, const uint, const double*,
		     const double*, const double*, const uint*);
double logLikelihoodI(const uint trt, const uint ind, const double *p,
		      double **V, double *Data, const uint *value,
		      const uint N);
void WAIC(int trt, int ind, double *lnL, double *V,
	  double *p, double *Data, uint *value);
//double logLikelihood(int, int, double*, double*, double*, int*);
double UserPDF(double);
void   ScaleTime(struct variables*, double);


/*
 *  vbHmmTsFret.c
 *  Model-specific core functions for VB-HMM-TS.
 *
 *  Created by OKAMOTO Kenji, SAKO Yasushi and RIKEN
 *  Copyright 2011-2015
 *  Cellular Informatics Laboratory, Advance Science Institute, RIKEN, Japan.
 *  All rights reserved.
 *
 *  Ver. 1.1.0
 *  Last modified on 2015.09.17
 */

#include "vbHmmTsFret.h"
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_psi.h>
#include <string.h>
#include "rand.h"

#ifdef _OPENMP
#include "omp.h"
#endif

//// Uncomment one/both of the following defenitions to activate constraint on I or/and K.
//#define  INTENSITY_CAP
//#define  TRANSITION_RATE_CAP
#ifdef INTENSITY_CAP
#define  maxIntensityRatio  10.0
#endif
#ifdef TRANSITION_RATE_CAP
#define  minPhotonNumPerState  10.0
#endif

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

//static int isGlobalAnalysis = 0;

void setFunctions_tsFret(){
    commonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_tsFret;
    funcs.freeModelParameters   = freeModelParameters_tsFret;
    funcs.newModelStats         = newModelStats_tsFret;
    funcs.freeModelStats        = freeModelStats_tsFret;
    funcs.initializeVbHmm       = initializeVbHmm_tsFret;
    funcs.pTilde_z1             = pTilde_z1_tsFret;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_tsFret;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_tsFret;
    funcs.calcStatsVars         = calcStatsVars_tsFret;
    funcs.maximization          = maximization_tsFret;
    funcs.varLowerBound         = varLowerBound_tsFret;
    funcs.reorderParameters     = reorderParameters_tsFret;
    funcs.outputResults         = outputResults_tsFret;
    setFunctions( funcs );
}

//void setGFunctions_tsFret(){
//    gCommonFunctions funcs;
//    funcs.newModelParameters    = newModelParameters_tsFret;
//    funcs.freeModelParameters   = freeModelParameters_tsFret;
//    funcs.newModelStats         = newModelStats_tsFret;
//    funcs.freeModelStats        = freeModelStats_tsFret;
//    funcs.newModelStatsG        = newModelStatsG_tsFret;
//    funcs.freeModelStatsG       = freeModelStatsG_tsFret;
//    funcs.initializeVbHmmG      = initializeVbHmmG_tsFret;
//    funcs.pTilde_z1             = pTilde_z1_tsFret;
//    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_tsFret;
//    funcs.pTilde_xn_zn          = pTilde_xn_zn_tsFret;
//    funcs.calcStatsVarsG        = calcStatsVarsG_tsFret;
//    funcs.maximizationG         = maximizationG_tsFret;
//    funcs.varLowerBoundG        = varLowerBoundG_tsFret;
//    funcs.reorderParametersG    = reorderParametersG_tsFret;
//    funcs.outputResultsG        = outputResultsG_tsFret;
//    setGFunctions( funcs );
//    isGlobalAnalysis = 1;
//}

void outputResults_tsFret( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    outputTsFretResults( xn, gv, iv, logFP );
}

//void outputResultsG_tsFret( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//    outputTsFretResultsG( xns, gv, ivs, logFP );
//}

void *newModelParameters_tsFret( xn, sNo )
xnDataSet *xn;
int sNo;
{
    int i;
    tsFretParameters *p = (void*)malloc( sizeof(tsFretParameters) );

    p->uPiArr = (double*)malloc( sNo * sizeof(double) );
    p->sumUPi = 0.0;
    // hyperparameter for p( k(i,j) ) (i != j)
    p->uKMat = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->uKMat[i] = (double*)malloc( sNo * sizeof(double) );
    }
    p->sumUKArr = (double*)malloc( sNo * sizeof(double) );
    // hyperparameter for p( I(k) )
    p->aIArr = (double*)malloc( sNo * sizeof(double) );
    p->bIArr = (double*)malloc( sNo * sizeof(double) );
    // hyperparameter for p( E(i) )
    p->uEArr = (double*)malloc( sNo * sizeof(double) );
    p->vEArr = (double*)malloc( sNo * sizeof(double) );

    // parameters
    p->avgPi = (double *)malloc( sNo * sizeof(double) );
    p->avgLnPi = (double *)malloc( sNo * sizeof(double) );
    p->avgI = (double *)malloc( sNo * sizeof(double) );
    p->avgLnI = (double *)malloc( sNo * sizeof(double) );
    p->avgK = (double **)malloc( sNo * sizeof(double*) );
    p->avgLnK = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->avgK[i] = (double *)malloc( sNo * sizeof(double) );
        p->avgLnK[i] = (double *)malloc( sNo * sizeof(double) );
    }
    p->avgLnKI = (double *)malloc( sNo * sizeof(double) );
    p->avgE = (double *)malloc( sNo * sizeof(double) );
    p->avgLnE = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   p->avgLnE[i] = (double *)malloc( 2 * sizeof(double) );   }

    return p;
}

void freeModelParameters_tsFret( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    tsFretParameters *gp = *p;
    int i;

    free( gp->uPiArr );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->uKMat[i] );
    }
    free( gp->uKMat );
    free( gp->sumUKArr );
    free( gp->aIArr );
    free( gp->bIArr );
    free( gp->uEArr );
    free( gp->vEArr );

    free( gp->avgPi );
    free( gp->avgLnPi );
    free( gp->avgI );
    free( gp->avgLnI );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgK[i] );
        free( gp->avgLnK[i] );
    }
    free( gp->avgK );
    free( gp->avgLnK );
    free( gp->avgLnKI );
    free( gp->avgE );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgLnE[i] );
    }
    free( gp->avgLnE );

    free( *p );
    *p = NULL;
}

void *newModelStats_tsFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        tsFretStats *s = (tsFretStats*)malloc( sizeof(tsFretStats) );

        int i;
        s->Ni = (double *)malloc( sNo * sizeof(double) );
        s->Ti = (double *)malloc( sNo * sizeof(double) );
        s->Nii = (double *)malloc( sNo * sizeof(double) );
        s->Nij = (double *)malloc( sNo * sizeof(double) );
        s->Mij = (double **)malloc( sNo * sizeof(double*) );
        for( i = 0 ; i < sNo ; i++ )
        {   s->Mij[i] = (double *)malloc( sNo * sizeof(double) );   }
        s->eps = (double *)malloc( sNo * sizeof(double) );

    return s;

//    } else {
//
//        return NULL;
//
//    }
}

void freeModelStats_tsFret( s, xn, gv, iv )
void **s;
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        tsFretStats *gs = *s;
        int i;
        free( gs->Ni );
        free( gs->Ti );
        free( gs->Nii );
        free( gs->Nij );
        for( i = 0 ; i < sNo ; i++ )
        {   free( gs->Mij[i] );   }
        free( gs->Mij );
        free( gs->eps );

        free( gs );
        *s = NULL;
//    }
}

//void *newModelStatsG_tsFret( xns, gv, ivs)
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//    int sNo = gv->sNo;
//    tsGlobalStats *gs = (tsGlobalStats*)malloc( sizeof(tsGlobalStats) );
//
//    return gs;
//}

//void freeModelStatsG_tsFret( gs, xns, gv, ivs )
//void **gs;
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//
//    free( *gs );
//    *gs = NULL;
//}

void initializeVbHmm_tsFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsFretData *d = xn->data;
    int sNo = gv->sNo;
    tsFretParameters *p = gv->params;
    int i, j;

    // hyperparameter for p( pi(i) )
    p->sumUPi = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        p->uPiArr[i] = 1.0;
        p->sumUPi += p->uPiArr[i];
    }

    // hyperparameter for p( k(i,j) ) (i != j)
    for( i = 0 ; i < sNo ; i++ ){
        p->uKMat[i] = (double*)malloc( sNo * sizeof(double) );
        p->sumUKArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            p->uKMat[i][j] = 1.0;
            if( j != i ){
                p->sumUKArr[i] += p->uKMat[i][j];
            }
        }
    }

    double meanI = (double)xn->N / d->T;

    // hyperparameter for p( I(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->aIArr[i] = 1.0;
        p->bIArr[i] = 1.0 / meanI;
    }

    // hyperparameter for p( E(i) )
    for( i = 0 ; i < sNo ; i++ ){
        p->uEArr[i] = 1.0;
        p->vEArr[i] = 1.0;
    }

    initialize_indVars_tsFret( xn, gv, iv );

    calcStatsVars_tsFret( xn, gv, iv );
    maximization_tsFret( xn, gv, iv );
}

//void initializeVbHmmG_tsFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void initialize_indVars_tsFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;

    int i;
    size_t n;
    double sumPar;
    for( n = 0 ; n < dLen ; n++ ){
        sumPar = 0.0;
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] = enoise(1.0) + 1.0;
            sumPar += gmMat[n][i];
        }
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] /= sumPar;
        }
    }
}

xnDataSet *newXnDataSet_tsFret( filename )
const char *filename;
{
    xnDataSet *xn = (xnDataSet*)malloc( sizeof(xnDataSet) );
    xn->name = (char*)malloc( strlen(filename) + 2 );
    strncpy( xn->name, filename, strlen(filename)+1 );
    xn->data = (tsFretData*)malloc( sizeof(tsFretData) );
    tsFretData *d = (tsFretData*)xn->data;
    d->T = 0.0;
    d->dt = NULL;
    d->time = NULL;
    d->ch = NULL;
    return xn;
}

void freeXnDataSet_tsFret( xn )
xnDataSet **xn;
{
    tsFretData *d = (tsFretData*)(*xn)->data;
    free( d->dt );
    free( d->time );
    free( d->ch );
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

double pTilde_z1_tsFret( i, params )
int i;
void *params;
{
    tsFretParameters *p = (tsFretParameters*)params;
    return exp( p->avgLnPi[i] );
}

double pTilde_zn_zn1_tsFret( i, j, params )
int i, j;
void *params;
{
    tsFretParameters *p = (tsFretParameters*)params;
    if( i == j ){
        return exp( p->avgLnI[i] - p->avgLnKI[i] );
    } else {
        return exp( p->avgLnK[i][i] + p->avgLnK[i][j] - p->avgLnKI[i] );
    }
}

double pTilde_xn_zn_tsFret( xn, n, i, params )
xnDataSet *xn;
size_t n;
int i;
void *params;
{
    tsFretParameters *p = (tsFretParameters*)params;
    tsFretData *d = (tsFretData*)xn->data;
    return exp( p->avgLnI[i] - (p->avgI[i] * d->dt[n]) + p->avgLnE[i][d->ch[n]] );
}

void calcStatsVars_tsFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsFretData *d = (tsFretData*)xn->data;
    tsFretStats *s = (tsFretStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Ni = s->Ni, *Ti = s->Ti, *eps = s->eps;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i] = 1e-10;
        Ti[i] = 1e-10;
        eps[i] = 1e-10;
        Nii[i] = 0.0;
        Nij[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            Mij[i][j] = 1e-10;
        }
    }
    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){
            Ni[i]  += gmMat[n][i];
            Ti[i]  += gmMat[n][i] * d->dt[n];
            eps[i] += gmMat[n][i] * d->ch[n];
            Nii[i] += xiMat[n][i][i];
            for( j = 0 ; j < sNo ; j++ ){
                if( j != i ){
                    Mij[i][j] += xiMat[n][i][j];
                    Nij[i]    += xiMat[n][i][j];
                }
            }
        }
    }
    for( i = 0 ; i < sNo ; i++ ){
        Nii[i] = MAX( Nii[i], 1.0 );
        Nij[i] = MAX( Nij[i], 1.0 );
    }
}

//void calcStatsVarsG_tsFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void maximization_tsFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsFretParameters *p = (tsFretParameters*)gv->params;
    tsFretStats *s = (tsFretStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgK = p->avgK, **avgLnK = p->avgLnK;
    double *avgLnKI = p->avgLnKI, *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *avgE = p->avgE, **avgLnE = p->avgLnE;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uKMat = p->uKMat, *sumUKArr = p->sumUKArr;
    double *uEArr = p->uEArr, *vEArr = p->vEArr;
    double *Ni = s->Ni, *Ti = s->Ti, *eps = s->eps;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        avgPi[i] = ( uPiArr[i] + gmMat[0][i] ) / ( sumUPi + 1.0 );
        avgLnPi[i] = gsl_sf_psi( uPiArr[i] + gmMat[0][i] ) - gsl_sf_psi( sumUPi + 1.0 );

        avgK[i][i] = (Ni[i] + aIArr[i]) * Nij[i] / (Nii[i] - 1.0) / (Ti[i] + bIArr[i]);
        avgLnK[i][i]  = gsl_sf_psi(Ni[i] + aIArr[i]) + gsl_sf_psi(Nij[i]);
        avgLnK[i][i] += -gsl_sf_psi(Nii[i]) - log(Ti[i] + bIArr[i]);

        avgI[i] = (Ni[i] + aIArr[i]) / (Ti[i] + bIArr[i]);
        avgLnI[i] = gsl_sf_psi(Ni[i] + aIArr[i]) - log(Ti[i] + bIArr[i]);

        avgLnKI[i]  = gsl_sf_psi(Ni[i] + aIArr[i]) + gsl_sf_psi(Nii[i] + Nij[i]);
        avgLnKI[i] += -gsl_sf_psi(Nii[i]) - log(Ti[i] + bIArr[i]);

#ifdef INTENSITY_CAP
        double meanI = (double)xnWv->N / xnWv->T;
        avgI[i]  = MIN( avgI[i], maxIntensityRatio * meanI );
        avgLnI[i]  = MIN( avgLnI[i], log(maxIntensityRatio * meanI) );
#endif
#ifdef TRANSITION_RATE_CAP
        avgK[i][i] = MIN( avgK[i][i], avgI[i]/minPhotonNumPerState );
        avgLnK[i][i]  = MIN( avgLnK[i][i], avgLnI[i] - log(minPhotonNumPerState) );
        avgLnKI[i]  = MIN( avgLnKI[i], avgLnI[i] + log(1.0 + 1.0/minPhotonNumPerState) );
#endif

        for( j = 0 ; j < sNo ; j++ ){
            if( i != j ){
                avgK[i][j] = ( uKMat[i][j] + Mij[i][j] ) / ( sumUKArr[i] + Nij[i] );
                avgLnK[i][j] = gsl_sf_psi( uKMat[i][j] + Mij[i][j] ) - gsl_sf_psi( sumUKArr[i] + Nij[i] );
            }
        }

        avgE[i] = ( uEArr[i] + eps[i] ) / ( uEArr[i] + vEArr[i] + Ni[i] );
        avgLnE[i][0]  = gsl_sf_psi( vEArr[i] + Ni[i] - eps[i] );    // ln(1-E) for donor
        avgLnE[i][0] -= gsl_sf_psi( uEArr[i] + vEArr[i] + Ni[i] );
        avgLnE[i][1]  = gsl_sf_psi( uEArr[i] + eps[i] );            // ln(E) for acceptor
        avgLnE[i][1] -= gsl_sf_psi( uEArr[i] + vEArr[i] + Ni[i] );
    }
}

//void maximizationG_tsFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

double varLowerBound_tsFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsFretParameters *p = (tsFretParameters*)gv->params;
    tsFretStats *s = (tsFretStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, *cn = iv->cn;
    double *avgLnPi = p->avgLnPi, **avgLnK = p->avgLnK, *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *avgLnKI = p->avgLnKI, **avgLnE = p->avgLnE;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uKMat = p->uKMat, *sumUKArr = p->sumUKArr;
    double *uEArr = p->uEArr, *vEArr = p->vEArr;
    double *Ni = s->Ni, *Ti = s->Ti, *eps = s->eps;
    double *Nii = s->Nii, *Nij = s->Nij, **Mij = s->Mij;
    size_t n;
    int i, j;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double Ck = 1.0, lnpKii = sNo * log(Ck);
    double lnpKij = 0.0;
    double lnpI = 0.0;
    double lnpE = 0.0;
    double lnqPi = gsl_sf_lngamma(sumUPi + 1);
    double lnqKiiI = 0.0;
    double lnqKij = 0.0;
    double lnqE = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpKii -= avgLnK[i][i];

        lnpI += aIArr[i] * log(bIArr[i]) - gsl_sf_lngamma(aIArr[i]);
        lnpI += (aIArr[i]-1.0)*avgLnI[i] - bIArr[i]*avgI[i];

        lnpE += gsl_sf_lngamma(uEArr[i]+vEArr[i]) - gsl_sf_lngamma(uEArr[i]);
        lnpE += -gsl_sf_lngamma(vEArr[i]);
        lnpE += (uEArr[i]-1.0)*avgLnE[i][1] + (vEArr[i]-1.0)*avgLnE[i][0];

        lnqPi += (uPiArr[i]+gmMat[0][i]-1.0) * (gsl_sf_psi(uPiArr[i]+gmMat[0][i]) - gsl_sf_psi(sumUPi+1.0));
        lnqPi -= gsl_sf_lngamma(uPiArr[i] + gmMat[0][i]);

        lnqKiiI += gsl_sf_lngamma(Nii[i] + Nij[i]) + (Ni[i] + aIArr[i]) * log(Ti[i] + bIArr[i]);
        lnqKiiI += -gsl_sf_lngamma(Ni[i] + aIArr[i]) -gsl_sf_lngamma(Nii[i]);
        lnqKiiI += -gsl_sf_lngamma(Nij[i]) + (Nij[i]-1.0)*avgLnK[i][i];
        lnqKiiI += (Ni[i] + Nii[i] + aIArr[i] - 1.0)*avgLnI[i];
        lnqKiiI += -(Nii[i] + Nij[i])*avgLnKI[i] - (Ti[i] + bIArr[i])*avgI[i];

        lnqE += gsl_sf_lngamma(uEArr[i] + vEArr[i] + Ni[i]);
        lnqE -= gsl_sf_lngamma(uEArr[i] + eps[i]);
        lnqE -= gsl_sf_lngamma(vEArr[i] + Ni[i] - eps[i]);
        lnqE += (uEArr[i] + eps[i] - 1.0)*avgLnE[i][1];
        lnqE += (vEArr[i] + Ni[i] - eps[i] - 1.0)*avgLnE[i][0];

        lnpKij += gsl_sf_lngamma(sumUKArr[i]);
        lnqKij += gsl_sf_lngamma(sumUKArr[i] + Nij[i]);
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                lnpKij += (uKMat[i][j]-1.0)*avgLnK[i][j] - gsl_sf_lngamma(uKMat[i][j]);
                lnqKij += (uKMat[i][j]+Mij[i][j]-1)*(gsl_sf_psi(uKMat[i][j]+Mij[i][j])-gsl_sf_psi(sumUKArr[i]+Nij[i]));
                lnqKij -= gsl_sf_lngamma( uKMat[i][j] + Mij[i][j] );
            }
        }
    }

    double lnpX = 0.0;
    for( n = 0 ; n < dLen ; n++ ){
        lnpX += log( cn[n] );
    }

    double val;
    val  = lnpPi + lnpKii + lnpKij + lnpI + lnpE;
    val -= lnqPi + lnqKiiI + lnqKij + lnqE;
    val += lnpX;

    return val;
}

//double varLowerBoundG_tsFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void reorderParameters_tsFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    tsFretParameters *p = (tsFretParameters*)gv->params;
    tsFretStats *s = (tsFretStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgK = p->avgK;
    double **avgLnK = p->avgLnK, *avgLnKI = p->avgLnKI;
    double *avgI = p->avgI, *avgLnI = p->avgLnI, *avgE = p->avgE, **avgLnE = p->avgLnE;
    double *Ni = s->Ni, *Ti = s->Ti, *eps = s->eps;
    size_t n;
    int i, j;

    int *index = (int*)malloc( sNo * sizeof(int) );
    double *store = (double*)malloc( sNo * sizeof(double) );
    double **s2D = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   s2D[i] = (double*)malloc( MAX(sNo,2) * sizeof(double) );   }

    // index indicates order of avgE values (0=biggest avgE -- sNo=smallest avgE).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgE[i] < avgE[j] ){
                    index[i]--;
                } else if( avgE[i] == avgE[j] ){
                    if( j > i )
                    {   index[i]--;   }
                }
            }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgI[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnI[i] = store[i];   }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgK[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgK[i][j] = s2D[i][j];   }
    }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgLnK[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgLnK[i][j] = s2D[i][j];   }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnKI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnKI[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgE[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgE[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ )
    {   s2D[index[i]][0] = avgLnE[i][0];
        s2D[index[i]][1] = avgLnE[i][1];   }
    for( i = 0 ; i < sNo ; i++ )
    {   avgLnE[i][0] = s2D[i][0];
        avgLnE[i][1] = s2D[i][1];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ni[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ni[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ti[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ti[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = eps[i];   }
    for( i = 0 ; i < sNo ; i++ ){   eps[i] = store[i];   }

    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = gmMat[n][i];   }
        for( i = 0 ; i < sNo ; i++ ){   gmMat[n][i] = store[i];   }
    }

    for( n = 0 ; n < dLen ; n++ ){
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = xiMat[n][i][j];   }
        }
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   xiMat[n][i][j] = s2D[i][j];   }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   free( s2D[i] );   }
    free( s2D );
    free( store );
    free( index );
}

//void reorderParametersG_tsFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void outputTsFretResults( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    tsFretParameters *p = (tsFretParameters*)gv->params;
    int sNo = gv->sNo;
    int i, j;

    fprintf(logFP, "  results: K = %d \n", sNo);

    fprintf(logFP, "   intensities: ( %g", p->avgI[0]);
    for( i = 1 ; i < sNo ; i++ )
    {   fprintf(logFP, ", %g", p->avgI[i]);   }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   FRET efficiencies: ( %g", p->avgE[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgE[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   pi: ( %g", p->avgPi[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgPi[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   k_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
            fprintf(logFP, " ( %g", p->avgK[i][0]);
            for( j = 1 ; j < sNo ; j++ )
            {   fprintf(logFP, ", %g", p->avgK[i][j]);   }
            fprintf(logFP, ")");
    }
    fprintf(logFP, " ] \n\n");

    char fn[256];
    FILE *fp;
    size_t n;

    sprintf( fn, "%s.param%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        fprintf(fp, "I, E, pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", K%dx", i);   }
        fprintf(fp, "\n");

        for( i = 0 ; i < sNo ; i++ ){
            fprintf(fp, "%g, %g, %g", p->avgI[i], p->avgE[i], p->avgPi[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgK[j][i]);   }
            fprintf(fp, "\n");
        }
        fclose(fp);
    }

    sprintf( fn, "%s.Lq%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < gv->iteration ; n++ ){
            fprintf( fp, "%24.20e\n", gv->LqArr[n] );
        }
        fclose(fp);
    }

    sprintf( fn, "%s.maxS%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < xn->N ; n++ ){
            fprintf( fp, "%d\n", iv->stateTraj[n] );
        }
        fclose(fp);
    }
}

//void outputTsFretResultsG( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//}

//