/*Python3 includes*/
#include <Python.h>

/*System includes*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <sys/stat.h>
#include <float.h>
#include <sys/time.h>

/*GSL includes*/
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_cblas.h>
#include <pthread.h>

/*User includes*/
#include "vbgmmmodule.h"

char szMLInputFile[BUFSIZ]; // mustlink file path

// Module method definition
static PyObject *get_wrapper_version(PyObject *self, PyObject *args)
{
	(void)self;
	(void)args;
	return Py_BuildValue("s", "0.3");
}

static PyObject *vbgmm_get_n_jobs(PyObject *self, PyObject *args)
{
	(void)self;
	(void)args;
	return PyLong_FromLong(N_RTHREADS);
}

static PyObject *vbgmm_testfit(PyObject *self, PyObject *args)
{
	(void)self;
	const char *szFileStub = "/home/kgravouil/THESE/data/FENNEC/fennec/c-concoct/tests";
	int nKStart = 60, nLMin = 1000, nMaxIter = 30;
	unsigned long lSeed = 666;
	double dEpsilon = 1e-1;
	int bCout = 0;
	int sts;

	sts = driver(szFileStub, nKStart, nLMin, lSeed, nMaxIter, dEpsilon, bCout);
	return Py_BuildValue("i", sts);
}

static PyObject *vbgmm_fit(PyObject *self, PyObject *args)
{
	(void)self;
	const char *szFileStub = NULL;
	int nKStart = 0, nLMin = 0, nMaxIter;
	unsigned long lSeed;
	double dEpsilon;
	int bCout = 0;
	int sts;

	if (!PyArg_ParseTuple(args, "siikidi", &szFileStub, &nKStart, &nLMin, &lSeed, &nMaxIter,
						  &dEpsilon, &bCout))
		return NULL;
	//TODO: return the np.array of clustering result
	sts = driver(szFileStub, nKStart, nLMin, lSeed, nMaxIter, dEpsilon, bCout);
	return Py_BuildValue("i", sts);
}

//Method definition object for this extension, these argumens mean:
//ml_name: The name of the method
//ml_meth: Function pointer to the method implementation
//ml_flags: Flags indicating special features of this method, such as
//          accepting arguments, accepting keyword arguments, being a
//          class method, or being a static method of a class.
//ml_doc:  Contents of this method's docstring
static PyMethodDef vbgmm_methods[] = {
	{"get_wrapper_version",
	 get_wrapper_version,
	 METH_NOARGS,
	 "Return the python wrapper version"},
	{"get_n_jobs",
	 vbgmm_get_n_jobs,
	 METH_NOARGS,
	 "Return number of jobs to run."},
	{"fit",
	 vbgmm_fit,
	 METH_VARARGS,
	 "Fit a variational Bayesian Gaussian mixture."},
	{"test_fit",
	 vbgmm_testfit,
	 METH_VARARGS,
	 "Quick test of fit function."},
	{NULL, NULL, 0, NULL} /* Sentinel */
};

//Module definition
//The arguments of this structure tell Python what to call your extension,
//what its methods are and where to look for its method definitions
static struct PyModuleDef vbgmm_definition = {
	PyModuleDef_HEAD_INIT,
	"vbgmm",
	"A Python module that compute VBGMM",
	-1,
	vbgmm_methods};

//Module initialization
//Python calls this function when importing your extension. It is important
//that this function is named PyInit_[[your_module_name]] exactly, and matches
//the name keyword argument in setup.py's setup() call.
PyMODINIT_FUNC PyInit_vbgmm(void)
{
	Py_Initialize();
	return PyModule_Create(&vbgmm_definition);
}

/*** core functions ***/
int driver(const char *szFileStub, int nKStart, int nLMin, unsigned long lSeed,
		   int nMaxIter, double dEpsilon, int bCOut)
{
	t_Params tParams;
	t_Data tData;
	gsl_rng *ptGSLRNG = NULL;
	const gsl_rng_type *ptGSLRNGType = NULL;
	int nD = 0, nN = 0;
	char szOFile[MAX_LINE_LENGTH];
	t_VBParams tVBParams;
	t_Cluster *ptBestCluster = NULL;
	gsl_matrix *ptTemp = NULL;
	gsl_matrix *ptTVar = NULL;

	sprintf(szMLInputFile, "%s%s%d.dat", szFileStub, MLINPUT_FILE, nLMin);

	/*initialise GSL RNG */
	gsl_rng_env_setup();

	gsl_set_error_handler_off();

	ptGSLRNGType = gsl_rng_default;
	ptGSLRNG = gsl_rng_alloc(ptGSLRNGType);

	/*get command line params */
	tParams.nKStart = nKStart;
	tParams.nLMin = nLMin;
	tParams.nMaxIter = nMaxIter;
	tParams.dEpsilon = dEpsilon;
	tParams.lSeed = lSeed;

	setParams(&tParams, szFileStub);

	/*read in input data */
	readInputData(tParams.szInputFile, &tData);
	readPInputData(tParams.szPInputFile, &tData);

	nD = tData.nD;
	nN = tData.nN;

	ptTemp = gsl_matrix_alloc(tData.nT, nD);
	ptTVar = gsl_matrix_alloc(tData.nT, tData.nT);

	setVBParams(&tVBParams, &tData);

	ptBestCluster = (t_Cluster *)malloc(sizeof(t_Cluster));

	ptBestCluster->nN = nN;
	ptBestCluster->nK = tParams.nKStart;
	ptBestCluster->nD = nD;
	ptBestCluster->ptData = &tData;
	ptBestCluster->ptVBParams = &tVBParams;
	ptBestCluster->lSeed = tParams.lSeed;
	ptBestCluster->nMaxIter = tParams.nMaxIter;
	ptBestCluster->dEpsilon = tParams.dEpsilon;

	if (bCOut > 0)
		ptBestCluster->szCOutFile = (char *)szFileStub;
	else
		ptBestCluster->szCOutFile = NULL;

	runRThreads((void *)&ptBestCluster);

	compressCluster(ptBestCluster);

	calcCovarMatrices(ptBestCluster, &tData);

	// printing clustering results
	printf("[DEBUG] Printing clustering\n");
	sprintf(szOFile, "%sclustering_gt%d.csv", tParams.szOutFileStub,
			tParams.nLMin);
	writeClusters(szOFile, ptBestCluster, &tData);

	/*free up memory in data object */
	printf("[DEBUG] Cleaning memory\n");
	destroyData(&tData);

	/*free up best BIC clusters */
	destroyCluster(ptBestCluster);
	free(ptBestCluster);

	destroyParams(&tParams);
	gsl_rng_free(ptGSLRNG);
	gsl_matrix_free(tVBParams.ptInvW0);
	gsl_matrix_free(ptTemp);
	gsl_matrix_free(ptTVar);

	printf("[INFO] done\n");
	return EXIT_SUCCESS;
}

