#ifdef __cplusplus
extern "C"
{
#endif // __cplusplus
#ifdef USE_BLAS
#include <cblas.h>

int _use_blas()
{
    return 1;
}
#else // USE_BLAS
#include <xmmintrin.h>

#if defined(_MSC_VER)
#define ALIGNAS(byte_alignment) __declspec(align(byte_alignment))
#elif defined(__GNUC__)
#define ALIGNAS(byte_alignment) __attribute__((aligned(byte_alignment)))
#endif

float cblas_snrm2(const int N, const float *m1, const int incX)
{
    if (N % 4 != 0) {
        fprintf(stderr, "cblas_snrm2() expects N to be a multiple of 4.\n");
        exit(EXIT_FAILURE);
    }

    float norm = 0;
    ALIGNAS(16) float z[4];
    __m128 X;
    __m128 Z = _mm_setzero_ps();

    for (int i=0; i<N; i+=4) {
        X = _mm_load_ps(&m1[i]);
        X = _mm_mul_ps(X, X);
        Z = _mm_add_ps(X, Z);
    }

    _mm_store_ps(z, Z);
    norm += z[0] + z[1] + z[2] + z[3];
    return sqrtf(norm);
}

float cblas_sdot(const int N, const float *m1, const int incX,
                 const float *m2, const int incY)
{
    if (N % 4 != 0) {
        fprintf(stderr, "cblas_sdot() expects N to be a multiple of 4.\n");
        exit(EXIT_FAILURE);
    }

    float dot = 0;
    ALIGNAS(16) float z[4];
    __m128 X, Y;
    __m128 Z = _mm_setzero_ps();

    for (int i=0; i<N; i+=4) {
        X = _mm_load_ps(&m1[i]);
        Y = _mm_load_ps(&m2[i]);
        X = _mm_mul_ps(X, Y);
        Z = _mm_add_ps(X, Z);
    }

    _mm_store_ps(z, Z);
    dot += z[0] + z[1] + z[2] + z[3];
    return dot;
}

int _use_blas()
{
    return 0;
}
#endif // USE_BLAS
#ifdef __cplusplus
}
#endif // __cplusplus


#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_randist.h>
#include <errno.h>
#include <string.h>
#include <time.h>

const int SAMPLENUM = 1000;
int obsnum;
gsl_rng * r;

typedef struct {
	int prey;
	int predator;
} lvstate;

typedef struct {
	double time;
	double rawdata;
} obsdata;

void simPrior(lvstate *simData);
void rowsample(int *rows, double *w);
void stepLV(lvstate *state, double *t0p, double *dtp, double *lvParam);
void peturb(double *lvParam);

double obsLik(lvstate *mystate, double obs) {
	const double SIGMA = 10.0;
	return log(gsl_ran_gaussian_pdf((obs - mystate->prey), SIGMA));
}

void callStepLV(lvstate *simData, double *t0p, double *dtp, double *lvParam) {
	int j;
	for (j=0;j<SAMPLENUM;j++) {
		stepLV(simData + j, t0p, dtp, lvParam);
	}

}

//particle filter: output new estimated lvstate and its likehood
void pfPropPar(lvstate ppstate[16], obsdata *myobsdata, double *lvParam,
		double *ll) {
	//printf("Starting pfPropPar...\n");
	typedef struct {
		lvstate *vpptuple;
	} stateVec;

	int i, j, k, l;

	double timeUnit = 2.0;
	int deltaTimeUnit = 1;
	double delTime = timeUnit * deltaTimeUnit;
	double curTime = 0.0;

	double lw[SAMPLENUM];
	double w[SAMPLENUM];
	double wSum;
	double maxLw;
	double likehood = 0.0;
	unsigned int rows[SAMPLENUM];

	//printf("Starting pfPropPar 1 ...\n");
	stateVec *myStateVec = malloc(obsnum * sizeof(stateVec));
	if (myStateVec == NULL) {
		printf("Fail to initiate myStateVec...Exit");
		goto freeMem;
	}
	memset(myStateVec, 0, obsnum * sizeof(stateVec));

	stateVec *tempStateVec = malloc(obsnum * sizeof(stateVec));
	if (tempStateVec == NULL) {
		printf("Fail to initiate tempStateVec...Exit");
		goto freeMem;
	}
	memset(tempStateVec, 0, obsnum * sizeof(stateVec));

	lvstate *simData = malloc(SAMPLENUM * sizeof(lvstate));
	if (simData == NULL) {
		printf("Fail to initiate simData...EXIT!");
		goto freeMem;
	}
	memset(simData, 0, SAMPLENUM * sizeof(lvstate));
	//initiate state: get simulated prey-predator tuple
	simPrior(simData);

	*ll = 0.0;

	if ((tempStateVec + 0)->vpptuple == NULL) {
		(tempStateVec + 0)->vpptuple = (lvstate*) malloc(
				SAMPLENUM * sizeof(lvstate));
		if ((tempStateVec + 0)->vpptuple == NULL) {
			printf("Fail to initiate myStateVec->vpptuple...Exit");
			goto freeMem;
			//free(tempStateVec);
			//goto error;
		}
	}

	memset(tempStateVec[0].vpptuple, 0, SAMPLENUM * sizeof(lvstate));
	for (j = 0; j < SAMPLENUM; j++) {
		((tempStateVec + 0)->vpptuple + j)->predator = (simData + j)->predator;
		((tempStateVec + 0)->vpptuple + j)->prey = (simData + j)->prey;
	}

	for (i = 1; i < obsnum; i++) {
		curTime = myobsdata[i - 1].time;

		wSum = 0.0;

		if ((tempStateVec + i)->vpptuple == NULL) {
			//printf("init temp \n");
			(tempStateVec + i)->vpptuple = (lvstate*) malloc(
					SAMPLENUM * sizeof(lvstate));
			if ((tempStateVec + i)->vpptuple == NULL) {
				printf("Fail to initiate myStateVec->vpptuple...Exit");
				goto freeMem;
			}
			memset((tempStateVec + i)->vpptuple, 0,
					SAMPLENUM * sizeof(lvstate));
		}

		//printf("tempvec 1 %i\n", ((tempStateVec+0)->vpptuple+1)->prey);
		callStepLV(simData, &curTime, &delTime, lvParam);

		for (j = 0; j < SAMPLENUM; j++) {
			//stepLV(simData + j, &curTime, &delTime, lvParam);
			((tempStateVec + i)->vpptuple + j)->predator =
					(simData + j)->predator;
			((tempStateVec + i)->vpptuple + j)->prey = (simData + j)->prey;
			//printf("tempState %i vpptuple %i , %p\n", i,j,(tempStateVec+i)->vpptuple+j);
			lw[j] = obsLik(((tempStateVec + i)->vpptuple + j),
					myobsdata[i].rawdata);

			if (j == 0) {
				maxLw = lw[0];
			} else {
				if (lw[j] > maxLw) {
					maxLw = lw[j];
				}
			}
		}

		for (l = 0; l < SAMPLENUM; l++) {
			w[l] = exp(lw[l] - maxLw);
			wSum = wSum + w[l];
		}
		rowsample(rows, w);

		//reorganize the vector with only keeping the rows in 'rows'
		for (k = 0; k <= i; k++) {
			if ((myStateVec + k)->vpptuple == NULL) {
				//printf("init mystatevec  \n");
				(myStateVec + k)->vpptuple = (lvstate*) malloc(
						SAMPLENUM * sizeof(lvstate));
				if ((myStateVec + k)->vpptuple == NULL) {
					printf("Fail to initiate myStateVec->vpptuple...Exit");
					goto freeMem;
				}
			}
//			memset(myStateVec[k].vpptuple,0,SAMPLENUM * sizeof(lvstate));

			for (j = 0; j < SAMPLENUM; j++) {
				((myStateVec + k)->vpptuple + j)->predator =
						((tempStateVec + k)->vpptuple + rows[j])->predator;
				((myStateVec + k)->vpptuple + j)->prey =
						((tempStateVec + k)->vpptuple + rows[j])->prey;
				//printf("row %i %i,%i,%i,%i,%i\n", i,k,j,rows[j],((myStateVec+k)->vpptuple+j)->prey,((tempStateVec+k)->vpptuple+rows[j])->prey);
			}
			for (j = 0; j < SAMPLENUM; j++) {
				((tempStateVec + k)->vpptuple + j)->predator =
						((myStateVec + k)->vpptuple + j)->predator;
				((tempStateVec + k)->vpptuple + j)->prey =
						((myStateVec + k)->vpptuple + j)->prey;
			}
		}
		for (j = 0; j < SAMPLENUM; j++) {
			simData[j].predator = ((myStateVec + i)->vpptuple + j)->predator;
			simData[j].prey = ((myStateVec + i)->vpptuple + j)->prey;
		}
		likehood = likehood + maxLw + log(wSum / SAMPLENUM);
		*ll = likehood;
		rows[SAMPLENUM] = NULL;
	}

	for (i = 0; i < obsnum; i++) {
		ppstate[i].prey = ((myStateVec + i)->vpptuple + 0)->prey;
		ppstate[i].predator = ((myStateVec + i)->vpptuple + 0)->predator;
	}

	freeMem:
	//printf("free mem...");
	if (myStateVec != NULL) {
		//printf("free mem 2...");
		for (i = 0; ((myStateVec + i)->vpptuple != NULL) && i < obsnum; i++) {
			//printf("free myStateVec...i %i\n", i);
			free((myStateVec + i)->vpptuple);
			(myStateVec + i)->vpptuple = NULL;
		}
		free(myStateVec);
		myStateVec = NULL;
	}
	if (tempStateVec != NULL) {
		for (i = 0; NULL != (tempStateVec + i)->vpptuple && i < obsnum; i++) {
			//printf("free tempStateVec...i %i\n", i);
			free((tempStateVec + i)->vpptuple);
			(tempStateVec + i)->vpptuple = NULL;
		}
		free(tempStateVec);
		tempStateVec = NULL;
	}
	if (simData != NULL) {
		free(simData);
		simData = NULL;
	}

}

void stepLV(lvstate *state, double *t0p, double *dtp, double *lvParam) {
	//printf("Starting stepLV...\n");
	register double t0 = *t0p, dt = *dtp, t;
	register double h0, h1, h2, h3, u, l0, l1, l2;
	register int prey, predator;
	prey = state->prey;
	predator = state->predator;
	l0 = lvParam[0];
	l1 = lvParam[1];
	l2 = lvParam[2];

	while (dt > 0) {
		h1 = l0 * prey;
		h2 = l1 * prey * predator;
		h3 = l2 * predator;
		h0 = h1 + h2 + h3;

		if ((h0 < (1e-10)) || (prey >= 1000000))
			t = 1e99;
		else {
			t = gsl_ran_exponential(r, 1/h0);
		}
		if (t > dt) {
			state->prey = prey;
			state->predator = predator;
			return;
		} else {
			u = gsl_rng_uniform(r);
			if (u < (h1 / h0)) {
				prey = prey + 1;
				t0 = t0 + t;
				dt = dt - t;
			} else if (u < ((h1 + h2) / h0)) {
				prey = prey - 1;
				predator = predator + 1;
				t0 = t0 + t;
				dt = dt - t;
			} else {
				predator = predator - 1;
				t0 = t0 + t;
				dt = dt - t;
			}
		}
	}

	return;
}

//mcmc process
void runPmmhPath(int its, double *lvParam, double *obslik, lvstate ppstate[16],
		obsdata *myobsdata) {
	//printf("Starting runPmmhPath...\n");
	int i, j;
	double propParam[3];
	double curParam[3];
	double ll;
	double propMll, curMll = -1e99;
	lvstate curPath[obsnum];
	lvstate propPath[obsnum];

	FILE *fp = fopen("mcmc-out.txt", "wab+");
	if (fp == NULL) {
		printf("file is null.\n");
		return;
	}
	fprintf(fp, "th1,th2,th3,");
	for (i = 0; i <= 30; i++) {
		if ((i % 2) == 0) {
			if (i == 30) {
				fprintf(fp, "x%i,y%i", i, i);
			} else {
				fprintf(fp, "x%i,y%i,", i, i);
			}
		}
	}
	fprintf(fp, "\n");

	memcpy(curPath, ppstate, sizeof(lvstate) * obsnum);
	memcpy(curParam, lvParam, sizeof(double) * 3);
	for (i = its; i > 0; i--) {
		//calculate measurements and decide moving to next state or not
		memcpy(propParam, curParam, sizeof(double) * 3);
		peturb(propParam);

		pfPropPar(propPath, myobsdata, propParam, &ll);
		propMll = ll;
		if (log(gsl_ran_flat(r, 0.0, 1.0)) < (propMll - curMll)) {
			curMll = propMll;
			memcpy(curParam, propParam, sizeof(double) * 3);
			memcpy(curPath, propPath, sizeof(lvstate) * obsnum);
		}
		//write current parameters and state sequence into output
		fprintf(fp, "%f,%f,%f,", curParam[0], curParam[1], curParam[2]);

		for (j = 0; j < obsnum; j++) {
			if (j == (obsnum - 1)) {
				fprintf(fp, "%i,%i", (int)curPath[j].prey, (int)curPath[j].predator);
			} else {
				fprintf(fp, "%i,%i,", (int)curPath[j].prey, (int)curPath[j].predator);
			}
		}
		fprintf(fp, "\n");
	}
	fclose(fp);
}

