/* interpolation/spline2d.c
 *
 * Copyright 2012 David Zaslavsky
 *
 * 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 <string.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

gsl_spline2d *
gsl_spline2d_alloc(const gsl_interp2d_type * T, size_t xsize, size_t ysize)
{
  double * array_mem;
  gsl_spline2d * interp;

  if (xsize < T->min_size || ysize < T->min_size)
    {
      GSL_ERROR_NULL("insufficient number of points for interpolation type", GSL_EINVAL);
    }

  interp = calloc(1, sizeof(gsl_spline2d));
  if (interp == NULL)
    {
      GSL_ERROR_NULL("failed to allocate space for gsl_spline2d struct", GSL_ENOMEM);
    }

  interp->interp_object.type = T;
  interp->interp_object.xsize = xsize;
  interp->interp_object.ysize = ysize;
  if (interp->interp_object.type->alloc == NULL)
    {
      interp->interp_object.state = NULL;
    }
  else
    {
      interp->interp_object.state = interp->interp_object.type->alloc(xsize, ysize);
      if (interp->interp_object.state == NULL)
        {
          gsl_spline2d_free(interp);
          GSL_ERROR_NULL("failed to allocate space for gsl_spline2d state", GSL_ENOMEM);
        }
    }

  /*
   * Use one contiguous block of memory for all three data arrays.
   * That way the code fails immediately if there isn't sufficient space for everything,
   * rather than allocating one or two and then having to free them.
   */
  array_mem = (double *)calloc(xsize + ysize + xsize * ysize, sizeof(double));
  if (array_mem == NULL)
    {
      gsl_spline2d_free(interp);
      GSL_ERROR_NULL("failed to allocate space for data arrays", GSL_ENOMEM);
    }

  interp->xarr = array_mem;
  interp->yarr = array_mem + xsize;
  interp->zarr = array_mem + xsize + ysize;

  return interp;
} /* gsl_spline2d_alloc() */

int
gsl_spline2d_init(gsl_spline2d * interp, const double xarr[],
                  const double yarr[], const double zarr[],
                  size_t xsize, size_t ysize)
{
  int status = gsl_interp2d_init(&(interp->interp_object), xarr, yarr, zarr, xsize, ysize);

  memcpy(interp->xarr, xarr, xsize * sizeof(double));
  memcpy(interp->yarr, yarr, ysize * sizeof(double));
  memcpy(interp->zarr, zarr, xsize * ysize * sizeof(double));

  return status;
} /* gsl_spline2d_init() */

void
gsl_spline2d_free(gsl_spline2d * interp)
{
  RETURN_IF_NULL(interp);

  if (interp->interp_object.type->free)
    interp->interp_object.type->free(interp->interp_object.state);

  /*
   * interp->xarr points to the beginning of one contiguous block of memory
   * that holds interp->xarr, interp->yarr, and interp->zarr. So it all gets
   * freed with one call. cf. gsl_spline2d_alloc() implementation
   */
  if (interp->xarr)
    free(interp->xarr);

  free(interp);
} /* gsl_spline2d_free() */

double
gsl_spline2d_eval(const gsl_spline2d * interp, const double x,
                  const double y, gsl_interp_accel * xa, gsl_interp_accel * ya)
{
  return gsl_interp2d_eval(&(interp->interp_object), interp->xarr, interp->yarr,
                           interp->zarr, x, y, xa, ya);
}

int
gsl_spline2d_eval_e(const gsl_spline2d * interp, const double x,
                    const double y, gsl_interp_accel * xa, gsl_interp_accel * ya,
                    double * z)
{
  return gsl_interp2d_eval_e(&(interp->interp_object), interp->xarr, interp->yarr,
                             interp->zarr, x, y, xa, ya, z);
}

double
gsl_spline2d_eval_extrap(const gsl_spline2d * interp, const double x,
                         const double y, gsl_interp_accel * xa, gsl_interp_accel * ya)
{
  return gsl_interp2d_eval_extrap(&(interp->interp_object), interp->xarr, interp->yarr,
                                  interp->zarr, x, y, xa, ya);
}

int
gsl_spline2d_eval_extrap_e(const gsl_spline2d * interp, const double x,
                           const double y, gsl_interp_accel * xa, gsl_interp_accel * ya,
                           double * z)
{
  return gsl_interp2d_eval_extrap_e(&(interp->interp_object), interp->xarr, interp->yarr,
                                    interp->zarr, x, y, xa, ya, z);
}

double
gsl_spline2d_eval_deriv_x(const gsl_spline2d * interp, const double x,
                          const double y, gsl_interp_accel * xa, gsl_interp_accel * ya)
{
  return gsl_interp2d_eval_deriv_x(&(interp->interp_object), interp->xarr, interp->yarr,
                                   interp->zarr, x, y, xa, ya);
}

int
gsl_spline2d_eval_deriv_x_e(const gsl_spline2d * interp, const double x,
                            const double y, gsl_interp_accel * xa, gsl_interp_accel * ya,
                            double * z)
{
  return gsl_interp2d_eval_deriv_x_e(&(interp->interp_object), interp->xarr, interp->yarr,
                                     interp->zarr, x, y, xa, ya, z);
}

double
gsl_spline2d_eval_deriv_y(const gsl_spline2d * interp, const double x,
                          const double y, gsl_interp_accel * xa, gsl_interp_accel * ya)
{
  return gsl_interp2d_eval_deriv_y(&(interp->interp_object), interp->xarr, interp->yarr,
                                   interp->zarr, x, y, xa, ya);
}

int
gsl_spline2d_eval_deriv_y_e(const gsl_spline2d * interp, const double x,
                            const double y, gsl_interp_accel * xa, gsl_interp_accel * ya,
                            double * z)
{
  return gsl_interp2d_eval_deriv_y_e(&(interp->interp_object), interp->xarr, interp->yarr,
                                     interp->zarr, x, y, xa, ya, z);
}

double
gsl_spline2d_eval_deriv_xx(const gsl_spline2d * interp, const double x,
                           const double y, gsl_interp_accel * xa, gsl_interp_accel * ya)
{
  return gsl_interp2d_eval_deriv_xx(&(interp->interp_object), interp->xarr, interp->yarr,
                                    interp->zarr, x, y, xa, ya);
}

int
gsl_spline2d_eval_deriv_xx_e(const gsl_spline2d * interp, const double x,
                             const double y, gsl_interp_accel * xa, gsl_interp_accel * ya,
                             double * z)
{
  return gsl_interp2d_eval_deriv_xx_e(&(interp->interp_object), interp->xarr, interp->yarr,
                                      interp->zarr, x, y, xa, ya, z);
}

double
gsl_spline2d_eval_deriv_yy(const gsl_spline2d * interp, const double x,
                           const double y, gsl_interp_accel * xa, gsl_interp_accel * ya)
{
  return gsl_interp2d_eval_deriv_yy(&(interp->interp_object), interp->xarr, interp->yarr,
                                    interp->zarr, x, y, xa, ya);
}

