#include <stdio.h>
#include <malloc.h>
#include <stdlib.h>
#include <stdbool.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <functions.h>
#define FOR(i,n) for(i=0;i<n;i++)
typedef double DD, LD;
typedef long LL;

void quicksort_mds(double* x, long first, long last)
{
	long pivot, j, i;
	double temp;
	if (first<last)
	{
		pivot = first;
		i = first;
		j = last;
		while (i<j)
		{
			while (x[i]<=x[pivot] && i<last)
				i++;
			while (x[j]>x[pivot])
				j--;
			if (i<j)
			{
				temp = x[i];
				x[i] = x[j];
				x[j] = temp;
			}
		}
		temp = x[pivot];
		x[pivot] = x[j];
		x[j] = temp;
		quicksort(x, first, j - 1);
		quicksort(x, j + 1, last);

	}
}

void find_eigen(double **mat, int n, double*** res_vec, double** res_val)
{
	int i, j;
	double *data = (double*)calloc(((long)(n*n)), sizeof(double));
	double *eig_values = (double*)calloc(n, sizeof(double));
	double** eig_vec = (double**)calloc(n, sizeof(double*));
	for (i = 0; i<n; i++)eig_vec[i] = (double*)calloc(n, sizeof(double));
	for (i = 0; i<n; i++)
	{
		for (j = 0; j<n; j++)
		{
			data[i*n + j] = mat[i][j];
		}
	}
	gsl_matrix_view m= gsl_matrix_view_array(data, n, n);
	gsl_vector *eval = gsl_vector_alloc(n);
	gsl_matrix *evec = gsl_matrix_alloc(n, n);
	gsl_eigen_symmv_workspace * w =gsl_eigen_symmv_alloc(n);
	gsl_eigen_symmv(&m.matrix, eval, evec, w);
	gsl_eigen_symmv_free(w);
	gsl_eigen_symmv_sort(eval, evec,GSL_EIGEN_SORT_VAL_ASC);
	for (i = 0; i<n; i++)
	{
		eig_values[i] = gsl_vector_get(eval, i);
		for (j = 0; j<n; j++)
		{
			eig_vec[i][j] = gsl_matrix_get(evec, i, j);
		}
	}
	*res_val = eig_values;
	*res_vec = eig_vec;
	gsl_vector_free(eval);
	gsl_matrix_free(evec);
	free(data);
}

DD** Multiply_Matrices_mds(DD **Mat1, DD **Mat2, LL M, LL Q, LL P)
{
	LL i, c, d, k;
	DD Sum = 0;
	DD** Temp = (DD**)calloc(M, sizeof(DD*));
	FOR(i, M)Temp[i] = (DD*)calloc(Q, sizeof(DD));
	FOR(c, M)
	{
		FOR(d, Q)
		{
			FOR(k, P)
			{
				Sum += (Mat1[c][k] * Mat2[k][d]);
			}
			Temp[c][d] = Sum;
			Sum = 0;
		}
	}
	return Temp;
}

double ** H_mat(long n)
{
	long i, j;
	DD** k = (DD**)calloc(n, sizeof(DD*));
	FOR(i, n)
	{
		k[i] = (DD*)calloc(n, sizeof(DD));
	}
	FOR(i, n){
		FOR(j, n){ k[i][j] = (i == j) ? (1.00 - 1.00 / n) : (-1.00 / n);
		}
    }
	return k;
}
typedef struct{
	MAT Y;
	DD* eigen;
	LL number;
} Output_mds;

Output_mds MDS(DD** D,long rows,long columns, DD delta)
{
	//      ************** function to calculate Classical Multidimensional Scaling *************
	long  i, j, n;
	DD    **H, **new_D, **K, **new_k;
	new_D = (DD**)calloc(rows, sizeof(DD*));
	n = max(rows, columns);
	FOR(i, rows)
	{
		new_D[i] = (DD*)calloc(columns, sizeof(DD));
	}
	if (delta >= 0)
	{
		H = H_mat(n);
		new_k = (DD**)calloc(n, sizeof(DD*));
		FOR(i, rows)
		{
			new_k[i] = (DD*)calloc(n, sizeof(DD));
		}
		FOR(i, rows)
		{
			FOR(j, columns)
				new_D[i][j] = -0.5*D[i][j];
		}
		K = Multiply_Matrices_mds(Multiply_Matrices_mds(H, new_D, n, columns, n), H, columns, n, n);
        FOR(i,rows)free(new_D[i]);free(new_D);
        FOR(i,n)free(H[i]);free(H);
		DD* idx = (DD*)calloc(n, sizeof(DD));
		DD  ** temp, *val;
		FOR(i, n)
		{
			idx[i] = n - i;
			FOR(j, rows)
				new_k[i][j] = (K[i][j] + K[j][i]) / 2;
		}
		find_eigen(new_k, n, &temp, &val);
		find_eigen(new_k, n, &temp, &val);
		j = n - 1;
		for (i = 0; i < n / 2; i++)
		{
			DD temporary = val[i];
			val[i] = val[j];
			val[j] = temporary;
			j--;
		}
		LL* keep;
		DD max_val = val[0];
		LL magn_keep = 0;
		FOR(i, n){
			if (val[i] > max_val)max_val = val[i];
		}
		max_val = max_val*exp(-10);
		FOR(i, n){
			if (val[i] > max_val)magn_keep++;
		}
		keep = (LL*)calloc(magn_keep, sizeof(LL));
		magn_keep = 0;
		FOR(i, n){
			if (val[i] > max_val){
				keep[magn_keep++] = i;
			}
		}
		DD** V = (DD**)calloc(n, sizeof(DD*));
		DD** mult_V = (DD**)calloc(magn_keep, sizeof(DD*));
		FOR(i, n)V[i] = (DD*)calloc(magn_keep, sizeof(DD));
		FOR(i, magn_keep)
		{
			mult_V[i] = (DD*)calloc(magn_keep, sizeof(DD));
			FOR(j, n)
			{
				if (j < magn_keep)
				{
					if (i != j)mult_V[i][j] = 0;
					else mult_V[i][j] = sqrt(val[keep[i]]);
				}
				V[j][i] = temp[j][(int)idx[keep[i]]-1];
			}
		}
		V = Multiply_Matrices_mds(V, mult_V, n, magn_keep, magn_keep);
		MAT return_MAT;
		return_MAT.matrix = V;
		return_MAT.rows = n;
		return_MAT.columns = magn_keep;
		Output_mds t;
		t.Y = return_MAT; t.eigen = val; t.number=n;
		return t;
	}
	Output_mds t;
    return t;
}


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:45:10 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -hc2hc-backward 4 */

/*
 * This function contains 34 FP additions, 18 FP multiplications,
 * (or, 28 additions, 12 multiplications, 6 fused multiply/add),
 * 15 stack variables, and 32 memory accesses
 */
static const fftw_real K1_414213562 = FFTW_KONST(+1.414213562373095048801688724209698078569671875);
static const fftw_real K2_000000000 = FFTW_KONST(+2.000000000000000000000000000000000000000000000);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_hc2hc_backward_4(fftw_real *A, const fftw_complex *W, int iostride, int m, int dist)
{
     int i;
     fftw_real *X;
     fftw_real *Y;
     X = A;
     Y = A + (4 * iostride);
     {
	  fftw_real tmp39;
	  fftw_real tmp42;
	  fftw_real tmp37;
	  fftw_real tmp40;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp38;
	       fftw_real tmp41;
	       fftw_real tmp35;
	       fftw_real tmp36;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp38 = X[iostride];
	       tmp39 = K2_000000000 * tmp38;
	       tmp41 = Y[-iostride];
	       tmp42 = K2_000000000 * tmp41;
	       tmp35 = X[0];
	       tmp36 = X[2 * iostride];
	       tmp37 = tmp35 + tmp36;
	       tmp40 = tmp35 - tmp36;
	  }
	  X[2 * iostride] = tmp37 - tmp39;
	  X[0] = tmp37 + tmp39;
	  X[3 * iostride] = tmp40 + tmp42;
	  X[iostride] = tmp40 - tmp42;
     }
     X = X + dist;
     Y = Y - dist;
     for (i = 2; i < m; i = i + 2, X = X + dist, Y = Y - dist, W = W + 3) {
	  fftw_real tmp9;
	  fftw_real tmp28;
	  fftw_real tmp18;
	  fftw_real tmp25;
	  fftw_real tmp12;
	  fftw_real tmp24;
	  fftw_real tmp21;
	  fftw_real tmp29;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp7;
	       fftw_real tmp8;
	       fftw_real tmp16;
	       fftw_real tmp17;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp7 = X[0];
	       tmp8 = Y[-2 * iostride];
	       tmp9 = tmp7 + tmp8;
	       tmp28 = tmp7 - tmp8;
	       tmp16 = Y[0];
	       tmp17 = X[2 * iostride];
	       tmp18 = tmp16 - tmp17;
	       tmp25 = tmp16 + tmp17;
	  }
	  {
	       fftw_real tmp10;
	       fftw_real tmp11;
	       fftw_real tmp19;
	       fftw_real tmp20;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp10 = X[iostride];
	       tmp11 = Y[-3 * iostride];
	       tmp12 = tmp10 + tmp11;
	       tmp24 = tmp10 - tmp11;
	       tmp19 = Y[-iostride];
	       tmp20 = X[3 * iostride];
	       tmp21 = tmp19 - tmp20;
	       tmp29 = tmp19 + tmp20;
	  }
	  X[0] = tmp9 + tmp12;
	  {
	       fftw_real tmp14;
	       fftw_real tmp22;
	       fftw_real tmp13;
	       fftw_real tmp15;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp14 = tmp9 - tmp12;
	       tmp22 = tmp18 - tmp21;
	       tmp13 = c_re(W[1]);
	       tmp15 = c_im(W[1]);
	       X[2 * iostride] = (tmp13 * tmp14) + (tmp15 * tmp22);
	       Y[-iostride] = (tmp13 * tmp22) - (tmp15 * tmp14);
	  }
	  Y[-3 * iostride] = tmp18 + tmp21;
	  {
	       fftw_real tmp26;
	       fftw_real tmp30;
	       fftw_real tmp23;
	       fftw_real tmp27;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp26 = tmp24 + tmp25;
	       tmp30 = tmp28 - tmp29;
	       tmp23 = c_re(W[0]);
	       tmp27 = c_im(W[0]);
	       Y[-2 * iostride] = (tmp23 * tmp26) - (tmp27 * tmp30);
	       X[iostride] = (tmp27 * tmp26) + (tmp23 * tmp30);
	  }
	  {
	       fftw_real tmp32;
	       fftw_real tmp34;
	       fftw_real tmp31;
	       fftw_real tmp33;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp32 = tmp25 - tmp24;
	       tmp34 = tmp28 + tmp29;
	       tmp31 = c_re(W[2]);
	       tmp33 = c_im(W[2]);
	       Y[0] = (tmp31 * tmp32) - (tmp33 * tmp34);
	       X[3 * iostride] = (tmp33 * tmp32) + (tmp31 * tmp34);
	  }
     }
     if (i == m) {
	  fftw_real tmp1;
	  fftw_real tmp2;
	  fftw_real tmp3;
	  fftw_real tmp4;
	  fftw_real tmp5;
	  fftw_real tmp6;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = X[0];
	  tmp2 = X[iostride];
	  tmp3 = tmp1 - tmp2;
	  tmp4 = Y[0];
	  tmp5 = Y[-iostride];
	  tmp6 = tmp4 + tmp5;
	  X[0] = K2_000000000 * (tmp1 + tmp2);
	  X[2 * iostride] = -(K2_000000000 * (tmp4 - tmp5));
	  X[iostride] = K1_414213562 * (tmp3 - tmp6);
	  X[3 * iostride] = -(K1_414213562 * (tmp3 + tmp6));
     }
}

static const int twiddle_order[] =
{1, 2, 3};
fftw_codelet_desc fftw_hc2hc_backward_4_desc =
{
     "fftw_hc2hc_backward_4",
     (void (*)()) fftw_hc2hc_backward_4,
     4,
     FFTW_BACKWARD,
     FFTW_HC2HC,
     102,
     3,
     twiddle_order,
};


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C header for the geometric coefficients entering the response for LIGO-VIRGO detectors.
 *
 *
 */

#ifndef _LLVGEOMETRY_H
#define _LLVGEOMETRY_H

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"

/********************************************************/
/**************** Type definitions **********************/

/* Enumerator for the detector selector and network selector */
typedef enum {LHO, LLO, VIRGO} Detectortag;
typedef enum {L, H, V, LH, LV, HV, LHV} Networktag;

/*************************************************************/
/**************** Geometrical constants **********************/

/**
 * \name LIGO Hanford Observatory 4km Interferometric Detector constants
 * The following constants describe the location and geometry of the
 * LIGO Hanford Observatory 4km Interferometric Detector.
 */
