/* ndlinear.c
 *
 * Copyright (C) 2006, 2007 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 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <stdlib.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_blas.h>

#include "gsl_multifit_ndlinear.h"

static int ndlinear_construct_row(const gsl_vector *d, gsl_vector *x,
                                  gsl_multifit_ndlinear_workspace *w);

/*
gsl_multifit_ndlinear_alloc()
  Allocate a ndlinear workspace

Inputs: n_dim  - dimension of fit function
        N      - number of terms in each sum; N[i] = N_i, 0 <= i < n_dim
        u      - basis functions to call
                 u[j] = u^{(j)}, 0 <= j < n_dim
        params - parameters to pass to basis functions

Return: pointer to new workspace

Notes: the supplied basis functions 'u[j]' must accept three
       arguments:

int uj(double x, double y[], void *params)

and fill the y[] vector so that y[i] = u_{i}^{(j)}(x) (the ith
basis function for the jth parameter evaluated at x)
*/

gsl_multifit_ndlinear_workspace *
gsl_multifit_ndlinear_alloc(size_t n_dim, size_t N[],
                            int (**u)(double x, double y[], void *p),
                            void *params)
{
  gsl_multifit_ndlinear_workspace *w;
  size_t n_coeffs; /* total number of fit coefficients */
  size_t i, idx;
  size_t sum_N;

  if (n_dim == 0)
    {
      GSL_ERROR_NULL("n_dim must be at least 1", GSL_EINVAL);
    }

  w = calloc(1, sizeof(gsl_multifit_ndlinear_workspace));
  if (!w)
    {
      GSL_ERROR_NULL("failed to allocate space for workspace", GSL_ENOMEM);
    }

  w->N = calloc(n_dim, sizeof(size_t));
  if (!w->N)
    {
      gsl_multifit_ndlinear_free(w);
      GSL_ERROR_NULL("failed to allocate space for N vector", GSL_ENOMEM);
    }

  n_coeffs = 1;
  sum_N = 0;
  for (i = 0; i < n_dim; ++i)
    {
      if (N[i] == 0)
        {
          gsl_multifit_ndlinear_free(w);
          GSL_ERROR_NULL("one of the sums is empty", GSL_EINVAL);
        }

      /* The total number of coefficients is: N_1 * N_2 * ... * N_n */
      n_coeffs *= N[i];
      w->N[i] = N[i];
      sum_N += N[i];
    }

  w->n_dim = n_dim;
  w->n_coeffs = n_coeffs;

  w->work = gsl_vector_alloc(n_coeffs);
  w->work2 = gsl_vector_alloc(sum_N);
  if (!w->work || !w->work2)
    {
      gsl_multifit_ndlinear_free(w);
      GSL_ERROR_NULL("failed to allocate space for basis vector",
                     GSL_ENOMEM);
    }

  w->v = calloc(n_dim, sizeof(gsl_vector_view));
  if (!w->v)
    {
      gsl_multifit_ndlinear_free(w);
      GSL_ERROR_NULL("failed to allocate space for basis vector",
                     GSL_ENOMEM);
    }

  w->u = calloc(n_dim, sizeof(int *));
  if (!w->u)
    {
      gsl_multifit_ndlinear_free(w);
      GSL_ERROR_NULL("failed to allocate space for basis functions",
                     GSL_ENOMEM);
    }

  idx = 0;
  for (i = 0; i < n_dim; ++i)
    {
      w->v[i] = gsl_vector_subvector(w->work2, idx, N[i]);
      idx += N[i];

      w->u[i] = u[i];
    }

  w->params = params;

  return (w);
} /* gsl_multifit_ndlinear_alloc() */

/*
gsl_multifit_ndlinear_free()
  Free workspace w
*/

void
gsl_multifit_ndlinear_free(gsl_multifit_ndlinear_workspace *w)
{
  if (w->N)
    free(w->N);

  if (w->work)
    gsl_vector_free(w->work);

  if (w->work2)
    gsl_vector_free(w->work2);

  if (w->v)
    free(w->v);

  if (w->u)
    free(w->u);

  free(w);
} /* gsl_multifit_ndlinear_free() */

/*
gsl_multifit_ndlinear_design()
  This function constructs the coefficient design matrix 'X'

Inputs: vars  - independent variable vectors for matrix X
                vars is a ndata-by-n_dim matrix where the ith row
                specifies the n_dim independent variables for the
                ith observation, so that
                vars_{ij} = (x_i)_j, the jth element of the
                ith input variable vector
        X     - (output) design matrix (must be ndata-by-w->n_coeffs)
        w     - workspace

Return: success or error
*/

int
gsl_multifit_ndlinear_design(const gsl_matrix *vars, gsl_matrix *X,
                             gsl_multifit_ndlinear_workspace *w)
{
  const size_t ndata = vars->size1;

  if ((X->size1 != ndata) || (X->size2 != w->n_coeffs))
    {
      GSL_ERROR("X matrix has wrong dimensions", GSL_EBADLEN);
    }
  else
    {
      size_t i; /* looping */
      int s;

      for (i = 0; i < ndata; ++i)
        {
          gsl_vector_const_view d = gsl_matrix_const_row(vars, i);
          gsl_vector_view xv = gsl_matrix_row(X, i);

          s = ndlinear_construct_row(&d.vector, &xv.vector, w);
          if (s != GSL_SUCCESS)
            return s;
        }

      return GSL_SUCCESS;
    }
} /* gsl_multifit_ndlinear_design() */

/*
gsl_multifit_ndlinear_est()
  Compute the model function at a given data point with errors

Inputs: x     - data point (w->n_dim elements)
        c     - coefficient vector
        cov   - covariance matrix
        y     - where to store fit function result
        y_err - standard deviation of fit
        w     - workspace

Return: success or error
*/

int
gsl_multifit_ndlinear_est(const gsl_vector *x, const gsl_vector *c,
                          const gsl_matrix *cov, double *y, double *y_err,
                          gsl_multifit_ndlinear_workspace *w)
{
  if (c->size != w->n_coeffs)
    {
      GSL_ERROR("c vector has wrong size", GSL_EBADLEN);
    }
  else
    {
      int s;

      s = ndlinear_construct_row(x, w->work, w);
      if (s != GSL_SUCCESS)
        return s;

      /*
       * Now w->work contains the appropriate basis functions
       * evaluated at the given point - compute the function value
       */
      s = gsl_multifit_linear_est(w->work, c, cov, y, y_err);

      return s;
    }
} /* gsl_multifit_ndlinear_est() */

/*
gsl_multifit_ndlinear_calc()
  Compute the model function at a given data point

Inputs: x - data point (w->n_dim elements)
        c - coefficient vector
        w - workspace

Return: model value
*/

