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

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_hyperg.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_integration.h>
#include <time.h>

#define ADIABATIC_CUTOFF 1

#define INTEGRATION_POINTS 81
#define INTEGRAL_FUDGE_FACTOR 1.0125
#define INTEGRATION_SECTIONS 8
#define CUTOFF_SCALE 1E3
#define min(x,y) (x < y? x : y)
#define sign(x) (x >= 0? 1.0 : -1.0)
#define dot_macro(A, B) ((A).x * (B).x + (A).y * (B).y + (A).z * (B).z)
#define mag_sq(A) dot_macro(A, A)
#define mag(A) sqrt(dot_macro(A, A))
#define assign_vec(A, x_val, y_val, z_val) {(A).x = x_val; (A).y = y_val; (A).z = z_val;}
#define assign_vec_diff(d, A, B) {(d).x = (A).x - (B).x; (d).y = (A).y - (B).y; (d).z = (A).z - (B).z;}
#define make_unit(A) {double len = mag(A); assign_vec(A, (A.x)/len, (A.y)/len, (A.z)/len);}
#define rho_profile(x, a, b) (pow(x, 2 - a)*pow((1 + x), a - b))

gsl_rng *RNG;
gsl_integration_workspace * w;

const int default_log_num_trials = 6;
int num_trials, num_points = 100;

const double pi = 3.14159265;
double crit_density = 1.3211775E-7,
	f = 0.1,
	g = 1E-4,
	p = 1.9,
	G = 0.0045,
	k = 2,
	M_prim = 1E12,
	T_age = 1E4;

double m_max, m_min, expected_num, R_core_prim, R_max_prim, c_prim;

double MaxRadius(double M)
{
    return pow(3.0*M/(4.0 * pi * 200.0 * crit_density), 1.0/3.0);
}

typedef struct
{
	double x, y, z;
} vector;

typedef struct
{
	double m, s, max_val;
}data_cell;

void update_cell(data_cell *ptr, double new_data, int k)
{
	double old_m = ptr->m;
	ptr->m += (new_data - old_m)/(double) k;
	ptr->s += (new_data - old_m)*(new_data - ptr->m);
	if(ptr->max_val < new_data) ptr->max_val = new_data;
}

void reset_cell(data_cell *ptr)
{
	ptr->m = 0;
	ptr->s = 0;
	ptr->max_val = 0;
}

typedef struct
{
	double R, theta, phi;
	double cos_theta;
	double M, r_core, r_max, alpha, beta, c, normalization;
	double integs[INTEGRATION_SECTIONS];
	vector v, position;
} halo;

double Power(double x, int y)
{
	if(y > 0)
	{
		double result = 1;
		while(y > 0)
		{
			if(y % 2 == 0)
			{
				x = x*x;
				y = y/2;
			}
			else
			{
				result = x*result;
				y--;
			}
		}
		return result;
	}
	else if(y < 0)
		return 1/Power(x, -y);
	else
		return 1;
}

void print_vector(vector *A)
{
	printf("(%.3f, %.3f, %.3f)\n", A->x, A->y, A->z);
}

double perp_mag_sq(vector *A, vector *u)
{
	double dot_prod = dot_macro(*A, *u);
	return mag_sq(*A) - dot_prod*dot_prod/mag_sq(*u);
}

double MFreeNFW(double r)
{
    if(r < R_max_prim)
        return M_prim*(log(1 + r/R_core_prim)-r/(r + R_core_prim))/(log(1 + c_prim) - c_prim/( 1 + c_prim));
    else
        return M_prim;
}

double DFreeNFW(double r)
{
    if(r < R_max_prim)
        return 200.0/3.0 * crit_density / (log(1 + c_prim) - c_prim/(1 + c_prim))*pow(c_prim, 3) * 1.0/(r/R_core_prim*pow(1+r/R_core_prim, 2.0));
    else
        return 0.0;
}

double PhiFreeNFW(double r)
{
    if(r < R_max_prim)
        return -M_prim*G*((R_max_prim/r * log(1 + r/R_core_prim) - log(1 + c_prim))/(log(1 + c_prim) - c_prim/(1 + c_prim)) + 1)/R_max_prim;
    else
        return -M_prim*G/r;
}

double TidalRadius(double M, double R)
{
	return R*pow(M/(2*MFreeNFW(R)), 1.0/3.0);
}

double NFW_func(double x, double r)
{
    return (log(1.0 + x) - x/(1.0 + x))/(log(1.0 + c_prim) - c_prim/(1.0 + c_prim)) - r;
}

double df (double x)
{
    return x/((1.0 + x)*(1.0 + x)) * 1.0/(log(1.0 + c_prim) - c_prim/(1.0 + c_prim));
}

double newton(double r)
{
    int itr, maxmitr = 100;
    double h, x0 = 0.1, x1, allerr = pow(10, -6);

    for (itr = 0; itr < maxmitr; itr++)
    {
        h = NFW_func(x0, r)/df(x0);
        x1 = x0 - h;

        if (fabs(h) < allerr)
        {
            return x1;
        }
        x0 = x1;
    }
    return x1;
}

double rho_profile_func(double x, void *params)
{
	double *param_ptr = (double *)params;
	return pow(x, 2 - param_ptr[0])*pow((1 + x), param_ptr[0] - param_ptr[1]);
}

double integ_profile(double alpha, double beta, double r)
{
	/*if(alpha > 3)
		return 1;
	double dist = 0, step = r/INTEGRATION_POINTS, sum = 0;
	int i;

	for(i = 0; i < INTEGRATION_POINTS; i++)
	{
		if(i == 0 && alpha > 2 && alpha < 3)
			sum += 8.0/(3.0*step) * 1/(3.0 - alpha) * pow(step, 3 - alpha);
		else if(i == 0 || i == INTEGRATION_POINTS - 1)
			sum += rho_profile(dist, alpha, beta);
		else if(i%3 == 0)
			sum += 2*rho_profile(dist, alpha, beta);
		else
			sum += 3*rho_profile(dist, alpha, beta);
		dist += step;
	}

	return 3.0*step/8.0*sum * INTEGRAL_FUDGE_FACTOR;*/
	//printf("int %f\n", 3*step/8*sum);

	double result, error;
	double arr[2];
	arr[0] = alpha;
	arr[1] = beta;

	gsl_function F;
	F.function = &rho_profile_func;
  	F.params = arr;

  	gsl_integration_qags (&F, 0, r, 0, 1e-7, 1000, w, &result, &error);
	if(error/result > 1e-7)printf("LARGE INTEG ERROR WARNING %f for result %f", error, result);

	//printf("alpha: %.3f beta: %.3f r: %.3f err %f\n", alpha, beta, r, (result - quick_result)/result);

	return result;
}

double worst_thing_ever(int lr, double a, double b, double r)
{
	switch(lr)
	{
		case -2: return pow(2,-2 + b)*pow(3,-4 + a - b)*(-20.25 - ((8*a*a*a + 12*a*a*(-2 + b) + b*(38 + (-15 + b)*b) +
					2*a*(8 + 3*(-7 + b)*b))*(-0.0078125 + Power(1 - 2*r,4)/8.))/2. +
					81*r - 27*(-6 + 2*a + b)*(0.1875 + (-1 + r)*r) + (3*(4*a*a + 4*a*(-4 + b) + (-9 + b)*(-2 + b))*(-1 + 4*r)*(7 + 4*r*(-5 + 4*r)))/32.);
		break;

		case -1: return (pow(2,-4 + a - b)*(-24 + 48*r - ((-2 + a + b)*(a*a + (-7 + b)*b + a*(-1 + 2*b))*(-3 + 2*r)*(-1 + 2*r)*(5 + 4*(-2 + r)*r))/64. -
					3*(-4 + a + b)*(3 - 8*r + 4*r*r) + 6*(8 + a*a - 7*b + b*b + a*(-5 + 2*b))*(-0.2916666666666667 + (r*(3 + (-3 + r)*r))/3.)))/3.0;
		break;

		case 0: return pow(2,-4 - a)*pow(3,-4 + a - b)*(-(a*a*a*(-1 + Power(-2 + r,4))) -
					16*(27 + (3*b)/4. + (b*b*b*(-1 + Power(-2 + r,4)))/2. + 40*b*r - 48*b*r*r - 27*r*r*r + 4*b*r*r*r +
					(13*b*r*r*r*r)/4. - 3*b*b*(1 + Power(-2 + r,3)*r)) - 3*a*a*(-1 + r)*(-41 + 2*b*(-3 + r)*(5 + (-4 + r)*r) - r*(-23 + r + r*r)) -
					2*a*(-1 + r)*(-75 + 6*b*b*(-3 + r)*(5 + (-4 + r)*r) + r*(-187 + r*(77 + r)) - 3*b*(37 + r*(5 + r*(-19 + 5*r)))));
		break;

		case 1: return  -(pow(2,-5 - 2*a)*pow(5,-3 + a - b)*(-2 + r)*(a*a*a*(-120 + 68*r - 14*r*r + r*r*r) +
					a*a*(-1880 + 596*r - 38*r*r - 3*r*r*r + 12*b*(-120 + 68*r - 14*r*r + r*r*r)) +
					2*a*(-2200 - 1932*r + 426*r*r + r*r*r + b*(-3920 + 344*r + 268*r*r - 42*r*r*r) + 24*b*b*(-120 + 68*r - 14*r*r + r*r*r)) +
					8*(-500*(4 + 2*r + r*r) + 8*b*b*b*(-120 + 68*r - 14*r*r + r*r*r) - 4*b*b*(40 + 212*r - 86*r*r + 9*r*r*r) +
					b*(-200 - 1892*r + 206*r*r + 31*r*r*r))))/3.0;
		break;

		case 2: return  pow(2,-4 - 3*a)*pow(3,-7 + 2*a - 2*b)*(-8957952 - ((-2 + a)*(-1 + a)*a + 8*(182 + 3*(-11 + a)*a)*b + 192*(-10 + a)*b*b + 512*b*b*b)*
					(-64 + Power(-8 + r,4)/4.) + 2239488*r - 15552*(-18 + a + 8*b)*(48 - 16*r + r*r) +
					72*(162 + a*a - 224*b + 64*b*b + a*(-19 + 16*b))*(-448 + r*(192 + (-24 + r)*r)));

		break;

		case 3: return (pow(2,-5 - 4*a)*pow(17,-3 + a - b)*(-965935104 - (a*a*a + a*a*(-3 + 48*b) + 32*b*(307 - 432*b + 128*b*b) + a*(2 - 912*b + 768*b*b))*
					(-1024 + Power(-16 + r,4)/4.) + 120741888*r - 221952*(-34 + a + 16*b)*(192 - 32*r + r*r) +
					272*(578 + a*a - 832*b + 256*b*b + a*(-35 + 32*b))*(-3584 + r*(768 + (-48 + r)*r))))/3.0;

		break;

		case 4: return pow(2,-6 - 5*a)*pow(33,-4 + a - b)*11*(-113048027136 -
				(a*a*a + a*a*(-3 + 96*b) + 64*b*(1123 - 1632*b + 512*b*b) + a*(2 - 3360*b + 3072*b*b))*(-16384 + Power(-32 + r,4)/4.) + 7065501696*r -
				3345408*(-66 + a + 32*b)*(768 - 64*r + r*r) + 1056*(2178 + a*a - 3200*b + 1024*b*b + a*(-67 + 64*b))*(-28672 + r*(3072 + (-96 + r)*r)));

		break;

		case 5: return (pow(2,-7 - 6*a)*pow(65,-3 + a - b)*
     			(-13822328832000 + 431947776000*r - (((-2 + a)*(-1 + a)*a + 64*(8582 + 3*(-67 + a)*a)*b +
            	12288*(-66 + a)*b*b + 262144*b*b*b)*(-96 + r)*(-32 + r)*
          		(5120 + (-128 + r)*r))/4. - 51916800*(-130 + a + 64*b)*(3072 - 128*r + r*r) + 4160*(8450 + a*a - 12544*b + 4096*b*b + a*(-131 + 128*b))*
        		(-229376 + r*(12288 + (-192 + r)*r))))/3.0;
		break;
	};

	return 1;
}

double sec_integ_profile(halo *ptr, double r)
{
	double a = ptr->alpha, b = ptr->beta;
	int lr = floor(log2(r)); //MAKE BETTER!
	if(lr < -2) return pow(r, 3-a)*(1/(3-a) + r*(a-b)/(4-a) + (a*a - a + b - 2*a*b + b*b)/(5-a)*r*r/2.0);
	if(lr >= INTEGRATION_SECTIONS - 2) return integ_profile(a, b, r); //MAKE BETTER
	return ptr->integs[lr + 2] + worst_thing_ever(lr, a, b, r);
}

double get_M()
{
	double r = gsl_rng_uniform(RNG);
	return M_prim*pow(pow(m_max/M_prim, 1.0-p)*r + pow(m_min/M_prim, 1.0-p)*(1.0-r), 1.0/(1.0-p));
}

double c_bar(double M)
{
	return pow(10.0, 1.0 - 0.1 * (log10(M) - 12));
}

void set_shape(halo *ptr, double M, double R)
{
	ptr->r_max = pow(3*M/(4 * pi * 200.0 * crit_density), 1.0/3.0);
	ptr->alpha = gsl_ran_gaussian_ziggurat(RNG, 0.2) + 1.25; //suggested by J. Ostriker
	ptr->beta = 3; //probalby need to change
	ptr->c = gsl_ran_lognormal(RNG, log(c_bar(M)), 0.25); //Ludlow el. al. (2013) and  Frank van den Bosch
	ptr->r_core = ptr->r_max/ptr->c;
	int i;
	double r = 1.0/4.0, a = ptr->alpha, b = ptr->beta;
	ptr->integs[0] = pow(r, 3-a)*(1/(3-a) + r*(a-b)/(4-a) + (a*a - a + b - 2*a*b + b*b)/(5-a)*r*r/2.0);
	r *= 2;
	for(i = 0; i < INTEGRATION_SECTIONS - 1; i++)
	{
		ptr->integs[i + 1] = ptr->integs[i]  + worst_thing_ever(i - 2, ptr->alpha, ptr->beta, r);
		r *= 2;
	}
	ptr->normalization = sec_integ_profile(ptr, ptr->c);

}

