#include <stdio.h>
#include <time.h>
#include <linux/limits.h>
#include <sys/stat.h>
#include <unistd.h>
#include <math.h>
#include "Globals.h"
#include "constitutive_equations.h"
#include "Watershed.h"
#include "manufactured_solution.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>

extern void GetLGLWeights(int N, double* w);

void *xcalloc(int items, int size)
{
	void *ptr = calloc(items, size);
	if (ptr == NULL)
	{
		printf("Unable to allocate memory\n");
		exit(EXIT_FAILURE);
	}
	return ptr;
}

static int firstOutput = 1;
static char folderName[500];
static char ChannelHeightFileName[PATH_MAX + 1];
static char ChannelQFileName[PATH_MAX + 1];
static char KinFileName[PATH_MAX + 1];
static char JuncHeightFileName[PATH_MAX + 1];
static char JuncQFileName[PATH_MAX + 1];
static char FpHeightFileName[PATH_MAX + 1];
static char FpQFileName[PATH_MAX + 1];

void outputFloodplainDataToFile(double currtime, double finalTime, double recordTimeIntervals)
{
	char openFileFormat[1];
	int writeHeader = 0;
	if (firstOutput)
	{
		time_t rawtime;
		time(&rawtime);
		struct tm *timeinfo = localtime(&rawtime);
		sprintf(folderName, "DataOutput_%d_%02d_%02d_%02d_%02d_%02d",timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
		if (mkdir(folderName, 0777) == -1)
			perror("The following error occured while making the data output directory");

		sprintf(FpHeightFileName, "%s/Floodplains.63", folderName);
		sprintf(FpQFileName, "%s/Floodplains.64", folderName);

		openFileFormat[0] = 'w';
		writeHeader = 1;
		firstOutput = 0;
	}
	else
	{
		openFileFormat[0] = 'a';
	}

	// Output Floodplain data
	FILE *file1 = fopen(FpHeightFileName, openFileFormat);
	FILE *file2 = fopen(FpQFileName, openFileFormat);
	if (!file1)
		printf("Error while opening file1\n");
	if (!file2)
		printf("Error while opening file2\n");
	if (writeHeader)
	{
		int numRecords;
		if (fmod(finalTime, recordTimeIntervals) == 0)
			numRecords = finalTime/recordTimeIntervals + 1;
		else
			numRecords = finalTime/recordTimeIntervals + 2;

		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file1, "Number of Floodplains = %d\n", NumFloodplains);
		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file2, "Number of Floodplains = %d\n", NumFloodplains);
		for (int j = 0; j < NumFloodplains; j++)
		{
			fprintf(file1, "Number of data points for junction %d = %d\n", j, FloodplainList[j]->NumEl);
			fprintf(file2, "Number of data points for junction %d = %d\n", j, FloodplainList[j]->NumEl);
		}
	}
	for (int i=0; i < NumFloodplains; i++)
	{
		fprintf(file1, "# Average H and Zeta at time %.3f for Junction %d\n", currtime, i);
		fprintf(file2, "# Average Qx and Qy at time %.3f for Junction %d\n", currtime, i);

		for (int j=0; j < FloodplainList[i]->NumEl; ++j)
		{
			double avgZeta = 0, avgHeight = 0, avgQx = 0, avgQy =0, avgz =0, zarr[3];
			for(int k=0; k<3; ++k)
			{
				double zeta = FloodplainList[i]->zeta[j][k];
				double Qx = FloodplainList[i]->Qx[j][k];
				double Qy = FloodplainList[i]->Qy[j][k];
				zarr[k]= FloodplainList[i]->NodalZ[j][k];
				double height = zeta + zarr[k];

				avgZeta += zeta;
				avgHeight += height;
				avgQx += Qx;
				avgQy += Qy;
				avgz += zarr[k];
			}
			fprintf(file1, "%d \t %.13f \t %.13f \n", j, avgHeight/3, avgZeta/3);
			fprintf(file2, "%d \t %.13f \t %.13f\n", j, avgQx/3, avgQy/3 );
		}
	}
	fclose(file1);
	fclose(file2);

}

void output2DNodalError(double time)
{
	char *fileName1 = "NodalErrorsH.out";
	char *fileName2 = "NodalErrorsQ.out";
	FILE* ef1 = fopen(fileName1, "w");
	FILE* ef2 = fopen(fileName2, "w");

	int i = 0;
	for (int j=0; j < FloodplainList[i]->NumVerts; ++j)
	{
		double xval = FloodplainList[i]->Vx[j];
		double yval = FloodplainList[i]->Vy[j];
		int num_conn_els = FloodplainList[i]->ElCount[i];
		double nodalZeta = 0;
		double nodalQx = 0;
		double nodalQy = 0;
		for (int k = 0; k < num_conn_els; k++)
		{
			int curr_el = FloodplainList[i]->VtoEl[j][k];
			int loc_v;
			// find the local vertex number for curr_el
			for (int v = 0; v < 3; v++)
			{
				if (j == FloodplainList[i]->EltoVert[curr_el*3+v])
				{
					loc_v = v;
					break;
				}
			}

			int loc_node = FloodplainList[i]->VtoNode[curr_el][loc_v];
			nodalZeta += FloodplainList[i]->zeta[curr_el][loc_node];
			nodalQx += FloodplainList[i]->Qx[curr_el][loc_node];
			nodalQy += FloodplainList[i]->Qy[curr_el][loc_node];

		}
		nodalZeta /= num_conn_els;
		nodalQx /= num_conn_els;
		nodalQy /= num_conn_els;

		double manzeta = getmanH(xval, yval, time);
		double manQx = getQx(xval, yval, time);
		double manQy = getQy(xval, yval, time);

		double er1 = -nodalZeta + manzeta;
		double er2 = -nodalQx + manQx;
		double er3 = -nodalQy + manQy;

		fprintf(ef1, "%d \t %.13f\n", j, er1);
		fprintf(ef2, "%d \t %.13f \t %.13f\n", j, er2, er3);
	}

	fclose(ef1);
	fclose(ef2);

}

void outputFloodplainNodalDataToFile(double currtime, double finalTime, double recordTimeIntervals)
{
	char openFileFormat[1];
	int writeHeader = 0;
	if (firstOutput)
	{
		time_t rawtime;
		time(&rawtime);
		struct tm *timeinfo = localtime(&rawtime);
		sprintf(folderName, "DataOutput_%d_%02d_%02d_%02d_%02d_%02d",timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
		if (mkdir(folderName, 0777) == -1)
			perror("The following error occured while making the data output directory");

		sprintf(FpHeightFileName, "%s/Floodplains.63", folderName);
		sprintf(FpQFileName, "%s/Floodplains.64", folderName);

		openFileFormat[0] = 'w';
		writeHeader = 1;
		firstOutput = 0;
	}
	else
	{
		openFileFormat[0] = 'a';
	}

	// Output Floodplain data
	FILE *file1 = fopen(FpHeightFileName, openFileFormat);
	FILE *file2 = fopen(FpQFileName, openFileFormat);
	if (!file1)
		printf("Error while opening file1\n");
	if (!file2)
		printf("Error while opening file2\n");
	if (writeHeader)
	{
		int numRecords;
		if (fmod(finalTime, recordTimeIntervals) == 0)
			numRecords = finalTime/recordTimeIntervals + 1;
		else
			numRecords = finalTime/recordTimeIntervals + 2;

		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file1, "Number of Floodplains = %d\n", NumFloodplains);
		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file2, "Number of Floodplains = %d\n", NumFloodplains);
		for (int j = 0; j < NumFloodplains; j++)
		{
			fprintf(file1, "Number of data points for junction %d = %d\n", j, FloodplainList[j]->NumVerts);
			fprintf(file2, "Number of data points for junction %d = %d\n", j, FloodplainList[j]->NumVerts);
		}
	}
	for (int i=0; i < NumFloodplains; i++)
	{
		fprintf(file1, "# Nodal H at time %.3f for Junction %d\n", currtime, i);
		fprintf(file2, "# Nodal Qx and Qy at time %.3f for Junction %d\n", currtime, i);

		for (int j=0; j < FloodplainList[i]->NumVerts; ++j)
		{
			int num_conn_els = FloodplainList[i]->ElCount[i];
			double nodalZeta = 0;
			double nodalQx = 0;
			double nodalQy = 0;
			for (int k = 0; k < num_conn_els; k++)
			{
				int curr_el = FloodplainList[i]->VtoEl[j][k];
				int loc_v;
				// find the local vertex number for curr_el
				for (int v = 0; v < 3; v++)
				{
					if (j == FloodplainList[i]->EltoVert[curr_el*3+v])
					{
						loc_v = v;
						break;
					}
				}

				int loc_node = FloodplainList[i]->VtoNode[curr_el][loc_v];
				nodalZeta += FloodplainList[i]->zeta[curr_el][loc_node];
				nodalQx += FloodplainList[i]->Qx[curr_el][loc_node];
				nodalQy += FloodplainList[i]->Qy[curr_el][loc_node];

			}
			nodalZeta /= num_conn_els;
			nodalQx /= num_conn_els;
			nodalQy /= num_conn_els;

			fprintf(file1, "%d \t %.13f\n", j, nodalZeta);
			fprintf(file2, "%d \t %.13f \t %.13f\n", j, nodalQx, nodalQy );
		}
	}
	fclose(file1);
	fclose(file2);

}
void outputDataToFile(double currtime, double finalTime, double recordTimeIntervals)
{
	char openFileFormat[1];
	int writeHeader = 0;
	if (firstOutput)
	{
		time_t rawtime;
		time(&rawtime);
		struct tm *timeinfo = localtime(&rawtime);
		sprintf(folderName, "DataOutput_%d_%02d_%02d_%02d_%02d_%02d",timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
		if (mkdir(folderName, 0777) == -1)
			perror("The following error occured while making the data output directory");

		sprintf(ChannelHeightFileName, "%s/Channels.63", folderName);
		sprintf(ChannelQFileName, "%s/Channels.64", folderName);
		sprintf(KinFileName, "%s/KinField.63", folderName);
		sprintf(JuncHeightFileName, "%s/Junctions.63", folderName);
		sprintf(JuncQFileName, "%s/Junctions.64", folderName);
		//sprintf(FpHeightFileName, "%s/Floodplains.63", folderName);
		//sprintf(FpQFileName, "%s/Floodplains.64", folderName);

		// Move junction grid data file to this newly created folder
		/*char NewJunctionGridFileName[PATH_MAX + 1];
		sprintf(NewJunctionGridFileName, "%s/JunctionMesh.14", folderName);
		int ret = rename("./Output/JunctionMesh.14", NewJunctionGridFileName);
		if (ret != 0)
			printf("Error: unable to move the junction grid file\n");
*/
		// Move flowpaths file to this newly created folder
		int ret;
		char NewFlowPathsFileName[PATH_MAX + 1];
		sprintf(NewFlowPathsFileName, "%s/FlowPaths.out", folderName);
		ret = rename("./Output/FlowPaths.out", NewFlowPathsFileName);
		if (ret != 0)
			printf("Error: unable to move the flow paths file\n");

		// Move ChannelNodes.14 to this newly created folder
/*		char NewChannelsGridFileName[PATH_MAX + 1];
		sprintf(NewChannelsGridFileName, "%s/Channels.14", folderName);
		ret = rename("./Output/Channels.14", NewChannelsGridFileName);
		if (ret != 0)
			printf("Error: unable to move channels grid file\n");
*/
		openFileFormat[0] = 'w';
		writeHeader = 1;
		firstOutput = 0;
	}
	else
	{
		openFileFormat[0] = 'a';
	}

		//char cwd[PATH_MAX+1];
	//if (getcwd(cwd, sizeof(cwd))==NULL)
	//	perror("Error while getting current working directory");

	//Output Channels data
	FILE* file1;
	FILE* file2;
	file1 = fopen(ChannelHeightFileName, openFileFormat);
	file2 = fopen(ChannelQFileName, openFileFormat);
	//if (!file1 || !file2)
	//{
	//	printf("Could not open file for writing. Exiting now\n");
	//	exit(1);
	//}
	if (writeHeader)
	{
		int numRecords;
		if (fmod(finalTime, recordTimeIntervals) == 0)
			numRecords = finalTime/recordTimeIntervals+1;
		else
			numRecords = finalTime/recordTimeIntervals + 2;
		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file1, "Number of Channels = %d\n", NumChannels);
		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file2, "NumChannels = %d\n", NumChannels);
		for (int c = 0; c < NumChannels; c++)
		{
			fprintf(file1, "Number of data points for channel %d = %d\n", c, ChannelList[c]->NumNodes);
			fprintf(file2, "Number of data points for channel %d = %d\n", c, ChannelList[c]->NumNodes);
		}
	}
	for (int i=0; i<NumChannels; i++)
	{
		fprintf(file1, "# H and Zeta at time %.3f for Channel %d\n", currtime, i);
		fprintf(file2, "#Q at time %.3f for Channel %d\n", currtime, i);

		int NumNodes = ChannelList[i]->NumNodes;

		for (int j = 0; j<NumNodes; j++)
		{
			double bval = ChannelList[i]->NodalB[j];
			double zval = ChannelList[i]->NodalZ[j];
			double x_val = ChannelList[i]->NodalX[j];
			double y_val = ChannelList[i]->NodalY[j];
			double A = ChannelList[i]->A[j+1];
			double Q = ChannelList[i]->Q[j+1];
			double m1val = ChannelList[i]->Nodalm1[j];
			double m2val = ChannelList[i]->Nodalm2[j];
			double H = getH(A, bval, m1val, m2val);
			double zeta = H - zval;
			fprintf(file1, "%.13f \t %.13f\n", H, zeta);
			fprintf(file2, "%.13f\n", Q);

		}

	}

	fclose(file1);
	fclose(file2);

	// Output Junction data
	file1 = fopen(JuncHeightFileName, openFileFormat);
	file2 = fopen(JuncQFileName, openFileFormat);
	if (!file1)
		printf("Error while opening file1\n");
	if (!file2)
		printf("Error while opening file2\n");
	if (writeHeader)
	{
		int numRecords;
		if (fmod(finalTime, recordTimeIntervals) == 0)
			numRecords = finalTime/recordTimeIntervals + 1;
		else
			numRecords = finalTime/recordTimeIntervals + 2;

		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file1, "Number of Junctions = %d\n", NumJunctions);
		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		fprintf(file2, "NumJunctions = %d\n", NumJunctions);
		for (int j = 0; j < NumJunctions; j++)
		{
			fprintf(file1, "Number of data points for junction %d = %d\n", j, JunctionList[j]->NumEl);
			fprintf(file2, "Number of data points for junction %d = %d\n", j, JunctionList[j]->NumEl);
		}
	}
	for (int i=0; i<NumJunctions; i++)
	{

		fprintf(file1, "# Average H and Zeta at time %.3f for Junction %d\n", currtime, i);
		fprintf(file2, "# Average Qx and Qy at time %.3f for Junction %d\n", currtime, i);

		for (int j=0; j<JunctionList[i]->NumEl; ++j)
		{
			double avgZeta = 0, avgHeight = 0, avgQx = 0, avgQy =0, avgz =0, zarr[3];
			for(int k=0; k<3; ++k)
			{
				double zeta = JunctionList[i]->zeta[j][k];
				double Qx = JunctionList[i]->Qx[j][k];
				double Qy = JunctionList[i]->Qy[j][k];
				zarr[k]= JunctionList[i]->NodalZ[j][k];
				double height = zeta + zarr[k];

				avgZeta += zeta;
				avgHeight += height;
				avgQx += Qx;
				avgQy += Qy;
				avgz += zarr[k];

			}
			fprintf(file1, "%d \t %.13f \t %.13f \n", j, avgHeight/3, avgZeta/3);
			fprintf(file2, "%d \t %.13f \t %.13f\n", j, avgQx/3, avgQy/3 );
		}
	}
	fclose(file1);
	fclose(file2);

	// Output Kinematic Field Data
	// Here we assume that we are working with first order polynomials only
	if (FloodplainList[0]->floodedStatus == 0)
	{
		file1 = fopen(KinFileName, openFileFormat);
		if (writeHeader)
		{
			int numRecords;
			if (fmod(finalTime, recordTimeIntervals) == 0)
				numRecords = finalTime/recordTimeIntervals + 1;
			else
				numRecords = finalTime/recordTimeIntervals + 2;

			fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
		}
		fprintf(file1,"# Average height at time %3.3f in Kinematic Field\n", currtime);

		int NumEl = FloodplainList[0]->NumEl;
		for (int i = 0; i < NumEl; i++)
		{
			if (KinematicElList[i]->isActive == 1)
			{
				double weq = KinematicElList[i]->weq;
				double height = KinematicElList[i]->A[0]/weq;
				if (KinematicElList[i]->numUpstreamEls > 0)
				{
					int numUpstreamEls = KinematicElList[i]->numUpstreamEls;
					height = KinematicElList[i]->A[0]/weq;
					for (int j = 0; j < numUpstreamEls; j++)
					{
						int el = KinematicElList[i]->upstreamEls[j];
						height += KinematicElList[el]->A[1]/weq;
					}
					height = height/(numUpstreamEls+1);
				}
				//printf("%d %.13f\n", i, height);
				fprintf(file1, "%d %.13f\n", i, height);
			}
			else
				fprintf(file1, "%d %1.13f\n", i, 1e-7);
		}
		fclose(file1);
	}

	//static int previouslyFlooded = 0;
	//static char fpOpenFileFormat[1];
	//int writefpHeader = 0;

	//// Output Floodplain data if the channels have flooded
	//if (FloodplainList[0]->floodedStatus == 1)
	//{
	//	if (previouslyFlooded)
	//	{
	//		fpOpenFileFormat[0] = 'a';
	//	}
	//	else
	//	{
	//		fpOpenFileFormat[0] = 'w';
	//		writefpHeader = 1;
	//	}

	//	file1 = fopen(FpHeightFileName, fpOpenFileFormat);
	//	file2 = fopen(FpQFileName, fpOpenFileFormat);

	//
	//	if (!file1)
	//		printf("Error while opening file1\n");
	//	if (!file2)
	//		printf("Error while opening file2\n");
	//	if (writefpHeader)
	//	{
	//		int numRecords;
	//		if (fmod(finalTime, recordTimeIntervals) == 0)
	//			numRecords = finalTime/recordTimeIntervals + 1;
	//		else
	//			numRecords = finalTime/recordTimeIntervals + 2;

	//		fprintf(file1, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
	//		fprintf(file2, "End of Simulation Time = %lf.\nTotal Number of Records = %d\n", finalTime, numRecords);
	//		fprintf(file1, "Number of data points for floodplain = %d\n", FloodplainList[0]->NumEl);
	//		fprintf(file2, "Number of data points for floodplain = %d\n", FloodplainList[0]->NumEl);
	//	}
	//
	//	fprintf(file1, "# Average H and Zeta at time %.3f\n", currtime);
	//	fprintf(file2, "# Average Qx and Qy at time %.3f\n", currtime);
	//
	//	for (int j=0; j<FloodplainList[0]->NumEl; ++j)
	//	{
	//		double avgZeta = 0, avgHeight = 0, avgQx = 0, avgQy =0, avgz =0, zarr[3];
	//		for(int k=0; k<3; ++k)
	//		{
	//			double zeta = FloodplainList[0]->zeta[j][k];
	//			double Qx = FloodplainList[0]->Qx[j][k];
	//			double Qy = FloodplainList[0]->Qy[j][k];
	//			zarr[k]= FloodplainList[0]->NodalZ[j][k];
	//			double height = zeta + zarr[k];
	//
	//			avgZeta += zeta;
	//			avgHeight += height;
	//			avgQx += Qx;
	//			avgQy += Qy;
	//			avgz += zarr[k];

	//		}
	//		fprintf(file1, "%d \t %.13f \t %.13f \n", j, avgHeight/3, avgZeta/3);
	//		fprintf(file2, "%d \t %.13f \t %.13f\n", j, avgQx/3, avgQy/3 );
	//	}
	//	fclose(file1);
	//	fclose(file2);

	//	previouslyFlooded = 1;

	//}

}

