#include <math.h>
#include <stdio.h>
#include <mpfr.h>
#include <gmp.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <gsl/gsl_sf_legendre.h>
#include <petsc.h>

#undef __FUNCT__
#define __FUNCT__ "DEwk"
PetscErrorCode DEwk(PetscInt k, mpfr_t h, mpfr_t *wk)
{
  PetscErrorCode ierr;
  mpfr_t kh, msinh, mcosh, piOver2, tmp;
  PetscFunctionBegin;

  mpfr_inits(kh, msinh, mcosh, piOver2, tmp, NULL);

  //init piOver2
  mpfr_const_pi(piOver2, MPFR_RNDN);
  mpfr_mul_d(piOver2, piOver2, 0.5, MPFR_RNDN);

  mpfr_set_d(kh, (double) k, MPFR_RNDN);
  mpfr_mul(kh, kh, h, MPFR_RNDN);
  mpfr_set(*wk, h, MPFR_RNDN);
  mpfr_sinh_cosh(msinh, mcosh, kh, MPFR_RNDN);
  mpfr_mul(msinh, msinh, piOver2, MPFR_RNDN);
  mpfr_mul(mcosh, mcosh, piOver2, MPFR_RNDN);
  mpfr_cosh(tmp, msinh, MPFR_RNDN);
  mpfr_sqr(tmp, tmp, MPFR_RNDN);
  mpfr_mul(*wk, *wk, mcosh, MPFR_RNDN);
  mpfr_div(*wk, *wk, tmp, MPFR_RNDN);

  PetscFunctionReturn(0);
}

/* have to make single exponential */
#undef __FUNCT__
#define __FUNCT__ "SEwk"
PetscErrorCode SEwk(PetscInt k, mpfr_t h, mpfr_t *wk)
{
  PetscErrorCode ierr;
  mpfr_t kh, msinh, mcosh, piOver2, tmp;
  PetscFunctionBegin;

  mpfr_inits(kh, msinh, mcosh, piOver2, tmp, NULL);

  //init piOver2
  mpfr_const_pi(piOver2, MPFR_RNDN);
  mpfr_mul_d(piOver2, piOver2, 0.5, MPFR_RNDN);

  mpfr_set_d(kh, (double) k, MPFR_RNDN);
  mpfr_mul(kh, kh, h, MPFR_RNDN);
  //printf("wow kh: %15.15f\n", mpfr_get_d(kh, MPFR_RNDN));
  mpfr_tanh(*wk, kh, MPFR_RNDN);
  //printf("wow wk: %15.15f\n", mpfr_get_d(*wk, MPFR_RNDN));
  mpfr_mul(*wk, *wk, *wk, MPFR_RNDN);
  mpfr_d_sub(*wk, 1.0, *wk, MPFR_RNDN);
  mpfr_mul(*wk, *wk, h, MPFR_RNDN);
  //mpfr_set(*wk, h, MPFR_RNDN);
  //mpfr_sinh_cosh(msinh, mcosh, kh, MPFR_RNDN);
  //mpfr_mul(msinh, msinh, piOver2, MPFR_RNDN);
  //mpfr_mul(mcosh, mcosh, piOver2, MPFR_RNDN);
  //mpfr_cosh(tmp, msinh, MPFR_RNDN);
  //mpfr_sqr(tmp, tmp, MPFR_RNDN);
  //mpfr_mul(*wk, *wk, mcosh, MPFR_RNDN);
  //mpfr_div(*wk, *wk, tmp, MPFR_RNDN);

  PetscFunctionReturn(0);
}

/* have to make single exponential */
#undef __FUNCT__
#define __FUNCT__ "ERFwk"
PetscErrorCode ERFwk(PetscInt k, mpfr_t h, mpfr_t *wk)
{
  PetscErrorCode ierr;
  mpfr_t kh, msinh, mcosh, piOver2, tmp;
  PetscFunctionBegin;

  mpfr_inits(kh, msinh, mcosh, piOver2, tmp, NULL);

  // init constant
  mpfr_const_pi(piOver2, MPFR_RNDN);
  mpfr_sqrt(piOver2, piOver2, MPFR_RNDN);
  mpfr_d_div(piOver2, 2.0, piOver2, MPFR_RNDN);

  mpfr_set_d(kh, (double) k, MPFR_RNDN);
  mpfr_mul(kh, kh, h, MPFR_RNDN);
  mpfr_mul(tmp, kh, kh, MPFR_RNDN);
  mpfr_mul_d(tmp, tmp, -1.0, MPFR_RNDN);
  mpfr_exp(*wk, tmp, MPFR_RNDN);
  mpfr_mul(*wk, *wk, h, MPFR_RNDN);
  mpfr_mul(*wk, *wk, piOver2, MPFR_RNDN);

  PetscFunctionReturn(0);
}