void set_velocity(halo *ptr, double R)
{
	double v_sigma = sqrt(1.0/3.0)*sqrt(-PhiFreeNFW(R));

	ptr->v.x = gsl_ran_gaussian_ziggurat(RNG, v_sigma);
	ptr->v.y = gsl_ran_gaussian_ziggurat(RNG, v_sigma);
	ptr->v.z = gsl_ran_gaussian_ziggurat(RNG, v_sigma);
}

double enclosed_mass(halo *ptr, double r)
{
	double x = r/(ptr->r_core);
	if(x > ptr->c)
		return ptr->M;
	else
	{
	/*	double sec = sec_integ_profile(ptr, x), integ = integ_profile(ptr->alpha, ptr->beta, x);
		double lerr = log(fabs(sec - integ));
		if(lerr > -4)
		{
			printf("alpha: %f beta: %f r: %f \n", ptr->alpha, ptr->beta, x);
			printf("lerr: %f sec: %f real: %f r: \n", lerr, sec, integ);
		}*/
		return ptr->M * sec_integ_profile(ptr, x)/ptr->normalization;
	}
}

void truncate(halo *ptr, double R)
{
	double l2 = R*R*perp_mag_sq(&(ptr->v), &(ptr->position));
	double r0 = l2/(M_prim * G);
	double E = 0.5*mag_sq(ptr->v) + PhiFreeNFW(R);
	double ecc =  sqrt(1.0 + 2.0*E*l2/(M_prim * G * M_prim * G));
	double R_min = min(fabs(r0/(1.0 + ecc)), fabs(r0/(1.0 - ecc)));

	/*printf("velocity: ");
	print_vector(ptr->v);
	printf("position ");
	print_vector(halo_pos(ptr));
	printf("R = %.3f theta = %.3f phi = %.3f\n l2 = %.3f e = %.3f \n", ptr->R, ptr->theta, ptr->phi, l2, ecc);*/

	if(E > 0 || R_min < 0.1)
	{
		ptr->M = 0;
	}
	else
	{
		double Rt = TidalRadius(ptr->M, R_min);
		double R_max = min(ptr->r_max, Rt);
		double new_M = enclosed_mass(ptr, R_max);

		ptr->r_max = R_max;
		ptr->c = R_max/ptr->r_core;
		ptr->normalization *= (new_M/ptr->M);
		ptr->M = new_M;
	}
}

halo *make_halo(halo *ptr)
{
	ptr->R = newton(gsl_rng_uniform(RNG)) * R_core_prim;
	ptr->cos_theta = 2*gsl_rng_uniform(RNG) - 1;
	ptr->theta = acos(ptr->cos_theta);
	ptr->phi = 2*pi*gsl_rng_uniform(RNG);
	assign_vec(ptr->position, ptr->R * sin(ptr->theta) * cos(ptr->phi), ptr->R * sin(ptr->theta) * sin(ptr->phi), ptr->R * ptr->cos_theta);

	ptr->M = get_M();
	set_shape(ptr, ptr->M, ptr->R);
	set_velocity(ptr, ptr->R);
	truncate(ptr, ptr->R);

	return ptr;
}

void print_halo_basic(halo *ptr)
{
	printf("\n R = %3f \n theta = %3f \n M = %3f \n", ptr->R, ptr->theta, ptr->M);
}

double Fluc(halo *halos, int num_halos, double D)
{
	double sum = 0;
	int i;
	vector my_pos = {0, 0, D}, diff;
	double natural_fluc = 2 * pi* pow(D, 3.0/2.0)/sqrt(MFreeNFW(D) * G) * 1/(PhiFreeNFW(D));
	for(i = 0; i < num_halos; i++)
	{
		double R = halos[i].R;
		double r = sqrt(R*R + D*D - 2.0*R*D*halos[i].cos_theta);

		assign_vec_diff(diff, halos[i].position, my_pos);
		make_unit(diff)
		double v_r = dot_macro(halos[i].v, diff);

		double produced_fluc = ((r > CUTOFF_SCALE) ? (enclosed_mass(halos + i, r) * G /(r*r) * v_r) : 0);
		//sum += produced_fluc; //KILLEM
		// kill heating which is adiabtic

		if(produced_fluc/natural_fluc > 1 || ! ADIABATIC_CUTOFF)
			sum += produced_fluc;
		/*printf("%.3f\n", D);
		printf("velocity: ");
		print_vector(halos[i]->v);
		printf("position ");
		print_vector(halo_pos(halos[i]));
		printf("R = %.3f theta = %.3f phi = %.3f\n v_r = %.3f \n", halos[i]->R, halos[i]->theta, halos[i]->phi, v_r);*/
	}
	return sum*sum;
}

double Fluc_mass_range(halo *halos, int num_halos, double D, double min_mass, double max_mass)
{
	double sum = 0;
	int i;
	vector my_pos = {0, 0, D}, diff;
	double natural_fluc = 2 * pi* pow(D, 3.0/2.0)/sqrt(MFreeNFW(D) * G) * 1/(PhiFreeNFW(D));
	for(i = 0; i < num_halos; i++)
	{
		double R = halos[i].R;
		double r = sqrt(R*R + D*D - 2.0*R*D*halos[i].cos_theta);
		if(halos[i].M < max_mass && halos[i].M > min_mass)
		{
			assign_vec_diff(diff, halos[i].position, my_pos);
			make_unit(diff)
			double v_r = dot_macro(halos[i].v, diff);

			double produced_fluc = ((r > CUTOFF_SCALE) ? (enclosed_mass(halos + i, r) * G /(r*r) * v_r) : 0);
			//sum += produced_fluc; //KILLEM
			// kill heating which is adiabtic

			if(produced_fluc/natural_fluc > 1 || ! ADIABATIC_CUTOFF)
				sum += produced_fluc;
			/*printf("%.3f\n", D);
			printf("velocity: ");
			print_vector(halos[i]->v);
			printf("position ");
			print_vector(halo_pos(halos[i]));
			printf("R = %.3f theta = %.3f phi = %.3f\n v_r = %.3f \n", halos[i]->R, halos[i]->theta, halos[i]->phi, v_r);*/
		}
	}
	return sum*sum;
}

double H_Density(halo *halos, int num_halos, double D, double dD)
{
	double sum = 0;
	int i;
	for(i = 0; i < num_halos; i++)
	{
		if(halos[i].R < D && halos[i].R > D - dD)
			sum += halos[i].M;
	}
	return sum/(4.0*pi*D*D*dD);
}

int print_out = 0;

void init(int argc, char **argv)
{
	const gsl_rng_type *T;
	gsl_rng_env_setup();
	T = gsl_rng_default;
	RNG = gsl_rng_alloc (T);
	w = gsl_integration_workspace_alloc (1000);

	if(argc >= 2)
		num_trials = (int)pow(10, atoi(argv[1]));
	else
		num_trials = (int)pow(10, default_log_num_trials);

	if(argc > 2)
		print_out = 1;

	R_max_prim = MaxRadius(M_prim);
	c_prim = c_bar(M_prim);
	R_core_prim = R_max_prim/c_prim;
	m_max = f*M_prim;
	m_min = g*M_prim;
	expected_num = (2.0-p)/(1.0-p)*f*(pow(m_max/M_prim, 1.0-p) - pow(m_min/M_prim, 1.0-p))
		/ (pow(m_max/M_prim, 2.0-p) - pow(m_min/M_prim, 2.0-p));
}

double std_err_mean(double sum_of_squares)
{
	return sqrt(sum_of_squares/((double) num_trials*(num_trials - 1)));
}

void sq_root_data(data_cell *data, int num)
{
	int i;
	for(i = 0; i < num; i++)
	{
		data[i].m = sqrt(data[i].m);
		data[i].s *= pow(1.0/(2.0*data[i].m), 2);
		data[i].max_val = sqrt(data[i].max_val);
	}
}

void print_to_file(char *name, double *Ds, data_cell *data)
{
	int j;
	char path[100], filename[50], snum[10], addendum[10];
	strcpy(path, "data_files/");
	strcpy(filename, name);
	strcpy(addendum, ".txt");
	snprintf(snum, 10, "%d_%d", (int)gsl_rng_default_seed, (int)log10(num_trials));
	strcat(path, filename);
	strcat(path, snum);
	strcat(path, addendum);

	FILE *f = NULL;

	if(!print_out)
		f = fopen(path, "w");

	if(!print_out && f != NULL)
	{
		for(j = 0; j < num_points; j++)
		{
			double sig = std_err_mean(data[j].s)/data[j].m/log(10);
			printf("%f, %f, %f, %f\n", data[j].m, data[j].s, sig, log10(data[j].m));
			if(data[j].m > 0)
				fprintf(f, "%f %f %f %f\n", log10(Ds[j]), log10(data[j].m), sig, log10(data[j].max_val));
		}
		fclose(f);
	}
	else
	{
		for(j = 0; j < num_points; j++)
		{
			double sig = std_err_mean(data[j].s)/data[j].m/log(10);
			printf("%f, %f, %f, %f\n", data[j].m, data[j].s, sig, log10(data[j].m));
			if(data[j].m > 0)
				printf("%f : %f +/- %f\n", log10(Ds[j]), log10(data[j].m), sig);
		}
	}
}

int main(int argc, char **argv)
{
	init(argc, argv);
	unsigned int max_allowed_halos = (int)(10*expected_num);

	double Ds[num_points];
	data_cell Flucs[num_points], Dens[num_points];
	halo *halolist = malloc(max_allowed_halos*sizeof(halo));

	double mass = 0, avg_mass = 0;
	int i,j;

	for(i = 0; i < num_points; i++)
	{
		Ds[i] = pow(10, 5.0*i/(double) num_points);
		reset_cell(&Flucs[i]);
		reset_cell(&Dens[i]);
	}
	int hist_num = 10;

	data_cell Hist_Flucs[hist_num];
	for(int i = 0; i < hist_num; i++)
		reset_cell(Hist_Flucs + i);

	for(i = 0; i < num_trials; i++)
	{
		if(i % 100 == 0)printf("%d out of %d \n", i/100, num_trials/100);
		unsigned int num_halos = gsl_ran_poisson(RNG, expected_num);
		if(num_halos > max_allowed_halos)
		{
			max_allowed_halos *= 2;
			free(halolist);
			halolist = malloc(max_allowed_halos*sizeof(halo));
			printf("ALLOCATING\n");
		}
		mass = 0;
		//printf("Expect: %f   Have: %lu \n", expected_num, num_halos);

		for(j = 0; j < num_halos; j++)
		{
			make_halo(halolist + j);
			//print_halo_basic(halolist + j);
			mass += halolist[j].M;

			if(mass != mass)
			{
				print_halo_basic(halolist + j);
				printf("%f %f %d %f\n", mass, halolist[j].M, j, halolist[j].alpha);
				return -1;
			}
		}

		for(j = 0; j < num_points; j++)
		{
			update_cell(Flucs + j, Fluc(halolist, num_halos, Ds[j]), i + 1);
			update_cell(Dens + j, (j == 0 ? H_Density(halolist, num_halos, Ds[j], Ds[j]) : H_Density(halolist, num_halos, Ds[j], Ds[j] - Ds[j-1])), i + 1);
		}
		//printf("Mass frac: %f \n", mass/M_prim);
		avg_mass += mass/num_trials;

		for(int j = 0; j < hist_num; j++)
			update_cell(Hist_Flucs + j, Fluc_mass_range(halolist, num_halos, 1e4, M_prim*g*pow(f/g, (double) j/ (double) hist_num), M_prim*g*pow(f/g, (double) (j + 1)/ (double) hist_num)), i + 1);

		/*for(j = 0; j < num_points; j++)
		{
			//printf("%f : %f\n", pow(10, 5.0*j/num_points), log10(Flucs[j]));
			//printf("%f : %f\n", pow(10, 5.0*j/num_points), log10(Dens[j]));
			//printf("%f : %f\n", pow(10, 5.0*j/num_points), Hist[j]);
		}*/

	}
	print_to_file("Density", Ds, Dens);
	sq_root_data(Flucs, num_points);
	print_to_file("Flucs", Ds, Flucs);

	printf("Mass frac: %f \n", avg_mass/M_prim);
	sq_root_data(Hist_Flucs, hist_num);
	for(int i = 0; i < hist_num; i++)
		printf("Mass: %f Fluc: %f SDEV: %f\n", M_prim*g*pow(f/g, (double) i/ (double) hist_num), Hist_Flucs[i].m, std_err_mean(Hist_Flucs[i].s));
	gsl_rng_free (RNG);
	gsl_integration_workspace_free (w);
	return 0;
}


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

// include files for optimized libraries
#if defined USE_ESSL
#include <essl.h>
#elif defined USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#elif defined USE_LAPACK
#include <cblas.h>
#include <lapacke.h>
#endif

// interface to f2c code

#include "f2c.h"
#include "mrrr.h"

static inline int PCA_ssytd2(char uplo, int n, real *a, int lda, real *d, real *e, real *tau)
{
    int info;
    ssytd2_(&uplo, &n, a, &lda, d, e, tau, &info, 1);
    return info;
}