void setParams(t_Params *ptParams, const char *szFileStub)
{
	ptParams->szInputFile = (char *)malloc(MAX_LINE_LENGTH * sizeof(char));
	if (!ptParams->szInputFile)
		goto memoryError;

	ptParams->szPInputFile = (char *)malloc(MAX_LINE_LENGTH * sizeof(char));
	if (!ptParams->szPInputFile)
		goto memoryError;

	sprintf(ptParams->szInputFile, "%s%s%d.csv", szFileStub, INPUT_FILE,
			ptParams->nLMin);

	sprintf(ptParams->szPInputFile, "%s%s%d.csv", szFileStub, PINPUT_FILE,
			ptParams->nLMin);

	ptParams->szOutFileStub = szFileStub;

	return;

memoryError:
	fprintf(stderr, "Failed allocating memory in setParams\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}

void destroyParams(t_Params *ptParams)
{
	free(ptParams->szInputFile);
	free(ptParams->szPInputFile);
}

void setVBParams(t_VBParams *ptVBParams, t_Data *ptData)
{
	int i = 0, nD = ptData->nD;
	double adVar[nD], adMu[nD];

	ptVBParams->dBeta0 = DEF_BETA0;
	ptVBParams->dNu0 = (double)nD;
	ptVBParams->ptInvW0 = gsl_matrix_alloc(nD, nD);

	calcSampleVar(ptData, adVar, adMu);
	gsl_matrix_set_zero(ptVBParams->ptInvW0);

	for (i = 0; i < nD; i++)
	{
		double dRD = adVar[i] * ((double)nD);

		gsl_matrix_set(ptVBParams->ptInvW0, i, i, dRD);
	}

	ptVBParams->dLogWishartB =
		dLogWishartB(ptVBParams->ptInvW0, nD, ptVBParams->dNu0, TRUE);
}

void readInputData(const char *szFile, t_Data *ptData)
{
	double **aadX = NULL;
	int i = 0, j = 0, nD = 0, nN = 0;
	char szLine[MAX_LINE_LENGTH];
	FILE *ifp = NULL;

	ifp = fopen(szFile, "r");

	if (ifp)
	{
		char *szTok = NULL;
		char *pcError = NULL;

		if (fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
			goto formatError;

		szTok = strtok(szLine, DELIM);
		/*count dimensions */
		while (strtok(NULL, DELIM) != NULL)
		{
			nD++;
		}
		/*count data points */
		while (fgets(szLine, MAX_LINE_LENGTH, ifp) != NULL)
		{
			nN++;
		}
		fclose(ifp);

		/*reopen input file */
		ifp = fopen(szFile, "r");

		if (fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
			goto formatError;

		/*allocate memory for dimension names */
		ptData->aszDimNames = (char **)malloc(nD * sizeof(char *));
		if (!ptData->aszDimNames)
			goto memoryError;

		szTok = strtok(szLine, DELIM);
		/*read in dim names */
		for (i = 0; i < nD; i++)
		{
			szTok = strtok(NULL, DELIM);
			ptData->aszDimNames[i] = strdup(szTok);
		}

		/*allocate memory for data matrix */
		aadX = (double **)malloc(nN * sizeof(double *));
		if (!aadX)
			goto memoryError;
		for (i = 0; i < nN; i++)
		{
			aadX[i] = (double *)malloc(nD * sizeof(double));
			if (!aadX[i])
				goto memoryError;
		}

		/*read in input data */
		ptData->aszSampleNames = (char **)malloc(nN * sizeof(char *));
		if (!ptData->aszSampleNames)
			goto memoryError;

		for (i = 0; i < nN; i++)
		{

			if (fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
				goto formatError;

			szTok = strtok(szLine, DELIM);
			ptData->aszSampleNames[i] = strdup(szTok);
			for (j = 0; j < nD; j++)
			{
				szTok = strtok(NULL, DELIM);

				aadX[i][j] = strtod(szTok, &pcError);

				if (*pcError != '\0')
				{
					goto formatError;
				}
			}
		}
		fclose(ifp);
	}
	else
	{
		fprintf(stderr,
				"Failed to open abundance data file %s aborting\n",
				szFile);
		fflush(stderr);
		exit(EXIT_FAILURE);
	}

	ptData->nD = nD;
	ptData->nN = nN;
	ptData->aadX = aadX;
	return;

memoryError:
	fprintf(stderr, "Failed allocating memory in readInputData\n");
	fflush(stderr);
	exit(EXIT_FAILURE);

formatError:
	fprintf(stderr, "Incorrectly formatted abundance data file\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}

void readPInputData(const char *szFile, t_Data *ptData)
{
	int i = 0, j = 0, nD = ptData->nD, nT = 0;
	char szLine[MAX_LINE_LENGTH];
	FILE *ifp = NULL;

	ifp = fopen(szFile, "r");

	if (ifp)
	{
		char *szTok = NULL;
		char *pcError = NULL;
		nT = 1;

		if (fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
			goto memoryError;

		szTok = strtok(szLine, DELIM);
		/*count dimensions */
		while (strtok(NULL, DELIM) != NULL)
		{
			nT++;
		}

		ptData->ptTMatrix = gsl_matrix_alloc(nT, nD);

		fclose(ifp);

		/*reopen input file */
		ifp = fopen(szFile, "r");

		for (i = 0; i < nD; i++)
		{
			double dTemp = 0.0;

			if (fgets(szLine, MAX_LINE_LENGTH, ifp) == NULL)
				goto formatError;

			szTok = strtok(szLine, DELIM);

			dTemp = strtod(szTok, &pcError);
			if (*pcError != '\0')
			{
				goto formatError;
			}

			gsl_matrix_set(ptData->ptTMatrix, 0, i, dTemp);

			for (j = 1; j < nT; j++)
			{
				szTok = strtok(NULL, DELIM);

				dTemp = strtod(szTok, &pcError);
				if (*pcError != '\0')
				{
					goto formatError;
				}

				gsl_matrix_set(ptData->ptTMatrix, j, i, dTemp);
			}
		}
		fclose(ifp);
	}
	else
	{
		fprintf(stderr,
				"Failed to open abundance data file %s aborting\n",
				szFile);
		fflush(stderr);
		exit(EXIT_FAILURE);
	}

	ptData->nT = nT;
	return;

formatError:
	fprintf(stderr, "Incorrectly formatted abundance data file\n");
	fflush(stderr);
	exit(EXIT_FAILURE);

memoryError:
	fprintf(stderr, "Failed allocating memory in readPInputFile\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}

void destroyData(t_Data *ptData)
{
	int nN = ptData->nN, nD = ptData->nD;
	int i = 0;

	gsl_matrix_free(ptData->ptTMatrix);

	for (i = 0; i < nD; i++)
	{
		free(ptData->aszDimNames[i]);
	}
	free(ptData->aszDimNames);

	for (i = 0; i < nN; i++)
	{
		free(ptData->aadX[i]);
	}
	free(ptData->aadX);

	for (i = 0; i < nN; i++)
	{
		free(ptData->aszSampleNames[i]);
	}
	free(ptData->aszSampleNames);

	return;
}

void destroyCluster(t_Cluster *ptCluster)
{
	int i = 0, nN = ptCluster->nN, nKSize = ptCluster->nKSize;

	if (ptCluster->szCOutFile != NULL)
	{
		free(ptCluster->szCOutFile);
	}

	free(ptCluster->anMaxZ);

	free(ptCluster->anW);

	for (i = 0; i < nN; i++)
	{
		free(ptCluster->aadZ[i]);
	}
	free(ptCluster->aadZ);

	free(ptCluster->adLDet);
	free(ptCluster->adPi);
	free(ptCluster->adBeta);
	free(ptCluster->adNu);

	for (i = 0; i < nKSize; i++)
	{
		free(ptCluster->aadMu[i]);
		free(ptCluster->aadM[i]);
	}

	free(ptCluster->aadMu);
	free(ptCluster->aadM);

	for (i = 0; i < nKSize; i++)
	{
		gsl_matrix_free(ptCluster->aptSigma[i]);
		gsl_matrix_free(ptCluster->aptCovar[i]);
	}
	free(ptCluster->aptSigma);
	free(ptCluster->aptCovar);
	return;
}

void allocateCluster(t_Cluster *ptCluster, int nN, int nK, int nD,
					 t_Data *ptData, long lSeed, int nMaxIter, double dEpsilon,
					 char *szCOutFile)
{
	int i = 0, j = 0, k = 0;

	ptCluster->szCOutFile = szCOutFile;
	ptCluster->ptVBParams = NULL;
	ptCluster->lSeed = lSeed;
	ptCluster->nMaxIter = nMaxIter;
	ptCluster->dEpsilon = dEpsilon;
	ptCluster->ptData = ptData;

	ptCluster->nN = nN;
	ptCluster->nK = nK;
	ptCluster->nKSize = nK;
	ptCluster->nD = nD;

	ptCluster->dVBL = 0.0;

	ptCluster->anMaxZ = (int *)malloc(nN * sizeof(int)); /*destroyed */
	if (!ptCluster->anMaxZ)
		goto memoryError;

	ptCluster->anW = (int *)malloc(nK * sizeof(int)); /*destroyed */
	if (!ptCluster->anW)
		goto memoryError;

	for (i = 0; i < nN; i++)
	{
		ptCluster->anMaxZ[i] = NOT_SET;
	}

	for (i = 0; i < nK; i++)
	{
		ptCluster->anW[i] = 0;
	}

	ptCluster->aadZ = (double **)malloc(nN * sizeof(double *)); /*destroyed */
	if (!ptCluster->aadZ)
		goto memoryError;

	for (i = 0; i < nN; i++)
	{
		ptCluster->aadZ[i] = (double *)malloc(nK * sizeof(double)); /*destroyed */
		if (!ptCluster->aadZ[i])
			goto memoryError;

		for (j = 0; j < nK; j++)
		{
			ptCluster->aadZ[i][j] = 0.0;
		}
	}

	ptCluster->adLDet = (double *)malloc(nK * sizeof(double)); /*all */
	ptCluster->adPi = (double *)malloc(nK * sizeof(double));
	ptCluster->adBeta = (double *)malloc(nK * sizeof(double));
	ptCluster->adNu = (double *)malloc(nK * sizeof(double)); /*destroyed */

	if (!ptCluster->adLDet || !ptCluster->adPi)
		goto memoryError;

	if (!ptCluster->adBeta || !ptCluster->adNu)
		goto memoryError;

	for (k = 0; k < nK; k++)
	{
		ptCluster->adLDet[k] = 0.0;
		ptCluster->adPi[k] = 0.0;
		ptCluster->adBeta[k] = 0.0;
		ptCluster->adNu[k] = 0.0;
	}

	ptCluster->aadMu = (double **)malloc(nK * sizeof(double *));
	if (!ptCluster->aadMu)
		goto memoryError;

	ptCluster->aadM = (double **)malloc(nK * sizeof(double *));
	if (!ptCluster->aadM)
		goto memoryError;

	for (i = 0; i < nK; i++)
	{
		ptCluster->aadM[i] = (double *)malloc(nD * sizeof(double));
		if (!ptCluster->aadM[i])
			goto memoryError;

		ptCluster->aadMu[i] = (double *)malloc(nD * sizeof(double));
		if (!ptCluster->aadMu[i])
			goto memoryError;
	}

	ptCluster->aptSigma = (gsl_matrix **)malloc(nK * sizeof(gsl_matrix *));
	if (!ptCluster->aptSigma)
		goto memoryError;

	for (i = 0; i < nK; i++)
	{
		ptCluster->aptSigma[i] =
			(gsl_matrix *)gsl_matrix_alloc(nD, nD);
	}

	ptCluster->aptCovar = (gsl_matrix **)malloc(nK * sizeof(gsl_matrix *));
	if (!ptCluster->aptCovar)
		goto memoryError;

	for (i = 0; i < nK; i++)
	{
		ptCluster->aptCovar[i] =
			(gsl_matrix *)gsl_matrix_alloc(nD, nD);
	}

	return;

memoryError:
	fprintf(stderr, "Failed allocating memory in allocateCluster\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}

void writeSquareMatrix(char *szFile, gsl_matrix *ptMatrix, int nD)
{
	int i = 0, j = 0;
	FILE *ofp = fopen(szFile, "w");

	if (ofp)
	{
		for (i = 0; i < nD; i++)
		{
			for (j = 0; j < nD - 1; j++)
			{
				fprintf(ofp, "%f,",
						gsl_matrix_get(ptMatrix, i, j));
			}
			fprintf(ofp, "%f\n", gsl_matrix_get(ptMatrix, i, j));
		}
	}
	else
	{
		fprintf(stderr,
				"Failed to open %s for writing in writeSquareMatrix\n",
				szFile);
		fflush(stderr);
	}
}

double decomposeMatrix(gsl_matrix *ptSigmaMatrix, int nD)
{
	double dDet = 0.0;
	int status;
	int l = 0;

	status = gsl_linalg_cholesky_decomp(ptSigmaMatrix);

	if (status == GSL_EDOM)
	{
		fprintf(stderr,
				"Failed Cholesky decomposition in decomposeMatrix\n");
		fflush(stderr);
		exit(EXIT_FAILURE);
	}
	else
	{
		for (l = 0; l < nD; l++)
		{
			double dT = gsl_matrix_get(ptSigmaMatrix, l, l);
			dDet += 2.0 * log(dT);
		}
		gsl_linalg_cholesky_invert(ptSigmaMatrix);
		return dDet;
	}
}

void performMStep(t_Cluster *ptCluster, t_Data *ptData)
{
	int i = 0, j = 0, k = 0, l = 0, m = 0;
	int nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
	double **aadZ = ptCluster->aadZ, **aadX = ptData->aadX;
	double *adLDet = ptCluster->adLDet, *adPi = ptCluster->adPi;
	double **aadCovar = NULL, **aadInvWK = NULL;
	t_VBParams *ptVBParams = ptCluster->ptVBParams;

	// gsl_matrix *ptSigmaMatrix = gsl_matrix_alloc(nD, nD);

	aadCovar = (double **)malloc(nD * sizeof(double *));
	if (!aadCovar)
		goto memoryError;

	for (i = 0; i < nD; i++)
	{
		aadCovar[i] = (double *)malloc(nD * sizeof(double));
		if (!aadCovar[i])
			goto memoryError;
	}

	aadInvWK = (double **)malloc(nD * sizeof(double *));
	if (!aadInvWK)
		goto memoryError;

	for (i = 0; i < nD; i++)
	{
		aadInvWK[i] = (double *)malloc(nD * sizeof(double));
		if (!aadInvWK[i])
			goto memoryError;
	}

	/*perform M step */
	for (k = 0; k < nK; k++)
	{ /*loop components */
		double *adMu = ptCluster->aadMu[k];
		gsl_matrix *ptSigmaMatrix = ptCluster->aptSigma[k];
		double dF = 0.0;
		/*recompute mixture weights and means */
		for (j = 0; j < nD; j++)
		{
			adMu[j] = 0.0;
			for (l = 0; l < nD; l++)
			{
				aadCovar[j][l] = 0.0;
			}
		}

		/* compute weight associated with component k */
		adPi[k] = 0.0;
		for (i = 0; i < nN; i++)
		{
			if (aadZ[i][k] > MIN_Z)
			{
				adPi[k] += aadZ[i][k];
				for (j = 0; j < nD; j++)
				{
					adMu[j] += aadZ[i][k] * aadX[i][j];
				}
			}
		}

		/*normalise means */
		if (adPi[k] > MIN_PI)
		{
			/*Equation 10.60 */
			ptCluster->adBeta[k] = ptVBParams->dBeta0 + adPi[k];

			for (j = 0; j < nD; j++)
			{
				/*Equation 10.61 */
				ptCluster->aadM[k][j] =
					adMu[j] / ptCluster->adBeta[k];
				adMu[j] /= adPi[k];
			}

			ptCluster->adNu[k] = ptVBParams->dNu0 + adPi[k];

			/*calculate covariance matrices */
			for (i = 0; i < nN; i++)
			{
				if (aadZ[i][k] > MIN_Z)
				{
					double adDiff[nD];

					for (j = 0; j < nD; j++)
					{
						adDiff[j] =
							aadX[i][j] - adMu[j];
					}

					for (l = 0; l < nD; l++)
					{
						for (m = 0; m <= l; m++)
						{
							aadCovar[l][m] +=
								aadZ[i][k] *
								adDiff[l] *
								adDiff[m];
						}
					}
				}
			}

			for (l = 0; l < nD; l++)
			{
				for (m = l + 1; m < nD; m++)
				{
					aadCovar[l][m] = aadCovar[m][l];
				}
			}

			/*save sample covariances for later use */
			for (l = 0; l < nD; l++)
			{
				for (m = 0; m < nD; m++)
				{
					double dC = aadCovar[l][m] / adPi[k];
					gsl_matrix_set(ptCluster->aptCovar[k],
								   l, m, dC);
				}
			}

			/*Now perform equation 10.62 */
			dF = (ptVBParams->dBeta0 * adPi[k]) /
				 ptCluster->adBeta[k];
			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					aadInvWK[l][m] =
						gsl_matrix_get(ptVBParams->ptInvW0,
									   l,
									   m) +
						aadCovar[l][m] +
						dF * adMu[l] * adMu[m];
				}
			}

			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					aadCovar[l][m] /= adPi[k];
					gsl_matrix_set(ptSigmaMatrix, l, m,
								   aadInvWK[l][m]);
					gsl_matrix_set(ptSigmaMatrix, m, l,
								   aadInvWK[l][m]);
				}
			}

			/*Implement Equation 10.65 */
			adLDet[k] = ((double)nD) * log(2.0);

			for (l = 0; l < nD; l++)
			{
				double dX =
					0.5 * (ptCluster->adNu[k] - (double)l);
				adLDet[k] += gsl_sf_psi(dX);
			}

			adLDet[k] -= decomposeMatrix(ptSigmaMatrix, nD);
		}
		else
		{
			/*Equation 10.60 */
			adPi[k] = 0.0;

			ptCluster->adBeta[k] = ptVBParams->dBeta0;

			for (j = 0; j < nD; j++)
			{
				/*Equation 10.61 */
				ptCluster->aadM[k][j] = 0.0;
				adMu[j] = 0.0;
			}

			ptCluster->adNu[k] = ptVBParams->dNu0;

			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					aadInvWK[l][m] =
						gsl_matrix_get(ptVBParams->ptInvW0,
									   l, m);
				}
			}

			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					aadInvWK[l][m] =
						gsl_matrix_get(ptVBParams->ptInvW0,
									   l, m);
				}
			}

			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					gsl_matrix_set(ptSigmaMatrix, l, m,
								   aadInvWK[l][m]);
					gsl_matrix_set(ptSigmaMatrix, m, l,
								   aadInvWK[l][m]);
				}
			}

			/*Implement Equation 10.65 */
			adLDet[k] = ((double)nD) * log(2.0);

			for (l = 0; l < nD; l++)
			{
				double dX =
					0.5 * (ptCluster->adNu[k] - (double)l);
				adLDet[k] += gsl_sf_psi(dX);
			}

			adLDet[k] -= decomposeMatrix(ptSigmaMatrix, nD);
		}
	}

	/*Normalise pi */
	if (1)
	{
		double dNP = 0.0;

		for (k = 0; k < nK; k++)
		{
			dNP += adPi[k];
		}

		for (k = 0; k < nK; k++)
		{
			adPi[k] /= dNP;
		}
	}

	/*free up memory */
	for (i = 0; i < nD; i++)
	{
		free(aadCovar[i]);
		free(aadInvWK[i]);
	}

	free(aadCovar);
	free(aadInvWK);

	return;

memoryError:
	fprintf(stderr, "Failed allocating memory in performMStep\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}

void calcCovarMatrices(t_Cluster *ptCluster, t_Data *ptData)
{
	int i = 0, j = 0, k = 0, l = 0, m = 0;
	int nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
	double **aadZ = ptCluster->aadZ, **aadX = ptData->aadX;
	double *adPi = ptCluster->adPi, **aadCovar = NULL;
	double dN = (double)nN;

	aadCovar = (double **)malloc(nD * sizeof(double *));
	if (!aadCovar)
		goto memoryError;

	for (i = 0; i < nD; i++)
	{
		aadCovar[i] = (double *)malloc(nD * sizeof(double));
		if (!aadCovar[i])
			goto memoryError;
	}

	for (k = 0; k < nK; k++)
	{ /*loop components */
		double *adMu = ptCluster->aadMu[k];
		gsl_matrix *ptSigmaMatrix = ptCluster->aptSigma[k];
		/*recompute mixture weights and means */
		for (j = 0; j < nD; j++)
		{
			adMu[j] = 0.0;
			for (l = 0; l < nD; l++)
			{
				aadCovar[j][l] = 0.0;
			}
			/*prevents singularities */
			aadCovar[j][j] = MIN_COVAR;
		}

		/* compute weight associated with component k */
		adPi[k] = 0.0;
		for (i = 0; i < nN; i++)
		{
			adPi[k] += aadZ[i][k];
			for (j = 0; j < nD; j++)
			{
				adMu[j] += aadZ[i][k] * aadX[i][j];
			}
		}
		/*normalise means */
		for (j = 0; j < nD; j++)
		{
			adMu[j] /= adPi[k];
		}

		/*calculate covariance matrices */
		for (i = 0; i < nN; i++)
		{
			double adDiff[nD];

			for (j = 0; j < nD; j++)
			{
				adDiff[j] = aadX[i][j] - adMu[j];
			}

			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					aadCovar[l][m] +=
						aadZ[i][k] * adDiff[l] * adDiff[m];
				}
			}
		}

		for (l = 0; l < nD; l++)
		{
			for (m = l + 1; m < nD; m++)
			{
				aadCovar[l][m] = aadCovar[m][l];
			}
		}

		for (l = 0; l < nD; l++)
		{
			for (m = 0; m < nD; m++)
			{
				aadCovar[l][m] /= adPi[k];
				gsl_matrix_set(ptSigmaMatrix, l, m,
							   aadCovar[l][m]);
			}
		}

		adPi[k] /= dN; /*normalise weights */
	}
	/*free up memory */
	for (i = 0; i < nD; i++)
	{
		free(aadCovar[i]);
	}

	//gsl_matrix_free(ptSigmaMatrix);
	free(aadCovar);

	return;
memoryError:
	fprintf(stderr, "Failed allocating memory in performMStep\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}

void calcCovarMatricesVB(t_Cluster *ptCluster, t_Data *ptData)
{
	int i = 0, j = 0, k = 0, l = 0, m = 0;
	int nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
	double **aadZ = ptCluster->aadZ, **aadX = ptData->aadX;
	double *adLDet = ptCluster->adLDet, *adPi = ptCluster->adPi;
	double **aadCovar = NULL, **aadInvWK = NULL;
	t_VBParams *ptVBParams = ptCluster->ptVBParams;

	aadCovar = (double **)malloc(nD * sizeof(double *));
	if (!aadCovar)
		goto memoryError;

	for (i = 0; i < nD; i++)
	{
		aadCovar[i] = (double *)malloc(nD * sizeof(double));
		if (!aadCovar[i])
			goto memoryError;
	}

	aadInvWK = (double **)malloc(nD * sizeof(double *));
	if (!aadInvWK)
		goto memoryError;

	for (i = 0; i < nD; i++)
	{
		aadInvWK[i] = (double *)malloc(nD * sizeof(double));
		if (!aadInvWK[i])
			goto memoryError;
	}

	/*perform M step */
	for (k = 0; k < nK; k++)
	{ /*loop components */
		double *adMu = ptCluster->aadMu[k];
		gsl_matrix *ptSigmaMatrix = ptCluster->aptSigma[k];
		double dF = 0.0;

		/*recompute mixture weights and means */
		for (j = 0; j < nD; j++)
		{
			adMu[j] = 0.0;
			for (l = 0; l < nD; l++)
			{
				aadCovar[j][l] = 0.0;
			}
		}

		/* compute weight associated with component k */
		adPi[k] = 0.0;
		for (i = 0; i < nN; i++)
		{
			adPi[k] += aadZ[i][k];
			for (j = 0; j < nD; j++)
			{
				adMu[j] += aadZ[i][k] * aadX[i][j];
			}
		}
		/*normalise means */
		if (adPi[k] > MIN_PI)
		{

			/*Equation 10.60 */
			ptCluster->adBeta[k] = ptVBParams->dBeta0 + adPi[k];

			for (j = 0; j < nD; j++)
			{
				/*Equation 10.61 */
				ptCluster->aadM[k][j] =
					adMu[j] / ptCluster->adBeta[k];
				adMu[j] /= adPi[k];
			}

			ptCluster->adNu[k] = ptVBParams->dNu0 + adPi[k];

			/*calculate covariance matrices */
			for (i = 0; i < nN; i++)
			{
				double adDiff[nD];

				for (j = 0; j < nD; j++)
				{
					adDiff[j] = aadX[i][j] - adMu[j];
				}

				for (l = 0; l < nD; l++)
				{
					for (m = 0; m <= l; m++)
					{
						aadCovar[l][m] +=
							aadZ[i][k] * adDiff[l] *
							adDiff[m];
					}
				}
			}

			for (l = 0; l < nD; l++)
			{
				for (m = l + 1; m < nD; m++)
				{
					aadCovar[l][m] = aadCovar[m][l];
				}
			}

			/*save sample covariances for later use */
			for (l = 0; l < nD; l++)
			{
				for (m = 0; m < nD; m++)
				{
					double dC = aadCovar[l][m] / adPi[k];
					gsl_matrix_set(ptCluster->aptCovar[k],
								   l, m, dC);
				}
			}

			/*Now perform equation 10.62 */
			dF = (ptVBParams->dBeta0 * adPi[k]) /
				 ptCluster->adBeta[k];
			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					aadInvWK[l][m] =
						gsl_matrix_get(ptVBParams->ptInvW0,
									   l,
									   m) +
						aadCovar[l][m] +
						dF * adMu[l] * adMu[m];
				}
			}

			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					//aadCovar[l][m] /= adPi[k];
					gsl_matrix_set(ptSigmaMatrix, l, m,
								   aadInvWK[l][m]);
					gsl_matrix_set(ptSigmaMatrix, m, l,
								   aadInvWK[l][m]);
				}
			}
		}
		else
		{
			/*Equation 10.60 */
			adPi[k] = 0.0;

			ptCluster->adBeta[k] = ptVBParams->dBeta0;

			for (j = 0; j < nD; j++)
			{
				/*Equation 10.61 */
				ptCluster->aadM[k][j] = 0.0;
				adMu[j] = 0.0;
			}

			ptCluster->adNu[k] = ptVBParams->dNu0;

			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					aadInvWK[l][m] =
						gsl_matrix_get(ptVBParams->ptInvW0,
									   l, m);
				}
			}

			for (l = 0; l < nD; l++)
			{
				for (m = 0; m <= l; m++)
				{
					//aadCovar[l][m] /= adPi[k];
					gsl_matrix_set(ptSigmaMatrix, l, m,
								   aadInvWK[l][m]);
					gsl_matrix_set(ptSigmaMatrix, m, l,
								   aadInvWK[l][m]);
				}
			}

			/*Implement Equation 10.65 */
			adLDet[k] = ((double)nD) * log(2.0);

			for (l = 0; l < nD; l++)
			{
				double dX =
					0.5 * (ptCluster->adNu[k] - (double)l);
				adLDet[k] += gsl_sf_psi(dX);
			}

			adLDet[k] -= decomposeMatrix(ptSigmaMatrix, nD);
		}
	}

	/*Normalise pi */
	if (1)
	{
		double dNP = 0.0;

		for (k = 0; k < nK; k++)
		{
			dNP += adPi[k];
		}

		for (k = 0; k < nK; k++)
		{
			adPi[k] /= dNP;
		}
	}

	/*free up memory */
	for (i = 0; i < nD; i++)
	{
		free(aadCovar[i]);
		free(aadInvWK[i]);
	}

	//gsl_matrix_free(ptSigmaMatrix);
	free(aadCovar);
	free(aadInvWK);
	return;