void calculate_l2_error_2d(double time)
{
	int NumEl = FloodplainList[0]->NumEl;
	int Np = FloodplainList[0]->Np;
	double errsqH = 0, errsqQx = 0, errsqQy = 0;

	gsl_vector *diffH = gsl_vector_alloc(Np);
	gsl_vector *diffQx = gsl_vector_alloc(Np);
	gsl_vector *diffQy = gsl_vector_alloc(Np);

	//double max = 0;
	for (int i = 0; i < NumEl; i++)
	{
		for (int j = 0; j < Np; j++)
		{
			double xval = FloodplainList[0]->NodalX[i][j];
			double yval = FloodplainList[0]->NodalY[i][j];
			double exactH = getmanH(xval, yval, time);
			double exactQx = getQx(xval, yval, time);
			double exactQy = getQy(xval, yval, time);
			double compH = FloodplainList[0]->zeta[i][j];
			double compQx =  FloodplainList[0]->Qx[i][j];
			double compQy = FloodplainList[0]->Qy[i][j];
			gsl_vector_set(diffH, j, fabs(exactH-compH));
			gsl_vector_set(diffQx, j, fabs(exactQx-compQx));
			gsl_vector_set(diffQy, j, fabs(exactQy-compQy));
			//gsl_vector_set(diffH, j, RHSZeta[i*Np+j]);
			//gsl_vector_set(diffQx, j,  RHSQx[i*Np+j]);
			//gsl_vector_set(diffQy,j, RHSQy[i*Np+j]);
			//max = fmax(fabs(exactQx - compQx), max);

		}

		gsl_vector *tmpH = gsl_vector_alloc(Np);
		gsl_vector *tmpQx = gsl_vector_alloc(Np);
		gsl_vector *tmpQy = gsl_vector_alloc(Np);
		double jac = FloodplainList[0]->jac[i];
		gsl_blas_dgemv(CblasNoTrans, jac, MassMatrix2D, diffH, 0.0, tmpH);
		gsl_blas_dgemv(CblasNoTrans, jac, MassMatrix2D, diffQx, 0.0, tmpQx);
		gsl_blas_dgemv(CblasNoTrans, jac, MassMatrix2D, diffQy, 0.0, tmpQy);

		double elerrHsq, elerrQxsq, elerrQysq;
		gsl_blas_ddot(diffH, tmpH, &elerrHsq);
		gsl_blas_ddot(diffQx, tmpQx, &elerrQxsq);
		gsl_blas_ddot(diffQy, tmpQy, &elerrQysq);

		errsqH += elerrHsq;
		errsqQx += elerrQxsq;
		errsqQy += elerrQysq;

		gsl_vector_free(tmpH);
		gsl_vector_free(tmpQx);
		gsl_vector_free(tmpQy);
	}

	gsl_vector_free(diffH);
	gsl_vector_free(diffQx);
	gsl_vector_free(diffQy);

	printf("time at error computation = %lf\n", time);
	printf("l2 error in H = %lf \n", sqrt(errsqH));
	printf("l2 error in Qx = %lf \n", sqrt(errsqQx));
	printf("l2 error in Qy = %lf \n", sqrt(errsqQy));
	//printf("maximum difference in Qx = %lf\n", max);

}

void append_to_file(char *fileName, double time, double data)
{
	FILE* file = fopen(fileName, "a");
	fprintf(file, "%lf %lf\n", time,data);
	fclose(file);
}

double calculateTotalWater(struct channel *Chan)
{
	int NumEl = Chan->NumEl;
	int Np = Chan->Np;
	int P = Chan->P;

	double totalWater = 0;
	double LGLWeight[Np];
	GetLGLWeights(P, LGLWeight);
	for (int k = 0; k < NumEl; k++)
	{
		double water = 0;
		double dh = Chan->dh[k];
		for (int i = 0; i < Np; i++)
		{
			double m1val = Chan->Nodalm1[k*Np+i];
			double m2val = Chan->Nodalm2[k*Np+i];
			double bval = Chan->NodalB[k*Np+i];
			double A = Chan->A[k*Np+i+1];
			double h = getH(A, bval, m1val, m2val);
			water += LGLWeight[i]*(h-0.0000001);
		}
		totalWater += 0.5*dh*water;
	}

	return totalWater;

}

double ftTom(double ft)
{
	double meter = ft*0.3048;
	return meter;
}

double mToft (double m)
{
	double ft = m*3.28084;
	return ft;

}


#include <stdio.h>
#include <unistd.h>
#include <assert.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include "dlib.h"
#include "svec.h"
#include "rng.h"

const char *usage = "Usage: scode [OPTIONS] < file\n"
  "file should have columns of arbitrary tokens\n"
  "-r RESTART: number of restarts (default 1)\n"
  "-i NITER: number of iterations over data (default UINT32_MAX)\n"
  "-t THRESHOLD: quit if logL increase for iter <= this (default .001)\n"
  "-d NDIM: number of dimensions (default 25)\n"
  "-z Z: partition function approximation (default 0.166)\n"
  "-p PHI0: learning rate parameter (default 50.0)\n"
  "-u ETA0: learning rate parameter (default 0.2)\n"
  "-s SEED: random seed (default 0)\n"
  "-c calculate real Z (default false)\n"
  "-w The first line of the input is weights (default false)\n"
  "-v verbose messages (default false)\n";

//typedef uint32_t u32;
//typedef uint64_t u64;
u32 RESTART = 1;
u32 NITER = UINT32_MAX;
double THRESHOLD = 0.001;
u32 NDIM = 25;
double Z = 0.166;
double PHI0 = 50.0;
double ETA0 = 0.2;
unsigned long int SEED = 0;
bool CALCZ = false;
bool WEIGHT = false;
bool VERBOSE = false;

u32 NTOK = 0;
u64 NTUPLE = 0;

const gsl_rng_type *rng_T;
gsl_rng *rng_R = NULL;
darr_t data;
u64 **update_cnt;
double * weight = NULL;
double * uweight = NULL; /*Updated weights*/
u64 **cnt;
#define frq(i,j) ((double)cnt[i][j]*NTOK/len(data))
svec **vec;
svec **best_vec;
svec dummy_vec;
sym_t qmax;
sym_t NULLFEATID;
#define NULLFEATMARKER "/XX/"

int main(int argc, char **argv);
void init_rng();
void free_rng();
u64 init_data();
u32 init_weight();
void free_weight();
void randomize_vectors();
void copy_best_vec();
void free_data();
void update_tuple(sym_t *t);
double logL();
double calcZ();

#define vmsg(...) if(VERBOSE)msg(__VA_ARGS__)