static inline int PCA_sstemr(char jobz, char range, int n, real *d, real *e, real vl, real vu,
    int il, int iu, int *m, real *w, real *z, int ldz, int nzc, int *isuppz, int *tryrac,
    real *work, int lwork, int *iwork, int liwork)
{
    int info;
    pca_sstemr__(&jobz, &range, &n, d, e, &vl, &vu, &il, &iu, m, w, z, &ldz, &nzc, isuppz,
        tryrac, work, &lwork, iwork, &liwork, &info, 1, 1);
    return info;
}

static inline int PCA_sorm2l(char side, char trans, int m, int n, int k, real *a, int lda, real *tau, real *c, int ldc, real *work)
{
    int info;
    sorm2l_(&side, &trans, &m, &n, &k, a, &lda, tau, c, &ldc, work, &info, 1, 1);
    return info;
}

#ifndef NO_PULP
#include "utils.h"
#include "hwTrace.h"
#endif

#if 1

#define ALLOC(t, v, s) t v[s];
#define FREE(v)

#else

#define ALLOC(t, v, s) t *v = malloc(sizeof(t) * (s));
#define FREE(v) free(v);

#endif

// PCA main routine
// input is a column-major matrix with a row for each sample and a column for each variable
// output is a column-major matrix with a row for each sample and a column for each component

void PCA_mrrr(int samples, int variables, float *input, int components, float *output)
{
    int lwork = 18 * variables;
    int liwork = 10 * variables;
    ALLOC(real, A, variables * variables);
    ALLOC(real, T, samples * variables);
    ALLOC(real, d, variables);
    ALLOC(real, e, variables);
    ALLOC(real, tau, variables);
    ALLOC(int, isuppz, variables * 2);
    ALLOC(real, w, variables);
    ALLOC(real, Z, variables * variables);
    ALLOC(real, work, lwork);
    ALLOC(int, iwork, liwork);

    // pulp_trace_kernel_declare(0, "kernel 0");
    // pulp_trace_kernel_start(0, 1);

    // compute and subtract mean
    for (int j = 0; j < variables; j++) {
        real mean = 0.0;
        #pragma omp parallel for reduction(+:mean)
        for (int i = 0; i < samples; i++)
            mean += input[j * samples + i];
        mean /= samples;
        #pragma omp parallel for
        for (int i = 0; i < samples; i++)
            T[j * samples + i] = input[j * samples + i] - mean;
    }

    // compute A=T^T*T
    for (int j = 0; j < variables; j++)
        for (int i = 0; i <= j; i++) {
            real dot = 0;
            #pragma omp parallel for reduction(+:dot)
            for (int k = 0; k < samples; k++)
                dot += T[j * samples + k] * T[i * samples + k];
            A[i + j * variables] = dot;
        }

    // tridiagonalization
#if defined USE_MKL || USE_LAPACK
    int info = LAPACKE_ssytrd(LAPACK_COL_MAJOR, 'U', variables, A, variables, d, e, tau);
#else
    int info = PCA_ssytd2('U', variables, A, variables, d, e, tau);
#endif
    if (info != 0) {
        printf("Error in SSYTRD/SSYTD2: %i\n", info);
        abort();
    }

    // compute eigenvalues
    int il = variables - components + 1, iu = variables, m, tryrac = 1;
    real vl = 0.0, vu = 0.0;
    info = PCA_sstemr('V', 'I', variables, d, e, vl, vu, il, iu, &m, w, Z, variables, variables,
        isuppz, &tryrac, work, lwork, iwork, liwork);
    if (info != 0) {
        printf("Error in SSTEMR: %i\n", info);
        abort();
    }
    printf("%d: ", m);
    for (int i = 0; i < m; i++) printf("%d ", (int)w[i]); printf("\n");

    // compute eigenvectors
#if defined USE_MKL || USE_LAPACK
    info = LAPACKE_sormtr(LAPACK_COL_MAJOR, 'L', 'U', 'N', variables, m, A, variables, tau,
        Z, variables);
#else
    info = PCA_sorm2l('L', 'N', variables - 1, m, variables - 1, A + variables, variables, tau,
        Z, variables, work);
#endif
    if (info != 0) {
        printf("Error in SORMTR/SORM2L: %i\n", info);
        abort();
    }

#if defined USE_ESSL || USE_MKL || USE_LAPACK
    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, samples, components, variables,
        1.0, T, samples, Z, variables, 0.0, output, samples);
#else
    #pragma omp parallel for
    for (int i = 0; i < samples; i++)
        for (int j = 0; j < components; j++) {
            real t = 0;
            for (int k = 0; k < variables; k++)
                t += T[i + k * samples] * Z[j * variables + k];
            output[i + j * samples] = t;
        }
#endif

    FREE(T);
    FREE(A);
    FREE(e);
    FREE(d);
    FREE(tau);
    FREE(w);
    FREE(Z);
    FREE(isuppz);
    FREE(work);
    FREE(iwork);
    // pulp_trace_kernel_stop(0, 1);
}


// Alessandro Casalino
//
// Compile with (Mac with default homebrew gsl 2.6) gcc-9 -O2 quintessence_evolve.c -o quintessence_evolve.exe -L/usr/local/Cellar/gsl/2.6/lib -I/usr/local/Cellar/gsl/2.6/include -lgsl
// Run with ./quintessence_evolve.exe

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

// PHYSICAL PARAMETERS VALUES

// INITIAL CONDITIONS
// Initial value of the scale factor
double a_init = 1e-15;
// Final value of the scale factor
double a_end = 1.1;

// Values of fractional density for the cosmological matter today
double Omega_rad_0 = 8e-5;
double Omega_b_0 = 0.0486;
double Omega_Lambda_0 = 0.;//0.6911;
double Omega_cdm_0 = 0.2589;

// Physical constants for output conversions
double _G_ = 6.67428e-11;                 /**< Newton constant in m^3/Kg/s^2 */
double _MPc_over_m_ = 3.085677581282e22;  // Conversion factor from Mpc to m
double _Gyr_over_Mpc_ = 3.06601394e2;     // Conversion factor from Gyr to Mpc
double _c_ = 2.99792458e8;                // Speed of light in m/s
double _H0_ = 67.74;                      // H0 of LCDM in Km/s/Mpc

double fourpiG = 1.;

double T_CONF = 1.;

// COMPUTATION PARAMETERS VALUES

// Number of points used in the computation
int points = (int) 1e6;

// Raise this value to make the csv file smaller, but decreasing resolution
// The value inserted is the ratio between the number of values written in a full resolution file / decreased resolution file
int csv_resolution = 10;

double delta_bisection = 1e-2;
double mg_field_init_min = 1e-20;
double mg_field_init_max = 2e1; //1e16 max for LCDM (model 1)

// QUINTESSENCE PARAMETERS
// Initial value of the quintessence velocity (phi derivated with respect to tau)
double mg_field_p_bk_0 = 0.;

// TEST MODE
// Provides some informations on the terminal and the .csv file during the computation (1: on, others: off)
int TEST_MODE = 1;

// Definition of the POTENTIAL
// For a list of the models see above
double mg_pot(const double mg_field_bk) {

  return mg_field_bk * mg_field_bk / 2.;

}

double mg_pot_p(const double mg_field_bk) {

  return mg_field_bk;

}

// Definitions of DIFFERENTIAL EQUATION system
// Need to have first order equations to use Runge Kutta
//
double a_p_rk4(const double a, const double a_p, const double mg_field_bk, const double mg_field_p_bk)
{
	return a_p;
}
double a_pp_rk4(const double a, const double a_p, const double mg_field_bk, const double mg_field_p_bk)
{
  double a3 = a * a * a;
  double rhoa3 = (Omega_cdm_0 + Omega_b_0) + (Omega_Lambda_0 * a3) + (Omega_rad_0 / a);
  double Pa3 = - (Omega_Lambda_0 * a3) + 1./3. * (Omega_rad_0 / a );

  return fourpiG / 3. * ( (rhoa3 - 3. * Pa3) - a * mg_field_p_bk * mg_field_p_bk + 4. * a3 * mg_pot(mg_field_bk) );
}
double mg_field_bk_p_rk4(const double a, const double a_p, const double mg_field_bk, const double mg_field_p_bk)
{
	return mg_field_p_bk;
}
double mg_field_bk_pp_rk4(const double a, const double a_p, const double mg_field_bk, const double mg_field_p_bk)
{
	return - 2. * a_p/a * mg_field_p_bk - mg_pot_p(mg_field_bk) * a * a;
}
double Hconf(const double a, const double mg_field_bk, const double mg_field_p_bk)
{
	return sqrt((2. * fourpiG / 3.) * ( ((Omega_cdm_0 + Omega_b_0) / a) + (Omega_Lambda_0 * a * a) + (Omega_rad_0 / a / a) + (mg_field_p_bk * mg_field_p_bk / 2.) + (a * a * mg_pot(mg_field_bk)) ));
}

// Integrand for the particle horizon integral
double particleHorizonIntegrand(double a, double mg_field_bk, double mg_field_p_bk)
{
  //return 2. / (sqrt(a) * Hconf(a, mg_field_bk, mg_field_p_bk));
	return 1. / ( a * Hconf(a, mg_field_bk, mg_field_p_bk) );
}
// Particle horizon integral step
double particleHorizon(const int i, double * a, double * mg_field_bk, double * mg_field_p_bk) {
  double h = a[i]-a[i-1];
  double fa = particleHorizonIntegrand(a[i-1],mg_field_bk[i-1],mg_field_p_bk[i-1]);
  double fb = particleHorizonIntegrand(a[i],mg_field_bk[i],mg_field_p_bk[i]);

  return h*(fa+fb)/2.;
}

// Function used to print results stored in vectors as a csv file
void csv(double * t, double * a, double * a_p, double * mg_field_bk, double * mg_field_p_bk, double * particleHorizonVec, char * filename) {

    FILE *fp;
    fp = fopen (filename, "w+");
    fprintf(fp, "%s, %s, %s, %s, %s, %s, %s, %s", "t", "a(t)", "H(t)/H0", "H_prime(t)/H0^2", "Omega_df", "Omega_r", "Omega_b", "Omega_cdm");
    fprintf(fp, ", %s", "omega_df");
    fprintf(fp, ", %s", "PH"); // Particle horizon
    fprintf(fp, ", %s", "mg_field");
    fprintf(fp, ", %s", "mg_field_p");
    if(TEST_MODE==1) fprintf(fp, ", %s", "H_check(t)/H0");
    fprintf(fp, "\n");

    // Time conversion factor
    // double tcf = 1./_H0_/(60.*60.*24.*365.*1e9)*_MPc_over_m_/1000.;

    int i = 0;
    double particleHorizonRes = 0.;

    for(i=1;i<points;i++){
      particleHorizonRes += particleHorizon(i,a,mg_field_bk,mg_field_p_bk);
      particleHorizonVec[i] = particleHorizonRes;
    }

    i = 0;

    double H_test, H_cosmo = 0.;

    while( a[i] <= a_end){

      double H = Hconf(a[i],mg_field_bk[i],mg_field_p_bk[i]); // this is the Hubble constant with conformal time
      double H_cosmo = H / a[i];
      double H_prime = 1. / a[i] * a_pp_rk4(a[i], a_p[i], mg_field_bk[i], mg_field_p_bk[i]) - H * H; // this is Hubble prime with conformal time
      if(T_CONF==0) H_prime = ( H_prime - H * H )/a[i]/a[i]; // this is Hubble prime with cosmological time
      if(T_CONF==0) H = H_cosmo; // this it the Hubble constant with cosmological time

      fprintf(fp, "%e, %e, %e, %e, %e, %e, %e, %e", t[i], a[i], H / sqrt(2. * fourpiG / 3.), H_prime / sqrt(2. * fourpiG / 3.) / sqrt(2. * fourpiG / 3.), 2. * fourpiG / 3. * ( (mg_field_p_bk[i] * mg_field_p_bk[i] / 2.) + (a[i] * a[i] * mg_pot(mg_field_bk[i])) ) /H_cosmo /H_cosmo , 2. * fourpiG / 3. * Omega_rad_0 /a[i] /a[i] /a[i] /a[i] /H_cosmo  / H_cosmo , 2. * fourpiG / 3. * Omega_b_0 /a[i] /a[i] /a[i] /H_cosmo  / H_cosmo , 2. * fourpiG / 3. * Omega_cdm_0 /a[i] /a[i] /a[i] /H_cosmo  / H_cosmo );
      fprintf(fp, ", %e", (-mg_pot(mg_field_bk[i])+mg_field_p_bk[i]*mg_field_p_bk[i]/2./a[i]/a[i])/(mg_pot(mg_field_bk[i])+mg_field_p_bk[i]*mg_field_p_bk[i]/2./a[i]/a[i]));

      fprintf(fp, ", %e", particleHorizonVec[i]);

      fprintf(fp, ", %e", mg_field_bk[i]);
      fprintf(fp, ", %e", mg_field_p_bk[i]);

      if(TEST_MODE==1){

        H_test = a_p[i]/a[i];
        if(T_CONF==0) H_test = H_test / a[i];

        fprintf(fp, ", %e", H_test / sqrt(2. * fourpiG / 3.));

      }

      fprintf(fp, "\n");

      if(a[i+csv_resolution]<= a_end){
        i=i+csv_resolution;
      }
      else{
        i++;
      }

    }

    fclose(fp);

}

int scan_for_a0 (double * a) {

  int i=0;

  for(i=0; a[i] <= a_end; i++);

  return i;

}

// This divides an interval in a logarithmic scale of basis 10, and store results in input vector v
void logscale10 (double * v, double A, double B, int points){

    int i;

    double a = log10(A);
    double b = log10(B);

    double h = (b - a) / (points-1.0);

    for(i=0;i<points;i++){
        v[i] = pow(10.0, a + i * h);
    }

}

