#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <pthread.h>
#include <semaphore.h>
#include <apop.h>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_multifit.h>

/********************************HOPE: Sum of norm of residuals fitting for Hough Mode Extension Data*****************************************/
//Copyright (C) 2012 Daniel Strano

/*'Hope' is an open-source project based on Oberheide and Forbes GEOPHYSICAL RESEARCH LETTERS, VOL. 35, L04816, doi:10.1029/2007GL032397, 2008. The program was originally designed for the purpose of handling the challenge of empirically fitting multi-component complex wave data, in situations where the goodness of fit of a least squares model is questionable. The program was originally designed to work with three self-consistent variables to fit simultaneously. The variances of the variables are first scaled to satisify the assumptions of constant variance and independence of errors, necessary for a consistent least squares model. Then, the program takes advantage of the finite phase domain to indirectly search for a maximization of the sum of absolute (norms of) residuals.*/

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

#define depVar 3
const int rawblock = 29;
const int block = 33;
const int heightsOut = 8;
const int heightsIn = 8;
const int HME_N = 2;
const int HMEBlockCount = 8;
const int satrows = 264;

const double PI = 3.141592653589793;
const double PHASE_INTERVAL = 0.001;
const int PHASE_INT_INV = 1000;
const double P_I_x_PI = 0.001 * 3.141592653589793;

int outrows=0;
double totalSumSqr;
double TotalSN[depVar];

int global_phase_iter1 = 0;
int global_phase_iter2 = 0;

int threadtracker[4];

pthread_mutex_t iter_update;
pthread_mutex_t compare;

sem_t threadsAvail;

gsl_matrix *Full_Descrip;
gsl_vector *dependent;
gsl_vector *BestModel;

int minIndepOffset;
double *threadIndep;

double BestPhase1;
double BestPhase2;

double BestResid = 0;
double BestRNorm = 0;

int itemscount;

