#include "emulator_struct.h"
#include "libEmu/emulator.h"
#include "libEmu/regression.h"
#include "libEmu/emulate-fns.h"
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>

/*********************************************************************
Allocates emulator_struct.
*
* ccs, should be thread safe now
*********************************************************************/
emulator_struct * alloc_emulator_struct(modelstruct * model)
{
	emulator_struct * e = (emulator_struct *)malloc(sizeof(emulator_struct));
	double determinant_c	= 0.0;
	e->nparams = model->options->nparams;
	e->nmodel_points = model->options->nmodel_points;
	e->nregression_fns = model->options->nregression_fns;
	e->nthetas = model->options->nthetas;
	e->model = model;
	gsl_matrix * c_matrix = gsl_matrix_alloc(e->nmodel_points, e->nmodel_points);
	e->cinverse = gsl_matrix_alloc(e->nmodel_points, e->nmodel_points);
	e->beta_vector = gsl_vector_alloc(e->nregression_fns);
	e->h_matrix = gsl_matrix_alloc(e->nmodel_points, e->nregression_fns);
	gsl_matrix * temp_matrix = gsl_matrix_alloc(e->nmodel_points, e->nmodel_points);

	makeCovMatrix_es(c_matrix, e);
	gsl_matrix_memcpy(temp_matrix, c_matrix);
	chol_inverse_cov_matrix(model->options, temp_matrix, e->cinverse, &determinant_c);
	makeHMatrix_es(e->h_matrix, e);
	estimateBeta_es(e->beta_vector, e);

	gsl_matrix_free(c_matrix);
	gsl_matrix_free(temp_matrix);
	return e;
}

/*********************************************************************
Free emulator_struct.
*********************************************************************/
void free_emulator_struct(emulator_struct * e)
{
	gsl_matrix_free(e->cinverse);
	gsl_matrix_free(e->h_matrix);
	gsl_vector_free(e->beta_vector);
	free((void *)e);
}

/**
 * the following fns are wrapped versions of commonly used fns in emulator.h etc, this
 * should make manipulations a little cleaner without breaking libRbind
 * they also use the emulator_struct->model->fnptrs instead
 */

/**
 * make the matrix of regression fns HMatrix, (thread safe?)
 * uses modelstruct fnptr: (void)(makeHvector)(gsl_vector*, gsl_vector*, int);
 *
 * although emulator_struct contains a h_matrix, we might want this for somethign else, so it is given as the 1st arg
 * the h_matrix in e is *not set* by this function
 */
void makeHMatrix_es(gsl_matrix *h_matrix, emulator_struct *e)
{
	makeHMatrix_fnptr(h_matrix,
							e->model->xmodel,
							e->nmodel_points,
							e->nparams,
							e->nregression_fns,
							e->model->makeHVector);
}

/**
 * make a covariance matrix: makeCovMatrix_es, (thread safe?)
 * uses the modelstruct fnptr (double)(*covariance_fn)(gsl_vector*, gsl_vector*, gsl_vector*, int, int)
 */
void makeCovMatrix_es(gsl_matrix *cov_matrix, emulator_struct *e)
{
	makeCovMatrix_fnptr(cov_matrix,
											e->model->xmodel,
											e->model->thetas,
											e->nmodel_points,
											e->nthetas,
											e->nparams,
											e->model->covariance_fn);
}

/**
 * compute a kvector at point k(point, emulator_struct) := { c(point, x_1), ... , c(point, x_n) }^T
 *
 * threadsafe
 *
 * requires: e->model to be init
 * e->model->thetas needs to contain sensible values
 */
void makeKVector_es(gsl_vector *kvector, gsl_vector *point, emulator_struct *e)
{
	makeKVector_fnptr(kvector, e->model->xmodel, point,
										e->model->thetas, e->nmodel_points, e->nthetas, e->nparams,
										e->model->covariance_fn);
}

/**
 * estimate betas from an emulator_struct, no need to modify anything to make this threadsafe
 *
 * betas are set into the first vector, not in the emulator_struct
 *
 * @require e->cinverse and e->h_matrix to be init
 */
void estimateBeta_es(gsl_vector *beta_vector, emulator_struct *e)
{
	estimateBeta(beta_vector, e->h_matrix, e->cinverse, e->model->training_vector,
							 e->nmodel_points, e->nregression_fns);
}

/*********************************************************************
return mean and variance at a point.
thread-safe, uses fnptrs defined in e->modelstruct
*********************************************************************/
void emulate_point(emulator_struct* e, gsl_vector * point,  double * mean, double * variance)
{
	gsl_vector * kplus = gsl_vector_alloc(e->nmodel_points); //allocate variable storage
	gsl_vector * h_vector = gsl_vector_alloc(e->nregression_fns); //allocate variable storage

	makeKVector_es(kplus, point, e);
	/* use the fnptr in modelstruct */
	e->model->makeHVector(h_vector, point, e->nparams);

	(*mean) = makeEmulatedMean(e->cinverse, e->model->training_vector,
		kplus, h_vector, e->h_matrix, e->beta_vector, e->nmodel_points);
	double kappa = e->model->covariance_fn(point, point, e->model->thetas, e->nthetas, e->nparams);
	(*variance) = makeEmulatedVariance(e->cinverse, kplus, h_vector,
		e->h_matrix, kappa, e->nmodel_points, e->nregression_fns);
	gsl_vector_free(kplus);
	gsl_vector_free(h_vector);

	// can we really return on void?
	return;
}


/**
 *
 * @file core_zgetrf_reclap.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.8.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Piotr Luszczek
 * @date 2009-11-15
 *
 * @precisions normal z -> c d s
 *
 **/

#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include "common.h"

struct CORE_zgetrf_data_s {
    volatile PLASMA_Complex64_t *CORE_zamax;
    volatile int                *CORE_zstep;
};

static inline void
CORE_zgetrf_reclap_update(CORE_zgetrf_data_t *data,
                          int M, int column, int n1, int n2,
                          PLASMA_Complex64_t *A, int LDA, int *IPIV,
                          int thidx, int thcnt);
static inline void
CORE_zgetrf_reclap_rec(CORE_zgetrf_data_t *data,
                       int M, int N,
                       PLASMA_Complex64_t *A, int LDA,
                       int *IPIV, int *info,
                       int thidx, int thcnt, int column);

/***************************************************************************//**
 *
 * @ingroup dplasma_cores_complex64
 *
 *  CORE_zgetrf_reclap computes a LU factorization of a general M-by-N
 *  matrix A stored in CCRB layout using partial pivoting with row
 *  interchanges.
 *
 *  The factorization has the form
 *
 *    A = P * L * U
 *
 *  where P is a permutation matrix, L is lower triangular with unit
 *  diagonal elements (lower trapezoidal if m > n), and U is upper
 *  triangular (upper trapezoidal if m < n).
 *
 *  This is the recursive version of the algorithm applied on column
 *  major layout.
 *
 *  WARNINGS:
 *     - The function CORE_zgetrf_reclap_init has to be called prior
 *     to any call to this function.
 *     - You cannot call this kernel on different matrices at the same
 *     time.
 *     - The matrix A cannot be more than one tile wide.
 *     - The number of threads calling this function has to be excatly
 *     the number defined by info[2] with each one of them a different
 *     index between 0 included and info[2] excluded.
 *
 *******************************************************************************
 *
 *  @param[in] data
 *          Common data structure to all threads initialized by
 *          CORE_zgetrf_reclap_init() that contains information for thread
 *          synchronization and maximum reductions. All threads working on a
 *          given matrix A must provide the same data structure.
 *
 *  @param[in] M
 *          The number of rows of the matrix A.  M >= 0.
 *
 *  @param[in] N
 *          The number of columns of the matrix A.  N >= 0.
 *
 *  @param[in,out] A
 *          On entry, the M-by-N matrix to be factorized.
 *          On exit, the factors L and U from the factorization
 *          A = P*L*U; the unit diagonal elements of L are not stored.
 *
 *  @param[in] LDA
 *          The leading dimension of the array A.  LDA >= max(1,M).
 *
 *  @param[out] IPIV
 *          The pivot indices; for 1 <= i <= min(M,N), row i of the
 *          matrix was interchanged with row IPIV(i).
 *          1 <= IPIV[i] <= M.
 *
 *  @param[in,out] info
 *          Array of 3 integers
 *          - info[0], see returned value
 *          - info[1], is the thread index 0 <= info[0] < info[2]
 *          - info[2], on entry is the number of threads trying to
 *                     participate to the factorization,
 *                     on exit is the real number of threads used to
 *                     perform the factorization.
 *    Info[2] threads, and exactly info[2], have to call this function
 *    to avoid dead lock.
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval -k, the k-th argument had an illegal value
 *          \retval k if U(k,k) is exactly zero. The factorization
 *                  has been completed, but the factor U is exactly
 *                  singular, and division by zero will occur if it is used
 *                  to solve a system of equations.
 *
 */
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_zgetrf_reclap = PCORE_zgetrf_reclap
#define CORE_zgetrf_reclap PCORE_zgetrf_reclap
#endif
int CORE_zgetrf_reclap(CORE_zgetrf_data_t *data,
                       int M, int N,
                       PLASMA_Complex64_t *A, int LDA,
                       int *IPIV, int *info)
{
    int thidx = info[1];
    int divmn = M / N;
    int thcnt = min( info[2], ((divmn == 0) && (M != 0))? 1 : divmn );
    int minMN = min(M, N);

    info[0] = 0;
    info[2] = thcnt;

    if( M < 0 ) {
        coreblas_error(1, "illegal value of M");
        return -1;
    }
    if( N < 0 ) {
        coreblas_error(2, "illegal value of N");
        return -2;
    }
    if( LDA < max(1, M) ) {
        coreblas_error(5, "illegal value of LDA");
        return -5;
    }

    /*
     * Quick return
     */
    if ( (M == 0) || (N == 0) || (thidx >= thcnt) ){
      return PLASMA_SUCCESS;
    }

    *info = 0;
    CORE_zgetrf_reclap_rec( data, M, minMN, A, LDA, IPIV, info,
                            thidx, thcnt, 0 );

    if ( N > minMN ) {
        CORE_zgetrf_reclap_update(data, M, 0, minMN, N-minMN,
                                  A, LDA, IPIV,
                                  thidx, thcnt);
    }

    return info[0];
}

/*******************************************************************
 *   Additional routines
 */
static double sfmin = 0.;

CORE_zgetrf_data_t *
CORE_zgetrf_reclap_init(int nbthrd)
{
    int i;
    CORE_zgetrf_data_t *data;

    data = (CORE_zgetrf_data_t*)malloc( nbthrd * (sizeof(PLASMA_Complex64_t)+sizeof(int))
                                        + 2 * sizeof(void*) );
    data->CORE_zamax = (PLASMA_Complex64_t*)((char*)data + 2 * sizeof(void*));
    data->CORE_zstep = (int*)((char*)data + 2 * sizeof(void*) + nbthrd * sizeof(PLASMA_Complex64_t));

    for (i = 0; i < nbthrd; ++i) {
        data->CORE_zamax[i] = 0.;
        data->CORE_zstep[i] = -1;
    }
    if ( sfmin == 0. ) {
        sfmin = LAPACKE_dlamch_work('S');
    }
    return data;
}

static inline void
psplit(int n, int pidx, int pcnt, int *poff_p, int *psiz_p)
{
    int q = n / pcnt, r = n % pcnt;

    if (pidx < r) {
        q++;
        *psiz_p = q;
        *poff_p = pidx * q;
    } else {
        *psiz_p = q;
        *poff_p = r * (q + 1) + (pidx - r) * q;
    }
}

static inline void
CORE_zamax1_thread(CORE_zgetrf_data_t *data,
                   PLASMA_Complex64_t localamx,
                   int thidx, int thcnt, int *thwinner,
                   PLASMA_Complex64_t *globalamx,
                   int pividx, int *ipiv)
{
    volatile PLASMA_Complex64_t *CORE_zamax = data->CORE_zamax;
    volatile int                *CORE_zstep = data->CORE_zstep;

    if (thidx == 0) {
        int i, j = 0;
        PLASMA_Complex64_t curval = localamx, tmp;
        double curamx = cabs(localamx);

        /* make sure everybody filled in their value */
        for (i = 1; i < thcnt; ++i) {
            while (CORE_zstep[i] == -1) { /* wait for thread i to store its value */
            }
        }

        /* better not fuse the loop above and below to make sure data is sync'd */

        for (i = 1; i < thcnt; ++i) {
            tmp = CORE_zamax[i];
            if (cabs(tmp) > curamx) {
                curamx = cabs(tmp);
                curval = tmp;
                j = i;
            }
        }

        if (0 == j)
            ipiv[0] = pividx;

        /* make sure everybody knows the amax value */
        for (i = 1; i < thcnt; ++i)
            CORE_zamax[ i ] = curval;

        CORE_zstep[0] = -j - 2; /* set the index of the winning thread */

        *thwinner = j;
        *globalamx = curval;

        for (i = 1; i < thcnt; ++i)
            CORE_zstep[i] = -3;

        /* make sure everybody read the max value */
        for (i = 1; i < thcnt; ++i) {
            while (CORE_zstep[i] != -1) {
            }
        }

        CORE_zstep[0] = -1;
    } else {
        CORE_zamax[thidx] = localamx;
        CORE_zstep[thidx] = -2;  /* announce to thread 0 that local amax was stored */
        while (CORE_zstep[0] == -1) { /* wait for thread 0 to finish calculating the global amax */
        }
        while (CORE_zstep[thidx] != -3) { /* wait for thread 0 to store amax */
        }
        *thwinner  = -CORE_zstep[0] - 2;
        *globalamx = CORE_zamax[thidx];     /* read the amax from the location adjacent to the one in the above loop */
        CORE_zstep[thidx] = -1;  /* signal thread 0 that this thread is done reading */

        if (thidx == *thwinner)
            ipiv[0] = pividx;

        while (CORE_zstep[0] != -1) { /* wait for thread 0 to finish */
        }
    }
}