/* generates n weights spaced h apart in the positive direction */
/* includes "center" n=0 weight as first entry */
#undef __FUNCT__
#define __FUNCT__ "DExkwk"
PetscErrorCode DExkwk(PetscInt n, mpfr_t h, mpfr_t **xk, mpfr_t **wk)
{
  PetscErrorCode ierr;
  PetscInt k;
  mpfr_t kh, msinh, mcosh, piOver2, tmp;
  PetscReal alp, bet;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  mpfr_inits(kh, msinh, mcosh, piOver2, tmp, NULL);

  // init piOver2
  mpfr_const_pi(piOver2, MPFR_RNDN);
  mpfr_mul_d(piOver2, piOver2, 0.5, MPFR_RNDN); flopCount ++;

  for(k=0; k<n; ++k) {
    /* calculate wk */
    mpfr_set_d(kh, (double) k, MPFR_RNDN);
    mpfr_mul(kh, kh, h, MPFR_RNDN);
    mpfr_set((*wk)[k], h, MPFR_RNDN);
    mpfr_sinh_cosh(msinh, mcosh, kh, MPFR_RNDN);
    mpfr_mul(msinh, msinh, piOver2, MPFR_RNDN);
    mpfr_mul(mcosh, mcosh, piOver2, MPFR_RNDN);
    mpfr_cosh(tmp, msinh, MPFR_RNDN);
    mpfr_sqr(tmp, tmp, MPFR_RNDN);
    mpfr_mul((*wk)[k], (*wk)[k], mcosh, MPFR_RNDN);
    mpfr_div((*wk)[k], (*wk)[k], tmp, MPFR_RNDN); flopCount += 9;

    /* calculate xk */
    mpfr_set_d((*xk)[k], 1.0, MPFR_RNDZ);
    mpfr_cosh(tmp, msinh, MPFR_RNDN);
    mpfr_div((*xk)[k], (*xk)[k], tmp, MPFR_RNDZ);
    mpfr_exp(tmp, msinh, MPFR_RNDN);
    mpfr_div((*xk)[k], (*xk)[k], tmp, MPFR_RNDZ); flopCount += 4;

  }
  mpfr_clears(kh, msinh, mcosh, piOver2, tmp, NULL);

  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

/* generates n weights spaced h apart in the positive direction */
/* includes "center" n=0 weight as first entry */
#undef __FUNCT__
#define __FUNCT__ "SExkwk"
PetscErrorCode SExkwk(PetscInt n, mpfr_t h, mpfr_t **xk, mpfr_t **wk)
{
  PetscErrorCode ierr;
  PetscInt k;
  mpfr_t kh, msinh, mcosh, piOver2, tmp;
  PetscReal alp, bet;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  mpfr_inits(kh, msinh, mcosh, piOver2, tmp, NULL);

  // init piOver2
  mpfr_const_pi(piOver2, MPFR_RNDN);
  mpfr_mul_d(piOver2, piOver2, 0.5, MPFR_RNDN); flopCount ++;

  for(k=0; k<n; ++k) {
    /* calculate wk */
    mpfr_set_d(kh, (double) k, MPFR_RNDN);
    mpfr_mul(kh, kh, h, MPFR_RNDN);
    mpfr_tanh((*wk)[k], kh, MPFR_RNDN);
    mpfr_mul((*wk)[k], (*wk)[k], (*wk)[k], MPFR_RNDN);
    mpfr_d_sub((*wk)[k], 1.0, (*wk)[k], MPFR_RNDN);
    mpfr_mul((*wk)[k], (*wk)[k], h, MPFR_RNDN); flopCount += 5;

    /* calculate xk */
    mpfr_tanh((*xk)[k], kh, MPFR_RNDN); flopCount += 1;
    //mpfr_set_d((*xk)[k], 1.0, MPFR_RNDZ);
    //mpfr_cosh(tmp, msinh, MPFR_RNDN);
    //mpfr_div((*xk)[k], (*xk)[k], tmp, MPFR_RNDZ);
    //mpfr_exp(tmp, msinh, MPFR_RNDN);
    //mpfr_div((*xk)[k], (*xk)[k], tmp, MPFR_RNDZ); flopCount += 4;
    //mpfr_d_sub(tmp, 1.0, (*xk)[k], MPFR_RNDN);
    //printf("xk: %15.15e\n1-xk: %15.15e\nwk: %3.3e\n\n", mpfr_get_d((*xk)[k], MPFR_RNDN), mpfr_get_d(tmp, MPFR_RNDN), mpfr_get_d((*wk)[k], MPFR_RNDN));
  }
  mpfr_clears(kh, msinh, mcosh, piOver2, tmp, NULL);

  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

/* generates n weights spaced h apart in the positive direction */
/* includes "center" n=0 weight as first entry */
#undef __FUNCT__
#define __FUNCT__ "ERFxkwk"
PetscErrorCode ERFxkwk(PetscInt n, mpfr_t h, mpfr_t **xk, mpfr_t **wk)
{
  PetscErrorCode ierr;
  PetscInt k;
  mpfr_t kh, msinh, mcosh, piOver2, tmp;
  PetscReal alp, bet;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  mpfr_inits(kh, msinh, mcosh, piOver2, tmp, NULL);

  // init piOver2
  mpfr_const_pi(piOver2, MPFR_RNDN);
  mpfr_sqrt(piOver2, piOver2, MPFR_RNDN);
  mpfr_d_div(piOver2, 2.0, piOver2, MPFR_RNDN);
  //mpfr_mul_d(piOver2, piOver2, 0.5, MPFR_RNDN); flopCount ++;

  for(k=0; k<n; ++k) {
    /* calculate wk */
    mpfr_set_d(kh, (double) k, MPFR_RNDN);
    mpfr_mul(kh, kh, h, MPFR_RNDN);
    mpfr_mul(tmp, kh, kh, MPFR_RNDN);
    mpfr_mul_d(tmp, tmp, -1.0, MPFR_RNDN);
    mpfr_exp((*wk)[k], tmp, MPFR_RNDN);
    mpfr_mul((*wk)[k], (*wk)[k], h, MPFR_RNDN);
    mpfr_mul((*wk)[k], (*wk)[k], piOver2, MPFR_RNDN);

    /* calculate xk */
    mpfr_erf((*xk)[k], kh, MPFR_RNDN);
  }
  mpfr_clears(kh, msinh, mcosh, piOver2, tmp, NULL);

  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "inte"
PetscErrorCode inte(mpfr_t *x, mpfr_t *val, void *ctx)
{
  PetscErrorCode ierr;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  //mpfr_mul(*val, *x, *x, MPFR_RNDN);
  //mpfr_mul(*val, *val, *x, MPFR_RNDN); flopCount += 2;
  mpfr_const_pi(*val, MPFR_RNDN);
  mpfr_mul_d(*val, *val, 2.0, MPFR_RNDN);
  mpfr_mul(*val, *val, *x, MPFR_RNDN);
  mpfr_sin(*val, *val, MPFR_RNDN);
  mpfr_mul(*val, *val, *x, MPFR_RNDN);
  //printf("xk: %15.15f\n", mpfr_get_d(*x, MPFR_RNDN));
  //printf("val: %15.15f\n", mpfr_get_d(*val, MPFR_RNDN));
  //mpfr_set_d(*val, 1.0, MPFR_RNDN);
  //mpfr_set(*val, *x, MPFR_RNDN);

  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "FindStepSize"
PetscErrorCode FindStepSize(PetscInt prec, PetscInt nPts, mpfr_t *step)
{
  PetscErrorCode ierr;
  const PetscReal tol = 1e-10;
  mpfr_t wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error;
  PetscInt cont;
  PetscFunctionBegin;

  if(prec < 1) {
    printf("prec should be positive\n");
    PetscFunctionReturn(1);
  }

  mpfr_inits(wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error, NULL);

  mpfr_set_d(hl, 0.0, MPFR_RNDN);
  mpfr_set_d(hr, 2.0, MPFR_RNDN);

  /* make hr large enough */
  mpfr_set_d(hr, 1.0, MPFR_RNDN);
  cont = 1;
  while(cont == 1) {
    ierr = DEwk(nPts, hr, &rwk);CHKERRQ(ierr);
    mpfr_log10(srwk, rwk, MPFR_RNDN);
    if(mpfr_get_d(srwk, MPFR_RNDN) < -2*prec)
      cont = 0;
    else
      mpfr_mul_d(hr, hr, 2.0, MPFR_RNDN); // make hr larger if value not small
  }
  //printf("hr: %15.15f\n", mpfr_get_d(hr, MPFR_RNDN));
  /* make hl small enough */
  mpfr_mul_d(hl, hr, 0.5, MPFR_RNDN);
  cont = 1;
  while(cont == 1) {
    ierr = DEwk(nPts, hl, &lwk);CHKERRQ(ierr);
    mpfr_log10(slwk, lwk, MPFR_RNDN);
    if(mpfr_get_d(slwk, MPFR_RNDN) > -2*prec)
      cont = 0;
    else
      mpfr_mul_d(hl, hl, 0.5, MPFR_RNDN); // make hl larger if value not small
  }
  //printf("hl: %15.15f\n", mpfr_get_d(hl, MPFR_RNDN));
  //ierr = DEwk(nPts, hl, &lwk);CHKERRQ(ierr);
  //ierr = DEwk(nPts, hr, &rwk);CHKERRQ(ierr);

  cont = 1;
  while(cont == 1) {
    // average hl and hr
    mpfr_add(h, hl, hr, MPFR_RNDN);
    mpfr_mul_d(h, h, 0.5, MPFR_RNDN);

    // calculate weight and size
    ierr = DEwk(nPts, h, &wk);CHKERRQ(ierr);
    ierr = DEwk(nPts, hl, &lwk);CHKERRQ(ierr);
    ierr = DEwk(nPts, hr, &rwk);CHKERRQ(ierr);
    mpfr_log10(swk, wk, MPFR_RNDN);
    mpfr_log10(slwk, lwk, MPFR_RNDN);
    mpfr_log10(srwk, rwk, MPFR_RNDN);
    if( mpfr_get_d(swk, MPFR_RNDN) < -2*prec)
      mpfr_set(hr, h, MPFR_RNDN);
    else
      mpfr_set(hl, h, MPFR_RNDN);
    // calculate maximum possible error
    mpfr_sub(error, slwk, srwk, MPFR_RNDN);
    mpfr_abs(error, error, MPFR_RNDN);
    if(mpfr_get_d(error, MPFR_RNDN) < tol)
       cont = 0;
  }

  mpfr_set(*step, h, MPFR_RNDN);

  mpfr_clears(wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error, NULL);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "FindStepSizeSE"
PetscErrorCode FindStepSizeSE(PetscInt prec, PetscInt nPts, mpfr_t *step)
{
  PetscErrorCode ierr;
  const PetscReal tol = 1e-25;
  mpfr_t wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error;
  PetscInt cont;
  PetscFunctionBegin;

  if(prec < 1) {
    printf("prec should be positive\n");
    PetscFunctionReturn(1);
  }

  mpfr_inits(wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error, NULL);

  mpfr_set_d(hl, 0.0, MPFR_RNDN);
  mpfr_set_d(hr, 2.0, MPFR_RNDN);

  /* make hr large enough */
  mpfr_set_d(hr, 1.0, MPFR_RNDN);
  cont = 1;
  while(cont == 1) {
    ierr = SEwk(nPts, hr, &rwk);CHKERRQ(ierr);
    mpfr_log10(srwk, rwk, MPFR_RNDN);
    if(mpfr_get_d(srwk, MPFR_RNDN) < -2*prec)
      cont = 0;
    else
      mpfr_mul_d(hr, hr, 2.0, MPFR_RNDN); // make hr larger if value not small
  }
  //printf("hr: %15.15f\n", mpfr_get_d(hr, MPFR_RNDN));
  /* make hl small enough */
  mpfr_mul_d(hl, hr, 0.5, MPFR_RNDN);
  cont = 1;
  while(cont == 1) {
    ierr = SEwk(nPts, hl, &lwk);CHKERRQ(ierr);
    mpfr_log10(slwk, lwk, MPFR_RNDN);
    if(mpfr_get_d(slwk, MPFR_RNDN) > -2*prec)
      cont = 0;
    else
      mpfr_mul_d(hl, hl, 0.5, MPFR_RNDN); // make hl larger if value not small
  }
  //printf("hl: %15.15f\n", mpfr_get_d(hl, MPFR_RNDN));
  ierr = SEwk(nPts, hl, &lwk);CHKERRQ(ierr);
  ierr = SEwk(nPts, hr, &rwk);CHKERRQ(ierr);
  //printf("lwk: %3.3e\n", mpfr_get_d(lwk, MPFR_RNDN));
  //printf("rwk: %3.3e\n", mpfr_get_d(rwk, MPFR_RNDN));

  cont = 1;
  while(cont == 1) {
    // average hl and hr
    mpfr_add(h, hl, hr, MPFR_RNDN);
    mpfr_mul_d(h, h, 0.5, MPFR_RNDN);

    // calculate weight and size
    ierr = SEwk(nPts, h, &wk);CHKERRQ(ierr);
    ierr = SEwk(nPts, hl, &lwk);CHKERRQ(ierr);
    ierr = SEwk(nPts, hr, &rwk);CHKERRQ(ierr);
    mpfr_log10(swk, wk, MPFR_RNDN);
    mpfr_log10(slwk, lwk, MPFR_RNDN);
    mpfr_log10(srwk, rwk, MPFR_RNDN);
    if( mpfr_get_d(swk, MPFR_RNDN) < -2*prec)
      mpfr_set(hr, h, MPFR_RNDN);
    else
      mpfr_set(hl, h, MPFR_RNDN);
    // calculate maximum possible error
    mpfr_sub(error, slwk, srwk, MPFR_RNDN);
    mpfr_abs(error, error, MPFR_RNDN);
    if(mpfr_get_d(error, MPFR_RNDN) < tol)
       cont = 0;
    //printf("h: %15.15f\nlwk: %3.3e\nrwk: %3.3e\n", mpfr_get_d(h, MPFR_RNDN), mpfr_get_d(lwk, MPFR_RNDN), mpfr_get_d(rwk, MPFR_RNDN));
  }

  mpfr_set(*step, h, MPFR_RNDN);

  mpfr_clears(wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error, NULL);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "FindStepSizeERF"
PetscErrorCode FindStepSizeERF(PetscInt prec, PetscInt nPts, mpfr_t *step)
{
  PetscErrorCode ierr;
  const PetscReal tol = 1e-25;
  mpfr_t wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error;
  PetscInt cont;
  PetscFunctionBegin;

  if(prec < 1) {
    printf("prec should be positive\n");
    PetscFunctionReturn(1);
  }

  mpfr_inits(wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error, NULL);

  mpfr_set_d(hl, 0.0, MPFR_RNDN);
  mpfr_set_d(hr, 2.0, MPFR_RNDN);

  /* make hr large enough */
  mpfr_set_d(hr, 1.0, MPFR_RNDN);
  cont = 1;
  while(cont == 1) {
    ierr = ERFwk(nPts, hr, &rwk);CHKERRQ(ierr);
    mpfr_log10(srwk, rwk, MPFR_RNDN);
    if(mpfr_get_d(srwk, MPFR_RNDN) < -2*prec)
      cont = 0;
    else
      mpfr_mul_d(hr, hr, 2.0, MPFR_RNDN); // make hr larger if value not small
  }
  //printf("hr: %15.15f\n", mpfr_get_d(hr, MPFR_RNDN));
  /* make hl small enough */
  mpfr_mul_d(hl, hr, 0.5, MPFR_RNDN);
  cont = 1;
  while(cont == 1) {
    ierr = ERFwk(nPts, hl, &lwk);CHKERRQ(ierr);
    mpfr_log10(slwk, lwk, MPFR_RNDN);
    if(mpfr_get_d(slwk, MPFR_RNDN) > -2*prec)
      cont = 0;
    else
      mpfr_mul_d(hl, hl, 0.5, MPFR_RNDN); // make hl larger if value not small
  }
  //printf("hl: %15.15f\n", mpfr_get_d(hl, MPFR_RNDN));
  ierr = ERFwk(nPts, hl, &lwk);CHKERRQ(ierr);
  ierr = ERFwk(nPts, hr, &rwk);CHKERRQ(ierr);
  //printf("lwk: %3.3e\n", mpfr_get_d(lwk, MPFR_RNDN));
  //printf("rwk: %3.3e\n", mpfr_get_d(rwk, MPFR_RNDN));

  cont = 1;
  while(cont == 1) {
    // average hl and hr
    mpfr_add(h, hl, hr, MPFR_RNDN);
    mpfr_mul_d(h, h, 0.5, MPFR_RNDN);

    // calculate weight and size
    ierr = ERFwk(nPts, h, &wk);CHKERRQ(ierr);
    ierr = ERFwk(nPts, hl, &lwk);CHKERRQ(ierr);
    ierr = ERFwk(nPts, hr, &rwk);CHKERRQ(ierr);
    mpfr_log10(swk, wk, MPFR_RNDN);
    mpfr_log10(slwk, lwk, MPFR_RNDN);
    mpfr_log10(srwk, rwk, MPFR_RNDN);
    if( mpfr_get_d(swk, MPFR_RNDN) < -2*prec)
      mpfr_set(hr, h, MPFR_RNDN);
    else
      mpfr_set(hl, h, MPFR_RNDN);
    // calculate maximum possible error
    mpfr_sub(error, slwk, srwk, MPFR_RNDN);
    mpfr_abs(error, error, MPFR_RNDN);
    if(mpfr_get_d(error, MPFR_RNDN) < tol)
       cont = 0;
    //printf("h: %15.15f\n", mpfr_get_d(h, MPFR_RNDN));
  }
  //printf("h: %15.15f\n", mpfr_get_d(h, MPFR_RNDN));
  //printf("h: %15.15f\nlwk: %3.3e\nrwk: %3.3e\n", mpfr_get_d(h, MPFR_RNDN), mpfr_get_d(lwk, MPFR_RNDN), mpfr_get_d(rwk, MPFR_RNDN));
  mpfr_set(*step, h, MPFR_RNDN);

  mpfr_clears(wk, tmp, h, hl, hr, lwk, rwk, swk, slwk, srwk, error, NULL);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "DEQuad"
PetscErrorCode DEQuad(PetscErrorCode (*f)(mpfr_t*,mpfr_t*,void*), mpfr_t a, mpfr_t b, PetscInt prec, PetscInt nPts, PetscReal *integral, void *ctx)
{
  PetscErrorCode ierr;
  PetscInt k;
  PetscInt nSide;
  mpfr_t alpha, beta;
  mpfr_t leftXk, rightXk;
  mpfr_t wk;
  mpfr_t *wkVals;
  mpfr_t *xkVals;
  mpfr_t lx, rx;
  mpfr_t leftFx, rightFx;
  mpfr_t pi2;
  mpfr_t kh, h;
  mpfr_t sum;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  if(nPts % 2 == 0) {
    nSide = nPts/2; flopCount++;
  }
  if(nPts % 2 == 1) {
    nSide = (nPts-1)/2; flopCount++;
  }

  /* initializations */
  mpfr_inits(alpha, beta, leftXk, rightXk, wk, leftFx, rightFx, pi2, kh, h, sum, lx, rx, NULL);
  wkVals = (mpfr_t*) malloc(sizeof(mpfr_t)*(nSide+1));
  xkVals = (mpfr_t*) malloc(sizeof(mpfr_t)*(nSide+1));
  for(k=0; k<nSide+1; ++k)
    mpfr_inits(xkVals[k], wkVals[k], NULL);

  // init alpha, beta
  mpfr_set(alpha, b, MPFR_RNDN);
  mpfr_sub(alpha, alpha, a, MPFR_RNDN);
  mpfr_mul_d(alpha, alpha, 0.5, MPFR_RNDN);
  mpfr_set(beta, b, MPFR_RNDN);
  mpfr_add(beta, beta, a, MPFR_RNDN);
  mpfr_mul_d(beta, beta, 0.5, MPFR_RNDN); flopCount += 4;

  /* determine right step size */
  ierr = FindStepSize(prec, nSide, &h); // not including in flop count
  //printf("DE step size: %15.15f\n", mpfr_get_d(h, MPFR_RNDN));
  //mpfr_set_d(h, 0.04, MPFR_RNDN);

  /* calculate abscissas, weights */
  ierr = DExkwk(nSide+1, h, &xkVals, &wkVals);CHKERRQ(ierr);

  /* center term */
  mpfr_set_d(sum, 0.0, MPFR_RNDN);
  mpfr_set(lx, b, MPFR_RNDN);
  mpfr_add(lx, lx, a, MPFR_RNDN);
  mpfr_mul_d(lx, lx, .5, MPFR_RNDN);
  f(&lx, &sum, ctx);
  mpfr_mul(sum, sum, wkVals[0], MPFR_RNDN);
  mpfr_mul(sum, sum, alpha,     MPFR_RNDN); flopCount += 4;

  for(k=1; k <= nSide; ++k) {
    // adjust xk to [a,b]
    mpfr_sub_d(lx, xkVals[k], 1.0, MPFR_RNDZ);
    mpfr_mul(lx, lx, alpha, MPFR_RNDU);
    mpfr_add(lx, lx, beta, MPFR_RNDU);
    mpfr_d_sub(rx, 1.0, xkVals[k], MPFR_RNDZ);
    mpfr_mul(rx, rx, alpha, MPFR_RNDD);
    mpfr_add(rx, rx, beta , MPFR_RNDD); flopCount += 6;

    double left = mpfr_get_d(lx, MPFR_RNDN);
    double right = mpfr_get_d(rx, MPFR_RNDN);
    double xk = mpfr_get_d(xkVals[k], MPFR_RNDN);

    /* calculate function values */
    f(&lx, &leftFx , ctx);
    f(&rx, &rightFx, ctx);

    /* update sum */
    mpfr_mul(leftFx, leftFx, wkVals[k], MPFR_RNDN);
    mpfr_mul(rightFx, rightFx, wkVals[k], MPFR_RNDN);
    mpfr_mul(leftFx, leftFx, alpha, MPFR_RNDN);
    mpfr_mul(rightFx, rightFx, alpha, MPFR_RNDN);
    //double l1 = mpfr_get_d(leftFx, MPFR_RNDN);
    //double l2 = mpfr_get_d(rightFx, MPFR_RNDN);
    //printf("leftval: %15.15f\n", l1);
    //printf("rightval: %15.15f\n", l2);
    mpfr_add(sum, sum, leftFx, MPFR_RNDN);
    mpfr_add(sum, sum, rightFx, MPFR_RNDN); flopCount += 6;
  }
  PetscReal prog = mpfr_get_d(sum, MPFR_RNDN);
  //printf("sum: %15.15f\n", prog);

  *integral = prog;

  mpfr_clears(alpha, beta, leftXk, rightXk, wk, leftFx, rightFx, pi2, kh, h, sum, lx, rx, NULL);
  for(k=0; k<nSide+1; ++k)
    mpfr_clears(wkVals[k], xkVals[k], NULL);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "SEQuad"
PetscErrorCode SEQuad(PetscErrorCode (*f)(mpfr_t*,mpfr_t*,void*), mpfr_t a, mpfr_t b, PetscInt prec, PetscInt nPts, PetscReal *integral, void *ctx)
{
  PetscErrorCode ierr;
  PetscInt k;
  PetscInt nSide;
  mpfr_t alpha, beta;
  mpfr_t leftXk, rightXk;
  mpfr_t wk;
  mpfr_t *wkVals;
  mpfr_t *xkVals;
  mpfr_t lx, rx;
  mpfr_t leftFx, rightFx;
  mpfr_t pi2;
  mpfr_t kh, h;
  mpfr_t sum;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  if(nPts % 2 == 0) {
    nSide = nPts/2; flopCount++;
  }
  if(nPts % 2 == 1) {
    nSide = (nPts-1)/2; flopCount++;
  }

  /* initializations */
  mpfr_inits(alpha, beta, leftXk, rightXk, wk, leftFx, rightFx, pi2, kh, h, sum, lx, rx, NULL);
  wkVals = (mpfr_t*) malloc(sizeof(mpfr_t)*(nSide+1));
  xkVals = (mpfr_t*) malloc(sizeof(mpfr_t)*(nSide+1));
  for(k=0; k<nSide+1; ++k)
    mpfr_inits(xkVals[k], wkVals[k], NULL);

  // init alpha, beta
  mpfr_set(alpha, b, MPFR_RNDN);
  mpfr_sub(alpha, alpha, a, MPFR_RNDN);
  mpfr_mul_d(alpha, alpha, 0.5, MPFR_RNDN);
  mpfr_set(beta, b, MPFR_RNDN);
  mpfr_add(beta, beta, a, MPFR_RNDN);
  mpfr_mul_d(beta, beta, 0.5, MPFR_RNDN); flopCount += 4;

  /* determine right step size */
  ierr = FindStepSizeSE(prec, nSide, &h); // not including in flop count
  //printf("step size: %15.15f\n", mpfr_get_d(h, MPFR_RNDN));
  //printf("step size: %15.15f\n", mpfr_get_d(h, MPFR_RNDN));
  //mpfr_set_d(h, 0.04, MPFR_RNDN);

  /* calculate abscissas, weights */
  ierr = SExkwk(nSide+1, h, &xkVals, &wkVals);CHKERRQ(ierr);

  /* center term */
  mpfr_set_d(sum, 0.0, MPFR_RNDN);
  mpfr_set(lx, b, MPFR_RNDN);
  mpfr_add(lx, lx, a, MPFR_RNDN);
  mpfr_mul_d(lx, lx, .5, MPFR_RNDN);
  f(&lx, &sum, ctx);
  mpfr_mul(sum, sum, wkVals[0], MPFR_RNDN);
  mpfr_mul(sum, sum, alpha,     MPFR_RNDN); flopCount += 4;

  for(k=1; k <= nSide; ++k) {
    // adjust xk to [a,b]
    //mpfr_sub_d(lx, xkVals[k], 1.0, MPFR_RNDZ);
    mpfr_mul_d(lx, xkVals[k], -1.0, MPFR_RNDN);
    mpfr_mul(lx, lx, alpha, MPFR_RNDU);
    mpfr_add(lx, lx, beta, MPFR_RNDU);
    //mpfr_d_sub(rx, 1.0, xkVals[k], MPFR_RNDZ);
    mpfr_set(rx, xkVals[k], MPFR_RNDN);
    mpfr_mul(rx, rx, alpha, MPFR_RNDD);
    mpfr_add(rx, rx, beta , MPFR_RNDD); flopCount += 6;

    double left = mpfr_get_d(lx, MPFR_RNDN);
    double right = mpfr_get_d(rx, MPFR_RNDN);
    double xk = mpfr_get_d(xkVals[k], MPFR_RNDN);

    /* calculate function values */
    f(&lx, &leftFx , ctx);
    f(&rx, &rightFx, ctx);

    /* update sum */
    mpfr_mul(leftFx, leftFx, wkVals[k], MPFR_RNDN);
    mpfr_mul(rightFx, rightFx, wkVals[k], MPFR_RNDN);
    mpfr_mul(leftFx, leftFx, alpha, MPFR_RNDN);
    mpfr_mul(rightFx, rightFx, alpha, MPFR_RNDN);
    //double l1 = mpfr_get_d(leftFx, MPFR_RNDN);
    //double l2 = mpfr_get_d(rightFx, MPFR_RNDN);
    //printf("leftval: %15.15f\n", l1);
    //printf("rightval: %15.15f\n", l2);
    mpfr_add(sum, sum, leftFx, MPFR_RNDN);
    mpfr_add(sum, sum, rightFx, MPFR_RNDN); flopCount += 6;
    //printf("sum[%d] = %15.15f\n", k, mpfr_get_d(sum, MPFR_RNDN));
    //printf("wk[%d] = %3.3e\n\n", k, mpfr_get_d(wkVals[k], MPFR_RNDN));
  }
  PetscReal prog = mpfr_get_d(sum, MPFR_RNDN);
  //printf("sum: %15.15f\n", prog);

  *integral = prog;

  mpfr_clears(alpha, beta, leftXk, rightXk, wk, leftFx, rightFx, pi2, kh, h, sum, lx, rx, NULL);
  for(k=0; k<nSide+1; ++k)
    mpfr_clears(wkVals[k], xkVals[k], NULL);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "ERFQuad"
PetscErrorCode ERFQuad(PetscErrorCode (*f)(mpfr_t*,mpfr_t*,void*), mpfr_t a, mpfr_t b, PetscInt prec, PetscInt nPts, PetscReal *integral, void *ctx)
{
  PetscErrorCode ierr;
  PetscInt k;
  PetscInt nSide;
  mpfr_t alpha, beta;
  mpfr_t leftXk, rightXk;
  mpfr_t wk;
  mpfr_t *wkVals;
  mpfr_t *xkVals;
  mpfr_t lx, rx;
  mpfr_t leftFx, rightFx;
  mpfr_t pi2;
  mpfr_t kh, h;
  mpfr_t sum;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  if(nPts % 2 == 0) {
    nSide = nPts/2; flopCount++;
  }
  if(nPts % 2 == 1) {
    nSide = (nPts-1)/2; flopCount++;
  }

  /* initializations */
  mpfr_inits(alpha, beta, leftXk, rightXk, wk, leftFx, rightFx, pi2, kh, h, sum, lx, rx, NULL);
  wkVals = (mpfr_t*) malloc(sizeof(mpfr_t)*(nSide+1));
  xkVals = (mpfr_t*) malloc(sizeof(mpfr_t)*(nSide+1));
  for(k=0; k<nSide+1; ++k)
    mpfr_inits(xkVals[k], wkVals[k], NULL);

  // init alpha, beta
  mpfr_set(alpha, b, MPFR_RNDN);
  mpfr_sub(alpha, alpha, a, MPFR_RNDN);
  mpfr_mul_d(alpha, alpha, 0.5, MPFR_RNDN);
  mpfr_set(beta, b, MPFR_RNDN);
  mpfr_add(beta, beta, a, MPFR_RNDN);
  mpfr_mul_d(beta, beta, 0.5, MPFR_RNDN); flopCount += 4;

  /* determine right step size */
  ierr = FindStepSizeERF(prec, nSide, &h); // not including in flop count
  //mpfr_mul_d(h, h, 0.3, MPFR_RNDN);
  //printf("step size: %15.15f\n", mpfr_get_d(h, MPFR_RNDN));
  //printf("step size: %15.15f\n", mpfr_get_d(h, MPFR_RNDN));
  //mpfr_set_d(h, 0.04, MPFR_RNDN);

  /* calculate abscissas, weights */
  ierr = ERFxkwk(nSide+1, h, &xkVals, &wkVals);CHKERRQ(ierr);

  /* center term */
  mpfr_set_d(sum, 0.0, MPFR_RNDN);
  mpfr_set(lx, b, MPFR_RNDN);
  mpfr_add(lx, lx, a, MPFR_RNDN);
  mpfr_mul_d(lx, lx, .5, MPFR_RNDN);
  f(&lx, &sum, ctx);
  mpfr_mul(sum, sum, wkVals[0], MPFR_RNDN);
  mpfr_mul(sum, sum, alpha,     MPFR_RNDN); flopCount += 4;

  for(k=1; k <= nSide; ++k) {
    // adjust xk to [a,b]
    //mpfr_sub_d(lx, xkVals[k], 1.0, MPFR_RNDZ);
    mpfr_mul_d(lx, xkVals[k], -1.0, MPFR_RNDN);
    mpfr_mul(lx, lx, alpha, MPFR_RNDU);
    mpfr_add(lx, lx, beta, MPFR_RNDU);
    //mpfr_d_sub(rx, 1.0, xkVals[k], MPFR_RNDZ);
    mpfr_mul(rx, xkVals[k], alpha, MPFR_RNDD);
    mpfr_add(rx, rx, beta , MPFR_RNDD); flopCount += 6;

    double left = mpfr_get_d(lx, MPFR_RNDN);
    double right = mpfr_get_d(rx, MPFR_RNDN);
    double xk = mpfr_get_d(xkVals[k], MPFR_RNDN);
    //printf("left: %15.15f\n", left);
    //printf("right: %15.15f\n", right);
    //printf("\n");
    /* calculate function values */
    f(&lx, &leftFx , ctx);
    f(&rx, &rightFx, ctx);

    /* update sum */
    mpfr_mul(leftFx, leftFx, wkVals[k], MPFR_RNDN);
    mpfr_mul(rightFx, rightFx, wkVals[k], MPFR_RNDN);
    mpfr_mul(leftFx, leftFx, alpha, MPFR_RNDN);
    mpfr_mul(rightFx, rightFx, alpha, MPFR_RNDN);
    //double l1 = mpfr_get_d(leftFx, MPFR_RNDN);
    //double l2 = mpfr_get_d(rightFx, MPFR_RNDN);
    //printf("leftval: %15.15f\n", l1);
    //printf("rightval: %15.15f\n", l2);
    mpfr_add(sum, sum, leftFx, MPFR_RNDN);
    mpfr_add(sum, sum, rightFx, MPFR_RNDN); flopCount += 6;
    //printf("sum[%d] = %15.15f\n", k, mpfr_get_d(sum, MPFR_RNDN));
    //printf("wk[%d] = %3.3e\n\n", k, mpfr_get_d(wkVals[k], MPFR_RNDN));
  }
  PetscReal prog = mpfr_get_d(sum, MPFR_RNDN);
  //printf("sum: %15.15f\n", prog);

  *integral = prog;

  mpfr_clears(alpha, beta, leftXk, rightXk, wk, leftFx, rightFx, pi2, kh, h, sum, lx, rx, NULL);
  for(k=0; k<nSide+1; ++k)
    mpfr_clears(wkVals[k], xkVals[k], NULL);
  PetscFunctionReturn(0);
}


#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <mpfr.h>
#include <petsc.h>
#include "ellipsoid.h"
#include "../sphere/sphere.h"
#include "ellSolv.h"

double calcEnp(EllipsoidalSystem *e, Point *point, int n, int p) {
  //if(point->type == 'c')
  //cartesianToEllipsoidal(e, point);
  int signm = 1;
  int signn = 1;
  if(point->x2 < 0)
    signm = -1;
  if(point->x3 < 0)
    signn = -1;
  double eL, eM, eN;
  calcLame(e, n, p, point->x1, signm, signn, &eL);
  calcLame(e, n, p, point->x2, signm, signn, &eM);
  calcLame(e, n, p, point->x3, signm, signn, &eN);

  return eL*eM*eN;
}

double calcGnp(EllipsoidalSystem *e, Point *positions, double *charges, int nCharges, int n, int p)
{
  clock_t start, diff;
  int msec;
  start = clock();
  double normConstant;
  calcNormalization(e, n, p, &normConstant);
  diff = clock() - start;
  msec = diff * 1000 / CLOCKS_PER_SEC;
  //printf("normConstant took %d seconds and %d milliseconds\n", msec/1000, msec%1000);
  //double normConstant = 1.0;
  double sum = 0;
  double Enp;

  for(int k=0; k<nCharges; ++k) {

    Enp = calcEnp(e, positions+k, n, p);

    sum += charges[k] * Enp;
  }

  sum *= (4*PETSC_PI)/((2.0*n+1.0)*normConstant);

  return sum;
}

double calcFnp(EllipsoidalSystem *e, Point *point, int n, int p)
{
  int signm = 1;
  int signn = 1;
  if(point->x2 < 0)
    signm = -1;
  if(point->x3 < 0)
    signn = -1;

  double enpval = calcEnp(e, point, n, p);
  double ival;
  calcI(e, n, p, point->x1, signm, signn, &ival);
  //printf("ENP: %15.15f\nI: %15.15f\n", enpval, ival);
  return (2*n + 1) * enpval * ival;
}

void calcBnpAndCnpFromGnp(Problem *problem, int n, int p, double Gnp, double *Bnp, double *Cnp)
{
  EllipsoidalSystem *e = problem->e;
  double Ea;
  calcLame(e, n, p, e->a, 1, 1, &Ea);
  double Ia;
  calcI(problem->e, n, p, e->a, 1, 1, &Ia);
  double Fa = (2*n + 1) * Ea * Ia;
  double EaDer;
  calcLameDerivative(e, n, p, e->a, 1, 1, &EaDer);
  double IaDer;
  calcIDerivative(e, n, p, e->a, 1, 1, &IaDer);
  double FaDer = (2*n+1) * (Ea*IaDer + EaDer*Ia); //chain rule?
  double e1 = problem->e1;
  double e2 = problem->e2;

  //calc Bnp
  double first = (Fa/Ea)*(e1 - e2)/(e1*e2);
  first /= (1-(e1/e2)*((EaDer*Fa)/(FaDer*Ea)));
  *Bnp = first*Gnp;

  //calc Cnp
  first = (e1/e2)*(EaDer/FaDer)*(*Bnp);
  first += (Gnp/e2);
  *Cnp = first;
}

double calcCoulomb(Problem *problem, int maxDeg, Point *r)
{
  //FILE *fp = fopen("fnpnorm.txt", "w");
  double Gnp, Fnp, Enp;
  double sum = 0;
  int count = 0;
  for(int n=0; n<=maxDeg; ++n) {
    for(int p=0; p < 2*n+1; ++p) {
      clock_t start = clock(), diff;
      Gnp = calcGnp(problem->e, problem->positions, problem->charges, problem->nCharges, n, p);
      diff = clock() - start;
      int msec = diff * 1000 / CLOCKS_PER_SEC;
      //printf("Gnp took %d seconds %d milliseconds\n", msec/1000, msec%1000);
      start = clock();
      Fnp = calcFnp(problem->e, r, n, p);
      diff = clock() - start;
      msec = diff * 1000 / CLOCKS_PER_SEC;
      //printf("Fnp took %d seconds %d milliseconds\n\n", msec/1000, msec%1000);
      //fprintf(fp, "%8.8e %8.8e\n", (double)count, fabs(Fnp));

      sum += (Gnp*Fnp)/problem->e1;

      count++;
    }
    printf("n: %d\n", n);
    printf("Gnp: %16.16f\n", Gnp);
    printf("Fnp: %16.16f\n", Fnp);
    //printf("enp: %16.16f\n", Enp);
    printf("sum: %16.16f\n", sum);

  }
  //fclose(fp);
  return sum;
}

double calcCoulombEllipsoidalGrid(Problem *problem, int maxDeg, Point *r, int nPoints, double *solution) {

  EllipsoidalSystem *e = problem->e;
  double Gnp, Bnp, Fnp, Enp, Cnp;
  for(int i=0; i<nPoints; ++i)
    solution[i] = 0;
  int count = 0;
  double *fnpvals = (double*) malloc(sizeof(double)*nPoints);

  for(int n=0; n<=maxDeg; ++n) {

    for(int p=0; p < 2*n+1; ++p) {
      Gnp = calcGnp(problem->e, problem->positions, problem->charges, problem->nCharges, n, p);
      calcBnpAndCnpFromGnp(problem, n, p, Gnp, &Bnp, &Cnp);

      for(int k=0; k<nPoints; ++k) {
	//if on exterior
	if(fabs(r[k].x1) >= e->a) {
	  Fnp = calcFnp(problem->e, r+k, n, p);
	  solution[k] += Cnp*Fnp;
	}
	else {
	  Enp = calcEnp(problem->e, r+k, n, p);
	  solution[k] += Bnp*Enp;
	  if(n==3 && p==2) {
	    printf("do we get here2\n");
	    printf("(%d,%d)\n", n, p);
	    printf("Enp: %15.15f\n", Enp);
	    printf("Bnp: %15.15f\n", Bnp);
	    printf("\n");
	  }
	}
      }

      count++;
    }
    //printf("n: %d\n", n);
    //printf("Gnp: %16.16f\n", Gnp);
    //printf("Cnp: %16.16f\n", Cnp);
    //printf("Fnp: %16.16f\n", Fnp);
    //printf("enp: %16.16f\n", Enp);
    //printf("sum: %16.16f\n", solution[0]);

  }
  //fclose(fp);
  return solution[0];
}


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

/* Author:  G. Jungman */

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

#define PRINT(n) printf("%22.18g  %22.18g  %22.18g  %22.18g\n", F[n], Fp[n], G[n], Gp[n])

#define WKB_TOL (1.0e+04 * TEST_SQRT_TOL0)

int test_coulomb(void)
{
  gsl_sf_result r;
  int status = 0;
  int s = 0;

  char message_buff[2048];

  /* const int kmax = 20; */
  /* double F[kmax+1], Fp[kmax+1], G[kmax+1], Gp[kmax+1]; */
  gsl_sf_result F, Fp, G, Gp;
  double Fe, Ge;
  double lam_min;
  double lam_F;
  double eta, x;
  int k_G;

  TEST_SF(s, gsl_sf_hydrogenicR_1_e, (3.0, 2.0, &r),  0.025759948256148471036,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hydrogenicR_1_e, (3.0, 10.0, &r), 9.724727052062819704e-13, TEST_TOL1, GSL_SUCCESS);
  status += s;

  TEST_SF(s, gsl_sf_hydrogenicR_e, (4, 1, 3.0, 0.0, &r),  0.0,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hydrogenicR_e, (4, 0, 3.0, 2.0, &r), -0.03623182256981820062,  TEST_TOL2, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hydrogenicR_e, (4, 1, 3.0, 2.0, &r), -0.028065049083129581005, TEST_TOL2, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hydrogenicR_e, (4, 2, 3.0, 2.0, &r),  0.14583027278668431009,  TEST_TOL0, GSL_SUCCESS);
  status += s;

  TEST_SF(s, gsl_sf_hydrogenicR_e, (100,  0, 3.0, 2.0, &r), -0.00007938950980052281367, TEST_TOL3, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hydrogenicR_e, (100, 10, 3.0, 2.0, &r),  7.112823375353605977e-12,  TEST_TOL2, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hydrogenicR_e, (100, 90, 3.0, 2.0, &r),  5.845231751418131548e-245, TEST_TOL2, GSL_SUCCESS);
  status += s;

  lam_F = 0.0;
  k_G   = 0;
  eta = 1.0;
  x = 5.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  0.6849374120059439677, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp, -0.7236423862556063963, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G, -0.8984143590920205487, TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp, -0.5108047585190350106, TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s,"  gsl_sf_coulomb_wave_FG_e(1.0, 5.0, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 10.0;
  k_G   = 2;
  eta = 1.0;
  x = 5.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  0.0006423773354915823698, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  0.0013299570958719702545, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  33.27615734455096130,     TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp, -45.49180102261540580,     TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s,"  gsl_sf_coulomb_wave_FG_e(1.0, 5.0, lam_F=10, lam_G=8)");
  status += s;

  lam_F = 4.0;
  k_G   = 2;
  eta = 50.0;
  x = 120.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  0.0735194711823798495, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  0.6368149124126783325, TEST_TOL3);
  /*
  s += test_sf_check_result(message_buff,  G,  , TEST_TOL5);
  s += test_sf_check_result(message_buff, Gp, , TEST_TOL5);
  */
  printf("%s", message_buff);
  gsl_test(s,"  gsl_sf_coulomb_wave_FG_e(50.0, 120.0, lam_F=4, lam_G=2)");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = -1000.0;
  x = 1.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  9.68222518991341e-02, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  5.12063396274631e+00, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  1.13936784379472e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp, -4.30243486522438e+00, TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(-1000.0, 1.0, lam_F=0, lam_G=0)");
  status += s;

  lam_min = 0.0;
  eta = -50.0;
  x = 5.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  1.52236975714236e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  2.03091041166137e+00, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  4.41680690236251e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp, -6.76485374766869e-01, TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(-50.0, 5.0, lam_F=0, lam_G=0)");
  status += s;

  lam_min = 0.0;
  eta = -50.0;
  x = 1000.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F, -0.2267212182760888523, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp, -0.9961306810018401525, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G, -0.9497684438900352186, TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp,  0.2377656295411961399, TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(-50.0, 1000.0, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 10.0;
  k_G = 0;
  eta = -50.0;
  x = 5.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F, -3.681143602184922e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  1.338467510317215e+00, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  3.315883246109351e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp,  1.510888628136180e+00, TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(-50.0, 5.0, lam_F=10, lam_G=10)");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = -4.0;
  x = 5.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  4.078627230056172e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  1.098212336357310e+00, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  6.743270353832442e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp, -6.361104272804447e-01, TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(-4.0, 5.0, lam_F=0, lam_G=0");
  status += s;

  lam_F = 3.0;
  k_G = 0;
  eta = -4.0;
  x = 5.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F, -2.568630935581323e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  1.143229422014827e+00, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  7.879899223927996e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp,  3.859905878106713e-01, TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(-4.0, 5.0, lam_F=3, lam_G=3");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = 1.0;
  x = 2.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  6.61781613832681e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  4.81557455709949e-01, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  1.27577878476828e+00, TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp, -5.82728813097184e-01, TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(1.0, 2.0, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = 1.0;
  x = 0.5;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  0.08315404535022023302, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  0.22693874616222787568, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  3.1060069279548875140,  TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp, -3.549156038719924236,   TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(1.0, 0.5, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 0.5;
  k_G = 0;
  eta = 1.0;
  x = 0.5;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  0.04049078073829290935, TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  0.14194939168094778795, TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  4.720553853049677897,   TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp, -8.148033852319180005,   TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(1.0, 0.5, lam_F=0.5, lam_G=0.5)");
  status += s;

  lam_F = 0.1;
  k_G = 0;
  eta = 1.0;
  x = 0.5;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  0.07365466672379703418, TEST_TOL5);
  s += test_sf_check_result(message_buff, Fp,  0.21147121807178518647, TEST_TOL5);
  s += test_sf_check_result(message_buff,  G,  3.306705446241024890, TEST_TOL5);
  s += test_sf_check_result(message_buff, Gp, -4.082931670935696644, TEST_TOL5);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(1.0, 0.5, lam_F=0.1, lam_G=0.1)");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = 8.0;
  x = 1.05;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  9.882706082810274357e-09, TEST_TOL5);
  s += test_sf_check_result(message_buff, Fp,  4.005167028235547770e-08, TEST_TOL5);
  s += test_sf_check_result(message_buff,  G,  1.333127992006686320e+07, TEST_SQRT_TOL0);
  s += test_sf_check_result(message_buff, Gp, -4.715914530842402330e+07, TEST_SQRT_TOL0);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(8.0, 1.05, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 0.1;
  k_G = 0;
  eta = 8.0;
  x = 1.05;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  9.611416736061987761e-09, TEST_TOL5);
  s += test_sf_check_result(message_buff, Fp,  3.909628126126824140e-08, TEST_TOL5);
  s += test_sf_check_result(message_buff,  G,  1.365928464219262581e+07, 4.0*TEST_SQRT_TOL0);
  s += test_sf_check_result(message_buff, Gp, -4.848117385783386850e+07, 4.0*TEST_SQRT_TOL0);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(8.0, 1.05, lam_F=0.1, lam_G=0.1)");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = 50.0;
  x = 0.1;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  2.807788027954216071e-67, TEST_TOL5);
  s += test_sf_check_result(message_buff, Fp,  9.677600748751576606e-66, TEST_TOL5);
  s += test_sf_check_result(message_buff,  G,  5.579810686998358766e+64, TEST_SQRT_TOL0);
  s += test_sf_check_result(message_buff, Gp, -1.638329512756321424e+66, TEST_SQRT_TOL0);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(50.0, 0.1, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = 10.0;
  x = 5.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  1.7207454091787930614e-06, 10.0*WKB_TOL);
  s += test_sf_check_result(message_buff, Fp,  3.0975994706405458046e-06, 10.0*WKB_TOL);
  s += test_sf_check_result(message_buff,  G,  167637.56609459967623, 10.0*WKB_TOL);
  s += test_sf_check_result(message_buff, Gp, -279370.76655361803075, 10.0*WKB_TOL);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(10.0, 5.0, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = 25.0;
  x = 10.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  1.5451274501076114315e-16, 5.0*WKB_TOL);
  s += test_sf_check_result(message_buff, Fp,  3.1390869393378630928e-16, 5.0*WKB_TOL);
  s += test_sf_check_result(message_buff,  G,  1.6177129008336318136e+15, 5.0*WKB_TOL);
  s += test_sf_check_result(message_buff, Gp, -3.1854062013149740860e+15, 5.0*WKB_TOL);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(25.0, 10.0, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 0.0;
  k_G = 0;
  eta = 1.0;
  x = 9.2;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F, -0.25632012319757955655, TEST_TOL5);
  s += test_sf_check_result(message_buff, Fp,  0.91518792286724220370, TEST_TOL5);
  s += test_sf_check_result(message_buff,  G,  1.03120585918973466110, TEST_SQRT_TOL0);
  s += test_sf_check_result(message_buff, Gp,  0.21946326717491250193, TEST_SQRT_TOL0);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(1.0, 9.2, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 0.0;
  eta = 10.0;
  x = 10.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  0.0016262711250135878249, WKB_TOL);
  s += test_sf_check_result(message_buff, Fp,  0.0017060476320792806014, WKB_TOL);
  s += test_sf_check_result(message_buff,  G,  307.87321661090837987, WKB_TOL);
  s += test_sf_check_result(message_buff, Gp, -291.92772380826822871, WKB_TOL);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(10.0, 10.0, lam_F=0, lam_G=0)");
  status += s;

  lam_F = 0.0;
  eta = 100.0;
  x = 1.0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  8.999367996930662705e-126, 10.0*WKB_TOL);
  s += test_sf_check_result(message_buff, Fp,  1.292746745757069321e-124, 10.0*WKB_TOL);
  s += test_sf_check_result(message_buff,  G,  3.936654148133683610e+123, 10.0*WKB_TOL);
  s += test_sf_check_result(message_buff, Gp, -5.456942268061526371e+124, 10.0*WKB_TOL);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(100.0, 1.0, lam_F=0, lam_G=0)");
  status += s;

  /* compute F_1(eta=0,x=3.25), F'_1 and G_1(eta=0,x=3.25), G'_1 */

  lam_F = 1.0;
  eta = 0.0;
  x = 3.25;
  k_G = 0;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  sin(x)/x - cos(x), TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  -sin(x)/(x*x) + cos(x)/x +sin(x), TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  cos(x)/x + sin(x), TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp,  -cos(x)/(x*x) - sin(x)/x + cos(x), TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(3.25, 0.0, lam_F=1, lam_G=1)");
  status += s;

  /* compute F_1(eta=0,x=3.25), F'_1 and G_0(eta=0,x=3.25), G'_0 */

  lam_F = 1.0;
  eta = 0.0;
  x = 3.25;
  k_G = 1;
  gsl_sf_coulomb_wave_FG_e(eta, x, lam_F, k_G, &F, &Fp, &G, &Gp, &Fe, &Ge);
  s = 0;
  message_buff[0] = 0;
  s += test_sf_check_result(message_buff,  F,  sin(x)/x - cos(x), TEST_TOL3);
  s += test_sf_check_result(message_buff, Fp,  -sin(x)/(x*x) + cos(x)/x +sin(x), TEST_TOL3);
  s += test_sf_check_result(message_buff,  G,  cos(x), TEST_TOL3);
  s += test_sf_check_result(message_buff, Gp,  -sin(x), TEST_TOL3);
  printf("%s", message_buff);
  gsl_test(s, "  gsl_sf_coulomb_wave_FG_e(3.25, 0.0, lam_F=1, lam_G=0)");
  status += s;

  return status;
}


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

typedef struct ornsteinuhlenbeckprocess_{
  int steps;
  double dt;
  double sigma;
  double *x;
  gsl_rng *rng;
} OrnsteinUhlenbeckProcess;

void OrnsteinUhlenbeckProcess_run(OrnsteinUhlenbeckProcess *self) {
  int i;
  double sgm = self->sigma * sqrt(self->dt);
  for (i = 1; i < self->steps; ++i){
    self->x[i] = (1 - self->dt) * self->x[i-1] \
      + gsl_ran_gaussian_ziggurat(self->rng, sgm);
  }
}


#ifndef CONVOLUTION_H
#define CONVOLUTION_H

#include <cstddef>

#include <cstring>

#include <iostream>

extern "C" {
  #include <cblas.h>
}

using namespace std;

template<typename T>
constexpr T relu(T x) {
  return (x > 0) ? x : 0;
}

template<typename T>
void mul_mat(T *A, double *B, double *C, size_t m, size_t k, size_t n) {
  for(size_t r = 0; r < m; r++) {
    for(size_t c = 0; c < n; c++) {
      double x = 0.0;
      for(size_t z = 0; z < k; z++) {
        x += A[r * k + z] * B[z * n + c];
      }
      C[r * n + c] = x;
    }
  }
}

template<size_t in_maps, size_t in_height, size_t in_width,
         size_t conv_height, size_t conv_width,
         size_t vert_stride, size_t horiz_stride,
         size_t out_maps>
struct ConvolutionLayer {
  static constexpr size_t conv_size = conv_height * conv_width;
  static constexpr size_t in_map_size = in_height * in_width;
  static constexpr size_t inputs_no = in_maps * in_height * in_width;
  static constexpr size_t parameters_no = out_maps * (conv_width * conv_height * in_maps + 1);
  static constexpr size_t bias_idx = conv_width * conv_height * in_maps;
  static constexpr size_t out_height = (in_height - conv_height) / vert_stride +1;
  static constexpr size_t out_width = (in_width - conv_width) / horiz_stride + 1;
  static constexpr size_t out_map_size = out_height * out_width;
  static constexpr size_t internal_no = out_maps * out_height * out_width;
  static constexpr size_t outputs_no = out_maps * out_height * out_width;

  static constexpr size_t p_idx(size_t out_map, size_t in_map, size_t conv_r, size_t conv_c) {
    return out_map * (in_maps * conv_height * conv_width + 1) + in_map * (conv_height * conv_width)
      + conv_r * conv_width + conv_c;
  }

  static void forward(double* inputs, double * parameters, double* outputs) {
    for (size_t j = 0; j < out_maps; j++) {
      double *pj = &(parameters[j*(conv_height * conv_width * in_maps + 1)]);

      for (size_t out_r = 0; out_r < out_height; out_r++) {
        for (size_t out_c = 0; out_c < out_width; out_c++) {
          const size_t out_idx = j * (out_map_size) + out_r * out_width + out_c;
          double a = 0;
          for (size_t i = 0; i < in_maps; i++) {
            const size_t i_idx = i * conv_size;
            const size_t i2_idx = i * in_map_size;
            for (size_t conv_r = 0; conv_r < conv_height; conv_r++) {
              for (size_t conv_c = 0; conv_c < conv_width; conv_c++) {
                size_t p2_idx =  i_idx + conv_r * conv_width + conv_c;
                size_t in_idx =
                  i2_idx + (out_r * vert_stride + conv_r) * in_width + out_c * horiz_stride + conv_c;
                a += pj[p2_idx] * inputs[in_idx];
              }
            }
          }
          a += pj[bias_idx];
          outputs[out_idx] = relu(a);
        }
      }
    }
  }
};

template<size_t in_maps, size_t in_height, size_t in_width,
         size_t conv_height, size_t conv_width,
         size_t vert_stride, size_t horiz_stride,
         size_t out_maps>
struct ConvolutionLayer2 {
  static constexpr size_t in_map_size = in_height * in_width;
  static constexpr size_t inputs_no = in_maps * in_map_size;
  static constexpr size_t out_height = (in_height - conv_height) / vert_stride +1;
  static constexpr size_t out_width = (in_width - conv_width) / horiz_stride + 1;
  static constexpr size_t out_map_size = out_height * out_width;
  static constexpr size_t internal_no = out_maps * out_map_size;
  static constexpr size_t outputs_no = out_maps * out_map_size;
  static constexpr size_t conv_size = conv_height * conv_width;
  static constexpr size_t parameters_no = out_maps * (in_maps * conv_size + 1);

  static constexpr size_t p_idx(size_t out_map, size_t in_map, size_t conv_r, size_t conv_c) {
    return out_map * (in_maps * conv_height * conv_width + 1) + in_map * (conv_height * conv_width)
      + conv_r * conv_width + conv_c;
  }

  static void forward(double* inputs, double *parameters, double* outputs) {
    double* _in = (double*) malloc(sizeof(double) * in_maps * conv_size * out_map_size);

    for (size_t out_r = 0; out_r < out_height; out_r++) {
      for (size_t out_c = 0; out_c < out_width; out_c++) {
        for (size_t i = 0; i < in_maps; i++) {
          for (size_t conv_r = 0; conv_r < conv_height; conv_r++) {
            for (size_t conv_c = 0; conv_c < conv_width; conv_c++) {
              size_t in_idx =
                i * in_map_size +
                (out_r * vert_stride + conv_r) * in_width +
                out_c * horiz_stride + conv_c;
              size_t row_idx = i * conv_size + conv_r * conv_width + conv_c;
              size_t _in_idx = row_idx * out_map_size + out_r * out_width + out_c;
              _in[_in_idx] = inputs[in_idx];
            }
          }
        }
      }
    }

    for (size_t j = 0; j < out_maps;j++) {
      double bias = parameters[(j+1) * (in_maps * conv_size + 1) - 1];
      for (size_t i = 0; i < out_map_size; i++) {
        outputs[j * out_map_size + i] = bias;
      }
    }

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                out_maps, out_map_size, in_maps * conv_size,
                1.0, parameters, in_maps * conv_size + 1,
                _in, out_map_size,
                1.0, outputs, out_map_size);

    for(size_t j = 0; j < out_maps * out_map_size; j++) {
      outputs[j] = relu(outputs[j]);
    }
  }
};

#endif


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

#define N 8

int
main (void)
{
  int i;
  double xi, yi;
  double x[N] = { 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0 };
  double y[N] = { 0.0, 0.0, 2.0, 0.0,-1.0,-2.0,-1.0, 0.0 };

  printf ("#m=0,S=2\n");

  for (i = 0; i < N; i++)
    {
      printf ("%g %g\n", x[i], y[i]);
    }

  printf ("#m=1,S=0\n");

  {
    gsl_interp_accel *acc = gsl_interp_accel_alloc ();
    gsl_spline *spline = gsl_spline_alloc (gsl_interp_akima, N);
    gsl_spline_init (spline, x, y, N);

    for (xi = x[0]; xi < x[N-1]; xi += 0.01)
      {
        double yi = gsl_spline_eval (spline, xi, acc);
        printf ("%g %g\n", xi, yi);
      }
    gsl_spline_free (spline);
    gsl_interp_accel_free(acc);
  }
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include "fermi.h"

int main(int argc, char *argv[]) {
    int num_bands = 2;
    double t = 1.0;
    double E0 = 6.0;
    double deltaE = 14.0;
    void Efn(double k[3], gsl_vector *energies) {
        double cx = cos(2.0 * M_PI * k[0]);
        double cy = cos(2.0 * M_PI * k[1]);
        double cz = cos(2.0 * M_PI * k[2]);
        double tk = -2.0 * t * (cx + cy + cz);
        int i;
        double E0_band;
        for (i = 0; i < num_bands; i++) {
            E0_band = E0 + ((double)i) * deltaE;
            gsl_vector_set(energies, i, E0_band + tk);
        }
    }
    int na = 8;
    int nb = 8;
    int nc = 8;
    int G_order[3] = {0, 1, 2};
    int G_neg[3] = {1, 1, 1};
    bool use_cache = true;

    EnergyCache *Ecache = init_EnergyCache(na, nb, nc, num_bands, G_order, G_neg, Efn, use_cache);

    double num_electrons = 1.0;
    double E_Fermi = 0.0;
    int err = FindFermi(num_electrons, Ecache, &E_Fermi);
    printf("Got E_Fermi = %f\n", E_Fermi);
    if (err != CTETRA_BISECT_OK) {
        printf("Error = %d returned from FindFermi.\n", err);
        return err;
    }

    double expected = 13.0;
    if (E_Fermi < 12.0 || E_Fermi > 14.0) {
        printf("Incorrect E_Fermi; got %f, expected %f\n", E_Fermi, expected);
        return 1;
    }

    num_electrons = 0.5;
    err = FindFermi(num_electrons, Ecache, &E_Fermi);
    printf("Got E_Fermi = %f\n", E_Fermi);
    if (err != CTETRA_BISECT_OK) {
        printf("Error = %d returned from FindFermi.\n", err);
        return err;
    }

    double tol = 1e-8;
    expected = 6.0;
    if (fabs(E_Fermi - expected) > tol) {
        printf("Incorrect E_Fermi; got %f, expected %f\n", E_Fermi, expected);
        return 1;
    }

    printf("Fermi test passed.\n");
    return 0;
}


#include "ridc.h"
#include <stdlib.h>
#include <stdio.h>
#include <cmath>
#include <gsl/gsl_linalg.h>

#ifndef _BRUSSELATOR_H_
#define _BRUSSELATOR_H_

class Brusselator_GSL : public ODE {
public:
  Brusselator_GSL(int my_neq, int my_nt, double my_ti, double my_tf, double my_dt) {
    neq = my_neq;
    nt = my_nt;
    ti = my_ti;
    tf = my_tf;
    dt = my_dt;
  }

  void rhs(double t, double *u, double *f) {
    /** user implemented rhs function, u'=rhs(t,u)
       @return (by reference) f: rhs(t,u)
       @param t current time step
       @param u solution u at time t
       @param f rhs(t,u)
    */
    double A = 1.0;
    double B = 3.0;
    double alpha = 0.02;
    double dx = 1.0/(neq+1);
    int Nx = neq/2;

    f[0] = A+u[0]*u[0]*u[Nx] -(B+1.0)*u[0] +alpha/dx/dx*(u[1]-2*u[0]+1.0);
    f[Nx-1] = A+u[Nx-1]*u[Nx-1]*u[2*Nx-1] -(B+1.0)*u[Nx-1]
      + alpha/dx/dx*(1.0-2*u[Nx-1]+u[Nx-2]);

    f[Nx] = B*u[0]- u[0]*u[0]*u[Nx] + alpha/dx/dx*(u[Nx+1]-2*u[Nx]+3.0);
    f[2*Nx-1] = B*u[Nx-1]- u[Nx-1]*u[Nx-1]*u[2*Nx-1]
      + alpha/dx/dx*(3.0-2*u[2*Nx-1]+u[2*Nx-2]);

    for (int i=1; i<Nx-1; i++) {
      f[i] = A + u[i]*u[i]*u[Nx+i] -(B+1.0)*u[i] +alpha/dx/dx*(u[i+1]-2*u[i]+u[i-1]);
      f[Nx+i] = B*u[i]- u[i]*u[i]*u[Nx+i] + alpha/dx/dx*(u[Nx+i+1]-2*u[Nx+i]+u[Nx+i-1]);
    }
  }

  void step(double t, double * u, double * unew) {
    /** user implemented step function, for advancing the solution from t to t+dt
       @return (by reference) unew: solution at time t+dt
       @param t current time step
       @param u solution u at time t
       @param unew solution at time t+dt
    */
    double tnew = t + dt;

    double NEWTON_TOL = 1.0e-14;
    int NEWTON_MAXSTEP = 1000;

    double * J;
    double * stepsize;
    double * uguess;

    stepsize = new double[neq];
    uguess = new double[neq];

    J = new double[neq*neq];

    // GSL
    gsl_matrix_view m;
    gsl_vector_view b;
    gsl_vector *x = gsl_vector_alloc(neq);
    gsl_permutation *p = gsl_permutation_alloc(neq);

    // set initial condition
    for (int i=0; i<neq; i++) {
      uguess[i] = u[i];
    }

    double maxstepsize;

    int counter=0;
    int * pivot = new int[neq];

    while (1) {

      // create rhs
      newt(tnew,u,uguess,stepsize);

      // compute numerical Jacobian
      jac(tnew,uguess,J);

      m = gsl_matrix_view_array(J,neq,neq);
      b = gsl_vector_view_array(stepsize,neq);

      int s; // presumably, this is some flag info

      gsl_linalg_LU_decomp (&m.matrix,p,&s);
      gsl_linalg_LU_solve (&m.matrix,p,&b.vector, x);

      // check for convergence
      maxstepsize = 0.0;
      for (int i = 0; i<neq; i++) {
	uguess[i] = uguess[i] - x->data[i];
	if (std::abs(stepsize[i])>maxstepsize) {
	  maxstepsize = std::abs(x->data[i]);
	}
      }

      // if update sufficiently small enough
      if ( maxstepsize < NEWTON_TOL) {
	break;
      }

      counter++;
      //error if too many steps
      if (counter > NEWTON_MAXSTEP) {
	fprintf(stderr,"max Newton iterations reached\n");
	exit(42);
      }
    } // end Newton iteration

    for (int i=0; i<neq; i++) {
      unew[i] = uguess[i];
    }

    delete [] pivot;
    delete [] uguess;
    delete [] stepsize;
    delete [] J;

    gsl_permutation_free (p);
    gsl_vector_free (x);
  }

 private:
  void newt(double t, double *uprev, double *uguess,
	    double *g){
    /** Helper function to compute the next Newton step
       for solving a system of equations

       @return (by reference) g how far from zero we are
       @param t current time step
       @param uguess current solution guess
       @param uprev solution at previous time step
       @param g how far from zero we are, returned by reference
    */
    rhs(t,uguess,g);
    for (int i =0; i<neq; i++) {
      g[i] = uguess[i]-uprev[i]-dt*g[i];
    }
  }

  void jac(double t, double *u, double *J){
    /** Helper function to the Jacobian matrix (using finite differences)
       for advancing the solution from time t(n) to t(n+1) using an
       implicit Euler step on a system of equations

       @return (by reference) J the Jacobian for the Newton step
       @param t current time step
       @param u function value at the current time step
       @param J Jacobian, returned by reference
    */
    double d = 1e-5; // numerical Jacobian approximation
    double *u1;
    double *f1;
    double *f;

    // need to allocate memory
    u1 = new double[neq];
    f1 = new double[neq];
    f = new double[neq];

    // note, instead of using newt, use rhs for efficiency
    rhs(t,u,f);

    for (int i = 0; i<neq; i++) {
      for (int j = 0; j<neq; j++) {
	u1[j] = u[j];
      }
      u1[i] = u1[i] + d;

      rhs(t,u1,f1);

      for (int j = 0; j<neq; j++) {
	J[j*neq+i] = -dt*(f1[j]-f[j])/d;
      }
      J[i*neq+i] = 1.0+J[i*neq+i];
    }

    // need to delete memory
    delete [] u1;
    delete [] f1;
    delete [] f;
  }

};

#endif // _BRUSSELATOR_H_


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <mpfr.h>
#include <petsc.h>

#include "../ellipsoid/ellipsoid.h"
#include "../ellipsoid/ellSolv.h"
#include "../sphere/sphere.h"
#include "../sphere/sphSolv.h"
#include "../constants.h"

#undef __FUNCT__
#define __FUNCT__ "CoulombExact"
PetscErrorCode CoulombExact(PetscReal eps1, Vec srcXYZ, Vec srcMag, Vec tarXYZ, Vec sol)
{
  PetscErrorCode ierr;
  PetscReal srcX, srcY, srcZ;
  PetscReal tarX, tarY, tarZ;
  PetscReal dx2, dy2, dz2;
  PetscReal val, mag;
  const PetscScalar *srcXYZArray;
  const PetscScalar *srcMagArray;
  const PetscScalar *tarXYZArray;
  PetscScalar *solArray;
  PetscInt nSrc, nTar;
  PetscInt i, k;
  PetscFunctionBeginUser;

  ierr = VecGetSize(srcMag, &nSrc);CHKERRQ(ierr);
  ierr = VecGetSize(sol, &nTar);CHKERRQ(ierr);

  ierr = VecGetArrayRead(srcXYZ, &srcXYZArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(tarXYZ, &tarXYZArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(srcMag, &srcMagArray);CHKERRQ(ierr);
  ierr = VecGetArray    (sol   , &solArray   );CHKERRQ(ierr);
  for(i=0; i<nTar; ++i) {
    /* get target X,Y,Z */
    tarX = tarXYZArray[3*i+0];
    tarY = tarXYZArray[3*i+1];
    tarZ = tarXYZArray[3*i+2];
    for(k=0; k<nSrc; ++k) {
      /* get source X,Y,Z */
      srcX = srcXYZArray[3*k+0];
      srcY = srcXYZArray[3*k+1];
      srcZ = srcXYZArray[3*k+2];
      mag  = srcMagArray[k];

      dx2 = (tarX-srcX)*(tarX-srcX);
      dy2 = (tarY-srcY)*(tarY-srcY);
      dz2 = (tarZ-srcZ)*(tarZ-srcZ);

      val = mag/(eps1*PetscSqrtReal(dx2 + dy2 + dz2));
      solArray[i] += val;
    }
  }
  ierr = VecRestoreArrayRead(srcXYZ, &srcXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(tarXYZ, &tarXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(srcMag, &srcMagArray);CHKERRQ(ierr);
  ierr = VecRestoreArray    (sol   , &solArray   );CHKERRQ(ierr);

  PetscFunctionReturn(0);
}


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

/*
 * The code in this module is based on IQPACK, specifically the LGPL
 * implementation found in HERMITE_RULE:
 * https://people.sc.fsu.edu/~jburkardt/c_src/hermite_rule/hermite_rule.html
 */

#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_sf_gamma.h>

static int
jacobi_check(const size_t n, const gsl_integration_fixed_params * params)
{
  (void) n;

  if (fabs(params->b - params->a) <= GSL_DBL_EPSILON)
    {
      GSL_ERROR("|b - a| too small", GSL_EDOM);
    }
  else if (params->a >= params->b)
    {
      GSL_ERROR("lower integration limit must be smaller than upper limit", GSL_EDOM);
    }
  else if (params->alpha <= -1.0 || params->beta <= -1.0)
    {
      GSL_ERROR("alpha and beta must be > -1", GSL_EDOM);
    }
  else
    {
      return GSL_SUCCESS;
    }
}

static int
jacobi_init(const size_t n, double * diag, double * subdiag, gsl_integration_fixed_params * params)
{
  const double absum = params->beta + params->alpha;
  const double abdiff = params->beta - params->alpha;
  const double a2b2 = absum * abdiff; /* beta^2 - alpha^2 */
  size_t i;

  /* construct the diagonal and subdiagonal elements of Jacobi matrix */
  diag[0] = abdiff/(absum + 2.0);
  subdiag[0] = 2.0*sqrt((params->alpha + 1.0)*(params->beta + 1.0)/(absum + 3.0))/(absum + 2.0);
  for (i = 1; i < n; i++)
    {
      diag[i] = a2b2 / ( (absum + 2.0*i) * (absum + 2.0*i + 2.0) );
      subdiag[i] = sqrt ( 4.0*(i + 1.0) * (params->alpha + i + 1.0) * (params->beta + i + 1.0) * (absum + i + 1.0) / ( pow((absum + 2.0*i + 2.0), 2.0) - 1.0 ) ) / ( absum + 2.0*i + 2.0 );
    }

  params->zemu = pow(2.0, absum + 1.0) * gsl_sf_gamma(params->alpha + 1.0) * gsl_sf_gamma(params->beta + 1.0) / gsl_sf_gamma(absum + 2.0);
  params->shft = 0.5*(params->b + params->a);
  params->slp = 0.5*(params->b - params->a);
  params->al = params->alpha;
  params->be = params->beta;

  return GSL_SUCCESS;
}

static const gsl_integration_fixed_type jacobi_type =
{
  jacobi_check,
  jacobi_init
};

const gsl_integration_fixed_type *gsl_integration_fixed_jacobi = &jacobi_type;


#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

#ifndef gauge_config
#include "config.h"
#define gauge_config 1

#define sq(x) ((x)*(x))
#define cb(x) ((x)*(x)*(x))
#endif

// SU(3) Matrix
// only the first two rows carries useful information (Lang p.80)
// note that the first two rows must be orthonormal
// use su3_normalize() for this
typedef struct{
  gsl_complex a1;
  gsl_complex a2;
  gsl_complex a3;
  gsl_complex b1;
  gsl_complex b2;
  gsl_complex b3;
} su3_matrix;

// SU(3) matrix methods --------------------------------------

// print matrix
void su3_print(const su3_matrix A){
  printf("%f + i*%f \t", GSL_REAL(A.a1), GSL_IMAG(A.a1));
  printf("%f + i*%f \t", GSL_REAL(A.a2), GSL_IMAG(A.a2));
  printf("%f + i*%f \n", GSL_REAL(A.a3), GSL_IMAG(A.a3));
  printf("%f + i*%f \t", GSL_REAL(A.b1), GSL_IMAG(A.b1));
  printf("%f + i*%f \t", GSL_REAL(A.b2), GSL_IMAG(A.b2));
  printf("%f + i*%f \n", GSL_REAL(A.b3), GSL_IMAG(A.b3));
}

// the identity matrix
su3_matrix su3_unit(){
  su3_matrix u;
  u.a1=GSL_COMPLEX_ONE;
  u.a2=GSL_COMPLEX_ZERO;
  u.a3=GSL_COMPLEX_ZERO;
  u.b1=GSL_COMPLEX_ZERO;
  u.b2=GSL_COMPLEX_ONE;
  u.b3=GSL_COMPLEX_ZERO;
  return u;
}

// normalize matrix
su3_matrix su3_norm(const su3_matrix A){
  su3_matrix result;
  // normalize first row
  double norm = sqrt(gsl_complex_abs2(A.a1)+gsl_complex_abs2(A.a2)+gsl_complex_abs2(A.a3));
  result.a1 = gsl_complex_div(A.a1,gsl_complex_rect(norm,0));
  result.a2 = gsl_complex_div(A.a2,gsl_complex_rect(norm,0));
  result.a3 = gsl_complex_div(A.a3,gsl_complex_rect(norm,0));

  // orthogonize second row
  gsl_complex dprod = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.b1,gsl_complex_conjugate(result.a1)),
                      gsl_complex_mul(A.b2,gsl_complex_conjugate(result.a2))),
                      gsl_complex_mul(A.b3,gsl_complex_conjugate(result.a3)));
  result.b1 = gsl_complex_sub(A.b1,gsl_complex_mul(result.a1,dprod));
  result.b2 = gsl_complex_sub(A.b2,gsl_complex_mul(result.a2,dprod));
  result.b3 = gsl_complex_sub(A.b3,gsl_complex_mul(result.a3,dprod));

  // normalize second row
  norm = sqrt(gsl_complex_abs2(result.b1)+gsl_complex_abs2(result.b2)+gsl_complex_abs2(result.b3));
  result.b1 = gsl_complex_div(result.b1,gsl_complex_rect(norm,0));
  result.b2 = gsl_complex_div(result.b2,gsl_complex_rect(norm,0));
  result.b3 = gsl_complex_div(result.b3,gsl_complex_rect(norm,0));
  return result;
}

// multiply matrices
su3_matrix su3_mul(const su3_matrix A, const su3_matrix B){
  su3_matrix result;
  gsl_complex w1,w2,w3;

  // Third row of B as a cross product (c.f. Lang p.80)
  w1 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(B.a2),gsl_complex_conjugate(B.b3)), gsl_complex_mul(gsl_complex_conjugate(B.a3),gsl_complex_conjugate(B.b2)) );
  w2 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(B.a3),gsl_complex_conjugate(B.b1)), gsl_complex_mul(gsl_complex_conjugate(B.a1),gsl_complex_conjugate(B.b3)) );
  w3 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(B.a1),gsl_complex_conjugate(B.b2)), gsl_complex_mul(gsl_complex_conjugate(B.a2),gsl_complex_conjugate(B.b1)) );

  result.a1 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.a1,B.a1),gsl_complex_mul(A.a2,B.b1)), gsl_complex_mul(A.a3,w1));
  result.a2 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.a1,B.a2),gsl_complex_mul(A.a2,B.b2)), gsl_complex_mul(A.a3,w2));
  result.a3 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.a1,B.a3),gsl_complex_mul(A.a2,B.b3)), gsl_complex_mul(A.a3,w3));

  result.b1 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.b1,B.a1),gsl_complex_mul(A.b2,B.b1)), gsl_complex_mul(A.b3,w1));
  result.b2 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.b1,B.a2),gsl_complex_mul(A.b2,B.b2)), gsl_complex_mul(A.b3,w2));
  result.b3 = gsl_complex_add(gsl_complex_add(gsl_complex_mul(A.b1,B.a3),gsl_complex_mul(A.b2,B.b3)), gsl_complex_mul(A.b3,w3));

  return result;
}