int main(int argc, char **argv) {
  int opt;
  while((opt = getopt(argc, argv, "r:i:t:d:z:p:u:s:cwv")) != -1) {
    switch(opt) {
    case 'r': RESTART = atoi(optarg); break;
    case 'i': NITER = atoi(optarg); break;
    case 't': THRESHOLD = atof(optarg); break;
    case 'd': NDIM = atoi(optarg); break;
    case 'z': Z = atof(optarg); break;
    case 'p': PHI0 = atof(optarg); break;
    case 'u': ETA0 = atof(optarg); break;
    case 's': SEED = atoi(optarg); break;
    case 'c': CALCZ = true; break;
    case 'w': WEIGHT = true; break;
    case 'v': VERBOSE = true; break;
    default: die("%s",usage);
    }
  }

  vmsg("scode -r %u -i %u -t %g -d %u -z %g -p %g -u %g -s %lu %s%s%s",
       RESTART, NITER, THRESHOLD, NDIM, Z, PHI0, ETA0, SEED,
       (CALCZ ? "-c " : ""), (WEIGHT ? "-w " : ""), (VERBOSE ? "-v " : ""));

  init_rng();
  if (SEED) gsl_rng_set(rng_R, SEED);
  if (WEIGHT) NTOK = init_weight();
  NTUPLE = init_data();
  vmsg("Read %zu tuples %u uniq tokens", NTUPLE, qmax);

  double best_logL = 0;
  for (u32 start = 0; start < RESTART; start++) {
    randomize_vectors();
    double ll = logL();
    vmsg("Restart %u/%u logL0=%g best=%g", 1+start, RESTART, ll, best_logL);
    if (CALCZ) vmsg("Z=%g (approx %g)", calcZ(), Z);
    for (u32 iter = 0; iter < NITER; iter++) {
      for (u64 di = 0; di < NTUPLE; di++) {
	update_tuple(&val(data, di * NTOK, sym_t));
      }
      double ll0 = ll;
      ll = logL();
      vmsg("Iteration %u/%u logL=%g", 1+iter, NITER, ll);
      if (ll - ll0 <= THRESHOLD) break;
    }
    if (start == 0 || ll > best_logL) {
      vmsg("Updating best_vec with logL=%g", ll);
      best_logL = ll;
      copy_best_vec();
    }
    vmsg("Restart %u/%u logL1=%g best=%g", 1+start, RESTART, ll, best_logL);
    if (CALCZ) vmsg("Z=%g (approx %g)", calcZ(), Z);
  }
  for (u32 t = 0; t < NTOK; t++) {
    for (sym_t q = 1; q <= qmax; q++) {
      if (best_vec[t][q] == NULL) continue;
      printf("%u:%s\t%zu\t", t, sym2str(q), cnt[t][q]);
      svec_print(best_vec[t][q]);
      putchar('\n');
    }
  }
  fflush(stdout);
  free_data();
  free_rng();
  if (WEIGHT) free_weight();
  symtable_free();
  dfreeall();
  fprintf(stderr, "%f\n", best_logL);
  vmsg("bye");
}

double logL() {
  double l = 0;
  for (u64 i = 0; i < NTUPLE; i++) {
    sym_t *t = &val(data, i * NTOK, sym_t);
    sym_t x = t[0];
    sym_t y = t[1];
    float px = frq(0, x);
    float py = frq(1, y);
    svec vx = vec[0][x];
    svec vy = vec[1][y];
    float xy = svec_sqdist(vx, vy);
    l += log(px * py) - xy;
  }
  return (l / NTUPLE - log(Z));
}

double calcZ() {
  double z = 0;
  for (sym_t x = 1; x <= qmax; x++) {
    if (VERBOSE && (x % 1000 == 0)) fputc('.', stderr);
    if (cnt[0][x] == 0) continue;
    float px = frq(0, x);
    svec vx = vec[0][x];
    for (sym_t y = 1; y <= qmax; y++) {
      if (cnt[1][y] == 0) continue;
      float py = frq(1, y);
      svec vy = vec[1][y];
      float xy = svec_sqdist(vx, vy);
      z += px * py * exp(-xy);
    }
  }
  if (VERBOSE) fputc('\n', stderr);
  return z;
}

void update_tuple(sym_t *t) {
  /*weighted update*/
  static svec *u = NULL;
  static svec *v = NULL;
  static svec dx = NULL;
  if (u == NULL) u = _d_malloc(NTOK * sizeof(svec));
  if (v == NULL) v = _d_malloc(NTOK * sizeof(svec));
  if (dx == NULL) dx = svec_alloc(NDIM);
  for (u32 i = 0; i < NTOK; i++) u[i] = vec[i][t[i]];
  for (u32 i = 0; i < NTOK; i++) {
    /* Sampling values from the marginal distributions. */
    /* Can this be done once, or do we have to resample for every x? */
    if(i > 0 && t[i] == NULLFEATID) continue;
    for (u32 j = 0; j < NTOK; j++) {
      if (j==i) { v[j] = u[i]; continue;}
      u64 r = gsl_rng_get(rng_R);
      r = (r << 32) | gsl_rng_get(rng_R);
      r = r % NTUPLE;
      sym_t y = val(data, r * NTOK + j, sym_t);
      v[j] = vec[j][y];
      if(i > 0) break;
    }
    /* Compute the move for u[i] */
    svec_set_zero(dx);
    double ww;
    for (u32 j = 0; j < NTOK; j++) {
      if (j == i) continue;
      ww = weight == NULL ? 1 : (i > 0 ? weight[i] : weight[j]);
      double push = 0, pull = 0;
      if (v[j] == NULL) v[j] = dummy_vec;
      else push = exp(-svec_sqdist(u[i], v[j])) / Z;
      if(u[j] == NULL)  u[j] = dummy_vec;
      else pull = 1;
      if(push != 0 || pull != 0){
	for (u32 d = 0; d < NDIM; d++) {
	  float dxd = svec_get(dx, d);
	  float x = svec_get(u[i], d);
	  float y = svec_get(u[j], d);
	  float z = svec_get(v[j], d);
	  svec_set(dx, d, dxd + ww * ( pull * (y - x) + push * (x - z)));
	}
      }
      /*restore the vectors to original forms*/
      if(push == 0) v[j] = NULL;
      if(pull == 0) u[j] = NULL;
      if(i > 0) break;
      }
    /* Apply the move scaled by learning parameter */
    u64 cx = update_cnt[i][t[i]]++;
    float nx = ETA0 * (PHI0 / (PHI0 + cx));
    svec_scale(dx, nx);
    svec_add(u[i], dx);
    svec_normalize(u[i]);
  }
}

u32 init_weight(){
  u32 size = 100, i = 0;
  weight = _d_malloc(size * sizeof(double));
  forline (buf, NULL) {
    fortok (tok, buf) {
      weight[i] = atof(tok);
      assert(weight[i++] >= 0);
      if(i >= 100) {
	size *= 2;
	weight = _d_realloc(weight, size);
      }
    }
    assert(i > 0);
    break;
  }
  return i;
}

void free_weight() {
  if (weight != NULL) _d_free(weight);
}

u64 init_data() {
  qmax = 0;
  data = darr(0, sym_t);
  forline (buf, NULL) {
    u32 ntok = 0;
    fortok (tok, buf) {
      sym_t q = str2sym(tok, true);
      if (q > qmax) qmax = q;
      size_t lendata = len(data);
      val(data, lendata, sym_t) = q;
      if(strcmp(tok, NULLFEATMARKER) == 0) NULLFEATID = q;
      ntok++;
    }
    if(NTOK == 0) NTOK = ntok;
    assert(ntok == NTOK); //Each line has equal number of tokens
  }
  assert(NTOK > 0);
  update_cnt = _d_malloc(NTOK * sizeof(ptr_t));
  cnt = _d_malloc(NTOK * sizeof(ptr_t));
  vec = _d_malloc(NTOK * sizeof(ptr_t));
  best_vec = _d_malloc(NTOK * sizeof(ptr_t));
  dummy_vec = svec_alloc(NDIM);
  svec_zero(dummy_vec);
  uweight = _d_calloc(NTOK, sizeof(double));
  for (u32 i = 0; i < NTOK; i++) {
    update_cnt[i] = _d_calloc(qmax+1, sizeof(u64));
    cnt[i] = _d_calloc(qmax+1, sizeof(u64));
    vec[i] = _d_calloc(qmax+1, sizeof(svec));
    best_vec[i] = _d_calloc(qmax+1, sizeof(svec));
  }
  u64 N = len(data) / NTOK;
  for (u64 i = 0; i < N; i++) {
    sym_t *p = &val(data, i * NTOK, sym_t);
    for (u32 j = 0; j < NTOK; j++) {
      sym_t k = p[j];
      assert(k <= qmax);
      cnt[j][k]++;
      if(k == NULLFEATID){
	vec[j][k] = best_vec[j][k] = NULL;
      }
      else if (vec[j][k] == NULL) {
	vec[j][k] = svec_alloc(NDIM);
	best_vec[j][k] = svec_alloc(NDIM);
      }
    }
  }
  return N;
}

void free_data() {
  for (u32 i = 0; i < NTOK; i++) {
    for (sym_t j = 0; j <= qmax; j++) {
      if (vec[i][j] != NULL) {
	svec_free(vec[i][j]);
	svec_free(best_vec[i][j]);
      }
    }
    _d_free(best_vec[i]);
    _d_free(vec[i]);
    _d_free(cnt[i]);
    _d_free(update_cnt[i]);
  }
  _d_free(uweight);
  svec_free(dummy_vec);
  _d_free(best_vec);
  _d_free(vec);
  _d_free(cnt);
  _d_free(update_cnt);
  darr_free(data);
}

void randomize_vectors() {
  for (u32 j = 0; j < NTOK; j++) {
    for (sym_t q = 1; q <= qmax; q++) {
      if (vec[j][q] != NULL) {
	svec_randomize(vec[j][q]);
	update_cnt[j][q] = 0;
      }
    }
  }
}

void copy_best_vec() {
  for (u32 j = 0; j < NTOK; j++) {
    for (sym_t q = 1; q <= qmax; q++) {
      if (vec[j][q] != NULL) {
	svec_memcpy(best_vec[j][q], vec[j][q]);
      }
    }
  }
}

void init_rng() {
  gsl_rng_env_setup();
  rng_T = gsl_rng_mt19937;
  rng_R = gsl_rng_alloc(rng_T);
}

void free_rng() {
  gsl_rng_free(rng_R);
}


#include <pthread.h>
#include <math.h>
#include <fftw.h>
#include <stdio.h>

//C -> Very ugly code

//DEFINITION OF COMPLEX NUMBERS
typedef struct {
  double re, im;
} mycom;

//DEFINITION OF DATA ARRAY NEEDED FOR THE TRANSFORMATION (small space)
typedef struct {
  double* m;
  double* vi;
  double* vo;
  int*    nrop;
} transdata;

//DEFINITION OF DATA ARRAY NEEDED FOR THE TRANSFORMATION (large space)
typedef struct {
  double* m;
  double* vi;
  double* vo;
  long long int*    nrop;
} ltransdata;

//THREAD FUNCTION FOR FORWARD TRANSFORMATION (small space)
void transforeT(void* td){
  transdata* Td = (transdata *)td;
  double* Mat       = Td->m;
  double* Vec_in    = Td->vi;
  double* Vec_out   = Td->vo;
  int*       Nrop   = Td->nrop;
  int x, y;

  for(x = 0; x < 2* *Nrop; x++){
    Vec_out[x] = 0.;
  }

  for(x = 0; x < *Nrop; x++){
    for(y = 0; y < *Nrop; y++){
      Vec_out[2*x]    +=     Mat[(x* (*Nrop))+y]*Vec_in[2*y];
      Vec_out[2*x+1]  +=     Mat[(x* (*Nrop))+y]*Vec_in[2*y+1];
    }
  }

}

//THREAD FUNCTION FOR FORWARD TRANSFORMATION (large space)
void ltransforeT(void* td){
  ltransdata* Td = (ltransdata *)td;
  double* Mat       = Td->m;
  double* Vec_in    = Td->vi;
  double* Vec_out   = Td->vo;
  long long int*       Nrop   = Td->nrop;
  long long int x, y;

  for(x = 0; x < 2* *Nrop; x++){
    Vec_out[x] = 0.;
  }

  for(x = 0; x < *Nrop; x++){
    for(y = 0; y < *Nrop; y++){
      Vec_out[2*x]    +=     Mat[(x* (*Nrop))+y]*Vec_in[2*y];
      Vec_out[2*x+1]  +=     Mat[(x* (*Nrop))+y]*Vec_in[2*y+1];
    }
  }

}

//THREAD FUNCTION FOR BACKWARD TRANSFORMATION (small space)
void transbackT(void* td){
  transdata* Td = (transdata *)td;
  double* Mat       = Td->m;
  double* Vec_in    = Td->vi;
  double* Vec_out   = Td->vo;
  int*       Nrop   = Td->nrop;
  int x, y;

  for(x = 0; x < 2* *Nrop; x++){
    Vec_out[x] = 0.;
  }

  for(x = 0; x < *Nrop; x++){
    for(y = 0; y < *Nrop; y++){
      Vec_out[2*y]    +=     Mat[x* *Nrop+y]*Vec_in[2*x];
      Vec_out[2*y+1]  +=     Mat[x* *Nrop+y]*Vec_in[2*x+1];
    }
  }

}