#define LAL_LHO_4K_DETECTOR_NAME               	"LHO_4k"	/**< LHO_4k detector name string */
#define LAL_LHO_4K_DETECTOR_PREFIX             	"H1"	/**< LHO_4k detector prefix string */
#define LAL_LHO_4K_DETECTOR_LONGITUDE_RAD      	-2.08405676917	/**< LHO_4k vertex longitude (rad) */
#define LAL_LHO_4K_DETECTOR_LATITUDE_RAD       	0.81079526383	/**< LHO_4k vertex latitude (rad) */
#define LAL_LHO_4K_DETECTOR_ELEVATION_SI       	142.554	/**< LHO_4k vertex elevation (m) */
#define LAL_LHO_4K_DETECTOR_ARM_X_AZIMUTH_RAD  	5.65487724844	/**< LHO_4k x arm azimuth (rad) */
#define LAL_LHO_4K_DETECTOR_ARM_Y_AZIMUTH_RAD  	4.08408092164	/**< LHO_4k y arm azimuth (rad) */
#define LAL_LHO_4K_DETECTOR_ARM_X_ALTITUDE_RAD 	-0.00061950000	/**< LHO_4k x arm altitude (rad) */
#define LAL_LHO_4K_DETECTOR_ARM_Y_ALTITUDE_RAD 	0.00001250000	/**< LHO_4k y arm altitude (rad) */
#define LAL_LHO_4K_DETECTOR_ARM_X_MIDPOINT_SI  	1997.54200000000	/**< LHO_4k x arm midpoint (m) */
#define LAL_LHO_4K_DETECTOR_ARM_Y_MIDPOINT_SI  	1997.52200000000	/**< LHO_4k y arm midpoint (m) */
#define LAL_LHO_4K_VERTEX_LOCATION_X_SI        	-2.16141492636e+06	/**< LHO_4k x-component of vertex location in Earth-centered frame (m) */
#define LAL_LHO_4K_VERTEX_LOCATION_Y_SI        	-3.83469517889e+06	/**< LHO_4k y-component of vertex location in Earth-centered frame (m) */
#define LAL_LHO_4K_VERTEX_LOCATION_Z_SI        	4.60035022664e+06	/**< LHO_4k z-component of vertex location in Earth-centered frame (m) */
#define LAL_LHO_4K_ARM_X_DIRECTION_X           	-0.22389266154	/**< LHO_4k x-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_LHO_4K_ARM_X_DIRECTION_Y           	0.79983062746	/**< LHO_4k y-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_LHO_4K_ARM_X_DIRECTION_Z           	0.55690487831	/**< LHO_4k z-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_LHO_4K_ARM_Y_DIRECTION_X           	-0.91397818574	/**< LHO_4k x-component of unit vector pointing along y arm in Earth-centered frame */
#define LAL_LHO_4K_ARM_Y_DIRECTION_Y           	0.02609403989	/**< LHO_4k y-component of unit vector pointing along y arm in Earth-centered frame */
#define LAL_LHO_4K_ARM_Y_DIRECTION_Z           	-0.40492342125	/**< LHO_4k z-component of unit vector pointing along y arm in Earth-centered frame */

/**
 * \name LIGO Livingston Observatory 4km Interferometric Detector constants
 * The following constants describe the location and geometry of the
 * LIGO Livingston Observatory 4km Interferometric Detector.
 */
#define LAL_LLO_4K_DETECTOR_NAME               	"LLO_4k"	/**< LLO_4k detector name string */
#define LAL_LLO_4K_DETECTOR_PREFIX             	"L1"	/**< LLO_4k detector prefix string */
#define LAL_LLO_4K_DETECTOR_LONGITUDE_RAD      	-1.58430937078	/**< LLO_4k vertex longitude (rad) */
#define LAL_LLO_4K_DETECTOR_LATITUDE_RAD       	0.53342313506	/**< LLO_4k vertex latitude (rad) */
#define LAL_LLO_4K_DETECTOR_ELEVATION_SI       	-6.574	/**< LLO_4k vertex elevation (m) */
#define LAL_LLO_4K_DETECTOR_ARM_X_AZIMUTH_RAD  	4.40317772346	/**< LLO_4k x arm azimuth (rad) */
#define LAL_LLO_4K_DETECTOR_ARM_Y_AZIMUTH_RAD  	2.83238139666	/**< LLO_4k y arm azimuth (rad) */
#define LAL_LLO_4K_DETECTOR_ARM_X_ALTITUDE_RAD 	-0.00031210000	/**< LLO_4k x arm altitude (rad) */
#define LAL_LLO_4K_DETECTOR_ARM_Y_ALTITUDE_RAD 	-0.00061070000	/**< LLO_4k y arm altitude (rad) */
#define LAL_LLO_4K_DETECTOR_ARM_X_MIDPOINT_SI  	1997.57500000000	/**< LLO_4k x arm midpoint (m) */
#define LAL_LLO_4K_DETECTOR_ARM_Y_MIDPOINT_SI  	1997.57500000000	/**< LLO_4k y arm midpoint (m) */
#define LAL_LLO_4K_VERTEX_LOCATION_X_SI        	-7.42760447238e+04	/**< LLO_4k x-component of vertex location in Earth-centered frame (m) */
#define LAL_LLO_4K_VERTEX_LOCATION_Y_SI        	-5.49628371971e+06	/**< LLO_4k y-component of vertex location in Earth-centered frame (m) */
#define LAL_LLO_4K_VERTEX_LOCATION_Z_SI        	3.22425701744e+06	/**< LLO_4k z-component of vertex location in Earth-centered frame (m) */
#define LAL_LLO_4K_ARM_X_DIRECTION_X           	-0.95457412153	/**< LLO_4k x-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_LLO_4K_ARM_X_DIRECTION_Y           	-0.14158077340	/**< LLO_4k y-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_LLO_4K_ARM_X_DIRECTION_Z           	-0.26218911324	/**< LLO_4k z-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_LLO_4K_ARM_Y_DIRECTION_X           	0.29774156894	/**< LLO_4k x-component of unit vector pointing along y arm in Earth-centered frame */
#define LAL_LLO_4K_ARM_Y_DIRECTION_Y           	-0.48791033647	/**< LLO_4k y-component of unit vector pointing along y arm in Earth-centered frame */
#define LAL_LLO_4K_ARM_Y_DIRECTION_Z           	-0.82054461286	/**< LLO_4k z-component of unit vector pointing along y arm in Earth-centered frame */

/**
 * \name VIRGO 3km Interferometric Detector constants
 * The following constants describe the location and geometry of the
 * VIRGO 3km Interferometric Detector.
 */
#define LAL_VIRGO_DETECTOR_NAME               	"VIRGO"	/**< VIRGO detector name string */
#define LAL_VIRGO_DETECTOR_PREFIX             	"V1"	/**< VIRGO detector prefix string */
#define LAL_VIRGO_DETECTOR_LONGITUDE_RAD      	0.18333805213	/**< VIRGO vertex longitude (rad) */
#define LAL_VIRGO_DETECTOR_LATITUDE_RAD       	0.76151183984	/**< VIRGO vertex latitude (rad) */
#define LAL_VIRGO_DETECTOR_ELEVATION_SI       	51.884	/**< VIRGO vertex elevation (m) */
#define LAL_VIRGO_DETECTOR_ARM_X_AZIMUTH_RAD  	0.33916285222	/**< VIRGO x arm azimuth (rad) */
#define LAL_VIRGO_DETECTOR_ARM_Y_AZIMUTH_RAD  	5.05155183261	/**< VIRGO y arm azimuth (rad) */
#define LAL_VIRGO_DETECTOR_ARM_X_ALTITUDE_RAD 	0.00000000000	/**< VIRGO x arm altitude (rad) */
#define LAL_VIRGO_DETECTOR_ARM_Y_ALTITUDE_RAD 	0.00000000000	/**< VIRGO y arm altitude (rad) */
#define LAL_VIRGO_DETECTOR_ARM_X_MIDPOINT_SI  	1500.00000000000	/**< VIRGO x arm midpoint (m) */
#define LAL_VIRGO_DETECTOR_ARM_Y_MIDPOINT_SI  	1500.00000000000	/**< VIRGO y arm midpoint (m) */
#define LAL_VIRGO_VERTEX_LOCATION_X_SI        	4.54637409900e+06	/**< VIRGO x-component of vertex location in Earth-centered frame (m) */
#define LAL_VIRGO_VERTEX_LOCATION_Y_SI        	8.42989697626e+05	/**< VIRGO y-component of vertex location in Earth-centered frame (m) */
#define LAL_VIRGO_VERTEX_LOCATION_Z_SI        	4.37857696241e+06	/**< VIRGO z-component of vertex location in Earth-centered frame (m) */
#define LAL_VIRGO_ARM_X_DIRECTION_X           	-0.70045821479	/**< VIRGO x-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_VIRGO_ARM_X_DIRECTION_Y           	0.20848948619	/**< VIRGO y-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_VIRGO_ARM_X_DIRECTION_Z           	0.68256166277	/**< VIRGO z-component of unit vector pointing along x arm in Earth-centered frame */
#define LAL_VIRGO_ARM_Y_DIRECTION_X           	-0.05379255368	/**< VIRGO x-component of unit vector pointing along y arm in Earth-centered frame */
#define LAL_VIRGO_ARM_Y_DIRECTION_Y           	-0.96908180549	/**< VIRGO y-component of unit vector pointing along y arm in Earth-centered frame */
#define LAL_VIRGO_ARM_Y_DIRECTION_Z           	0.24080451708	/**< VIRGO z-component of unit vector pointing along y arm in Earth-centered frame */

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _LLVGEOMETRY_H */


/*
 * BRAINS
 * (B)LR (R)everberation-mapping (A)nalysis (I)n AGNs with (N)ested (S)ampling
 * Yan-Rong Li, liyanrong@ihep.ac.cn
 * Thu, Aug 4, 2016
 */

/*! \file dnest_sa.c
 *  \brief run dnest sampling for sa analysis.
 */

#ifdef SA

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
 #include <stddef.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_interp.h>
#include <mpi.h>

#include "brains.h"

DNestFptrSet *fptrset_sa;

int dnest_sa(int argc, char **argv)
{
  int i;
  double logz_sa;

  set_sa_blr_model();

  num_params_blr = 0;  /* RM parameter numbers */
  num_params_sa_blr = num_params_sa_blr_model + num_params_sa_extpar;
  num_params_blr_tot = num_params_sa_blr;
  num_params_sa = num_params_sa_blr;
  num_params = num_params_sa;

  par_fix = (int *) malloc(num_params * sizeof(int));
  par_fix_val = (double *) malloc(num_params * sizeof(double));
  par_range_model = malloc( num_params * sizeof(double *));
  par_prior_gaussian = malloc( num_params * sizeof(double *));
  for(i=0; i<num_params; i++)
  {
    par_range_model[i] = malloc(2*sizeof(double));
    par_prior_gaussian[i] = malloc(2*sizeof(double));
  }
  par_prior_model = malloc( num_params * sizeof(int));

  fptrset_sa = dnest_malloc_fptrset();
  /* setup functions used for dnest*/
  fptrset_sa->from_prior = from_prior_sa;
  fptrset_sa->print_particle = print_particle_sa;
  fptrset_sa->restart_action = restart_action_sa;
  fptrset_sa->accept_action = accept_action_sa;
  fptrset_sa->kill_action = kill_action_sa;
  fptrset_sa->perturb = perturb_sa;
  fptrset_sa->read_particle = read_particle_sa;

  fptrset_sa->log_likelihoods_cal_initial = log_likelihoods_cal_initial_sa;
  fptrset_sa->log_likelihoods_cal_restart = log_likelihoods_cal_restart_sa;
  fptrset_sa->log_likelihoods_cal = log_likelihoods_cal_sa;

  set_par_range_sa();
  set_par_fix_sa_blrmodel();

  for(i=num_params_sa_blr_model; i<num_params; i++)
  {
    par_fix[i] = 0;
    par_fix_val[i] = -DBL_MAX;
  }

  /* fix DA */
  par_fix[num_params_sa_blr_model] = 1;
  par_fix_val[num_params_sa_blr_model] = log(550.0);

  /* fix FA */
  par_fix[num_params_sa_blr_model+2] = 1;
  par_fix_val[num_params_sa_blr_model+2] = log(1.0);

  print_par_names_sa();

  force_update = parset.flag_force_update;
  if(parset.flag_para_name != 1)
    logz_sa = dnest(argc, argv, fptrset_sa, num_params, dnest_options_file);

  dnest_free_fptrset(fptrset_sa);

  return 0;
}