void rowsample(int *rows, double *w) {
	//printf("Starting rowsample...\n");
	gsl_ran_discrete_t * grdp;
	int row;
	int i;
	grdp = gsl_ran_discrete_preproc(SAMPLENUM, w);
	for (i = 0; i < SAMPLENUM; i++) {

		row = (int) gsl_ran_discrete(r, grdp);
		rows[i] = row;

	}
	gsl_ran_discrete_free(grdp);
	return;
}

void peturb(double *lvParam) {
	//printf("Starting peturb...\n");

	const double SIGMA = 0.01;
	//printf("lvParam_before %f, %f, %f\n", lvParam[0], lvParam[1], lvParam[2]);
	lvParam[0] = lvParam[0] * exp(gsl_ran_gaussian(r, SIGMA));
	lvParam[1] = lvParam[1] * exp(gsl_ran_gaussian(r, SIGMA));
	lvParam[2] = lvParam[2] * exp(gsl_ran_gaussian(r, SIGMA));
	//printf("lvParam_after  %f, %f, %f\n", lvParam[0], lvParam[1], lvParam[2]);
}

//to get the prior simulate value of prey and predator
void simPrior(lvstate *simData) {
	//printf("Starting simPrior...\n");
	int i;

	const double PREY_MEAN = 50.0;
	const double PRED_MEAN = 100.0;

	for (i = 0; i < SAMPLENUM; i++) {
		simData[i].prey = gsl_ran_poisson(r, PREY_MEAN);
		simData[i].predator = gsl_ran_poisson(r, PRED_MEAN);
		//printf("simPrior %i\n", simData[i].prey);
	}
	//printf("End simPrior...\n");

}

void runModel(int its) {
	obsnum = 16;
	obsdata myobsdata[obsnum];
	lvstate mylvstate[obsnum];
	double ll = 0.0;

	double lvParam[3] = { 1.0, 0.005, 0.6 };

	//produce time list(with even number in)
	int i;
	int j = 0;

	for (i = 0; i <= 30; i++) {
		if ((i % 2) == 0) {
			myobsdata[j].time = (double) i;
			j++;
		}
	}

	//read external txt file
	FILE *file = fopen("LVpreyNoise10.txt", "r");
	char line[1024];
	if (file == NULL) {
		printf("file is null.\n");
		exit(EXIT_FAILURE);
	}
	if (file != NULL) {
		i = 0;
		while (fgets(line, 1024, file)) {
			myobsdata[i].rawdata = atof(line);
			i++;
		}
		fclose(file);
	}
	runPmmhPath(its, lvParam, &ll, mylvstate, myobsdata);
	return;
}

int main(int argc, char *argv[]) {
	const gsl_rng_type * T;
	long seed;
	gsl_rng_env_setup();
	T = gsl_rng_default;
	r = gsl_rng_alloc(T);
	seed = time(NULL) * getpid();    // set seed
	gsl_rng_set(r, seed);
	clock_t begin, end;
	double time_spent;

	begin = clock();
	int its;
	printf("Starting main...\n");

	if (argc == 1) {
		its = 5;
	} else {
		its = atoi(argv[1]);
	}
	runModel(its);

	gsl_rng_free(r);

	end = clock();
	time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
	printf("Time consuming in total is %f\n", time_spent);

	printf("Running for %i", its);
	printf(" iterations, Done.");
	return 0;
}


/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "xform.h"
#include "imgfeatures.h"
#include "utils.h"

//#include "cxcore.h"
//#include <highgui.h>
// Universal include for all versions of OpenCV
#include <mrpt/otherlibs/do_opencv_includes.h>

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

#include <time.h>

/************************* Local Function Prototypes *************************/

static __inline struct feature* get_match( struct feature*, int );
int get_matched_features( struct feature*, int, int, struct feature*** );
int calc_min_inliers( int, int, double, double );
struct feature** draw_ransac_sample( struct feature**, int, int, gsl_rng* );
void extract_corresp_pts( struct feature**, int, int, CvPoint2D64f**,
						 CvPoint2D64f** );
int find_consensus( struct feature**, int, int, CvMat*, ransac_err_fn,
				   double, struct feature*** );
static __inline void release_mem( CvPoint2D64f*, CvPoint2D64f*,
struct feature** );

/********************** Functions prototyped in xform.h **********************/

/*
Calculates a best-fit image transform from image feature correspondences
using RANSAC.

For more information refer to:

Fischler, M. A. and Bolles, R. C.  Random sample consensus: a paradigm for
model fitting with applications to image analysis and automated cartography.
Communications of the ACM, 24, 6 (1981), pp. 381--395.

@param features an array of features; only features with a non-NULL match
	of type mtype are used in homography computation
@param n number of features in feat
@param mtype determines which of each feature's match fields to use
	for model computation; should be one of FEATURE_FWD_MATCH,
	FEATURE_BCK_MATCH, or FEATURE_MDL_MATCH; if this is FEATURE_MDL_MATCH,
	correspondences are assumed to be between a feature's img_pt field
	and its match's mdl_pt field, otherwise correspondences are assumed to
	be between the the feature's img_pt field and its match's img_pt field
@param xform_fn pointer to the function used to compute the desired
	transformation from feature correspondences
@param m minimum number of correspondences necessary to instantiate the
	model computed by xform_fn
@param p_badxform desired probability that the final transformation
	returned by RANSAC is corrupted by outliers (i.e. the probability that
	no samples of all inliers were drawn)
@param err_fn pointer to the function used to compute a measure of error
	between putative correspondences and a computed model
@param err_tol correspondences within this distance of a computed model are
	considered as inliers
@param inliers if not NULL, output as an array of pointers to the final
	set of inliers
@param n_in if not NULL and \a inliers is not NULL, output as the final
	number of inliers

@return Returns a transformation matrix computed using RANSAC or NULL
	on error or if an acceptable transform could not be computed.
*/
CvMat* ransac_xform( struct feature* features, int n, int mtype,
					ransac_xform_fn xform_fn, int m, double p_badxform,
					ransac_err_fn err_fn, double err_tol,
struct feature*** inliers, int* n_in )
{
	struct feature** matched, ** sample, ** consensus, ** consensus_max = NULL;
	struct ransac_data* rdata;
	CvPoint2D64f* pts, * mpts;
	CvMat* M = NULL;
	gsl_rng* rng;
	double p, in_frac = RANSAC_INLIER_FRAC_EST;
	int i, nm, in, in_min, in_max = 0, k = 0;

	nm = get_matched_features( features, n, mtype, &matched );
	if( nm < m )
	{
		fprintf( stderr, "Warning: not enough matches to compute xform, %s" \
			" line %d\n", __FILE__, __LINE__ );
		goto end;
	}

	/* initialize random number generator */
	rng = gsl_rng_alloc( gsl_rng_mt19937 );
	gsl_rng_set( rng, time(NULL) );

	in_min = calc_min_inliers( nm, m, RANSAC_PROB_BAD_SUPP, p_badxform );
	p = pow( 1.0 - pow( in_frac, m ), k );
	i = 0;
	while( p > p_badxform )
	{
		sample = draw_ransac_sample( matched, nm, m, rng );
		extract_corresp_pts( sample, m, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, m );
		if( ! M )
			goto iteration_end;
		in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);
		if( in > in_max )
		{
			if( consensus_max )
				free( consensus_max );
			consensus_max = consensus;
			in_max = in;
			in_frac = (double)in_max / nm;
		}
		else
			free( consensus );
		cvReleaseMat( &M );

iteration_end:
		release_mem( pts, mpts, sample );
		p = pow( 1.0 - pow( in_frac, m ), ++k );
	}

	/* calculate final transform based on best consensus set */
	if( in_max >= in_min )
	{
		extract_corresp_pts( consensus_max, in_max, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, in_max );
		in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);
		cvReleaseMat( &M );
		release_mem( pts, mpts, consensus_max );
		extract_corresp_pts( consensus, in, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, in );
		if( inliers )
		{
			*inliers = consensus;
			consensus = NULL;
		}
		if( n_in )
			*n_in = in;
		release_mem( pts, mpts, consensus );
	}
	else if( consensus_max )
	{
		if( inliers )
			*inliers = NULL;
		if( n_in )
			*n_in = 0;
		free( consensus_max );
	}

	gsl_rng_free( rng );
end:
	for( i = 0; i < nm; i++ )
	{
		rdata = feat_ransac_data( matched[i] );
		matched[i]->feature_data = rdata->orig_feat_data;
		free( rdata );
	}
	free( matched );
	return M;
}

/*
Calculates a least-squares planar homography from point correspondeces.

@param pts array of points
@param mpts array of corresponding points; each pts[i], i=0..n-1, corresponds
	to mpts[i]
@param n number of points in both pts and mpts; must be at least 4

@return Returns the 3 x 3 least-squares planar homography matrix that
	transforms points in pts to their corresponding points in mpts or NULL if
	fewer than 4 correspondences were provided
*/
CvMat* lsq_homog( CvPoint2D64f* pts, CvPoint2D64f* mpts, int n )
{
	CvMat* H, * A, * B, X;
	double x[9];
	int i;

	if( n < 4 )
	{
		fprintf( stderr, "Warning: too few points in lsq_homog(), %s line %d\n",
			__FILE__, __LINE__ );
		return NULL;
	}

	/* set up matrices so we can unstack homography into X; AX = B */
	A = cvCreateMat( 2*n, 8, CV_64FC1 );
	B = cvCreateMat( 2*n, 1, CV_64FC1 );
	X = cvMat( 8, 1, CV_64FC1, x );
	H = cvCreateMat(3, 3, CV_64FC1);
	cvZero( A );
	for( i = 0; i < n; i++ )
	{
		cvmSet( A, i, 0, pts[i].x );
		cvmSet( A, i+n, 3, pts[i].x );
		cvmSet( A, i, 1, pts[i].y );
		cvmSet( A, i+n, 4, pts[i].y );
		cvmSet( A, i, 2, 1.0 );
		cvmSet( A, i+n, 5, 1.0 );
		cvmSet( A, i, 6, -pts[i].x * mpts[i].x );
		cvmSet( A, i, 7, -pts[i].y * mpts[i].x );
		cvmSet( A, i+n, 6, -pts[i].x * mpts[i].y );
		cvmSet( A, i+n, 7, -pts[i].y * mpts[i].y );
		cvmSet( B, i, 0, mpts[i].x );
		cvmSet( B, i+n, 0, mpts[i].y );
	}
	cvSolve( A, B, &X, CV_SVD );
	x[8] = 1.0;
	X = cvMat( 3, 3, CV_64FC1, x );
	cvConvert( &X, H );

	cvReleaseMat( &A );
	cvReleaseMat( &B );
	return H;
}

/*
Calculates the transfer error between a point and its correspondence for
a given homography, i.e. for a point x, it's correspondence x', and
homography H, computes d(x', Hx)^2.

@param pt a point
@param mpt pt's correspondence
@param H a homography matrix

@return Returns the transfer error between pt and mpt given H
*/
double homog_xfer_err( CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H )
{
	CvPoint2D64f xpt = persp_xform_pt( pt, H );

	return sqrt( dist_sq_2D( xpt, mpt ) );
}

/*
Performs a perspective transformation on a single point.  That is, for a
point (x, y) and a 3 x 3 matrix T this function returns the point
(u, v), where

[x' y' w']^T = T * [x y 1]^T,

and

(u, v) = (x'/w', y'/w').

Note that affine transforms are a subset of perspective transforms.

@param pt a 2D point
@param T a perspective transformation matrix

@return Returns the point (u, v) as above.
*/
CvPoint2D64f persp_xform_pt( CvPoint2D64f pt, CvMat* T )
{
	CvMat XY, UV;
	double xy[3] = { pt.x, pt.y, 1.0 }, uv[3] = { 0 };
	CvPoint2D64f rslt;

	cvInitMatHeader( &XY, 3, 1, CV_64FC1, xy, CV_AUTOSTEP );
	cvInitMatHeader( &UV, 3, 1, CV_64FC1, uv, CV_AUTOSTEP );
	cvMatMul( T, &XY, &UV );
	rslt = cvPoint2D64f( uv[0] / uv[2], uv[1] / uv[2] );

	return rslt;
}

/************************ Local funciton definitions *************************/

/*
Returns a feature's match according to a specified match type

@param feat feature
@param mtype match type, one of FEATURE_FWD_MATCH, FEATURE_BCK_MATCH, or
FEATURE_MDL_MATCH

@return Returns feat's match corresponding to mtype or NULL for bad mtype
*/
static __inline struct feature* get_match( struct feature* feat, int mtype )
{
	if( mtype == FEATURE_MDL_MATCH )
		return feat->mdl_match;
	if( mtype == FEATURE_BCK_MATCH )
		return feat->bck_match;
	if( mtype == FEATURE_FWD_MATCH )
		return feat->fwd_match;
	return NULL;
}

/*
Finds all features with a match of a specified type and stores pointers
to them in an array.  Additionally initializes each matched feature's
feature_data field with a ransac_data structure.

@param features array of features
@param n number of features in features
@param mtype match type, one of FEATURE_{FWD,BCK,MDL}_MATCH
@param matched output as an array of pointers to features with a match of
the specified type

@return Returns the number of features output in matched.
*/
int get_matched_features( struct feature* features, int n, int mtype,
struct feature*** matched )
{
	struct feature** _matched;
	struct ransac_data* rdata;
	int i, m = 0;

