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

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

/*
 * This module contains an implementation of the Powell dogleg
 * algorithm for nonlinear optimization problems. This implementation
 * closely follows the following works:
 *
 * [1] H. B. Nielsen, K. Madsen, Introduction to Optimization and
 *     Data Fitting, Informatics and Mathematical Modeling,
 *     Technical University of Denmark (DTU), 2010.
 *
 * [2] J. E. Dennis and H. H. W. Mei, Two new unconstrained optimization
 *     algorithms which use function and gradient values, J. Opt. Theory and
 *     Appl., 28(4), 1979.
 */

typedef struct
{
  size_t n;                  /* number of observations */
  size_t p;                  /* number of parameters */
  gsl_vector *dx_gn;         /* Gauss-Newton step, size p */
  gsl_vector *dx_sd;         /* steepest descent step, size p */
  double norm_Dgn;           /* || D dx_gn || */
  double norm_Dsd;           /* || D dx_sd || */
  double norm_Dinvg;         /* || D^{-1} g || */
  double norm_JDinv2g;       /* || J D^{-2} g || */
  gsl_vector *workp;         /* workspace, length p */
  gsl_vector *workn;         /* workspace, length n */

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

#include "common.c"

static void * dogleg_alloc (const void * params, const size_t n, const size_t p);
static void dogleg_free(void *vstate);
static int dogleg_init(const void *vtrust_state, void *vstate);
static int dogleg_preloop(const void * vtrust_state, void * vstate);
static int dogleg_step(const void * vtrust_state, const double delta,
                       gsl_vector * dx, void * vstate);
static int dogleg_double_step(const void * vtrust_state, const double delta,
                              gsl_vector * dx, void * vstate);
static int dogleg_preduction(const void * vtrust_state, const gsl_vector * dx,
                             double * pred, void * vstate);
static int dogleg_calc_gn(const gsl_multifit_nlinear_trust_state * trust_state, gsl_vector * dx);
static double dogleg_beta(const double t, const double delta,
                          const gsl_vector * diag, dogleg_state_t * state);

static void *
dogleg_alloc (const void * params, const size_t n, const size_t p)
{
  const gsl_multifit_nlinear_parameters *mparams = (const gsl_multifit_nlinear_parameters *) params;
  dogleg_state_t *state;

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

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

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

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

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

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

  return state;
}

static void
dogleg_free(void *vstate)
{
  dogleg_state_t *state = (dogleg_state_t *) vstate;

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

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

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

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

  free(state);
}

/*
dogleg_init()
  Initialize dogleg solver

Inputs: vtrust_state - trust state
        vstate       - workspace

Return: success/error
*/

static int
dogleg_init(const void *vtrust_state, void *vstate)
{
  (void)vtrust_state;
  (void)vstate;

  return GSL_SUCCESS;
}

/*
dogleg_preloop()
  Initialize dogleg method prior to iteration loop.
This involves computing the steepest descent step. The
Gauss-Newton step is computed if needed in the _step()
routines.

Notes: on output,
1) state->dx_sd contains steepest descent step
2) state->norm_Dinvg contains || D^{-1} g ||
3) state->norm_JDinv2g contains || J D^{-2} g ||
*/

static int
dogleg_preloop(const void * vtrust_state, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  dogleg_state_t *state = (dogleg_state_t *) vstate;
  double u;
  double alpha; /* ||g||^2 / ||Jg||^2 */

  /* calculate the steepest descent step */

  /* compute workp = D^{-1} g and its norm */
  gsl_vector_memcpy(state->workp, trust_state->g);
  gsl_vector_div(state->workp, trust_state->diag);
  state->norm_Dinvg = gsl_blas_dnrm2(state->workp);

  /* compute workp = D^{-2} g */
  gsl_vector_div(state->workp, trust_state->diag);

  /* compute: workn = J D^{-2} g */
  gsl_blas_dgemv(CblasNoTrans, 1.0, trust_state->J, state->workp, 0.0, state->workn);
  state->norm_JDinv2g = gsl_blas_dnrm2(state->workn);

  u = state->norm_Dinvg / state->norm_JDinv2g;
  alpha = u * u;

  /* dx_sd = -alpha D^{-2} g */
  gsl_vector_memcpy(state->dx_sd, state->workp);
  gsl_vector_scale(state->dx_sd, -alpha);

  state->norm_Dsd = scaled_enorm(trust_state->diag, state->dx_sd);
  state->norm_Dgn = -1.0; /* computed later if needed */

  return GSL_SUCCESS;
}

/*
dogleg_step()
  Calculate a new step vector
*/

static int
dogleg_step(const void * vtrust_state, const double delta,
            gsl_vector * dx, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  dogleg_state_t *state = (dogleg_state_t *) vstate;

  if (state->norm_Dsd >= delta)
    {
      /* steepest descent step is outside trust region;
       * truncate steepest descent step to trust region boundary */
      gsl_vector_memcpy(dx, state->dx_sd);
      gsl_vector_scale(dx, delta / state->norm_Dsd);
    }
  else
    {
      /* compute Gauss-Newton step if needed */
      if (state->norm_Dgn < 0.0)
        {
          int status = dogleg_calc_gn(trust_state, state->dx_gn);

          if (status)
            return status;

          /* compute || D dx_gn || */
          state->norm_Dgn = scaled_enorm(trust_state->diag, state->dx_gn);
        }

      if (state->norm_Dgn <= delta)
        {
          /* Gauss-Newton step is inside trust region, use it as final step
           * since it is the global minimizer of the quadratic model function */
          gsl_vector_memcpy(dx, state->dx_gn);
        }
      else
        {
          /* Gauss-Newton step is outside trust region, but steepest
           * descent is inside; use dogleg step */

          double beta = dogleg_beta(1.0, delta, trust_state->diag, state);

          /* compute: workp = dx_gn - dx_sd */
          scaled_addition(1.0, state->dx_gn, -1.0, state->dx_sd, state->workp);

          /* dx = dx_sd + beta*(dx_gn - dx_sd) */
          scaled_addition(beta, state->workp, 1.0, state->dx_sd, dx);
        }
    }

  return GSL_SUCCESS;
}

/*
dogleg_double_step()
  Calculate a new step with double dogleg method. Based on
section 3 of [2]
*/

static int
dogleg_double_step(const void * vtrust_state, const double delta,
                   gsl_vector * dx, void * vstate)
{
  const double alpha_fac = 0.8; /* recommended value from Dennis and Mei */
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  dogleg_state_t *state = (dogleg_state_t *) vstate;

  if (state->norm_Dsd >= delta)
    {
      /* steepest descent step is outside trust region;
       * truncate steepest descent step to trust region boundary */
      gsl_vector_memcpy(dx, state->dx_sd);
      gsl_vector_scale(dx, delta / state->norm_Dsd);
    }
  else
    {
      /* compute Gauss-Newton step if needed */
      if (state->norm_Dgn < 0.0)
        {
          int status = dogleg_calc_gn(trust_state, state->dx_gn);

          if (status)
            return status;

          /* compute || D dx_gn || */
          state->norm_Dgn = scaled_enorm(trust_state->diag, state->dx_gn);
        }

      if (state->norm_Dgn <= delta)
        {
          /* Gauss-Newton step is inside trust region, use it as final step
           * since it is the global minimizer of the quadratic model function */
          gsl_vector_memcpy(dx, state->dx_gn);
        }
      else
        {
          double t, u, v, c;

          /* compute: u = ||D^{-1} g||^2 / ||J D^{-2} g||^2 */
          v = state->norm_Dinvg / state->norm_JDinv2g;
          u = v * v;

          /* compute: v = g^T dx_gn */
          gsl_blas_ddot(trust_state->g, state->dx_gn, &v);

          /* compute: c = ||D^{-1} g||^4 / (||J D^{-2} g||^2 * |g^T dx_gn|) */
          c = u * (state->norm_Dinvg / fabs(v)) * state->norm_Dinvg;

          /* compute: t = 1 - alpha_fac*(1-c) */
          t = 1.0 - alpha_fac*(1.0 - c);

          if (t * state->norm_Dgn <= delta)
            {
              /* set dx = (delta / ||D dx_gn||) dx_gn */
              gsl_vector_memcpy(dx, state->dx_gn);
              gsl_vector_scale(dx, delta / state->norm_Dgn);
            }
          else
            {
              /* Cauchy point is inside, Gauss-Newton is outside trust region;
               * use double dogleg step */

              double beta = dogleg_beta(t, delta, trust_state->diag, state);

              /* compute: workp = t*dx_gn - dx_sd */
              scaled_addition(t, state->dx_gn, -1.0, state->dx_sd, state->workp);

              /* dx = dx_sd + beta*(t*dx_gn - dx_sd) */
              scaled_addition(beta, state->workp, 1.0, state->dx_sd, dx);
            }
        }
    }

  return GSL_SUCCESS;
}

static int
dogleg_preduction(const void * vtrust_state, const gsl_vector * dx,
                  double * pred, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  dogleg_state_t *state = (dogleg_state_t *) vstate;

  *pred = quadratic_preduction(trust_state->f, trust_state->J, dx, state->workn);

  return GSL_SUCCESS;
}

/*
dogleg_calc_gn()
  Calculate Gauss-Newton step which satisfies:

J dx_gn = -f

Inputs: trust_state - trust state variables
        dx          - (output) Gauss-Newton step

Return: success/error
*/

static int
dogleg_calc_gn(const gsl_multifit_nlinear_trust_state * trust_state, gsl_vector * dx)
{
  int status;
  const gsl_multifit_nlinear_parameters *params = trust_state->params;

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

  /* prepare the linear solver to compute Gauss-Newton step */
  status = (params->solver->presolve)(0.0, trust_state, trust_state->solver_state);
  if (status)
    return status;

  /* solve: J dx_gn = -f for Gauss-Newton step */
  status = (params->solver->solve)(trust_state->f,
                                   dx,
                                   trust_state,
                                   trust_state->solver_state);
  if (status)
    return status;

  return GSL_SUCCESS;
}

/*
dogleg_beta()
  This function finds beta in [0,1] such that the step

dx = dx_sd + beta*(t*dx_gn - dx_sd)

has norm

||D dx|| = delta

beta is the positive root of the quadratic:

a beta^2 + b beta + c = 0

with

a = ||D(t*dx_gn - dx_sd)||^2
b = 2 dx_sd^T D^T D (t*dx_gn - dx_sd)
c = ||D dx_sd||^2 - delta^2

Inputs: t     - amount of Gauss-Newton step to use for dogleg
                (= 1 for classical dogleg, <= 1 for double dogleg)
        delta - trust region radius
        diag  - diag(D) scaling matrix
        state - workspace
*/

static double
dogleg_beta(const double t, const double delta,
            const gsl_vector * diag, dogleg_state_t * state)
{
  double beta;
  double a, b, c;

  /* compute: workp = t*dx_gn - dx_sd */
  scaled_addition(t, state->dx_gn, -1.0, state->dx_sd, state->workp);

  /* a = || D (t*dx_gn - dx_sd) ||^2 */
  a = scaled_enorm(diag, state->workp);
  a *= a;

  /* workp = D^T D (t*dx_gn - dx_sd) */
  gsl_vector_mul(state->workp, diag);
  gsl_vector_mul(state->workp, diag);

  /* b = 2 dx_sd^T D^T D (t*dx_gn - dx-sd) */
  gsl_blas_ddot(state->dx_sd, state->workp, &b);
  b *= 2.0;

  /* c = || D dx_sd ||^2 - delta^2 = (||D dx_sd|| + delta) (||D dx_sd|| - delta) */
  c = (state->norm_Dsd + delta) * (state->norm_Dsd - delta);

  if (b > 0.0)
    {
      beta = (-2.0 * c) / (b + sqrt(b*b - 4.0*a*c));
    }
  else
    {
      beta = (-b + sqrt(b*b - 4.0*a*c)) / (2.0 * a);
    }

  return beta;
}

static const gsl_multifit_nlinear_trs dogleg_type =
{
  "dogleg",
  dogleg_alloc,
  dogleg_init,
  dogleg_preloop,
  dogleg_step,
  dogleg_preduction,
  dogleg_free
};

const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_dogleg = &dogleg_type;

static const gsl_multifit_nlinear_trs ddogleg_type =
{
  "double-dogleg",
  dogleg_alloc,
  dogleg_init,
  dogleg_preloop,
  dogleg_double_step,
  dogleg_preduction,
  dogleg_free
};

const gsl_multifit_nlinear_trs *gsl_multifit_nlinear_trs_ddogleg = &ddogleg_type;


#include <stdlib.h>
#include <assert.h>
#include <cconfigspace.h>
#include <gsl/gsl_rng.h>

static void test_rng_create_with_type() {
	const gsl_rng_type **t1, **t2, *t;
	size_t               type_count = 0;
	int32_t              selected, refcount;
	ccs_rng_t            rng = NULL;
	ccs_result_t         err = CCS_SUCCESS;
	ccs_object_type_t    otype;

	t1 = t2 = gsl_rng_types_setup();
	while (*t1++)
		type_count++;
	selected = rand() % type_count;
	err = ccs_rng_create_with_type(t2[selected], NULL);
	assert( err == -CCS_INVALID_VALUE );
	err = ccs_rng_create_with_type(NULL, &rng);
	assert( err == -CCS_INVALID_VALUE );
	err = ccs_rng_create_with_type(t2[selected], &rng);
	assert( err == CCS_SUCCESS );
	assert( rng );
	err = ccs_rng_get_type(rng, &t);
	assert( err == CCS_SUCCESS );
	assert( t == t2[selected] );
	err = ccs_object_get_type(rng, &otype);
	assert( err == CCS_SUCCESS );
	assert( otype == CCS_RNG );
	err = ccs_object_get_refcount(rng, &refcount);
	assert( err == CCS_SUCCESS );
	assert( refcount == 1 );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

static void test_rng_create() {
	ccs_rng_t           rng = NULL;
	ccs_result_t        err = CCS_SUCCESS;
	const gsl_rng_type *t;

	err = ccs_rng_create(NULL);
	assert( err == -CCS_INVALID_VALUE );
	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );
	assert( rng );
	err = ccs_rng_get_type(rng, &t);
	assert( err == CCS_SUCCESS );
	assert( t == gsl_rng_default );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

static void test_rng_min_max() {
	ccs_rng_t         rng = NULL;
	ccs_result_t      err = CCS_SUCCESS;
	unsigned long int imin = 0;
	unsigned long int imax = 0;
	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );
	err = ccs_rng_min(NULL, &imin);
	assert( err == -CCS_INVALID_OBJECT );
	err = ccs_rng_min(rng, NULL);
	assert( err == -CCS_INVALID_VALUE );
	err = ccs_rng_min(rng, &imin);
	assert( err == CCS_SUCCESS );
	err = ccs_rng_max(NULL, &imax);
	assert( err == -CCS_INVALID_OBJECT );
	err = ccs_rng_max(rng, NULL);
	assert( err == -CCS_INVALID_VALUE );
	err = ccs_rng_max(rng, &imax);
	assert( err == CCS_SUCCESS );
	assert( imin < imax );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

static void test_rng_get() {
	ccs_rng_t         rng = NULL;
	ccs_result_t      err = CCS_SUCCESS;
	unsigned long int i = 0;
	unsigned long int imin = 0;
	unsigned long int imax = 0;

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );
	err = ccs_rng_min(rng, &imin);
	assert( err == CCS_SUCCESS );
	err = ccs_rng_max(rng, &imax);
	assert( err == CCS_SUCCESS );
	for (int j = 0; j < 100; j++) {
		err = ccs_rng_get(rng, &i);
		assert( err == CCS_SUCCESS );
		assert( i >= imin );
		assert( i <= imax );
	}
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

static void test_rng_uniform() {
	ccs_rng_t    rng = NULL;
	ccs_result_t err = CCS_SUCCESS;
	double       d = -1.0;

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );
	for (int j = 0; j < 100; j++) {
		err = ccs_rng_uniform(rng, &d);
		assert( err == CCS_SUCCESS );
		assert( d >= 0.0 );
		assert( d <  1.0 );
	}
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

int main() {
	ccs_init();
	test_rng_create_with_type();
	test_rng_create();
	test_rng_min_max();
	test_rng_get();
	test_rng_uniform();
	ccs_fini();
	return 0;
}


/**
 *
 * @file timing.c
 *
 *  PLASMA auxiliary routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author ???
 * @author Mathieu Faverge
 * @author Dulceneia Becker
 * @date 2010-11-15
 *
 **/

#if defined( _WIN32 ) || defined( _WIN64 )
#define int64_t __int64
#endif

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

#ifdef PLASMA_EZTRACE
#include <eztrace.h>
#endif

#if defined( _WIN32 ) || defined( _WIN64 )
#include <windows.h>
#include <time.h>
#include <sys/timeb.h>
#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
#define DELTA_EPOCH_IN_MICROSECS  11644473600000000Ui64
#else
#define DELTA_EPOCH_IN_MICROSECS  11644473600000000ULL
#endif

struct timezone
{
    int  tz_minuteswest; /* minutes W of Greenwich */
    int  tz_dsttime;     /* type of dst correction */
};

int gettimeofday(struct timeval* tv, struct timezone* tz)
{
    FILETIME         ft;
    unsigned __int64 tmpres = 0;
    static int       tzflag;

    if (NULL != tv)
        {
            GetSystemTimeAsFileTime(&ft);
            tmpres |=  ft.dwHighDateTime;
            tmpres <<= 32;
            tmpres |=  ft.dwLowDateTime;

            /*converting file time to unix epoch*/
            tmpres /= 10;  /*convert into microseconds*/
            tmpres -= DELTA_EPOCH_IN_MICROSECS;

            tv->tv_sec  = (long)(tmpres / 1000000UL);
            tv->tv_usec = (long)(tmpres % 1000000UL);
        }
    if (NULL != tz)
        {
            if (!tzflag)
                {
                    _tzset();
                    tzflag++;
                }
            tz->tz_minuteswest = _timezone / 60;
            tz->tz_dsttime     = _daylight;
        }
    return 0;
}

#else  /* Non-Windows */
#include <unistd.h>
#include <sys/time.h>
#include <sys/resource.h>
#endif

#include <cblas.h>
#include <lapacke.h>
#include <plasma.h>
#include <core_blas.h>
#include "flops.h"
#include "timing.h"
#include "auxiliary.h"
#include <omp.h>

static int RunTest(int *iparam, _PREC *dparam, double *t_);

double cWtime(void);

int ISEED[4] = {0,0,0,1};   /* initial seed for zlarnv() */

/*
 * struct timeval {time_t tv_sec; suseconds_t tv_usec;};
 */
double cWtime(void)
{
    struct timeval tp;
    gettimeofday( &tp, NULL );
    return tp.tv_sec + 1e-6 * tp.tv_usec;
}

static int
Test(int64_t n, int *iparam) {
    int      i, j, iter;
    int      thrdnbr, niter;
    int64_t  M, N, K, NRHS;
    double  *t;
    _PREC    eps = _LAMCH( 'e' );
    _PREC    dparam[IPARAM_DNBPARAM];
    double   fmuls, fadds, fp_per_mul, fp_per_add;
    double   sumgf, sumgf2, sumt, sd, flops, gflops;
    char    *s;
    char    *env[] = {
        "OMP_NUM_THREADS",
        "MKL_NUM_THREADS",
        "GOTO_NUM_THREADS",
        "ACML_NUM_THREADS",
        "ATLAS_NUM_THREADS",
        "BLAS_NUM_THREADS", ""
    };
    int gnuplot = 0;

    memset( &dparam, 0, IPARAM_DNBPARAM * sizeof(_PREC) );

    thrdnbr = iparam[IPARAM_THRDNBR];
    niter   = iparam[IPARAM_NITER];

    M    = iparam[IPARAM_M];
    N    = iparam[IPARAM_N];
    K    = iparam[IPARAM_K];
    NRHS = K;
    (void)M;(void)N;(void)K;(void)NRHS;

    if ( (n < 0) || (thrdnbr < 0 ) ) {
        if (gnuplot) {
            printf( "set title '%d_NUM_THREADS: ", thrdnbr );
            for (i = 0; env[i][0]; ++i) {
                s = getenv( env[i] );

                if (i) printf( " " ); /* separating space */

                for (j = 0; j < 5 && env[i][j] && env[i][j] != '_'; ++j)
                    printf( "%c", env[i][j] );

                if (s)
                    printf( "=%s", s );
                else
                    printf( "->%s", "?" );
            }
            printf( "'\n" );
            printf( "%s\n%s\n%s\n%s\n%s%s%s\n",
                    "set xlabel 'Matrix size'",
                    "set ylabel 'Gflop/s'",
                    "set key bottom",
                    gnuplot > 1 ? "set terminal png giant\nset output 'timeplot.png'" : "",
                    "plot '-' using 1:5 title '", _NAME, "' with linespoints" );
        }

        return 0;
    }

    printf( "%7d %7d %7d ", iparam[IPARAM_M], iparam[IPARAM_N], iparam[IPARAM_K] );
    fflush( stdout );

    t = (double*)malloc(niter*sizeof(double));
    memset(t, 0, niter*sizeof(double));

    if (sizeof(_TYPE) == sizeof(_PREC)) {
        fp_per_mul = 1;
        fp_per_add = 1;
    } else {
        fp_per_mul = 6;
        fp_per_add = 2;
    }

    fadds = (double)(_FADDS);
    fmuls = (double)(_FMULS);
    flops = 1e-9 * (fmuls * fp_per_mul + fadds * fp_per_add);
    gflops = 0.0;

    if ( iparam[IPARAM_WARMUP] ) {
        RunTest( iparam, dparam, &(t[0]));
    }

    sumgf  = 0.0;
    sumgf2 = 0.0;
    sumt   = 0.0;

    for (iter = 0; iter < niter; iter++)
    {
        if( iter == 0 ) {
          if ( iparam[IPARAM_TRACE] )
            iparam[IPARAM_TRACE] = 2;
          if ( iparam[IPARAM_DAG] )
            iparam[IPARAM_DAG] = 2;

          RunTest( iparam, dparam, &(t[iter]));

          iparam[IPARAM_TRACE] = 0;
          iparam[IPARAM_DAG] = 0;
        }
        else
            RunTest( iparam, dparam, &(t[iter]));

        gflops = flops / t[iter];

        sumt   += t[iter];
        sumgf  += gflops;
        sumgf2 += gflops*gflops;
    }

    gflops = sumgf / niter;
    sd = sqrt((sumgf2 - (sumgf*sumgf)/niter)/niter);

    if ( iparam[IPARAM_CHECK] )
        printf( "%9.3f %9.2f %9.2f %8.5e %8.5e %8.5e %8.5e %8.5e\n",
                sumt/niter, gflops, sd, dparam[IPARAM_RES], dparam[IPARAM_ANORM], dparam[IPARAM_XNORM], dparam[IPARAM_BNORM],
                dparam[IPARAM_RES] / n / eps / (dparam[IPARAM_ANORM] * dparam[IPARAM_XNORM] + dparam[IPARAM_BNORM] ));
    else{
        printf( "%9.3f %9.2f %9.2f\n", sumt/niter, gflops, sd );
    }
    fflush( stdout );
    free(t);

    return 0;
}

static int
startswith(const char *s, const char *prefix) {
    size_t n = strlen( prefix );
    if (strncmp( s, prefix, n ))
        return 0;
    return 1;
}

static int
get_range(char *range, int *start_p, int *stop_p, int *step_p) {
    char *s, *s1, buf[21];
    int colon_count, copy_len, nbuf=20, n;
    int start=1000, stop=10000, step=1000;

    colon_count = 0;
    for (s = strchr( range, ':'); s; s = strchr( s+1, ':'))
        colon_count++;

    if (colon_count == 0) { /* No colon in range. */
        if (sscanf( range, "%d", &start ) < 1 || start < 1)
            return -1;
        step = start / 10;
        if (step < 1) step = 1;
        stop = start + 10 * step;

    } else if (colon_count == 1) { /* One colon in range.*/
        /* First, get the second number (after colon): the stop value. */
        s = strchr( range, ':' );
        if (sscanf( s+1, "%d", &stop ) < 1 || stop < 1)
            return -1;

        /* Next, get the first number (before colon): the start value. */
        n = s - range;
        copy_len = n > nbuf ? nbuf : n;
        strncpy( buf, range, copy_len );
        buf[copy_len] = 0;
        if (sscanf( buf, "%d", &start ) < 1 || start > stop || start < 1)
            return -1;

        /* Let's have 10 steps or less. */
        step = (stop - start) / 10;
        if (step < 1)
            step = 1;
    } else if (colon_count == 2) { /* Two colons in range. */
        /* First, get the first number (before the first colon): the start value. */
        s = strchr( range, ':' );
        n = s - range;
        copy_len = n > nbuf ? nbuf : n;
        strncpy( buf, range, copy_len );
        buf[copy_len] = 0;
        if (sscanf( buf, "%d", &start ) < 1 || start < 1)
            return -1;

        /* Next, get the second number (after the first colon): the stop value. */
        s1 = strchr( s+1, ':' );
        n = s1 - (s + 1);
        copy_len = n > nbuf ? nbuf : n;
        strncpy( buf, s+1, copy_len );
        buf[copy_len] = 0;
        if (sscanf( buf, "%d", &stop ) < 1 || stop < start)
            return -1;

        /* Finally, get the third number (after the second colon): the step value. */
        if (sscanf( s1+1, "%d", &step ) < 1 || step < 1)
            return -1;
    } else

        return -1;

    *start_p = start;
    *stop_p = stop;
    *step_p = step;

    return 0;
}

static void
show_help(char *prog_name) {
    printf( "Usage:\n%s [options]\n\n", prog_name );
    printf( "Options are:\n"
            "  --help           Show this help\n"
            "\n"
            "  --threads=X      Number of CPU workers (default: _SC_NPROCESSORS_ONLN)\n"
            "\n"
            "  --[a]sync        Enable/Disable synchronous calls in wrapper function such as POTRI. (default: async)\n"
            "  --[no]check      Check result (default: nocheck)\n"
            "  --[no]inv        Check on inverse (default: noinv)\n"
            "  --[no]warmup     Perform a warmup run to pre-load libraries (default: warmup)\n"
#if defined(PLASMA_EZTRACE)
            "  --[no]trace      Enable/Disable trace generation (default: notrace)\n"
#endif
            "  --[no]dag        Enable/Disable DAG generation (default: nodag)\n"
            "                   Generates a dot_dag_file.dot.\n"
            "  --[no]atun       Activate autotuning (default: noatun) [Deprecated]\n"
            "  --inplace        Enable layout conversion inplace for lapack interface timers (default: enable)\n"
            "  --outplace       Enable layout conversion out of place for lapack interface timers (default: disable)\n"
            "\n"
            "  --n_range=R      Range of N values\n"
            "                   with R=Start:Stop:Step (default: 500:5000:500)\n"
            "  --m=X            dimension (M) of the matrices (default: N)\n"
            "  --k=X            dimension (K) of the matrices (default: 1)\n"
            "  --nrhs=X         Number of right-hand size (default: 1)\n"
            "  --nb=N           Nb size. (default: 128)\n"
            "  --ib=N           IB size. (default: 32)\n"
            "\n"
            "  --niter=N        Number of iterations performed for each test (default: 1)\n"
            "\n"
            "  --rhblk=N        If N > 0, enable Householder mode for QR and LQ factorization\n"
            "                   N is the size of each subdomain (default: 0)\n"
            "  --tntmode=x      Factorization used to select the pivots in the tournament pivoting algorithm (default: 1)\n"
            "                     1 - LU factorization with partial pivoting\n"
            "                     2 - QR rank revealing factorization\n"
            "  --tntsize=N      If N > 0, number of tiles grouped together during the tournament (default=4)\n"
            "\n"
            " Options specific to the conversion format timings xgetri and xgecfi:\n"
            "  --ifmt           Input format. (default: 0)\n"
            "  --ofmt           Output format. (default: 1)\n"
            "                   The possible values are:\n"
            "                     0 - PlasmaCM, Column major\n"
            "                     1 - PlasmaCCRB, Column-Colum rectangular block\n"
            "                     2 - PlasmaCRRB, Column-Row rectangular block\n"
            "                     3 - PlasmaRCRB, Row-Colum rectangular block\n"
            "                     4 - PlasmaRRRB, Row-Row rectangular block\n"
            "                     5 - PlasmaRM, Row Major\n"
            "  --thrdbypb       Number of threads per subproblem for inplace transformation (default: 1)\n"
            "\n");
}

static void
print_header(char *prog_name, int * iparam) {
    _PREC    eps = _LAMCH( 'e' );
    printf( "#\n"
            "# PLASMA %d.%d.%d, %s\n"
            "# Nb threads: %d\n"
            "# NB:         %d\n"
            "# IB:         %d\n"
            "# eps:        %e\n"
            "#\n",
            PLASMA_VERSION_MAJOR,
            PLASMA_VERSION_MINOR,
            PLASMA_VERSION_MICRO,
            prog_name,
            iparam[IPARAM_THRDNBR],
            iparam[IPARAM_NB],
            iparam[IPARAM_IB],
            eps );
    if  (iparam[IPARAM_CHECK] )
        printf( "#     M       N  K/NRHS   seconds   Gflop/s Deviation"
                "    ||Ax-b||       ||A||       ||x||       ||b|| ||Ax-b||/N/eps/(||A||||x||+||b||)\n" );
    else
        printf( "#     M       N  K/NRHS   seconds   Gflop/s Deviation\n" );

    return;
}

static void
get_thread_count(int *thrdnbr) {
#if defined WIN32 || defined WIN64
    sscanf( getenv( "NUMBER_OF_PROCESSORS" ), "%d", thrdnbr );
#else
    *thrdnbr = sysconf(_SC_NPROCESSORS_ONLN);
#endif
}

int
main(int argc, char *argv[]) {
    int i, m, mx, nx;
    int start =  500;
    int stop  = 5000;
    int step  =  500;
    int iparam[IPARAM_SIZEOF];

    memset(iparam, 0, IPARAM_SIZEOF*sizeof(int));

    iparam[IPARAM_THRDNBR       ] = 1;
    iparam[IPARAM_THRDNBR_SUBGRP] = 1;
    iparam[IPARAM_SCHEDULER     ] = 1;
    iparam[IPARAM_RUNTIME       ] = 2;
    iparam[IPARAM_M             ] = -1;
    iparam[IPARAM_N             ] = 500;
    iparam[IPARAM_K             ] = 500;
    iparam[IPARAM_LDA           ] = 500;
    iparam[IPARAM_LDB           ] = 500;
    iparam[IPARAM_LDC           ] = 500;
    iparam[IPARAM_MB            ] = 128;
    iparam[IPARAM_NB            ] = 128;
    iparam[IPARAM_IB            ] = 32;
    iparam[IPARAM_NITER         ] = 1;
    iparam[IPARAM_WARMUP        ] = 1;
    iparam[IPARAM_CHECK         ] = 1;
    iparam[IPARAM_VERBOSE       ] = 1;
    iparam[IPARAM_AUTOTUNING    ] = 1;
    iparam[IPARAM_INPUTFMT      ] = 0;
    iparam[IPARAM_OUTPUTFMT     ] = 0;
    iparam[IPARAM_TRACE         ] = 0;
    iparam[IPARAM_DAG           ] = 0;
    iparam[IPARAM_ASYNC         ] = 1;
    iparam[IPARAM_MX            ] = -1;
    iparam[IPARAM_NX            ] = -1;
    iparam[IPARAM_RHBLK         ] = 0;
    iparam[IPARAM_TNTPIV_MODE   ] = PLASMA_TOURNAMENT_LU;
    iparam[IPARAM_TNTPIV_SIZE   ] = 4;
    iparam[IPARAM_INPLACE       ] = PLASMA_OUTOFPLACE;

    get_thread_count( &(iparam[IPARAM_THRDNBR]) );

    for (i = 1; i < argc && argv[i]; ++i) {
        if (startswith( argv[i], "--help" )) {
            show_help( argv[0] );
            return EXIT_SUCCESS;
        } else if (startswith( argv[i], "--threads=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_THRDNBR]) );
        } else if (startswith( argv[i], "--check" )) {
            iparam[IPARAM_CHECK] = 1;
        } else if (startswith( argv[i], "--nocheck" )) {
            iparam[IPARAM_CHECK] = 0;
        } else if (startswith( argv[i], "--warmup" )) {
            iparam[IPARAM_WARMUP] = 1;
        } else if (startswith( argv[i], "--nowarmup" )) {
            iparam[IPARAM_WARMUP] = 0;
        } else if (startswith( argv[i], "--dyn" )) {
            iparam[IPARAM_SCHEDULER] = 1;
        } else if (startswith( argv[i], "--nodyn" )) {
            iparam[IPARAM_SCHEDULER] = 0;
        } else if (startswith( argv[i], "--quark" )) {
            iparam[IPARAM_RUNTIME] = 1;
        } else if (startswith( argv[i], "--ompss" )) {
            iparam[IPARAM_RUNTIME] = 2;
        /* } else if (startswith( argv[i], "--atun" )) { */
        /*     iparam[IPARAM_AUTOTUNING] = 1; */
        /* } else if (startswith( argv[i], "--noatun" )) { */
        /*     iparam[IPARAM_AUTOTUNING] = 0; */
        } else if (startswith( argv[i], "--trace" )) {
            iparam[IPARAM_TRACE] = 1;
        } else if (startswith( argv[i], "--notrace" )) {
            iparam[IPARAM_TRACE] = 0;
        } else if (startswith( argv[i], "--dag" )) {
            iparam[IPARAM_DAG] = 1;
        } else if (startswith( argv[i], "--nodag" )) {
            iparam[IPARAM_DAG] = 0;
        } else if (startswith( argv[i], "--sync" )) {
            iparam[IPARAM_ASYNC] = 0;
        } else if (startswith( argv[i], "--async" )) {
            iparam[IPARAM_ASYNC] = 1;
        } else if (startswith( argv[i], "--n_range=" )) {
            get_range( strchr( argv[i], '=' ) + 1, &start, &stop, &step );
        } else if (startswith( argv[i], "--m=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_M]) );
        } else if (startswith( argv[i], "--n=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_N]) );
            start = iparam[IPARAM_N];
            stop  = iparam[IPARAM_N];
            step  = 1;
        } else if (startswith( argv[i], "--nb=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_NB]) );
            iparam[IPARAM_MB] = iparam[IPARAM_NB];
        } else if (startswith( argv[i], "--nrhs=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_K]) );
        } else if (startswith( argv[i], "--k=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_K]) );
        } else if (startswith( argv[i], "--ib=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_IB]) );
        } else if (startswith( argv[i], "--ifmt=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_INPUTFMT]) );
        } else if (startswith( argv[i], "--ofmt=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_OUTPUTFMT]) );
        } else if (startswith( argv[i], "--thrdbypb=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_THRDNBR_SUBGRP]) );
        } else if (startswith( argv[i], "--niter=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &iparam[IPARAM_NITER] );
        } else if (startswith( argv[i], "--mx=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_MX]) );
        } else if (startswith( argv[i], "--nx=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_NX]) );
        } else if (startswith( argv[i], "--rhblk=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_RHBLK]) );
        } else if (startswith( argv[i], "--tntmode=" )) {
            int tmp;
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &tmp );
            iparam[IPARAM_TNTPIV_MODE] = (tmp == 1) ? PLASMA_TOURNAMENT_LU : PLASMA_TOURNAMENT_QR;
        } else if (startswith( argv[i], "--tntsize=" )) {
            sscanf( strchr( argv[i], '=' ) + 1, "%d", &(iparam[IPARAM_TNTPIV_SIZE]) );
        } else if (startswith( argv[i], "--inplace" )) {
            iparam[IPARAM_INPLACE] = PLASMA_INPLACE;
        } else if (startswith( argv[i], "--outplace" )) {
            iparam[IPARAM_INPLACE] = PLASMA_OUTOFPLACE;
        } else {
            fprintf( stderr, "Unknown option: %s\n", argv[i] );
        }
    }

    m  = iparam[IPARAM_M];
    mx = iparam[IPARAM_MX];
    nx = iparam[IPARAM_NX];

    /* Initialize Plasma */
	if ( iparam[IPARAM_RUNTIME] == PLASMA_QUARK )
		PLASMA_Runtime_Init(omp_get_num_threads(), PLASMA_QUARK);
	else
		PLASMA_Runtime_Init(omp_get_num_threads(), PLASMA_OMPSS);

    if ( iparam[IPARAM_SCHEDULER] ){
        PLASMA_Set(PLASMA_SCHEDULING_MODE, PLASMA_DYNAMIC_SCHEDULING );
        if ( iparam[IPARAM_RUNTIME] == PLASMA_QUARK )
            PLASMA_Set(PLASMA_RUNTIME_MODE, PLASMA_QUARK );
        else
            PLASMA_Set(PLASMA_RUNTIME_MODE, PLASMA_OMPSS );
    }
    else
        PLASMA_Set(PLASMA_SCHEDULING_MODE, PLASMA_STATIC_SCHEDULING );

    /* if ( !iparam[IPARAM_AUTOTUNING] ) { */
    PLASMA_Disable(PLASMA_AUTOTUNING);
    PLASMA_Set(PLASMA_TILE_SIZE,        iparam[IPARAM_NB] );
    PLASMA_Set(PLASMA_INNER_BLOCK_SIZE, iparam[IPARAM_IB] );
    /* } else { */
    /*     PLASMA_Get(PLASMA_TILE_SIZE,        &iparam[IPARAM_NB] ); */
    /*     PLASMA_Get(PLASMA_INNER_BLOCK_SIZE, &iparam[IPARAM_IB] ); */
    /* } */

    /* Householder mode */
    if (iparam[IPARAM_RHBLK] < 1) {
        PLASMA_Set(PLASMA_HOUSEHOLDER_MODE, PLASMA_FLAT_HOUSEHOLDER);
    } else {
        PLASMA_Set(PLASMA_HOUSEHOLDER_MODE, PLASMA_TREE_HOUSEHOLDER);
        PLASMA_Set(PLASMA_HOUSEHOLDER_SIZE, iparam[IPARAM_RHBLK]);
    }

    /* Tournament pivoting */
    PLASMA_Set(PLASMA_TNTPIVOTING_MODE, iparam[IPARAM_TNTPIV_MODE]);
    PLASMA_Set(PLASMA_TNTPIVOTING_SIZE, iparam[IPARAM_TNTPIV_SIZE]);

    /* Layout conversion */
    PLASMA_Set(PLASMA_TRANSLATION_MODE, iparam[IPARAM_INPLACE]);

    print_header( argv[0], iparam);

    if (step < 1) step = 1;

    Test( -1, iparam ); /* print header */
    for (i = start; i <= stop; i += step)
    {
        if ( nx > 0 ) {
            iparam[IPARAM_M] = i;
            iparam[IPARAM_N] = max(1, i/nx);
        } else if ( mx > 0 ) {
            iparam[IPARAM_M] = max(1, i/mx);
            iparam[IPARAM_N] = i;
        } else {
            if ( m == -1 )
                iparam[IPARAM_M] = i;
            iparam[IPARAM_N] = i;
        }
        Test( iparam[IPARAM_N], iparam );
    }

    PLASMA_Finalize();

    /* if (gnuplot) { */
    /*         printf( "%s\n%s\n", */
    /*                 "e", */
    /*                 gnuplot > 1 ? "" : "pause 10" ); */
    /* } */

    return EXIT_SUCCESS;
}


#include <gsl/gsl_matrix.h>
#include "cimple_safe_mode.h"

/**
 * Adds a node to the beginning of the list
 */
void push_burn_node(burn_graph_node ** head,
                    abstract_state *state)
{

    burn_graph_node * new_node;
    new_node = malloc(sizeof(burn_graph_node));

    new_node->state = state;
    new_node->next = *head;
    *head = new_node;
};

/**
 * Remove a node from the beginning of the list
 */
void pop_burn_node(burn_graph_node ** head)
{

    burn_graph_node * next_node = NULL;

    next_node = (*head)->next;
    free(*head);
    *head = next_node;

};

/**
 * Delete and free memory of all nodes of a list
 */
void clear_burn_list(burn_graph_node **head)
{
    while (head !=NULL){
        pop_burn_node(head);
    }
};

/**
 * Given a set of abstract states (associated with the transitions going out from a state, where the function was called)
 * it finds the shortest distance to a state with an invariant set.
 */
abstract_state* fastest_burn(abstract_state **transitions,
                             int transitions_count)
{
    //Initialize
    abstract_state* fastest = transitions[0];
    int distance = transitions[0]->distance_invariant_set;

    //Check that there are more than 1 transition to evaluate, else trivial
    if(transitions_count >1){

        //Compare transitions
        for(int i = 1; i<transitions_count; i++){
            if(transitions[i]->distance_invariant_set<distance){
                distance = transitions[i]->distance_invariant_set;
                fastest = transitions[i];
            }
        }

    }

    return fastest;
}

/**
 * Given two polytopes, computes one step backwards from second polytope towards first polytope.
 */
polytope* previous_polytope(polytope *P1,
                            polytope *P2,
                            system_dynamics *s_dyn)
{

    size_t n = s_dyn->A->size2;  // State space dimension;
    size_t m = s_dyn->B->size2;  // Input space dimension;
    polytope *scaled_W_set = polytope_linear_transform(s_dyn->W_set, s_dyn->E); // multiplication: EW

    polytope * robust_P1 = polytope_pontryagin(P1, scaled_W_set);
    polytope * robust_P2 = polytope_pontryagin(P2, scaled_W_set);

    size_t sum_dim = robust_P1->H->size1+robust_P2->H->size1;

    polytope *precedent_polytope = polytope_alloc(sum_dim+s_dyn->U_set->H->size1, n+m);

    // FOR precedent_polytope G
    /*
     *     |P1_G|
     * G = |P2_G|
     *     |  0 |
     */
    gsl_vector_set_zero(precedent_polytope->G);
    gsl_vector_view G_P1 = gsl_vector_subvector(precedent_polytope->G, 0, robust_P1->G->size);
    gsl_vector_memcpy(&G_P1.vector, robust_P1->G);
    gsl_vector_view G_P2 = gsl_vector_subvector(precedent_polytope->G, robust_P1->G->size,robust_P2->G->size);
    gsl_vector_memcpy(&G_P2.vector, robust_P2->G);
//    gsl_vector * P2_HdotK = gsl_vector_alloc(robust_P2->G->size);
//    gsl_blas_dgemv(CblasNoTrans,1.0, robust_P2->H, s_dyn->K, 0.0, P2_HdotK);
//    gsl_vector_sub(&G_P2.vector, P2_HdotK);
//    //Clean up!
//    gsl_vector_free(P2_HdotK);

    // FOR Dist
    /*
     *         |  0   |
     * Dist =  |P2_H.E|
     *         |  0   |
     */
//    gsl_matrix *Dist = gsl_matrix_alloc(sum_dim+s_dyn->U_set->H->size1, p);
//    gsl_matrix_set_zero(Dist);
//    gsl_matrix_view Dist_P2 = gsl_matrix_submatrix(Dist, P1->H->size1, 0, P2->H->size1, Dist->size2);
//    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, P2->H, s_dyn->E, 0.0, &Dist_P2.matrix);

    // FOR precedent_polytope H
    /*
     *  H = |H1   0 |  OR  H = |H1    0 |
     *      |H2A H2B|          |H2A  H2B|
     *      | 0  HU |          |   HU   |
     */
    gsl_matrix_set_zero(precedent_polytope->H);

    gsl_matrix_view H_P1 = gsl_matrix_submatrix(precedent_polytope->H, 0, 0, robust_P1->H->size1, n);
    gsl_matrix_memcpy(&H_P1.matrix, robust_P1->H);
    gsl_matrix_view H_P2_1 = gsl_matrix_submatrix(precedent_polytope->H, robust_P1->H->size1, 0, robust_P2->H->size1, n);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, robust_P2->H, s_dyn->A, 0.0, &H_P2_1.matrix);
    gsl_matrix_view H_P2_2 = gsl_matrix_submatrix(precedent_polytope->H, robust_P1->H->size1, n, robust_P2->H->size1, m);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, robust_P2->H, s_dyn->B, 0.0, &H_P2_2.matrix);

    if (s_dyn->U_set->H->size2 == m){
        gsl_matrix_view HU = gsl_matrix_submatrix(precedent_polytope->H,sum_dim, n, s_dyn->U_set->H->size1,m);
        gsl_matrix_memcpy(&HU.matrix,s_dyn->U_set->H);
    } else if (s_dyn->U_set->H->size2 == m+n){
        // transforms U_set.H from |constraints_ input constraints_state| to |constraints_state constraints_input|
        /*
         * |m m m m n n n|    |n n n m m m m|
         * |m m m m n n n| => |n n n m m m m|
         * |m m m m n n n|    |n n n m m m m|
         */
        gsl_matrix_view HU = gsl_matrix_submatrix(precedent_polytope->H,sum_dim, 0, s_dyn->U_set->H->size1,m+n);
        gsl_matrix * exchange_matrix = gsl_matrix_alloc(n+m,n+m);
        gsl_matrix_set_zero(exchange_matrix);
        gsl_matrix_view eye_m = gsl_matrix_submatrix(exchange_matrix, 0, n, m, m);
        gsl_matrix_set_identity(&eye_m.matrix);
        gsl_matrix_view eye_n = gsl_matrix_submatrix(exchange_matrix, m, 0, n, n);
        gsl_matrix_set_identity(&eye_n.matrix);
        gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0, s_dyn->U_set->H,exchange_matrix, 0.0, &HU.matrix);
    }

    polytope *return_polytope = polytope_projection(precedent_polytope, n);

    //Clean up!
//    gsl_matrix_free(Dist);
//    gsl_vector_free(D_hat);
    polytope_free(precedent_polytope);

    return return_polytope;
};

/**
 * Computes N-1 polytopes the system has to transition to go from origin to target in N time steps
 */
polytope ** compute_path(polytope *origin,
                         polytope *target,
                         system_dynamics *s_dyn,
                         int N)
{

    //Initialize
    polytope **path = malloc(sizeof(polytope) * N);

    //Instead of just pointing path[N] = target, the memory is copied,
    // thus when the target polytope is freed somewhere the path stays complete
    path[N] = polytope_alloc(target->H->size1,target->H->size2);
    gsl_matrix_memcpy(path[N]->H,target->H);

    path[0] = polytope_alloc(origin->H->size1,origin->H->size2);
    gsl_matrix_memcpy(path[0]->H,origin->H);

    //Compute intermediate steps
    for(int j = N-1; j>0;j--){
        path[j]=previous_polytope(path[0], path[j+1], s_dyn);
    }

    return path;
};

polytope *pre_alpha(polytope *R_i,
                    gsl_matrix *A,
                    gsl_matrix *B,
                    polytope *W_set,
                    polytope *U_set,
                    polytope *scaled_unit_cube)
{
    size_t n = A->size2;
    size_t m = B->size2;
    polytope *W_set_scaled = polytope_minkowski(W_set, scaled_unit_cube);
    polytope *R_i_scaled = polytope_pontryagin(R_i, W_set_scaled);

    polytope *P0 = polytope_alloc(R_i_scaled->H->size1+U_set->H->size1, n+m);
    gsl_matrix_set_zero(P0->H);
    gsl_matrix_view H_P2_1 = gsl_matrix_submatrix(P0->H, 0, 0, R_i_scaled->H->size1, n);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, R_i_scaled->H, A, 0.0, &H_P2_1.matrix);
    gsl_matrix_view H_P2_2 = gsl_matrix_submatrix(P0->H, 0, n, R_i_scaled->H->size1, m);
    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, R_i_scaled->H, B, 0.0, &H_P2_2.matrix);
    if (U_set->H->size2 == m){
        gsl_matrix_view HU = gsl_matrix_submatrix(P0->H,R_i_scaled->H->size1, n, U_set->H->size1,m);
        gsl_matrix_memcpy(&HU.matrix,U_set->H);
    } else if (U_set->H->size2 == m+n){
        // transforms U_set.H from |constraints_ input constraints_state| to |constraints_state constraints_input|
        /*
         * |m m m m n n n|    |n n n m m m m|
         * |m m m m n n n| => |n n n m m m m|
         * |m m m m n n n|    |n n n m m m m|
         */
        gsl_matrix_view HU = gsl_matrix_submatrix(P0->H,R_i_scaled->H->size1, 0, U_set->H->size1,m+n);
        gsl_matrix * exchange_matrix = gsl_matrix_alloc(n+m,n+m);
        gsl_matrix_set_zero(exchange_matrix);
        gsl_matrix_view eye_m = gsl_matrix_submatrix(exchange_matrix, 0, n, m, m);
        gsl_matrix_set_identity(&eye_m.matrix);
        gsl_matrix_view eye_n = gsl_matrix_submatrix(exchange_matrix, m, 0, n, n);
        gsl_matrix_set_identity(&eye_n.matrix);
        gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0, U_set->H,exchange_matrix, 0.0, &HU.matrix);
    }
    gsl_vector_set_zero(P0->G);
    gsl_vector_view G_P1 = gsl_vector_subvector(P0->G, 0, R_i_scaled->G->size);
    gsl_vector_memcpy(&G_P1.vector, R_i_scaled->G);
    gsl_vector_view G_P2 = gsl_vector_subvector(P0->G, R_i_scaled->G->size,U_set->G->size);
    gsl_vector_memcpy(&G_P2.vector, U_set->G);

    polytope *pre_R_1 = polytope_projection(P0, n);
    polytope_free(P0);
    return pre_R_1;
}
polytope * compute_invariant_set(polytope* X,
                                 gsl_matrix *A,
                                 gsl_matrix *B,
                                 polytope *W_set,
                                 polytope *U_set,
                                 double alpha)
{
    polytope * scaled_unit_cube = polytope_scaled_unit_cube(alpha, (int)X->H->size2);

    polytope *R_i = polytope_alloc(X->H->size1,X->H->size2);
    gsl_matrix_memcpy(R_i->H,X->H);
    gsl_vector_memcpy(R_i->G,X->G);
    polytope *pre_R_i = pre_alpha(R_i,A,B,W_set,U_set,scaled_unit_cube);
    polytope *R_i_plus = polytope_unite_inequalities(pre_R_i, X);
    dd_ErrorType err = dd_NoError;
    dd_WriteMatrix(stdout,dd_CopyGenerators(polytope_to_cdd(R_i_plus,&err)));

    polytope *R_scaled = polytope_minkowski(R_i_plus, scaled_unit_cube);
    while(!(polytope_is_subset(R_i, R_scaled))){
        polytope_free(pre_R_i);
        polytope_free(R_scaled);
        polytope_free(R_i);
        R_i = R_i_plus;
        pre_R_i = pre_alpha(R_i,A,B,W_set,U_set,scaled_unit_cube);
        R_i_plus = polytope_unite_inequalities(pre_R_i, X);
        R_scaled = polytope_minkowski(R_i_plus, scaled_unit_cube);
    }
    polytope_free(pre_R_i);
    polytope_free(R_i_plus);
    polytope_free(R_scaled);
    return R_i;

};

bool has_transition(abstract_state *state,
                    abstract_state *transition)
{
    bool transition_found = false;
    for(int i = 0; i<state->transitions_in_count; i++){
        if(state->transitions_in[i] == transition){
            transition_found = true;
        }
    }

    return transition_found;
};
/**
 * 1) Runs through all abstract states,
 * 2) checks if invariant set is contained
 * 3) sets path for all cells in those states containing an invariant set if possible
 */
burn_graph_node *set_invariant_sets(discrete_dynamics *d_dyn,
                                    system_dynamics *s_dyn)
{

    //Initialize graph
    burn_graph_node * head_node = NULL;
    head_node = malloc(sizeof(burn_graph_node));
    head_node->state = NULL;
    head_node->next = NULL;
    int N = (int)d_dyn->time_horizon;
    double unit_cube_scale = 1;
    //Create graph by
    // 1) running through all abstract_states and
    // 2) check whether it contains an invariant set
    for(int i = 0; i< d_dyn->abstract_states_count; i++){
        bool found_set = false;
        for(int j=0; j<d_dyn->abstract_states_set[i]->cells_count; j++){
            //Try to compute invariant set
            polytope *invariant_set = compute_invariant_set(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                            s_dyn->A,
                                                            s_dyn->B,
                                                            s_dyn->W_set,
                                                            s_dyn->U_set,
                                                            unit_cube_scale);

            //if invariant set is found
            if(invariant_set != NULL){
                found_set = true;
                //Set invariant_set cell to A cell containing an invariant set
                //(may be more than one invariant set!!)
                d_dyn->abstract_states_set[i]->invariant_set = d_dyn->abstract_states_set[i]->cells[j];
                //Compute path for rest of cell to invariant set (may be just a subset of that polytope)
                d_dyn->abstract_states_set[i]->cells[j]->safe_mode = compute_path(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                                      invariant_set,
                                                                      s_dyn,
                                                                      N);
            }else{
                //Set NULL because later safe_mode==NULL cells will be picked out
                d_dyn->abstract_states_set[i]->cells[j]->safe_mode = NULL;
            }
        }

        //If abstract state contains at least one invariant set it is appended to the graph
        if(found_set){
            //Check whether state has a transition to itself
            if(has_transition(d_dyn->abstract_states_set[i], d_dyn->abstract_states_set[i])){
                //if yes compute the safe_mode path for the cells that do not contain an invariant set
                for(int j = 0; j < d_dyn->abstract_states_set[i]->cells_count; j++){
                    if(d_dyn->abstract_states_set[i]->cells[j]->safe_mode == NULL){
                        d_dyn->abstract_states_set[i]->cells[j]->safe_mode = compute_path(d_dyn->abstract_states_set[i]->cells[j]->polytope_description,
                                                                              d_dyn->abstract_states_set[i]->invariant_set->polytope_description,
                                                                              s_dyn,
                                                                              N);
                    }
                }
            }
            //Append state to graph
            if (head_node->state == NULL) {
                head_node->state = d_dyn->abstract_states_set[i];
            }else{
                push_burn_node(&head_node,d_dyn->abstract_states_set[i]);
            }
        }
    }
    return head_node;

};

/**
 * Gets list of states containing invariant sets.
 * Burns through transition system, setting all other safe mode paths.
 */
//void burning_method(burn_graph_node *seeds,
//                    discrete_dynamics *d_dyn,
//                    system_dynamics *s_dyn)
//{
//
//    //Initialize needed variables
//    bool graph_burnt = false;
//    int N = (int)d_dyn->time_horizon;
//    burn_graph_node *current_burning = seeds;
//    int burning_round = 0;
//
//
//    while(!graph_burnt){
//        burning_round +=1;
//        burn_graph_node *next_burning = current_burning;
//        while (current_burning != NULL) {
//            if(current_burning->state->distance_invariant_set == INFINITY){
//                for(int i = 0; i<current_burning->state->cells_count;i++){
//                    if(current_burning->state->cells[i]->safe_mode == NULL){
//                        current_burning->state->cells[i]->safe_mode = compute_path(current_burning->state->cells[i]->polytope_description,
//                                                                                current_burning->state->next_state->convex_hull,
//                                                                                s_dyn,
//                                                                                N);
//                    }
//
//                }
//                current_burning->state->distance_invariant_set = burning_round;
//            }
//            current_burning = current_burning->next;
//        }
//        while(next_burning != NULL){
//            for(int i = 0;i<next_burning->state->transitions_in_count;i++){
//                if(next_burning->state->transitions_in[i]->distance_invariant_set == NULL){
//                    push_burn_node(&current_burning,next_burning->state->transitions_in[i]);
//                }
//            }
//            next_burning = next_burning->next;
//        }
//        if(current_burning == NULL){
//            graph_burnt = true;
//        }
//    }
//    while (seeds !=NULL){
//        if(seeds->state->next_state != seeds->state){
//            abstract_state* fastest = fastest_burn(seeds->state->transitions_out,seeds->state->transitions_out_count);
//            for(int i = 0; i< seeds->state->cells_count; i++){
//                if(seeds->state->cells[i]->safe_mode == NULL){
//                    seeds->state->cells[i]->safe_mode = compute_path(seeds->state->cells[i]->polytope_description, fastest->convex_hull,s_dyn,N);
//                }
//            }
//        }
//        seeds = seeds->next;
//    }
//};

/**
 * Set the safe_mode path for every cell in every abstract_state:
 * 1) Compute invariant sets
 * 2) Find path towards invariant sets for other abstract_states
 */
//void compute_safe_mode(discrete_dynamics *d_dyn,
//                       system_dynamics *s_dyn)
//{
//
//    burn_graph_node * invariant_sets = set_invariant_sets(d_dyn, s_dyn);
//    burning_method(invariant_sets, d_dyn, s_dyn);
//    free(invariant_sets);
//};

//gsl_vector *one_step_input(polytope *A, polytope *B)
//{
//    gsl_vector *u;
//    return u;
//};

//abstract_state *find_current_state(gsl_vector *x,
//                                   discrete_dynamics *d_dyn)
//{
//    abstract_state *now;
//    return now;
//};
//
//void *apply_safe_mode(current_state *now,
//                      discrete_dynamics *d_dyn,
//                      system_dynamics *s_dyn,
//                      void *arg)
//{
//
//    //find correct cell
//    cell * current_cell = NULL;
//    int N = (int)d_dyn->time_horizon;
//    int found = 0;
//    int i;
//    for(i = 0; i < d_dyn->abstract_states_set[now->current_abs_state]->cells_count; i++){
//        found = polytope_check_state(d_dyn->abstract_states_set[now->current_abs_state]->cells[i]->polytope_description, now->x);
//        if(found){
//            current_cell = d_dyn->abstract_states_set[now->current_abs_state]->cells[i];
//            break;
//        }
//    }
//
//    //if not found check all other abstract_states_set all other cells
//    if(current_cell ==NULL){
//        for(i = 0; i<d_dyn->abstract_states_count; i++){
//            for(int j = 0; j < d_dyn->abstract_states_set[i]->cells_count; j++){
//                found = polytope_check_state(d_dyn->abstract_states_set[i]->cells[j]->polytope_description, now->x);
//                if(found){
//                    now->current_abs_state = i;
//                    current_cell = d_dyn->abstract_states_set[i]->cells[j];
//                }
//            }
//        }
//    }
//    int safe_mode_state = 0;
//    found = 0;
//    for(i = N; i>0; i--){
//        found = polytope_check_state(current_cell->safe_mode[i], now->x);
//        if(found){
//            safe_mode_state = i;
//            break;
//        }
//    }
//    pthread_mutex_t *mx = arg;
//
//    //while ! in invariant set
//    while(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set == NULL && !needQuit(mx)){
//        //find polytope of safe_mode
//        int check = polytope_check_state(current_cell->safe_mode[safe_mode_state], now->x);
//        if(!check){
//            safe_mode_state = 0;
//            found = 0;
//            for(i = N; i>0; i--){
//                found = polytope_check_state(current_cell->safe_mode[i], now->x);
//                if(found){
//                    safe_mode_state = i;
//                    break;
//                }
//            }
//        }
//
//        //linear combination to next polytope in line
//        gsl_vector *u = one_step_input(current_cell->safe_mode[safe_mode_state],
//                                               current_cell->safe_mode[safe_mode_state+1]);
//        gsl_vector *w = gsl_vector_alloc(s_dyn->E->size2);
//        simulate_disturbance(w, 0, 0.01);
//        apply_control(now->x, u, s_dyn->A, s_dyn->B, s_dyn->E, w, (size_t)safe_mode_state);
//
//    }
//
//    //while not interrupted stay in invariant set
//
//    while( !needQuit(mx) ) {
//        int check = polytope_check_state(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description, now->x);
//        if(!check){
//            safe_mode_state = 0;
//            found = 0;
//            for(i = N; i>0; i--){
//                found = polytope_check_state(current_cell->safe_mode[i], now->x);
//                if(found){
//                    safe_mode_state = i;
//                    break;
//                }
//            }
//        }
//        one_step_input(d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description, d_dyn->abstract_states_set[now->current_abs_state]->invariant_set->polytope_description);
//    }
//    return NULL;
//};


/*
  NAME:
     proj_EM_step
  PURPOSE:
     one proj_EM step
  CALLING SEQUENCE:
     proj_EM_step(struct datapoint * data, int N, struct gaussian * gaussians,
     int K,bool * fixamp, bool * fixmean, bool * fixcovar,
     double * avgloglikedata, bool likeonly, double w, bool noproj,
     bool diagerrs, bool noweight)
  INPUT:
     data         - the data
     N            - number of data points
     gaussians    - model gaussians
     K            - number of model gaussians
     fixamp       - fix the amplitude?
     fixmean      - fix the mean?
     fixcovar     - fix the covar?
     likeonly     - only compute likelihood?
     w            - regularization parameter
     noproj       - don't perform any projections
     diagerrs     - the data->SS errors-squared are diagonal
     noweight     - don't use data-weights
  OUTPUT:
     avgloglikedata - average loglikelihood of the data
  REVISION HISTORY:
     2008-09-21 - Written Bovy
     2010-03-01 Added noproj option - Bovy
     2010-04-01 Added noweight option - Bovy
*/
#ifdef _OPENMP
#include <omp.h>
#endif

#include <math.h>
//#include <time.h>
//#include <sys/time.h>
#include <float.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <proj_gauss_mixtures.h>

#define CHUNKSIZE 1

void proj_EM_step(struct datapoint * data, int N,
		  struct gaussian * gaussians, int K,bool * fixamp,
		  bool * fixmean, bool * fixcovar, double * avgloglikedata,
		  bool likeonly, double w, bool noproj, bool diagerrs,
		  bool noweight){
  *avgloglikedata = 0.0;
  //struct timeval start,time1, time2, time3, time4, time5,end;
  struct datapoint * thisdata;
  struct gaussian * thisgaussian;
  struct gaussian * thisnewgaussian;
  int signum,di;
  double exponent;
  double currqij;
  struct modelbs * thisbs;
  int d = (gaussians->VV)->size1;//dim of mm

  //gettimeofday(&start,NULL);
  //Initialize new parameters
  int kk;
  for (kk=0; kk != K*nthreads; ++kk){
    newgaussians->alpha = 0.0;
    gsl_vector_set_zero(newgaussians->mm);
    gsl_matrix_set_zero(newgaussians->VV);
    ++newgaussians;
  }
  newgaussians= startnewgaussians;

  //gettimeofday(&time1,NULL);
  //check whether for some Gaussians none of the parameters get updated
  double sumfixedamps= 0;
  bool * allfixed = (bool *) calloc(K, sizeof (bool) );
  double ampnorm;
  for (kk=0; kk != K; ++kk){
    if (*fixamp == true){
      sumfixedamps += gaussians->alpha;
    }
    ++gaussians;
    if (*fixamp == true && *fixmean == true && *fixcovar == true)
      *allfixed= true;
    ++allfixed;
    ++fixamp;
    ++fixmean;
    ++fixcovar;
  }
  gaussians -= K;
  allfixed -= K;
  fixamp -= K;
  fixmean -= K;
  fixcovar -= K;

  //gettimeofday(&time2,NULL);

  //now loop over data and gaussians to update the model parameters
  int ii, jj, ll;
  double sumSV;
  int chunk;
  chunk= CHUNKSIZE;
#pragma omp parallel for schedule(static,chunk) \
  private(tid,di,signum,exponent,ii,jj,ll,kk,Tij,Tij_inv,wminusRm,p,VRTTinv,sumSV,VRT,TinvwminusRm,Rtrans,thisgaussian,thisdata,thisbs,thisnewgaussian,currqij) \
  shared(newgaussians,gaussians,bs,allfixed,K,d,data,avgloglikedata)
  for (ii = 0 ; ii < N; ++ii){
    thisdata= data+ii;
#ifdef _OPENMP
    tid= omp_get_thread_num();
#else
    tid = 0;
#endif
    di = (thisdata->SS)->size1;
    //printf("Datapoint has dimension %i\n",di);
    p = gsl_permutation_alloc (di);
    wminusRm = gsl_vector_alloc (di);
    TinvwminusRm = gsl_vector_alloc (di);
    Tij = gsl_matrix_alloc(di,di);
    Tij_inv = gsl_matrix_alloc(di,di);
    if ( ! noproj ) VRT = gsl_matrix_alloc(d,di);
    VRTTinv = gsl_matrix_alloc(d,di);
    if ( ! noproj ) Rtrans = gsl_matrix_alloc(d,di);
    for (jj = 0; jj != K; ++jj){
      //printf("%i,%i\n",(thisdata->ww)->size,wminusRm->size);
      gsl_vector_memcpy(wminusRm,thisdata->ww);
      //fprintf(stdout,"Where is the seg fault?\n");
      thisgaussian= gaussians+jj;
      //prepare...
      if ( ! noproj ) {
	if ( diagerrs ) {
	  gsl_matrix_set_zero(Tij);
	  for (ll = 0; ll != di; ++ll)
	    gsl_matrix_set(Tij,ll,ll,gsl_matrix_get(thisdata->SS,ll,0));}
	else
	  gsl_matrix_memcpy(Tij,thisdata->SS);
      }
      //Calculate Tij
      if ( ! noproj ) {
	gsl_matrix_transpose_memcpy(Rtrans,thisdata->RR);
	gsl_blas_dsymm(CblasLeft,CblasUpper,1.0,thisgaussian->VV,Rtrans,0.0,VRT);//Only the upper right part of VV is calculated --> use only that part
	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,thisdata->RR,VRT,1.0,Tij);}//This is Tij
      else {
	if ( diagerrs ) {
	  for (kk = 0; kk != d; ++kk){
	    gsl_matrix_set(Tij,kk,kk,gsl_matrix_get(thisdata->SS,kk,0)+gsl_matrix_get(thisgaussian->VV,kk,kk));
	    for (ll = kk+1; ll != d; ++ll){
	      sumSV= gsl_matrix_get(thisgaussian->VV,kk,ll);
	      gsl_matrix_set(Tij,kk,ll,sumSV);
	      gsl_matrix_set(Tij,ll,kk,sumSV);}}}
	else {
	  for (kk = 0; kk != d; ++kk){
	    gsl_matrix_set(Tij,kk,kk,gsl_matrix_get(thisdata->SS,kk,kk)+gsl_matrix_get(thisgaussian->VV,kk,kk));
	    for (ll = kk+1; ll != d; ++ll){
	      sumSV= gsl_matrix_get(thisdata->SS,kk,ll)+gsl_matrix_get(thisgaussian->VV,kk,ll);
	      gsl_matrix_set(Tij,kk,ll,sumSV);
	      gsl_matrix_set(Tij,ll,kk,sumSV);}}}}
      //gsl_matrix_add(Tij,thisgaussian->VV);}
      //Calculate LU decomp of Tij and Tij inverse
      gsl_linalg_LU_decomp(Tij,p,&signum);
      gsl_linalg_LU_invert(Tij,p,Tij_inv);
      //Calculate Tijinv*(w-Rm)
      if ( ! noproj ) gsl_blas_dgemv(CblasNoTrans,-1.0,thisdata->RR,thisgaussian->mm,1.0,wminusRm);
      else gsl_vector_sub(wminusRm,thisgaussian->mm);
      //printf("wminusRm = %f\t%f\n",gsl_vector_get(wminusRm,0),gsl_vector_get(wminusRm,1));
      gsl_blas_dsymv(CblasUpper,1.0,Tij_inv,wminusRm,0.0,TinvwminusRm);
      //printf("TinvwminusRm = %f\t%f\n",gsl_vector_get(TinvwminusRm,0),gsl_vector_get(TinvwminusRm,1));
      gsl_blas_ddot(wminusRm,TinvwminusRm,&exponent);
      //printf("Exponent = %f\nDet = %f\n",exponent,gsl_linalg_LU_det(Tij,signum));
      gsl_matrix_set(qij,ii,jj,log(thisgaussian->alpha) - di * halflogtwopi - 0.5 * gsl_linalg_LU_lndet(Tij) -0.5 * exponent);//This is actually the log of qij
      //printf("Here we have = %f\n",gsl_matrix_get(qij,ii,jj));
      //Now calculate bij and Bij
      thisbs= bs+tid*K+jj;
      gsl_vector_memcpy(thisbs->bbij,thisgaussian->mm);
      //printf("%i,%i,%i\n",tid,ii,jj);
      if ( ! noproj ) gsl_blas_dgemv(CblasNoTrans,1.0,VRT,TinvwminusRm,1.0,thisbs->bbij);
      else gsl_blas_dsymv(CblasUpper,1.0,thisgaussian->VV,TinvwminusRm,1.0,thisbs->bbij);
      //printf("bij = %f\t%f\n",gsl_vector_get(bs->bbij,0),gsl_vector_get(bs->bbij,1));
      gsl_matrix_memcpy(thisbs->BBij,thisgaussian->VV);
      if ( ! noproj ) {
	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,VRT,Tij_inv,0.0,VRTTinv);
	gsl_blas_dgemm(CblasNoTrans,CblasTrans,-1.0,VRTTinv,VRT,1.0,thisbs->BBij);}
      else {
	gsl_blas_dsymm(CblasLeft,CblasUpper,1.0,thisgaussian->VV,Tij_inv,0.0,VRTTinv);
	gsl_blas_dsymm(CblasRight,CblasUpper,-1.0,thisgaussian->VV,VRTTinv,1.0,thisbs->BBij);}
      gsl_blas_dsyr(CblasUpper,1.0,thisbs->bbij,thisbs->BBij);//This is bijbijT + Bij, which is the relevant quantity
      }
    gsl_permutation_free (p);
    gsl_vector_free(wminusRm);
    gsl_vector_free(TinvwminusRm);
    gsl_matrix_free(Tij);
    gsl_matrix_free(Tij_inv);
    if ( ! noproj ) gsl_matrix_free(VRT);
    gsl_matrix_free(VRTTinv);
    if ( ! noproj ) gsl_matrix_free(Rtrans);
    //Again loop over the gaussians to update the model(can this be more efficient? in any case this is not so bad since generally K << N)
#pragma omp critical
    {
      //Normalize qij properly
      *avgloglikedata += normalize_row(qij,ii,true,noweight,thisdata->logweight);
    }
      //printf("qij = %f\t%f\n",gsl_matrix_get(qij,ii,0),gsl_matrix_get(qij,ii,1));
      //printf("avgloglgge = %f\n",*avgloglikedata);
      for (jj = 0; jj != K; ++jj){
	currqij = exp(gsl_matrix_get(qij,ii,jj));
	//printf("Current qij = %f\n",currqij);
	thisbs= bs+tid*K+jj;
	thisnewgaussian= newgaussians+tid*K+jj;
	gsl_vector_scale(thisbs->bbij,currqij);
	gsl_vector_add(thisnewgaussian->mm,thisbs->bbij);
	gsl_matrix_scale(thisbs->BBij,currqij);
	gsl_matrix_add(thisnewgaussian->VV,thisbs->BBij);
	//printf("bij = %f\t%f\n",gsl_vector_get(bs->bbij,0),gsl_vector_get(bs->bbij,1));
	//printf("Bij = %f\t%f\t%f\n",gsl_matrix_get(bs->BBij,0,0),gsl_matrix_get(bs->BBij,1,1),gsl_matrix_get(bs->BBij,0,1));
      }
  }
  *avgloglikedata /= N;
  if (likeonly)
    return;
  //gettimeofday(&time3,NULL);

  //gather newgaussians
  if ( nthreads != 1 )
#pragma omp parallel for schedule(static,chunk) \
  private(ll,jj)
    for (jj = 0; jj < K; ++jj)
      for (ll = 1; ll != nthreads; ++ll) {
	gsl_vector_add((newgaussians+jj)->mm,(newgaussians+ll*K+jj)->mm);
	gsl_matrix_add((newgaussians+jj)->VV,(newgaussians+ll*K+jj)->VV);
      }

  //gettimeofday(&time4,NULL);

  //Now update the parameters
  //Thus, loop over gaussians again!
  double qj;
#pragma omp parallel for schedule(dynamic,chunk) \
  private(jj,qj)
  for (jj = 0; jj < K; ++jj){
    if (*(allfixed+jj)){
      continue;
    }
    else {
      qj = exp(logsum(qij,jj,false));
      (qj < DBL_MIN) ? qj = 0: 0;
      //printf("qj = %f\n",qj);
      if (*(fixamp+jj) != true) {
	(gaussians+jj)->alpha = qj;
	if (qj == 0) {//rethink this
	  *(fixamp+jj)=1;
	  *(fixmean+jj)=1;
	  *(fixcovar+jj)=1;
	  continue;
	}
      }
      gsl_vector_scale((newgaussians+jj)->mm,1.0/qj);
      if (*(fixmean+jj) != true){
	gsl_vector_memcpy((gaussians+jj)->mm,(newgaussians+jj)->mm);
    }
      if (*(fixcovar+jj) != true){
	//	if (*(fixmean+jj) != true)
	//  gsl_blas_dsyr(CblasUpper,-qj,(gaussians+jj)->mm,(newgaussians+jj)->VV);
	//else {
	gsl_blas_dsyr(CblasUpper,qj,(gaussians+jj)->mm,(newgaussians+jj)->VV);
	gsl_blas_dsyr2(CblasUpper,-qj,(gaussians+jj)->mm,(newgaussians+jj)->mm,(newgaussians+jj)->VV);
	//}
      if (w > 0.){
	gsl_matrix_add((newgaussians+jj)->VV,I);
	gsl_matrix_scale((newgaussians+jj)->VV,1.0/(qj+1.0));
      }
      else gsl_matrix_scale((newgaussians+jj)->VV,1.0/qj);
      gsl_matrix_memcpy((gaussians+jj)->VV,(newgaussians+jj)->VV);
      }
    }
  }
  //gettimeofday(&time5,NULL);

  //normalize the amplitudes
  if ( sumfixedamps == 0. && noweight ){
    for (kk=0; kk != K; ++kk){
      if ( noweight ) (gaussians++)->alpha /= (double) N;
    }
  }
  else {
    ampnorm= 0;
    for (kk=0; kk != K; ++kk){
      if (*(fixamp++) == false) ampnorm += gaussians->alpha;
      ++gaussians;
    }
    fixamp -= K;
    gaussians -= K;
    for (kk=0; kk != K; ++kk){
      if (*(fixamp++) == false){
	gaussians->alpha /= ampnorm;
	gaussians->alpha *= (1. - sumfixedamps);
      }
      ++gaussians;
    }
    fixamp -= K;
    gaussians -= K;
  }
  //gettimeofday(&end,NULL);
  //double diff, diff1, diff2, diff3, diff4, diff5,diff6;
  //diff= difftime (end.tv_sec,start.tv_sec)+difftime (end.tv_usec,start.tv_usec)/1000000;
  //diff1= (difftime(time1.tv_sec,start.tv_sec)+difftime(time1.tv_usec,start.tv_usec)/1000000)/diff;
  //diff2= (difftime(time2.tv_sec,time1.tv_sec)+difftime(time2.tv_usec,time1.tv_usec)/1000000)/diff;
  //diff3= (difftime(time3.tv_sec,time2.tv_sec)+difftime(time3.tv_usec,time2.tv_usec)/1000000)/diff;
  //diff4= (difftime(time4.tv_sec,time3.tv_sec)+difftime(time4.tv_usec,time3.tv_usec)/1000000)/diff;
  //diff5= (difftime(time5.tv_sec,time4.tv_sec)+difftime(time5.tv_usec,time4.tv_usec)/1000000)/diff;
  //diff6= (difftime(end.tv_sec,time5.tv_sec)+difftime(end.tv_usec,time5.tv_usec)/1000000)/diff;
  //printf("%f,%f,%f,%f,%f,%f,%f\n",diff,diff1,diff2,diff3,diff4,diff5,diff6);

  free(allfixed);

  return;
}


/*
 *  mvdens.h
 *  likely
 *
 *  Created by Karim Benabed on 10/03/08.
 *  Copyright 2008 __MyCompanyName__. All rights reserved.
 *
 */

#ifndef __MVDENS_H
#define __MVDENS_H
#ifdef __PLANCK__

#include "HL2_likely/tools/errorlist.h"
#include "HL2_likely/tools/io.h"
#include "HL2_likely/tools/maths_base.h"

#else

#include "errorlist.h"
#include "io.h"
#include "maths_base.h"

#endif

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_vector_int.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_gamma.h>

/*#ifdef HAS_LAPACK
#ifdef HAS_MKL
#include "mkl_blas.h"
#include "mkl_lapack.h"
#else
#include "blas.h"
#include "lapack.h"
#endif
#endif
*/

#define __MVDENS_PARANOID_DEBUG__

/* errors */
#define mv_base       -700
#define mv_allocate   -1 + mv_base
#define mv_serialize  -2 + mv_base
#define mv_outOfBound -3 + mv_base
#define mv_badComm    -4 + mv_base
#define mv_negWeight  -5 + mv_base
#define mv_cholesky   -6 + mv_base
#define mv_negative   -7 + mv_base
#define mv_undef      -8 + mv_base
#define mv_file       -9 + mv_base
#define mv_io        -10 + mv_base
#define mv_tooManySteps -11 + mv_base
#define mv_dimension -12 + mv_base
#define mv_type      -13 + mv_base
#define mv_negHatCl  -14 + mv_base

#define PI         3.141592653589793
#define LOGSQRT2PI 0.918938533204673
#ifndef DYNMAX
#define DYNMAX 500.0
#endif
#define mv_INF 1e99

#define mv_EXP  2
#define mv_NORM 1
#define mv_UNORM 0

#define MVT_DF 3

typedef struct {
  size_t ndim;
  void *buf;
  int own_buf;
  double *mean;
  double *std;
  double *x_tmp;
  gsl_vector_view  mean_view_container;
  gsl_vector *mean_view;
  gsl_vector_view  x_tmp_view_container;
  gsl_vector *x_tmp_view;
  gsl_matrix_view  std_view_container;
  gsl_matrix  *std_view;
  size_t band_limit;
  int chol;
  int df;
  double detL;   /* Determinant of L, where L^t L = Covariance matrix */
} mvdens;

typedef struct {
  size_t ncomp, ndim;
  int init_cwght;
  double *wght, *cwght;
  gsl_vector_view wght_view_container, cwght_view_container;
  gsl_vector *wght_view, *cwght_view;
  mvdens **comp;
  void *buf_comp;
} mix_mvdens;

// init and stuff
mvdens * mvdens_alloc(size_t ndim, error **err);
mvdens * mvdens_init(mvdens *g, size_t nndim, double* mn, double* std, error **err);
void mvdens_empty(mvdens *g);
void mvdens_free(mvdens ** g);
void mvdens_free_void(void **g);
void mvdens_print(FILE* where, mvdens* what);
mvdens *mvdens_dwnp(FILE* where, error **err);
mvdens *mvdens_read_and_chol(FILE *where, error **err);
void mvdens_dump(FILE* where, mvdens* what);
void mvdens_chdump(const char *name, mvdens* what, error **err);
void mvdens_set_band_limit(mvdens *self, size_t value);
void mvdens_from_meanvar(mvdens *m, const double *pmean, const double *pvar, double correct);

//computations
double determinant(const double *L, size_t ndim);
void mvdens_cholesky_decomp(mvdens* self, error **err);
double* mvdens_ran(double* dest, mvdens * g, gsl_rng * r,error **err);
double scalar_product(mvdens *g, const double *x, error **err);
double mvdens_log_pdf(mvdens *g, const double * x, error ** err);
double mvdens_log_pdf_void(void *g, const double *x, error **err);
double mvdens_inverse(mvdens *m, error **err);

/* mix_mvdens functions */
mix_mvdens *mix_mvdens_alloc(size_t ncomp, size_t size,error **err);
void mix_mvdens_copy(mix_mvdens *target, const mix_mvdens *source, error **err);
void mix_mvdens_free_void(void **m);
void mix_mvdens_free(mix_mvdens **m);
void mix_mvdens_print(FILE* where,mix_mvdens* what);
void mix_mvdens_dump(FILE* where,mix_mvdens* what);
mix_mvdens * mix_mvdens_dwnp(FILE* where,error **err);
void mix_mvdens_set_band_limit(mix_mvdens *self, size_t value);
double effective_number_of_components(const mix_mvdens *self, error **err);

size_t mix_mvdens_size(size_t ncomp,size_t ndim);
void* serialize_mix_mvdens(mix_mvdens* self, size_t *sz,error** err);
mix_mvdens* deserialize_mix_mvdens(void* serialized, size_t sz,error **err);

void mix_mvdens_cholesky_decomp(mix_mvdens* self, error **err);
double* mix_mvdens_ran(double* dest,size_t *index, mix_mvdens *m, gsl_rng * r, error **err);
size_t ranindx(double *cw, size_t nE, gsl_rng * r, error ** err);
double mix_mvdens_log_pdf(mix_mvdens *m, const double *x, error **err);
double mix_mvdens_log_pdf_void(void *m, const double *x, error **err);

#define MALLOC_IF_NEEDED(ret,dest,size,err) { \
  if (dest==NULL) { \
    ret= (double*) malloc_err(size,err); \
  } else { \
    ret=dest; \
  } \
}

#ifdef HAS_HDF5
#include "hdf5.h"
#define hdf5_base mv_io
void mvdens_hdfdump_infile(mvdens* mv,hid_t loc_id,error **err);
void mvdens_hdfdump(mvdens* mv,char *fname,error **err);
void mix_mvdens_hdfdump(mix_mvdens* mmv,char *fname,error **err);
mvdens* mvdens_hdfdwnp_infile(mvdens* mv,hid_t loc_id,error **err);
mvdens* mvdens_hdfdwnp(char *fname,error **err);
mix_mvdens* mix_mvdens_hdfdwnp(char *fname,error **err);
#endif

#endif


/* permutation/test.c
 *
 * Copyright (C) 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 <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_test.h>

unsigned int p5[120][5] = {
  {0, 1, 2, 3, 4}, {0, 1, 2, 4, 3}, {0, 1, 3, 2, 4}, {0, 1, 3, 4, 2},
  {0, 1, 4, 2, 3}, {0, 1, 4, 3, 2}, {0, 2, 1, 3, 4}, {0, 2, 1, 4, 3},
  {0, 2, 3, 1, 4}, {0, 2, 3, 4, 1}, {0, 2, 4, 1, 3}, {0, 2, 4, 3, 1},
  {0, 3, 1, 2, 4}, {0, 3, 1, 4, 2}, {0, 3, 2, 1, 4}, {0, 3, 2, 4, 1},
  {0, 3, 4, 1, 2}, {0, 3, 4, 2, 1}, {0, 4, 1, 2, 3}, {0, 4, 1, 3, 2},
  {0, 4, 2, 1, 3}, {0, 4, 2, 3, 1}, {0, 4, 3, 1, 2}, {0, 4, 3, 2, 1},
  {1, 0, 2, 3, 4}, {1, 0, 2, 4, 3}, {1, 0, 3, 2, 4}, {1, 0, 3, 4, 2},
  {1, 0, 4, 2, 3}, {1, 0, 4, 3, 2}, {1, 2, 0, 3, 4}, {1, 2, 0, 4, 3},
  {1, 2, 3, 0, 4}, {1, 2, 3, 4, 0}, {1, 2, 4, 0, 3}, {1, 2, 4, 3, 0},
  {1, 3, 0, 2, 4}, {1, 3, 0, 4, 2}, {1, 3, 2, 0, 4}, {1, 3, 2, 4, 0},
  {1, 3, 4, 0, 2}, {1, 3, 4, 2, 0}, {1, 4, 0, 2, 3}, {1, 4, 0, 3, 2},
  {1, 4, 2, 0, 3}, {1, 4, 2, 3, 0}, {1, 4, 3, 0, 2}, {1, 4, 3, 2, 0},
  {2, 0, 1, 3, 4}, {2, 0, 1, 4, 3}, {2, 0, 3, 1, 4}, {2, 0, 3, 4, 1},
  {2, 0, 4, 1, 3}, {2, 0, 4, 3, 1}, {2, 1, 0, 3, 4}, {2, 1, 0, 4, 3},
  {2, 1, 3, 0, 4}, {2, 1, 3, 4, 0}, {2, 1, 4, 0, 3}, {2, 1, 4, 3, 0},
  {2, 3, 0, 1, 4}, {2, 3, 0, 4, 1}, {2, 3, 1, 0, 4}, {2, 3, 1, 4, 0},
  {2, 3, 4, 0, 1}, {2, 3, 4, 1, 0}, {2, 4, 0, 1, 3}, {2, 4, 0, 3, 1},
  {2, 4, 1, 0, 3}, {2, 4, 1, 3, 0}, {2, 4, 3, 0, 1}, {2, 4, 3, 1, 0},
  {3, 0, 1, 2, 4}, {3, 0, 1, 4, 2}, {3, 0, 2, 1, 4}, {3, 0, 2, 4, 1},
  {3, 0, 4, 1, 2}, {3, 0, 4, 2, 1}, {3, 1, 0, 2, 4}, {3, 1, 0, 4, 2},
  {3, 1, 2, 0, 4}, {3, 1, 2, 4, 0}, {3, 1, 4, 0, 2}, {3, 1, 4, 2, 0},
  {3, 2, 0, 1, 4}, {3, 2, 0, 4, 1}, {3, 2, 1, 0, 4}, {3, 2, 1, 4, 0},
  {3, 2, 4, 0, 1}, {3, 2, 4, 1, 0}, {3, 4, 0, 1, 2}, {3, 4, 0, 2, 1},
  {3, 4, 1, 0, 2}, {3, 4, 1, 2, 0}, {3, 4, 2, 0, 1}, {3, 4, 2, 1, 0},
  {4, 0, 1, 2, 3}, {4, 0, 1, 3, 2}, {4, 0, 2, 1, 3}, {4, 0, 2, 3, 1},
  {4, 0, 3, 1, 2}, {4, 0, 3, 2, 1}, {4, 1, 0, 2, 3}, {4, 1, 0, 3, 2},
  {4, 1, 2, 0, 3}, {4, 1, 2, 3, 0}, {4, 1, 3, 0, 2}, {4, 1, 3, 2, 0},
  {4, 2, 0, 1, 3}, {4, 2, 0, 3, 1}, {4, 2, 1, 0, 3}, {4, 2, 1, 3, 0},
  {4, 2, 3, 0, 1}, {4, 2, 3, 1, 0}, {4, 3, 0, 1, 2}, {4, 3, 0, 2, 1},
  {4, 3, 1, 0, 2}, {4, 3, 1, 2, 0}, {4, 3, 2, 0, 1}, {4, 3, 2, 1, 0}
} ;

int
main (void)
{
  int i = 0, j, status = 0;
  gsl_permutation * p ;

  p = gsl_permutation_alloc (5);

  gsl_permutation_init (p);

  do
    {
      for (j = 0; j < 5; j++)
        {
          status |= (p->data[j] != p5[i][j]);
        }

      i++;
    }
  while (gsl_permutation_next(p) == GSL_SUCCESS);

  gsl_test(status, "gsl_permutation_next, 5-th order permutation, 120 steps");

  do
    {
      i--;

      for (j = 0; j < 5; j++)
        {
          status |= (p->data[j] != p5[i][j]);
        }
    }
  while (gsl_permutation_prev(p) == GSL_SUCCESS);

  gsl_test(status, "gsl_permutation_prev, 5-th order permutation, 120 steps");

  gsl_permutation_free (p);

  exit (gsl_test_summary());
}


#include <pthread.h>
#include <stdlib.h>
#include <string.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

// MAX and MIN aren't defined in the standard library. O:
#define MAX(x, y) (((x) > (y)) ? (x) : (y))
#define MIN(x, y) (((x) < (y)) ? (x) : (y))

typedef struct PopulationParametersStruct {
    int n; ///< Population size, used for determining the number of generations between events.
    float f; ///< Scaling factor for the total mutation rate. Smaller = shorter time to coalescence.
    float c; ///< Constant bias for the upward mutation rate.
    float d; ///< Linear bias for the downward mutation rate.
    int kappa; ///< Lower bound of repeat lengths.
    int omega; ///< Upper bound of repeat lengths.
} PopulationParameters;

typedef struct CoalescentStruct {
    PopulationParameters theta; ///< Mutation model parameters.
    int *coalescent_tree; ///< Pointer to our tree, stored as an array.
    int offset; ///< We generalize to include 1+ ancestors. Determine the offset for array representation of tree.
    gsl_rng *r; ///< Pointer to our RNG. This is preserved across the trace and evolve steps.
} PopulationTree;

int _triangle (int a) { return (int) (a * (a + 1) / 2.0); }
int _round_num (int a) { return (a < 0) ? (int) (a - 0.5) : (int) (a + 0.5); }
typedef int (*_mutate_f) (int, int, float, float, int, int, const gsl_rng *);

int _mutate_generation (int t, int ell, float c, float d, int kappa, int omega, const gsl_rng *r) {
    for (int k = 0; k < t; k++) {
        // Compute our upward mutation rate. We are bounded by omega.
        ell = (gsl_ran_flat(r, 0, 1) < c) ? MIN(omega, ell + 1) : ell;

        // Compute our downward mutation rate. We are bounded by kappa.
        ell = (gsl_ran_flat(r, 0, 1) < ell * d) ? MAX(kappa, ell - 1) : ell;
    }

    // Return the result of mutating over several generations.
    return ell;
}

int _mutate_draw (int t, int ell, float c, float d, int kappa, int omega, const gsl_rng *r) {
    return MIN((unsigned) omega, MAX((unsigned) kappa, ell + gsl_ran_poisson(r, t * c) -
                                                       gsl_ran_poisson(r, t * ell * d)));
}

void _trace_tree (int *coalescent_tree, int n, const gsl_rng *r) {
    for (int tau = 0; tau < 2 * n - 1; tau++) { // Diploid!
        int tau_0 = _triangle(tau + 0), tau_1 = _triangle(tau + 1); // Start at 2nd coalescence.

        // We save the indices of our ancestors to our descendants.
        for (int k = 0; k < tau_1 - tau_0; k++) {
        	coalescent_tree[tau_1 + k + 1] = tau_0 + k;
        }

        // Determine the individual whose frequency increases.
        gsl_ran_choose(r, &coalescent_tree[tau_1], 1, &coalescent_tree[tau_1 + 1], tau_1 - tau_0, sizeof(int));

        // Finally, shuffle the individuals in this generation.
        gsl_ran_shuffle(r, &coalescent_tree[tau_1], tau_1 - tau_0 + 1, sizeof(int));
    }
}

void _evolve_individual (int k, int *descendants, int t_coalescence, _mutate_f mutate, PopulationTree *p) {
    int *descendant_to_evolve = &(p->coalescent_tree[descendants[k]]);

    // Evolve each ancestor according to the average time to coalescence and the scaling factor f.
    *descendant_to_evolve = (*mutate)(t_coalescence, *descendant_to_evolve, p->theta.c, p->theta.d,
                                      p->theta.kappa, p->theta.omega, p->r);

    // Save our descendant state.
    descendants[k] = *descendant_to_evolve;
}

/**
 * We assumed our tree has been traced. Given the population tree structure and a pointer to a mutation function,
 * determine the repeat length of all individuals in our tree. We do so by iterating through each coalescent event.
 */
void _evolve_event (PopulationTree *p, _mutate_f mutate) {
    int descendants_size, expected_time, t_coalescence;
    int *descendants = NULL;

    for (int tau = p->offset; tau < 2 * p->theta.n - 1; tau++) {
        // We define our descendants for the tau'th coalescent.
        descendants = &(p->coalescent_tree[_triangle(tau + 1)]);
        descendants_size = _triangle(tau + 2) - _triangle(tau + 1);

        // Determine time to coalescence. This is exponentially distributed, but the mean stays the same. Scale by f.
        expected_time = (int) (p->theta.f * 2 * p->theta.n / (float) _triangle(tau + 1));
        t_coalescence = MAX(1, _round_num(gsl_ran_exponential(p->r, expected_time)));

        // Iterate through each of the descendants (currently indices) and determine each ancestor.
        for (int k = 0; k < descendants_size; k++) {
            _evolve_individual(k, descendants, t_coalescence, mutate, p);
        }
    }
}

void _evolve (int *i_0, int i_0_size, PopulationTree *p) {
    // Determine our offset, and seed our ancestors for the tree.
    p->offset = i_0_size - 1;
    for (int k = 0; k < i_0_size; k++) {
        p->coalescent_tree[k + _triangle(p->offset)] = i_0[k];
    }

    // From our common ancestors, descend forward in time and populate our tree with repeat lengths.
    _evolve_event(p, _mutate_draw);
}

void _cleanup (PopulationTree *p) {
    free(p->coalescent_tree);
    gsl_rng_free(p->r);
}


/**
 * @file      EigenAdaptors.h
 *
 * @brief     This header contains adaptor functions for Eigen types
 *
 * @author    Stefan Reinhold
 * @copyright Copyright (C) 2018 Stefan Reinhold  -- All Rights Reserved.
 *            You may use, distribute and modify this code under the terms of
 *            the AFL 3.0 license; see LICENSE for full license details.
 */

#pragma once

#include <Eigen/Core>
#include <gsl/gsl>

#include <array>

namespace CortidQCT {
namespace Internal {

namespace Adaptor {

namespace Detail_ {

template <class T, class I>
constexpr inline auto subscript(T &&x,
                                I i) noexcept(noexcept(std::declval<T>()[i])) {
  return x[i];
}

template <class Derived, class I>
constexpr inline auto
subscript(Eigen::MatrixBase<Derived> const &x,
          I i) noexcept(noexcept(x(std::declval<Eigen::Index>()))) {
  return x(gsl::narrow_cast<Eigen::Index>(i));
}

template <class Derived, class I>
constexpr inline auto
subscript(Eigen::MatrixBase<Derived> &&x,
          I i) noexcept(noexcept(x(std::declval<Eigen::Index>()))) {
  return x(gsl::narrow_cast<Eigen::Index>(i));
}

template <class T, std::size_t N> struct ConstructionHelper {
  template <class P, class... Args>
  inline constexpr T operator()(P &&param, Args &&... args) const
      noexcept(noexcept(ConstructionHelper<T, N - 1>{}(
          std::forward<P>(param), subscript(param, N - 1),
          std::forward<Args>(args)...))) {
    return ConstructionHelper<T, N - 1>{}(std::forward<P>(param),
                                          subscript(param, N - 1),
                                          std::forward<Args>(args)...);
  }
};

template <class T> struct ConstructionHelper<T, 0> {
  template <class P, class... Args>
  inline constexpr T operator()(P &&, Args &&... args) const
      noexcept(noexcept(T{std::forward<Args>(args)...})) {
    return T{std::forward<Args>(args)...};
  }
};

template <class T, std::size_t N>
struct ConstructionHelper<std::array<T, N>, 0> {
  template <class P, class... Args>
  inline constexpr std::array<T, N> operator()(P &&, Args &&... args) const
      noexcept(noexcept(std::array<T, N>{{std::forward<Args>(args)...}})) {
    return std::array<T, N>{{std::forward<Args>(args)...}};
  }
};

} // namespace Detail_

/// Converts an std::array based vector into an Eigen vector
template <class T, std::size_t N>
inline Eigen::Matrix<T, static_cast<Eigen::Index>(N), 1>
vec(std::array<T, N> const &arr) {
  using Result = Eigen::Matrix<T, static_cast<Eigen::Index>(N), 1>;
  return Detail_::ConstructionHelper<Result, N>{}(arr);
}

/// Converts an Eigen vector to a std::array based one
template <class Derived>
inline std::array<typename Derived::Scalar,
                  static_cast<std::size_t>(Derived::RowsAtCompileTime)>
arr(Eigen::MatrixBase<Derived> const &vec) noexcept {
  static_assert(Derived::RowsAtCompileTime != Eigen::Dynamic &&
                    Derived::ColsAtCompileTime == 1,
                "Conversion only available for fix sized vectors");

  using T = typename Derived::Scalar;
  auto constexpr N = static_cast<std::size_t>(Derived::RowsAtCompileTime);
  using Result = std::array<T, N>;
  return Detail_::ConstructionHelper<Result, N>{}(vec);
}

template <class T>
inline Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1> const>
map(std::vector<T> const &v) {
  return Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1> const>{
      v.data(), gsl::narrow_cast<Eigen::Index>(v.size())};
}

template <class T>
inline Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> map(std::vector<T> &v) {
  return Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>>{
      v.data(), gsl::narrow_cast<Eigen::Index>(v.size())};
}

template <std::size_t N, class T>
inline Eigen::Map<
    Eigen::Matrix<T, static_cast<Eigen::Index>(N), Eigen::Dynamic> const>
map(std::vector<std::array<T, N>> const &m) {
  return Eigen::Map<
      Eigen::Matrix<T, static_cast<Eigen::Index>(N), Eigen::Dynamic> const>{
      reinterpret_cast<T const *>(m.data()), gsl::narrow_cast<Eigen::Index>(N),
      gsl::narrow_cast<Eigen::Index>(m.size())};
}

template <std::size_t N, class T>
inline Eigen::Map<
    Eigen::Matrix<T, static_cast<Eigen::Index>(N), Eigen::Dynamic>>
map(std::vector<std::array<T, N>> &m) {
  return Eigen::Map<
      Eigen::Matrix<T, static_cast<Eigen::Index>(N), Eigen::Dynamic>>{
      reinterpret_cast<T *>(m.data()), gsl::narrow_cast<Eigen::Index>(N),
      gsl::narrow_cast<Eigen::Index>(m.size())};
}

} // namespace Adaptor
} // namespace Internal
} // namespace CortidQCT


// Authors: David Blei (blei@cs.princeton.edu)
//          Sean Gerrish (sgerrish@cs.princeton.edu)
//
// Copyright 2011 Sean Gerrish and David Blei
// All Rights Reserved.
//
// See the README for this package for details about modifying or
// distributing this software.

/*
 * state space language model variational inference
 *
 */

#ifndef SSLM_H
#define SSLM_H

#include "gsl-wrappers.h"
#include "params.h"
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <assert.h>
#include <math.h>

#include "data.h"

#define SSLM_MAX_ITER 2 // maximum number of optimization iters
#define SSLM_FIT_THRESHOLD 1e-6 // convergence criterion for fitting sslm
#define INIT_MULT 1000 // multiplier to variance for first obs
// #define OBS_NORM_CUTOFF 10 // norm cutoff after which we use all 0 obs
//#define OBS_NORM_CUTOFF 8 // norm cutoff after which we use all 0 obs
#define OBS_NORM_CUTOFF 2 // norm cutoff after which we use all 0 obs

/*
 * functions for variational inference
 *
 */

// allocate new state space language model variational posterior
sslm_var* sslm_var_alloc(int W, int T);

// allocate extra parameters for inference
void sslm_inference_alloc(sslm_var* var);

// free extra parameters for inference
void sslm_inference_free(sslm_var* var);

// initialize with zero observations
void sslm_zero_init(sslm_var* var,
                    double obs_variance,
                    double chain_variance);

// initialize with counts
void sslm_counts_init(sslm_var* var,
                      double obs_variance,
                      double chain_variance,
                      const gsl_vector* counts);

// initialize from variational observations
void sslm_obs_file_init(sslm_var* var,
                        double obs_variance,
                        double chain_variance,
                        const char* filename);

// compute E[\beta_{w,t}] for t = 1:T
void compute_post_mean(int w, sslm_var* var, double chain_variance);

// compute Var[\beta_{w,t}] for t = 1:T
void compute_post_variance(int w, sslm_var* var, double chain_variance);

// optimize \hat{beta}
void optimize_var_obs(sslm_var* var);

// compute dE[\beta_{w,t}]/d\obs_{w,s} for t = 1:T
void compute_mean_deriv(int word, int time, sslm_var* var,
                        gsl_vector* deriv);

// compute d bound/d obs_{w, t} for t=1:T.
void compute_obs_deriv(int word, gsl_vector* word_counts,
                       gsl_vector* total_counts, sslm_var* var,
                       gsl_matrix* mean_deriv_mtx, gsl_vector* deriv);

// update observations
void update_obs(gsl_matrix* word_counts, gsl_vector* totals,
                sslm_var* var);

// log probability bound
double compute_bound(gsl_matrix* word_counts, gsl_vector* totals,
                     sslm_var* var);

// fit variational distribution
double fit_sslm(sslm_var* var, gsl_matrix* word_counts);

// read and write variational distribution
void write_sslm_var(sslm_var* var, char* out);
sslm_var* read_sslm_var(char* in);

void compute_expected_log_prob(sslm_var* var);
// !!! old function (from doc mixture...)
double expected_log_prob(int w, int t, sslm_var* var);

// update zeta
void update_zeta(sslm_var* var);

#endif


#ifndef OPENMC_PHOTON_H
#define OPENMC_PHOTON_H

#include "openmc/endf.h"
#include "openmc/memory.h" // for unique_ptr
#include "openmc/particle.h"
#include "openmc/vector.h"

#include <gsl/gsl>
#include <hdf5.h>
#include "xtensor/xtensor.hpp"

#include <string>
#include <unordered_map>
#include <utility> // for pair

namespace openmc {

//==============================================================================
//! Photon interaction data for a single element
//==============================================================================

class ElectronSubshell {
public:
  // Constructors
  ElectronSubshell() { };

  int index_subshell;  //!< index in SUBSHELLS
  int threshold;
  double n_electrons;
  double binding_energy;
  xt::xtensor<double, 1> cross_section;

  // Transition data
  int n_transitions;
  xt::xtensor<int, 2> transition_subshells;
  xt::xtensor<double, 1> transition_energy;
  xt::xtensor<double, 1> transition_probability;
};

class PhotonInteraction {
public:
  // Constructors/destructor
  PhotonInteraction(hid_t group);
  ~PhotonInteraction();

  // Methods
  void calculate_xs(Particle& p) const;

  void compton_scatter(double alpha, bool doppler, double* alpha_out,
    double* mu, int* i_shell, uint64_t* seed) const;

  double rayleigh_scatter(double alpha, uint64_t* seed) const;

  void pair_production(double alpha, double* E_electron, double* E_positron,
    double* mu_electron, double* mu_positron, uint64_t* seed) const;

  void atomic_relaxation(const ElectronSubshell& shell, Particle& p) const;

  // Data members
  std::string name_; //!< Name of element, e.g. "Zr"
  int Z_; //!< Atomic number
  gsl::index index_; //!< Index in global elements vector

  // Microscopic cross sections
  xt::xtensor<double, 1> energy_;
  xt::xtensor<double, 1> coherent_;
  xt::xtensor<double, 1> incoherent_;
  xt::xtensor<double, 1> photoelectric_total_;
  xt::xtensor<double, 1> pair_production_total_;
  xt::xtensor<double, 1> pair_production_electron_;
  xt::xtensor<double, 1> pair_production_nuclear_;
  xt::xtensor<double, 1> heating_;

  // Form factors
  Tabulated1D incoherent_form_factor_;
  Tabulated1D coherent_int_form_factor_;
  Tabulated1D coherent_anomalous_real_;
  Tabulated1D coherent_anomalous_imag_;

  // Photoionization and atomic relaxation data
  std::unordered_map<int, int> shell_map_; //!< Given a shell designator, e.g. 3, this
                                           //!< dictionary gives an index in shells_
  vector<ElectronSubshell> shells_;

  // Compton profile data
  xt::xtensor<double, 2> profile_pdf_;
  xt::xtensor<double, 2> profile_cdf_;
  xt::xtensor<double, 1> binding_energy_;
  xt::xtensor<double, 1> electron_pdf_;

  // Stopping power data
  double I_; // mean excitation energy
  xt::xtensor<int, 1> n_electrons_;
  xt::xtensor<double, 1> ionization_energy_;
  xt::xtensor<double, 1> stopping_power_radiative_;

  // Bremsstrahlung scaled DCS
  xt::xtensor<double, 2> dcs_;

private:
  void compton_doppler(double alpha, double mu, double* E_out, int* i_shell,
                       uint64_t* seed) const;
};

//==============================================================================
// Non-member functions
//==============================================================================

std::pair<double, double> klein_nishina(double alpha, uint64_t* seed);

void free_memory_photon();

//==============================================================================
// Global variables
//==============================================================================

namespace data {

extern xt::xtensor<double, 1> compton_profile_pz; //! Compton profile momentum grid

//! Photon interaction data for each element
extern std::unordered_map<std::string, int> element_map;
extern vector<unique_ptr<PhotonInteraction>> elements;

} // namespace data

} // namespace openmc

#endif // OPENMC_PHOTON_H


#ifndef KGS_EIGENVALUE_H
#define KGS_EIGENVALUE_H

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>

class Eigenvalue {
    protected:
        const int m, n; ///< Dimensions of matrix

    public:
        Eigenvalue(gsl_matrix* matrix1, gsl_matrix* matrix2, gsl_matrix* matrix3);
        gsl_matrix * const Jacobianmatrix;  //TODO: Make private
        gsl_matrix * const Hessiancartesian;       //TODO: Make private
        gsl_matrix * const Hessiantorsionangle;       //TODO: Make private
        gsl_matrix * const Massmatrix;
        gsl_vector_complex * const eigenvaluealpha;
        gsl_vector * const eigenvaluebeta;

        gsl_matrix_complex * const eigenvector;
        gsl_matrix* times(gsl_matrix* matrix1, gsl_matrix* matrix2) const;

        virtual ~Eigenvalue();

        gsl_matrix* getHessiantorsionangle() const;

        void setSingularvalue() const;

        gsl_vector* getSingularvalue() const;

        gsl_vector * const singularvalue;
    };

#endif //KGS_EIGENVALUE_H