memoryError:
	fprintf(stderr, "Failed allocating memory in performMStep\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}

void updateMeans(t_Cluster *ptCluster, t_Data *ptData)
{
	int i = 0, j = 0, k = 0;
	int nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
	int *anMaxZ = ptCluster->anMaxZ;
	int *anW = ptCluster->anW;
	double **aadX = ptData->aadX, **aadMu = ptCluster->aadMu;

	for (k = 0; k < nK; k++)
	{

		for (j = 0; j < nD; j++)
		{
			aadMu[k][j] = 0.0;
		}
	}

	for (i = 0; i < nN; i++)
	{
		int nZ = anMaxZ[i];

		for (j = 0; j < nD; j++)
		{
			aadMu[nZ][j] += aadX[i][j];
		}
	}

	for (k = 0; k < nK; k++)
	{ /*loop components */

		/*normalise means */
		if (anW[k] > 0)
		{
			for (j = 0; j < nD; j++)
			{
				aadMu[k][j] /= (double)anW[k];
			}
		}
		else
		{
			for (j = 0; j < nD; j++)
			{
				aadMu[k][j] = 0.0;
			}
		}
	}

	return;
}

void initRandom(gsl_rng *ptGSLRNG, t_Cluster *ptCluster, t_Data *ptData)
{
	/*very simple initialisation assign each data point to random cluster */
	int i = 0, k = 0;

	for (i = 0; i < ptData->nN; i++)
	{
		int nIK = -1;

		for (k = 0; k < ptCluster->nK; k++)
		{
			ptCluster->aadZ[i][k] = 0.0;
		}

		nIK = gsl_rng_uniform_int(ptGSLRNG, ptCluster->nK);

		ptCluster->aadZ[i][nIK] = 1.0;
	}

	performMStep(ptCluster, ptData);

	return;
}

void initMustlink(gsl_rng *ptGSLRNG, t_Cluster *ptCluster, t_Data *ptData)
{
	/* initialiation which assign set of individuals which must link to random cluster */
	int i = 0, k = 0;
	char buffer[MAX_LINE_LENGTH];
	FILE *f;

	for (i = 0; i < ptData->nN; i++)
		for (k = 0; k < ptCluster->nK; k++)
			ptCluster->aadZ[i][k] = 0.0;

	f = fopen(szMLInputFile, "r");
	if (f == NULL)
		return;

	while (fgets(buffer, MAX_LINE_LENGTH, f))
	{
		int nIK = gsl_rng_uniform_int(ptGSLRNG, ptCluster->nK);
		char *tok = buffer, *pos;
		while ((pos = strchr(tok, ',')))
		{
			*pos = 0;
			i = atoi(tok);
			tok = pos + 1;

			ptCluster->aadZ[i][nIK] = 1.0;
		}
		i = atoi(tok);
		ptCluster->aadZ[i][nIK] = 1.0;
	}

	fclose(f);

	performMStep(ptCluster, ptData);
}

double calcDist(double *adX, double *adMu, int nD)
{
	double dDist = 0.0;
	int i = 0;

	for (i = 0; i < nD; i++)
	{
		double dV = adX[i] - adMu[i];
		dDist += dV * dV;
	}

	return sqrt(dDist);
}

void initKMeans(gsl_rng *ptGSLRNG, t_Cluster *ptCluster, t_Data *ptData)
{
	/*very simple initialisation assign each data point to random cluster */
	int i = 0, k = 0, nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD;
	double **aadMu = ptCluster->aadMu, **aadX = ptData->aadX;
	int *anMaxZ = ptCluster->anMaxZ, *anW = ptCluster->anW, nChange = nN;
	int nIter = 0, nMaxIter = ptCluster->nMaxIter;
	for (i = 0; i < nN; i++)
	{
		int nIK = gsl_rng_uniform_int(ptGSLRNG, nK);

		ptCluster->anMaxZ[i] = nIK;
		anW[nIK]++;
	}

	updateMeans(ptCluster, ptData);

	while (nChange > 0 && nIter < nMaxIter)
	{
		nChange = 0;
		/*reassign vectors */
		for (i = 0; i < nN; i++)
		{
			double dMinDist = DBL_MAX;
			int nMinK = NOT_SET;

			for (k = 0; k < nK; k++)
			{
				double dDist = calcDist(aadX[i], aadMu[k], nD);

				if (dDist < dMinDist)
				{
					nMinK = k;
					dMinDist = dDist;
				}
			}

			if (nMinK != anMaxZ[i])
			{
				int nCurr = anMaxZ[i];
				nChange++;
				anW[nCurr]--;
				anW[nMinK]++;
				anMaxZ[i] = nMinK;

				/*check for empty clusters */
				if (anW[nCurr] == 0)
				{
					int nRandI =
						gsl_rng_uniform_int(ptGSLRNG, nN);
					int nKI = 0;
					/*select at random from non empty clusters */

					while (anW[anMaxZ[nRandI]] == 1)
					{
						nRandI =
							gsl_rng_uniform_int(ptGSLRNG, nN);
					}

					nKI = anMaxZ[nRandI];
					anW[nKI]--;
					anW[nCurr] = 1;
					anMaxZ[nRandI] = nCurr;
				}
			}
		}
		//printf("%d %d\n",nIter,nChange);
		nIter++;
		updateMeans(ptCluster, ptData);
	}

	for (i = 0; i < nN; i++)
	{
		for (k = 0; k < nK; k++)
		{
			ptCluster->aadZ[i][k] = 0.0;
		}
		ptCluster->aadZ[i][anMaxZ[i]] = 1.0;
	}

	performMStep(ptCluster, ptData);
	return;
}

/*note assuming you are using inverse W matrix*/
double dLogWishartB(gsl_matrix *ptInvW, int nD, double dNu, int bInv)
{
	int i = 0;
	double dRet = 0.0, dT = 0.0;
	double dLogDet = 0.0, dD = (double)nD;
	gsl_matrix *ptTemp = gsl_matrix_alloc(nD, nD);

	gsl_matrix_memcpy(ptTemp, ptInvW);

	dLogDet = decomposeMatrix(ptTemp, nD);

	if (bInv == TRUE)
	{
		dRet = 0.5 * dNu * dLogDet;
	}
	else
	{
		dRet = -0.5 * dNu * dLogDet;
	}

	dT = 0.5 * dNu * dD * log(2.0);

	dT += 0.25 * dD * (dD - 1.0) * log(M_PI);

	for (i = 0; i < nD; i++)
	{
		dT += gsl_sf_lngamma(0.5 * (dNu - (double)i));
	}

	gsl_matrix_free(ptTemp);

	return dRet - dT;
}

double dWishartExpectLogDet(gsl_matrix *ptW, double dNu, int nD)
{
	int i = 0;
	double dRet = 0.0, dLogDet = 0.0, dD = (double)nD;
	gsl_matrix *ptTemp = gsl_matrix_alloc(nD, nD);

	gsl_matrix_memcpy(ptTemp, ptW);

	dLogDet = decomposeMatrix(ptW, nD);

	dRet = dD * log(2.0) + dLogDet;

	for (i = 0; i < nD; i++)
	{
		dRet += gsl_sf_psi(0.5 * (dNu - (double)i));
	}

	gsl_matrix_free(ptTemp);

	return dRet;
}

double dWishartEntropy(gsl_matrix *ptW, double dNu, int nD)
{
	double dRet = -dLogWishartB(ptW, nD, dNu, FALSE);
	double dExpLogDet = dWishartExpectLogDet(ptW, dNu, nD);
	double dD = (double)nD;

	dRet -= 0.5 * (dNu - dD - 1.0) * dExpLogDet;

	dRet += 0.5 * dNu * dD;

	return dRet;
}

double calcVBL(t_Cluster *ptCluster)
{
	int i = 0, k = 0, l = 0, nN = ptCluster->nN;
	int nK = ptCluster->nK, nD = ptCluster->nD;
	double dBishop1 = 0.0, dBishop2 = 0.0, dBishop3 = 0.0, dBishop4 = 0.0, dBishop5 = 0.0; /*Bishop equations 10.71... */
	gsl_matrix *ptRes = gsl_matrix_alloc(nD, nD);
	gsl_vector *ptDiff = gsl_vector_alloc(nD);
	gsl_vector *ptR = gsl_vector_alloc(nD);
	double dD = (double)nD;
	double **aadMu = ptCluster->aadMu, **aadM = ptCluster->aadM, **aadZ = ptCluster->aadZ;
	double *adBeta = ptCluster->adBeta, *adNu = ptCluster->adNu, *adLDet = ptCluster->adLDet, *adPi = ptCluster->adPi;
	double adNK[nK];
	double d2Pi = 2.0 * M_PI, dBeta0 = ptCluster->ptVBParams->dBeta0, dNu0 = ptCluster->ptVBParams->dNu0, dRet = 0.0;
	double dK = 0.0;

	for (k = 0; k < nK; k++)
	{
		adNK[k] = 0.0;
	}

	/*Equation 10.72 */
	for (i = 0; i < nN; i++)
	{
		for (k = 0; k < nK; k++)
		{
			adNK[k] += aadZ[i][k];
			if (adPi[k] > 0.0)
			{
				dBishop2 += aadZ[i][k] * log(adPi[k]);
			}
		}
	}

	for (k = 0; k < nK; k++)
	{
		if (adNK[k] > 0.0)
		{
			dK++;
		}
	}

	/*Equation 10.71 */
	for (k = 0; k < nK; k++)
	{
		if (adNK[k] > 0.0)
		{
			double dT1 = 0.0, dT2 = 0.0, dF = 0.0;

			gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
						   ptCluster->aptCovar[k],
						   ptCluster->aptSigma[k], 0.0, ptRes);

			for (l = 0; l < nD; l++)
			{
				dT1 += gsl_matrix_get(ptRes, l, l);
			}

			for (l = 0; l < nD; l++)
			{
				gsl_vector_set(ptDiff, l,
							   aadMu[k][l] - aadM[k][l]);
			}

			gsl_blas_dsymv(CblasLower, 1.0, ptCluster->aptSigma[k],
						   ptDiff, 0.0, ptR);

			gsl_blas_ddot(ptDiff, ptR, &dT2);

			dF = adLDet[k] - adNu[k] * (dT1 + dT2) -
				 dD * (log(d2Pi) + (1.0 / adBeta[k]));

			dBishop1 += 0.5 * adNK[k] * dF;
		}
	}

	/*Equation 10.74 */
	for (k = 0; k < nK; k++)
	{
		if (adNK[k] > 0.0)
		{
			double dT1 = 0.0, dT2 = 0.0, dF = 0.0;

			gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
						   ptCluster->ptVBParams->ptInvW0,
						   ptCluster->aptSigma[k], 0.0, ptRes);

			for (l = 0; l < nD; l++)
			{
				dT1 += gsl_matrix_get(ptRes, l, l);
			}

			for (l = 0; l < nD; l++)
			{
				gsl_vector_set(ptDiff, l, aadM[k][l]);
			}

			gsl_blas_dsymv(CblasLower, 1.0, ptCluster->aptSigma[k],
						   ptDiff, 0.0, ptR);

			gsl_blas_ddot(ptDiff, ptR, &dT2);

			dF = dD * log(dBeta0 / d2Pi) + adLDet[k] -
				 ((dD * dBeta0) / adBeta[k]) -
				 dBeta0 * adNu[k] * dT2 - adNu[k] * dT1;

			dBishop3 += 0.5 * (dF + (dNu0 - dD - 1.0) * adLDet[k]);
		}
	}

	dBishop3 += dK * ptCluster->ptVBParams->dLogWishartB;

	/*Equation 10.75 */
	for (i = 0; i < nN; i++)
	{
		for (k = 0; k < nK; k++)
		{
			if (aadZ[i][k] > 0.0)
			{
				dBishop4 += aadZ[i][k] * log(aadZ[i][k]);
			}
		}
	}

	/*Equation 10.77 */
	for (k = 0; k < nK; k++)
	{
		if (adNK[k] > 0.0)
		{
			dBishop5 +=
				0.5 * adLDet[k] + 0.5 * dD * log(adBeta[k] / d2Pi) -
				0.5 * dD -
				dWishartExpectLogDet(ptCluster->aptSigma[k],
									 adNu[k], nD);
		}
	}

	gsl_matrix_free(ptRes);
	gsl_vector_free(ptDiff);
	gsl_vector_free(ptR);

	dRet = dBishop1 + dBishop2 + dBishop3 - dBishop4 - dBishop5;

	return dRet;
}

