/*System includes*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <sys/stat.h>
#include <float.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_cblas.h>
#include <pthread.h>

#include <Python.h>

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

static PyObject *vbgmm_fit(PyObject *self, PyObject *args)
{
    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;
    sts = driver(szFileStub,nKStart,nLMin,lSeed,nMaxIter,dEpsilon,bCout);
    return Py_BuildValue("i", sts);
}

static PyMethodDef VbgmmMethods[] = {
    {"fit",  vbgmm_fit, METH_VARARGS,
     "Fit a variational Bayesian Gaussian mixture."},
    {NULL, NULL, 0, NULL}        /* Sentinel */
};

/* Python 2.x code */

PyMODINIT_FUNC
initvbgmm(void)
{
    (void) Py_InitModule("vbgmm", VbgmmMethods);
}

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 i = 0, k = 0, nD = 0, nN = 0;
  char szOFile[MAX_LINE_LENGTH];
  FILE *ofp = NULL;
  t_VBParams tVBParams;
  t_Cluster  *ptBestCluster = NULL;
  gsl_matrix *ptTemp = NULL;
  gsl_matrix *ptTVar = NULL;
  /*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 = szFileStub;
  }
  else{
	ptBestCluster->szCOutFile = NULL;
  }
  runRThreads((void *) &ptBestCluster);

  compressCluster(ptBestCluster);

  calcCovarMatrices(ptBestCluster,&tData);

  sprintf(szOFile,"%sclustering_gt%d.csv",tParams.szOutFileStub,tParams.nLMin);
  writeClusters(szOFile,ptBestCluster,&tData);

  sprintf(szOFile,"%spca_means_gt%d.csv",tParams.szOutFileStub,tParams.nLMin);
  writeMeans(szOFile,ptBestCluster);

  sprintf(szOFile,"%smeans_gt%d.csv",tParams.szOutFileStub,tParams.nLMin);
  writeTMeans(szOFile,ptBestCluster,&tData);

  for(k = 0; k < ptBestCluster->nK; k++){
    sprintf(szOFile,"%spca_variances_gt%d_dim%d.csv",tParams.szOutFileStub,tParams.nLMin,k);

    writeSquareMatrix(szOFile, ptBestCluster->aptSigma[k], nD);

    /*not entirely sure this is correct?*/
    gsl_blas_dgemm (CblasNoTrans,CblasNoTrans,1.0,tData.ptTMatrix,ptBestCluster->aptSigma[k],0.0,ptTemp);

    gsl_blas_dgemm (CblasNoTrans,CblasTrans,1.0,ptTemp,tData.ptTMatrix,0.0,ptTVar);

    sprintf(szOFile,"%svariances_gt%d_dim%d.csv",tParams.szOutFileStub,tParams.nLMin,k);

    writeSquareMatrix(szOFile, ptTVar, nD);
  }

  sprintf(szOFile,"%sresponsibilities.csv",tParams.szOutFileStub);

  ofp = fopen(szOFile,"w");
  if(ofp){

    for(i = 0; i < nN; i++){
      for(k = 0; k < ptBestCluster->nK - 1; k++){
	fprintf(ofp,"%f,",ptBestCluster->aadZ[i][k]);
      }
      fprintf(ofp,"%f\n",ptBestCluster->aadZ[i][ptBestCluster->nK - 1]);
    }

    fclose(ofp);
  }
  else{
    fprintf(stderr,"Failed openining %s in main\n", szOFile);
    fflush(stderr);
  }

  sprintf(szOFile,"%svbl.csv",tParams.szOutFileStub);

  ofp = fopen(szOFile,"w");
  if(ofp){
    fprintf(ofp,"%d,%f,%d\n",ptBestCluster->nK,ptBestCluster->dVBL,ptBestCluster->nThread);

    fclose(ofp);
  }
  else{
    fprintf(stderr,"Failed openining %s in main\n", szOFile);
    fflush(stderr);
  }

  /*free up memory in data object*/
  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);

  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->szInputFile)
    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;
	}
      }
    }
  }
  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);
      }
    }
  }
  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;
}

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;
  FILE   *ofp = NULL;

  if(ptCluster->szCOutFile){
	ofp = fopen(ptCluster->szCOutFile,"w");
	if(!ofp){
		fprintf(stderr, "Failed to open file %s in gmmTrainVB\n",ptCluster->szCOutFile);
		fflush(stderr);
	}
  }

  /*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);

    if(ofp){
    	fprintf(ofp,"%d,%f,%f,",nIter, ptCluster->dVBL, dDelta);
    	for(k = 0; k < nK-1; k++){
    		fprintf(ofp,"%f,",ptCluster->adPi[k]);
    	}
    	fprintf(ofp,"%f\n",ptCluster->adPi[nK - 1]);
	fflush(ofp);
    }
    nIter++;
  }

  if(ofp){
	fclose(ofp);
  }

  /*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]);
    }
  }
  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]);
    }
  }
  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));
    }
  }
  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;

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

  gsl_rng_set (ptGSLRNG, ptCluster->lSeed);

  initKMeans(ptGSLRNG, ptCluster, ptCluster->ptData);

  gmmTrainVB(ptCluster, ptCluster->ptData);

  gsl_rng_free(ptGSLRNG);

  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         iret[N_RTHREADS];
  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;
    iret[r] = pthread_create(&atRestarts[r], NULL, fitEM, (void*) aptCluster[r]);
  }

  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;
    }
  }

  *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);
}


#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_vector_double.h>

#ifndef UTILS_H
#define UTILS_H
#define max
int get_nlines(char *);
void print_gsl_vector(gsl_vector *);
void print_intarray(int *, int);
gsl_vector *vector_sub(gsl_vector *, gsl_vector *);
double vector_dist(gsl_vector *, gsl_vector *);
gsl_vector **load_vectors_from_file(char *, int);
int **permutations(int);
int factorial(int);
double min(double, double);
void crit_err(char *);

#endif


#ifndef FUSED_LASSO_B_H
#define FUSED_LASSO_B_H

#include "ALM_Katyusha.h"
#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <math.h>

//This class solves problem: min_{x} sum_i 0.5*(Ax- b)^2+ sum_i |x^i- x^{i+1}|+ P(x) by IPALM_Katyusha
// where P(x)=frac{sig2}{2}|x|_2^2 +sig1|x|_1.

// f^i(x)= 0.5*(x- b_i)^2, h^i(x)= |x|, Mx= (x^1- x^2, ..., x_n- x_1)

template<typename L, typename D>
class Fused_lasso_b: public ALM_Katyusha<L, D>
{
private:

	D lambda3;

	Matrix<L,D> my_M;

	Matrix<L,D> my_A;

	D val_lambda_f;

protected:

public:

	D sig1;
	D sig2;

	Fused_lasso_b(const char* Matrix_file, D val_lambda1, D val_lambda2, D val_lambda3)
	:ALM_Katyusha<L,D>(),my_A(Matrix_file)
	{
		my_M.construct_fused_matrix(my_A);
		this->matrix_merge(my_M,my_A);
		sig1=val_lambda1;
		sig2=val_lambda2;
		lambda3=val_lambda3;
		this->L_h= val_lambda3;
		this->mu_g= sig2;
	}

	L get_n(){return my_A.nfeatures;}
	L get_m(){return my_A.nfeatures+ my_A.nsamples;}

	inline void set_matrix_A(){
		this->data_A= my_A;
	}

	inline void set_matrix_M(){
		this->data_M= my_M;
	}

	inline D value_of_f_j(D x, L i){
		return 0.5*(x- my_A.b[i])*(x- my_A.b[i]);
	}

	inline D gradient_of_f_j(D x, L i){
		return x- my_A.b[i];
	}

	inline D value_of_f_star_j(D x, L i){
		return 0.5*x*x+ my_A.b[i]*x;
	}

	inline void rescale(){
		this->lambda1= sig1;
		this->lambda2= sig2+ this->tau_s*this->beta_s;
	}

	inline D value_of_P_j(D x, L j){
		return sig1*fabs(x)+ sig2/2*x*x;
	}

	inline D prox_of_P_j(D x1, D x2, L j){
		if (x1*x2- sig1> 0){
			return (x1*x2- sig1)/(sig2+ x2);
		}
		else if(x1*x2+ sig1< 0){
			return (x1*x2+ sig1)/(sig2+ x2);
		}
		else{
			return 0;
		}
	}

	inline D value_of_h_j(D x, L i){
		return lambda3*fabs(x);
	}

	inline D value_of_h_star_j(D x, L j){
		if (x<= 2*lambda3 && x>= -2*lambda3){
			return 0;
		}
		else{
			cout<< "error in h*(x)"<< endl;
			cout<< "lambda3= "<< lambda3<< "; x= "<< x<< endl;
			system("pause");
			return std::numeric_limits<double>::max();
		}
	}

	inline D compute_one_step(D tau, D u, D x){
		D new_x;
		if(x>tau*(this->lambda1+u))
		new_x=(x-tau*(this->lambda1+u))/(1+this->lambda2*tau);
		else if(x<tau*(u-this->lambda1))
		new_x=(x-tau*(u-this->lambda1))/(1+this->lambda2*tau);
		else
		new_x=0;
		return new_x;
	}

	inline D prox_of_h_star_j(D x1, D x2, L j){
		if (x1> lambda3){
			return lambda3;
		}
		else if (x1< -lambda3){
			return -lambda3;
		}
		else{
			return x1;
		}
	}

	inline D feasible_dual(vector<D> & x){

		if(this->lambda2>0)
		{
			return 1;
		}
		else
		{
			D scal=1;
			L l=x.size();
			for(L i=0;i<l;i++)
			if(fabs(x[i])>this->lambda1)
			scal=min(this->lambda1/fabs(x[i]),scal);
			return scal;
		}
	}

	//We implement Section 6.2 of SPDC paper for 'just-in-time' update
	//t0 is t0+1 in the paper SPDC ; x is the z vector in Katyusha
	inline void compute_just_in_time_prox_grad_without_x_s(D tau, D u, D &x, L t0,L t1, D w, D &y, L j){
		if(t0==t1)
		return;
		else{
			if(this->lambda2>0)
			{
				D theta=1./(1+this->lambda2*tau);
				D hplus=(u+this->lambda1)/this->lambda2;
				D hminus=(u-this->lambda1)/this->lambda2;
				D p=pow(theta,t1-t0);
				if(this->lambda1==0){
					y=compute_aid_2(p,theta, hplus, t1-t0, x, y, w);
					x=p*(x)-(1-p)*u/this->lambda2;
				}else{
					if(x==0){
						if(this->lambda1+u<0){
							y=compute_aid_2(p,theta, hplus, t1-t0, x, y, w);
							x=p*x-(1-p)*hplus;
						}
						else if(u-this->lambda1>0){
							y=compute_aid_2(p,theta, hminus, t1-t0, x, y, w);
							x=p*x-(1-p)*hminus;
						}else{
							x=0;
							D p2=pow(this->theta3,t1-t0);
							D tmp2=(1-p2)/(this->theta1+this->theta2);
							y=(this->theta2*w)*tmp2+p2*y;
						}
					}else if(x>0){
						if(this->lambda1+u<=0){
							y=compute_aid_2(p,theta, hplus, t1-t0, x, y, w);
							x=p*x-(1-p)*hplus;
						}else{
							D t=t0-log(1+this->lambda2*x/(this->lambda1+u))/log(theta);
							if(t<t1){
								L t_tmp=floor(t);
								p=pow(theta,t_tmp-t0);
								y=compute_aid_2(p,theta, hplus, t_tmp-t0, x, y, w);
								x=p*x-(1-p)*hplus;
								x=compute_one_step(tau,u,x);
								y=this->theta1*x+this->theta2*w+this->theta3*y;
								compute_just_in_time_prox_grad_without_x_s(tau, u,  x,  t_tmp+1, t1, w, y, j);
							}
							else{
								y=compute_aid_2(p,theta, hplus, t1-t0, x, y, w);
								x=p*x-(1-p)*hplus;
							}
						}

					}else if(x<0){
						D minusw=-w;
						D minusx=-x;
						D minusy=-y;
						compute_just_in_time_prox_grad_without_x_s(tau, -u,  minusx,  t0, t1,minusw, minusy, j);
						x=-minusx;
						y=-minusy;
					}

				}

			}
			else{

				if(this->lambda1==0){
					y=compute_aid(tau*u, t1-t0, x, y, w);
					x=x-(t1-t0)*tau*u;
				}
				else{
					if(x==0){
						if(this->lambda1+u<0){
							y=compute_aid(tau*(this->lambda1+u), t1-t0, x, y, w);
							x=x-(t1-t0)*tau*(this->lambda1+u);
						}
						else if(u-this->lambda1>0){
							y=compute_aid(tau*(-this->lambda1+u), t1-t0, x, y, w);
							x=x-(t1-t0)*tau*(u-this->lambda1);
						}else{
							D p2=pow(this->theta3,t1-t0);
							D tmp2=(1-p2)/(this->theta1+this->theta2);
							y=(this->theta2*w)*tmp2+p2*y;
							x=0;
						}
					}else if(x>0){
						if(this->lambda1+u<=0){
							y=compute_aid(tau*(this->lambda1+u), t1-t0, x, y, w);
							x=x-(t1-t0)*tau*(this->lambda1+u);
						}else{
							D t=t0+x/(this->lambda1+u);
							if(t<t1){
								L t_tmp=floor(t);
								y=compute_aid(tau*(this->lambda1+u), t_tmp-t0, x, y, w);
								x=x-(t_tmp-t0)*tau*(this->lambda1+u);
								x=compute_one_step(tau,u,x);
								y=this->theta1*x+this->theta2*w+this->theta3*y;
								compute_just_in_time_prox_grad_without_x_s(tau, u,  x,  t_tmp+1, t1, w, y, j);
							}
							else{
								y=compute_aid(tau*(this->lambda1+u), t1-t0, x, y, w);
								x=x-(t1-t0)*tau*(this->lambda1+u);
							}
						}

					}else if(x<0){
						D minusw=-w;
						D minusx=-x;
						D minusy=-y;
						compute_just_in_time_prox_grad_without_x_s(tau, -u,  minusx,  t0, t1,minusw, minusy, j);
						x=-minusx;
						y=-minusy;
					}

				}
			}
		}

	};

	// compute_aid returns the value of y_{k+s} given by: y_{k+1}=theta1*z_{k+1}+theta2*w+(1-theta1-theta2)*y_k; z_{k+i}=z_k-i*h
	D compute_aid(D h, D s, D x, D y, D w){
		D p2=pow(this->theta3,s);
		D tmp2=(1-p2)/(this->theta1+this->theta2);
		D tmp3=(tmp2*this->theta3-p2*s)/(this->theta1+this->theta2);
		y=this->theta1*(tmp2*(x-s*h)+h*tmp3)+this->theta2*w*tmp2+p2*y;
		return y;
	}

	// compute_aid_2 returns the value of y_{k+s} given by: y_{k+1}=theta1*z_{k+1}+theta2*w+(1-theta1-theta2)*y_k; z_{k+i}=q^i(z_k+h)-h
	//p=q^s;
	D compute_aid_2(D p, D q, D h, D s, D x, D y, D w){
		D theta3overq=this->theta3/q;
		D p2=pow(this->theta3,s);
		D p3=pow(theta3overq,s);
		D tmp=(1-p3)/(1-theta3overq);
		D tmp2=(1-p2)/(this->theta1+this->theta2);
		y=this->theta1*p*(x+h)*tmp+(this->theta2*w-this->theta1*h)*tmp2+p2*y;
		return y;
	}

	void Katyusha_solver(D beta_0, D epsilon_0,  D eta , D rho, vector<D> & x0,vector<D> & y0,L val_tau, L max_nb_outer, L p_N_1, L p_N_2,string filename1, string filename2, D time){
		this->ALM_solve_with_L_Katyusha(beta_0, epsilon_0,  eta, rho, x0,y0, val_tau, max_nb_outer,  p_N_1,  p_N_2,  filename1,  filename2, time);
	}

};

#endif


/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#ifndef utility_77d83a8d_bf54_4fbe_ab3c_b06d0be9e40e_h
#define utility_77d83a8d_bf54_4fbe_ab3c_b06d0be9e40e_h

#include <gslib/std.h>
#include <gslib/type.h>

/*
 * This file include a series of intersection operations.
 * Here are all the details:
 *                      point       ratio
 * linear - linear:     available   available
 * linear - quad:       none        available
 * linear - cubic:      none        available
 * quad - quad:         none        available
 * quad - cubic:        available   none
 * cubic - cubic:       available   none
 * To convert from ratio to point was quite simple, this was why I suggest ratio first.
 * other cases limited by the algorithm where the point was the first result, I hope you
 * notice this and use certain methods to get your result, so I didn't offer an altered
 * way, but you can simply achieve that by calling the reparameterize functions.
 * Caution that the precision of intersection may cause loss of roots,
 * set tolerance around 0.2 - 0.3 might be good.
 */