void set_par_range_sa()
{
  int i;

  /* setup parameter range, BLR parameters first */
  for(i=0; i<num_params_sa_blr_model; i++)
  {
    par_range_model[i][0] = sa_blr_range_model[i][0];
    par_range_model[i][1] = sa_blr_range_model[i][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;
  }

  /* sa extra parameters */
  for(i=num_params_sa_blr_model; i<num_params_sa; i++)
  {
    par_range_model[i][0] = sa_extpar_range[i-num_params_sa_blr_model][0];
    par_range_model[i][1] = sa_extpar_range[i-num_params_sa_blr_model][1];

    par_prior_model[i] = UNIFORM;
    par_prior_gaussian[i][0] = 0.0;
    par_prior_gaussian[i][1] = 0.0;
  }
  return;
}

/*!
 *  print names and prior ranges for parameters
 *
 */
void print_par_names_sa()
{
  if(thistask != roottask)
    return;

  int i, j;
  FILE *fp;
  char fname[BRAINS_MAX_STR_LENGTH], str_fmt[BRAINS_MAX_STR_LENGTH];

  sprintf(fname, "%s/%s", parset.file_dir, "data/para_names_sa.txt");
  fp = fopen(fname, "w");
  if(fp == NULL)
  {
    fprintf(stderr, "# Error: Cannot open file %s.\n", fname);
    exit(0);
  }

  strcpy(str_fmt, "%4d %-15s %10.6f %10.6f %4d %4d %15.6e\n");

  printf("# Print parameter name in %s\n", fname);

  fprintf(fp, "#*************************************************\n");
  fprint_version(fp);
  fprintf(fp, "#*************************************************\n");

  fprintf(fp, "%4s %-15s %10s %10s %4s %4s %15s\n", "#", "Par", "Min", "Max", "Prior", "Fix", "Val");

  i=-1;
  for(j=0; j<num_params_sa_blr_model; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "SA BLR model", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }

  for(j=0; j<num_params_sa_extpar; j++)
  {
    i++;
    fprintf(fp, str_fmt, i, "SA ExtPar", par_range_model[i][0], par_range_model[i][1], par_prior_model[i],
                            par_fix[i], par_fix_val[i]);
  }
  fclose(fp);
  return;
}

/*!
 * this function generates a sample from prior.
 */
void from_prior_sa(void *model)
{
  int i;
  double *pm = (double *)model;

  for(i=0; i<num_params; i++)
  {
    if(par_prior_model[i] == GAUSSIAN)
    {
      pm[i] = dnest_randn()*par_prior_gaussian[i][1] + par_prior_gaussian[i][0];
      dnest_wrap(&pm[i], par_range_model[i][0], par_range_model[i][1]);
    }
    else
    {
      pm[i] = par_range_model[i][0] + dnest_rand() * (par_range_model[i][1] - par_range_model[i][0]);
    }
  }

  /* cope with fixed parameters */
  for(i=0; i<num_params; i++)
  {
    if(par_fix[i] == 1)
      pm[i] = par_fix_val[i];
  }

  which_parameter_update = -1;

  return;
}

/*!
 * this function calculates likelihood at initial step.
 */
double log_likelihoods_cal_initial_sa(const void *model)
{
  double logL;
  logL = prob_sa(model);
  return logL;
}

/*!
 * this function calculates likelihood at initial step.
 */
double log_likelihoods_cal_restart_sa(const void *model)
{
  double logL;
  logL = prob_sa(model);
  return logL;
}

/*!
 * this function calculates likelihood.
 */
double log_likelihoods_cal_sa(const void *model)
{
  double logL;
  logL = prob_sa(model);
  return logL;
}

/*!
 * this function prints out parameters.
 */
void print_particle_sa(FILE *fp, const void *model)
{
  int i;
  double *pm = (double *)model;

  for(i=0; i<num_params; i++)
  {
    fprintf(fp, "%e ", pm[i] );
  }
  fprintf(fp, "\n");
  return;
}

/*!
 * This function read the particle from the file.
 */
void read_particle_sa(FILE *fp, void *model)
{
  int j;
  double *psample = (double *)model;

  for(j=0; j < dnest_num_params; j++)
  {
    if(fscanf(fp, "%lf", psample+j) < 1)
    {
      printf("%f\n", *psample);
      fprintf(stderr, "#Error: Cannot read file %s.\n", options.sample_file);
      exit(0);
    }
  }
  return;
}

/*!
 * this function perturbs parameters.
 */
double perturb_sa(void *model)
{
  double *pm = (double *)model;
  double logH = 0.0, limit1, limit2, width;
  int which, which_level, count_saves;

  /*
   * fixed parameters need not to update
   * perturb important parameters more frequently
   */
  do
  {
    which = dnest_rand_int(num_params);
  }while(par_fix[which] == 1);

  which_parameter_update = which;

  /* level-dependent width */
  count_saves = dnest_get_count_saves();
  which_level_update = dnest_get_which_level_update();
  which_level = which_level_update > (size_levels-10)?(size_levels-10):which_level_update;

  if( which_level > 0 && count_saves > 1000)
  {
    limit1 = limits[(which_level-1) * num_params *2 + which *2];
    limit2 = limits[(which_level-1) * num_params *2 + which *2 + 1];
    width = (limit2 - limit1);
    width /= (3*2.35);
  }
  else
  {
    limit1 = par_range_model[which][0];
    limit2 = par_range_model[which][1];
    width = (par_range_model[which][1] - par_range_model[which][0]);
    width /= (2.35);
  }

  if(par_prior_model[which] == GAUSSIAN)
  {
    logH -= (-0.5*pow((pm[which] - par_prior_gaussian[which][0])/par_prior_gaussian[which][1], 2.0) );
    pm[which] += dnest_randh() * width;
    dnest_wrap(&pm[which], par_range_model[which][0], par_range_model[which][1]);
    //dnest_wrap(&pm[which], limit1, limit2);
    logH += (-0.5*pow((pm[which] - par_prior_gaussian[which][0])/par_prior_gaussian[which][1], 2.0) );
  }
  else
  {
    pm[which] += dnest_randh() * width;
    dnest_wrap(&(pm[which]), par_range_model[which][0], par_range_model[which][1]);
    //dnest_wrap(&pm[which], limit1, limit2);
  }

  return logH;
}

/*!
 * this function calculates likelihood.
 */
double log_likelihoods_cal_sa_exam(const void *model)
{
  return 0.0;
}

/*
 * action when a particle's move is accepted.
 * usually store some values with no need to recompute.
 */
void accept_action_sa()
{
  return;
}

/*
 * action when particle i is killed in cdnest sampling.
 * particle i_copy's properties is copyed to particle i.
 */
void kill_action_sa(int i, int i_copy)
{
  return;
}

#endif


// THE VIRTUAL BRAIN -- C
// A fast implementation of TVB-style brain network models based on
// DYNAMIC MEAN FIELD MODEL Deco et al. 2014 Journal of Neuroscience
//
//  m.schirner@fu-berlin.de
//  michael.schirner@charite.de
//
// MIT LICENSE
// Copyright 2020 Michael Schirner
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include <stdio.h>
#include <xmmintrin.h>
#include <emmintrin.h>
#include <stdlib.h>
#include <time.h>
#include <string.h>
#include <math.h>
#include <pthread.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
//#include <semaphore.h>

/*

 Definition of pthread barrier -- since OSX doesn't seem to have it

 */

 typedef int pthread_barrierattr_t;
 typedef struct
 {
 pthread_mutex_t mutex;
 pthread_cond_t cond;
 int count;
 int tripCount;
 } pthread_barrier_t;

 int pthread_barrier_init(pthread_barrier_t *barrier, const pthread_barrierattr_t *attr, unsigned int count)
 {
 if(count == 0)
 {
 //errno = EINVAL;
 return -1;
 }
 if(pthread_mutex_init(&barrier->mutex, 0) < 0)
 {
 return -1;
 }
 if(pthread_cond_init(&barrier->cond, 0) < 0)
 {
 pthread_mutex_destroy(&barrier->mutex);
 return -1;
 }
 barrier->tripCount = count;
 barrier->count = 0;

 return 0;
 }

 int pthread_barrier_destroy(pthread_barrier_t *barrier)
 {
 pthread_cond_destroy(&barrier->cond);
 pthread_mutex_destroy(&barrier->mutex);
 return 0;
 }

 int pthread_barrier_wait(pthread_barrier_t *barrier)
 {
 pthread_mutex_lock(&barrier->mutex);
 ++(barrier->count);
 if(barrier->count >= barrier->tripCount)
 {
 barrier->count = 0;
 pthread_cond_broadcast(&barrier->cond);
 pthread_mutex_unlock(&barrier->mutex);
 return 1;
 }
 else
 {
 pthread_cond_wait(&barrier->cond, &(barrier->mutex));
 pthread_mutex_unlock(&barrier->mutex);
 return 0;
 }
 }

/*

 End: Definition of pthread barrier

 */

pthread_mutex_t mutex_thrcount;

struct Xi_p{
    float **Xi_elems;
};

struct SC_capS{
    float *cap;
};

struct SC_inpregS{
    int *inpreg;
};

pthread_barrier_t mybarrier_base, mybarrier1, mybarrier2, mybarrier3;

#define REAL float
//#define REAL double

int divRoundClosest(const int n, const int d)
{
    return ((n < 0) ^ (d < 0)) ? ((n - d/2)/d) : ((n + d/2)/d);
}

int divRoundUp(const int x, const int y)
{
    return (x + y - 1) / y;
}

/* Compute Pearson's correlation coefficient */
float corr(float *x, float *y, int n){
    int i;
    float mx=0, my=0;

    /* Calculate the mean of the two series x[], y[] */
    for (i=0; i<n; i++) {
        mx += x[i];
        my += y[i];
    }
    mx /= n;
    my /= n;

    /* Calculate the correlation */
    float sxy = 0, sxsq = 0, sysq = 0, tmpx, tmpy;
    for (i=0; i<n; i++) {
        tmpx = x[i] - mx;
        tmpy = y[i] - my;
        sxy += tmpx*tmpy;
        sxsq += tmpx*tmpx;
        sysq += tmpy*tmpy;
    }

    return (sxy / (sqrt(sxsq)*sqrt(sysq)));
}

//float gaussrand_ret()
//{
//    static double V1=0.0, V2=0.0, S=0.0, U1=0.0, U2=0.0;
//    S=0.0;
//    do {
//        U1 = (double)rand() / RAND_MAX;
//        U2 = (double)rand() / RAND_MAX;
//        V1 = 2 * U1 - 1;
//        V2 = 2 * U2 - 1;
//        S = V1 * V1 + V2 * V2;
//    } while(S >= 1 || S == 0);
//
//    return (float)(V1 * sqrt(-2 * log(S) / S));
//}
//
//static inline void gaussrand(float *randnum)
//{
//    static double V1=0.0, V2=0.0, S=0.0, U1=0.0, U2=0.0;
//
//    S=0.0;
//    do {
//        U1 = (double)rand() / RAND_MAX;
//        U2 = (double)rand() / RAND_MAX;
//        V1 = 2 * U1 - 1;
//        V2 = 2 * U2 - 1;
//        S = V1 * V1 + V2 * V2;
//    } while(S >= 1 || S == 0);
//
//    randnum[0] = (float)(V1 * sqrt(-2 * log(S) / S));
//    randnum[1] = (float)(V2 * sqrt(-2 * log(S) / S));
//
//    S=0.0;
//    do {
//        U1 = (double)rand() / RAND_MAX;
//        U2 = (double)rand() / RAND_MAX;
//        V1 = 2 * U1 - 1;
//        V2 = 2 * U2 - 1;
//        S = V1 * V1 + V2 * V2;
//    } while(S >= 1 || S == 0);
//
//    randnum[2] = (float)(V1 * sqrt(-2 * log(S) / S));
//    randnum[3] = (float)(V2 * sqrt(-2 * log(S) / S));
//}

int importGlobalConnectivity(char *SC_cap_filename, char *SC_dist_filename, char *SC_inputreg_filename, int regions, float **region_activity, struct Xi_p **reg_globinp_p, float global_trans_v, int **n_conn_table, float G_J_NMDA, struct SC_capS **SC_cap, float **SC_rowsums, struct SC_inpregS **SC_inpreg)
{

    int i,j,k, maxdelay=0, tmpint;
    float *region_activity_p;
    double tmp, tmp2;
    struct Xi_p *reg_globinp_pp;
    struct SC_capS      *SC_capp;
    struct SC_inpregS   *SC_inpregp;

    // Open SC files
    FILE *file_cap, *file_dist;
    file_cap=fopen(SC_cap_filename, "r");
    file_dist=fopen(SC_dist_filename, "r");
    if (file_cap==NULL || file_dist==NULL)
    {
        printf( "\nERROR: Could not open SC files. Terminating... \n\n");
        exit(0);
    }

    // Allocate a counter that stores number of region input connections for each region and the SCcap array
    *SC_rowsums = (float *)_mm_malloc(regions*sizeof(float),16);
    *n_conn_table = (int *)_mm_malloc(regions*sizeof(int),16);
    *SC_cap = (struct SC_capS *)_mm_malloc(regions*sizeof(struct SC_capS),16);
    SC_capp = *SC_cap;
    *SC_inpreg = (struct SC_inpregS *)_mm_malloc(regions*sizeof(struct SC_inpregS),16);
    SC_inpregp = *SC_inpreg;
    if(*n_conn_table==NULL || SC_capp==NULL || SC_rowsums==NULL || SC_inpregp==NULL){
        printf("Running out of memory. Terminating.\n");fclose(file_dist);fclose(file_cap);exit(2);
    }

    // Read out the maximal fiber length and the degree of each node and rewind SC files
    int n_entries = 0, curr_col = 0, curr_row = 0, curr_row_nonzero = 0;
    double tmp_max = -9999;
    while(fscanf(file_dist,"%lf",&tmp) != EOF && fscanf(file_cap,"%lf",&tmp2) != EOF){
        if (tmp_max < tmp) tmp_max = tmp;
        n_entries++;

        if (tmp2 > 0.0) curr_row_nonzero++;

        curr_col++;
        if (curr_col == regions) {
            curr_col = 0;
            (*n_conn_table)[curr_row] = curr_row_nonzero;
            curr_row_nonzero = 0;
            curr_row++;
        }
    }

    if (n_entries < regions * regions) {
        printf( "ERROR: Unexpected end-of-file in file. File contains less input than expected. Terminating... \n");
        exit(0);
    }

    rewind(file_dist);
    rewind(file_cap);

    maxdelay = (int)(((tmp_max/global_trans_v)*10)+0.5); // *10 for getting from m/s to 10kHz sampling, +0.5 for rounding by casting
    if (maxdelay < 1) maxdelay = 1; // Case: no time delays

    // Allocate ringbuffer that contains region activity for each past time-step until maxdelay and another ringbuffer that contains pointers to the first ringbuffer
    *region_activity = (float *)_mm_malloc(maxdelay*regions*sizeof(float),16);
    region_activity_p = *region_activity;
    *reg_globinp_p = (struct Xi_p *)_mm_malloc(maxdelay*regions*sizeof(struct Xi_p),16);
    reg_globinp_pp = *reg_globinp_p;
    if(region_activity_p==NULL || reg_globinp_p==NULL){
        printf("Running out of memory. Terminating.\n");fclose(file_dist);exit(2);
    }
    for (j=0; j<maxdelay*regions; j++) {
        region_activity_p[j]=0.001;
    }

    // Read SC files and set pointers for each input region and correspoding delay for each ringbuffer time-step
    int ring_buff_position;
    for (i=0; i<regions; i++) {

        if ((*n_conn_table)[i] > 0) {
            // SC strength and inp region numbers
            SC_capp[i].cap          = (float *)_mm_malloc(((*n_conn_table)[i])*sizeof(float),16);
            SC_inpregp[i].inpreg    = (int *)_mm_malloc(((*n_conn_table)[i])*sizeof(int),16);
            if(SC_capp[i].cap==NULL || SC_inpregp[i].inpreg==NULL){
                printf("Running out of memory. Terminating.\n");exit(2);
            }

            // Allocate memory for input-region-pointer arrays for each time-step in ringbuffer
            for (j=0; j<maxdelay; j++){
                reg_globinp_pp[i+j*regions].Xi_elems=(float **)_mm_malloc(((*n_conn_table)[i])*sizeof(float *),16);
                if(reg_globinp_pp[i+j*regions].Xi_elems==NULL){
                    printf("Running out of memory. Terminating.\n");exit(2);
                }
            }

            float sum_caps=0.0;
            // Read incoming connections and set pointers
            curr_row_nonzero = 0;
            for (j=0; j<regions; j++) {
                if(fscanf(file_cap,"%lf",&tmp) != EOF && fscanf(file_dist,"%lf",&tmp2) != EOF){
                    if (tmp > 0.0) {
                        tmpint = (int)(((tmp2/global_trans_v)*10)+0.5); //  *10 for getting from m/s or mm/ms to 10kHz sampling, +0.5 for rounding by casting
                        if (tmpint < 0 || tmpint > maxdelay){
                            printf( "\nERROR: Negative or too high (larger than maximum specified number) connection length/delay %d -> %d. Terminating... \n\n",i,j);exit(0);
                        }
                        if (tmpint <= 0) tmpint = 1; // If time delay is smaller than integration step size, than set time delay to one integration step

                        SC_capp[i].cap[curr_row_nonzero] = (float)tmp * G_J_NMDA;
                        //sum_caps                += (float)tmp;
                        sum_caps                += SC_capp[i].cap[curr_row_nonzero];
                        SC_inpregp[i].inpreg[curr_row_nonzero]  =  j;

                        ring_buff_position=maxdelay*regions - tmpint*regions + j;
                        for (k=0; k<maxdelay; k++) {
                            reg_globinp_pp[i+k*regions].Xi_elems[curr_row_nonzero]=&region_activity_p[ring_buff_position];
                            ring_buff_position += regions;
                            if (ring_buff_position > (maxdelay*regions-1)) ring_buff_position -= maxdelay*regions;
                        }

                        curr_row_nonzero++;
                    }
                } else{
                    printf( "\nERROR: Unexpected end-of-file in file %s or %s. File contains less input than expected. Terminating... \n\n", SC_cap_filename, SC_dist_filename);
                    exit(0);
                }

            }
            if (sum_caps <= 0) {
                printf( "\nERROR: Sum of connection strenghts is negative or zero. sum-caps node %d = %f. Terminating... \n\n",i,sum_caps);exit(0);
            }
            (*SC_rowsums)[i] = sum_caps;
        }
    }

    fclose(file_dist);fclose(file_cap);
    return maxdelay;
}

/*
 Initialize thread barriers
 */
void initialize_thread_barriers(int n_threads) {
    pthread_barrier_init(&mybarrier1, NULL, n_threads);
    pthread_barrier_init(&mybarrier2, NULL, n_threads);
    pthread_barrier_init(&mybarrier3, NULL, n_threads);
    return;
}

/* create thread argument struct for thr_func() */
typedef struct _thread_data_t {
    int                 tid, rand_num_seed;
    int                 nodes, nodes_vec, fake_nodes, n_threads, vectorization_grade, time_steps, BOLD_TR, BOLD_ts_len;
    float               *J_i;
    int                 reg_act_size;
    float               *region_activity;
    float               model_dt;
    __m128              _gamma;
    __m128              _one;
    __m128              _imintau_E;
    __m128              _dt;
    __m128              _sigma_sqrt_dt;
    __m128              _sigma;
    __m128              _gamma_I;
    __m128              _imintau_I;
    __m128              _min_d_I;
    __m128              _b_I;
    __m128              _J_NMDA;
    __m128              _w_I__I_0;
    __m128              _a_I;
    __m128              _min_d_E;
    __m128              _b_E;
    __m128              _a_E;
    __m128              _w_plus_J_NMDA;
    __m128              _w_E__I_0;
    struct SC_capS      *SC_cap;
    struct SC_inpregS   *SC_inpreg;
    struct Xi_p         *reg_globinp_p;
    int                 *n_conn_table;
    float               *BOLD_ex;
    char                *output_file;
} thread_data_t;

/*

 Function for multi-threaded FIC tuning

 */
void *run_simulation(void *arg)
{

    /* Local function parameters (no concurrency) */
    int j, i_node_vec, i_node_vec_local, k, int_i, ts;
    float tmpglobinput, tmpglobinput_FFI;
    __m128 _tmp_H_E, _tmp_H_I, _tmp_I_I, _tmp_I_E;
    float tmp_exp_E[4]          __attribute__((aligned(16)));
    float tmp_exp_I[4]          __attribute__((aligned(16)));
    __m128          *_tmp_exp_E         = (__m128*)tmp_exp_E;
    __m128          *_tmp_exp_I         = (__m128*)tmp_exp_I;
    float rand_number[4]        __attribute__((aligned(16)));
    __m128          *_rand_number       = (__m128*)rand_number;
    int     ring_buf_pos                = 0;

    /* Global function parameters (some with concurrency) */
    thread_data_t *thr_data = (thread_data_t *)arg;
    //printf("Hello from thread %d, process %d\n", thr_data->tid, thr_data->rank);
    int     t_id                    = thr_data->tid;
    int     rand_num_seed           = thr_data->rand_num_seed;
    int     nodes                   = thr_data->nodes;
    int     fake_nodes              = thr_data->fake_nodes;
    const   int nodes_vec           = thr_data->nodes_vec;
    int     n_threads               = thr_data->n_threads;
    int     vectorization_grade     = thr_data->vectorization_grade;
    int     reg_act_size            = thr_data->reg_act_size;
    int     *n_conn_table           = thr_data->n_conn_table;
    float   *J_i                    = thr_data->J_i;
    float   *region_activity        = thr_data->region_activity;
    float   *BOLD_ex                = thr_data->BOLD_ex;
    const   __m128 _gamma           = thr_data->_gamma;
    const   __m128 _one             = thr_data->_one;
    const   __m128 _imintau_E       = thr_data->_imintau_E;
    const   __m128 _dt              = thr_data->_dt;
    const   __m128 _sigma_sqrt_dt   = thr_data->_sigma_sqrt_dt;
    const   __m128 _gamma_I         = thr_data->_gamma_I;
    const   __m128 _imintau_I       = thr_data->_imintau_I;
    const   __m128 _min_d_I         = thr_data->_min_d_I;
    const   __m128 _b_I             = thr_data->_b_I;
    const   __m128 _J_NMDA          = thr_data->_J_NMDA;
    const   __m128 _w_I__I_0        = thr_data->_w_I__I_0;
    const   __m128 _a_I             = thr_data->_a_I;
    const   __m128 _min_d_E         = thr_data->_min_d_E;
    const   __m128 _b_E             = thr_data->_b_E;
    const   __m128 _a_E             = thr_data->_a_E;
    const   __m128 _w_plus_J_NMDA   = thr_data->_w_plus_J_NMDA;
    const   __m128 _w_E__I_0        = thr_data->_w_E__I_0;
    struct  SC_capS *SC_cap         = thr_data->SC_cap;
    struct  Xi_p *reg_globinp_p     = thr_data->reg_globinp_p;
    int     time_steps              = thr_data->time_steps;
    int     BOLD_TR                 = thr_data->BOLD_TR;
    float   model_dt                = thr_data->model_dt;
    int     BOLD_ts_len             = thr_data->BOLD_ts_len;

    /* Parallelism: divide work among threads */
    int nodes_vec_mt        = divRoundUp(nodes_vec, n_threads);
    int nodes_mt            = nodes_vec_mt  * vectorization_grade;

    int start_nodes_vec_mt  = t_id         * nodes_vec_mt;
    int start_nodes_mt      = t_id         * nodes_mt;
    int end_nodes_vec_mt    = (t_id + 1)   * nodes_vec_mt;
    int end_nodes_mt        = (t_id + 1)   * nodes_mt;
    int end_nodes_mt_glob   = end_nodes_mt; // end_nodes_mt_glob may differ from end_nodes_mt in the last thread (fake-nodes vs nodes)

    /* Correct settings for last thread */
    if (end_nodes_mt > nodes) {
        end_nodes_vec_mt    = nodes_vec;
        end_nodes_mt        = fake_nodes;
        end_nodes_mt_glob   = nodes;
        nodes_mt            = end_nodes_mt - start_nodes_mt;
        nodes_vec_mt        = end_nodes_vec_mt - start_nodes_vec_mt;
    }

    printf("thread %d: start: %d end: %d size: %d\n", thr_data->tid, start_nodes_mt, end_nodes_mt, nodes_mt);

    /* Check whether splitting makes sense (i.e. each thread has something to do). Terminate if not. */
    if (nodes_vec_mt <= 1) {
        printf("The splitting is ineffective (e.g. fewer nodes than threads or some threads have nothing to do). Use different numbers of threads. Terminating.\n");
        exit(0);
    }

    /* Set up barriers */
    if (t_id == 0) {
        initialize_thread_barriers(n_threads);
    }
    pthread_barrier_wait(&mybarrier_base);

    /* Initialize random number generator */
    //const gsl_rng_type * T;
    gsl_rng *r = gsl_rng_alloc (gsl_rng_mt19937);
    gsl_rng_set (r, (t_id+rand_num_seed)); // Random number seed -- otherwise every node gets the same random nums
    srand((unsigned)(t_id+rand_num_seed));

    // All threads
    float   *meanFR                 = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // summation array for mean firing rate over all threads
    float   *meanFR_INH             = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // summation array for mean firing rate over all threads
    float   *global_input           = (float *)_mm_malloc(nodes_mt * sizeof(float),16);
    float   *global_input_FFI       = (float *)_mm_malloc(nodes_mt * sizeof(float),16);
    float   *S_i_E                  = (float *)_mm_malloc(nodes_mt * sizeof(float),16);
    float   *S_i_I                  = (float *)_mm_malloc(nodes_mt * sizeof(float),16);
    //float   *r_i_E                    = (float *)_mm_malloc(nodes_mt * sizeof(float),16);
    //float   *r_i_I                    = (float *)_mm_malloc(nodes_mt * sizeof(float),16);
    float   *J_i_local              = (float *)_mm_malloc(nodes_mt * sizeof(float),16);

    if (meanFR == NULL || meanFR_INH == NULL || global_input == NULL || global_input_FFI == NULL || S_i_E==NULL || S_i_I==NULL || J_i_local==NULL) {
        printf( "ERROR: Running out of memory. Aborting... \n");
        exit(0);
    }

    // Cast to vector/SSE arrays
    __m128          *_meanFR         = (__m128*)meanFR;
    __m128          *_meanFR_INH     = (__m128*)meanFR_INH;
    __m128          *_global_input   = (__m128*)global_input;
    __m128          *_global_input_FFI = (__m128*)global_input_FFI;
    __m128          *_S_i_E          = (__m128*)S_i_E;
    __m128          *_S_i_I          = (__m128*)S_i_I;
    //__m128          *_r_i_E             = (__m128*)r_i_E;
    //__m128          *_r_i_I             = (__m128*)r_i_I;
    __m128          *_J_i_local      = (__m128*)J_i_local;

    //Balloon-Windkessel model parameters / arrays
    float rho = 0.34, alpha = 0.32, tau = 0.98, y = 1.0/0.41, kappa = 1.0/0.65;
    float V_0 = 0.02, k1 = 7 * rho, k2 = 2.0, k3 = 2 * rho - 0.2, ialpha = 1.0/alpha, itau = 1.0/tau, oneminrho = (1.0 - rho);
    float f_tmp;
    float *BOLD       = (float *)_mm_malloc(nodes_mt * BOLD_ts_len * sizeof(float),16);  // resulting BOLD data
    float *bw_x_ex    = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // State-variable 1 of BW-model (exc. pop.)
    float *bw_f_ex    = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // State-variable 2 of BW-model (exc. pop.)
    float *bw_nu_ex   = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // State-variable 3 of BW-model (exc. pop.)
    float *bw_q_ex    = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // State-variable 4 of BW-model (exc. pop.)
    //float *bw_x_in    = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // State-variable 1 of BW-model (inh. pop.)
    //float *bw_f_in    = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // State-variable 2 of BW-model (inh. pop.)
    //float *bw_nu_in   = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // State-variable 3 of BW-model (inh. pop.)
    //float *bw_q_in    = (float *)_mm_malloc(nodes_mt * sizeof(float),16);  // State-variable 4 of BW-model (inh. pop.)
    //if (bw_x_ex == NULL || bw_f_ex == NULL || bw_nu_ex==NULL || bw_q_ex==NULL || bw_x_in==NULL || bw_f_in==NULL || bw_nu_in==NULL || bw_q_in==NULL) {}
    if (bw_x_ex == NULL || bw_f_ex == NULL || bw_nu_ex==NULL || bw_q_ex==NULL) {
        printf( "ERROR: Running out of memory. Aborting... \n");
        exit(0);
    }

    // Reset arrays and variables
    ring_buf_pos     = 0;
    for (j = 0; j < nodes_mt; j++) {
        meanFR[j]           = 0.0;
        meanFR_INH[j]       = 0.0;
        global_input[j]     = 0.00;
        global_input_FFI[j] = 0.00;
        S_i_E[j]            = 0.00;
        S_i_I[j]            = 0.00;
        J_i_local[j]        = J_i[j+start_nodes_mt];
    }
    if (t_id == 0) {
        for (j=0; j<reg_act_size; j++) {
            region_activity[j]=0.00;
        }
    }

    // Reset Balloon-Windkessel model parameters and arrays
    for (j = 0; j < nodes_mt; j++) {
        bw_x_ex[j] = 0.0;
        bw_f_ex[j] = 1.0;
        bw_nu_ex[j] = 1.0;
        bw_q_ex[j] = 1.0;
        //bw_x_in[j] = 0.0;
        //bw_f_in[j] = 1.0;
        //bw_nu_in[j] = 1.0;
        //bw_q_in[j] = 1.0;
    }

    /* Barrier: only start simulating when arrays were cleared */
    pthread_barrier_wait(&mybarrier1);

    /*  Simulation loop */
    int ts_bold_i = 0;
    for (ts = 0; ts < time_steps; ts++) {

        if (t_id == 0){
            printf("%.1f %% \r", ((float)ts / (float)time_steps) * 100.0f );
        }

        // dt = 0.1 ms => 10 integration iterations per ms
        for (int_i = 0; int_i < 10; int_i++) {
            /* Barrier: only start integrating input when all other threads are finished copying their results to buffer */
            pthread_barrier_wait(&mybarrier2);

            /* Compute global coupling */
            i_node_vec_local = 0;
            for(j=start_nodes_mt; j<end_nodes_mt_glob; j++){
                tmpglobinput     = 0;
                tmpglobinput_FFI = 0;
                for (k=0; k<n_conn_table[j]; k++) {
                    tmpglobinput     += *reg_globinp_p[j+ring_buf_pos].Xi_elems[k] * SC_cap[j].cap[k];
                    //tmpglobinput_FFI += *reg_globinp_p[j+ring_buf_pos].Xi_elems[k] * SC_cap_FFI[j].cap[k];
                }
                global_input[i_node_vec_local]     = tmpglobinput;
                //global_input_FFI[i_node_vec_local] = tmpglobinput_FFI;

                i_node_vec_local++;
            }

            i_node_vec_local = 0;
            for (i_node_vec = start_nodes_vec_mt; i_node_vec < end_nodes_vec_mt; i_node_vec++) {

                // Excitatory population firing rate
                _tmp_I_E    = _mm_sub_ps(_mm_mul_ps(_a_E,_mm_add_ps(_mm_add_ps(_w_E__I_0,_mm_mul_ps(_w_plus_J_NMDA, _S_i_E[i_node_vec_local])),_mm_sub_ps(_global_input[i_node_vec_local],_mm_mul_ps(_J_i_local[i_node_vec_local], _S_i_I[i_node_vec_local])))),_b_E);

                *_tmp_exp_E     = _mm_mul_ps(_min_d_E, _tmp_I_E);
                tmp_exp_E[0]    = tmp_exp_E[0] != 0 ? expf(tmp_exp_E[0]) : 0.9;
                tmp_exp_E[1]    = tmp_exp_E[1] != 0 ? expf(tmp_exp_E[1]) : 0.9;
                tmp_exp_E[2]    = tmp_exp_E[2] != 0 ? expf(tmp_exp_E[2]) : 0.9;
                tmp_exp_E[3]    = tmp_exp_E[3] != 0 ? expf(tmp_exp_E[3]) : 0.9;
                _tmp_H_E        = _mm_div_ps(_tmp_I_E, _mm_sub_ps(_one, *_tmp_exp_E));
                _meanFR[i_node_vec_local] = _mm_add_ps(_meanFR[i_node_vec_local],_tmp_H_E);
                //_r_i_E[i_node_vec_local]  = _tmp_H_E;

                // Inhibitory population firing rate
                _tmp_I_I = _mm_sub_ps(_mm_mul_ps(_a_I,_mm_sub_ps(_mm_add_ps(_mm_add_ps(_w_I__I_0,_global_input_FFI[i_node_vec_local]),_mm_mul_ps(_J_NMDA, _S_i_E[i_node_vec_local])), _S_i_I[i_node_vec_local])),_b_I);
                *_tmp_exp_I   = _mm_mul_ps(_min_d_I, _tmp_I_I);
                tmp_exp_I[0]  = tmp_exp_I[0] != 0 ? expf(tmp_exp_I[0]) : 0.9;
                tmp_exp_I[1]  = tmp_exp_I[1] != 0 ? expf(tmp_exp_I[1]) : 0.9;
                tmp_exp_I[2]  = tmp_exp_I[2] != 0 ? expf(tmp_exp_I[2]) : 0.9;
                tmp_exp_I[3]  = tmp_exp_I[3] != 0 ? expf(tmp_exp_I[3]) : 0.9;
                _tmp_H_I  = _mm_div_ps(_tmp_I_I, _mm_sub_ps(_one, *_tmp_exp_I));
                _meanFR_INH[i_node_vec_local] = _mm_add_ps(_meanFR_INH[i_node_vec_local],_tmp_H_I);
                //_r_i_I[i_node_vec_local] = _tmp_H_I;

                //gaussrand(rand_number);
                rand_number[0] = (float)gsl_ran_gaussian(r, 1.0);
                rand_number[1] = (float)gsl_ran_gaussian(r, 1.0);
                rand_number[2] = (float)gsl_ran_gaussian(r, 1.0);
                rand_number[3] = (float)gsl_ran_gaussian(r, 1.0);

                _S_i_I[i_node_vec_local] = _mm_add_ps(_mm_add_ps(_mm_mul_ps(_sigma_sqrt_dt, *_rand_number),_S_i_I[i_node_vec_local]),_mm_mul_ps(_dt,_mm_add_ps(_mm_mul_ps(_imintau_I, _S_i_I[i_node_vec_local]),_mm_mul_ps(_tmp_H_I,_gamma_I))));

                //gaussrand(rand_number);
                rand_number[0] = (float)gsl_ran_gaussian(r, 1.0);
                rand_number[1] = (float)gsl_ran_gaussian(r, 1.0);
                rand_number[2] = (float)gsl_ran_gaussian(r, 1.0);
                rand_number[3] = (float)gsl_ran_gaussian(r, 1.0);

                _S_i_E[i_node_vec_local] = _mm_add_ps(_mm_add_ps(_mm_mul_ps(_sigma_sqrt_dt, *_rand_number),_S_i_E[i_node_vec_local]),_mm_mul_ps(_dt, _mm_add_ps(_mm_mul_ps(_imintau_E, _S_i_E[i_node_vec_local]),_mm_mul_ps(_mm_mul_ps(_mm_sub_ps(_one, _S_i_E[i_node_vec_local]),_gamma),_tmp_H_E))));

                i_node_vec_local++;
            }

            /* ensure that synaptic activity is within its boundaries */
            for(j=0; j<nodes_mt; j++){
                S_i_E[j] = S_i_E[j] >= 0.0 ? S_i_E[j] : 0.0;
                S_i_E[j] = S_i_E[j] <= 1.0 ? S_i_E[j] : 1.0;
                S_i_I[j] = S_i_I[j] >= 0.0 ? S_i_I[j] : 0.0;
                S_i_I[j] = S_i_I[j] <= 1.0 ? S_i_I[j] : 1.0;
            }

            /* Copy result of this thread into global region_activity buffer */
            memcpy(&region_activity[ring_buf_pos+start_nodes_mt], S_i_E, nodes_mt*sizeof( float ));

            /* Shift region_activity ring-buffer start */
            ring_buf_pos = ring_buf_pos<(reg_act_size-nodes) ? (ring_buf_pos+nodes) : 0;

        }

        /*
         Compute BOLD for that time-step (subsampled to 1 ms)
         */
        for (j = 0; j < nodes_mt; j++) {
            bw_x_ex[j]  = bw_x_ex[j]  +  model_dt * (S_i_E[j] - kappa * bw_x_ex[j] - y * (bw_f_ex[j] - 1.0));
            f_tmp       = bw_f_ex[j]  +  model_dt * bw_x_ex[j];
            bw_nu_ex[j] = bw_nu_ex[j] +  model_dt * itau * (bw_f_ex[j] - powf(bw_nu_ex[j], ialpha));
            bw_q_ex[j]  = bw_q_ex[j]  +  model_dt * itau * (bw_f_ex[j] * (1.0 - powf(oneminrho,(1.0/bw_f_ex[j]))) / rho  - powf(bw_nu_ex[j],ialpha) * bw_q_ex[j] / bw_nu_ex[j]);
            bw_f_ex[j]  = f_tmp;

            /*
             bw_x_in[j]  = bw_x_in[j]  +  model_dt * (S_i_I[j] - kappa * bw_x_in[j] - y * (bw_f_in[j] - 1.0));
             f_tmp       = bw_f_in[j]  +  model_dt * bw_x_in[j];
             bw_nu_in[j] = bw_nu_in[j] +  model_dt * itau * (bw_f_in[j] - powf(bw_nu_in[j], ialpha));
             bw_q_in[j]  = bw_q_in[j]  +  model_dt * itau * (bw_f_in[j] * (1.0 - powf(oneminrho,(1.0/bw_f_in[j]))) / rho  - powf(bw_nu_in[j],ialpha) * bw_q_in[j] / bw_nu_in[j]);
             bw_f_in[j]  = f_tmp;
             */
        }

        if (ts % BOLD_TR == 0) {
            for (j = 0; j < nodes_mt; j++) {
                BOLD[ts_bold_i + j * BOLD_ts_len] = 100 / rho * V_0 * (k1 * (1 - bw_q_ex[j]) + k2 * (1 - bw_q_ex[j]/bw_nu_ex[j]) + k3 * (1 - bw_nu_ex[j]));
            }
            ts_bold_i++;
        }
    }

    /* Copy results of this thread into shared memory buffer */
    memcpy(&BOLD_ex[start_nodes_mt*BOLD_ts_len], BOLD, nodes_mt * BOLD_ts_len * sizeof( float ));

    /* Wait until other threads finished before writing out result */
    pthread_barrier_wait(&mybarrier3);

    /* Write out fMRI time series */
    if (t_id == 0){
        FILE *FCout = fopen(thr_data->output_file, "w");
        for (j = 0; j < nodes; j++) {
            for (k = 0; k < ts_bold_i; k++) {
                fprintf(FCout, "%.5f ",BOLD_ex[j*BOLD_ts_len + k]);
            }
            fprintf(FCout, "\n");
        }
        fclose(FCout);
    }

    gsl_rng_free (r);
    pthread_exit(NULL);
}

/*
 Usage: tvbii <paramfile> <subject_id>
 */

int main(int argc, char *argv[])
{
    /*
     Check whether right number of arguments was passed to program call
     */
    if (argc != 4 || atoi(argv[3]) <= 0) {
        printf("\nERROR: Invalid arguments.\n\nUsage: tvbii <paramset_file> <sub_id> <#threads>\n\nTerminating... \n\n");
        printf("\nProvided arguments:\n");
        int i;
        for (i=0; i<argc; i++) {
            printf("%s\n", argv[i]);
        }
        exit(0);
    }

    /*
     Get current time and do some initializations
     */
    time_t  start = time(NULL);
    int     i, j;
    int     n_threads = atoi(argv[3]);

    /*
     Global model and integration parameters
     */
    const float dt                  = 0.1;      // Integration step length dt = 0.1 ms
    const float sqrt_dt             = sqrtf(dt);// Noise in the diffusion part scaled by sqrt of dt
    const float model_dt            = 0.001;    // Time-step of model (sampling-rate=1000 Hz)
    const int   vectorization_grade = 4;        // How many operations can be done simultaneously. Depends on CPU Architecture and available intrinsics.
    int         time_steps          = 667*1.94*1000;    // Simulation length
    int         nodes               = 84;    // Number of surface vertices; must be a multiple of vectorization grade
    int         fake_nodes          = 84;
    float       global_trans_v      = 1.0;     // Global transmission velocity (m/s); Local time-delays can be ommited since smaller than integration time-step
    float       G                   = 0.5;        // Global coupling strength
    int         BOLD_TR             = 1940;     // TR of BOLD data
    int         rand_num_seed       = 1403;

    /*
     Local model: DMF-Parameters from Deco et al. JNeuro 2014
     */
    float w_plus  = 1.4;          // local excitatory recurrence
    //float I_ext   = 0;            // External stimulation
    float J_NMDA  = 0.15;         // (nA) excitatory synaptic coupling
    //float J_i     = 1.0;          // 1 for no-FIC, !=1 for Feedback Inhibition Control
    const float a_E     = 310;          // (n/C)
    const float b_E     = 125;          // (Hz)
    const float d_E     = 0.16;         // (s)
    const float a_I     = 615;          // (n/C)
    const float b_I     = 177;          // (Hz)
    const float d_I     = 0.087;        // (s)
    const float gamma   = 0.641/1000.0; // factor 1000 for expressing everything in ms
    const float tau_E   = 100;          // (ms) Time constant of NMDA (excitatory)
    const float tau_I   = 10;           // (ms) Time constant of GABA (inhibitory)
    float       sigma   = 0.00316228;   // (nA) Noise amplitude
    const float I_0     = 0.382;        // (nA) overall effective external input
    const float w_E     = 1.0;          // scaling of external input for excitatory pool
    const float w_I     = 0.7;          // scaling of external input for inhibitory pool
    const float gamma_I = 1.0/1000.0;   // for expressing inhib. pop. in ms
    float       tmpJi   = 1.0;          // Feedback inhibition J_i

    /* Read parameters from input file. Input file is a simple text file that contains one line with parameters and white spaces in between. */
    FILE *file;
    char param_file[300];memset(param_file, 0, 300*sizeof(char));
    snprintf(param_file, sizeof(param_file), "/input/%s",argv[1]);
    file=fopen(param_file, "r");
    if (file==NULL){
        printf( "\nERROR: Could not open file %s. Terminating... \n\n", param_file);
        exit(0);
    }
    if(fscanf(file,"%d",&nodes) != EOF && fscanf(file,"%f",&G) != EOF && fscanf(file,"%f",&J_NMDA) != EOF && fscanf(file,"%f",&w_plus) != EOF && fscanf(file,"%f",&tmpJi) != EOF && fscanf(file,"%f",&sigma) != EOF && fscanf(file,"%d",&time_steps) != EOF && fscanf(file,"%d",&BOLD_TR) != EOF && fscanf(file,"%f",&global_trans_v) != EOF && fscanf(file,"%d",&rand_num_seed) != EOF){
    } else{
        printf( "\nERROR: Unexpected end-of-file in file %s. File contains less input than expected. Terminating... \n\n", param_file);
        exit(0);
    }
    fclose(file);

    char output_file[300];memset(output_file, 0, 300*sizeof(char));
    snprintf(output_file, sizeof(output_file), "/output/%s_%s_fMRI.txt",argv[2],argv[1]);

    /* Add fake regions to make region count a multiple of vectorization grade */
    if (nodes % vectorization_grade != 0){
        printf( "\nWarning: Specified number of nodes (%d) is not a multiple of vectorization degree (%d). Will add some fake nodes... \n\n", nodes, vectorization_grade);

        int remainder   = nodes%vectorization_grade;
        if (remainder > 0) {
            fake_nodes = nodes + (vectorization_grade - remainder);
        }
    } else{
        fake_nodes = nodes;
    }

    /* Initialize random number generator */
    srand((unsigned)rand_num_seed);

    /* Derived parameters */
    const float sigma_sqrt_dt = sqrt_dt * sigma;
    const int   nodes_vec     = fake_nodes/vectorization_grade;
    const float min_d_E       = -1.0 * d_E;
    const float min_d_I       = -1.0 * d_I;
    const float imintau_E     = -1.0 / tau_E;
    const float imintau_I     = -1.0 / tau_I;
    const float w_E__I_0      = w_E * I_0;
    const float w_I__I_0      = w_I * I_0;
    const float one           = 1.0;
    const float w_plus__J_NMDA= w_plus * J_NMDA;
    const float G_J_NMDA      = G * J_NMDA;
          float TR            = (float)BOLD_TR / 1000;   // (s) TR of fMRI data
          int   BOLD_ts_len   = time_steps / (TR / model_dt) + 1;  // Length of BOLD time-series written to HDD

    /*
     Import and setup global and local connectivity
     */
    int         *n_conn_table;
    float       *region_activity, *SC_rowsums;
    struct Xi_p *reg_globinp_p;
    struct SC_capS      *SC_cap;
    struct SC_inpregS   *SC_inpreg;
    char cap_file[300];memset(cap_file, 0, 300*sizeof(char));
    snprintf(cap_file, sizeof(cap_file), "/input/%s_SC_weights.txt",argv[2]);
    char dist_file[300];memset(dist_file, 0, 300*sizeof(char));
    snprintf(dist_file, sizeof(dist_file), "/input/%s_SC_distances.txt",argv[2]);
    char reg_file[300];memset(reg_file, 0, 300*sizeof(char));
    snprintf(reg_file, sizeof(reg_file), "/input/%s_SC_regionids.txt",argv[2]);

    int         maxdelay = importGlobalConnectivity(cap_file, dist_file, reg_file, nodes, &region_activity, &reg_globinp_p, global_trans_v, &n_conn_table, G_J_NMDA, &SC_cap, &SC_rowsums, &SC_inpreg);

    int         reg_act_size = nodes * maxdelay;

    /*
     Initialize and/or cast to vector-intrinsics types for variables & parameters
     */
    float *J_i      = (float *)_mm_malloc(fake_nodes * sizeof(float),16); // (nA) inhibitory synaptic coupling
    float *BOLD_ex  = (float *)_mm_malloc(nodes * BOLD_ts_len * sizeof(float),16);

    if (J_i == NULL || BOLD_ex == NULL) {
        printf( "ERROR: Running out of memory. Aborting... \n");
        _mm_free(J_i);_mm_free(BOLD_ex);
        return 1;
    }
    // Initialize state variables / parameters
    for (j = 0; j < fake_nodes; j++) {
        J_i[j]              = tmpJi;
    }
    const __m128    _dt                 = _mm_load1_ps(&dt);
    const __m128    _sigma_sqrt_dt      = _mm_load1_ps(&sigma_sqrt_dt);
    const __m128    _w_plus_J_NMDA      = _mm_load1_ps(&w_plus__J_NMDA);
    const __m128    _a_E                = _mm_load1_ps(&a_E);
    const __m128    _b_E                = _mm_load1_ps(&b_E);
    const __m128    _min_d_E            = _mm_load1_ps(&min_d_E);
    const __m128    _a_I                = _mm_load1_ps(&a_I);
    const __m128    _b_I                = _mm_load1_ps(&b_I);
    const __m128    _min_d_I            = _mm_load1_ps(&min_d_I);
    const __m128    _gamma              = _mm_load1_ps(&gamma);
    const __m128    _gamma_I            = _mm_load1_ps(&gamma_I);
    const __m128    _imintau_E          = _mm_load1_ps(&imintau_E);
    const __m128    _imintau_I          = _mm_load1_ps(&imintau_I);
    const __m128    _w_E__I_0           = _mm_load1_ps(&w_E__I_0);
    const __m128    _w_I__I_0           = _mm_load1_ps(&w_I__I_0);
    float           tmp_sigma           = sigma*dt;// pre-compute dt*sigma for the integration of sigma*randnumber in equations (9) and (10) of Deco2014
    const __m128    _sigma              = _mm_load1_ps(&tmp_sigma);
    const __m128    _one                = _mm_load1_ps(&one);
    const __m128    _J_NMDA             = _mm_load1_ps(&J_NMDA);

    /*
     Start multithread simulation
     */

    /* Create threads */
    pthread_barrier_init(&mybarrier_base, NULL, n_threads);
    pthread_t       thr[n_threads];
    thread_data_t   thr_data[n_threads];
    int rc;
    for (i = 0; i < n_threads; ++i) {
        thr_data[i].tid             =	i               ;
        thr_data[i].n_threads       =	n_threads       ;
        thr_data[i].vectorization_grade = vectorization_grade;
        thr_data[i].nodes			=	nodes			;
        thr_data[i].fake_nodes      =   fake_nodes      ;
        thr_data[i].J_i             =	J_i            	;
        thr_data[i].reg_act_size	=	reg_act_size	;
        thr_data[i].region_activity	=	region_activity	;
        thr_data[i]._gamma			=	_gamma			;
        thr_data[i]._one			=	_one			;
        thr_data[i]._imintau_E		=	_imintau_E		;
        thr_data[i]._dt             =	_dt             ;
        thr_data[i]._sigma_sqrt_dt  =   _sigma_sqrt_dt  ;
        thr_data[i]._sigma			=	_sigma			;
        thr_data[i]._gamma_I		=	_gamma_I		;
        thr_data[i]._imintau_I		=	_imintau_I		;
        thr_data[i]._min_d_I		=	_min_d_I		;
        thr_data[i]._b_I			=	_b_I			;
        thr_data[i]._J_NMDA         =	_J_NMDA         ;
        thr_data[i]._w_I__I_0		=	_w_I__I_0		;
        thr_data[i]._a_I			=	_a_I			;
        thr_data[i]._min_d_E		=	_min_d_E		;
        thr_data[i]._b_E			=	_b_E			;
        thr_data[i]._a_E			=	_a_E			;
        thr_data[i]._w_plus_J_NMDA	=	_w_plus_J_NMDA	;
        thr_data[i]._w_E__I_0		=	_w_E__I_0		;
        thr_data[i].nodes_vec		=	nodes_vec		;
        thr_data[i].SC_cap			=	SC_cap			;
        thr_data[i].SC_inpreg       =	SC_inpreg       ;
        thr_data[i].reg_globinp_p	=	reg_globinp_p	;
        thr_data[i].n_conn_table	=	n_conn_table	;
        thr_data[i].time_steps      =   time_steps      ;
        thr_data[i].BOLD_TR         =   BOLD_TR         ;
        thr_data[i].model_dt        =   model_dt        ;
        thr_data[i].BOLD_ex         =   BOLD_ex         ;
        thr_data[i].rand_num_seed   =   rand_num_seed   ;
        thr_data[i].BOLD_ts_len     =   BOLD_ts_len     ;
        thr_data[i].output_file     =   output_file     ;

        if ((rc = pthread_create(&thr[i], NULL, run_simulation, &thr_data[i]))) {
            fprintf(stderr, "error: pthread_create, rc: %d\n", rc);
            return EXIT_FAILURE;
        }
    }

    /* block until all threads complete */
    for (i = 0; i < n_threads; ++i) {
        pthread_join(thr[i], NULL);
    }
    printf("Threads finished. Back to main thread.\n");

    printf("MT-TVBii finished. Execution took %.2f s for %d nodes. Goodbye!\n", (float)(time(NULL) - start), nodes);

    return 0;
}


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

/* A variable-coefficient linear multistep Adams method in Nordsieck
   form. This stepper uses explicit Adams-Bashforth (predictor) and
   implicit Adams-Moulton (corrector) methods in P(EC)^m functional
   iteration mode. Method order varies dynamically between 1 and 12.

   References:

   Byrne, G. D., and Hindmarsh, A. C., A Polyalgorithm for the
   Numerical Solution of Ordinary Differential Equations,
   ACM Trans. Math. Software, 1 (1975), pp. 71-96.

   Brown, P. N., Byrne, G. D., and Hindmarsh, A. C., VODE: A
   Variable-coefficient ODE Solver, SIAM J. Sci. Stat. Comput. 10,
   (1989), pp. 1038-1051.

   Hindmarsh, A. C., Brown, P. N., Grant, K. E., Lee, S. L., Serban,
   R., Shumaker, D. E., and Woodward, C. S., SUNDIALS: Suite of
   Nonlinear and Differential/Algebraic Equation Solvers, ACM
   Trans. Math. Software 31 (2005), pp. 363-396.

   Note: The algorithms have been adapted for GSL ode-initval2
   framework.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_blas.h>

#include "odeiv_util.h"

/* Maximum order of Adams methods */
#define MSADAMS_MAX_ORD 12

typedef struct
{
  /* Nordsieck history matrix. Includes concatenated
     Nordsieck vectors [y_n, h*y_n', (h^2/2!)*y_n'', ...,
     (h^ord/ord!)*d^(ord)(y_n)]. Nordsieck vector number i is located
     at z[i*dim] (i=0..ord).
   */
  double *z;

  double *zbackup;              /* backup of Nordsieck matrix */
  double *ytmp;                 /* work area */
  double *ytmp2;                /* work area */
  double *pc;                   /* product term coefficients */
  double *l;                    /* polynomial coefficients */
  double *hprev;                /* previous step sizes */
  double *hprevbackup;          /* backup of hprev */
  double *errlev;               /* desired error level of y */
  gsl_vector *abscor;           /* absolute y values for correction */
  gsl_vector *relcor;           /* relative y values for correction */
  gsl_vector *svec;             /* saved abscor & work area */
  gsl_vector *tempvec;          /* work area */
  const gsl_odeiv2_driver *driver;      /* pointer to gsl_odeiv2_driver object */

  long int ni;                  /* stepper call counter */
  size_t ord;                   /* current order of method */
  size_t ordprev;               /* order of previous call */
  size_t ordprevbackup;         /* backup of ordprev */
  double tprev;                 /* t point of previous call */
  size_t ordwait;               /* counter for order change */
  size_t ordwaitbackup;         /* backup of ordwait */
  size_t failord;               /* order of convergence failure */
  double failt;                 /* t point of convergence failure */
  double ordm1coeff;            /* saved order-1 coefficiet */
  double ordp1coeffprev;        /* saved order+1 coefficient */
  size_t failcount;             /* counter for rejected steps */
}
msadams_state_t;

/* Introduce msadams_reset for use in msadams_alloc and _apply */

static int msadams_reset (void *, size_t);

static void *
msadams_alloc (size_t dim)
{
  msadams_state_t *state =
    (msadams_state_t *) malloc (sizeof (msadams_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for msadams_state",
                      GSL_ENOMEM);
    }

  state->z =
    (double *) malloc ((MSADAMS_MAX_ORD + 1) * dim * sizeof (double));

  if (state->z == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for z", GSL_ENOMEM);
    }

  state->zbackup =
    (double *) malloc ((MSADAMS_MAX_ORD + 1) * dim * sizeof (double));

  if (state->zbackup == 0)
    {
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for zbackup", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->ytmp2 = (double *) malloc (dim * sizeof (double));

  if (state->ytmp2 == 0)
    {
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp2", GSL_ENOMEM);
    }

  state->pc = (double *) malloc ((MSADAMS_MAX_ORD + 1) * sizeof (double));

  if (state->pc == 0)
    {
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for pc", GSL_ENOMEM);
    }

  state->l = (double *) malloc ((MSADAMS_MAX_ORD + 1) * sizeof (double));

  if (state->l == 0)
    {
      free (state->pc);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for l", GSL_ENOMEM);
    }

  state->hprev = (double *) malloc (MSADAMS_MAX_ORD * sizeof (double));

  if (state->hprev == 0)
    {
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprev", GSL_ENOMEM);
    }

  state->hprevbackup = (double *) malloc (MSADAMS_MAX_ORD * sizeof (double));

  if (state->hprevbackup == 0)
    {
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprevbackup", GSL_ENOMEM);
    }

  state->errlev = (double *) malloc (dim * sizeof (double));

  if (state->errlev == 0)
    {
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for errlev", GSL_ENOMEM);
    }

  state->abscor = gsl_vector_alloc (dim);

  if (state->abscor == 0)
    {
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for abscor", GSL_ENOMEM);
    }

  state->relcor = gsl_vector_alloc (dim);

  if (state->relcor == 0)
    {
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for relcor", GSL_ENOMEM);
    }

  state->svec = gsl_vector_alloc (dim);

  if (state->svec == 0)
    {
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for svec", GSL_ENOMEM);
    }

  state->tempvec = gsl_vector_alloc (dim);

  if (state->tempvec == 0)
    {
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for tempvec", GSL_ENOMEM);
    }

  msadams_reset ((void *) state, dim);

  state->driver = NULL;

  return state;
}

static int
msadams_failurehandler (void *vstate, const size_t dim, const double t)
{
  /* Internal failure handler routine for msadams. Adjusted strategy
     for GSL: Decrease order if this is the second time a failure
     has occurred at this order and point.
   */

  msadams_state_t *state = (msadams_state_t *) vstate;

  const size_t ord = state->ord;

  if (ord > 1 && (ord - state->ordprev == 0) &&
      ord == state->failord && t == state->failt)
    {
      state->ord--;
    }

  /* Save information about failure */

  state->failord = ord;
  state->failt = t;
  state->ni++;

  /* Force reinitialization if failure took place at lowest
     order
   */

  if (ord == 1)
    {
      msadams_reset (vstate, dim);
    }

  return GSL_SUCCESS;
}

static int
msadams_calccoeffs (const size_t ord, const size_t ordwait,
                    const double h, const double hprev[],
                    double pc[], double l[],
                    double *errcoeff, double *ordm1coeff,
                    double *ordp1coeff, double *ordp2coeff)
{

  /* Calculates coefficients (l) of polynomial Lambda, error and
     auxiliary order change evaluation coefficients.
   */

  if (ord == 1)
    {
      l[0] = 1.0;
      l[1] = 1.0;
      *errcoeff = 0.5;
      *ordp1coeff = 1.0;
      *ordp2coeff = 12.0;
    }
  else
    {
      size_t i, j;
      double hsum = h;
      double st1 = 0.0;         /* sum term coefficients */
      double st2 = 0.0;

      /* Calculate coefficients (pc) of product terms */

      DBL_ZERO_MEMSET (pc, MSADAMS_MAX_ORD + 1);

      pc[0] = 1.0;

      for (i = 1; i < ord; i++)
        {
          /* Calculate auxiliary coefficient used in evaluation of
             change of order
           */

          if (i == ord - 1 && ordwait == 1)
            {
              int s = 1;

              *ordm1coeff = 0.0;

              for (j = 0; j < ord - 1; j++)
                {
                  *ordm1coeff += s * pc[j] / (j + 2);
                  s = -s;
                }

              *ordm1coeff = pc[ord - 2] / (ord * (*ordm1coeff));
            }

          for (j = i; j > 0; j--)
            {
              pc[j] += pc[j - 1] * h / hsum;
            }

          hsum += hprev[i - 1];
        }

      /* Calculate sum term 1 for error estimation */

      {
        int s = 1;

        for (i = 0; i < ord; i++)
          {
            st1 += s * pc[i] / (i + 1.0);
            s = -s;
          }
      }

      /* Calculate sum term 2 for error estimation */

      {
        int s = 1;

        for (i = 0; i < ord; i++)
          {
            st2 += s * pc[i] / (i + 2.0);
            s = -s;
          }
      }

      /* Calculate the actual polynomial coefficients (l) */

      DBL_ZERO_MEMSET (l, MSADAMS_MAX_ORD + 1);

      l[0] = 1.0;

      for (i = 1; i < ord + 1; i++)
        {
          l[i] = pc[i - 1] / (i * st1);
        }

#ifdef DEBUG
      {
        size_t di;

        printf ("-- calccoeffs l: ");
        for (di = 0; di < ord + 1; di++)
          {
            printf ("%.5e ", l[di]);
          }
        printf ("\n");

        printf ("-- calccoeffs pc: ");
        for (di = 0; di < ord; di++)
          {
            printf ("%.5e ", pc[di]);
          }
        printf ("\n");

        printf ("-- calccoeffs st1=%.5e, st2=%.5e\n", st1, st2);
      }
#endif

      /* Calculate error coefficient */

      *errcoeff = (h / hsum) * (st2 / st1);

      /* Calculate auxiliary coefficients used in evaluation of change
         of order
       */

      if (ordwait < 2)
        {
          int s = 1;

          *ordp1coeff = hsum / (h * l[ord]);

          *ordp2coeff = 0.0;

          for (i = ord; i > 0; i--)
            {
              pc[i] += pc[i - 1] * (h / hsum);
            }

          for (i = 0; i < ord + 1; i++)
            {
              *ordp2coeff += s * pc[i] / (i + 2);
              s = -s;
            }

          *ordp2coeff = (ord + 1) * st1 / (*ordp2coeff);
        }
    }

#ifdef DEBUG
  printf ("-- calccoeffs ordm1coeff=%.5e ", *ordm1coeff);
  printf ("ordp1coeff=%.5e ", *ordp1coeff);
  printf ("ordp2coeff=%.5e ", *ordp2coeff);
  printf ("errcoeff=%.5e\n", *errcoeff);
#endif

  return GSL_SUCCESS;
}

static int
msadams_corrector (void *vstate, const gsl_odeiv2_system * sys,
                   const double t, const double h, const size_t dim,
                   const double z[], const double errlev[],
                   const double l[], const double errcoeff,
                   gsl_vector * abscor, gsl_vector * relcor,
                   double ytmp[], double ytmp2[])
{
  /* Calculates the correction step (abscor). Non-linear equation
     system is solved by functional iteration.
   */

  size_t mi, i;
  const size_t max_iter = 3;    /* Maximum number of iterations */
  double convrate = 1.0;        /* convergence rate */
  double stepnorm = 0.0;        /* norm of correction step */
  double stepnormprev = 0.0;

  /* Evaluate at predicted values */

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, ytmp);

    if (s == GSL_EBADFUNC)
      {
        return s;
      }

    if (s != GSL_SUCCESS)
      {
        msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
        printf ("-- FAIL at user function evaluation\n");
#endif
        return s;
      }
  }

  /* Calculate correction step (abscor) */

  gsl_vector_set_zero (abscor);

  for (mi = 0; mi < max_iter; mi++)
    {
      const double safety = 0.3;
      const double safety2 = 0.1;

      /* Calculate new y values to ytmp2 */

      for (i = 0; i < dim; i++)
        {
          ytmp[i] *= h;
          ytmp[i] -= z[1 * dim + i];
          ytmp[i] /= l[1];
          ytmp2[i] = z[0 * dim + i] + ytmp[i];
        }

#ifdef DEBUG
      {
        size_t di;
        printf ("-- dstep: ");
        for (di = 0; di < dim; di++)
          {
            printf ("%.5e ", ytmp[di]);
          }
        printf ("\n");
      }
#endif
      /* Convergence test. Norms used are root-mean-square norms. */

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (relcor, i,
                          (ytmp[i] - gsl_vector_get (abscor, i)) / errlev[i]);
          gsl_vector_set (abscor, i, ytmp[i]);
        }

      stepnorm = gsl_blas_dnrm2 (relcor) / sqrt (dim);

      if (mi > 0)
        {
          convrate = GSL_MAX_DBL (safety * convrate, stepnorm / stepnormprev);
        }
      else
        {
          convrate = 1.0;
        }

      {
        const double convtest =
          GSL_MIN_DBL (convrate, 1.0) * stepnorm * errcoeff / safety2;

#ifdef DEBUG
        printf
          ("-- func iter loop %d, errcoeff=%.5e, stepnorm =%.5e, convrate = %.5e, convtest = %.5e\n",
           (int) mi, errcoeff, stepnorm, convrate, convtest);
#endif
        if (convtest <= 1.0)
          {
            break;
          }
      }

      /* Check for divergence during iteration */
      {
        const double div_const = 2.0;

        if (mi > 1 && stepnorm > div_const * stepnormprev)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL, diverging functional iteration\n");
#endif
            return GSL_FAILURE;
          }
      }

      /* Evaluate at new y */

      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp2, ytmp);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

      stepnormprev = stepnorm;
    }

