#include <string.h>
#include <sys/types.h>
#include <stdint.h>
#include <math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_wavelet.h>
#include <fftw3.h>
#include "common.h"
#include "utils.h"
#include "filters.h"

filters_t *filters_init(ANALYSIS_WAVEFORM_BASE_TYPE *inWav, size_t n)
{
    filters_t *fHdl;
    fHdl = (filters_t*)malloc(sizeof(filters_t));

    fHdl->wavLen = n;
    fHdl->respLen = 0;
    fHdl->malloced = 0;
    fHdl->fftUsed = 0;
    fHdl->fftwNThreads = FFTW_NTHREADS_DEFAULT;
    fHdl->fftwFlags = FFTW_FLAGS_DEFAULT;

    if(inWav == NULL) {
        fHdl->inWav = (ANALYSIS_WAVEFORM_BASE_TYPE*)
            calloc(fHdl->wavLen, sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
        fHdl->malloced = 1;
    } else {
        fHdl->inWav = inWav;
    }

    fHdl->outWav = (ANALYSIS_WAVEFORM_BASE_TYPE*)
        calloc(fHdl->wavLen, sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
    fHdl->waveletWav = (WAVELET_BASE_TYPE*)
        calloc(fHdl->wavLen, sizeof(WAVELET_BASE_TYPE));

    fHdl->gslDWT = gsl_wavelet_alloc(gsl_wavelet_daubechies_centered, 10);
    fHdl->gslDWTWork = gsl_wavelet_workspace_alloc(fHdl->wavLen);

    return fHdl;
}

/* for convolution */
filters_t *filters_init_for_convolution(ANALYSIS_WAVEFORM_BASE_TYPE *inWav, size_t n, size_t np)
{
    filters_t *fHdl;

    if((np > 0) && (np % 2 == 0)) {
        error_printf("%s(): np = %zd is not odd!\n", __FUNCTION__, np);
        return NULL;
    }

    fHdl = filters_init(inWav, n);
    fHdl->respLen = np;
    fHdl->fftUsed = 1;
    fHdl->respWav = (ANALYSIS_WAVEFORM_BASE_TYPE*)
        calloc(fHdl->respLen, sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
    if(np > 0) {
        fHdl->fftLen = (fHdl->wavLen + fHdl->respLen+1); /* zero padding */
        if(fHdl->fftLen % 2) fHdl->fftLen++; /* ensure fHdl->fftLen is even */
    } else {
        fHdl->fftLen = fHdl->wavLen; /* for spectrum calculation */
    }

    if(fHdl->fftwNThreads > 0) {
        if(FFTW(init_threads)() == 0) {
            error_printf("fftw_init_threads error!\n");
        }
        FFTW(plan_with_nthreads)(fHdl->fftwNThreads);
    }

    fHdl->fftwWork = (FFT_BASE_TYPE*) FFTW(malloc)(sizeof(FFT_BASE_TYPE) * fHdl->fftLen);
    fHdl->fftwWork1 = (FFT_BASE_TYPE*) FFTW(malloc)(sizeof(FFT_BASE_TYPE) * fHdl->fftLen);
    fHdl->fftwWin = (FFT_BASE_TYPE*) FFTW(malloc)(sizeof(FFT_BASE_TYPE) * fHdl->fftLen);
    filters_hanning_window(fHdl); /* Hanning window as the default */
    fHdl->dt = 1.0;

    fHdl->fftwPlan = FFTW(plan_r2r_1d)(fHdl->fftLen, fHdl->fftwWork, fHdl->fftwWork,
                                       FFTW_R2HC, fHdl->fftwFlags);
    fHdl->fftwPlan1 = FFTW(plan_r2r_1d)(fHdl->fftLen, fHdl->fftwWork1, fHdl->fftwWork1,
                                        FFTW_R2HC, fHdl->fftwFlags);
    fHdl->fftwPlan2 = FFTW(plan_r2r_1d)(fHdl->fftLen, fHdl->fftwWork, fHdl->fftwWork,
                                        FFTW_HC2R, fHdl->fftwFlags);

    return fHdl;
}

int filters_close(filters_t *fHdl)
{
    if(fHdl->malloced) {
        if(fHdl->inWav)
            free(fHdl->inWav);
    }
    if(fHdl->outWav)
        free(fHdl->outWav);
    if(fHdl->fftUsed) {
        if(fHdl->respWav)
            free(fHdl->respWav);
        FFTW(destroy_plan)(fHdl->fftwPlan);
        FFTW(destroy_plan)(fHdl->fftwPlan1);
        FFTW(destroy_plan)(fHdl->fftwPlan2);
        FFTW(free)(fHdl->fftwWork);
        FFTW(free)(fHdl->fftwWork1);
        FFTW(free)(fHdl->fftwWin);
        if(fHdl->fftwNThreads > 0) {
            FFTW(cleanup_threads)();
            FFTW(cleanup)();
        }
    }
    gsl_wavelet_free(fHdl->gslDWT);
    gsl_wavelet_workspace_free(fHdl->gslDWTWork);
    if(fHdl->waveletWav)
        free(fHdl->waveletWav);
    return 0;
}

/* Filters should directly write into fHdl->respWav the real space
 * response waveform in wrapped around order */

int filters_SavitzkyGolay(filters_t *fHdl, int m, int ld)
/* m: order of polynomial, np: number of points, ld: degree of derivative*/
{
    int np;
    ANALYSIS_WAVEFORM_BASE_TYPE *c;
    int ipj, imj, mm, j, k, nl, nr;
    double fac, sum;
    gsl_permutation * p;
    gsl_vector *b;
    gsl_matrix *a;

    np = fHdl->respLen;
    if(np<1 || np<m-1 || np%2==0 || ld>m || np!=fHdl->respLen) {
        error_printf("%s(): improper arguments, returning...\n", __FUNCTION__);
        return 1;
    }

    c = calloc(np, sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));

    p = gsl_permutation_alloc (m+1);
    b = gsl_vector_alloc(m+1);
    a = gsl_matrix_alloc(m+1, m+1);

    nl = np/2;
    nr = nl;

    for(ipj=0;ipj<=(m << 1);ipj++) {
        sum=(ipj ? 0.0 : 1.0);
        for(k=1;k<=nr;k++) sum += pow((double)(k),(double)(ipj));
        for(k=1;k<=nl;k++) sum += pow((double)(-k),(double)(ipj));
        mm=MIN(ipj,2*m-ipj);
        for(imj=-mm;imj<=mm;imj+=2) gsl_matrix_set(a,(ipj+imj)/2,(ipj-imj)/2,sum);
    }

    gsl_linalg_LU_decomp(a, p, &k);
    for (j=0;j<m+1;j++) gsl_vector_set(b,j,0.0);
    gsl_vector_set(b,ld,1.0);

    gsl_linalg_LU_solve (a, p, b, b);

    for(k = -nl;k<=nr;k++) {
        sum = gsl_vector_get(b,0);
        fac = 1.0;
        for (mm=1;mm<=m;mm++) sum += gsl_vector_get(b,mm)*(fac *= k);

        j=(np-k) % np;
        c[j]=sum; // c is in wraparound order, convenient for fft convolute

        // c[nl + k] = sum;
    }
    memcpy(fHdl->respWav, c, np * sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
/*
    for(j=0; j<np; j++) {
        fprintf(stderr, "%g\n", c[j]);
    }
*/
    gsl_vector_free(b);
    gsl_matrix_free(a);
    gsl_permutation_free(p);

/*
    for(k=nl; k<wavlen - nr; k++) {
        sum = 0.0;
        for(j=0; j<np; j++) {
            sum += c[j] * inwav[k+j-nl];
            outwav[k] = sum;
        }
    }
*/
    free(c);
    return 0;
}

int filters_raisedCosine(filters_t *fHdl)
{
    ssize_t i;
    ANALYSIS_WAVEFORM_BASE_TYPE x;

    for(i=0; i<(fHdl->respLen+1)/2; i++) { /* positive side */
        x = 2.0*M_PI/(ANALYSIS_WAVEFORM_BASE_TYPE)(fHdl->respLen-1)*(ANALYSIS_WAVEFORM_BASE_TYPE)i;
        fHdl->respWav[i] = (1.0 + cos(x))/(ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->respLen;
    }
    for(i=-((fHdl->respLen-1)/2); i<0; i++) { /* negative side */
        /* i=-(fHdl->respLen-1)/2 cast to wrong value */
        x = 2.0*M_PI/(ANALYSIS_WAVEFORM_BASE_TYPE)(fHdl->respLen-1)*(ANALYSIS_WAVEFORM_BASE_TYPE)i;
        fHdl->respWav[fHdl->respLen+i] = (1.0 + cos(x))/(ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->respLen;
    }
    return 0;
}

int filters_convolute(filters_t *fHdl)
{
    size_t i;
    ANALYSIS_WAVEFORM_BASE_TYPE re, im;

    for(i=0; i<fHdl->wavLen; i++) {
        fHdl->fftwWork[i] = fHdl->inWav[i];
    }
    for(i=fHdl->wavLen; i<fHdl->fftLen; i++) {
        fHdl->fftwWork[i] = 0.0;
    }

    // fill in with respwav in wrap-around order
    fHdl->fftwWork1[0] = fHdl->respWav[0];
    for(i=1; i<(fHdl->respLen+1)/2; i++) {
        fHdl->fftwWork1[i] = fHdl->respWav[i];
        fHdl->fftwWork1[fHdl->fftLen-i] = fHdl->respWav[fHdl->respLen-i];
    }
    for(i=(fHdl->respLen+1)/2; i<(fHdl->fftLen-(fHdl->respLen+1)/2); i++) {
        fHdl->fftwWork1[i] = 0.0;
    }

    // do fft
    FFTW(execute)(fHdl->fftwPlan);
    FFTW(execute)(fHdl->fftwPlan1);

    // multiply in complex fourier space, half-complex format
    fHdl->fftwWork[0] = fHdl->fftwWork[0] * fHdl->fftwWork1[0]
        / (ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->fftLen;
    for(i=1; i<fHdl->fftLen/2; i++) {
        re = fHdl->fftwWork[i] * fHdl->fftwWork1[i]
            - fHdl->fftwWork[fHdl->fftLen-i] * fHdl->fftwWork1[fHdl->fftLen-i];
        im = fHdl->fftwWork[i] * fHdl->fftwWork1[fHdl->fftLen-i]
            + fHdl->fftwWork[fHdl->fftLen-i] * fHdl->fftwWork1[i];
        fHdl->fftwWork[i] = re / (ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->fftLen;
        fHdl->fftwWork[fHdl->fftLen-i] = im / (ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->fftLen;
    }
    fHdl->fftwWork[fHdl->fftLen/2] = fHdl->fftwWork[fHdl->fftLen/2] * fHdl->fftwWork1[fHdl->fftLen/2]
        / (ANALYSIS_WAVEFORM_BASE_TYPE)fHdl->fftLen;

    // ifft
    FFTW(execute)(fHdl->fftwPlan2);

    // copy the output to outwav
    memcpy(fHdl->outWav, fHdl->fftwWork, fHdl->wavLen * sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));
    return 0;
}

int filters_hanning_window(filters_t *fHdl)
{
    size_t i;

    fHdl->fftwS1 = 0.0; fHdl->fftwS2 = 0.0;
    for(i=0; i<fHdl->fftLen; i++) {
        fHdl->fftwWin[i] = 0.5 * (1.0 - cos(2*M_PI*i/(double)fHdl->fftLen));
        fHdl->fftwS1 += fHdl->fftwWin[i];
        fHdl->fftwS2 += fHdl->fftwWin[i] * fHdl->fftwWin[i];
    }

    return 0;
}

int filters_fft_spectrum(filters_t *fHdl)
{
    size_t i;

    for(i=0; i<fHdl->fftLen; i++) {
        fHdl->fftwWork[i] = fHdl->inWav[i] * fHdl->fftwWin[i];
    }
    FFTW(execute)(fHdl->fftwPlan);

    /* Compute linearized power spectrum into fftwWork1, in [V] for example, normalized.
     * Total length should be (int)(n/2)+1 */
    fHdl->fftwWork1[0] = fHdl->fftwWork[0] / fHdl->fftwS1;
    for(i=1; i<(fHdl->fftLen+1)/2; i++) {
        fHdl->fftwWork1[i] = hypot(fHdl->fftwWork[i],
                                   fHdl->fftwWork[fHdl->fftLen - i]) * sqrt(2.0) / fHdl->fftwS1;
    }
    if(fHdl->fftLen % 2 == 0) { /* even number */
        fHdl->fftwWork1[fHdl->fftLen/2] = fHdl->fftwWork[fHdl->fftLen/2] / fHdl->fftwS1;
    }
    /* For spectra density, normalization should be * sqrt(2.0 * dt / fftwS2) */
    for(i=0; i<=fHdl->fftLen/2; i++) {
        fHdl->fftwWork[i] = fHdl->fftwWork1[i] * fHdl->fftwS1 * sqrt(fHdl->dt / fHdl->fftwS2);
    }
    return 0;
}

int filters_DWT(filters_t *fHdl) /* discrete wavelet transform */
{
    gsl_wavelet_transform_forward(fHdl->gslDWT, fHdl->waveletWav, 1, fHdl->wavLen,
                                  fHdl->gslDWTWork);
    return 0;
}

int filters_median(filters_t *fHdl, size_t n) /* median filter with moving window size n */
{
    size_t i, mid=n/2;

    for(i=0; i<mid;i++) /* overhang at the beginning */
        fHdl->outWav[i] = quickselect(fHdl->inWav, mid+i+1, (mid+i+1)/2);
    for(i=0; i<fHdl->wavLen-1 - mid; i++)
        fHdl->outWav[mid+i] = quickselect(fHdl->inWav + i, n, n/2);
    for(i=0; i<mid; i++) /* overhang at the end */
        fHdl->outWav[fHdl->wavLen-1 - i] = quickselect(fHdl->inWav + fHdl->wavLen - (mid-i+1),
                                                       mid-i+1, (mid-i+1)/2);
    return 0;
}

int filters_trapezoidal(filters_t *fHdl, size_t k, size_t l, double M)
{
    double s, pp;
    ssize_t i, j, jk, jl, jkl;
    double vj, vjk, vjl, vjkl, dkl;

    s = 0.0; pp = 0.0;

    for(i=0; i<fHdl->wavLen; i++) {
        j=i; jk = j-k; jl = j-l; jkl = j-k-l;
        vj   = j>=0   ? fHdl->inWav[j]   : fHdl->inWav[0];
        vjk  = jk>=0  ? fHdl->inWav[jk]  : fHdl->inWav[0];
        vjl  = jl>=0  ? fHdl->inWav[jl]  : fHdl->inWav[0];
        vjkl = jkl>=0 ? fHdl->inWav[jkl] : fHdl->inWav[0];

        dkl = vj - vjk - vjl + vjkl;
        pp = pp + dkl;
        if(M>=0.0) {
            s = s + pp + dkl * M;
        } else { /* infinit decay time, so the input is a step function */
            s = s + dkl;
        }
        fHdl->outWav[i] = s / (fabs(M) * (double)k);
    }
    return 0;
}

#ifdef FILTERS_DEBUG_ENABLEMAIN
int main(int argc, char **argv)
{
    #define PLEN 1503
    ANALYSIS_WAVEFORM_BASE_TYPE pulse[PLEN] = {0.0};
    size_t i;
    filters_t *fHdl;

    i = 2;
    pulse[i++]=0.0; pulse[i++]=1.0; pulse[i++]=10.0; pulse[i++]=8.0; pulse[i++]=6.0;
    pulse[i++]=4.0; pulse[i++]=2.0; pulse[i++]=1.0; pulse[i++]=0.5; pulse[i++]=0.2;
    i = 100;
    pulse[i--]=0.0; pulse[i--]=1.0; pulse[i--]=10.0; pulse[i--]=8.0; pulse[i--]=6.0;
    pulse[i--]=4.0; pulse[i--]=2.0; pulse[i--]=1.0; pulse[i--]=0.5; pulse[i--]=0.2;

    fHdl = filters_init(pulse, PLEN);
    filters_median(fHdl, 11);
    for(i=0; i<fHdl->wavLen; i++) {
        printf("%g %g\n", fHdl->inWav[i], fHdl->outWav[i]);
    }
    printf("\n\n");

#if 0
    for(i=0; i<PLEN; i++) {
        pulse[i] += 10.0 * cos(2.0 * M_PI/3.0 * i);
    }

    fHdl = filters_init_for_convolution(pulse, PLEN, 0);
    filters_fft_spectrum(fHdl);
    for(i=0; i<fHdl->fftLen; i++) {
        printf("%g %g\n", fHdl->fftwWork[i], fHdl->fftwWork1[i]);
    }
    printf("\n\n");
#endif
#if 0
    fHdl = filters_init_for_convolution(NULL, PLEN, 31);
    memcpy(fHdl->inWav, pulse, PLEN * sizeof(ANALYSIS_WAVEFORM_BASE_TYPE));

    filters_raisedCosine(fHdl);
    filters_convolute(fHdl);

    for(i=0; i<fHdl->fftLen/*PLEN*/; i++) {
        // printf("%g %g\n", pulse[i], fHdl->outWav[i]);
        printf("%g %g\n", fHdl->fftwWork[i], fHdl->fftwWork1[i]);
    }
    printf("\n\n");

    filters_SavitzkyGolay(fHdl, 5, 0);
    filters_convolute(fHdl);

    for(i=0; i<fHdl->fftLen/*PLEN*/; i++) {
        // printf("%g %g\n", pulse[i], fHdl->outWav[i]);
        printf("%g %g\n", fHdl->fftwWork[i], fHdl->fftwWork1[i]);
    }
#endif

    filters_close(fHdl);
    return EXIT_SUCCESS;
}
#endif


#include <gsl/gsl_rng.h>
#include <gsl/gsl_sort_double.h>

int
main ()
{
  gsl_rng * r;

  int i, k = 5, N = 100000;

  double * x = malloc (N * sizeof(double));
  double * small = malloc (k * sizeof(double));

  gsl_rng_env_setup();

  r = gsl_rng_alloc (gsl_rng_default);

  for (i = 0; i < N; i++)
    {
      x[i] = gsl_rng_uniform(r);
    }

  gsl_sort_smallest (small, k, x, 1, N);

  printf("%d smallest values from %d\n", k, N);

  for (i = 0; i < k; i++)
    {
      printf ("%d: %.18f\n", i, small[i]);
    }

}


#include <gsl/gsl_vector.h>
#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include "data.h"
#include "prob_functions.h"

void EM (Dataset *data)
{
	int i, j;
	const double THRESHOLD = 1E-10;
	double Q, lastQ;

	srand(time(NULL));

	/* Initialize parameters to starting values */
	for (i = 0; i < data->numLabelers; i++) {
		data->alpha[i] = data->priorAlpha[i];
		/*data->alpha[i] = (double) rand() / RAND_MAX * 4 - 2;*/
	}
	for (j = 0; j < data->numImages; j++) {
		data->beta[j] = data->priorBeta[j];
		/*data->beta[j] = (double) rand() / RAND_MAX * 3;*/
	}

	Q = 0;
	EStep(data);
	Q = computeQ(data);
	/*printf("Q = %f\n", Q);*/
	do {
		lastQ = Q;

		/* Re-estimate P(Z|L,alpha,beta) */
		EStep(data);
		Q = computeQ(data);
		/*printf("Q = %f; L = %f\n", Q, 0.0 /*computeLikelihood(data)*/

		/*outputResults(data);*/
		MStep(data);

		Q = computeQ(data);
		/*printf("Q = %f; L = %f\n", Q, 0.0 /*computeLikelihood(data)*/
	} while (fabs((Q - lastQ)/lastQ) > THRESHOLD);
	/* outputResults(data); */
}

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

	if (argc < 2) {
		fprintf(stdout, "Usage: em <data>\n");
		fprintf(stdout, "where the specified data file is formatted as described in the README file.\n");
		exit(1);
	}

	readData(argv[1], &data);
	EM(&data);

	free(data.priorAlpha);
	free(data.priorBeta);
	free(data.labels);
	free(data.alpha);
	free(data.beta);
	free(data.probZ1);
	free(data.probZ0);
}


#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>

struct rparams
  {
    double a;
    double b;
  };

int
rosenbrock_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double a = ((struct rparams *) params)->a;
  double b = ((struct rparams *) params)->b;

  double x0 = gsl_vector_get (x, 0);
  double x1 = gsl_vector_get (x, 1);

  double y0 = a * (1 - x0);
  double y1 = b * (x1 - x0 * x0);

  gsl_vector_set (f, 0, y0);
  gsl_vector_set (f, 1, y1);

  return GSL_SUCCESS;
}

#ifdef DERIV
int
rosenbrock_df (const gsl_vector * x, void *params, gsl_matrix * df)
{
  double a = ((struct rparams *) params)->a;
  double b = ((struct rparams *) params)->b;

  double x0 = gsl_vector_get (x, 0);

  double df00 = -a;
  double df01 = 0;
  double df10 = -2 * b  * x0;
  double df11 = b;

  gsl_matrix_set (df, 0, 0, df00);
  gsl_matrix_set (df, 0, 1, df01);
  gsl_matrix_set (df, 1, 0, df10);
  gsl_matrix_set (df, 1, 1, df11);

  return GSL_SUCCESS;
}

int
rosenbrock_fdf (const gsl_vector * x, void *params,
                gsl_vector * f, gsl_matrix * df)
{
  rosenbrock_f (x, params, f);
  rosenbrock_df (x, params, df);

  return GSL_SUCCESS;
}
#endif

#ifdef DERIV
#define SOLVER gsl_multiroot_fdfsolver
#define SOLVER_TYPE gsl_multiroot_fdfsolver_type
#else
#define SOLVER gsl_multiroot_fsolver
#define SOLVER_TYPE gsl_multiroot_fsolver_type
#endif

int
main (void)
{
  const SOLVER_TYPE *T;
  SOLVER *s;

  int status;
  size_t i, iter = 0;

  const size_t n = 2;
  struct rparams p = {1.0, 10.0};
#ifdef DERIV
  gsl_multiroot_function_fdf f = {&rosenbrock_f, &rosenbrock_df, &rosenbrock_fdf, n, &p};
#else
  gsl_multiroot_function f = {&rosenbrock_f, n, &p};
#endif

  double x_init[2] = {-10.0, -5.0};
  gsl_vector *x = gsl_vector_alloc (n);

  gsl_vector_set (x, 0, x_init[0]);
  gsl_vector_set (x, 1, x_init[1]);

#ifdef DERIV
  T = gsl_multiroot_fdfsolver_gnewton;
  s = gsl_multiroot_fdfsolver_alloc (T, &f, x);
#else
  T = gsl_multiroot_fsolver_hybrids;
  s = gsl_multiroot_fsolver_alloc (T, &f, x);
#endif

  print_state (iter, s);

  do
    {
      iter++;
#ifdef DERIV
      status = gsl_multiroot_fdfsolver_iterate (s);
#else
      status = gsl_multiroot_fsolver_iterate (s);
#endif

      print_state (iter, s);

      if (status)
        break;

      status = gsl_multiroot_test_residual (s->f, 0.0000001);
    }
  while (status == GSL_CONTINUE && iter < 1000);

  printf ("status = %s\n", gsl_strerror (status));

#ifdef DERIV
  gsl_multiroot_fdfsolver_free (s);
#else
  gsl_multiroot_fsolver_free (s);
#endif

  gsl_vector_free (x);
}

int
print_state (size_t iter, SOLVER * s)
{
  printf ("iter = %3u x = % 15.8f % 15.8f  f(x) = % .3e % .3e\n",
          iter,
          gsl_vector_get (s->x, 0), gsl_vector_get (s->x, 1),
          gsl_vector_get (s->f, 0), gsl_vector_get (s->f, 1));
}


/*
** detrending, fit polynomial or order 1 (linear), 3 or 5
** G.Lohmann, Aug 2016, MPI-KYB
*/
#include <viaio/VImage.h>
#include <viaio/Vlib.h>
#include <viaio/mu.h>
#include <viaio/option.h>

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

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_multifit.h>

#define ABS(x) ((x) < 0 ? -(x) : (x))

void VGetStats(double *data,int nt,int i0,double *ave,double *sigma)
{
  int i;
  double u,sum1,sum2,nx;
  sum1 = sum2 = 0;
  for (i=i0; i<nt; i++) {
    u = data[i];
    sum1 += u;
    sum2 += u*u;
  }
  nx = (double)nt;
  double mean = sum1/nx;
  (*ave) = mean;
  (*sigma) = sqrt((sum2 - nx * mean * mean) / (nx - 1.0));
}

void VDetrend(VAttrList list,VFloat minval,VShort type,VShort i0)
{
  int slice,row,col,i,j;

  /* get image dimensions */
  int nslices = VAttrListNumImages(list);
  VImage *src = VAttrListGetImages(list,nslices);
  int nrows  = VImageNRows (src[0]);
  int ncols  = VImageNColumns (src[0]);
  int ntimesteps = VImageNBands (src[0]);
  if (nslices < 1) VError(" no slices");

  double smax = VPixelMaxValue(src[0]);
  double smin = VPixelMinValue(src[0]);

  /* ini regression matrix */
  int p = 4;
  if (type == 2) p = 6;
  gsl_vector *y = gsl_vector_calloc(ntimesteps);
  gsl_vector *c = gsl_vector_calloc(p);
  gsl_matrix *X = gsl_matrix_calloc(ntimesteps,p);
  for (j=0; j<ntimesteps; j++) {
    double u = (double)j;
    gsl_matrix_set(X,j,0,1);
    gsl_matrix_set(X,j,1,u);
    gsl_matrix_set(X,j,2,u*u);
    gsl_matrix_set(X,j,3,u*u*u);
    if (type == 2) {
      gsl_matrix_set(X,j,4,u*u*u*u);
      gsl_matrix_set(X,j,5,u*u*u*u*u);
    }
  }
  gsl_multifit_linear_workspace *workspace = gsl_multifit_linear_alloc ((size_t)ntimesteps,(size_t)p);
  gsl_matrix *cov = gsl_matrix_calloc(p,p);

  double *x = (double *) VCalloc(ntimesteps,sizeof(double));
  for (j=0; j<ntimesteps; j++) x[j] = (double)j;

  gsl_vector *w = gsl_vector_calloc(ntimesteps);
  for (j=0; j<ntimesteps; j++) w->data[j] = (double)1.0;
  for (i=0; i<=i0; i++) w->data[i] = 0.01; /* ignore initial time steps */

  /* remove baseline drift */
  double c0=0,c1=0,c2=0,c3=0,c4=0,c5=0,cov00,cov01,cov11,chisq;
  for (slice=0; slice<nslices; slice++) {
    if (slice%5==0)fprintf(stderr," slice  %5d  of  %d\r",slice,nslices);
    for (row=0; row<nrows; row++) {
      for (col=0; col<ncols; col++) {

	for (j=0; j<ntimesteps; j++) {
	  y->data[j] = VGetPixel(src[slice],j,row,col);
	}
	if (y->data[0] < minval+1.0e-8) {
	  for (j=0; j<ntimesteps; j++) VSetPixel(src[slice],j,row,col,0.0);
	  goto skip;
	}
	double mean=0,sigma=0;
	VGetStats(y->data,ntimesteps,i0,&mean,&sigma);

	/* remove linear trend */
	if (type == 0) {
	  gsl_fit_wlinear (x,1,w->data,1,y->data,1,j,&c0,&c1,&cov00,&cov01,&cov11,&chisq);
	  for (j=0; j<i0; j++) y->data[j] = 0;
	  for (j=i0; j<ntimesteps; j++) {
	    y->data[j] = y->data[j] - (c0 + c1*x[j]);
	  }
	}

	/* remove cubic polynomial */
	if (type == 1) {
	  gsl_multifit_wlinear (X,w,y,c,cov,&chisq,workspace);
	  c0 = c->data[0];
	  c1 = c->data[1];
	  c2 = c->data[2];
	  c3 = c->data[3];

	  for (j=0; j<i0; j++) y->data[j] = 0;
	  for (j=i0; j<ntimesteps; j++) {
	    double u = x[j];
	    y->data[j] = y->data[j] - (c0 + c1*u + c2*u*u + c3*u*u*u);
	  }
	}

	/* remove polynomial of order 5 */
	if (type == 2) {
	  gsl_multifit_wlinear (X,w,y,c,cov,&chisq,workspace);
	  c0 = c->data[0];
	  c1 = c->data[1];
	  c2 = c->data[2];
	  c3 = c->data[3];
	  c4 = c->data[4];
	  c5 = c->data[5];
	  for (j=0; j<i0; j++) y->data[j] = 0;
	  for (j=i0; j<ntimesteps; j++) {
	    double u = x[j];
	    double u2 = u*u;
	    double u3 = u2*u;
	    y->data[j] = y->data[j] - (c0 + c1*u + c2*u2 + c3*u3 + c4*u2*u2 + c5*u2*u3);
	  }
	}

	double a = 1.25;
	double sum=0,nx=0;
	for (j=i0; j<ntimesteps; j++) {
	  sum += a*y->data[j] + mean;
	  nx++;
	}
	if (nx > 0) sum /= nx;

	for (j=0; j<i0; j++) VSetPixel(src[slice],j,row,col,sum);
	for (j=i0; j<ntimesteps; j++) {
	  double u = a*y->data[j] + mean;
	  if (u > smax) u = smax;
	  if (u < smin) u = smin;
	  VSetPixel(src[slice],j,row,col,u);
	}
      skip: ;
      }
    }
  }
  fprintf(stderr,"\n");
}


/*=========================================================================

  Copyright (c) Yue Sheng, University of Manchester and David Worth, STFC
  All rights reserved.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

/** \file vvQuan3D.c
\brief This is a VolView plugin to calculate several characteristics from a
labelled image.

The following characteristics are calculated:
\li Volume by voxel counts
\li Equivalent sphere diameter by voxel counts
\li Bounding box diagonal
\li Principal Component Analysis
\li Ellipsoid fitting by PCA
\li Equivalent circle diameter by PCA
\li Isosurface by marching cube
\li Surface area
\li Surface volume
\li Equivalent sphere diameter from surface volume
\li Sphercity
\li Normalised surface area to volume ratio (Radius*Sa/Vol)
*/

#include "vvHelper.h"
#include "vtkVVPluginAPI.h"
#include <dlfcn.h>
#include <limits.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

/**
Struct used when calculating the number of voxels with each voxel value
*/
typedef struct {
  /** The voxel value */
  int value;
  /** The number of voxels with this value */
  int count;
} vvQuan3DVoxelCount;

/**
Comparison function for sorting the image data.
\param a Integer value
\param b Integer value
\return 1 if \c a>b; -1 if \c a<b; 0 if \c a==b.
*/
int cmpfun(void *a, void *b)
{
  return ( *(int*)a - *(int*)b );
}

/**
\brief Calculate sphere diameter from the sphere volume.

Use the formula \f$ 2*(3v/(4\pi))^{\frac{1}{3}} \f$ where \f$ v \f$ is the
sphere volume.

\param volume The sphere volume
\return Diameter of the sphere
*/
double vol2dia(double volume)
{
  return 2.0 * pow((3.0*volume)/(4.0*3.14159265),(1.0/3.0));
}

/**
\brief Find the (i,j,k) coordinates for each voxel id and also calculate
maximum, minimum and mean coordinates.

The positions pointer must be allocated by the calling function as follows
\code
positions = malloc(3*sizeof(int)*voxelCount);
\endcode

\param voxelSubArray Array of voxel ids
\param voxelCount Number of voxels in voxelSubArray <b>NB This is not the
length of the array</b>
\param dim The dimsnesions of the image
\param positions Pointer to matrix which will hold the (i,j,k) coordinates
\param min  The minimum coordinate in each dimension
\param max  The maximum coordinate in each dimension
\param mean The mean of the coordinates in each dimension
*/
void getCoords(int *voxelSubArray, int voxelCount, int dim[],
               gsl_matrix *positions, int min[], int max[], int mean[])
{
  int i;
  int voxelId;
  int icoord, jcoord, kcoord, rem;
  min[0] = min[1] = min[2] = INT_MAX;
  max[0] = max[1] = max[2] = INT_MIN;
  mean[0] = mean[1] = mean[2] = 0;

  /* Loop over the voxels and calulate i,j,k coords for each one */
  for (i = 0; i < voxelCount; i++) {
    voxelId = voxelSubArray[i];
    kcoord = floor(voxelId / (dim[0]*dim[1]));
    rem = voxelId % (dim[0]*dim[1]);
    jcoord = floor(rem / dim[0]);
    icoord = rem % dim[0];
    //printf("voxel %d (%d,%d,%d)\n",voxelId,icoord,jcoord,kcoord);
    gsl_matrix_set(positions,i,0,icoord);
    gsl_matrix_set(positions,i,1,jcoord);
    gsl_matrix_set(positions,i,2,kcoord);
    if (icoord > max[0]) max[0] = icoord;
    if (jcoord > max[1]) max[1] = jcoord;
    if (kcoord > max[2]) max[2] = kcoord;
    if (icoord < min[0]) min[0] = icoord;
    if (jcoord < min[1]) min[1] = jcoord;
    if (kcoord < min[2]) min[2] = kcoord;
    mean[0] += icoord;
    mean[1] += jcoord;
    mean[2] += kcoord;
  }
  mean[0] /= voxelCount;
  mean[1] /= voxelCount;
  mean[2] /= voxelCount;
}

/**
\brief Calculate the data from the image

\author David Worth, STFC
\date May 2012
\todo This only works for unsigned short image type. Make it do other integer
datatypes.

\param inf Pointer to object that holds data for this plugin. The voxel size
can be obtained from it.
\param pds Pointer to object that carries information on data set to be
processed. Includes actual buffer of voxel data, number of voxels along each
dimension, voxel spacing and voxel type.
*/
static int ProcessData(void *inf, vtkVVProcessDataStruct *pds)
{
  /* Information about the plugin and the image */
  vtkVVPluginInfo *info = (vtkVVPluginInfo *)inf;
  /* Number of voxels in each direction of image */
  int *dim = info->InputVolumeDimensions;
  /* Size of datatype in the image */
  unsigned int rsize = info->InputVolumeScalarSize;
  /* Voxel size - can be set by user in VolView */
  double voxelSize = atof(info->GetGUIProperty(info, 0, VVP_GUI_VALUE));
  /* Minimum and maximum raw values from image. */
  double imgMin = info->InputVolumeScalarRange[0],
         imgMax = info->InputVolumeScalarRange[1];
  /* Array of voxel counts for each value. Array index is the value. */
  int maxVoxelVals = (int)ceil(imgMax)-(int)floor(imgMin)+1;
  int *voxelCounts = malloc(sizeof(int)*maxVoxelVals);
  /* Array of arrays - each sub-array holding the ids of voxels with that value */
  int* *voxelArray = malloc(sizeof(int*)*maxVoxelVals);
  /* Pointer to sub array used when creating voxelArray */
  int *voxelSubArray = NULL;
  /* Index array used to write into voxelSubArray */
  int *indexVoxelSubArray = malloc(sizeof(int)*maxVoxelVals);
  /* Number of the non-background voxels */
  int sumVoxels = 0;
  /* Number of different voxel values */
  int totalParticles = 0;
  /* Array of (i,j,k) coordinates for voxels with a particular value */
  gsl_matrix *positions = NULL;
  /* Stats for voxel data for each value */
  int max[3], min[3], mean[3], boundingBoxSize[3];
  double boundingBoxDiam;
  /* Multiplication factors based on physical voxel size */
  double areaFactor = pow(voxelSize,2), volumeFactor = pow(voxelSize,3);
  /* Output data */
  int voxelLabel, voxelCount;
  double voxelCountVolume, voxelCountDia;
  /* Data used when covariance matrix is calculated */
  double* *matrix = NULL;
  double *row = NULL;
  double* *covMatrix = NULL;
  /* Data for eigenanalysis of covariance matrix */
  gsl_matrix *gslCovMatrix = NULL;
  gsl_vector *eval = NULL;
  gsl_matrix *evecs = NULL;
  gsl_eigen_symmv_workspace *workspace = NULL;
  gsl_vector vector;
  int result;

  void *gslcblas_handle, *gsl_handle;

  /* Buffer for output to user */
  char buffer[1000];
  int i, j = 0;

  /* The raw image data */
  unsigned short *ptr = (unsigned short *)pds->inData;

  /* Check the data type */
  switch (info->InputVolumeScalarType) {
    case VTK_CHAR:
    case VTK_UNSIGNED_CHAR:
    case VTK_FLOAT:
    case VTK_DOUBLE: {
      sprintf(buffer, "Image datatype must be integer or boolean. This \
      algorithm cannot continue");
      info->SetProperty(info, VVP_ERROR, buffer);
      return -1;
      break;
    }
  }

  printf("Image has type %d size %d\n", info->InputVolumeScalarType,rsize);
  printf("The dimensions of the image are: %d, %d, %d \n",
          dim[0], dim[1], dim[2]);
  sprintf(buffer, "The dimensions of the image are: %d, %d, %d \n",
          dim[0], dim[1], dim[2]);
  printf("There are %d components\n",info->InputVolumeNumberOfComponents);
  printf("The minimum value in the image is %d\n", (int)imgMin);
  printf("and the maximum value is %d\n", (int)imgMax);

  /* Sort the image values */
  /*qsort(ptr, (size_t)(dim[0]*dim[1]*dim[2]), (size_t)info->InputVolumeScalarSize,
        (__compar_fn_t)cmpfun);*/

  /*
   * Create array of voxel counts for each voxel value except the min value
   * which we take to be the background
   */
  for (i = 0; i < maxVoxelVals; i++) voxelCounts[i]=0;
  for (i = 0; i < dim[0]*dim[1]*dim[2]; i++) {
    if (ptr[i] != imgMin) { /* Not the minimum value */
      if (ptr[i] > imgMax) printf("%d %d\n",i,ptr[i]); /* Something wrong */
      /* Not seen this value before so it's a new particle */
      if (voxelCounts[ptr[i]] == 0) totalParticles++;
      voxelCounts[ptr[i]]++;
      sumVoxels++;
    }
  }

  /* Create the array of arrays with voxel ids for each voxel value */
  for (i = 0; i < maxVoxelVals; i++) {
    if (voxelCounts[i] > 0) {
      /*printf("Non zero voxel count for i = %d - count is %d\n",i,voxelCounts[i]);*/
      voxelArray[i] = malloc(voxelCounts[i]*sizeof(int));
    }
    else voxelArray[i] = NULL;
    indexVoxelSubArray[i] = 0;
  }
  /* Now populate that array with values */
  for (i = 0; i < dim[0]*dim[1]*dim[2]; i++) {
    if (ptr[i] != imgMin) {
      //printf("add entry to voxel array %d - entry id %d\n",ptr[i],i);
      voxelSubArray = voxelArray[ptr[i]];
      voxelSubArray[ indexVoxelSubArray[ptr[i]] ] = i;
      indexVoxelSubArray[ptr[i]]++;
    }
  }

  printf("Porosity is %f\n",(double)sumVoxels/(double)(dim[0]*dim[1]*dim[2]));
  sprintf(buffer+strlen(buffer), "Porosity is %f\n",
          (double)sumVoxels/(double)(dim[0]*dim[1]*dim[2]));
  printf("Number of particles is %d\n",totalParticles);

  /* Load the GSL BLAS and GSL itself. RTLD_GLOBAL means we can use the
   * function names from GSL rather than having to get function pointers with
   * dlsym. */
  gslcblas_handle = dlopen("/usr/lib/libgslcblas.so", RTLD_LAZY|RTLD_GLOBAL);
  if (!gslcblas_handle) {
    printf("Error opening libgslcblas.so - %s\n",dlerror());
  }
  gsl_handle = dlopen("/usr/lib/libgsl.so", RTLD_LAZY|RTLD_GLOBAL);
  if (!gsl_handle) {
    printf("Error opening libgsl.so - %s\n",dlerror());
  }

  /* Allocate the GSL matrices and vectors for eigenanalysis */
  gslCovMatrix = gsl_matrix_alloc(3,3);
  eval = gsl_vector_alloc(3);
  evecs = gsl_matrix_alloc(3,3);
  workspace = gsl_eigen_symmv_alloc(3);

  /* Loop over the particles */
  for (i = 0; i < 1/*totalParticles*/; i++) {
    /* Find the next particle with a non-zero voxel count */
    while ((j < maxVoxelVals) && (voxelCounts[j] == 0)) j++;
    /* Do some simple stuff */
    voxelLabel = j;
    voxelCount = voxelCounts[j];
    voxelCountVolume = voxelCount * volumeFactor;
    voxelCountDia = voxelSize*vol2dia(voxelCount);
    printf("%d, %d, %f, %f",voxelLabel,voxelCount,voxelCountVolume,
      voxelCountDia);

    /* Find the (i,j,k) coordinates in the image of the voxels */
    positions = gsl_matrix_alloc(voxelCount,3);
    voxelSubArray = voxelArray[voxelLabel];
    getCoords(voxelSubArray, voxelCount, dim, positions, min, max, mean);
    /*printf(" (%d, %d, %d)",min[0],min[1],min[2]);
    printf(" (%d, %d, %d)",max[0],max[1],max[2]);
    printf(" (%d, %d, %d)",mean[0],mean[1],mean[2]);*/
    boundingBoxSize[0] = max[0]-min[0]+1;
    boundingBoxSize[1] = max[1]-min[1]+1;
    boundingBoxSize[2] = max[2]-min[2]+1;
    boundingBoxDiam = sqrt(boundingBoxSize[0]*boundingBoxSize[0] +
                           boundingBoxSize[1]*boundingBoxSize[1] +
                           boundingBoxSize[2]*boundingBoxSize[2])*voxelSize;
    printf(" (%d, %d, %d)",boundingBoxSize[0],boundingBoxSize[1],
                           boundingBoxSize[2]);
    printf(" %f", boundingBoxDiam);

    /* Calculate positions-mean */
    matrix = malloc(voxelCount*sizeof(double*));
    for (j = 0; j < voxelCount; j++) {
      row = malloc(3*sizeof(double));
      row[0] = gsl_matrix_get(positions,j,0)-mean[0];
      row[1] = gsl_matrix_get(positions,j,1)-mean[1];
      row[2] = gsl_matrix_get(positions,j,2)-mean[2];
      matrix[j] = row;
    }
    covMatrix = CovMat(matrix, voxelCount, 3);

    for (j = 0; j < voxelCount; j++) {
      free(matrix[j]);
    }
    free(matrix);

    /*printf("\nCovariance matrix\n");
    for (j = 0; j < 3; j++) {
      printf("%f %f %f\n", covMatrix[j][0], covMatrix[j][1], covMatrix[j][2]);
    }*/

    /* To do the eigenanalysis use GNU Scientific library so create matrix
     * in that format */
    for (j = 0; j < 3; j++) {
      gsl_matrix_set(gslCovMatrix,0,j,covMatrix[0][j]);
      gsl_matrix_set(gslCovMatrix,1,j,covMatrix[1][j]);
      gsl_matrix_set(gslCovMatrix,2,j,covMatrix[2][j]);
    }

    /* Calculate eigenvalues and vectors */
    result = gsl_eigen_symmv(gslCovMatrix, eval, evecs, workspace);

    printf("\nEigenvalues\n");
    gsl_vector_fprintf(stdout, eval, "%g");
    printf("\nEigenvectors\n");
    vector = gsl_matrix_const_column(evecs,0).vector;
    gsl_vector_fprintf(stdout, &vector, "%g");
    printf("\n");
    vector = gsl_matrix_const_column(evecs,1).vector;
    gsl_vector_fprintf(stdout, &vector, "%g");
    printf("\n");
    vector = gsl_matrix_const_column(evecs,2).vector;
    gsl_vector_fprintf(stdout, &vector, "%g");

    for (j = 0; j < 3; j++) {
      free(covMatrix[j]);
    }
    free(covMatrix);

    gsl_matrix_free(positions);

    printf("\n");
    /* Move on to next particle */
    j++;
  }

  dlclose(gsl_handle);
  dlclose(gslcblas_handle);

  info->SetProperty(info, VVP_REPORT_TEXT, buffer);

  free(voxelCounts);
  for (i = 0; i < maxVoxelVals; i++) {
    if (voxelArray[i] != NULL) free(voxelArray[i]);
  }

  gsl_matrix_free(gslCovMatrix);
  gsl_vector_free(eval);
  gsl_matrix_free(evecs);
  gsl_eigen_symmv_free(workspace);

  free(voxelArray);
  free(indexVoxelSubArray);
  return 0;
}

/**
\brief Update the VolView GUI to display user parameters.

Sets one GUI parameter - the physical size of the voxels in the image.

\param inf Pointer to object that should be modified to set up GUI elements for
this plugin. It also contains details of the input and output images.
*/
static int UpdateGUI(void *inf)
{
  int i;
  vtkVVPluginInfo *info = (vtkVVPluginInfo *)inf;

  info->SetGUIProperty(info, 0, VVP_GUI_LABEL, "Voxel Size");
  info->SetGUIProperty(info, 0, VVP_GUI_TYPE, VVP_GUI_SCALE);
  info->SetGUIProperty(info, 0, VVP_GUI_DEFAULT , "1");
  info->SetGUIProperty(info, 0, VVP_GUI_HELP,
               "The physical size of a voxel in the image");

  /* what range should we show for possible output values */
  vvPluginSetGUIScaleRange(0);

  info->OutputVolumeScalarType = info->InputVolumeScalarType;
  info->OutputVolumeNumberOfComponents = info->InputVolumeNumberOfComponents;
  for (i = 0; i < 3; i++)
    {
    info->OutputVolumeDimensions[i] = info->InputVolumeDimensions[i];
    info->OutputVolumeSpacing[i] = info->InputVolumeSpacing[i];
    info->OutputVolumeOrigin[i] = info->InputVolumeOrigin[i];
    }

  return 1;
}

/**
\brief Initialise this plugin.

Sets the name and group for this plugin in the plugin list shown to the VolView
user and gives some documentation. Also defines properties so VolView can judge
the memory requirements and potential for undoing this plugin.

\param info Pointer to object that should be modified to give details about this
plugin.
*/
void VV_PLUGIN_EXPORT vvQuan3DCInit(vtkVVPluginInfo *info)
{
  /* always check the version */
  vvPluginVersionCheck();

  /* setup information that never changes */
  info->ProcessData = ProcessData;
  info->UpdateGUI = UpdateGUI;
  info->SetProperty(info, VVP_NAME, "Quantify 3D in C");
  info->SetProperty(info, VVP_GROUP, "Quantification");
  info->SetProperty(info, VVP_TERSE_DOCUMENTATION,
     "Quantify several characteristics from a labelled image");
  info->SetProperty(info, VVP_FULL_DOCUMENTATION,
    "Quantify the following characteristics from a labelled image: Volume by \
voxel counts, Equivalent sphere diameter by voxel counts, Bounding box \
diagonal, Principal Component Analysis, Ellipsoid fitting by PCA, Equivalent \
circle diameter by PCA, Isosurface by marching cube, Surface area, Surface \
volume, Equivalent sphere diameter from surface volume, Sphercity, Normalised \
surface area to volume ratio");

  info->SetProperty(info, VVP_SUPPORTS_IN_PLACE_PROCESSING, "1");
  info->SetProperty(info, VVP_SUPPORTS_PROCESSING_PIECES,   "1");
  info->SetProperty(info, VVP_REQUIRED_Z_OVERLAP,           "0");
  info->SetProperty(info, VVP_NUMBER_OF_GUI_ITEMS,          "1");
  info->SetProperty(info, VVP_REQUIRES_SERIES_INPUT,        "0");
  info->SetProperty(info, VVP_SUPPORTS_PROCESSING_SERIES_BY_VOLUMES, "0");
  info->SetProperty(info, VVP_PRODUCES_OUTPUT_SERIES, "0");
  info->SetProperty(info, VVP_PRODUCES_PLOTTING_OUTPUT, "0");
}


#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

int
main (void)
{
  int i;
  double x = 0, y = 0, dx, dy;

  const gsl_rng_type * T;
  gsl_rng * r;

  gsl_rng_env_setup();
  T = gsl_rng_default;
  r = gsl_rng_alloc (T);

  printf ("%g %g\n", x, y);

  for (i = 0; i < 10; i++)
    {
      gsl_ran_dir_2d (r, &dx, &dy);
      x += dx; y += dy;
      printf ("%g %g\n", x, y);
    }

  gsl_rng_free (r);
  return 0;
}


/*
* -----------------------------------------------------------------
*  bst_lib.h
*  Binary Search Tree Library
*  Version: 2.0
*  Last Update: Oct 9, 2019
*
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010-2019, Americo Barbosa da Cunha Junior
*  All rights reserved.
* -----------------------------------------------------------------
*  This is the header file for BST_LIB module, a computational
*  library with routines to manipulate a binary search tree.
* -----------------------------------------------------------------
*/

#ifndef __BST_LIB_H__
#define __BST_LIB_H__

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

#undef  BST_RIGHT
#define BST_RIGHT 1

#undef  BST_LEFT
#define BST_LEFT 0

/*
*------------------------------------------------------------
*   structure of binary search tree leaf
*
*   last update: Feb 2, 2009
*------------------------------------------------------------
*/

typedef struct leaf
{
    gsl_vector *phi;   /* composition */
    gsl_vector *Rphi;  /* reaction mapping */
    gsl_matrix *A;     /* mapping gradient matrix */
    gsl_matrix *L;     /* ellipsoid Cholesky matrix */
} bst_leaf;
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   structure of binary search tree node
*
*   last update: Feb 2, 2009
*------------------------------------------------------------
*/

typedef struct node
{
    gsl_vector *v;          /* cutting plane normal vector */
    double a;               /* scalar a */
    struct node *r_node;    /* right node */
    struct node *l_node;    /* left node */
    struct leaf *r_leaf;    /* right leaf */
    struct leaf *l_leaf;    /* left leaf */
} bst_node;
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
* function prototypes
*------------------------------------------------------------
*/

bst_leaf *bst_leaf_alloc();

bst_node* bst_node_alloc();

void bst_leaf_free(void **leaf_bl);

void bst_node_free(void **node_bl);

int bst_leaf_set(gsl_vector *phi,
                 gsl_vector *Rphi,
                 gsl_matrix *A,
                 gsl_matrix *L,
                 bst_leaf *leaf);

double bst_cutplane(gsl_vector *phi_l,
                    gsl_vector *phi_r,
                    gsl_vector *v);

int bst_node_set(bst_leaf *l_leaf,
                 bst_leaf *r_leaf,
                 bst_node *node);

int bst_node_add(unsigned int side,
                 bst_node *old_node,
                 bst_node *new_node);

int bst_search(bst_node *root,
               gsl_vector *phi,
               bst_node **end_node,
               bst_leaf **end_leaf);

int bst_height(bst_node *root);

#endif /* __BST_LIB_H__ */


#include <fftw.h>
#include <stdio.h>
#include <stdlib.h>
#define N 16
main()
{
     int i;
     fftw_complex in[N], out[N];
     float fact;
     fftw_plan p,p2;
printf(" stuff in data\n");
     for (i=0;i<N;i++) {
     	in[i].re=(i*i);
     	in[i].im=1;
     }
printf(" create plans\n");
     p = fftw_create_plan(N, FFTW_FORWARD,  FFTW_ESTIMATE);
     p2= fftw_create_plan(N, FFTW_BACKWARD, FFTW_ESTIMATE);
printf(" do it\n");
     fftw_one(p, in, out);
     for (i=0;i<N;i++) {
	printf("%12.4f %12.4f\n",out[i].re,out[i].im);
     }
printf(" \n");
printf(" undo it\n");
     fftw_one(p2, out,in);
     fact=1.0/(float)N;
     for (i=0;i<N;i++) {
	printf("%10.2f %10.2f\n",in[i].re*fact,in[i].im*fact);
     }
printf(" clean up\n");
     fftw_destroy_plan(p);
     fftw_destroy_plan(p2);
}


#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#define COMPEARTH_PRIVATE_UPDOWN_ARGSORT3 1
#define COMPEARTH_PRIVATE_UPDOWN_ABS_ARGSORT3 1
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_lapacke.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <lapacke.h>
#endif

/*
static int argsort3_upDown(const double *__restrict__ x,
                           const bool lascend,
                           int *__restrict__ iperm);
static int argsort3(const double *__restrict__ x, int *__restrict__ iperm);
*/

#define LWORK 102 //(NB+2)*N hence NB=32; must be atleast 3*N-1 which is 8

/*!
 * @brief Decomposes a set of moment tensors into eigenvalues + basis.
 *
 * @param[in] nmt    Number of moment tensors.
 * @param[in] M      6 x n moment tensors with some unspecified basis
 *                   (e.g., up-south-east) packed
 *                    M = [M11 M22 M33 M12 M13 M23].
 * @param[in] isort  Sorting of eigenvalues. \n
 *                   If 1 then the eigenvalues are sorted highest to lowest. \n
 *                   If 2 then the eigenvalues are sorted lowest to highest. \n
 *                   If 3 then the eigenvalues are sorted by absolute value
 *                   from highest to lowest. \n
 *                   If 4 then the eigenvalues are sorted by absolute value
 *                   from lowest to highest.
 *
 * @param[out] lam   3 x n set of eigenvalues.
 * @param[out] U     3 x 3 x n set of bases.  The first 3 x 3 matrix is in
 *                   column major order.  This has the same basis as M.
 *
 * @author Carl Tape.  Converted to C by Ben Baker.
 *
 * @copyright MIT
 *
 * @bug The
 */
int compearth_CMTdecom(const int nmt, const double *__restrict__ M,
                       const int isort,
                       double *__restrict__ lam,
                       double *__restrict__ U)
{
    double Lams[3], Mx[9], Ut[9], work[LWORK];
    int perm[3], c, i, ierr, info, r;
    // Error checks
    if (nmt < 1 || M == NULL || lam == NULL || U == NULL)
    {
        if (nmt < 1){fprintf(stderr, "%s: Error no mts\n", __func__);}
        if (M == NULL){fprintf(stderr, "%s: Error M is NULL\n", __func__);}
        if (lam == NULL){fprintf(stderr, "%s: Error lam is NULL\n", __func__);}
        if (U == NULL){fprintf(stderr, "%s: Error U is NULL\n", __func__);}
        return -1;
    }
    if (isort < 1 || isort > 4)
    {
        fprintf(stderr, "%s: Error isort=%d is invalid\n", __func__, isort);
        return -1;
    }
    // Decompose the moment tensors
    for (i=0; i<nmt; i++)
    {
        // Create the symmetric moment tensor matrix from the 6x1 vector
        compearth_Mvec2Mmat(1, M, 1, Mx);
        // Compute eigenvalues in ascending order
        info = LAPACKE_dsyev_work(LAPACK_COL_MAJOR, 'V', 'U', 3, Mx, 3,
                                  Lams, work, LWORK);
        if (info != 0)
        {
            fprintf(stderr, "%s: Error computing eigenvalues\n", __func__);
            return -1;
        }
        // Descending
        if (isort == 1)
        {
            argsort3_upDown(Lams, false, perm);
        }
        else if (isort == 2)
        {
            argsort3_upDown(Lams, true, perm);
        }
        // Descending on absolute value
        else if (isort == 3)
        {
            argsort3_absUpDown(Lams, false, perm);
        }
        else //if (isort == 4)
        {
            argsort3_absUpDown(Lams, true,  perm);
        }
        // And permute and save
        for (r=0; r<3; r++)
        {
            lam[3*i+r] = Lams[perm[r]];
        }
        // Permute columns
        for (c=0; c<3; c++)
        {
            for (r=0; r<3; r++)
            {
                Ut[3*c+r] = Mx[3*perm[c]+r];
            }
        }
        // Ensure the determinant is > 0
        ierr = compearth_Udetcheck(1, Ut, &U[9*i]);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error checking determinant\n", __func__);
            return -1;
        }
    }
    return 0;
}

/*
static int argsort3_upDown(const double *__restrict__ x,
                           const bool lascend,
                           int *__restrict__ iperm)
{
    int ipermt[3], ierr;
    if (lascend)
    {
        ierr = argsort3(x, iperm);
    }
    else
    {
        ierr = argsort3(x, ipermt);
        iperm[0] = ipermt[2];
        iperm[1] = ipermt[1];
        iperm[2] = ipermt[0];
    }
    return ierr;
}

static int argsort3(const double *__restrict__ x, int *__restrict__ iperm)
{
    int i, temp;
    const int a = 0;
    const int b = 1;
    const int c = 2;
    // Copy
    iperm[a] = a;
    iperm[b] = b;
    iperm[c] = c;
    if (x[iperm[a]] > x[iperm[c]])
    {
        temp = iperm[c];
        iperm[c] = iperm[a];
        iperm[a] = temp;
    }
    if (x[iperm[a]] > x[iperm[b]])
    {
        temp = iperm[b];
        iperm[b] = iperm[a];
        iperm[a] = temp;
    }
    //Now the smallest element is the first one. Just check the 2-nd and 3-rd
    if (x[iperm[b]] > x[iperm[c]])
    {
        temp = iperm[c];
        iperm[c] = iperm[b];
        iperm[b] = temp;
    }
    // Verify
    for (i=1; i<3; i++)
    {
        if (x[iperm[i-1]] > x[iperm[i]])
        {
            fprintf(stderr, "%s: Failed to sort numbers in ascending order\n",
                     __func__);
            return -1;
        }
    }
    return 0;
}
*/


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

/* Author:  L. Johnson */

#include <gsl/gsl_test.h>
#include <gsl/gsl_sf.h>
#include "test_sf.h"

#define NVAL 100

static double c[NVAL];

int test_mathieu(void)
{
  gsl_sf_result r;
  gsl_sf_mathieu_workspace *work;
  int s = 0;
  int sa;

  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 0.0, 0.0, &r),
          0.7071067811865475, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 0.0, M_PI_2, &r),
          0.7071067811865475, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 5.0, 0.0, &r),
          0.04480018165188902, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 5.0, M_PI_2, &r),
          1.334848674698019, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 10.0, 0.0, &r),
          0.007626517570935782, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 10.0, M_PI_2, &r),
          1.468660470712856, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 15.0, 0.0, &r),
          0.001932508315204592, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 15.0, M_PI_2, &r),
          1.550108146686649, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 20.0, 0.0, &r),
          0.0006037438292242197, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 20.0, M_PI_2, &r),
          1.609890857395926, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 25.0, 0.0, &r),
          0.0002158630184146612, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (0, 25.0, M_PI_2, &r),
          1.657510298323475, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (1, 0.0, 0.0, &r),
          1.00000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (1, 5.0, 0.0, &r),
          0.2565428793223637, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (1, 10.0, 0.0, &r),
          0.05359874774717657, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (1, 15.0, 0.0, &r),
          0.01504006645382623, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (1, 20.0, 0.0, &r),
          0.005051813764712904, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (1, 25.0, 0.0, &r),
          0.001911051506657645, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (1, 0.0, M_PI_2, &r),
          1.0000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (1, 5.0, M_PI_2, &r),
          1.337433887022345, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (1, 10.0, M_PI_2, &r),
          1.468755664102938, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (1, 15.0, M_PI_2, &r),
          1.550115074357552, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (1, 20.0, M_PI_2, &r),
          1.609891592603772, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (1, 25.0, M_PI_2, &r),
          1.657510398374516, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 0.0, 0.0, &r),
          1.00000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 0.0, M_PI_2, &r),
          -1.00000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 5.0, 0.0, &r),
          0.7352943084006845, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 5.0, M_PI_2, &r),
          -0.7244881519676682, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 10.0, 0.0, &r),
          0.2458883492913189, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 10.0, M_PI_2, &r),
          -0.9267592641263211, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 15.0, 0.0, &r),
          0.07879282784639313, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 15.0, M_PI_2, &r),
          -1.019966226030262, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 20.0, 0.0, &r),
          0.02864894314707431, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 20.0, M_PI_2, &r),
          -1.075293228779687, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 25.0, 0.0, &r),
          0.0115128663308875, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (2, 25.0, M_PI_2, &r),
          -1.116278953295253, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (5, 0.0, 0.0, &r),
          1.00000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (5, 5.0, 0.0, &r),
          1.12480725063848, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (5, 10.0, 0.0, &r),
          1.258019941308287, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (5, 15.0, 0.0, &r),
          1.193432230413072, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (5, 20.0, 0.0, &r),
          0.9365755314226215, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (5, 25.0, 0.0, &r),
          0.6106943100506986, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (5, 0.0, M_PI_2, &r),
          1.0000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (5, 5.0, M_PI_2, &r),
          0.9060779302023551, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (5, 10.0, M_PI_2, &r),
          0.8460384335355106, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (5, 15.0, M_PI_2, &r),
          0.837949340012484, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (5, 20.0, M_PI_2, &r),
          0.8635431218533667, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (5, 25.0, M_PI_2, &r),
          0.8992683245108413, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 0.0, 0.0, &r),
          1.00000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 0.0, M_PI_2, &r),
          -1.00000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 5.0, 0.0, &r),
          1.025995027089438, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 5.0, M_PI_2, &r),
          -0.975347487235964, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 10.0, 0.0, &r),
          1.053815992100935, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 10.0, M_PI_2, &r),
          -0.9516453181789554, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 15.0, 0.0, &r),
          1.084106311839221, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 15.0, M_PI_2, &r),
          -0.9285480638845388, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 20.0, 0.0, &r),
          1.117788631259397, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 20.0, M_PI_2, &r),
          -0.9057107845940974, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 25.0, 0.0, &r),
          1.156239918632239, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (10, 25.0, M_PI_2, &r),
          -0.8826919105636903, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (15, 0.0, 0.0, &r),
          1.00000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (15, 5.0, 0.0, &r),
          1.011293732529566, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (15, 10.0, 0.0, &r),
          1.022878282438181, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (15, 15.0, 0.0, &r),
          1.034793652236873, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (15, 20.0, 0.0, &r),
          1.047084344162887, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_ce_e, (15, 25.0, 0.0, &r),
          1.059800441813937, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (15, 0.0, M_PI_2, &r),
          -1.0000000, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (15, 5.0, M_PI_2, &r),
          -0.9889607027406357, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (15, 10.0, M_PI_2, &r),
          -0.9781423471832157, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (15, 15.0, M_PI_2, &r),
          -0.9675137031854538, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (15, 20.0, M_PI_2, &r),
          -0.9570452540612817, TEST_SNGL, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_mathieu_se_e, (15, 25.0, M_PI_2, &r),
          -0.9467086958780897, TEST_SNGL, GSL_SUCCESS);

  work = gsl_sf_mathieu_alloc(NVAL, 20.0);
  sa = 0;
  gsl_sf_mathieu_ce_array(0, 5, 0.0, M_PI_2, work, c);
  sa += (test_sf_frac_diff(c[0], 0.7071067811865475) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[2], -1.0) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[4], 1.0) > TEST_SNGL);
  gsl_test(sa, "gsl_sf_mathieu_ce_array");
  s += sa;

  sa = 0;
  gsl_sf_mathieu_ce_array(0, 15, 20.0, 0.0, work, c);
  sa += (test_sf_frac_diff(c[0], 0.0006037438292242197) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[1], 0.005051813764712904) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[2], 0.02864894314707431) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[5], 0.9365755314226215) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[10], 1.117788631259397) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[15], 1.047084344162887) > TEST_SNGL);
  gsl_test(sa, "gsl_sf_mathieu_ce_array");
  s += sa;

  sa = 0;
  gsl_sf_mathieu_se_array(1, 15, 20.0, M_PI_2, work, c);
  sa += (test_sf_frac_diff(c[0], 1.609891592603772) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[4], 0.8635431218533667) > TEST_SNGL);
  sa += (test_sf_frac_diff(c[14], -0.9570452540612817) > TEST_SNGL);
  gsl_test(sa, "gsl_sf_mathieu_se_array");
  s += sa;

  gsl_sf_mathieu_free(work);
  return s;
}


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

