#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <stdint.h>
#include <limits.h> // for int limits etc.
#include <assert.h>
#include <time.h>
#include <gsl/gsl_histogram.h>
#include <omp.h>

#include "main.h"
#include "dSFMT-src-2.1/dSFMT.h"
//#include "ziggurat/ziggurat.h"

//#define STATS
#define MULTIPLIER 1000000  // seperates the coordiantes (poor man's hash)

int main (int argc, char * argv[]) {
	int num_particles = DSFMT_N64*4;
	int num_defects = 1024*4;
	int box_size = 128;
	long long nsteps = 1024*1024*2;

	//omp_init_lock(&omplock);
	//omp_set_num_threads(1);
	if (argc == 5) {
		num_particles = atoi(argv[1]);
		num_defects = atoi(argv[2]);
		box_size = atoi(argv[3]);
		nsteps = atoll(argv[4]);
	}
	else {
		printf("\n# ***** Using default values! *****\n\n");
		printf("# usage: rw #particles #defects #box #steps\n");

	}

	// Information:
	printf("#-------------------- Paramters --------------------\n");
	printf("# Particles: %i\n", num_particles);
	printf("# Box size: %i\n", box_size);
	printf("# Defects: %i (Density: %.2e)\n", num_defects, (float) num_defects/pow(box_size,3) );
	printf("# Steps: %lli\n", nsteps);
	printf("#---------------------------------------------------\n");

	assert( (box_size & (box_size - 1)) == 0 ); // check if box_size is power of two
	//zigset(1);
	dsfmt_t dsfmt;

	int seed = 1;

	// int num_random  = (num_particles < 1024) ? 1024: num_particles;
	// int  *correlation_times = malloc(num_particles * sizeof(int)); // random distribution of correlation times
	// char *directions = malloc(nsteps*sizeof(char)); // directions
	int **particles = malloc2D_i(num_particles, 4); // 2d array for particles: x,y,z,mag
	int **defects = malloc2D_i(num_defects, 3); // 2d array for defect coordinates

	//int *particle;
	//int direction;

	// init random number generator
	dsfmt_init_gen_rand(&dsfmt, seed);

	// check if we can create the hashes
	//assert(box_size < MULTIPLIER);
	assert(num_defects < pow(box_size,3));

#ifdef STATS
	// statistics
	// histogram of diretions
	gsl_histogram * h = gsl_histogram_alloc (6);
	gsl_histogram_set_ranges_uniform (h, 0, 6);

	// histogram of visits
	gsl_histogram * hvisits = gsl_histogram_alloc (10000);
	gsl_histogram_set_ranges_uniform (hvisits, 0, 10000);

	// gsl histogram seems not to be thread safe, allow only 1 thread
	omp_set_num_threads(1);
#endif

	// Start simulation

	// distribute particles from 0 to +box_size
	for (int j = 0; j < 3; j++) {
		for (int i=0 ;i < num_particles; i++) {
			particles[i][j] = (int) (dsfmt_genrand_close_open(&dsfmt)*box_size);
		}
	}

	// distribute defects from 0 to +box_size
	for (int j = 0; j < 3; j++) {
		for (int i=0 ; i < num_defects; i++) {
			int val = (int) (dsfmt_genrand_close_open(&dsfmt)*box_size);
			defects[i][j] = val;
		}
	}

	// METHOD 1: now create a hashed list to find them later
	// This will be a fallback to METHOD 2, in case memory is not enough
	int64_t *hash_list = malloc( num_defects*sizeof(int64_t) );
	for (int i = 0; i < num_defects; i++) {
		hash_list[i] = hash(defects[i][0],defects[i][1],defects[i][2]);
	}

	qsort(hash_list, num_defects, sizeof(int64_t), int64_cmp);

	// METHOD 2: create lookup table for x -> lookup table for y -> lookup table for z
	// The smart thing is that the pointers are NULL if there is no defect in the corresponding slab,
	// so only the z coordinates are really arrays
	//
	// If I need more space one could use the chars as bit fields,
	// One needs to calculate the offset (or index of char) to get to the proper group though.
	// get offset: offset = coord/sizeof(char) oder coord >> log2(sizeof(char))
	// set bit: array[offset] |= 1 << coord%sizeof(char)

	// in check_defekt_3d:
	// check bit: array[offset] & 1 << coord%sizeof(char)  > 1
	//
/*
	int mem_size_table = 0;
	// first coordinate (x) will be an array of pointers to an array of pointers
	char ***lookup_table = malloc(box_size * sizeof(char**));
	mem_size_table += box_size * sizeof(char**);
	// initialize the arrays to NULL pointer
	for (int i = 0; i < box_size; i++) {
		lookup_table[i]=NULL;
	}

	for (int i = 0; i < num_defects; i++) {
		int x_index = defects[i][0];
		int y_index = defects[i][1];
		int z_index = defects[i][2] / sizeof(char);
		// check if there  is already an array at x_index ...
		if (lookup_table[x_index] == NULL) {
			// ... it's not! Create an array of pointers for the second coordinate
			lookup_table[x_index] = malloc(box_size * sizeof(char*)); // malloc second coordinate pointers
			mem_size_table += box_size * sizeof(char*);
			for (int i = 0; i < box_size; i++)
				lookup_table[x_index][i]=NULL; // initialize the second coordiante pointers to NULL
		}

		// check if there is already an array at [x_index][y_index]
		if (lookup_table[x_index][y_index] == NULL) { // check if third coordinate array exists
			lookup_table[x_index][y_index] = malloc(box_size * sizeof(char)); // malloc third coordinate array
			mem_size_table += box_size * sizeof(char);
			for (int i = 0; i < box_size; i++)
				lookup_table[x_index][y_index][i]=0; // initialize the third array to zero

		}
		// set the defect coordinate
		lookup_table[x_index][y_index][z_index] = 1;

		 }

	int test_particle[4];
	double start = omp_get_wtime();
	for (int i = 0; i < 1024*1024*128; i++) {
		for (int j = 0; j < 3; j++) {
			test_particle[j] = (int) (dsfmt_genrand_close_open(&dsfmt)*box_size);
		}
		check_defect_3d(test_particle, lookup_table, 10);
	}
	double stop = omp_get_wtime();
	printf("# Time Method 2: %.2fs\n", stop-start);
	printf("# Lookup table size M1: %10.1fkB %i %i\n",  mem_size_table/((float) (1024)), num_defects, box_size);

*/

	// Method 3: Using the scheme above but for x,y,z seperately
	int ltb_N = (int) ceil( ( (double)box_size ) / sizeof(int));
	int xltb[ltb_N];
	int yltb[ltb_N];
	int zltb[ltb_N];
	for (int i = 0; i < ltb_N; i++) {
		xltb[i]=0;
		yltb[i]=0;
		zltb[i]=0;
	}
	for (int i = 0; i < num_defects; i++) {
		int xi =  defects[i][0] / sizeof(int);
		int xbit = defects[i][0] % sizeof(int);
		//printf("x %i %i %i %i\n",i, defects[i][0], xi, xbit);

		xltb[xi] |= 1 << xbit;

		int yi = defects[i][1] / sizeof(int);
		int ybit = defects[i][1] % sizeof(int);
		yltb[yi] |= 1 << ybit;
		//printf("y %i %i %i %i\n",i, defects[i][1], yi, ybit);

		int zi = defects[i][2] / sizeof(int);
		int zbit = defects[i][2] % sizeof(int);
		zltb[zi] |= 1 << zbit;
		//printf("z %i %i %i %i\n",i, defects[i][2], zi, zbit);
	}

/*
	start = omp_get_wtime();
	for (int i = 0; i < 1024*1024*128; i++) {
		for (int j = 0; j < 3; j++) {
			test_particle[j] = (int) (dsfmt_genrand_close_open(&dsfmt)*box_size);
		}
		check_defect_ltb(test_particle, xltb, yltb, zltb , 0);
	}
	stop = omp_get_wtime();
	printf("Time Method 3: %.2fs\n", stop-start);
*/

	printf("# Lookup table size M2: %10.1fkB %i %i\n",  3*ltb_N*sizeof(int) / ((float) (1024)), num_defects, box_size);

	// check if the lookup table is correct
	for (int i = 0; i < num_defects; i++) {

		int x = defects[i][0];
		int y = defects[i][1];
		int z = defects[i][2];
		//printf("Test: %i\n",lookup_table[x][y][z]);
/*
		assert(lookup_table[x][y][z] == 1); // Method 2
*/
		// Method 3
		int xi = x/ sizeof(int);
		int xbit = x % sizeof(int);
		assert( (xltb[xi] & (1<<xbit)) != 0 );

		int yi = y/ sizeof(int);
		int ybit = y % sizeof(int);
		assert( (yltb[yi] & (1<<ybit)) != 0 );

		int zi = z/ sizeof(int);
		int zbit  = z % sizeof(int);
		assert( (zltb[zi] & (1<<zbit)) != 0 );

	}

	/*
	 for (int i = 0; i < num_particles; i++) {
	 if (particles[i][3] == 1) printf("%i\n", i);
	 }
	 */

	/******************************* loop *********************************/
	// exchange outer with inner loop
	printf("\n# Starting ...\n");

	int  *mags = malloc(nsteps * sizeof(int)); // magnetization per step
	for (int i = 0; i < nsteps; i++) {
		mags[i] = 0;
	}

	// loop over particles
	double calc_time=0;
	printf("MinSize: %i\n", DSFMT_N64);
	double *dir_pool = malloc(DSFMT_N64 * sizeof(double));
#pragma omp parallel for reduction(+:calc_time) firstprivate(dir_pool)
	for (int i = 0; i < num_particles; i+=DSFMT_N64) {
		// every thread gets its own RNG
		dsfmt_t dsfmt;
		dsfmt_init_gen_rand(&dsfmt, i);

		/*
		 double *random_numbers_steps = malloc(nsteps * sizeof(double));
		 // create random numbers for the movements (directions 1..6)
		 dsfmt_fill_array_open_close(&dsfmt, random_numbers_steps, nsteps);

		 // scale the to 0,1,2,3,4,5 (the 6 directions)
		 for (int i=0 ;i < nsteps; i++) {
		 directions[i] = (short) (random_numbers_steps[i]*6);
		 }
		 free(random_numbers_steps);
		 */

		// distribution of correlation times, rexp,rnor are NOT thread safe!
		//for (int i=0 ;i < num_particles; i++) {
		//	correlation_times[i] = (int) rexp()*30;
		//}

		// loop over steps
		for (int step = 0; step < nsteps; step++) {
			dsfmt_fill_array_open_close(&dsfmt, dir_pool, DSFMT_N64);
			double start = omp_get_wtime();
			// doing batches of particles
			for (int j = 0; j < DSFMT_N64; j++) {
				int* particle = particles[i+j];
				int direction = dir_pool[j];
				//int direction = (int) (dsfmt_genrand_close_open(&dsfmt)*6);
				// only move particles which have not met defect yet == 0
				// or see how often they met a defefct >= 0
				if (particle[3] == 0) {

					// random step
					move_particle(particle, direction);

					// obey periodic boundary conditions, i.e. fold back
					check_pbc(particle, box_size);

					int tc = 10;

					// check_defect(particle, tc , hash_list, num_defects);
					// check_defect_tlb(particle, tc, hash_min, span, defekt_ltb);
					// check_defect_3d(particle, lookup_table, tc);
					check_defect_ltb(particle, xltb, yltb, zltb , tc);

					// ref_check_defect(particle, defects, num_defects);
				}
				else { // particle is trapped, decrease the residual waiting time
					particle[3] -= 1;
				}
				#pragma omp atomic
				mags[step] += particle[3];
				//gsl_histogram_increment (hvisits, particle[3]);
				//if (magnetization == num_particles) main_loop_break = 1;
				//int tid = omp_get_thread_num();
				//printf("Thread %i:  %i %i\n", id, i, direction);
				/*if (step%2000 == 0) {
				 printf("# Step: %8i (MAG: %5i)\r", step, magnetization);
				 fflush(stdout);
				 }*/
				//printf("%8i %8i %8i %8i\n",particle[0],particle[1],particle[2],particle[3]);

			} // end sub particle loop
			double stop = omp_get_wtime();
			calc_time += (stop-start);

		} // end steps loop
		/*
		 if (i%32 == 0) {
		 double stop = omp_get_wtime();
		 printf("# Particle: %8i (%8.3f s) Magnetization: %8i\r",i , (stop-start)/32 , magnetization);
		 fflush(stdout);
		 }
		 */
		// open the file we are writing to

		//#pragma omp critical
		//printf("# Particle: %8i (%8.3f s) \n",i , stop-start);
	} // end particle loop
	printf("Speed: %.2e s/particle \n", calc_time/num_particles);

	FILE *outFile;
	char fname[] = "binout.omp";
	sprintf(fname, "binout.om%i",0);
	outFile = fopen(fname, "w");
	// use fwrite to write binary data to the file
	fwrite(mags, sizeof(mags[0]), nsteps, outFile);
	fclose(outFile);
	print_array(particles, 10, 3);

	//print_array(particles, num_particles, 4);
	free(mags);
	free2D_i(particles);
	free2D_i(defects);
#ifdef STATS
	printf("Directions drawn:\n");
	gsl_histogram_fprintf (stdout, h, "%g", "%g");
	printf("\n");
	gsl_histogram_free (h);
#endif
	//omp_destroy_lock(&omplock);
	return 0;
}

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