#ifdef DEBUG
  printf ("-- functional iteration exit at mi=%d\n", (int) mi);
#endif

  /* Handle convergence failure */

  if (mi == max_iter)
    {
      msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
      printf ("-- FAIL, max_iter reached\n");
#endif
      return GSL_FAILURE;
    }

  return GSL_SUCCESS;
}

static int
msadams_eval_order (gsl_vector * abscor, gsl_vector * tempvec,
                    gsl_vector * svec, const double errcoeff,
                    const size_t dim, const double errlev[],
                    const double ordm1coeff, const double ordp1coeff,
                    const double ordp1coeffprev, const double ordp2coeff,
                    const double hprev[],
                    const double h, const double z[],
                    size_t * ord, size_t * ordwait)
{
  /* Evaluates and executes change in method order (current, current-1
     or current+1). Order which maximizes the step length is selected.
   */

  size_t i;

  /* step size estimates at current order, order-1 and order+1 */
  double ordest = 0.0;
  double ordm1est = 0.0;
  double ordp1est = 0.0;

  const double safety = 1e-6;
  const double bias = 6.0;
  const double bias2 = 10.0;

  /* Relative step length estimate for current order */

  ordest = 1.0 / (pow (bias * gsl_blas_dnrm2 (abscor) / sqrt (dim)
                       * errcoeff, 1.0 / (*ord + 1)) + safety);

  /* Relative step length estimate for order ord - 1 */

  if (*ord > 1)
    {
      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (tempvec, i, z[*ord * dim + i] / errlev[i]);
        }

      ordm1est = 1.0 / (pow (bias * gsl_blas_dnrm2 (tempvec) / sqrt (dim)
                             / ordm1coeff, 1.0 / (*ord)) + safety);
    }
  else
    {
      ordm1est = 0.0;
    }

  /* Relative step length estimate for order ord + 1 */

  if (*ord < MSADAMS_MAX_ORD)
    {
      const double c = -ordp1coeff / ordp1coeffprev *
        pow (h / hprev[1], *ord + 1);

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (svec, i) * c +
                          gsl_vector_get (abscor, i));
        }

      ordp1est = 1.0 / (pow (bias2 * gsl_blas_dnrm2 (svec) / sqrt (dim)
                             / ordp2coeff, 1.0 / (*ord + 2)) + safety);
    }
  else
    {
      ordp1est = 0.0;
    }