int
gsl_spline2d_eval_deriv_yy_e(const gsl_spline2d * interp, const double x,
                             const double y, gsl_interp_accel * xa, gsl_interp_accel * ya,
                             double * z)
{
  return gsl_interp2d_eval_deriv_yy_e(&(interp->interp_object), interp->xarr, interp->yarr,
                                      interp->zarr, x, y, xa, ya, z);
}

double
gsl_spline2d_eval_deriv_xy(const gsl_spline2d * interp, const double x,
                           const double y, gsl_interp_accel * xa, gsl_interp_accel * ya)
{
  return gsl_interp2d_eval_deriv_xy(&(interp->interp_object), interp->xarr, interp->yarr,
                                    interp->zarr, x, y, xa, ya);
}

int
gsl_spline2d_eval_deriv_xy_e(const gsl_spline2d * interp, const double x,
                             const double y, gsl_interp_accel * xa, gsl_interp_accel * ya,
                             double * z)
{
  return gsl_interp2d_eval_deriv_xy_e(&(interp->interp_object), interp->xarr, interp->yarr,
                                      interp->zarr, x, y, xa, ya, z);
}

size_t
gsl_spline2d_min_size(const gsl_spline2d * interp)
{
  return gsl_interp2d_min_size(&(interp->interp_object));
}

const char *
gsl_spline2d_name(const gsl_spline2d * interp)
{
  return gsl_interp2d_name(&(interp->interp_object));
}

int
gsl_spline2d_set(const gsl_spline2d * interp, double zarr[],
                 const size_t i, const size_t j, const double z)
{
  return gsl_interp2d_set(&(interp->interp_object), zarr, i, j, z);
} /* gsl_spline2d_set() */

double
gsl_spline2d_get(const gsl_spline2d * interp, const double zarr[],
                 const size_t i, const size_t j)
{
  return gsl_interp2d_get(&(interp->interp_object), zarr, i, j);
} /* gsl_spline2d_get() */


#include "readArray_MPI.h"
#include "../include/paralleltt.h"

#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <lapacke.h>
#include <time.h>
#include <math.h>

void* p_hilbert_init(int d, int* n){
    p_hilbert* parameters = (p_hilbert*) malloc(sizeof(p_hilbert));
    parameters->d = d;
    parameters->n = (int*) malloc(d*sizeof(int));
    for (int ii = 0; ii < d; ++ii){
        parameters->n[ii] = n[ii];
    }
    parameters->n_stream = (int*) calloc(d, sizeof(int));
    parameters->n_prod = (long*) malloc(d * sizeof(long));
    return (void*) parameters;
}

void p_hilbert_free(void* parameters){
    p_hilbert* p_casted = (p_hilbert*) parameters;
    free(p_casted->n); p_casted->n = NULL;
    free(p_casted->n_stream); p_casted->n_stream = NULL;
    free(p_casted->n_prod); p_casted->n_prod = NULL;
    free(p_casted);
}

void f_hilbert(double* restrict X, int* ind1, int* ind2, const void* parameters)
{
    p_hilbert* p_casted = (p_hilbert*) parameters;
    int d = p_casted->d;
    int* n = p_casted->n;
    int* n_stream = p_casted->n_stream;
    long* n_prod = p_casted->n_prod;

    long N_stream = 1;
    for (int ii = 0; ii < d; ++ii){
        n_stream[ii] = ind2[ii] - ind1[ii];
        N_stream = N_stream*n_stream[ii];
    }

    n_prod[0] = 1;
    for (int ii = 1; ii < d; ++ii){
        n_prod[ii] = (long) n_prod[ii-1] * n_stream[ii-1];
    }

    int bias = 1;
    for (int jj = 0; jj < d; ++jj){
        bias = bias + ind1[jj];
    }

    for (long ii = 0; ii < N_stream; ++ii){
        int xinv = bias;
		long tmp = ii;
		for (int jj = d-1; jj >= 0; jj--){
			xinv += tmp/n_prod[jj];
			tmp = tmp % n_prod[jj];
		}

        X[ii] = (double) 1/xinv;
    }
}

void* p_gaussian_bumps_init(int d, int* n, int M, double gamma, double* region, double* centers)
{
    p_gaussian_bumps* parameters = (p_gaussian_bumps*) malloc(sizeof(p_gaussian_bumps));
    parameters->d = d;
    parameters->n = (int*) calloc(d, sizeof(int));
    for (int ii = 0; ii < d; ++ii){
        parameters->n[ii] = n[ii];
    }

    parameters->M = M;
    parameters->gamma = gamma;

    parameters->region = (double*) calloc(2*d, sizeof(double));
    for (int ii = 0; ii < 2*d; ++ii){
        parameters->region[ii] = region[ii];
    }
    parameters->centers = (double*) calloc(d*M, sizeof(double));
    for (int ii = 0; ii < d*M; ++ii){
        parameters->centers[ii] = centers[ii];
    }

//    parameters->ten_ind_ii = (int*) calloc(d, sizeof(int));
    parameters->x_ii = (double*) calloc(d, sizeof(double));
    parameters->n_stream = (int*) calloc(d, sizeof(double));
    parameters->n_prod = (long*) calloc(d, sizeof(long));

    return (void*) parameters;
}

void* unit_random_p_gaussian_bumps_init(int d, int* n, int M, double gamma, int seed)
{
    p_gaussian_bumps* parameters = (p_gaussian_bumps*) malloc(sizeof(p_gaussian_bumps));
    parameters->d = d;
    parameters->n = (int*) calloc(d, sizeof(int));
    for (int ii = 0; ii < d; ++ii){
        parameters->n[ii] = n[ii];
    }

    parameters->M = M;
    parameters->gamma = gamma;

    parameters->region = (double*) calloc(2*d, sizeof(double));
    for (int ii = 0; ii < d; ++ii){
        parameters->region[2*ii] = -1.0;
        parameters->region[2*ii+1] = 1.0;
    }

    parameters->centers = (double*) calloc(d*M, sizeof(double));
    srand(seed);
    int r1 = rand()%4096, r2 = rand()%4096, r3 = rand()%4096, r4 = rand()%4096;
    int iseed[4] = {r1, r2, r3, r4+(r4%2 == 0?1:0)};
    LAPACKE_dlarnv(2, iseed, d*M, parameters->centers);

    parameters->x_ii = (double*) calloc(d, sizeof(double));
    parameters->n_stream = (int*) calloc(d, sizeof(double));
    parameters->n_prod = (long*) calloc(d, sizeof(long));

    return (void*) parameters;
}