// inverse matrix
su3_matrix su3_inv(const su3_matrix A){
  su3_matrix result;
  gsl_complex w1,w2,w3;

  // Third row of B as a cross product (c.f. Lang p.80)
  w1 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(A.a2),gsl_complex_conjugate(A.b3)), gsl_complex_mul(gsl_complex_conjugate(A.a3),gsl_complex_conjugate(A.b2)) );
  w2 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(A.a3),gsl_complex_conjugate(A.b1)), gsl_complex_mul(gsl_complex_conjugate(A.a1),gsl_complex_conjugate(A.b3)) );
  w3 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(A.a1),gsl_complex_conjugate(A.b2)), gsl_complex_mul(gsl_complex_conjugate(A.a2),gsl_complex_conjugate(A.b1)) );

  result.a1 = gsl_complex_sub( gsl_complex_mul(A.b2,w3), gsl_complex_mul( A.b3,w2) );
  result.a2 = gsl_complex_sub( gsl_complex_mul(A.a3,w2), gsl_complex_mul( A.a2,w3) );
  result.a3 = gsl_complex_sub( gsl_complex_mul(A.a2,A.b3), gsl_complex_mul( A.a3,A.b2) );

  result.b1 = gsl_complex_sub( gsl_complex_mul(A.b3,w1), gsl_complex_mul( A.b1,w3) );
  result.b2 = gsl_complex_sub( gsl_complex_mul(A.a1,w3), gsl_complex_mul( A.a3,w1) );
  result.b3 = gsl_complex_sub( gsl_complex_mul(A.a3,A.b1), gsl_complex_mul( A.a1,A.b3) );

  return result;
}