void calcZ(t_Cluster *ptCluster, t_Data *ptData)
{
	double **aadX = ptData->aadX, **aadZ = ptCluster->aadZ;
	int i = 0, k = 0, l = 0;
	int nK = ptCluster->nK, nD = ptCluster->nD, nN = ptData->nN;
	gsl_vector *ptDiff = gsl_vector_alloc(nD);
	gsl_vector *ptRes = gsl_vector_alloc(nD);
	double adDist[nK], dD = (double)nD;
	double **aadM = ptCluster->aadM, *adPi = ptCluster->adPi;

	for (i = 0; i < nN; i++)
	{
		double dMinDist = DBL_MAX;
		double dTotalZ = 0.0;
		double dNTotalZ = 0.0;

		for (k = 0; k < nK; k++)
		{
			if (adPi[k] > 0.)
			{
				/*set vector to data point */
				for (l = 0; l < nD; l++)
				{
					gsl_vector_set(ptDiff, l,
								   aadX[i][l] - aadM[k][l]);
				}

				gsl_blas_dsymv(CblasLower, 1.0,
							   ptCluster->aptSigma[k], ptDiff,
							   0.0, ptRes);

				gsl_blas_ddot(ptDiff, ptRes, &adDist[k]);

				adDist[k] *= ptCluster->adNu[k];

				adDist[k] -= ptCluster->adLDet[k];

				adDist[k] += dD / ptCluster->adBeta[k];

				if (adDist[k] < dMinDist)
				{
					dMinDist = adDist[k];
				}
			}
		}

		for (k = 0; k < nK; k++)
		{
			if (adPi[k] > 0.)
			{
				aadZ[i][k] =
					adPi[k] * exp(-0.5 *
								  (adDist[k] - dMinDist));
				dTotalZ += aadZ[i][k];
			}
			else
			{
				aadZ[i][k] = 0.0;
			}
		}

		for (k = 0; k < nK; k++)
		{
			double dF = aadZ[i][k] / dTotalZ;
			if (dF < MIN_Z)
			{
				aadZ[i][k] = 0.0;
			}
			dNTotalZ += aadZ[i][k];
		}
		if (dNTotalZ > 0.)
		{
			for (k = 0; k < nK; k++)
			{
				aadZ[i][k] /= dNTotalZ;
			}
		}
	}

	gsl_vector_free(ptRes);
	gsl_vector_free(ptDiff);
	return;
}

void gmmTrainVB(t_Cluster *ptCluster, t_Data *ptData)
{
	int i = 0, k = 0, nIter = 0;
	int nN = ptData->nN, nK = ptCluster->nK;
	/*change in log-likelihood */
	double dLastVBL = 0.0, dDelta = DBL_MAX;
	double **aadZ = ptCluster->aadZ;
	int nMaxIter = ptCluster->nMaxIter;
	double dEpsilon = ptCluster->dEpsilon;

	/*calculate data likelihood */
	calcZ(ptCluster, ptData);
	ptCluster->dVBL = calcVBL(ptCluster);

	while (nIter < nMaxIter && dDelta > dEpsilon)
	{

		/*update parameter estimates */
		performMStep(ptCluster, ptData);

		/*calculate responsibilities */
		calcZ(ptCluster, ptData);

		dLastVBL = ptCluster->dVBL;
		ptCluster->dVBL = calcVBL(ptCluster);
		dDelta = fabs(ptCluster->dVBL - dLastVBL);

		printf("[DEBUG-%d] nIter=%d ; maxIter=%d ; dVBL=%.4f ; delta=%.4e ; epsilon=%.4e\n",
			   ptCluster->nThread, nIter, nMaxIter, ptCluster->dVBL, dDelta, dEpsilon);
		nIter++;
	}

	/*assign to best clusters */
	for (i = 0; i < nN; i++)
	{
		double dMaxZ = aadZ[i][0];
		int nMaxK = 0;
		for (k = 1; k < nK; k++)
		{
			if (aadZ[i][k] > dMaxZ)
			{
				nMaxK = k;
				dMaxZ = aadZ[i][k];
			}
		}
		ptCluster->anMaxZ[i] = nMaxK;
	}

	return;
}

void writeClusters(char *szOutFile, t_Cluster *ptCluster, t_Data *ptData)
{
	int nN = ptCluster->nN, i = 0;
	FILE *ofp = fopen(szOutFile, "w");

	if (ofp)
	{
		for (i = 0; i < nN; i++)
		{
			fprintf(ofp, "%s,%d\n", ptData->aszSampleNames[i],
					ptCluster->anMaxZ[i]);
		}
		fclose(ofp);
		printf("[DEBUG] file '%s' closed\n", szOutFile);
	}
	else
	{
		fprintf(stderr,
				"Failed to open %s for writing in writeClusters\n",
				szOutFile);
		fflush(stderr);
	}
}

void writeMeans(char *szOutFile, t_Cluster *ptCluster)
{
	int nK = ptCluster->nK, nD = ptCluster->nD, i = 0, j = 0;
	FILE *ofp = fopen(szOutFile, "w");

	if (ofp)
	{
		for (i = 0; i < nK; i++)
		{
			for (j = 0; j < nD - 1; j++)
			{
				fprintf(ofp, "%f,", ptCluster->aadMu[i][j]);
			}
			fprintf(ofp, "%f\n", ptCluster->aadMu[i][nD - 1]);
		}
		fclose(ofp);
	}
	else
	{
		fprintf(stderr,
				"Failed to open %s for writing in writeMeanss\n",
				szOutFile);
		fflush(stderr);
	}
}

void writeTMeans(char *szOutFile, t_Cluster *ptCluster, t_Data *ptData)
{
	int nK = ptCluster->nK, nD = ptCluster->nD, nT = ptData->nT, i = 0, j = 0;
	FILE *ofp = fopen(szOutFile, "w");
	gsl_vector *ptVector = gsl_vector_alloc(nD);
	gsl_vector *ptTVector = gsl_vector_alloc(nT);

	if (ofp)
	{
		for (i = 0; i < nK; i++)
		{
			for (j = 0; j < nD; j++)
			{
				gsl_vector_set(ptVector, j,
							   ptCluster->aadMu[i][j]);
			}

			gsl_blas_dgemv(CblasNoTrans, 1.0, ptData->ptTMatrix,
						   ptVector, 0.0, ptTVector);

			for (j = 0; j < nT - 1; j++)
			{
				fprintf(ofp, "%f,",
						gsl_vector_get(ptTVector, j));
			}
			fprintf(ofp, "%f\n", gsl_vector_get(ptTVector, nD - 1));
		}
		fclose(ofp);
	}
	else
	{
		fprintf(stderr,
				"Failed to open %s for writing in writeMeanss\n",
				szOutFile);
		fflush(stderr);
	}

	gsl_vector_free(ptVector);
	gsl_vector_free(ptTVector);
}

void *fitEM(void *pvCluster)
{
	t_Cluster *ptCluster = (t_Cluster *)pvCluster;
	gsl_rng *ptGSLRNG = NULL;
	const gsl_rng_type *ptGSLRNGType = NULL;
	time_t start_t, end_t;

	time(&start_t);
	/*initialise GSL RNG*/
	ptGSLRNGType = gsl_rng_default;
	ptGSLRNG = gsl_rng_alloc(ptGSLRNGType);

	gsl_rng_set(ptGSLRNG, ptCluster->lSeed);

	if (ptCluster->szCOutFile == NULL)
	{
		printf("[DEBUG-%d] init KMeans...\n", ptCluster->nThread);
		initKMeans(ptGSLRNG, ptCluster, ptCluster->ptData);
	}
	else
	{
		printf("[DEBUG-%d] init using must-link...\n", ptCluster->nThread);
		initMustlink(ptGSLRNG, ptCluster, ptCluster->ptData);
	}

	printf("[DEBUG-%d] train VBGMM...\n", ptCluster->nThread);
	gmmTrainVB(ptCluster, ptCluster->ptData);

	gsl_rng_free(ptGSLRNG);
	time(&end_t);
	printf("[DEBUG-%d] Lower bound=%.2f ; Execution time fitEM=%.2f sec\n",
		   ptCluster->nThread, ptCluster->dVBL, difftime(end_t, start_t));

	return NULL;
}