void p_gaussian_bumps_free(void* parameters)
{
    p_gaussian_bumps* p_casted = (p_gaussian_bumps*) parameters;

    free(p_casted->n);          p_casted->n = NULL;
    free(p_casted->region);     p_casted->region = NULL;
    free(p_casted->centers);    p_casted->centers = NULL;
    free(p_casted->x_ii);       p_casted->x_ii = NULL;
    free(p_casted->n_stream);   p_casted->n_stream = NULL;
    free(p_casted->n_prod);     p_casted->n_prod = NULL;
    free(p_casted);
}

void f_gaussian_bumps(double* restrict X, int* ind1, int* ind2, const void* parameters)
{
    p_gaussian_bumps* p_casted = (p_gaussian_bumps*) parameters;
    int d = p_casted->d;
    int* n = p_casted->n;
    int M = p_casted->M;
    double gamma = p_casted->gamma;
    double* region = p_casted->region;
    double* centers = p_casted->centers;
    double* x_ii = p_casted->x_ii;
    int* n_stream = p_casted->n_stream;
    long* n_prod = p_casted->n_prod;

    long N_stream = 1;
    for (int ii = 0; ii < d; ++ii){
        n_stream[ii] = ind2[ii] - ind1[ii];
        N_stream = N_stream*n_stream[ii];
    }

    n_prod[0] = 1;
    for (int ii = 1; ii < d; ++ii){
        n_prod[ii] = (long) n_prod[ii-1] * n_stream[ii-1];
    }

    for (long ii = 0; ii < N_stream; ++ii){
        long tmp = ii;
        for (int jj = d-1; jj >= 0; jj--){
            int ind_jj = tmp/n_prod[jj];
            x_ii[jj] = region[2*jj] + (ind_jj + ind1[jj]) * (region[2*jj + 1] - region[2*jj]) / (n[jj] - 1);
			tmp = tmp % n_prod[jj];
		}

        X[ii] = 0;
        for (int kk = 0; kk < M; ++kk){
            double exponent = 0;
            for (int jj = 0; jj < d; ++jj){
                exponent += (x_ii[jj] - centers[kk*d + jj]) * (x_ii[jj] - centers[kk*d + jj]);
            }
            X[ii] += exp(-gamma * exponent);
        }
    }
}

void* p_arithmetic_init(int d, int* n)
{
    p_arithmetic* parameters = (p_arithmetic*) malloc(sizeof(p_arithmetic));
    parameters->d = d;
    parameters->n = (int*) malloc(d*sizeof(int));
    for (int ii = 0; ii < d; ++ii){
        parameters->n[ii] = n[ii];
    }
    return (void*) parameters;
}

void p_arithmetic_free(void* parameters)
{
    p_arithmetic* p_casted = (p_arithmetic*) parameters;
    free(p_casted->n); p_casted->n = NULL;
    free(p_casted);
}

void f_arithmetic(double* restrict X, int* ind1, int* ind2, const void* parameters)
{
    p_arithmetic* p_casted = (p_arithmetic*) parameters;
    int d = p_casted->d;
    int* n = p_casted->n;

    int* n_stream = (int*) calloc(d, sizeof(int));
    long N_stream = 1;
    for (int ii = 0; ii < d; ++ii){
        n_stream[ii] = ind2[ii] - ind1[ii];
        N_stream = N_stream*n_stream[ii];
    }

    int* ind_ii = (int*) calloc(d, sizeof(int));
    for (int ii = 0; ii < N_stream; ++ii){
        to_tensor_ind(ind_ii, ii, n_stream, d);
        for (int jj = 0; jj < d; ++jj){
            ind_ii[jj] = ind_ii[jj] + ind1[jj];
        }
        X[ii] = 1 + to_vec_ind(ind_ii, n, d);
//        X[ii] = X[ii] + (0.000000001 / X[ii]);
    }

    free(n_stream);
    free(ind_ii);
}

void* p_tt_init(tensor_train* tt)
{
    p_tt* parameters = (p_tt*) malloc(sizeof(p_tt));
    parameters->tt = tt;
    return (void*) parameters;
}

// Doesn't actually free the tensor train. You gotta do that yourself
void p_tt_free(void* parameters)
{
    p_tt* p_casted = (p_tt*) parameters;
    p_casted->tt = NULL;
    free(p_casted);
}

void f_tt(double* restrict X, int* ind1, int* ind2, const void* parameters)
{
    p_tt* p_casted = (p_tt*) parameters;
    tensor_train* tt = p_casted->tt;
    int d = tt->d;
    int* n = tt->n;
    int* r = tt->r;

    matrix_tt** train_submats = (matrix_tt**) calloc(d, sizeof(matrix_tt*));
    for (int ii = 0; ii < d; ++ii){
        matrix_tt* train_mat_ii = matrix_tt_wrap(r[ii]*n[ii], r[ii+1], tt->trains[ii]);
        train_submats[ii] = submatrix(train_mat_ii, ind1[ii]*r[ii], ind2[ii]*r[ii], 0, r[ii+1]);
        free(train_mat_ii);
    }

    matrix_tt* mult = submatrix_copy(train_submats[d-1]);
    matrix_tt_reshape(r[d-1], ind2[d-1] - ind1[d-1], mult);

    for (int jj = d-2; jj >= 1; --jj){
        matrix_tt* new_mult = matrix_tt_init(train_submats[jj]->m, mult->n);
        matrix_tt_dgemm(train_submats[jj], mult, new_mult, 1.0, 0.0);
        matrix_tt_reshape(r[jj], (new_mult->n) * (new_mult->m) / r[jj], new_mult);
        matrix_tt_free(mult); mult = new_mult;
    }

    long X_m = 1;
    for (int ii = 1; ii < d; ++ii){
        X_m = X_m*(ind2[ii] - ind1[ii]);
    }

    matrix_tt* X_mat = matrix_tt_wrap(ind2[0] - ind1[0], X_m, X);
    matrix_tt_dgemm(train_submats[0], mult, X_mat, 1.0, 0.0);

    for (int ii = 0; ii < d; ++ii){
        free(train_submats[ii]); train_submats[ii] = NULL;
    }

    free(train_submats); train_submats = NULL;
    matrix_tt_free(mult);   mult = NULL;
    free(X_mat);         X_mat = NULL;
}