	_matched = calloc( n, sizeof( struct feature* ) );
	for( i = 0; i < n; i++ )
		if( get_match( features + i, mtype ) )
		{
			rdata = malloc( sizeof( struct ransac_data ) );
			memset( rdata, 0, sizeof( struct ransac_data ) );
			rdata->orig_feat_data = features[i].feature_data;
			_matched[m] = features + i;
			_matched[m]->feature_data = rdata;
			m++;
		}
		*matched = _matched;
		return m;
}

/*
Calculates the minimum number of inliers as a function of the number of
putative correspondences.  Based on equation (7) in

Chum, O. and Matas, J.  Matching with PROSAC -- Progressive Sample Consensus.
In Conference on Computer Vision and Pattern Recognition (CVPR),
(2005), pp. 220--226.

@param n number of putative correspondences
@param m min number of correspondences to compute the model in question
@param p_badsupp prob. that a bad model is supported by a correspondence
@param p_badxform desired prob. that the final transformation returned is bad

@return Returns the minimum number of inliers required to guarantee, based
	on p_badsupp, that the probability that the final transformation returned
	by RANSAC is less than p_badxform
*/
int calc_min_inliers( int n, int m, double p_badsupp, double p_badxform )
{
	double pi, sum;
	int i, j;

	for( j = m+1; j <= n; j++ )
	{
		sum = 0;
		for( i = j; i <= n; i++ )
		{
			pi = ( i - m ) * log( p_badsupp ) + ( n - i + m ) * log( 1.0 - p_badsupp ) +
				gsl_sf_lnchoose( n - m, i - m );
			sum += exp( pi );
		}
		if( sum < p_badxform )
			break;
	}
	return j;
}

/*
Draws a RANSAC sample from a set of features.

@param features array of pointers to features from which to sample
@param n number of features in features
@param m size of the sample
@param rng random number generator used to sample

@return Returns an array of pointers to the sampled features; the sampled
	field of each sampled feature's ransac_data is set to 1
*/
struct feature** draw_ransac_sample( struct feature** features, int n,
									int m, gsl_rng* rng )
{
	struct feature** sample, * feat;
	struct ransac_data* rdata;
	int i, x;

	for( i = 0; i < n; i++ )
	{
		rdata = feat_ransac_data( features[i] );
		rdata->sampled = 0;
	}

	sample = calloc( m, sizeof( struct feature* ) );
	for( i = 0; i < m; i++ )
	{
		do
		{
			x = gsl_rng_uniform_int( rng, n );
			feat = features[x];
			rdata = feat_ransac_data( feat );
		}
		while( rdata->sampled );
		sample[i] = feat;
		rdata->sampled = 1;
	}

	return sample;
}

/*
Extrancs raw point correspondence locations from a set of features

@param features array of features from which to extract points and match
	points; each of these is assumed to have a match of type mtype
@param n number of features
@param mtype match type; if FEATURE_MDL_MATCH correspondences are assumed
	to be between each feature's img_pt field and it's match's mdl_pt field,
	otherwise, correspondences are assumed to be between img_pt and img_pt
@param pts output as an array of raw point locations from features
@param mpts output as an array of raw point locations from features' matches
*/
void extract_corresp_pts( struct feature** features, int n, int mtype,
						 CvPoint2D64f** pts, CvPoint2D64f** mpts )
{
	struct feature* match;
	CvPoint2D64f* _pts, * _mpts;
	int i;

	_pts = calloc( n, sizeof( CvPoint2D64f ) );
	_mpts = calloc( n, sizeof( CvPoint2D64f ) );

	if( mtype == FEATURE_MDL_MATCH )
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			_pts[i] = features[i]->img_pt;
			_mpts[i] = match->mdl_pt;
		}

	else
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			_pts[i] = features[i]->img_pt;
			_mpts[i] = match->img_pt;
		}

		*pts = _pts;
		*mpts = _mpts;
}

/*
For a given model and error function, finds a consensus from a set of
feature correspondences.

@param features set of pointers to features; every feature is assumed to
	have a match of type mtype
@param n number of features in features
@param mtype determines the match field of each feature against which to
	measure error; if this is FEATURE_MDL_MATCH, correspondences are assumed
	to be between the feature's img_pt field and the match's mdl_pt field;
	otherwise matches are assumed to be between img_pt and img_pt
@param M model for which a consensus set is being found
@param err_fn error function used to measure distance from M
@param err_tol correspondences within this distance of M are added to the
	consensus set
@param consensus output as an array of pointers to features in the
	consensus set

@return Returns the number of points in the consensus set
*/
int find_consensus( struct feature** features, int n, int mtype,
				   CvMat* M, ransac_err_fn err_fn, double err_tol,
				   struct feature*** consensus )
{
	struct feature** _consensus;
	struct feature* match;
	CvPoint2D64f pt, mpt;
	double err;
	int i, in = 0;

	_consensus = calloc( n, sizeof( struct feature* ) );

	if( mtype == FEATURE_MDL_MATCH )
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			pt = features[i]->img_pt;
			mpt = match->mdl_pt;
			err = err_fn( pt, mpt, M );
			if( err <= err_tol )
				_consensus[in++] = features[i];
		}

	else
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			pt = features[i]->img_pt;
			mpt = match->img_pt;
			err = err_fn( pt, mpt, M );
			if( err <= err_tol )
				_consensus[in++] = features[i];
		}
	*consensus = _consensus;
	return in;
}

/*
Releases memory and reduces code size above

@param pts1 an array of points
@param pts2 an array of points
@param features an array of pointers to features; can be NULL
*/
static __inline void release_mem( CvPoint2D64f* pts1, CvPoint2D64f* pts2,
struct feature** features )
{
	free( pts1 );
	free( pts2 );
	if( features )
		free( features );
}


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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>
#include <gsl/gsl_integration.h>

#include "ccl.h"

//
// Macros for replacing relative paths
#define EXPAND_STR(s) STRING(s)
#define STRING(s) #s

const ccl_configuration default_config = {
  ccl_boltzmann_class, ccl_halofit, ccl_nobaryons,
  ccl_tinker10, ccl_duffy2008, ccl_emu_strict};

//Precision parameters
/**
 * Default relative precision if not otherwise specified
 */
#define GSL_EPSREL 1E-4

/**
 * Default number of iterations for integration and root-finding if not otherwise
 * specified
 */
#define GSL_N_ITERATION 1000

/**
 * Default number of Gauss-Kronrod points in QAG integration if not otherwise
 * specified
 */
#define GSL_INTEGRATION_GAUSS_KRONROD_POINTS GSL_INTEG_GAUSS41

/**
 * Relative precision in sigma_R calculations
 */
#define GSL_EPSREL_SIGMAR 1E-5

/**
 * Relative precision in k_NL calculations
 */
#define GSL_EPSREL_KNL 1E-5

/**
 * Relative precision in distance calculations
 */
#define GSL_EPSREL_DIST 1E-6

/**
 * Relative precision in growth calculations
 */
#define GSL_EPSREL_GROWTH 1E-6

/**
 * Relative precision in dNdz calculations
 */
#define GSL_EPSREL_DNDZ 1E-6

const ccl_gsl_params default_gsl_params = {
  GSL_N_ITERATION,                     // N_ITERATION
  GSL_INTEGRATION_GAUSS_KRONROD_POINTS,// INTEGRATION_GAUSS_KRONROD_POINTS
  GSL_EPSREL,                          // INTEGRATION_EPSREL
  GSL_INTEGRATION_GAUSS_KRONROD_POINTS,// INTEGRATION_LIMBER_GAUSS_KRONROD_POINTS
  GSL_EPSREL,                          // INTEGRATION_LIMBER_EPSREL
  GSL_EPSREL_DIST,                     // INTEGRATION_DISTANCE_EPSREL
  GSL_EPSREL_SIGMAR,                   // INTEGRATION_SIGMAR_EPSREL
  GSL_EPSREL_KNL,                      // INTEGRATION_KNL_EPSREL
  GSL_EPSREL,                          // ROOT_EPSREL
  GSL_N_ITERATION,                     // ROOT_N_ITERATION
  GSL_EPSREL_GROWTH,                   // ODE_GROWTH_EPSREL
  1E-6,                                // EPS_SCALEFAC_GROWTH
  1E7,                                 // HM_MMIN
  1E17,                                // HM_MMAX
  0.0,                                 // HM_EPSABS
  1E-4,                                // HM_EPSREL
  1000,                                // HM_LIMIT
  GSL_INTEG_GAUSS41                    // HM_INT_METHOD
  };

#undef GSL_EPSREL
#undef GSL_N_ITERATION
#undef GSL_INTEGRATION_GAUSS_KRONROD_POINTS
#undef GSL_EPSREL_SIGMAR
#undef GSL_EPSREL_KNL
#undef GSL_EPSREL_DIST
#undef GSL_EPSREL_GROWTH
#undef GSL_EPSREL_DNDZ

const ccl_spline_params default_spline_params = {

  // scale factor spline params
  250,  // A_SPLINE_NA
  0.1,  // A_SPLINE_MIN
  0.01,  // A_SPLINE_MINLOG_PK
  0.1,  // A_SPLINE_MIN_PK,
  0.01,  // A_SPLINE_MINLOG_SM,
  0.1,  // A_SPLINE_MIN_SM,
  1.0,  // A_SPLINE_MAX,
  0.0001,  // A_SPLINE_MINLOG,
  250,  // A_SPLINE_NLOG,

  // mass splines
  0.025,  // LOGM_SPLINE_DELTA
  50,  // LOGM_SPLINE_NM
  6,  // LOGM_SPLINE_MIN
  17,  // LOGM_SPLINE_MAX

  // PS a and k spline
  13,  // A_SPLINE_NA_SM
  6,  // A_SPLINE_NLOG_SM
  40,  // A_SPLINE_NA_PK
  11,  // A_SPLINE_NLOG_PK

  // k-splines and integrals
  50,  // K_MAX_SPLINE
  1E3,  // K_MAX
  5E-5,  // K_MIN
  0.025,  // DLOGK_INTEGRATION
  167,  // N_K
  100000,  // N_K_3DCOR

  // correlation function parameters
  0.01,  // ELL_MIN_CORR
  60000,  // ELL_MAX_CORR
  5000,  // N_ELL_CORR

  //Spline types
  NULL,
  NULL,
  NULL,
  NULL,
  NULL,
  NULL,
  NULL
};

ccl_physical_constants ccl_constants = {
  /**
   * Lightspeed / H0 in units of Mpc/h (from CODATA 2014)
   */
  2997.92458,

  /**
   * Newton's gravitational constant in units of m^3/Kg/s^2
   */
  //6.6738e-11,  /(from PDG 2013) in m^3/Kg/s^2
  //6.67428e-11, // CLASS VALUE
  6.67408e-11, // from CODATA 2014

  /**
   * Solar mass in units of kg (from GSL)
   */
  //GSL_CONST_MKSA_SOLAR_MASS,
  //1.9885e30, //(from PDG 2015) in Kg
  1.9884754153381438E+30, //from IAU 2015

  /**
   * Mpc to meters (from PDG 2016 and using M_PI)
   */
  3.085677581491367399198952281E+22,

  /**
   * pc to meters (from PDG 2016 and using M_PI)
   */
  3.085677581491367399198952281E+16,

  /**
   * Rho critical in units of M_sun/h / (Mpc/h)^3
   */
  ((3*100*100)/(8*M_PI*6.67408e-11)) * (1000*1000*3.085677581491367399198952281E+22/1.9884754153381438E+30),

  /**
   * Boltzmann constant in units of J/K
  */
  //GSL_CONST_MKSA_BOLTZMANN,
  1.38064852E-23, //from CODATA 2014

  /**
   * Stefan-Boltzmann constant in units of kg/s^3 / K^4
   */
  //GSL_CONST_MKSA_STEFAN_BOLTZMANN_CONSTANT,
  5.670367E-8, //from CODATA 2014

  /**
   * Planck's constant in units kg m^2 / s
   */
  //GSL_CONST_MKSA_PLANCKS_CONSTANT_H,
  6.626070040E-34, //from CODATA 2014

  /**
   * The speed of light in m/s
   */
  //GSL_CONST_MKSA_SPEED_OF_LIGHT,
  299792458.0, //from CODATA 2014

  /**
   * Electron volt to Joules convestion
   */
  //GSL_CONST_MKSA_ELECTRON_VOLT,
  1.6021766208e-19,  //from CODATA 2014

  /**
   * Temperature of the CMB in K
   */
  2.725,
  //2.7255, // CLASS value

  /**
   * T_ncdm, as taken from CLASS, explanatory.ini
   */
  0.71611,

  /**
   * neutrino mass splitting differences
   * See Lesgourgues and Pastor, 2012 for these values.
   * Adv. High Energy Phys. 2012 (2012) 608515,
   * arXiv:1212.6154, page 13
  */
  7.62E-5,
  2.55E-3,
  -2.43E-3
};