double
gsl_multifit_ndlinear_calc(const gsl_vector *x, const gsl_vector *c,
                           gsl_multifit_ndlinear_workspace *w)
{
  if (c->size != w->n_coeffs)
    {
      GSL_ERROR_VAL("c vector has wrong size", GSL_EBADLEN, 0.0);
    }
  else
    {
      double y;
      int s;

      s = ndlinear_construct_row(x, w->work, w);
      if (s != GSL_SUCCESS)
        {
          GSL_ERROR_VAL("constructing matrix row failed", s, 0.0);
        }

      gsl_blas_ddot(w->work, c, &y);

      return y;
    }
} /* gsl_multifit_ndlinear_calc() */

/*
gsl_multifit_ndlinear_ncoeffs()
  Return the total number of fit coefficients
*/

size_t
gsl_multifit_ndlinear_ncoeffs(gsl_multifit_ndlinear_workspace *w)
{
  return w->n_coeffs;
} /* gsl_multifit_ndlinear_ncoeffs() */

/******************************************
 *         INTERNAL ROUTINES              *
 ******************************************/

/*
ndlinear_construct_row()

  Compute a row of the design matrix X:

X(:,j) = u_{r_0}^{(0)}(d_0) * u_{r_1)^{(1)}(d_1) * ... *
         u_{r_{n-1}}^{(n-1)}(d_{n-1})

where 'd' is the corresponding data vector for that row

Inputs: d - data vector of length w->n_dim
        x - (output) where to store row of design matrix X
        w - workspace

Return: success or error
*/

static int
ndlinear_construct_row(const gsl_vector *d, gsl_vector *x,
                       gsl_multifit_ndlinear_workspace *w)
{
  size_t j;
  int k, s;
  size_t denom, rk;
  double melement;

  /* compute basis functions for this data point */
  for (j = 0; j < w->n_dim; ++j)
    {
      s = w->u[j](gsl_vector_get(d, j), w->v[j].vector.data, w->params);

      if (s != GSL_SUCCESS)
        return s;
    }

  for (j = 0; j < w->n_coeffs; ++j)
    {
      /*
       * The (:,j) element of the matrix X will be:
       *
       * X_{:,j} = u_{r_0}^{(0)}(d_0) *
       *           u_{r_1)^{(1)}(d_1) *
       *           ... *
       *           u_{r_{n-1}}^{(n-1)}(d_{n-1})
       *
       * with the basis function indices r_k given by
       *
       * r_k = floor(j / Prod_{i=(k+1)..(n-1)} [ N_i ]) (mod N_k)
       *
       * In the case where N_i = N for all i,
       *
       * r_k = floor(j / N^{n - k - 1}) (mod N)
       *
       * n: dimension of fit function (w->n_dim)
       * N_i: number of terms in sum i of fit function
       */

      /* calculate the r_k and the matrix element X_{:,j} */

      denom = 1;
      melement = 1.0;
      for (k = (int)(w->n_dim - 1); k >= 0; --k)
        {
          rk = (j / denom) % w->N[k];
          denom *= w->N[k];

          melement *= gsl_vector_get(&(w->v[k]).vector, rk);
        }

      /* set the matrix element */
      gsl_vector_set(x, j, melement);
    }

  return GSL_SUCCESS;
} /* ndlinear_construct_row() */


/*
 *  vbHmmGaussDiffusion.c
 *  Model-specific core functions for VB-HMM-GAUSS-DIFFUSION.
 *
 *  Created by OKAMOTO Kenji, SAKO Yasushi and RIKEN
 *  Copyright 2011-2016
 *  Cellular Informatics Laboratory, Advance Science Institute, RIKEN, Japan.
 *  All rights reserved.
 *
 *  Ver. 1.0.0
 *  Last modified on 2016.02.08
 */

#include "vbHmmGaussDiffusion.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

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

static int isGlobalAnalysis = 0;

void setFunctions_gaussDiff(){
    commonFunctions funcs;
    funcs.newModelParameters  = newModelParameters_gaussDiff;
    funcs.freeModelParameters = freeModelParameters_gaussDiff;
    funcs.newModelStats       = newModelStats_gaussDiff;
    funcs.freeModelStats      = freeModelStats_gaussDiff;
    funcs.initializeVbHmm     = initializeVbHmm_gaussDiff;
    funcs.pTilde_z1           = pTilde_z1_gaussDiff;
    funcs.pTilde_zn_zn1       = pTilde_zn_zn1_gaussDiff;
    funcs.pTilde_xn_zn        = pTilde_xn_zn_gaussDiff;
    funcs.calcStatsVars       = calcStatsVars_gaussDiff;
    funcs.maximization        = maximization_gaussDiff;
    funcs.varLowerBound       = varLowerBound_gaussDiff;
    funcs.reorderParameters   = reorderParameters_gaussDiff;
    funcs.outputResults       = outputResults_gaussDiff;
    setFunctions( funcs );
}

void setGFunctions_gaussDiff(){
    gCommonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_gaussDiff;
    funcs.freeModelParameters   = freeModelParameters_gaussDiff;
    funcs.newModelStats         = newModelStats_gaussDiff;
    funcs.freeModelStats        = freeModelStats_gaussDiff;
    funcs.newModelStatsG        = newModelStatsG_gaussDiff;
    funcs.freeModelStatsG       = freeModelStatsG_gaussDiff;
    funcs.initializeVbHmmG      = initializeVbHmmG_gaussDiff;
    funcs.pTilde_z1             = pTilde_z1_gaussDiff;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_gaussDiff;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_gaussDiff;
    funcs.calcStatsVarsG        = calcStatsVarsG_gaussDiff;
    funcs.maximizationG         = maximizationG_gaussDiff;
    funcs.varLowerBoundG        = varLowerBoundG_gaussDiff;
    funcs.reorderParametersG    = reorderParametersG_gaussDiff;
    funcs.outputResultsG        = outputResultsG_gaussDiff;
    setGFunctions( funcs );
    isGlobalAnalysis = 1;
}

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

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

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

    p->uPiArr = (double*)malloc( sNo * sizeof(double) );
    p->sumUPi = 0.0;
    p->uAMat = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->uAMat[i] = (double*)malloc( sNo * sizeof(double) );
    }
    p->sumUAArr = (double*)malloc( sNo * sizeof(double) );

    p->avgPi = (double *)malloc( sNo * sizeof(double) );
    p->avgLnPi = (double *)malloc( sNo * sizeof(double) );
    p->avgA = (double **)malloc( sNo * sizeof(double*) );
    p->avgLnA = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->avgA[i] = (double *)malloc( sNo * sizeof(double) );
        p->avgLnA[i] = (double *)malloc( sNo * sizeof(double) );
    }
    p->avgDlt = (double *)malloc( sNo * sizeof(double) );
    p->avgLnDlt = (double *)malloc( sNo * sizeof(double) );

    p->uAArr = (double *)malloc( sNo * sizeof(double) );
    p->uBArr = (double *)malloc( sNo * sizeof(double) );
    p->aDlt = (double *)malloc( sNo * sizeof(double) );
    p->bDlt = (double *)malloc( sNo * sizeof(double) );

    return p;
}