//THREAD FUNCTION FOR BACKWARD TRANSFORMATION (large space)
void ltransbackT(void* td){
  ltransdata* Td = (ltransdata *)td;
  double* Mat       = Td->m;
  double* Vec_in    = Td->vi;
  double* Vec_out   = Td->vo;
  long long int*       Nrop   = Td->nrop;
  long long int x, y;

  for(x = 0; x < 2* *Nrop; x++){
    Vec_out[x] = 0.;
  }

  for(x = 0; x < *Nrop; x++){
    for(y = 0; y < *Nrop; y++){
      Vec_out[2*y]    +=     Mat[x* *Nrop+y]*Vec_in[2*x];
      Vec_out[2*y+1]  +=     Mat[x* *Nrop+y]*Vec_in[2*x+1];
    }
  }

}

//MAKE TRANSFORMATION MULTI THREADED (small space) DIR=1 is forward !!
void mk_thread_trans(int dir, double* mat1, double* mat2, mycom* vec_in1, mycom* vec_in2,
		     mycom* vec_out1, mycom* vec_out2, int* nrop1, int* nrop2){
  pthread_t thread1, thread2;
  int  iret1, iret2;
  transdata td1, td2;

  td1.m    = mat1;
  td1.vi   = (double* ) vec_in1;
  td1.vo   = (double* ) vec_out1;
  td1.nrop = nrop1;

  td2.m    = mat2;
  td2.vi   = (double* ) vec_in2;
  td2.vo   = (double* ) vec_out2;
  td2.nrop = nrop2;

  if(dir == 1){
    iret1 = pthread_create( &thread1, NULL,  (void* ) &transforeT , (void*) &td1);
    iret2 = pthread_create( &thread2, NULL,  (void* ) &transforeT , (void*) &td2);
  }else{
    iret1 = pthread_create( &thread1, NULL,  (void* ) &transbackT , (void*) &td1);
    iret2 = pthread_create( &thread2, NULL,  (void* ) &transbackT , (void*) &td2);
  }

  pthread_join( thread1, NULL);
  pthread_join( thread2, NULL);

}

//MAKE TRANSFORMATION MULTI THREADED (large space) DIR=1 is forward !!
void lmk_thread_trans(int dir, double* mat1, double* mat2, mycom* vec_in1, mycom* vec_in2,
		      mycom* vec_out1, mycom* vec_out2, long long int* nrop1, long long int* nrop2){
  pthread_t thread1, thread2;
  int  iret1, iret2;
  ltransdata td1, td2;

  td1.m    = mat1;
  td1.vi   = (double* ) vec_in1;
  td1.vo   = (double* ) vec_out1;
  td1.nrop = nrop1;

  td2.m    = mat2;
  td2.vi   = (double* ) vec_in2;
  td2.vo   = (double* ) vec_out2;
  td2.nrop = nrop2;

  if(dir == 1){
    iret1 = pthread_create( &thread1, NULL,  (void* ) &ltransforeT , (void*) &td1);
    iret2 = pthread_create( &thread2, NULL,  (void* ) &ltransforeT , (void*) &td2);
  }else{
    iret1 = pthread_create( &thread1, NULL,  (void* ) &ltransbackT , (void*) &td1);
    iret2 = pthread_create( &thread2, NULL,  (void* ) &ltransbackT , (void*) &td2);
  }

  pthread_join( thread1, NULL);
  pthread_join( thread2, NULL);

}


/* psl.c
////
Provide a library of gsl functions.

Features:
- request calculation via message
- function lookup
- inlets-on-demand

Author: shakfu
Repo: https://github.com/shakfu/pd-psl.git

*/
#include <math.h>
#include <string.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_sf_airy.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_clausen.h>
#include <gsl/gsl_sf_dawson.h>
#include <gsl/gsl_sf_debye.h>

#include "m_pd.h"
#include "tinyexpr.h"

// macros and defines
//  ---------------------------------------------------------------------------

#define MAX_ARGS 6
#define STR_BUF_SIZE 1000

// function lookup infratructure
// ---------------------------------------------------------------------------

unsigned long hash(const char *str) {
    unsigned int h = 0;
    int c;

    while ((c = *str++))
        h += (h << 1) + c;

    return h;
}

enum FUNC {
    ADD = 1273,
    LOG1P = 12931,
    EXPM1 = 12805,
    HYPOT = 13148,
    HYPOT3 = 39495,
    ACOSH = 11978,
    ASINH = 12341,
    ATANH = 12296,
    LDEXP = 12829,
    POW_INT = 122216,
    POW_2 = 13475,
    POW_3 = 13476,
    POW_4 = 13477,
    POW_5 = 13478,
    POW_6 = 13479,
    POW_7 = 13480,
    POW_8 = 13481,
    POW_9 = 13482,
    RANDO = 13254,
    FCMP = 4084,
    AIRY_AI = 109980,
    AIRY_BI = 109983,
    BESSEL_J0 = 987963,
    BESSEL_J1 = 987964,
    BESSEL_JN = 988025,
    BESSEL_Y0 = 988008,
    BESSEL_Y1 = 988009,
    BESSEL_YN = 988070,
    BESSEL_I0 = 987960,
    BESSEL_I1 = 987961,
    BESSEL_IN = 988022,
    CLAUSEN = 110879,
    DAWSON = 36848,
    DEBYE_1 = 109891,
    DEBYE_2 = 109892,
    DEBYE_3 = 109893,
    DEBYE_4 = 109894,
};

// forward declarations / prototypes
// ---------------------------------------------------------------------------

typedef struct _psl t_psl;

void select_default_function(t_psl *x, t_symbol *s);

// psl class objects
// ---------------------------------------------------------------------------

static t_class *psl_class;

static t_class *psl_inlet_class;

// psl class struct (data-space)
// ---------------------------------------------------------------------------

typedef void (*unary_func)(t_psl *, t_floatarg);
typedef void (*binary_func)(t_psl *, t_floatarg, t_floatarg);
typedef void (*tri_func)(t_psl *, t_floatarg, t_floatarg, t_floatarg);

typedef struct _psl_inlet
{
    t_class *x_pd;  // minimal pd object.
    t_psl   *owner; // the owning object to forward inlet messages to.
    int     id;     // the number of this inlet.
} t_psl_inlet;

typedef struct _psl {
    t_object x_obj;

    // assigned function
    t_symbol *func_name;
    int nargs;

    // function slots
    unary_func ufunc;
    binary_func bfunc;
    tri_func tfunc;

    // param_array
    t_float *arg_array;

    // for expression
    char expr_buffer[MAXPDSTRING];

    // inlets
    int inlets;          // # of extra inlets in addition to default
    t_psl_inlet *ins;    // the inlets themselves

    // outlets
    t_outlet *out_f;
} t_psl;

// psl class methods (operation-space)
// ---------------------------------------------------------------------------

// typed-methods

void psl_bang(t_psl *x) {
    if (x->nargs == 1 && x->inlets == 0) {
        x->ufunc(x, x->arg_array[0]);
    }

    if (x->nargs == 2 && x->inlets == 1) {
        x->bfunc(x, x->arg_array[0], x->arg_array[1]);
    }

    if (x->nargs == 3 && x->inlets == 2) {
        x->tfunc(x, x->arg_array[0], x->arg_array[1], x->arg_array[2]);

    }

}

void psl_float(t_psl *x, t_floatarg f) {
    post("psl_float: %f", f);
    if (x->nargs > 0) {
        x->arg_array[0] = f;
        psl_bang(x);
    } else {
        post("nothing to do: no function selected.");
        outlet_float(x->out_f, f);
    }
}

void psl_list(t_psl *x, t_symbol *s, int argc, t_atom *argv) {

    // atom_post("psl_list: ", argc, argv);

    if (s == gensym("list")) {
        post("s: list");

        if (argc == 0) {
            return;
        }

        if (argc == 1) {
            if (argv->a_type == A_FLOAT) {
                float f = atom_getfloat(argv);
                post("got float: %f", f);
                x->ufunc(x, f);
                return;
            }
        }

        if (argc == 2) {
            char buf[STR_BUF_SIZE];
            for (int i = 0; i < argc; i++) {
                atom_string((argv+i), buf, STR_BUF_SIZE);
                post("arg+%i: %s", i, buf);
            }
            if (argv->a_type == A_FLOAT && (argv + 1)->a_type == A_FLOAT) {
                float f1 = atom_getfloat(argv+0);
                float f2 = atom_getfloat(argv+1);
                post("f(%.2f, %.2f)", f1, f2);
                x->bfunc(x, f1, f2);
                return;
            }
        }

        if (argc == 3) {
            char buf[STR_BUF_SIZE];
            for (int i = 0; i < argc; i++) {
                atom_string((argv+i), buf, STR_BUF_SIZE);
                post("arg+%i: %s", i, buf);
            }
            if (argv->a_type == A_FLOAT && (argv+1)->a_type == A_FLOAT && (argv+2)->a_type == A_FLOAT) {
                float f1 = atom_getfloat(argv+0);
                float f2 = atom_getfloat(argv+1);
                float f3 = atom_getfloat(argv+2);
                post("f(%.2f, %.2f, %.2f)", f1, f2, f3);
                x->tfunc(x, f1, f2, f3);
                return;
            }
        }
    }

    post("list body");
    return;

    // error:
    //     pd_error(x, "psl_list error: incorrect arg type");
}

void psl_symbol(t_psl *x, t_symbol *s) {
    post("s: %s", s->s_name);

    // local buffer
    int length = strlen(s->s_name);
    char *buf = (char *)malloc(length * sizeof(char));
    strcpy(buf, s->s_name);
    post("buf: %s", buf);

    // clear expr_buffer
    memset(x->expr_buffer, 0, MAXPDSTRING);

    int j = 0;
    for (int i = 0; i < length; i++) {
        // remove escape `\` required for commas
        if (buf[i] != '\\') {
            x->expr_buffer[j++] = buf[i];
        } else if (x->expr_buffer[j - 1] == ' ') {
            j--;
        }
    }
    x->expr_buffer[length] = '\0';
    free(buf);

    post("x->expr_buffer: %s", x->expr_buffer);

    te_variable vars[] = {
        {"hypot", gsl_hypot, TE_FUNCTION2, NULL} /* TE_FUNCTION2 used because my_sum takes two arguments. */
    };

    te_expr *expr = te_compile(x->expr_buffer, vars, 2, 0);
    const double res = te_eval(expr);
    te_free(expr);
    outlet_float(x->out_f, res);
}

// message-methods

void psl_add(t_psl *x, t_floatarg f1, t_floatarg f2) {
    outlet_float(x->out_f, f1+f2);
}

void psl_rando(t_psl *x, t_floatarg n, t_floatarg seed) {
    post("rando: n:%.2f seed:%.2f", n, seed);
    gsl_rng_env_setup();

    int argc = (int)n;
    int _seed = (int)seed;

    gsl_rng *r = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(r, _seed);

    t_atom av[argc];
    for (int i = 0; i < argc; i++) {
        SETFLOAT(av + i, gsl_rng_uniform(r));
    }
    outlet_list(x->out_f, gensym("list"), argc, av);

    gsl_rng_free(r);
}

void psl_airy_ai(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_airy_Ai(f, GSL_PREC_APPROX));
}

void psl_airy_bi(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_airy_Bi(f, GSL_PREC_APPROX));
}

void psl_log1p(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_log1p(f));
}

void psl_expm1(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_expm1(f));
}

void psl_hypot(t_psl *x, t_floatarg f1, t_floatarg f2) {
    outlet_float(x->out_f, gsl_hypot(f1, f2));
}

void psl_hypot3(t_psl *x, t_floatarg f1, t_floatarg f2, t_floatarg f3) {
    outlet_float(x->out_f, gsl_hypot3(f1, f2, f3));
}

void psl_acosh(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_acosh(f));
}

void psl_asinh(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_asinh(f));
}

void psl_atanh(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_atanh(f));
}

void psl_ldexp(t_psl *x, t_floatarg f1, t_floatarg f2) {
    outlet_float(x->out_f, gsl_ldexp(f1, f2));
}

void psl_pow_int(t_psl *x, t_floatarg f1, t_floatarg f2) {
    outlet_float(x->out_f, gsl_pow_int(f1, f2));
}

void psl_pow_2(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_pow_2(f));
}

void psl_pow_3(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_pow_3(f));
}

void psl_pow_4(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_pow_4(f));
}

void psl_pow_5(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_pow_5(f));
}

void psl_pow_6(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_pow_6(f));
}

void psl_pow_7(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_pow_7(f));
}

void psl_pow_8(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_pow_8(f));
}

void psl_pow_9(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_pow_9(f));
}

void psl_fcmp(t_psl *x, t_floatarg f1, t_floatarg f2, t_floatarg f3) {
    outlet_float(x->out_f, gsl_fcmp(f1, f2, f3));
}

void psl_bessel_j0(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_bessel_J0(f));
}

void psl_bessel_j1(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_bessel_J1(f));
}

void psl_bessel_jn(t_psl *x, t_floatarg f1, t_floatarg f2) {
    outlet_float(x->out_f, gsl_sf_bessel_Jn(f1, f2));
}

void psl_bessel_y0(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_bessel_Y0(f));
}

void psl_bessel_y1(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_bessel_Y1(f));
}

void psl_bessel_yn(t_psl *x, t_floatarg f1, t_floatarg f2) {
    outlet_float(x->out_f, gsl_sf_bessel_Yn(f1, f2));
}

void psl_bessel_i0(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_bessel_I0(f));
}