// This function is a temporal step of the Runge-Kutta 4 method:
// evolves the system for a time equal to dtau (h in the program)
void mg_rungekutta4bg(double * f, const double dtau)
{

  double k_F[4], k_A[4], k_rhof[4], k_rhor[4], k_rhob[4];
  double a = f[1];
  double a_p = f[2];
  double mg_field_bk = f[3];
  double mg_field_p_bk = f[4];

	double k1a, k2a, k3a, k4a;
	double k1ap, k2ap, k3ap, k4ap;
	double k1f, k2f, k3f, k4f;
	double k1fp, k2fp, k3fp, k4fp;

	k1a 	= a_p_rk4(a, a_p, mg_field_bk, mg_field_p_bk);
	k1ap 	= a_pp_rk4(a, a_p, mg_field_bk, mg_field_p_bk);
	k1f 	= mg_field_bk_p_rk4(a, a_p, mg_field_bk, mg_field_p_bk);
	k1fp 	= mg_field_bk_pp_rk4(a, a_p, mg_field_bk, mg_field_p_bk);

	k2a 	= a_p_rk4(a + k1a * dtau / 2., a_p + k1ap * dtau / 2., mg_field_bk + k1f * dtau / 2., mg_field_p_bk + k1fp * dtau / 2.);
	k2ap 	= a_pp_rk4(a + k1a * dtau / 2., a_p + k1ap * dtau / 2., mg_field_bk + k1f * dtau / 2., mg_field_p_bk + k1fp * dtau / 2.);
	k2f 	= mg_field_bk_p_rk4(a + k1a * dtau / 2., a_p + k1ap * dtau / 2., mg_field_bk + k1f * dtau / 2., mg_field_p_bk + k1fp * dtau / 2.);
	k2fp 	= mg_field_bk_pp_rk4(a + k1a * dtau / 2., a_p + k1ap * dtau / 2., mg_field_bk + k1f * dtau / 2., mg_field_p_bk + k1fp * dtau / 2.);

	k3a 	= a_p_rk4(a + k2a * dtau / 2., a_p + k2ap * dtau / 2., mg_field_bk + k2f * dtau / 2., mg_field_p_bk + k2fp * dtau / 2.);
	k3ap 	= a_pp_rk4(a + k2a * dtau / 2., a_p + k2ap * dtau / 2., mg_field_bk + k2f * dtau / 2., mg_field_p_bk + k2fp * dtau / 2.);
	k3f 	= mg_field_bk_p_rk4(a + k2a * dtau / 2., a_p + k2ap * dtau / 2., mg_field_bk + k2f * dtau / 2., mg_field_p_bk + k2fp * dtau / 2.);
	k3fp 	= mg_field_bk_pp_rk4(a + k2a * dtau / 2., a_p + k2ap * dtau / 2., mg_field_bk + k2f * dtau / 2., mg_field_p_bk + k2fp * dtau / 2.);

	k4a 	= a_p_rk4(a + k3a * dtau, a_p + k3ap * dtau, mg_field_bk + k3f * dtau, mg_field_p_bk + k3fp * dtau);
	k4ap 	= a_pp_rk4(a + k3a * dtau, a_p + k3ap * dtau, mg_field_bk + k3f * dtau, mg_field_p_bk + k3fp * dtau);
	k4f 	= mg_field_bk_p_rk4(a + k3a * dtau, a_p + k3ap * dtau, mg_field_bk + k3f * dtau, mg_field_p_bk + k3fp * dtau);
	k4fp 	= mg_field_bk_pp_rk4(a + k3a * dtau, a_p + k3ap * dtau, mg_field_bk + k3f * dtau, mg_field_p_bk + k3fp * dtau);

	f[1] += dtau * (k1a + 2. * k2a + 2. * k3a + k4a) / 6.;
	f[2] += dtau * (k1ap + 2. * k2ap + 2. * k3ap + k4ap) / 6.;
	f[3] += dtau * (k1f + 2. * k2f + 2. * k3f + k4f) / 6.;
	f[4] += dtau * (k1fp + 2. * k2fp + 2. * k3fp + k4fp) / 6.;

}

// This function evolves the system with the Runge-Kutta 4 method until t_stop
double rk4(double * t, double * a, double * a_p, double * mg_field_bk, double * mg_field_p_bk, double Omega_f_0) {

    double f[5];
    int i = 0;

    // Call the function for a logarithmic scale of time
    // We don't start from t = 0 to avoid problems with quintessence potentials
    logscale10(t,1e-9,20.,points);

    while( i < points - 1 ){

      f[0] = t[i];
      f[1] = a[i];
      f[2] = a_p[i];
      f[3] = mg_field_bk[i];
      f[4] = mg_field_p_bk[i];

      mg_rungekutta4bg(f, t[i+1]-t[i]);

      a[i+1] = f[1];
      a_p[i+1] = f[2];
      mg_field_bk[i+1] = f[3];
      mg_field_p_bk[i+1] = f[4];
      i++;

    }

    int j = scan_for_a0(a);

    double H        = Hconf(a[j], mg_field_bk[j], mg_field_p_bk[j]);
    double Omega_f  = (2. * fourpiG / 3.) * ( (mg_field_p_bk[j] * mg_field_p_bk[j] / 2.) + (a[j] * a[j] * mg_pot(mg_field_bk[j])) ) /H /H;
    return Omega_f  - Omega_f_0;

}

double bisection (double min, double max, double * t, double * a, double * a_p, double * mg_field_bk, double * mg_field_p_bk, const double Omega_f_0) {

  double C = (min+max) / 2.;

  while(fabs((max-min)/min)>delta_bisection){

    a_p[0] = a_init * Hconf(a[0], mg_field_init_min, mg_field_p_bk[0]);
    mg_field_bk[0] = min;
    double rk4_min = rk4(t, a, a_p, mg_field_bk, mg_field_p_bk, Omega_f_0);
    a_p[0] = a_init * Hconf(a[0], C, mg_field_p_bk[0]);
    mg_field_bk[0] = C;
    double rk4_C = rk4(t, a, a_p, mg_field_bk, mg_field_p_bk, Omega_f_0);

    if(rk4_min*rk4_C>=0) {
      min=C;
    }
    else {
      max=C;
    }

    C=(max+min)/2.;

    if (TEST_MODE == 1) printf("TEST_MODE ON - min: %e , max: %e, C: %e, rk4_min: %e , rk4_C: %e \n", min, max, C, rk4_min, rk4_C);

  }

  double result = (max+min)/2.;

  if(TEST_MODE==1) printf("\n");
  printf("\t-> Result of bisection method is mg_field_bk: %e (internal units).\n", result);
  //if(TEST_MODE==1) printf("\t--> Confront with LCDM value: %e (internal units).\n", (2. * fourpiG / 3.) * ( Omega_Lambda_0 + 6. * OmegaCDM_0 )/ pow(a_init,3.));

  return result;

}

int main() {

    double Omega_f_0 = 1. - Omega_Lambda_0 - Omega_rad_0 - Omega_b_0 - Omega_cdm_0;

    int i = 0;

    printf("\n\t\t----------------------------------------\n\n");

    // Definition of the vector needed for the evolution functions
    double * t; double * a; double * a_p; double * mg_field_bk; double * mg_field_p_bk; double * particleHorizonVec;
    t                   = (double *) malloc(sizeof(double) * points);
    a                   = (double *) malloc(sizeof(double) * points);
    a_p                 = (double *) malloc(sizeof(double) * points);
    mg_field_bk         = (double *) malloc(sizeof(double) * points);
    mg_field_p_bk       = (double *) malloc(sizeof(double) * points);
    particleHorizonVec  = (double *) malloc(sizeof(double) * points);

    if(!t||!a||!a_p||!mg_field_bk||!mg_field_p_bk||!particleHorizonVec){
      printf("Error! The memory cannot be allocated. The program will be terminated.\n");
      exit(1);
    }

    // Initial conditions (tau=0)
    a[0] = a_init;
    mg_field_p_bk[0] = mg_field_p_bk_0;

    printf(" Searching for best initial value for the dark fluid ... \n \n");
    mg_field_bk[0] = bisection(mg_field_init_min, mg_field_init_max, t, a, a_p, mg_field_bk, mg_field_p_bk, Omega_f_0);
    a_p[0] = a_init * Hconf(a_init, mg_field_bk[0], mg_field_p_bk[0]);

    printf("\n\n Evolving the system ...\n");

    rk4(t, a, a_p, mg_field_bk, mg_field_p_bk, Omega_f_0);

    char filename[50];
    sprintf (filename, "mg_bk.csv");
    int last_int = scan_for_a0(a);

    printf("\n RESULTS:\n");
    printf("\t-> H0: %f \n", a_p[last_int]/a[last_int]/sqrt(2. * fourpiG / 3.));
    printf("\t-> number of points %d \n", last_int);
    //printf("\t-> Age of the Universe: %f Gyr\n", t[last_int] /_H0_/(60.*60.*24.*365.*1e9)*_MPc_over_m_/1000.);

    csv(t, a, a_p, mg_field_bk, mg_field_p_bk, particleHorizonVec, filename);

    printf("\n The results are saved in '.csv' files. The name is labelled with the value of c, and the model (m).\n");

    if(TEST_MODE==1) printf("\n TEST_MODE ON: check the values of H in .csv file. They must be equal!");

    printf("\n\t\t----------------------------------------\n");

    double * a_int; double * a_p_int; double * mg_field_bk_int; double * mg_field_p_bk_int; double * particleHorizonVec_int;
    a_int                   = (double *) malloc(sizeof(double) * last_int);
    a_p_int                 = (double *) malloc(sizeof(double) * last_int);
    mg_field_bk_int         = (double *) malloc(sizeof(double) * last_int);
    mg_field_p_bk_int       = (double *) malloc(sizeof(double) * last_int);
    particleHorizonVec_int  = (double *) malloc(sizeof(double) * last_int);

    if(!a_int||!a_p_int||!mg_field_bk_int||!mg_field_p_bk_int||!particleHorizonVec_int){
      printf("Error! The memory cannot be allocated. The program will be terminated.\n");
      exit(1);
    }

    memcpy(a_int, a, last_int * sizeof(double));
    memcpy(a_p_int, a_p, last_int * sizeof(double));
    memcpy(mg_field_bk_int, mg_field_bk, last_int * sizeof(double));
    memcpy(mg_field_p_bk_int, mg_field_p_bk, last_int * sizeof(double));
    memcpy(particleHorizonVec_int, particleHorizonVec, last_int * sizeof(double));

    free(a);free(a_p);free(mg_field_bk);free(mg_field_p_bk);free(particleHorizonVec);

    // Spline interpolation with gsl
    gsl_interp_accel *acc_mg_field = gsl_interp_accel_alloc();
    gsl_spline * spline_mg_field = gsl_spline_alloc(gsl_interp_cspline,last_int);
    gsl_interp_accel *acc_mg_field_p = gsl_interp_accel_alloc();
    gsl_spline * spline_mg_field_p = gsl_spline_alloc(gsl_interp_cspline,last_int);
    gsl_interp_accel *acc_a_p = gsl_interp_accel_alloc();
    gsl_spline * spline_a_p = gsl_spline_alloc(gsl_interp_cspline,last_int);
    gsl_interp_accel *acc_particleHorizon = gsl_interp_accel_alloc();
    gsl_spline * spline_particleHorizon = gsl_spline_alloc(gsl_interp_cspline,last_int);

    gsl_spline_init(spline_mg_field,a_int,mg_field_bk_int,last_int);
    gsl_spline_init(spline_mg_field_p,a_int,mg_field_p_bk_int,last_int);
    gsl_spline_init(spline_a_p,a_int,a_p_int,last_int);
    gsl_spline_init(spline_particleHorizon,a_int,particleHorizonVec_int,last_int);

    double a_eval = 1e-2;
    printf("spline eval: %e %e \n",a_eval,gsl_spline_eval(spline_mg_field,a_eval,acc_mg_field));
    printf("spline eval: %e %e \n",a_eval,gsl_spline_eval(spline_mg_field_p,a_eval,acc_mg_field_p));
    printf("spline eval: %e %e \n",a_eval,gsl_spline_eval(spline_a_p,a_eval,acc_a_p));
    printf("spline eval: %e %e \n",a_eval,gsl_spline_eval(spline_particleHorizon,a_eval,acc_particleHorizon));
    printf("%d %e \n",last_int, particleHorizonVec_int[gsl_interp_bsearch(a_int,a_eval,0,last_int-1)]);

    gsl_spline_free(spline_mg_field);gsl_interp_accel_free(acc_mg_field);
    gsl_spline_free(spline_mg_field_p);gsl_interp_accel_free(acc_mg_field_p);
    gsl_spline_free(spline_a_p);gsl_interp_accel_free(acc_a_p);
    gsl_spline_free(spline_particleHorizon);gsl_interp_accel_free(acc_particleHorizon);
    free(t);free(a_int);free(a_p_int);free(mg_field_bk_int);free(mg_field_p_bk_int);free(particleHorizonVec_int);

    printf("\n");

    exit(0);

}


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

/*
 * This generator is taken from
 *
 * Donald E. Knuth, The Art of Computer Programming, Volume 2, Section 3.6
 * Third Edition, Addison-Wesley,
 *
 * The modifications introduced in the 9th printing (2002) are
 * included here; there's no backwards compatibility with the
 * original.  [ see http://www-cs-faculty.stanford.edu/~knuth/taocp.html ]
 *
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

#define BUFLEN 1009             /* length of the buffer aa[] */
#define KK 100                  /* the long lag */
#define LL 37                   /* the short lag */
#define MM (1L << 30)           /* the modulus */
#define TT 70                   /* guaranteed separation between streams */

#define is_odd(x) ((x) & 1)                       /* the units bit of x */
#define mod_diff(x, y) (((x) - (y)) & (MM - 1))   /* (x - y) mod MM */