/* ------- ROUTINE: ccl_cosmology_create ------
INPUTS: ccl_parameters params
        ccl_configuration config
TASK: creates the ccl_cosmology struct and passes some values to it
DEFINITIONS:
chi: comoving distance [Mpc]
growth: growth function (density)
fgrowth: logarithmic derivative of the growth (density) (dlnD/da?)
E: E(a)=H(a)/H0
growth0: growth at z=0, defined to be 1
sigma: ?
p_lnl: nonlinear matter power spectrum at z=0?
computed_distances, computed_growth,
computed_power, computed_sigma: store status of the computations
*/
ccl_cosmology * ccl_cosmology_create(ccl_parameters params, ccl_configuration config)
{
  ccl_cosmology * cosmo = malloc(sizeof(ccl_cosmology));
  cosmo->params = params;
  cosmo->config = config;
  cosmo->gsl_params = default_gsl_params;
  cosmo->spline_params = default_spline_params;
  cosmo->spline_params.A_SPLINE_TYPE = gsl_interp_akima;
  cosmo->spline_params.K_SPLINE_TYPE = gsl_interp_akima;
  cosmo->spline_params.M_SPLINE_TYPE = gsl_interp_akima;
  cosmo->spline_params.D_SPLINE_TYPE = gsl_interp_akima;
  cosmo->spline_params.PNL_SPLINE_TYPE = gsl_interp2d_bicubic;
  cosmo->spline_params.PLIN_SPLINE_TYPE = gsl_interp2d_bicubic;
  cosmo->spline_params.CORR_SPLINE_TYPE = gsl_interp_akima;

  cosmo->data.chi = NULL;
  cosmo->data.growth = NULL;
  cosmo->data.fgrowth = NULL;
  cosmo->data.E = NULL;
  cosmo->data.growth0 = 1.;
  cosmo->data.achi = NULL;

  cosmo->data.logsigma = NULL;

  cosmo->data.rsd_splines[0] = NULL;
  cosmo->data.rsd_splines[1] = NULL;
  cosmo->data.rsd_splines[2] = NULL;

  cosmo->computed_distances = false;
  cosmo->computed_growth = false;
  cosmo->computed_sigma = false;
  cosmo->status = 0;
  ccl_cosmology_set_status_message(cosmo, "");

  if(cosmo->spline_params.A_SPLINE_MAX !=1.) {
    cosmo->status = CCL_ERROR_SPLINE;
    ccl_cosmology_set_status_message(cosmo,
                                     "ccl_core.c: ccl_cosmology_create(): "
                                     "A_SPLINE_MAX needs to be 1.\n");
  }

  return cosmo;
}

/* ------ ROUTINE: ccl_parameters_fill_initial -------
INPUT: ccl_parameters: params
TASK: fill parameters not set by ccl_parameters_create with some initial values
DEFINITIONS:
Omega_g = (Omega_g*h^2)/h^2 is the radiation parameter; "g" is for photons, as in CLASS
T_CMB: CMB temperature in Kelvin
Omega_l: Lambda
A_s: amplitude of the primordial PS, enforced here to initially set to NaN
sigma8: variance in 8 Mpc/h spheres for normalization of matter PS, enforced here to initially set to NaN
z_star: recombination redshift
 */
void ccl_parameters_fill_initial(ccl_parameters * params, int *status)
{
  // Fixed radiation parameters
  // Omega_g * h**2 is known from T_CMB
  params->T_CMB =  ccl_constants.T_CMB;
  // kg / m^3
  double rho_g = 4. * ccl_constants.STBOLTZ / pow(ccl_constants.CLIGHT, 3) * pow(params->T_CMB, 4);
  // kg / m^3
  double rho_crit =
    ccl_constants.RHO_CRITICAL *
    ccl_constants.SOLAR_MASS/pow(ccl_constants.MPC_TO_METER, 3) *
    pow(params->h, 2);
  params->Omega_g = rho_g/rho_crit;

  // Get the N_nu_rel from Neff and N_nu_mass
  params->N_nu_rel = params->Neff - params->N_nu_mass * pow(ccl_constants.TNCDM, 4) / pow(4./11.,4./3.);

  // Temperature of the relativistic neutrinos in K
  double T_nu= (params->T_CMB) * pow(4./11.,1./3.);
  // in kg / m^3
  double rho_nu_rel =
    params->N_nu_rel* 7.0/8.0 * 4. *
    ccl_constants.STBOLTZ / pow(ccl_constants.CLIGHT, 3) *
    pow(T_nu, 4);
  params-> Omega_nu_rel = rho_nu_rel/rho_crit;

  // If non-relativistic neutrinos are present, calculate the phase_space integral.
  if((params->N_nu_mass)>0) {
    params->Omega_nu_mass = ccl_Omeganuh2(
      1.0, params->N_nu_mass, params->m_nu, params->T_CMB, status) / ((params->h)*(params->h));
  }
  else{
    params->Omega_nu_mass = 0.;
  }

  params->Omega_m = params->Omega_b + params-> Omega_c + params->Omega_nu_mass;
  params->Omega_l = 1.0 - params->Omega_m - params->Omega_g - params->Omega_nu_rel - params->Omega_k;
  // Initially undetermined parameters - set to nan to trigger
  // problems if they are mistakenly used.
  if (isfinite(params->A_s)) {params->sigma8 = NAN;}
  if (isfinite(params->sigma8)) {params->A_s = NAN;}
  params->z_star = NAN;

  if(fabs(params->Omega_k)<1E-6)
    params->k_sign=0;
  else if(params->Omega_k>0)
    params->k_sign=-1;
  else
    params->k_sign=1;
  params->sqrtk=sqrt(fabs(params->Omega_k))*params->h/ccl_constants.CLIGHT_HMPC;
}

/* ------ ROUTINE: ccl_parameters_create -------
INPUT: numbers for the basic cosmological parameters needed by CCL
TASK: fill params with some initial values provided by the user
DEFINITIONS:
Omega_c: cold dark matter
Omega_b: baryons
Omega_m: matter
Omega_k: curvature
little omega_x means Omega_x*h^2
Neff : Effective number of neutrino speces
mnu : Pointer to either sum of neutrino masses or list of three masses.
mnu_type : how the neutrino mass(es) should be treated
w0: Dark energy eq of state parameter
wa: Dark energy eq of state parameter, time variation
H0: Hubble's constant in km/s/Mpc.
h: Hubble's constant divided by (100 km/s/Mpc).
A_s: amplitude of the primordial PS
n_s: index of the primordial PS

 */
ccl_parameters ccl_parameters_create(double Omega_c, double Omega_b, double Omega_k,
				     double Neff, double* mnu, int n_mnu,
				     double w0, double wa, double h, double norm_pk,
				     double n_s, double bcm_log10Mc, double bcm_etab,
				     double bcm_ks, double mu_0, double sigma_0,
				     int nz_mgrowth, double *zarr_mgrowth,
				     double *dfarr_mgrowth, int *status)
{
  #ifndef USE_GSL_ERROR
    gsl_set_error_handler_off();
  #endif

  ccl_parameters params;
  // Initialize params
  params.m_nu = NULL;
  params.z_mgrowth=NULL;
  params.df_mgrowth=NULL;
  params.sigma8 = NAN;
  params.A_s = NAN;
  params.Omega_c = Omega_c;
  params.Omega_b = Omega_b;
  params.Omega_k = Omega_k;
  params.Neff = Neff;
  params.m_nu = malloc(n_mnu*sizeof(double));
  params.sum_nu_masses = 0.;
  for(int i = 0; i<n_mnu; i=i+1){
     params.m_nu[i] = mnu[i];
     params.sum_nu_masses = params.sum_nu_masses + mnu[i];
  }

  if(params.sum_nu_masses<1e-15){
    params.N_nu_mass = 0;
  }else{
    params.N_nu_mass = n_mnu;
   }

  // Dark Energy
  params.w0 = w0;
  params.wa = wa;

  // Hubble parameters
  params.h = h;
  params.H0 = h*100;

  // Primordial power spectra
  if(norm_pk<1E-5)
    params.A_s=norm_pk;
  else
    params.sigma8=norm_pk;
  params.n_s = n_s;

  //Baryonic params
  if(bcm_log10Mc<0)
    params.bcm_log10Mc=log10(1.2e14);
  else
    params.bcm_log10Mc=bcm_log10Mc;
  if(bcm_etab<0)
    params.bcm_etab=0.5;
  else
    params.bcm_etab=bcm_etab;
  if(bcm_ks<0)
    params.bcm_ks=55.0;
  else
    params.bcm_ks=bcm_ks;

  // Params of the mu / Sigma parameterisation of MG
  params.mu_0 = mu_0;
  params.sigma_0 = sigma_0;

  // Set remaining standard and easily derived parameters
  ccl_parameters_fill_initial(&params, status);

  //Trigger modified growth function if nz>0
  if(nz_mgrowth>0) {
    params.has_mgrowth=true;
    params.nz_mgrowth=nz_mgrowth;
    params.z_mgrowth=malloc(params.nz_mgrowth*sizeof(double));
    params.df_mgrowth=malloc(params.nz_mgrowth*sizeof(double));
    memcpy(params.z_mgrowth,zarr_mgrowth,params.nz_mgrowth*sizeof(double));
    memcpy(params.df_mgrowth,dfarr_mgrowth,params.nz_mgrowth*sizeof(double));
  }
  else {
    params.has_mgrowth=false;
    params.nz_mgrowth=0;
    params.z_mgrowth=NULL;
    params.df_mgrowth=NULL;
  }

  return params;
}

/**
 * Write a cosmology parameters object to a file in yaml format.
 * @param cosmo Cosmological parameters
 * @param f FILE* pointer opened for reading
 * @return void
 */
void ccl_parameters_write_yaml(ccl_parameters * params, const char * filename, int *status)
{
  FILE * f = fopen(filename, "w");

  if (!f){
    *status = CCL_ERROR_FILE_WRITE;
    return;
  }

#define WRITE_DOUBLE(name) fprintf(f, #name ": %le\n",params->name)
#define WRITE_INT(name) fprintf(f, #name ": %d\n",params->name)

  // Densities: CDM, baryons, total matter, curvature
  WRITE_DOUBLE(Omega_c);
  WRITE_DOUBLE(Omega_b);
  WRITE_DOUBLE(Omega_m);
  WRITE_DOUBLE(Omega_k);
  WRITE_INT(k_sign);

  // Dark Energy
  WRITE_DOUBLE(w0);
  WRITE_DOUBLE(wa);

  // Hubble parameters
  WRITE_DOUBLE(H0);
  WRITE_DOUBLE(h);

  // Neutrino properties
  WRITE_DOUBLE(Neff);
  WRITE_INT(N_nu_mass);
  WRITE_DOUBLE(N_nu_rel);

  if (params->N_nu_mass>0){
    fprintf(f, "m_nu: [");
    for (int i=0; i<params->N_nu_mass; i++){
      fprintf(f, "%le, ", params->m_nu[i]);
    }
    fprintf(f, "]\n");
  }

  WRITE_DOUBLE(sum_nu_masses);
  WRITE_DOUBLE(Omega_nu_mass);
  WRITE_DOUBLE(Omega_nu_rel);

  // Primordial power spectra
  WRITE_DOUBLE(A_s);
  WRITE_DOUBLE(n_s);

  // Radiation parameters
  WRITE_DOUBLE(Omega_g);
  WRITE_DOUBLE(T_CMB);

  // BCM baryonic model parameters
  WRITE_DOUBLE(bcm_log10Mc);
  WRITE_DOUBLE(bcm_etab);
  WRITE_DOUBLE(bcm_ks);

  // Modified gravity parameters
  WRITE_DOUBLE(mu_0);
  WRITE_DOUBLE(sigma_0);

  // Derived parameters
  WRITE_DOUBLE(sigma8);
  WRITE_DOUBLE(Omega_l);
  WRITE_DOUBLE(z_star);

  WRITE_INT(has_mgrowth);
  WRITE_INT(nz_mgrowth);

  if (params->has_mgrowth){
    fprintf(f, "z_mgrowth: [");
    for (int i=0; i<params->nz_mgrowth; i++){
      fprintf(f, "%le, ", params->z_mgrowth[i]);
    }
    fprintf(f, "]\n");

    fprintf(f, "df_mgrowth: [");
    for (int i=0; i<params->nz_mgrowth; i++){
      fprintf(f, "%le, ", params->df_mgrowth[i]);
    }
    fprintf(f, "]\n");
  }

#undef WRITE_DOUBLE
#undef WRITE_INT

  fclose(f);
}

/**
 * Write a cosmology parameters object to a file in yaml format.
 * @param cosmo Cosmological parameters
 * @param f FILE* pointer opened for reading
 * @return void
 */