void freeModelParameters_gaussDiff( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    gaussDiffParameters *gp = *p;
    int i;

    free( gp->uPiArr );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->uAMat[i] );
    }
    free( gp->uAMat );
    free( gp->sumUAArr );

    free( gp->avgPi );
    free( gp->avgLnPi );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgA[i] );
        free( gp->avgLnA[i] );
    }
    free( gp->avgA );
    free( gp->avgLnA );
    free( gp->avgDlt );
    free( gp->avgLnDlt );

    free( gp->uAArr );
    free( gp->uBArr );
    free( gp->aDlt );
    free( gp->bDlt );

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

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

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

        return s;

    } else {

        return NULL;

    }
}

void freeModelStats_gaussDiff( s, xn, gv, iv )
void **s;
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        gaussDiffStats *gs = *s;
        int i;

        free( gs->Ni );
        free( gs->Ri );
        for( i = 0 ; i < sNo ; i++ )
        {   free( gs->Nij[i] );   }
        free( gs->Nij );
        free( gs->Nii );

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

void *newModelStatsG_gaussDiff( xns, gv, ivs)
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    int sNo = gv->sNo;
    gaussDiffGlobalStats *gs = (gaussDiffGlobalStats*)malloc( sizeof(gaussDiffGlobalStats) );

    int i;
    gs->NiR = (double *)malloc( sNo * sizeof(double) );
    gs->RiR = (double *)malloc( sNo * sizeof(double) );
    gs->NijR = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   gs->NijR[i] = (double *)malloc( sNo * sizeof(double) );   }
    gs->NiiR = (double *)malloc( sNo * sizeof(double) );
    gs->z1iR = (double *)malloc( sNo * sizeof(double) );

    return gs;
}

void freeModelStatsG_gaussDiff( gs, xns, gv, ivs )
void **gs;
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    int sNo = gv->sNo;
    gaussDiffGlobalStats *ggs = *gs;
    int i;
    free( ggs->NiR );
    for( i = 0 ; i < sNo ; i++ )
    {   free( ggs->NijR[i] );   }
    free( ggs->NijR );
    free( ggs->NiiR );
    free( ggs->RiR );
    free( ggs->z1iR );

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

void initializeVbHmm_gaussDiff( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    int sNo = gv->sNo;
    gaussDiffParameters *p = gv->params;
    int i, j;

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

    // hyper parameter for p( A(i,j) )
    for( i = 0 ; i < sNo ; i++ ){
        p->sumUAArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            if( j == i ){
                p->uAMat[i][j] = 5.0;
            } else {
                p->uAMat[i][j] = 1.0;
            }
            p->sumUAArr[i] += p->uAMat[i][j];
        }
    }

    // hyper parameter for p( delta(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->uAArr[i] = 1.0;
        p->uBArr[i] = 0.0005;
    }

    initialize_indVars_gaussDiff( xn, gv, iv );

    calcStatsVars_gaussDiff( xn, gv, iv );
    maximization_gaussDiff( xn, gv, iv );
}

void initializeVbHmmG_gaussDiff( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    int sNo = gv->sNo, rNo = xns->R;
    gaussDiffParameters *p = gv->params;
    int i, j, r;

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

    for( i = 0 ; i < sNo ; i++ ){
        p->sumUAArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            if( j == i ){
                p->uAMat[i][j] = 5.0;
            } else {
                p->uAMat[i][j] = 1.0;
            }
            p->sumUAArr[i] += p->uAMat[i][j];
        }
    }

    // hyper parameter for p( delta(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->uAArr[i] = 1.0;
        p->uBArr[i] = 0.0005;
    }

    for( r = 0 ; r < rNo ; r++ ){
        initialize_indVars_gaussDiff( xns->xn[r], gv, ivs->indVars[r] );
    }

    calcStatsVarsG_gaussDiff( xns, gv, ivs );
    maximizationG_gaussDiff( xns, gv, ivs );
}

void initialize_indVars_gaussDiff( 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_gaussDiff( 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 = (gaussDiffData*)malloc( sizeof(gaussDiffData) );
    gaussDiffData *d = (gaussDiffData*)xn->data;
    d->v = NULL;
    return xn;
}

void freeXnDataSet_gaussDiff( xn )
xnDataSet **xn;
{
    gaussDiffData *d = (gaussDiffData*)(*xn)->data;
    free( d->v );
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

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

double pTilde_zn_zn1_gaussDiff( i, j, params )
int i, j;
void *params;
{
    gaussDiffParameters *p = (gaussDiffParameters*)params;
    return exp( p->avgLnA[i][j] );
}

double pTilde_xn_zn_gaussDiff( xnWv, n, i, params )
xnDataSet *xnWv;
size_t n;
int i;
void *params;
{
    gaussDiffParameters *p = (gaussDiffParameters*)params;
    gaussDiffData *xn = (gaussDiffData*)xnWv->data;
    double val;
    val  = p->avgLnDlt[i] - log(2.0);
    val -= log(xn->v[n]) + p->avgDlt[i] * pow( xn->v[n], 2.0) / 4.0;
    return exp(val);
}

void calcStatsVars_gaussDiff( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    gaussDiffData *d = (gaussDiffData*)xn->data;
    gaussDiffStats *s = (gaussDiffStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Ni = s->Ni, *Ri = s->Ri, *Nii = s->Nii, **Nij = s->Nij;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i]   = 1e-10;
        Ri[i]   = 1e-10;
        Nii[i]  = 1e-10;
        for( j = 0 ; j < sNo ; j++ ){
            Nij[i][j] = 1e-10;
        }
        for( n = 0 ; n < dLen ; n++ ){
            Ni[i] += gmMat[n][i];
            Ri[i] += gmMat[n][i] * pow( d->v[n], 2.0 );
            for( j = 0 ; j < sNo ; j++ ){
                Nii[i]    += xiMat[n][i][j];
                Nij[i][j] += xiMat[n][i][j];
            }
        }
    }
}

void calcStatsVarsG_gaussDiff( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    gaussDiffData *d;
    gaussDiffGlobalStats *gs = (gaussDiffGlobalStats*)ivs->stats;
    int sNo = gv->sNo, rNo = xns->R;
    double **gmMat, ***xiMat;
    double *NiR = gs->NiR, *RiR = gs->RiR, *NiiR = gs->NiiR;
    double **NijR = gs->NijR, *z1iR = gs->z1iR;
    size_t dLen, n;
    int i, j, r;

    for( i = 0 ; i < sNo ; i++ ){
        NiR[i]   = 1e-10;
        RiR[i]  = 1e-10;
        NiiR[i]  = 1e-10;
        for( j = 0 ; j < sNo ; j++ ){
            NijR[i][j] = 1e-10;
        }
        z1iR[i] = 1e-10;
    }
    for( r = 0 ; r < rNo ; r++ ){
        d = (gaussDiffData*)xns->xn[r]->data;
        dLen = xns->xn[r]->N;
        gmMat = ivs->indVars[r]->gmMat;
        xiMat = ivs->indVars[r]->xiMat;
        for( i = 0 ; i < sNo ; i++ ){
            z1iR[i] += gmMat[0][i];
            for( n = 0 ; n < dLen ; n++ ){
                NiR[i] += gmMat[n][i];
                RiR[i] += gmMat[n][i] * pow( d->v[n], 2.0 );
                for( j = 0 ; j < sNo ; j++ ){
                    NiiR[i]    += xiMat[n][i][j];
                    NijR[i][j] += xiMat[n][i][j];
                }
            }
        }
    }
}

void maximization_gaussDiff( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    gaussDiffParameters *p = (gaussDiffParameters*)gv->params;
    gaussDiffStats *s = (gaussDiffStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uAArr = p->uAArr, *uBArr = p->uBArr, *aDlt = p->aDlt, *bDlt = p->bDlt;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgDlt = p->avgDlt, *avgLnDlt = p->avgLnDlt;
    double *Ni = s->Ni, *Ri = s->Ri, *Nii = s->Nii, **Nij = s->Nij;
    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 );

        for( j = 0 ; j < sNo ; j++ ){
            avgA[i][j] = ( uAMat[i][j] + Nij[i][j] ) / ( sumUAArr[i] + Nii[i] );
            avgLnA[i][j] = gsl_sf_psi( uAMat[i][j] + Nij[i][j] ) - gsl_sf_psi( sumUAArr[i] + Nii[i] );
        }

        aDlt[i] = uAArr[i] + Ni[i];
        bDlt[i] = uBArr[i] + Ri[i] / 4.0;

        avgDlt[i]   = aDlt[i] / bDlt[i];
        avgLnDlt[i] = gsl_sf_psi( aDlt[i] ) - log( bDlt[i] );
    }
}