static inline void
CORE_zbarrier_thread(CORE_zgetrf_data_t *data,
                     int thidx, int thcnt)
{
    int idum1, idum2;
    PLASMA_Complex64_t ddum2 = 0.;
    /* it's probably faster to implement a dedicated barrier */
    CORE_zamax1_thread( data, 1.0, thidx, thcnt, &idum1, &ddum2, 0, &idum2 );
}

static inline void
CORE_zlaswap1(int ncol, PLASMA_Complex64_t *a, int lda,
              int idxStart, int idxMax, const int *piv)
{
    int i, j;
    PLASMA_Complex64_t tmp;

    for (j = 0; j < ncol; ++j) {
        for (i = idxStart; i < idxMax; ++i) {
            tmp = a[j*lda + piv[i] - 1];
            a[j*lda + piv[i] - 1] = a[i + j*lda];
            a[i + j*lda] = tmp;
        }
    }
}

static inline void
CORE_zgetrf_reclap_update(CORE_zgetrf_data_t *data,
                          int M, int column, int n1, int n2,
                          PLASMA_Complex64_t *A, int LDA, int *IPIV,
                          int thidx, int thcnt)
{
    static PLASMA_Complex64_t posone =  1.0;
    static PLASMA_Complex64_t negone = -1.0;
    PLASMA_Complex64_t *Atop  = A    + column*LDA;
    PLASMA_Complex64_t *Atop2 = Atop + n1    *LDA;
    int coff, ccnt, lm, loff;

    CORE_zbarrier_thread( data, thidx, thcnt );

    psplit( n2, thidx, thcnt, &coff, &ccnt );

    if (ccnt > 0) {
        CORE_zlaswap1( ccnt, Atop2 + coff*LDA, LDA, column, n1 + column, IPIV ); /* swap to the right */

        cblas_ztrsm( CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
                     n1, ccnt, CBLAS_SADDR(posone), Atop + column, LDA, Atop2 + coff*LDA + column, LDA );
    }

    /* __sync_synchronize(); */ /* hopefully we will not need memory fences */

    /* need to wait for pivoting and triangular solve to finish */
    CORE_zbarrier_thread( data, thidx, thcnt );

    psplit( M, thidx, thcnt, &loff, &lm );
    if (thidx == 0) {
        loff = column + n1;
        lm  -= column + n1;
    };

    cblas_zgemm( CblasColMajor, CblasNoTrans, CblasNoTrans, lm, n2, n1,
                 CBLAS_SADDR(negone), Atop+loff, LDA, Atop2 + column, LDA, CBLAS_SADDR(posone), Atop2+loff, LDA );
}

static void
CORE_zgetrf_reclap_rec(CORE_zgetrf_data_t *data, int M, int N,
                       PLASMA_Complex64_t *A, int LDA,
                       int *IPIV, int *info,
                       int thidx, int thcnt, int column)
{
    int jp, n1, n2, lm, loff;
    PLASMA_Complex64_t tmp1, tmp2, tmp3;
    PLASMA_Complex64_t *Atop = A + column*LDA;

    /* Assumption: N = min( M, N ); */
    if (N > 1) {
        int coff, ccnt;

        n1 = N / 2;
        n2 = N - n1;

        CORE_zgetrf_reclap_rec( data, M, n1, A, LDA, IPIV, info,
                                thidx, thcnt, column );
        if ( *info != 0 )
            return;

        CORE_zgetrf_reclap_update(data, M, column, n1, n2,
                                  A, LDA, IPIV,
                                  thidx, thcnt);

        CORE_zgetrf_reclap_rec( data, M, n2, A, LDA, IPIV, info,
                                thidx, thcnt, column + n1 );
        if ( *info != 0 )
            return;

        psplit( n1, thidx, thcnt, &coff, &ccnt );

        if (ccnt > 0) {
            CORE_zlaswap1( ccnt, Atop+coff*LDA, LDA, n1 + column, N + column, IPIV ); /* swap to the left */
        }

    } else {
        int thrd;

        CORE_zbarrier_thread( data, thidx, thcnt );

        psplit( M, thidx, thcnt, &loff, &lm );

        if (thidx == 0) {
            loff = column;
            lm -= column;
        }

        tmp2 = Atop[column]; /* all threads read the pivot element in case they need it */

        jp = cblas_izamax( lm, Atop + loff, 1 );
        tmp1 = Atop[loff + jp];

        CORE_zamax1_thread( data, tmp1, thidx, thcnt, &thrd,
                            &tmp3, loff + jp + 1, IPIV + column );

        Atop[column] = tmp3; /* all threads set the pivot element: no need for synchronization */

        if ( tmp3 != 0.0 ) {
            if ( cabs(tmp3) >= sfmin ) {
                PLASMA_Complex64_t tmp = (PLASMA_Complex64_t)1.0 / tmp3;
                n1 = (thidx == 0) ? 1 : 0;
                cblas_zscal( lm - n1, CBLAS_SADDR(tmp), Atop + loff + n1, 1 );
            } else {
                int i;
                PLASMA_Complex64_t *Atop2;
                n1 = (thidx == 0) ? 1 : 0;
                Atop2 = Atop + loff + n1;

                for( i=0; i < lm-n1; i++, Atop2++)
                    *Atop2 = *Atop2 / tmp3;
            }

            if (thrd == thidx) { /* the thread that owns the best pivot */
              if (loff + jp != column) /* if there is a need to exchange the pivot */
                Atop[loff + jp] = tmp2 / tmp3;
            }

        } else {
            *info = column + 1;
            return;
        }

        CORE_zbarrier_thread( data, thidx, thcnt );
    }
}


/*
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "aXe_grism.h"
#include "aXe_utils.h"
#include "aXe_errors.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_spline.h>
#include "fringe_conf.h"
#include "fringe_model.h"

#define AXE_IMAGE_PATH  "AXE_IMAGE_PATH"
#define AXE_OUTPUT_PATH "AXE_OUTPUT_PATH"
#define AXE_CONFIG_PATH "AXE_CONFIG_PATH"

int
main(int argc, char *argv[])
{
  char        *opt;

  char        fconf_file[MAXCHAR];
  char        fconf_file_path[MAXCHAR];

  char        fimage_file[MAXCHAR];
  char        fimage_file_path[MAXCHAR];

  fringe_conf  *fconf;
  //interpolator *filter_through;
  gsl_matrix   *fringe_image;

  if ((argc < 3) || (opt = get_online_option("help", argc, argv))) {
    fprintf(stdout,
	    "aXe_FRIGEN Version %s:\n"
	    "\n", RELEASE);
    exit(1);
  }
  fprintf(stdout, "aXe_FRIGEN: Starting...\n");

  /* Get the name of the fringe configuration file*/
  strcpy(fconf_file, argv[1]);
  build_path(AXE_CONFIG_PATH, fconf_file, fconf_file_path);

  /* Get the configuration file name */
  strcpy(fimage_file, argv[2]);
  build_path(AXE_OUTPUT_PATH, fimage_file, fimage_file_path);

  /* report on the input and output that will be used */
  fprintf(stdout,
	  "aXe_FRIGEN: Input fringe configuration file:   %s\n",
	  fconf_file_path);
  fprintf(stdout,
	  "aXe_FRIGEN: Output fringe image name:          %s\n",
	  fimage_file_path);

  // load the fringe configuration file
  fconf = load_fringe_conf(fconf_file_path);

  // check whether all necessary information
  // is in place, provide defaults
  check_fringe_conf(fconf);

  // check whether a filter throughput file is set
  if (!fconf->filter_through)
    aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		 "aXe_FRIGEN: No filter throughput table\n"
		 "is set in the fringe configuration file!\n");

  // comute the fringe amplitude
  fringe_image = compute_fringe_amplitude(fconf);

  // save the matrix to a fits-image
  gsl_to_FITSimage (fringe_image, fimage_file_path, 1, NULL);

  // release the space in the fringe image
  gsl_matrix_free(fringe_image);

  // release the allocated memory
  free_fringe_conf(fconf);

  // exit the program
  fprintf(stdout, "aXe_FRIGEN: Done...\n");
  exit(0);
}


#ifndef UBLAS_LIB_PLASMA_H
#define UBLAS_LIB_PLASMA_H

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <pthread.h>
#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>

#include <ublas_types.h>

int ubf_init(void **ctx);
int ubf_gemm(void *ctx, ublas_matrix *a, ublas_matrix *b, ublas_matrix *c, double alpha, double beta);
int ubf_free(void *ctx);

#endif


#ifndef S3D_CV_DISPARITY_DISPARITY_ANALYZER_STAN_H
#define S3D_CV_DISPARITY_DISPARITY_ANALYZER_STAN_H

#include <s3d/disparity/disparity_analyzer.h>

#include "s3d/cv/features/match_finder_surf.h"
#include "s3d/multiview/sampson_distance_function.h"
#include "s3d/multiview/stan_fundamental_matrix_solver.h"
#include "s3d/multiview/stan_results.h"
#include "s3d/robust/ransac.h"
#include "s3d/robust/lmeds.h"
#include "s3d/utilities/math.h"

#include "s3d/utilities/eigen.h"

#include <opencv2/core/types.hpp>

#include <gsl/gsl>

namespace s3d {
struct StanAlignment;
class MatchFinderCV;
class MatchFinderSurf;

class DisparityAnalyzerSTAN : public DisparityAnalyzer {
 public:
  // can alternatively use Lmeds
  using RansacAlgorithmSTAN =
      s3d::robust::Lmeds<s3d::StanFundamentalMatrixSolver, s3d::SampsonDistanceFunction>;

  struct Results {
    explicit Results();

    void updateParameters(double minDisparity,
                          double maxDisparity,
                          float widthRatio,
                          RansacAlgorithmSTAN::ModelType model);

    void updatePoints(const std::vector<Eigen::Vector2d>& bestPtsLeft,
                      const std::vector<Eigen::Vector2d>& bestPtsRight,
                      std::vector<double> disparities,
                      float widthRatio,
                      float resizeRatio);

    StanResults stan;
    double minDisparityPercent{};
    double maxDisparityPercent{};

    std::vector<float> disparitiesPercent{};
  };

  DisparityAnalyzerSTAN();

  gsl::owner<DisparityAnalyzerSTAN*> clone() const override;

  void setMatchFinder(std::unique_ptr<MatchFinderCV> matchFinder);
  void setMaxNumberOfFeatures(int maxNumberOfFeatures);

  bool analyze(const cv::Mat& left, const cv::Mat& right);
  bool analyze(const Image<uint8_t>& left, const Image<uint8_t>& right) override;
  const std::vector<float>& getDisparitiesPercent() const override;
  const std::vector<Eigen::Vector2f>& getFeaturePointsLeft() const override;
  const std::vector<Eigen::Vector2f>& getFeaturePointsRight() const override;
  void setMinimumNumberOfInliers(int minNbInliers);

  // outputs
  Results results;

 private:
  MatchFinder::Matches findMatches(const cv::Mat& left, const cv::Mat& right);
  bool enoughMatches(int nbOfMatches);
  RansacAlgorithmSTAN createRansac(Size imageSize);

  std::unique_ptr<MatchFinderCV> matchFinder_{std::make_unique<MatchFinderCV>()};
  int minNbInliers_{4};
};
}  // namespace s3d

#endif  // S3D_CV_DISPARITY_DISPARITY_ANALYZER_STAN_H


#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <gsl/gsl_rng.h>

#ifdef SUBFIND

#include "fof.h"

#include "allvars.h"
#include "proto.h"
#include "domain.h"
#include "subfind.h"

static struct id_list
{
  MyIDType ID;
  int GrNr;
  int SubNr;
  float BindingEgy;

#ifdef SUBFIND_SAVE_PARTICLELISTS
  float Pos[3];
  float Vel[3];
  int Type;
#ifdef STELLARAGE
  float Mass;
  float StellarAge;
#endif
#endif
}
 *ID_list;