void psl_bessel_i1(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_bessel_I1(f));
}

void psl_bessel_in(t_psl *x, t_floatarg f1, t_floatarg f2) {
    outlet_float(x->out_f, gsl_sf_bessel_In(f1, f2));
}

void psl_clausen(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_clausen(f));
}

void psl_dawson(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_dawson(f));
}

void psl_debye_1(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_debye_1(f));
}

void psl_debye_2(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_debye_2(f));
}

void psl_debye_3(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_debye_3(f));
}

void psl_debye_4(t_psl *x, t_floatarg f) {
    outlet_float(x->out_f, gsl_sf_debye_4(f));
}

// function selection
//---------------------------------------------------------------------------

// set default function from symbol
void select_default_function(t_psl *x, t_symbol *s) {
    x->func_name = s;
    post("func %s selected", s->s_name);

    switch (hash(s->s_name)) {
        case ADD:
            x->nargs = 2;
            x->bfunc = &psl_add;
            break;
        case LOG1P:
            x->nargs = 1;
            x->ufunc = &psl_log1p;
            break;
        case EXPM1:
            x->nargs = 1;
            x->ufunc = &psl_expm1;
            break;
        case HYPOT:
            x->nargs = 2;
            x->bfunc = &psl_hypot;
            break;
        case HYPOT3:
            x->nargs = 3;
            x->tfunc = &psl_hypot3;
            break;
        case ACOSH:
            x->nargs = 1;
            x->ufunc = &psl_acosh;
            break;
        case ASINH:
            x->nargs = 1;
            x->ufunc = &psl_asinh;
            break;
        case ATANH:
            x->nargs = 1;
            x->ufunc = &psl_atanh;
            break;
        case LDEXP:
            x->nargs = 2;
            x->bfunc = &psl_ldexp;
            break;
        case POW_INT:
            x->nargs = 2;
            x->bfunc = &psl_pow_int;
            break;
        case POW_2:
            x->nargs = 1;
            x->ufunc = &psl_pow_2;
            break;
        case POW_3:
            x->nargs = 1;
            x->ufunc = &psl_pow_3;
            break;
        case POW_4:
            x->nargs = 1;
            x->ufunc = &psl_pow_4;
            break;
        case POW_5:
            x->nargs = 1;
            x->ufunc = &psl_pow_5;
            break;
        case POW_6:
            x->nargs = 1;
            x->ufunc = &psl_pow_6;
            break;
        case POW_7:
            x->nargs = 1;
            x->ufunc = &psl_pow_7;
            break;
        case POW_8:
            x->nargs = 1;
            x->ufunc = &psl_pow_8;
            break;
        case POW_9:
            x->nargs = 1;
            x->ufunc = &psl_pow_9;
            break;
        case RANDO:
            x->nargs = 2;
            x->bfunc = &psl_rando;
            break;
        case FCMP:
            x->nargs = 3;
            x->tfunc = &psl_fcmp;
            break;
        case AIRY_AI:
            x->nargs = 1;
            x->ufunc = &psl_airy_ai;
            break;
        case AIRY_BI:
            x->nargs = 1;
            x->ufunc = &psl_airy_bi;
            break;
        case BESSEL_J0:
            x->nargs = 1;
            x->ufunc = &psl_bessel_j0;
            break;
        case BESSEL_J1:
            x->nargs = 1;
            x->ufunc = &psl_bessel_j1;
            break;
        case BESSEL_JN:
            x->nargs = 2;
            x->bfunc = &psl_bessel_jn;
            break;
        case BESSEL_Y0:
            x->nargs = 1;
            x->ufunc = &psl_bessel_y0;
            break;
        case BESSEL_Y1:
            x->nargs = 1;
            x->ufunc = &psl_bessel_y1;
            break;
        case BESSEL_YN:
            x->nargs = 2;
            x->bfunc = &psl_bessel_yn;
            break;
        case BESSEL_I0:
            x->nargs = 1;
            x->ufunc = &psl_bessel_i0;
            break;
        case BESSEL_I1:
            x->nargs = 1;
            x->ufunc = &psl_bessel_i1;
            break;
        case BESSEL_IN:
            x->nargs = 2;
            x->bfunc = &psl_bessel_in;
            break;
        case CLAUSEN:
            x->nargs = 1;
            x->ufunc = &psl_clausen;
            break;
        case DAWSON:
            x->nargs = 1;
            x->ufunc = &psl_dawson;
            break;
        case DEBYE_1:
            x->nargs = 1;
            x->ufunc = &psl_debye_1;
            break;
        case DEBYE_2:
            x->nargs = 1;
            x->ufunc = &psl_debye_2;
            break;
        case DEBYE_3:
            x->nargs = 1;
            x->ufunc = &psl_debye_3;
            break;
        case DEBYE_4:
            x->nargs = 1;
            x->ufunc = &psl_debye_4;
            break;
        default:
            post("func selection failed, reverting to defaults");
            break;
   }
}

// psl-inlet funcs
// ---------------------------------------------------------------------------

static void psl_inlet_float(t_psl_inlet *x, float f)
{
    x->owner->arg_array[x->id+1] = f;
    // outlet_float(x->owner->out_f, x->id + f);
    post("x->owner->arg_array[x->id]: %.02f", x->owner->arg_array[x->id+1]);
    psl_bang(x->owner);
}

// psl class constructor
// ---------------------------------------------------------------------------

void *psl_new(t_symbol *s) {
    t_psl *x = (t_psl *)pd_new(psl_class);

    // initialize variables
    x->nargs = 0;
    x->inlets = 0;
    x->ufunc = NULL;
    x->bfunc = NULL;
    x->tfunc = NULL;

    select_default_function(x, s);
    // sets x->nargs to correct number

    // create inlets
    x->inlets = x->nargs - 1;
    x->ins = (t_psl_inlet *)getbytes(x->inlets * sizeof(*x->ins));
    x->arg_array = malloc(x->inlets * sizeof(float));

    for (int i=0; i < x->inlets; i++) {
        x->ins[i].x_pd = psl_inlet_class;
        x->ins[i].owner = x;
        x->ins[i].id = i;
        x->arg_array[i] = 0.0;
        inlet_new((t_object *)x, &(x->ins[i].x_pd), 0, 0);
    }

    // initialize outlets
    x->out_f = outlet_new(&x->x_obj, &s_float);

    return (void *)x;
}

// psl class destructor
// ---------------------------------------------------------------------------

// TODO: not sure if this is correct!
void psl_free(t_psl *x) {
    free(x->arg_array);
    freebytes(x->ins, x->inlets * sizeof(*x->ins));
    post("DONE");
}

// psl class setup
// ---------------------------------------------------------------------------