double tt_error(tensor_train* tt, MPI_tensor* ten)
{
    MPI_Comm comm = ten->comm;
    tt_broadcast(comm, tt);

    void* parameters_tt = p_tt_init(tt);
    MPI_tensor* ten_tt = MPI_tensor_init(ten->d, ten->n, ten->nps, ten->comm, &f_tt, parameters_tt);
    int rank = ten_tt->rank;
    int* schedule_rank = ten_tt->schedule[rank];

    double true_norm_squared = 0;
    double diff_norm_squared = 0;

    flattening_info* fi = flattening_info_init(ten, 0, 1, 0);
    for (int ii = 0; ii < ten->n_schedule; ++ii){
        int block = schedule_rank[ii];

        if (block != -1){
            stream(ten, block);
            stream(ten_tt, block);

            flattening_info_update(fi, ten, block);
            long N = fi->t_N;

            for (int jj = 0; jj < N; ++jj){
                ten_tt->X[jj] = ten_tt->X[jj] - ten->X[jj];
            }

            matrix_tt* mat_true = matrix_tt_wrap(N, 1, ten->X);
            double tmp = frobenius_norm(mat_true);
            true_norm_squared = true_norm_squared + tmp*tmp;

            matrix_tt* mat_diff = matrix_tt_wrap(N, 1, ten_tt->X);
            tmp = frobenius_norm(mat_diff);
            diff_norm_squared = diff_norm_squared + tmp*tmp;
//            printf("r%d ii%d true_norm_squared = %e, diff_norm_squared = %e\n", rank, ii, true_norm_squared, diff_norm_squared);

            free(mat_true); mat_true = NULL;
            free(mat_diff); mat_diff = NULL;
        }
    }
    int head = 0;

    double true_norm_reduced;
    double diff_norm_reduced;
    MPI_Reduce(&true_norm_squared, &true_norm_reduced, 1, MPI_DOUBLE, MPI_SUM, head, comm);
    true_norm_reduced = sqrt(true_norm_reduced);

    MPI_Reduce(&diff_norm_squared, &diff_norm_reduced, 1, MPI_DOUBLE, MPI_SUM, head, comm);
    diff_norm_reduced = sqrt(diff_norm_reduced);

    double rel_err = diff_norm_reduced/true_norm_reduced;

    p_tt_free(parameters_tt); parameters_tt = NULL;
    flattening_info_free(fi); fi = NULL;
    MPI_tensor_free(ten_tt); ten_tt = NULL;

    return rel_err;
}


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <stdlib.h>
#include <fftw.h>

#include "mex.h"

/**************************************************************************/

/* MEX programs need to use special memory allocation routines,
   so we use the hooks provided by FFTW to ensure that MEX
   allocation is used: */

void *fftw_mex_malloc_hook(size_t n)
{
     void *buf;

     buf = mxMalloc(n);

     /* Call this routine so that we can retain allocations and
	data between calls to the FFTW MEX: */
     mexMakeMemoryPersistent(buf);

     return buf;
}

void fftw_mex_free_hook(void *buf)
{
     mxFree(buf);
}

void install_fftw_hooks(void)
{
     fftw_malloc_hook = fftw_mex_malloc_hook;
     fftw_free_hook = fftw_mex_free_hook;
}

/**************************************************************************/

/* We retain various information between calls to the FFTW MEX in
   order to maximize performance.  (Reusing plans, data, and
   allocated blocks where possible.)  This information is referenced
   by the following variables, which are initialized in the function
   initialize_fftw_mex_data. */

#define MAX_RANK 10

int first_call = 1; /* 1 if this is the first call to the FFTW MEX,
		       and nothing has been initialized yet.  0 otherwise. */

/* Keep track of the array dimensions that stored data (below) is for.
   When these dimensions changed, we have to recompute the plans,
   work arrays, etc. */
int  cur_rank = 0,         /* rank of the array */
     cur_dims[MAX_RANK],   /* dimensions */
     cur_N;                /* product of the dimensions */

/* Work arrays.  MATLAB stores complex numbers as separate real/imag.
   arrays, so we have to translate into our format before the FFT.
   In case allocation is slow, we retain these work arrays between
   calls so that they can be reused. */
fftw_complex *input_work = 0, *output_work = 0;

/* The number of floating point operations required for the FFT.
   (Starting with FFTW 1.3, an exact count is computed by the planner.)
   This is used to update MATLAB's flops count. */
int fftw_mex_flops = 0, ifftw_mex_flops = 0;

/* Plans.  These are computed once and then reused as long as the
   dimensions of the array don't changed.  At any point in time,
   at most two plans are cached: a forward and backwards plan,
   either for one- or multi-dimensional transforms. */
fftw_plan p = 0, ip = 0;
fftwnd_plan pnd = 0, ipnd = 0;

/**************************************************************************/

int compute_fftw_mex_flops(fftw_direction dir)
{
#ifdef FFTW_HAS_COUNT_PLAN_OPS /* this feature will be in FFTW 1.3 */
     fftw_op_count ops;

     if (dir == FFTW_FORWARD) {
	  if (cur_rank == 1)
	       fftw_count_plan_ops(p,&ops);
	  else
	       fftwnd_count_plan_ops(pnd,&ops);
     }
     else {
	  if (cur_rank == 1)
	       fftw_count_plan_ops(ip,&ops);
	  else
	       fftwnd_count_plan_ops(ipnd,&ops);
     }

     return (ops.fp_additions + ops.fp_multiplications);
#else
     return 0;
#endif
}

/**************************************************************************/

/* The following functions destroy and/or initialize the data that
   FFTW-MEX caches between calls. */

void destroy_fftw_mex_data(void) {
     if (output_work != input_work)
	  fftw_free(output_work);
     if (input_work)
	  fftw_free(input_work);
     if (p)
	  fftw_destroy_plan(p);
     if (pnd)
	  fftwnd_destroy_plan(pnd);
     if (ip)
	  fftw_destroy_plan(ip);
     if (ipnd)
	  fftwnd_destroy_plan(ipnd);

     cur_rank = 0;
     input_work = output_work = 0;
     ip = p = 0;
     ipnd = pnd = 0;
}

/* This function is called when MATLAB exits or the MEX file is
   cleared, in which case we want to dispose of all data and
   free any allocated blocks. */

void fftw_mex_exit_function(void)
{
     if (!first_call) {
	  destroy_fftw_mex_data();
	  fftw_forget_wisdom();
	  first_call = 1;
     }
}

#define MAGIC(x) #x
#define STRINGIZE(x) MAGIC(x)

/* Initialize the cached data each time the MEX file is called.  First,
   we check if we have previously computed plans and data for these
   array dimensions.  Only if the dimensions have changed since the
   last call must we recompute the plans, etc. */