__gslib_begin__

gs_export extern void linear_interpolate(vec2 c[], const vec2& p1, const vec2& p2, int step);
gs_export extern void quadratic_interpolate(vec2 c[], const vec2& p1, const vec2& p2, const vec2& p3, int step);
gs_export extern void cubic_interpolate(vec2 c[], const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4, int step);
gs_export extern void hermite_interpolate(vec2 c[], const vec2& p1, const vec2& p2, const vec2& t1, const vec2& t2, int step);
gs_export extern float linear_reparameterize(const vec2& p1, const vec2& p2, const vec2& p);
gs_export extern float quad_reparameterize(const vec3 para[2], const vec2& p);
gs_export extern float cubic_reparameterize(const vec4 para[2], const vec2& p);
gs_export extern float best_quad_reparameterize(const vec3 para[2], const vec2& p);
gs_export extern float best_cubic_reparameterize(const vec4 para[2], const vec2& p);
gs_export extern bool is_concave_angle(const vec2& p1, const vec2& p2, const vec2& p3);
gs_export extern bool is_concave_angle(const vec2& p1, const vec2& p2, const vec2& p3, bool cw);
gs_export extern bool is_approx_line(const vec2& p1, const vec2& p2, const vec2& p3, float tolerance);
gs_export extern void get_linear_coefficient(vec3& coef, const vec2& p, const vec2& d);
gs_export extern void get_linear_parameter_equation(vec2 para[2], const vec2& a, const vec2& b);
gs_export extern void get_quad_parameter_equation(vec3 para[2], const vec2& a, const vec2& b, const vec2& c);
gs_export extern void get_cubic_parameter_equation(vec4 para[2], const vec2& a, const vec2& b, const vec2& c, const vec2& d);
gs_export extern void get_first_derivate_factor(vec2 df1[2], const vec3 para[2]);
gs_export extern void get_first_derivate_factor(vec3 df1[2], const vec4 para[2]);
gs_export extern void get_second_derivate_factor(vec2 df2[2], const vec4 para[2]);
gs_export extern void get_first_derivate_factor(vec3 df1[2], const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4);
gs_export extern void get_second_derivate_factor(vec2 df2[2], const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4);
gs_export extern void get_first_derivate(vec2& d, const vec3 df1[2], float t);
gs_export extern void get_second_derivate(vec2& d, const vec2 df2[2], float t);
gs_export extern float get_cubic_curvature(const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4, float t);
gs_export extern int solve_univariate_quadratic(float t[2], const vec3& coef);
gs_export extern int solve_univariate_cubic(float t[3], const vec4& coef);
gs_export extern int solve_univariate_quartic(float t[4], const float coef[5]);
gs_export extern int get_cubic_inflection(float t[2], const vec2& a, const vec2& b, const vec2& c, const vec2& d);
gs_export extern int get_cubic_inflection(float t[2], const vec3 ff[2], const vec2 sf[2]);
gs_export extern int get_quad_extrema(float t[], const vec2& ff);
gs_export extern int get_cubic_extrema(float t[2], const vec3& ff);
gs_export extern void get_quad_bound_box(rectf& rc, const vec2& p1, const vec2& p2, const vec2& p3);
gs_export extern void get_cubic_bound_box(rectf& rc, const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4);
gs_export extern int intersection_quad_linear(float t[2], const vec3 quad[2], const vec3& linear);
gs_export extern int intersection_cubic_linear(float t[3], const vec4 cubic[2], const vec3& linear);
gs_export extern int intersection_quad_quad(float ts[4][2], const vec3 quad1[2], const vec3 quad2[2]);
gs_export extern void intersectp_linear_linear(vec2& ip, const vec2& p1, const vec2& p2, const vec2& d1, const vec2& d2);
gs_export extern int intersectp_cubic_quad(vec2 ip[6], const vec2 cp1[4], const vec2 cp2[3], float tolerance);
gs_export extern int intersectp_cubic_cubic(vec2 ip[9], const vec2 cp1[4], const vec2 cp2[4], float tolerance);
gs_export extern bool get_self_intersection(float ts[2], const vec2& a, const vec2& b, const vec2& c, const vec2& d);
gs_export extern void split_quad_bezier(vec2 c[5], const vec2 p[3], float t);
gs_export extern void split_quad_bezier(vec2 c[7], const vec2 p[3], float t1, float t2);
gs_export extern void split_cubic_bezier(vec2 c[7], const vec2 p[4], float t);
gs_export extern void split_cubic_bezier(vec2 c[10], const vec2 p[4], float t1, float t2);
gs_export extern void offset_cubic_bezier(vec2 c[4], const vec2 p[4], float d);
gs_export extern float quad_bezier_length(const vec2 cp[3]);
gs_export extern float cubic_bezier_length(const vec2 cp[4], float tolerance);
gs_export extern int cubic_to_quad_bezier(vector<vec2>& quadctl, const vec2 cp[4], float tolerance);
gs_export extern float quad_control_length(const vec2& a, const vec2& b, const vec2& c);
gs_export extern float cubic_control_length(const vec2& a, const vec2& b, const vec2& c, const vec2& d);
gs_export extern float point_line_distance(const vec2& p, const vec2& p1, const vec2& p2);
gs_export extern int get_interpolate_step(const vec2& a, const vec2& b, const vec2& c, float step_len = -1.f);
gs_export extern int get_interpolate_step(const vec2& a, const vec2& b, const vec2& c, const vec2& d, float step_len = -1.f);
gs_export extern int get_rough_interpolate_step(const vec2& a, const vec2& b, const vec2& c);
gs_export extern int get_rough_interpolate_step(const vec2& a, const vec2& b, const vec2& c, const vec2& d);
gs_export extern float get_cubic_klmcoords(vec3 m[4], const vec2& p1, const vec2& p2, const vec2& p3, const vec2& p4,
    const vec2& ac1, const vec2& ac2, const vec2& ac3, const vec2& ac4  /* absolute coord for error control */
    );