ccl_parameters ccl_parameters_read_yaml(const char * filename, int *status) {
  FILE * f = fopen(filename, "r");

  if (!f) {
    *status = CCL_ERROR_FILE_READ;
    ccl_parameters bad_params;
    ccl_raise_warning(CCL_ERROR_FILE_READ,
                      "ccl_core.c: ccl_parameters_read_yaml(): "
                      "Failed to read parameters from file.");

    return bad_params;
  }

#define READ_DOUBLE(name) double name; *status |= (0==fscanf(f, #name ": %le\n",&name));
#define READ_INT(name) int name; *status |= (0==fscanf(f, #name ": %d\n",&name))

  // Densities: CDM, baryons, total matter, curvature
  READ_DOUBLE(Omega_c);
  READ_DOUBLE(Omega_b);
  READ_DOUBLE(Omega_m);
  READ_DOUBLE(Omega_k);
  READ_INT(k_sign);

  // Dark Energy
  READ_DOUBLE(w0);
  READ_DOUBLE(wa);

  // Hubble parameters
  READ_DOUBLE(H0);
  READ_DOUBLE(h);

  // Neutrino properties
  READ_DOUBLE(Neff);
  READ_INT(N_nu_mass);
  READ_DOUBLE(N_nu_rel);

  double mnu[3] = {0.0, 0.0, 0.0};
  if (N_nu_mass>0){
    *status |= (0==fscanf(f, "m_nu: ["));
    for (int i=0; i<N_nu_mass; i++){
      *status |= (0==fscanf(f, "%le, ", mnu+i));
    }
    *status |= (0==fscanf(f, "]\n"));
  }

  READ_DOUBLE(sum_nu_masses);
  READ_DOUBLE(Omega_nu_mass);
  READ_DOUBLE(Omega_nu_rel);

  // Primordial power spectra
  READ_DOUBLE(A_s);
  READ_DOUBLE(n_s);

  // Radiation parameters
  READ_DOUBLE(Omega_g);
  READ_DOUBLE(T_CMB);

  // BCM baryonic model parameters
  READ_DOUBLE(bcm_log10Mc);
  READ_DOUBLE(bcm_etab);
  READ_DOUBLE(bcm_ks);

  // Modified gravity parameters
  READ_DOUBLE(mu_0);
  READ_DOUBLE(sigma_0);

  // Derived parameters
  READ_DOUBLE(sigma8);
  READ_DOUBLE(Omega_l);
  READ_DOUBLE(z_star);

  READ_INT(has_mgrowth);
  READ_INT(nz_mgrowth);

  double *z_mgrowth;
  double *df_mgrowth;

  if (has_mgrowth){
    z_mgrowth = malloc(nz_mgrowth*sizeof(double));
    df_mgrowth = malloc(nz_mgrowth*sizeof(double));
    *status |= (0==fscanf(f, "z_mgrowth: ["));
    for (int i=0; i<nz_mgrowth; i++){
      *status |= (0==fscanf(f, "%le, ", z_mgrowth+i));
    }
    *status |= (0==fscanf(f, "]\n"));

    *status |= (0==fscanf(f, "df_mgrowth: ["));
    for (int i=0; i<nz_mgrowth; i++){
      *status |= (0==fscanf(f, "%le, ", df_mgrowth+i));
    }
    *status |= (0==fscanf(f, "]\n"));
  }
  else{
    z_mgrowth = NULL;
    df_mgrowth = NULL;
  }

#undef READ_DOUBLE
#undef READ_INT

  fclose(f);

  if (*status) {
    ccl_raise_warning(
      *status,
      "ccl_core.c: ccl_parameters_read_yaml():"
      "Structure of YAML file incorrect: %s",
      filename);
  }

  double norm_pk;

  if (isnan(A_s)){
    norm_pk = sigma8;
  }
  else{
   norm_pk = A_s;
  }

  ccl_parameters params = ccl_parameters_create(
    Omega_c, Omega_b, Omega_k,
    Neff, mnu, N_nu_mass,
    w0, wa, h, norm_pk,
    n_s, bcm_log10Mc, bcm_etab,
    bcm_ks, mu_0, sigma_0, nz_mgrowth, z_mgrowth,
    df_mgrowth, status);

  if(z_mgrowth) free(z_mgrowth);
  if (df_mgrowth) free(df_mgrowth);

  return params;
}

/* ------- ROUTINE: ccl_data_free --------
INPUT: ccl_data
TASK: free the input data
*/
void ccl_data_free(ccl_data * data) {
  //We cannot assume that all of these have been allocated
  //TODO: it would actually make more sense to do this within ccl_cosmology_free,
  //where we could make use of the flags "computed_distances" etc. to figure out
  //what to free up
  gsl_spline_free(data->chi);
  gsl_spline_free(data->growth);
  gsl_spline_free(data->fgrowth);
  gsl_spline_free(data->E);
  gsl_spline_free(data->achi);
  gsl_spline2d_free(data->logsigma);
  ccl_f1d_t_free(data->rsd_splines[0]);
  ccl_f1d_t_free(data->rsd_splines[1]);
  ccl_f1d_t_free(data->rsd_splines[2]);
}

/* ------- ROUTINE: ccl_cosmology_set_status_message --------
INPUT: ccl_cosmology struct, status_string
TASK: set the status message safely.
*/
void ccl_cosmology_set_status_message(ccl_cosmology * cosmo, const char * message, ...) {
  const int trunc = 480; /* must be < 500 - 4 */

  va_list va;
  va_start(va, message);

  #pragma omp critical
  {
    vsnprintf(cosmo->status_message, trunc, message, va);

    /* if truncation happens, message[trunc - 1] is not NULL, ... will show up. */
    strcpy(&cosmo->status_message[trunc], "...");
  }

  va_end(va);
}

/* ------- ROUTINE: ccl_parameters_free --------
INPUT: ccl_parameters struct
TASK: free allocated quantities in the parameters struct
*/
void ccl_parameters_free(ccl_parameters * params) {
  if (params->m_nu != NULL){
    free(params->m_nu);
    params->m_nu = NULL;
  }
  if (params->z_mgrowth != NULL){
    free(params->z_mgrowth);
    params->z_mgrowth = NULL;
  }
  if (params->df_mgrowth != NULL){
    free(params->df_mgrowth);
    params->df_mgrowth = NULL;
  }
}

/* ------- ROUTINE: ccl_cosmology_free --------
INPUT: ccl_cosmology struct
TASK: free the input data and the cosmology struct
*/
void ccl_cosmology_free(ccl_cosmology * cosmo) {
  if (cosmo != NULL)
    ccl_data_free(&cosmo->data);
  free(cosmo);
}

int ccl_get_pk_spline_na(ccl_cosmology *cosmo) {
  return cosmo->spline_params.A_SPLINE_NA_PK + cosmo->spline_params.A_SPLINE_NLOG_PK - 1;
}

void ccl_get_pk_spline_a_array(ccl_cosmology *cosmo,int ndout,double* doutput,int *status) {
  double *d = NULL;
  if (ndout != ccl_get_pk_spline_na(cosmo))
    *status = CCL_ERROR_INCONSISTENT;
  if (*status == 0) {
    d = ccl_linlog_spacing(cosmo->spline_params.A_SPLINE_MINLOG_PK,
      cosmo->spline_params.A_SPLINE_MIN_PK,
      cosmo->spline_params.A_SPLINE_MAX,
      cosmo->spline_params.A_SPLINE_NLOG_PK,
      cosmo->spline_params.A_SPLINE_NA_PK);
    if (d == NULL)
      *status = CCL_ERROR_MEMORY;
  }
  if(*status==0)
    memcpy(doutput, d, ndout*sizeof(double));

  free(d);
}

int ccl_get_pk_spline_nk(ccl_cosmology *cosmo) {
  double ndecades = log10(cosmo->spline_params.K_MAX) - log10(cosmo->spline_params.K_MIN);
  return (int)ceil(ndecades*cosmo->spline_params.N_K);
}

void ccl_get_pk_spline_lk_array(ccl_cosmology *cosmo,int ndout,double* doutput,int *status) {
  double *d = NULL;
  if (ndout != ccl_get_pk_spline_nk(cosmo))
    *status = CCL_ERROR_INCONSISTENT;
  if (*status == 0) {
    d = ccl_log_spacing(cosmo->spline_params.K_MIN, cosmo->spline_params.K_MAX, ndout);
    if (d == NULL)
      *status = CCL_ERROR_MEMORY;
  }
  if (*status == 0) {
    for(int ii=0; ii < ndout; ii++)
      doutput[ii] = log(d[ii]);
  }
  free(d);
}


/*
 * Barcode
 * Copyright E.G.P. Bos and F.S. Kitaura
 *
 * Distributed under the terms of the MIT License.
 * The full license is in the file LICENSE, distributed with this software.
 */

#pragma once

// EGP: cleared out most includes, just left the two here:
#include <gsl/gsl_rng.h>  // For the gsl_rng type // EGP: added this one
#include "define_opt.h"  // For the ULONG and real_prec types
// #include "curses_funcs.h"
// EGP: toch maar weer terug gezet, gingen dingen mis
/* #include <math.h> */
/* #include <fstream> */
/* #include <iomanip> */
// #include <iostream>
/* #include <string>          */
/* #include <string.h> */
/* #include <cassert> */
/* #include <cfloat> */
/* #include <stdlib.h> */
/* #include <stdio.h> */
/* #include <fftw3.h> */
/* #include <omp.h> */
/* #include <sstream> */
/* #include <netinet/in.h> */

/* #include <gsl/gsl_errno.h> */
/* #include <gsl/gsl_spline.h> */
/* #include <gsl/gsl_randist.h> */
/* #include <gsl/gsl_integration.h> */
/* #include <gsl/gsl_sf.h> */

/* #include "fftw_array.h" */

/* #include "define_opt.h" */

#include <vector>
#include "HMC_models.h"
#include "struct_main.h"
#include <sstream>

using namespace std;

/* --- structures --- */

struct HAMIL_NUMERICAL {
  unsigned N1;  /* Number of cells per axis */
  unsigned N2;  /* Number of cells per axis */
  unsigned N3;  /* Number of cells per axis */
  ULONG N;
  ULONG Nhalf;

  int mk;

  real_prec L1;
  real_prec L2;
  real_prec L3;

  real_prec vol;

  real_prec d1;
  real_prec d2;
  real_prec d3;

  real_prec min1;
  real_prec min2;
  real_prec min3;

  // Redshift space distortions
  real_prec xobs;
  real_prec yobs;
  real_prec zobs;
  bool planepar;
  bool periodic;

  ULONG N_bin;

  ULONG iGibbs;
  ULONG rejections;
  ULONG total_steps_lim;

  // EGP: some extra parameters for statistics on performance:
  bool accepted;
  real_prec epsilon;
  ULONG Neps;
  real_prec dH, dK, dE, dprior, dlikeli, psi_prior, psi_likeli, psi_prior_i,
  psi_prior_f, psi_likeli_i, psi_likeli_f, H_kin_i, H_kin_f;

  int method;			/* which method */
  int filter;			/* which filter method */
  ULONG itmax;			/* maximal number of iterations */
  int i_reset;			/* number to reset */
  int INV_SUCCESS;		/* indicate inversion success */

  real_prec epsi;  /* precision */

  real_prec kth;

  // EGP: pseudo-timestep parameters
  real_prec N_eps_fac;
  real_prec eps_fac;
  real_prec eps_fac_target;
  real_prec eps_fac_power;
  ULONG s_eps_total;

  // mass type
  bool mass_fs;
  bool mass_rs;
  int mass_type;
  ULONG massnum_init;
  ULONG massnum_burn;

  // EGP: testing:
  real_prec mass_factor;

  /// TESTING ///
  real_prec grad_psi_prior_factor;
  real_prec grad_psi_likeli_factor;
  bool grad_psi_prior_conjugate;
  bool grad_psi_likeli_conjugate;
  bool grad_psi_prior_times_i;
  bool grad_psi_likeli_times_i;
  bool div_dH_by_N;
  int calc_h;
  real_prec deltaQ_factor;

  bool correct_delta;

  int particle_kernel;
  real_prec particle_kernel_h;

  // FFTW
  real_prec *in_r2c;
  real_prec *out_c2r;
  complex_prec *in_c2r;
  complex_prec *out_r2c;
  struct plan_pkg *R2Cplan;
  struct plan_pkg *C2Rplan;
};

struct HAMIL_DATA {
  struct HAMIL_NUMERICAL *numerical;  // numerical parameters

  // set arrays
  real_prec *mass_f;
  real_prec *mass_r;
  // real_prec *massi;
  real_prec *gradpsi;
  real_prec *x;
  // real_prec *growth;

  real_prec *posx;
  real_prec *posy;
  real_prec *posz;

  real_prec *signal_PS;
  real_prec *window;
  real_prec *func;
  real_prec *noise;
  real_prec *nobs;
  real_prec *deltaX;
  real_prec *corrf;

  // function pointers
  void (*partial_f_delta_x_log_like)(struct HAMIL_DATA*, real_prec*,
                                     real_prec*);
  real_prec (*log_like)(struct HAMIL_DATA*, real_prec*);
  void (*grad_f_delta_x_comp)(struct HAMIL_DATA *, real_prec *, real_prec *, unsigned int);

  real_prec (*log_prior)(struct HAMIL_DATA*, real_prec*);
  void (*grad_log_prior)(struct HAMIL_DATA*, real_prec*, real_prec*);

  // model flags
  int sfmodel;
  bool rsd_model;

  // set scalars
  real_prec Nmean_Gal;
  real_prec rho_c;
  real_prec code_norm;
  real_prec mu;

  real_prec sigma_fac;
  real_prec sigma_min;
  real_prec delta_min;

  real_prec biasP;
  real_prec biasE;

  real_prec z;
  real_prec ascale;

  real_prec D1;
  real_prec D2;

  real_prec OM;
  real_prec OL;
  real_prec h;

  // external inputs
  real_prec *M;

  // curses
  // struct CURSES_STRUCT *curses;