void psl_setup(void) {

    psl_inlet_class = class_new(gensym("psl-inlet"),
                                0, 0,
                                sizeof(t_psl_inlet),
                                CLASS_PD,
                                0);

    if (psl_inlet_class) {
        class_addfloat(psl_inlet_class, (t_method)psl_inlet_float);
    }

    psl_class = class_new(gensym("psl"),
                        (t_newmethod)psl_new,
                        (t_method)psl_free,  // destructor
                        sizeof(t_psl),
                        CLASS_DEFAULT,
                        A_DEFSYMBOL,
                        0);

    // typed methods
    class_addbang(psl_class, psl_bang);
    class_addfloat(psl_class, psl_float);
    class_addlist(psl_class, psl_list);
    class_addsymbol(psl_class, psl_symbol);

    // message methods

    // class-addmethods
    class_addmethod(psl_class, (t_method)psl_add,  gensym("add"), A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_log1p,  gensym("log1p"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_expm1,  gensym("expm1"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_hypot,  gensym("hypot"), A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_hypot3,  gensym("hypot3"), A_DEFFLOAT, A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_acosh,  gensym("acosh"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_asinh,  gensym("asinh"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_atanh,  gensym("atanh"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_ldexp,  gensym("ldexp"), A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_int,  gensym("pow_int"), A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_2,  gensym("pow_2"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_3,  gensym("pow_3"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_4,  gensym("pow_4"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_5,  gensym("pow_5"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_6,  gensym("pow_6"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_7,  gensym("pow_7"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_8,  gensym("pow_8"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_pow_9,  gensym("pow_9"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_rando,  gensym("rando"), A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_fcmp,  gensym("fcmp"), A_DEFFLOAT, A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_airy_ai,  gensym("airy_ai"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_airy_bi,  gensym("airy_bi"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_j0,  gensym("bessel_j0"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_j1,  gensym("bessel_j1"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_jn,  gensym("bessel_jn"), A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_y0,  gensym("bessel_y0"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_y1,  gensym("bessel_y1"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_yn,  gensym("bessel_yn"), A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_i0,  gensym("bessel_i0"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_i1,  gensym("bessel_i1"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_bessel_in,  gensym("bessel_in"), A_DEFFLOAT, A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_clausen,  gensym("clausen"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_dawson,  gensym("dawson"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_debye_1,  gensym("debye_1"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_debye_2,  gensym("debye_2"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_debye_3,  gensym("debye_3"), A_DEFFLOAT, 0);
    class_addmethod(psl_class, (t_method)psl_debye_4,  gensym("debye_4"), A_DEFFLOAT, 0);

    // create alias
    class_addcreator((t_newmethod)psl_new, gensym("gsl"), A_DEFSYMBOL, 0);

    // set name of default help file
    class_sethelpsymbol(psl_class, gensym("help-psl"));
}


/* monte/vegas.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Michael Booth
 *
 * 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: MJB */
/* Modified by: Brian Gough, 12/2000 */

/* This is an implementation of the adaptive Monte-Carlo algorithm
   of G. P. Lepage, originally described in J. Comp. Phys. 27, 192(1978).
   The current version of the algorithm was described in the Cornell
   preprint CLNS-80/447 of March, 1980.

   This code follows most closely the c version by D.R.Yennie, coded
   in 1984.

   The input coordinates are x[j], with upper and lower limits xu[j]
   and xl[j].  The integration length in the j-th direction is
   delx[j].  Each coordinate x[j] is rescaled to a variable y[j] in
   the range 0 to 1.  The range is divided into bins with boundaries
   xi[i][j], where i=0 corresponds to y=0 and i=bins to y=1.  The grid
   is refined (ie, bins are adjusted) using d[i][j] which is some
   variation on the squared sum.  A third parameter used in defining
   the real coordinate using random numbers is called z.  It ranges
   from 0 to bins.  Its integer part gives the lower index of the bin
   into which a call is to be placed, and the remainder gives the
   location inside the bin.

   When stratified sampling is used the bins are grouped into boxes,
   and the algorithm allocates an equal number of function calls to
   each box.

   The variable alpha controls how "stiff" the rebinning algorithm is.
   alpha = 0 means never change the grid.  Alpha is typically set between
   1 and 2.

   */

/* configuration headers */
#include <config.h>

/* standard headers */
#include <math.h>
#include <stdio.h>

/* gsl headers */
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_monte_vegas.h>

/* lib-specific headers */
#define BINS_MAX 50		/* even integer, will be divided by two */

/* A separable grid with coordinates and values */
#define COORD(s,i,j) ((s)->xi[(i)*(s)->dim + (j)])
#define NEW_COORD(s,i) ((s)->xin[(i)])
#define VALUE(s,i,j) ((s)->d[(i)*(s)->dim + (j)])

/* predeclare functions */

typedef int coord;

static void init_grid (gsl_monte_vegas_state * s, double xl[], double xu[],
		size_t dim);
static void reset_grid_values (gsl_monte_vegas_state * s);
static void init_box_coord (gsl_monte_vegas_state * s, coord box[]);
static int change_box_coord (gsl_monte_vegas_state * s, coord box[]);
static void accumulate_distribution (gsl_monte_vegas_state * s, coord bin[],
                                     double y);
static void random_point (double x[], coord bin[], double *bin_vol,
                          const coord box[],
                          const double xl[], const double xu[],
                          gsl_monte_vegas_state * s, gsl_rng * r);
static void resize_grid (gsl_monte_vegas_state * s, unsigned int bins);
static void refine_grid (gsl_monte_vegas_state * s);

static void print_lim (gsl_monte_vegas_state * state,
                       double xl[], double xu[], unsigned long dim);
static void print_head (gsl_monte_vegas_state * state,
                        unsigned long num_dim, unsigned long calls,
                        unsigned int it_num,
                        unsigned int bins, unsigned int boxes);
static void print_res (gsl_monte_vegas_state * state,
                       unsigned int itr, double res, double err,
                       double cum_res, double cum_err,
                       double chi_sq);
static void print_dist (gsl_monte_vegas_state * state, unsigned long dim);
static void print_grid (gsl_monte_vegas_state * state, unsigned long dim);

int
gsl_monte_vegas_integrate (gsl_monte_function * f,
			   double xl[], double xu[],
			   size_t dim, size_t calls,
			   gsl_rng * r,
			   gsl_monte_vegas_state * state,
			   double *result, double *abserr)
{
  double cum_int, cum_sig;
  size_t i, k, it;

  if (dim != state->dim)
    {
      GSL_ERROR ("number of dimensions must match allocated size", GSL_EINVAL);
    }

  for (i = 0; i < dim; i++)
    {
      if (xu[i] <= xl[i])
	{
	  GSL_ERROR ("xu must be greater than xl", GSL_EINVAL);
	}

      if (xu[i] - xl[i] > GSL_DBL_MAX)
	{
	  GSL_ERROR ("Range of integration is too large, please rescale",
		     GSL_EINVAL);
	}
    }

  if (state->stage == 0)
    {
      init_grid (state, xl, xu, dim);

      if (state->verbose >= 0)
	{
	  print_lim (state, xl, xu, dim);
	}
    }

  if (state->stage <= 1)
    {
      state->wtd_int_sum = 0;
      state->sum_wgts = 0;
      state->chi_sum = 0;
      state->it_num = 1;
      state->samples = 0;
    }

  if (state->stage <= 2)
    {
      unsigned int bins = state->bins_max;
      unsigned int boxes = 1;

      if (state->mode != GSL_VEGAS_MODE_IMPORTANCE_ONLY)
	{
	  /* shooting for 2 calls/box */

	  boxes = floor (pow (calls / 2.0, 1.0 / dim));
	  state->mode = GSL_VEGAS_MODE_IMPORTANCE;

	  if (2 * boxes >= state->bins_max)
	    {
	      /* if bins/box < 2 */
	      int box_per_bin = GSL_MAX (boxes / state->bins_max, 1);

	      bins = GSL_MIN(boxes / box_per_bin, state->bins_max);
	      boxes = box_per_bin * bins;

	      state->mode = GSL_VEGAS_MODE_STRATIFIED;
	    }
	}

      {
	double tot_boxes = pow ((double) boxes, (double) dim);
	state->calls_per_box = GSL_MAX (calls / tot_boxes, 2);
	calls = state->calls_per_box * tot_boxes;
      }

      /* total volume of x-space/(avg num of calls/bin) */
      state->jac = state->vol * pow ((double) bins, (double) dim) / calls;

      state->boxes = boxes;

      /* If the number of bins changes from the previous invocation, bins
         are expanded or contracted accordingly, while preserving bin
         density */

      if (bins != state->bins)
	{
	  resize_grid (state, bins);

	  if (state->verbose > 1)
	    {
	      print_grid (state, dim);
	    }
	}

      if (state->verbose >= 0)
	{
	  print_head (state,
		      dim, calls, state->it_num, state->bins, state->boxes);
	}
    }

  state->it_start = state->it_num;

  cum_int = 0.0;
  cum_sig = 0.0;

  state->chisq = 0.0;

  for (it = 0; it < state->iterations; it++)
    {
      double intgrl = 0.0, intgrl_sq = 0.0;
      double sig = 0.0;
      double wgt;
      size_t calls_per_box = state->calls_per_box;
      double jacbin = state->jac;
      double *x = state->x;
      coord *bin = state->bin;

      state->it_num = state->it_start + it;

      reset_grid_values (state);
      init_box_coord (state, state->box);

      do
	{
          double m = 0, q = 0;
	  double f_sq_sum = 0.0;

	  for (k = 0; k < calls_per_box; k++)
	    {
	      double fval, bin_vol;

	      random_point (x, bin, &bin_vol, state->box, xl, xu, state, r);

	      fval = jacbin * bin_vol * GSL_MONTE_FN_EVAL (f, x);

              /* recurrence for mean and variance */

              {
                double d = fval - m;
                m += d / (k + 1.0);
                q += d * d * (k / (k + 1.0));
              }

	      if (state->mode != GSL_VEGAS_MODE_STRATIFIED)
		{
                  double f_sq = fval * fval;
		  accumulate_distribution (state, bin, f_sq);
		}
	    }

	  intgrl += m * calls_per_box;

	  f_sq_sum = q * calls_per_box ;

	  sig += f_sq_sum ;

	  if (state->mode == GSL_VEGAS_MODE_STRATIFIED)
	    {
	      accumulate_distribution (state, bin, f_sq_sum);
	    }
	}
      while (change_box_coord (state, state->box));

      /* Compute final results for this iteration   */

      sig = sig / (calls_per_box - 1.0)  ;

      if (sig > 0)
        {
          wgt = 1.0 / sig;
        }
      else if (state->sum_wgts > 0)
        {
          wgt = state->sum_wgts / state->samples;
        }
      else
        {
          wgt = 0.0;
        }

     intgrl_sq = intgrl * intgrl;

     state->result = intgrl;
     state->sigma  = sqrt(sig);

     if (wgt > 0.0)
       {
         state->samples++ ;
         state->sum_wgts += wgt;
         state->wtd_int_sum += intgrl * wgt;
         state->chi_sum += intgrl_sq * wgt;

         cum_int = state->wtd_int_sum / state->sum_wgts;
         cum_sig = sqrt (1 / state->sum_wgts);

         if (state->samples > 1)
           {
             state->chisq = (state->chi_sum - state->wtd_int_sum * cum_int) /
               (state->samples - 1.0);
           }
       }
     else
       {
         cum_int += (intgrl - cum_int) / (it + 1.0);
         cum_sig = 0.0;
       }

      if (state->verbose >= 0)
	{
	  print_res (state,
		     state->it_num, intgrl, sqrt (sig), cum_int, cum_sig,
		     state->chisq);
	  if (it + 1 == state->iterations && state->verbose > 0)
	    {
	      print_grid (state, dim);
	    }
	}

      if (state->verbose > 1)
	{
          print_dist (state, dim);
        }

      refine_grid (state);

      if (state->verbose > 1)
	{
	  print_grid (state, dim);
	}

    }

  /* By setting stage to 1 further calls will generate independent
     estimates based on the same grid, although it may be rebinned. */

  state->stage = 1;

  *result = cum_int;
  *abserr = cum_sig;

  return GSL_SUCCESS;
}

gsl_monte_vegas_state *
gsl_monte_vegas_alloc (size_t dim)
{
  gsl_monte_vegas_state *s =
    (gsl_monte_vegas_state *) malloc (sizeof (gsl_monte_vegas_state));

  if (s == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for vegas state struct",
		     GSL_ENOMEM, 0);
    }

  s->delx = (double *) malloc (dim * sizeof (double));

  if (s->delx == 0)
    {
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for delx", GSL_ENOMEM, 0);
    }

  s->d = (double *) malloc (BINS_MAX * dim * sizeof (double));

  if (s->d == 0)
    {
      free (s->delx);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for d", GSL_ENOMEM, 0);
    }

  s->xi = (double *) malloc ((BINS_MAX + 1) * dim * sizeof (double));

  if (s->xi == 0)
    {
      free (s->d);
      free (s->delx);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for xi", GSL_ENOMEM, 0);
    }

  s->xin = (double *) malloc ((BINS_MAX + 1) * sizeof (double));

  if (s->xin == 0)
    {
      free (s->xi);
      free (s->d);
      free (s->delx);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for xin", GSL_ENOMEM, 0);
    }

  s->weight = (double *) malloc (BINS_MAX * sizeof (double));

  if (s->weight == 0)
    {
      free (s->xin);
      free (s->xi);
      free (s->d);
      free (s->delx);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for xin", GSL_ENOMEM, 0);
    }

  s->box = (coord *) malloc (dim * sizeof (coord));

  if (s->box == 0)
    {
      free (s->weight);
      free (s->xin);
      free (s->xi);
      free (s->d);
      free (s->delx);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for box", GSL_ENOMEM, 0);
    }

  s->bin = (coord *) malloc (dim * sizeof (coord));

  if (s->bin == 0)
    {
      free (s->box);
      free (s->weight);
      free (s->xin);
      free (s->xi);
      free (s->d);
      free (s->delx);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for bin", GSL_ENOMEM, 0);
    }

  s->x = (double *) malloc (dim * sizeof (double));

  if (s->x == 0)
    {
      free (s->bin);
      free (s->box);
      free (s->weight);
      free (s->xin);
      free (s->xi);
      free (s->d);
      free (s->delx);
      free (s);
      GSL_ERROR_VAL ("failed to allocate space for x", GSL_ENOMEM, 0);
    }

  s->dim = dim;
  s->bins_max = BINS_MAX;

  gsl_monte_vegas_init (s);

  return s;
}

/* Set some default values and whatever */
int
gsl_monte_vegas_init (gsl_monte_vegas_state * state)
{
  state->stage = 0;
  state->alpha = 1.5;
  state->verbose = -1;
  state->iterations = 5;
  state->mode = GSL_VEGAS_MODE_IMPORTANCE;
  state->chisq = 0;
  state->bins = state->bins_max;
  state->ostream = stdout;

  return GSL_SUCCESS;
}

void
gsl_monte_vegas_free (gsl_monte_vegas_state * s)
{
  free (s->x);
  free (s->delx);
  free (s->d);
  free (s->xi);
  free (s->xin);
  free (s->weight);
  free (s->box);
  free (s->bin);
  free (s);
}

static void
init_box_coord (gsl_monte_vegas_state * s, coord box[])
{
  size_t i;

  size_t dim = s->dim;

  for (i = 0; i < dim; i++)
    {
      box[i] = 0;
    }
}

/* change_box_coord steps through the box coord like
   {0,0}, {0, 1}, {0, 2}, {0, 3}, {1, 0}, {1, 1}, {1, 2}, ...
*/
static int
change_box_coord (gsl_monte_vegas_state * s, coord box[])
{
  int j = s->dim - 1;

  int ng = s->boxes;

  while (j >= 0)
    {
      box[j] = (box[j] + 1) % ng;

      if (box[j] != 0)
	{
	  return 1;
	}

      j--;
    }

  return 0;
}

static void
init_grid (gsl_monte_vegas_state * s, double xl[], double xu[], size_t dim)
{
  size_t j;

  double vol = 1.0;

  s->bins = 1;

  for (j = 0; j < dim; j++)
    {
      double dx = xu[j] - xl[j];
      s->delx[j] = dx;
      vol *= dx;

      COORD (s, 0, j) = 0.0;
      COORD (s, 1, j) = 1.0;
    }

  s->vol = vol;
}

static void
reset_grid_values (gsl_monte_vegas_state * s)
{
  size_t i, j;

  size_t dim = s->dim;
  size_t bins = s->bins;

  for (i = 0; i < bins; i++)
    {
      for (j = 0; j < dim; j++)
	{
	  VALUE (s, i, j) = 0.0;
	}
    }
}

static void
accumulate_distribution (gsl_monte_vegas_state * s, coord bin[], double y)
{
  size_t j;
  size_t dim = s->dim;

  for (j = 0; j < dim; j++)
    {
      int i = bin[j];
      VALUE (s, i, j) += y;
    }
}

static void
random_point (double x[], coord bin[], double *bin_vol,
	      const coord box[], const double xl[], const double xu[],
	      gsl_monte_vegas_state * s, gsl_rng * r)
{
  /* Use the random number generator r to return a random position x
     in a given box.  The value of bin gives the bin location of the
     random position (there may be several bins within a given box) */

  double vol = 1.0;

  size_t j;

  size_t dim = s->dim;
  size_t bins = s->bins;
  size_t boxes = s->boxes;

  DISCARD_POINTER(xu); /* prevent warning about unused parameter */

  for (j = 0; j < dim; ++j)
    {
      /* box[j] + ran gives the position in the box units, while z
         is the position in bin units.  */

      double z = ((box[j] + gsl_rng_uniform_pos (r)) / boxes) * bins;

      int k = z;

      double y, bin_width;

      bin[j] = k;

      if (k == 0)
	{
	  bin_width = COORD (s, 1, j);
	  y = z * bin_width;
	}
      else
	{
	  bin_width = COORD (s, k + 1, j) - COORD (s, k, j);
	  y = COORD (s, k, j) + (z - k) * bin_width;
	}

      x[j] = xl[j] + y * s->delx[j];

      vol *= bin_width;
    }

  *bin_vol = vol;
}

static void
resize_grid (gsl_monte_vegas_state * s, unsigned int bins)
{
  size_t j, k;
  size_t dim = s->dim;

  /* weight is ratio of bin sizes */

  double pts_per_bin = (double) s->bins / (double) bins;

  for (j = 0; j < dim; j++)
    {
      double xold;
      double xnew = 0;
      double dw = 0;
      int i = 1;

      for (k = 1; k <= s->bins; k++)
	{
	  dw += 1.0;
	  xold = xnew;
	  xnew = COORD (s, k, j);

	  for (; dw > pts_per_bin; i++)
	    {
	      dw -= pts_per_bin;
	      NEW_COORD (s, i) = xnew - (xnew - xold) * dw;
	    }
	}

      for (k = 1 ; k < bins; k++)
        {
          COORD(s, k, j) = NEW_COORD(s, k);
        }

      COORD (s, bins, j) = 1;
    }

  s->bins = bins;
}

static void
refine_grid (gsl_monte_vegas_state * s)
{
  size_t i, j, k;
  size_t dim = s->dim;
  size_t bins = s->bins;

  for (j = 0; j < dim; j++)
    {
      double grid_tot_j, tot_weight;
      double * weight = s->weight;

      double oldg = VALUE (s, 0, j);
      double newg = VALUE (s, 1, j);

      VALUE (s, 0, j) = (oldg + newg) / 2;
      grid_tot_j = VALUE (s, 0, j);

      /* This implements gs[i][j] = (gs[i-1][j]+gs[i][j]+gs[i+1][j])/3 */

      for (i = 1; i < bins - 1; i++)
	{
	  double rc = oldg + newg;
	  oldg = newg;
	  newg = VALUE (s, i + 1, j);
	  VALUE (s, i, j) = (rc + newg) / 3;
	  grid_tot_j += VALUE (s, i, j);
	}
      VALUE (s, bins - 1, j) = (newg + oldg) / 2;

      grid_tot_j += VALUE (s, bins - 1, j);

      tot_weight = 0;

      for (i = 0; i < bins; i++)
	{
	  weight[i] = 0;

	  if (VALUE (s, i, j) > 0)
	    {
	      oldg = grid_tot_j / VALUE (s, i, j);
	      /* damped change */
	      weight[i] = pow (((oldg - 1) / oldg / log (oldg)), s->alpha);
	    }

	  tot_weight += weight[i];

#ifdef DEBUG
          printf("weight[%d] = %g\n", i, weight[i]);
#endif
	}

      {
	double pts_per_bin = tot_weight / bins;

	double xold;
	double xnew = 0;
	double dw = 0;
	i = 1;

	for (k = 0; k < bins; k++)
	  {
	    dw += weight[k];
	    xold = xnew;
	    xnew = COORD (s, k + 1, j);

	    for (; dw > pts_per_bin; i++)
	      {
		dw -= pts_per_bin;
		NEW_COORD (s, i) = xnew - (xnew - xold) * dw / weight[k];
	      }
	  }

        for (k = 1 ; k < bins ; k++)
          {
            COORD(s, k, j) = NEW_COORD(s, k);
          }

	COORD (s, bins, j) = 1;
      }
    }
}

static void
print_lim (gsl_monte_vegas_state * state,
	   double xl[], double xu[], unsigned long dim)
{
  unsigned long j;

  fprintf (state->ostream, "The limits of integration are:\n");
  for (j = 0; j < dim; ++j)
    fprintf (state->ostream, "\nxl[%lu]=%f    xu[%lu]=%f", j, xl[j], j, xu[j]);
  fprintf (state->ostream, "\n");
  fflush (state->ostream);
}

static void
print_head (gsl_monte_vegas_state * state,
	    unsigned long num_dim, unsigned long calls,
	    unsigned int it_num, unsigned int bins, unsigned int boxes)
{
  fprintf (state->ostream,
	   "\nnum_dim=%lu, calls=%lu, it_num=%d, max_it_num=%d ",
	   num_dim, calls, it_num, state->iterations);
  fprintf (state->ostream,
	   "verb=%d, alph=%.2f,\nmode=%d, bins=%d, boxes=%d\n",
	   state->verbose, state->alpha, state->mode, bins, boxes);
  fprintf (state->ostream,
	   "\n       single.......iteration                   ");
  fprintf (state->ostream, "accumulated......results   \n");

  fprintf (state->ostream,
	   "iteration     integral    sigma             integral   ");
  fprintf (state->ostream, "      sigma     chi-sq/it\n\n");
  fflush (state->ostream);

}

static void
print_res (gsl_monte_vegas_state * state,
	   unsigned int itr,
           double res, double err,
           double cum_res, double cum_err,
	   double chi_sq)
{
  fprintf (state->ostream,
	   "%4d        %6.4e %10.2e          %6.4e      %8.2e  %10.2e\n",
	   itr, res, err, cum_res, cum_err, chi_sq);
  fflush (state->ostream);
}

static void
print_dist (gsl_monte_vegas_state * state, unsigned long dim)
{
  unsigned long i, j;
  int p = state->verbose;
  if (p < 1)
    return;

  for (j = 0; j < dim; ++j)
    {
      fprintf (state->ostream, "\n axis %lu \n", j);
      fprintf (state->ostream, "      x   g\n");
      for (i = 0; i < state->bins; i++)
	{
	  fprintf (state->ostream, "weight [%11.2e , %11.2e] = ",
                   COORD (state, i, j), COORD(state,i+1,j));
	  fprintf (state->ostream, " %11.2e\n", VALUE (state, i, j));

	}
      fprintf (state->ostream, "\n");
    }
  fprintf (state->ostream, "\n");
  fflush (state->ostream);

}

static void
print_grid (gsl_monte_vegas_state * state, unsigned long dim)
{
  unsigned long i, j;
  int p = state->verbose;
  if (p < 1)
    return;

  for (j = 0; j < dim; ++j)
    {
      fprintf (state->ostream, "\n axis %lu \n", j);
      fprintf (state->ostream, "      x   \n");
      for (i = 0; i <= state->bins; i++)
	{
	  fprintf (state->ostream, "%11.2e", COORD (state, i, j));
	  if (i % 5 == 4)
	    fprintf (state->ostream, "\n");
	}
      fprintf (state->ostream, "\n");
    }
  fprintf (state->ostream, "\n");
  fflush (state->ostream);

}


#pragma once
#include <boost/operators.hpp>
#include <cmath>
#include <gsl/gsl>
#include <iostream>
#include <nlohmann/json.hpp>
#include <numeric>

namespace Simulator {
template <class T, class Category> class UnitBase : public boost::multiplicative<T, double> {
  public:
    UnitBase() = default;
    explicit constexpr UnitBase(double val) : _val(val) {}
    explicit constexpr UnitBase(long double val) : _val(gsl::narrow_cast<double>(val)) {}
    explicit constexpr UnitBase(int val) : _val(gsl::narrow<double>(val)) {}
    explicit constexpr UnitBase(std::int64_t val) : _val(gsl::narrow<double>(val)) {}

    explicit constexpr operator double() const noexcept { return _val; }
    explicit constexpr operator int() const noexcept { return gsl::narrow_cast<int>(_val); }
    explicit constexpr operator std::int64_t() const noexcept { return gsl::narrow_cast<std::int64_t>(_val); }
    explicit constexpr operator float() const noexcept { return gsl::narrow_cast<float>(_val); }

    constexpr bool operator<(const T &rhs) const noexcept { return _val < rhs._val; }
    constexpr bool operator<=(const T &rhs) const noexcept { return _val <= rhs._val; }
    constexpr bool operator>(const T &rhs) const noexcept { return _val > rhs._val; }
    constexpr bool operator>=(const T &rhs) const noexcept { return _val >= rhs._val; }
    constexpr bool operator==(const T &rhs) const noexcept { return _val == rhs._val; }
    constexpr bool operator!=(const T &rhs) const noexcept { return !(*this == rhs); }

    constexpr T &operator+=(const T &rhs) noexcept {
        _val += rhs._val;
        return static_cast<T &>(*this);
    }

    template <class U> constexpr T operator+(const UnitBase<U, Category> &rhs) const noexcept {
        auto ret = *this;
        return ret += rhs;
    }

    constexpr T &operator-=(const T &rhs) noexcept {
        _val -= rhs._val;
        return static_cast<T &>(*this);
    }

    template <class U> constexpr T operator-(const UnitBase<U, Category> &rhs) const noexcept {
        auto ret = *this;
        return ret -= rhs;
    }

    constexpr T &operator*=(double rhs) noexcept {
        _val *= rhs;
        return static_cast<T &>(*this);
    }

    constexpr T &operator/=(double rhs) noexcept {
        _val /= rhs;
        return static_cast<T &>(*this);
    }

    constexpr T operator-() const noexcept { return T(-_val); }

    constexpr double operator/(const T &rhs) const noexcept { return _val / rhs._val; }

    friend constexpr T abs(const T &x) noexcept { return T(std::abs(x._val)); }
    friend constexpr T round(const T &x) noexcept { return T(std::round(x._val)); }
    friend constexpr T floor(const T &x) noexcept { return T(std::floor(x._val)); }
    friend constexpr T ceil(const T &x) noexcept { return T(std::ceil(x._val)); }
    friend constexpr T fmod(const T &x, const T &y) { return T(std::fmod(x._val, y._val)); }
    friend std::string to_string(const T &x) { return std::to_string(x._val); }

    friend std::ostream &operator<<(std::ostream &os, const T &rhs) { return os << to_string(rhs); }
    friend std::istream &operator>>(std::istream &is, T &rhs) {
        is >> rhs._val;
        return is;
    }
    [[nodiscard]] constexpr double getValue() const noexcept { return _val; }

  protected:
    double _val = 0;
};

// NOLINTNEXTLINE
#define DEFINE_UNIT(name, cat, s)                                                                                \
    class name : public UnitBase<name, cat> {                                                                          \
      public:                                                                                \
        static constexpr name infinity() { return name(std::numeric_limits<double>::infinity()); }                     \
        using category = cat;                                                                                \
        static constexpr double scale = (s);                                                                           \
        using UnitBase<name, cat>::UnitBase;                                                                           \
        constexpr name(const UnitBase<name, cat> &other) : UnitBase<name, cat>(other) {}                               \
        template <class U>                                                                                \
        constexpr name(const UnitBase<U, cat> &other) : UnitBase<name, cat>(other.getValue() * (U::scale / scale)) {   \
            static_assert(U::scale == U::scale && scale == scale);                                                     \
        }                                                                                \
    };                                                                                \
    inline void from_json(const nlohmann::json &j, name &v) { v = name(j.get<double>()); }                             \
    inline void to_json(nlohmann::json &j, const name &v) { j = v.getValue(); }

struct MasteryCategory {};
DEFINE_UNIT(Mastery, MasteryCategory, 1.0);
struct CriticalRatingCategory {};
DEFINE_UNIT(CriticalRating, CriticalRatingCategory, 1.0);
struct AlacrityRatingCategory {};
DEFINE_UNIT(AlacrityRating, AlacrityRatingCategory, 1.0);
struct PowerCategory {};
DEFINE_UNIT(Power, PowerCategory, 1.0);
struct ForceTechDamageCategory {};
DEFINE_UNIT(FTPower, ForceTechDamageCategory, 1.0);
struct AccuracyRatingCategory {};
DEFINE_UNIT(AccuracyRating, AccuracyRatingCategory, 1.0);
struct TimeCategory {};
DEFINE_UNIT(Second, TimeCategory, 1.0);
DEFINE_UNIT(Minute, TimeCategory, 60.0);
constexpr Second _tinyTime{1e-7};
struct HealthCategory {};
DEFINE_UNIT(HealthPoints, HealthCategory, 1.0);
struct ArmorCatergory {};
DEFINE_UNIT(Armor, ArmorCatergory, 1.0);
struct EnergyCategory {};
DEFINE_UNIT(EnergyCost, EnergyCategory, 1.0);
} // namespace Simulator


#include "defines.h"
#include <string.h>
#include "lib.h"
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <sys/types.h>
#include <sys/stat.h>

extern REAL * opa;
extern int NTERMS;

/**
 * @param float * w
 * @param float * sig
 * @param float * spectro
 * @param int nspectro
 * @param float * spectra
 * @param float * d_spectra
 * @param float * beta
 * @param float * alpha
 *
 * */
int covarm(REAL *w,REAL *sig,float *spectro,int nspectro,REAL *spectra,REAL  *d_spectra,REAL *beta,REAL *alpha){

	int j,i,bt_nf,bt_nc,aux_nf,aux_nc;

	REAL AP[NTERMS*NTERMS*NPARMS],BT[NPARMS*NTERMS];

	REAL *BTaux,*APaux;

	//printf("\nVALORES DEL SIGMA SQUARE\n");

	for(j=0;j<NPARMS;j++){
		for(i=0;i<nspectro;i++){
			opa[i]= w[j]*(spectra[i+nspectro*j]-spectro[i+nspectro*j]);
		}

		BTaux=BT+(j*NTERMS);
		APaux=AP+(j*NTERMS*NTERMS);
		multmatrixIDLValueSigma(opa,nspectro,1,d_spectra+j*nspectro*NTERMS,NTERMS,nspectro,BTaux,&bt_nf,&bt_nc,sig+(nspectro*j)); //bt de tam NTERMS x 1
		multmatrix_transpose_sigma(d_spectra+j*nspectro*NTERMS,NTERMS,nspectro,d_spectra+j*nspectro*NTERMS,NTERMS,nspectro,APaux,&aux_nf,&aux_nc,w[j], sig+(nspectro*j));//ap de tam NTERMS x NTERMS

	}

	totalParcialf(BT,NPARMS,NTERMS,beta); //beta de tam 1 x NTERMS
	totalParcialMatrixf(AP,NTERMS,NTERMS,NPARMS,alpha); //alpha de tam NTERMS x NTERMS

	return 1;
}

/**
 * @param REAL * W
 * @param REAL * sig
 * @param float * spectro
 * @param int nspectro
 * @param REAL * spectra
 * @param REAL * d_spectra
 * @param REAL * beta
 * @param REAL * alpha
 *
 * */
int covarm2(REAL *w,REAL *sig,float *spectro,int nspectro,REAL *spectra,REAL  *d_spectra,REAL *beta,REAL *alpha){

	int j,i,h,k,aux_nf,aux_nc;

	REAL AP[NTERMS*NTERMS*NPARMS],BT[NPARMS*NTERMS];

	REAL *BTaux,*APaux;
	REAL sum,sum2;

	for(j=0;j<NPARMS;j++){
		for(i=0;i<nspectro;i++){
			opa[i]= w[j]*(spectra[i+nspectro*j]-spectro[i+nspectro*j]);
		}

		BTaux=BT+(j*NTERMS);
		APaux=AP+(j*NTERMS*NTERMS);

		for ( i = 0; i < NTERMS; i++){
		    for ( h = 0; h < NTERMS; h++){
				sum=0;
				if(i==0){
					sum2=0;
				}
				for ( k = 0;  k < nspectro; k++){
					REAL dAux = (*(d_spectra+j*nspectro*NTERMS+h*nspectro+k));
					sum += (*(d_spectra+j*nspectro*NTERMS+i*nspectro+k) * dAux ) * (w[j]/sig[nspectro*j+k]);

					if(i==0){
						sum2+= ((opa[k] * dAux  ))/sig[nspectro*j+k];
					}
				}

				APaux[NTERMS*i+h] = sum;
				if(i==0){
					BTaux[h] = sum2;
				}
     		}

		}
	}

	totalParcialf(BT,NPARMS,NTERMS,beta); //beta de tam 1 x NTERMS
	totalParcialMatrixf(AP,NTERMS,NTERMS,NPARMS,alpha); //alpha de tam NTERMS x NTERMS

	return 1;
}

/**
 * @param spectra: array with synthetic spectro
 * @param nspectro: size of spectro
 * @param spectro: original spectro
 * @param w: array of weight for I,Q,U,V
 * @param sig: array with sigma for I,Q,U,V
 * @param nfree: (nspectro * NPARMS) - NTERMS, NPARAMs is 4 and NTERMS 11.
 * */
REAL fchisqr(REAL * spectra,int nspectro,float *spectro,REAL *w,REAL *sig,REAL nfree){

	REAL TOT,dif1,dif2,dif3,dif4;
	REAL opa1,opa2,opa3,opa4;
	int i,j;

	TOT=0;
	opa1=0;
	opa2=0;
	opa3=0;
	opa4=0;
	for(i=0;i<nspectro;i++){
		dif1=spectra[i]-spectro[i];
		dif2=spectra[i+nspectro]-spectro[i+nspectro];
		dif3=spectra[i+nspectro*2]-spectro[i+nspectro*2];
		dif4=spectra[i+nspectro*3]-spectro[i+nspectro*3];

		opa1+= (((dif1*dif1)*w[0])/(sig[i]));
		opa2+= (((dif2*dif2)*w[1])/(sig[i+nspectro]));
		opa3+= (((dif3*dif3)*w[2])/(sig[i+nspectro*2]));
		opa4+= (((dif4*dif4)*w[3])/(sig[i+nspectro*3]));
	}
	TOT+= opa1+opa2+opa3+opa4;

	return TOT/nfree;

}

/*

	Multiplica la matriz a (tamaño naf,nac)
	por la matriz b (de tamaño nbf,nbc)
	al estilo IDL, es decir, filas de a por columnas de b,
	el resultado se almacena en resultOut (de tamaño fil,col)

	El tamaño de salida (fil,col) corresponde con (nbf,nac).

	El tamaño de columnas de b, nbc, debe de ser igual al de filas de a, naf.

*/

/**
 * @param REAL * a
 * @param int naf
 * @param int nac
 * @param REAL * b
 * @param int nbf
 * @param int nbc
 * @param REAL * result
 * @param int * fil
 * @param int * col
 * @param REAL value
 * */
int multmatrixIDLValue(REAL *a,int naf,int nac,REAL *b,int nbf,int nbc,REAL *result,int *fil,int *col,REAL value){

   int i,j,k;
   REAL sum;

	if(naf==nbc){
		(*fil)=nbf;
		(*col)=nac;

		for ( i = 0; i < nbf; i++){
		    for ( j = 0; j < nac; j++){
				sum=0;
				for ( k = 0;  k < naf; k++){
					sum += a[k*nac+j] * b[i*nbc+k];
				}
				result[((nac)*i)+j] = sum/value;
      		}
		}
		return 1;
	}
	else
		printf("\n \n Error en multmatrixIDLValue no coinciden nac y nbf!!!! ..\n\n");
	return 0;
}

/**
 *
 * */
int multmatrixIDLValueSigma(REAL *a,int naf,int nac,REAL *b,int nbf,int nbc,REAL *result,int *fil,int *col, REAL * sigma){

   int i,j,k;
   REAL sum;

	if(naf==nbc){
		(*fil)=nbf;
		(*col)=nac;

		for ( i = 0; i < nbf; i++){
				sum=0;
				for ( k = 0;  k < naf; k++){
						sum += (((a[k] * b[i*nbc+k])))/sigma[k];
				}
				result[i] = sum;
		}
		return 1;
	}
	else
		printf("\n \n Error en multmatrixIDLValue no coinciden nac y nbf!!!! ..\n\n");
	return 0;
}

/**
 * @param REAL * A
 * @param int f
 * @param int c
 * @param REAL * result
 * */
void totalParcialf(REAL * A, int f,int c,REAL * result){

	int i,j;
	REAL sum;
	for(i=0;i<c;i++){
		sum = 0;
		for(j=0;j<f;j++){
			sum+=A[j*c+i];
		}
		result[i] = sum;
	}
}

/**
 * @param REAL * A
 * @param int f
 * @param int c
 * @param int p
 * @param REAL * result
 * */
void totalParcialMatrixf(REAL * A, int f,int c,int p,REAL *result){

	int i,j,k;
	REAL sum;
	for(i=0;i<f;i++)
		for(j=0;j<c;j++){
			sum=0;
			for(k=0;k<p;k++)
				sum+=A[i*c+j+f*c*k];
			result[i*c+j] = sum;
		}
}

/**
 * @param PRECISION * a
 * @param int naf
 * @param int nac
 * @param PRECISION * b
 * @param int nbf
 * @param int nbc
 * @param PRECISION * result
 * @param int * fil
 * @param int * col
 *
 * Multiply matrix "a"(naf,nac) with matrix "b" (nbf,nbc). Algebraic matrix multiplication style, that is,
 * columns of "a" by rows of "b". The result is stored in "result"(fil,col).
 * The size of columns of "a", nac, must be the same of rows of "b", nbf.
 * */
int multmatrix(PRECISION *a,int naf,int nac, PRECISION *b,int nbf,int nbc,PRECISION *result,int *fil,int *col){

    int i,j,k;
    PRECISION sum;

	if(nac==nbf){
		(*fil)=naf;
		(*col)=nbc;

		for ( i = 0; i < naf; i++)
		    for ( j = 0; j < nbc; j++){
				sum=0;
				for ( k = 0;  k < nbf; k++){
					sum += a[i*nac+k] * b[k*nbc+j];
				}
				result[(*col)*i+j] = sum;
      		}
		return 1;
	}
	return 0;

}

/**
 * @param REAL * a
 * @param int naf
 * @param int nac
 * @param REAL *b
 * @param int nbf
 * @param int nbc
 * @param REAL * result
 * @param int * fil
 * @param int * col
 * @param REAL value
 * */
int multmatrix_transpose(REAL *a,int naf,int nac, REAL *b,int nbf,int nbc,REAL *result,int *fil,int *col,REAL value){

    int i,j,k;
    REAL sum;

	if(nac==nbc){
		(*fil)=naf;
		(*col)=nbf;

		for ( i = 0; i < naf; i++){
		    for ( j = 0; j < nbf; j++){
				sum=0;
				for ( k = 0;  k < nbc; k++){
					sum += a[i*nac+k] * b[j*nbc+k];
				}

				result[(*col)*i+j] = (sum)*value;
     		}

		}
		return 1;
	}else{
		printf("\n \n Error en multmatrix_transpose no coinciden nac y nbc!!!! ..\n\n");
	}

	return 0;
}

/**
 * @param REAL * a
 * @param int naf
 * @param int nac
 * @param REAL *b
 * @param int nbf
 * @param int nbc
 * @param REAL * result
 * @param int * fil
 * @param int * col
 * @param REAL weigth
 * @param REAL * sigma
 *
 * */
int multmatrix_transpose_sigma(REAL *a,int naf,int nac, REAL *b,int nbf,int nbc,REAL *result,int *fil,int *col,REAL weigth, REAL * sigma){

    int i,j,k;
    REAL sum;

	if(nac==nbc){
		(*fil)=naf;
		(*col)=nbf;

		for ( i = 0; i < naf; i++){
		    for ( j = 0; j < nbf; j++){
				sum=0;
				for ( k = 0;  k < nbc; k++){
						sum += (a[i*nac+k] * b[j*nbc+k]) * (weigth/sigma[k]);
				}

				result[(*col)*i+j] = sum;
     		}

		}
		return 1;
	}else{
		printf("\n \n Error en multmatrix_transpose no coinciden nac y nbc!!!! ..\n\n");
	}

	return 0;
}
//Media de un vector de longitud numl

/**
 * @param int nspectro
 */
int CalculaNfree(int nspectro)
{
	int nfree;
	nfree = 0;

	nfree = (nspectro * NPARMS) - NTERMS;

	return nfree;
}

/**
 * @param const char * path
 * */
int isDirectory(const char *path) {
   struct stat statbuf;
   if (stat(path, &statbuf) != 0)
       return 0;
   return S_ISDIR(statbuf.st_mode);
}

/**
 * @param tpuntero * cabeza
 * @param char * fileName
 * */
void insert_in_linked_list (tpuntero *cabeza, char * fileName){
    tpuntero nuevo;
    nuevo = malloc(sizeof(tnodo));
    strcpy(nuevo->d_name,fileName);
    nuevo->next = *cabeza;
    *cabeza = nuevo;
}

/**
 * @param tpuntero  cabeza
 * @param char * fileName
 * */
int checkNameInLista(tpuntero cabeza,char * fileName){
	int found = 0;
    while(cabeza != NULL && !found){ //Mientras cabeza no sea NULL
		if(strcmp(cabeza->d_name,fileName)==0)
			found = 1;
		else
        	cabeza = cabeza->next; //Pasamos al siguiente nodo
    }
	return found;
}

/**
 * @param tpuntero * cabeza
 * */

void deleteList(tpuntero *cabeza){
    tpuntero actual; //Puntero auxiliar para eliminar correctamente la lista

    while(*cabeza != NULL){ //Mientras cabeza no sea NULL
        actual = *cabeza; //Actual toma el valor de cabeza
        *cabeza = (*cabeza)->next; //Cabeza avanza 1 posicion en la lista
        free(actual); //Se libera la memoria de la posicion de Actual (el primer nodo), y cabeza queda apuntando al que ahora es el primero
    }
}


#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <galpy_potentials.h>
// Reflex Motion potential - based on Moving Object Potential
// 3 arguments: amp, t0, tf
void constrain_range2(double * d) {
  // Constrains index to be within interpolation range
  if (*d < 0) *d = 0.0;
  if (*d > 1) *d = 1.0;
}
double ReflexMotionRforce(double R,double z, double phi,
				   double t,
				   struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z,obj_R,obj_phi,RF;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range2(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  // Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  obj_R = pow(pow(obj_x,2) + pow(obj_y,2),0.5);
  obj_phi = atan2(obj_y,obj_x);
  // Calculate R force
  RF= calcRforce(obj_R,obj_z,obj_phi,t,potentialArgs->nwrapped,
		 potentialArgs->wrappedPotentialArg);
  return -amp*RF*(cos(phi)*cos(obj_phi)+sin(phi)*sin(obj_phi));
}

double ReflexMotionzforce(double R,double z,double phi,
				      double t,
				      struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z, obj_R, obj_phi;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range2(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  // Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  obj_R = pow(pow(obj_x,2) + pow(obj_y,2),0.5);
  obj_phi = atan2(obj_y,obj_x);

  // Calculate z force
  return -amp * calczforce(obj_R,obj_z,obj_phi,t,potentialArgs->nwrapped,
			   potentialArgs->wrappedPotentialArg);
}

double ReflexMotionphiforce(double R,double z,double phi,
					double t,
					struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z, obj_R, obj_phi, RF;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range2(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  // Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  obj_R = pow(pow(obj_x,2) + pow(obj_y,2),0.5);
  obj_phi = atan2(obj_y,obj_x);

  // Calculate phiforce
  RF= calcRforce(obj_R,obj_z,obj_phi,t,potentialArgs->nwrapped,
		 potentialArgs->wrappedPotentialArg);
  return amp*RF*(sin(phi)*cos(obj_phi)-cos(phi)*sin(obj_phi));
}

// double MovingObjectPotentialPlanarRforce(double R, double phi,
// 				      double t,
// 				      struct potentialArg * potentialArgs){
//  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,Rdist,RF;
//  double * args= potentialArgs->args;
  //Get args
//  amp= *args;
//  t0= *(args+1);
//  tf= *(args+2);
//  d_ind= (t-t0)/(tf-t0);
//  x= R*cos(phi);
//  y= R*sin(phi);
//  constrain_range(&d_ind);
  // Interpolate x, y
//  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
//  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
//			 *(potentialArgs->acc1d+1));
//  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate R force
//  RF= calcPlanarRforce(Rdist, phi, t, potentialArgs->nwrapped,
//		       potentialArgs->wrappedPotentialArg);
//  return -amp*RF*(cos(phi)*(obj_x-x)+sin(phi)*(obj_y-y))/Rdist;
//}

// double MovingObjectPotentialPlanarphiforce(double R, double phi,
//					double t,
//					struct potentialArg * potentialArgs){
//  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,Rdist,RF;
//  double * args= potentialArgs->args;
  // Get args
//  amp= *args;
//  t0= *(args+1);
//  tf= *(args+2);
//  d_ind= (t-t0)/(tf-t0);
//  x= R*cos(phi);
//  y= R*sin(phi);
//  constrain_range(&d_ind);
  // Interpolate x, y
//  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
//  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
//			 *(potentialArgs->acc1d+1));
//  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate phiforce
//  RF= calcPlanarRforce(Rdist, phi, t, potentialArgs->nwrapped,
//		       potentialArgs->wrappedPotentialArg);
//  return -amp*RF*R*(cos(phi)*(obj_y-y)-sin(phi)*(obj_x-x))/Rdist;
//}