void *threadParallel(void *arg) {

	int *threadNum = (int *)arg; //Argument is used for thread scheduling (used at bottom of thread function)

	int phase_iter1, phase_iter2; //Local phase iteration holder variables

	//Lock the current global phase iteration counters, pull the next value for the local iteration, and update global counters:
	pthread_mutex_lock(&iter_update);
		phase_iter1 = global_phase_iter1;
		phase_iter2 = global_phase_iter2;
		++global_phase_iter1;
		if (global_phase_iter1 >= PHASE_INT_INV) {
			global_phase_iter1 = 0;
			++global_phase_iter2;
		}
	pthread_mutex_unlock(&iter_update);

	int loop1, loop2, loop3, loop4, loop5;
	int returncode;
	float phase_shift[2]= {P_I_x_PI * phase_iter1, P_I_x_PI * phase_iter2}; //Phase shifts calculated from phase iteration counters
	float Rotation[8]; //Used to rotate complex vectors

	int holder, holder2, holder3, holder4, holder5, holder6; //For loop optimization

	double chiSqr; //Sum of squares of residuals of each iterative model
	float coeff_of_determ; //Normalized coefficient of determination ""

	//Calculate rotation factors to apply phase shift corresponding to the current iteration:
	for (loop1 = 0; loop1 < HME_N; loop1++) {
		holder = loop1 * 4;
		holder2 = holder + 2;
		for (loop2 = 0; loop2 < 2; loop2++) {
			Rotation[holder + loop2] = cos(phase_shift[loop1]);
			if (loop2) {
				Rotation[holder2 + loop2] = -1 * sin(phase_shift[loop1]);
			}
			else {
				Rotation[holder2 + loop2] = sin(phase_shift[loop1]);
			}
		}
	}

	//Declare objects for GSL least squares model for fixed phases:
	gsl_matrix *Local_Descrip = gsl_matrix_alloc(itemscount, HME_N); //Local description of amplitude-only model
	gsl_matrix *covariance = gsl_matrix_alloc(HME_N, HME_N);
	gsl_multifit_linear_workspace *modelspace = gsl_multifit_linear_alloc(itemscount, HME_N);
	gsl_vector *beta = gsl_vector_alloc(HME_N); //fitting parameters

	//Apply rotations and initialize 'Local_Descrip'
	for (loop5 = 0; loop5 < HME_N; loop5++) {
		holder = loop5 * 4;
		holder2 = holder + 1;
		holder3 = holder2 + 1;
		holder4 = holder3 + 1;
		holder6 = loop5 * minIndepOffset;
		for (loop1 = 0; loop1 < itemscount; loop1 += 2) {
			holder5 = loop1 + 1;
			gsl_matrix_set(Local_Descrip, loop1, loop5,
				Rotation[holder] * threadIndep[holder6 + loop1] + Rotation[holder4] * threadIndep[holder6 + holder5]);
			gsl_matrix_set(Local_Descrip, holder5, loop5,
				Rotation[holder2] * threadIndep[holder6 + holder5] + Rotation[holder3] * threadIndep[holder6 + loop1]);
		}
	}

	//Find least squares fit:
	returncode = gsl_multifit_linear(Local_Descrip, dependent, beta, covariance, &chiSqr, modelspace);
	coeff_of_determ = 1 - chiSqr/totalSumSqr;

	//Free model space for fit (covariance is also not used):
	gsl_matrix_free(covariance);
	gsl_multifit_linear_free(modelspace);

	//From partial model beta parameters, calculate equivalent full model parameters:
	double sumNormResid[3] = {0, 0, 0};
	double R_norm = 0;
	double R_resid, I_resid;
	double Full_Beta[2 * HME_N];
	for (loop1 = 0; loop1 < HME_N; loop1++) {
		Full_Beta[2 * loop1] = gsl_vector_get(beta, loop1) * cos(phase_shift[loop1]);
		Full_Beta[2 * loop1 + 1] = gsl_vector_get(beta, loop1) * sin(phase_shift[loop1]);
	}

	//Re-weight variables to equal total weight for sum of norms of residuals model,
	//calculate sum of norms of residuals for full model from least squares beta coefficients:
	int maxiter1 = 2 * outrows;
	int maxiter2 = maxiter1 * depVar;
	int maxiter3 = HME_N * 2;
	for (loop2 = 0; loop2 < maxiter2; loop2 += maxiter1) {
		holder3 = (int)(loop2/maxiter1);
		for (loop1 = 0; loop1 < maxiter1; loop1 += 2) {
			R_resid = gsl_vector_get(dependent, loop1 + loop2);
			holder = loop2 + loop1;
			holder2 = holder + 1;
			for (loop3 = 0; loop3 < maxiter3; loop3++) R_resid -= Full_Beta[loop3] * gsl_matrix_get(Full_Descrip, holder, loop3);
			I_resid = gsl_vector_get(dependent, holder2);
			for (loop3 = 0; loop3 < maxiter3; loop3++) I_resid -= Full_Beta[loop3] * gsl_matrix_get(Full_Descrip, holder2, loop3);
			sumNormResid[holder3] += sqrt((R_resid * R_resid) + (I_resid * I_resid));
		}
	}

	//Calculated normalized coefficient of determination for sum of norms of residuals:
	for (loop1 = 0; loop1 < depVar; loop1++) R_norm -= sumNormResid[loop1]/TotalSN[loop1];
	R_norm = R_norm/3;
	R_norm++;

	//Lock global best model comparisons and updates,
	//compare this thread's "R-norm" and update global best model if a better model has been found:
	pthread_mutex_lock(&compare);
		if (BestRNorm < R_norm) {
			BestRNorm = R_norm;
			BestResid = coeff_of_determ;
			BestPhase1 = phase_shift[0];
			BestPhase2 = phase_shift[1];
			for (loop1 = 0; loop1 < HME_N; loop1++) {
				gsl_vector_set(BestModel, loop1, gsl_vector_get(beta, loop1));
			}
		}
	pthread_mutex_unlock(&compare);

	//Free remaining manually allocated memory objects:
	gsl_matrix_free(Local_Descrip);
	gsl_vector_free(beta);

	//Prepare to signal to loop in main() that this thread identity is becoming free, then post to semaphore:
	threadtracker[*threadNum] = 0;
	sem_post(&threadsAvail);

	return NULL;
}

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

	//Thread expects input files as specified below:
	if (argc != 4) {
		printf("usage: hope [saber_t] [tidi_e] [tidi_n]\n");
		printf("Note: Satellite data should be in *.csv form, in a single column, aligned by row, with no column or row names. Dummy value is -999. \n");
	}
	else {

		gsl_matrix *satellite[3];

		double *minIndep[2];

		int lev_rows;
		double height[heightsIn];
		double latitudes[rawblock];

		double *HME_ptr[8][HME_N];

		double **uninterp[HME_N][depVar*2];
		double **interp_lat[HME_N][depVar*2];
		double **interp_lev[HME_N][depVar*2];

		BestModel = gsl_vector_alloc(2);

		pthread_t zero;
		pthread_t one;
		pthread_t two;
		pthread_t three;

		sem_init(&threadsAvail, 0, 4);

		long long maxpage;
		long long pagein = 1;
		int comp, comp_int, tenth_percent_complete = 0, percent_complete = 0;

		int loop, loop1, loop2, loop3, loop4, loop5, loop6;

		int threadID[4];

		double TotalSumSqr[depVar];
		double Variance[depVar];
		double AbsDeviance[depVar];
		double Scale[depVar], DevScale[depVar];

		double scaledRow;

		int holder;
		int holder2;

		for (loop = 0; loop < depVar; loop++) {
			TotalSumSqr[loop] = 0;
			TotalSN[loop] = 0;
			Variance[loop] = 0;  //optional
			Scale[loop] = 0; //optional
		}

		for (loop = 0; loop < 4; loop++) {
			threadtracker[loop] = 0;
			threadID[loop] = loop;
		}

			//Read in raw data:

				//(Independent) descriptor data:
		apop_text_to_db(.text_file="HME1.csv", .tabname="X1", .has_row_names = 0, .has_col_names = 1);
		HME_ptr[0][0] = apop_vector_to_array(apop_query_to_vector("SELECT lat FROM X1"));
		HME_ptr[1][0] = apop_vector_to_array(apop_query_to_vector("SELECT lev FROM X1"));
		HME_ptr[2][0] = apop_vector_to_array(apop_query_to_vector("SELECT amp_t FROM X1"));
		HME_ptr[3][0] = apop_vector_to_array(apop_query_to_vector("SELECT phase_t FROM X1"));
		HME_ptr[4][0] = apop_vector_to_array(apop_query_to_vector("SELECT amp_e FROM X1"));
		HME_ptr[5][0] = apop_vector_to_array(apop_query_to_vector("SELECT phase_e FROM X1"));
		HME_ptr[6][0] = apop_vector_to_array(apop_query_to_vector("SELECT amp_n FROM X1"));
		HME_ptr[7][0] = apop_vector_to_array(apop_query_to_vector("SELECT phase_n FROM X1"));

		apop_text_to_db(.text_file="HME2.csv", .tabname="X2", .has_row_names = 0, .has_col_names = 1);
		HME_ptr[0][1] = apop_vector_to_array(apop_query_to_vector("SELECT lat FROM X2"));
		HME_ptr[1][1] = apop_vector_to_array(apop_query_to_vector("SELECT lev FROM X2"));
		HME_ptr[2][1] = apop_vector_to_array(apop_query_to_vector("SELECT amp_t FROM X2"));
		HME_ptr[3][1] = apop_vector_to_array(apop_query_to_vector("SELECT phase_t FROM X2"));
		HME_ptr[4][1] = apop_vector_to_array(apop_query_to_vector("SELECT amp_e FROM X2"));
		HME_ptr[5][1] = apop_vector_to_array(apop_query_to_vector("SELECT phase_e FROM X2"));
		HME_ptr[6][1] = apop_vector_to_array(apop_query_to_vector("SELECT amp_n FROM X2"));
		HME_ptr[7][1] = apop_vector_to_array(apop_query_to_vector("SELECT phase_n FROM X2"));

				//(Dependent) variable data:
		apop_text_to_db(.text_file=argv[1], .tabname="Y_t", .has_row_names = 0, .has_col_names = 1);
		satellite[0] = apop_query_to_matrix("SELECT * FROM Y_t");

		apop_text_to_db(.text_file=argv[2], .tabname="Y_e", .has_row_names = 0, .has_col_names = 1);
		satellite[1] = apop_query_to_matrix("SELECT * FROM Y_e");

		apop_text_to_db(.text_file=argv[3], .tabname="Y_n", .has_row_names = 0, .has_col_names = 1);
		satellite[2] = apop_query_to_matrix("SELECT * FROM Y_n");

		//Need to know how many non-dummy rows are in satellite data
		//Calculate total sum of squares and total sum of norms of satellite data while tallying non-dummy rows in 'outrows':
		outrows = 0;
		for (loop1 = 0; loop1 < satrows; loop1++) {
			if (gsl_matrix_get(satellite[0], loop1, 0) > -990) {
				if (gsl_matrix_get(satellite[1], loop1, 0) > -990) {
					if (gsl_matrix_get(satellite[2], loop1, 0) > -990) {
						for (loop2 = 0; loop2 < depVar; loop2++) {
							for (loop3 = 0; loop3 < 2; loop3++) {
								TotalSumSqr[loop2] += (gsl_matrix_get(satellite[loop2], loop1, loop3) * gsl_matrix_get(satellite[loop2], loop1, loop3));
							}
							TotalSN[loop2] += sqrt((gsl_matrix_get(satellite[loop2], loop1, 0) * gsl_matrix_get(satellite[loop2], loop1, 0)) + (gsl_matrix_get(satellite[loop2], loop1, 1) * gsl_matrix_get(satellite[loop2], loop1, 1)));
						}
						++outrows;
					}
				}
			}
		}

		itemscount = outrows * depVar * 2; //Total number of data items (rows * columns)

		//Allocate arrays for uninterpolated data, lattitude interpolated data, and data interpolated by both lattitude and elevation:
		for (loop1 = 0; loop1 < HME_N; loop1++) {

			for (loop2 = 0; loop2 < depVar * 2; loop2++) {
				uninterp[loop1][loop2]   = malloc(HMEBlockCount * sizeof(double *));
				interp_lat[loop1][loop2] = malloc(block * sizeof(double *));
				interp_lev[loop1][loop2] = malloc(block * sizeof(double *));
				for (loop3 = 0; loop3 < HMEBlockCount; loop3++) {
					uninterp[loop1][loop2][loop3]   = malloc(rawblock * sizeof(double));
				}
				for (loop3 = 0; loop3 < block; loop3++) {
					interp_lat[loop1][loop2][loop3] = malloc(HMEBlockCount * sizeof(double));
					interp_lev[loop1][loop2][loop3] = malloc(heightsOut * sizeof(double));
				}
			}
		}

		//Initialize block of uninterpolated data:
		for (loop1 = 0; loop1 < HMEBlockCount; loop1++) {
			holder = rawblock * loop1;
			for (loop2 = 0; loop2 < 2 * depVar; loop2 += 2) {
				for (loop3 = 0; loop3 < HME_N; loop3++) {
					for (loop4 = 0; loop4 < rawblock; loop4++) {
						uninterp[loop3][0 + loop2][loop1][(rawblock - 1) - loop4] = *(HME_ptr[2 + loop2][loop3] + (holder + loop4)) * cos(*(HME_ptr[3 + loop2][loop3] + holder + loop4) / 12 * PI);
						uninterp[loop3][1 + loop2][loop1][(rawblock - 1) - loop4] = *(HME_ptr[2 + loop2][loop3] + (holder + loop4)) * sin(*(HME_ptr[3 + loop2][loop3] + holder + loop4) / 12 * PI);
					}
				}
			}
		}

		//Read lattitudes from input:
		for (loop1 = 0; loop1 < rawblock; loop1++) {
			latitudes[(rawblock - 1) - loop1] = *(HME_ptr[0][0] + loop1);
		}

		//Read elevations from input:
		for (loop1 = 0; loop1 < heightsIn; loop1++) {
			height[loop1] = *(HME_ptr[1][0] + (loop1 * rawblock));
		}

	//GSL Spline Interpolation:

		//Lattitude interpolation:
		for (loop = 0; loop < HME_N; loop++) {
			for (loop3 = 0; loop3 < 2; loop3++) {
				for (loop2 = 0; loop2 < HMEBlockCount; loop2++) {
					for (loop4 = 0; loop4 < depVar * 2; loop4 += 2) {
						gsl_interp_accel *lat_acc = gsl_interp_accel_alloc();
						gsl_spline *lat_spline = gsl_spline_alloc(gsl_interp_cspline, rawblock);
						gsl_spline_init(lat_spline, latitudes, uninterp[loop][loop4 + loop3][loop2], rawblock);

						for (loop1 = 0; loop1 < block; loop1++) {
							interp_lat[loop][loop4 + loop3][loop1][loop2] = gsl_spline_eval(lat_spline, (loop1 * 5) - 80, lat_acc);
						}

						gsl_spline_free (lat_spline);
						gsl_interp_accel_free (lat_acc);
					}
				}
			}
		}

		//Altitude ("lev") interpolation:
		holder = 0;
		for (loop = 0; loop < HME_N; loop++) {
			for (loop3 = 0; loop3 < 2; loop3++) {
				for (loop1 = 0; loop1 < block; loop1++) {
					for (loop4 = 0; loop4 < depVar * 2; loop4 += 2) {
						gsl_interp_accel *lev_acc = gsl_interp_accel_alloc();
						gsl_spline *lev_spline = gsl_spline_alloc(gsl_interp_cspline, heightsIn);
						gsl_spline_init(lev_spline, height, interp_lat[loop][loop4 + loop3][loop1], heightsIn);

						for (loop2 = 0; loop2 < heightsOut; loop2++) {
							interp_lev[loop][loop4 + loop3][loop1][loop2] = gsl_spline_eval(lev_spline, 87.5 + (loop2 * 2.5), lev_acc);

						}

						gsl_spline_free (lev_spline);
						gsl_interp_accel_free (lev_acc);
					}
				}
			}
		}

		//Variance and deviance calculation (for normalizing different variables):
		for (loop1 = 0; loop1 < depVar; loop1++) {
			Variance[loop1] = TotalSumSqr[loop1] / (outrows/2);
			AbsDeviance[loop1] = TotalSN[loop1] / (outrows/2);
		}

		for (loop1 = 0; loop1 < depVar; loop1++) {
			Scale[loop1] =  sqrt(Variance[0]/ Variance[loop1]);
			DevScale[loop1] = AbsDeviance[0] / AbsDeviance[loop1];
		}

		//In interative fit, phases are held fixed while amplitudes are fit
		//Phases are incremented after each amplitude fit
		//Create an array with the minimum information from the independent matrix necessary to perform iterative fit:
		for (loop1 = 0; loop1 < HME_N; loop1++) {
			minIndep[loop1] = malloc(satrows * depVar * 2 * sizeof(double));
		}

		loop4 = 0;
		for (loop1 = 0; loop1 < heightsOut; loop1++) {
			for (loop3 = 0; loop3 < depVar; loop3++) {
				for (loop4 = 0; loop4 < block; loop4++) {
					for (loop5 = 0; loop5 < HME_N; loop5++) {
						for (loop = 0; loop < 2; loop++) {
							minIndep[loop5][(loop3 * 2 * satrows) + (block * 2 * loop1) + (loop4 * 2) + loop] = interp_lev[loop5][2 * loop3 + loop][block - 1 - loop4][loop1];
						}
					}
				}
			}
		}

		//Garbage collection after pre-processing:
		for (loop1 = 0; loop1 < HME_N; loop1++) {
			for (loop2 = 0; loop2 < depVar * 2; loop2++) {
				for (loop3 = 0; loop3 < HMEBlockCount; loop3++) {
					free(uninterp[loop1][loop2][loop3]);
				}
				for (loop3 = 0; loop3 < block; loop3++) {
					free(interp_lat[loop1][loop2][loop3]);
					free(interp_lev[loop1][loop2][loop3]);
				}
				free(uninterp[loop1][loop2]);
				free(interp_lat[loop1][loop2]);
				free(interp_lev[loop1][loop2]);
			}
		}

		//Dummy values in satellite data can not be passed to GSL fitting functions
		//Real and imaginary components alternate.
		//The dependent variables counted by "depVar" are concatenated as contigious blocks
		//Remove dummy values and concatenate satellite vectors;
		dependent = gsl_vector_alloc(2 * depVar * outrows);
		totalSumSqr = 0;
		for (loop2 = 0; loop2 < depVar; loop2++) {
			loop5 = 0;
			for (loop1 = 0; loop1 < satrows; loop1++) {
				if (gsl_matrix_get(satellite[0], loop1, 0) > -990) {
					if (gsl_matrix_get(satellite[1], loop1, 0) > -990) {
						if (gsl_matrix_get(satellite[2], loop1, 0) > -990) {
							for (loop3 = 0; loop3 < 2; loop3++) {
								scaledRow = Scale[loop2] * gsl_matrix_get(satellite[loop2], loop1, loop3);
								gsl_vector_set(dependent, (2 * outrows * loop2) + loop5 + loop3, scaledRow);
								totalSumSqr += scaledRow * scaledRow;
								for (loop4 = 0; loop4 < HME_N; loop4++){
									minIndep[loop4][(2 * outrows * loop2) + loop5 + loop3] = Scale[loop2] * minIndep[loop4][(2 * satrows * loop2) + 2 * loop1 + loop3];
								}
							}
							loop5+=2;
						}
					}
				}
			}
		}

		//Reallocate shrink minIndep once dummy values are removed:
		int *error_ptr;
		minIndepOffset = outrows * depVar * 2;
		for (loop1 = 0; loop1 < HME_N; loop1++) {
			 error_ptr = realloc(minIndep[loop1], sizeof(double) * minIndepOffset);
		}

		//Create a 1D array of the minimum data that needs to be passed to threads for iterative fit:
		threadIndep = malloc(sizeof(double) * minIndepOffset * HME_N);
		for (loop1 = 0; loop1 < HME_N; loop1++) {
			for (loop2 = 0; loop2 < minIndepOffset; loop2++) {
				threadIndep[minIndepOffset * loop1 + loop2] = minIndep[loop1][loop2];
			}
		}

		//Create an array with the full 'clean' independent data for an (analytical) R^2 fit to compare against:
		Full_Descrip = gsl_matrix_alloc((outrows * depVar) * 2, HME_N * 2);
		gsl_matrix *covariance = gsl_matrix_alloc(HME_N * 2, HME_N * 2);
		gsl_multifit_linear_workspace *modelspace = gsl_multifit_linear_alloc((outrows * depVar) * 2, HME_N * 2);
		gsl_vector *beta = gsl_vector_alloc(HME_N * 2);

		for (loop1 = 0; loop1 < 2 * outrows * depVar; loop1++) {
			for (loop2 = 0; loop2 < HME_N; loop2++) {
				for (loop3 = 0; loop3 < 2; loop3++) {
					if (loop3 && !(loop1 % 2)) {
						gsl_matrix_set(Full_Descrip, loop1, 2 * loop2 + loop3, -minIndep[loop2][loop1 + loop3]);
					}
					else {
						gsl_matrix_set(Full_Descrip, loop1, 2 * loop2 + loop3, minIndep[loop2][loop1 + loop3]);
					}
				}
			}
		}

		//'minIndep' is no longer needed
		//Release 'minIndep':
		for (loop1 = 0; loop1 < 2; loop1++) {
			free(minIndep[loop1]);
		}

		//Find R^2 fit for full independent array:
		double chiSqr;
		gsl_multifit_linear(Full_Descrip, dependent, beta, covariance, &chiSqr, modelspace);
		double coeff_of_determ = 1 - chiSqr/totalSumSqr;

		//Find sum of absolute norms of residuals for the returned least squares model (normalizing variables weights to new model moment):
		double sumNormResid[3] = {0, 0, 0};
		double R_norm = 0;
		double R_resid, I_resid;
		double Full_Beta[2 * HME_N];
		double amp_phase[2 * HME_N];
		for (loop1 = 0; loop1 < HME_N * 2; loop1++) {
			Full_Beta[loop1] = gsl_vector_get(beta, loop1);
		}

		for (loop1 = 0; loop1 < HME_N * 2; loop1+=2) {
			amp_phase[loop1] = sqrt(Full_Beta[loop1] * Full_Beta[loop1] + Full_Beta[loop1 + 1] * Full_Beta[loop1 + 1]);
			amp_phase[loop1+1] = atan(Full_Beta[loop1 + 1] / Full_Beta[loop1]);
		}

		int maxiter1 = 2 * outrows;
		int maxiter2 = maxiter1 * depVar;
		for (loop2 = 0; loop2 < maxiter2; loop2 += maxiter1) {
			for (loop1 = 0; loop1 < maxiter1; loop1 += 2) {
				R_resid = gsl_vector_get(dependent, loop1 + loop2);
				for (loop3 = 0; loop3 < HME_N * 2; loop3++) R_resid -= Full_Beta[loop3] * gsl_matrix_get(Full_Descrip, loop2 + loop1, loop3);
				I_resid = gsl_vector_get(dependent, loop1 + loop2 + 1);
				for (loop3 = 0; loop3 < HME_N * 2; loop3++) I_resid -= Full_Beta[loop3] * gsl_matrix_get(Full_Descrip, loop2 + loop1 + 1, loop3);
				sumNormResid[(int)(loop2/maxiter1)] += sqrt((R_resid * R_resid) + (I_resid * I_resid));
			}
		}

		for (loop1 = 0; loop1 < depVar; loop1++) R_norm -= sumNormResid[loop1]/TotalSN[loop1];
		R_norm = R_norm/3;
		R_norm += 1;

		//Pre-processing has completed

		//Start calling iterative fitting threads for R-norm:
		sem_wait(&threadsAvail);
		threadtracker[0] = 1;
		pthread_create(&zero,NULL,threadParallel, &threadID[0]);
		++pagein;

		sem_wait(&threadsAvail);
		threadtracker[1] = 1;
		pthread_create(&one,NULL,threadParallel, &threadID[1]);
		++pagein;

		sem_wait(&threadsAvail);
		threadtracker[2] = 1;
		pthread_create(&two,NULL,threadParallel, &threadID[2]);
		++pagein;

		sem_wait(&threadsAvail);
		threadtracker[3] = 1;
		pthread_create(&three,NULL,threadParallel, &threadID[3]);
		++pagein;

		//With every thread initially called, begin thread scheduling:

		maxpage = (PHASE_INT_INV) * (PHASE_INT_INV);

		comp = floor(maxpage/1000);
		comp_int = comp;

		while (pagein < maxpage) {

			sem_wait(&threadsAvail);
			if (!threadtracker[0]) {
				threadtracker[0] = 1;
				pthread_join(zero, NULL);
				pthread_create(&zero,NULL,threadParallel, &threadID[0]);
			}
			else if (!threadtracker[1]) {
				threadtracker[1] = 1;
				pthread_join(one, NULL);
				pthread_create(&one,NULL,threadParallel, &threadID[1]);
			}
			else if (!threadtracker[2]) {
				threadtracker[2] = 1;
				pthread_join(two, NULL);
				pthread_create(&two,NULL,threadParallel, &threadID[2]);
			}
			else {
				threadtracker[3] = 1;
				pthread_join(three, NULL);
				pthread_create(&three, NULL, threadParallel, &threadID[3]);
			}
			++pagein;

			//Output progress on percent or 1/10 of a percent of completion:
			if (pagein > comp) {
				comp = comp + comp_int;
				++tenth_percent_complete;
				if (tenth_percent_complete > 9) {
					++percent_complete;
					tenth_percent_complete = 0;
					//printf("Phase intercept 1: %f \n", BestPhase1);
					//printf("Phase intercept 2: %f \n", BestPhase2);
					//printf("R Squared average: %f \n", BestResid);
					//printf("%d.%d percent complete \n \n", percent_complete, tenth_percent_complete);
				}

				printf("Amplitude 1: %f \n", gsl_vector_get(BestModel, 0));
				printf("Phase intercept 1: %f \n", BestPhase1);
				printf("Amplitude 2: %f \n", gsl_vector_get(BestModel, 1));
				printf("Phase intercept 2: %f \n", BestPhase2);
				printf("Best R norm: %f \n", BestRNorm);
				printf("Corresponding R^2: %f \n", BestResid);
				printf("Best R^2: %f \n", coeff_of_determ);
				printf("Best R^2 Amplitude 1: %f \n", amp_phase[0]);
				printf("Best R^2 Phase intercept 1: %f \n", amp_phase[1]);
				printf("Best R^2 Amplitude 2: %f \n", amp_phase[2]);
				printf("Best R^2 Phase intercept 2: %f \n", amp_phase[3]);
				printf("R norm corresponding to analytical best R^2: %f \n", R_norm);
				printf("%d.%d percent complete \n \n", percent_complete, tenth_percent_complete);

			}
		}

		//All iterations have been called
		//Join last calls:
		pthread_join(zero, NULL);
		pthread_join(one, NULL);
		pthread_join(two, NULL);
		pthread_join(three, NULL);

		//Print final result:
		printf("Best phase 1 intercept: %f \n", BestPhase1);
		printf("Best phase 2 intercept: %f \n", BestPhase2);
		printf("Best R norm: %f \n", BestRNorm);
		printf("Best corresponding R squared: %f \n", BestResid);
		printf("Corresponding R^2: %f \n", BestResid);
		printf("Best R^2: %f \n", coeff_of_determ);
		printf("Best R^2 Amplitude 1: %f \n", amp_phase[0]);
		printf("Best R^2 Phase intercept 1: %f \n", amp_phase[1]);
		printf("Best R^2 Amplitude 2: %f \n", amp_phase[2]);
		printf("Best R^2 Phase intercept 2: %f \n", amp_phase[3]);
		printf("R norm corresponding to analytical best R^2: %f \n", R_norm);

		//Free remaining manually allocated memory objects:
		for (loop1 = 0; loop1 < depVar; loop1++) {
			gsl_matrix_free(satellite[loop1]);
		}
		gsl_vector_free(dependent);
		gsl_vector_free(BestModel);
		gsl_matrix_free(Full_Descrip);

	}
	return 0;
}