static int Nids;

void subfind(int num)
{
  double t0, t1, tstart, tend;
  int i, gr, nlocid, offset, limit, ncount;

#ifdef DENSITY_SPLIT_BY_TYPE
  struct unbind_data *d;
  int j, n, count[6], countall[6];
  double a3inv, dmax1, dmax2;
#endif

  if(ThisTask == 0)
    printf("\nWe now execute a parallel version of SUBFIND.\n");

  tstart = second();

#ifndef ONLY_PRODUCE_HSML_FILES
  if(!All.ComovingIntegrationOn)
    {
      if(ThisTask == 0)
	printf("works only for comoving integration.\n");
      endrun(0);
    }
#endif

#ifdef DENSITY_SPLIT_BY_TYPE
  a3inv = 1 / (All.Time * All.Time * All.Time);

  for(j = 0; j < 6; j++)
    count[j] = 0;

  /* let's count number of particles of selected species */
  for(i = 0; i < NumPart; i++)
    count[P[i].Type]++;

  MPI_Allreduce(count, countall, 6, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

  /* do first loop: basically just defining the hsml for different species */
  for(j = 0; j < 6; j++)
    {
      if((1 << j) & (DENSITY_SPLIT_BY_TYPE))
	{
#ifdef BLACK_HOLES
	  if(j == 5)
	    countall[j] = 0;	/* this will prevent that the black holes are treated separately */
#endif

	  force_treeallocate((int) (All.TreeAllocFactor * All.MaxPart) + NTopnodes, All.MaxPart);

	  if(countall[j] > All.DesNumNgb)
	    {
	      /* build index list of particles of selectes species */
	      d = (struct unbind_data *) mymalloc(count[j] * sizeof(struct unbind_data));
	      for(i = 0, n = 0; i < NumPart; i++)
		if(P[i].Type == j)
		  d[n++].index = i;

	      t0 = second();
	      if(ThisTask == 0)
		printf("Tree construction for species %d (%d).\n", j, countall[j]);

	      CPU_Step[CPU_FOF] += measure_time();

	      force_treebuild(count[j], d);

	      myfree(d);

	      t1 = second();
	      if(ThisTask == 0)
		printf("tree build for species %d took %g sec\n", j, timediff(t0, t1));
	    }
	  else
	    {
	      t0 = second();
	      if(ThisTask == 0)
		printf("Tree construction.\n");

	      CPU_Step[CPU_FOF] += measure_time();

	      force_treebuild(NumPart, NULL);

	      t1 = second();
	      if(ThisTask == 0)
		printf("tree build took %g sec\n", timediff(t0, t1));
	    }

	  /* let's determine the local densities */
	  t0 = second();
	  subfind_setup_smoothinglengths(j);
	  subfind_density(j);
	  t1 = second();
	  if(ThisTask == 0)
	    printf("density and smoothing length for species %d took %g sec\n", j, timediff(t0, t1));

	  force_treefree();

	  /* let's save density contribution of own species */
	  for(i = 0; i < NumPart; i++)
	    if(P[i].Type == j)
	      P[i].w.density_sum = P[i].u.DM_Density;

	}
    }

  /* do second loop: now calculate all density contributions */
  for(j = 0; j < 6; j++)
    {
      if((1 << j) & (DENSITY_SPLIT_BY_TYPE))
	{
	  force_treeallocate((int) (All.TreeAllocFactor * All.MaxPart) + NTopnodes, All.MaxPart);

	  /* build index list of particles of selectes species */
	  d = (struct unbind_data *) mymalloc(count[j] * sizeof(struct unbind_data));
	  for(i = 0, n = 0; i < NumPart; i++)
	    if(P[i].Type == j)
	      d[n++].index = i;

	  t0 = second();
	  if(ThisTask == 0)
	    printf("Tree construction for species %d (%d).\n", j, countall[j]);

	  CPU_Step[CPU_FOF] += measure_time();

	  force_treebuild(count[j], d);

	  myfree(d);

	  t1 = second();
	  if(ThisTask == 0)
	    printf("tree build for species %d took %g sec\n", j, timediff(t0, t1));

	  /* let's determine the local densities */
	  t0 = second();
	  for(i = 0; i < 6; i++)
	    if((1 << i) & (DENSITY_SPLIT_BY_TYPE))
	      if(j != i)
		{
		  if(countall[i] > All.DesNumNgb)
		    {
		      if(ThisTask == 0)
			printf("calculating density contribution of species %d to species %d\n", j, i);
		      subfind_density(-(i + 1));
		    }
		}
	  t1 = second();
	  if(ThisTask == 0)
	    printf("density() of species %d took %g sec\n", j, timediff(t0, t1));

	  force_treefree();

	  /* let's sum up density contribution */
	  for(i = 0; i < NumPart; i++)
	    if((1 << P[i].Type) & (DENSITY_SPLIT_BY_TYPE))
	      if(j != P[i].Type)
		if(countall[P[i].Type] > All.DesNumNgb)
		  P[i].w.density_sum += P[i].u.DM_Density;
	}
    }

  for(i = 0; i < NumPart; i++)
    {
      P[i].u.DM_Density = P[i].w.density_sum;

      if(P[i].Type == 0)
	P[i].w.int_energy = DMAX(All.MinEgySpec,
				 SphP[i].Entropy / GAMMA_MINUS1 * pow(SphP[i].d.Density * a3inv,
								      GAMMA_MINUS1));
      else
	P[i].w.int_energy = 0;

    }
#else
  force_treeallocate((int) (All.TreeAllocFactor * All.MaxPart) + NTopnodes, All.MaxPart);

  t0 = second();
  if(ThisTask == 0)
    printf("Tree construction.\n");

  CPU_Step[CPU_FOF] += measure_time();

  force_treebuild(NumPart, NULL);

  t1 = second();
  if(ThisTask == 0)
    printf("tree build took %g sec\n", timediff(t0, t1));

  /* let's determine the local dark matter densities */
  t0 = second();
  subfind_setup_smoothinglengths();
  subfind_density();
  t1 = second();
  if(ThisTask == 0)
    printf("dark matter density() took %g sec\n", timediff(t0, t1));

  force_treefree();
#endif /* DENSITY_SPLIT_BY_TYPE */

  if(DumpFlag)
    {
      /* let's save the densities to a file (for making images) */
      t0 = second();
      subfind_save_densities(num);
      t1 = second();
      if(ThisTask == 0)
	printf("saving densities took %g sec\n", timediff(t0, t1));
    }

#ifdef ONLY_PRODUCE_HSML_FILES
  return;
#endif

  /* count how many groups we have that should be done collectively */
  limit = 0.6 * All.TotNumPart / NTask;

  for(i = 0, ncount = 0; i < Ngroups; i++)
    if(Group[i].Len >= limit)
      ncount++;
  MPI_Allreduce(&ncount, &Ncollective, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

  if(ThisTask == 0)
    {
      printf("\nNumber of FOF halos treated with collective SubFind code = %d\n", Ncollective);
      printf("(the adopted size-limit for the collective algorithm was %d particles.)\n", limit);
      printf("the other %d FOF halos are treated in parallel with serial code\n\n", TotNgroups - Ncollective);
    }

  /*  to decide on which task a group should be:
   *  if   GrNr <= Ncollective:  collective groupfinding.
   *  the task where the group info is put is TaskNr = (GrNr - 1) % NTask
   */

  /* now we distribute the particles such that small groups are assigned in
   *  total to certain CPUs, and big groups are left where they are
   */

  t0 = second();

  for(i = 0; i < NumPart; i++)
    {
      if(P[i].GrNr > Ncollective && P[i].GrNr <= TotNgroups)	/* particle is in small group */
	P[i].targettask = (P[i].GrNr - 1) % NTask;
      else
	P[i].targettask = ThisTask;
    }

  subfind_exchange();		/* distributes gas particles as well if needed */

  t1 = second();
  if(ThisTask == 0)
    printf("subfind_exchange()() took %g sec\n", timediff(t0, t1));

  subfind_distribute_groups();

  qsort(Group, Ngroups, sizeof(struct group_properties), fof_compare_Group_GrNr);

  for(i = 0; i < NumPart; i++)
    if(P[i].GrNr > Ncollective && P[i].GrNr <= TotNgroups)
      if(((P[i].GrNr - 1) % NTask) != ThisTask)
	{
	  printf("i=%d %d task=%d\n", i, P[i].GrNr, ThisTask);
	  endrun(87);
	}

  /* lets estimate the maximum number of substructures we need to store on the local CPU */
  for(i = 0, nlocid = 0; i < Ngroups; i++)
    nlocid += Group[i].Len;

  MaxNsubgroups = nlocid / All.DesLinkNgb;	/* this is a quite conservative upper limit */
  Nsubgroups = 0;
  SubGroup = (struct subgroup_properties *) mymalloc(MaxNsubgroups * sizeof(struct subgroup_properties));

  for(i = 0; i < NumPart; i++)
    P[i].SubNr = (1 << 30);	/* default */

  /* we begin by applying the collective version of subfind to distributed groups */
  t0 = second();
  for(GrNr = 1; GrNr <= Ncollective; GrNr++)
    subfind_process_group_collectively(num);
  t1 = second();
  if(ThisTask == 0)
    printf("processing of collective halos took %g sec\n", timediff(t0, t1));

#ifdef SUBFIND_COLLECTIVE_STAGE1
  if(ThisTask == 0)
    printf("stage 1 ended\n");
  endrun(0);
#endif

  for(i = 0; i < NumPart; i++)
    {
      P[i].origindex = i;
      P[i].origintask = ThisTask;
    }

  t0 = second();
  qsort(P, NumPart, sizeof(struct particle_data), subfind_compare_P_GrNr_DM_Density);
  t1 = second();
  if(ThisTask == 0)
    printf("sort of local particles()() took %g sec\n", timediff(t0, t1));

  /* now we have the particles of groups consecutively, but SPH particles are
     not aligned. They can however be accessed via SphP[P[i].originindex] */

  /* let's count how many local particles we have in small groups */
  for(i = 0, nlocid = 0; i < NumPart; i++)
    if(P[i].GrNr > Ncollective && P[i].GrNr <= Ngroups)	/* particle is in small group */
      nlocid++;

  if(ThisTask == 0)
    printf("contructing tree for serial subfind of local groups\n");

  subfind_loctree_treeallocate((int) (All.TreeAllocFactor * All.MaxPart) + NTopnodes, All.MaxPart);

  if(ThisTask == 0)
    printf("Start to do local groups with serial subfind algorithm\n");

  t0 = second();

  /* we now apply a serial version of subfind to the local groups */
  for(gr = 0, offset = 0; gr < Ngroups; gr++)
    {
      if(Group[gr].GrNr > Ncollective)
	{
	  if(((Group[gr].GrNr - 1) % NTask) == ThisTask)
	    offset = subfind_process_group_serial(gr, offset);
	}
    }

  t1 = second();
  if(ThisTask == 0)
    printf("processing of local groups took took %g sec\n", timediff(t0, t1));

  subfind_loctree_treefree();

  /* bringing back particles in original positions, such that gas particles are aligned */
  t0 = second();
  qsort(P, NumPart, sizeof(struct particle_data), subfind_compare_P_origindex);
  t1 = second();
  if(ThisTask == 0)
    printf("unsorting of local particles()() took %g sec\n", timediff(t0, t1));

  GrNr = -1;			/* to ensure that domain decomposition acts normally again */

  /* now determine the remaining spherical overdensity values for the non-local groups */

  domain_free_trick();

  CPU_Step[CPU_FOF] += measure_time();

#ifdef DENSITY_SPLIT_BY_TYPE
  printf("Task %d: testing particles ...\n", ThisTask);
  for(i = 0; i < NumPart; i++)
    {
      if(P[i].origintask != ThisTask)
	printf("Task %d: Holding particle of task %d !\n", ThisTask, P[i].origintask);
      if(P[i].origindex != i)
	printf("Task %d: Particles is in wrong position (is=%d, was=%d) !\n", ThisTask, i, P[i].origindex);
    }
#endif

  All.DoDynamicUpdate = 0;
  domain_Decomposition();

  force_treebuild(NumPart, NULL);

  /* compute spherical overdensities for FOF groups */
  t0 = second();

  subfind_overdensity();

  t1 = second();
  if(ThisTask == 0)
    printf("determining spherical overdensity masses took %g sec\n", timediff(t0, t1));

  /* determine which halos are contaminated by boundary particles */
  t0 = second();

  subfind_contamination();

  t1 = second();
  if(ThisTask == 0)
    printf("determining contamination of halos took %g sec\n", timediff(t0, t1));

  force_treefree();
  domain_free();

  domain_allocate_trick();

  /* now assemble final output */
  subfind_save_final(num);

  tend = second();

  if(ThisTask == 0)
    printf("\nFinished with SUBFIND.  (total time=%g sec)\n\n", timediff(tstart, tend));

  myfree(SubGroup);

  CPU_Step[CPU_FOF] += measure_time();
}

void subfind_save_final(int num)
{
  int i, j, totsubs, masterTask, groupTask, nprocgroup;
  char buf[1000];
  double t0, t1;

  /* prepare list of ids with assigned group numbers */

  parallel_sort(Group, Ngroups, sizeof(struct group_properties), fof_compare_Group_GrNr);
  parallel_sort(SubGroup, Nsubgroups, sizeof(struct subgroup_properties),
		subfind_compare_SubGroup_GrNr_SubNr);

  ID_list = mymalloc(sizeof(struct id_list) * NumPart);

  for(i = 0, Nids = 0; i < NumPart; i++)
    {
      if(P[i].GrNr <= TotNgroups)
	{
	  ID_list[Nids].GrNr = P[i].GrNr;
	  ID_list[Nids].SubNr = P[i].SubNr;
	  ID_list[Nids].BindingEgy = P[i].v.DM_BindingEnergy;
	  ID_list[Nids].ID = P[i].ID;
#ifdef SUBFIND_SAVE_PARTICLELISTS
	  for(j = 0; j < 3; j++)
	    {
	      ID_list[Nids].Pos[j] = P[i].Pos[j];
	      ID_list[Nids].Vel[j] = P[i].Vel[j];
	    }
	  ID_list[Nids].Type = P[i].Type;
#ifdef STELLARAGE
          ID_list[Nids].Mass = P[i].Mass;
          if(P[i].Type == 4)
            ID_list[Nids].StellarAge = P[i].StellarAge;
          else
            ID_list[Nids].StellarAge = 0;
#endif
#endif
	  Nids++;
	}
    }

  parallel_sort(ID_list, Nids, sizeof(struct id_list), subfind_compare_ID_list);

  MPI_Allreduce(&Nsubgroups, &TotNsubgroups, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

  /* fill in the FirstSub-values */
  for(i = 0, totsubs = 0; i < Ngroups; i++)
    {
      if(i > 0)
	Group[i].FirstSub = Group[i - 1].FirstSub + Group[i - 1].Nsubs;
      else
	Group[i].FirstSub = 0;
      totsubs += Group[i].Nsubs;
    }

  MPI_Allgather(&totsubs, 1, MPI_INT, Send_count, 1, MPI_INT, MPI_COMM_WORLD);
  for(j = 1, Send_offset[0] = 0; j < NTask; j++)
    Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];

  for(i = 0; i < Ngroups; i++)
    Group[i].FirstSub += Send_offset[ThisTask];

  MPI_Allgather(&Nids, 1, MPI_INT, Send_count, 1, MPI_INT, MPI_COMM_WORLD);
  for(j = 1, Send_offset[0] = 0; j < NTask; j++)
    Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];

  if(ThisTask == 0)
    {
      sprintf(buf, "%s/groups_%03d", All.OutputDir, num);
      mkdir(buf, 02755);
    }
  MPI_Barrier(MPI_COMM_WORLD);

  if(NTask < All.NumFilesWrittenInParallel)
    {
      printf
	("Fatal error.\nNumber of processors must be a smaller or equal than `NumFilesWrittenInParallel'.\n");
      endrun(241931);
    }

  t0 = second();

  nprocgroup = NTask / All.NumFilesWrittenInParallel;
  if((NTask % All.NumFilesWrittenInParallel))
    nprocgroup++;
  masterTask = (ThisTask / nprocgroup) * nprocgroup;
  for(groupTask = 0; groupTask < nprocgroup; groupTask++)
    {
      if(ThisTask == (masterTask + groupTask))	/* ok, it's this processor's turn */
	subfind_save_local_catalogue(num);
      MPI_Barrier(MPI_COMM_WORLD);	/* wait inside the group */
    }

  t1 = second();

  if(ThisTask == 0)
    {
      printf("Subgroup catalogues saved. took = %g sec\n", timediff(t0, t1));
      fflush(stdout);
    }

  myfree(ID_list);
}

void subfind_save_local_catalogue(int num)
{
  FILE *fd;
  char buf[500], fname[500];
  float *mass, *pos, *vel, *spin;

#ifdef SAVE_MASS_TAB
  float *masstab;
#endif
  int i, j, *len;
  MyIDType *ids;

  sprintf(fname, "%s/groups_%03d/%s_%03d.%d", All.OutputDir, num, "subhalo_tab", num, ThisTask);
  strcpy(buf, fname);
  if(!(fd = fopen(buf, "w")))
    {
      printf("can't open file `%s`\n", buf);
      endrun(1183);
    }

  my_fwrite(&Ngroups, sizeof(int), 1, fd);
  my_fwrite(&TotNgroups, sizeof(int), 1, fd);
  my_fwrite(&Nids, sizeof(int), 1, fd);
  my_fwrite(&TotNids, sizeof(long long), 1, fd);
  my_fwrite(&NTask, sizeof(int), 1, fd);
  my_fwrite(&Nsubgroups, sizeof(int), 1, fd);
  my_fwrite(&TotNsubgroups, sizeof(int), 1, fd);

  /* group len */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].Len;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* offset into id-list */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].Offset;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* mass */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].Mass;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* location (potential minimum) */
  pos = mymalloc(Ngroups * 3 * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    for(j = 0; j < 3; j++)
      pos[i * 3 + j] = Group[i].Pos[j];
  my_fwrite(pos, Ngroups, 3 * sizeof(float), fd);
  myfree(pos);

  /* M_Mean200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].M_Mean200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* R_Mean200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].R_Mean200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* M_Crit200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].M_Crit200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* R_Crit200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].R_Crit200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* M_TopHat200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].M_TopHat200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* R_TopHat200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].R_TopHat200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

#ifdef SO_VEL_DISPERSIONS
  /* VelDisp_Mean200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].VelDisp_Mean200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* VelDisp_Crit200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].VelDisp_Crit200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* VelDisp_TopHat200 */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].VelDisp_TopHat200;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);