  // stuff for SPH_kernel_3D_cells
  const int N_cells;
  const std::vector<int> kernel_cells_i, kernel_cells_j, kernel_cells_k;

  // default constructor; initialize everything yourself!
  HAMIL_DATA() : N_cells(0), kernel_cells_i(0, 0), kernel_cells_j(0, 0),
  kernel_cells_k(0, 0) {
    numerical = new HAMIL_NUMERICAL;
  }

  // secondary constructor; used by call_hamil
  HAMIL_DATA(struct DATA *data, real_prec *A, real_prec *B_f, real_prec *B_r,
             real_prec *C, real_prec *D, real_prec *E, vector<int> cells_i,
             std::vector<int> cells_j, std::vector<int> cells_k,
             int N_cells_i) :
  N_cells(N_cells_i),
  kernel_cells_i(cells_i.begin(), cells_i.end()),
  kernel_cells_j(cells_j.begin(), cells_j.end()),
  kernel_cells_k(cells_k.begin(), cells_k.end()) {
    numerical = new HAMIL_NUMERICAL;

    numerical->N1 = data->numerical->N1;
    numerical->N2 = data->numerical->N2;
    numerical->N3 = data->numerical->N3;
    numerical->N = data->numerical->N;
    numerical->Nhalf = data->numerical->Nhalf;
    numerical->L1 = data->numerical->L1;
    numerical->L2 = data->numerical->L2;
    numerical->L3 = data->numerical->L3;
    numerical->vol = data->numerical->vol;
    numerical->d1 = data->numerical->d1;
    numerical->d2 = data->numerical->d2;
    numerical->d3 = data->numerical->d3;
    numerical->min1 = data->numerical->xllc;
    numerical->min2 = data->numerical->yllc;
    numerical->min3 = data->numerical->zllc;
    // Redshift space distortions
    numerical->xobs = data->numerical->xobs;
    numerical->yobs = data->numerical->yobs;
    numerical->zobs = data->numerical->zobs;
    numerical->planepar = data->numerical->planepar;
    numerical->periodic = data->numerical->periodic;

    numerical->N_bin = data->numerical->N_bin;
    numerical->mk = data->numerical->mk;

    numerical->kth = data->numerical->slength;

    numerical->iGibbs = data->numerical->iGibbs;

    numerical->rejections = 0; // EGP: added for rejection rate
    numerical->total_steps_lim = data->numerical->total_steps_lim;

    numerical->itmax = 2000;
    numerical->INV_SUCCESS = 0;/* indicate inversion success initialize with 0*/

    numerical->N_eps_fac = data->numerical->N_eps_fac;
    numerical->eps_fac = data->numerical->eps_fac;
    numerical->eps_fac_target = data->numerical->eps_fac_target;
    numerical->eps_fac_power = data->numerical->eps_fac_power;
    numerical->s_eps_total = data->numerical->s_eps_total;

    numerical->mass_type = data->numerical->mass_type;
    switch (numerical->mass_type) {
      case 0:  // 0: R: all ones (essentially: no mass term)
      numerical->mass_rs = true;
      numerical->mass_fs = false;
      break;
      case 1:  // 1: FS: inverse power spectrum
      numerical->mass_rs = false;
      numerical->mass_fs = true;
      break;
      case 2:  // 2: FS+FS: inverse power spectrum + likelihood force spectrum
      numerical->mass_rs = false;
      numerical->mass_fs = true;
      break;
      case 3:  // 3: FS+FS: inverse power spectrum + mean likelihood force (Wang+13)
      numerical->mass_rs = false;
      numerical->mass_fs = true;
      break;
      case 4:  // 4: FS: power spectrum
      numerical->mass_rs = false;
      numerical->mass_fs = true;
      break;
      case 5:  // 5: FS+R: inverse power spectrum + 1st order likelihood force expansion (Jasche+13)
      numerical->mass_rs = true;
      numerical->mass_fs = true;
      break;
      case 6:  // 1st order likelihood force expansion (Jasche+13) (R)
      numerical->mass_rs = true;
      numerical->mass_fs = false;
      break;
      case 60:  // type 0 until burn-in, type 6 afterwards
      numerical->mass_rs = true;
      numerical->mass_fs = false;
      break;
      default:
      stringstream message;
      message << "mass_type " << numerical->mass_type << " is not a valid value!";
      throw runtime_error(message.str());
    }

    numerical->massnum_init = data->numerical->massnum_init;
    numerical->massnum_burn = data->numerical->massnum_burn;

    gradpsi = A;
    mass_f = B_f;
    mass_r = B_r;
    posx = C;
    posy = D;
    posz = E;

    z = data->cosmology->z;
    ascale = data->cosmology->ascale;

    D1 = data->cosmology->D1;
    D2 = data->cosmology->D2;

    OM = data->cosmology->omega_m;
    OL = data->cosmology->omega_q;
    h = data->cosmology->h;

    biasP = data->observational->biasP;
    biasE = data->observational->biasE;

    x = data->observational->signal;
    nobs = data->observational->nobs;
    deltaX = data->observational->signalX;
    noise = data->observational->noise_sf;
    corrf = data->observational->corrf;

    Nmean_Gal = data->observational->Nmean_Gal;
    rho_c = data->observational->rho_c;
    code_norm = data->numerical->code_norm;
    mu = data->observational->mu;

    sigma_fac = data->observational->sigma_fac;
    sigma_min = data->observational->sigma_min;
    delta_min = data->observational->delta_min;

    signal_PS = data->observational->Power;

    window = data->observational->window;

    // function pointers
    log_like = data->observational->log_like;
    partial_f_delta_x_log_like = data->observational->partial_f_delta_x_log_like;
    grad_f_delta_x_comp = data->observational->grad_f_delta_x_comp;
    log_prior = data->observational->log_prior;
    grad_log_prior = data->observational->grad_log_prior;

    // model flags
    sfmodel = data->numerical->sfmodel;
    rsd_model = data->numerical->rsd_model;

    // Testing:
    numerical->mass_factor = data->numerical->mass_factor;
    numerical->grad_psi_prior_factor = data->numerical->grad_psi_prior_factor;
    numerical->grad_psi_likeli_factor = data->numerical->grad_psi_likeli_factor;
    numerical->grad_psi_prior_conjugate = data->numerical->grad_psi_prior_conjugate;
    numerical->grad_psi_likeli_conjugate = data->numerical->grad_psi_likeli_conjugate;
    numerical->grad_psi_prior_times_i = data->numerical->grad_psi_prior_times_i;
    numerical->grad_psi_likeli_times_i = data->numerical->grad_psi_likeli_times_i;
    numerical->div_dH_by_N = data->numerical->div_dH_by_N;
    numerical->calc_h = data->numerical->calc_h;
    numerical->deltaQ_factor = data->numerical->deltaQ_factor;

    numerical->particle_kernel = data->numerical->particle_kernel;
    numerical->particle_kernel_h = data->numerical->particle_kernel_h;

    // FFTW
    numerical->in_r2c = data->numerical->in_r2c;
    numerical->out_c2r = data->numerical->out_c2r;
    numerical->in_c2r = data->numerical->in_c2r;
    numerical->out_r2c = data->numerical->out_r2c;
    numerical->R2Cplan = data->numerical->R2Cplan;
    numerical->C2Rplan = data->numerical->C2Rplan;

    // curses
    // curses = data->curses;

    numerical->correct_delta = data->numerical->correct_delta;
  }

  ~HAMIL_DATA() {
    delete numerical;
  }
};


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#ifdef _WIN32
    #include "blas.c"
#else
    #include "blas.c"
    // #include <cblas.h>
#endif
#include <moss.h>
#include <objects/string.h>
#include <objects/list.h>
#include <modules/bs.h>
#include <modules/la.h>

double mf_float(mt_object* x, int* e);
extern mt_table* mv_type_array;

void mf_array_delete(mt_array* a){
    if(a->data->refcount==1){
        mf_free(a->data);
    }else{
        a->data->refcount--;
    }
    mf_free(a);
}

void mf_array_dec_refcount(mt_array* a){
    if(a->refcount==1){
        mf_array_delete(a);
    }else{
        a->refcount--;
    }
}

static
mt_array* mf_new_array(long size, int type){
    mt_array* a = mf_malloc(sizeof(mt_array));
    if(type==mv_array_float){
        a->data = mf_malloc(sizeof(mt_array_data)+size*sizeof(double));
    }else{
        abort();
    }
    a->data->refcount = 1;
    a->data->type = type;
    a->base = a->data->a;
    a->refcount = 1;
    a->size = size;
    return a;
}

static
mt_array* mf_array_copy(mt_array* a){
    mt_array* b;
    if(a->n==2){
        if(a->plain){
            goto plain;
        }else{
            unsigned long m=a->shape[0];
            unsigned long n=a->shape[1];
            long si=a->stride[0];
            long sj=a->stride[1];
            b = mf_new_array(m*n,mv_array_float);
            b->n = a->n; b->plain = 1;
            b->shape[0] = m; b->shape[1] = n;
            b->stride[0] = 1; b->stride[1] = m;
            double* pa = (double*)a->base;
            double* pb = (double*)b->base;
            unsigned long i,j,mj;
            long sjj;
            for(j=0; j<n; j++){
                mj=m*j; sjj=sj*j;
                for(i=0; i<m; i++){
                    pb[i+mj]=pa[si*i+sjj];
                }
            }
            return b;
        }
    }else if(a->n==1){
        b = mf_new_array(a->shape[0],mv_array_float);
        b->n = 1;
        b->shape[0] = a->shape[0];
        b->stride[0] = 1;
        cblas_dcopy(a->shape[0],(double*)a->base,a->stride[0],(double*)b->base,1);
        return b;
    }else{
        if(a->plain) goto plain;
        abort();
    }

    plain:
    b = mf_new_array(a->size,mv_array_float);
    b->n = a->n;
    b->plain = 1;
    unsigned long i;
    for(i=0; i<a->n; i++){
        b->shape[i] = a->shape[i];
        b->stride[i] = a->stride[i];
    }
    cblas_dcopy(a->size,(double*)a->base,1,(double*)b->base,1);
    return b;
}

void mf_ensure_plain(mt_array* a){
    if(a->n==2){
        if(a->plain) return;
        mt_array_data* data = mf_malloc(sizeof(mt_array_data)+a->size*sizeof(double));
        data->refcount = 1;
        data->type = mv_array_float;
        double* pa = (double*)a->base;
        double* pb = (double*)data->a;

        unsigned long m = a->shape[0];
        unsigned long n = a->shape[1];
        long si = a->stride[0];
        long sj = a->stride[1];
        unsigned long i,j,mj;
        long sjj;
        for(j=0; j<n; j++){
            mj = m*j; sjj = sj*j;
            for(i=0; i<m; i++){
                pb[i+mj] = pa[si*i+sjj];
            }
        }
        if(a->data->refcount==1){
            mf_free(a->data);
        }else{
            a->data->refcount--;
        }
        a->stride[0] = 1;
        a->stride[1] = m;
        a->data = data;
        a->base = data->a;
        a->plain = 1;
    }else if(a->n==1){
        abort();
    }else{
        if(a->plain) return;
        abort();
    }
}

static
mt_array* mf_array_map_dd(mt_array* a, double(*f)(double)){
    mt_array* b;
    if(a->n==2){
        if(a->plain){
            goto plain;
        }else{
            unsigned long m = a->shape[0];
            unsigned long n = a->shape[1];
            long si = a->stride[0];
            long sj = a->stride[1];
            b = mf_new_array(m*n,mv_array_float);
            b->n = a->n; b->plain = 1;
            b->shape[0] = m; b->shape[1] = n;
            b->stride[0] = 1; b->stride[1] = m;
            double* pa = (double*)a->base;
            double* pb = (double*)b->base;
            unsigned long i,j,mj;
            long sjj;
            for(j=0; j<n; j++){
                mj = m*j; sjj = sj*j;
                for(i=0; i<m; i++){
                    pb[i+mj] = f(pa[si*i+sjj]);
                }
            }
            return b;
        }
    }else if(a->n==1){
        unsigned long n=a->shape[0];
        b = mf_new_array(n,mv_array_float);
        b->n = 1; b->shape[0] = n;
        b->stride[0] = 1;
        long s = a->stride[0];
        unsigned long k;
        double* pa = (double*)a->base;
        double* pb = (double*)b->base;
        for(k=0; k<n; k++){
            pb[k] = f(pa[k*s]);
        }
        return b;
    }else{
        if(a->plain) goto plain;
        abort();
    }

    plain:
    b = mf_new_array(a->size,mv_array_float);
    b->n = a->n;
    b->plain = 1;
    unsigned long i;
    for(i=0; i<a->n; i++){
        b->shape[i] = a->shape[i];
        b->stride[i] = a->stride[i];
    }
    double* pa = (double*)a->base;
    double* pb = (double*)b->base;
    unsigned long size = a->size;
    unsigned long k;
    for(k=0; k<size; k++){
        pb[k] = f(pa[k]);
    }
    return b;
}

long mf_array_size(mt_array* a){
    long p;
    unsigned long k;
    switch(a->n){
    case 1:
        return a->shape[0];
    case 2:
        return a->shape[0]*a->shape[1];
    default:
        p=1;
        for(k=0; k<a->n; k++){
            p = p*a->shape[k];
        }
        return p;
    }
}