//IN method.
//Bilinear transformation (3D tensor of weights, 1D vector of biases)
//of Ni1 inputs from X1 and Ni2 inputs from X2 to No outputs in Y.
//This version uses CBLAS.

//Input X1 has Ni1 neurons, X2 has Ni2 neurons, and output Y has No neurons.

//If col-major: Y[o,l] = X1[:,l]' * W[:,:,o] * X2[:,l] + B[o]
//where:
//X1 has size Ni1 x L
//X2 has size Ni2 x L
//Y  has size No x L
//W  has size Ni1 x Ni2 x No
//B  has size No x 1

//If row-major: Y[l,o] = X1[l,:] * W[o,:,:] * X2[l,:]' + B[o]
//X1 has size L x Ni1
//X2 has size L x Ni2
//Y  has size L x No
//W  has size No x Ni2 x Ni1
//B  has size 1 x No

//This is equal to the outer product of X1 and X2 at each l (time point),
//and then take the weighted sum by each of the No matrices within W,
//producing No scalars, one for each output neuron in Y.

//I retain the for loop through L for compatibility with real-time streaming.

//This CBLAS version not working yet!!

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

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int bilinear_cblas_s (float *Y, const float *X1, const float *X2, const float *W, const float *B, const size_t Ni1, const size_t Ni2, const size_t No, const size_t L);
int bilinear_cblas_d (double *Y, const double *X1, const double *X2, const double *W, const double *B, const size_t Ni1, const size_t Ni2, const size_t No, const size_t L);
int bilinear_cblas_c (float *Y, const float *X1, const float *X2, const float *W, const float *B, const size_t Ni1, const size_t Ni2, const size_t No, const size_t L);
int bilinear_cblas_z (double *Y, const double *X1, const double *X2, const double *W, const double *B, const size_t Ni1, const size_t Ni2, const size_t No, const size_t L);

int bilinear_cblas_s (float *Y, const float *X1, const float *X2, const float *W, const float *B, const size_t Ni1, const size_t Ni2, const size_t No, const size_t L)
{
    const size_t N12 = Ni1*Ni2, Nw = N12*No;

    float *outer_prod;
    outer_prod = (float *)malloc(N12*sizeof(float));
    if (!outer_prod) { fprintf(stderr,"error in bilinear_cblas_s: problem with malloc. "); perror("malloc"); return 1; }

    for (size_t l=L; l>0u; --l, X1+=Ni1, X2+=Ni2, B-=No, W-=Nw)
    {
        //Re-zero outer_prod
        for (size_t n=N12; n>0u; --n) { outer_prod[n] = 0.0f; }

        //Outer product of X1, X2
        cblas_sger(CblasRowMajor,(int)Ni1,(int)Ni2,1.0f,X1,1,X2,1,outer_prod,(int)Ni2);

        //Weight by W for each output
        for (size_t o=No; o>0u; --o, ++B, ++Y, W+=N12)
        {
            *Y = *B + cblas_sdot((int)N12,W,1,outer_prod,1);
            //*Y = cblas_sdsdot((int)N12,*B,W,1,outer_prod,1);
        }
    }

    free(outer_prod);

    return 0;
}

int bilinear_cblas_d (double *Y, const double *X1, const double *X2, const double *W, const double *B, const size_t Ni1, const size_t Ni2, const size_t No, const size_t L)
{
    const size_t N12 = Ni1*Ni2, Nw = N12*No;

    double *outer_prod;
    outer_prod = (double *)malloc(N12*sizeof(double));
    if (!outer_prod) { fprintf(stderr,"error in bilinear_cblas_d: problem with malloc. "); perror("malloc"); return 1; }

    for (size_t l=L; l>0u; --l, X1+=Ni1, X2+=Ni2, B-=No, W-=Nw)
    {
        //Re-zero outer_prod
        for (size_t n=N12; n>0u; --n) { outer_prod[n] = 0.0; }

        //Outer product of X1, X2
        cblas_dger(CblasRowMajor,(int)Ni1,(int)Ni2,1.0,X1,1,X2,1,outer_prod,(int)Ni2);

        //Weight by W for each output
        for (size_t o=No; o>0u; --o, ++B, ++Y, W+=N12)
        {
            *Y = *B + cblas_ddot((int)N12,W,1,outer_prod,1);
        }
    }

    free(outer_prod);

    return 0;
}

int bilinear_cblas_c (float *Y, const float *X1, const float *X2, const float *W, const float *B, const size_t Ni1, const size_t Ni2, const size_t No, const size_t L)
{
    const size_t N12 = 2u*Ni1*Ni2, Nw = N12*No;
    const float oz[2] = {1.0f,0.0f};

    float *outer_prod;
    outer_prod = (float *)malloc(N12*sizeof(float));
    if (!outer_prod) { fprintf(stderr,"error in bilinear_cblas_c: problem with malloc. "); perror("malloc"); return 1; }

    for (size_t l=L; l>0u; --l, X1+=2u*Ni1, X2+=2u*Ni2, B-=2u*No, W-=Nw)
    {
        //Re-zero outer_prod
        for (size_t n=N12; n>0u; --n) { outer_prod[n] = 0.0f; }

        //Outer product of X1, X2
        cblas_cgeru(CblasRowMajor,(int)Ni1,(int)Ni2,oz,X1,1,X2,1,outer_prod,(int)Ni2);

        //Weight by W for each output
        for (size_t o=No; o>0u; --o, W+=N12)
        {
            cblas_cdotu_sub((int)N12,W,1,outer_prod,1,Y);
            *Y++ += *B++; *Y++ += *B++;
        }
    }

    free(outer_prod);

    return 0;
}

int bilinear_cblas_z (double *Y, const double *X1, const double *X2, const double *W, const double *B, const size_t Ni1, const size_t Ni2, const size_t No, const size_t L)
{
    const size_t N12 = 2u*Ni1*Ni2, Nw = N12*No;
    const double oz[2] = {1.0,0.0};

    double *outer_prod;
    outer_prod = (double *)malloc(N12*sizeof(double));
    if (!outer_prod) { fprintf(stderr,"error in bilinear_cblas_z: problem with malloc. "); perror("malloc"); return 1; }

    for (size_t l=L; l>0u; --l, X1+=2u*Ni1, X2+=2u*Ni2, B-=2u*No, W-=Nw)
    {
        //Re-zero outer_prod
        for (size_t n=N12; n>0u; --n) { outer_prod[n] = 0.0; }

        //Outer product of X1, X2
        cblas_zgeru(CblasRowMajor,(int)Ni1,(int)Ni2,oz,X1,1,X2,1,outer_prod,(int)Ni2);

        //Weight by W for each output
        for (size_t o=No; o>0u; --o, W+=N12)
        {
            cblas_zdotu_sub((int)N12,W,1,outer_prod,1,Y);
            *Y++ += *B++; *Y++ += *B++;
        }
    }

    free(outer_prod);

    return 0;
}

#ifdef __cplusplus
}
}
#endif


static char help[] =
"Solve a hyperbolic system in one space dimension (1D):\n"
"    q_t + F(t,x,q)_x = g(t,x,q)\n"
"where solution q(t,x), flux F(t,x,q), and source g(t,x,q) are column vectors\n"
"of length n.  The domain is (t,x) in [0,T] x [a,b].  The initial condition is\n"
"q(0,x) = f(x).  The flux may be of the form\n"
"    F(t,x,q) = A(t,x,q) q\n"
"but this is not required.  Flux boundary conditions are used in all cases,\n"
"    F = bdryflux_a(t,qright)\n"
"    F = bdryflux_b(t,qleft)\n"
"including nonreflecting, reflecting, outflow, and periodic boundary\n"
"conditions.\n\n"
"Uses a finite volume discretization so the grid values represent cell\n"
"averages.  Each case implements a Riemann solver\n"
"    F = faceflux(t,x,qleft,qright)\n"
"at cell faces.  (The Riemann solver computes the value of the solution on\n"
"the cell face going forward in time.)  Implements the following\n"
"slope-limiters when computing fluxes:\n"
"    -limiter none       Godunov's method, i.e. first-order upwinding\n"
"    -limiter fromm      formula (6.14) in LeVeque 2002\n"
"    -limiter mc         formula (6.29)\n"
"    -limiter minmod     formula (6.26)\n"
"Control the spatial grid by PETSc option\n"
"    -da_grid_x M                             [grid of M cells/points]\n\n"
"Time stepping is by semi-discretization in space (method of lines) and then\n"
"application of PETSc's (generally) adaptive and higher-order TS solvers.\n"
"Control time stepping and solution information by these PETSc options, among\n"
"others:\n"
"    -ts_monitor                              [shows time steps]\n"
"    -ts_monitor_solution draw                [generate simple movie]\n"
"        -draw_pause 0.1 -draw_size 2000,200  [control the movie]\n"
"    -ts_type                                 [default is rk]\n"
"        -ts_rk_type X                        [default is 3bs]\n"
"    -ts_dt 0.01 -ts_adapt_type none          [turn off adaptive]\n"
"Note that TS solver type SSP (-ts_type ssp) is recommended for these\n"
"hyperbolic problems.\n\n"
"Use option -problem selects the problem case:\n"
"    -problem acoustic   wave equation in system form (n=2) [default]\n"
"    -problem advection  scalar advection equation (n=1)\n"
"    -problem swater     shallow water equations (n=2)\n"
"    -problem traffic    scalar, nonlinear traffic equation (n=1)\n"
"To see possible initial conditions for problem X see the corresponding\n"
".h file.  Based on the problem-specific possiblities, set\n"
"    -initial Y\n\n"
"See the makefile for test examples, and do 'make test' to test.\n\n"
"This program is documented by the slides in the fvolume/ directory at\n"
"https://github.com/bueler/slide-teach\n\n";

#include <petsc.h>

/* The struct "ProblemCtx" is defined in cases.h.  The comments in cases.h
show how to add new problems. */
#include "cases.h"

// minmod(a,b) as define on LeVeque page 111
static PetscReal minmod(PetscReal a, PetscReal b) {
    if (a*b > 0) // both signs agree
        return (PetscAbs(a) < PetscAbs(b)) ? a : b;
    else
        return 0.0;
}

typedef enum {NONE,FROMM,MC,MINMOD} LimiterType;
static const char* LimiterTypes[] = {"none","fromm","mc","minmod",
                                     "LimiterType", "", NULL};

static LimiterType limiter = NONE;     // slope-limiter

extern PetscErrorCode FormInitial(DMDALocalInfo*, Vec, PetscReal, ProblemCtx*);
extern PetscErrorCode GetMaxSpeed(DMDALocalInfo*, Vec, PetscReal, PetscReal*, ProblemCtx*);
extern PetscErrorCode FormRHSFunctionLocal(DMDALocalInfo*, PetscReal,
        PetscReal*, PetscReal*, void*);