#ifdef DEBUG
  printf
    ("-- eval_order ord=%d, ordest=%.5e, ordm1est=%.5e, ordp1est=%.5e\n",
     (int) *ord, ordest, ordm1est, ordp1est);
#endif

  /* Choose order that maximises step size and increases step
     size markedly compared to current step
   */

  {
    const double min_incr = 1.5;

    if (ordm1est > ordest && ordm1est > ordp1est && ordm1est > min_incr)
      {
        *ord -= 1;
#ifdef DEBUG
        printf ("-- eval_order order DECREASED to %d\n", (int) *ord);
#endif
      }

    else if (ordp1est > ordest && ordp1est > ordm1est && ordp1est > min_incr)
      {
        *ord += 1;
#ifdef DEBUG
        printf ("-- eval_order order INCREASED to %d\n", (int) *ord);
#endif
      }
  }

  *ordwait = *ord + 2;

  return GSL_SUCCESS;
}

static int
msadams_apply (void *vstate, size_t dim, double t, double h,
               double y[], double yerr[],
               const double dydt_in[], double dydt_out[],
               const gsl_odeiv2_system * sys)
{
  /* Conducts a step by Adams linear multistep methods. */

  msadams_state_t *state = (msadams_state_t *) vstate;

  double *const z = state->z;
  double *const zbackup = state->zbackup;
  double *const ytmp = state->ytmp;
  double *const ytmp2 = state->ytmp2;
  double *const pc = state->pc;
  double *const l = state->l;
  double *const hprev = state->hprev;
  double *const hprevbackup = state->hprevbackup;
  double *const errlev = state->errlev;
  gsl_vector *const abscor = state->abscor;
  gsl_vector *const relcor = state->relcor;
  gsl_vector *const svec = state->svec;
  gsl_vector *const tempvec = state->tempvec;

  size_t ord = state->ord;
  double ordm1coeff = 0.0;
  double ordp1coeff = 0.0;
  double ordp2coeff = 0.0;
  double errcoeff = 0.0;        /* error coefficient */

  int deltaord;

#ifdef DEBUG
  {
    size_t di;

    printf ("msadams_apply: t=%.5e, ord=%d, h=%.5e, y:", t, (int) ord, h);

    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", y[di]);
      }
    printf ("\n");
  }