/* qsort C-string comparison function */
int cstring_cmp(const void *a, const void *b)
{
	//    const char **ia = (const char **)a;
	//    const char **ib = (const char **)b;
	//    return strcmp(*ia, *ib);
	return strcmp ( (const char*)a, (const char*)b);

	/* strcmp functions works exactly as expected from
	 comparison function */
}

/* asm long long comparison function */
/*
 int asm64_comp(const void *a, const void *b) {
 int i=0;
 __asm__(
 "mov (%%rdi), %%rdx\n\t"    // Subtract low word
 "sub (%%rsi), %%rdx\n\t"
 "mov 8(%%rdi), %%rdi\n\t"    // Subtract high word
 "sbb 8(%%rsi), %%rdi\n\t"
 "sbb %%eax, %%eax\n\t"    // %eax = -1 if below, zero otherwise
 "or %%rdx, %%rdi\n\t"    // %rdi is non-zero if comparison is non-zero
 "neg %%rdi\n\t"    // carry flag is 1 if comparison is non-zero
 "adc %%eax, %%eax\n\t" // Result in %eax
 "movl %%eax, %0\n\t"
 : "=a" (i)
 :"r" (a), "r" (b)
 );
 return i;
 }
 */

int int64_cmp(const void *a, const void *b)
{
	const int64_t *x = a, *y = b;
	if(*x > *y)
		return 1;
	else
		return (*x < *y) ? -1 : 0;
}

/* qsort int comparison function */
int int_cmp(const void *a, const void *b)
{
	//    const int *ia = (const int *)a; // casting pointer types
	//    const int *ib = (const int *)b;
    //return *ia  - *ib;
	/* integer comparison: returns negative if b > a
	 and positive if a > b */
	return ( *(int*)a - *(int*)b );
}

static inline int64_t hash(int x, int y, int z) {
	return  (int64_t) MULTIPLIER* (int64_t) MULTIPLIER * (int64_t) x + (int64_t) MULTIPLIER * (int64_t) y + (int64_t) z;
}

int** malloc2D_i(long nrows, long ncolumns){
	int **array = malloc(nrows * sizeof(int *));
	array[0] = malloc(nrows * ncolumns * sizeof(int));
	if (array[0] == NULL) printf("Could not allocate memory");
	for(int i = 1; i < nrows; i++)
		array[i] = array[0] + i * ncolumns;

	// set all elements to 0
	for (int i = 0; i < nrows; i++) {
		for (int j = 0; j < ncolumns; j++) {
			array[i][j] = 0;
		}
	}
	return array;
}

/*
 char** malloc2D_char(long nrows, long ncolumns){
 char **array = malloc(nrows * sizeof(char *));
 array[0] = malloc(nrows * ncolumns * sizeof(char));
 if (array[0] == NULL) printf("Could not allocate memory");
 for(int i = 1; i < nrows; i++)
 array[i] = array[0] + i * ncolumns;
 return array;
 }
 */
char** malloc2D_char(long nrows, long ncolumns){
	char **array = malloc(nrows * sizeof(char *));
	for(int i = 0; i < nrows; i++)
		array[i] = malloc(ncolumns * sizeof(char));
	return array;
}

void free2D_i(int** array) {
	//free(&array[0]);
	free(array);
}

void print_array(int **array, int nrows, int ncolumns){
	for (int i = 0; i < nrows; i++) {
		for (int j = 0; j < ncolumns-1; j++) {
			printf("%i ",array[i][j]);
		}
		printf("%i\n",array[i][ncolumns-1]);
	}
}

void move_particle(int *particle, int direction){
	switch (direction) {
		case 0:
			particle[0] += 1;
			break;
		case 1:
			particle[0] -= 1;
			break;
		case 2:
			particle[1] += 1;
			break;
		case 3:
			particle[1] -= 1;
			break;
		case 4:
			particle[2] += 1;
			break;
		case 5:
			particle[2] -= 1;
			break;
	} // end switch statement
}

static inline void check_pbc(int* particle, int box_size) {
	for (int i = 0; i < 3; i++) {
		// % is NOT the mod operator, but the REMAINDER, it is not working for negative numbers (of course in C only)
		//particle[i] = particle[i] % box_size + (particle[i]<0?box_size:0);
		particle[i] &= (box_size - 1);
	}
}
/* binary search */
void check_defect(int* particle, int correlation_time, int64_t* hash_list, int num_defects ){
	int64_t hash_val;
	hash_val = hash(particle[0],particle[1],particle[2]);
	int * ptr;
	ptr = bsearch( &hash_val, hash_list, num_defects , sizeof(int64_t), int64_cmp);
	if (ptr != NULL)
		particle[3] = correlation_time;
}

/* lookup table 1 */

void check_defect_hash(int* particle, int correlation_time, int64_t hash_list_min, int64_t span, char* defekt_ltb){
	int64_t hash_val, offset;
	hash_val = hash(particle[0],particle[1],particle[2]);
	offset = hash_val -  hash_list_min;
	if ((offset >= 0) && (offset < span)) {
		if (defekt_ltb[ offset ] == 1)
			particle[3] = correlation_time;
	}
}

/* check Method 3 */

void check_defect_3d(int* particle, char*** lookup, int correlation_time){
	int x = particle[0];
	int y = particle[1];
	int z = particle[2];
	if (lookup[x] != NULL) {
		if (lookup[x][y] != NULL) {
			if (lookup[x][y][z] == 1) {
				particle[3] = correlation_time;
			}
		}
	}
}

void check_defect_ltb(int* particle, int* x, int* y, int* z, int correlation_time){
	int i = particle[0] / sizeof(int);
	int bit = 1<< particle[0] % sizeof(int);
	if (  (x[i] & 1 << bit) > 0) {
		i = particle[1] / sizeof(int);
		bit = 1<< particle[1] % sizeof(int);
		if (  (y[i] & 1 << bit) > 0) {
			i = particle[2] / sizeof(int);
			bit = 1<< particle[2] % sizeof(int);
			if (  (z[i] & 1 << bit) > 0) {
				particle[3] = correlation_time;
			}
		}
	}
		/*
	 switch ( (x[particle[0] / sizeof(int)]) & (1<< particle[0] % sizeof(int))  ) {
		 case 0:
			 break;
		 default:
			 switch ( (y[particle[1] / sizeof(int)]) & (1<< particle[1] % sizeof(int))  ) {
				 case 0:
					 break;
				 default:
					 switch ( (z[particle[2] / sizeof(int)]) & (1<< particle[2] % sizeof(int))  ) {
						 case 0:
							 break;
						 default:
							 particle[3] = correlation_time;
					 }
			 }
	 }*/
}

/*
 void check_defect(int* particle, int correlation_time, int64_t* hash_list, int num_defects ){
 int64_t hash_val;
 hash_val = hash(particle[0],particle[1],particle[2]);

 int bsearch = 0;
 int left = 0;
 int right = num_defects-1;

 while (bsearch == 0 && left <= right) {
 // int middle = (left + right) / 2;
 // better: avoid integer overflow
 int middle = left + (right-left) / 2;

 if (hash_val == hash_list[middle]) {
 bsearch = 1;
 particle[3] = correlation_time;
 }
 else {
 if (hash_val < hash_list[middle]) right = middle - 1;
 if (hash_val > hash_list[middle]) left = middle + 1;
 }
 }
 }

 */

// REFERENCE METHOD
void ref_check_defect(int* particle, int** defect_coords, int num_defects){
	int isDefect = 0;
	for (int i = 0; (i < num_defects) && (isDefect == 0); i++) {
		for (int j=0; j<3; j++ ){
			if (particle[j] != defect_coords[i][j]) {
				break; // coordinate mismatch, go to next particle (break loop over coordinates)
			}
			else {
				if (j==2) { // x,y and z ccordinate match
					particle[3] = (int) (rexp()*3) ; // set scalar value
					isDefect = 1; // break outer loop
				}
			}
		}
	}
}

/*

 void HT_check_defect(int* particle, Fnv64_t* hash_list, int num_defects ){
 int* pItem;
 char hash_string[18];
 Fnv64_t hash_val;
 snprintf(hash_string, sizeof(hash_string), "%5i %5i %5i", particle[0], particle[1], particle[2]);
 hash_val = fnv_64_str(hash_string, FNV0_64_INIT);
 pItem = (int*) bsearch (&hash_val, hash_list, num_defects, sizeof (Fnv64_t), fnv64_cmp);
 if (pItem != 0) particle[3] = 1; // set scalar value

 }

 */

/*
 // now create a hashed list to find them easier (hopefully)
 Fnv64_t hash_val;

 Fnv64_t *hash_list = malloc( num_defects*sizeof(Fnv64_t) );
 for (int i = 0; i < num_defects; i++) {
 snprintf(hash_string, sizeof(hash_string), "%5i %5i %5i", defects[i][0], defects[i][1], defects[i][2]);
 hash_val = fnv_64_str(hash_string, FNV1_64_INIT);
 hash_list[i] = hash_val;
 }
 qsort(hash_list, num_defects,sizeof(Fnv64_t), fnv64_cmp);
 */

/* mit chars
 char **hash_list = malloc2D_char(num_defects, 18);
 //char hash_list[20][18];
 for (int i = 0; i < num_defects; i++) {
 snprintf(hash_string, sizeof(hash_string), "%5i %5i %5i", defects[i][0], defects[i][1], defects[i][2]);
 hash_list[i] = hash_string;
 printf("%03i %s\n",i, hash_list[i]);
 }

 printf("Sorted\n");

 qsort(hash_list, num_defects, sizeof(hash_list[0]), cstring_cmp);//cmpstring_up);
 */


#include "stdio.h"
#include "gsl/gsl_integration.h"
#include "gsl/gsl_errno.h"
#include "limber.h"
#include <gsl/gsl_interp2d.h>
//#include <gsl/gsl_spline2d.h>
//#include <gsl/gsl_spline.h>

// This is a workspace size for the gsl integrator
#define LIMBER_FIXED_TABLE_SIZE 4096

// data that is passed into the integrator
// This is everything we need to compute the
// integrand
typedef struct IntegrandData{
	double chimin;
	double chimax;
	double ell;
	gsl_spline * WbX; //K1
	gsl_spline * WfX; //K2
	gsl_spline * WmX; //K3
	gsl_spline * WbY; //K4
	gsl_spline * WfY; //K5
	gsl_spline * WmY; //K6
	Interpolator2D * P;
	Interpolator2D * f; //for the f(k,z(x))
	Interpolator2D * D; //for the D(k,z(x))
	Interpolator2D * BB; //for the b(k,z(x))
	gsl_interp_accel * accelerator_xb;
	gsl_interp_accel * accelerator_yb;
	gsl_interp_accel * accelerator_xf;
	gsl_interp_accel * accelerator_yf;
	gsl_interp_accel * accelerator_xm;
	gsl_interp_accel * accelerator_ym;
} IntegrandData;