int main(int argc,char **argv) {
    PetscErrorCode   ierr;
    TS               ts;                 // ODE solver for method-of-lines (MOL)
    DM               da;                 // structured grid
    Vec              q;                  // the solution
    DMDALocalInfo    info;               // structured grid info
    ProblemType      problem = ACOUSTIC; // which problem we are solving
    ProblemCtx       user;               // problem-specific information
    PetscInt         swidth, k, steps;
    PetscBool        flg;
    PetscReal        hx, qmin, qmax, t0, tf, dt, c;

    PetscInitialize(&argc,&argv,(char*)0,help);

    // get which problem we are solving
    // (ProblemType, ProblemTypes, InitializerPtrs are defined in cases.h)
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"",
               "riemann (hyperbolic system solver) options",""); CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-problem", "problem type",
               "riemann.c",ProblemTypes,(PetscEnum)(problem),(PetscEnum*)&problem,
               NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-limiter", "limiter type",
               "riemann.c",LimiterTypes,(PetscEnum)(limiter),(PetscEnum*)&limiter,
               NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    // call the initializer for the given case
    // (it allocates list of strings in user->field_names thus PetscFree below)
    ierr = (*InitializerPtrs[problem])(&user); CHKERRQ(ierr);

    // create grid
    swidth = (limiter == NONE) ? 1 : 2;
    ierr = DMDACreate1d(PETSC_COMM_WORLD,
                        user.periodic_bcs ? DM_BOUNDARY_PERIODIC : DM_BOUNDARY_NONE,
                        4,          // default resolution
                        user.n_dim, // system dimension (d.o.f.)
                        swidth,     // stencil (half) width
                        NULL,&da); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    hx = (user.b_right - user.a_left) / info.mx;
    ierr = DMDASetUniformCoordinates(da,user.a_left+hx/2.0,user.b_right-hx/2.0,
                                     0.0,1.0,0.0,1.0); CHKERRQ(ierr);

    // set field names so that visualization makes sense
    for (k = 0; k < info.dof; k++) {
        ierr = DMDASetFieldName(da,k,(user.field_names)[k]); CHKERRQ(ierr);
    }

    // create TS:  dq/dt = G(t,q)  form
    ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
    ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);
    ierr = TSSetDM(ts,da); CHKERRQ(ierr);
    ierr = DMDATSSetRHSFunctionLocal(da,INSERT_VALUES,
           (DMDATSRHSFunctionLocal)FormRHSFunctionLocal,&user); CHKERRQ(ierr);
    ierr = TSSetType(ts,TSRK); CHKERRQ(ierr);  // defaults to -ts_rk_type 3bs

    // set up time axis
    ierr = TSSetTime(ts,user.t0_default); CHKERRQ(ierr);
    ierr = TSSetMaxTime(ts,user.tf_default); CHKERRQ(ierr);
    dt = user.tf_default - user.t0_default;
    ierr = TSSetTimeStep(ts,dt); CHKERRQ(ierr);  // usually reset below
    ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);
    ierr = TSSetFromOptions(ts); CHKERRQ(ierr);

    // get initial values
    ierr = DMCreateGlobalVector(da,&q); CHKERRQ(ierr);
    ierr = TSGetTime(ts,&t0); CHKERRQ(ierr);
    ierr = FormInitial(&info,q,t0,&user); CHKERRQ(ierr);
    //ierr = VecView(q,PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);

    // use CFL to reset initial time-step dt (unless user sets)
    ierr = PetscOptionsHasName(NULL,NULL,"-ts_dt",&flg); CHKERRQ(ierr);
    ierr = GetMaxSpeed(&info,q,t0,&c,&user); CHKERRQ(ierr);
    if (!flg && c > 0.0) {
        ierr = TSGetMaxTime(ts,&tf); CHKERRQ(ierr);
        dt = PetscMin(hx / c, tf-t0);
        ierr = TSSetTimeStep(ts,dt); CHKERRQ(ierr);
    } else {
        ierr = TSGetTimeStep(ts,&dt); CHKERRQ(ierr);
    }

    // solve
    ierr = PetscPrintf(PETSC_COMM_WORLD,
               "solving problem %s, a system of %d equations,\n"
               "  on %d-point grid with dx=%.6f and initial dt=%.6f...\n",
               ProblemTypes[problem],info.dof,info.mx,hx,dt); CHKERRQ(ierr);
    ierr = TSSolve(ts,q); CHKERRQ(ierr);

    // report on solution
    ierr = TSGetStepNumber(ts,&steps); CHKERRQ(ierr);
    ierr = TSGetTime(ts,&tf); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
               "  ... completed %d steps for %.4f <= t <= %.4f\n",
               steps,t0,tf); CHKERRQ(ierr);
    for (k = 0; k < info.dof; k++) {
        ierr = VecStrideMin(q,k,NULL,&qmin); CHKERRQ(ierr);
        ierr = VecStrideMax(q,k,NULL,&qmax); CHKERRQ(ierr);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
               "  range of %d component: %8.5f <= %s <= %8.5f\n",
               k,qmin,(user.field_names)[k],qmax); CHKERRQ(ierr);
    }

    // free memory
    VecDestroy(&q);  TSDestroy(&ts);  DMDestroy(&da);
    ierr = PetscFree(user.field_names); CHKERRQ(ierr);
    return PetscFinalize();
}

PetscErrorCode FormInitial(DMDALocalInfo *info, Vec q, PetscReal t0, ProblemCtx *user) {
    PetscErrorCode   ierr;
    const PetscReal  hx = (user->b_right - user->a_left) / info->mx;
    PetscInt         j;
    PetscReal        x, *aq;

    ierr = DMDAVecGetArray(info->da, q, &aq); CHKERRQ(ierr);
    for (j=info->xs; j<info->xs+info->xm; j++) {
        x = user->a_left + (j+0.5) * hx;
        ierr = user->f_initial(t0,x,&aq[(info->dof)*j]); CHKERRQ(ierr);
    }
    ierr = DMDAVecRestoreArray(info->da, q, &aq); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode GetMaxSpeed(DMDALocalInfo *info, Vec q, PetscReal t,
                           PetscReal *maxspeed, ProblemCtx *user) {
    PetscErrorCode   ierr;
    const PetscReal  hx = (user->b_right - user->a_left) / info->mx;
    PetscInt         j;
    PetscReal        x, cj, locmax, *aq;
    MPI_Comm         comm;

    ierr = DMDAVecGetArray(info->da, q, &aq); CHKERRQ(ierr);
    locmax = 0.0;
    for (j=info->xs; j<info->xs+info->xm; j++) {
        x = user->a_left + (j+0.5) * hx;
        ierr = user->maxspeed(t,x,&aq[(info->dof)*j],&cj); CHKERRQ(ierr);
        locmax = PetscMax(locmax,cj);
    }
    ierr = DMDAVecRestoreArray(info->da, q, &aq); CHKERRQ(ierr);
    ierr = PetscObjectGetComm((PetscObject)info->da,&comm); CHKERRQ(ierr);
    ierr = MPI_Allreduce(&locmax,maxspeed,1,MPIU_REAL,MPIU_MAX,comm); CHKERRQ(ierr);
    return 0;
}

static inline void ncopy(PetscInt n, PetscReal *src, PetscReal *tgt) {
    PetscInt k;
    for (k = 0; k < n; k++)
        tgt[k] = src[k];
}

static inline void slopemodify(PetscInt n, PetscReal C,
                               PetscReal *sig, PetscReal *Q_in,
                               PetscReal *Q_out) {
    PetscInt k;
    for (k = 0; k < n; k++)
        Q_out[k] = Q_in[k] + C * sig[k];
}

// Right-hand-side of method-of-lines discretization form of PDE.  Implements
// Gudonov (i.e. Riemann-solver upwind) method with a slope limiter.
PetscErrorCode FormRHSFunctionLocal(DMDALocalInfo *info, PetscReal t,
        PetscReal *aq, PetscReal *aG, void *ctx) {
    PetscErrorCode   ierr;
    ProblemCtx       *user = (ProblemCtx*)ctx;
    const PetscInt   n = info->dof;
    const PetscReal  hx = (user->b_right - user->a_left) / info->mx;
    Vec              sig;
    PetscInt         j, k;
    PetscReal        x, *asig, sl, sr,
                     *Ql, *Qr,  // slope-limited values of solution at either
                                //     side of current face
                     *Fl, *Fr;  // face fluxes on either end of current cell

    // for each owned cell get slope using the slope-limiter
    ierr = DMCreateLocalVector(info->da,&sig); CHKERRQ(ierr);
    ierr = VecSet(sig,0.0); CHKERRQ(ierr);  // implements limiter == NONE
    ierr = DMDAVecGetArray(info->da,sig,&asig); CHKERRQ(ierr);
    if (limiter != NONE) {  // following block assumes swidth >= 2
        for (j = info->xs-1; j < info->xs + info->xm+1; j++) {   // x_j is cell center
            for (k = 0; k < n; k++) {
                if ((j < 0 || j > info->mx-1) && user->periodic_bcs == PETSC_FALSE)
                    continue;
                if (j == 0 && user->periodic_bcs == PETSC_FALSE)
                    asig[n*j+k] = (aq[n*(j+1) + k] - aq[n*j + k]) / hx;
                else if (j == info->mx-1 && user->periodic_bcs == PETSC_FALSE)
                    asig[n*j+k] = (aq[n*j + k] - aq[n*(j-1) + k]) / hx;
                else {
                    if (limiter == FROMM) {
                        asig[n*j+k] = (aq[n*(j+1) + k] - aq[n*(j-1) + k]) / (2.0 * hx);
                    } else {
                        sr = (aq[n*(j+1) + k] - aq[n*j + k]) / hx;
                        sl = (aq[n*j + k] - aq[n*(j-1) + k]) / hx;
                        if (limiter == MINMOD)
                            asig[n*j+k] = minmod(sl,sr);
                        else if (limiter == MC)
                            asig[n*j+k] = minmod(0.5*(sl+sr),2.0*minmod(sl,sr));
                        else {
                            SETERRQ(PETSC_COMM_SELF,1,"how did I get here?\n");
                        }
                    }
                }
            }
        }
    }

    // get left-face flux Fl for first cell owned by process; may be at x=a
    ierr = PetscMalloc4(n,&Ql,n,&Qr,n,&Fl,n,&Fr); CHKERRQ(ierr);
    if (info->xs == 0 && user->periodic_bcs == PETSC_FALSE) {
        // use right slope
        slopemodify(n,-hx/2.0,&asig[0],&aq[0],Qr);
        ierr = user->bdryflux_a(t,hx,Qr,Fl); CHKERRQ(ierr);
    } else {
        // use left and right (limited) slope [left is owned by other process]
        x = user->a_left + (info->xs+0.5) * hx;
        slopemodify(n,hx/2.0,&asig[n*(info->xs-1)],&aq[n*(info->xs-1)],Ql);
        slopemodify(n,-hx/2.0,&asig[n*(info->xs)],&aq[n*(info->xs)],Qr);
        ierr = user->faceflux(t,x-hx/2.0,Ql,Qr,Fl); CHKERRQ(ierr);
    }

    // for each owned cell, compute RHS  G(t,x,q)
    for (j = info->xs; j < info->xs + info->xm; j++) {   // x_j is cell center
        x = user->a_left + (j+0.5) * hx;
        // set aG[n j + k] = g(t,x_j,u)_k
        ierr = user->g_source(t,x,&aq[n*j],&aG[n*j]); CHKERRQ(ierr);
        // get right-face flux Fr for cell; may be at x=b
        if (j == info->mx-1 && user->periodic_bcs == PETSC_FALSE) {
            // user left slope
            slopemodify(n,hx/2.0,&asig[n*j],&aq[n*j],Ql);
            ierr = user->bdryflux_b(t,hx,Ql,Fr); CHKERRQ(ierr);
        } else {
            // use left and right (limited) slope; see formulas LeVeque page 193
            slopemodify(n,hx/2.0,&asig[n*j],&aq[n*j],Ql);
            slopemodify(n,-hx/2.0,&asig[n*(j+1)],&aq[n*(j+1)],Qr);
            ierr = user->faceflux(t,x+hx/2.0,Ql,Qr,Fr); CHKERRQ(ierr);
        }
        // complete the RHS:
        //   aG[n j + k] = g(t,x_j,u)_k + (F_{j-1/2}_k - F_{j+1/2}_k) / hx
        for (k = 0; k < n; k++)
            aG[n*j+k] += (Fl[k] - Fr[k]) / hx;
        ncopy(n,Fr,Fl); // transfer Fr to Fl, for next loop
    }

    // clean up
    ierr = PetscFree4(Ql,Qr,Fl,Fr); CHKERRQ(ierr);
    ierr = DMDAVecRestoreArray(info->da,sig,&asig); CHKERRQ(ierr);
    ierr = VecDestroy(&sig); CHKERRQ(ierr);
    return 0;
}


/* siman/siman_test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Mark Galassi
 *
 * 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 <math.h>
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_siman.h>
#include <gsl/gsl_ieee_utils.h>

/* A cute example of this program to then look at stuff would be:
   ./siman_test D3 | grep -v "^#" | awk '{print $3}' | sed 's/::/ /gp' > output

  then go into gnuplot and try typing:
  gnuplot> set parametric
  gnuplot> splot 'output' with lines

  Or you can plot from a pipe:
  gnuplot> plot '<../siman/siman_test | grep -v "^#"' using 1:2 with lines
  gnuplot> plot '<../siman/siman_test | grep -v "^#"' using 1:3 with lines
  and so forth.  */

/* set up parameters for this simulated annealing run */

#define N_TRIES 200		/* how many points do we try before stepping */
#define ITERS_FIXED_T 1000	/* how many iterations for each T? */
#define STEP_SIZE 1.0		/* max step size in random walk */
#define K 1.0			/* Boltzmann constant */
#define T_INITIAL 0.008		/* initial temperature */
#define MU_T 1.003		/* damping factor for temperature */
#define T_MIN 2.0e-6

gsl_siman_params_t params = {N_TRIES, ITERS_FIXED_T, STEP_SIZE,
			     K, T_INITIAL, MU_T, T_MIN};

double square(double x);

double test_E_1D(Element x);
void test_step_1D(const gsl_rng * r, Element *x_p, double step_size);
void print_pos_1D(Element x);
double distance_1D(Element x, Element y);
double test_E_2D(Element x);
void test_step_2D(const gsl_rng * r, Element *x_p, double step_size);
void print_pos_2D(Element x);
double distance_2D(Element x, Element y);
double test_E_3D(Element x);
void test_step_3D(const gsl_rng * r, Element *x_p, double step_size);
void print_pos_3D(Element x);
double distance_3D(Element x, Element y);
double E1(void *xp);
double M1(void *xp, void *yp);
void S1(const gsl_rng * r, void *xp, double step_size);
void P1(void *xp);

int main(int argc, char *argv[])
{
  Element x0;			/* initial guess for search */

/*   double x_initial = 2.5; */
  double x_initial = -10.0;

  gsl_rng * r = gsl_rng_alloc (gsl_rng_env_setup()) ;

  gsl_ieee_env_setup ();

  gsl_siman_solve(r, &x_initial, E1, S1, M1, P1, NULL, NULL, NULL,
		  sizeof(double), params);

  return 0;

  if (argc != 2) {
/*     fprintf(stderr, "usage: %s [D1, D2, D3, CA]\n", argv[0]); */
    fprintf(stderr, "usage: %s [D1 | D2 | D3]\n", argv[0]);
    return 1;
  }

  printf("#testing the simulated annealing routines\n");
  if (strcmp(argv[1], "D1") == 0) {
    x0.D1 = 12.0;
    printf("#one dimensional problem, x0 = %f\n", x0.D1);
/*     gsl_siman_Usolve(r, &x0, test_E_1D, test_step_1D, distance_1D, */
/* 		    print_pos_1D, params); */
    return 0;
  }

  if (strcmp(argv[1], "D2") == 0) {
    x0.D2[0] = 12.0;
    x0.D2[1] = 5.5;
    printf("#two dimensional problem, (x0,y0) = (%f,%f)\n",
	   x0.D2[0], x0.D2[1]);
/*     gsl_siman_Usolve(r, &x0, test_E_2D, test_step_2D, distance_2D, */
/* 		    print_pos_2D, params); */
    return 0;
  }

  if (strcmp(argv[1], "D3") == 0) {
    x0.D3[0] = 12.2;
    x0.D3[1] = 5.5;
    x0.D3[2] = -15.5;
    printf("#three dimensional problem, (x0,y0,z0) = (%f,%f,%f)\n",
	   x0.D3[0], x0.D3[1], x0.D3[2]);
/*     gsl_siman_Usolve(r, &x0, test_E_3D, test_step_3D, distance_3D, */
/* 		    print_pos_3D, params); */
  }
/*
  x0.D2[0] = 12.2;
  x0.D2[1] = 5.5;

  gsl_siman_solve(r, &x0, test_E_2D, test_step_2D, distance_2D, print_pos_2D, params);
*/
/*
  x0.D3[0] = 12.2;
  x0.D3[1] = 5.5;
  x0.D3[2] = -15.5;

  gsl_siman_solve(r, &x0, test_E_3D, test_step_3D, distance_3D, print_pos_3D, params);
  */

  return 0;
}

double square(double x)
{
  return x*x;
}

double test_E_1D(Element x)
{
  double val = x.D1;
/*return sin(sin(val*val) - cos(val)) + cos(sin(val) + sin(val)*sin(val));*/
  return exp(-square(val-1))*sin(8*val);
/*  return 1.0/(square(x-1.0) + 1.0)*sin(8.0*x); */
/*  return 1.0/(square(x-1.0) + 1.0)*sin(8.0*x) + x*x/100.0; */
}

/* takes a step for the test function; max distance: step_size.
 * the new point is put in x_p and returned.
 */
void test_step_1D(const gsl_rng * r, Element *x_p, double step_size)
{
  double old_x = x_p->D1;
  double new_x;

  new_x = gsl_rng_uniform(r);
  new_x = new_x*2*step_size;
  new_x = new_x - step_size + old_x;

  x_p->D1 = new_x;
}

/* simple routine to print out a position value */
void print_pos_1D(Element x)
{
  printf("%12g", x.D1);
}

/* a metric for the 2D space */
double distance_1D(Element x, Element y)
{
  return fabs(y.D1 - x.D1);
}

/* a 2-D function to be minimized */
double test_E_2D(Element x)
{
  double old_x = x.D2[0], old_y = x.D2[1];

  return exp(-square(old_x-1) - square(old_y - 0.8))*sin(8*old_x + 8 * old_y);
}

/* takes a step for the test function; max distance: step_size.  the
   new point is put in x_p and returned. */
void test_step_2D(const gsl_rng * r, Element *x_p, double step_size)
{
  double old_x =  x_p->D2[0], old_y = x_p->D2[1], new_x, new_y;

  new_x = gsl_rng_uniform(r);
  new_x = new_x*2*step_size;
  new_x = new_x - step_size + old_x;

  new_y = gsl_rng_uniform(r);
  new_y = new_y*2*step_size;
  new_y = new_y - step_size + old_y;

  x_p->D2[0] = new_x;
  x_p->D2[1] = new_y;
}

/* simple routine to print out a position value */
void print_pos_2D(Element x)
{
  printf("%g::%g", x.D2[0], x.D2[1]);
}

/* a metric for the 2D space */
double distance_2D(Element x, Element y)
{
  return sqrt(square(y.D2[0]-x.D2[0]) + square(y.D2[1]-x.D2[1]));
}

/**********************************************/
/************ 3-dimensional search ************/
/**********************************************/

/* a 3-D function to be minimized */
double test_E_3D(Element x)
{
  return exp(-square(x.D3[0]-1) - square(x.D3[1] - 0.8)
    - square(x.D3[2] - 0.8)) * sin(8*x.D3[0] + 8*x.D3[1] + 8*x.D3[2])
      + (square(x.D3[0]) + square(x.D3[1]) + square(x.D3[2]))/10000.0;
}

/* takes a step for the test function; max distance: step_size.
 * the new point is put in x_p and returned.
 */
void test_step_3D(const gsl_rng * r, Element *x_p, double step_size)
{
  double old_x = x_p->D3[0], old_y = x_p->D3[1], old_z = x_p->D3[2];
  double new_x, new_y, new_z;

  new_x = gsl_rng_uniform(r);
  new_x = new_x*2*step_size;
  new_x = new_x - step_size + old_x;

  new_y = gsl_rng_uniform(r);
  new_y = new_y*2*step_size;
  new_y = new_y - step_size + old_y;

  new_z = gsl_rng_uniform(r);
  new_z = new_z*2*step_size;
  new_z = new_z - step_size + old_z;

  x_p->D3[0] = new_x;
  x_p->D3[1] = new_y;
  x_p->D3[2] = new_z;
}

/* simple routine to print out a position value */
void print_pos_3D(Element x)
{
  printf("%g::%g::%g", x.D3[0], x.D3[1], x.D3[2]);
}

/* a metric for the 2D space */
double distance_3D(Element x, Element y)
{
  return sqrt(square(y.D3[0]-x.D3[0]) + square(y.D3[1]-x.D3[1])
	      + square(y.D3[2]-x.D3[2]));
}

/* now some functions to test in one dimension */
double E1(void *xp)
{
  double x = * ((double *) xp);

  return exp(-square(x-1))*sin(8*x) - exp(-square(x-1000))*0.89;
/*   return exp(-square(x-1))*sin(8*x); */
}

double M1(void *xp, void *yp)
{
  double x = *((double *) xp);
  double y = *((double *) yp);

  return fabs(x - y);
}

void S1(const gsl_rng * r, void *xp, double step_size)
{
  double old_x = *((double *) xp);
  double new_x;

  new_x = gsl_rng_uniform(r)*2*step_size - step_size + old_x;
/*   new_x = new_x*2*step_size; */
/*   new_x = new_x - step_size + old_x; */

  memcpy(xp, &new_x, sizeof(new_x));
}

void P1(void *xp)
{
  printf(" %12g ", *((double *) xp));
}


/* dht/dht.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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.
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf_bessel.h>
#include "gsl_dht.h"

gsl_dht *
gsl_dht_alloc (size_t size)
{
  gsl_dht * t;

  if(size == 0) {
    GSL_ERROR_VAL("size == 0", GSL_EDOM, 0);
  }

  t = (gsl_dht *)malloc(sizeof(gsl_dht));

  if(t == 0) {
    GSL_ERROR_VAL("out of memory", GSL_ENOMEM, 0);
  }

  t->size = size;

  t->xmax = -1.0; /* Make it clear that this needs to be calculated. */
  t->nu   = -1.0;

  t->j = (double *)malloc((size+2)*sizeof(double));

  if(t->j == 0) {
    free(t);
    GSL_ERROR_VAL("could not allocate memory for j", GSL_ENOMEM, 0);
  }

  t->Jjj = (double *)malloc(size*(size+1)/2 * sizeof(double));

  if(t->Jjj == 0) {
    free(t->j);
    free(t);
    GSL_ERROR_VAL("could not allocate memory for Jjj", GSL_ENOMEM, 0);
  }

  t->J2 = (double *)malloc((size+1)*sizeof(double));

  if(t->J2 == 0) {
    free(t->Jjj);
    free(t->j);
    free(t);
    GSL_ERROR_VAL("could not allocate memory for J2", GSL_ENOMEM, 0);
  }

  return t;
}