mt_array* mf_array(long size, mt_object* v){
    mt_array* a;
    if(size>0 && v[0].type==mv_list){
        long i,j;
        mt_list* list = (mt_list*)v[0].value.p;
        long n = list->size;
        a = mf_new_array(size*n,mv_array_float);
        a->n = 2;
        a->shape[0] = size; a->shape[1] = n;
        a->stride[0] = 1; a->stride[1] = size;
        a->plain = 1;
        double* b = (double*)a->base;
        int e=0;
        for(i=0; i<size; i++){
            if(v[i].type!=mv_list) abort();
            list = (mt_list*)v[i].value.p;
            if(list->size!=n) abort();
            for(j=0; j<n; j++){
                b[i+size*j] = mf_float(list->a+j,&e);
                if(e) abort();
            }
        }
    }else{
        a = mf_new_array(size,mv_array_float);
        a->n = 1;
        a->shape[0] = size;
        a->stride[0] = 1;
        long i;
        double* b = (double*)a->base;
        int e=0;
        for(i=0; i<size; i++){
            b[i] = mf_float(v+i,&e);
        }
    }
    return a;
}

int la_array(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"array");
        return 1;
    }
    if(v[1].type!=mv_list){
        mf_type_error1("in array(a): a (type: %s) is not a list.",v+1);
        return 1;
    }
    mt_list* list = (mt_list*)v[1].value.p;
    mt_array* a = mf_array(list->size, list->a);
    if(a==NULL) return 1;
    x->type = mv_array;
    x->value.p = (mt_basic*)a;
    return 0;
}

int la_vector(mt_object* x, int argc, mt_object* v){
    mt_array* a = mf_array(argc,v+1);
    if(a==NULL) return 1;
    x->type = mv_array;
    x->value.p = (mt_basic*)a;
    return 0;
}

int la_matrix(mt_object* x, int argc, mt_object* v){
    mt_array* a = mf_array(argc,v+1);
    if(a==NULL) return 1;
    if(a->n==1){
        a->n = 2;
        unsigned long size = a->shape[0];
        a->shape[0] = size;
        a->shape[1] = 1;
        a->stride[0] = 1;
        a->stride[1] = size;
        a->plain = 0;
    }
    x->type = mv_array;
    x->value.p = (mt_basic*)a;
    return 0;
}

static
void vector_str(mt_bs* bs, mt_array* T){
    char buffer[100];
    unsigned long i;
    double* a = (double*)T->base;
    unsigned long size = T->shape[0];
    long step = T->stride[0];

    mf_bs_push_cstr(bs,"[");
    for(i=0; i<size; i++){
        snprintf(buffer,100,i==0?"%.4g":", %.4g",a[step*i]);
        mf_bs_push_cstr(bs,buffer);
    }
    mf_bs_push_cstr(bs,"]");
}

static
void matrix_str(mt_bs* bs, mt_array* T){
    char buffer[100];
    unsigned long i,j;
    double* a = (double*)T->base;
    unsigned long m,n,is,js;
    m = T->shape[0];
    n = T->shape[1];
    is = T->stride[0];
    js = T->stride[1];
    int max = 0;
    int size;
    for(i=0; i<m; i++){
        for(j=0; j<n; j++){
            size = snprintf(buffer,100,"%.4g",a[i*is+j*js]);
            if(size>max) max=size;
        }
    }
    mf_bs_push_cstr(bs,"[");
    for(i=0; i<m; i++){
        mf_bs_push_cstr(bs,i==0? "[": ",\n [");
        for(j=0; j<n; j++){
            snprintf(buffer,100,j==0?"%*.4g":", %*.4g",max,a[i*is+j*js]);
            mf_bs_push_cstr(bs,buffer);
        }
        mf_bs_push_cstr(bs,"]");
    }
    mf_bs_push_cstr(bs,"]");
}

static
int array_str(mt_object* x, int argc, mt_object* v){
    if(argc!=0){
        mf_argc_error(argc,0,0,"array.type.str");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a.str(): a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* T = (mt_array*)v[0].value.p;
    mt_bs bs;
    mf_bs_init(&bs);
    if(T->n==1){
        vector_str(&bs,T);
    }else if(T->n==2){
        matrix_str(&bs,T);
    }else{
        abort();
    }

    x->type = mv_string;
    x->value.p = (mt_basic*)mf_str_new(bs.size,(const char*)bs.a);
    mf_free(bs.a);
    return 0;
}

static
int array_list(mt_object* x, int argc, mt_object* v){
    if(argc!=0){
        mf_argc_error(argc,0,0,"array.type.list");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a.list(): a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* T = (mt_array*)v[0].value.p;
    mt_list* list = mf_raw_list(T->size);
    unsigned long i;
    double* a = (double*)T->base;
    unsigned long size = T->shape[0];
    long step = T->stride[0];
    for(i=0; i<size; i++){
        list->a[i].type = mv_float;
        list->a[i].value.f = a[step*i];
    }

    x->type = mv_list;
    x->value.p = (mt_basic*)list;
    return 0;
}

double addc;
double add(double x){
    return x+addc;
}
double subc;
double sub(double x){
    return x-subc;
}

static
int array_add(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"Array.add");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a+b: a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    mt_array* c;
    if(v[1].type!=mv_array){
        int e = 0;
        addc = mf_float(&v[1],&e);
        if(e){
            mf_type_error1("in a+r: cannot convert r (type: %s) to float.",&v[1]);
            return 1;
        }
        c = mf_array_map_dd(a,add);
        if(c==NULL) return 1;
        x->type = mv_array;
        x->value.p = (mt_basic*)c;
        return 0;
    }
    mt_array* b = (mt_array*)v[1].value.p;
    if(a->n!=b->n){
        mf_value_error("value error in a+b: a and b have unequal order.");
        return 1;
    }
    if(a->n==1){
        unsigned long size=a->shape[0];
        if(size!=b->shape[0]){
            mf_value_error("Value error in a+b: a and b have unequal shape.");
            return 1;
        }
        c = mf_new_array(size,mv_array_float);
        c->n = 1; c->stride[0] = 1;
        c->shape[0] = size;
        cblas_dcopy(size,(double*)a->base,a->stride[0],(double*)c->base,1);
        cblas_daxpy(size,1,(double*)b->base,b->stride[0],(double*)c->base,1);
    }else{
        unsigned long i;
        for(i=0; i<a->n; i++){
            if(a->shape[i]!=b->shape[i]) abort();
        }
        c = mf_array_copy(a);
        mf_ensure_plain(b);
        cblas_daxpy(c->size,1,(double*)b->base,1,(double*)c->base,1);
    }
    x->type = mv_array;
    x->value.p = (mt_basic*)c;
    return 0;
}

static
int array_radd(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"Array.radd");
        return 1;
    }
    if(v[1].type!=mv_array){
        mf_type_error1("in r*a: a (type: %s) is not an array.",&v[1]);
        return 1;
    }
    int e = 0;
    addc = mf_float(&v[0],&e);
    if(e){
        mf_type_error1("in r+a: cannot convert r (type: %s) to float.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[1].value.p;
    mt_array* y = mf_array_map_dd(a,add);
    if(y==NULL) return 1;
    x->type = mv_array;
    x->value.p = (mt_basic*)y;
    return 0;
}

static
int array_sub(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"Array.sub");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a-b: a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    mt_array* c;
    if(v[1].type!=mv_array){
        int e = 0;
        subc = mf_float(&v[1],&e);
        if(e){
            mf_type_error1("in a-r: cannot convert r (type: %s) to float.",&v[1]);
            return 1;
        }
        c = mf_array_map_dd(a,sub);
        if(c==NULL) return 1;
        x->type = mv_array;
        x->value.p = (mt_basic*)c;
        return 0;
    }
    mt_array* b = (mt_array*)v[1].value.p;
    if(a->n==1){
        unsigned long size = a->shape[0];
        if(size!=b->shape[0]){
            mf_value_error("Value error in a-b: a and b have unequal shape.");
            return 1;
        }

        c = mf_new_array(size,mv_array_float);
        c->n = 1;
        c->shape[0] = size;
        c->stride[0] = 1;
        cblas_dcopy(size,(double*)a->base,a->stride[0],(double*)c->base,1);
        cblas_daxpy(size,-1,(double*)b->base,b->stride[0],(double*)c->base,1);
    }else{
        unsigned long i;
        for(i=0; i<a->n; i++){
            if(a->shape[i]!=b->shape[i]) abort();
        }
        c = mf_array_copy(a);
        mf_ensure_plain(b);
        cblas_daxpy(c->size,-1,(double*)b->base,1,(double*)c->base,1);
    }
    x->type = mv_array;
    x->value.p = (mt_basic*)c;
    return 0;
}

static
mt_array* mf_array_scal(double r, mt_array* a){
    mt_array* b;
    if(a->n==1){
        unsigned long size = a->shape[0];
        b = mf_new_array(size,mv_array_float);
        b->n = 1;
        b->shape[0] = size;
        b->stride[0] = 1;
        cblas_dcopy(size,(double*)a->base,a->stride[0],(double*)b->base,1);
    }else{
        b = mf_array_copy(a);
    }
    cblas_dscal(b->size,r,(double*)b->base,1);
    return b;
}

static
mt_array* array_mpy_array(mt_array* a, mt_array* b){
    if(b->n==1){
        unsigned long m = a->shape[0];
        unsigned long n = a->shape[1];
        if(n!=b->shape[0]){
            mf_value_error("Value error in A*x: shape does not match.");
            return NULL;
        }
        if(a->stride[0]!=1){
            mf_ensure_plain(a);
        }
        mt_array* c = mf_new_array(m,mv_array_float);
        c->n = 1;
        c->shape[0] = m;
        c->stride[0] = 1;
        cblas_dgemv(CblasColMajor,CblasNoTrans,m,n,1,
            (double*)a->base, a->stride[1],
            (double*)b->base, b->stride[0],
            0,(double*)c->base,1
        );
        return c;
    }else if(b->n==2){
        if(a->n==2){
            if(a->shape[1]!=b->shape[0]){
                mf_value_error("Value error in A*B: incompatible matrices.");
                return NULL;
            }
            if(a->stride[0]!=1){
                mf_ensure_plain(a);
            }
            if(b->stride[0]!=1){
                mf_ensure_plain(b);
            }

            unsigned long m = a->shape[0];
            unsigned long k = a->shape[1];
            unsigned long n = b->shape[1];
            mt_array* c = mf_new_array(m*n,mv_array_float);
            c->n = 2;
            c->shape[0] = m;
            c->shape[1] = n;
            c->stride[0] = 1;
            c->stride[1] = m;
            c->plain = 1;
            cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1,
                (double*)a->base, a->stride[1],
                (double*)b->base, b->stride[1],
                0, (double*)c->base, m
            );
            return c;
        }else{
            abort();
        }
    }else{
        abort();
    }
}

static
mt_array* matrix_identity(unsigned long n){
    mt_array* a = mf_new_array(n*n,mv_array_float);
    a->n = 2;
    a->plain = 1;
    a->shape[0] = n;
    a->shape[1] = n;
    a->stride[0] = 1;
    a->stride[1] = n;
    double* b = (double*)a->base;
    unsigned long i,j,nj;
    for(j=0; j<n; j++){
        nj = n*j;
        for(i=0; i<n; i++){
            b[i+nj] = i==j? 1: 0;
        }
    }
    return a;
}

static
mt_array* square_matrix_mpy(mt_array* a, mt_array* b){
    if(a->stride[0]!=1){
        mf_ensure_plain(a);
    }
    if(b->stride[0]!=1){
        mf_ensure_plain(b);
    }
    unsigned long n = a->shape[0];
    mt_array* c = mf_new_array(n*n,mv_array_float);
    c->n = 2;
    c->shape[0] = n;
    c->shape[1] = n;
    c->stride[0] = 1;
    c->stride[1] = n;
    c->plain = 1;
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n, n, n, 1,
        (double*)a->base, a->stride[1],
        (double*)b->base, b->stride[1],
        0, (double*)c->base, n
    );
    return c;
}

static
mt_array* square_matrix_pow(mt_array* a, unsigned long n){
    if(n==0){
        return matrix_identity(a->shape[0]);
    }
    mt_array *p,*h;
    p = mf_array_copy(a);
    a = p;
    a->refcount++;
    n--;
    while(n){
        if(n&1){
            h = p;
            p = square_matrix_mpy(p,a);
            mf_array_dec_refcount(h);
        }
        n>>=1;
        if(n==0) break;
        h = a;
        a = square_matrix_mpy(a,a);
        mf_array_dec_refcount(h);
    }
    mf_array_dec_refcount(a);
    return p;
}