// generate random matrix
su3_matrix su3_rand(const gsl_rng* r){
  su3_matrix result;
  result.a1 = gsl_complex_rect(1,eps*(gsl_rng_uniform(r)*2-1));
  result.a2 = gsl_complex_rect(0,eps*(gsl_rng_uniform(r)*2-1));
  result.a3 = gsl_complex_rect(0,eps*(gsl_rng_uniform(r)*2-1));

  result.b1 = gsl_complex_rect(0,eps*(gsl_rng_uniform(r)*2-1));
  result.b2 = gsl_complex_rect(1,eps*(gsl_rng_uniform(r)*2-1));
  result.b3 = gsl_complex_rect(0,eps*(gsl_rng_uniform(r)*2-1));
  return su3_norm(result);
}

double su3_trace(const su3_matrix A){
  gsl_complex c3;
  c3 = gsl_complex_sub( gsl_complex_mul(gsl_complex_conjugate(A.a1),gsl_complex_conjugate(A.b2)), gsl_complex_mul(gsl_complex_conjugate(A.a2),gsl_complex_conjugate(A.b1)) );

  return GSL_REAL(gsl_complex_add(gsl_complex_add(A.a1,A.b2),c3));
}

// end of SU(3) matrix methods ---------------------------------------------


#include <omp.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <errno.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include "mpi.h"