/* Handle internal calculation of Bessel zeros. */
static int
dht_bessel_zeros(gsl_dht * t)
{
  unsigned int s;
  gsl_sf_result z;
  int stat_z = 0;
  t->j[0] = 0.0;
  for(s=1; s < t->size + 2; s++) {
    stat_z += gsl_sf_bessel_zero_Jnu_e(t->nu, s, &z);
    t->j[s] = z.val;
  }
  if(stat_z != 0) {
    GSL_ERROR("could not compute bessel zeroes", GSL_EFAILED);
  }
  else {
    return GSL_SUCCESS;
  }
}

gsl_dht *
gsl_dht_new (size_t size, double nu, double xmax)
{
  int status;

  gsl_dht * dht = gsl_dht_alloc (size);

  if (dht == 0)
    return 0;

  status = gsl_dht_init(dht, nu, xmax);

  if (status)
    return 0;

  return dht;
}

int
gsl_dht_init(gsl_dht * t, double nu, double xmax)
{
  if(xmax <= 0.0) {
    GSL_ERROR ("xmax is not positive", GSL_EDOM);
  } else if(nu < 0.0) {
    GSL_ERROR ("nu is negative", GSL_EDOM);
  }
  else {
    size_t n, m;
    int stat_bz = GSL_SUCCESS;
    int stat_J  = 0;
    double jN;

    if(nu != t->nu) {
      /* Recalculate Bessel zeros if necessary. */
      t->nu = nu;
      stat_bz = dht_bessel_zeros(t);
    }

    jN = t->j[t->size+1];

    t->xmax = xmax;
    t->kmax = jN / xmax;

    t->J2[0] = 0.0;
    for(m=1; m<t->size+1; m++) {
      gsl_sf_result J;
      stat_J += gsl_sf_bessel_Jnu_e(nu + 1.0, t->j[m], &J);
      t->J2[m] = J.val * J.val;
    }

    /* J_nu(j[n] j[m] / j[N]) = Jjj[n(n-1)/2 + m - 1], 1 <= n,m <= size
     */
    for(n=1; n<t->size+1; n++) {
      for(m=1; m<=n; m++) {
        double arg = t->j[n] * t->j[m] / jN;
        gsl_sf_result J;
        stat_J += gsl_sf_bessel_Jnu_e(nu, arg, &J);
        t->Jjj[n*(n-1)/2 + m - 1] = J.val;
      }
    }

    if(stat_J != 0) {
      GSL_ERROR("error computing bessel function", GSL_EFAILED);
    }
    else {
      return stat_bz;
    }
  }
}

double gsl_dht_x_sample(const gsl_dht * t, int n)
{
  return t->j[n+1]/t->j[t->size+1] * t->xmax;
}

double gsl_dht_k_sample(const gsl_dht * t, int n)
{
  return t->j[n+1] / t->xmax;
}

void gsl_dht_free(gsl_dht * t)
{
  free(t->J2);
  free(t->Jjj);
  free(t->j);
  free(t);
}

int
gsl_dht_apply(const gsl_dht * t, double * f_in, double * f_out)
{
  const double jN = t->j[t->size + 1];
  const double r  = t->xmax / jN;
  size_t m;
  size_t i;

  for(m=0; m<t->size; m++) {
    double sum = 0.0;
    double Y;
    for(i=0; i<t->size; i++) {
      /* Need to find max and min so that we
       * address the symmetric Jjj matrix properly.
       * FIXME: we can presumably optimize this
       * by just running over the elements of Jjj
       * in a deterministic manner.
       */
      size_t m_local;
      size_t n_local;
      if(i < m) {
        m_local = i;
	n_local = m;
      }
      else {
        m_local = m;
	n_local = i;
      }
      Y = t->Jjj[n_local*(n_local+1)/2 + m_local] / t->J2[i+1];
      sum += Y * f_in[i];
    }
    f_out[m] = sum * 2.0 * r*r;
  }

  return GSL_SUCCESS;
}


#include <jni.h>
#include <assert.h>
#include <lapacke.h>

/*extern void dtrsm_(char *side, char *uplo, char *transa, char *diag, int *m,
                   int *n, double *alpha, double *A, int *lda, double *B,
                   int *ldb);*/

JNIEXPORT jint Java_JAMAJni_QRDecomposition_dgeqrf (JNIEnv *env, jclass klass, jint matrix_layout,
                                                    jint m, jint n, jdoubleArray a, jint lda,
                                                    jdoubleArray tau){

    double *aElems, *tauElems;
    int info;

    aElems = (*env)-> GetDoubleArrayElements (env, a, NULL);
    tauElems = (*env)-> GetDoubleArrayElements (env, tau, NULL);

    assert(aElems && tauElems);

    info = LAPACKE_dgeqrf ((int) matrix_layout, (lapack_int) m, (lapack_int) n, aElems, (lapack_int) lda, tauElems);

    (*env)-> ReleaseDoubleArrayElements (env, a, aElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, tau, tauElems, 0);

    return info;

}

JNIEXPORT jint Java_JAMAJni_QRDecomposition_dorgqr (JNIEnv *env, jclass klass, jint matrix_layout,
                                                    jint m, jint n, jint k, jdoubleArray a, jint lda,
                                                    jdoubleArray tau){

    double *aElems, *tauElems;
    int info;

    aElems = (*env)-> GetDoubleArrayElements (env, a, NULL);
    tauElems = (*env)-> GetDoubleArrayElements (env, tau, NULL);

    assert(aElems && tauElems);

    info = LAPACKE_dorgqr((int) matrix_layout, (lapack_int) m, (lapack_int) n, (lapack_int) k, aElems, (lapack_int) lda, tauElems);

    (*env)-> ReleaseDoubleArrayElements (env, a, aElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, tau, tauElems, 0);

    return info;

}

JNIEXPORT jint Java_JAMAJni_QRDecomposition_dgeqp3  (JNIEnv *env, jclass klass, jint matrix_layout,
                                                     jint m, jint n, jdoubleArray a, jint lda,
                                                     jintArray jpvt, jdoubleArray tau){

    double *aElems, *tauElems;
    lapack_int *jpvtElems;
    int info;

    aElems = (*env)-> GetDoubleArrayElements (env, a, NULL);
    tauElems = (*env)-> GetDoubleArrayElements (env, tau, NULL);
    jpvtElems = (*env)-> GetIntArrayElements (env, jpvt, NULL);

    assert(aElems && tauElems && jpvtElems);

    info = LAPACKE_dgeqp3 ((int) matrix_layout, (lapack_int) m, (lapack_int) n, aElems, (lapack_int) lda, jpvtElems, tauElems);

    (*env)-> ReleaseDoubleArrayElements (env, a, aElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, tau, tauElems, 0);
    (*env)-> ReleaseIntArrayElements (env, jpvt, jpvtElems, 0);

    return info;
}

JNIEXPORT jint Java_JAMAJni_QRDecomposition_dormqr
(JNIEnv *env, jclass klass, jint matrix_layout, jchar side, jchar trans,
 jint m, jint n, jint k, jdoubleArray a, jint lda, jdoubleArray tau,
 jdoubleArray c, jint ldc){

    double *aElems, *tauElems, *cElems;
    int info;

    aElems = (*env)-> GetDoubleArrayElements (env, a, NULL);
    tauElems = (*env)-> GetDoubleArrayElements (env, tau, NULL);
    cElems = (*env)-> GetDoubleArrayElements (env, c, NULL);

    assert(aElems && tauElems && cElems);

    info = LAPACKE_dormqr ((int) matrix_layout, (char) side, (char) trans,
                           (lapack_int) m, (lapack_int) n, (lapack_int) k,
                           aElems, (lapack_int) lda, tauElems, cElems,
                           (lapack_int) ldc);

    (*env)-> ReleaseDoubleArrayElements (env, a, aElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, tau, tauElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, c, cElems, JNI_ABORT);

    return info;

}