void *runRThreads(void *pvpDCluster)
{
	t_Cluster **pptDCluster = (t_Cluster **)pvpDCluster;
	t_Cluster *ptDCluster = (t_Cluster *)*pptDCluster;
	double dBestVBL = -DBL_MAX;
	t_Cluster **aptCluster = NULL;
	pthread_t atRestarts[N_RTHREADS]; /*run each restart on a separate thread */
	int r = 0, nBestR = -1;
	char *szCOutFile = NULL;
	aptCluster = (t_Cluster **)malloc(N_RTHREADS * sizeof(t_Cluster *));
	if (!aptCluster)
		goto memoryError;

	for (r = 0; r < N_RTHREADS; r++)
	{
		if (ptDCluster->szCOutFile != NULL)
		{
			szCOutFile =
				(char *)malloc(sizeof(char) * MAX_LINE_LENGTH);
			sprintf(szCOutFile, "%sr%d.csv", ptDCluster->szCOutFile,
					r);
		}

		aptCluster[r] = (t_Cluster *)malloc(sizeof(t_Cluster));

		allocateCluster(aptCluster[r], ptDCluster->nN, ptDCluster->nK,
						ptDCluster->nD, ptDCluster->ptData,
						ptDCluster->lSeed + r * R_PRIME,
						ptDCluster->nMaxIter, ptDCluster->dEpsilon,
						szCOutFile);
		aptCluster[r]->ptVBParams = ptDCluster->ptVBParams;
		aptCluster[r]->nThread = r;
		pthread_create(&atRestarts[r], NULL, fitEM, (void *)aptCluster[r]);
	}
	//TODO: use pthread_tryjoin_np to join pthreads or display some info
	for (r = 0; r < N_RTHREADS; r++)
	{
		pthread_join(atRestarts[r], NULL);
	}

	/*free up memory associated with input cluster */
	free(ptDCluster);

	for (r = 0; r < N_RTHREADS; r++)
	{
		if (aptCluster[r]->dVBL > dBestVBL)
		{
			nBestR = r;
			dBestVBL = aptCluster[r]->dVBL;
		}
	}
	printf("[DEBUG] best clustering result dVBL = %.4f\n", dBestVBL);

	*pptDCluster = aptCluster[nBestR];
	for (r = 0; r < N_RTHREADS; r++)
	{
		if (r != nBestR)
		{
			destroyCluster(aptCluster[r]);
			free(aptCluster[r]);
		}
	}
	free(aptCluster);

	return NULL;

memoryError:
	fprintf(stderr, "Failed allocating memory in runRThreads\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}

void calcSampleVar(t_Data *ptData, double *adVar, double *adMu)
{
	double **aadX = ptData->aadX;
	int i = 0, n = 0;
	int nD = ptData->nD, nN = ptData->nN;
	/*sample means */
	double dN = (double)nN;

	for (i = 0; i < nD; i++)
	{
		adMu[i] = 0.0;
		adVar[i] = 0.0;
	}

	for (i = 0; i < nD; i++)
	{
		for (n = 0; n < nN; n++)
		{
			adMu[i] += aadX[n][i];
			adVar[i] += aadX[n][i] * aadX[n][i];
		}

		adMu[i] /= dN;

		adVar[i] = (adVar[i] - dN * adMu[i] * adMu[i]) / (dN - 1.0);
	}

	return;
}

void compressCluster(t_Cluster *ptCluster)
{
	int i = 0, k = 0, nNewK = 0, nN = ptCluster->nN;
	double **aadNewZ = NULL, dN = (double)nN;

	for (i = 0; i < ptCluster->nK; i++)
	{
		if (ptCluster->adPi[i] > 0.0)
		{
			nNewK++;
		}
	}

	aadNewZ = (double **)malloc(nN * sizeof(double *));
	if (!aadNewZ)
		goto memoryError;

	for (i = 0; i < nN; i++)
	{
		aadNewZ[i] = (double *)malloc(nNewK * sizeof(double));
		if (!aadNewZ[i])
			goto memoryError;
	}

	for (i = 0; i < nN; i++)
	{
		int nC = 0;
		for (k = 0; k < ptCluster->nK; k++)
		{
			if (ptCluster->adPi[k] > 0.0)
			{
				aadNewZ[i][nC] = ptCluster->aadZ[i][k];
				nC++;
			}
		}
	}

	for (i = 0; i < nN; i++)
	{
		free(ptCluster->aadZ[i]);
	}
	free(ptCluster->aadZ);

	/*reset Z and K */
	ptCluster->aadZ = aadNewZ;
	ptCluster->nK = nNewK;

	/*recalculate Pi */
	for (k = 0; k < ptCluster->nK; k++)
	{
		ptCluster->adPi[k] = 0.0;
		for (i = 0; i < nN; i++)
		{
			ptCluster->adPi[k] += ptCluster->aadZ[i][k];
		}
		ptCluster->adPi[k] /= dN;
	}

	/*assign to best clusters */
	for (i = 0; i < nN; i++)
	{
		double dMaxZ = ptCluster->aadZ[i][0];
		int nMaxK = 0;
		for (k = 1; k < ptCluster->nK; k++)
		{
			if (ptCluster->aadZ[i][k] > dMaxZ)
			{
				nMaxK = k;
				dMaxZ = ptCluster->aadZ[i][k];
			}
		}
		ptCluster->anMaxZ[i] = nMaxK;
	}

	for (k = 0; k < ptCluster->nK; k++)
	{
		ptCluster->anW[k] = 0;
	}

	for (i = 0; i < nN; i++)
	{
		ptCluster->anW[ptCluster->anMaxZ[i]]++;
	}

	return;

memoryError:
	fprintf(stderr, "Failed allocating memory in main\n");
	fflush(stderr);
	exit(EXIT_FAILURE);
}


#ifndef __INCLUDE_EMUPLUSPLUS__
#define __INCLUDE_EMUPLUSPLUS__

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <string>
#include <vector>

extern "C"{
 #include "multi_modelstruct.h"
 #include "multivar_support.h"
}

using namespace std;

/**
 * @file emuplusplus.h
 * \brief a simple c++ interface to a trained emulator
 *
 * Instance of the emulator class are built from a trained emulator,
 * the emulator can then be sampled at a point in the parameter space by the function QueryEmulator
 *
 * They must be initialized from an interactive_emulator statefile, this includes all the data for
 * a trained emulator
 *
 */

class emulator{
 public:
	emulator(string StateFilePath); // default constructor will return Y values
	emulator(string StateFilePath, bool PcaOnly);
	~emulator();

	/** query the emulator with a vector xpoint in the parameter space
	 * could add a method to output the means, errors and a covaraince matrix */
		void QueryEmulator(const vector<double> &xpoint, vector<double> &Means, vector<double> &Errors);

	/**
	 * get the emulator pca decomp
	 */
	void getEmulatorPCA(vector<double> *pca_evals, vector< vector<double> > *pca_evecs, vector<double> *pca_mean);

	int getRegressionOrder(void){return(the_model->regression_order);};
	int getCovFnIndex(void){return(the_model->cov_fn_index);};

	int number_params;
	int number_outputs;

 private:

	// if true the values are output in the pca space, otherwise they're in the real space
	bool outputPCAValues;
	string StateFilePath;
	multi_modelstruct *the_model;  // the c structure which defines the model
	multi_emulator *the_emulator; // the c structure which defines the emulator

	gsl_vector *the_emulate_point;
	gsl_vector *the_emulate_mean;
	gsl_vector *the_emulate_var;

};

#endif


#ifndef INCLUDED_BLAS_L3_SYRK_H
#define INCLUDED_BLAS_L3_SYRK_H

#include <complex>
#include <cblas.h>

namespace blas{

    void syrk( CBLAS_ORDER order, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans,
        std::size_t n, std::size_t k, float alpha, const float* A,
        std::size_t lda, float beta, float* C, std::size_t ldc ){
        return cblas_ssyrk( order, uplo, trans, n, k, alpha, A, lda, beta, C,
            ldc );
    }
    void syrk( CBLAS_ORDER order, CBLAS_UPLO uplo,
        CBLAS_TRANSPOSE trans, std::size_t n, std::size_t k, double alpha,
        const double* A, std::size_t lda, double beta, double* C,
        std::size_t ldc ){
        return cblas_dsyrk( order, uplo, trans, n, k, alpha, A, lda, beta, C,
            ldc );
    }
    void syrk( CBLAS_ORDER order, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans,
        std::size_t n, std::size_t k, const std::complex<float>* alpha,
        const std::complex<float>* A, std::size_t lda,
        const std::complex<float>* beta, std::complex<float>* C,
        std::size_t ldc ){
        return cblas_csyrk( order, uplo, trans, n, k, (float*) alpha,
            (float*) A, lda, (float*) beta, (float*) C, ldc );
    }
    void syrk( CBLAS_ORDER order, CBLAS_UPLO uplo, CBLAS_TRANSPOSE trans,
        std::size_t n, std::size_t k, const std::complex<double>* alpha,
        const std::complex<double>* A, std::size_t lda,
        const std::complex<double>* beta, std::complex<double>* C,
        std::size_t ldc ){
        return cblas_zsyrk( order, uplo, trans, n, k, (double*) alpha,
            (double*) A, lda, (double*) beta, (double*) C, ldc );
    }

} //namespace blas

#endif


#include <stdio.h>
#include <pthread.h>
#include <time.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>

#if defined(PARSEC_HAVE_SCHED_SETAFFINITY) && defined (__USE_GNU) || 1
#include <linux/unistd.h>
#define gettid() syscall(__NR_gettid)
#else
#warning Not using sched_setaffinity
#endif  /* PARSEC_HAVE_SCHED_SETAFFINITY */

#include <sched.h>

#include <cblas.h>

#define MAX_THREADS 128

static int NB = 120;
static volatile int running = 1;

#define TYPE      float
#define GEMM_FUNC cblas_sgemm
/*
#define TYPE      double
#define GEMM_FUNC cblas_dgemm
*/
static void run_gemm(const TYPE *A, const TYPE *B, TYPE *C)
{
    GEMM_FUNC( (const enum CBLAS_ORDER)CblasColMajor,
               (const enum CBLAS_TRANSPOSE)CblasNoTrans, (const enum CBLAS_TRANSPOSE)CblasNoTrans,
               NB /* A.rows */,
               NB /* A.cols */,
               NB /* B.rows */,
               1.0, (TYPE*)A, NB /* A.data_stride */,
                    (TYPE*)B, NB /* B.data_stride */,
               1.0, (TYPE*)C, NB /* C.data_stride */);
}

static TYPE *init_matrix(void)
{
    TYPE *res;
    int i, j;
    res = (TYPE*)calloc(NB*NB, sizeof(TYPE));
    for(i = 0; i < NB; i++)
        for(j = 0; j < NB; j++)
            res[i*NB+j] = (TYPE)rand() / (TYPE)RAND_MAX;
    return res;
}

static void *thread_loop(void *_proc)
{
    TYPE *A, *B, *C;
    unsigned long int proc = (unsigned long int)_proc;
    int i;
    unsigned long long int time;
    struct timespec start, end;

    A = init_matrix();
    B = init_matrix();
    C = init_matrix();

#if defined(PARSEC_HAVE_SCHED_SETAFFINITY) && defined (__USE_GNU)
    {
        cpu_set_t cpuset;

        CPU_ZERO(&cpuset);
        CPU_SET(proc, &cpuset);

        if( -1 == sched_setaffinity(gettid(), sizeof(cpu_set_t), &cpuset) ) {
            printf( "Unable to set the thread affinity (%s)\n", strerror(errno) );
        }
    }
#endif  /* PARSEC_HAVE_SCHED_SETAFFINITY */

    if( proc == 0 ) {
        for(i = 0; i < 100; i++) {
            /* Ensures that all threads run cblas_dgemm on another core */
            /* while warming up */
            run_gemm(A, B, C);
        }

        for(i = 0; i < 1000; i++) {
            /* Take the time */
            clock_gettime(CLOCK_REALTIME, &start);
            run_gemm(A, B, C);
            clock_gettime(CLOCK_REALTIME, &end);
            time = end.tv_nsec - start.tv_nsec + ( (end.tv_sec - start.tv_sec) * 1000000000ULL );
            printf("NB = %d TIME = %llu ns  %f GFlops\n", NB, time,
                   (2*(NB/1e3)*(NB/1e3)*(NB/1e3)) / ((double)time  / 1e9));
        }

        running = 0;
    } else {
        while( running ) {
            run_gemm(A, B, C);
        }
    }
    return NULL;
}

int main(int argc, char *argv[])
{
    unsigned long int i, nbcores;
    pthread_t threads[MAX_THREADS];

    if( argc != 3 ) {
        fprintf(stderr, "usage: %s <Matrix Size> <Nb Cores>\n", argv[0]);
        return 1;
    }

    NB = atoi(argv[1]);
    nbcores = atoi(argv[2]);

    if( NB <= 1 || nbcores < 1 ) {
        fprintf(stderr, "usage: %s <Matrix Size> <Nb Cores>\n", argv[0]);
        return 1;
    }

    for(i = 0; i < nbcores; i++) {
        pthread_create(&threads[i], NULL, thread_loop, (void*)(i+1));
    }

    thread_loop((void*)0);

    for(i = 0; i < nbcores; i++) {
        pthread_join(threads[i], NULL);
    }
    return 0;
}


static char help[] = "Load a collection of PETSc vectors and stack them into a tall-skinny matrix\n\n";

#include <petsc.h>
#include <petscvec.h>
#include <petscmat.h>

/* Mesh size (129, 33, 257) <xdmf reverse order>, so ni = 257, nk = 129 */
PetscErrorCode SnapshotView(Vec u,const char suffix[])
{
  const PetscInt M = 257;
  const PetscInt N = 33;
  const PetscInt P = 129;
  DM             dm;
  Vec            cu;
  PetscViewer    viewer;
  char           ofile[PETSC_MAX_PATH_LEN];
  PetscErrorCode ierr;

  PetscFunctionBegin;
  ierr = DMDACreate3d(PETSC_COMM_WORLD,DM_BOUNDARY_NONE,DM_BOUNDARY_NONE,DM_BOUNDARY_NONE,
                      DMDA_STENCIL_BOX,M,N,P,PETSC_DECIDE,PETSC_DECIDE,PETSC_DECIDE,
                      1,1,NULL,NULL,NULL,&dm);CHKERRQ(ierr);
  ierr = DMSetUp(dm);CHKERRQ(ierr);
  ierr = DMDASetUniformCoordinates(dm,0.0,12.0,-1.5,0.0,0.0,6.0);CHKERRQ(ierr);
  ierr = DMCreateGlobalVector(dm,&cu);CHKERRQ(ierr);
  ierr = VecCopy(u,cu);CHKERRQ(ierr);

  PetscSNPrintf(ofile,PETSC_MAX_PATH_LEN-1,"%s.vts",suffix);
  ierr = PetscViewerVTKOpen(PETSC_COMM_WORLD,ofile,FILE_MODE_WRITE,&viewer);CHKERRQ(ierr);
  ierr = VecView(cu,viewer);CHKERRQ(ierr);

  ierr = PetscViewerDestroy(&viewer);CHKERRQ(ierr);
  ierr = DMDestroy(&dm);CHKERRQ(ierr);
  ierr = VecDestroy(&cu);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

PetscErrorCode SnapshotViewFromFile(const char coor_fname[],const char u_fname[],const char suffix[])
{
  const PetscInt M = 257;
  const PetscInt N = 33;
  const PetscInt P = 129;
  DM             dm;
  Vec            u,coor,_u,_coor;
  PetscViewer    viewer;
  char           ofile[PETSC_MAX_PATH_LEN];
  PetscErrorCode ierr;

  PetscFunctionBegin;
  PetscPrintf(PETSC_COMM_WORLD,"Loading solution: %s\n",u_fname);
  ierr = VecCreate(PETSC_COMM_WORLD,&u);CHKERRQ(ierr);
  ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,u_fname,FILE_MODE_READ,&viewer);CHKERRQ(ierr);
  ierr = VecLoad(u,viewer);CHKERRQ(ierr);
  ierr = PetscViewerDestroy(&viewer);CHKERRQ(ierr);

  PetscPrintf(PETSC_COMM_WORLD,"Loading coordinates: %s\n",coor_fname);
  ierr = VecCreate(PETSC_COMM_WORLD,&coor);CHKERRQ(ierr);
  ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,coor_fname,FILE_MODE_READ,&viewer);CHKERRQ(ierr);
  ierr = VecLoad(coor,viewer);CHKERRQ(ierr);
  ierr = PetscViewerDestroy(&viewer);CHKERRQ(ierr);

  ierr = DMDACreate3d(PETSC_COMM_WORLD,DM_BOUNDARY_NONE,DM_BOUNDARY_NONE,DM_BOUNDARY_NONE,
                      DMDA_STENCIL_BOX,M,N,P,PETSC_DECIDE,PETSC_DECIDE,PETSC_DECIDE,
                      1,1,NULL,NULL,NULL,&dm);CHKERRQ(ierr);
  ierr = DMSetUp(dm);CHKERRQ(ierr);
  ierr = DMDASetUniformCoordinates(dm,0.0,1.0,0.0,1.0,0.0,1.0);CHKERRQ(ierr);

  ierr = DMCreateGlobalVector(dm,&_u);CHKERRQ(ierr);
  ierr = VecCopy(u,_u);CHKERRQ(ierr);

  ierr = DMGetCoordinates(dm,&_coor);CHKERRQ(ierr);
  ierr = VecCopy(coor,_coor);CHKERRQ(ierr);

  PetscSNPrintf(ofile,PETSC_MAX_PATH_LEN-1,"%s.vts",suffix);
  PetscPrintf(PETSC_COMM_WORLD,"Writing output: %s\n",ofile);
  ierr = PetscViewerVTKOpen(PETSC_COMM_WORLD,ofile,FILE_MODE_WRITE,&viewer);CHKERRQ(ierr);
  ierr = VecView(_u,viewer);CHKERRQ(ierr);
  ierr = PetscViewerDestroy(&viewer);CHKERRQ(ierr);

  ierr = DMDestroy(&dm);CHKERRQ(ierr);
  ierr = VecDestroy(&_u);CHKERRQ(ierr);
  ierr = VecDestroy(&u);CHKERRQ(ierr);
  ierr = VecDestroy(&coor);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

PetscErrorCode SnapshotMatCreate(Mat *snapshots)
{
  PetscErrorCode  ierr;
  Vec             u;
  PetscViewer     viewer;
  const PetscInt  steps[] = { 200, 300, 400, 500, 600, 700, 800, 900, 1000, 1100, 1200, 1300, 1400 };
  char            fname[PETSC_MAX_PATH_LEN];
  PetscInt        i,len,m,n,k;
  Mat             S;
  PetscInt        *idx;

  PetscFunctionBegin;
  len = sizeof(steps) / sizeof(PetscInt);
  n = len;
  ierr = PetscOptionsGetInt(NULL,NULL,"-truncate",&n,NULL);CHKERRQ(ierr);
  if (n > len) SETERRQ1(PETSC_COMM_WORLD,PETSC_ERR_USER,"Truncate value cannot be larger than %D",len);

  for (i=0; i<n; i++) {
    PetscSNPrintf(fname,PETSC_MAX_PATH_LEN-1,"data/step%1.6d_energy.pbvec",steps[i]);
    PetscPrintf(PETSC_COMM_WORLD,"[%d] Loading: %s\n",i,fname);

    ierr = VecCreate(PETSC_COMM_WORLD,&u);CHKERRQ(ierr);

    ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,fname,FILE_MODE_READ,&viewer);CHKERRQ(ierr);
    ierr = VecLoad(u,viewer);CHKERRQ(ierr);
    ierr = PetscViewerDestroy(&viewer);CHKERRQ(ierr);

    if (i == 0) {
      ierr = VecGetSize(u,&m);CHKERRQ(ierr);
      PetscPrintf(PETSC_COMM_WORLD,"S: m %d, n %d\n",m,n);
      ierr = MatCreate(PETSC_COMM_WORLD,&S);CHKERRQ(ierr);
      ierr = MatSetSizes(S,PETSC_DECIDE,PETSC_DECIDE,m,n);CHKERRQ(ierr);
      ierr = MatSetType(S,MATDENSE);CHKERRQ(ierr);
      ierr = MatSetUp(S);CHKERRQ(ierr);

      ierr = PetscCalloc1(m,&idx);CHKERRQ(ierr);
      for (k=0; k<m; k++) { idx[k] = k; }
    }
    {
      const PetscScalar *_u;

      ierr = VecGetArrayRead(u,&_u);CHKERRQ(ierr);
      ierr = MatSetValues(S,m,idx,1,&i,_u,INSERT_VALUES);CHKERRQ(ierr);
      ierr = VecRestoreArrayRead(u,&_u);CHKERRQ(ierr);
    }
    ierr = MatAssemblyBegin(S,MAT_FLUSH_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(S,MAT_FLUSH_ASSEMBLY);CHKERRQ(ierr);

    if (i == 0) {
      ierr = SnapshotView(u,"snapshot0");CHKERRQ(ierr);
    }

    ierr = VecDestroy(&u);CHKERRQ(ierr);
  }
  ierr = MatAssemblyBegin(S,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  ierr = MatAssemblyEnd(S,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);

  *snapshots = S;

  ierr = PetscFree(idx);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

int main(int argc,char **args)
{
  PetscErrorCode ierr;
  Mat            S;
  PetscInt       step = -1;
  PetscBool      found = PETSC_FALSE;

  ierr = PetscInitialize(&argc,&args,(char*)0,help);if (ierr) return ierr;

  ierr = SnapshotMatCreate(&S);CHKERRQ(ierr);

  /* Dump S(i,j) as ascii to stdout */
  /*ierr = MatView(S,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);*/

  /* Dump S(i,j) as bindary to file */
  {
    PetscViewer  viewer;

    ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,"snapshot.pbmat",FILE_MODE_WRITE,&viewer);CHKERRQ(ierr);
    ierr = MatView(S,viewer);CHKERRQ(ierr);
    ierr = PetscViewerDestroy(&viewer);CHKERRQ(ierr);
  }

  ierr = PetscOptionsGetInt(NULL,NULL,"-solution_view",&step,&found);CHKERRQ(ierr);
  if (found) {
    char s_fname[PETSC_MAX_PATH_LEN];
    char c_fname[PETSC_MAX_PATH_LEN];
    char ofname[PETSC_MAX_PATH_LEN];

    PetscSNPrintf(s_fname,PETSC_MAX_PATH_LEN-1,"data/step%1.6d_energy.pbvec",step);
    PetscSNPrintf(c_fname,PETSC_MAX_PATH_LEN-1,"data/step%1.6d_coor.pbvec",step);
    PetscSNPrintf(ofname,PETSC_MAX_PATH_LEN-1,"step%d_temperature",step);
    PetscPrintf(PETSC_COMM_WORLD,"Snapshot to visualize: %d\n",step);
    ierr = SnapshotViewFromFile(c_fname,s_fname,ofname);CHKERRQ(ierr);
  }

  ierr = PetscFinalize();
  return ierr;
}


#include "mpi.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <string.h>
#include <stdbool.h>

#ifdef __APPLE__

#include <cblas.h>
#include <libiomp/omp.h>

#define set_num_threads(x) openblas_set_num_threads(x)
#define get_num_threads() openblas_get_num_threads()
#else
#include <mkl.h>
#include <omp.h>
#define set_num_threads(x) mkl_set_num_threads(x)
#define get_num_threads() mkl_get_num_threads()
#endif

double norm_inf_mat_Y11(double *X, int n);

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

    char *filename = NULL;
    int option = 0, omp_threads = 1;
    while ((option = getopt(argc, argv, "f:t:")) != -1) {
        switch (option) {
            case 'f':
                filename = optarg;
                break;
            case 't':
                omp_threads = atoi(optarg);
                break;
            default:
                printf("Usage: mpi_mexp -f string -t num_threads \n");
                return 0;
        }
    }
    if (filename == NULL) {
        printf("Usage: mpi_mexp -f string -t num_threads \n");
        return 0;
    }

    set_num_threads(omp_threads);
    omp_set_num_threads(omp_threads);

    int numTasks, rank;
    MPI_Init(&argc, &argv);
    MPI_Comm_size(MPI_COMM_WORLD, &numTasks);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);

    int qProcs = (int) sqrt(numTasks);
    if (qProcs - sqrt(numTasks) != 0) {
        if (rank == 0)
            printf("np must be a square number\n");
        MPI_Finalize();
        return 0;
    }

    int q_root = qProcs - 1;
    int world_root = numTasks - 1;
    int n, dims[2] = {qProcs, qProcs}, periods[2] = {0, 0}, reorder = 0, coords[2], rc, i;
    MPI_Comm cartComm;
    MPI_Cart_create(MPI_COMM_WORLD, 2, dims, periods, reorder, &cartComm);
    MPI_Comm_rank(cartComm, &rank);
    MPI_Cart_coords(cartComm, rank, 2, coords);

    MPI_File matFile;
    rc = MPI_File_open(cartComm, filename, MPI_MODE_RDONLY, MPI_INFO_NULL, &matFile);
    if (rc && (rank == 0)) {
        printf("Unable to open file %s\n", filename);
        fflush(stdout);
    }

    MPI_Status status;
    MPI_File_read(matFile, &n, 1, MPI_INT, &status);
    int blockSize = (n + qProcs) / qProcs;  /* number of rows in _block_ */
    int lastBlock = n + 1 - (qProcs - 1) * blockSize;

    double *LocalA = (double *) malloc(blockSize * blockSize * sizeof(double));

    MPI_Datatype readFileType;
    MPI_Type_vector(blockSize, blockSize, n, MPI_DOUBLE, &readFileType);
    MPI_Type_commit(&readFileType);
    MPI_Offset offset = (MPI_Offset) (1 * sizeof(int) +
                                      sizeof(double) * (blockSize * coords[1] + blockSize * n * coords[0]));
    MPI_File_set_view(matFile, offset, MPI_DOUBLE, readFileType,
                      "native", MPI_INFO_NULL);

    MPI_File_read_at(matFile, 0, LocalA, blockSize * blockSize, MPI_DOUBLE, &status);
    MPI_Type_free(&readFileType);
    if (coords[0] == q_root) {
        memset(LocalA + (lastBlock - 1) * blockSize, 0, (blockSize + 1 - lastBlock) * blockSize * sizeof(double));
    }
    if (coords[1] == q_root) {
        int kk = 0;
        for (kk = 0; kk < blockSize; ++kk) {
            memset(LocalA + kk * blockSize + lastBlock - 1, 0, (blockSize + 1 - lastBlock) * sizeof(double));
        }
    }

    double loc_d_var;
    double *loc_vec_var = (double *) malloc(blockSize * sizeof(double));
    memset(loc_vec_var, 0, blockSize * sizeof(double));
    offset = (MPI_Offset) (1 * sizeof(int) + sizeof(double) * n * n);
    MPI_File_set_view(matFile, offset, MPI_DOUBLE, MPI_DOUBLE,
                      "native", MPI_INFO_NULL);
    offset = coords[0] * blockSize;
    MPI_File_read_at_all(matFile, offset, loc_vec_var, coords[0] == qProcs - 1 ? lastBlock : blockSize, MPI_DOUBLE,
                         &status);

    MPI_File_close(&matFile);

    MPI_Comm rowComm, colComm;
    MPI_Comm_split(MPI_COMM_WORLD, coords[0], rank, &rowComm);
    MPI_Comm_split(MPI_COMM_WORLD, coords[1], rank, &colComm);

    int row_rank, col_rank;
    MPI_Comm_rank(colComm, &col_rank);
    MPI_Comm_rank(rowComm, &row_rank);

    if (coords[1] == qProcs - 1) {
        cblas_dcopy(blockSize, loc_vec_var, 1, LocalA + lastBlock - 1, blockSize);
    }

    double mpi_start = MPI_Wtime();
    //compute inf norm of matA
    for (i = 0; i < (col_rank == q_root ? lastBlock - 1 : blockSize); ++i) {
        loc_vec_var[i] = cblas_dasum((row_rank == q_root ? lastBlock - 1 : blockSize), LocalA + i * blockSize, 1);
    }

    if (row_rank == q_root) {
        MPI_Reduce(MPI_IN_PLACE, loc_vec_var, blockSize, MPI_DOUBLE, MPI_SUM, q_root, rowComm);
    } else {
        MPI_Reduce(loc_vec_var, loc_vec_var, blockSize, MPI_DOUBLE, MPI_SUM, q_root, rowComm);
    }

    if (row_rank == q_root) {
        loc_d_var = 0;
        for (i = 0; i < (col_rank == q_root ? lastBlock - 1 : blockSize); ++i) {
            if (loc_vec_var[i] > loc_d_var)
                loc_d_var = loc_vec_var[i];
        }
        if (col_rank == q_root) {
            MPI_Reduce(MPI_IN_PLACE, &loc_d_var, 1, MPI_DOUBLE, MPI_MAX, q_root, colComm);
        } else {
            MPI_Reduce(&loc_d_var, &loc_d_var, 1, MPI_DOUBLE, MPI_MAX, q_root, colComm);
        }
    }

    MPI_Bcast(&loc_d_var, 1, MPI_DOUBLE, world_root, MPI_COMM_WORLD);

    //init Y martix

    for (i = 0; i < (col_rank == q_root ? lastBlock - 1 : blockSize); ++i) {
        cblas_dscal((row_rank == q_root ? lastBlock - 1 : blockSize), -1 / loc_d_var, LocalA + i * blockSize, 1);
    }
    if (row_rank == q_root) {
        cblas_dscal((col_rank == q_root ? lastBlock - 1 : blockSize), 1 / loc_d_var, LocalA + lastBlock - 1, blockSize);

    }
    if (row_rank == col_rank) {
        for (i = 0; i < (row_rank == q_root ? lastBlock-1 : blockSize); ++i) {
            LocalA[i * blockSize + i] += 1;
        }
    }
    if (rank == world_root) {
        LocalA[(lastBlock - 1) * (blockSize + 1)] = 1;
    }

    //loop
    double tolerance = 1e-8;
    int loopBreak = 0, iteration = 0;
    double *col_mat = (double *) malloc(qProcs * blockSize * blockSize * sizeof(double));
    memset(col_mat, 0, qProcs * blockSize * blockSize * sizeof(double));

    double *row_mat = (double *) malloc(qProcs * blockSize * blockSize * sizeof(double));
    memset(row_mat, 0, qProcs * blockSize * blockSize * sizeof(double));

    while (true) {
        //norm mat
        //compute inf norm of matA
        for (i = 0; i < (col_rank == q_root ? lastBlock - 1 : blockSize); ++i) {
            loc_vec_var[i] = cblas_dasum((row_rank == q_root ? lastBlock - 1 : blockSize), LocalA + i * blockSize, 1);
        }

        if (row_rank == q_root) {
            MPI_Reduce(MPI_IN_PLACE, loc_vec_var, blockSize, MPI_DOUBLE, MPI_SUM, q_root, rowComm);
        } else {
            MPI_Reduce(loc_vec_var, loc_vec_var, blockSize, MPI_DOUBLE, MPI_SUM, q_root, rowComm);
        }

        if (row_rank == q_root) {
            loc_d_var = 0;
            for (i = 0; i < (col_rank == q_root ? lastBlock - 1 : blockSize); ++i) {
                if (loc_vec_var[i] > loc_d_var)
                    loc_d_var = loc_vec_var[i];
            }
            if (col_rank == q_root) {
                MPI_Reduce(MPI_IN_PLACE, &loc_d_var, 1, MPI_DOUBLE, MPI_MAX, q_root, colComm);
            } else {
                MPI_Reduce(&loc_d_var, &loc_d_var, 1, MPI_DOUBLE, MPI_MAX, q_root, colComm);
            }

            size_t index = cblas_idamax((col_rank == q_root ? lastBlock - 1 : blockSize), LocalA + lastBlock - 1,
                                        blockSize);
            double norm2b;
            MPI_Reduce(&LocalA[index * blockSize + lastBlock - 1], &norm2b, 1, MPI_DOUBLE, MPI_MAX, q_root, colComm);

            if (col_rank == q_root) {
                printf("normM:%.4f, normB:%.4f\n", loc_d_var, norm2b);
                loopBreak = (loc_d_var / norm2b < tolerance) ? 1 : 0;
            }

        }
        MPI_Bcast(&loopBreak, 1, MPI_INT, world_root, MPI_COMM_WORLD);
        if (loopBreak > 0)
            break;

        iteration++;

        //gather loc mat
        MPI_Allgather(LocalA, blockSize * blockSize, MPI_DOUBLE, col_mat, blockSize * blockSize, MPI_DOUBLE, colComm);
        MPI_Allgather(LocalA, blockSize * blockSize, MPI_DOUBLE, row_mat, blockSize * blockSize, MPI_DOUBLE, rowComm);

        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, blockSize, blockSize, blockSize,
                    1, row_mat, blockSize, col_mat, blockSize, 0.0, LocalA, blockSize);
        for (i = 1; i < qProcs; ++i) {
            cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, blockSize, blockSize, blockSize,
                        1, row_mat + i * (blockSize * blockSize), blockSize, col_mat + i * (blockSize * blockSize),
                        blockSize, 1, LocalA, blockSize);
        }
    }

    if (rank == world_root) {
        printf("mpi_mexp_iter:%d \n", iteration);
        printf("mpi_mexp_time: %f\n", MPI_Wtime() - mpi_start);

    }

    if (row_rank == q_root) {
        for (i = 0; i < (row_rank == qProcs - 1 ? lastBlock : blockSize); ++i) {
            if (fabs(LocalA[i * blockSize + lastBlock - 1] - 1) > 1e-3) {
                printf("MPI_MEXP_ERR %f \n", LocalA[i * blockSize + lastBlock - 1]);
                break;
            }
        }
    }

    MPI_Comm_free(&cartComm);
    MPI_Comm_free(&colComm);
    MPI_Comm_free(&rowComm);

    MPI_Finalize();
}


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

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_vector_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>