//data specific parameters
#define KMEANS 3 //number of cluster groups
#define DIMENSIONS 4 //number of dimensions of data
#define DATA_ROWS 150

//run specific parameters
#define THREAD_COUNT 6 //count of thread used by OpenMP
#define FITNESS_FUNCTION fitness5 //specify fitness function used, 5 possibilities (fitness1,fitness2,...,fitness5)
#define SWARM_SIZE (200)
#define MAX_ITERATIONS (10000)
#define GEN_MUTATION 4 //how many parts of genes will be mutated
#define ELITE 4 //how many genes will be preserved without change

#define REAL long double
#define size_t int
#define ROOT 0

long double vectors[DATA_ROWS][DIMENSIONS]; //here are stored data vectors

long double fitness[SWARM_SIZE];
int chrom[SWARM_SIZE][DATA_ROWS];
int new_chromosoms[SWARM_SIZE][DATA_ROWS];

long double fit_data_vectors[KMEANS][DIMENSIONS][DATA_ROWS]; //used to compute fitness
#pragma omp threadprivate(fit_data_vectors)
char dataset[100];

/*
 * FITNEES FUNCTION
 * fitness1 - vseobecne kriterium (VVV)
 * fitness2 - spriemerovane kovariancne matice sa pouziju na fitness (EEE)
 * fitness3 - stopa kovariancnych matic (VII)
 * fitness4 - sucet stvorcov euklidovskych vzdialenosti fitness (KMEANS)
 * fitness5 - spriemerovane stopy kovariancnych matic (EII)
 */