#ifndef __gslInterp_
#define __gslInterp__

#include "include/stdinc.h"
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_integration.h>
#include <vector>

class gslInterp {
protected:
  gsl_interp_accel *acc;
  gsl_interp       *interp;
  vector<double>   x, y;
public:

  gslInterp(vector<double> &x0, vector<double> &y0) {
    acc   = gsl_interp_accel_alloc();
    for (size_t i = 0; i < x0.size(); i++) {
      x.push_back(x0[i]);
      y.push_back(y0[i]);
    }
    interp = gsl_interp_alloc(gsl_interp_linear, x.size());
    gsl_interp_init(interp, &x[0], &y[0], x.size());
  }
  ~gslInterp() {
    gsl_interp_accel_free(acc);
    gsl_interp_free(interp);
  }
  void init(vector<double> &x0, vector<double> &y0) {
    gsl_interp_accel_free(acc);
    gsl_interp_free(interp);
    acc    = gsl_interp_accel_alloc();
    x.clear();
    y.clear();
    for (size_t i = 0; i < x0.size(); i++) {
      x.push_back(x0[i]);
      y.push_back(y0[i]);
    }
    interp = gsl_interp_alloc(gsl_interp_linear, x.size());
    gsl_interp_init(interp, &x[0], &y[0], x.size());
  }

  inline double eval(double xc) {
    return gsl_interp_eval(interp, &x[0], &y[0], xc, acc);
  }
  inline double deriv(double xc) {
    return gsl_interp_eval_deriv(interp, &x[0], &y[0], xc, acc);
  }
  inline double deriv2(double xc) {
    return gsl_interp_eval_deriv2(interp, &x[0], &y[0], xc, acc);
  }
  inline double integ(double x_lo, double x_up) {
    return gsl_interp_eval_integ(interp, &x[0], &y[0], x_lo, x_up, acc);
  }
};

#endif // __gslInterp__


/*
 * rw.c -- Loesung des Randwertproblems mit der GSL
 *
 * (c) 2016 Prof Dr Andreas Mueller, Hochschule Rapperswil
 */
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

static double	g = 9.81;

static long	fcounter = 0;

static int	f(double x, const double y[], double f[], void *params) {
	fcounter++;
	double	g = *(double *)params;
	f[0] =  y[2];
	f[1] =  y[3];
	f[2] =     0;
	f[3] =    -g;
	return GSL_SUCCESS;
}

static long	dfcounter = 0;

static int	df(double x, const double y[], double *dfdy, double dfdx[],
			void *params) {
	dfcounter++;

	dfdy[0 * 4 + 0] = 0;
	dfdy[1 * 4 + 0] = 0;
	dfdy[2 * 4 + 0] = 0;
	dfdy[3 * 4 + 0] = 0;

	dfdy[0 * 4 + 1] = 0;
	dfdy[1 * 4 + 1] = 0;
	dfdy[2 * 4 + 1] = 0;
	dfdy[3 * 4 + 1] = 0;

	dfdy[0 * 4 + 2] = 1;
	dfdy[1 * 4 + 2] = 0;
	dfdy[2 * 4 + 2] = 0;
	dfdy[3 * 4 + 2] = 0;

	dfdy[0 * 4 + 3] = 0;
	dfdy[1 * 4 + 4] = 1;
	dfdy[2 * 4 + 4] = 0;
	dfdy[3 * 4 + 4] = 0;

	dfdx[0] = 0;
	dfdx[1] = 0;
	dfdx[2] = 0;
	dfdx[3] = 0;

	return GSL_SUCCESS;
}

static long	Fcounter = 0;

static int	F(double x, const double J[], double F[], void *params) {
	Fcounter++;
	for (int i = 0; i < 2; i++) {
		for (int j = 0; j < 4; j++) {
			F[i * 4 + j] = J[(i + 2) * 4 + j];
		}
	}
	for (int i = 2; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			F[i * 4 + j] = 0;
		}
	}
	return GSL_SUCCESS;
}