#endif

  /* contamination particle count */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].ContaminationLen;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* contamination mass */
  mass = mymalloc(Ngroups * sizeof(float));
  for(i = 0; i < Ngroups; i++)
    mass[i] = Group[i].ContaminationMass;
  my_fwrite(mass, Ngroups, sizeof(float), fd);
  myfree(mass);

  /* number of substructures in FOF group  */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].Nsubs;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* first substructure in FOF group  */
  len = mymalloc(Ngroups * sizeof(int));
  for(i = 0; i < Ngroups; i++)
    len[i] = Group[i].FirstSub;
  my_fwrite(len, Ngroups, sizeof(int), fd);
  myfree(len);

  /* ------------------------------ */

  /* Len of substructure  */
  len = mymalloc(Nsubgroups * sizeof(int));
  for(i = 0; i < Nsubgroups; i++)
    len[i] = SubGroup[i].Len;
  my_fwrite(len, Nsubgroups, sizeof(int), fd);
  myfree(len);

  /* offset of substructure  */
  len = mymalloc(Nsubgroups * sizeof(int));
  for(i = 0; i < Nsubgroups; i++)
    len[i] = SubGroup[i].Offset;
  my_fwrite(len, Nsubgroups, sizeof(int), fd);
  myfree(len);

  /* parent of substructure  */
  len = mymalloc(Nsubgroups * sizeof(int));
  for(i = 0; i < Nsubgroups; i++)
    len[i] = SubGroup[i].SubParent;
  my_fwrite(len, Nsubgroups, sizeof(int), fd);
  myfree(len);

  /* Mass of substructure  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].Mass;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* Pos of substructure  */
  pos = mymalloc(Nsubgroups * 3 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 3; j++)
      pos[i * 3 + j] = SubGroup[i].Pos[j];
  my_fwrite(pos, Nsubgroups, 3 * sizeof(float), fd);
  myfree(pos);

  /* Vel of substructure  */
  vel = mymalloc(Nsubgroups * 3 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 3; j++)
      vel[i * 3 + j] = SubGroup[i].Vel[j];
  my_fwrite(vel, Nsubgroups, 3 * sizeof(float), fd);
  myfree(vel);

  /* Center of mass of substructure  */
  pos = mymalloc(Nsubgroups * 3 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 3; j++)
      pos[i * 3 + j] = SubGroup[i].CM[j];
  my_fwrite(pos, Nsubgroups, 3 * sizeof(float), fd);
  myfree(pos);

  /* Spin of substructure  */
  spin = mymalloc(Nsubgroups * 3 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 3; j++)
      spin[i * 3 + j] = SubGroup[i].Spin[j];
  my_fwrite(spin, Nsubgroups, 3 * sizeof(float), fd);
  myfree(spin);

  /* velocity dispesion  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].SubVelDisp;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* maximum circular velocity  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].SubVmax;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* radius of maximum circular velocity  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].SubVmaxRad;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* radius of half the mass  */
  mass = mymalloc(Nsubgroups * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    mass[i] = SubGroup[i].SubHalfMass;
  my_fwrite(mass, Nsubgroups, sizeof(float), fd);
  myfree(mass);

  /* ID of most bound particle  */
  ids = mymalloc(Nsubgroups * sizeof(MyIDType));
  for(i = 0; i < Nsubgroups; i++)
    ids[i] = SubGroup[i].SubMostBoundID;
  my_fwrite(ids, Nsubgroups, sizeof(MyIDType), fd);
  myfree(ids);

  /* GrNr of substructure  */
  len = mymalloc(Nsubgroups * sizeof(int));
  for(i = 0; i < Nsubgroups; i++)
    len[i] = SubGroup[i].GrNr;
  my_fwrite(len, Nsubgroups, sizeof(int), fd);
  myfree(len);

  /* Masstab of substructure  */
#ifdef SAVE_MASS_TAB
  masstab = mymalloc(Nsubgroups * 6 * sizeof(float));
  for(i = 0; i < Nsubgroups; i++)
    for(j = 0; j < 6; j++)
      masstab[i * 6 + j] = SubGroup[i].MassTab[j];
  my_fwrite(masstab, Nsubgroups, 6 * sizeof(float), fd);
  myfree(masstab);
#endif

  fclose(fd);

#ifdef SUBFIND_SAVE_PARTICLELISTS
  sprintf(buf, "%s/groups_%03d/%s_%03d.%d", All.OutputDir, num, "subhalo_posvel", num, ThisTask);
  if(!(fd = fopen(buf, "w")))
    {
      printf("can't open file `%s`\n", buf);
      endrun(1184);
    }

  my_fwrite(&Ngroups, sizeof(int), 1, fd);
  my_fwrite(&TotNgroups, sizeof(int), 1, fd);
  my_fwrite(&Nids, sizeof(int), 1, fd);
  my_fwrite(&TotNids, sizeof(long long), 1, fd);
  my_fwrite(&NTask, sizeof(int), 1, fd);
  my_fwrite(&Send_offset[ThisTask], sizeof(int), 1, fd);
  my_fwrite(&All.Time, sizeof(double), 1, fd);

  float *posdata;
  double a3inv;
  char *types;

  if(All.ComovingIntegrationOn)
    a3inv = 1.0 / (All.Time * All.Time * All.Time);
  else
    a3inv = 1.0;

  posdata = (float *) mymalloc(3 * Nids * sizeof(float));

  for(i = 0; i < Nids; i++)
    for(j = 0; j < 3; j++)
      posdata[i * 3 + j] = ID_list[i].Pos[j];

  my_fwrite(posdata, 3 * sizeof(float), Nids, fd);

  for(i = 0; i < Nids; i++)
    for(j = 0; j < 3; j++)
      posdata[i * 3 + j] = ID_list[i].Vel[j] * sqrt(a3inv);

  my_fwrite(posdata, 3 * sizeof(float), Nids, fd);

  types = (char *) posdata;

  for(i = 0; i < Nids; i++)
    types[i] = ID_list[i].Type;

  my_fwrite(types, sizeof(char), Nids, fd);

#ifdef STELLARAGE
  for(i = 0; i < Nids; i++)
    posdata[i] = ID_list[i].Mass;

  my_fwrite(posdata, sizeof(float), Nids, fd);

  for(i = 0; i < Nids; i++)
    posdata[i] = ID_list[i].StellarAge;

  my_fwrite(posdata, sizeof(float), Nids, fd);
#endif

  myfree(posdata);

  fclose(fd);
#endif

  ids = (MyIDType *) ID_list;

  for(i = 0; i < Nids; i++)
    ids[i] = ID_list[i].ID;

  sprintf(buf, "%s/groups_%03d/%s_%03d.%d", All.OutputDir, num, "subhalo_ids", num, ThisTask);
  if(!(fd = fopen(buf, "w")))
    {
      printf("can't open file `%s`\n", buf);
      endrun(1184);
    }

  my_fwrite(&Ngroups, sizeof(int), 1, fd);
  my_fwrite(&TotNgroups, sizeof(int), 1, fd);
  my_fwrite(&Nids, sizeof(int), 1, fd);
  my_fwrite(&TotNids, sizeof(long long), 1, fd);
  my_fwrite(&NTask, sizeof(int), 1, fd);
  my_fwrite(&Send_offset[ThisTask], sizeof(int), 1, fd);
  my_fwrite(ids, sizeof(MyIDType), Nids, fd);

  fclose(fd);
}