static double fun(double x) {
    return x + cos(x * x);
}

int main(void) {
    const double a = 1.0;
    const double b = 10.0;
    const int steps = 10;
    double xi, yi, x[100], y[100], dx;
    FILE *input, *output;
    int i;
    const gsl_interp_type *ts[] = {
            gsl_interp_linear,
            gsl_interp_polynomial,
            gsl_interp_cspline,
            gsl_interp_cspline_periodic,
            gsl_interp_akima,
            gsl_interp_akima_periodic,
            gsl_interp_steffen,
    };
    char *tsn[] = {
            "gsl_interp_linear.txt",
            "gsl_interp_polynomial.txt",
            "gsl_interp_cspline.txt",
            "gsl_interp_cspline_periodic.txt",
            "gsl_interp_akima.txt",
            "gsl_interp_akima_periodic.txt",
            "gsl_interp_steffen.txt",
    };
    int tsc = sizeof(ts) / sizeof(const gsl_interp_type *);
    input = fopen("wartosci.txt", "w");
    dx = (b - a) / (double) steps;
    for (i = 0; i <= steps; ++i) {
        x[i] = a + (double) i * dx + 0.5 * sin((double) i * dx);
        y[i] = fun(x[i]);
        fprintf(input, "%g %g\n", x[i], y[i]);
    }
    for (i = 0; i < tsc; i++) {
        output = fopen(tsn[i], "w");
        gsl_interp_accel *acc = gsl_interp_accel_alloc();
        gsl_spline *spline = gsl_spline_alloc(ts[i], steps + 1);
        gsl_spline_init(spline, x, y, steps + 1);
        for (xi = a; xi <= b; xi += 0.01) {
            yi = gsl_spline_eval(spline, xi, acc);
            fprintf(output, "%g %g\n", xi, yi);
        }
        gsl_spline_free(spline);
        gsl_interp_accel_free(acc);
    }
    return EXIT_SUCCESS;
}