/*
 * This module computes the eigenvalues of a real upper hessenberg
 * matrix, using the classical double shift Francis QR algorithm.
 * It will also optionally compute the full Schur form and matrix of
 * Schur vectors.
 *
 * See Golub & Van Loan, "Matrix Computations" (3rd ed),
 * algorithm 7.5.2
 */

/* exceptional shift coefficients - these values are from LAPACK DLAHQR */
#define GSL_FRANCIS_COEFF1        (0.75)
#define GSL_FRANCIS_COEFF2        (-0.4375)

static inline void francis_schur_decomp(gsl_matrix * H,
                                        gsl_vector_complex * eval,
                                        gsl_eigen_francis_workspace * w);
static inline size_t francis_search_subdiag_small_elements(gsl_matrix * A);
static inline int francis_qrstep(gsl_matrix * H,
                                 gsl_eigen_francis_workspace * w);
static inline void francis_schur_standardize(gsl_matrix *A,
                                             gsl_complex *eval1,
                                             gsl_complex *eval2,
                                             gsl_eigen_francis_workspace *w);
static inline size_t francis_get_submatrix(gsl_matrix *A, gsl_matrix *B);
static void francis_standard_form(gsl_matrix *A, double *cs, double *sn);

/*
gsl_eigen_francis_alloc()

Allocate a workspace for solving the nonsymmetric eigenvalue problem.
The size of this workspace is O(1)

Inputs: none

Return: pointer to workspace
*/