int subfind_compare_ID_list(const void *a, const void *b)
{
  if(((struct id_list *) a)->GrNr < ((struct id_list *) b)->GrNr)
    return -1;

  if(((struct id_list *) a)->GrNr > ((struct id_list *) b)->GrNr)
    return +1;

  if(((struct id_list *) a)->SubNr < ((struct id_list *) b)->SubNr)
    return -1;

  if(((struct id_list *) a)->SubNr > ((struct id_list *) b)->SubNr)
    return +1;

  if(((struct id_list *) a)->BindingEgy < ((struct id_list *) b)->BindingEgy)
    return -1;

  if(((struct id_list *) a)->BindingEgy > ((struct id_list *) b)->BindingEgy)
    return +1;

  return 0;
}

int subfind_compare_SubGroup_GrNr_SubNr(const void *a, const void *b)
{
  if(((struct subgroup_properties *) a)->GrNr < ((struct subgroup_properties *) b)->GrNr)
    return -1;

  if(((struct subgroup_properties *) a)->GrNr > ((struct subgroup_properties *) b)->GrNr)
    return +1;

  if(((struct subgroup_properties *) a)->SubNr < ((struct subgroup_properties *) b)->SubNr)
    return -1;

  if(((struct subgroup_properties *) a)->SubNr > ((struct subgroup_properties *) b)->SubNr)
    return +1;

  return 0;
}

int subfind_compare_P_GrNr_DM_Density(const void *a, const void *b)
{
  if(((struct particle_data *) a)->GrNr < (((struct particle_data *) b)->GrNr))
    return -1;

  if(((struct particle_data *) a)->GrNr > (((struct particle_data *) b)->GrNr))
    return +1;

  if(((struct particle_data *) a)->u.DM_Density > (((struct particle_data *) b)->u.DM_Density))
    return -1;

  if(((struct particle_data *) a)->u.DM_Density < (((struct particle_data *) b)->u.DM_Density))
    return +1;

  return 0;
}

int subfind_compare_P_origindex(const void *a, const void *b)
{
  if(((struct particle_data *) a)->origindex < (((struct particle_data *) b)->origindex))
    return -1;

  if(((struct particle_data *) a)->origindex > (((struct particle_data *) b)->origindex))
    return +1;

  return 0;
}

#endif


/*
 *
 *
 * The functions in this file are directly taken from
 https://github.com/s5248/cppconvnet
 The license and the copying notice are replicated below.
----------
LICENSE
----------
Copyright (c) 2015, BiaoZhi Huang.
All rights reserved.

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

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

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

* Neither the name of cppconvnet nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

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

---------------
COPYING
---------------

Copyright (c) 2014 The MatConvNet team.
Copyright (c) 2015 BiaoZhi Huang.
All rights reserved.

Redistribution and use in source and binary forms are permitted
provided that the above copyright notice and this paragraph are
duplicated in all such forms and that any documentation,
advertising materials, and other materials related to such
distribution and use acknowledge that the software was developed
by the <organization>. The name of the
<organization> may not be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

#include "common_types.h"
#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#include <cblas.h>
#include "utils.h"

static inline int floor_divide(int a, int b) {
  if (a >= 0) return a / b;
  else return (a - b + 1) / b;
}

static inline int ceil_divide(int a, int b) {
  if (a >= 0) return (a + b - 1) / b;
  else return a / b;
}

static inline int min (int a, int b) {
  return a < b ? a : b;
}

static void CppConvnetIm2Row(float* stacked, const float* data, int numPatchesX,
                      int numPatchesY, int numRows, const TensorDim input_dim,
                      const TensorDim filter_dim,
                      int stride, int pad)
{

  /*
  Fill a row of the stacked image at a time. Since patches are stored
  aint the columns, scanning a row menas visiting all patche once.
  Each row corresponds to a particular offset within each patch.

  In this manner, as we fill a row
  we tend to access spatially adiacent elements
  in the input image, particulary for small strides.
  */
  float *im2row = stacked;
  // numRows = KxKXC, numPatchesX = H, numPatchesY = W
  for (int row = 0; row < numRows; ++row) {
    /*
    Get the patch offset corresponding to this row of the stacked
    image.
    */
    int u = row;
    int v = u / filter_dim.w;
    int z = v / filter_dim.h; // filter channel no
    u %= filter_dim.w; // filter col no
    v %= filter_dim.h; // filter row no

    /*
    Filling this row amounts to visiting all the pixels in the input
    image that appear at a given offset in the outut patches. Accounting
    for the subsampling of the output patches and input padding,
    these pixels are given by

    x_data(x) = x * m_stride[1] + u - m_pad[2],  0 <= x < numPatchesX
    y_data(y) = y * m_stride[0] + v - m_pad[0],   0 <= y < numPatchesY
    z_data(z) = z.

    Here (x,y) are the spatial indexes of the output patches. Depending
    on the padding, some of these values will read pixels outised
    the input image, which should default to 0. In particular, x lands
    on a x_data(x) within the image if x0 <= x < x1 where:

    x_data(x) >= 0 <=> x >= (m_pad[2] - u) / stride
    <=> x >= ceil((m_pad[2] - u) / stride) = x0
    x_data(x) <= m_inputDims[1]-1 <=> x <= (m_inputDims[1]-1 + m_pad[2] - u) / stride
    <=> x <= floor((m_inputDims[1]-1 + m_pad[2] - u) / stride)
    <=> x <  floor((m_inputDims[1]-1 + m_pad[2] - u) / stride) + 1 = x1

    and the same for y. Note that, while usually x0 <= x1, there are
    special cases for which x1 < x0. This is accounted for in the loops
    below.
    */

    int x0 = min(numPatchesX, (int)ceil_divide(pad - u, stride));
    int y0 = min(numPatchesY, (int)ceil_divide(pad - v, stride));
    int x1 = min(numPatchesX, (int)floor_divide(input_dim.w - 1 + pad - u, stride) + 1);
    int y1 = min(numPatchesY, (int)floor_divide(input_dim.h - 1 + pad - v, stride) + 1);
    int x;
    int y;

    for (y = 0; y < y0; ++y) {
      for (x = 0; x < numPatchesX; ++x) {
        *stacked++ = 0;
      }
    }
    for (; y < y1; ++y) {
      for (x = 0; x < x0; ++x) {
        *stacked++ = 0;
      }
      int y_data = y * stride + v - pad;
      int x_data = x * stride + u - pad;
      float const * b = data + (z * input_dim.h + y_data) * input_dim.w + x_data;
      for (; x < x1; ++x) {
        *stacked++ = *b;
        b += stride;
      }
      for (; x < numPatchesX; ++x) {
        *stacked++ = 0;
      }
    }
    for (; y < numPatchesY; ++y) {
      for (x = 0; x < numPatchesX; ++x) {
        *stacked++ = 0;
      }
    }
  }

  PrintMat("im2row", im2row, 1, numRows*numPatchesX*numPatchesY, CblasRowMajor);

}

// Not working for group != 1
bool CppConvnetConvLayer(const float *in_data, const float *filters,
                         const float *bias, TensorDim in_dim,
                         TensorDim filt_dim, int stride, int pad, int group,
                         float *output)
{
  float dataMult=1.0;
  float outputMult=0.0;
  TensorDim out_dim;
  out_dim.w = (in_dim.w + (pad + pad) - filt_dim.w) / stride + 1;
  out_dim.h = (in_dim.h + (pad + pad) - filt_dim.h) / stride + 1;
  out_dim.c = filt_dim.n;
  out_dim.n = in_dim.n;
  int m_biasDims = out_dim.c;

  //int numGroups = in_dim.c / filt_dim.c;// assume = 1
  bool fullyConnectedMode = (out_dim.w == 1 &&
      out_dim.h == 1 &&
    stride == 1 &&
    pad == 0 &&
    group == 1);
  if (fullyConnectedMode){
    return false;
  }
  printf("---------Inputs------\n");
  PrintTensor(in_data, in_dim);
  printf("---------------------\n");
  printf("-------Filters-------\n");
  PrintTensor(filters, filt_dim);
  int numFiltersPerGroup = filt_dim.n / group; // M
  int numOutputPixels = out_dim.w * out_dim.h; // H*W
  int filtersVolume = filt_dim.w * filt_dim.h * filt_dim.c/group; // K*K*C
  int tempVolume = numOutputPixels * filtersVolume * group; // H*W*K*K*C --> looks to be size of im2row buffer
  float* tempMemory = malloc( sizeof(float) *tempVolume); // im2row buffer?
  float* tempOnes = malloc(sizeof(float)*numOutputPixels); // buffer of size H*W all set to 1
  for (int i = 0; i < numOutputPixels; i++) tempOnes[i] = 1.0;

  for (int image = 0; image < in_dim.n; ++image) {
    int dataOffset = (in_dim.w * in_dim.h * in_dim.c) * image;// pointer to batch of images
    int outputOffset = (out_dim.w * out_dim.h * out_dim.c) * image;// pointer to batch of output maps.
    //under sample
    CppConvnetIm2Row(tempMemory, in_data + dataOffset, out_dim.w, out_dim.h,
                     filtersVolume*group, in_dim, filt_dim, stride, pad);

    for (int g = 0; g < group; ++g) {
      int filterGrpOffset = filtersVolume * numFiltersPerGroup * g;//0
      int tempGrpOffset = numOutputPixels * filtersVolume * g;//0
      int outputGrpOffset = numOutputPixels * numFiltersPerGroup * g;//0
      float alpha = dataMult;
      float beta = outputMult;
      //convolution
      // A -> MxK B-> KxN
      PrintMat("im2row", tempMemory + tempGrpOffset, numOutputPixels, filtersVolume, CblasColMajor);
      PrintMat("filters", filters + filterGrpOffset, filtersVolume, numFiltersPerGroup, CblasColMajor);
      //PrintMat("im2row", tempMemory, 1, filtersVolume* numOutputPixels, CblasColMajor);
      cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                  numOutputPixels/*m = HxW*/,
                  numFiltersPerGroup/*n = M*/, filtersVolume/*k = KxKxC/group*/,
                  alpha, tempMemory + tempGrpOffset, /*A*/
                  numOutputPixels/*lda*/,
                  filters + filterGrpOffset, // B
                  filtersVolume, // ldb
                  beta,
                  output + outputOffset + outputGrpOffset, // C
                  numOutputPixels // ldc
                  );

    }
    if (bias!=NULL) {
      float alpha = 1;
      float beta = 1;
      int k = 1;
      //add bias
      cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, numOutputPixels,
                  m_biasDims, k, alpha, tempOnes, numOutputPixels,
                  bias, k, beta, output + outputOffset, numOutputPixels);
    }
  }
  free(tempMemory);
  free(tempOnes);
  return true;
}


#include "quantum_gates.h"
#include "quac_p.h"
#include <stdlib.h>
#include <stdio.h>
#include <petsc.h>
#include <stdarg.h>
#include "qasm_parser.h"
#include "error_correction.h"
#include "dm_utilities.h"
#include <ctype.h>

void quil_read(char filename[],PetscInt *num_qubits,circuit *circ){
  FILE *fp;
  int ch = 0,lines=0,finished_allocate=0,num_pragma=0,this_qubit,max_qubit=-1;
  char *line = NULL;
  size_t len = 0;
  ssize_t read;
  PetscReal time=1.0;

  fp = fopen(filename,"r");

  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_qasm_read!\n");
    }
  }

  //Count number of lines
  while(!feof(fp)){
    ch = fgetc(fp);
    if(ch == '\n'){
      lines++;
    }
  }
  //Rewind file
  rewind(fp);

  *num_qubits = 0;
  num_pragma  = 0;
  this_qubit  = -1;
  while ((read = getline(&line, &len, fp)) != -1){
    if (strstr(line,"PRAGMA")){
      //We will skip pragma lines
      num_pragma = num_pragma + 1;
    } else {
      this_qubit = line[strlen(line) - 2] - '0';
      if (this_qubit>max_qubit){
        max_qubit = this_qubit;
      }
    }
  }
  *num_qubits = max_qubit+1;

  lines = lines - num_pragma;

  create_circuit(circ,lines);
  rewind(fp); //Rewind the file (again)

  while ((read = getline(&line, &len, fp)) != -1){
    if (strstr(line,"PRAGMA")){
      //We will skip pragma lines
    } else if (strlen(line)>1){
      _quil_add_gate(line,circ,time);
      time = time + 1.0;
    }
  }

  fclose(fp);
  if (line) free(line);
  return;
}