// the integrand for all
static double integrand(double chi, void * data_void)
{
	IntegrandData * data = (IntegrandData*) data_void;
	// Return 0 if outside range, for convenience.
	// Important to ensure that ranges are wide enough.
	if(chi < data->chimin || chi > data->chimax) return 0.0;

//exactly the same notation of paperIII

	double wx0 = gsl_spline_eval(data->WbX,chi,data->accelerator_xb);
	double wy0 = gsl_spline_eval(data->WbY,chi,data->accelerator_yb);

	double wx0m = gsl_spline_eval(data->WmX,chi,data->accelerator_xm);
	double wy0m = gsl_spline_eval(data->WmY,chi,data->accelerator_ym);

	double wx1 = gsl_spline_eval(data->WfX,chi,data->accelerator_xf);
	double wy1 = gsl_spline_eval(data->WfY,chi,data->accelerator_yf);

	double wx2 = gsl_spline_eval(data->WfX,chi*(2.0*data->ell-3.0)/(2.0*data->ell+1.0),data->accelerator_xf);
	double wy2 = gsl_spline_eval(data->WfY,chi*(2.0*data->ell-3.0)/(2.0*data->ell+1.0),data->accelerator_yf);

	double chi5 = chi*(2.0*data->ell+5.0)/(2.0*data->ell+1.0) < data->chimax ? chi*(2.0*data->ell+5.0)/(2.0*data->ell+1.0) : 0.0;
	double wx3 = gsl_spline_eval(data->WfX, chi5, data->accelerator_xf);
	double wy3 = gsl_spline_eval(data->WfY, chi5, data->accelerator_yf);

        double c1 = (2.*data->ell*data->ell+2.*data->ell-1.)/((2.*data->ell-1.)*(2.*data->ell+3.));
        double c2 = -((data->ell*(data->ell-1.))/((2.*data->ell-1.)*sqrt((2.*data->ell+1.)*(2.*data->ell-3.))));
        double c3 = -(((data->ell+1.)*(data->ell+2.))/((2.*data->ell+3.)*sqrt((2.*data->ell+1.)*(2.*data->ell+5.))));

	// Get P(k,0) using k=ell/chi.
	// The interp_2d interpolator returns 0 if either
	// parameter is outside its range
	double ka = (data->ell+0.5) / chi;
	double pa = interp_2d(ka, chi, data->P);

        //for the scale dependent bias b(k,z)
        double b0 = interp_2d(ka, chi, data->BB);

        //for the scale dependent growth factor D(k,z)
        double d0 = interp_2d(ka, chi, data->D);
        double d1 = d0;
	double d2 = interp_2d(ka, chi*(2.0*data->ell-3.0)/(2.0*data->ell+1.0), data->D);
	double d3 = interp_2d(ka, chi5, data->D);

        //for the scale dependent growth rate f(k,z)
        double f1 = interp_2d(ka, chi, data->f);
	double f2 = interp_2d(ka, chi*(2.0*data->ell-3.0)/(2.0*data->ell+1.0), data->f);
	double f3 = interp_2d(ka, chi5, data->f);

        double WWX= (wx0*b0*d0 + wx0m*d0 + c1*wx1*f1*d1 + c2*wx2*f2*d2 + c3*wx3*f3*d3);
        double WWY= (wy0*b0*d0 + wy0m*d0 + c1*wy1*f1*d1 + c2*wy2*f2*d2 + c3*wy3*f3*d3);

	double result = WWX * WWY * pa / chi / chi;

	return result;

}

// These two convenience functions
// peer into the internals of the gsl_spline.
// This is probably a bit naughty, since they could
// in theory change the internals.
static double inline limber_gsl_spline_min_x(gsl_spline * s)
{
	return s->x[0];
}
static double inline limber_gsl_spline_max_x(gsl_spline * s)
{
	return s->x[s->size-1];
}

double get_kernel_peak(gsl_spline * WbX, gsl_spline * WbY, int n_chi)
{
  double chimin_x = limber_gsl_spline_min_x(WbX);
  double chimin_y = limber_gsl_spline_min_x(WbY);
  double chimax_x = limber_gsl_spline_max_x(WbX);
  double chimax_y = limber_gsl_spline_max_x(WbY);
  double chimin = chimin_x>chimin_y ? chimin_x : chimin_y;
  double chimax = chimax_x<chimax_y ? chimax_x : chimax_y;
  double dchi = (chimax - chimin)/n_chi;
  double chi_peak = chimin;
  double chi;
  double kernel_val=0.;
  double kernel_peak=0.;
  for (int i_chi=0; i_chi<=n_chi; i_chi++){
    chi=chimin+i_chi*dchi;
    kernel_val = gsl_spline_eval(WbX,chi,NULL) * gsl_spline_eval(WbY,chi,NULL) / chi / chi;
    if (kernel_val>kernel_peak){
      kernel_peak = kernel_val;
      chi_peak = chi;
    }
  }
  // printf("chi_peak = %f\n",chi_peak);
  return chi_peak;
}

gsl_integration_workspace * W = NULL;
gsl_integration_glfixed_table *table = NULL;

void setup_integration_workspaces(){
	if (W==NULL){
		W = gsl_integration_workspace_alloc(LIMBER_FIXED_TABLE_SIZE);
	}
	if (table==NULL){
		table = gsl_integration_glfixed_table_alloc((size_t) LIMBER_FIXED_TABLE_SIZE);
	}
}

static
void limber_gsl_fallback_integrator(gsl_function * F, double chimin, double chimax,
	double abstol, double reltol, double * c_ell, double * error){

	// Only one warning per process
	static int fallback_warning_given = 0;

	// Deactivate error handling - if this one fails we will fall back to a more reliable but slower integrator
	gsl_error_handler_t * old_handler = gsl_set_error_handler_off();

	// Try the fast but flaky integrator.
	int status = gsl_integration_qag(F, chimin, chimax, abstol, reltol, LIMBER_FIXED_TABLE_SIZE, GSL_INTEG_GAUSS61, W, c_ell, error);

	// Restore the old error handler
	gsl_set_error_handler(old_handler);

	// If the fast integrator failed fall back to the old one.
	if (status){
		IntegrandData * data = (IntegrandData*) F->params;
		double ell = data->ell;
		if (fallback_warning_given==0){
			fprintf(stderr, "Falling back to the old integrator for ell=%lf (status=%d)\n", ell,status);
			fallback_warning_given=1;
		}
		*c_ell = gsl_integration_glfixed(F,chimin,chimax,table);
	}

}

// The only function in this little library callable from the outside
// world.  The limber_config structure is defined in limber.h but is fairly
// obvious.  The splines and the interpolator need to be functions of
// chi NOT z.
gsl_spline * limber_integral(limber_config * config, gsl_spline * WbX,  gsl_spline * WfX,  gsl_spline * WmX, gsl_spline * WbY,
	                 gsl_spline * WfY, gsl_spline * WmY, Interpolator2D * P, Interpolator2D * f, Interpolator2D * D, Interpolator2D * BB)