gs_export extern float get_include_angle(const vec2& d1, const vec2& d2);
gs_export extern float get_include_angle(const vec2& a, const vec2& b, const vec2& c);
gs_export extern float get_rotate_angle(const vec2& d1, const vec2& d2);
gs_export extern float get_rotate_angle(const vec2& a, const vec2& b, const vec2& c);
gs_export extern void get_reduce_point(vec2& p, const vec2& a, const vec2& b, const vec2& c, const vec2& d);
gs_export extern bool point_in_triangle(const vec2& p, const vec2& p1, const vec2& p2, const vec2& p3);
gs_export extern int point_in_polygon(const vec2& p, const vector<vec2>& poly);   /* 0: outside; 1 : inside; -1 : coincide */
gs_export extern float get_triangle_area(const vec2& p1, const vec2& p2, const vec2& p3);
gs_export extern vec3 get_barycentric_coords(const vec2& p, const vec2& p1, const vec2& p2, const vec2& p3);
gs_export extern bool clip_line_rect(float t[2], const vec2& p1, const vec2& p2, const rect& clipbox);
gs_export extern bool clip_line_rectf(float t[2], const vec2& p1, const vec2& p2, const rectf& clipbox);
gs_export extern void trace_quad_strip(const vec2 cp[], int size);
gs_export extern void trace_cubic_strip(const vec2 cp[], int size);