void maximizationG_gaussDiff( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    gaussDiffParameters *p = (gaussDiffParameters*)gv->params;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uAArr = p->uAArr, *uBArr = p->uBArr, *aDlt = p->aDlt, *bDlt = p->bDlt;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgDlt = p->avgDlt, *avgLnDlt = p->avgLnDlt;
    gaussDiffGlobalStats *gs = (gaussDiffGlobalStats*)ivs->stats;
    double *NiR = gs->NiR, *NiiR = gs->NiiR, **NijR = gs->NijR, *RiR = gs->RiR;
    double *z1iR = gs->z1iR, dR = (double)(xns->R);
    int sNo = gv->sNo;
    int i, j;

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

        for( j = 0 ; j < sNo ; j++ ){
            avgA[i][j] = ( uAMat[i][j] + NijR[i][j] ) / ( sumUAArr[i] + NiiR[i] );
            avgLnA[i][j] = gsl_sf_psi( uAMat[i][j] + NijR[i][j] ) - gsl_sf_psi( sumUAArr[i] + NiiR[i] );
        }

        aDlt[i] = uAArr[i] + NiR[i];
        bDlt[i] = uBArr[i] + RiR[i] / 4.0;

        avgDlt[i]   = aDlt[i] / bDlt[i];
        avgLnDlt[i] = gsl_sf_psi( aDlt[i] ) - log( bDlt[i] );
    }
}

double varLowerBound_gaussDiff( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    gaussDiffParameters *p = (gaussDiffParameters*)gv->params;
    gaussDiffStats *s = (gaussDiffStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, *cn = iv->cn;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uAArr = p->uAArr, *uBArr = p->uBArr;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    double *avgDlt = p->avgDlt, *avgLnDlt = p->avgLnDlt;
    double *Nii = s->Nii, **Nij = s->Nij;
    double *aDlt = p->aDlt, *bDlt = p->bDlt;
    size_t n;
    int i, j;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpDlt = 0.0;
    double lnqPi = gsl_sf_lngamma(sumUPi + 1.0);
    double lnqA = 0.0;
    double lnqDlt = - sNo / 2.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpDlt += - gsl_sf_lngamma(uAArr[i]) + uAArr[i] * log(uBArr[i]);
        lnpDlt += (uAArr[i] - 1.0) * avgLnDlt[i] - uBArr[i] * avgDlt[i];

        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]);

        lnpA += gsl_sf_lngamma(sumUAArr[i]);
        lnqA += gsl_sf_lngamma(sumUAArr[i] + Nii[i]);
        for( j = 0 ; j < sNo ; j++ ){
            lnpA += (uAMat[i][j]-1.0)*avgLnA[i][j] - gsl_sf_lngamma(uAMat[i][j]);

            lnqA += (uAMat[i][j] + Nij[i][j] - 1.0) * (gsl_sf_psi(uAMat[i][j]+Nij[i][j]) - gsl_sf_psi(sumUAArr[i]+Nii[i]));
            lnqA -= gsl_sf_lngamma( uAMat[i][j] + Nij[i][j] );
        }

        lnqDlt += - gsl_sf_lngamma(aDlt[i]) + aDlt[i] * log(bDlt[i]);
        lnqDlt += (aDlt[i] - 1.0) * avgLnDlt[i] - aDlt[i];
    }

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

    double val;
    val  = lnpPi + lnpA + lnpDlt;
    val -= lnqPi + lnqA + lnqDlt;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

    return val;
}

double varLowerBoundG_gaussDiff( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    gaussDiffParameters *p = (gaussDiffParameters*)gv->params;
    int sNo = gv->sNo, rNo = xns->R;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uAArr = p->uAArr, *uBArr = p->uBArr;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    double *avgDlt = p->avgDlt, *avgLnDlt = p->avgLnDlt;
    double *aDlt = p->aDlt, *bDlt = p->bDlt;
    gaussDiffGlobalStats *gs = (gaussDiffGlobalStats*)ivs->stats;
    double *NiiR = gs->NiiR, **NijR = gs->NijR, *z1iR = gs->z1iR, dR = (double)xns->R;
    size_t n;
    int i, j, r;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpDlt = 0.0;
    double lnqPi = gsl_sf_lngamma(sumUPi + dR);
    double lnqA = 0.0;
    double lnqDlt = - sNo / 2.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpDlt += - gsl_sf_lngamma(uAArr[i]) + uAArr[i] * log(uBArr[i]);
        lnpDlt += (uAArr[i] - 1.0) * avgLnDlt[i] - uBArr[i] * avgDlt[i];

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

        lnpA += gsl_sf_lngamma(sumUAArr[i]);
        lnqA += gsl_sf_lngamma(sumUAArr[i] + NiiR[i]);
        for( j = 0 ; j < sNo ; j++ ){
            lnpA += (uAMat[i][j]-1.0)*avgLnA[i][j] - gsl_sf_lngamma(uAMat[i][j]);

            lnqA += (uAMat[i][j] + NijR[i][j] - 1.0) * (gsl_sf_psi(uAMat[i][j]+NijR[i][j]) - gsl_sf_psi(sumUAArr[i]+NiiR[i]));
            lnqA -= gsl_sf_lngamma( uAMat[i][j] + NijR[i][j] );
        }

        lnqDlt += - gsl_sf_lngamma(aDlt[i]) + aDlt[i] * log(bDlt[i]);
        lnqDlt += (aDlt[i] - 1.0) * avgLnDlt[i] - aDlt[i];
    }

    double lnpX = 0.0;
    for( r = 0 ; r < rNo ; r++ ){
        size_t dLen = xns->xn[r]->N;
        for( n = 0 ; n < dLen ; n++ ){
            lnpX += log( ivs->indVars[r]->cn[n] );
        }
    }

    double val;
    val  = lnpPi + lnpA + lnpDlt;
    val -= lnqPi + lnqA + lnqDlt;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

    return val;
}