#endif

  /* Check if t is the same as on previous stepper call (or last
     failed call). This means that calculation of previous step failed
     or the step was rejected, and therefore previous state will be
     restored or the method will be reset.
   */

  if (state->ni > 0 && (t == state->tprev || t == state->failt))
    {
      if (state->ni == 1)
        {
          /* No step has been accepted yet, reset method */

          msadams_reset (vstate, dim);
#ifdef DEBUG
          printf ("-- first step was REJECTED, msadams_reset called\n");
#endif
        }
      else
        {
          /* A succesful step has been saved, restore previous state. */

          /* If previous step suggests order increase, but the step was
             rejected, then do not increase order.
           */

          if (ord > state->ordprev)
            {
              state->ord = state->ordprev;
              ord = state->ord;
            }

          /* Restore previous state */

          DBL_MEMCPY (z, zbackup, (MSADAMS_MAX_ORD + 1) * dim);
          DBL_MEMCPY (hprev, hprevbackup, MSADAMS_MAX_ORD);
          state->ordprev = state->ordprevbackup;
          state->ordwait = state->ordwaitbackup;

#ifdef DEBUG
          printf ("-- previous step was REJECTED, state restored\n");
#endif
        }

      /* If step is repeatedly rejected, then reset method */

      state->failcount++;

      {
        const size_t max_failcount = 3;

        if (state->failcount > max_failcount && state->ni > 1)
          {
            msadams_reset (vstate, dim);
            ord = state->ord;

#ifdef DEBUG
            printf ("-- max_failcount reached, msadams_reset called\n");
#endif
          }
      }
    }
  else
    {
      /* The previous step was accepted. Backup current state. */

      DBL_MEMCPY (zbackup, z, (MSADAMS_MAX_ORD + 1) * dim);
      DBL_MEMCPY (hprevbackup, hprev, MSADAMS_MAX_ORD);
      state->ordprevbackup = state->ordprev;
      state->ordwaitbackup = state->ordwait;

      state->failcount = 0;

#ifdef DEBUG
      if (state->ni > 0)
        {
          printf ("-- previous step was ACCEPTED, state saved\n");
        }
#endif
    }