long double my_mean(const long double data[], const size_t stride,
		const size_t size) {
	long double mean = 0;
	size_t i;

	for (i = 0; i < size; i++) {
		mean += (data[i * stride] - mean) / (i + 1);
	}

	return mean;
}

long double _my_covariance(const long double data1[], const size_t stride1,
		const long double data2[], const size_t stride2, const size_t n,
		const long double mean1, const long double mean2) {

	long double covariance = 0;

	size_t i;

	/* find the sum of the squares */
	for (i = 0; i < n; i++) {
		const long double delta1 = (data1[i * stride1] - mean1);
		const long double delta2 = (data2[i * stride2] - mean2);
		covariance += (delta1 * delta2 - covariance) / (i + 1);
	}

	return covariance;
}

long double my_gsl_stats_covariance(const long double data1[],
		const size_t stride1, const long double data2[], const size_t stride2,
		const size_t n) {
	const long double mean1 = my_mean(data1, stride1, n);
	const long double mean2 = my_mean(data2, stride2, n);

	return _my_covariance(data1, stride1, data2, stride2, n, mean1, mean2);
}

long double my_gsl_linalg_LU_det(gsl_matrix_long_double * LU, int signum) {
	size_t i, n = LU->size1;
	long double det = (long double) signum;

	for (i = 0; i < n; i++) {
		det *= gsl_matrix_long_double_get(LU, i, i);
	}

	return det;
}