inline bool is_isosigned(float a, float b) { return (int)((*(uint*)&a ^ *(uint*)&b)) >= 0; }
inline bool is_isosigned(double a, double b) { return (long long)((*(unsigned long long*)&a ^ *(unsigned long long*)&b)) >= 0; }
inline bool fuzzy_zero(float f) { return (f <= 1e-4f && f >= -1e-4f); }
inline bool fuzzy_zero(float f, float tol) { return f <= tol && f >= -tol; }
inline bool fuzzy_between(float a, float b, float c, float tol) { return b > a - tol && b < c + tol; }
inline bool fuzzy_greater_inclusive(float m, float n, float tol) { return m > (n - tol); }
inline bool fuzzy_less_inclusive(float m, float n, float tol) { return m < (n + tol); }
inline bool fuzzy_greater_exclusive(float m, float n, float tol) { return m > (n + tol); }
inline bool fuzzy_less_exclusive(float m, float n, float tol) { return m < (n - tol); }
inline bool is_parallel(const vec2& a, const vec2& b) { return fuzzy_zero(a.ccw(b)); }

inline vec2& eval_linear(vec2& v, const vec2 para[2], float t)
{
    vec2 sv(t, 1.f);
    v.x = para[0].dot(sv);
    v.y = para[1].dot(sv);
    return v;
}

inline vec2& eval_quad(vec2& v, const vec3 para[2], float t)
{
    vec3 sv(t * t, t , 1.f);
    v.x = para[0].dot(sv);
    v.y = para[1].dot(sv);
    return v;
}

inline vec2& eval_cubic(vec2& v, const vec4 para[2], float t)
{
    vec4 sv(t * t * t, t * t, t, 1.f);
    v.x = para[0].dot(sv);
    v.y = para[1].dot(sv);
    return v;
}

__gslib_end__

#endif


#include <stdio.h>
#include <gsl/gsl_cblas.h>

int
main ()
{
  int lda = 3;

  float A[] = { 0.11, 0.12, 0.13,
                0.21, 0.22, 0.23 };

  int ldb = 2;

  float B[] = { 1011, 1012,
                1021, 1022,
                1031, 1032 };

  int ldc = 2;

  float C[] = { 0.00, 0.00,
                0.00, 0.00 };

  /* Compute C = A B */

  cblas_sgemm (CblasRowMajor, CblasNoTrans, CblasNoTrans, 2, 2, 3,
               1.0, A, lda, B, ldb, 0.0, C, ldc);

  printf("[ %g, %g\n", C[0], C[1]);
  printf("  %g, %g ]\n", C[2], C[3]);

}


// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#pragma once

#include <units/bits/external/hacks.h>
#include <units/bits/math_concepts.h>
#include <units/bits/ratio_maths.h>
#include <units/bits/pow.h>
#include <units/bits/root.h>
#include <cstdint>
#include <numeric>
#include <type_traits>
#include <tuple>
#include <gsl/gsl-lite.hpp>

namespace units {

struct ratio;
constexpr ratio inverse(const ratio& r);

/**
 * @brief Provides compile-time rational arithmetic support.
 *
 * This class is really similar to @c std::ratio but gets an additional `Exp`
 * template parameter that defines the exponent of the ratio. Another important
 * difference is the fact that the objects of that class are used as class NTTPs
 * rather then a type template parameter kind.
 */
struct ratio {
  std::intmax_t num;
  std::intmax_t den;
  std::intmax_t exp;

  constexpr explicit ratio(std::intmax_t n, std::intmax_t d = 1, std::intmax_t e = 0): num(n), den(d), exp(e)
  {
    gsl_Expects(den != 0);
    detail::normalize(num, den, exp);
  }

  [[nodiscard]] friend constexpr bool operator==(const ratio&, const ratio&) = default;

  [[nodiscard]] friend constexpr ratio operator*(const ratio& lhs, const ratio& rhs)
  {
    const std::intmax_t gcd1 = std::gcd(lhs.num, rhs.den);
    const std::intmax_t gcd2 = std::gcd(rhs.num, lhs.den);
    return ratio(detail::safe_multiply(lhs.num / gcd1, rhs.num / gcd2),
                 detail::safe_multiply(lhs.den / gcd2, rhs.den / gcd1),
                 lhs.exp + rhs.exp);
  }

  [[nodiscard]] friend constexpr ratio operator/(const ratio& lhs, const ratio& rhs)
  {
    return lhs * inverse(rhs);
  }
};

[[nodiscard]] constexpr ratio inverse(const ratio& r)
{
  return ratio(r.den, r.num, -r.exp);
}

[[nodiscard]] constexpr bool is_integral(const ratio& r)
{
  if(r.exp < 0) {
    return false;
  } else {
    return detail::gcdpow(r.num, r.exp, r.den) == r.den;
  }
}

namespace detail {

[[nodiscard]] constexpr auto make_exp_align(const ratio& r, std::intmax_t alignment)
{
  gsl_Expects(alignment > 0);
  const std::intmax_t rem = r.exp % alignment;

  if (rem == 0) {  // already aligned
    return std::array{r.num, r.den, r.exp};
  }

  if (r.exp > 0) {  // remainder is positive
    return std::array{r.num * ipow10(rem), r.den, r.exp - rem};
  }

  // remainder is negative
  return std::array{r.num, r.den * ipow10(-rem), r.exp - rem};
}

template<std::intmax_t N>
  requires gt_zero<N>
[[nodiscard]] constexpr ratio root(const ratio& r)
{
  if constexpr (N == 1) {
    return r;
  } else {
    if (r.num == 0) {
      return ratio(0);
    }

    const auto aligned = make_exp_align(r, N);
    return ratio(iroot<N>(aligned[0]), iroot<N>(aligned[1]), aligned[2] / N);
  }
}

}  // namespace detail

template<std::intmax_t Num, std::intmax_t Den = 1>
  requires detail::non_zero<Den>
[[nodiscard]] constexpr ratio pow(const ratio& r)
{
  if constexpr (Num == 0) {
    return ratio(1);
  } else if constexpr (Num == Den) {
    return r;
  } else {
    // simplify factors first and compute power for positive exponent
    constexpr std::intmax_t gcd = std::gcd(Num, Den);
    constexpr std::intmax_t num = detail::abs(Num / gcd);
    constexpr std::intmax_t den = detail::abs(Den / gcd);

    // integer root loses precision so do pow first
    const ratio result = detail::root<den>(detail::pow_impl<num>(r));

    if constexpr (Num * Den < 0) {  // account for negative exponent
      return inverse(result);
    } else {
      return result;
    }
  }
}

[[nodiscard]] constexpr ratio sqrt(const ratio& r) { return pow<1, 2>(r); }

[[nodiscard]] constexpr ratio cbrt(const ratio& r) { return pow<1, 3>(r); }

// common_ratio
[[nodiscard]] constexpr ratio common_ratio(const ratio& r1, const ratio& r2)
{
  const auto res = detail::gcd_frac(r1.num, r1.den, r1.exp, r2.num, r2.den, r2.exp);
  return ratio(res[0], res[1], res[2]);
}

}  // namespace units