static inline void ran_array (long int aa[], unsigned int n,
                              long int ran_x[]);
static inline unsigned long int ran_get (void *vstate);
static double ran_get_double (void *vstate);
static void ran_set (void *state, unsigned long int s);

typedef struct
{
  unsigned int i;
  long int aa[BUFLEN];
  long int ran_x[KK];  /* the generator state */
}
ran_state_t;

static inline void
ran_array (long int aa[], unsigned int n, long int ran_x[])
{
  unsigned int i;
  unsigned int j;

  for (j = 0; j < KK; j++)
    aa[j] = ran_x[j];

  for (; j < n; j++)
    aa[j] = mod_diff (aa[j - KK], aa[j - LL]);

  for (i = 0; i < LL; i++, j++)
    ran_x[i] = mod_diff (aa[j - KK], aa[j - LL]);

  for (; i < KK; i++, j++)
    ran_x[i] = mod_diff (aa[j - KK], ran_x[i - LL]);
}

static inline unsigned long int
ran_get (void *vstate)
{
  ran_state_t *state = (ran_state_t *) vstate;

  unsigned int i = state->i;
  unsigned long int v;

  if (i  == 0)
    {
      /* fill buffer with new random numbers */
      ran_array (state->aa, BUFLEN, state->ran_x);
    }

  v = state->aa[i];

  state->i = (i + 1) % KK;

  return v;
}

static double
ran_get_double (void *vstate)
{
  ran_state_t *state = (ran_state_t *) vstate;

  return ran_get (state) / 1073741824.0;        /* RAND_MAX + 1 */
}

static void
ran_set (void *vstate, unsigned long int s)
{
  ran_state_t *state = (ran_state_t *) vstate;

  long x[KK + KK - 1];          /* the preparation buffer */

  register int j;
  register int t;
  register long ss;

  if (s == 0 )
    s = 314159;                 /* default seed used by Knuth */

  ss = (s + 2)&(MM-2);

  for (j = 0; j < KK; j++)
    {
      x[j] = ss;                /* bootstrap the buffer */
      ss <<= 1;
      if (ss >= MM)             /* cyclic shift 29 bits */
        ss -= MM - 2;
    }
  x[1]++;                       /* make x[1] (and only x[1]) odd */

  ss = s & (MM - 1);
  t = TT - 1;
  while (t)
    {
      for (j = KK - 1; j > 0; j--)      /* square */
        {
          x[j + j] = x[j];
          x[j + j - 1] = 0;
        }

      for (j = KK + KK - 2; j >= KK; j--)
        {
          x[j - (KK - LL)] = mod_diff (x[j - (KK - LL)], x[j]);
          x[j - KK] = mod_diff (x[j - KK], x[j]);
        }

      if (is_odd (ss))
        {                       /* multiply by "z" */
          for (j = KK; j > 0; j--)
            {
              x[j] = x[j - 1];
            }
          x[0] = x[KK];         /* shift the buffer cyclically */
          x[LL] = mod_diff (x[LL], x[KK]);
        }

      if (ss)
        ss >>= 1;
      else
        t--;
    }

  for (j = 0; j < LL; j++)
    state->ran_x[j + KK - LL] = x[j];
  for (; j < KK; j++)
    state->ran_x[j - LL] = x[j];

  for (j = 0; j< 10; j++)
    ran_array(x, KK+KK-1, state->ran_x);  /* warm things up */

  state->i = 0;

  return;
}

static const gsl_rng_type ran_type = {
  "knuthran2002",                /* name */
  0x3fffffffUL,                 /* RAND_MAX = (2 ^ 30) - 1 */
  0,                            /* RAND_MIN */
  sizeof (ran_state_t),
  &ran_set,
  &ran_get,
  &ran_get_double
};

const gsl_rng_type *gsl_rng_knuthran2002 = &ran_type;


/* Copied from the article:
http://amitsaha.github.io/site/notes/articles/c_scientific/article.html
*/

/*Listing-1: gsl_vector.c*/

/* Simple demo of the vector support in GSL
 *  Also uses the random number generation feature
 */

#include <stdio.h>
#include <gsl/gsl_vector.h> /*For Vectors*/
#include <gsl/gsl_rng.h> /* For Random numbers*/

int main ()
{
  int i,n;
  /* Setup the Random number generator*/
  const gsl_rng_type * T;
  gsl_rng * r;
  gsl_rng_env_setup();
  T = gsl_rng_default;
  r = gsl_rng_alloc (T);

  n = 10;

  /* Allocate the vector of the specified size*/
  gsl_vector * v = gsl_vector_alloc (n);

  /* Set the elements to a uniform random number in [0,1]*/
  for (i = 0; i < n; i++)
    {
      gsl_vector_set (v, i, gsl_rng_uniform (r));
    }

  /* Print the vector*/
  for (i = 0; i < n; i++)
    {
      printf ("v_%d = %g\n", i, gsl_vector_get (v, i));
    }

  gsl_vector_free (v);

  return 0;
}


/* complex/test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>

struct f
{
  char *name;
  double (*f) (gsl_complex z);
  double x;
  double y;
  double fx;
  double fy;
};

struct fz
{
  char *name;
  gsl_complex (*f) (gsl_complex z);
  double x;
  double y;
  double fx;
  double fy;
};

#define FN(x) "gsl_complex_" #x, gsl_complex_ ## x
#define ARG(x,y) x, y
#define RES(x,y) x, y

struct f list[] =
{
#include "results1.h"
  {"", 0, 0, 0, 0, 0}
};

struct fz listz[] =
{
#include "results.h"
  {"", 0, 0, 0, 0, 0}
};

int
main (void)
{
  size_t i = 0;

  gsl_ieee_env_setup();

  for (i = 0 ; i < 10; i++)
    {
      double r = (i - 5.0) * 0.3 ;
      double t = 2.0 * M_PI * i / 5 ;
      double x = r * cos(t), y = r * sin(t) ;
      gsl_complex z = gsl_complex_polar (r, t) ;
      gsl_test_rel (GSL_REAL(z), x, 10 * GSL_DBL_EPSILON, "gsl_complex_polar real part at (r=%g,t=%g)", r, t);

      gsl_test_rel (GSL_IMAG(z), y, 10 * GSL_DBL_EPSILON, "gsl_complex_polar imag part at (r=%g,t=%g)", r, t);
    }

    i = 0;

  while (list[i].f)
    {
      struct f t = list[i];
      gsl_complex z = gsl_complex_rect (t.x, t.y);
      double f = (t.f) (z);
      gsl_test_rel (f, t.fx, 10 * GSL_DBL_EPSILON, "%s at (%g,%g)", t.name, t.x, t.y);
      i++;
    }

  i = 0;

  while (listz[i].f)
    {
      struct fz t = listz[i];
      gsl_complex z = gsl_complex_rect (t.x, t.y);
      gsl_complex fz = (t.f) (z);
      double fx = GSL_REAL (fz), fy = GSL_IMAG (fz);

#ifdef DEBUG
      printf("x = "); gsl_ieee_fprintf_double (stdout, &t.x); printf("\n");
      printf("y = "); gsl_ieee_fprintf_double (stdout, &t.y); printf("\n");
      printf("fx = "); gsl_ieee_fprintf_double (stdout, &fx); printf("\n");
      printf("ex = "); gsl_ieee_fprintf_double (stdout, &t.fx); printf("\n");
      printf("fy = "); gsl_ieee_fprintf_double (stdout, &fy); printf("\n");
      printf("ey = "); gsl_ieee_fprintf_double (stdout, &t.fy); printf("\n");
#endif

      gsl_test_rel (fx, t.fx, 10 * GSL_DBL_EPSILON, "%s real part at (%g,%g)", t.name, t.x, t.y);
      gsl_test_rel (fy, t.fy, 10 * GSL_DBL_EPSILON, "%s imag part at (%g,%g)", t.name, t.x, t.y);
      i++;
    }

  exit (gsl_test_summary ());
}


static const char help[] =
"Solves a 2D dam-saturation problem.  No options.\n"
"The exact soluution is not known, but\n"
"a coarse-grid discrete solution can be checked against that source.\n"
"Note Poisson2DFunctionLocal() sets-up this unconstrained problem:\n"
"    - u_xx - u_yy = - 1\n"
"while we want this complementarity problem:\n"
"    F(u) = - u_xx - u_yy + 1 >= 0\n"
"    u >= 0\n"
"    u F(u) = 0.\n"
"As with obstacle.c, this is solved (default) by -snes_type vinewtonrsls.\n"
"Reference:  pages 667-668 of Brandt & Cryer (1983).\n\n";

/*
note PFAS is not implemented in PETSc, but the following runs for X = 1,2,3,4,5,6
quickly solve the same problems as in Brandt & Cryer Table 4.2:
   s ./dam -snes_monitor -pc_type mg -snes_grid_sequence X

on ed-galago I can go up to X = 11 giving 4097 x 6145 grid and serial runtime of 297 seconds
(the next step runs out of memory)

on parallel I am getting error messages with vinewtonrsls + mg:
    mpiexec -n 4 ./dam -snes_converged_reason -snes_grid_sequence 5 -snes_type vinewtonrsls -pc_type mg
but this works with either vinewtonssls or another PC (gamg or bjacobi+ilu or asm+lu or etc.)

run as:
    ./dam -da_refine 1 -snes_view_solution :foo.m:ascii_matlab -snes_converged_reason -snes_rtol 1.0e-12
Nonlinear solve converged due to CONVERGED_FNORM_RELATIVE iterations 4
done on   5 x   7 grid

then in Matlab/Octave you can compare to Brandt & Cryer Table 4.1:
>> foo
>> format long g
>> u = flipud(reshape(Vec_0x84000000_0,5,7)')    % use loaded name here
u =
           0                     0                     0                     0             0
           8      2.53716015237691                     0                     0             0
          32       18.148640609503      6.78414305345868                     0             0
          72      47.2732592321936      24.9879316043211      7.91201621181146             0
         128      89.9564647149903       53.982307919772      22.6601332428759             0
         200      146.570291707977      94.3247021169195      44.7462088399388             0
         288                   218                   148                    78             8

regarding computing seepage face height, runs
    ./dam -snes_converged_reason -pc_type mg -snes_rtol 1.0e-8 -snes_grid_sequence X
gives:
    X    grid       height
    6    129x193    8.8750000
    7    257x385    8.7500000
    8    513x769    8.7500000
    9    1025x1537  8.7343750
    10   2049x3073  8.7187500
    11   4097x6145  8.7109375
but note these numbers depend (at the second digit, even) on the value of
"wetthreshold" in GetSeepageFaceHeight()
*/

#include <petsc.h>
#include "../../ch6/poissonfunctions.h"

typedef struct {
    double  a, y1, y2;
} DamCtx;

double g_fcn(double x, double y, double z, void *ctx) {
    PoissonCtx *user = (PoissonCtx*)ctx;
    DamCtx     *dctx = (DamCtx*)(user->addctx);
    const double a = dctx->a,
                 y1 = dctx->y1,
                 y2 = dctx->y2,
                 tol = a * 1.0e-8;
    // see (4.2) in Brandt & Cryer for following:
    if (x < tol) {
       return (y1 - y) * (y1 - y) / 2.0;                      // AB
    } else if (x > a - tol) {
       if (y < y2) {
           return (y2 - y) * (y2 - y) / 2.0;                  // CD
       } else {
           return 0.0;                                        // DF
       }
    } else if (y < tol) {
       return (y1 * y1 * (a - x) + y2 * y2 * x) / (2.0 * a);  // BC
    } else if (y > y1 - tol) {
       return 0.0;                                            // FA
    } else {
       return NAN;
    }
}

double f_fcn(double x, double y, double z, void *ctx) {
    return -1.0;
}

extern PetscErrorCode FormBounds(SNES, Vec, Vec);
extern PetscErrorCode GetSeepageFaceHeight(DMDALocalInfo*, Vec, double*, DamCtx*);

int main(int argc,char **argv) {
  PetscErrorCode ierr;
  DM             da, da_after;
  SNES           snes;
  Vec            u;
  PoissonCtx     user;
  DamCtx         dctx;
  DMDALocalInfo  info;
  double         height;

  PetscInitialize(&argc,&argv,NULL,help);

  dctx.a  = 16.0;  // a, y1, y2 from Brandt & Cryer
  dctx.y1 = 24.0;
  dctx.y2 = 4.0;
  user.cx = 1.0;
  user.cy = 1.0;
  user.cz = 1.0;
  user.g_bdry = &g_fcn;
  user.f_rhs = &f_fcn;
  user.addctx = &dctx;

  ierr = DMDACreate2d(PETSC_COMM_WORLD,
      DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_STAR,
      3,4,                       // override with -da_refine or -da_grid_x,_y
      PETSC_DECIDE,PETSC_DECIDE, // num of procs in each dim
      1,1,NULL,NULL,             // dof = 1 and stencil width = 1
      &da);CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMDASetUniformCoordinates(da,0.0,dctx.a,0.0,dctx.y1,-1.0,-1.0);CHKERRQ(ierr);
  ierr = DMSetApplicationContext(da,&user);CHKERRQ(ierr);

  ierr = SNESCreate(PETSC_COMM_WORLD,&snes);CHKERRQ(ierr);
  ierr = SNESSetDM(snes,da);CHKERRQ(ierr);
  ierr = SNESSetApplicationContext(snes,&user);CHKERRQ(ierr);

  ierr = SNESSetType(snes,SNESVINEWTONRSLS);CHKERRQ(ierr);
  ierr = SNESVISetComputeVariableBounds(snes,&FormBounds);CHKERRQ(ierr);

  ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
             (DMDASNESFunction)Poisson2DFunctionLocal,&user); CHKERRQ(ierr);
  ierr = DMDASNESSetJacobianLocal(da,
             (DMDASNESJacobian)Poisson2DJacobianLocal,&user); CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes);CHKERRQ(ierr);

  ierr = DMCreateGlobalVector(da,&u);CHKERRQ(ierr);
  // initial iterate has u=g on boundary and u=0 in interior
  ierr = InitialState(da, ZEROS, PETSC_TRUE, u, &user); CHKERRQ(ierr);

  /* solve */
  ierr = SNESSolve(snes,NULL,u);CHKERRQ(ierr);
  ierr = VecDestroy(&u); CHKERRQ(ierr);
  ierr = DMDestroy(&da); CHKERRQ(ierr);

  // report seepage face
  ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr); /* do not destroy u */
  ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
  ierr = GetSeepageFaceHeight(&info,u,&height,&dctx); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
      "done on %3d x %3d grid; computed seepage face height = %.7f\n",
      info.mx,info.my,height); CHKERRQ(ierr);

  SNESDestroy(&snes);
  return PetscFinalize();
}