void my_gsl_linalg_LU_decomp(gsl_matrix_long_double * A, gsl_permutation * p,
		int *signum) {
	if (A->size1 != A->size2) {
		printf("LU decomposition requires square matrix");
	} else if (p->size != A->size1) {
		printf("permutation length must match matrix size");
	} else {
		const size_t N = A->size1;
		size_t i, j, k;

		*signum = 1;
		gsl_permutation_init(p);

		for (j = 0; j < N - 1; j++) {
			/* Find maximum in the j-th column */

			long double ajj, max = fabs(gsl_matrix_long_double_get(A, j, j));
			size_t i_pivot = j;

			for (i = j + 1; i < N; i++) {
				long double aij = fabs(gsl_matrix_long_double_get(A, i, j));

				if (aij > max) {
					max = aij;
					i_pivot = i;
				}
			}

			if (i_pivot != j) {
				gsl_matrix_long_double_swap_rows(A, j, i_pivot);
				gsl_permutation_swap(p, j, i_pivot);
				*signum = -(*signum);
			}

			ajj = gsl_matrix_long_double_get(A, j, j);

			if (ajj != 0.0) {
				for (i = j + 1; i < N; i++) {
					long double aij = gsl_matrix_long_double_get(A, i, j) / ajj;
					gsl_matrix_long_double_set(A, i, j, aij);

					for (k = j + 1; k < N; k++) {
						long double aik = gsl_matrix_long_double_get(A, i, k);
						long double ajk = gsl_matrix_long_double_get(A, j, k);
						gsl_matrix_long_double_set(A, i, k, aik - aij * ajk);
					}
				}
			}
		}
	}
}

int random_int(int min_num, int max_num) {
	int result = 0;
	result = (rand() % (max_num - min_num)) + min_num;
	return result;
}

int my_ceil(float num) {
	int inum = (int) num;
	if (num == (float) inum) {
		return inum;
	}
	return inum + 1;
}

void my_export(char filename[100]) {
	int i;
	FILE *fp;
	fp = fopen(filename, "w");
	for (i = 0; i < DATA_ROWS; i++) {
		fprintf(fp, "%d\n", chrom[0][i] + 1);
	}
	fclose(fp);
}

long double fitness1(int i) {
	int j, k, h, s;
	long double det;
	int fit_cluster_volume[KMEANS];
	gsl_matrix_long_double *fit_matrix[KMEANS];
	gsl_permutation * p = gsl_permutation_alloc(DIMENSIONS);
	long double result = 0;

	for (j = 0; j < KMEANS; j++) {
		fit_cluster_volume[j] = 0;
	}

	for (j = 0; j < DATA_ROWS; j++) {
		for (h = 0; h < DIMENSIONS; h++) {
			fit_data_vectors[chrom[i][j]][h][fit_cluster_volume[chrom[i][j]]] =
					vectors[j][h];
		}

		fit_cluster_volume[chrom[i][j]]++;
	}

	for (j = 0; j < KMEANS; j++) {
		fit_matrix[j] = gsl_matrix_long_double_calloc(DIMENSIONS, DIMENSIONS);
		for (k = 0; k < DIMENSIONS; k++) {
			for (h = 0; h < DIMENSIONS; h++) {
				gsl_matrix_long_double_set(fit_matrix[j], k, h,
						my_gsl_stats_covariance(fit_data_vectors[j][k], 1,
								fit_data_vectors[j][h], 1,
								fit_cluster_volume[j]));
			}
		}
	}

	for (j = 0; j < KMEANS; j++) {
		s = 1;
		my_gsl_linalg_LU_decomp(fit_matrix[j], p, &s);
		det = my_gsl_linalg_LU_det(fit_matrix[j], s);
		if (det != 0.0) {
			result += gsl_sf_log_abs(det) * (double) fit_cluster_volume[j];
		}
		gsl_matrix_long_double_free(fit_matrix[j]);
	}
	return result;
}

long double fitness2(int i) {
	int j, k, h, s;
	long double m, det;
	int fit_cluster_volume[KMEANS];
	gsl_matrix_long_double *fit_matrix[KMEANS];
	gsl_matrix_long_double *fit_reuslt_matrix;
	gsl_permutation * p = gsl_permutation_alloc(DIMENSIONS);
	long double result = 0;

	for (j = 0; j < KMEANS; j++) {
		fit_cluster_volume[j] = 0;
	}

	for (j = 0; j < DATA_ROWS; j++) {
		for (h = 0; h < DIMENSIONS; h++) {
			fit_data_vectors[chrom[i][j]][h][fit_cluster_volume[chrom[i][j]]] =
					vectors[j][h];
		}

		fit_cluster_volume[chrom[i][j]]++;
	}

	for (j = 0; j < KMEANS; j++) {
		fit_matrix[j] = gsl_matrix_long_double_calloc(DIMENSIONS, DIMENSIONS);
		for (k = 0; k < DIMENSIONS; k++) {
			for (h = 0; h < DIMENSIONS; h++) {
				gsl_matrix_long_double_set(fit_matrix[j], k, h,
						my_gsl_stats_covariance(fit_data_vectors[j][k], 1,
								fit_data_vectors[j][h], 1,
								fit_cluster_volume[j]));
			}
		}
	}

	fit_reuslt_matrix = gsl_matrix_long_double_calloc(DIMENSIONS, DIMENSIONS);

	for (j = 0; j < KMEANS; j++) {
		for (k = 0; k < DIMENSIONS; k++) {
			for (h = 0; h < DIMENSIONS; h++) {

				m = gsl_matrix_long_double_get(fit_reuslt_matrix, k, h);
				m += fit_cluster_volume[j]
						* gsl_matrix_long_double_get(fit_matrix[j], k, h);
				gsl_matrix_long_double_set(fit_reuslt_matrix, k, h, m);
			}
		}
		gsl_matrix_long_double_free(fit_matrix[j]);
	}

	for (k = 0; k < DIMENSIONS; k++) {
		for (h = 0; h < DIMENSIONS; h++) {
			m = gsl_matrix_long_double_get(fit_reuslt_matrix, k, h);
			m = m / (double) DATA_ROWS;
			gsl_matrix_long_double_set(fit_reuslt_matrix, k, h, m);
		}
	}

	s = 1;
	my_gsl_linalg_LU_decomp(fit_reuslt_matrix, p, &s);
	det = my_gsl_linalg_LU_det(fit_reuslt_matrix, s);
	if (det != 0.0) {
		result += gsl_sf_log_abs(det);
	}
	gsl_matrix_long_double_free(fit_reuslt_matrix);
	return result;
}

long double fitness3(int i) {
	int j, k, h;
	int fit_cluster_volume[KMEANS];
	gsl_matrix_long_double *fit_matrix[KMEANS];
	long double result = 0;

	for (j = 0; j < KMEANS; j++) {
		fit_cluster_volume[j] = 0;
	}

	for (j = 0; j < DATA_ROWS; j++) {
		for (h = 0; h < DIMENSIONS; h++) {
			fit_data_vectors[chrom[i][j]][h][fit_cluster_volume[chrom[i][j]]] =
					vectors[j][h];
		}

		fit_cluster_volume[chrom[i][j]]++;
	}

	for (j = 0; j < KMEANS; j++) {
		fit_matrix[j] = gsl_matrix_long_double_calloc(DIMENSIONS, DIMENSIONS);
		for (k = 0; k < DIMENSIONS; k++) {
			for (h = 0; h < DIMENSIONS; h++) {
				gsl_matrix_long_double_set(fit_matrix[j], k, h,
						my_gsl_stats_covariance(fit_data_vectors[j][k], 1,
								fit_data_vectors[j][h], 1,
								fit_cluster_volume[j]));
			}
		}
	}

	for (j = 0; j < KMEANS; j++) {
		for (k = 0; k < DIMENSIONS; k++) {
			result += gsl_matrix_long_double_get(fit_matrix[j], k, k);
		}
		gsl_matrix_long_double_free(fit_matrix[j]);
	}
	return result;
}

long double fitness4(int agent) {
	int i, j, v;
	int weight_matrix[DATA_ROWS][KMEANS];
	long double center_matrix[KMEANS][DIMENSIONS];
	long double sum_w;
	long double sum_wx;
	long double result = 0;

	for (i = 0; i < DATA_ROWS; i++) {
		for (j = 0; j < KMEANS; j++) {
			if (chrom[agent][i] == j) {
				weight_matrix[i][j] = 1;
			} else {
				weight_matrix[i][j] = 0;
			}
		}
	}

	for (j = 0; j < KMEANS; j++) {
		for (v = 0; v < DIMENSIONS; v++) {
			sum_w = 0.0;
			for (i = 0; i < DATA_ROWS; i++) {
				sum_w += weight_matrix[i][j];
			}

			sum_wx = 0.0;
			for (i = 0; i < DATA_ROWS; i++) {
				sum_wx += weight_matrix[i][j] * vectors[i][v];
			}

			center_matrix[j][v] = sum_wx / sum_w;
		}
	}

	for (j = 0; j < KMEANS; j++) {
		for (i = 0; i < DATA_ROWS; i++) {
			sum_w = 0.0;
			for (v = 0; v < DIMENSIONS; v++) {
				sum_w += weight_matrix[i][j]
						* ((vectors[i][v] - center_matrix[j][v])
								* (vectors[i][v] - center_matrix[j][v]));
			}
			result += sqrt(sum_w);

		}
	}
	return result;
}

long double fitness5(int i) {
	int j, k, h;
	long double m;
	int fit_cluster_volume[KMEANS];
	gsl_matrix_long_double *fit_matrix[KMEANS];
	gsl_matrix_long_double *fit_reuslt_matrix;
	long double result = 0;

	for (j = 0; j < KMEANS; j++) {
		fit_cluster_volume[j] = 0;
	}

	for (j = 0; j < DATA_ROWS; j++) {
		for (h = 0; h < DIMENSIONS; h++) {
			fit_data_vectors[chrom[i][j]][h][fit_cluster_volume[chrom[i][j]]] =
					vectors[j][h];
		}

		fit_cluster_volume[chrom[i][j]]++;
	}

	for (j = 0; j < KMEANS; j++) {
		fit_matrix[j] = gsl_matrix_long_double_calloc(DIMENSIONS, DIMENSIONS);
		for (k = 0; k < DIMENSIONS; k++) {
			for (h = 0; h < DIMENSIONS; h++) {
				gsl_matrix_long_double_set(fit_matrix[j], k, h,
						my_gsl_stats_covariance(fit_data_vectors[j][k], 1,
								fit_data_vectors[j][h], 1,
								fit_cluster_volume[j]));
			}
		}
	}

	fit_reuslt_matrix = gsl_matrix_long_double_calloc(DIMENSIONS, DIMENSIONS);

	for (j = 0; j < KMEANS; j++) {
		for (k = 0; k < DIMENSIONS; k++) {
			for (h = 0; h < DIMENSIONS; h++) {

				m = gsl_matrix_long_double_get(fit_reuslt_matrix, k, h);
				m += fit_cluster_volume[j]
						* gsl_matrix_long_double_get(fit_matrix[j], k, h);
				gsl_matrix_long_double_set(fit_reuslt_matrix, k, h, m);
			}
		}
		gsl_matrix_long_double_free(fit_matrix[j]);
	}

	for (k = 0; k < DIMENSIONS; k++) {
		for (h = 0; h < DIMENSIONS; h++) {
			m = gsl_matrix_long_double_get(fit_reuslt_matrix, k, h);
			m = m / (double) DATA_ROWS;
			gsl_matrix_long_double_set(fit_reuslt_matrix, k, h, m);
		}
	}

	for (k = 0; k < DIMENSIONS; k++) {
		result += gsl_matrix_long_double_get(fit_reuslt_matrix, k, k);
	}
	gsl_matrix_long_double_free(fit_reuslt_matrix);

	return result;
}