// -*- C++ -*-
//
// michael a.g. aïvázis <michael.aivazis@para-sim.com>
//
// (c) 2013-2020 parasim inc
// (c) 2010-2020 california institute of technology
// all rights reserved
//

// code guard
#if !defined(altar_bayesian_COV_h)
#define altar_bayesian_COV_h

// externals
#include <gsl/gsl_rng.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

// place everything in the local namespace
namespace altar {
    namespace bayesian {

        // forward declarations
        class COV;
        class CoolingStep;

    } // of namespace bayesian
} // of namespace altar

// declaration
class altar::bayesian::COV
{
    // types
public:
    typedef CoolingStep state_t;

    typedef gsl_rng rng_t;
    typedef gsl_vector vector_t;
    typedef gsl_matrix matrix_t;

    // accessors
public:
    inline auto cov() const;
    inline auto beta() const;

    // interface
public:
    virtual double dbeta_brent(vector_t * llk, double llkMedian, vector_t * w);
    virtual double dbeta_grid(vector_t * llk, double llkMedian, vector_t * w);

    // meta-methods
public:
    virtual ~COV();
    inline COV(rng_t * rng, double tolerance=.001, size_t maxIterations=1000, double target=1.0);

    // data
private:
    const double _betaMin;
    const double _betaMax;

    rng_t * _rng;
    double _tolerance;
    size_t _maxIterations;
    double  _target;
    double _beta, _cov;

    // disallow
private:
    COV(const COV &) = delete;
    const COV & operator=(const COV &) = delete;
};

// get the inline definitions
#define altar_bayesian_COV_icc
#include "COV.icc"
#undef altar_bayesian_COV_icc

# endif
// end of file


#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_integration.h>

#include "testint.c"

gsl_integration_workspace *w;
int inverse;

double
cauchy (double x, void *p)
{
  return gsl_ran_cauchy_pdf (x, *(double*)p);
}

double
gaussian (double x, void *p)
{
  return gsl_ran_gaussian_pdf (x, *(double*)p);
}

double
laplace (double x, void *p)
{
  return gsl_ran_laplace_pdf (x, *(double*)p);
}

double
rayleigh (double x, void *p)
{
  return gsl_ran_rayleigh_pdf (x, *(double*)p);
}

double
flat (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_flat_pdf (x, c[0], c[1]);
}

double
lognormal (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_lognormal_pdf (x, c[0], c[1]);
}

double
gamma (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_gamma_pdf (x, c[0], c[1]);
}

double
chisq (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_chisq_pdf (x, c[0]);
}

double
fdist (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_fdist_pdf (x, c[0], c[1]);
}

double
tdist (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_tdist_pdf (x, c[0]);
}

double
beta (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_beta_pdf (x, c[0], c[1]);
}

double
gumbel1 (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_gumbel1_pdf (x, c[0], c[1]);
}

double
gumbel2 (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_gumbel2_pdf (x, c[0], c[1]);
}

double
weibull (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_weibull_pdf (x, c[0], c[1]);
}

double
pareto (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_pareto_pdf (x, c[0], c[1]);
}

double
logistic (double x, void *p)
{
  double * c = (double *)p;
  return gsl_ran_logistic_pdf (x, c[0]);
}

double
integrate (gsl_function * f, double a, double b)
{
  double res = 0, err = 0;
  double tol = 1e-12;
  gsl_integration_qag (f, a, b, 0, tol, 1000, 0,  w, &res, &err);
  return res;
}

double
integrate_lower (gsl_function * f, double b)
{
  double res = 0, err = 0;
  qagil (f, b, 0, 1e-12, 1000, w, &res, &err);
  return res;
}

double
integrate_upper (gsl_function * f, double a)
{
  double res = 0, err = 0;
  qagiu (f, a, 0, 1e-12, 1000, w, &res, &err);
  return res;
}

int
test (const char * name, gsl_function * f, double x[], int N, char *fmt)
{
  int i;
  double res, err, sum = 0, sumerr = 0;

  printf ("void test_auto_%s (void);\n\n", name);

  printf ("void\ntest_auto_%s (void)\n{\n", name);

  /* gsl_set_error_handler_off(); */

  w = gsl_integration_workspace_alloc (1000);

  for (i = 0; i < N; i++)
    {
      res = 0;
      err = 0;

      if (x[0] < -1000)
        {
          if (x[i] < 0)
            {
              res = integrate_lower (f, x[i]);
            }
          else
            {
              res = 1 - integrate_upper (f, x[i]);
            }
          sum = res;
        }
      else
        {
          if (i == 0)
            sum += 0;
          else
            sum += integrate(f, x[i-1], x[i]);
        }

      if (res < 0)
        continue;

      printf (fmt, "_P", x[i], sum);

      if (inverse && (sum != 0 && sum != 1) && (x[i] == 0 || sum * 1e-4 < GSL_FN_EVAL(f,x[i]) * fabs(x[i])))
        printf (fmt, "_Pinv", sum, x[i]);
    }

  printf("\n");

  sum=0;
  sumerr=0;

  for (i = N-1; i >= 0; i--)
    {
      res = 0;
      err = 0;

      if (x[N-1] > 1000)
        {
          if (x[i] > 0)
            {
              res = integrate_upper (f, x[i]);
            }
          else
            {
              res = 1-integrate_lower (f, x[i]);
            }

          sum = res;
        }
      else
        {
          if (i == N-1)
            sum += 0;
          else
            sum += integrate(f, x[i], x[i+1]);
        }

      printf (fmt, "_Q", x[i], sum);

      if (inverse && (sum != 0 && sum != 1) && (x[i] == 0 || sum * 1e-4 < GSL_FN_EVAL(f,x[i]) * fabs(x[i])))
        printf (fmt, "_Qinv", sum, x[i]);
    }

  printf ("}\n\n");

  gsl_integration_workspace_free (w);

}