void initialize_fftw_mex_data(int rank, const int *dims, fftw_direction dir)
{
     int new_plan = 0;

     if (first_call) {
	  /* The following things need only be done once: */
	  install_fftw_hooks();
	  mexAtExit(fftw_mex_exit_function);
	  first_call = 0;
     }

     if (rank == 1) {
	  if (cur_rank != 1 || cur_dims[0] != dims[0]) {
	       destroy_fftw_mex_data();

	       cur_rank = 1;
	       cur_dims[0] = cur_N = dims[0];

	       input_work = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * cur_N);
	       output_work = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * cur_N);

	       new_plan = 1;
	  }
	  else if (dir == FFTW_FORWARD && !p ||
		   dir == FFTW_BACKWARD && !ip)
	       new_plan = 1;

	  if (new_plan) {
	       if (dir == FFTW_FORWARD) {
		    p = fftw_create_plan(cur_N,dir,
					 FFTW_MEASURE | FFTW_USE_WISDOM);

		    fftw_mex_flops = compute_fftw_mex_flops(dir);
	       }
	       else {
		    ip = fftw_create_plan(cur_N,dir,
					  FFTW_MEASURE | FFTW_USE_WISDOM);

		    ifftw_mex_flops = compute_fftw_mex_flops(dir);
	       }
	  }
     }
     else {
	  int same_dims = 1, dim;

	  if (cur_rank == rank)
	       for (dim = 0; dim < rank && same_dims; ++dim)
		    same_dims = (cur_dims[dim] == dims[rank-1-dim]);
	  else
	       same_dims = 0;

	  if (!same_dims) {
	       if (rank > MAX_RANK)
		    mexErrMsgTxt("Sorry, dimensionality > " STRINGIZE(MAX_RANK)
				 " is not supported.");

	       destroy_fftw_mex_data();

	       cur_rank = rank;

	       cur_N = 1;
	       for (dim = 0; dim < rank; ++dim)
		    cur_N *= (cur_dims[dim] = dims[rank-1-dim]);

	       input_work = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * cur_N);
	       output_work = input_work;

	       new_plan = 1;
	  }
          else if (dir == FFTW_FORWARD && !pnd ||
                   dir == FFTW_BACKWARD && !ipnd)
               new_plan = 1;

	  if (new_plan) {
	       if (dir == FFTW_FORWARD) {
		    pnd = fftwnd_create_plan(rank,cur_dims,dir,
					     FFTW_IN_PLACE |
					     FFTW_MEASURE | FFTW_USE_WISDOM);

		    fftw_mex_flops = compute_fftw_mex_flops(dir);
	       }
	       else {
		    ipnd = fftwnd_create_plan(rank,cur_dims,dir,
					      FFTW_IN_PLACE |
					      FFTW_MEASURE | FFTW_USE_WISDOM);

		    ifftw_mex_flops = compute_fftw_mex_flops(dir);
	       }
	  }

     }
}

/**************************************************************************/

/* MATLAB stores complex numbers as separate arrays for real and
   imaginary parts.  The following functions take the data in
   this format and pack it into a fftw_complex work array, or
   unpack it, respectively. The globals input_work and output_work
   are used as the arrays to pack to/unpack from.*/

void pack_input_work(double *input_re, double *input_im)
{
     int i;

     if (input_im)
	  for (i = 0; i < cur_N; ++i) {
	       c_re(input_work[i]) = input_re[i];
	       c_im(input_work[i]) = input_im[i];
	  }
     else
	  for (i = 0; i < cur_N; ++i) {
	       c_re(input_work[i]) = input_re[i];
	       c_im(input_work[i]) = 0.0;
	  }
}

void unpack_output_work(double *output_re, double *output_im)
{
     int i;

     for (i = 0; i < cur_N; ++i) {
	  output_re[i] = c_re(output_work[i]);
	  output_im[i] = c_im(output_work[i]);
     }
}

/**************************************************************************/

/* The following function is called by MATLAB when the FFTW
   MEX is invoked from within the program.

   The rhs parameters are the list of arrays on the right-hand-side
   (rhs) of the MATLAB command--the arguments to FFTW.  The lhs
   parameters are the list of arrays on the left-hand-side (lhs) of
   the MATLAB command--these are what the output(s) of FFTW are
   assigned to.

   The syntax for the FFTW call in MATLAB is fftw(array,sign),
   as described in fftw.m  */

void mexFunction(int nlhs, mxArray *plhs[],
		 int nrhs, const mxArray *prhs[])
{
     int rank;
     const int *dims;
     int m, n;  /* Array is m x n, C-ordered */
     fftw_direction dir;

     if (nrhs != 2)
	  mexErrMsgTxt("Two input arguments are expected.");

     if (!mxIsDouble(prhs[0]))
	  mexErrMsgTxt("First input must be a double precision matrix.");
     if (mxIsSparse(prhs[0]))
	  mexErrMsgTxt("Sorry, sparse matrices are not currently supported.");

     if (mxGetM(prhs[1]) * mxGetN(prhs[1]) != 1)
	  mexErrMsgTxt("Second input must be a scalar (+/- 1).");

     if (mxGetScalar(prhs[1]) > 0.0)
	  dir = FFTW_BACKWARD;
     else
	  dir = FFTW_FORWARD;

     if ((rank = mxGetNumberOfDimensions(prhs[0])) == 2) {
	  int dims2[2];
	  m = mxGetM(prhs[0]);
	  n = mxGetN(prhs[0]);
	  if (m == 1 || n == 1) {
	       dims2[0] = m * n;
	       initialize_fftw_mex_data(1,dims2,dir);
	  }
	  else {
	       dims2[0] = m;
	       dims2[1] = n;
	       initialize_fftw_mex_data(2,dims2,dir);
	  }
     }
     else
	  initialize_fftw_mex_data(rank,dims = mxGetDimensions(prhs[0]),dir);

     pack_input_work(mxGetPr(prhs[0]),mxGetPi(prhs[0]));

     if (dir == FFTW_FORWARD) {
	  if (cur_rank == 1)
	       fftw(p,1, input_work,1,0, output_work,1,0);
	  else
	       fftwnd(pnd,1, input_work,1,0, 0,0,0);

	  mexAddFlops(fftw_mex_flops);
     }
     else {
	  if (cur_rank == 1)
	       fftw(ip,1, input_work,1,0, output_work,1,0);
	  else
	       fftwnd(ipnd,1, input_work,1,0, 0,0,0);

	  mexAddFlops(ifftw_mex_flops);
     }

     /* Create a matrix for the return argument. */
     if (cur_rank <= 2)
	  plhs[0] = mxCreateDoubleMatrix(m, n, mxCOMPLEX);
     else
	  plhs[0] = mxCreateNumericArray(rank,dims,
					 mxDOUBLE_CLASS,mxCOMPLEX);

     unpack_output_work(mxGetPr(plhs[0]),mxGetPi(plhs[0]));
}


/* 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);
    }
}

//


/* Starting from version 7.8, MATLAB BLAS expects ptrdiff_t arguments for integers */
#if MATLAB_VERSION >= 0x0708
#include <stddef.h>
#include <stdlib.h>
#endif
#include <string.h>

/* Define MX_HAS_INTERLEAVED_COMPLEX for version <9.4 */
#ifndef MX_HAS_INTERLEAVED_COMPLEX
#define MX_HAS_INTERLEAVED_COMPLEX 0
#endif

/* Starting from version 7.6, MATLAB BLAS is seperated */
#if MATLAB_VERSION >= 0x0705
#include <blas.h>
#endif
#include <lapack.h>
#include "f2c.h"