#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpRNG.h>
#include <gbpMCMC.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_interp.h>

void set_MCMC_covariance(MCMC_info *MCMC, double *V) {
    int i_P, j_P;
    if(V == NULL)
        SID_log("Initializing the covariance matrix...", SID_LOG_OPEN);
    else
        SID_log("Updating the covariance matrix...", SID_LOG_OPEN);
    if(MCMC->V == NULL)
        MCMC->V = (double *)SID_malloc(sizeof(double) * MCMC->n_P * MCMC->n_P);
    if(MCMC->m == NULL)
        MCMC->m = gsl_matrix_calloc(MCMC->n_P, MCMC->n_P);
    if(MCMC->b == NULL)
        MCMC->b = gsl_vector_calloc(MCMC->n_P);
    if(V == NULL) {
        for(i_P = 0; i_P < MCMC->n_P; i_P++) {
            for(j_P = 0; j_P < MCMC->n_P; j_P++) {
                if(i_P == j_P) {
                    // Matrix decomposition fails if we initialize an element to zero here.
                    //   Check for this and start with an different value if true.
                    if(MCMC->P_init[i_P] == 0.)
                        MCMC->V[i_P * MCMC->n_P + j_P] = 1e-3 * MCMC->P_limit_max[i_P];
                    else
                        MCMC->V[i_P * MCMC->n_P + j_P] = pow(MCMC->P_init[i_P], 2.);
                } else
                    MCMC->V[i_P * MCMC->n_P + j_P] = 0.;
            }
        }
    } else
        memcpy(MCMC->V, V, (size_t)(MCMC->n_P * MCMC->n_P) * sizeof(double));
    for(i_P = 0; i_P < MCMC->n_P; i_P++)
        for(j_P = 0; j_P < MCMC->n_P; j_P++)
            gsl_matrix_set(MCMC->m, i_P, j_P, MCMC->V[i_P * MCMC->n_P + j_P]);
    gsl_linalg_cholesky_decomp(MCMC->m);

    // Set the upper-diagonal to zero to get L where covariance=L*L^T
    for(i_P = 0; i_P < MCMC->n_P; i_P++)
        for(j_P = i_P + 1; j_P < MCMC->n_P; j_P++)
            gsl_matrix_set(MCMC->m, i_P, j_P, 0.);
    SID_log("Done.", SID_LOG_CLOSE);
}