int
main (void)
{
  const int N = 43;

  double xall[] = {
    -1e10, -1e9, -1e8, -1e7, -1e6, -1e5, -1e4, -1e3, -1e2, -1e1,
    -1,
    -1e-1, -1e-2, -1e-3, -1e-4, -1e-5, -1e-6, -1e-7, -1e-8, -1e-9, -1e-10,
    0,
    1e-10, 1e-9, 1e-8, 1e-7, 1e-6, 1e-5, 1e-4, 1e-3, 1e-2, 1e-1,
    1,
    1e1, 1e2, 1e3, 1e4, 1e5, 1e6, 1e7, 1e8, 1e9, 1e10
  };

  const int Npos = 22;

  double xpos[] = {
    0,
    1e-10, 1e-9, 1e-8, 1e-7, 1e-6, 1e-5, 1e-4, 1e-3, 1e-2, 1e-1,
    1,
    1e1, 1e2, 1e3, 1e4, 1e5, 1e6, 1e7, 1e8, 1e9, 1e10
  };

  const int N01 = 23;

  double x01[] = {
    0,
    1e-10, 1e-9, 1e-8, 1e-7, 1e-6, 1e-5, 1e-4, 1e-3, 1e-2, 1e-1,
    0.2, 0.3, 0.4, 0.5, 0.6, 0.8, 0.9,
    0.99, 0.999, 0.9999, 0.99999,
    1.0
  };

#define TEST(name,params,range,n) { double p[] = { params } ; gsl_function f = {&name, p}; test(#name, &f, range, n, "    TEST(gsl_cdf_" #name "%s, (%.16e," #params "), %.12e, TEST_TOL6);\n"); }

#define TEST2(name,p1,p2,range,n) { double p[] = { p1,p2 } ; gsl_function f = {&name, p}; test(#name, &f, range, n, "    TEST(gsl_cdf_" #name "%s, (%.16e," #p1 "," #p2 "), %.12e, TEST_TOL6);\n"); }

#define TEST2A(desc,name,p1,p2,range,n) { double p[] = { p1,p2 } ; gsl_function f = {&name, p}; test(#desc, &f, range, n, "    TEST(gsl_cdf_" #name "%s, (%.16e," #p1 "," #p2 "), %.12e, TEST_TOL6);\n"); }

  inverse = 0;
  TEST2(beta, 1.3, 2.7, x01, N01);
  TEST2(fdist, 5.3, 2.7, xpos, Npos);

  inverse = 1;
  TEST(cauchy, 1.3, xall, N);
  TEST(gaussian, 1.3, xall, N);
  TEST(laplace, 1.3, xall, N);
  TEST(rayleigh, 1.3, xpos, Npos);

  TEST2(flat, 1.3, 750.0, xpos, Npos);
  TEST2(lognormal, 1.3,2.7, xpos, Npos);
  TEST2(gamma, 1.3,2.7, xpos, Npos);

  TEST(chisq, 1.3, xpos, Npos);
  TEST(tdist, 1.3, xall, N);

  TEST2(gumbel1, 1.3, 2.7, xall, N);
  TEST2(gumbel2, 1.3, 2.7, xpos, Npos);
  TEST2(weibull, 1.3, 2.7, xpos, Npos);

  TEST2(pareto, 1.3, 2.7, xpos, Npos);
  TEST(logistic, 1.3, xall, N);

  TEST2A(gammalarge, gamma, 1.3,123.0, xpos, Npos);
}


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

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_sf.h>

#include "test_sf.h"

#define WKB_TOL (1.0e+04 * TEST_SQRT_TOL0)

/*
 * Test the identities:
 *
 * Sum_{k=0}^n (n choose k) (2 y)^{n - k} H_k(x) = H_n(x + y)
 *
 * and
 *
 * Sum_{k=0}^n (n choose k) y^{n-k} He_k(x) = He_n(x + y)
 *
 * see: http://mathworld.wolfram.com/HermitePolynomial.html (Eq. 55)
 */
void
test_hermite_id1(const int n, const double x, const double y)
{
  double *a = malloc((n + 1) * sizeof(double));
  double *b = malloc((n + 1) * sizeof(double));
  double lhs, rhs;
  int k;

  a[0] = gsl_pow_int(2.0 * y, n);
  b[0] = gsl_pow_int(y, n);
  for (k = 1; k <= n; ++k)
    {
      double fac = (n - k + 1.0) / (k * y);
      a[k] = 0.5 * fac * a[k - 1];
      b[k] = fac * b[k - 1];
    }

  lhs = gsl_sf_hermite_series(n, x, a);
  rhs = gsl_sf_hermite(n, x + y);
  gsl_test_rel(lhs, rhs, TEST_TOL4, "identity1 phys n=%d x=%g y=%g", n, x, y);

  lhs = gsl_sf_hermite_prob_series(n, x, b);
  rhs = gsl_sf_hermite_prob(n, x + y);
  gsl_test_rel(lhs, rhs, TEST_TOL3, "identity1 prob n=%d x=%g y=%g", n, x, y);

  free(a);
  free(b);
}