void _quil_get_angle_pi(char angle_pi[32],PetscReal *angle){
  char numerator[32],denominator[32];
  int found_denom=1,i_n,i_d,i,denom,numer,factor;
  //Gate was printed with 'pi' or 'pi/2'
  angle_pi[strlen(angle_pi)-1] = 0;
  factor = 1;
  //Search for digits
  i_d = 0;   i_n = 0;
  denom = 1; numer = 1;
  for (i=0;angle_pi[i] != '\0'; i++){
    if (angle_pi[i]=='-'){
      //Found negative sign
      factor = -1;
    } else if (angle_pi[i]=="/") {
      // Found division; all digits after this
      // belong to the denominator
      found_denom = 1;
    } if (isdigit(angle_pi[i])){
      if (found_denom==0){
        //Numerator
        numerator[i_n] = angle_pi[i];
        i_n = i_n + 1;
      } else {
        //Denominator
        denominator[i_n] = angle_pi[i];
        i_d = i_d + 1;
      }
    }
  }
  if (i_d>0){
    denom = atoi(denominator);
  }
  if (i_n>0){
    numer = atoi(numerator);
  }
  *angle = factor * numer * PETSC_PI / denom;
  return;
}

void _quil_add_gate(char *line,circuit *circ,PetscReal time){
  char *token=NULL,*ptr=NULL;
  const char s[2] = " ";
  char angle_pi[32];
  int qubit1=-1,qubit2=-1;
  PetscReal angle;
  gate_type my_gate_type;
  // Split string on " " to separate the gate and the qubits
  while (token=strsep(&line," ")) {
    size_t i, j;
    for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
    //FIXME: Not exhaustive
    if (isdigit(token[0])){
      // check qubit numbers
      if (qubit1<0){
        qubit1 = atoi(token);
      } else {
        qubit2 = atoi(token);
      }
    } else {
      // gate types
      if (strcmp(token,"CNOT")==0){
        my_gate_type = CNOT;
      } else if (strcmp(token,"I")==0){
        my_gate_type = EYE;
      } else if (strcmp(token,"CZ")==0){
        my_gate_type = CZ;;
      } else if (strcmp(token,"H")==0) {
        my_gate_type = HADAMARD;
      } else if (strcmp(token,"Z")==0) {
        my_gate_type = SIGMAZ;
      } else if (strcmp(token,"X")==0) {
        my_gate_type = SIGMAX;
      } else if (strcmp(token,"Y")==0) {
        my_gate_type = SIGMAY;
      } else if (strstr(token,"RX")) {
        my_gate_type = RX;
        if (strstr(token,"pi")){
          sscanf(token,"RX(%s)",&angle_pi);
          _quil_get_angle_pi(angle_pi,&angle);
        } else {
          //Not sure if quil prints like this
          sscanf(token,"RX(%lf)",&angle);
        }
      } else if (strstr(token,"RY")) {
        my_gate_type = RY;
        if (strstr(token,"pi")){
          sscanf(token,"RY(%s)",&angle_pi);
          _quil_get_angle_pi(angle_pi,&angle);
        } else {
          //Not sure if quil prints like this
          sscanf(token,"RY(%lf)",&angle);
        }
      } else if (strstr(token,"RZ")) {
        if (strstr(token,"pi")){
          sscanf(token,"RZ(%s)",&angle_pi);
          _quil_get_angle_pi(angle_pi,&angle);
        } else {
          //Not sure if quil prints like this
          sscanf(token,"RZ(%lf)",&angle);
        }
        my_gate_type = RZ;
      }
    }
  }

  if (qubit2>0){
    //Multiqubit gate
    add_gate_to_circuit(circ,time,my_gate_type,qubit1,qubit2);
  } else {
    //Single qubit gate
    if (my_gate_type==6||my_gate_type==7||my_gate_type==8){
      //Rotation gate
      add_gate_to_circuit(circ,time,my_gate_type,qubit1,angle);
    } else {
      add_gate_to_circuit(circ,time,my_gate_type,qubit1);
    }
  }
  return;
}

void projectq_qasm_read(char filename[],PetscInt *num_qubits,circuit *circ){
  FILE *fp;
  int ch = 0,lines=0,finished_allocate=0;
  char *line = NULL;
  size_t len = 0;
  ssize_t read;
  PetscReal time=1.0;

  fp = fopen(filename,"r");

  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_qasm_read!\n");
    }
  }

  //Count number of lines
  while(!feof(fp)){
    ch = fgetc(fp);
    if(ch == '\n')
      {
        lines++;
      }
  }
  //Rewind file
  rewind(fp);

  *num_qubits = 0;
  while ((read = getline(&line, &len, fp)) != -1){
    if (!finished_allocate){
      if (strstr(line,"Allocate")){
        //Get number of qubits by reading number of lines with 'Allocate'
        *num_qubits = *num_qubits+1;
      } else {
        /*Subtract off the allocate lines to get the total number
         * of gates.
         * Factor of 2 because the end has DEALLOCATE statements
         * WARNING! Comments will be included, too - not too big of a problem,
         * just will overallocate a bit */
        lines = lines - 2*(*num_qubits);
        //Allocate the circuit
        create_circuit(circ,lines);
        finished_allocate = 1;
        //Add the first gate to list
        _projectq_qasm_add_gate(line,circ,time);
        time = time + 1.0;
        //        gate_list[0]
      }
    } else if (strstr(line,"Deallocate")){
      //Breakout since we've finished going through the circuit
      break;
    } else {
      _projectq_qasm_add_gate(line,circ,time);
      time = time + 1.0;
    }

  }

  fclose(fp);
  if (line) free(line);
  return;
}

void _projectq_qasm_add_gate(char *line,circuit *circ,PetscReal time){
  char *token=NULL;
  int qubit1,qubit2;
  PetscReal angle;
  gate_type my_gate_type;
  // Split string on | to separate gate type and qubits
  while (token=strsep(&line,"|")) {
    //Strip whitespace
    size_t i, j;
    for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
    //Do direct strcmp for some, strstr for others
    //FIXME: Not exhaustive
    if (strstr(token,"Qureg")){
      // qubit numbers
      if (my_gate_type<0){
        //Multiqubit gate
        sscanf(token,"(Qureg[%d],Qureg[%d])",&qubit1,&qubit2);
        add_gate_to_circuit(circ,time,my_gate_type,qubit1,qubit2);
      } else {
        //Single qubit gate
        sscanf(token,"Qureg[%d]",&qubit1);
        if (my_gate_type==6||my_gate_type==7||my_gate_type==8){
          //Rotation gate
          add_gate_to_circuit(circ,time,my_gate_type,qubit1,angle);
        } else {
          add_gate_to_circuit(circ,time,my_gate_type,qubit1);
        }
      }
    } else {
      // gate types
      if (strcmp(token,"CX")==0){
        my_gate_type = CNOT;
      } else if (strcmp(token,"H")==0) {
        my_gate_type = HADAMARD;
      } else if (strcmp(token,"Z")==0) {
        my_gate_type = SIGMAZ;
      } else if (strcmp(token,"X")==0) {
        my_gate_type = SIGMAX;
      } else if (strcmp(token,"Y")==0) {
        my_gate_type = SIGMAY;
      } else if (strstr(token,"Rx")) {
        my_gate_type = RX;
        sscanf(token,"Rx(%lf)",&angle);
      } else if (strstr(token,"Ry")) {
        my_gate_type = RY;
        sscanf(token,"Ry(%lf)",&angle);
      } else if (strstr(token,"Rz")) {
        sscanf(token,"Rz(%lf)",&angle);
        my_gate_type = RZ;
      }
    }
  }
}

void projectq_vqe_get_expectation(char filename[],Vec rho,PetscScalar *trace_val){
  FILE *fp;
  char *token=NULL,*token2=NULL;
  char *line = NULL,gate_char;
  size_t len = 0;
  ssize_t read;
  int token_number,num_ops,qubit_number;
  operator ops[100];
  PetscReal scalar_multiply;
  PetscScalar temp_trace_val;
  fp = fopen(filename,"r");
  *trace_val = 0.0;
  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_vqe_get_expectation!\n");
    }
  }

  while ((read = getline(&line, &len, fp)) != -1){
    token_number = 0;
    while (token=strsep(&line,"[")) {
      if(token_number==0){
        //Strip whitespace
        size_t i, j;
        for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
        scalar_multiply = atof(token);
        token_number = 1;
      } else {
        token2=strsep(&token,"]");
        num_ops = 0;
        while (token=strsep(&token2," ")) {
          if (strcmp(token,"")==0){
            *trace_val = *trace_val + scalar_multiply;
          } else {
            //Assume qubit number in file is global system number
            //FIXME: Put logical->physical qubit mapping here
            sscanf(token,"%c%d",&gate_char,&qubit_number);
            if (gate_char=='X'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_x;
            } else if (gate_char=='Y'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_y;
            } else if (gate_char=='Z'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_z;
            }
            num_ops = num_ops + 1;
          }
        }
        if (num_ops!=0){
          //This is a hack where I pass many ops, even though many of them
          //may not exist or be valid; they won't be accessed, at least.
          //Consider passing the array instead, and iterating inside?
          get_expectation_value(rho,&temp_trace_val,num_ops,
                                ops[0],ops[1],ops[2],ops[3],
                                ops[4],ops[5],ops[6],ops[7],
                                ops[8],ops[9],ops[10],ops[11],
                                ops[12],ops[13],ops[14],ops[15],
                                ops[16],ops[17],ops[18],ops[19]);
          temp_trace_val = temp_trace_val * scalar_multiply;
          *trace_val = *trace_val + temp_trace_val;
        }
      }
    }
  }
  return;
}

void projectq_vqe_get_expectation_squared(char filename[],Vec rho,PetscScalar *trace_val){
  FILE *fp;
  char *token=NULL,*token2=NULL;
  char *line = NULL,gate_char;
  size_t len = 0;
  ssize_t read;
  int token_number,num_ops,qubit_number;
  operator ops[100];
  PetscReal scalar_multiply;
  PetscScalar temp_trace_val;
  fp = fopen(filename,"r");
  *trace_val = 0.0;
  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_vqe_get_expectation!\n");
    }
  }

  while ((read = getline(&line, &len, fp)) != -1){
    token_number = 0;
    while (token=strsep(&line,"[")) {
      if(token_number==0){
        //Strip whitespace
        size_t i, j;
        for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
        scalar_multiply = atof(token);
        token_number = 1;
      } else {
        token2=strsep(&token,"]");
        num_ops = 0;
        while (token=strsep(&token2," ")) {
          if (strcmp(token,"")==0){
            *trace_val = *trace_val + scalar_multiply;
          } else {
            //Assume qubit number in file is global system number
            //FIXME: Put logical->physical qubit mapping here
            sscanf(token,"%c%d",&gate_char,&qubit_number);
            if (gate_char=='X'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_x;
            } else if (gate_char=='Y'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_y;
            } else if (gate_char=='Z'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_z;
            }
            num_ops = num_ops + 1;
          }
        }
        if (num_ops!=0){
          //This is a hack where I pass many ops, even though many of them
          //may not exist or be valid; they won't be accessed, at least.
          //Consider passing the array instead, and iterating inside?
          get_expectation_value(rho,&temp_trace_val,num_ops,
                                ops[0],ops[1],ops[2],ops[3],
                                ops[4],ops[5],ops[6],ops[7],
                                ops[8],ops[9],ops[10],ops[11],
                                ops[12],ops[13],ops[14],ops[15],
                                ops[16],ops[17],ops[18],ops[19]);
          temp_trace_val = temp_trace_val * scalar_multiply;
          *trace_val = *trace_val + temp_trace_val;
        }
      }
    }
  }
  return;
}

void projectq_vqe_get_expectation_encoded(char filename[],Vec rho,PetscScalar *trace_val,
                                          PetscInt num_encoders,...){
  FILE *fp;
  char *token=NULL,*token2=NULL;
  char *line = NULL,gate_char;
  size_t len = 0;
  ssize_t read;
  int token_number,num_ops,qubit_number;
  va_list ap;
  operator ops[100];
  PetscInt i;
  PetscReal scalar_multiply;
  PetscScalar temp_trace_val;
  encoded_qubit encoders[50]; //50 is more systems than we will be able to do

  va_start(ap,num_encoders);
  for (i=0;i<num_encoders;i++){
    encoders[i] = va_arg(ap,encoded_qubit);
  }

  fp = fopen(filename,"r");
  *trace_val = 0.0;
  if (fp == NULL){
    if (nid==0){
      printf("ERROR! File not found in projectq_vqe_get_expectation!\n");
    }
  }

  while ((read = getline(&line, &len, fp)) != -1){
    token_number = 0;
    while (token=strsep(&line,"[")) {
      if(token_number==0){
        //Strip whitespace
        size_t i, j;
        for (i=0, j=0; token[j]=token[i]; j+=!isspace(token[i++]));
        scalar_multiply = atof(token);
        token_number = 1;
      } else {
        token2=strsep(&token,"]");
        num_ops = 0;
        while (token=strsep(&token2," ")) {
          if (strcmp(token,"")==0){
            *trace_val = *trace_val + scalar_multiply;
          } else {
            //Assume qubit number in file is global system number
            //FIXME: Put logical->physical qubit mapping here
            sscanf(token,"%c%d",&gate_char,&qubit_number);
            qubit_number = encoders[qubit_number].qubits[0];
            if (gate_char=='X'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_x;
            } else if (gate_char=='Y'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_y;
            } else if (gate_char=='Z'){
              ops[num_ops] = subsystem_list[qubit_number]->sig_z;
            }
            num_ops = num_ops + 1;
          }
        }
        if (num_ops!=0){
          if (num_ops>4){
            if (nid==0) {
              printf("ERROR! vqe_get_expectation only supports 4 ops for now\n");
              exit(0);
            }
          }
          get_expectation_value(rho,&temp_trace_val,num_ops,ops[0],ops[1],ops[2],ops[3]);
          temp_trace_val = temp_trace_val * scalar_multiply;
          *trace_val = *trace_val + temp_trace_val;
        }
      }
    }
  }
  return;
}