gsl_eigen_francis_workspace *
gsl_eigen_francis_alloc(void)
{
  gsl_eigen_francis_workspace *w;

  w = (gsl_eigen_francis_workspace *)
      calloc (1, sizeof (gsl_eigen_francis_workspace));

  if (w == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for workspace", GSL_ENOMEM);
    }

  /* these are filled in later */

  w->size = 0;
  w->max_iterations = 0;
  w->n_iter = 0;
  w->n_evals = 0;

  w->compute_t = 0;
  w->Z = NULL;
  w->H = NULL;

  return (w);
} /* gsl_eigen_francis_alloc() */

/*
gsl_eigen_francis_free()
  Free francis workspace w
*/

void
gsl_eigen_francis_free (gsl_eigen_francis_workspace *w)
{
  RETURN_IF_NULL (w);
  free(w);
} /* gsl_eigen_francis_free() */

/*
gsl_eigen_francis_T()
  Called when we want to compute the Schur form T, or no longer
compute the Schur form T

Inputs: compute_t - 1 to compute T, 0 to not compute T
        w         - francis workspace
*/

void
gsl_eigen_francis_T (const int compute_t, gsl_eigen_francis_workspace *w)
{
  w->compute_t = compute_t;
}

/*
gsl_eigen_francis()

Solve the nonsymmetric eigenvalue problem

H x = \lambda x

for the eigenvalues \lambda using algorithm 7.5.2 of
Golub & Van Loan, "Matrix Computations" (3rd ed)

Inputs: H    - upper hessenberg matrix
        eval - where to store eigenvalues
        w    - workspace

Return: success or error - if error code is returned,
        then the QR procedure did not converge in the
        allowed number of iterations. In the event of non-
        convergence, the number of eigenvalues found will
        still be stored in the beginning of eval,

Notes: On output, the diagonal of H contains 1-by-1 or 2-by-2
       blocks containing the eigenvalues. If T is desired,
       H will contain the full Schur form on output.
*/

int
gsl_eigen_francis (gsl_matrix * H, gsl_vector_complex * eval,
                   gsl_eigen_francis_workspace * w)
{
  /* check matrix and vector sizes */

  if (H->size1 != H->size2)
    {
      GSL_ERROR ("matrix must be square to compute eigenvalues", GSL_ENOTSQR);
    }
  else if (eval->size != H->size1)
    {
      GSL_ERROR ("eigenvalue vector must match matrix size", GSL_EBADLEN);
    }
  else
    {
      const size_t N = H->size1;
      int j;

      /*
       * Set internal parameters which depend on matrix size.
       * The Francis solver can be called with any size matrix
       * since the workspace does not depend on N.
       * Furthermore, multishift solvers which call the Francis
       * solver may need to call it with different sized matrices
       */
      w->size = N;
      w->max_iterations = 30 * N;

      /*
       * save a pointer to original matrix since francis_schur_decomp
       * is recursive
       */
      w->H = H;

      w->n_iter = 0;
      w->n_evals = 0;

      /*
       * zero out the first two subdiagonals (below the main subdiagonal)
       * needed as scratch space by the QR sweep routine
       */
      for (j = 0; j < (int) N - 3; ++j)
        {
          gsl_matrix_set(H, (size_t) j + 2, (size_t) j, 0.0);
          gsl_matrix_set(H, (size_t) j + 3, (size_t) j, 0.0);
        }

      if (N > 2)
        gsl_matrix_set(H, N - 1, N - 3, 0.0);

      /*
       * compute Schur decomposition of H and store eigenvalues
       * into eval
       */
      francis_schur_decomp(H, eval, w);

      if (w->n_evals != N)
        return GSL_EMAXITER;

      return GSL_SUCCESS;
    }
} /* gsl_eigen_francis() */

/*
gsl_eigen_francis_Z()

Solve the nonsymmetric eigenvalue problem for a Hessenberg
matrix

H x = \lambda x

for the eigenvalues \lambda using the Francis double-shift
method.

Here we compute the real Schur form

T = Q^t H Q

with the diagonal blocks of T giving us the eigenvalues.
Q is the matrix of Schur vectors.

Originally, H was obtained from a general nonsymmetric matrix
A via a transformation

H = U^t A U

so that

T = (UQ)^t A (UQ) = Z^t A Z

Z is the matrix of Schur vectors computed by this algorithm

Inputs: H    - upper hessenberg matrix
        eval - where to store eigenvalues
        Z    - where to store Schur vectors
        w    - workspace

Notes: 1) If T is computed, it is stored in H on output. Otherwise,
          the diagonal of H will contain 1-by-1 and 2-by-2 blocks
          containing the eigenvalues.

       2) The matrix Z must be initialized to the Hessenberg
          similarity matrix U. Or if you want the eigenvalues
          of H, initialize Z to the identity matrix.
*/

int
gsl_eigen_francis_Z (gsl_matrix * H, gsl_vector_complex * eval,
                     gsl_matrix * Z, gsl_eigen_francis_workspace * w)
{
  int s;

  /* set internal Z pointer so we know to accumulate transformations */
  w->Z = Z;

  s = gsl_eigen_francis(H, eval, w);

  w->Z = NULL;

  return s;
} /* gsl_eigen_francis_Z() */

/********************************************
 *           INTERNAL ROUTINES              *
 ********************************************/

/*
francis_schur_decomp()
  Compute the Schur decomposition of the matrix H

Inputs: H     - hessenberg matrix
        eval  - where to store eigenvalues
        w     - workspace

Return: none
*/

static inline void
francis_schur_decomp(gsl_matrix * H, gsl_vector_complex * eval,
                     gsl_eigen_francis_workspace * w)
{
  gsl_matrix_view m;   /* active matrix we are working on */
  size_t N;            /* size of matrix */
  size_t q;            /* index of small subdiagonal element */
  gsl_complex lambda1, /* eigenvalues */
              lambda2;

  N = H->size1;
  m = gsl_matrix_submatrix(H, 0, 0, N, N);

  while ((N > 2) && ((w->n_iter)++ < w->max_iterations))
    {
      q = francis_search_subdiag_small_elements(&m.matrix);

      if (q == 0)
        {
          /*
           * no small subdiagonal element found - perform a QR
           * sweep on the active reduced hessenberg matrix
           */
          francis_qrstep(&m.matrix, w);
          continue;
        }

      /*
       * a small subdiagonal element was found - one or two eigenvalues
       * have converged or the matrix has split into two smaller matrices
       */

      if (q == (N - 1))
        {
          /*
           * the last subdiagonal element of the matrix is 0 -
           * m_{NN} is a real eigenvalue
           */
          GSL_SET_COMPLEX(&lambda1,
                          gsl_matrix_get(&m.matrix, q, q), 0.0);
          gsl_vector_complex_set(eval, w->n_evals, lambda1);
          w->n_evals += 1;
          w->n_iter = 0;

          --N;
          m = gsl_matrix_submatrix(&m.matrix, 0, 0, N, N);
        }
      else if (q == (N - 2))
        {
          gsl_matrix_view v;

          /*
           * The bottom right 2-by-2 block of m is an eigenvalue
           * system
           */

          v = gsl_matrix_submatrix(&m.matrix, q, q, 2, 2);
          francis_schur_standardize(&v.matrix, &lambda1, &lambda2, w);

          gsl_vector_complex_set(eval, w->n_evals, lambda1);
          gsl_vector_complex_set(eval, w->n_evals + 1, lambda2);
          w->n_evals += 2;
          w->n_iter = 0;

          N -= 2;
          m = gsl_matrix_submatrix(&m.matrix, 0, 0, N, N);
        }
      else if (q == 1)
        {
          /* the first matrix element is an eigenvalue */
          GSL_SET_COMPLEX(&lambda1,
                          gsl_matrix_get(&m.matrix, 0, 0), 0.0);
          gsl_vector_complex_set(eval, w->n_evals, lambda1);
          w->n_evals += 1;
          w->n_iter = 0;

          --N;
          m = gsl_matrix_submatrix(&m.matrix, 1, 1, N, N);
        }
      else if (q == 2)
        {
          gsl_matrix_view v;

          /* the upper left 2-by-2 block is an eigenvalue system */

          v = gsl_matrix_submatrix(&m.matrix, 0, 0, 2, 2);
          francis_schur_standardize(&v.matrix, &lambda1, &lambda2, w);

          gsl_vector_complex_set(eval, w->n_evals, lambda1);
          gsl_vector_complex_set(eval, w->n_evals + 1, lambda2);
          w->n_evals += 2;
          w->n_iter = 0;

          N -= 2;
          m = gsl_matrix_submatrix(&m.matrix, 2, 2, N, N);
        }
      else
        {
          gsl_matrix_view v;

          /*
           * There is a zero element on the subdiagonal somewhere
           * in the middle of the matrix - we can now operate
           * separately on the two submatrices split by this
           * element. q is the row index of the zero element.
           */

          /* operate on lower right (N - q)-by-(N - q) block first */
          v = gsl_matrix_submatrix(&m.matrix, q, q, N - q, N - q);
          francis_schur_decomp(&v.matrix, eval, w);

          /* operate on upper left q-by-q block */
          v = gsl_matrix_submatrix(&m.matrix, 0, 0, q, q);
          francis_schur_decomp(&v.matrix, eval, w);

          N = 0;
        }
    }

  /* handle special cases of N = 1 or 2 */

  if (N == 1)
    {
      GSL_SET_COMPLEX(&lambda1, gsl_matrix_get(&m.matrix, 0, 0), 0.0);
      gsl_vector_complex_set(eval, w->n_evals, lambda1);
      w->n_evals += 1;
      w->n_iter = 0;
    }
  else if (N == 2)
    {
      francis_schur_standardize(&m.matrix, &lambda1, &lambda2, w);
      gsl_vector_complex_set(eval, w->n_evals, lambda1);
      gsl_vector_complex_set(eval, w->n_evals + 1, lambda2);
      w->n_evals += 2;
      w->n_iter = 0;
    }
} /* francis_schur_decomp() */

/*
francis_qrstep()
  Perform a Francis QR step.

See Golub & Van Loan, "Matrix Computations" (3rd ed),
algorithm 7.5.1

Inputs: H - upper Hessenberg matrix
        w - workspace

Notes: The matrix H must be "reduced", ie: have no tiny subdiagonal
       elements. When computing the first householder reflection,
       we divide by H_{21} so it is necessary that this element
       is not zero. When a subdiagonal element becomes negligible,
       the calling function should call this routine with the
       submatrices split by that element, so that we don't divide
       by zeros.
*/

static inline int
francis_qrstep(gsl_matrix * H, gsl_eigen_francis_workspace * w)
{
  const size_t N = H->size1;
  size_t i;        /* looping */
  gsl_matrix_view m;
  double tau_i;    /* householder coefficient */
  double dat[3];   /* householder vector */
  double scale;    /* scale factor to avoid overflow */
  gsl_vector_view v2, v3;
  size_t q, r;
  size_t top = 0;  /* location of H in original matrix */
  double s,
         disc;
  double h_nn,     /* H(n,n) */
         h_nm1nm1, /* H(n-1,n-1) */
         h_cross,  /* H(n,n-1) * H(n-1,n) */
         h_tmp1,
         h_tmp2;

  v2 = gsl_vector_view_array(dat, 2);
  v3 = gsl_vector_view_array(dat, 3);

  if ((w->n_iter % 10) == 0)
    {
      /*
       * exceptional shifts: we have gone 10 iterations
       * without finding a new eigenvalue, try a new choice of shifts.
       * See LAPACK routine DLAHQR
       */
      s = fabs(gsl_matrix_get(H, N - 1, N - 2)) +
          fabs(gsl_matrix_get(H, N - 2, N - 3));
      h_nn = gsl_matrix_get(H, N - 1, N - 1) + GSL_FRANCIS_COEFF1 * s;
      h_nm1nm1 = h_nn;
      h_cross = GSL_FRANCIS_COEFF2 * s * s;
    }
  else
    {
      /*
       * normal shifts - compute Rayleigh quotient and use
       * Wilkinson shift if possible
       */

      h_nn = gsl_matrix_get(H, N - 1, N - 1);
      h_nm1nm1 = gsl_matrix_get(H, N - 2, N - 2);
      h_cross = gsl_matrix_get(H, N - 1, N - 2) *
                gsl_matrix_get(H, N - 2, N - 1);

      disc = 0.5 * (h_nm1nm1 - h_nn);
      disc = disc * disc + h_cross;
      if (disc > 0.0)
        {
          double ave;

          /* real roots - use Wilkinson's shift twice */
          disc = sqrt(disc);
          ave = 0.5 * (h_nm1nm1 + h_nn);
          if (fabs(h_nm1nm1) - fabs(h_nn) > 0.0)
            {
              h_nm1nm1 = h_nm1nm1 * h_nn - h_cross;
              h_nn = h_nm1nm1 / (disc * GSL_SIGN(ave) + ave);
            }
          else
            {
              h_nn = disc * GSL_SIGN(ave) + ave;
            }

          h_nm1nm1 = h_nn;
          h_cross = 0.0;
        }
    }

  h_tmp1 = h_nm1nm1 - gsl_matrix_get(H, 0, 0);
  h_tmp2 = h_nn - gsl_matrix_get(H, 0, 0);

  /*
   * These formulas are equivalent to those in Golub & Van Loan
   * for the normal shift case - the terms have been rearranged
   * to reduce possible roundoff error when subdiagonal elements
   * are small
   */

  dat[0] = (h_tmp1*h_tmp2 - h_cross) / gsl_matrix_get(H, 1, 0) +
           gsl_matrix_get(H, 0, 1);
  dat[1] = gsl_matrix_get(H, 1, 1) - gsl_matrix_get(H, 0, 0) - h_tmp1 -
           h_tmp2;
  dat[2] = gsl_matrix_get(H, 2, 1);

  scale = fabs(dat[0]) + fabs(dat[1]) + fabs(dat[2]);
  if (scale != 0.0)
    {
      /* scale to prevent overflow or underflow */
      dat[0] /= scale;
      dat[1] /= scale;
      dat[2] /= scale;
    }

  if (w->Z || w->compute_t)
    {
      /*
       * get absolute indices of this (sub)matrix relative to the
       * original Hessenberg matrix
       */
      top = francis_get_submatrix(w->H, H);
    }

  for (i = 0; i < N - 2; ++i)
    {
      tau_i = gsl_linalg_householder_transform(&v3.vector);

      if (tau_i != 0.0)
        {
          /* q = max(1, i - 1) */
          q = (1 > ((int)i - 1)) ? 0 : (i - 1);

          /* r = min(i + 3, N - 1) */
          r = ((i + 3) < (N - 1)) ? (i + 3) : (N - 1);

          if (w->compute_t)
            {
              /*
               * We are computing the Schur form T, so we
               * need to transform the whole matrix H
               *
               * H -> P_k^t H P_k
               *
               * where P_k is the current Householder matrix
               */

              /* apply left householder matrix (I - tau_i v v') to H */
              m = gsl_matrix_submatrix(w->H,
                                       top + i,
                                       top + q,
                                       3,
                                       w->size - top - q);
              gsl_linalg_householder_hm(tau_i, &v3.vector, &m.matrix);

              /* apply right householder matrix (I - tau_i v v') to H */
              m = gsl_matrix_submatrix(w->H,
                                       0,
                                       top + i,
                                       top + r + 1,
                                       3);
              gsl_linalg_householder_mh(tau_i, &v3.vector, &m.matrix);
            }
          else
            {
              /*
               * We are not computing the Schur form T, so we
               * only need to transform the active block
               */

              /* apply left householder matrix (I - tau_i v v') to H */
              m = gsl_matrix_submatrix(H, i, q, 3, N - q);
              gsl_linalg_householder_hm(tau_i, &v3.vector, &m.matrix);

              /* apply right householder matrix (I - tau_i v v') to H */
              m = gsl_matrix_submatrix(H, 0, i, r + 1, 3);
              gsl_linalg_householder_mh(tau_i, &v3.vector, &m.matrix);
            }

          if (w->Z)
            {
              /* accumulate the similarity transformation into Z */
              m = gsl_matrix_submatrix(w->Z, 0, top + i, w->size, 3);
              gsl_linalg_householder_mh(tau_i, &v3.vector, &m.matrix);
            }
        } /* if (tau_i != 0.0) */

      dat[0] = gsl_matrix_get(H, i + 1, i);
      dat[1] = gsl_matrix_get(H, i + 2, i);
      if (i < (N - 3))
        {
          dat[2] = gsl_matrix_get(H, i + 3, i);
        }

      scale = fabs(dat[0]) + fabs(dat[1]) + fabs(dat[2]);
      if (scale != 0.0)
        {
          /* scale to prevent overflow or underflow */
          dat[0] /= scale;
          dat[1] /= scale;
          dat[2] /= scale;
        }
    } /* for (i = 0; i < N - 2; ++i) */

  scale = fabs(dat[0]) + fabs(dat[1]);
  if (scale != 0.0)
    {
      /* scale to prevent overflow or underflow */
      dat[0] /= scale;
      dat[1] /= scale;
    }

  tau_i = gsl_linalg_householder_transform(&v2.vector);

  if (w->compute_t)
    {
      m = gsl_matrix_submatrix(w->H,
                               top + N - 2,
                               top + N - 3,
                               2,
                               w->size - top - N + 3);
      gsl_linalg_householder_hm(tau_i, &v2.vector, &m.matrix);

      m = gsl_matrix_submatrix(w->H,
                               0,
                               top + N - 2,
                               top + N,
                               2);
      gsl_linalg_householder_mh(tau_i, &v2.vector, &m.matrix);
    }
  else
    {
      m = gsl_matrix_submatrix(H, N - 2, N - 3, 2, 3);
      gsl_linalg_householder_hm(tau_i, &v2.vector, &m.matrix);

      m = gsl_matrix_submatrix(H, 0, N - 2, N, 2);
      gsl_linalg_householder_mh(tau_i, &v2.vector, &m.matrix);
    }

  if (w->Z)
    {
      /* accumulate transformation into Z */
      m = gsl_matrix_submatrix(w->Z, 0, top + N - 2, w->size, 2);
      gsl_linalg_householder_mh(tau_i, &v2.vector, &m.matrix);
    }

  return GSL_SUCCESS;
} /* francis_qrstep() */