#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_permutation.h>
#include "invwishpdf.h"

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

	int args;
	int m, n;
	double pdf, dof;

	args = 1;

	m = atoi(argv[args++]);
	n = atoi(argv[args++]);

	// allocate space for matrix X, test_copy, and Scale.
	gsl_matrix *X = gsl_matrix_alloc(m, n);
	gsl_matrix *inv = gsl_matrix_alloc(m,n);
	gsl_matrix *Scale = gsl_matrix_alloc(m, n);

	gsl_matrix_set_identity(Scale);
	fill_matrix(X);

	dof = m + 1;
	pdf = iwishpdf(X, Scale, inv, dof);

	// Print the data
	printf("\n%s", "Inverse Wishart matrix");
	print_matrix(X);
	printf("\n%s", "Scale matrix");
	print_matrix(Scale);
	printf("\n%s", "Wishart matrix");
	print_matrix(inv);
	printf("\n%s", "Density");
	printf("\n%f\n", pdf);

	return(0);
}

gsl_matrix fill_matrix(gsl_matrix *X)
{
	int i,j;
	int step = 0;
	int n = X->size1;
	int m = X->size2;

	// Create a test matrix
	double a_vect[] = {0.17955079, -0.09332222, -0.01273225, -0.09332222,  0.28282278,  0.01724885, -0.01273225,  0.01724885,  0.14424177};
	for (i = 0; i < m; ++i){
		for (j = 0; j < n; ++j){
			gsl_matrix_set(X, i, j, a_vect[step++]);
		}
	}
	return(*X);
}