/**
 *
 * @file qwrapper_cgemv_tile.c
 *
 *  PLASMA core_blas quark wrapper
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mark Gates
 * @date 2010-11-15
 * @generated c Tue Jan  7 11:44:59 2014
 *
 **/
#include <cblas.h>
#include "common.h"

/***************************************************************************//**
 *
 * Version of zgemv for tile storage, to avoid dependency problem when
 * computations are done within the tile. alpha and beta are passed as
 * pointers so they can depend on runtime values.
 *
 * @param[in] Alock
 *          Pointer to tile owning submatrix A.
 *
 * @param[in] xlock
 *          Pointer to tile owning subvector x.
 *
 * @param[in] ylock
 *          Pointer to tile owning subvector y.
 *
 **/
void QUARK_CORE_cgemv_tile(Quark *quark, Quark_Task_Flags *task_flags,
                           PLASMA_enum trans,
                           int m, int n,
                           const PLASMA_Complex32_t *alpha, const PLASMA_Complex32_t *A, int lda,
                                                            const PLASMA_Complex32_t *x, int incx,
                           const PLASMA_Complex32_t *beta,        PLASMA_Complex32_t *y, int incy,
                           const PLASMA_Complex32_t *Alock,
                           const PLASMA_Complex32_t *xlock,
                           const PLASMA_Complex32_t *ylock)
{
    /* Quick return. Bad things happen if sizeof(...)*m*n is zero in QUARK_Insert_Task */
    if ( m == 0 || n == 0 )
        return;

    DAG_SET_PROPERTIES("gemv", "lightslateblue");
    QUARK_Insert_Task(quark, CORE_cgemv_tile_quark, task_flags,
        sizeof(PLASMA_enum),             &trans,  VALUE,
        sizeof(int),                     &m,      VALUE,
        sizeof(int),                     &n,      VALUE,
        sizeof(PLASMA_Complex32_t),      alpha,           INPUT,
        sizeof(PLASMA_Complex32_t)*m*n,  A,               NODEP,          /* input; see Alock */
        sizeof(int),                     &lda,    VALUE,
        sizeof(PLASMA_Complex32_t)*n,    x,               NODEP,          /* input; see xlock */
        sizeof(int),                     &incx,   VALUE,
        sizeof(PLASMA_Complex32_t),      beta,            INPUT,
        sizeof(PLASMA_Complex32_t)*m,    y,                       NODEP,  /* inout; see ylock */
        sizeof(int),                     &incy,   VALUE,
        sizeof(PLASMA_Complex32_t)*m*n,  Alock,           INPUT,
        sizeof(PLASMA_Complex32_t)*n,    xlock,           INPUT,
        sizeof(PLASMA_Complex32_t)*m,    ylock,                   INOUT,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_cgemv_tile_quark = PCORE_cgemv_tile_quark
#define CORE_cgemv_tile_quark PCORE_cgemv_tile_quark
#endif
void CORE_cgemv_tile_quark(Quark *quark)
{
    PLASMA_enum trans;
    int m, n, lda, incx, incy;
    const PLASMA_Complex32_t *alpha, *beta;
    const PLASMA_Complex32_t *A, *x;
    PLASMA_Complex32_t *y;

    quark_unpack_args_11( quark, trans, m, n, alpha, A, lda, x, incx, beta, y, incy );
    cblas_cgemv(
        CblasColMajor,
        (CBLAS_TRANSPOSE)trans,
        m, n,
        CBLAS_SADDR(*alpha), A, lda,
                             x, incx,
        CBLAS_SADDR(*beta),  y, incy );
}


#include "imd.h"
#include <sys/time.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>

// #define USEFLOAT  // hauptsächlich in der funktion genexptint. Profiling zeigte, dass
                  // hier die meiste zeit verbraucht wird -> float verdoppelt performance

#ifdef USEFLOAT
typedef float Real;
#define REALTYPE MPI_FLOAT
#else
typedef double Real;
#define REALTYPE MPI_DOUBLE
#endif

#ifdef USEFLOAT
  #define EXPR     expf //exp zu floaten ist eine ganz mieeese idee
  #define SQRTR    sqrtf
  #define POWR     powf
  #define LOGR     logf
#else
  #define EXPR     exp
  #define SQRTR    sqrt
  #define POWR     pow
#define LOGR       log
#endif

// *********************************************************
// PHYSICAL CONSTANTS
// *********************************************************
// const double eV2J=1.6021766E-19;
const Real eV2H=0.03674932; //eV to Hartree
const Real colrad_reltol=1e-5;
const Real colrad_abstol=10.0;

// const Real J2eV=6.2415091E18;
const Real planck=6.62607004E-34; // J/s
const Real bohr_radius=0.52917721067E-10; // m
const Real bohr_radius_sq=2.800285202924816e-21;
const Real hbar_cub=1.172812163789953e-102; //hbar^3
const Real double_emass_pow_3_2 = 2.459112949719466e-45; // (2*emass)^3/2
const int MAXLINE = 255;
const Real  pi=3.141592653589793;
const Real  pi_sq=9.869604401089358;
const Real E_ion_H=13.6; // eV
const Real E_ion_H_J=2.178960176000000e-18; // J
const Real E_ion_H_sq_J=4.747867448593952e-36;

const Real colrad_tequi=1e-12;//TEST// 1e-12; //bei initial equi ohne Temperatur-variation erst einmal
                                //die Saha-besetzungsdichten equilibrieren

//const double  LIGHTSPEED=2.997925458e8; // m/s
Real  LASERFREQ;

int colrad_ydot(double t, N_Vector u, N_Vector udot, void *user_data);
void do_Saha(Real Te,Real totalc,Real ne,N_Vector y);
int colrad_GetCoeffs(N_Vector y,Real It, void * user_data);

// Die Zwei müssen nach Prototypes.h
// void do_colrad(double dt);
// void colrad_init(void);

void colrad_read_states(void);
void colrad_Saha_init(int i,int j,int k);

// ******************************************************************************
// *                CROSS SECTION INTEGRATION STUFF
// ******************************************************************************

gsl_integration_workspace * winteg_inner=NULL;
gsl_integration_workspace * winteg_outer=NULL;
gsl_integration_workspace * winteg_fermi=NULL;
gsl_integration_workspace * winteg_exc=NULL; //excitation

gsl_integration_romberg_workspace * winteg_rb_inner=NULL;
gsl_integration_romberg_workspace * winteg_rb_outer=NULL;

struct my_f_params { Real ne; Real T;Real mu; Real E;Real DeltaE; int allowed;};
// struct my_f_params fparams_inner; //For inner integrand
// struct my_f_params fparams_outer; //outer integrand
// struct my_f_params fparams_fermi;
// struct my_f_params fparams_exc;

double inner_integrand_ionization(double x, void *p); // integrate along E'
double outer_integrand_ionization(double x,void *p);  // integrate along E

Real double_integral_ionization(Real ne,Real T, Real mu, Real DeltaE); //evaluates double integral

double inner_integrand_recombination(double x, void *p);
double outer_integrand_recombination(double x,void *p);
Real double_integral_recombination(Real ne,Real T, Real mu, Real DeltaE);

double integrand_excitation(double x,void *p);
Real eval_excitation_integral(Real ne,Real T,Real mu, Real DeltaE, int allowed);

Real eval_dexcitation_integral(Real ne,Real T,Real mu, Real DeltaE, int allowed);
double integrand_deexcitation(double x,void *p);

double fermi_integrand(double x, void *p);
Real eval_fermi_integrand(Real ne,Real T, Real mu);

double integrand_excitation_debug(double x,void *p);

double outer_integrand_ionization2(double x,struct my_f_params* p);
Real double_integral_ionization2(Real ne,Real T, Real mu, Real DeltaE); //evaluates double integral
double inner_integrand_ionization2(double x, struct my_f_params* p);

// **********************************************************************************************
// *          PAR INTEGRAL STUFF
// **********************************************************************************************
int terminate_gkq;
int terminate_gkq_outer;
int terminate_gkq_inner;
int terminate_serial;
int gkq_iter_serial; // nr of iterations

const double gkq_alpha=0.816496580927726;
const double gkq_beta=0.447213595499958;
static const double xgkq[12] =
{
  0.0,
  -0.942882415695480,
  -0.816496580927726,
  -0.641853342345781,
  -0.447213595499958,
  -0.236383199662150,
  0.0,
  0.236383199662150,
  0.447213595499958,
  0.641853342345781,
  0.816496580927726,
  0.942882415695480
};

Real integral_simpson(Real (*f)(Real, void*), Real a, Real b,int n,void* p);

int simpson_error;
const Real tolmax=1e-20;
const Real simpson_itermax=120;
#define INITIAL_STACK_SIZE 128   /* initial size of new stacks */

/* the stack structure */
struct stack_s{
  int el_count;            /* count of elements on stack */
  int el_size;             /* size of an element */
  int mem_reserve;         /* allocated memory for stack */
  void* elements;          /* pointer to begin of stack */
};

typedef struct _work_t{
  double a;
  double b;
  double tol;
  double S;
  double fa;
  double fb;
  double fm;
  double rec;
  int iter;
  struct my_f_params * p; //pointer auf params
} work_t;

typedef struct _work_t_gkq{
  double a;
  double b;
  double toler;
  double I_13;
  double I_prev;
  double fa;
  double fb;
  struct my_f_params * p; //pointer auf params
  shortint iter;
} work_gkq;

typedef struct stack_s* stack_t;
double integral_simpson_par(double (*f)(double, struct my_f_params*), stack_t stack);

double gkq_adapt_OMP(double (*f)(double, struct my_f_params*), stack_t stack);
double gkq_OMP(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p,stack_t stack);

double gkq_serial(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p);
double gkq_adapt_serial(double (*f)(double, struct my_f_params*), double a, double b,
                        double fa,double fb, double toler,double I_13, struct my_f_params* p);

// void create_stack(stack_t* stack, int element_size);
// int  empty_stack(stack_t stack);
// void push_stack(stack_t stack, void* element);
// void pop_stack(stack_t stack, void* element);

/******************************************
 * create new stack
 ******************************************/
void create_stack(
             stack_t* stack,     /* stack to create */
             int element_size)   /* size of a stack element */
{
  int initial_size = INITIAL_STACK_SIZE;

  /* allocate memory for new stack struct */
  (*stack) = (stack_t) malloc(sizeof(struct stack_s));
  if (!(*stack)){
    char errstr[255];
    sprintf(errstr, "error: could not allocate memory for stack.. Abort.\n");
    error(errstr);
    // exit(1);
  }

  /* allocate memory for stack elements */
  (*stack)->elements = (void*) malloc(element_size * initial_size);
  (*stack)->mem_reserve = initial_size;
  if (!(*stack)->elements){
    char errstr[255];
    sprintf(errstr, "error: could not allocate memory for stack.. Abort.\n");
    error(errstr);
  }

  (*stack)->el_size = element_size;
  (*stack)->el_count = 0;

}

/*****************************************
 * check if the stack is empty
 *****************************************/
int empty_stack(stack_t stack)
{
  return stack->el_count <= 0;
}

/*****************************************
 * push a element on stack
 *****************************************/
void push_stack(stack_t stack,    /* target stack */
           void* element)    /* element to push */
{
  int i, new_reserve;
  int log2_count;

  /* check if we need more memory for stack */
  if (stack->el_count >= stack->mem_reserve)
  {

      /* calculate new size for the stack
         it should be a power of two */
      for (i = stack->el_count, log2_count = 0;
           i > 0;
           i>>1, log2_count++);
      new_reserve = 1 << log2_count;

      /* reallocate memory for phase thread tables
         and nullify new values */
      stack->elements = (void *) realloc(stack->elements,
                      stack->el_size * new_reserve);
      if (!stack->elements){
        char errstr [255];
        sprintf(errstr, "error: can't reallocate stack.. Aborting\n");
        error(errstr);
        // exit(1);
      }

      stack->mem_reserve = new_reserve;
  }

  /* now push the element on top of the stack */
  memcpy((char*)stack->elements + stack->el_count*stack->el_size,
            element, stack->el_size);
  stack->el_count++;

}

/*****************************************
 * pop an element from stack
 *****************************************/
void pop_stack(
          stack_t stack,    /* target stack */
          void* element)    /* where poped el. should be stored */
{
  if (stack->el_count <= 0){
    char errstr[255];
    sprintf(errstr, "error: trying to pop from empty stack.\n");
    error(errstr);
    // exit(2);
  }

  stack->el_count--;
  memcpy(element,
          (char*)stack->elements + stack->el_count*stack->el_size,
          stack->el_size);

}

// ***************************************************************************
// *      Gauss-kronard quadrature, parallel
// ***************************************************************************
double gkq_OMP(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p,stack_t stack)
{
  //1st integration
  double result=0.0;
// *********************************************

  double m=0.5*(a+b);
  double h=0.5*(b-a);
  double y[13];
  double fa=y[0]=f(a,p);
  double fb=y[12]=f(b,p);
  int i;
  for(i=1;i<12;i++)
    y[i]=f(m+xgkq[i]*h,p);

  double I_4= (h/6.0)*(y[0]+y[12]+5.0*(y[4]+y[8]));  // 4-point gauss-lobatto
  double I_7= (h/1470.0)*(77.0*(y[0]+y[12])+432.0*(y[2]+y[10])+  // 7-point kronrod
               625.0*(y[4]+y[8])+672.0*y[6]);

  double I_13= h*(0.0158271919734802*(y[0]+y[12])+0.0942738402188500*(y[1]+y[11])+0.155071987336585*(y[2]+y[10])+
                  0.188821573960182*(y[3]+y[9])+0.199773405226859*(y[4]+y[8])+0.224926465333340*(y[5]+y[7])+
                  0.242611071901408*y[6]);   //13-point Kronrod

  double Err1=fabs(I_7-I_13);
  double Err2=fabs(I_4-I_13);

  double r=(Err2 != 0.0) ? Err1/Err2 : 1.0;
  double toler=(r > 0.0 && r < 1.0) ? TOL/r : TOL;

  if(I_13 == 0)
    I_13=b-a;
  I_13=fabs(I_13);

  //Prepare work and push onto stack
  work_gkq work;

  work.a = a;
  work.b = b;
  work.toler = toler;
  work.I_13=I_13;
  work.fa=fa;
  work.fb=fb;
  work.p=p;
  work.I_prev=I_7;

  //ANTI-FOLGENDES:
  //OUT OF TOLERANCE !!!, mll:3.0162e-18, a:3.0162e-18, b:3.0162e-18, mrr:3.0162e-18,I_7-I_4:0.0000e+00, tol:1.6002e-315,I_13:7.0585e-313
  if(I_13 < 1e-150)
    return 0;

  push_stack(stack, &work);
  result=gkq_adapt(f,stack);
  return result;
}

double gkq_serial(double (*f)(double, struct my_f_params*), double a, double b, double TOL, struct my_f_params* p)
{
  //1st integration

  double result=0.0;
  gkq_iter_serial=0;
// *********************************************

  double m=0.5*(a+b);
  double h=0.5*(b-a);
  double y[13];
  double fa=y[0]=f(a,p);
  double fb=y[12]=f(b,p);
  int i;
  for(i=1;i<12;i++)
    y[i]=f(m+xgkq[i]*h,p);

  double I_4= (h/6.0)*(y[0]+y[12]+5.0*(y[4]+y[8]));  // 4-point gauss-lobatto
  double I_7= (h/1470.0)*(77.0*(y[0]+y[12])+432.0*(y[2]+y[10])+  // 7-point kronrod
               625.0*(y[4]+y[8])+672.0*y[6]);

  double I_13= h*(0.0158271919734802*(y[0]+y[12])+0.0942738402188500*(y[1]+y[11])+0.155071987336585*(y[2]+y[10])+
                  0.188821573960182*(y[3]+y[9])+0.199773405226859*(y[4]+y[8])+0.224926465333340*(y[5]+y[7])+
                  0.242611071901408*y[6]);   //13-point Kronrod

  double Err1=fabs(I_7-I_13);
  double Err2=fabs(I_4-I_13);

  double r=(Err2 != 0.0) ? Err1/Err2 : 1.0;
  double toler=(r > 0.0 && r < 1.0) ? TOL/r : TOL;

  if(I_13 == 0)
    I_13=b-a;
  I_13=fabs(I_13);

  result=gkq_adapt_serial(f,a,b,fa,fb,toler,I_13, p);

  return result;
}

// ***********************************************
// * RECURSIVE ADAPTION ROUTINE FOR PARALLEL-GK-QUADRATURE
// **********************************************
double gkq_adapt_OMP(double (*f)(double, struct my_f_params*), stack_t stack)
{
  work_gkq work;
  work.iter=0;
  int ready, idle, busy;
  double integral_result = 0.0;

  busy = 0;
  terminate_gkq=0;

#pragma omp parallel default(none) \
    shared(stack, integral_result,f,busy,terminate_gkq,myid) \
    private(work, idle, ready)
  {
// printf("me:%d, err:%d\n",omp_get_thread_num(),simpson_error);

    ready = 0;
    idle = 1;

    while(!ready) // && !terminate_gkq)//  && !simpson_error) //<-- so NICHT!
    {
      #pragma omp critical (stack)
      {
        if (!empty_stack(stack))
        {
          /* we have new work */
          pop_stack(stack, &work);
          if (idle)
          {
            /* say others i'm busy */
            busy += 1;
            idle = 0;
          }
        }
        else
        {
          /* no new work on stack */
          if (!idle){
            busy -= 1;
            idle = 1;
          }

          /* nobody has anything to do; let us leave the loop */
          if (busy == 0)
          {
            ready = 1;
          }
        }
      } /* end critical(stack) */

      if (idle)
        continue; //if ready==1 --> leave loop
double I_prev=work.I_prev;

      double a = work.a;
      double b = work.b;
      double toler = work.toler;
      double I_13=work.I_13;
      double fa=work.fa;
      double fb=work.fb;
      int iter=work.iter;
      // double *y= work.y; // brauch ich nicht!
      struct my_f_params * p = work.p;

      double m = (a+b)/2;
      double h = (b -a)/2;
      double mll=m-gkq_alpha*h;
      double ml=m-gkq_beta*h;
      double mr=m+gkq_beta*h;
      double mrr=m+gkq_alpha*h;

      double fmll=f(mll,p);
      double fml=f(ml,p);
      double fm=f(m,p);
      double fmr=f(mr,p);
      double fmrr=f(mrr,p);
      double I_4=h/6.0*(fa+fb+5.0*(fml+fmr));   // 4-point Gauss-Lobatto formula.
      double I_7=h/1470.0*(77.0*(fa+fb)+432.0*(fmll+fmrr)+625.0*(fml+fmr)+672.0*fm);

// if(myid==1)
//   printf("I_7:%.4e, I_13:%.4e,I_4:%.4e, minus:%.4e, to:%.4e\n",I_7,I_13,I_4,I_7-I_4, toler*I_13);
int maxiter=50; //max. subdivisions
double abstol=1e-30;
work.I_prev=I_7; // für abstolcheck in nächster recursion

      if (fabs(I_7-I_4) <= toler*I_13 || mll <= a || b <= mrr || iter > maxiter || fabs(I_7-I_prev) < abstol )
      {
        if ((mll <= a || b <= mrr)) //Error
         {
           // out_of_tolerance=true; // Interval contains no more machine numbers
           // printf("OUT OF TOLERANCE !!!, mll:%.4e, a:%.4e, b:%.4e, mrr:%.4e,I_7-I_4:%.4e, tol:%.4e,I_13:%.4e\n",
           //        mll,b,b,mrr,I_7-I_4, toler*I_13,I_13);
           terminate_gkq=1;
         }
        #pragma omp critical (integral_result)
        {
          integral_result += I_7;      //Terminate recursion.
        }
// printf("me ok:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f,ubteg;%.4e\n", omp_get_thread_num(), a,b,toler,I_4,I_7,integral_result);

      }
      else  //subdivide interval and push new work on stack
      {
        #pragma omp critical (stack)
        {

          // printf("me NOOOO:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f\n", omp_get_thread_num(), a,b,toler,I_4,I_7);

          work.iter=iter+1;

          work.a=a;
          work.b=mll;
          work.fa=fa;
          work.fb=fmll;
          push_stack(stack, &work);

          work.a=mll;
          work.b=ml;
          work.fa=fmll;
          work.fb=fml;
          push_stack(stack, &work);

          work.a=ml;
          work.b=m;
          work.fa=fml;
          work.fb=fm;
          push_stack(stack, &work);

          work.a=m;
          work.b=mr;
          work.fa=fm;
          work.fb=fmr;
          push_stack(stack, &work);

          work.a=mr;
          work.b=mrr;
          work.fa=fmr;
          work.fb=fmrr;
          push_stack(stack, &work);

          work.a=mrr;
          work.b=b;
          work.fa=fmrr;
          work.fb=fb;
          push_stack(stack, &work);

        } // pragma critical stack
      }   // else ..non-acceptable error
    } // while
  } /* end omp parallel */
  return integral_result;
}

double gkq_adapt_serial(double (*f)(double, struct my_f_params*), double a, double b, double fa,
                        double fb, double toler,double I_13, struct my_f_params* p)
{
  double m = (a+b)/2;
  double h = (b -a)/2;
  double mll=m-gkq_alpha*h;
  double ml=m-gkq_beta*h;
  double mr=m+gkq_beta*h;
  double mrr=m+gkq_alpha*h;

  double fmll=f(mll,p);
  double fml=f(ml,p);
  double fm=f(m,p);
  double fmr=f(mr,p);
  double fmrr=f(mrr,p);
  double I_4=h/6.0*(fa+fb+5.0*(fml+fmr));   // 4-point Gauss-Lobatto formula.
  double I_7=h/1470.0*(77.0*(fa+fb)+432.0*(fmll+fmrr)+625.0*(fml+fmr)+672.0*fm);

  gkq_iter_serial++;

  if ( (fabs(I_7-I_4) <= toler*I_13 || mll <= a || b <= mrr) && gkq_iter_serial)
  {
    if ((mll <= a || b <= mrr) && !terminate_serial) //Error
     {
      // out_of_tolerance=true; // Interval contains no more machine numbers
      printf("OUT OF TOLERANCE !!!, mll:%.4e, a:%.4e, b:%.4e, mrr:%.4e\n", mll,b,b,mrr);
      terminate_serial=1;
     }

// printf("me ok:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f\n", omp_get_thread_num(), a,b,toler,I_4,I_7);
    return I_7;
  }
  else
  {

    // printf("me NOOOO:%d, a:%f,b:%f, tler:%.5e,I_4:%f,I_7:%f\n", omp_get_thread_num(), a,b,toler,I_4,I_7);

    return  gkq_adapt_serial(f, a,mll,fa,fmll,toler,I_13,p) +
            gkq_adapt_serial(f, mll,ml,fmll,fml,toler,I_13,p) +
            gkq_adapt_serial(f, ml,m,fml,fm,toler,I_13,p) +
            gkq_adapt_serial(f, m,mr,fm,fmr,toler,I_13,p) +
            gkq_adapt_serial(f, mr,mrr,fmr,fmrr,toler,I_13,p) +
            gkq_adapt_serial(f, mrr,b,fmrr,fb,toler,I_13,p);
  }

}


#ifndef areaproblemdata_h
#define areaproblemdata_h

#include <ceed.h>
#include <petsc.h>
#include "../include/structs.h"
#include "../qfunctions/area/areacube.h"
#include "../qfunctions/area/areasphere.h"

// -----------------------------------------------------------------------------
// Problem Option Data
// -----------------------------------------------------------------------------

// Problem options
typedef enum {
  CUBE = 0, SPHERE = 1
} ProblemType;

static BPData problem_options[6] = {
  [CUBE] = {
    .num_comp_x = 3,
    .num_comp_u = 1,
    .topo_dim = 2,
    .q_data_size = 1,
    .q_extra = 1,
    .setup_geo = SetupMassGeoCube,
    .apply = Mass,
    .setup_geo_loc = SetupMassGeoCube_loc,
    .apply_loc = Mass_loc,
    .in_mode = CEED_EVAL_INTERP,
    .out_mode = CEED_EVAL_INTERP,
    .q_mode = CEED_GAUSS,
    .enforce_bc = PETSC_FALSE,
  },
  [SPHERE] = {
    .num_comp_x = 3,
    .num_comp_u = 1,
    .topo_dim = 2,
    .q_data_size = 1,
    .q_extra = 1,
    .setup_geo = SetupMassGeoSphere,
    .apply = Mass,
    .setup_geo_loc = SetupMassGeoSphere_loc,
    .apply_loc = Mass_loc,
    .in_mode = CEED_EVAL_INTERP,
    .out_mode = CEED_EVAL_INTERP,
    .q_mode = CEED_GAUSS,
    .enforce_bc = PETSC_FALSE,
  }
};

#endif // areaproblemdata_h