void quicksort(long double list[], int ch[][DATA_ROWS], int n) {
	int i, j;
	long double pivot;
	long double temp;
	int temp_ch[DATA_ROWS];

	if (n < 2)
		return;
	pivot = list[n / 2];
	for (i = 0, j = n - 1;; i++, j--) {
		while (list[i] < pivot)
			i++;
		while (pivot < list[j])
			j--;
		if (i >= j)
			break;
		//swap
		temp = list[i];
		list[i] = list[j];
		list[j] = temp;
		memcpy(&temp_ch, &ch[i][0], sizeof(int) * DATA_ROWS);
		memcpy(&ch[i][0], &ch[j][0], sizeof(int) * DATA_ROWS);
		memcpy(&ch[j][0], &temp_ch, sizeof(int) * DATA_ROWS);

	}
	quicksort(list, ch, i);
	quicksort(list + i, ch + i, n - i);
}

int *int_array_2dto1d(int array[][DATA_ROWS], int first_dimension,
		int second_dimension) {
	int i, j;
	int *subarray = malloc(sizeof(int) * first_dimension * second_dimension);
	for (i = 0; i < first_dimension; i++) {
		for (j = 0; j < second_dimension; j++) {
			subarray[i * second_dimension + j] = array[i][j];
		}
	}
	return subarray;
}

void int_array_1dto2d(int *array, int solver, int elements_per_proc,
		int first_dimension, int second_dimension) {
	int i, j;
	for (i = 0; i < first_dimension; i++) {
		for (j = 0; j < second_dimension; j++) {
			chrom[solver * elements_per_proc + i][j] = array[i
					* second_dimension + j];
		}
	}
}

int main(int argc, char *argv[]) {
	int i, j, k, h, it;

	int temp_ch[DATA_ROWS];
	int *sub_chrom;
	int * chrom1d = NULL;
	long double *sub_fitness;

	unsigned long long int mating_h[SWARM_SIZE];
	unsigned long long int max_mating = 0;
	int mates[SWARM_SIZE];

	int r1, r2;

	time_t t, tt;

	int solver, solvers_count;
	MPI_Init(&argc, &argv);
	MPI_Comm_rank(MPI_COMM_WORLD, &solver);
	MPI_Comm_size(MPI_COMM_WORLD, &solvers_count);

	MPI_Datatype my_type_scatter;
	MPI_Type_contiguous(DATA_ROWS, MPI_INT, &my_type_scatter);
	MPI_Type_commit(&my_type_scatter);

	if (SWARM_SIZE % 2 == 1) {
		printf("SWARM SIZE need to be dividable/partible by 2\n");
		exit(1);
	}

	if (argc >= 2) {
		strcpy(dataset, argv[1]);
	} else {
		printf("Program argument (dataset to read) not found.\n");
		exit(1);
	}

	if (SWARM_SIZE % solvers_count != 0) {
		printf(
				"SWARM_SIZE need to be divadable/partible by count of mpi solvers\n");
		exit(1);
	}

	srand(time(NULL));
	omp_set_num_threads(THREAD_COUNT);

	if (solver == ROOT) {
		for (i = 0; i < SWARM_SIZE / 2; i++) {
			mating_h[i] = gsl_pow_int((SWARM_SIZE / 2 - i), 5);
			max_mating += mating_h[i];
			mating_h[i + SWARM_SIZE / 2] = 0;
		}
	}

//read data
	FILE *fp;
	char *token;
	fp = fopen(dataset, "r");

	if (fp != NULL) {
		int lineNumber = 0;
		char line[1024];
		while (fgets(line, sizeof line, fp) != NULL) {
			token = strtok(line, ",");
			for (i = 0; i < DIMENSIONS - 1; i++) {
				vectors[lineNumber][i] = atof(token);
				token = strtok(NULL, ",");
			}
			vectors[lineNumber][DIMENSIONS - 1] = atof(token);
			lineNumber++;
		}
		fclose(fp);
	} else {
		printf("%s\n", strerror(errno));
		exit(1);
	}
//end of read data

	time(&t);

//Generate initial population.
	if (solver == ROOT) {
		for (i = 0; i < SWARM_SIZE; i++) {
			for (j = 0; j < DATA_ROWS; j++) {
				chrom[i][j] = random_int(0, KMEANS);
			}
		}
	}

	//Compute fitness of each chromosome.
#pragma omp parallel for shared(i,fitness)
	for (i = 0; i < SWARM_SIZE; i++) {
		fitness[i] = FITNESS_FUNCTION(i);
	}

	quicksort(fitness, chrom, SWARM_SIZE);

	for (it = 0; it < MAX_ITERATIONS; it++) {
		//Natural selection. Select mates.
		if (solver == ROOT) {
			for (i = 0; i < SWARM_SIZE; i++) {
				j = random_int(1, max_mating);
				h = 0;
				k = mating_h[h];
				while (j > k) {
					k += mating_h[++h];
				}
				mates[i] = h;
			}
		}

		//Mating
		if (solver == ROOT) {
			for (i = 0; i < SWARM_SIZE; i++) {
				do {
					r1 = random_int(0, DATA_ROWS);
					r2 = random_int(0, DATA_ROWS);
				} while (r1 == r2);

				if (r1 < r2) {
					j = r1;
					k = r2;
				} else {
					j = r2;
					k = r1;
				}

				//first kid
				memcpy(&temp_ch[0], &chrom[mates[i]][0], j * sizeof(int));
				memcpy(&temp_ch[j], &chrom[mates[i + 1]][j],
						(k - j) * sizeof(int));
				memcpy(&temp_ch[k], &chrom[mates[i]][k],
						(DATA_ROWS - k) * sizeof(int));
				memcpy(new_chromosoms[i], temp_ch, DATA_ROWS * sizeof(int));

				//second kid
				memcpy(&temp_ch[0], &chrom[mates[i + 1]][0], j * sizeof(int));
				memcpy(&temp_ch[j], &chrom[mates[i]][j], (k - j) * sizeof(int));
				memcpy(&temp_ch[k], &chrom[mates[i + 1]][k],
						(DATA_ROWS - k) * sizeof(int));
				memcpy(new_chromosoms[i + 1], temp_ch, DATA_ROWS * sizeof(int));
				i++;
			}
		}

		//copy new population
		if (solver == ROOT) {
			for (i = ELITE; i < SWARM_SIZE; i++) {
				memcpy(&chrom[i], &new_chromosoms[i], sizeof(int) * DATA_ROWS);
			}
		}

		//Mutation.
		if (solver == ROOT) {
			for (i = ELITE; i < SWARM_SIZE; i++) {
				for (j = 0; j < GEN_MUTATION; j++) {
					k = random_int(0, DATA_ROWS);
					chrom[i][k] = random_int(0, KMEANS);
				}
			}
		}

		//mpi section
		if (solver == ROOT) {
			chrom1d = int_array_2dto1d(chrom, SWARM_SIZE, DATA_ROWS);
		}

		sub_chrom = malloc(
				sizeof(int) * (SWARM_SIZE / solvers_count) * DATA_ROWS);

		MPI_Scatter(chrom1d,
		SWARM_SIZE / solvers_count, my_type_scatter, sub_chrom,
		SWARM_SIZE / solvers_count, my_type_scatter,
		ROOT,
		MPI_COMM_WORLD);

		int_array_1dto2d(sub_chrom, solver, SWARM_SIZE / solvers_count,
				SWARM_SIZE / solvers_count, DATA_ROWS);
		sub_fitness = malloc(
				sizeof(long double) * (SWARM_SIZE / solvers_count));

		//Compute fitness of each chromosome.
#pragma omp parallel for shared(i,fitness,sub_fitness)
		for (i = solver * (SWARM_SIZE / solvers_count);
				i
						< solver * (SWARM_SIZE / solvers_count)
								+ (SWARM_SIZE / solvers_count); i++) {
			sub_fitness[i - solver * (SWARM_SIZE / solvers_count)] =
					FITNESS_FUNCTION(i);
		}

		MPI_Gather(sub_fitness,
		SWARM_SIZE / solvers_count,
		MPI_LONG_DOUBLE, fitness,
		SWARM_SIZE / solvers_count,
		MPI_LONG_DOUBLE,
		ROOT,
		MPI_COMM_WORLD);

		free(sub_chrom);
		free(sub_fitness);
		if (solver == ROOT && chrom1d != NULL) {
			free(chrom1d);
		}
		//end of mpi section

		//Order population
		if (solver == ROOT) {
			quicksort(fitness, chrom, SWARM_SIZE);
		}
	}

	if (solver == ROOT) {

		time(&tt);
		printf("Program bezal %.f sekund.\n", difftime(tt, t));

		printf("fitness: %Lf\n", fitness[0]);

		if (argc >= 3) {
			my_export(argv[2]);
		} else {
			my_export("/home/lukas/workspace_parallel/mpi_gen/export.txt");
		}

		printf("SWARM SIZE: %d\n", SWARM_SIZE);
		printf("MAX_ITERATIONS: %d\n", MAX_ITERATIONS);
		printf("GEN_MUTATION: %d\n", GEN_MUTATION);
		printf("ELITE: %d\n", ELITE);
		printf("DIMENSIONS: %d\n", DIMENSIONS);
		printf("KMEANS: %d\n", KMEANS);
		printf("THREAD_COUNT per instance: %d\n", THREAD_COUNT);
		printf("mpi solvers_count: %d\n", solvers_count);
		printf(" FITNESS_FUNCTION ");

	}

	MPI_Finalize();

	return 0;

}

Impl of clus alg/GenClust/MPI_GenClust.c


#ifndef BASIC_DATA_STRUCTURE_H
#define BASIC_DATA_STRUCTURE_H

#include <vector>
#include <array>
#include <string>
#include <fstream>
#include <iostream>
#include <petsc.h>
#include <petscksp.h>

#include "petscsys.h"
#include "petscmat.h"
using namespace std;
/////////////////////////////////

// const int degree = 3;
// const int dim = 3;
const int phydim = 3;
// const int bzpt_num = (dim == 3) ? 64 : 16;
// const int dof_all = 3;

class Vertex3D
{
public:
	double coor[3];
	int label; //ID for inlet and outlet
	Vertex3D();
};

class Element3D
{
public:
	int degree;
	int order;
	int nbf;
	int type;	//0 for interior and 1 for boundary, for visualization purpose
	int bzflag; //0 for spline element, 1 for Bezier element
	int label;

	vector<int> IEN;
	vector<array<double, 3>> pts; //tmp
	vector<array<double, 64>> cmat;
	vector<int> IDBC;
	double velocity[3];

	Element3D(int p = 3);
	void BezierPolyn(double u, vector<double> &Nu, vector<double> &dNdu) const;
	void Basis(double u, double v, double w, vector<double> &Nt, vector<array<double, 3>> &dNdt) const;
	void Para2Phys(double u, double v, double w, double pt[3]) const;
};

class Vertex2D
{
public:
	double coor[3];
	int label; //ID for inlet and outlet
	Vertex2D();
};

class Element2D
{
public:
	int degree;
	int order;
	int nbf;
	int type;	//0 for interior and 1 for boundary, for visualization purpose
	int bzflag; //0 for spline element, 1 for Bezier element
	int label;

	vector<int> IEN;
	vector<array<double, phydim>> pts; //tmp
	vector<array<double, 16>> cmat;
	vector<int> IDBC;
	double velocity[2];

	Element2D(int p = 3);
	void BezierPolyn(double u, vector<double> &Nu, vector<double> &dNdu) const;
	void Basis(double u, double v, vector<double> &Nt, vector<array<double, 2>> &dNdt) const;
	void Para2Phys(double u, double v, double pt[phydim]) const;
};

//mesh format conversion

void Raw2Vtk_hex(string fn);

void Rawn2Vtk_hex(string fn);

#endif


#ifndef _SEARCHBEST_
#define _SEARCHBEST_

#include <assert.h>
#include <cmath>
#include <float.h>
#include <climits>
// use openblas
#include <cblas.h>
#include "cosine_similarity.h"

// Step 1, g++ main.cpp search_best.cpp cosine_similarity.cpp -std=c++11
// Step 2, g++ main.cpp search_best.cpp cosine_similarity.cpp -std=c++11 -O3
// Step 3, g++ main.cpp search_best.cpp cosine_similarity.cpp -std=c++11 -O3 -Ofast -ffast-math
template <typename T>
int SearchBest(const T* __restrict__ const pVecA,  // 待搜索的单个特征向量首地址
               const int lenA,  // 待搜索特征向量长度(1 x 单个特征维数)
               const T* __restrict__ const pVecDB, // 底库首地址
               const int lenDB) // 底库长度(特征个数 x 单个特征维数)
{
    assert(lenDB%lenA == 0);
    const int featsize = lenA;
    const int facenum  = lenDB / lenA;

    int best_index = - INT_MAX;
    T best_similarity = - FLT_MAX;
#if 0
// Step 5, 加上OpenMP
//GCC很聪明，OpenMP默认线程数就是多核处理器的核心数量，不必显示指定
//OpenMP起线程，收回线程也是有开销的，所以要合理安排每个线程的任务量大小，不宜放入内层for循环（任务量太小划不来）
//#pragma omp parallel for num_threads(8)
#pragma omp parallel for
    for(int i = 0; i < facenum; i++) {
        // 普通C++代码实现的余弦相似度计算
        T similarity = Cosine_similarity(pVecA, pVecDB + i*featsize, featsize);
        // 使用向量化代码实现的余弦相似度计算
        //T similarity = Cosine_similarity_avx(pVecA, pVecDB + i*featsize, featsize);
        if(similarity > best_similarity) {
            best_similarity = similarity;
            best_index = i;
        }
    }

#else
// Step 12，使用OpenBLAS
    T simAll[facenum] = {0.0f};
    cblas_sgemv(CblasRowMajor, CblasNoTrans, facenum, featsize, 1, pVecDB, featsize, pVecA, 1, 0, simAll, 1);
    // 寻找simAll里面最大的，它的序号就是要找的id
    for(int i = 0; i < facenum; i++) {
        if(simAll[i] > best_similarity) {
            best_similarity = simAll[i];
            best_index = i;
        }
    }
#endif

    return best_index;
}

#endif //!_SEARCHBEST_