// for call-back: tell SNESVI we want  0 <= u < +infinity
PetscErrorCode FormBounds(SNES snes, Vec Xl, Vec Xu) {
    PetscErrorCode ierr;
    ierr = VecSet(Xl,0.0);CHKERRQ(ierr);
    ierr = VecSet(Xu,PETSC_INFINITY);CHKERRQ(ierr);
    return 0;
}

PetscErrorCode GetSeepageFaceHeight(DMDALocalInfo *info, Vec u, double *height, DamCtx *dctx) {
    PetscErrorCode ierr;
    MPI_Comm       comm;
    const double   dy = dctx->y1 / (PetscReal)(info->my-1),
                   wetthreshhold = 1.0e-6;  // what does "u>0" mean?
    int            j;
    double         **au, locwetmax = - PETSC_INFINITY;
    ierr = DMDAVecGetArrayRead(info->da,u,&au); CHKERRQ(ierr);
    if (info->xs+info->xm == info->mx) { // do we even own (part of) the x=a side of the rectangle?
        for (j=info->ys; j<info->ys+info->ym; j++) {
           if (au[j][info->mx-2] > wetthreshhold) {  // is the first inter point wet?
               locwetmax = PetscMax(j*dy,locwetmax);
           }
        }
    }
    ierr = DMDAVecRestoreArrayRead(info->da,u,&au); CHKERRQ(ierr);
    ierr = PetscObjectGetComm((PetscObject)(info->da),&comm); CHKERRQ(ierr);
    ierr = MPI_Allreduce(&locwetmax,height,1,MPI_DOUBLE,MPI_MAX,comm); CHKERRQ(ierr);
    *height -= dctx->y2;   // height is segment ED in figure
    return 0;
}


/* linalg/bidiag.c
 *
 * Copyright (C) 2001, 2007 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

/* Factorise a matrix A into
 *
 * A = U B V^T
 *
 * where U and V are orthogonal and B is upper bidiagonal.
 *
 * On exit, B is stored in the diagonal and first superdiagonal of A.
 *
 * U is stored as a packed set of Householder transformations in the
 * lower triangular part of the input matrix below the diagonal.
 *
 * V is stored as a packed set of Householder transformations in the
 * upper triangular part of the input matrix above the first
 * superdiagonal.
 *
 * The full matrix for U can be obtained as the product
 *
 *       U = U_1 U_2 .. U_N
 *
 * where
 *
 *       U_i = (I - tau_i * u_i * u_i')
 *
 * and where u_i is a Householder vector
 *
 *       u_i = [0, .. , 0, 1, A(i+1,i), A(i+3,i), .. , A(M,i)]
 *
 * The full matrix for V can be obtained as the product
 *
 *       V = V_1 V_2 .. V_(N-2)
 *
 * where
 *
 *       V_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [0, .. , 0, 1, A(i,i+2), A(i,i+3), .. , A(i,N)]
 *
 * See Golub & Van Loan, "Matrix Computations" (3rd ed), Algorithm 5.4.2
 *
 * Note: this description uses 1-based indices. The code below uses
 * 0-based indices
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

int
gsl_linalg_bidiag_decomp (gsl_matrix * A, gsl_vector * tau_U, gsl_vector * tau_V)
{
  if (A->size1 < A->size2)
    {
      GSL_ERROR ("bidiagonal decomposition requires M>=N", GSL_EBADLEN);
    }
  else if (tau_U->size  != A->size2)
    {
      GSL_ERROR ("size of tau_U must be N", GSL_EBADLEN);
    }
  else if (tau_V->size + 1 != A->size2)
    {
      GSL_ERROR ("size of tau_V must be (N - 1)", GSL_EBADLEN);
    }
  else
    {
      const size_t M = A->size1;
      const size_t N = A->size2;
      gsl_vector * tmp = gsl_vector_alloc(M);
      size_t j;

      for (j = 0 ; j < N; j++)
        {
          /* apply Householder transformation to current column */
          gsl_vector_view v = gsl_matrix_subcolumn(A, j, j, M - j);
          double tau_j = gsl_linalg_householder_transform (&v.vector);

          /* apply the transformation to the remaining columns */
          if (j + 1 < N)
            {
              gsl_matrix_view m = gsl_matrix_submatrix (A, j, j + 1, M - j, N - j - 1);
              gsl_vector_view work = gsl_vector_subvector(tau_U, j, N - j - 1);
              gsl_linalg_householder_left (tau_j, &v.vector, &m.matrix, &work.vector);
            }

          gsl_vector_set (tau_U, j, tau_j);

          /* apply Householder transformation to current row */
          if (j + 1 < N)
            {
              v = gsl_matrix_subrow (A, j, j + 1, N - j - 1);
              tau_j = gsl_linalg_householder_transform (&v.vector);

              /* apply the transformation to the remaining rows */
              if (j + 1 < M)
                {
                  gsl_matrix_view m = gsl_matrix_submatrix (A, j + 1, j + 1, M - j - 1, N - j - 1);
                  gsl_vector_view work = gsl_vector_subvector(tmp, 0, M - j - 1);
                  gsl_linalg_householder_right (tau_j, &v.vector, &m.matrix, &work.vector);
                }

              gsl_vector_set (tau_V, j, tau_j);
            }
        }

      gsl_vector_free(tmp);

      return GSL_SUCCESS;
    }
}

/* Form the orthogonal matrices U, V, diagonal d and superdiagonal sd
   from the packed bidiagonal matrix A */