// Conversion of optimal problems with coupling weighting terms to standard problems
#define sb02mt FORTRAN_WRAPPER(sb02mt)
extern void sb02mt(
        const char *jobg,
        const char *jobl,
        const char *fact,
        const char *uplo,
        const ptrdiff_t *n,
        const ptrdiff_t *m,
        double *a,
        const ptrdiff_t *lda,
        double *b,
        const ptrdiff_t *ldb,
        double *q,
        const ptrdiff_t *ldq,
        double *r,
        const ptrdiff_t *ldr,
        double *l,
        const ptrdiff_t *ldl,
        ptrdiff_t *ipiv,
        ptrdiff_t *oufact,
        double *g,
        const ptrdiff_t *ldg,
        ptrdiff_t *iwork,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Conversion of optimal problems with coupling weighting terms to standard problems (more flexibility)
#define sb02mx FORTRAN_WRAPPER(sb02mx)
extern void sb02mx(
        const char *jobg,
        const char *jobl,
        const char *fact,
        const char *uplo,
        const char *trans,
        const char *flag,
        const char *def,
        const ptrdiff_t *n,
        const ptrdiff_t *m,
        double *a,
        const ptrdiff_t *lda,
        double *b,
        const ptrdiff_t *ldb,
        double *q,
        const ptrdiff_t *ldq,
        double *r,
        const ptrdiff_t *ldr,
        double *l,
        const ptrdiff_t *ldl,
        ptrdiff_t *ipiv,
        ptrdiff_t *oufact,
        double *g,
        const ptrdiff_t *ldg,
        ptrdiff_t *iwork,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Constructing the 2n-by-2n Hamiltonian or symplectic matrix for linear-quadratic optimization problems
#define sb02mu FORTRAN_WRAPPER(sb02mu)
extern void sb02mu(
        const char *dico,
        const char *hinv,
        const char *uplo,
        const ptrdiff_t *n,
        double *a,
        const ptrdiff_t *lda,
        const double *g,
        const ptrdiff_t *ldg,
        const double *q,
        const ptrdiff_t *ldq,
        double *s,
        const ptrdiff_t *lds,
        ptrdiff_t *iwork,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Constructing the 2n-by-2n Hamiltonian or symplectic matrix for linear-quadratic optimization problems (improved)
#define sb02ru FORTRAN_WRAPPER(sb02ru)
extern void sb02ru(
        const char *dico,
        const char *hinv,
        const char *trana,
        const char *uplo,
        const ptrdiff_t *n,
        const double *a,
        const ptrdiff_t *lda,
        double *g,
        const ptrdiff_t *ldg,
        double *q,
        const ptrdiff_t *ldq,
        double *s,
        const ptrdiff_t *lds,
        ptrdiff_t *iwork,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Constructing the extended Hamiltonian or symplectic matrix pairs for linear-quadratic optimization problems, and compressing them to 2N-by-2N matrices
#define sb02oy FORTRAN_WRAPPER(sb02oy)
extern void sb02oy(
        const char *type,
        const char *dico,
        const char *jobb,
        const char *fact,
        const char *uplo,
        const char *jobl,
        const char *jobe,
        const ptrdiff_t *n,
        const ptrdiff_t *m,
        const ptrdiff_t *p,
        const double *a,
        const ptrdiff_t *lda,
        const double *b,
        const ptrdiff_t *ldb,
        const double *q,
        const ptrdiff_t *ldq,
        const double *r,
        const ptrdiff_t *ldr,
        const double *l,
        const ptrdiff_t *ldl,
        const double *e,
        const ptrdiff_t *lde,
        double *af,
        const ptrdiff_t *ldaf,
        double *bf,
        const ptrdiff_t *ldbf,
        double *tol,
        ptrdiff_t *iwork,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Optimal state feedback matrix for an optimal control problem
#define sb02nd FORTRAN_WRAPPER(sb02nd)
extern void sb02nd(
        const char *dico,
        const char *fact,
        const char *uplo,
        const char *jobl,
        const ptrdiff_t *n,
        const ptrdiff_t *m,
        const ptrdiff_t *p,
        const double *a,
        const ptrdiff_t *lda,
        double *b,
        const ptrdiff_t *ldb,
        double *r,
        const ptrdiff_t *ldr,
        ptrdiff_t *ipiv,
        const double *l,
        const ptrdiff_t *ldl,
        double *x,
        const ptrdiff_t *ldx,
        const double *rnorm,
        double *f,
        const ptrdiff_t *ldf,
        ptrdiff_t *oufact,
        ptrdiff_t *iwork,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Solution of continuous- or discrete-time algebraic Riccati equations for descriptor systems
#define sg02nd FORTRAN_WRAPPER(sg02nd)
extern void sg02nd(
        const char *dico,
        const char *jobe,
        const char *job,
        const char *jobx,
        const char *fact,
        const char *uplo,
        const char *jobl,
        const char *trans,
        const ptrdiff_t *n,
        const ptrdiff_t *m,
        const ptrdiff_t *p,
        const double *a,
        const ptrdiff_t *lda,
        const double *e,
        const ptrdiff_t *lde,
        double *b,
        const ptrdiff_t *ldb,
        double *r,
        const ptrdiff_t *ldr,
        ptrdiff_t *ipiv,
        const double *l,
        const ptrdiff_t *ldl,
        double *x,
        const ptrdiff_t *ldx,
        const double *rnorm,
        double *k,
        const ptrdiff_t *ldk,
        double *h,
        const ptrdiff_t *ldh,
        double *xe,
        const ptrdiff_t *ldxe,
        ptrdiff_t *oufact,
        ptrdiff_t *iwork,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Column interchanges in a complex matrix
#define ma02gz FORTRAN_WRAPPER(ma02gz)
extern void ma02gz(
        const ptrdiff_t *n,
        double *a,
        const ptrdiff_t *lda,
        const ptrdiff_t *k1,
        const ptrdiff_t *k2,
        const ptrdiff_t *ipiv,
        const ptrdiff_t *incx
        );

// Solution of linear equations X op(A) = B
#define mb02vd FORTRAN_WRAPPER(mb02vd)
extern void mb02vd(
        const char *trans,
        const ptrdiff_t *m,
        const ptrdiff_t *n,
        double *a,
        const ptrdiff_t *lda,
        ptrdiff_t *ipiv,
        double *b,
        const ptrdiff_t *ldb,
        ptrdiff_t *info
        );

// Periodic Hessenberg form of a product of p matrices using orthogonal similarity transformations
#define mb03vd FORTRAN_WRAPPER(mb03vd)
extern void mb03vd(
        const ptrdiff_t *n,
        const ptrdiff_t *p,
        const ptrdiff_t *ilo,
        const ptrdiff_t *ihi,
        double *a,
        const ptrdiff_t *lda1,
        const ptrdiff_t *lda2,
        double *tau,
        const ptrdiff_t *ldtau,
        double *dwork,
        ptrdiff_t *info
        );

// Orthogonal matrices for reduction to periodic Hessenberg form of a product of matrices
#define mb03vy FORTRAN_WRAPPER(mb03vy)
extern void mb03vy(
        const ptrdiff_t *n,
        const ptrdiff_t *p,
        const ptrdiff_t *ilo,
        const ptrdiff_t *ihi,
        double *a,
        const ptrdiff_t *lda1,
        const ptrdiff_t *lda2,
        double *tau,
        const ptrdiff_t *ldtau,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Schur decomposition and eigenvalues of a product of matrices in periodic Hessenberg form
#define mb03wd FORTRAN_WRAPPER(mb03wd)
extern void mb03wd(
        const char *job,
        const char *compz,
        const ptrdiff_t *n,
        const ptrdiff_t *p,
        const ptrdiff_t *ilo,
        const ptrdiff_t *ihi,
        const ptrdiff_t *iloz,
        const ptrdiff_t *ihiz,
        double *h,
        const ptrdiff_t *ldh1,
        const ptrdiff_t *ldh2,
        double *z,
        const ptrdiff_t *ldz1,
        const ptrdiff_t *ldz2,
        double *wr,
        double *wi,
        double *dwork,
        const ptrdiff_t *ldwork,
        ptrdiff_t *info
        );

// Computes the general product of K complex scalars trying to avoid over- and underflow.
#define zlapr1 FORTRAN_WRAPPER(zlapr1)
extern int zlapr1(
        doublereal *base,
        integer *k,
        integer *s,
        doublecomplex *a,
        integer *inca,
        doublecomplex *alpha,
        doublecomplex *beta,
        integer *scal
        );

// Finding the eigenvalues of the complex generalized matrix product.
#define zpgeqz FORTRAN_WRAPPER(zpgeqz)
extern int zpgeqz(
        char *job,
        char *compq,
        integer *k,
        integer *n,
        integer *ilo,
        integer *ihi,
        integer *s,
        doublecomplex *a,
        integer *lda1,
        integer *lda2,
        doublecomplex *alpha,
        doublecomplex *beta,
        integer *scal,
        doublecomplex *q,
        integer *ldq1,
        integer *ldq2,
        doublereal *dwork,
        integer *ldwork,
        doublecomplex *zwork,
        integer *lzwork,
        integer *info
        );

// Swaps adjacent diagonal 1-by-1 blocks in a complex generalized matrix product.
#define zpgex2 FORTRAN_WRAPPER(zpgex2)
extern int zpgex2(
        logical *wantq,
        integer *k,
        integer *n,
        integer *j,
        integer *s,
        doublecomplex *a,
        integer *lda1,
        integer *lda2,
        doublecomplex *q,
        integer *ldq1,
        integer *ldq2,
        doublecomplex *zwork,
        integer *info
        );

// Swaps adjacent diagonal 1-by-1 blocks in a complex generalized matrix product.
#define zpghrd FORTRAN_WRAPPER(zpghrd)
extern int zpghrd(
        char *compq,
        integer *k,
        integer *n,
        integer *ilo,
        integer *ihi,
        integer *s,
        doublecomplex *a,
        integer *lda1,
        integer *lda2,
        doublecomplex *q,
        integer *ldq1,
        integer *ldq2,
        doublereal *dwork,
        integer *ldwork,
        doublecomplex *zwork,
        integer *lzwork,
        integer *info
        );

// Reorders the periodic Schur decomposition of a complex generalized matrix product.
#define zpgord FORTRAN_WRAPPER(zpgord)
extern int zpgord(
        logical *wantq,
        integer *k,
        integer *n,
        integer *s,
        logical *select,
        doublecomplex *a,
        integer *lda1,
        integer *lda2,
        doublecomplex *alpha,
        doublecomplex *beta,
        integer *scal,
        doublecomplex *q,
        integer *ldq1,
        integer *ldq2,
        integer *m,
        doublecomplex *zwork,
        integer *lzwork,
        integer *info
        );


/* gsl_histogram2d_calloc_range.c
 * Copyright (C) 2000  Simone Piccardi
 *
 * This library 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 library; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */
/***************************************************************
 *
 * File gsl_histogram2d_calloc_range.c:
 * Routine to create a variable binning 2D histogram providing
 * the input range vectors. Need GSL library and header.
 * Do range check and allocate the histogram data.
 *
 * Author: S. Piccardi
 * Jan. 2000
 *
 ***************************************************************/
#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram2d.h>
/*
 * Routine that create a 2D histogram using the given
 * values for X and Y ranges
 */
gsl_histogram2d *
gsl_histogram2d_calloc_range (size_t nx, size_t ny,
                              double *xrange,
                              double *yrange)
{
  size_t i, j;
  gsl_histogram2d *h;

  /* check arguments */

  if (nx == 0)
    {
      GSL_ERROR_VAL ("histogram length nx must be positive integer",
                        GSL_EDOM, 0);
    }

  if (ny == 0)
    {
      GSL_ERROR_VAL ("histogram length ny must be positive integer",
                        GSL_EDOM, 0);
    }

  /* init ranges */

  for (i = 0; i < nx; i++)
    {
      if (xrange[i] >= xrange[i + 1])
        {
          GSL_ERROR_VAL ("histogram xrange not in increasing order",
                            GSL_EDOM, 0);
        }
    }

  for (j = 0; j < ny; j++)
    {
      if (yrange[j] >= yrange[j + 1])
        {
          GSL_ERROR_VAL ("histogram yrange not in increasing order"
                            ,GSL_EDOM, 0);
        }
    }

  /* Allocate histogram  */

  h = (gsl_histogram2d *) malloc (sizeof (gsl_histogram2d));

  if (h == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for histogram struct",
                        GSL_ENOMEM, 0);
    }

  h->xrange = (double *) malloc ((nx + 1) * sizeof (double));

  if (h->xrange == 0)
    {
      /* exception in constructor, avoid memory leak */
      free (h);

      GSL_ERROR_VAL ("failed to allocate space for histogram xrange",
                        GSL_ENOMEM, 0);
    }

  h->yrange = (double *) malloc ((ny + 1) * sizeof (double));

  if (h->yrange == 0)
    {
      /* exception in constructor, avoid memory leak */
      free (h);

      GSL_ERROR_VAL ("failed to allocate space for histogram yrange",
                        GSL_ENOMEM, 0);
    }

  h->bin = (double *) malloc (nx * ny * sizeof (double));

  if (h->bin == 0)
    {
      /* exception in constructor, avoid memory leak */
      free (h->xrange);
      free (h->yrange);
      free (h);

      GSL_ERROR_VAL ("failed to allocate space for histogram bins",
                        GSL_ENOMEM, 0);
    }

  /* init histogram */

  /* init ranges */

  for (i = 0; i <= nx; i++)
    {
      h->xrange[i] = xrange[i];
    }

  for (j = 0; j <= ny; j++)
    {
      h->yrange[j] = yrange[j];
    }

  /* clear contents */

  for (i = 0; i < nx; i++)
    {
      for (j = 0; j < ny; j++)
        {
          h->bin[i * ny + j] = 0;
        }
    }

  h->nx = nx;
  h->ny = ny;

  return h;
}


/* this manages and calls function to execute function
* from beginning to end
*/
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_statistics.h>

#include "fileio.h"
#include "data_process.h"
#include "sensor_history.h"
#include "sensor_validation.h"

#include "externs.h"

void handle_files(char *, double*, double*);
void sensor_validation(double*);
double data_processing(double*);

int sensor_number;
int temperature_min = 30;
int temperature_max = 60;
int data_number;
int group_number;
float q_percent;

/* path and names of the files used in the program
* which include input file, output file and sensor history file */
char input_file_name[255] = "../data/input_data/sample_input.csv";
char history_file_name[255] = "../data/sensor_history/sensor_history.csv";
char output_file_name[255] = "../data/output_data/output.csv";

int main() {

    printf("\nApplication is running....\n");

    /* user input for the number of sensors to process */
    printf("Insert number of sensors:");
    scanf("%d", &sensor_number);

    /* determining the time intervals for the sensors */
    int time_interval;

    printf("Insert collected time intervals:");
    scanf("%d", &time_interval);

    printf("Insert q percent(Should be 0~1):");
    scanf("%f", &q_percent);

    if ((q_percent<0) || (q_percent>1)) {
        printf("Invalid q_percent! It will be regarded as 0.7 of default value.\n");
        q_percent = 0.7;
    }

    double values[256];
    double time_value[256];
    double group_values[sensor_number];

    /* function performs reading of sensor data
    * @input : file name
    * @output : sensor time and associated values
    */
    handle_files(input_file_name, &time_value[0], &values[0]);

    /* number of calculations to be performed based on the user input */
    group_number = data_number / sensor_number;

    for (int i = 0; i < group_number; i++) {
        time_value[i] = time_value[i * sensor_number];
    }

    /* check whether the user input and actual file data matches */
    if (group_number != time_interval) {
        printf("ERROR: Time interval numbers, sensor numbers do not match with your input file!\n");
        exit(0);
    }

    sensor_validation(&values[0]);

    /* seperating multiple data computations in the output file*/
    FILE *fpout = fopen(output_file_name, "a");
    fprintf(fpout, "--------------\n");
    fclose(fpout);

    FILE *fphis = fopen(history_file_name, "a");
    fprintf(fphis, "--------------\n");
    fclose(fphis);

    for (int i = 0 ; i < group_number; i++) {
        for (int j = 0; j < sensor_number; j++) {
            group_values[j]  = values[i*sensor_number + j];
        }
        printf("\n---------------------Time interval %d---------------------\n\n", i);

        double fused = data_processing(&group_values[0]);
        float time_val_file = (float)time_value[i];

        write_data(time_val_file, fused, output_file_name);
    }

    return 1;
}

/* function checks whether an attempt made to open a file is succuessful or not */
void handle_files(char* input_file_name, double* time_value, double* values) {
    printf("\n");
    if (input_file_name != NULL) {
        data_number = read_data(&time_value[0], &values[0], input_file_name);

        if ( data_number == -1 ) {
            printf("ERROR: File open failed!\n");
            exit(0);
        } else if ( data_number == 0 ) {
            printf("ERROR: Sensor numbers inserted and file are different!\n");
            exit(0);
        } else if ( data_number > 0) {
            printf("Read data successfully!\n");
        }
    } else {
        printf("ERROR: Input file does not exist!\n");
        exit(0);
    }
}

/* function performs sensor validation for group values
* @input : group Values
*/
void sensor_validation(double* group_values) {

    int res = reading_validation(&group_values[0]);
    if (res == 1) {
        frozen_value_check(&group_values[0]);

    }

    if (res == 0) {
        printf("ERROR: Temperature values are out of range! Check history file.\n");
        frozen_value_check(&group_values[0]);
    }

}

/* function performs sensor fusion algo using group Values
* @input : group Values
* @output : fused result
*/
double data_processing(double* group_values) {

    printf("Sensor Values:\n");

    for (int i = 0; i < sensor_number; i++) {
        printf("x%d=%f, ", i, group_values[i]);
    }

    /* Step 1: Calc the Support Degree Matrix */
    gsl_matrix* D = gsl_matrix_alloc(sensor_number,sensor_number);
    support_degree_generator(D, &group_values[0]);    //function in data_process.c to get D - Support Degree Matrix

    /* Step 2: Calc eigenval & eigenvec */
    gsl_matrix* T = gsl_matrix_alloc(sensor_number,sensor_number);
    gsl_vector* evec = gsl_vector_alloc(sensor_number);
    gsl_matrix* Temp = gsl_matrix_alloc(sensor_number,sensor_number);

    gsl_matrix_memcpy(Temp, D);
    eigenvec_calc(evec, T, Temp);    //function in data_process.c to get evec - eigen group_values & T - vectors

    /* Step 3: Principal Comp Calc */
    gsl_matrix* y = gsl_matrix_alloc(sensor_number,sensor_number);
    principal_comp_calc(T, D, y);    //function in data_process.c to get T - Principal Components

    /* Step 4: Calc the contri rate of the kth principal comp */
    double alpha[sensor_number];
    contri_rate_calc_kth(evec, &alpha[0]);    //function in data_process.c to get alpha

    /* Step 5: Calc the contri rate of the m principal comp */
    double phi[sensor_number];
    major_contri_calc(&alpha[0], &phi[0]);    //function in data_process.c to get phi

    /* Step 6: Compute the integrated support degree score */
    gsl_vector* Z = gsl_vector_alloc(sensor_number);
    integ_supp_score_calc(&alpha[0], y, Z);    //function in data_process.c to get z_i

    /* Step 7-1: Eliminate incorrect data */
    int sensor_correction[sensor_number];
    elliminate_incorrect_data(Z, &sensor_correction[0]);    //function in data_process.c to elliminate incorrect datas

    /* Step 7-2: Compute the weight coefficient for each sensor */
    double omega[sensor_number];
    weight_coeff_calc(Z, &sensor_correction[0], &omega[0]);     //function in data_process.c to get omega

    /* Step 7-3: Compute the fused output */
    double fused;
    fused = fused_output(&omega[0], &group_values[0]);    //function in data_process.c to get fused output
    printf("FINAL STEP: \nThe fused output is %f\n", fused);

    /* Free memory */
    gsl_matrix_free(D);
    gsl_matrix_free(Temp);
    gsl_matrix_free(T);
    gsl_vector_free(evec);
    gsl_matrix_free(y);
    gsl_vector_free(Z);

    return fused;

}