double iwishpdf(gsl_matrix *X, gsl_matrix *Scale, gsl_matrix *inv, double dof)
{
	double X_det, scale_det, denom, pdf, trace, numerator;
	int m = X->size1;
	int n = X->size2;

	// Allocate matrix for inv(X)
	gsl_matrix *for_mult = gsl_matrix_alloc(m, n);

	// Get determinant of X and Scale matrix
	X_det = matrix_determ(X);
	scale_det = matrix_determ(Scale);

	// Invert X
	inv_matrix(X,inv);

	// Multiple Scale * inv(X)
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Scale, inv, 1.0, for_mult);

	// Get trace of above.
	trace = matrix_trace(for_mult);

	numerator =  pow(scale_det, dof / 2.0) * pow(X_det, (-dof-m-1)/ 2.0) * exp(-0.5 * trace);
	denom = pow(2,dof * m / 2) * mv_gamma(dof/2, m);

	pdf = (numerator/denom);

	return(pdf);
}

gsl_matrix inv_matrix(gsl_matrix *X, gsl_matrix *inv)
{
	int s;
	int n = X->size1;
	int m = X->size2;
	gsl_matrix *a_copy = gsl_matrix_alloc(m, n);
	gsl_matrix_memcpy( a_copy, X );
	gsl_permutation *P = gsl_permutation_alloc(n);
	gsl_linalg_LU_decomp(a_copy, P, &s);
	gsl_linalg_LU_invert(a_copy, P, inv);
	return(*inv);
}