int	main(int argc, char *argv[]) {
	gsl_odeiv2_system	system = { f, df, 4, &g };
	gsl_odeiv2_system	Jsystem = { F, NULL, 16, NULL };

	gsl_odeiv2_driver	*driver
		= gsl_odeiv2_driver_alloc_y_new(&system, gsl_odeiv2_step_rk8pd,
			1e-6, 1e-6, 0.0);
	gsl_odeiv2_driver	*Jdriver
		= gsl_odeiv2_driver_alloc_y_new(&Jsystem, gsl_odeiv2_step_rk8pd,
			1e-6, 1e-6, 0.0);

	double	t = 0.0;
	double	vx = 8;
	double	vy = 7;
	double	tnext = 20 / vx;
	double	delta;
	int	counter = 0;
	printf(" n     v_y        t        x        y        dy/dv_y        vynew          delta\n");
	do {

		// compute the solution up to x = 1.5
		t = 0;
		double	y[4] = { 0.0, 0.0, vx, vy };
		int	status = gsl_odeiv2_driver_apply(driver, &t, tnext, y);
		if (status != GSL_SUCCESS) {
			fprintf(stderr, "error: return value = %d\n", status);
		}

		// compute the jacobi matrix up to x = 1.5
		t = 0;
		double	J[16] = {	1.0, 0.0, 0.0, 0.0,
					0.0, 1.0, 0.0, 0.0,
					0.0, 0.0, 1.0, 0.0,
					0.0, 0.0, 0.0, 1.0 };
		status = gsl_odeiv2_driver_apply(Jdriver, &t, tnext, J);
		if (status != GSL_SUCCESS) {
			fprintf(stderr, "error: return value = %d\n", status);
		}

		// the derivative is J_{24}
		double	 derivative = J[1 * 4 + 3];

		// compute the newton correction
		double	vynew = vy - y[1] / derivative;
		delta = vy - vynew;
		printf("%2d%8.4f %8.4f %8.4f %10.6f %12.8f %12.8f %14.10f\n",
			 counter, vy, t, y[0], y[1], derivative, vynew, delta);
		vy = vynew;
		counter++;
	} while ((fabs(delta) > 1e-6) && (counter <= 50));
	printf("calls to f: %ld, to df: %ld, to F: %ld\n",
		fcounter, dfcounter, Fcounter);
	return EXIT_SUCCESS;
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *
 *
 *       Filename:  stationary.c
 *
 *    Description:  Find the stationary state for the given generator
 *
 *        Version:  1.0
 *        Created:  05/05/2014 15:27:48
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include "funcs.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

/*
 *      FUNCTION
 *         Name:  stationary
 *  Description:  Given the dissipator in Bloch form, reduce to a 3x3 problem and store
 *  			the stationary state in the 3x1 vector *X
 *
 * 			M X = 0
 *
 * 	        	|  0    0    0    0  |  | 1  |    0
 * 		        | M10  M11  M12  M13 |  | X1 |    0
 * 		        | M20  M21  M22  M23 |  | X2 | =  0
 * 			| M30  M31  M32  M33 |  | X3 |    0
 *
 *
 * 			A x = b
 *
 * 			| M11  M12  M13 |  | X1 |   | -M10 |
 * 			| M21  M22  M23 |  | X2 | = | -M20 |
 * 			| M31  M32  M33 |  | X3 |   | -M30 |
 */
int stationary ( const gsl_matrix* M, gsl_vector* stat_state )
{
	/* Store space for the stationary state */
	gsl_vector* req = gsl_vector_calloc ( 4 ) ;
	gsl_vector_set ( req, 0, 1 ) ;

	/* Copy the dissipator matrix in a temporary local matrix m
	 * (because the algorithm destroys it...) */
	gsl_matrix* m = gsl_matrix_calloc ( 4, 4 ) ;
	gsl_matrix_memcpy ( m, M ) ;

	/* Create a view of the spatial part of vector req */
	gsl_vector_view x = gsl_vector_subvector ( req, 1, 3 ) ;

	/* Create a submatrix view of the spatial part of m and a vector view
	 * of the spatial part of the 0-th column, which goes into -b in the system
	 * A x = b */
	gsl_matrix_view A = gsl_matrix_submatrix ( m, 1, 1, 3, 3 ) ;
	gsl_vector_view b = gsl_matrix_subcolumn ( m, 0, 1, 3 ) ;
	int status1 = gsl_vector_scale ( &b.vector, -1.0 ) ;

	/* Solve the system A x = b using Householder transformations.
	 * Changing the view x of req => also req is changed, in the spatial part */
	int status2 = gsl_linalg_HH_solve ( &A.matrix, &b.vector, &x.vector ) ;

	/* Set the returning value for the state stat_state */
	*stat_state = *req ;

	/* Free memory */
	gsl_matrix_free(m) ;

	return status1 + status2 ;
}		/* -----  end of function stationary  ----- */

/*
 *      FUNCTION
 *         Name:  J
 *  Description:  Ohmic spectral density (divided by alpha)
 *
 */
double J ( double w, double oc )
{
	double W = w*exp(-w/oc) ;
	return (W);
}		/* -----  end of function J  ----- */

/*
 *      FUNCTION
 *         Name:  polarization
 *  Description:  Polarization P through the CP formula
 *
 */
double polarization ( void* params, double oc )
{
	struct f_params* pars = (struct f_params*) params ;
	double Omega, omega_1, b ;
	Omega   = pars->Omega ;
	omega_1 = pars->omega_1 ;
	b       = pars->beta ;

	double P, num, den, add1, add2, Jplus, Jminus ;
	Jplus  = J(omega_1 + Omega, oc) ;
	Jminus = J(omega_1 - Omega, oc) ;
	add1 = POW_2(omega_1 - Omega)*Jplus ;
	add2 = POW_2(omega_1 + Omega)*Jminus ;
	num = add1 + add2 ;
	den = add1/(tanh((omega_1+Omega)*b/2)) +
		add2/(tanh((omega_1-Omega)*b/2)) ;
	P = num/den ;

	return (P);
}		/* -----  end of function polarization  ----- */


/* specfunc/gsl_sf_mathieu.h
 *
 * Copyright (C) 2002 Lowell Johnson
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  L. Johnson */

#ifndef __GSL_SF_MATHIEU_H__
#define __GSL_SF_MATHIEU_H__

#include <gsl/gsl_sf_result.h>
#include <gsl/gsl_eigen.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

#define GSL_SF_MATHIEU_COEFF 100

typedef struct
{
  size_t size;
  size_t even_order;
  size_t odd_order;
  int extra_values;
  double qa;   /* allow for caching of results: not implemented yet */
  double qb;   /* allow for caching of results: not implemented yet */
  double *aa;
  double *bb;
  double *dd;
  double *ee;
  double *tt;
  double *e2;
  double *zz;
  gsl_vector *eval;
  gsl_matrix *evec;
  gsl_eigen_symmv_workspace *wmat;
} gsl_sf_mathieu_workspace;

/* Compute an array of characteristic (eigen) values from the recurrence
   matrices for the Mathieu equations. */
int gsl_sf_mathieu_a_array(int order_min, int order_max, double qq, gsl_sf_mathieu_workspace *work, double result_array[]);
int gsl_sf_mathieu_b_array(int order_min, int order_max, double qq,  gsl_sf_mathieu_workspace *work, double result_array[]);

/* Compute the characteristic value for a Mathieu function of order n and
   type ntype. */
int gsl_sf_mathieu_a(int order, double qq, gsl_sf_result *result);
int gsl_sf_mathieu_b(int order, double qq, gsl_sf_result *result);

/* Compute the Fourier coefficients for a Mathieu function. */
int gsl_sf_mathieu_a_coeff(int order, double qq, double aa, double coeff[]);
int gsl_sf_mathieu_b_coeff(int order, double qq, double aa, double coeff[]);

/* Allocate computational storage space for eigenvalue solution. */
gsl_sf_mathieu_workspace *gsl_sf_mathieu_alloc(const size_t nn,
                                               const double qq);
void gsl_sf_mathieu_free(gsl_sf_mathieu_workspace *workspace);

/* Compute an angular Mathieu function. */
int gsl_sf_mathieu_ce(int order, double qq, double zz, gsl_sf_result *result);
int gsl_sf_mathieu_se(int order, double qq, double zz, gsl_sf_result *result);
int gsl_sf_mathieu_ce_array(int nmin, int nmax, double qq, double zz,
                            gsl_sf_mathieu_workspace *work,
                            double result_array[]);
int gsl_sf_mathieu_se_array(int nmin, int nmax, double qq, double zz,
                            gsl_sf_mathieu_workspace *work,
                            double result_array[]);

/* Compute a radial Mathieu function. */
int gsl_sf_mathieu_Mc(int kind, int order, double qq, double zz,
                      gsl_sf_result *result);
int gsl_sf_mathieu_Ms(int kind, int order, double qq, double zz,
                      gsl_sf_result *result);
int gsl_sf_mathieu_Mc_array(int kind, int nmin, int nmax, double qq,
                            double zz, gsl_sf_mathieu_workspace *work,
                            double result_array[]);
int gsl_sf_mathieu_Ms_array(int kind, int nmin, int nmax, double qq,
                            double zz, gsl_sf_mathieu_workspace *work,
                            double result_array[]);

__END_DECLS

#endif /* !__GSL_SF_MATHIEU_H__ */


#include <stdio.h>
#include <string.h>
#include <stdlib.h>
//#include </usr/local/include/gsl/gsl_matrix.h>
#include <gsl/gsl_matrix.h> // suprisingly, both work
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

/*
 *  * Program that reads reference spectra and measure spectra data
 *  and linearly unmixes the data using method of least squares fit.
 *  Assumes readings is set to 400 and file path is preset to:
 *  "/mnt/c/Users/jakei/'OneDrive - McGill University'/'Year 2'/1BIEN210/'Group Project'/".
 *
 * syntax: ./linearUnmixer MUfilename ref1filename ref2filename ref3filename ...
 * returns: completion status, prints composition of ref1, ref2, ... , refn
 *
 *  * ****************************************************************
 *  * Author       Dept.           Date            Notes
 *  * ****************************************************************
 *  * Jake Z       Bio. Eng.       Mar 12 2020     Initial version
 *  * Jake Z		"	   Apr 03 2020     It works now! TODO: investigate the discrepancy between what you set $readings and the amount of space you allocate to the matrices as you forgot to account for the initial data clearing of the title headers in the data.
 *  * */
const int READINGS = 1300;		//note, if the readings specified here is more then the number of readings in file, funky stuff happens, TODO: implent check for this
const int SKIP = 50;
int normalizeVector( gsl_vector *vector )
{
	double min = gsl_vector_min( vector );
	double norm = gsl_vector_max( vector ) - min;
	gsl_vector_add_constant( vector, -min );
	gsl_vector_scale( vector, 1/norm );
	return 0;
}

int readFileVector( gsl_vector *vector, char *filePath, char *fileName )		//read file vals into vector
{
	char tmpFilepath[120] = "\0";
	char tmpReading[20];
	char *tmp2;
	double reading;					//WARNING this is similar to global var $readings, consider renaming

	strcat( tmpFilepath, filePath );		//TODO replace this and the first instance of strcat into strcpy, that should work
	strcat( tmpFilepath, fileName ); 	//combines the filepath and name into one searchable /path/file
	FILE *file = fopen( tmpFilepath, "rt");
	printf("%s\n", tmpFilepath);			//TODO DELETE THIS
	if ( file == NULL ) {			//check if file exists
		fprintf( stderr, "Error, unable to locate the Mu data file\n");
		exit(100);
	}

	for ( int i = 0; i < SKIP; i++)		//move past header of read files
	{
		fgets( tmpReading, 19, file );
	}
	for ( int i = 0; i < READINGS; i++ )
	{
		fgets( tmpReading, 19, file );

		tmp2 = strrchr( tmpReading, '\t' );
		tmp2 = tmp2 + 1;
		reading = atof(tmp2);
		gsl_vector_set( vector, i, reading);
	}
	fclose(file);
	printf("successful vector \n");			//TODO delete this
	return 0;
}

int readFileMatrix( gsl_matrix *matrix, char *filePath, int argc, char *argv[] )
{
	char *tmp2;
	double reading;
	for ( int j = 2; j < argc; j++ )
	{
		gsl_vector_view column;
		char tmpFilepath[120] = "\0";				//TODO replace this and the first instance of strcat into strcpy, that should work
		char tmpReading[20];
		strcat( tmpFilepath, filePath );
		strcat( tmpFilepath, argv[j] ); 	//combines the filepath and name into one searchable /path/file
		FILE *file = fopen( tmpFilepath, "rt");
		printf("%s\n", tmpFilepath);				//TODO DELETE THIS
		if ( file == NULL ) {			//check if file exists
			fprintf( stderr, "Error, unable to locate a reference data file\n");
			exit(100);
		}
		for ( int i = 0; i < SKIP; i++)		//move past header of read files
		{
			fgets( tmpReading, 20, file);
		}
		for ( int i = 0; i < READINGS; i++ ) //fill the ith element of the jth column
		{
			fgets( tmpReading, 19, file );

			tmp2 = strrchr( tmpReading, '\t' );
			tmp2 = tmp2 + 1;
			reading = atof(tmp2);
			gsl_matrix_set( matrix, i, j-2, reading );
		}
		fclose(file);
		column = gsl_matrix_column( matrix, j-2 );				//NORMALISE EACH REFERENCE VECOTR.
		normalizeVector( &column.vector );
	}
	printf("success, matrix filled \n");			//TODO DELETE THIS
	return 0;
}

int main(int argc, char *argv[])
{
	if ( argc <= 2 )
	{
		fprintf( stderr, "Error, incorrect usage.\n./linearUnmixer MU_SPECTRA REF_SPECTRA\n");
		return 3;
	}
	//spectrum files labelled  /mnt/c/Users/jakei/'OneDrive - McGill University'/'Year 2'/1BIEN210/'Group Project'/SpectrumFile_00N.txt
	//intialize and allocate memory for variabels
	int numRefs = argc - 2;
	//char *filePath = "";
	char *filePath = "/mnt/c/Users/jakei/OneDrive - McGill University/Year 2/1BIEN210/Group Project/";
	gsl_vector *muVector = gsl_vector_alloc(READINGS);
	gsl_vector *fVector = gsl_vector_alloc(numRefs);
	gsl_vector *x = gsl_vector_alloc(numRefs);			//TODO RENAME x AND PROBECOMPVECTOR
	gsl_matrix *refMatrix = gsl_matrix_alloc(READINGS, numRefs);
	gsl_matrix *C = gsl_matrix_alloc( numRefs, numRefs );
	gsl_matrix *Cinverse = gsl_matrix_alloc( numRefs, numRefs );

	int s;
	gsl_permutation *p = gsl_permutation_alloc(numRefs);			//CHANGED ALLOC TO CALLOC

	//initailize the variables to 0
	gsl_vector_set_zero(muVector);
	gsl_vector_set_zero(fVector);
	gsl_vector_set_zero(x);
	gsl_matrix_set_zero(refMatrix);
	gsl_matrix_set_zero(C);

	gsl_permutation_init(p);			//INITIALISED THE PERMUTATION

	readFileVector( muVector, filePath, argv[1] ); //call function which reads in values of first arguement file into the vector mu
	readFileMatrix( refMatrix, filePath, argc, argv ); //obtain matrix of reference vals, (400, 2) in 400 READINGS w/ 2 probes
	//normalize values, vector is already normalised
	normalizeVector( muVector );

	//print vector to csv file for plotting
	FILE *csvfile = fopen( "/mnt/c/Users/jakei/OneDrive - McGill University/Year 2/1BIEN210/Group Project/muVector.csv", "wt" );
	gsl_vector_fprintf( csvfile, muVector, "%f" );
	fclose(csvfile);
	FILE *csvfile2 = fopen( "/mnt/c/Users/jakei/OneDrive - McGill University/Year 2/1BIEN210/Group Project/refVectors.txt", "wt" );
	gsl_matrix_fprintf( csvfile2, refMatrix, "%f" );
	fclose(csvfile2);

	//now we have variable muVector with vals and refMatrix with vals.
	gsl_blas_dgemm( CblasTrans, CblasNoTrans, 1, refMatrix, refMatrix, 0, C);	//matrix C is now A^T A
	gsl_blas_dgemv( CblasTrans, 1, refMatrix, muVector, 0, fVector);	//fVector is 'b'
	gsl_linalg_LU_decomp( C, p, &s);
//	gsl_linalg_LU_solve( C, p, fVector, x );	//can replace with gsl_linalg_LU_svx and delete x vector from program

	gsl_linalg_LU_invert( C, p, Cinverse );		//TODO maybe implement the other way you can solve for basis using inverse on both sides, (A^T A )^-1 A^T A x = (a^t a)^-1 (a^t b) = x
	gsl_blas_dgemv( CblasNoTrans, 1, Cinverse, fVector, 0, x);
/*
	gsl_vector_fprintf( stdout, muVector, "%f" );
	printf("***********************************************\n");
*/
	gsl_vector_fprintf( stdout, x, "%f" );

	gsl_matrix_free(Cinverse);
	gsl_matrix_free(C);
	gsl_matrix_free(refMatrix);
	gsl_vector_free(muVector);
	gsl_vector_free(fVector);
	gsl_vector_free(x);
	return 0;

}


/* FacSexNe.c

Simulation of tracking neutral mutation under facultative sex
To calculate Ne under different scenarios
Code based on that used in Agrawal and Hartfield 2016 (https://github.com/MattHartfield/BalSelSims).

Simulation uses routines found with the GNU Scientific Library (GSL)
(http://www.gnu.org/software/gsl/)
Since GSL is distributed under the GNU General Public License
(http://www.gnu.org/copyleft/gpl.html), you must download it
separately from this file.

Run by executing:
./FacSexNe N sex gc reps
Where:
- N is the population size
- sex is the frequency of sex (a value between 0 = obligate asex, and 1 = obligate sex)
- gc is the frequency of mitotic gene conversion
- reps is how many times to introduce linked neutral allele

*/

/* Preprocessor statements */
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stddef.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* Function prototypes */
void neutinit(double *geninit, unsigned int N);
void reproduction(double *geninit, double sex);
void gconv(double *geninit, double gc);
double ncheck(double *geninit);

/* Main program */
int main(int argc, char *argv[]){
	unsigned int N = 0;				/* Population size */
	unsigned int g, i; 				/* Counters. Reps counter, geno counter */
	unsigned int reps = 0;			/* Length of simulation (no. of introductions of neutral site) */
	double sex = 0;					/* Frequency of sexual reproduction */
	double gc = 0;					/* Frequency of gene conversion */
	double Acheck = 0;				/* Frequency of derived neutral allele (A) after each reproduction */
	double Hsum = 0;				/* Summed heterozygosity over transit time of neutral allele */
	char Hout[128];					/* String to hold filename in */
	FILE *ofp_hs = NULL;			/* Pointer for results output */

	/* GSL random number definitions */
	const gsl_rng_type * T;
	gsl_rng * r;

	/* This reads in data from command line. */
	if(argc != 5){
		fprintf(stderr,"Invalid number of input values.\n");
		exit(1);
	}
	N = atoi(argv[1]);
	sex = strtod(argv[2],NULL);
	gc = strtod(argv[3],NULL);
	reps = strtod(argv[4],NULL);

	/* Arrays definition and memory assignment */
	double *genotype = calloc(3,sizeof(double));				/* Genotype frequencies */
	unsigned int *gensamp = calloc(3,sizeof(unsigned int));		/* New population samples */

	/* create a generator chosen by the
    environment variable GSL_RNG_TYPE */

	gsl_rng_env_setup();
	if (!getenv("GSL_RNG_SEED")) gsl_rng_default_seed = time(0);
	T = gsl_rng_default;
	r = gsl_rng_alloc(T);

	/* Setting up neutral genotype */
	neutinit(genotype,N);

	/* Reintroducing neutral genotype, resetting hap sum */
    Acheck = ncheck(genotype);
    Hsum = Acheck*(1.0-Acheck);

    /* Introduce and track neutral mutations 'reps' times */
    g = 0;
    while(g < reps){

    	/* Reproduction routine */
    	reproduction(genotype,sex);

    	/* Gene conversion routine */
       	gconv(genotype,gc);

    	/* Sampling based on new frequencies */
       	gsl_ran_multinomial(r,3,N,genotype,gensamp);
       	for(i = 0; i < 3; i++){
       		*(genotype + i) = (*(gensamp + i))/(1.0*N);
       	}

       	/* Checking state of haplotypes: if A fixed reset so can start fresh next time */
		Acheck = ncheck(genotype);
		Hsum += Acheck*(1.0-Acheck);

       	if(Acheck == 0 || Acheck == 1){
       		sprintf(Hout,"/scratch/mhartfield/CC_FSC_Ne/temp_s%.8lf_gc%.8lf.out",sex,gc);
       		if(g != 0){
				ofp_hs = fopen(Hout,"a");
			}else if(g == 0){
				ofp_hs = fopen(Hout,"w");
			}
			fprintf(ofp_hs,"%.10lf\n",Hsum);
			fclose(ofp_hs);
       		g++;

     		/* Reintroducing neutral genotype, resetting hap sum */
	    	neutinit(genotype,N);
			Acheck = ncheck(genotype);
       		Hsum = Acheck*(1-Acheck);
       	}

	}	/* End of simulation */

	/* Freeing memory and wrapping up */
 	gsl_rng_free(r);
 	free(gensamp);
	free(genotype);
	return 0;
}

/* Setting up neutral polymorphism routine */
void neutinit(double *geninit, unsigned int N){
	*(geninit + 0) = 1 - 1/(1.0*N);
	*(geninit + 1) = 1/(1.0*N);
	*(geninit + 2) = 0;
}	/* End of neutral setup routine */

/* Reproduction routine */
void reproduction(double *geninit, double sex){
	/* Fed-in genotype frequencies (for ease of programming) */
	double gaas, gAas, gAAs;
	/* Genotype frequencies after sex */
	double gaaSX, gAaSX, gAASX;
	/* Genotype frequencies after asex */
	double gaaAS, gAaAS, gAAAS;

	/* Initial definition of genotypes */
	gaas = *(geninit + 0);
	gAas = *(geninit + 1);
	gAAs = *(geninit + 2);

	/* Change in frequencies due to sex */
	gaaSX = sex*(gaas + gAas/2.0)*(gaas + gAas/2.0);
	gAaSX = sex*2.0*(gaas + gAas/2.0)*(gAAs + gAas/2.0);
	gAASX = sex*(gAAs + gAas/2.0)*(gAAs + gAas/2.0);

	/* Change in frequencies due to asex */
	gaaAS = gaas*(1 - sex);
	gAaAS = gAas*(1 - sex);
	gAAAS = gAAs*(1 - sex);

	/* Combining to give overall frequency change following reproduction */
	*(geninit + 0) = gaaAS + gaaSX;
	*(geninit + 1) = gAaAS + gAaSX;
	*(geninit + 2) = gAAAS + gAASX;

}	/* End of reproduction routine */

/* Gene conversion routine */
void gconv(double *geninit, double gc){

	/* Fed-in genotype frequencies (for ease of programming) */
	double gaar, gAar, gAAr;
	/* Frequencies after gene conversion */
	double gaagc, gAagc, gAAgc;

	/* Initial definition of genotypes */
	gaar = *(geninit + 0);
	gAar = *(geninit + 1);
	gAAr = *(geninit + 2);

	/* Gene conversion equations */
	gaagc = gaar + gAar*gc/2.0;
	gAagc = gAar*(1.0-gc);
	gAAgc = gAAr + gAar*gc/2.0;

	/* Output */
	*(geninit + 0) = gaagc;
	*(geninit + 1) = gAagc;
	*(geninit + 2) = gAAgc;

}	/* End of gene conversion routine */

/* Has neutral allele fixed or not? Measuring its frequency */
double ncheck(double *geninit){
	/* Fed-in genotype frequencies (for ease of programming) */
	double gAas, gAAs;
	double Atot = 0.0;        /* Total frequency of A */

	/* Initial definition of genotypes */
	gAas = *(geninit + 1);
	gAAs = *(geninit + 2);

	/* Checking frequency of A */
	Atot = gAAs + gAas/2.0;
	return Atot;

}	/* End of neutral check routine */

/* End of program */


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <builtin_types.h>
#include "intT.h"
#include <fenv.h>
#include <signal.h>

  //electron mass squared
  double m2 = 1.0;
  //electron charge
  double e = 0.30282212;
  //double m2=1.0;
  //double e=1.0;

double EV(double T, void * p, int* WLlist);

double Exact(double T, double B, int fermion)
//Return the exact integrand for constant fields
{
  double TB = e*T*B;
  double Exact;
  if( B < 1.0e-40)
	Exact = 0.0;
  else
    if(fermion == 1)
	Exact = exp(-m2*T)/(T*T*T)*(TB/tanh(TB)-1.0-1.0/3.0*TB*TB);
    else
	if (TB < 50.0)
		Exact = exp(-m2*T)/(T*T*T)*(TB/sinh(TB)-1.0+1.0/6.0*TB*TB);
	else
		Exact = exp(-m2*T)/(T*T*T)*(1.0/6.0*TB*TB-1.0);
  //printf("%f %e\n",T, exp(-T));
  return Exact;
}

double bumpd(double x)
//The bump function
{
  if (abs(x) < 1.0)
	return exp(-1.0/(1.0-x*x));
  else
	return 0.0;
}

double fixfluxB(double rho2, double lambda2)
//Magnetic field strength for the fixflux profile
{
  float l = sqrt(lambda2);
  float lmlmin = (l - lmin)/(tubedist - lmin);
  if(sqrt(rho2) > tubedist/2.0) printf("Warning: inappropriate magnetic field being used. rho=%f\n", sqrt(rho2));
  return  2.0/(lambda2*q2)*(1.0-0.75*lmlmin)*bumpd(2.0*sqrt(rho2/lambda2))
	+ 3.0/(tubedist*tubedist)*lmlmin;
}

double periodicB(double rho2, double lambda2)
//magnetic field for the periodic flux tube profile
{
  double expr, rho, lambda, flraf, f, ceilraf;
  double expral, exprapl;
  rho = sqrt(rho2);
  lambda = sqrt(lambda2);
  expr = exp(-rho/lambda);
  flraf = floor(rho/tubedist);
  ceilraf = ceil(rho/tubedist);
  if(rho < tubedist)
  {
	expral = 0.0;
	if((tubedist-rho)/lambda < 10.0)
		expral = exp(-(rho-tubedist)/lambda);
	f = expr/(tubedist*(1.0 + expr)*(1.0 + expr));
	if(rho > 0.0)
		f += expral/(rho*(1.0+expral)*(1.0+expral));
  }
  else
  {
        if(abs(fmod(rho,tubedist)) < 1.0e-6)
 	{
		f = flraf/(4.0*rho);
	}
	else
	{
	  expral = exp(-(rho-flraf*tubedist)/lambda);
	  exprapl = exp(-(rho-ceilraf*tubedist)/lambda);
          f = flraf*expral/(rho*(1.0+expral)*(1.0+expral)) +
		ceilraf*exprapl/(rho*(1.0+exprapl)*(1.0+exprapl));
        }
  }
  //f=1.0f/lambda2*exp(-1.0f*rho2/lambda2);
  //f=lambda2/8.0f;
  f=tubedist/(2.0*log(2.0)*lambda2)*f;
  //f=1.0;
  return f;
}

double MagField(double rho2, void* p)
//Compute the magnetic field.
{
  double B;
  double sinrho;
  struct Wparams *params = (struct Wparams *) p;
  switch(profile){
	case step:
	  if(rho2<params->l2)
		B=1.0/params->l2;
	  else
		B=0.0;
	  break;
	case smooth:
	  B=params->l2/((params->l2+rho2)*(params->l2+rho2));
	  break;
	case quadratic:
	  if(rho2<params->l2)
		B=2.0/params->l2*(1.0-rho2/params->l2);
	  else
		B=0.0;
	  break;
	case gaussian:
	  if(rho2<100.0*params->l2)
	  	B=1.0/(params->l2*exp(rho2/params->l2));
	  else
		B=0.0;
	  break;
	case periodic:
	  B=periodicB(rho2,params->l2);
	  break;
	case spline:
	  sinrho=sin(pi*sqrt(rho2)/tubedist);
	  B=1.0/(params->l2*exp(sinrho*sinrho/params->l2));
	  break;
	case fixflux:
	  B = fixfluxB(rho2,params->l2);
	  break;
  }
  B=2.0*params->F/e*B;

  return B;
}

double smTscal(double B, double T)
//Effective action for T<<1.0 for scalar QED
{
	double B2=e*e*B*B;
	double B4=B2*B2;
	double B6=B2*B4;
	double T2=T*T;
	double T3=T2*T;
	double T4=T3*T;
	double result;
	result = 7.0/360.0*B4*T*(1.0-m2*T)+(147.0*B4*m2*m2-31.0*B6)/15120.0*T3
		+(31.0*B6*m2-49.0*B4*m2*m2*m2)/15120.0*T4;
	return result;
}

double smTferm(double B, double T)
//Effective action for T<<1.0 for fermionic QED
{
	double B2=e*e*B*B;
	double B4=B2*B2;
	double B6=B2*B4;
	double T2=T*T;
	double T3=T2*T;
	double T4=T3*T;
	double result;
	result = 1.0/45.0*B4*T*(m2*T-1.0) + (2.0*B6/945.0 - B4*m2*m2/90.0)*T3
		+(7.0*B4*m2*m2*m2-4.0*B6*m2)/1890.0*T4;
	//result = 1.0/45.0*B4*T*(T-1.0) + (2.0*B6/945.0 - B4/90.0)*T3
	//	+(7.0*B4-4.0*B6)/1890.0*T4;
	return result;
}

double meanigrand(double* igrand, int ngroups, double* igranderr)
//compute the mean and std. err. for igrand
{
  double meanig=0.0;
  int i;
  *igranderr=0.0;
  for(i=0;i<ngroups;i++)
  {
	meanig+=igrand[i];
  }
  meanig/=ngroups;
  for(i=0; i < ngroups; i++)
  {
	*igranderr+=(igrand[i]-meanig)*(igrand[i]-meanig);
  }
  *igranderr = sqrt(*igranderr);
  *igranderr /= ngroups;
  return meanig;
}

double* Tfunc(double T, void* p, int* order)
//The integrand of the proper time, T, integral
{
  struct Wparams *params = (struct Wparams *) p;
  double rho2 = params->xcm.x*params->xcm.x + params->xcm.y*params->xcm.y;
  double smallT;
  double B;
  double TB;
  double* igrand = malloc(params->ng*sizeof(*igrand));
  int groupsize = 128; //loops per group
  int* WLlist = malloc(groupsize*sizeof(*WLlist));
  int i, j;
  double rho = params->xcm.x;
  double l2 = params->l2;
  double sin2rho = sin(2.0*pi*rho/tubedist);
  double denominator;
  double igmean; double igerr;
  if(!igrand || !WLlist) printf("error Tfunc():  malloc failed\n");

  //printf("func: T=%f\n",T);
  B=MagField(rho2,p);
  TB=e*T*B;
  //printf("T=%f f'/f''=%f\n",T,(params->l2+params->xcm.x*params->xcm.x));
  //determine a suitable definition for small T
  switch(profile)
  {
	case step:
	  smallT=abs(rho*rho-l2);
          break;
        case smooth:
	  smallT= 0.5*(l2+rho*rho);
          break;
        case quadratic:
	  smallT= 0.5*(l2+rho*rho);
          break;
        case gaussian:
	  smallT=l2;
	  break;
        case periodic:
	  smallT=0.5*(l2+rho*rho);
          break;
	case spline:
	  if(rho/tubedist>1.0e-8)
	  	smallT=tubedist*tubedist*l2/(pi*pi*sin2rho*sin2rho);
	  else
		smallT=1.0e12;
	  break;
	case fixflux:
	  //small if T << l2, or if the loop is far from any bump functions.
	  if(rho*rho<l2/4.0)
		smallT = 0.25*l2;
	  else
		smallT = 0.25*l2+(rho-sqrt(l2)/2.0)*(rho-sqrt(l2)/2.0);
  }
  for(i = 0; i < params->ng; i++)
  {
	//printf("func igrand[%d]=%f\n",i,igrand[i]);
	if(T < 0.1 && T < 0.01*smallT)
	//if(T<0.7)
  	{
		if(verbosity >= 2)
		  printf("using small T expression: %f %f %f\n", rho2, T, smallT);
		if(params->fermion == 1)
			igrand[i] = smTferm(B, T);
		else
			igrand[i] = smTscal(B, T);
  	}
        else if(T < 0.01*smallT)
	//use the exact expression
	{
		if(verbosity >= 2)
		  printf("using const. field expression %f %f %f\n", rho2, T, smallT);
		if(TB > 1.0e-6)
			igrand[i] = Exact(T, B, params->fermion);
		else
			igrand[i] = 0.0;
	}
	else if(T > 49.0)
	//for large T, the contribution is exponentially supressed.
	{
		if(verbosity >= 2)
                  printf("using large T expression %f %f %f\n", rho2, T);
		if(params->fermion==1)
		{
			igrand[i]=-1.0*exp(-m2*T)/(3.0*T)*e*e*B*B;
			//if(B>1e-6)
			//	igrand[i]=exp(-m2*T)/(T*T*T)*(TB/tanh(TB)-1.0-1.0/3.0*TB*TB);
			//else
			//	igrand[i]=0.0;
			//printf("igrand(%f)=%f\n",T,igrand[i]);
		}
		else
			igrand[i]=exp(-m2*T)/(6.0*T)*e*e*B*B;
	}
  	else
  	{
		for(j=0;j<groupsize;j++)
        	{
			//printf("func j=%d\n",j);
			WLlist[j]=order[j + i*groupsize];
        	}
		if(verbosity >= 3)
		  printf("EV= %e Exact = %e\n",EV(T,p, WLlist),T/sinh(T));
		if(params->fermion==1)
			igrand[i]= exp(-m2*T)/(T*T*T)*(EV(T, p, WLlist)-1.0-1.0/3.0*TB*TB);
		else
			igrand[i]= exp(-m2*T)/(T*T*T)*(EV(T, p, WLlist)-1.0+1.0/6.0*TB*TB);
		//igrand[i]=Exact(T,1.0);
		//return Exact(T,1.0);

  	}

  }
  if(T > 0.0f && verbosity >= 2){
	igmean = meanigrand(igrand, params->ng, &igerr);
	printf("WLvconstExpression: ");
  		printf("%e %e %e %e %e %e\n",
  		  rho2, T, Exact(T, B, params->fermion), smTscal(B, T), igmean, igerr);
  }
  free(WLlist);
  return igrand;
}

void signal_handler(int sig)
//Floating point signal handler
{
  printf( "Error: Floating point signal detected.\n");
  exit(1);
}

void EVMean(double *EV, float4 *Wsscal_h, float4 *Wsferm_h, int n, int *WL, double T, int fermion)
//Compute the expectation value and error of the Wilson loops
//WL is a list representing a group of wordline indices
//to compute the expectation value of
{
  int i, WLi;
  double EVWlNb2, EVWlN, EVWlNb4;
  double SEWlNb2, SEWlN, SEWlNb4;
  double *EVpart1;
  double *EVpart2;
  double *EVpart4;
  double Nby4part;

  //Turn on floating point exceptions:
  feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW | FE_UNDERFLOW);

  // Set the signal handler:
  signal(SIGFPE, signal_handler);

  *EV = 0.0;
  EVWlNb2 = 0.0; EVWlN = 0.0; EVWlNb4 = 0.0;
  //allocate arrays to store the wilson loop of each worldline
  EVpart1 = (double *) malloc(n*sizeof(*EVpart1));
  EVpart2 = (double *) malloc(n*sizeof(*EVpart2));
  EVpart4 = (double *) malloc(n*sizeof(*EVpart4));
  if(!EVpart1 || !EVpart2 || !EVpart4) printf("EVonly(): malloc failed\n");
  //determine the sum of the contributions
  for(i = 0; i < n; i++){
	WLi=WL[i];
	//Compute the scalar part
	Nby4part = Wsscal_h[WLi].y + Wsscal_h[WLi].z;
	EVpart1[i] = cos(0.5*Wsscal_h[WLi].x+0.25*(Nby4part));
	EVpart2[i] = cos(Wsscal_h[WLi].x);
	EVpart4[i] = cos(0.5*(Nby4part));
	if(fermion == 1)
	{
		if(isinf(Wsferm_h[WLi].x) !=0 ||isinf(Wsferm_h[WLi].y) !=0 ||isinf(Wsferm_h[WLi].z) !=0
		  || isnan(Wsferm_h[WLi].x) !=0 || isnan(Wsferm_h[WLi].y) !=0 ||isnan(Wsferm_h[WLi].z) !=0 )
		{
			printf("Warning: WSferm is infinite. Worldline=%d\n",WLi);
		}
		//Compute the fermion part
		//printf("Wsferm_h[%d].x=%f\n",WLi,Wsferm_h[WLi].x);
		//printf("Wsferm_h[%d].y=%f\n",WLi,Wsferm_h[WLi].y);
		//printf("Wsferm_h[%d].z=%f\n",WLi,Wsferm_h[WLi].z);
		Nby4part = Wsferm_h[WLi].y+Wsferm_h[WLi].z;
		if(abs(Nby4part) > 600.0 | abs(Wsferm_h[WLi].x) > 600.0)
		{
			printf("Warning: Large cosh argument.  T=%f, WLi=%d\n",T,WLi);
			EVpart1[i] *= 1.0e10;
			EVpart2[i] *= 1.0e10;
			EVpart4[i] *= 1.0e10;
		}
                else
		{
			EVpart1[i] *= cosh(0.5*(double)Wsferm_h[WLi].x+0.25*((double)Nby4part));
			EVpart2[i] *= cosh((double)Wsferm_h[WLi].x);
			EVpart4[i] *= cosh(0.5*((double)Nby4part));
			//EVpart1[i]*=cosh(T);
			//EVpart2[i]*=cosh(T);
			//EVpart4[i]*=cosh(T);
		}
		if(isnan(EVpart1[i]) != 0 || isnan(EVpart2[i]) != 0 || isnan(EVpart4[i]) != 0
			|| isinf(EVpart1[i]) != 0 || isinf(EVpart2[i]) != 0 || isinf(EVpart4[i]) != 0)
			printf("Warning: Infinity detected: WL# %d\n",WLi);
	}
	EVWlNb4 += EVpart4[i];
	EVWlNb2 += EVpart2[i];
	EVWlN   += EVpart1[i];
        //printf("EVpart1[%d]=%f\n",i,EVpart1[i]);
  }
  EVWlNb4 /=n;
  EVWlNb2 /=n;
  EVWlN  /=n;
  //Extrapolate to N=infinity
  *EV=8.0/3.0*EVWlN - 2.0*EVWlNb2 + 1.0/3.0*EVWlNb4;

  free(EVpart1);
  free(EVpart2);
  free(EVpart4);

}