void reorderParameters_gaussDiff( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    gaussDiffParameters *p = (gaussDiffParameters*)gv->params;
    gaussDiffStats *s = (gaussDiffStats*)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, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgDlt = p->avgDlt, *avgLnDlt = p->avgLnDlt;
    double *Ni = s->Ni;
    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 avgDlt values (0=biggest avgDlt -- sNo=smallest avgDlt).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgDlt[i] < avgDlt[j] ){
                    index[i]--;
                } else if( avgDlt[i] == avgDlt[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]] = avgDlt[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgDlt[i] = store[i];   }

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

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

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

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ni[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ni[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_gaussDiff( xns, gv, ivs )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
{
    gaussDiffParameters *p = (gaussDiffParameters*)gv->params;
    size_t dLen;
    int sNo = gv->sNo, rNo = xns->R;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgDlt = p->avgDlt, *avgLnDlt = p->avgLnDlt;
    size_t n;
    int i, j, r;

    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 avgMu values (0=biggest avgMu -- sNo=smallest avgMu).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgDlt[i] < avgDlt[j] ){
                    index[i]--;
                } else if( avgDlt[i] == avgDlt[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]] = avgDlt[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgDlt[i] = store[i];   }

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

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

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

    double *NiR = ((gaussDiffGlobalStats*)ivs->stats)->NiR;
    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = NiR[i];   }
    for( i = 0 ; i < sNo ; i++ ){   NiR[i] = store[i];   }

    for( r = 0 ; r < rNo ; r++ ){
        double **gmMat = ivs->indVars[r]->gmMat;
        dLen = xns->xn[r]->N;
        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( r = 0 ; r < rNo ; r++ ){
        double ***xiMat = ivs->indVars[r]->xiMat;
        dLen = xns->xn[r]->N;
        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 outputGaussDiffResults( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    gaussDiffParameters *p = (gaussDiffParameters*)gv->params;
    int sNo = gv->sNo;
    int i, j;
    fprintf(logFP, "  results: K = %d \n", sNo);

    fprintf(logFP, "   delta: ( %g", p->avgDlt[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgDlt[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, "   A_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
            fprintf(logFP, " ( %g", p->avgA[i][0]);
            for( j = 1 ; j < sNo ; j++ )
            {   fprintf(logFP, ", %g", p->avgA[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, "delta, pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", A%dx", i);   }
        fprintf(fp, "\n");

        for( i = 0 ; i < sNo ; i++ ){
            fprintf(fp, "%g, %g", p->avgDlt[i], p->avgPi[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgA[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 outputGaussDiffResultsG( xns, gv, ivs, logFP )
xnDataBundle *xns;
globalVars *gv;
indVarBundle *ivs;
FILE *logFP;
{
    int sNo = gv->sNo, rNo = xns->R;
    gaussDiffParameters *p = (gaussDiffParameters*)gv->params;
    int i, j, r;

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

    fprintf(logFP, "   delta: ( %g", p->avgDlt[0]);
    for( i = 1 ; i < sNo ; i++ )
    {   fprintf(logFP, ", %g", p->avgDlt[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, "   A_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
        fprintf(logFP, " ( %g", p->avgA[i][0]);
        for( j = 1 ; j < sNo ; j++ )
        {   fprintf(logFP, ", %g", p->avgA[i][j]);   }
        fprintf(logFP, ")");
    }
    fprintf(logFP, " ] \n\n");

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

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

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

    sprintf( fn, "%s.Lq%03d", xns->xn[0]->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", xns->xn[0]->name, sNo );
    int flag = 0;
    if( (fp = fopen( fn, "w")) != NULL ){
        n = 0;
        do{
            flag = 1;
            for( r = 0 ; r < rNo ; r++ ){
                xnDataSet *xn = xns->xn[r];
                indVars *iv = ivs->indVars[r];

                if( r > 0 ){
                    fprintf( fp, "," );
                }
                if( n < xn->N ){
                    fprintf( fp, "%d", iv->stateTraj[n] );
                }
                flag &= (n >= (xn->N - 1));
            }
            fprintf( fp, "\n" );
            n++;
        }while( !flag );
        fclose(fp);
    }
}

//


#pragma once

#include <type_traits>
#include <utility>

#include <cuda/define_specifiers.hpp>
#include <cuda/runtime_api.hpp>

#include <gsl-lite/gsl-lite.hpp>

namespace thrustshift {

namespace kernel {

template <typename SrcT, typename DstT, class F>
__global__ void transform(gsl_lite::span<const SrcT> src,
                          gsl_lite::span<DstT> dst,
                          F f) {

	const auto gtid = threadIdx.x + blockIdx.x * blockDim.x;
	if (gtid < src.size()) {
		dst[gtid] = f(src[gtid]);
	}
}

} // namespace kernel

namespace async {

template <class SrcRange, class DstRange, class UnaryFunctor>
void transform(cuda::stream_t& stream,
               SrcRange&& src,
               DstRange&& dst,
               UnaryFunctor f) {
	gsl_Expects(src.size() == dst.size());

	if (src.empty()) {
		return;
	}

	using src_value_type =
	    typename std::remove_reference<SrcRange>::type::value_type;
	using dst_value_type =
	    typename std::remove_reference<DstRange>::type::value_type;

	constexpr cuda::grid::block_dimension_t block_dim = 128;
	const cuda::grid::dimension_t grid_dim =
	    (src.size() + block_dim - 1) / block_dim;
	auto c = cuda::make_launch_config(grid_dim, block_dim);
	auto k = kernel::transform<src_value_type, dst_value_type, decltype(f)>;
	cuda::enqueue_launch(k, stream, c, src, dst, f);
}

} // namespace async

namespace array {

namespace detail {

template <typename T,
          std::size_t N,
          template <typename, std::size_t>
          class Arr,
          class F,
          std::size_t... I>
CUDA_FHD auto transform_impl(const Arr<T, N>& arr,
                             F&& f,
                             std::index_sequence<I...>) {
	return Arr<T, N>({f(arr[I])...});
}

} // namespace detail

template <typename T,
          std::size_t N,
          template <typename, std::size_t>
          class Arr,
          class F,
          typename Indices = std::make_index_sequence<N>>
CUDA_FHD auto transform(const Arr<T, N>& arr, F&& f) {
	return detail::transform_impl(arr, std::forward<F>(f), Indices{});
}

} // namespace array

namespace tuple {

namespace detail {

template <typename Tuple, typename F, std::size_t... I>
CUDA_FHD auto transform_impl(Tuple&& t, F&& f, std::index_sequence<I...>) {
	using TupleT = typename std::remove_reference<Tuple>::type;
	using std::get;
	return TupleT(f(get<I>(t))...);
}

} // namespace detail

template <typename... Ts, template <typename...> class TupleT, typename F>
CUDA_FHD auto transform(TupleT<Ts...> const& t, F&& f) {
	using std::tuple_size;
	auto seq = std::make_index_sequence<tuple_size<TupleT<Ts...>>::value>{};
	return detail::transform_impl(t, std::forward<F>(f), seq);
}

} // namespace tuple

} // namespace thrustshift


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

#ifndef __GSL_MATRIX_INT_H__
#define __GSL_MATRIX_INT_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_inline.h>
#include <gsl/gsl_check_range.h>
#include <gsl/gsl_vector_int.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t size1;
  size_t size2;
  size_t tda;
  int * data;
  gsl_block_int * block;
  int owner;
} gsl_matrix_int;

typedef struct
{
  gsl_matrix_int matrix;
} _gsl_matrix_int_view;

typedef _gsl_matrix_int_view gsl_matrix_int_view;

typedef struct
{
  gsl_matrix_int matrix;
} _gsl_matrix_int_const_view;

typedef const _gsl_matrix_int_const_view gsl_matrix_int_const_view;

/* Allocation */

gsl_matrix_int *
gsl_matrix_int_alloc (const size_t n1, const size_t n2);

gsl_matrix_int *
gsl_matrix_int_calloc (const size_t n1, const size_t n2);

gsl_matrix_int *
gsl_matrix_int_alloc_from_block (gsl_block_int * b,
                                   const size_t offset,
                                   const size_t n1,
                                   const size_t n2,
                                   const size_t d2);

gsl_matrix_int *
gsl_matrix_int_alloc_from_matrix (gsl_matrix_int * m,
                                    const size_t k1,
                                    const size_t k2,
                                    const size_t n1,
                                    const size_t n2);

gsl_vector_int *
gsl_vector_int_alloc_row_from_matrix (gsl_matrix_int * m,
                                        const size_t i);

gsl_vector_int *
gsl_vector_int_alloc_col_from_matrix (gsl_matrix_int * m,
                                        const size_t j);

void gsl_matrix_int_free (gsl_matrix_int * m);

/* Views */

_gsl_matrix_int_view
gsl_matrix_int_submatrix (gsl_matrix_int * m,
                            const size_t i, const size_t j,
                            const size_t n1, const size_t n2);

_gsl_vector_int_view
gsl_matrix_int_row (gsl_matrix_int * m, const size_t i);

_gsl_vector_int_view
gsl_matrix_int_column (gsl_matrix_int * m, const size_t j);

_gsl_vector_int_view
gsl_matrix_int_diagonal (gsl_matrix_int * m);

_gsl_vector_int_view
gsl_matrix_int_subdiagonal (gsl_matrix_int * m, const size_t k);

_gsl_vector_int_view
gsl_matrix_int_superdiagonal (gsl_matrix_int * m, const size_t k);

_gsl_vector_int_view
gsl_matrix_int_subrow (gsl_matrix_int * m, const size_t i,
                         const size_t offset, const size_t n);

_gsl_vector_int_view
gsl_matrix_int_subcolumn (gsl_matrix_int * m, const size_t j,
                            const size_t offset, const size_t n);

_gsl_matrix_int_view
gsl_matrix_int_view_array (int * base,
                             const size_t n1,
                             const size_t n2);

_gsl_matrix_int_view
gsl_matrix_int_view_array_with_tda (int * base,
                                      const size_t n1,
                                      const size_t n2,
                                      const size_t tda);

_gsl_matrix_int_view
gsl_matrix_int_view_vector (gsl_vector_int * v,
                              const size_t n1,
                              const size_t n2);

_gsl_matrix_int_view
gsl_matrix_int_view_vector_with_tda (gsl_vector_int * v,
                                       const size_t n1,
                                       const size_t n2,
                                       const size_t tda);

_gsl_matrix_int_const_view
gsl_matrix_int_const_submatrix (const gsl_matrix_int * m,
                                  const size_t i, const size_t j,
                                  const size_t n1, const size_t n2);

_gsl_vector_int_const_view
gsl_matrix_int_const_row (const gsl_matrix_int * m,
                            const size_t i);

_gsl_vector_int_const_view
gsl_matrix_int_const_column (const gsl_matrix_int * m,
                               const size_t j);

_gsl_vector_int_const_view
gsl_matrix_int_const_diagonal (const gsl_matrix_int * m);

_gsl_vector_int_const_view
gsl_matrix_int_const_subdiagonal (const gsl_matrix_int * m,
                                    const size_t k);

_gsl_vector_int_const_view
gsl_matrix_int_const_superdiagonal (const gsl_matrix_int * m,
                                      const size_t k);

_gsl_vector_int_const_view
gsl_matrix_int_const_subrow (const gsl_matrix_int * m, const size_t i,
                               const size_t offset, const size_t n);

_gsl_vector_int_const_view
gsl_matrix_int_const_subcolumn (const gsl_matrix_int * m, const size_t j,
                                  const size_t offset, const size_t n);

_gsl_matrix_int_const_view
gsl_matrix_int_const_view_array (const int * base,
                                   const size_t n1,
                                   const size_t n2);

_gsl_matrix_int_const_view
gsl_matrix_int_const_view_array_with_tda (const int * base,
                                            const size_t n1,
                                            const size_t n2,
                                            const size_t tda);

_gsl_matrix_int_const_view
gsl_matrix_int_const_view_vector (const gsl_vector_int * v,
                                    const size_t n1,
                                    const size_t n2);

_gsl_matrix_int_const_view
gsl_matrix_int_const_view_vector_with_tda (const gsl_vector_int * v,
                                             const size_t n1,
                                             const size_t n2,
                                             const size_t tda);

/* Operations */

void gsl_matrix_int_set_zero (gsl_matrix_int * m);
void gsl_matrix_int_set_identity (gsl_matrix_int * m);
void gsl_matrix_int_set_all (gsl_matrix_int * m, int x);

int gsl_matrix_int_fread (FILE * stream, gsl_matrix_int * m) ;
int gsl_matrix_int_fwrite (FILE * stream, const gsl_matrix_int * m) ;
int gsl_matrix_int_fscanf (FILE * stream, gsl_matrix_int * m);
int gsl_matrix_int_fprintf (FILE * stream, const gsl_matrix_int * m, const char * format);

int gsl_matrix_int_memcpy(gsl_matrix_int * dest, const gsl_matrix_int * src);
int gsl_matrix_int_swap(gsl_matrix_int * m1, gsl_matrix_int * m2);

int gsl_matrix_int_swap_rows(gsl_matrix_int * m, const size_t i, const size_t j);
int gsl_matrix_int_swap_columns(gsl_matrix_int * m, const size_t i, const size_t j);
int gsl_matrix_int_swap_rowcol(gsl_matrix_int * m, const size_t i, const size_t j);
int gsl_matrix_int_transpose (gsl_matrix_int * m);
int gsl_matrix_int_transpose_memcpy (gsl_matrix_int * dest, const gsl_matrix_int * src);

int gsl_matrix_int_max (const gsl_matrix_int * m);
int gsl_matrix_int_min (const gsl_matrix_int * m);
void gsl_matrix_int_minmax (const gsl_matrix_int * m, int * min_out, int * max_out);

void gsl_matrix_int_max_index (const gsl_matrix_int * m, size_t * imax, size_t *jmax);
void gsl_matrix_int_min_index (const gsl_matrix_int * m, size_t * imin, size_t *jmin);
void gsl_matrix_int_minmax_index (const gsl_matrix_int * m, size_t * imin, size_t * jmin, size_t * imax, size_t * jmax);

int gsl_matrix_int_equal (const gsl_matrix_int * a, const gsl_matrix_int * b);

int gsl_matrix_int_isnull (const gsl_matrix_int * m);
int gsl_matrix_int_ispos (const gsl_matrix_int * m);
int gsl_matrix_int_isneg (const gsl_matrix_int * m);
int gsl_matrix_int_isnonneg (const gsl_matrix_int * m);

int gsl_matrix_int_add (gsl_matrix_int * a, const gsl_matrix_int * b);
int gsl_matrix_int_sub (gsl_matrix_int * a, const gsl_matrix_int * b);
int gsl_matrix_int_mul_elements (gsl_matrix_int * a, const gsl_matrix_int * b);
int gsl_matrix_int_div_elements (gsl_matrix_int * a, const gsl_matrix_int * b);
int gsl_matrix_int_scale (gsl_matrix_int * a, const double x);
int gsl_matrix_int_add_constant (gsl_matrix_int * a, const double x);
int gsl_matrix_int_add_diagonal (gsl_matrix_int * a, const double x);

/***********************************************************************/
/* The functions below are obsolete                                    */
/***********************************************************************/
int gsl_matrix_int_get_row(gsl_vector_int * v, const gsl_matrix_int * m, const size_t i);
int gsl_matrix_int_get_col(gsl_vector_int * v, const gsl_matrix_int * m, const size_t j);
int gsl_matrix_int_set_row(gsl_matrix_int * m, const size_t i, const gsl_vector_int * v);
int gsl_matrix_int_set_col(gsl_matrix_int * m, const size_t j, const gsl_vector_int * v);
/***********************************************************************/

/* inline functions if you are using GCC */

INLINE_DECL int   gsl_matrix_int_get(const gsl_matrix_int * m, const size_t i, const size_t j);
INLINE_DECL void    gsl_matrix_int_set(gsl_matrix_int * m, const size_t i, const size_t j, const int x);
INLINE_DECL int * gsl_matrix_int_ptr(gsl_matrix_int * m, const size_t i, const size_t j);
INLINE_DECL const int * gsl_matrix_int_const_ptr(const gsl_matrix_int * m, const size_t i, const size_t j);

#ifdef HAVE_INLINE
INLINE_FUN
int
gsl_matrix_int_get(const gsl_matrix_int * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_VAL("first index out of range", GSL_EINVAL, 0) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_VAL("second index out of range", GSL_EINVAL, 0) ;
        }
    }
#endif
  return m->data[i * m->tda + j] ;
}

INLINE_FUN
void
gsl_matrix_int_set(gsl_matrix_int * m, const size_t i, const size_t j, const int x)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_VOID("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_VOID("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  m->data[i * m->tda + j] = x ;
}

INLINE_FUN
int *
gsl_matrix_int_ptr(gsl_matrix_int * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  return (int *) (m->data + (i * m->tda + j)) ;
}

INLINE_FUN
const int *
gsl_matrix_int_const_ptr(const gsl_matrix_int * m, const size_t i, const size_t j)
{
#if GSL_RANGE_CHECK
  if (GSL_RANGE_COND(1))
    {
      if (i >= m->size1)
        {
          GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
        }
      else if (j >= m->size2)
        {
          GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
        }
    }
#endif
  return (const int *) (m->data + (i * m->tda + j)) ;
}

#endif

__END_DECLS

#endif /* __GSL_MATRIX_INT_H__ */


/* ieee-utils/fp-x86linux.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

#include <stdio.h>
#include <fpu_control.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_ieee_utils.h>

  /* Handle libc5, where _FPU_SETCW is not available, suggested by
     OKUJI Yoshinori <okuji@gnu.org> and Evgeny Stambulchik
     <fnevgeny@plasma-gate.weizmann.ac.il> */

#ifndef _FPU_SETCW
#include <i386/fpu_control.h>
#define _FPU_SETCW(cw) __setfpucw(cw)
#endif

int
gsl_ieee_set_mode (int precision, int rounding, int exception_mask)
{
  unsigned short mode = 0 ;

  switch (precision)
    {
    case GSL_IEEE_SINGLE_PRECISION:
      mode |= _FPU_SINGLE ;
      break ;
    case GSL_IEEE_DOUBLE_PRECISION:
      mode |= _FPU_DOUBLE ;
      break ;
    case GSL_IEEE_EXTENDED_PRECISION:
      mode |= _FPU_EXTENDED ;
      break ;
    default:
      mode |= _FPU_EXTENDED ;
    }

  switch (rounding)
    {
    case GSL_IEEE_ROUND_TO_NEAREST:
      mode |= _FPU_RC_NEAREST ;
      break ;
    case GSL_IEEE_ROUND_DOWN:
      mode |= _FPU_RC_DOWN ;
      break ;
    case GSL_IEEE_ROUND_UP:
      mode |= _FPU_RC_UP ;
      break ;
    case GSL_IEEE_ROUND_TO_ZERO:
      mode |= _FPU_RC_ZERO ;
      break ;
    default:
      mode |= _FPU_RC_NEAREST ;
    }

  if (exception_mask & GSL_IEEE_MASK_INVALID)
    mode |= _FPU_MASK_IM ;

  if (exception_mask & GSL_IEEE_MASK_DENORMALIZED)
    mode |= _FPU_MASK_DM ;

  if (exception_mask & GSL_IEEE_MASK_DIVISION_BY_ZERO)
    mode |= _FPU_MASK_ZM ;

  if (exception_mask & GSL_IEEE_MASK_OVERFLOW)
    mode |= _FPU_MASK_OM ;

  if (exception_mask & GSL_IEEE_MASK_UNDERFLOW)
    mode |= _FPU_MASK_UM ;

  if (exception_mask & GSL_IEEE_TRAP_INEXACT)
    {
      mode &= ~ _FPU_MASK_PM ;
    }
  else
    {
      mode |= _FPU_MASK_PM ;
    }

  _FPU_SETCW(mode) ;

  return GSL_SUCCESS ;
}


/**
 * original author: Jochen K"upper
 * author: Pierre Schnizer
 * created: Jan 2002
 * modified: May 2009
 * file: pygsl/src/statistics/longmodule.c
 * $Id: charmodule.c,v 1.9 2009/05/11 08:53:03 schnizer Exp $
 *
 *
 *"
 */

#include <Python.h>
#include <pygsl/error_helpers.h>
#include <pygsl/block_helpers.h>
#include <gsl/gsl_statistics.h>

/* include real functions for different data-types */

#define STATMOD_APPEND_PY_TYPE(X) X ## Int

#if (defined PyGSL_NUMARRAY) || (defined PyGSL_NUMERIC)
#define STATMOD_APPEND_PYC_TYPE(X) X ## CHAR
#else /* PyGSL_NUMARRAY */
#define STATMOD_APPEND_PYC_TYPE(X) X ## BYTE
#endif /* PyGSL_NUMARRAY */

#define STATMOD_FUNC_EXT(X, Y) X ## _char ## Y
#define STATMOD_PY_AS_C PyInt_AsLong
#define STATMOD_C_TYPE char
#include "functions.c"

PyGSL_STATISTICS_INIT(char, "char")

/*
 * Local Variables:
 * mode: c
 * c-file-style: "python"
 * End:
 */


#ifndef CPP_MICROBENCH_BENCH_OUTPUT_H
#define CPP_MICROBENCH_BENCH_OUTPUT_H

#include <string>
#include <chrono>
#include <fstream>
#include <iomanip>

#include <fmt/os.h>
#include <fmt/ostream.h>
#include <fmt/chrono.h>

#include <gsl/util>

#include "util/aligned_vector.hpp"
#include "util/alloc_array.hpp"
#include "util/trivial_aligned_array.hpp"
#include "util/plain_array.hpp"

template <typename array_type>
auto generate_array(std::size_t max) {
  using namespace std::chrono;
  auto t1 = high_resolution_clock::now();
  array_type v(max);
  auto t2 = high_resolution_clock::now();
  for (std::size_t i = 0; i < max; ++i) {
    v[i] = 1.0F / gsl::narrow_cast<float>(i+1);
  }
  auto t3 = high_resolution_clock::now();
  fmt::print("Create: {}\n", duration_cast<microseconds>(t2-t1));
  fmt::print("Fill: {}\n", duration_cast<microseconds>(t3-t2));
  return v;
}

template <typename vector_type>
auto generate_vector(std::size_t max) {
  using namespace std::chrono;
  auto t1 = high_resolution_clock::now();
  vector_type v;
  v.reserve(max);
  auto t2 = high_resolution_clock::now();
  for (std::size_t i = 0; i < max; ++i) {
    v.push_back(1.0F / gsl::narrow_cast<float>(i+1));
  }
  auto t3 = high_resolution_clock::now();
  fmt::print("Create: {}\n", duration_cast<microseconds>(t2-t1));
  fmt::print("Fill: {}\n", duration_cast<microseconds>(t3-t2));
  return v;
}

void fmt_output_vector(const auto & v) {
  auto file_name = std::string(static_cast<const char *>(__func__)) + ".txt";
  auto file = fmt::output_file(file_name);
  file.print("{}\n", v.size());

  for (const auto & x : v) {
    file.print("{:.18f}\n", x);
  }
}

void stream_output_vector(const auto & v) {
  auto file_name = std::string(static_cast<const char *>(__func__)) + ".txt";
  std::ofstream file{file_name};
  file << v.size() << '\n';

  for (const auto & x : v) {
    file << std::setprecision(18) << x << '\n';
  }
}

template <typename array_type>
auto bench_fmt_output_array(std::size_t n) {
  using namespace std::chrono;

  auto t1 = high_resolution_clock::now();
  auto v = generate_array<array_type>(n);
  auto t2 = high_resolution_clock::now();
  fmt_output_vector(v);
  auto t3 = high_resolution_clock::now();

  return std::tuple {__func__, t2 - t1, t3 - t2};
}

template <typename vector_type>
auto bench_fmt_output_vector(std::size_t n) {
  using namespace std::chrono;

  auto t1 = high_resolution_clock::now();
  auto v = generate_vector<vector_type>(n);
  auto t2 = high_resolution_clock::now();
  fmt_output_vector(v);
  auto t3 = high_resolution_clock::now();

  return std::tuple {__func__, t2 - t1, t3 - t2};
}

template <typename vector_type>
auto bench_stream_vector(std::size_t n) {
  using namespace std::chrono;

  auto t1 = high_resolution_clock::now();
  auto v = generate_vector<vector_type>(n);
  auto t2 = high_resolution_clock::now();
  stream_output_vector(v);
  auto t3 = high_resolution_clock::now();

  return std::tuple {__func__, t2 - t1, t3 - t2};
}

void print_bench(auto t) {
  using namespace std::chrono;
  auto d1 = duration_cast<microseconds>(std::get<1>(t));
  auto d2 = duration_cast<microseconds>(std::get<2>(t));
  fmt::print("\nVersion: {}\n", std::get<0>(t));
  fmt::print("Generation {} us\n", d1.count());
  fmt::print("Writing {} us\n", d2.count());
}

#endif//CPP_MICROBENCH_BENCH_OUTPUT_H


/* histogram/get2d.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram2d.h>

#include "find.c"

double
gsl_histogram2d_get (const gsl_histogram2d * h, const size_t i, const size_t j)
{
  const size_t nx = h->nx;
  const size_t ny = h->ny;

  if (i >= nx)
    {
      GSL_ERROR_VAL ("index i lies outside valid range of 0 .. nx - 1",
			GSL_EDOM, 0);
    }

  if (j >= ny)
    {
      GSL_ERROR_VAL ("index j lies outside valid range of 0 .. ny - 1",
			GSL_EDOM, 0);
    }

  return h->bin[i * ny + j];
}

int
gsl_histogram2d_get_xrange (const gsl_histogram2d * h, const size_t i,
			    double *xlower, double *xupper)
{
  const size_t nx = h->nx;

  if (i >= nx)
    {
      GSL_ERROR ("index i lies outside valid range of 0 .. nx - 1", GSL_EDOM);
    }

  *xlower = h->xrange[i];
  *xupper = h->xrange[i + 1];

  return GSL_SUCCESS;
}

int
gsl_histogram2d_get_yrange (const gsl_histogram2d * h, const size_t j,
			    double *ylower, double *yupper)
{
  const size_t ny = h->ny;

  if (j >= ny)
    {
      GSL_ERROR ("index j lies outside valid range of 0 .. ny - 1", GSL_EDOM);
    }

  *ylower = h->yrange[j];
  *yupper = h->yrange[j + 1];

  return GSL_SUCCESS;
}

int
gsl_histogram2d_find (const gsl_histogram2d * h,
		      const double x, const double y,
		      size_t * i, size_t * j)
{
  int status = find (h->nx, h->xrange, x, i);

  if (status)
    {
      GSL_ERROR ("x not found in range of h", GSL_EDOM);
    }

  status = find (h->ny, h->yrange, y, j);

  if (status)
    {
      GSL_ERROR ("y not found in range of h", GSL_EDOM);
    }

  return GSL_SUCCESS;
}