double matrix_determ(gsl_matrix *X)
{
	int s;
	int n = X->size1;
	int m = X->size2;
	gsl_matrix *a_copy = gsl_matrix_alloc(m, n);
	gsl_matrix_memcpy(a_copy, X );
	gsl_permutation *P = gsl_permutation_alloc(n);
	gsl_linalg_LU_decomp(a_copy, P, &s);
	double my_det = gsl_linalg_LU_det (a_copy, s);
	return(my_det);
}

double matrix_trace(gsl_matrix *X){
	int i, m;
	m = X->size1;
	double trace = 0.0;
	for(i=0;i<m;i++){
		trace += gsl_matrix_get(X,i,i);
	}
	return(trace);
}

double mv_gamma(double a, double d){
	double val = 1.0;
	int i;
	for(i = 1; i <= d; i++){
		val *= gsl_sf_gamma(a - (0.5 * (i - 1)));
	}
	val *=  pow(M_PI, (d * (d - 1) / 4.0));
	return(val);
}

void print_matrix(gsl_matrix *X)
{
	int i, j;
	int n = X->size1;
	int m = X->size2;
	printf("\n");
	for(i=0; i<m; i++){
		for(j=0; j<n; j++){
			printf("%05.2f ", gsl_matrix_get(X, i, j));
		}
		printf("\n");
	}
}


/* cdf/negbinom.c
 *
 * Copyright (C) 2004 Jason H. Stover.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_cdf.h>

#include "error.h"

/*
 * Pr(X <= k) for a negative binomial random variable X, i.e.,
 * the probability of k or fewer failuers before success n.
 */

double
gsl_cdf_negative_binomial_P (const unsigned int k, const double p, const double n)
{
  double P;
  double a;
  double b;

  if (p > 1.0 || p < 0.0)
    {
      CDF_ERROR ("p < 0 or p > 1", GSL_EDOM);
    }

  if (n < 0)
    {
      CDF_ERROR ("n < 0", GSL_EDOM);
    }

  a = (double) n;
  b = (double) k + 1.0;
  P = gsl_cdf_beta_P (p, a, b);

  return P;
}

/*
 * Pr ( X > k ).
 */

double
gsl_cdf_negative_binomial_Q (const unsigned int k, const double p, const double n)
{
  double Q;
  double a;
  double b;

  if (p > 1.0 || p < 0.0)
    {
      CDF_ERROR ("p < 0 or p > 1", GSL_EDOM);
    }

  if (n < 0)
    {
      CDF_ERROR ("n < 0", GSL_EDOM);
    }

  a = (double) n;
  b = (double) k + 1.0;
  Q = gsl_cdf_beta_Q (p, a, b);

  return Q;
}


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

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_diff.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_test.h>

double
f1 (double x, void *params)
{
  return exp (x);
}

double
df1 (double x, void *params)
{
  return exp (x);
}

double
f2 (double x, void *params)
{
  if (x >= 0.0)
    {
      return x * sqrt (x);
    }
  else
    {
      return 0.0;
    }
}

double
df2 (double x, void *params)
{
  if (x >= 0.0)
    {
      return 1.5 * sqrt (x);
    }
  else
    {
      return 0.0;
    }
}

double
f3 (double x, void *params)
{
  if (x != 0.0)
    {
      return sin (1 / x);
    }
  else
    {
      return 0.0;
    }
}

double
df3 (double x, void *params)
{
  if (x != 0.0)
    {
      return -cos (1 / x) / (x * x);
    }
  else
    {
      return 0.0;
    }
}

double
f4 (double x, void *params)
{
  return exp (-x * x);
}

double
df4 (double x, void *params)
{
  return -2.0 * x * exp (-x * x);
}

double
f5 (double x, void *params)
{
  return x * x;
}

double
df5 (double x, void *params)
{
  return 2.0 * x;
}

double
f6 (double x, void *params)
{
  return 1.0 / x;
}

double
df6 (double x, void *params)
{
  return -1.0 / (x * x);
}

typedef int (diff_fn) (const gsl_function * f, double x, double * res, double *abserr);

void
test (diff_fn * diff, gsl_function * f, gsl_function * df, double x,
      const char * desc)
{
  double result, abserr;
  double expected = GSL_FN_EVAL (df, x);
  (*diff) (f, x, &result, &abserr);
  gsl_test_abs (result, expected, abserr, desc);
  gsl_test (fabs(result-expected) >  abserr, "%s, valid error estimate", desc);
}