static
int array_mul(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"Array.mul");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a*b: a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    mt_array* b;
    double r;
    if(v[1].type==mv_array){
        b=(mt_array*)v[1].value.p;
        if(a->n==2){
            if(b->n==2 && a->shape[0]==1 && b->shape[1]==1){
                if(a->shape[1]!=b->shape[0]){
                    mf_value_error("Value error in a*b: incompatible matrices.");
                    return 1;
                }
                // todo: strides
                r = cblas_ddot(a->shape[1],
                    (double*)a->base,a->stride[1],
                    (double*)b->base,b->stride[0]
                );
                x->type = mv_float;
                x->value.f = r;
                return 0;
            }
            mt_array* c = array_mpy_array(a,b);
            if(c==NULL) return 1;
            x->type = mv_array;
            x->value.p = (mt_basic*)c;
            return 0;
        }
        if(a->n!=1){
            mf_value_error("Value error in a*b: a and b have incompatible shape.");
            return 1;
        }
        if(b->n==2){
            mt_array* c = array_mpy_array(b,a);
            if(c==NULL) return 1;
            x->type = mv_array;
            x->value.p = (mt_basic*)c;
            return 0;
        }
        if(a->shape[0]!=b->shape[0]){
            mf_value_error("Value error in a*b: a and b have unequal shape.");
            return 1;
        }
        r = cblas_ddot(a->shape[0],
            (double*)a->base,a->stride[0],
            (double*)b->base,b->stride[0]
        );
        x->type = mv_float;
        x->value.f = r;
        return 0;
    }else{
        int e = 0;
        r = mf_float(&v[1],&e);
        if(e){
            mf_type_error1("in a*r: cannot convert r (type: %s) to float.",&v[1]);
            return 1;
        }
        x->type = mv_array;
        x->value.p = (mt_basic*)mf_array_scal(r,a);
        return 0;
    }
}

static
int array_pow(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"Array.pow");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a^n: a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;

    double r;
    if(a->n!=1){
        if(a->n==2 && v[1].type==mv_int){
            long n = v[1].value.i;
            if(a->shape[0]!=a->shape[1]){
                mf_value_error("Value error in A^n: A is not a square matrix");
                return 1;
            }
            x->type = mv_array;
            x->value.p = (mt_basic*)square_matrix_pow(a,(unsigned long)n);
            return 0;
        }
        mf_value_error("Value error in a^n: a is not a vector.");
        return 1;
    }
    r = cblas_ddot(a->shape[0],
        (double*)a->base,a->stride[0],
        (double*)a->base,a->stride[0]
    );
    x->type = mv_float;
    if(v[1].type==mv_int && v[1].value.i==2){
        x->value.f = r;
    }else{
        int e = 0;
        double n = mf_float(&v[1],&e);
        if(e){
            mf_type_error1("in a^n: cannot convert n (type: %s) to float.",&v[1]);
            return 1;
        }
        x->value.f = pow(r,n);
    }
    return 0;
}

static
int array_rmul(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"Array.rmul");
        return 1;
    }
    if(v[1].type!=mv_array){
        mf_type_error1("in r*a: a (type: %s) is not an array.",&v[1]);
        return 1;
    }
    int e=0;
    double r = mf_float(&v[0],&e);
    if(e){
        mf_type_error1("in r*a: cannot convert r (type: %s) to float.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[1].value.p;
    x->type = mv_array;
    x->value.p = (mt_basic*)mf_array_scal(r,a);;
    return 0;
}

static
int array_neg(mt_object* x, int argc, mt_object* v){
    if(argc!=0){
        mf_argc_error(argc,0,0,"Array.neg");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in -a: a (type: %s) is not an array.",&v[1]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    x->type = mv_array;
    x->value.p = (mt_basic*)mf_array_scal(-1,a);;
    return 0;
}

static
int array_div(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"Array.div");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a/r: a (type: %s) is not an array.",&v[1]);
        return 1;
    }
    int e = 0;
    double r = mf_float(&v[1],&e);
    if(e){
        mf_type_error1("in a/r: cannot convert r (type: %s) to float.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    x->type = mv_array;
    x->value.p = (mt_basic*)mf_array_scal(1/r,a);;
    return 0;
}

static
int array_T(mt_object* x, int argc, mt_object* v){
    if(argc!=0){
        mf_argc_error(argc,0,0,"array.type.T");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a.T(): a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    if(a->n==2){
        mt_array* b = mf_malloc(sizeof(mt_array));
        b->refcount = 1;
        b->n = 2;
        a->data->refcount++;
        b->data = a->data;
        b->base = a->base;
        b->size = a->size;
        b->plain = 0;
        b->shape[0] = a->shape[1];
        b->shape[1] = a->shape[0];
        b->stride[0] = a->stride[1];
        b->stride[1] = a->stride[0];
        x->type = mv_array;
        x->value.p = (mt_basic*)b;
    }else{
        a->refcount++;
        x->type = mv_array;
        x->value.p = (mt_basic*)a;
    }
    return 0;
}

static
int array_plain(mt_object* x, int argc, mt_object* v){
    if(v[0].type!=mv_array) abort();
    mt_array* a = (mt_array*)v[0].value.p;
    mf_ensure_plain(a);
    a->refcount++;
    x->type = mv_array;
    x->value.p = (mt_basic*)a;
    return 0;
}

static
int array_copy(mt_object* x, int argc, mt_object* v){
    if(v[0].type!=mv_array) abort();
    mt_array* a = (mt_array*)v[0].value.p;
    x->type = mv_array;
    x->value.p = (mt_basic*)mf_array_copy(a);
    return 0;
}

static
mt_array* vector_slice(mt_array* a, mt_range* r){
    long i,j;
    if(r->a.type!=mv_int || r->b.type!=mv_int){
        mf_type_error("Type error in a[r]: r is not an integer range.");
        return NULL;
    }
    i = r->a.value.i;
    j = r->b.value.i;
    unsigned long n = a->shape[0];
    if(i<0 || j<0){
        mf_value_error("Value error in a[r]: range with negative values.");
        return NULL;
    }
    if((unsigned long)i>=n || (unsigned long)j>=n){
        mf_value_error("Value error in a[r]: range is out of bounds.");
        return NULL;
    }
    mt_array* b = mf_malloc(sizeof(mt_array));
    b->refcount = 1;
    b->size = a->size;
    a->data->refcount++;
    b->data = a->data;
    a->data = b->data;
    b->n = 1;
    b->base = (unsigned char*)((double*)a->base+a->stride[0]*i);
    b->shape[0] = j-i+1;
    b->stride[0] = a->stride[0];
    return b;
}

static
int array_get(mt_object* x, int argc, mt_object* v){
    if(v[0].type!=mv_array){
        mf_type_error1("in a[i]: a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    if(argc==1){
        if(a->n!=1){
            mf_value_error("Value error in a[i]: a is not a vector.");
            return 1;
        }
        if(v[1].type!=mv_int){
            if(v[1].type==mv_range){
                 mt_array* b = vector_slice(a,(mt_range*)v[1].value.p);
                 if(b==NULL) return 1;
                 x->type = mv_array;
                 x->value.p = (mt_basic*)b;
                 return 0;
            }
            mf_type_error1("in a[i]: i (type: %s) is not an integer.",&v[1]);
            return 1;
        }
        long index = v[1].value.i;
        unsigned long n = a->shape[0];
        if(index<0 || (unsigned long)index>=n){
            mf_value_error("Value error in a[i]: i is out of bounds.");
            return 1;
        }
        double* b = (double*)a->base;
        double t = b[index*a->stride[0]];
        x->type = mv_float;
        x->value.f = t;
        return 0;
    }else{
        mf_argc_error(argc,1,1,"Array.get");
        return 1;
    }
}

static
int la_idm(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"idm");
        return 1;
    }
    if(v[1].type!=mv_int){
        mf_type_error1("in idm(n): n (type: %s) is not an integer.",v+1);
        return 1;
    }
    long n = v[1].value.i;
    x->type = mv_array;
    x->value.p = (mt_basic*)matrix_identity(n);
    return 0;
}

static
mt_array* diag(mt_array* v){
    if(v->n!=1){
        mf_value_error("Value error in diag(v): v is not a vector.");
        return NULL;
    }
    unsigned long n = v->shape[0];
    mt_array* a = mf_new_array(n*n,mv_array_float);
    a->n = 2;
    a->plain = 1;
    a->shape[0] = n;
    a->shape[1] = n;
    a->stride[0] = 1;
    a->stride[1] = n;
    double* b = (double*)a->base;
    double* vb = (double*)v->base;
    long vs = v->stride[0];
    unsigned long i,j,nj;
    for(j=0; j<n; j++){
        nj = n*j;
        for(i=0; i<n; i++){
            b[i+nj] = i==j? vb[i*vs]: 0;
        }
    }
    return a;
}

static
int la_diag(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"diag");
        return 1;
    }
    if(v[1].type!=mv_array){
        mf_type_error1("in diag(a): a is not an array.",v+1);
        return 1;
    }
    mt_array* u = (mt_array*)v[1].value.p;
    mt_array* a = diag(u);
    if(a==NULL) return 1;
    x->type = mv_array;
    x->value.p = (mt_basic*)a;
    return 0;
}

static
mt_array* diag_slice(mt_array* a){
    if(a->n!=2 || a->shape[0]!=a->shape[1]){
        mf_value_error("Value error in a.diag(): a is not an quadratic matrix.");
        return NULL;
    }
    if(a->stride[1]!=1){
        mf_ensure_plain(a);
    }
    mt_array* v = mf_malloc(sizeof(mt_array));
    v->refcount = 1;
    v->n = 1;
    a->data->refcount++;
    v->data = a->data;
    v->base = a->base;
    v->shape[0] = a->shape[0];
    v->stride[0] = (a->shape[0]+1)*a->stride[0];
    return v;
}

static
int array_diag(mt_object* x, int argc, mt_object* v){
    if(argc!=0){
        mf_argc_error(argc,0,0,"diag");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a.diag(): a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    mt_array* u = diag_slice(a);
    if(u==NULL) return 1;
    x->type = mv_array;
    x->value.p = (mt_basic*)u;
    return 0;
}

int mf_array_map(mt_array* a, mt_function* f){
    mt_object argv[2];
    argv[0].type = mv_null;
    double* ba = (double*)a->base;
    long size = a->size;
    long i;
    mt_object x;
    int e = 0;
    for(i=0; i<size; i++){
        argv[1].type = mv_float;
        argv[1].value.f = ba[i];
        if(mf_call(f,&x,1,argv)){
            mf_traceback("map");
            return 1;
        }
        ba[i] = mf_float(&x,&e);
        if(e){
            mf_type_error1("in a.map(f): cannot convert f(a[k]) (type: %s) to float.",&x);
            return 1;
        }
    }
    return 0;
}

static
int array_map(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"map");
        return 1;
    }
    if(v[0].type!=mv_array){
        mf_type_error1("in a.map(f): a (type: %s) is not an array.",&v[0]);
        return 1;
    }
    if(v[1].type!=mv_function){
        mf_type_error1("in a.map(f): f (type: %s) is not a function.",&v[1]);
        return 1;
    }
    mt_array* a = (mt_array*)v[0].value.p;
    mt_function* f = (mt_function*)v[1].value.p;
    mt_array* b = mf_array_copy(a);
    if(mf_array_map(b,f)) return 1;
    x->type = mv_array;
    x->value.p = (mt_basic*)b;
    return 0;
}

static
int mf_trace(mt_object* x, mt_array* a){
    if(a->n!=2 || a->shape[0]!=a->shape[1]){
        mf_value_error("Value error in trace(a): a is not a quadratic matrix.");
        return 1;
    }
    double s = 0;
    long n = a->shape[0];
    double* ba = (double*)a->base;
    long i,m;
    m = a->stride[0]+a->stride[1];
    for(i=0; i<n; i++){
        s+=ba[i*m];
    }
    x->type = mv_float;
    x->value.f = s;
    return 0;
}

static
int la_trace(mt_object* x, int argc, mt_object* v){
    if(argc!=1){
        mf_argc_error(argc,1,1,"trace");
        return 1;
    }
    if(v[1].type!=mv_array){
        mf_type_error1("in la.trace(a): a (type: %s) is not an array.",&v[1]);
        return 1;
    }
    mt_array* a = (mt_array*)v[1].value.p;
    if(mf_trace(x,a)) return 1;
    return 0;
}

mt_table* mf_la_load(){
    mt_map* m = mv_type_array->m;
    mf_insert_function(m,0,0,"str",array_str);
    mf_insert_function(m,0,0,"list",array_list);
    mf_insert_function(m,1,1,"add",array_add);
    mf_insert_function(m,1,1,"radd",array_radd);
    mf_insert_function(m,1,1,"sub",array_sub);
    mf_insert_function(m,1,1,"mul",array_mul);
    mf_insert_function(m,1,1,"rmul",array_rmul);
    mf_insert_function(m,0,0,"neg",array_neg);
    mf_insert_function(m,1,1,"div",array_div);
    mf_insert_function(m,1,1,"pow",array_pow);
    mf_insert_function(m,1,-1,"get",array_get);
    mf_insert_function(m,0,0,"T",array_T);
    mf_insert_function(m,0,0,"plain",array_plain);
    mf_insert_function(m,0,0,"copy",array_copy);
    mf_insert_function(m,0,0,"diag",array_diag);
    mf_insert_function(m,1,1,"map",array_map);

    mt_table* la = mf_table(NULL);
    la->name = mf_cstr_to_str("module la");
    la->m = mf_empty_map();
    m = la->m;
    mf_insert_function(m,1,1,"array",la_array);
    mf_insert_function(m,1,1,"vector",la_vector);
    mf_insert_function(m,1,1,"matrix",la_matrix);
    mf_insert_function(m,1,1,"idm",la_idm);
    mf_insert_function(m,1,1,"diag",la_diag);
    mf_insert_function(m,1,1,"trace",la_trace);
    m->frozen = 1;
    return la;
}