#ifdef DEBUG
  printf ("-- ord=%d, ni=%ld, ordwait=%d\n", (int) ord, state->ni,
          (int) state->ordwait);
  printf ("-- ordprev: %d\n", (int) state->ordprev);
#endif

  /* Get desired error levels via gsl_odeiv2_control object through driver
     object, which is a requirement for this stepper.
   */

  if (state->driver == NULL)
    {
      return GSL_EFAULT;
    }
  else
    {
      size_t i;

      for (i = 0; i < dim; i++)
        {
          if (dydt_in != NULL)
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           dydt_in[i], h, i, &errlev[i]);
            }
          else
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           0.0, h, i, &errlev[i]);
            }
        }
    }

#ifdef DEBUG
  {
    size_t di;
    printf ("-- errlev: ");
    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", errlev[di]);
      }
    printf ("\n");
  }
#endif

  /* On first call initialize Nordsieck matrix */

  if (state->ni == 0)
    {
      size_t i;

      DBL_ZERO_MEMSET (z, (MSADAMS_MAX_ORD + 1) * dim);

      if (dydt_in != NULL)
        {
          DBL_MEMCPY (ytmp, dydt_in, dim);
        }
      else
        {
          int s = GSL_ODEIV_FN_EVAL (sys, t, y, ytmp);

          if (s != GSL_SUCCESS)
            {
              return s;
            }
        }

      DBL_MEMCPY (&z[0 * dim], y, dim);
      DBL_MEMCPY (&z[1 * dim], ytmp, dim);

      for (i = 0; i < dim; i++)
        {
          z[1 * dim + i] *= h;
        }
    }

  /* Sanity check  */

  deltaord = ord - state->ordprev;

  if (deltaord > 1 || deltaord < -1)
    {
      printf ("-- order change %d\n", deltaord);
      GSL_ERROR_NULL ("msadams_apply too large order change", GSL_ESANITY);
    }

  /* Modify Nordsieck matrix if order or step length has been changed */

  /* If order increased by 1, initialize new Nordsieck vector */

  if (deltaord == 1)
    {
      DBL_ZERO_MEMSET (&z[ord * dim], dim);

#ifdef DEBUG
      printf ("-- order increase detected, Nordsieck modified\n");
#endif
    }

  /* If order decreased by 1, adjust Nordsieck matrix */

  if (deltaord == -1)
    {
      double hsum = 0.0;
      size_t i, j;

      /* Calculate coefficients used in adjustment to l */

      DBL_ZERO_MEMSET (l, MSADAMS_MAX_ORD + 1);

      l[1] = 1.0;

      for (i = 1; i < ord; i++)
        {
          hsum += hprev[i - 1];

          for (j = i + 1; j > 0; j--)
            {
              l[j] *= hsum / hprev[0];
              l[j] += l[j - 1];
            }
        }

      for (i = 1; i < ord; i++)
        {
          l[i + 1] = (ord + 1) * l[i] / (i + 1);
        }

      /* Scale Nordsieck matrix */

      for (i = 2; i < ord + 1; i++)
        for (j = 0; j < dim; j++)
          {
            z[i * dim + j] += -l[i] * z[(ord + 1) * dim + j];
          }

#ifdef DEBUG
      printf ("-- order decrease detected, Nordsieck modified\n");
#endif
    }

  /* Scale Nordsieck vectors if step size has been changed */

  if (state->ni > 0 && h != hprev[0])
    {
      size_t i, j;
      const double hrel = h / hprev[0];
      double coeff = hrel;

      for (i = 1; i < ord + 1; i++)
        {
          for (j = 0; j < dim; j++)
            {
              z[i * dim + j] *= coeff;
            }

          coeff *= hrel;
        }

#ifdef DEBUG
      printf ("-- h != hprev, Nordsieck modified\n");
#endif
    }

  /* Calculate polynomial coefficients (l), error coefficient and
     auxiliary coefficients
   */

  msadams_calccoeffs (ord, state->ordwait, h, hprev, pc, l, &errcoeff,
                      &ordm1coeff, &ordp1coeff, &ordp2coeff);

  /* Carry out the prediction step */

  {
    size_t i, j, k;

    for (i = 1; i < ord + 1; i++)
      for (j = ord; j > i - 1; j--)
        for (k = 0; k < dim; k++)
          {
            z[(j - 1) * dim + k] += z[j * dim + k];
          }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- predicted y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif
  }

  /* Calculate correction step to abscor */
  {
    int s;
    s = msadams_corrector (vstate, sys, t, h, dim, z, errlev, l, errcoeff,
                           abscor, relcor, ytmp, ytmp2);
    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    /* Add accepted final correction step to Nordsieck matrix */

    size_t i, j;

    for (i = 0; i < ord + 1; i++)
      for (j = 0; j < dim; j++)
        {
          z[i * dim + j] += l[i] * gsl_vector_get (abscor, j);
        }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- corrected y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif

    /* Derivatives at output */

    if (dydt_out != NULL)
      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, dydt_out);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

    /* Calculate error estimate */

    for (i = 0; i < dim; i++)
      {
        yerr[i] = fabs (gsl_vector_get (abscor, i)) * errcoeff;
      }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- yerr: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", yerr[di]);
        }
      printf ("\n");
    }