{

    config->status = LIMBER_STATUS_ERROR;
    int any_parameter_error=0;
    if (WbX==NULL){
        fprintf(stderr, "NULL WbX parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (WbY==NULL){
        fprintf(stderr, "NULL WbY parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (WfX==NULL){
        fprintf(stderr, "NULL WfX parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (WfY==NULL){
        fprintf(stderr, "NULL WfY parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (WmX==NULL){
        fprintf(stderr, "NULL WmX parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (WmY==NULL){
        fprintf(stderr, "NULL WmY parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (P==NULL){
        fprintf(stderr, "NULL P parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (f==NULL){
        fprintf(stderr, "NULL f parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (D==NULL){
        fprintf(stderr, "NULL D parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (config->n_ell<0){
        fprintf(stderr, "Negative n_ell parameter in limber_integral\n");
        any_parameter_error = 1;
    }
    if (config->n_ell==0){
        fprintf(stderr, "Error: n_ell=0 in limber calculation.\n");
        any_parameter_error = 1;
    }
    if (any_parameter_error){
        return NULL;
    }

	config->status = LIMBER_STATUS_OK;

	// Get the appropriate ranges over which to integrate
	// It is assumed that (at least one of) the kernel
	// splines should go to zero in some kind of reasonable
	// place, so we just use the range they specify
	IntegrandData data;
	double chimin_xb = limber_gsl_spline_min_x(WbX);
	double chimin_yb = limber_gsl_spline_min_x(WbY);
	double chimax_xb = limber_gsl_spline_max_x(WbX);
	double chimax_yb = limber_gsl_spline_max_x(WbY);

	double chimin_xf = limber_gsl_spline_min_x(WfX);
	double chimin_yf = limber_gsl_spline_min_x(WfY);
	double chimax_xf = limber_gsl_spline_max_x(WfX);
	double chimax_yf = limber_gsl_spline_max_x(WfY);

	double chimin_xm = limber_gsl_spline_min_x(WmX);
	double chimin_ym = limber_gsl_spline_min_x(WmY);
	double chimax_xm = limber_gsl_spline_max_x(WmX);
	double chimax_ym = limber_gsl_spline_max_x(WmY);

        //my ifs here
        double chimin_x = chimin_xb > chimin_xf ? chimin_xb : chimin_xf;
        double chimin_y = chimin_yb > chimin_yf ? chimin_yb : chimin_yf;
        double chimax_x = chimax_xb < chimax_xf ? chimax_xb : chimax_xf;
        double chimax_y = chimax_yb < chimax_yf ? chimax_yb : chimax_yf;
	double c_ell, error;

	// Workspaces for the main and falback integrators.
	// Static, so only allocated once as it has a fixed size.
	setup_integration_workspaces();

	double reltol = config->relative_tolerance;
	double abstol = config->absolute_tolerance;
	// double reltol = 0.001;
	// double abstol = 0.00001;
	// printf("TOLS: %le %le\n",reltol,abstol);

	// Take the smallest range since we want both the
	// splines to be valid there.
	// This range as well as all the data needed to compute
	// the integrand is put into a struct to be passed
	// through the integrator to the function above.
	data.chimin = chimin_x>chimin_y ? chimin_x : chimin_y;
	data.chimax = chimax_x<chimax_y ? chimax_x : chimax_y;
	data.WbX = WbX;
	data.WfX = WfX;
	data.WmX = WmX;
	data.WbY = WbY;
	data.WfY = WfY;
	data.WmY = WmY;
	data.P = P;
	data.f = f;//new
	data.D = D;//new
	data.BB = BB;//new
	data.accelerator_xb = gsl_interp_accel_alloc();
	data.accelerator_xf = gsl_interp_accel_alloc();
	data.accelerator_xm = gsl_interp_accel_alloc();
	data.accelerator_yb = gsl_interp_accel_alloc();
	data.accelerator_yf = gsl_interp_accel_alloc();
	data.accelerator_ym = gsl_interp_accel_alloc();

	// Set up the workspace and inputs to the integrator.
	// Not entirely sure what the table is.
	gsl_function F;
	F.function = integrand;
	F.params = &data;

	//gsl_integration_workspace * workspace = gsl_integration_workspace_alloc(2048);

	// results of the integration go into these arrays.
	double c_ell_vector[config->n_ell];
	double ell_vector[config->n_ell];

	// loop through ell values according to the input configuration
	for (int i_ell = 0; i_ell<config->n_ell; i_ell++){
		double ell = config->ell[i_ell];
		data.ell=ell;

		// Perform the main integration.
		// This particular function is used because that's what Matt Becker
		// found to work best.
		//c_ell = gsl_integration_glfixed(&F,data.chimin,data.chimax,table);
		// New function still attributable to the legacy of Matt Becker's integrator wisdom.
		// gsl_integration_qag(&F, data.chimin, data.chimax, abstol, reltol, LIMBER_FIXED_TABLE_SIZE, GSL_INTEG_GAUSS61, W, table, &c_ell, &error);
		//printf("%d %f %f\n",i_ell,c_ell_old,c_ell);
		limber_gsl_fallback_integrator(&F, data.chimin, data.chimax,
			abstol, reltol, &c_ell, &error);

		//Include the prefactor scaling
		c_ell *= config->prefactor;

		// Record the results into arrays
		c_ell_vector[i_ell] = c_ell;
		ell_vector[i_ell] = ell;
	}

		// It is often useful to interpolate into the logs of the functions
		// This is optional in the config. We move this outside the main loop
		// since we may have all zeros in the output
		if (config->xlog) {
			for (int i_ell = 0; i_ell<config->n_ell; i_ell++){
				ell_vector[i_ell] = log(ell_vector[i_ell]);
			}
		}
		if (config->ylog){
			for (int i_ell = 0; i_ell<config->n_ell; i_ell++){
				if (c_ell_vector[i_ell]<0){
					config->status = LIMBER_STATUS_NEGATIVE;
				}
					// negative is worse than zero so only set to zero it not already negative
				else if ((c_ell_vector[i_ell]==0) && (config->status<LIMBER_STATUS_ZERO)){
					config->status = LIMBER_STATUS_ZERO;
				}
			}
			// If none of the values are <= 0 then we are okay to go ahead and take the logs.
			if (config->status == LIMBER_STATUS_OK){
				for (int i_ell = 0; i_ell<config->n_ell; i_ell++) c_ell_vector[i_ell] = log(c_ell_vector[i_ell]);
			}

		}

	// Create a spline of the arrays as the output
	gsl_spline * output = gsl_spline_alloc(gsl_interp_akima, (size_t) config->n_ell);
	gsl_spline_init(output, ell_vector, c_ell_vector, (size_t) config->n_ell);

	// Tidy up
	gsl_interp_accel_free(data.accelerator_xb);
	gsl_interp_accel_free(data.accelerator_yb);
	gsl_interp_accel_free(data.accelerator_xf);
	gsl_interp_accel_free(data.accelerator_yf);
	gsl_interp_accel_free(data.accelerator_xm);
	gsl_interp_accel_free(data.accelerator_ym);

	// These two are not deallocated because they are static and only initialized once.
	// gsl_integration_glfixed_table_free(table);
	// gsl_integration_workspace_free(W);

	// And that's it
	return output;
}


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

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

#define SCALE 0

/*
 * This module contains an implementation of the Levenberg-Marquardt
 * algorithm for nonlinear optimization problems. This implementation
 * closely follows the following works:
 *
 * [1] H. B. Nielsen, K. Madsen, Introduction to Optimization and
 *     Data Fitting, Informatics and Mathematical Modeling,
 *     Technical University of Denmark (DTU), 2010.
 */

typedef struct
{
  gsl_matrix *A;             /* J^T J */
  gsl_matrix *A_copy;        /* copy of J^T J */
  gsl_matrix *J;             /* Jacobian J(x) */
  gsl_vector *diag;          /* D = diag(J^T J) */
  gsl_vector *rhs;           /* rhs vector = -g = -J^T f */
  gsl_vector *x_trial;       /* trial parameter vector */
  gsl_vector *f_trial;       /* trial function vector */
  gsl_vector *work;          /* workspace length p */
  long nu;                   /* nu */
  double mu;                 /* LM damping parameter mu */
  double tau;                /* initial scale factor for mu */
} lmniel_state_t;

#include "lmmisc.c"

#define LM_ONE_THIRD         (0.333333333333333)

static int lmniel_alloc (void *vstate, const size_t n, const size_t p);
static void lmniel_free(void *vstate);
static int lmniel_set(void *vstate, const gsl_vector * swts,
                      gsl_multifit_function_fdf *fdf,
                      gsl_vector *x, gsl_vector *f, gsl_vector *dx);
static int lmniel_iterate(void *vstate, const gsl_vector *swts,
                          gsl_multifit_function_fdf *fdf,
                          gsl_vector *x, gsl_vector *f, gsl_vector *dx);

static int
lmniel_alloc (void *vstate, const size_t n, const size_t p)
{
  lmniel_state_t *state = (lmniel_state_t *) vstate;

  state->A = gsl_matrix_alloc(p, p);
  if (state->A == NULL)
    {
      GSL_ERROR ("failed to allocate space for A", GSL_ENOMEM);
    }

  state->J = gsl_matrix_alloc(n, p);
  if (state->J == NULL)
    {
      GSL_ERROR ("failed to allocate space for J", GSL_ENOMEM);
    }

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

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

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

  state->A_copy = gsl_matrix_alloc(p, p);
  if (state->A_copy == NULL)
    {
      GSL_ERROR ("failed to allocate space for A_copy", GSL_ENOMEM);
    }

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

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

  state->tau = 1.0e-3;

  return GSL_SUCCESS;
} /* lmniel_alloc() */

static void
lmniel_free(void *vstate)
{
  lmniel_state_t *state = (lmniel_state_t *) vstate;

  if (state->A)
    gsl_matrix_free(state->A);

  if (state->J)
    gsl_matrix_free(state->J);

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

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

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

  if (state->A_copy)
    gsl_matrix_free(state->A_copy);

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

  if (state->f_trial)
    gsl_vector_free(state->f_trial);
} /* lmniel_free() */

static int
lmniel_set(void *vstate, const gsl_vector *swts,
           gsl_multifit_function_fdf *fdf, gsl_vector *x,
           gsl_vector *f, gsl_vector *dx)
{
  int status;
  lmniel_state_t *state = (lmniel_state_t *) vstate;
  const size_t p = x->size;
  size_t i;

  /* initialize counters for function and Jacobian evaluations */
  fdf->nevalf = 0;
  fdf->nevaldf = 0;

  /* evaluate function and Jacobian at x and apply weight transform */
  status = gsl_multifit_eval_wf(fdf, x, swts, f);
  if (status)
   return status;

  if (fdf->df)
    status = gsl_multifit_eval_wdf(fdf, x, swts, state->J);
  else
    status = gsl_multifit_fdfsolver_dif_df(x, swts, fdf, f, state->J);
  if (status)
    return status;

  /* compute rhs = -J^T f */
  gsl_blas_dgemv(CblasTrans, -1.0, state->J, f, 0.0, state->rhs);

#if SCALE
  gsl_vector_set_zero(state->diag);
#else
  gsl_vector_set_all(state->diag, 1.0);
#endif

  /* set default parameters */
  state->nu = 2;

#if SCALE
  state->mu = state->tau;
#else
  /* compute mu_0 = tau * max(diag(J^T J)) */
  state->mu = -1.0;
  for (i = 0; i < p; ++i)
    {
      gsl_vector_view c = gsl_matrix_column(state->J, i);
      double result; /* (J^T J)_{ii} */

      gsl_blas_ddot(&c.vector, &c.vector, &result);
      state->mu = GSL_MAX(state->mu, result);
    }

  state->mu *= state->tau;
#endif

  return GSL_SUCCESS;
} /* lmniel_set() */

/*
lmniel_iterate()
  This function performs 1 iteration of the LM algorithm 6.18
from [1]. The algorithm is slightly modified to loop until we
find an acceptable step dx, in order to guarantee that each
function call contains a new input vector x.

Args: vstate - lm workspace
      swts   - data weights (NULL if unweighted)
      fdf    - function and Jacobian pointers
      x      - on input, current parameter vector
               on output, new parameter vector x + dx
      f      - on input, f(x)
               on output, f(x + dx)
      dx     - (output only) parameter step vector

Notes:
1) On input, the following must be initialized in state:
nu, mu, rhs, J

2) On output, the following are updated with the current iterates:
nu, mu, rhs, J

rhs needs to be set on each output, so that lmniel_gradient supplies
the correct g = J^T f
*/

static int
lmniel_iterate(void *vstate, const gsl_vector *swts,
               gsl_multifit_function_fdf *fdf, gsl_vector *x,
               gsl_vector *f, gsl_vector *dx)
{
  int status;
  lmniel_state_t *state = (lmniel_state_t *) vstate;
  gsl_matrix *J = state->J;                   /* Jacobian J(x) */
  gsl_matrix *A = state->A;                   /* J^T J */
  gsl_vector *rhs = state->rhs;               /* -g = -J^T f */
  gsl_vector *x_trial = state->x_trial;       /* trial x + dx */
  gsl_vector *f_trial = state->f_trial;       /* trial f(x + dx) */
  gsl_vector *diag = state->diag;             /* diag(D) */
  double dF;                                  /* F(x) - F(x + dx) */
  double dL;                                  /* L(0) - L(dx) */
  int foundstep = 0;                          /* found step dx */

  /* compute A = J^T J */
  status = gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, J, 0.0, A);
  if (status)
    return status;

  /* copy lower triangle to upper */
  gsl_matrix_transpose_tricpy('L', 0, A, A);

#if SCALE
  lmniel_update_diag(J, diag);
#endif

  /* loop until we find an acceptable step dx */
  while (!foundstep)
    {
      /* solve (A + mu*I) dx = g */
      status = lmniel_calc_dx(state->mu, A, rhs, dx, state);
      if (status)
        return status;

      /* compute x_trial = x + dx */
      lmniel_trial_step(x, dx, x_trial);

      /* compute f(x + dx) */
      status = gsl_multifit_eval_wf(fdf, x_trial, swts, f_trial);
      if (status)
       return status;

      /* compute dF = F(x) - F(x + dx) */
      dF = lmniel_calc_dF(f, f_trial);

      /* compute dL = L(0) - L(dx) = dx^T (mu*dx - g) */
      dL = lmniel_calc_dL(state->mu, diag, dx, rhs);

      /* check that rho = dF/dL > 0 */
      if ((dL > 0.0) && (dF >= 0.0))
        {
          /* reduction in error, step acceptable */

          double tmp;

          /* update LM parameter mu */
          tmp = 2.0 * (dF / dL) - 1.0;
          tmp = 1.0 - tmp*tmp*tmp;
          state->mu *= GSL_MAX(LM_ONE_THIRD, tmp);
          state->nu = 2;

          /* compute J <- J(x + dx) */
          if (fdf->df)
            status = gsl_multifit_eval_wdf(fdf, x_trial, swts, J);
          else
            status = gsl_multifit_fdfsolver_dif_df(x_trial, swts, fdf, f_trial, J);
          if (status)
            return status;

          /* update x <- x + dx */
          gsl_vector_memcpy(x, x_trial);

          /* update f <- f(x + dx) */
          gsl_vector_memcpy(f, f_trial);

          /* compute new rhs = -J^T f */
          gsl_blas_dgemv(CblasTrans, -1.0, J, f, 0.0, rhs);

          foundstep = 1;
        }
      else
        {
          long nu2;

          /* step did not reduce error, reject step */
          state->mu *= (double) state->nu;
          nu2 = state->nu << 1; /* 2*nu */
          if (nu2 <= state->nu)
            {
              gsl_vector_view d = gsl_matrix_diagonal(A);

              /*
               * nu has wrapped around / overflown, reset mu and nu
               * to original values and break to force another iteration
               */
              /*GSL_ERROR("nu parameter has overflown", GSL_EOVRFLW);*/
              state->nu = 2;
              state->mu = state->tau * gsl_vector_max(&d.vector);
              break;
            }
          state->nu = nu2;
        }
    } /* while (!foundstep) */

  return GSL_SUCCESS;
} /* lmniel_iterate() */

static int
lmniel_gradient(void *vstate, gsl_vector * g)
{
  lmniel_state_t *state = (lmniel_state_t *) vstate;
  gsl_vector_memcpy(g, state->rhs);
  gsl_vector_scale(g, -1.0);
  return GSL_SUCCESS;
}

static int
lmniel_jac(void *vstate, gsl_matrix * J)
{
  lmniel_state_t *state = (lmniel_state_t *) vstate;
  int s = gsl_matrix_memcpy(J, state->J);

  return s;
}

static const gsl_multifit_fdfsolver_type lmniel_type =
{
  "lmniel",
  sizeof(lmniel_state_t),
  &lmniel_alloc,
  &lmniel_set,
  &lmniel_iterate,
  &lmniel_gradient,
  &lmniel_jac,
  &lmniel_free
};

const gsl_multifit_fdfsolver_type *gsl_multifit_fdfsolver_lmniel = &lmniel_type;


#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <dc1394/dc1394.h>
#include <math.h>
#include <fitsio.h>
#include <gsl/gsl_statistics.h>
#include <xpa.h>
#include <sys/time.h>
#include <string.h>
#include <inttypes.h>

#define NXPA 10

typedef struct _Box {
    double  x;
    double  y;
    double  fwhm;
    double  cenx;
    double  ceny;
    double  counts;
    double  background;
    double  noise;
    double  sigmaxythresh;
    double  sigmaxy;
    double  sigmafwhmthresh;
    double  sigmafwhm;
    int     r;
} Box;

typedef struct _Back {
    double x;
    double y;
    double background;
    double sigma;
    int r;
    int width;
} Back;

Box box[3];
Back back;
long nelements, naxes[2], fpixel;
int boxsize = 30;
double pixel_scale = 1.22;

double stardist(int i, int j) {
    return( sqrt( (box[i].cenx-box[j].cenx)*(box[i].cenx-box[j].cenx) +
		  (box[i].ceny-box[j].ceny)*(box[i].ceny-box[j].ceny) ) );
}

/* this routine uses a. tokovinin's modified equation given in
   2002, PASP, 114, 1156
*/
double seeing(double var, double d, double r) {
    double lambda;
    double b, K, seeing;

    lambda = 0.65e-6;
    b = r/d;

    /* pixel scale in "/pixel, convert to rad */
    var = var*pow(pixel_scale/206265.0,2);

    K = 0.364*(1.0 - 0.532*pow(b, -1/3) - 0.024*pow(b, -7/3));

    seeing = 206265.0*0.98*pow(d/lambda, 0.2)*pow(var/K, 0.6);

    return seeing;
}

/* this routine uses the classic DIMM equation */
double old_seeing(double var, double d, double r) {
    double lambda;
    double r0;

    lambda = 0.65e-6;

    /* pixel scale in "/pixel, convert to rad */
    var = var*pow(pixel_scale/206265.0,2);

    r0 = pow(2.0*(lambda*lambda)*(
		 ( 0.1790*pow(d, (-1.0/3.0)) -
		   0.0968*pow(r, (-1.0/3.0)) )/var
		 ), 0.6);

    return 206265.0*0.98*lambda/r0;
}

/* measure the background in an annulus around the spot pattern */
int background(char *image, int imwidth, int imheight) {

    int i, j, backpix;
    int low_y, up_y, low_x, up_x;
    double dist, sum, sumsq;

    backpix = 0;
    sum = 0.0;
    sumsq = 0.0;

    low_y = back.y - back.r - back.width;
    up_y  = back.y + back.r + back.width;
    low_x = back.x - back.r - back.width;
    up_x  = back.x + back.r + back.width;
    if (low_y < 0) {
	low_y = 0;
    }
    if (up_y >= imheight) {
	up_y = imheight;
    }
    if (low_x < 0) {
	low_x = 0;
    }
    if (up_x >= imwidth) {
	up_x = imwidth;
    }

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    dist = sqrt(pow(back.x-j, 2) + pow(back.y-i, 2));
	    if (dist >= back.r && dist <= back.r+back.width) {
		sum += image[i*naxes[0]+j];
		backpix++;
	    }
	}
    }

    back.background = sum/backpix;
    //back.background = 0.0;

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    dist = sqrt(pow(back.x-j, 2) + pow(back.y-i, 2));
	    if (dist >= back.r && dist <= back.r+back.width) {
		sumsq += (image[i*naxes[0]+j]-back.background)*
		    (image[i*naxes[0]+j]-back.background);
	    }
	}
    }

    back.sigma = sqrt(sumsq/backpix);
    //back.sigma = 1.0;

    return 1;

}

/* measure centroid using center-of-mass algorithm */
int centroid(char *image, int imwidth, int imheight, int num) {

    int i, j;
    double  sum    = 0.0;
    double  sumx   = 0.0;
    double  sumxx  = 0.0;
    double  sumy   = 0.0;
    double  sumyy  = 0.0;
    double  val = 0.0;
    double  gain = 0.5;
    double  rmom;
    double  dist;
    double nsigma = 5.0;
    int low_y, up_y, low_x, up_x;
    int sourcepix = 0;

    low_y = box[num].y - box[num].r;
    up_y  = box[num].y + box[num].r;
    low_x = box[num].x - box[num].r;
    up_x  = box[num].x + box[num].r;
    if (low_y < 0) {
	low_y = 0;
    }
    if (up_y >= imheight) {
	up_y = imheight;
    }
    if (low_x < 0) {
	low_x = 0;
    }
    if (up_x >= imwidth) {
	up_x = imwidth;
    }

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {

	    dist = sqrt(pow(box[num].x-j, 2) + pow(box[num].y-i, 2));
	    if (dist <= box[num].r) {
		val = image[i*naxes[0]+j] - back.background;
		if (val >= nsigma*back.sigma) {
		    sum   += val;
		    sumx  += val*j;
		    sumxx += val*j*j;
		    sumy  += val*i;
		    sumyy += val*i*i;
		    sourcepix++;
		}
	    }

	}
    }

    if ( sum <= 0.0 ) {
	box[num].sigmaxy = -1.0;
	box[num].sigmafwhm = -1.0;
	//box[num].x = imwidth/2.0;
	//box[num].y = imheight/2.0;
	box[num].fwhm = -1.0;
    } else {
	rmom = ( sumxx - sumx * sumx / sum + sumyy - sumy * sumy / sum ) / sum;

	if ( rmom <= 0 ) {
	    box[num].fwhm = -1.0;
	} else {
	    box[num].fwhm = sqrt(rmom)  * 2.354 / sqrt(2.0);
	}

	box[num].counts = sum;
	box[num].cenx   = sumx / sum;
	box[num].ceny   = sumy / sum;
	box[num].x += gain*(box[num].cenx - box[num].x);
	box[num].y += gain*(box[num].ceny - box[num].y);
	box[num].sigmaxy= box[num].noise * sourcepix / box[num].counts / sqrt(6.0);
	box[num].sigmafwhm = box[num].noise * pow(sourcepix,1.5) / 10.
	    / box[num].fwhm / box[num].counts
	    * 2.354 * 2.354 / 2.0;
    }

    return 1;

}

int grab_frame(dc1394camera_t *cam, char *buf, int nbytes) {
    dc1394video_frame_t *frame=NULL;
    dc1394error_t err;

    err = dc1394_capture_dequeue(cam, DC1394_CAPTURE_POLICY_WAIT, &frame);
    if (err != DC1394_SUCCESS) {
	dc1394_log_error("Unable to capture.");
	dc1394_capture_stop(cam);
	dc1394_camera_free(cam);
	exit(1);
    }

    memcpy(buf, frame->image, nbytes);
    dc1394_capture_enqueue(cam, frame);
    return 1;
}

int add_gaussian(char *buffer, float cenx, float ceny, float a, float sigma) {
    float gauss, rsq;
    int i, j, low_x, up_x, low_y, up_y, size;

    size = 30;
    low_x = (int)(cenx-size);
    up_x = (int)(cenx+size);
    low_y = (int)(ceny-size);
    up_y = (int)(ceny+size);

    for (i=low_y; i<up_y; i++) {
	for (j=low_x; j<up_x; j++) {
	    rsq = (cenx - j)*(cenx - j) + (ceny - i)*(ceny - i);
	    gauss = a*expf(-1.0*rsq/(sigma*sigma));
	    if (gauss > 255)
		gauss = 255;
	    buffer[i*naxes[0]+j] += (char)gauss;
	}
    }

    return 1;
}

int main() {

    dc1394camera_t *camera;
    char *buffer, *buffer2, *average;
    fitsfile *fptr;
    int i, j, f, status, nimages, anynul, nboxes, test;
    char fitsfile[256], xpastr[256];
    char *froot, *timestr;
    FILE *init, *out;
    float xx = 0.0, yy = 0.0, xsum = 0.0, ysum = 0.0;
    double dist[2000], sig[2000], dist_l[2000], sig_l[2000];
    double mean, var, var_l, avesig;
    double seeing_short, seeing_long, seeing_ave;
    struct timeval start_time, end_time;
    time_t start_sec, end_sec;
    suseconds_t start_usec, end_usec;
    float elapsed_time, fps;

    dc1394_t * dc;
    dc1394camera_list_t * list;
    dc1394error_t err;

    unsigned int min_bytes, max_bytes, max_height, max_width;
    unsigned int actual_bytes, winleft, wintop;
    uint64_t total_bytes = 0;
    char *names[NXPA];
    char *messages[NXPA];

    double d = 0.060;
    double r = 0.130;

    XPA xpa;
    xpa = XPAOpen(NULL);

    stderr = freopen("measure_seeing.log", "w", stderr);

    status = 0;
    anynul = 0;
    naxes[0] = 320;
    naxes[1] = 240;
    fpixel = 1;

    nelements = naxes[0]*naxes[1];

    dc = dc1394_new();
    if (!dc)
	return 1;
    err = dc1394_camera_enumerate(dc, &list);
    DC1394_ERR_RTN(err, "Failed to enumerate cameras.");

    if (list->num == 0) {
	dc1394_log_error("No cameras found.");
	return 1;
    }

    camera = dc1394_camera_new(dc, list->ids[0].guid);
    if (!camera) {
	dc1394_log_error("Failed to initialize camera with guid %"PRIx64".",
			 list->ids[0].guid);
	return 1;
    }
    dc1394_camera_free_list(list);

    printf("Using camera with GUID %"PRIx64"\n", camera->guid);

    // need to use legacy firewire400 mode for now.  800 not quite reliable.
    dc1394_video_set_iso_speed(camera, DC1394_ISO_SPEED_400);

    // configure camera for format7
    err = dc1394_video_set_mode(camera, DC1394_VIDEO_MODE_FORMAT7_1);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't choose format7_0");
    printf("I: video mode is format7_0\n");

    err = dc1394_format7_get_max_image_size(camera, DC1394_VIDEO_MODE_FORMAT7_1,
					    &max_width, &max_height);
    DC1394_ERR_CLN_RTN(err,dc1394_camera_free (camera),"cannot get max image size for format7_0");
    printf ("I: max image size is: height = %d, width = %d\n", max_height, max_width);
    printf ("I: current image size is: height = %ld, width = %ld\n", naxes[1], naxes[0]);

    //winleft = (max_width - naxes[0])/2;
    //wintop = (max_height - naxes[1])/2;
    winleft = 0;
    wintop = 0;

    err = dc1394_format7_set_roi(camera,
				 DC1394_VIDEO_MODE_FORMAT7_1,
				 DC1394_COLOR_CODING_MONO8,
				 DC1394_USE_MAX_AVAIL,
				 winleft, wintop, // left, top
				 naxes[0], naxes[1]);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't set ROI.");
    printf("I: ROI is (%d, %d) - (%ld, %ld)\n",
	   winleft, wintop, winleft+naxes[0], wintop+naxes[1]);

    err = dc1394_format7_get_total_bytes(camera, DC1394_VIDEO_MODE_FORMAT7_1, &total_bytes);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Can't get total bytes.");
    printf("I: total bytes per frame are %"PRIu64"\n", total_bytes);

    err = dc1394_capture_setup(camera, 16, DC1394_CAPTURE_FLAGS_DEFAULT);
    DC1394_ERR_CLN_RTN(err, dc1394_camera_free(camera), "Error capturing.");

    // start the camera up
    err = dc1394_video_set_transmission(camera, DC1394_ON);
    if (err != DC1394_SUCCESS) {
	dc1394_log_error("Unable to start camera iso transmission.");
	dc1394_capture_stop(camera);
	dc1394_camera_free(camera);
	exit(1);
    }

    out = fopen("seeing.dat", "a");
    init = fopen("init_cen_all", "r");
    i = 0;
    while (fscanf(init, "%f %f\n", &xx, &yy) != EOF) {
	box[i].x = xx;
	box[i].cenx = xx;
	box[i].y = yy;
	box[i].ceny = yy;
	box[i].r = boxsize/2.0;
	i++;
    }
    nboxes = i;
    fclose(init);

    back.r = 80;
    back.width = 10;

    /* allocate the buffers */
    if (!(buffer = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate Image Buffer\n");
	exit(-1);
    }
    if (!(buffer2 = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate 2nd Image Buffer\n");
	exit(-1);
    }
    if (!(average = malloc(nelements*sizeof(char)))) {
	printf("Couldn't Allocate Average Image Buffer\n");
	exit(-1);
    }

    nimages = 2000;
    froot = "seeing.fits";

    gettimeofday(&start_time, NULL);

    for (f=0; f<nimages; f++) {
	/* first do a single exposure */
	grab_frame(camera, buffer, nelements*sizeof(char));
	// add_gaussian(buffer, 175.0, 130.0, 50.0, 3.0);
	// add_gaussian(buffer, 155.0, 115.0, 50.0, 3.0);

	// find center of star images and calculate background
	xsum = 0.0;
	ysum = 0.0;
	for (i=0; i<nboxes; i++) {
	    xsum += box[i].cenx;
	    ysum += box[i].ceny;
	}
	back.x = xsum/nboxes;
	back.y = ysum/nboxes;
	background(buffer, naxes[0], naxes[1]);

	for (i=0; i<nboxes; i++) {
	    box[i].noise = back.sigma;
	    box[i].r = boxsize/2.0;
	    //centroid(buffer, i);
	    //box[i].r = boxsize/3.0;
	    //centroid(buffer, i);
	    //box[i].r = boxsize/4.0;
	    centroid(buffer, naxes[0], naxes[1], i);
	}

	dist[f] = stardist(0, 1);
	sig[f] = box[0].sigmaxy*box[0].sigmaxy + box[1].sigmaxy*box[1].sigmaxy;

	/* now average two exposures */
	grab_frame(camera, buffer, nelements*sizeof(char));
	grab_frame(camera, buffer2, nelements*sizeof(char));

	for (j=0; j<nelements; j++) {
	    test = buffer[j]+buffer2[j];
	    if (test <= 254) {
		average[j] = buffer[j]+buffer2[j];
	    } else {
		average[j] = 254;
	    }
	}
	// add_gaussian(average, 175.0, 130.0, 50.0, 3.0);
	// add_gaussian(average, 155.0, 115.0, 50.0, 3.0);

	xsum = 0.0;
	ysum = 0.0;
	for (i=0; i<nboxes; i++) {
	    xsum += box[i].cenx;
	    ysum += box[i].ceny;
	}
	back.x = xsum/nboxes;
	back.y = ysum/nboxes;
	background(average, naxes[0], naxes[1]);

	for (i=0; i<nboxes; i++) {
	    box[i].noise = back.sigma;
	    box[i].r = boxsize/2.0;
	    //centroid(average, i);
	    //box[i].r = boxsize/3.0;
	    //centroid(average, i);
	    //box[i].r = boxsize/4.0;
	    centroid(average, naxes[0], naxes[1], i);
	}

	dist_l[f] = stardist(0, 1);
	sig_l[f] = box[0].sigmaxy*box[0].sigmaxy + box[1].sigmaxy*box[1].sigmaxy;

	if (f % 40 == 0) {
	    status = XPASet(xpa, "ds9", "array [xdim=320,ydim=240,bitpix=8]", "ack=false",
			    average, nelements, names, messages, NXPA);
	    sprintf(xpastr, "image; box %f %f %d %d 0.0",
		    box[0].x, box[0].y, boxsize, boxsize);
	    status = XPASet(xpa, "ds9", "regions", "ack=false",
			    xpastr, strlen(xpastr), names, messages, NXPA);
	    sprintf(xpastr, "image; box %f %f %d %d 0.0",
		    box[1].x, box[1].y, boxsize, boxsize);
	    status = XPASet(xpa, "ds9", "regions", "ack=false",
			    xpastr, strlen(xpastr), names, messages, NXPA);
	}

    }

    gettimeofday(&end_time, NULL);
    printf("End capture.\n");

    /*-----------------------------------------------------------------------
     *  stop data transmission
     *-----------------------------------------------------------------------*/
    start_sec = start_time.tv_sec;
    start_usec = start_time.tv_usec;
    end_sec = end_time.tv_sec;
    end_usec = end_time.tv_usec;

    elapsed_time = (float)((end_sec + 1.0e-6*end_usec) - (start_sec + 1.0e-6*start_usec));
    fps = 3*nimages/elapsed_time;
    printf("Elapsed time = %g seconds.\n", elapsed_time);
    printf("Framerate = %g fps.\n", fps);

    err=dc1394_video_set_transmission(camera,DC1394_OFF);
    DC1394_ERR_RTN(err,"couldn't stop the camera?");

    /*
    sprintf(fitsfile, "!%s", froot);
    fits_create_file(&fptr, fitsfile, &status);
    fits_create_img(fptr, BYTE_IMG, 2, naxes, &status);
    fits_write_img(fptr, TBYTE, fpixel, nelements, buffer, &status);
    fits_close_file(fptr, &status);
    fits_report_error(stderr, status);
    */

    /* analyze short exposure */
    printf("\t SHORT EXPOSURE\n");
    mean = gsl_stats_mean(dist, 1, nimages);
    avesig = gsl_stats_mean(sig, 1, nimages);
    printf("mean = %f, avesig = %f\n", mean, avesig);

    printf("\n");

    var = gsl_stats_variance_m(dist, 1, nimages, mean);
    var = var - avesig;
    seeing_short = seeing(var, d, r);
    printf("sigma = %f, seeing = %f\n", sqrt(var), seeing_short);

    /* analyze long exposure */
    printf("\t LONG EXPOSURE\n");
    mean = gsl_stats_mean(dist_l, 1, nimages);
    avesig = gsl_stats_mean(sig_l, 1, nimages);
    printf("mean_l = %f, avesig_l = %f\n", mean, avesig);

    printf("\n");

    var_l = gsl_stats_variance_m(dist_l, 1, nimages, mean);
    var_l = var_l - avesig;
    seeing_long = seeing(var_l, d, r);
    printf("sigma_l = %f, seeing_l = %f\n", sqrt(var_l), seeing_long);

    seeing_ave = pow(seeing_short, 1.75)*pow(seeing_long,-0.75);
    printf("Exposure corrected seeing = %4.2f\"\n\n", seeing_ave);

    timestr = ctime(&end_sec);
    fprintf(out, "%s %f %f %f %f %f\n", timestr, var, var_l, seeing_short, seeing_long, seeing_ave);

    init = fopen("init_cen_all", "w");
    for (i=0; i<nboxes; i++) {
	fprintf(init, "%f %f\n", box[i].cenx, box[i].ceny);
    }
    fclose(init);

    fclose(out);

    return (status);
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/*
 Contains implementations of AdaDIF, TunedRwR, and PPR.

 Dimitris Berberidis
 University of Minnesota 2017-2018
*/

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <cblas.h>
#include <inttypes.h>
#include <pthread.h>
#include <sys/sysinfo.h>
#include <stdbool.h>

#include "comp_engine.h"
#include "csr_handling.h"
#include "my_defs.h"
#include "parameter_opt.h"
#include "my_utils.h"

static sz_short get_threads_and_width(sz_short* , sz_short );
static double* get_coef_A(sz_long , sz_med , sz_med , sz_long* , double* ,double* , const sz_long* , double );
static double* get_coef_b(sz_long , sz_med , sz_med ,sz_med , sz_long* , double* , sz_long* );

//Multi-threaded AdaDIF method (output is soft labels)
void AdaDIF_core_multi_thread( double* soft_labels, csr_graph graph, sz_med num_seeds,
			       const sz_long* seed_indices, sz_short num_class, sz_short* class_ind,
			       sz_med* num_per_class, sz_med walk_length, double lambda, bool no_constr, bool single_thread){

	sz_short NUM_THREADS, width;

	NUM_THREADS = get_threads_and_width(&width, num_class);

	if(single_thread) NUM_THREADS = 1;

	printf("NUMBER OF THREADS: %"PRIu16" \n", (uint16_t) NUM_THREADS);

	#if DEBUG
	printf("WIDTH= %"PRIu16"\n", (uint16_t) width);
	#endif

	clock_t begin = clock();

	//The following three blocks create copies of the CSR matrix
	//Each copy will be used by a thread

	csr_graph* graph_copies = csr_mult_deep_copy( graph, NUM_THREADS );

	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

	printf("Time spent copying csr_matrix: %lf\n",time_spent);

	//MAIN LOOP
	//Prepare data to be passed to each thread

	pthread_t tid[NUM_THREADS];
	pass_to_thread_type_2* data= (pass_to_thread_type_2*)malloc(NUM_THREADS*sizeof(pass_to_thread_type_2));
	for(sz_short i=0;i<NUM_THREADS;i++){
		(*(data+i))= (pass_to_thread_type_2) {.soft_labels=soft_labels,
			     			      .num_seeds=num_seeds,
						      .num_per_class=num_per_class,
						      .class_ind=class_ind,
						      .graph = graph_copies[i],
						      .seeds=seed_indices,
						      .walk_length=walk_length,
						      .lambda=lambda,
						      .from=i*width,
						      .no_constr = no_constr };

		if(i==NUM_THREADS-1){
			(data+i)->to=NUM_THREADS;
			(data+i)->num_local_classes= num_class -i*width;
		}else{
			(data+i)->to=(i+1)*width;
			(data+i)->num_local_classes=width;
		}
	}

	//Spawn threads and start running
	for(sz_short i=0;i<NUM_THREADS;i++){
		pthread_create(&tid[i],NULL,AdaDIF_squezze_to_one_thread,(void*)(data+i));
	}

	//Wait for all threads to finish before continuing
	for(sz_short i=0;i<NUM_THREADS;i++){pthread_join(tid[i], NULL);}

	//Free copies and temporary arrays

	csr_array_destroy(graph_copies,(sz_short)NUM_THREADS);
	free(data);

}

//Here I slice the output(soft labels) and input (class_ind)
//Using aliasing on the shifted pointers such that single threaded AdaDIF_core is compeltely "blind" to the slicing process
void* AdaDIF_squezze_to_one_thread( void* param){
	pass_to_thread_type_2* data = param;

	double* soft_labels = data->soft_labels + (data->graph.num_nodes * data->from);
	sz_short* class_ind = data->class_ind + data->num_seeds*data->from;
	sz_med* num_per_class = data->num_per_class + data->from;

	clock_t begin = clock();

	AdaDIF_core( soft_labels, data->graph,  data->num_seeds, data->seeds, data->num_local_classes, class_ind, num_per_class,
		     data->walk_length, data->lambda, data->no_constr);

	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

	printf("Thread from classes with index %"PRIu16" to %"PRIu16" finished in %lf sec \n",(uint16_t) data->from, (uint16_t) data->to, time_spent);

	pthread_exit(0);

}

//Core of AdaDIF method that runs on single thread (output is soft labels)
void AdaDIF_core( double* soft_labels, csr_graph graph, sz_med num_seeds,
		  const sz_long* seed_indices, sz_short num_class, sz_short* class_ind,
		  sz_med* num_per_class, sz_med walk_length, double lambda, bool no_constr){

	for(sz_short i=0;i<num_class;i++){

		double* land_prob = (double*) malloc(walk_length*graph.num_nodes*sizeof(double));
		double* dif_land_prob = (double*) malloc(walk_length*graph.num_nodes*sizeof(double));
		sz_long* local_seeds =(sz_long*)malloc(num_per_class[i] *sizeof(sz_long));

		sz_med k=0;
		for(sz_med j=0;j<num_seeds;j++){
			if( class_ind[i*num_seeds + j] ==1 )
				local_seeds[k++] = seed_indices[j];
		}

		perform_random_walk(land_prob, dif_land_prob, graph , walk_length , local_seeds , num_per_class[i] );

		double* theta = get_AdaDIF_parameters(graph.num_nodes,graph.degrees,land_prob,dif_land_prob,seed_indices,local_seeds,
		                                      class_ind+i*num_seeds,walk_length,num_seeds,num_per_class[i],lambda,no_constr );
		#if PRINT_THETAS
		printf("THETA: ");
		for(int n=0;n<walk_length;n++) printf(" %.3lf",theta[n]);
		printf("\n");
		#endif

		matvec_trans_long( soft_labels+i*graph.num_nodes , land_prob, theta, graph.num_nodes, walk_length );

		//free landing probabilities
		free(local_seeds);
		free(land_prob);
		free(dif_land_prob);
		free(theta);
	}
}

//Extract landing and dif probabilities by performing K steps of simple random walk on graph

void perform_random_walk(double* land_prob, double* dif_land_prob, csr_graph graph,
			 sz_med walk_length , sz_long* seeds , sz_med num_seeds ){

	double* seed_vector = (double*) malloc(graph.num_nodes* sizeof(double));
	double one_over_num_seeds = 1.0f /(double) num_seeds ;

	//prepare seed vector
	for(sz_long i=0;i<graph.num_nodes;i++) seed_vector[i] = 0.0f ;

	for(sz_med j=0;j<num_seeds;j++) seed_vector[seeds[j]] = one_over_num_seeds ;

	//do the random walk
	my_CSR_matvec( land_prob, seed_vector , graph);

	for(sz_med j=1;j<walk_length;j++){
		my_CSR_matvec( land_prob+j*graph.num_nodes, land_prob+(j-1)*graph.num_nodes , graph);
		my_array_sub( dif_land_prob+(j-1)*graph.num_nodes, land_prob+(j-1)*graph.num_nodes,
			      land_prob+j*graph.num_nodes, graph.num_nodes);
	}

	#if DEBUG
	printf("LAST LAND PROBs: \n");
	for(sz_long i=0;i<=100;i++) printf(" %lf ",land_prob[(walk_length-1)*graph.num_nodes + i ]) ;
	printf("...................... \n");
	#endif

	//do one final step to obtain the last differential

	double* extra_step = (double*) malloc(graph.num_nodes* sizeof(double));
	my_CSR_matvec( extra_step, land_prob+ (walk_length -1)*graph.num_nodes , graph);
	my_array_sub( dif_land_prob+(walk_length-1)*graph.num_nodes, land_prob+(walk_length-1)*graph.num_nodes,
		      extra_step, graph.num_nodes);

        //free
	free(seed_vector);
	free(extra_step);
}

//Extract the AdaDIF diffusion coefficients

double* get_AdaDIF_parameters( sz_long N, sz_long* degrees, double* land_prob,
			       double* dif_land_prob, const sz_long* seed_indices,
			       sz_long* local_seeds, sz_short* class_ind, sz_med walk_length,
			        sz_med num_seeds, sz_med num_pos,double lambda, bool no_constr){

	double* theta = (double*) malloc(walk_length*sizeof(double));

        //A and b are the Hessian and linear component of the quadratic cost

	double* A = get_coef_A(N,walk_length,num_seeds,degrees, land_prob, dif_land_prob,seed_indices,lambda);

	double* b = get_coef_b(N,walk_length,num_seeds,num_pos,degrees, land_prob, local_seeds);

	if(!no_constr){
		simplex_constr_QP_with_PG(theta,A,b,walk_length);
	}else{
		hyperplane_constr_QP(theta,A,b,walk_length);
	}

        //free
	free(b);
	free(A);

	return theta;
}

// This function computes how many threads will be used and how many classes will be allocated per thread
static sz_short get_threads_and_width(sz_short* width ,sz_short num_class){
	sz_short num_procs=get_nprocs();
	sz_short num_threads;

	if(num_class<=num_procs){
		num_threads = num_class;
		*width=1;
	}else{
		num_threads = num_procs;
		*width = (sz_short)ceil((double)(num_class/(double)num_threads));
	}

	return num_threads;
}

//Obtain slice of G  as stationary distributions. (Unweighted) Seed set must be defined
//Returns numbr of itrations untill convergence
sz_med get_slice_of_G( double* G_s, sz_long* seeds, sz_med num_seeds, double tel_prob,
			 csr_graph graph, bool single_thread ){

	//Do it SMART: Use a temporary G_s_next where you store the left hand side of iteration
	//Then at eah iteration just flip pointers between G_s and G_s_next
	//Multiple threads compute different slices of G_s
	sz_short NUM_THREADS = get_nprocs();
	printf("NUMBER OF THREADS: %"PRIu16" \n",(uint16_t) NUM_THREADS);

	if(single_thread) NUM_THREADS = 1;

	sz_med iter[NUM_THREADS];

	csr_graph graph_scaled = csr_deep_copy_and_scale(graph,1.0f-tel_prob);

	double* G_s_next=malloc(graph.num_nodes*num_seeds*sizeof(double));

	//Initialization
	for(sz_long i=0;i<graph.num_nodes*num_seeds;i++) G_s[i]=0.0f;

	for(sz_med j=0;j<num_seeds;j++) G_s[ num_seeds*seeds[j] +j]=1.0f;

	clock_t begin = clock();

	//The following three blocks create copies of the CSR matrix
	//Each copy will be used by a thread

	csr_graph* graph_copies = csr_mult_deep_copy( graph_scaled, (sz_short) NUM_THREADS );

	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

	printf("Time spent copying csr_matrix: %lf\n",time_spent);

	//MAIN LOOP
	//Prepare data to be passed to each thread
	sz_med width;
	width=(sz_med)floor((double)(num_seeds/(double)NUM_THREADS));
	pthread_t tid[NUM_THREADS];
	pass_to_thread_type_1* data= (pass_to_thread_type_1*)malloc(NUM_THREADS*sizeof(pass_to_thread_type_1));
	for(sz_short i=0;i<NUM_THREADS;i++){

	        (*(data+i))= (pass_to_thread_type_1) {.G_s=G_s,
			 			      .G_s_next=G_s_next,
						      .graph = graph_copies[i],
						      .seeds=seeds,
						      .M=num_seeds,
						      .tel_prob=tel_prob,
						      .from=i*width };

		if(i==NUM_THREADS-1){
			(data+i)->to=num_seeds;
		}else{
			(data+i)->to=(i+1)*width;
		}
		(data+i)->iter=&iter[i];
	}

	//Spawn threads and start running
	for(sz_short i=0;i<NUM_THREADS;i++){
		pthread_create(&tid[i],NULL,my_power_iter,(void*)(data+i));
	}

	//Wait for all threads to finish before continuing
	for(sz_short i=0;i<NUM_THREADS;i++){pthread_join(tid[i], NULL);}

	//Free copies and temporary arrays
	csr_destroy(graph_scaled);
	csr_array_destroy(graph_copies,(sz_short)NUM_THREADS);
	free(G_s_next);
	free(data);

	return iter[0];
}

//Power iteration that computes a subset (from,to) of the collumns of G_s
//Data are passed via struct that can be readily used by thread
void* my_power_iter(void* param){

	pass_to_thread_type_1* data = param;

	sz_long* seeds=data->seeds;
	double* G_s=data->G_s;
	double* G_s_next= data->G_s_next;
	csr_graph graph=data->graph;

	//Iterate using this "back and forth method" to minimize memory access
	clock_t begin = clock();

	sz_med iter=0;
	sz_short flag=0;

	do{
		iter++;
		flag=(flag==1) ? 0 : 1 ;
		if(flag==1){
			my_CSR_matmat( G_s_next , G_s  , graph , data->M , data->from, data->to);
			for(sz_med j=data->from;j<data->to;j++) G_s_next[data->M*seeds[j] +j]+=data->tel_prob;
		}
		else{
			my_CSR_matmat( G_s, G_s_next  , graph , data->M, data->from, data->to);
			for(sz_med j=data->from;j<data->to;j++) G_s[data->M*seeds[j] +j]+= data->tel_prob;
		}
	}while(iter<MAXIT && max_seed_val_difference( G_s, G_s_next , seeds ,data->M, data->from, data->to) >TOL);

	if(flag==1){
		for(sz_long i=0;i<graph.num_nodes;i++){
			for(sz_med j=data->from;j<data->to;j++)
				G_s[i*data->M + j]=G_s_next[i*data->M + j];
		}
	}

	*(data->iter)=iter;

	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

	printf("Thread from column %"PRIu32" to %"PRIu32" finished in %lf sec \n", (uint32_t) data->from, (uint32_t) data->to, time_spent);

	pthread_exit(0);
}

//Personalized-Pagerank single thread
void my_PPR_single_thread( double* soft_labels, csr_graph graph, sz_med num_seeds,
			   const sz_long* seed_indices, sz_short num_class, sz_short* class_ind,
			   sz_med* num_per_class, sz_med walk_length, double tel_prob){

	csr_graph graph_scaled = csr_deep_copy_and_scale(graph,1.0f-tel_prob);

	for(sz_short i=0;i<num_class;i++){
		sz_long* local_seeds =(sz_long*)malloc(num_per_class[i] *sizeof(sz_long));

		sz_med k = 0;
		for(sz_med j=0;j<num_seeds;j++){
			if( class_ind[i*num_seeds + j] ==1 )
				local_seeds[k++] = seed_indices[j];
		}

		//prepare seed vector
		double* soft = soft_labels + i*graph.num_nodes;
		double one_over_num_seeds = 1.0f /(double) num_per_class[i] ;
		for(sz_long n=0;n<graph.num_nodes;n++)  soft[n] = 0.0f ;
		for(sz_med j=0;j<num_per_class[i];j++) soft[local_seeds[j]] = one_over_num_seeds ;

		//perform random walk with restart
		sz_short flag=0;
		double* soft_next=malloc(graph.num_nodes*sizeof(double));
		for(sz_med j=0;j<walk_length;j++){
			flag=(flag==1) ? 0 : 1 ;
			if(flag==1){
				my_CSR_matvec(soft_next, soft, graph_scaled );
				for(sz_med k=0;k<num_per_class[i] ;k++)
					soft_next[local_seeds[k]] += tel_prob*one_over_num_seeds;
			}
			else{
				my_CSR_matvec(soft, soft_next, graph_scaled );
				for(sz_med k=0;k<num_per_class[i] ;k++)
					soft[local_seeds[k]] += tel_prob*one_over_num_seeds;
			}

		}

		if(flag==1){
			memcpy(soft, soft_next, graph.num_nodes*sizeof(double));
		}

		free(soft_next);
		free(local_seeds);

	}
	csr_destroy(graph_scaled);
}

//find l_max norm between vecors of known length
double max_seed_val_difference(double* A, double* B, sz_long* points , sz_med num_points, sz_med from, sz_med to){
	double dif;
	double max_dif=0.0f;
	for(sz_med i=from;i<to;i++){
		dif=fabs( A[ num_points*points[i] + i] - B[ num_points*points[i] + i] );
		max_dif = ( dif > max_dif ) ? dif : max_dif ;
	}
	return max_dif;
}

//Extract square submatrix of slice G_l that corresponds to labeled only
void extract_G_ll(double* G_ll, double* G_s, sz_long* seeds, sz_med num_seeds){

	for(sz_long i=0;i<num_seeds;i++){
		for(sz_med j=0;j<num_seeds;j++)
			G_ll[i*num_seeds + j]=G_s[seeds[i]*num_seeds + j];
	}
}

//The following two functions generate the coefficients required by the AdaDIF QP
static double* get_coef_A(sz_long N, sz_med K, sz_med L , sz_long* d ,
			  double* P ,double* P_dif , const sz_long* L_ind , double lambda ){

	double* A = (double*) malloc(K*K*sizeof(double));

	double* d_inv_times_lambda = (double*)malloc(N*sizeof(double));

	for(sz_long i=0;i<N;i++) d_inv_times_lambda[i] =lambda/(double)d[i];

	double* P_temp =(double*) malloc(K*N*sizeof(double));

	for(sz_long i=0;i<N;i++){
		for(sz_med j=0;j<K;j++)
			P_temp[j*N+i]=d_inv_times_lambda[i]*P_dif[j*N+i];
	}

	for(sz_med i=0;i<L;i++){
		for(sz_med j=0;j<K;j++)
			P_temp[j*N + L_ind[i] ]+= P[j*N+L_ind[i]]/(double)d[L_ind[i]];
	}

	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, (int)K, (int)K, (int)N, 1.0f, P, (int)N, P_temp, (int)N, 0.0f, A, (int)K);

	free(d_inv_times_lambda);
	free(P_temp);
	return A;
}

static double* get_coef_b(sz_long N, sz_med K, sz_med L ,sz_med num_pos , sz_long* d , double* P , sz_long* L_c ){

	double two_over_L = -2.0f/(double) L;

        double*	b = (double*) malloc(K*sizeof(double));

	for(sz_med i=0;i<K;i++) b[i]=0.0f;

	for(sz_med i=0;i<K;i++){
		for(sz_med j=0;j<num_pos;j++)
			b[i]+=P[i*N + L_c[j]]/(double)d[L_c[j]];
		b[i]*= two_over_L;
	}

	return b;
}


/*
Copyright 2017 InitialDLab

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#pragma once

//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>
#include <random>
#include <cassert>

inline
size_t
next_fanout(size_t size, size_t min_fanout, size_t max_fanout)
{
    if(size >= min_fanout + max_fanout)
        return max_fanout;

    if(size < max_fanout)
    {
        return size;
    }

    return size - min_fanout;
}

inline
size_t
calc_node_count(size_t size, size_t min_fanout, size_t max_fanout)
{
    return (size % max_fanout)
        ? (size / max_fanout + 1)
        : (size / max_fanout)
        ;
}

template<typename Point>
void dump_point(std::ostream & out, Point const& point)
{
    out << boost::geometry::wkt(point);
}

template<typename Box>
void dump_box(std::ostream & out, Box const& box)
{
    out << boost::geometry::wkt(box.min_corner())
        << ' ' << boost::geometry::wkt(box.max_corner());
}

/*
template<typename RNG>
size_t alternate_binomial(size_t n, double p, RNG & rng)
{
    if(p > 0.5)
        return n - alternate_binomial(n, 1.0-p, rng);

    std::uniform_real_distribution<float> coin_dist(0,1);
    double log_q = std::log(1.0 - p);
    size_t x = 0;
    double sum = 0;
    for(;;) {
        sum += std::log(coin_dist(rng)) / (n - x);
        if(sum < log_q)
            return x;
        ++x;
    }
}
*/

template<typename RNG>
inline
size_t
next_sample_size(size_t total_sample_size, size_t cur_subtree_size, size_t total_subtree_size, RNG & rng)
{
    if(cur_subtree_size == 0) return 0;
    if(cur_subtree_size == total_sample_size) return total_sample_size;
    if(total_sample_size < 10)
    {
        size_t s = 0;
        std::uniform_real_distribution<float> dist(0,1);
        float prob = ((float)cur_subtree_size) / total_sample_size;
        for(size_t i = 0; i < total_sample_size; ++i)
            if(dist(rng) < prob)
                ++s;
        return s;
    }

    //std::binomial_distribution<int> distribution(((double)cur_subtree_size) / total_subtree_size, total_sample_size);
    std::binomial_distribution<int> distribution(total_sample_size, ((double)cur_subtree_size) / total_subtree_size);
    return distribution(rng);

    //static gsl_rng *_gsl_rng = nullptr;
    //if(!_gsl_rng){ _gsl_rng = gsl_rng_alloc (gsl_rng_taus); }
    //return gsl_ran_binomial(_gsl_rng, ((double)cur_subtree_size)/total_subtree_size, total_sample_size);

/*
    return alternate_binomial(
        total_sample_size,
        ((double)cur_subtree_size) / total_subtree_size,
        rng
    );
*/
/*
    return std::binomial_distribution<size_t>(
        total_sample_size,
        ((double)cur_subtree_size) / total_subtree_size
    )(rng);
*/
}

template<typename RNG>
inline
void
next_sample_size_bulk(int trials, const std::vector<int64_t> &prefix_weights, std::vector<int> &output_counts, RNG & rng)
{
    assert(prefix_weights.size() == output_counts.size());

    // special case
    if (prefix_weights.size() == 1)
    {
        output_counts[0] = trials;
        return;
    }

    int64_t accum = 0;
    for (int i = 0; i < prefix_weights.size(); ++i)
    {
        output_counts[i] = next_sample_size(trials, prefix_weights[i] - accum, prefix_weights.back(), rng);
        accum = prefix_weights[i];
    }

    //std::vector<int64_t> each_trial;
    //each_trial.resize(trials);

    //std::uniform_int_distribution<int64_t> dist(0, prefix_weights.back());

    //std::generate(each_trial.begin(), each_trial.end(), [&rng, &dist]() {return dist(rng);});
    //std::sort(each_trial.begin(), each_trial.end());
    //auto last_max = each_trial.begin();
    //auto output_itr = output_counts.begin();
    //for (auto weight : prefix_weights)
    //{
    //    auto next_max = std::lower_bound(last_max, each_trial.end(), weight);
    //    *output_itr = std::distance(last_max, next_max);
    //    ++output_itr;
    //    last_max = next_max;
    //}

    //if (last_max != each_trial.end())
    //    output_counts.back() += std::distance(last_max, each_trial.end());

    //for (int64_t samples_inserted = 0; samples_inserted < trials; ++samples_inserted)
    //{
    //    int64_t r_val = dist(rng);
    //    auto i_itr = std::lower_bound(prefix_weights.begin(), prefix_weights.end(), r_val);
    //    int index = i_itr - prefix_weights.begin();
    //    ++output_counts[index];
    //}

    //std::uniform_int_distribution<int64_t> dist(0, prefix_weights.back());
    //for (int64_t samples_inserted = 0; samples_inserted < trials; ++samples_inserted)
    //{
    //    int64_t r_val = dist(rng);
    //    auto i_itr = std::lower_bound(prefix_weights.begin(), prefix_weights.end(), r_val);
    //    int index = i_itr - prefix_weights.begin();
    //    ++output_counts[index];
    //}
}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C header for functions manipulating waveforms.
 *
 */

#ifndef _WAVEFORM_H
#define _WAVEFORM_H

#define _XOPEN_SOURCE 500

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

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

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

#include "constants.h"
#include "struct.h"
#include "EOBNRv2HMROM.h"

#if defined(__cplusplus)
extern "C" {
#define complex _Complex
#elif 0
} /* so that editors will match preceding brace */
#endif

/* NOTE: uses the list of modes of EOBNRv2HMROM (listmode), to be extended when more waveform models are added */

/***************** Function estimating frequency corresponding to a given time to coalescence ****************/

/* Functions computing relations between chirp mass, eta, m1, m2*/
double Mchirpofm1m2(
  const double m1,   /* Mass 1 */
  const double m2);  /* Mass 2 */
double etaofm1m2(
  const double m1,   /* Mass 1 */
  const double m2);  /* Mass 2 */
double m1ofMchirpeta(
  const double Mchirp,   /* Chirp mass */
  const double eta);     /* Symmetric mass ratio */
double m2ofMchirpeta(
  const double Mchirp,   /* Chirp mass */
  const double eta);     /* Symmetric mass ratio */

/* Newtonian estimate of the relation Mf(deltat/M) (for the 22 mode) - gives the starting geometric frequency for a given mass ratio and a given geometric duration of the observations */
double NewtonianfoftGeom(const double q, const double t); /* t here is t/M */
/* Newtonian estimate of the relation f(deltat) (for the 22 mode) - gives the starting frequency in Hz for a given mass ratio and a given geometric duration of the observations */
double Newtonianfoft(const double m1, const double m2, const double t); /* t here is in years, m1-m2 in solar masses */
/* Newtonian estimate of the relation f(deltat) (for the 22 mode freq) - gives the starting geometric frequency for a given time to merger and chirp mass - output in Hz */
double Newtonianfoftchirp(
  const double mchirp,                 /* Chirp mass (solar masses) */
  const double t);                     /* Time in years */
/* Newtonian estimate of the relation deltat(f) (for the 22 mode freq) - gives the time to merger from a starting frequency for a given chirp mass - output in years */
double Newtoniantoffchirp(
  const double mchirp,                 /* Chirp mass (solar masses) */
  const double t);                     /* Freq in Hz */

/***************** Function estimating time to coalescence and min/max frequency ****************/

/* Functions reading from a list of modes the minimal and maximal frequencies */
double ListmodesCAmpPhaseFrequencySeries_maxf(ListmodesCAmpPhaseFrequencySeries* listhlm);
double ListmodesCAmpPhaseFrequencySeries_minf(ListmodesCAmpPhaseFrequencySeries* listhlm);

/* Function estimating initial time from Psi22, according to tf_SPA = -1/(2pi)dPsi/df */
double EstimateInitialTime(ListmodesCAmpPhaseFrequencySeries* listhlm, double fLow);

/***************** Functions to manipulate ReImFrequencySeries structure ****************/

void ReImFrequencySeries_AddCAmpPhaseFrequencySeries(
  struct tagReImFrequencySeries* freqseriesReIm,              /* Output Re/Im frequency series */
  struct tagCAmpPhaseFrequencySeries* freqseriesCAmpPhase,    /* Input CAmp/Phase frequency series, to be interpolated and added to the output */
  double fLow,                                                /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                               /* Maximal frequency - set to 0 to ignore */
  double fstartobsmode);                                      /* Starting frequency in case of limited duration of observations- assumed to have been scaled with the proper factor m/2 for this mode - set to 0 to ignore */
/* Function evaluating a ReImFrequencySeries by interpolating wach mode of a ListmodesCAmpPhaseFrequencySeries and summing them, given a set of frequencies */
void ReImFrequencySeries_SumListmodesCAmpPhaseFrequencySeries(
  struct tagReImFrequencySeries* freqseriesReIm,                    /* Output Re/Im frequency series - already initialized */
  struct tagListmodesCAmpPhaseFrequencySeries* listmodesCAmpPhase,  /* Input CAmp/Phase frequency series, to be interpolated */
  gsl_vector* freq,                                                 /* Input set of frequencies on which evaluating */
  double fLow,                                                      /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                                     /* Maximal frequency - set to 0 to ignore */
  double fstartobs);                                                /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
/* Helper function to add a mode to hplus, hcross in Fourier domain
 * - copies the function XLALSimAddMode, which was done only for TD structures */
int FDAddMode(
  ReImFrequencySeries* hptilde,       /* Output: frequency series for hplus */
  ReImFrequencySeries* hctilde,       /* Output: frequency series for hcross */
  ReImFrequencySeries* hlmtilde,      /* Input: frequency series for the mode hlm */
  double theta,                       /* First angle for position in the sky of observer */
  double phi,                         /* Second angle for position in the sky of observer  */
  int l,                              /* First mode number l */
  int m,                              /* Second mode number m */
  int sym);                           /* If 1, assume planar symmetry and add also mode l,-m. Do not if set to 0. */
/* Function evaluating the FD frequency series for hplus, hcross from the modes hlm */
int GeneratehphcFDReImFrequencySeries(
  ReImFrequencySeries** hptilde,                 /* Output: frequency series for hplus */
  ReImFrequencySeries** hctilde,                 /* Output: frequency series for hcross */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: frequency series for the mode hlm  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt,                                      /* Number of points of output - if 0, determined from deltaF and maximal frequency in input */
  int nbmode,                                    /* Number of modes to add */
  double theta,                                  /* First angle for position in the sky of observer */
  double phi,                                    /* Second angle for position in the sky of observer */
  int sym);                                      /* If 1, assume planar symmetry and add also mode l,-m. Do not if set to 0. */
/* Function evaluating the FD frequency series by summing mode contributions from each hlm */
int GenerateFDReImFrequencySeries(
  ReImFrequencySeries** freqseries,              /* Output: frequency series */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: FD modes hlm in the form AmpReal/AmpIm/Phase  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt);                                     /* Number of points of output - if 0, determined from deltaf and maximal frequency in input */
/* Function evaluating the FD frequency series for a single mode contribution (l,m) */
int GenerateFDReImFrequencySeriesSingleMode(
  ReImFrequencySeries** freqseries,              /* Output: frequency series */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: FD modes hlm in the form AmpReal/AmpIm/Phase  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt,                                      /* Number of points of output - if 0, determined from deltaf and maximal frequency in input */
  int l,                                         /* Mode index l */
  int m);                                        /* Mode index m */
/* Function to restrict a frequency series (typically output of a FFT) to a given frequency range */
int RestrictFDReImFrequencySeries(
  ReImFrequencySeries** freqseriesout,           /* Output: truncated frequency series */
  ReImFrequencySeries* freqseriesin,             /* Input: frequency series */
  double fLow,                                   /* Minimal frequency */
  double fHigh);                                 /* Maximal frequency */
/* Function for getting a phase mod 2pi (between -pi and pi) */
double mod2pi(double phase);
/* Function for getting a phase mod pi (between 0 and pi, e.g. polarization) */
double modpi(double phase);
/* Function to unwrap the phase mod 2pi  - acts directly on the gsl_vector representing the phase */
int UnwrapPhase(
  gsl_vector*  phaseout,   /* Output: unwrapped phase vector - already allocated */
  gsl_vector*  phasein);   /* Input: phase vector */
/* Function to convert a time series from Re/Im form to Amp/Phase form - unwrapping the phase */
int ReImTimeSeries_ToAmpPhase(
  AmpPhaseTimeSeries** timeseriesout,             /* Output: Amp/Phase time series */
  ReImTimeSeries* timeseriesin);                  /* Input: Re/Im time series */
/* Function to convert a time series from Amp/Phase form to Re/Im form */
int AmpPhaseTimeSeries_ToReIm(
  ReImTimeSeries** timeseriesout,                 /* Output: Re/Im time series */
  AmpPhaseTimeSeries* timeseriesin);              /* Input: Amp/Phase time series */
/* Function to compute a linear resampling at high frequencies to enforce a maximal deltaf */
/* NOTE: Assumes input frequencies are logarithmic (except maybe first interval) to evaluate when to resample */
int SetMaxdeltafResampledFrequencies(
  gsl_vector** freqr,              /* Output: resampled frequencies */
  gsl_vector* freq,                /* Input: original frequencies */
  const double maxf,               /* Input: maximal frequency - set to 0. to ignore */
  const double deltaf);            /* Input: maximal deltaf aimed for - 0.002Hz appropriate for LISA */
/* Function to compute a linear-in-time resampling at low frequencies to enforce a maximal deltat */
int SetMaxdeltatResampledFrequencies(
  gsl_vector** freqr,              /* Output: resampled frequencies */
  gsl_vector* freq,                /* Input: original frequencies */
  const double deltat,             /* Input: maximal deltat aimed for - fraction of a year, 1/24 (half month) appropriate for 1e-4 interpolation errors */
  const double mchirp,             /* Input: chirp mass, used for approximate t-f correspondence */
  const int m);                    /* Input: chirp mass, used for approximate t-f correspondence */
/* Function to resample a CAmp/Phase frequency series on the specified frequencies */
int CAmpPhaseFrequencySeries_Resample(
  CAmpPhaseFrequencySeries** freqseriesout,         /* Output: CAmp/Phase freq series */
  CAmpPhaseFrequencySeries* freqseriesin,           /* Input: CAmp/Phase freq series */
  gsl_vector* freqr);                                 /* Input: freq vector to resample on */

/***************** Spin weighted spherical harmonics ****************/

/* Additional function reproducing XLALSpinWeightedSphericalHarmonic */
double complex SpinWeightedSphericalHarmonic(double theta, double phi, int s, int l, int m); /* Currently only supports s=-2, l=2,3,4,5 modes */

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

#endif /* _WAVEFORM_H */