int
gsl_linalg_bidiag_unpack (const gsl_matrix * A,
                          const gsl_vector * tau_U,
                          gsl_matrix * U,
                          const gsl_vector * tau_V,
                          gsl_matrix * V,
                          gsl_vector * diag,
                          gsl_vector * superdiag)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  const size_t K = GSL_MIN(M, N);

  if (M < N)
    {
      GSL_ERROR ("matrix A must have M >= N", GSL_EBADLEN);
    }
  else if (tau_U->size != K)
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (tau_V->size + 1 != K)
    {
      GSL_ERROR ("size of tau must be MIN(M,N) - 1", GSL_EBADLEN);
    }
  else if (U->size1 != M || U->size2 != N)
    {
      GSL_ERROR ("size of U must be M x N", GSL_EBADLEN);
    }
  else if (V->size1 != N || V->size2 != N)
    {
      GSL_ERROR ("size of V must be N x N", GSL_EBADLEN);
    }
  else if (diag->size != K)
    {
      GSL_ERROR ("size of diagonal must match size of A", GSL_EBADLEN);
    }
  else if (superdiag->size + 1 != K)
    {
      GSL_ERROR ("size of subdiagonal must be (diagonal size - 1)", GSL_EBADLEN);
    }
  else
    {
      size_t i, j;

      /* Copy diagonal into diag */

      for (i = 0; i < N; i++)
        {
          double Aii = gsl_matrix_get (A, i, i);
          gsl_vector_set (diag, i, Aii);
        }

      /* Copy superdiagonal into superdiag */

      for (i = 0; i < N - 1; i++)
        {
          double Aij = gsl_matrix_get (A, i, i+1);
          gsl_vector_set (superdiag, i, Aij);
        }

      /* Initialize V to the identity */

      gsl_matrix_set_identity (V);

      for (i = N - 1; i-- > 0;)
        {
          /* Householder row transformation to accumulate V */
          gsl_vector_const_view h = gsl_matrix_const_subrow (A, i, i + 1, N - i - 1);
          double ti = gsl_vector_get (tau_V, i);
          gsl_matrix_view m = gsl_matrix_submatrix (V, i + 1, i + 1, N- i - 1, N - i - 1);
          gsl_vector_view work = gsl_matrix_subrow(U, 0, 0, N - i - 1);

          gsl_linalg_householder_left (ti, &h.vector, &m.matrix, &work.vector);
        }

      /* Initialize U to the identity */

      gsl_matrix_set_identity (U);

      for (j = N; j-- > 0;)
        {
          /* Householder column transformation to accumulate U */
          gsl_vector_const_view h = gsl_matrix_const_subcolumn (A, j, j, M - j);
          double tj = gsl_vector_get (tau_U, j);
          gsl_matrix_view m = gsl_matrix_submatrix (U, j, j, M - j, N - j);

          gsl_linalg_householder_hm (tj, &h.vector, &m.matrix);
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_bidiag_unpack2 (gsl_matrix * A,
                           gsl_vector * tau_U,
                           gsl_vector * tau_V,
                           gsl_matrix * V)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  const size_t K = GSL_MIN(M, N);

  if (M < N)
    {
      GSL_ERROR ("matrix A must have M >= N", GSL_EBADLEN);
    }
  else if (tau_U->size != K)
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (tau_V->size + 1 != K)
    {
      GSL_ERROR ("size of tau must be MIN(M,N) - 1", GSL_EBADLEN);
    }
  else if (V->size1 != N || V->size2 != N)
    {
      GSL_ERROR ("size of V must be N x N", GSL_EBADLEN);
    }
  else
    {
      size_t i, j;

      /* Initialize V to the identity */

      gsl_matrix_set_identity (V);

      for (i = N - 1; i-- > 0;)
        {
          /* Householder row transformation to accumulate V */
          gsl_vector_const_view r = gsl_matrix_const_row (A, i);
          gsl_vector_const_view h =
            gsl_vector_const_subvector (&r.vector, i + 1, N - (i+1));

          double ti = gsl_vector_get (tau_V, i);

          gsl_matrix_view m =
            gsl_matrix_submatrix (V, i + 1, i + 1, N-(i+1), N-(i+1));

          gsl_linalg_householder_hm (ti, &h.vector, &m.matrix);
        }

      /* Copy superdiagonal into tau_v */

      for (i = 0; i < N - 1; i++)
        {
          double Aij = gsl_matrix_get (A, i, i+1);
          gsl_vector_set (tau_V, i, Aij);
        }

      /* Allow U to be unpacked into the same memory as A, copy
         diagonal into tau_U */

      for (j = N; j-- > 0;)
        {
          /* Householder column transformation to accumulate U */
          double tj = gsl_vector_get (tau_U, j);
          double Ajj = gsl_matrix_get (A, j, j);
          gsl_matrix_view m = gsl_matrix_submatrix (A, j, j, M-j, N-j);

          gsl_vector_set (tau_U, j, Ajj);
          gsl_linalg_householder_hm1 (tj, &m.matrix);
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_bidiag_unpack_B (const gsl_matrix * A,
                            gsl_vector * diag,
                            gsl_vector * superdiag)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  const size_t K = GSL_MIN(M, N);

  if (diag->size != K)
    {
      GSL_ERROR ("size of diagonal must match size of A", GSL_EBADLEN);
    }
  else if (superdiag->size + 1 != K)
    {
      GSL_ERROR ("size of subdiagonal must be (matrix size - 1)", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* Copy diagonal into diag */

      for (i = 0; i < K; i++)
        {
          double Aii = gsl_matrix_get (A, i, i);
          gsl_vector_set (diag, i, Aii);
        }

      /* Copy superdiagonal into superdiag */

      for (i = 0; i < K - 1; i++)
        {
          double Aij = gsl_matrix_get (A, i, i+1);
          gsl_vector_set (superdiag, i, Aij);
        }

      return GSL_SUCCESS;
    }
}


/*
   LAPACKE_dgesv Example
   =====================

   The program computes the solution to the system of linear
   equations with a square matrix A and multiple
   right-hand sides B, where A is the coefficient matrix
   and b is the right-hand side matrix:

   Description
   ===========

   The routine solves for X the system of linear equations A*X = B,
   where A is an n-by-n matrix, the columns of matrix B are individual
   right-hand sides, and the columns of X are the corresponding
   solutions.

   The LU decomposition with partial pivoting and row interchanges is
   used to factor A as A = P*L*U, where P is a permutation matrix, L
   is unit lower triangular, and U is upper triangular. The factored
   form of A is then used to solve the system of equations A*X = B.

   LAPACKE Interface
   =================

   LAPACKE_dgesv (row-major, high-level) Example Program Results

  -- LAPACKE Example routine (version 3.7.0) --
  -- LAPACK is a software package provided by Univ. of Tennessee,    --
  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
     December 2016

*/
#include <stdio.h>
#include <stdlib.h>
#include <omp.h>
#include <math.h>
#include <sys/time.h>
#include <unistd.h>
#include <lapacke.h>
#include <string.h>
#ifndef _LAPACKE_EXAMPLE_AUX_
#define _LAPACKE_EXAMPLE_AUX_

void print_matrix_rowmajor( char* desc, lapack_int m, lapack_int n, double* mat, lapack_int ldm );
void print_matrix_colmajor( char* desc, lapack_int m, lapack_int n, double* mat, lapack_int ldm );
void print_vector( char* desc, lapack_int n, lapack_int* vec );

#endif /* _LAPACKE_EXAMPLE_AUX_*/

#define FLT double
//void mset(FLT **m, int n, int in) {
void mset(FLT *m, int n, int in) {
	int i,j;
    for(i=0;i<n;i++)
       for(j=0;j<n;j++) {
           if(i == j) {
               //m[i][j]=in;
               m[i+j*n]=in;
           } else {
              // m[i][j]=0.1;
               m[i+j*n]=0.1;
           }
       }

}
FLT system_clock() {
	FLT t;
	FLT six=1.0e-6;
	struct timeval tb;
	struct timezone tz;
	gettimeofday(&tb,&tz);
	t=(FLT)tb.tv_sec+((FLT)tb.tv_usec)*six;
 	return(t);
}

/* Main program */
int main(int argc, char **argv) {

        /* Locals */
        lapack_int n, nrhs, lda, ldb, info;
		int i, j;
		int DOP=0;
        /* Local arrays */
		double *A, *b;
		lapack_int *ipiv;
		int icount,jcount;
		double t1,t2,tmin,tmax,dt,tstart;
		double *tvect;
		tmin=1e30;
		tmax=0.0;

        /* Default Value */
	    n = 5; nrhs = 1;

        /* Arguments */
	    for( i = 1; i < argc; i++ ) {
	    	if( strcmp( argv[i], "-n" ) == 0 ) {
		    	n  = atoi(argv[i+1]);
			    i++;
		    }
			if( strcmp( argv[i], "-nrhs" ) == 0 ) {
				nrhs  = atoi(argv[i+1]);
				i++;
			}
		}
        scanf("%d",&n);
        /* Initialization */
        lda=n, ldb=nrhs;
		A = (double *)malloc(n*n*sizeof(double)) ;
		if (A==NULL){ printf("error of memory allocation\n"); exit(0); }
		b = (double *)malloc(n*nrhs*sizeof(double)) ;
		if (b==NULL){ printf("error of memory allocation\n"); exit(0); }
		ipiv = (lapack_int *)malloc(n*sizeof(lapack_int)) ;
		if (ipiv==NULL){ printf("error of memory allocation\n"); exit(0); }
		tstart=system_clock();
		tvect=(double*)malloc(500*sizeof(double)) ;
        for (icount=0; icount<500;icount++){
			mset(A,n,10);

			for(i=0;i<n*nrhs;i++)
				b[i] = 10.0;
				//b[i] = ((double) rand()) / ((double) RAND_MAX) - 0.5;

			/* Print Entry Matrix */
			if(DOP)print_matrix_rowmajor( "Entry Matrix A", n, n, A, lda );
			/* Print Right Rand Side */
			if(DOP)print_matrix_rowmajor( "Right Rand Side b", n, nrhs, b, ldb );
			if(DOP)printf( "\n" );
			/* Executable statements */
			if(DOP)printf( "LAPACKE_dgesv (row-major, high-level) Example Program Results\n" );
			/* Solve the equations A*X = B */
			t1=system_clock();
			info = LAPACKE_dgesv( LAPACK_ROW_MAJOR, n, nrhs, A, lda, ipiv,
							b, ldb );
			t2=system_clock();
			dt=t2-t1;
			jcount=icount+1;
			if(dt > tmax)tmax=dt;
			if(dt < tmin)tmin=dt;
			tvect[icount]=dt;
			if(t2-tstart> 120.0)icount=1000000;
			/* Check for the exact singularity */
			if( info > 0 ) {
					printf( "The diagonal element of the triangular factor of A,\n" );
					printf( "U(%i,%i) is zero, so that A is singular;\n", info, info );
					printf( "the solution could not be computed.\n" );
					exit( 1 );
			}
			if (info <0) exit( 1 );
			/* Print solution */
			if(DOP)print_matrix_rowmajor( "Solution", n, nrhs, b, ldb );
			/* Print details of LU factorization */
			if(DOP)print_matrix_rowmajor( "Details of LU factorization", n, n, A, lda );
			/* Print pivot indices */
			if(DOP)print_vector( "Pivot indices", n, ipiv );
        }
        printf("size= %d min=%g max=%g total=%g inverts=%d\n",n,tmin,tmax,t2-tstart,jcount);
        n=0;
        for(icount=0;icount<jcount;icount++){
        	printf("%f10.5 ",tvect[icount]);
        	n=n+1;
        	if(n==8){
        		printf("\n");
        		n=0;}
        }
        exit( 0 );
} /* End of LAPACKE_dgesv Example */


/* specfunc/test_sincos_pi.c
 *
 * Copyright (C) 2017 Konrad Griessinger
 *
 * 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.
 */

/* Author: Konrad Griessinger */

#include <config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_sf.h>
#include "test_sf.h"

/* Any double precision number bigger than this is automatically an even integer. */
#define BIGDBL (2.0 / GSL_DBL_EPSILON)

int
test_sincos_pi(void)
{
  gsl_sf_result r;
  int s = 0;
  int k = 0, kmax = 12;
  double x = 0.0, ix = 0.0, fx = 0.0, exact = 0.0;

  /* sin_pi tests */

  fx = 0.5;
  exact = 1.0;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
  fx = -0.5;
  exact = -1.0;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 1.5;
  exact = -1.0;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
  fx = -1.5;
  exact = 1.0;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 2.5;
  exact = 1.0;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
  fx = -2.5;
  exact = -1.0;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 3.5;
  exact = -1.0;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
  fx = -3.5;
  exact = 1.0;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 0.375;
  exact = 0.923879532511286756128183189397;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = -0.375;
  exact = -0.923879532511286756128183189397;

  TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 0.0;
  exact = 0.0;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.5;
  exact = 1.0;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.03125;
  exact = 0.0980171403295606019941955638886;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.0625;
  exact = 0.195090322016128267848284868477;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.75;
  exact = 0.707106781186547524400844362105;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.0078125;
  exact = 0.0245412285229122880317345294593;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  /* sin_pi tests for very large arguments */

  fx = 0.0625;
  exact = 0.195090322016128267848284868477;
  ix = LONG_MAX + 1.0;
  ix += fabs(fmod(ix,2.0)); /* make sure of even number */

  for (k=0; k<kmax; k++) {
    x = ix + fx;
    x -= ix; /* careful with compiler optimization */
    if ( ( x != fx ) || ( fabs(ix+fx) >= BIGDBL ) ) break;
    printf("ix+fx= %.18e\n", ix+fx);
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix += 101.0;
    exact = -exact;
  }

  fx = -0.0625;
  exact = -0.195090322016128267848284868477;
  ix = LONG_MIN - 1.0;
  ix -= fabs(fmod(ix,2.0)); /* make sure of even number */

  for (k=0; k<kmax; k++) {
    x = ix + fx;
    x -= ix; /* careful with compiler optimization */
    if ( ( x != fx ) || ( fabs(ix+fx) >= BIGDBL ) ) break;
    printf("ix+fx= %.18e\n", ix+fx);
    TEST_SF(s, gsl_sf_sin_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix -= 101.0;
    exact = -exact;
  }

  /* cos_pi tests */

  ix = 0.0;
  fx = 0.0;
  exact = 1.0;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 1.0;
  exact = -1.0;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = -1.0;
  exact = -1.0;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 2.0;
  exact = 1.0;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = -2.0;
  exact = 1.0;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 3.0;
  exact = -1.0;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = -3.0;
  exact = -1.0;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 0.375;
  exact = 0.382683432365089771728459984030;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = -0.375;
  exact = 0.382683432365089771728459984030;

  TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);

  fx = 0.0;
  exact = 1.0;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.5;
  exact = 0.0;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.0625;
  exact = 0.980785280403230449126182236134;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.4375;
  exact = 0.195090322016128267848284868477;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  fx = 0.4921875;
  exact = 0.0245412285229122880317345294593;

  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(3.0,k+1);
    if (k==0) exact = -exact;
  }

  exact = fabs(exact);
  ix = 0.0;
  for (k=0; k<kmax; k++) {
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix = pow(10.0,k+1);
  }

  /* cos_pi tests for very large arguments */

  fx = 0.0625;
  exact = 0.980785280403230449126182236134;
  ix = LONG_MAX + 1.0;
  ix += fabs(fmod(ix,2.0)); /* make sure of even number */

  for (k=0; k<kmax; k++) {
    x = ix + fx;
    x -= ix; /* careful with compiler optimization */
    if ( ( x != fx ) || ( fabs(ix+fx) >= BIGDBL ) ) break;
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix += 101.0;
    exact = -exact;
  }

  fx = -0.0625;
  exact = 0.980785280403230449126182236134;
  ix = LONG_MIN - 1.0;
  ix -= fabs(fmod(ix,2.0)); /* make sure of even number */

  for (k=0; k<kmax; k++) {
    x = ix + fx;
    x -= ix; /* careful with compiler optimization */
    if ( ( x != fx ) || ( fabs(ix+fx) >= BIGDBL ) ) break;
    TEST_SF(s, gsl_sf_cos_pi_e, (ix+fx, &r), exact, TEST_TOL0, GSL_SUCCESS);
    ix -= 101.0;
    exact = -exact;
  }

  return s;
}


/*  Copyright (c) 2014, J.M. Hernandez-Lobato, M.W. Hoffman, Z. Ghahramani
 This function is from the code for the paper
 Hernández-Lobato J. M., Hoffman M. W. and Ghahramani Z.
 Predictive Entropy Search for Efficient Global Optimization of Black-box
 Functions, In NIPS, 2014.
 https://bitbucket.org/jmh233/codepesnips2014
 */
#include "mex.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>

/* Function that computes the inverse of a matrix using its Cholesky decomposition */

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	#define L_matlab prhs[0]
	#define ret_matlab plhs[0]

	int i, j, n, m;
	double *L, *ret;
	gsl_matrix *result;

	n = mxGetN(L_matlab);
	m = mxGetM(L_matlab);

	/* We ask for memory to store the matrix in the gnu library format */

	result = gsl_matrix_alloc(n, m);

	/* We copy the matrix from the matlab format to the gnu library format */

	L = mxGetPr(L_matlab);
	for (i = 0 ; i < n ; i++)
		for (j = 0 ; j < m ; j++)
			gsl_matrix_set(result, i, j, L[ i + n * j ]);

	gsl_linalg_cholesky_decomp(result);

	/* We obtain the inverse of the matrix */

	gsl_linalg_cholesky_invert(result);

	/* We ask for memory to return the solution */

	ret_matlab = mxCreateDoubleMatrix(n, m, mxREAL);

	/* We copy the solution from the gnu library representation to the matlab representation */

	ret = mxGetPr(ret_matlab);
	for (i = 0 ; i < n ; i++)
		for (j = 0 ; j < m ; j++)
			ret[ i + j * n ] = gsl_matrix_get(result, i, j);

	/* We are done */

	gsl_matrix_free(result);

	return;
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_math.h>

#define pi M_PI

//#############################################################################
//#############################################################################
//Ray path solver

//Data structure for pointers to julia cavity data and functions
typedef struct {
    //pointer to Boundary object
    void *bnd;
    //pointer to radius function
    double (*rfunc_p)(void *bnd,double theta);
    //pointer to (mutating) radius and normal vector angle function
    void (*rsys_p)(void *bnd, double theta, double results[]);
    //pointer to RefractiveIndex object
    void *idx;
    //pointer to refractive index value function
    double (*nfunc_p)(void *idx, double r, double theta);
    //pointer to (mutating) refractive index value and derivative function
    void (*nderiv_p)(void *idx, double r, double theta, double results[]);
} cavinfo;

//#############################################################################
//Functions for ODE

//ODE derivatives function
int odefunc(double t, const double y[], double f[], void *params){
    //Get Julia pointers
    cavinfo *cip = (cavinfo *)params;

    //Calculate n and its derivatives and store in results array
    double results[3];
    (*(*cip).nderiv_p)((*cip).idx,y[0],y[1],results);
    //results[0] = n, results[1] = dn/dr, results[2] = dn/dtheta

    //Debugging purposes
    //printf("n = %.5f, dr_n = %.5f, dtheta_n = %.5f\n",n,dr_n,dtheta_n);

    //Calculate and store derivative of ODE coordinate vector
    //Equations are:
    //r' = p_r/n
    //theta' = p_theta/(r^2*n)
    //p_r' = p_theta^2/(r^3*n) + dn/dr
    //p_theta' = dn/dtheta
    f[0] = y[2]/results[0];
    f[1] = y[3]/(y[0]*y[0]*results[0]);
    f[2] = y[3]*y[3]/(y[0]*y[0]*y[0]*results[0]) + results[1];
    f[3] = results[2];

    return GSL_SUCCESS;
}

//ODE Jacobian function
int odejac(double t, const double y[], double *dfdy, double dfdt[], void *params){
    //RK8PD does not require the Jacobian
    return GSL_SUCCESS;
}

//#############################################################################
//Solver functions

//Ray reflection
//Re-compute a ray coordinate vector to simulate a bounce, using the ODE coordinate vectors S0 and S immediately before and after the bounce, where S0 = (r0,theta0,pr0,ptheta0), and S = (r,theta,pr,ptheta).
double raybounce(cavinfo *cip, double S0[], double S[]){

    //Binary search for intersection of trajectory with cavity boundary
    //Assume that light trajectory between (r0,theta0) and (r,theta) is a straight line
    //(fastest way to interpolate, after all this is already within 1 stepsize of time)
    const double x0 = S0[0]*cos(S0[1]), y0 = S0[0]*sin(S0[1]);
    const double x1 = S[0]*cos(S[1]), y1 = S[0]*sin(S[1]);
    double uA = 0.0, uB = 1.0; //bounds of the binary search
    double xC,yC,rC,thetaC,RC,uC;
    do{
        uC = 0.5*(uA+uB); //get middle point
        xC = (1-uC)*x0 + uC*x1; yC = (1-uC)*y0 + uC*y1;
        rC = hypot(xC,yC); thetaC = atan2(yC,xC);
        RC = (*(*cip).rfunc_p)((*cip).bnd,thetaC);
        //Change boundary
        if(rC > RC) uB = uC;
        else uA = uC;
    } while(fabs(rC-RC) > 1e-12);

    //Get angle of incidence and store in results array
    double results[2];
    (*(*cip).rsys_p)((*cip).bnd,thetaC,results);
    //results[0] = rC, results[1] = alpha
    //Use linear interpolation to find ray angle phi at intersection
    const double phi0 = S0[1] + atan2(S0[3],S0[0]*S0[2]);
    const double phi1 = S[1] + atan2(S[3],S[0]*S[2]);
    const double phiC = (1-uC)*phi0 + uC*phi1;
    //Angle of incidence
    const double chi = phiC - results[1];

    //Store interpolated and reflected ODE coordinate vector
    const double phi = pi - chi + results[1]; //ray angle after reflection
    const double n = (*(*cip).nfunc_p)((*cip).idx,rC,thetaC);
    const double pr = n*cos(phi-thetaC);
    const double ptheta = n*rC*sin(phi-thetaC);
    S[0] = rC; S[1] = thetaC; S[2] = pr; S[3] = ptheta;

    //Report bounce information
    return chi;
}

//Ray evolution
void rayevolve(
    //Storage arrays
    double raypath_r[], double raypath_theta[], long bounceindices[],
    double bouncepts_chi[], long lengths[],
    //Initial conditions
    double r0, double theta0, double pr0, double ptheta0,
    //Simulation parameters
    double tmax, long bouncemax, double reltol, double abstol,
    //Cavity parameters
    void *bnd, double (*rfunc_p)(void *bnd,double theta),
    void (*rsys_p)(void *bnd, double theta,double results[]),
    void *idx, double (*nfunc_p)(void *idx, double r, double theta),
    void (*nderiv_p)(void *idx, double r, double theta,double results[])){

        //Condense input cavity data into cavinfo struct
        cavinfo ci = {bnd,rfunc_p,rsys_p,idx,nfunc_p,nderiv_p};

        //Initialize solver
        gsl_odeiv2_system sys = {odefunc,odejac,4,&ci};
        const gsl_odeiv2_step_type *steptype = gsl_odeiv2_step_rk8pd;
        gsl_odeiv2_step *step = gsl_odeiv2_step_alloc(steptype,4);
        gsl_odeiv2_control *control = gsl_odeiv2_control_y_new(abstol,reltol);
        gsl_odeiv2_evolve *evolve = gsl_odeiv2_evolve_alloc(4);

        //Initialize parameters
        double t = 0.0, dt = 0.0001;
        double y0[4], y[4] = {r0,theta0,pr0,ptheta0};

        //Prepare results record
        long bouncenum = 0, stepnum = 1; //indicates postion to record next
        const long prealloc = 250*ceil(tmax); //length of preallocated raypath array
        raypath_r[0] = y[0]; raypath_theta[0] = y[1];

        //Solver loop
        while(t < tmax && bouncenum < bouncemax && stepnum < prealloc){
            //Record initial position
            y0[0] = y[0], y0[1] = y[1], y0[2] = y[2], y0[3] = y[3];

            //Run Solver
            int status =
                gsl_odeiv2_evolve_apply(evolve,control,step,&sys,&t,tmax,&dt,y);
            if(status != GSL_SUCCESS) break;

            //Check Hamiltonian once in a while for sanity check
            if(stepnum%1000 == 0){
                double H = y[2]*y[2]+y[3]*y[3]/(y[0]*y[0]) - gsl_pow_2((*ci.nfunc_p)(ci.idx,y[0],y[1]));
                if(H > 1e-9) printf("Warning: Error in Hamiltonian is %.f\n",H);
            }

            //Check difference in ray and boundary radial positions
            double dr = y[0] - (*ci.rfunc_p)(ci.bnd,y[1]);
            if(dr > 0){
                //Boundary crossing!
                //get chi and corrected y
                bouncepts_chi[bouncenum] = raybounce(&ci,y0,y);
                //store (Julia's 1-based) index for thetaC values recorded in
                //raypath_theta array
                bounceindices[bouncenum] = stepnum+1;
                bouncenum += 1;
            }

            //Record position
            raypath_r[stepnum] = y[0]; raypath_theta[stepnum] = y[1];
            stepnum += 1;

            //Display progress for debugging
            //printf("t = %.5f, y = [%.5f,%.5f,%.5f,%.5f]\n",t,y[0],y[1],y[2],y[3]);

        }
        //Store lengths of arrays
        lengths[0] = stepnum; lengths[1] = bouncenum;

        //Free memory
        gsl_odeiv2_step_free(step);
        gsl_odeiv2_control_free(control);
        gsl_odeiv2_evolve_free(evolve);

}


#ifndef CG_H_
#define CG_H_

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "util.h"
#include "parameters.h"
#include <cblas.h>

void cgsolver( double *A, double *b, double *x, int m, int n );
double * init_source_term(int n, double h);
void smvm(int m, const double* val, const int* col, const int* row, const double* x, double* y);
void cgsolver_sparse( double *Aval, int *I, int *J, double *b, double *x, int n);

#endif /*CG_H_*/


/* specfunc/sincos_pi.c
 *
 * Copyright (C) 2017 Gerard Jungman, Konrad Griessinger (konradg@gmx.net)
 *
 * 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.
 */

/* routines for computing sin(pi*x) and cos(pi*x), respectively, with argument reduction */

#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_sincos_pi.h>

/* Any double precision number bigger than this is automatically an even integer. */
#define TWOBIG (2.0 / GSL_DBL_EPSILON)

/* routine computing sin(pi*x) valid for |x| <= 0.25 using a Taylor expansion around the origin and otherwise a rational approximation from the reference below. Spot-checked to give around 2e-16 relative accuracy. */
/* I. Koren and O. Zinaty. Evaluating elementary functions in a numerical
coprocessor based on rational approximations. IEEE Transactions on
Computers, Vol.39, No.8, August 1990, pp 1030-1037. */
/*
static int
sin_pi_koren(const double x, gsl_sf_result *result)
{
  result->val = 0.0;
  result->err = 0.0;
  if (16.0*fabs(x) < 1.0) {
    const double y = M_PI * x;
    const double a = y*y;
    result->val = y*(1.0 - a*(1.0 - a*(1.0 - a*(1.0 - a*(1.0 - a/110.0)/72.0)/42.0)/20.0)/6.0);
  }
  else {
    const double a0 = 1805490264.690988571178600370234394843221;
    const double a1 = -164384678.227499837726129612587952660511;
    const double a2 =    3664210.647581261810227924465160827365;
    const double a3 =     -28904.140246461781357223741935980097;
    const double a4 =         76.568981088717405810132543523682;
    const double b0 = 2298821602.638922662086487520330827251172;
    const double b1 =   27037050.118894436776624866648235591988;
    const double b2 =     155791.388546947693206469423979505671;
    const double b3 =        540.567501261284024767779280700089;
    const double t = 16.0*x*x;
    result->val = 4.0*x*(((( a4*t + a3 )*t + a2 )*t + a1 )*t + a0)/(((( t + b3 )*t + b2 )*t + b1 )*t + b0);
  }

  result->err = GSL_DBL_EPSILON*fabs(result->val);

  return GSL_SUCCESS;
}
*/

/* routine computing cos(pi*x) valid for |x| <= 0.25 using a Taylor expansion around the origin and otherwise a rational approximation from the reference below. Spot-checked to give around 2e-16 relative accuracy. */
/* I. Koren and O. Zinaty. Evaluating elementary functions in a numerical
coprocessor based on rational approximations. IEEE Transactions on
Computers, Vol.39, No.8, August 1990, pp 1030-1037. */
/*
static int
cos_pi_koren(const double x, gsl_sf_result *result)
{
  result->val = 0.0;
  result->err = 0.0;
  if (20.0*fabs(x) < 1.0) {
    const double y = M_PI * x;
    const double a = y*y;
    result->val = 1.0 - 0.5*a*(1.0 - a*(1.0 - a*(1.0 - a*(1.0 - a/90.0)/56.0)/30.0)/12.0);
  }
  else {
    const double a0 = 1090157078.174871420428849017262549038606;
    const double a1 = -321324810.993150712401352959397648541681;
    const double a2 =   12787876.849523878944051885325593878177;
    const double a3 =    -150026.206045948110568310887166405972;
    const double a4 =        538.333564203182661664319151379451;
    const double b0 = 1090157078.174871420428867295670039506886;
    const double b1 =   14907035.776643879767410969509628406502;
    const double b2 =     101855.811943661368302608146695082218;
    const double b3 =        429.772865107391823245671264489311;
    const double t = 16.0*x*x;
    result->val = (((( a4*t + a3 )*t + a2 )*t + a1 )*t + a0)/(((( t + b3 )*t + b2 )*t + b1 )*t + b0);
  }

  result->err = GSL_DBL_EPSILON*fabs(result->val);

  return GSL_SUCCESS;
}
*/

/* routine computing sin(pi*x) using a Taylor expansion around the origin and otherwise the library function. */
static int
sin_pi_taylor(const double x, gsl_sf_result *result)
{
  result->val = 0.0;
  result->err = 0.0;
  if (16.0*fabs(x) < 1.0) {
    const double y = M_PI * x;
    const double a = y*y;
    result->val = y*(1.0 - a*(1.0 - a*(1.0 - a*(1.0 - a*(1.0 - a/110.0)/72.0)/42.0)/20.0)/6.0);
  }
  else {
    result->val = sin(M_PI*x);
  }

  result->err = GSL_DBL_EPSILON*fabs(result->val);

  return GSL_SUCCESS;
}

/* routine computing sin(pi*x) using a Taylor expansion around the origin and otherwise the library function. */
static int
cos_pi_taylor(const double x, gsl_sf_result *result)
{
  result->val = 0.0;
  result->err = 0.0;
  if (20.0*fabs(x) < 1.0) {
    const double y = M_PI * x;
    const double a = y*y;
    result->val = 1.0 - 0.5*a*(1.0 - a*(1.0 - a*(1.0 - a*(1.0 - a/90.0)/56.0)/30.0)/12.0);
  }
  else {
    result->val = cos(M_PI*x);
  }

  result->err = GSL_DBL_EPSILON*fabs(result->val);

  return GSL_SUCCESS;
}

int
gsl_sf_sin_pi_e(const double x, gsl_sf_result *result)
{
  double intx = 0.0, fracx = 0.0;
  long q;
  int sign = 1, status;

  result->val = 0.0;
  result->err = 0.0;
  fracx = modf(x,&intx);
  if (fracx == 0.0) return GSL_SUCCESS;
  if(fabs(intx) >= TWOBIG) return GSL_SUCCESS; /* to be sure. Actually should be covered by the line above */

  q = ( ( (intx >= LONG_MIN) && (intx <= LONG_MAX) ) ? intx : fmod(intx, 2.0) );
  sign = ( q % 2 ? -1 : 1 );

  /* int sign = 1 - 2*((int)round(fmod(fabs(intx),2.0))); */
  if (fabs(fracx) == 0.5) { /* probably unnecessary */
    if (fracx < 0.0) sign = -sign;
    result->val = ( sign != 1 ? -1.0 : 1.0 );
    return GSL_SUCCESS;
  }
  if (fabs(fracx) > 0.5) {
    sign = -sign;
    fracx = ( fracx > 0.0 ? fracx-1.0 : fracx+1.0 );
  }

  status = 0;
  if (fracx > 0.25) {
    status = cos_pi_taylor((fracx-0.5), result);
  }
  else if (fracx < -0.25) {
    status = cos_pi_taylor((fracx+0.5), result);
    sign = -sign;
  }
  else {
    status = sin_pi_taylor(fracx, result);
  }
  if (sign != 1) result->val = -result->val;
  return status;
}

int
gsl_sf_cos_pi_e(const double x, gsl_sf_result *result)
{
  double intx = 0.0, fracx = 0.0;
  long q;
  int sign = 1, status;

  result->val = 0.0;
  result->err = 0.0;
  fracx = modf(x,&intx);
  if (fabs(fracx) == 0.5) return GSL_SUCCESS;

  if(fabs(intx) >= TWOBIG) {
    result->val = 1.0;
    return GSL_SUCCESS;
  }

  q = ( ( (intx >= LONG_MIN) && (intx <= LONG_MAX) ) ? intx : fmod(intx, 2.0) );
  sign = ( q % 2 ? -1 : 1 );

  /* int sign = 1 - 2*((int)round(fmod(fabs(intx),2.0))); */
  if (fracx == 0.0) { /* probably unnecessary */
    result->val = ( sign != 1 ? -1.0 : 1.0 );
    return GSL_SUCCESS;
  }
  if (fabs(fracx) > 0.5) {
    sign = -sign;
    fracx = ( fracx > 0.0 ? fracx-1.0 : fracx+1.0 );
  }

  status = 0;
  if (fracx > 0.25) {
    status = sin_pi_taylor((fracx-0.5), result);
    sign = -sign;
  }
  else if (fracx < -0.25) {
    status = sin_pi_taylor((fracx+0.5), result);
  }
  else {
    status = cos_pi_taylor(fracx, result);
  }
  if (sign != 1) result->val = -result->val;
  return status;
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double
gsl_sf_sin_pi(const double x)
{
  EVAL_RESULT(gsl_sf_sin_pi_e(x, &result));
}

double
gsl_sf_cos_pi(const double x)
{
  EVAL_RESULT(gsl_sf_cos_pi_e(x, &result));
}