#endif

    /* Save y values */

    for (i = 0; i < dim; i++)
      {
        y[i] = z[0 * dim + i];
      }
  }

  /* Scale abscor with errlev for later use in norm calculations */
  {
    size_t i;

    for (i = 0; i < dim; i++)
      {
        gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) / errlev[i]);
      }
  }

  /* Save items needed for evaluation of order increase on next
     call, if needed
   */

  if (state->ordwait == 1 && ord < MSADAMS_MAX_ORD)
    {
      size_t i;

      state->ordp1coeffprev = ordp1coeff;
      state->ordm1coeff = ordm1coeff;

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (abscor, i));
        }
    }

  /* Consider and execute order change for next step */

  if (state->ordwait == 0)
    {
      msadams_eval_order (abscor, tempvec, svec, errcoeff, dim, errlev,
                          state->ordm1coeff, ordp1coeff,
                          state->ordp1coeffprev, ordp2coeff,
                          hprev, h, z, &(state->ord), &(state->ordwait));
    }

  /* Save information about current step in state and update counters */
  {
    size_t i;

    state->ordprev = ord;

    for (i = MSADAMS_MAX_ORD - 1; i > 0; i--)
      {
        hprev[i] = hprev[i - 1];
      }
    hprev[0] = h;

#ifdef DEBUG
    {
      size_t di;
      printf ("-- hprev: ");
      for (di = 0; di < MSADAMS_MAX_ORD; di++)
        {
          printf ("%.5e ", hprev[di]);
        }
      printf ("\n");
    }
#endif

    state->tprev = t;
    state->ordwait--;
    state->ni++;
  }

  return GSL_SUCCESS;
}

static int
msadams_set_driver (void *vstate, const gsl_odeiv2_driver * d)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  state->driver = d;

  return GSL_SUCCESS;
}

static int
msadams_reset (void *vstate, size_t dim)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  state->ni = 0;
  state->ord = 1;
  state->ordprev = 1;
  state->ordprevbackup = 1;
  state->ordwait = 2;
  state->ordwaitbackup = 2;
  state->failord = 0;
  state->failt = GSL_NAN;
  state->failcount = 0;

  DBL_ZERO_MEMSET (state->hprev, MSADAMS_MAX_ORD);
  DBL_ZERO_MEMSET (state->z, (MSADAMS_MAX_ORD + 1) * dim);

#ifdef DEBUG
  printf ("-- msadams_reset called\n");
#endif

  return GSL_SUCCESS;
}

static unsigned int
msadams_order (void *vstate)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  return state->ord;
}

static void
msadams_free (void *vstate)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  gsl_vector_free (state->tempvec);
  gsl_vector_free (state->svec);
  gsl_vector_free (state->relcor);
  gsl_vector_free (state->abscor);
  free (state->errlev);
  free (state->hprevbackup);
  free (state->hprev);
  free (state->l);
  free (state->pc);
  free (state->ytmp2);
  free (state->ytmp);
  free (state->zbackup);
  free (state->z);
  free (state);
}

static const gsl_odeiv2_step_type msadams_type = {
  "msadams",                    /* name */
  1,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &msadams_alloc,
  &msadams_apply,
  &msadams_set_driver,
  &msadams_reset,
  &msadams_order,
  &msadams_free
};

const gsl_odeiv2_step_type *gsl_odeiv2_step_msadams = &msadams_type;