int
main ()
{
  gsl_function F1, DF1, F2, DF2, F3, DF3, F4, DF4, F5, DF5, F6, DF6;

  F1.function = &f1;
  DF1.function = &df1;

  F2.function = &f2;
  DF2.function = &df2;

  F3.function = &f3;
  DF3.function = &df3;

  F4.function = &f4;
  DF4.function = &df4;

  F5.function = &f5;
  DF5.function = &df5;

  F6.function = &f6;
  DF6.function = &df6;

  test (&gsl_diff_central, &F1, &DF1, 1.0, "exp(x), x=1, central diff");
  test (&gsl_diff_forward, &F1, &DF1, 1.0, "exp(x), x=1, forward diff");
  test (&gsl_diff_backward, &F1, &DF1, 1.0, "exp(x), x=1, backward diff");

  test (&gsl_diff_central, &F2, &DF2, 0.1, "x^(3/2), x=0.1, central diff");
  test (&gsl_diff_forward, &F2, &DF2, 0.1, "x^(3/2), x=0.1, forward diff");
  test (&gsl_diff_backward, &F2, &DF2, 0.1, "x^(3/2), x=0.1, backward diff");

  test (&gsl_diff_central, &F3, &DF3, 0.45, "sin(1/x), x=0.45, central diff");
  test (&gsl_diff_forward, &F3, &DF3, 0.45, "sin(1/x), x=0.45, forward diff");
  test (&gsl_diff_backward, &F3, &DF3, 0.45, "sin(1/x), x=0.45, backward diff");

  test (&gsl_diff_central, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, central diff");
  test (&gsl_diff_forward, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, forward diff");
  test (&gsl_diff_backward, &F4, &DF4, 0.5, "exp(-x^2), x=0.5, backward diff");

  test (&gsl_diff_central, &F5, &DF5, 0.0, "x^2, x=0, central diff");
  test (&gsl_diff_forward, &F5, &DF5, 0.0, "x^2, x=0, forward diff");
  test (&gsl_diff_backward, &F5, &DF5, 0.0, "x^2, x=0, backward diff");

  test (&gsl_diff_central, &F6, &DF6, 10.0, "1/x, x=10, central diff");
  test (&gsl_diff_forward, &F6, &DF6, 10.0, "1/x, x=10, forward diff");
  test (&gsl_diff_backward, &F6, &DF6, 10.0, "1/x, x=10, backward diff");

  exit (gsl_test_summary ());
}


/// \file
/// A helper class defining numerical complex matrices for use with BLAS.
/// Needs a CBLAS compatible BLAS library!

#pragma once
#include <array>
#include <complex>
#include <iomanip>
#include <iostream>
#include <limits>

#ifndef USE_MKL
#include <cblas.h>
#else
#include <mkl.h>
#endif

// **********************************************************************
/// Numerical complex matrix class
/// \brief This class defines a complex matrix with n rows and n columns
///
/// This class implements a complex matrix. The entries are stored in an
/// std::array in row-major order, so that it is easy to implement linear
/// algebra functions using the standard CBLAS interface.
// **********************************************************************
template <std::size_t n>
class nummat {

 private:
  std::array<std::complex<double>, n * n> mat;

 public:
  // **********************************************************************
  // Constructors
  // **********************************************************************

  constexpr nummat() { mat.fill(std::complex<double>(0.0)); }
  /// Construct a matrix from an array
  constexpr nummat(std::array<std::complex<double>, n * n> const &init)
    : mat(init)
  {
  }
  constexpr nummat(std::complex<double> const &c)
  {
    mat.fill(std::complex<double>(0.0));
    for (auto i = 0LU; i < n; ++i) {
      mat[i + n * i] = c;
    }
  }

  ///! Copy constructor
  constexpr nummat(nummat<n> const &orig) = default;

  // **********************************************************************
  // Methods
  // **********************************************************************

  /// Access elements of matrix
  std::complex<double> &operator()(std::size_t const i, std::size_t const j)
  {
    return mat[i * n + j];
  }

  constexpr std::complex<double> const &operator()(std::size_t const i,
                                                   std::size_t const j) const
  {
    return mat[i * n + j];
  }

  std::complex<double> *data() noexcept { return mat.data(); }

  std::complex<double> const *data() const noexcept { return mat.data(); }

  /// Matrix multiplication
  constexpr nummat<n> operator*(nummat<n> const &rhs) const
  {

    constexpr std::complex<double> alpha(1.0);
    constexpr std::complex<double> beta(0.0);

    nummat<n> res;

    cblas_zgemm(CBLAS_ORDER::CblasRowMajor, CBLAS_TRANSPOSE::CblasNoTrans,
                CBLAS_TRANSPOSE::CblasNoTrans, n, n, n,
                reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(this->data()), n,
                reinterpret_cast<const double *>(rhs.data()), n,
                reinterpret_cast<const double *>(&beta),
                reinterpret_cast<double *>(res.data()), n);
    return res;
  }

  /// Matrix subtraction
  constexpr nummat<n> operator-(nummat<n> const &rhs) const
  {

    constexpr std::complex<double> alpha(-1.0);

    nummat<n> res(this->mat);

    cblas_zaxpy(n * n, reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(rhs.data()), 1,
                reinterpret_cast<double *>(res.data()), 1);
    return res;
  }

  /// Matrix addition
  constexpr nummat<n> operator+(nummat<n> const &rhs) const
  {

    std::complex<double> alpha(1.0);

    nummat<n> res(this->mat);

    cblas_zaxpy(n * n, reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(rhs.data()), 1,
                reinterpret_cast<double *>(res.data()), 1);
    return res;
  }

  /// Operator "+=" for matrix addition
  void operator+=(nummat<n> const &rhs)
  {

    constexpr std::complex<double> alpha(1.0);

    cblas_zaxpy(n * n, reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(rhs.data()), 1,
                reinterpret_cast<double *>(this->data()), 1);
  }

  /// Operator "-=" for matrix subtraction
  void operator-=(nummat<n> const &rhs)
  {

    constexpr std::complex<double> alpha(-1.0);

    cblas_zaxpy(n * n, reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(rhs.data()), 1,
                reinterpret_cast<double *>(this->data()), 1);
  }

  /// Scalar multiplication (from right)
  nummat<n> operator*(std::complex<double> const &rhs) const
  {

    nummat<n> res(this->mat);

    cblas_zscal(n * n, reinterpret_cast<const double *>(&rhs),
                reinterpret_cast<double *>(res.data()), 1);
    return res;
  }

  /// Operator "*=" for scalar multiplication. (Rescaling)
  void operator*=(std::complex<double> const &a)
  {
    cblas_zscal(n * n, reinterpret_cast<const double *>(&a),
                reinterpret_cast<double *>(this->data()), 1);
  }

  /// Scalar division
  constexpr nummat<n> operator/(std::complex<double> const &rhs) const
  {
    return (*this) * (1. / rhs);
  }

  /// Equality check \todo Better float equal?
  constexpr bool operator==(nummat<n> const &right) const
  {
    bool res = true;
    for (auto i = 0LU; i < n; ++i) {
      for (auto j = 0LU; j < n; ++j) {
        res = ((*this)(i, j) == right(i, j));
        if (not res)
          return res;
      }
    }
    return res;
  }

  // Multiplication with Hermitian conjugate
  constexpr nummat<n> mult_conj(nummat<n> const &rhs) const
  {

    constexpr std::complex<double> alpha(1.0);
    constexpr std::complex<double> beta(0.0);

    nummat<n> res;

    cblas_zgemm(CBLAS_ORDER::CblasRowMajor, CBLAS_TRANSPOSE::CblasNoTrans,
                CBLAS_TRANSPOSE::CblasConjTrans, n, n, n,
                reinterpret_cast<const double *>(&alpha),
                reinterpret_cast<const double *>(this->data()), n,
                reinterpret_cast<const double *>(rhs.data()), n,
                reinterpret_cast<const double *>(&beta),
                reinterpret_cast<double *>(res.data()), n);
    return res;
  }

  // Real part
  // constexpr nummat<n> real() const {

  //     nummat<n> res;

  //     for (auto i=0LU; i<n;++i){
  // 	for(auto j=0LU; j<n;++j){
  // 	    res(i,j)= this->mat[i*n+j];
  // 	}
  //     }

  //     return res;
  // }

  /// Hermitian conjugate
  constexpr nummat<n> dagger() const
  {
    nummat<n> res;

    for (auto i = 0LU; i < n; ++i) {
      for (auto j = 0LU; j < n; ++j) {
        res(i, j) = std::conj(this->mat[j * n + i]);
      }
    }

    return res;
  }

  /// Matrix inverse \todo Implement for general matrix
  constexpr nummat<n> inverse() const
  {

    if (*this == nummat<n>(1.)) {
      return nummat<n>(this->mat);
    }
    else {
      throw std::runtime_error("Inverse only implemented for unit matrix!");
    }
  }

  /// Matrix log \todo Implement for general matrix
  constexpr nummat<n> log() const
  {

    nummat<n> res(0.0);

    if (*this == nummat<n>(1.)) {
      return res;
    }
    else {
      throw std::runtime_error("Log only implemented for unit matrix!");
    }
  }

  // **********************************************************************
  // Friends
  // **********************************************************************

  /// Multiplication with scalar from the left
  friend constexpr nummat<n> operator*(std::complex<double> const &lhs,
                                       nummat<n> const &rhs)
  {
    auto res = rhs;
    return res * lhs;
  }

  /// Simple printing of nummat
  friend std::ostream &operator<<(std::ostream &stream, nummat<n> const &nm)
  {

    stream.precision(6);

    for (auto i = 0ul; i < n * n; ++i) {
      stream << std::fixed << "(" << std::setw(13) << std::real(nm.mat[i])
             << "," << std::setw(13) << std::imag(nm.mat[i]) << ")\t";

      if (0 == (i + 1) % n) {
        stream << "\n";
      }
    }
    return stream;
  }
};

// Non-member functions

/// Trace
template <std::size_t n>
constexpr std::complex<double> trace(nummat<n> const &M)
{
  std::complex<double> res(0.0);
  for (auto i = 0LU; i < n; ++i) {
    res += M(i, i);
  }

  return res;
}

/// Dagger (needed as a non-member function for expansions)
template <std::size_t n>
constexpr nummat<n> dagger(nummat<n> const &M)
{
  return M.dagger();
}

/// Inverse (needed as a non-member function for expansions)
///
/// Wraps member function
template <std::size_t n>
constexpr nummat<n> inverse(nummat<n> const &M)
{
  return M.inverse();
}

/// Matrix Log (needed as a non-member function for expansions)
///
/// Wraps member function
template <std::size_t n>
constexpr nummat<n> log(nummat<n> const &M)
{
  return M.log();
}

/// Construct diagonal matrix from array
template <std::size_t n>
constexpr nummat<n> diag(std::array<std::complex<double>, n> const &init)
{
  nummat<n> M;
  for (auto i = 0LU; i < n; ++i) {
    M(i, i) = init[i];
  }

  return M;
}

/// Frobenius norm of a matrix
template <std::size_t n>
double normF(nummat<n> const &M)
{
  return cblas_dznrm2(n * n, reinterpret_cast<const double *>(M.data()), 1);
}

/// Matrix exponential
/// (Via Taylor series. This is probably neither fast nor stable.)
template <std::size_t n>
nummat<n> exp(nummat<n> const &M,
              const double &error = std::numeric_limits<double>::epsilon(),
              const size_t &maxiter = 100LU)
{

  double prec_goal = error;
  // Sanity check
  if (prec_goal < std::numeric_limits<double>::epsilon()) {
    prec_goal = std::numeric_limits<double>::epsilon();
    std::cout
      << "Warning! Precision goal was reset to machine epsilon in matrix exp"
      << prec_goal << std::endl;
  }

  nummat<n> res{ 1.0 }, old{ M }, aux{ 1.0 };

  double eps = 1.0;
  double fac = 1.0;

  size_t iter = 0LU;
  while (eps > error && iter < maxiter) {

    ++iter;
    fac *= static_cast<double>(iter);
    aux = aux * M;

    res += aux / fac;
    // Error estimate. (Quite pessimistic)
    eps = normF(old - res);
    // std::cout << "Error: " << eps << std::endl;
    old = res;
  }

  // std::cout << iter << std::endl;
  return res;
}

/// Inflates compressed Hermitian matrix (CblasUpper format) to a dense matrix
template <size_t NN>
void inflate(nummat<NN> &M)
{
  for (auto i = 1LU; i < NN; ++i) {
    for (auto j = 0LU; j < i; ++j) {
      M(i, j) = std::conj(M(j, i));
    }
  }
}


#ifndef BASIS_PURSUIT_A_H
#define BASIS_PURSUIT_A_H

#include "Matrix.h"
#include "APPROX2.h"
#include "ALM_I_APPROX.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 of the form f(x)+g(x) under the constraint Mx=c by IPALM_APPROX;
where f(x)=0 and g(x)=frac{lambda2}{2}|x|_2+lambda1 |x|_1.
*/

template<typename L, typename D>
class Basis_pursuit_a: public ALM_I_APPROX<L, D>
{
private:

  D lambda1;

  D lambda2;

  Matrix<L,D> my_M;

  Matrix<L,D> my_A;

  D val_lambda_f;

protected:

public:

  Basis_pursuit_a(const char* Matrix_file,D val_lambda1, D val_lambda2)
  :ALM_I_APPROX<L,D>(), my_M(Matrix_file)
  {
    lambda1=val_lambda1;
    lambda2=val_lambda2;
    val_lambda_f=0;
    this->val_mu_f=0;
    this->val_mu_g=lambda2;
  }

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

  inline D gradient_of_phi_j(D x1, L i){
    return 0;
  }

  inline D value_of_g_i(D x, L i){
    return lambda2*x*x/2+lambda1*fabs(x);
  }

  inline D value_of_phi_j(D x1, L i){return 0;}

  inline D prox_of_g_i(D x1,D x2,D x3, L i){
    return compute_one_step(1.0/x2, x1, x3)-x3;
  }

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

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

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

  inline D rescale(vector<D> & x, vector<D> & y){
    D scal=1;
    L l=x.size();
    for(L i=0;i<l;i++){
      if(fabs(x[i]+y[i])>lambda1){
        scal=min(lambda1/fabs(x[i]+y[i]),scal);
      }
    }
    return scal;
  }

  inline D value_of_phistar_i(D x,L i) {return 0;}

  inline D value_of_g_tilde_star(D scal,vector<D> & x, vector<D> & y){

    if(lambda2+this->tau_s*this->beta_s>0)
    {
      D res=0;
      D ip=0;
      L l=x.size();
      for(L i=0;i<l;i++)
      {
        ip=max(fabs(x[i]+y[i]+this->tau_s*this->beta_s*this->x_s[i])-lambda1,0.0);
        res+=ip*ip;
      }
      D xsnorm=0;
      for(L i=0;i<l;i++)
      xsnorm+=this->tau_s*this->beta_s/2*this->x_s[i]*this->x_s[i];
      return res/2/(lambda2+this->tau_s*this->beta_s)-xsnorm;
    }
    else
    {
      return 0;
    }
  }

  inline D value_of_g_star(D scal,vector<D> & x, vector<D> & y){
    return 0;
  }

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

  inline void set_matrix_A(){
    this->data_A.nsamples=0;
    this->data_A.nfeatures=this->data_M.nfeatures;
    this->data_A.nnz=0;
    this->data_A.ptr.resize(1,0);
    this->data_A.ptr_t.resize(this->data_M.nfeatures+1,0);
  }

  inline D distance_to_subgradient_of_g(){
    D res=0;
    D tmp;
    D xv;
    for(L i=0;i<this->n;i++){
      tmp=-this->gradient_of_f[i]-this->M_tlambda_s[i];
      xv=this->x_s[i];
      if(xv>0) res+=(tmp-lambda2*xv-lambda1)*(tmp-lambda2*xv-lambda1);
      else if(xv<0) res+=(tmp-lambda2*xv+lambda1)*(tmp-lambda2*xv+lambda1);
      else if(tmp-lambda2*xv>lambda1) res+=(tmp-lambda2*xv-lambda1)*(tmp-lambda2*xv-lambda1);
      else if(tmp-lambda2*xv<-lambda1) res+=(tmp-lambda2*xv+lambda1)*(tmp-lambda2*xv+lambda1);
      else res+=0;
    }
    return sqrt(res);
  }

  inline D subgradient_of_Tx(){
    D res=0;
    D tmp;
    D xv;
    for(L i=0;i<this->n;i++){
      xv=this->Tx[i];
      tmp=this->gradient_of_Tx[i]+ lambda2*xv+this->tau_s*this->beta_s*(xv- this->x_s[i]);
      if(xv>0) res+=(tmp+ lambda1)*(tmp+ lambda1);
      else if(xv<0) res+=(tmp-lambda1)*(tmp-lambda1);
      else if(tmp>lambda1) res+=(tmp-lambda1)*(tmp-lambda1);
      else if(tmp<-lambda1) res+=(tmp+lambda1)*(tmp+lambda1);
      else res+=0;
    }
    return sqrt(res);
  }

  void ALM_I_APPROX_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_I_APPROX_solve_with_APPROX(beta_0, epsilon_0,  eta, rho,x0,y0, val_tau, max_nb_outer,  p_N_1,  p_N_2,  val_lambda_f, filename1,  filename2, time);
  }

};

#endif


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include <sys/time.h>
#include <inttypes.h>

#include <omp.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_heapsort.h>

#include "sph_data_types.h"
#include "sph_linked_list.h"
#include "sph_compute.h"

int compute_density_3d_naive(int N,double h,
                             double* restrict x, double* restrict y,
                             double* restrict z,double* restrict nu,
                             double* restrict Fx){
  #pragma omp parallel for
  for(int64_t ii=0;ii<N;ii+=1){
    Fx[ii] = 0;
    for(int64_t jj=0;jj<N;jj+=1){
      double dist = 0.;

      dist += (x[ii]-x[jj])*(x[ii]-x[jj]);
      dist += (y[ii]-y[jj])*(y[ii]-y[jj]);
      dist += (z[ii]-z[jj])*(z[ii]-z[jj]);

      dist = sqrt(dist);

      Fx[ii] += nu[jj]*w_bspline_3d(dist,h);
    }
  }

  return 0;
}

int compute_density_3d_ref(int N,double h,
                           double* restrict x, double* restrict y,
                           double* restrict z, double* restrict nu,
                           double* restrict Fx){
  const double inv_h = 1./h;
  const double kernel_constant = w_bspline_3d_constant(h);
  #pragma omp parallel for
  for(int64_t ii=0;ii<N;ii+=1){
    double xii = x[ii];
    double yii = y[ii];
    double zii = z[ii];
    double rhoii = 0.0;

    #pragma omp simd reduction(+:rhoii) aligned(x,y,z,nu)
    for(int64_t jj=0;jj<N;jj+=1){
      double q = 0.;

      double xij = xii-x[jj];
      double yij = yii-y[jj];
      double zij = zii-z[jj];

      q += xij*xij;
      q += yij*yij;
      q += zij*zij;

      //q = sqrt(q);//*inv_h;
      q = sqrt(q)*inv_h;

      rhoii += nu[jj]*w_bspline_3d_simd(q);
    }
    Fx[ii] = kernel_constant*rhoii;
  }

  return 0;
}

int compute_density_3d_tiled(int N,double h,
                             double* restrict x, double* restrict y,
                             double* restrict z, double* restrict nu,
                             double* restrict Fx){
  const double inv_h = 1./h;
  const double kernel_constant = w_bspline_3d_constant(h);
  const int64_t STRIP = 1000;
  const int64_t N_tiles = N/STRIP;
  const int64_t N_prime = N - N%STRIP;

  #pragma omp parallel for num_threads(24)
  for(int64_t ii=0;ii<N;ii+=1)
    Fx[ii] = 0.;

  #pragma omp parallel for
  for(int64_t i=0;i<N_prime;i+=STRIP){
    for(int64_t j=0;j<N_prime;j+=STRIP){
      for(int64_t ii=i;ii<i+STRIP;ii+=1){
        double xii = x[ii];
        double yii = y[ii];
        double zii = z[ii];
        double rhoii = 0.0;

        #pragma omp simd reduction(+:rhoii) aligned(x,y,z,nu)
        for(int64_t jj=j;jj<j+STRIP;jj+=1){
          double q = 0.;

          double xij = xii-x[jj];
          double yij = yii-y[jj];
          double zij = zii-z[jj];

          q += xij*xij;
          q += yij*yij;
          q += zij*zij;

          q = sqrt(q)*inv_h;

          rhoii += nu[jj]*w_bspline_3d_simd(q);
        }
        Fx[ii] += kernel_constant*rhoii;
      }
    }
  }

  #pragma omp parallel for
  for(int64_t j=0;j<N_prime;j+=STRIP){
    for(int64_t ii=N_prime;ii<N;ii+=1){
      double xii = x[ii];
      double yii = y[ii];
      double zii = z[ii];
      double rhoii = 0.0;

      #pragma omp simd reduction(+:rhoii) aligned(x,y,z,nu)
      for(int64_t jj=j;jj<j+STRIP;jj+=1){
        double q = 0.;

        double xij = xii-x[jj];
        double yij = yii-y[jj];
        double zij = zii-z[jj];

        q += xij*xij;
        q += yij*yij;
        q += zij*zij;

        q = sqrt(q)*inv_h;

        rhoii += nu[jj]*w_bspline_3d_simd(q);
      }
      Fx[ii] += kernel_constant*rhoii;
    }
  }

  return 0;
}

void swap_int64_t2(int64_t* a, int64_t* b) {
  int64_t t0 = *a;
  int64_t t1 = *(a+1);

  *a     = *b;
  *(a+1) = *(b+1);
  *b     = t0;
  *(b+1) = t1;
}

int64_t partition_int64_t2(int64_t arr[], int64_t low, int64_t high)
{
  int64_t pivot = arr[2*high]; // pivot
  int64_t i     = (low - 1);   // Index of smaller element and indicates the right position of pivot found so far

  for (int64_t j = low; j <= high - 1; j++){
    // If current element is smaller than the pivot
    if (arr[2*j] < pivot){
      i++; // increment index of smaller element
      swap_int64_t2(&arr[2*i], &arr[2*j]);
    }
  }

  swap_int64_t2(&arr[2*(i + 1)], &arr[2*high]);
  return (i + 1);
}

/*
void insertionSort_int64_t2(int64_t *arr, int64_t n){
    // https://www.geeksforgeeks.org/insertion-sort/

    int64_t i=0, key0=0, key1=0, j=0;
    for (i = 1; i < n; i++){
        key0 = arr[2*i+0];
        key1 = arr[2*i+1];
        j = i - 1;

        // Move elements of arr[0..i-1], that are
        // greater than key, to one position ahead
        // of their current position
        while (j >= 0 && arr[2*j] > key0){
            arr[2*(j+1)+0] = arr[2*j+0];
            arr[2*(j+1)+1] = arr[2*j+1];

            j = j-1;
        }

        arr[2*(j+1)+0] = key0;
        arr[2*(j+1)+1] = key1;
    }
}*/

void insertionSort_int64_t2(int64_t *arr, int64_t low, int64_t high)
{
    int64_t i, key0, key1, j;
    for (i = low+1; i < high; i++){
        key0 = arr[2*i+0];
        key1 = arr[2*i+1];

        j = i - 1;

        /* Move elements of arr[0..i-1], that are
        greater than key, to one position ahead
        of their current position */
        while (j >= 0 && arr[2*j] > key0){
            arr[2*(j+1)+0] = arr[2*j+0];
            arr[2*(j+1)+1] = arr[2*j+1];

            j = j-1;
        }
        arr[2*(j+1)+0] = key0;
        arr[2*(j+1)+1] = key1;
    }
}

#define MIN_DEPTH 120

void quickSort_int64_t2(int64_t *arr, int64_t low, int64_t high){
  // https://www.geeksforgeeks.org/quick-sort/

  if (low < high){
    // pi is partitioning index, arr[p] is now
    //   at right place
    int64_t pi = partition_int64_t2(arr, low, high);

    // Separately sort elements before
    // partition and after partition
    //quickSort_int64_t2(arr, low, pi-1);
    //quickSort_int64_t2(arr, pi + 1, high);

    if(pi-low-1 >= MIN_DEPTH){
      #pragma omp task shared(arr)
      quickSort_int64_t2(arr, low, pi-1);
    }
    else{
      //quickSort_int64_t2(arr, low, pi-1);
      //#pragma omp task shared(arr)
      insertionSort_int64_t2(arr,low,pi-1);
    }

    if(high - (pi+1) >= MIN_DEPTH){
      #pragma omp task shared(arr)
      quickSort_int64_t2(arr, pi+1, high);
    }
    else{
      //quickSort_int64_t2(arr, low, pi-1);
      //#pragma omp task shared(arr)
      insertionSort_int64_t2(arr,pi+1,high);
    }
  }
}

int main(){

  int err,dbg=0;
  int64_t N = 1000000;
  double h=0.05;
  linkedListBox *box;
  SPHparticle *lsph;

  err = SPHparticle_SoA_malloc(N,&lsph);
  if(err)
    printf("error in SPHparticle_SoA_malloc\n");

  err = gen_unif_rdn_pos( N,123123123,lsph);
  if(err)
    printf("error in gen_unif_rdn_pos\n");

  box = (linkedListBox*)malloc(1*sizeof(linkedListBox));
  if(box==NULL)
    printf("error alocating the linkedListBox\n");

  box->Xmin = -1.0; box->Ymin = -1.0; box->Zmin = -1.0;
  box->Xmax =  2.0; box->Ymax =  2.0; box->Zmax =  2.0;
  box->Nx = (int)( (box->Xmax-box->Xmin)/(2*h) );
  box->Ny = (int)( (box->Ymax-box->Ymin)/(2*h) );
  box->Nz = (int)( (box->Zmax-box->Zmin)/(2*h) );
  box->N  = (box->Nx)*(box->Ny)*(box->Nz);
  double min_val = fmin((box->Xmax-box->Xmin)/box->Nx,fmin((box->Ymax-box->Ymin)/box->Ny,(box->Zmax-box->Zmin)/box->Nz));
  box->width = (int)( 0.5 + 2*h/min_val );
  box->hbegin = kh_init(0);
  box->hend = kh_init(1);

  double t0,t1,t2,t3,t4,t5,t6,t7;
  t0 = omp_get_wtime();

  if(dbg)
    printf("hello - 3\n");
  err = compute_hash_MC3D(N,lsph,box);

  t1 = omp_get_wtime();

  if(dbg)
    printf("hello - 4\n");

  //qsort(lsph->hash,N,2*sizeof(int64_t),compare_int64_t);
  //quickSort_int64_t2(lsph->hash,0,N-1);

  #pragma omp parallel num_threads(1)
  {
    #pragma omp single
    quickSort_int64_t2(lsph->hash,0,N-1);
  }

  /*
  #pragma omp parallel num_threads(1)
  {
    #pragma omp single
    quicksort_omp(lsph->hash,0,N);
  }*/

  t2 = omp_get_wtime();

  if(dbg)
    printf("hello - 5\n");
  void *swap_arr = malloc(N*sizeof(double));
  err = reorder_lsph_SoA(N,lsph,swap_arr);
  if(err)
    printf("error in reorder_lsph_SoA\n");

  t3 = omp_get_wtime();

  if(dbg)
    printf("hello - 6\n");
  err = setup_interval_hashtables(N,lsph,box);
  if(err)
    printf("error in setup_interval_hashtables\n");

  t4 = omp_get_wtime();

  if(dbg)
    printf("hello - 7\n");

  //err = compute_density_3d(N,h,lsph,box);
  //err = compute_density_3d_load_ballanced(N,h,lsph,box);
  err = compute_density_3d_symmetrical_load_ballance(N,h,lsph,box);
  //err = compute_density_3d_symmetrical_lb_branching(N,h,lsph,box);
  //err = compute_density_3d_symmetrical_lb(N,h,lsph,box);
  if(err)
    printf("error in setup_interval_hashtables\n");

  t5 = omp_get_wtime();

  if(dbg)
    printf("hello - 7\n");

  //err = compute_density_3d(N,h,lsph,box);
  //err = compute_density_3d_innerOmp(N,h,lsph,box);
  //err = compute_density_3d_loopswapped(N,h,lsph,box);
  //
  if(err)
    printf("error in setup_interval_hashtables\n");

  t6 = omp_get_wtime();

  if(dbg)
    printf("hello - 8\n");

  //err = compute_density_3d_tiled(N,h,lsph->x,lsph->y,lsph->z,lsph->nu,lsph->Fx);
  //err = compute_density_3d_ref(N,h,lsph->x,lsph->y,lsph->z,lsph->nu,lsph->Fx);
  //err = compute_density_3d_naive(N,h,lsph->x,lsph->y,lsph->z,lsph->nu,lsph->Fx);
  err = compute_density_3d_load_ballanced(N,h,lsph,box);
  if(err)
    printf("error in compute_density_3d_ref\n");

  t7 = omp_get_wtime();

  printf("compute_hash_MC3D calculation time : %lf : %lf\n",t1-t0,100*(t1-t0)/(t5-t0));
  printf("qsort calculation time : %lf : %lf\n",t2-t1,100*(t2-t1)/(t5-t0));
  printf("reorder_lsph_SoA calculation time : %lf : %lf\n",t3-t2,100*(t3-t2)/(t5-t0));
  printf("setup_interval_hashtables calculation time : %lf : %lf\n",t4-t3,100*(t4-t3)/(t5-t0));
  printf("compute_density_3d base calculation time : %lf : %lf\n",t5-t4,100*(t5-t4)/(t5-t0));
  printf("compute_density_3d load balanced calculation time : %lf : %lf\n",t6-t5,0.);
  printf("Total Linked-List compute_density_3d calculation time : %lf : %lf\n",t5-t0,100*(t5-t0)/(t5-t0));
  printf("Reference tiled compute_density_3d calculation time : %lf\n",t7-t6);

  if(dbg)
    printf("hello - 9\n");

  FILE *fp = fopen("data/sph_density_compute_ref.csv","w");
  if(fp!=NULL){
    for(int64_t i=0;i<N;i+=1)
      fprintf(fp,"%ld %.12lf %.12lf %.6lg\n",i,
                                              lsph->rho[i],
                                              lsph->Fx[i],
                                              fabs(lsph->rho[i]-lsph->Fx[i]));
    fclose(fp);
  }

  if(dbg)
    printf("hello - 10\n");
  SPHparticleSOA_safe_free(N,&lsph);
  safe_free_box(box);
  free(swap_arr);

  return 0;
}


#include <stdio.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

int main()
{
  int MAT_DIM = 4;
  int i,j;
  int myArray[16] = {4, 3, 2, 1, 1,10, 3, 4, 5, 3, 2, -4, 4, 8, 7, 9};

  gsl_matrix *m = gsl_matrix_alloc(MAT_DIM,MAT_DIM);

  for (i=0; i<MAT_DIM; i++)
    for (j=0; j<MAT_DIM; j++)
      gsl_matrix_set (m, i, j, (double) myArray[i*MAT_DIM + j]);

  for (i=0; i<MAT_DIM; i++)
    for (j=0; j<MAT_DIM; j++)
      printf("m(%d,%d) = %g\n", i, j,
              gsl_matrix_get (m, i, j));

  double det;
  int signum;
  gsl_permutation *p = gsl_permutation_alloc(m->size1);

  gsl_linalg_LU_decomp(m, p, &signum);
  det = gsl_linalg_LU_det(m, signum);
  gsl_permutation_free(p);
  gsl_matrix_free(m);

  printf("Determinant: %g\n", det);

  return 0;
}