int
test_hermite(void)
{
  gsl_sf_result r;

  int s = 0;
  int m, n, sa;
  double res[256];
  double x;
  const double aizero1 = -2.3381074104597670384891972524467; /* first zero of the Airy function Ai */

  /* test some known identities */
  test_hermite_id1(10, 0.75, 0.33);
  test_hermite_id1(8, -0.75, 1.20);
  test_hermite_id1(7, 2.88, -3.2);

  TEST_SF(s, gsl_sf_hermite_prob_e, (0, 0.75, &r),  1.,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_prob_e, (1, 0.75, &r),  0.75,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_prob_e, (25, 0., &r),  0.,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_prob_e, (28, 0., &r),  2.13458046676875e14,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_prob_e, (30, 0., &r), -6.190283353629375e15,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_prob_e, (25, 0.75, &r),  -1.08128685847680748265939328423e12,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_prob_e, (28, 0.75, &r),  -1.60620252094658918105511125135e14,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF_RETURN(s, gsl_sf_hermite_prob_e, (2800, 0.75, &r), GSL_EOVRFLW);
  TEST_SF_RETURN(s, gsl_sf_hermite_prob_e, (2800, 1.75, &r), GSL_EOVRFLW);

  TEST_SF(s, gsl_sf_hermite_prob_deriv_e, (225, 128, 0.75, &r),  0.,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_prob_deriv_e, (5, 128, 0.75, &r),  -3.0288278964712702882066404e112,  TEST_TOL1, GSL_SUCCESS);

  x = 0.75;
  sa = 0;
  gsl_sf_hermite_prob_array(0, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array(0, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array(1, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   x,   TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array(1, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array(100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   x, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   823.810509681701660156250, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,  1.03749254986255755872498e78, TEST_TOL1);
  gsl_test(sa, "gsl_sf_hermite_prob_array(100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array_deriv(0, 100, 0.75, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   823.810509681701660156250, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,  1.03749254986255755872498e78, TEST_TOL1);
  gsl_test(sa, "gsl_sf_hermite_prob_array_deriv(0, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array_deriv(1000, 100, 0.75, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   0.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array_deriv(1000, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array_deriv(99, 100, 0.75, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[99], +0.0,    9.332621544394415268169923886e155, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   6.999466158295811451127442914e157, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array_deriv(99, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array_deriv(100, 100, 0.75, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   9.332621544394415268169923886e157, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array_deriv(100, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array_deriv(23, 100, 0.75, res);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[23], +0.0,  2.5852016738884976640000000000e22, TEST_TOL0);
  TEST_SF_VAL(sa, res[24], +0.0,  4.6533630129992957952000000000e23, TEST_TOL0);
  TEST_SF_VAL(sa, res[37], +0.0,  2.3592417210568968566591219172e37, TEST_TOL0);
  TEST_SF_VAL(sa, res[60], +0.0, -9.2573208827175536243052086845e59, TEST_TOL0);
  TEST_SF_VAL(sa, res[61], +0.0,  5.6259625429686219137261735305e59, TEST_TOL1);
  TEST_SF_VAL(sa, res[100], +0.0, 2.6503570965896336273549197e100, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array_deriv(23, 37, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_deriv_array(100, 50, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   -3.88338863813139372375411561e31, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   -4.04757862431646677625108652e32, TEST_TOL1);
  TEST_SF_VAL(sa, res[10],  +0.0,    7.9614368698398116765703194e38, TEST_TOL0);
  TEST_SF_VAL(sa, res[30],  +0.0,   -9.1416928183915197188795338e54, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,    0.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_deriv_array(100, 50, 0.75)");
  s += sa;

  n = 128;
  res[0] = 1.;
  for(m=1; m<=n; m++){
    res[m] = res[m-1]/2.;
  }
  TEST_SF(s, gsl_sf_hermite_prob_series_e, (n, x, res, &r),  -4.0451066556993485405907339548e68,  TEST_TOL0, GSL_SUCCESS);

  /* phys */

  x = 0.75;

  TEST_SF(s, gsl_sf_hermite_e, (0, 0.75, &r),  1.0,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_e, (1, 0.75, &r),  1.5,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_e, (25, 0., &r),  0.,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_e, (28, 0., &r),  3.497296636753920000e18,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_e, (30, 0., &r), -2.028432049317273600e20,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_e, (25, 0.75, &r),  -9.7029819451106077507781088352e15,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_e, (28, 0.75, &r),   3.7538457078067672096408339776e18,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF_RETURN(s, gsl_sf_hermite_e, (2800, 0.75, &r), GSL_EOVRFLW);
  TEST_SF_RETURN(s, gsl_sf_hermite_e, (2800, 10.1, &r), GSL_EOVRFLW);

  TEST_SF(s, gsl_sf_hermite_deriv_e, (225, 128, 0.75, &r),  0.,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_deriv_e, (5, 128, 0.75, &r),  2.89461215568095657569833e132,  TEST_TOL0, GSL_SUCCESS);

  x = 0.75;
  sa = 0;
  gsl_sf_hermite_array(0, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_array(0, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_array(1, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   2.*x,   TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_array(1, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_array(100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   2.0*x, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   38740.4384765625, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,  -1.4611185395125104593177790757e93, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_array(100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_array_deriv(0, 100, 0.75, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   38740.4384765625, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,  -1.4611185395125104593177790757e93, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_array_deriv(0, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_array_deriv(1000, 100, 0.75, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   0.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_array_deriv(1000, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_array_deriv(99, 100, 0.75, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[99], +0.0,    5.915251651227242890408570128e185, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   8.872877476840864335612855192e187, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_array_deriv(99, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_array_deriv(100, 100, 0.75, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   1.183050330245448578081714026e188, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_array_deriv(100, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_array_deriv(23, 100, 0.75, res);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[23],  +0.0,   2.168624344319444261221171200e29, TEST_TOL0);
  TEST_SF_VAL(sa, res[24],  +0.0,   7.807047639549999340396216320e30, TEST_TOL0);
  TEST_SF_VAL(sa, res[37],  +0.0,   1.930387357696033719818118732e46, TEST_TOL0);
  TEST_SF_VAL(sa, res[60],  +0.0,   6.775378005383186748501182409e71, TEST_TOL0);
  TEST_SF_VAL(sa, res[61],  +0.0,  -4.451215867508936256056845902e73, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   2.957966000491202678467161e118, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_array_deriv(23, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_deriv_array(100, 50, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,  -8.26632218305863100726861832e38, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   2.40954750392844799126151557e40, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   1.52281030265187793605875604e49, TEST_TOL0);
  TEST_SF_VAL(sa, res[30],  +0.0,   9.52199419132990437892101915e65, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   0.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_deriv_array(100, 50, 0.75)");
  s += sa;

  n = 128;
  /* arbitrary weights */
  res[0] = 1.;
  for(m=1; m<=n; m++){
    res[m] = res[m-1]/2.;
  }
  TEST_SF(s, gsl_sf_hermite_series_e, (n, x, res, &r),  1.07772223811696567390619566842e88,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (0,  1.3, &r),  0.322651504564963746879400858624,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (0,  1.3, &r),  0.322651504564963746879400858624,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (1,  1.3, &r),  0.593187573778613235895531272243,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (1,  1.3, &r),  0.593187573778613235895531272243,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (1, -1.3, &r), -0.593187573778613235895531272243,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (1, -1.3, &r), -0.593187573778613235895531272243,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (27, 0, &r),  0.0,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (27, 0, &r),  0.0,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (28, 0, &r),  0.290371943657199641200016132937,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (28, 0, &r),  0.290371943657199641200016132937,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (28, 0.75, &r),  0.23526280808621240649319140441,  TEST_TOL1, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (28, 0.75, &r),  0.23526280808621240649319140441,  TEST_TOL1, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (200, 0.75, &r), -0.13725356483699291817038427801, TEST_TOL3, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (200, 0.75, &r), -0.13725356483699291817038427801, TEST_TOL3, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (100028, 0.75, &r),  -0.02903467369856961147236598086,  TEST_TOL4, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (100028, 0.75, &r),  -0.02903467369856961147236598086,  TEST_TOL5, GSL_SUCCESS);

  n = 10025;
  x = ((sqrt(2*n+1.)+aizero1/pow(8.*n,1/6.))/2.5);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  -0.05301278920004176459797680403,  TEST_TOL4, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (n, x, &r),  -0.05301278920004176459797680403,  TEST_TOL4, GSL_SUCCESS);

  n = 10028;
  x = ((sqrt(2*n+1.)+aizero1/pow(8.*n,1/6.))/2.5);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  0.06992968509693993526829596970,  TEST_TOL3, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (n, x, &r),  0.06992968509693993526829596970,  TEST_TOL3, GSL_SUCCESS);

  n = 10025;
  x = (sqrt(2*n+1.)-(aizero1/pow(8.*n,1/6.))/2.5);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  0.08049000991742150521366671021,  TEST_TOL4, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (n, x, &r),  0.08049000991742150521366671021,  TEST_TOL4, GSL_SUCCESS);

  n = 10028;
  x = (sqrt(2*n+1.)-(aizero1/pow(8.*n,1/6.))/2.5);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  0.08048800667512084252723933250,  TEST_TOL4, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (n, x, &r),  0.08048800667512084252723933250,  TEST_TOL4, GSL_SUCCESS);

  n = 10025;
  x = (sqrt(2*n+1.)-2.5*(aizero1/pow(8.*n,1/6.)));
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  7.97206830806663013555068100e-6,  TEST_TOL4, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (n, x, &r),  7.97206830806663013555068100e-6,  1.0e-9, GSL_SUCCESS);

  n = 10028;
  x = (sqrt(2*n+1.)-2.5*(aizero1/pow(8.*n,1/6.)));
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  7.97188517397786729928465829e-6,  TEST_TOL4, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (n, x, &r),  7.97188517397786729928465829e-6,  1.0e-8, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_e, (10000, 60.0, &r), 0.03162606955427450540143292572, TEST_TOL3, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_fast_e, (10000, 60.0, &r), 0.03162606955427450540143292572, TEST_TOL4, GSL_SUCCESS);

  x = 0.75;

  sa = 0;
  gsl_sf_hermite_func_array(100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.566979307027693616978839335983, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   0.601372369187597546203014795470, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.360329854170806945032958735574, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   -0.07616422890563462489003733382, TEST_TOL1);
  gsl_test(sa, "gsl_sf_hermite_func_array(100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_func_array(250, 20.0, res);
  TEST_SF_VAL(sa, res[199], +0.0,   0.254621970261340422399150964119, TEST_TOL2);
  TEST_SF_VAL(sa, res[200], +0.0,   0.288364948026205642290411878357, TEST_TOL2);
  TEST_SF_VAL(sa, res[210], +0.0,   0.266625427572822255774117774166, TEST_TOL2);
  TEST_SF_VAL(sa, res[220], +0.0,  -0.296028413764592652216845612832, TEST_TOL2);
  TEST_SF_VAL(sa, res[230], +0.0,   0.229657495510699141971182819560, TEST_TOL2);
  TEST_SF_VAL(sa, res[240], +0.0,  -0.024027870622288819185159269632, TEST_TOL2);
  TEST_SF_VAL(sa, res[250], +0.0,  -0.236101289420398152417654177998, TEST_TOL2);
  gsl_test(sa, "gsl_sf_hermite_func_array(250, 20.0)");
  s += sa;

  n = 128;
  /* arbitrary weights */
  res[0] = 1.;
  for(m=1; m<=n; m++){
    res[m] = res[m-1]/2.;
  }
  TEST_SF(s, gsl_sf_hermite_func_series_e, (n, x, res, &r),  0.81717103529960997134154552556,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_der_e, (0, 28, 0.75, &r),  0.235262808086212406493191404,  TEST_TOL1, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (1, 28, 0.75, &r),  1.289485094958329643927802330,  TEST_TOL1, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (2, 28, 0.75, &r), -13.27764473136561269145948989,  TEST_TOL1, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (3, 28, 0.75, &r), -72.42242083458141066943555691,  TEST_TOL1, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (4, 28, 0.75, &r),  753.6960554274941800190147503,  TEST_TOL1, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (5, 28, 0.75, &r),  4035.32788513029308540826835,   TEST_TOL1, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_func_der_e, (0, 380, 0.75, &r), -0.0400554661321992411631174,    TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (1, 380, 0.75, &r), -4.0417244263030600591206553,    TEST_TOL1, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (2, 380, 0.75, &r),  30.4596785269042604519780923,   TEST_TOL2, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (3, 380, 0.75, &r),  3073.4187352276349348458186556, TEST_TOL2, GSL_SUCCESS);

  {
    /* positive zeros of the probabilists' Hermite polynomial of order 17 */
    double He17z[8] = { 0.751842600703896170737870774614, 1.50988330779674075905491513417, 2.28101944025298889535537879396,
                        3.07379717532819355851658337833, 3.90006571719800990903311840097, 4.77853158962998382710540812497,
                        5.74446007865940618125547815768,6.88912243989533223256205432938 };

    n = 17;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_prob_zero_e, (n, m, &r),  He17z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the probabilists' Hermite polynomial of order 18 */
    double He18z[9] = { 0.365245755507697595916901619097, 1.09839551809150122773848360538, 1.83977992150864548966395498992,
                        2.59583368891124032910545091458, 3.37473653577809099529779309480, 4.18802023162940370448450911428,
                        5.05407268544273984538327527397, 6.00774591135959752029303858752, 7.13946484914647887560975631213 };

    n = 18;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_prob_zero_e, (n, m, &r),  He18z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the probabilists' Hermite polynomial of order 23 */
    double He23z[11] = { 0.648471153534495816722576841197, 1.29987646830397886997876116860, 1.95732755293342410739100839243,
                         2.62432363405918177067330340783, 3.30504002175296456723204112903, 4.00477532173330406712238633738,
                         4.73072419745147329426707133987, 5.49347398647179412855289367747, 6.31034985444839982842886078177,
                         7.21465943505186138595859194492, 8.29338602741735258945596770157 };

    n = 23;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_prob_zero_e, (n, m, &r),  He23z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the probabilists' Hermite polynomial of order 24 */
    double He24z[12] = { 0.317370096629452319318170455994, 0.953421922932109084904629632351, 1.59348042981642010695074168129,
                         2.24046785169175236246653790858, 2.89772864322331368932008199475, 3.56930676407356024709649151613,
                         4.26038360501990548884317727406, 4.97804137463912033462166468006, 5.73274717525120114834341822330,
                         6.54167500509863444148277523364, 7.43789066602166310850331715501, 8.50780351919525720508386233432 };

    n = 24;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_prob_zero_e, (n, m, &r),  He24z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the physicists' Hermite polynomial of order 17 */
    double H17z[8] = { 0.531633001342654731349086553718, 1.06764872574345055363045773799, 1.61292431422123133311288254454,
                       2.17350282666662081927537907149, 2.75776291570388873092640349574, 3.37893209114149408338327069289,
                       4.06194667587547430689245559698, 4.87134519367440308834927655662 };

    n = 17;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_zero_e, (n, m, &r),  H17z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the physicists' Hermite polynomial of order 18 */
    double H18z[9] = { 0.258267750519096759258116098711, 0.776682919267411661316659462284, 1.30092085838961736566626555439,
                       1.83553160426162889225383944409, 2.38629908916668600026459301424, 2.96137750553160684477863254906,
                       3.57376906848626607950067599377, 4.24811787356812646302342016090, 5.04836400887446676837203757885 };

    n = 18;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_zero_e, (n, m, &r),  H18z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the physicists' Hermite polynomial of order 23 */
    double H23z[11] = { 0.458538350068104797757887329284, 0.919151465442563765431719239593, 1.38403958568249523732634717118,
                        1.85567703767137106251504753718, 2.33701621147445578644623502174, 2.83180378712615690144806140734,
                        3.34512715994122457247439814585, 3.88447270810610186607248760288, 4.46209117374000667673186157071,
                        5.10153461047667712968749766165, 5.86430949898457256538748413474 };

    n = 23;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_zero_e, (n, m, &r),  H23z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the physicists' Hermite polynomial of order 24 */
    double H24z[12] = { 0.224414547472515585151136715527, 0.674171107037212236000245923730, 1.12676081761124507213306126773,
                        1.58425001096169414850563336202, 2.04900357366169891178708399532, 2.52388101701142697419907602333,
                        3.01254613756556482565453858421, 3.52000681303452471128987227609, 4.05366440244814950394766297923,
                        4.62566275642378726504864923776, 5.25938292766804436743072304398, 6.01592556142573971734857350899 };

    n = 24;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_zero_e, (n, m, &r),  H24z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  n = 2121;
  x = -1.*n;
  res[0] = (double) n;
  sa = 0;
  for (m=1; m<=n/2; m++) {
    gsl_sf_hermite_prob_zero_e(n, m, &r);
    if (x>=r.val) {
      sa += TEST_SF_INCONS;
      printf("sanity check failed! (gsl_sf_hermite_prob_zero)\n");
    }
    res[0] = GSL_MIN(res[0],fabs(x-r.val));
    x = r.val;
  }
  gsl_test(sa, "gsl_sf_hermite_prob_zero(n, m, r)");

  n = 2121;
  x = -1.*n;
  res[0] = (double) n;
  sa = 0;
  for (m=1; m<=n/2; m++) {
    gsl_sf_hermite_zero_e(n, m, &r);
    if (x>=r.val) {
      sa += TEST_SF_INCONS;
      printf("sanity check failed! (gsl_sf_hermite_zero)\n");
    }
    res[0] = GSL_MIN(res[0],fabs(x-r.val));
    x = r.val;
  }
  gsl_test(sa, "gsl_sf_hermite_zero(n, m, r)");

  return s;
}