/*
francis_search_subdiag_small_elements()
  Search for a small subdiagonal element starting from the bottom
of a matrix A. A small element is one that satisfies:

|A_{i,i-1}| <= eps * (|A_{i,i}| + |A_{i-1,i-1}|)

Inputs: A - matrix (must be at least 3-by-3)

Return: row index of small subdiagonal element or 0 if not found

Notes: the first small element that is found (starting from bottom)
       is set to zero
*/

static inline size_t
francis_search_subdiag_small_elements(gsl_matrix * A)
{
  const size_t N = A->size1;
  size_t i;
  double dpel = gsl_matrix_get(A, N - 2, N - 2);

  for (i = N - 1; i > 0; --i)
    {
      double sel = gsl_matrix_get(A, i, i - 1);
      double del = gsl_matrix_get(A, i, i);

      if ((sel == 0.0) ||
          (fabs(sel) < GSL_DBL_EPSILON * (fabs(del) + fabs(dpel))))
        {
          gsl_matrix_set(A, i, i - 1, 0.0);
          return (i);
        }

      dpel = del;
    }

  return (0);
} /* francis_search_subdiag_small_elements() */

/*
francis_schur_standardize()
  Convert a 2-by-2 diagonal block in the Schur form to standard form
and update the rest of T and Z matrices if required.

Inputs: A     - 2-by-2 matrix
        eval1 - where to store eigenvalue 1
        eval2 - where to store eigenvalue 2
        w     - francis workspace
*/

static inline void
francis_schur_standardize(gsl_matrix *A, gsl_complex *eval1,
                          gsl_complex *eval2,
                          gsl_eigen_francis_workspace *w)
{
  const size_t N = w->size;
  double cs, sn;
  size_t top;

  /*
   * figure out where the submatrix A resides in the
   * original matrix H
   */
  top = francis_get_submatrix(w->H, A);

  /* convert 2-by-2 block to standard form */
  francis_standard_form(A, &cs, &sn);

  /* set eigenvalues */

  GSL_SET_REAL(eval1, gsl_matrix_get(A, 0, 0));
  GSL_SET_REAL(eval2, gsl_matrix_get(A, 1, 1));
  if (gsl_matrix_get(A, 1, 0) == 0.0)
    {
      GSL_SET_IMAG(eval1, 0.0);
      GSL_SET_IMAG(eval2, 0.0);
    }
  else
    {
      double tmp = sqrt(fabs(gsl_matrix_get(A, 0, 1)) *
                        fabs(gsl_matrix_get(A, 1, 0)));
      GSL_SET_IMAG(eval1, tmp);
      GSL_SET_IMAG(eval2, -tmp);
    }

  if (w->compute_t)
    {
      gsl_vector_view xv, yv;

      /*
       * The above call to francis_standard_form transformed a 2-by-2 block
       * of T into upper triangular form via the transformation
       *
       * U = [ CS -SN ]
       *     [ SN  CS ]
       *
       * The original matrix T was
       *
       * T = [ T_{11} | T_{12} | T_{13} ]
       *     [   0*   |   A    | T_{23} ]
       *     [   0    |   0*   | T_{33} ]
       *
       * where 0* indicates all zeros except for possibly
       * one subdiagonal element next to A.
       *
       * After francis_standard_form, T looks like this:
       *
       * T = [ T_{11} | T_{12}  | T_{13} ]
       *     [   0*   | U^t A U | T_{23} ]
       *     [   0    |    0*   | T_{33} ]
       *
       * since only the 2-by-2 block of A was changed. However,
       * in order to be able to back transform T at the end,
       * we need to apply the U transformation to the rest
       * of the matrix T since there is no way to apply a
       * similarity transformation to T and change only the
       * middle 2-by-2 block. In other words, let
       *
       * M = [ I 0 0 ]
       *     [ 0 U 0 ]
       *     [ 0 0 I ]
       *
       * and compute
       *
       * M^t T M = [ T_{11} | T_{12} U |   T_{13}   ]
       *           [ U^t 0* | U^t A U  | U^t T_{23} ]
       *           [   0    |   0* U   |   T_{33}   ]
       *
       * So basically we need to apply the transformation U
       * to the i x 2 matrix T_{12} and the 2 x (n - i + 2)
       * matrix T_{23}, where i is the index of the top of A
       * in T.
       *
       * The BLAS routine drot() is suited for this.
       */

      if (top < (N - 2))
        {
          /* transform the 2 rows of T_{23} */

          xv = gsl_matrix_subrow(w->H, top, top + 2, N - top - 2);
          yv = gsl_matrix_subrow(w->H, top + 1, top + 2, N - top - 2);
          gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
        }

      if (top > 0)
        {
          /* transform the 2 columns of T_{12} */

          xv = gsl_matrix_subcolumn(w->H, top, 0, top);
          yv = gsl_matrix_subcolumn(w->H, top + 1, 0, top);
          gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
        }
    } /* if (w->compute_t) */

  if (w->Z)
    {
      gsl_vector_view xv, yv;

      /*
       * Accumulate the transformation in Z. Here, Z -> Z * M
       *
       * So:
       *
       * Z -> [ Z_{11} | Z_{12} U | Z_{13} ]
       *      [ Z_{21} | Z_{22} U | Z_{23} ]
       *      [ Z_{31} | Z_{32} U | Z_{33} ]
       *
       * So we just need to apply drot() to the 2 columns
       * starting at index 'top'
       */

      xv = gsl_matrix_column(w->Z, top);
      yv = gsl_matrix_column(w->Z, top + 1);

      gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
    } /* if (w->Z) */
} /* francis_schur_standardize() */

/*
francis_get_submatrix()
  B is a submatrix of A. The goal of this function is to
compute the indices in A of where the matrix B resides
*/

static inline size_t
francis_get_submatrix(gsl_matrix *A, gsl_matrix *B)
{
  size_t diff;
  double ratio;
  size_t top;

  diff = (size_t) (B->data - A->data);

  ratio = (double)diff / ((double) (A->tda + 1));

  top = (size_t) floor(ratio);

  return top;
} /* francis_get_submatrix() */

/*
francis_standard_form()
  Compute the Schur factorization of a real 2-by-2 matrix in
standard form:

[ A B ] = [ CS -SN ] [ T11 T12 ] [ CS SN ]
[ C D ]   [ SN  CS ] [ T21 T22 ] [-SN CS ]

where either:
1) T21 = 0 so that T11 and T22 are real eigenvalues of the matrix, or
2) T11 = T22 and T21*T12 < 0, so that T11 +/- sqrt(|T21*T12|) are
   complex conjugate eigenvalues

Inputs: A  - 2-by-2 matrix
        cs - where to store cosine parameter of rotation matrix
        sn - where to store sine parameter of rotation matrix

Notes: 1) based on LAPACK routine DLANV2
       2) On output, A is modified to contain the matrix in standard form
*/

static void
francis_standard_form(gsl_matrix *A, double *cs, double *sn)
{
  double a, b, c, d; /* input matrix values */
  double tmp;
  double p, z;
  double bcmax, bcmis, scale;
  double tau, sigma;
  double cs1, sn1;
  double aa, bb, cc, dd;
  double sab, sac;

  a = gsl_matrix_get(A, 0, 0);
  b = gsl_matrix_get(A, 0, 1);
  c = gsl_matrix_get(A, 1, 0);
  d = gsl_matrix_get(A, 1, 1);

  if (c == 0.0)
    {
      /*
       * matrix is already upper triangular - set rotation matrix
       * to the identity
       */
      *cs = 1.0;
      *sn = 0.0;
    }
  else if (b == 0.0)
    {
      /* swap rows and columns to make it upper triangular */

      *cs = 0.0;
      *sn = 1.0;

      tmp = d;
      d = a;
      a = tmp;
      b = -c;
      c = 0.0;
    }
  else if (((a - d) == 0.0) && (GSL_SIGN(b) != GSL_SIGN(c)))
    {
      /* the matrix has complex eigenvalues with a == d */
      *cs = 1.0;
      *sn = 0.0;
    }
  else
    {
      tmp = a - d;
      p = 0.5 * tmp;
      bcmax = GSL_MAX(fabs(b), fabs(c));
      bcmis = GSL_MIN(fabs(b), fabs(c)) * GSL_SIGN(b) * GSL_SIGN(c);
      scale = GSL_MAX(fabs(p), bcmax);
      z = (p / scale) * p + (bcmax / scale) * bcmis;

      if (z >= 4.0 * GSL_DBL_EPSILON)
        {
          /* real eigenvalues, compute a and d */

          z = p + GSL_SIGN(p) * fabs(sqrt(scale) * sqrt(z));
          a = d + z;
          d -= (bcmax / z) * bcmis;

          /* compute b and the rotation matrix */

          tau = gsl_hypot(c, z);
          *cs = z / tau;
          *sn = c / tau;
          b -= c;
          c = 0.0;
        }
      else
        {
          /*
           * complex eigenvalues, or real (almost) equal eigenvalues -
           * make diagonal elements equal
           */

          sigma = b + c;
          tau = gsl_hypot(sigma, tmp);
          *cs = sqrt(0.5 * (1.0 + fabs(sigma) / tau));
          *sn = -(p / (tau * (*cs))) * GSL_SIGN(sigma);

          /*
           * Compute [ AA BB ] = [ A B ] [ CS -SN ]
           *         [ CC DD ]   [ C D ] [ SN  CS ]
           */
          aa = a * (*cs) + b * (*sn);
          bb = -a * (*sn) + b * (*cs);
          cc = c * (*cs) + d * (*sn);
          dd = -c * (*sn) + d * (*cs);

          /*
           * Compute [ A B ] = [ CS SN ] [ AA BB ]
           *         [ C D ]   [-SN CS ] [ CC DD ]
           */
          a = aa * (*cs) + cc * (*sn);
          b = bb * (*cs) + dd * (*sn);
          c = -aa * (*sn) + cc * (*cs);
          d = -bb * (*sn) + dd * (*cs);

          tmp = 0.5 * (a + d);
          a = d = tmp;

          if (c != 0.0)
            {
              if (b != 0.0)
                {
                  if (GSL_SIGN(b) == GSL_SIGN(c))
                    {
                      /*
                       * real eigenvalues: reduce to upper triangular
                       * form
                       */
                      sab = sqrt(fabs(b));
                      sac = sqrt(fabs(c));
                      p = GSL_SIGN(c) * fabs(sab * sac);
                      tau = 1.0 / sqrt(fabs(b + c));
                      a = tmp + p;
                      d = tmp - p;
                      b -= c;
                      c = 0.0;

                      cs1 = sab * tau;
                      sn1 = sac * tau;
                      tmp = (*cs) * cs1 - (*sn) * sn1;
                      *sn = (*cs) * sn1 + (*sn) * cs1;
                      *cs = tmp;
                    }
                }
              else
                {
                  b = -c;
                  c = 0.0;
                  tmp = *cs;
                  *cs = -(*sn);
                  *sn = tmp;
                }
            }
        }
    }

  /* set new matrix elements */

  gsl_matrix_set(A, 0, 0, a);
  gsl_matrix_set(A, 0, 1, b);
  gsl_matrix_set(A, 1, 0, c);
  gsl_matrix_set(A, 1, 1, d);
} /* francis_standard_form() */