// Copyright (c) 2017, Lawrence Livermore National Security, LLC. Produced at
// the Lawrence Livermore National Laboratory. LLNL-CODE-734707. All Rights
// reserved. See files LICENSE and NOTICE for details.
//
// This file is part of CEED, a collection of benchmarks, miniapps, software
// libraries and APIs for efficient high-order finite element and spectral
// element discretizations for exascale applications. For more information and
// source code availability see http://github.com/ceed.
//
// The CEED research is supported by the Exascale Computing Project 17-SC-20-SC,
// a collaborative effort of two U.S. Department of Energy organizations (Office
// of Science and the National Nuclear Security Administration) responsible for
// the planning and preparation of a capable exascale ecosystem, including
// software, applications, hardware, advanced system engineering and early
// testbed platforms, in support of the nation's exascale computing imperative.

#ifndef setupsphere_h
#define setupsphere_h

#include <ceed.h>
#include <petsc.h>
#include <petscdmplex.h>
#include <petscfe.h>
#include <stdbool.h>
#include <string.h>
#include "qfunctions/bps/bp1sphere.h"
#include "qfunctions/bps/bp2sphere.h"
#include "qfunctions/bps/bp3sphere.h"
#include "qfunctions/bps/bp4sphere.h"
#include "qfunctions/bps/common.h"

#if PETSC_VERSION_LT(3,14,0)
#  define DMPlexGetClosureIndices(a,b,c,d,e,f,g,h,i) DMPlexGetClosureIndices(a,b,c,d,f,g,i)
#  define DMPlexRestoreClosureIndices(a,b,c,d,e,f,g,h,i) DMPlexRestoreClosureIndices(a,b,c,d,f,g,i)
#endif

#if PETSC_VERSION_LT(3,14,0)
#  define DMPlexCreateSphereMesh(a,b,c,d,e) DMPlexCreateSphereMesh(a,b,c,e)
#endif

// -----------------------------------------------------------------------------
// PETSc Operator Structs
// -----------------------------------------------------------------------------

// Data for PETSc
typedef struct UserO_ *UserO;
struct UserO_ {
  MPI_Comm comm;
  DM dm;
  Vec Xloc, Yloc, diag;
  CeedVector xceed, yceed;
  CeedOperator op;
  Ceed ceed;
};

// Data for PETSc Interp/Restrict operators
typedef struct UserIR_ *UserIR;
struct UserIR_ {
  MPI_Comm comm;
  DM dmc, dmf;
  Vec Xloc, Yloc, mult;
  CeedVector ceedvecc, ceedvecf;
  CeedOperator op;
  Ceed ceed;
};

// -----------------------------------------------------------------------------
// libCEED Data Struct
// -----------------------------------------------------------------------------

// libCEED data struct for level
typedef struct CeedData_ *CeedData;
struct CeedData_ {
  Ceed ceed;
  CeedBasis basisx, basisu, basisctof;
  CeedElemRestriction Erestrictx, Erestrictu, Erestrictui, Erestrictqdi;
  CeedQFunction qf_apply;
  CeedOperator op_apply, op_restrict, op_interp;
  CeedVector qdata, xceed, yceed;
};

// -----------------------------------------------------------------------------
// Command Line Options
// -----------------------------------------------------------------------------

// Coarsening options
typedef enum {
  COARSEN_UNIFORM = 0, COARSEN_LOGARITHMIC = 1
} coarsenType;
static const char *const coarsenTypes [] = {"uniform","logarithmic",
                                            "coarsenType","COARSEN",0
                                           };

// -----------------------------------------------------------------------------
// BP Option Data
// -----------------------------------------------------------------------------

// BP options
typedef enum {
  CEED_BP1 = 0, CEED_BP2 = 1, CEED_BP3 = 2,
  CEED_BP4 = 3, CEED_BP5 = 4, CEED_BP6 = 5
} bpType;
static const char *const bpTypes[] = {"bp1","bp2","bp3","bp4","bp5","bp6",
                                      "bpType","CEED_BP",0
                                     };

// BP specific data
typedef struct {
  CeedInt ncompu, qdatasize, qextra;
  CeedQFunctionUser setupgeo, setuprhs, apply, error;
  const char *setupgeofname, *setuprhsfname, *applyfname, *errorfname;
  CeedEvalMode inmode, outmode;
  CeedQuadMode qmode;
  PetscBool enforce_bc;
  PetscErrorCode (*bcs_func)(PetscInt, PetscReal, const PetscReal *,
                             PetscInt, PetscScalar *, void *);
} bpData;

static bpData bpOptions[6] = {
  [CEED_BP1] = {
    .ncompu = 1,
    .qdatasize = 1,
    .qextra = 1,
    .setupgeo = SetupMassGeo,
    .setuprhs = SetupMassRhs,
    .apply = Mass,
    .error = Error,
    .setupgeofname = SetupMassGeo_loc,
    .setuprhsfname = SetupMassRhs_loc,
    .applyfname = Mass_loc,
    .errorfname = Error_loc,
    .inmode = CEED_EVAL_INTERP,
    .outmode = CEED_EVAL_INTERP,
    .qmode = CEED_GAUSS
  },
  [CEED_BP2] = {
    .ncompu = 3,
    .qdatasize = 1,
    .qextra = 1,
    .setupgeo = SetupMassGeo,
    .setuprhs = SetupMassRhs3,
    .apply = Mass3,
    .error = Error3,
    .setupgeofname = SetupMassGeo_loc,
    .setuprhsfname = SetupMassRhs3_loc,
    .applyfname = Mass3_loc,
    .errorfname = Error3_loc,
    .inmode = CEED_EVAL_INTERP,
    .outmode = CEED_EVAL_INTERP,
    .qmode = CEED_GAUSS
  },
  [CEED_BP3] = {
    .ncompu = 1,
    .qdatasize = 4,
    .qextra = 1,
    .setupgeo = SetupDiffGeo,
    .setuprhs = SetupDiffRhs,
    .apply = Diff,
    .error = Error,
    .setupgeofname = SetupDiffGeo_loc,
    .setuprhsfname = SetupDiffRhs_loc,
    .applyfname = Diff_loc,
    .errorfname = Error_loc,
    .inmode = CEED_EVAL_GRAD,
    .outmode = CEED_EVAL_GRAD,
    .qmode = CEED_GAUSS
  },
  [CEED_BP4] = {
    .ncompu = 3,
    .qdatasize = 4,
    .qextra = 1,
    .setupgeo = SetupDiffGeo,
    .setuprhs = SetupDiffRhs3,
    .apply = Diff3,
    .error = Error3,
    .setupgeofname = SetupDiffGeo_loc,
    .setuprhsfname = SetupDiffRhs3_loc,
    .applyfname = Diff_loc,
    .errorfname = Error3_loc,
    .inmode = CEED_EVAL_GRAD,
    .outmode = CEED_EVAL_GRAD,
    .qmode = CEED_GAUSS
  },
  [CEED_BP5] = {
    .ncompu = 1,
    .qdatasize = 4,
    .qextra = 0,
    .setupgeo = SetupDiffGeo,
    .setuprhs = SetupDiffRhs,
    .apply = Diff,
    .error = Error,
    .setupgeofname = SetupDiffGeo_loc,
    .setuprhsfname = SetupDiffRhs_loc,
    .applyfname = Diff_loc,
    .errorfname = Error_loc,
    .inmode = CEED_EVAL_GRAD,
    .outmode = CEED_EVAL_GRAD,
    .qmode = CEED_GAUSS_LOBATTO
  },
  [CEED_BP6] = {
    .ncompu = 3,
    .qdatasize = 4,
    .qextra = 0,
    .setupgeo = SetupDiffGeo,
    .setuprhs = SetupDiffRhs3,
    .apply = Diff3,
    .error = Error3,
    .setupgeofname = SetupDiffGeo_loc,
    .setuprhsfname = SetupDiffRhs3_loc,
    .applyfname = Diff_loc,
    .errorfname = Error3_loc,
    .inmode = CEED_EVAL_GRAD,
    .outmode = CEED_EVAL_GRAD,
    .qmode = CEED_GAUSS_LOBATTO
  }
};

// -----------------------------------------------------------------------------
// PETSc sphere auxiliary functions
// -----------------------------------------------------------------------------

// Utility function taken from petsc/src/dm/impls/plex/examples/tutorials/ex7.c
static PetscErrorCode ProjectToUnitSphere(DM dm) {
  Vec            coordinates;
  PetscScalar   *coords;
  PetscInt       Nv, v, dim, d;
  PetscErrorCode ierr;

  PetscFunctionBeginUser;
  ierr = DMGetCoordinatesLocal(dm, &coordinates); CHKERRQ(ierr);
  ierr = VecGetLocalSize(coordinates, &Nv); CHKERRQ(ierr);
  ierr = VecGetBlockSize(coordinates, &dim); CHKERRQ(ierr);
  Nv  /= dim;
  ierr = VecGetArray(coordinates, &coords); CHKERRQ(ierr);
  for (v = 0; v < Nv; ++v) {
    PetscReal r = 0.0;

    for (d = 0; d < dim; ++d) r += PetscSqr(PetscRealPart(coords[v*dim+d]));
    r = PetscSqrtReal(r);
    for (d = 0; d < dim; ++d) coords[v*dim+d] /= r;
  }
  ierr = VecRestoreArray(coordinates, &coords); CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

// -----------------------------------------------------------------------------
// PETSc FE Boilerplate
// -----------------------------------------------------------------------------

// Create FE by degree
static int PetscFECreateByDegree(DM dm, PetscInt dim, PetscInt Nc,
                                 PetscBool isSimplex, const char prefix[],
                                 PetscInt order, PetscFE *fem) {
  PetscQuadrature q, fq;
  DM              K;
  PetscSpace      P;
  PetscDualSpace  Q;
  PetscInt        quadPointsPerEdge;
  PetscBool       tensor = isSimplex ? PETSC_FALSE : PETSC_TRUE;
  PetscErrorCode  ierr;

  PetscFunctionBeginUser;
  /* Create space */
  ierr = PetscSpaceCreate(PetscObjectComm((PetscObject) dm), &P); CHKERRQ(ierr);
  ierr = PetscObjectSetOptionsPrefix((PetscObject) P, prefix); CHKERRQ(ierr);
  ierr = PetscSpacePolynomialSetTensor(P, tensor); CHKERRQ(ierr);
  ierr = PetscSpaceSetFromOptions(P); CHKERRQ(ierr);
  ierr = PetscSpaceSetNumComponents(P, Nc); CHKERRQ(ierr);
  ierr = PetscSpaceSetNumVariables(P, dim); CHKERRQ(ierr);
  ierr = PetscSpaceSetDegree(P, order, order); CHKERRQ(ierr);
  ierr = PetscSpaceSetUp(P); CHKERRQ(ierr);
  ierr = PetscSpacePolynomialGetTensor(P, &tensor); CHKERRQ(ierr);
  /* Create dual space */
  ierr = PetscDualSpaceCreate(PetscObjectComm((PetscObject) dm), &Q);
  CHKERRQ(ierr);
  ierr = PetscDualSpaceSetType(Q,PETSCDUALSPACELAGRANGE); CHKERRQ(ierr);
  ierr = PetscObjectSetOptionsPrefix((PetscObject) Q, prefix); CHKERRQ(ierr);
  ierr = PetscDualSpaceCreateReferenceCell(Q, dim, isSimplex, &K); CHKERRQ(ierr);
  ierr = PetscDualSpaceSetDM(Q, K); CHKERRQ(ierr);
  ierr = DMDestroy(&K); CHKERRQ(ierr);
  ierr = PetscDualSpaceSetNumComponents(Q, Nc); CHKERRQ(ierr);
  ierr = PetscDualSpaceSetOrder(Q, order); CHKERRQ(ierr);
  ierr = PetscDualSpaceLagrangeSetTensor(Q, tensor); CHKERRQ(ierr);
  ierr = PetscDualSpaceSetFromOptions(Q); CHKERRQ(ierr);
  ierr = PetscDualSpaceSetUp(Q); CHKERRQ(ierr);
  /* Create element */
  ierr = PetscFECreate(PetscObjectComm((PetscObject) dm), fem); CHKERRQ(ierr);
  ierr = PetscObjectSetOptionsPrefix((PetscObject) *fem, prefix); CHKERRQ(ierr);
  ierr = PetscFESetFromOptions(*fem); CHKERRQ(ierr);
  ierr = PetscFESetBasisSpace(*fem, P); CHKERRQ(ierr);
  ierr = PetscFESetDualSpace(*fem, Q); CHKERRQ(ierr);
  ierr = PetscFESetNumComponents(*fem, Nc); CHKERRQ(ierr);
  ierr = PetscFESetUp(*fem); CHKERRQ(ierr);
  ierr = PetscSpaceDestroy(&P); CHKERRQ(ierr);
  ierr = PetscDualSpaceDestroy(&Q); CHKERRQ(ierr);
  /* Create quadrature */
  quadPointsPerEdge = PetscMax(order + 1,1);
  if (isSimplex) {
    ierr = PetscDTStroudConicalQuadrature(dim,   1, quadPointsPerEdge, -1.0, 1.0,
                                          &q); CHKERRQ(ierr);
    ierr = PetscDTStroudConicalQuadrature(dim-1, 1, quadPointsPerEdge, -1.0, 1.0,
                                          &fq); CHKERRQ(ierr);
  } else {
    ierr = PetscDTGaussTensorQuadrature(dim,   1, quadPointsPerEdge, -1.0, 1.0,
                                        &q); CHKERRQ(ierr);
    ierr = PetscDTGaussTensorQuadrature(dim-1, 1, quadPointsPerEdge, -1.0, 1.0,
                                        &fq); CHKERRQ(ierr);
  }
  ierr = PetscFESetQuadrature(*fem, q); CHKERRQ(ierr);
  ierr = PetscFESetFaceQuadrature(*fem, fq); CHKERRQ(ierr);
  ierr = PetscQuadratureDestroy(&q); CHKERRQ(ierr);
  ierr = PetscQuadratureDestroy(&fq); CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

// -----------------------------------------------------------------------------
// PETSc Setup for Level
// -----------------------------------------------------------------------------

// This function sets up a DM for a given degree
static int SetupDMByDegree(DM dm, PetscInt degree, PetscInt ncompu,
                           PetscInt dim) {
  PetscInt ierr;
  PetscFE fe;

  PetscFunctionBeginUser;

  // Setup FE
  ierr = PetscFECreateByDegree(dm, dim, ncompu, PETSC_FALSE, NULL, degree, &fe);
  CHKERRQ(ierr);
  ierr = DMAddField(dm, NULL, (PetscObject)fe); CHKERRQ(ierr);

  // Setup DM
  ierr = DMCreateDS(dm); CHKERRQ(ierr);
  ierr = DMPlexSetClosurePermutationTensor(dm, PETSC_DETERMINE, NULL);
  CHKERRQ(ierr);
  ierr = PetscFEDestroy(&fe); CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

// -----------------------------------------------------------------------------
// libCEED Setup for Level
// -----------------------------------------------------------------------------

// Destroy libCEED operator objects
static PetscErrorCode CeedDataDestroy(CeedInt i, CeedData data) {
  PetscInt ierr;

  CeedVectorDestroy(&data->qdata);
  CeedVectorDestroy(&data->xceed);
  CeedVectorDestroy(&data->yceed);
  CeedBasisDestroy(&data->basisx);
  CeedBasisDestroy(&data->basisu);
  CeedElemRestrictionDestroy(&data->Erestrictu);
  CeedElemRestrictionDestroy(&data->Erestrictx);
  CeedElemRestrictionDestroy(&data->Erestrictui);
  CeedElemRestrictionDestroy(&data->Erestrictqdi);
  CeedQFunctionDestroy(&data->qf_apply);
  CeedOperatorDestroy(&data->op_apply);
  if (i > 0) {
    CeedOperatorDestroy(&data->op_interp);
    CeedBasisDestroy(&data->basisctof);
    CeedOperatorDestroy(&data->op_restrict);
  }
  ierr = PetscFree(data); CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

// Get CEED restriction data from DMPlex
static int CreateRestrictionPlex(Ceed ceed, CeedInt P, CeedInt ncomp,
                                 CeedElemRestriction *Erestrict, DM dm) {
  PetscInt ierr;
  PetscInt c, cStart, cEnd, nelem, nnodes, *erestrict, eoffset;
  PetscSection section;
  Vec Uloc;

  PetscFunctionBegin;

  // Get Nelem
  ierr = DMGetSection(dm, &section); CHKERRQ(ierr);
  ierr = DMPlexGetHeightStratum(dm, 0, &cStart,& cEnd); CHKERRQ(ierr);
  nelem = cEnd - cStart;

  // Get indices
  ierr = PetscMalloc1(nelem*P*P, &erestrict); CHKERRQ(ierr);
  for (c=cStart, eoffset = 0; c<cEnd; c++) {
    PetscInt numindices, *indices, i;
    ierr = DMPlexGetClosureIndices(dm, section, section, c, PETSC_TRUE,
                                   &numindices, &indices, NULL, NULL);
    CHKERRQ(ierr);
    for (i=0; i<numindices; i+=ncomp) {
      for (PetscInt j=0; j<ncomp; j++) {
        if (indices[i+j] != indices[i] + copysign(j, indices[i]))
          SETERRQ1(PETSC_COMM_SELF, PETSC_ERR_ARG_INCOMP,
                   "Cell %D closure indices not interlaced", c);
      }
      // NO BC on closed surfaces
      PetscInt loc = indices[i];
      erestrict[eoffset++] = loc;
    }
    ierr = DMPlexRestoreClosureIndices(dm, section, section, c, PETSC_TRUE,
                                       &numindices, &indices, NULL, NULL);
    CHKERRQ(ierr);
  }

  // Setup CEED restriction
  ierr = DMGetLocalVector(dm, &Uloc); CHKERRQ(ierr);
  ierr = VecGetLocalSize(Uloc, &nnodes); CHKERRQ(ierr);

  ierr = DMRestoreLocalVector(dm, &Uloc); CHKERRQ(ierr);
  CeedElemRestrictionCreate(ceed, nelem, P*P, ncomp, 1, nnodes, CEED_MEM_HOST,
                            CEED_COPY_VALUES, erestrict, Erestrict);
  ierr = PetscFree(erestrict); CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

// Set up libCEED for a given degree
static int SetupLibceedByDegree(DM dm, Ceed ceed, CeedInt degree,
                                CeedInt topodim, CeedInt qextra,
                                PetscInt ncompx, PetscInt ncompu,
                                PetscInt gsize, PetscInt xlsize,
                                bpType bpChoice, CeedData data,
                                PetscBool setup_rhs, CeedVector rhsceed,
                                CeedVector *target) {
  int ierr;
  DM dmcoord;
  Vec coords;
  const PetscScalar *coordArray;
  CeedBasis basisx, basisu;
  CeedElemRestriction Erestrictx, Erestrictu, Erestrictui, Erestrictqdi;
  CeedQFunction qf_setupgeo, qf_apply;
  CeedOperator op_setupgeo, op_apply;
  CeedVector xcoord, qdata, xceed, yceed;
  CeedInt P, Q, cStart, cEnd, nelem, qdatasize = bpOptions[bpChoice].qdatasize;
  CeedScalar R = 1,                      // radius of the sphere
             l = 1.0/PetscSqrtReal(3.0); // half edge of the inscribed cube

  // CEED bases
  P = degree + 1;
  Q = P + qextra;
  CeedBasisCreateTensorH1Lagrange(ceed, topodim, ncompu, P, Q,
                                  bpOptions[bpChoice].qmode, &basisu);
  CeedBasisCreateTensorH1Lagrange(ceed, topodim, ncompx, 2, Q,
                                  bpOptions[bpChoice].qmode, &basisx);

  // CEED restrictions
  ierr = DMGetCoordinateDM(dm, &dmcoord); CHKERRQ(ierr);
  ierr = DMPlexSetClosurePermutationTensor(dmcoord, PETSC_DETERMINE, NULL);
  CHKERRQ(ierr);

  ierr = CreateRestrictionPlex(ceed, 2, ncompx, &Erestrictx, dmcoord);
  CHKERRQ(ierr);
  ierr = CreateRestrictionPlex(ceed, P, ncompu, &Erestrictu, dm); CHKERRQ(ierr);

  ierr = DMPlexGetHeightStratum(dm, 0, &cStart, &cEnd); CHKERRQ(ierr);
  nelem = cEnd - cStart;

  CeedElemRestrictionCreateStrided(ceed, nelem, Q*Q, ncompu, ncompu*nelem*Q*Q,
                                   CEED_STRIDES_BACKEND, &Erestrictui);
  CeedElemRestrictionCreateStrided(ceed, nelem, Q*Q, qdatasize,
                                   qdatasize*nelem*Q*Q,
                                   CEED_STRIDES_BACKEND, &Erestrictqdi);

  // Element coordinates
  ierr = DMGetCoordinatesLocal(dm, &coords); CHKERRQ(ierr);
  ierr = VecGetArrayRead(coords, &coordArray); CHKERRQ(ierr);

  CeedElemRestrictionCreateVector(Erestrictx, &xcoord, NULL);
  CeedVectorSetArray(xcoord, CEED_MEM_HOST, CEED_COPY_VALUES,
                     (PetscScalar *)coordArray);
  ierr = VecRestoreArrayRead(coords, &coordArray);

  // Create the persistent vectors that will be needed in setup and apply
  CeedInt nqpts;
  CeedBasisGetNumQuadraturePoints(basisu, &nqpts);
  CeedVectorCreate(ceed, qdatasize*nelem*nqpts, &qdata);
  CeedVectorCreate(ceed, xlsize, &xceed);
  CeedVectorCreate(ceed, xlsize, &yceed);

  // Create the Q-function that builds the operator (i.e. computes its
  // quadrature data) and set its context data
  CeedQFunctionCreateInterior(ceed, 1, bpOptions[bpChoice].setupgeo,
                              bpOptions[bpChoice].setupgeofname, &qf_setupgeo);
  CeedQFunctionAddInput(qf_setupgeo, "x", ncompx, CEED_EVAL_INTERP);
  CeedQFunctionAddInput(qf_setupgeo, "dx", ncompx*topodim, CEED_EVAL_GRAD);
  CeedQFunctionAddInput(qf_setupgeo, "weight", 1, CEED_EVAL_WEIGHT);
  CeedQFunctionAddOutput(qf_setupgeo, "qdata", qdatasize, CEED_EVAL_NONE);

  // Set up PDE operator
  CeedInt inscale = bpOptions[bpChoice].inmode==CEED_EVAL_GRAD ? topodim : 1;
  CeedInt outscale = bpOptions[bpChoice].outmode==CEED_EVAL_GRAD ? topodim : 1;
  CeedQFunctionCreateInterior(ceed, 1, bpOptions[bpChoice].apply,
                              bpOptions[bpChoice].applyfname, &qf_apply);
  CeedQFunctionAddInput(qf_apply, "u", ncompu*inscale,
                        bpOptions[bpChoice].inmode);
  CeedQFunctionAddInput(qf_apply, "qdata", qdatasize, CEED_EVAL_NONE);
  CeedQFunctionAddOutput(qf_apply, "v", ncompu*outscale,
                         bpOptions[bpChoice].outmode);

  // Create the operator that builds the quadrature data for the operator
  CeedOperatorCreate(ceed, qf_setupgeo, NULL, NULL, &op_setupgeo);
  CeedOperatorSetField(op_setupgeo, "x", Erestrictx, basisx,
                       CEED_VECTOR_ACTIVE);
  CeedOperatorSetField(op_setupgeo, "dx", Erestrictx, basisx,
                       CEED_VECTOR_ACTIVE);
  CeedOperatorSetField(op_setupgeo, "weight", CEED_ELEMRESTRICTION_NONE, basisx,
                       CEED_VECTOR_NONE);
  CeedOperatorSetField(op_setupgeo, "qdata", Erestrictqdi,
                       CEED_BASIS_COLLOCATED, CEED_VECTOR_ACTIVE);

  // Create the mass or diff operator
  CeedOperatorCreate(ceed, qf_apply, NULL, NULL, &op_apply);
  CeedOperatorSetField(op_apply, "u", Erestrictu, basisu, CEED_VECTOR_ACTIVE);
  CeedOperatorSetField(op_apply, "qdata", Erestrictqdi, CEED_BASIS_COLLOCATED,
                       qdata);
  CeedOperatorSetField(op_apply, "v", Erestrictu, basisu, CEED_VECTOR_ACTIVE);

  // Setup qdata
  CeedOperatorApply(op_setupgeo, xcoord, qdata, CEED_REQUEST_IMMEDIATE);

  // Set up RHS if needed
  if (setup_rhs) {
    CeedQFunction qf_setuprhs;
    CeedOperator op_setuprhs;
    CeedVectorCreate(ceed, nelem*nqpts*ncompu, target);

    // Create the q-function that sets up the RHS and true solution
    CeedQFunctionCreateInterior(ceed, 1, bpOptions[bpChoice].setuprhs,
                                bpOptions[bpChoice].setuprhsfname, &qf_setuprhs);
    CeedQFunctionAddInput(qf_setuprhs, "x", ncompx, CEED_EVAL_INTERP);
    CeedQFunctionAddInput(qf_setuprhs, "qdata", qdatasize, CEED_EVAL_NONE);
    CeedQFunctionAddOutput(qf_setuprhs, "true_soln", ncompu, CEED_EVAL_NONE);
    CeedQFunctionAddOutput(qf_setuprhs, "rhs", ncompu, CEED_EVAL_INTERP);

    // Create the operator that builds the RHS and true solution
    CeedOperatorCreate(ceed, qf_setuprhs, NULL, NULL, &op_setuprhs);
    CeedOperatorSetField(op_setuprhs, "x", Erestrictx, basisx, CEED_VECTOR_ACTIVE);
    CeedOperatorSetField(op_setuprhs, "qdata", Erestrictqdi, CEED_BASIS_COLLOCATED,
                         qdata);
    CeedOperatorSetField(op_setuprhs, "true_soln", Erestrictui,
                         CEED_BASIS_COLLOCATED,
                         *target);
    CeedOperatorSetField(op_setuprhs, "rhs", Erestrictu, basisu,
                         CEED_VECTOR_ACTIVE);

    // Set up the libCEED context
    CeedQFunctionContext rhsSetup;
    CeedQFunctionContextCreate(ceed, &rhsSetup);
    CeedScalar rhsSetupData[2] = {R, l};
    CeedQFunctionContextSetData(rhsSetup, CEED_MEM_HOST, CEED_COPY_VALUES,
                                sizeof rhsSetupData, &rhsSetupData);
    CeedQFunctionSetContext(qf_setuprhs, rhsSetup);
    CeedQFunctionContextDestroy(&rhsSetup);

    // Setup RHS and target
    CeedOperatorApply(op_setuprhs, xcoord, rhsceed, CEED_REQUEST_IMMEDIATE);
    CeedVectorTakeArray(rhsceed, CEED_MEM_HOST, NULL);

    // Cleanup
    CeedQFunctionDestroy(&qf_setuprhs);
    CeedOperatorDestroy(&op_setuprhs);
  }

  // Cleanup
  CeedQFunctionDestroy(&qf_setupgeo);
  CeedOperatorDestroy(&op_setupgeo);
  CeedVectorDestroy(&xcoord);

  // Save libCEED data required for level
  data->basisx = basisx; data->basisu = basisu;
  data->Erestrictx = Erestrictx;
  data->Erestrictu = Erestrictu;
  data->Erestrictui = Erestrictui;
  data->Erestrictqdi = Erestrictqdi;
  data->qf_apply = qf_apply;
  data->op_apply = op_apply;
  data->qdata = qdata;
  data->xceed = xceed;
  data->yceed = yceed;

  PetscFunctionReturn(0);
}

#ifdef multigrid
// Setup libCEED level transfer operator objects
static PetscErrorCode CeedLevelTransferSetup(Ceed ceed, CeedInt numlevels,
    CeedInt ncompu, bpType bpChoice, CeedData *data, CeedInt *leveldegrees,
    CeedQFunction qf_restrict, CeedQFunction qf_prolong) {
  // Return early if numlevels=1
  if (numlevels==1)
    PetscFunctionReturn(0);

  // Set up each level
  for (CeedInt i=1; i<numlevels; i++) {
    // P coarse and P fine
    CeedInt Pc = leveldegrees[i-1] + 1;
    CeedInt Pf = leveldegrees[i] + 1;

    // Restriction - Fine to corse
    CeedBasis basisctof;
    CeedOperator op_restrict;

    // Basis
    CeedBasisCreateTensorH1Lagrange(ceed, 3, ncompu, Pc, Pf,
                                    CEED_GAUSS_LOBATTO, &basisctof);

    // Create the restriction operator
    CeedOperatorCreate(ceed, qf_restrict, NULL, NULL, &op_restrict);
    CeedOperatorSetField(op_restrict, "input", data[i]->Erestrictu,
                         CEED_BASIS_COLLOCATED, CEED_VECTOR_ACTIVE);
    CeedOperatorSetField(op_restrict, "output", data[i-1]->Erestrictu,
                         basisctof, CEED_VECTOR_ACTIVE);

    // Save libCEED data required for level
    data[i]->basisctof = basisctof;
    data[i]->op_restrict = op_restrict;

    // Interpolation - Corse to fine
    CeedOperator op_interp;

    // Create the prolongation operator
    CeedOperatorCreate(ceed, qf_prolong, NULL, NULL, &op_interp);
    CeedOperatorSetField(op_interp, "uin", data[i-1]->Erestrictu,
                         basisctof, CEED_VECTOR_ACTIVE);
    CeedOperatorSetField(op_interp, "uout", data[i]->Erestrictu,
                         CEED_BASIS_COLLOCATED, CEED_VECTOR_ACTIVE);

    // Save libCEED data required for level
    data[i]->op_interp = op_interp;
  }

  PetscFunctionReturn(0);
}
#endif

// -----------------------------------------------------------------------------
// Mat Shell Functions
// -----------------------------------------------------------------------------

#ifdef multigrid
// This function returns the computed diagonal of the operator
static PetscErrorCode MatGetDiag(Mat A, Vec D) {
  PetscErrorCode ierr;
  UserO user;

  PetscFunctionBeginUser;
  ierr = MatShellGetContext(A, &user); CHKERRQ(ierr);

  ierr = VecCopy(user->diag, D); CHKERRQ(ierr);

  PetscFunctionReturn(0);
}
#endif

// This function uses libCEED to compute the action of the Laplacian with
// Dirichlet boundary conditions
static PetscErrorCode MatMult_Ceed(Mat A, Vec X, Vec Y) {
  PetscErrorCode ierr;
  UserO user;
  PetscScalar *x, *y;

  PetscFunctionBeginUser;
  ierr = MatShellGetContext(A, &user); CHKERRQ(ierr);

  // Global-to-local
  ierr = DMGlobalToLocalBegin(user->dm, X, INSERT_VALUES, user->Xloc);
  CHKERRQ(ierr);
  ierr = DMGlobalToLocalEnd(user->dm, X, INSERT_VALUES, user->Xloc);
  CHKERRQ(ierr);
  ierr = VecZeroEntries(user->Yloc); CHKERRQ(ierr);

  // Setup CEED vectors
  ierr = VecGetArrayRead(user->Xloc, (const PetscScalar **)&x); CHKERRQ(ierr);
  ierr = VecGetArray(user->Yloc, &y); CHKERRQ(ierr);
  CeedVectorSetArray(user->xceed, CEED_MEM_HOST, CEED_USE_POINTER, x);
  CeedVectorSetArray(user->yceed, CEED_MEM_HOST, CEED_USE_POINTER, y);

  // Apply CEED operator
  CeedOperatorApply(user->op, user->xceed, user->yceed, CEED_REQUEST_IMMEDIATE);

  // Restore PETSc vectors
  CeedVectorTakeArray(user->xceed, CEED_MEM_HOST, NULL);
  CeedVectorTakeArray(user->yceed, CEED_MEM_HOST, NULL);
  ierr = VecRestoreArrayRead(user->Xloc, (const PetscScalar **)&x);
  CHKERRQ(ierr);
  ierr = VecRestoreArray(user->Yloc, &y); CHKERRQ(ierr);

  // Local-to-global
  ierr = VecZeroEntries(Y); CHKERRQ(ierr);
  ierr = DMLocalToGlobalBegin(user->dm, user->Yloc, ADD_VALUES, Y);
  CHKERRQ(ierr);
  ierr = DMLocalToGlobalEnd(user->dm, user->Yloc, ADD_VALUES, Y);
  CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#ifdef multigrid
// This function uses libCEED to compute the action of the interp operator
static PetscErrorCode MatMult_Interp(Mat A, Vec X, Vec Y) {
  PetscErrorCode ierr;
  UserIR user;
  PetscScalar *x, *y;

  PetscFunctionBeginUser;
  ierr = MatShellGetContext(A, &user); CHKERRQ(ierr);

  // Global-to-local
  ierr = VecZeroEntries(user->Xloc); CHKERRQ(ierr);
  ierr = DMGlobalToLocalBegin(user->dmc, X, INSERT_VALUES, user->Xloc);
  CHKERRQ(ierr);
  ierr = DMGlobalToLocalEnd(user->dmc, X, INSERT_VALUES, user->Xloc);
  CHKERRQ(ierr);
  ierr = VecZeroEntries(user->Yloc); CHKERRQ(ierr);

  // Setup CEED vectors
  ierr = VecGetArrayRead(user->Xloc, (const PetscScalar **)&x); CHKERRQ(ierr);
  ierr = VecGetArray(user->Yloc, &y); CHKERRQ(ierr);
  CeedVectorSetArray(user->ceedvecc, CEED_MEM_HOST, CEED_USE_POINTER, x);
  CeedVectorSetArray(user->ceedvecf, CEED_MEM_HOST, CEED_USE_POINTER, y);

  // Apply CEED operator
  CeedOperatorApply(user->op, user->ceedvecc, user->ceedvecf,
                    CEED_REQUEST_IMMEDIATE);

  // Restore PETSc vectors
  CeedVectorTakeArray(user->ceedvecc, CEED_MEM_HOST, NULL);
  CeedVectorTakeArray(user->ceedvecf, CEED_MEM_HOST, NULL);
  ierr = VecRestoreArrayRead(user->Xloc, (const PetscScalar **)&x);
  CHKERRQ(ierr);
  ierr = VecRestoreArray(user->Yloc, &y); CHKERRQ(ierr);

  // Multiplicity
  ierr = VecPointwiseMult(user->Yloc, user->Yloc, user->mult);

  // Local-to-global
  ierr = VecZeroEntries(Y); CHKERRQ(ierr);
  ierr = DMLocalToGlobalBegin(user->dmf, user->Yloc, ADD_VALUES, Y);
  CHKERRQ(ierr);
  ierr = DMLocalToGlobalEnd(user->dmf, user->Yloc, ADD_VALUES, Y);
  CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

// This function uses libCEED to compute the action of the restriction operator
static PetscErrorCode MatMult_Restrict(Mat A, Vec X, Vec Y) {
  PetscErrorCode ierr;
  UserIR user;
  PetscScalar *x, *y;

  PetscFunctionBeginUser;
  ierr = MatShellGetContext(A, &user); CHKERRQ(ierr);

  // Global-to-local
  ierr = VecZeroEntries(user->Xloc); CHKERRQ(ierr);
  ierr = DMGlobalToLocalBegin(user->dmf, X, INSERT_VALUES, user->Xloc);
  CHKERRQ(ierr);
  ierr = DMGlobalToLocalEnd(user->dmf, X, INSERT_VALUES, user->Xloc);
  CHKERRQ(ierr);
  ierr = VecZeroEntries(user->Yloc); CHKERRQ(ierr);

  // Multiplicity
  ierr = VecPointwiseMult(user->Xloc, user->Xloc, user->mult); CHKERRQ(ierr);

  // Setup CEED vectors
  ierr = VecGetArrayRead(user->Xloc, (const PetscScalar **)&x); CHKERRQ(ierr);
  ierr = VecGetArray(user->Yloc, &y); CHKERRQ(ierr);
  CeedVectorSetArray(user->ceedvecf, CEED_MEM_HOST, CEED_USE_POINTER, x);
  CeedVectorSetArray(user->ceedvecc, CEED_MEM_HOST, CEED_USE_POINTER, y);

  // Apply CEED operator
  CeedOperatorApply(user->op, user->ceedvecf, user->ceedvecc,
                    CEED_REQUEST_IMMEDIATE);

  // Restore PETSc vectors
  CeedVectorTakeArray(user->ceedvecf, CEED_MEM_HOST, NULL);
  CeedVectorTakeArray(user->ceedvecc, CEED_MEM_HOST, NULL);
  ierr = VecRestoreArrayRead(user->Xloc, (const PetscScalar **)&x);
  CHKERRQ(ierr);
  ierr = VecRestoreArray(user->Yloc, &y); CHKERRQ(ierr);

  // Local-to-global
  ierr = VecZeroEntries(Y); CHKERRQ(ierr);
  ierr = DMLocalToGlobalBegin(user->dmc, user->Yloc, ADD_VALUES, Y);
  CHKERRQ(ierr);
  ierr = DMLocalToGlobalEnd(user->dmc, user->Yloc, ADD_VALUES, Y);
  CHKERRQ(ierr);

  PetscFunctionReturn(0);
}
#endif

// This function calculates the error in the final solution
static PetscErrorCode ComputeErrorMax(UserO user, CeedOperator op_error,
                                      Vec X, CeedVector target,
                                      PetscReal *maxerror) {
  PetscErrorCode ierr;
  PetscScalar *x;
  CeedVector collocated_error;
  CeedInt length;

  PetscFunctionBeginUser;
  CeedVectorGetLength(target, &length);
  CeedVectorCreate(user->ceed, length, &collocated_error);

  // Global-to-local
  ierr = DMGlobalToLocal(user->dm, X, INSERT_VALUES, user->Xloc); CHKERRQ(ierr);

  // Setup CEED vector
  ierr = VecGetArrayRead(user->Xloc, (const PetscScalar **)&x); CHKERRQ(ierr);
  CeedVectorSetArray(user->xceed, CEED_MEM_HOST, CEED_USE_POINTER, x);

  // Apply CEED operator
  CeedOperatorApply(op_error, user->xceed, collocated_error,
                    CEED_REQUEST_IMMEDIATE);

  // Restore PETSc vector
  VecRestoreArrayRead(user->Xloc, (const PetscScalar **)&x); CHKERRQ(ierr);

  // Reduce max error
  *maxerror = 0;
  const CeedScalar *e;
  CeedVectorGetArrayRead(collocated_error, CEED_MEM_HOST, &e);
  for (CeedInt i=0; i<length; i++) {
    *maxerror = PetscMax(*maxerror, PetscAbsScalar(e[i]));
  }
  CeedVectorRestoreArrayRead(collocated_error, &e);
  ierr = MPI_Allreduce(MPI_IN_PLACE, maxerror,
                       1, MPIU_REAL, MPIU_MAX, user->comm); CHKERRQ(ierr);

  // Cleanup
  CeedVectorDestroy(&collocated_error);

  PetscFunctionReturn(0);
}

#endif // setupsphere_h


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

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

#include "MZC3D64.h"
#include "MZC2D64.h"
#include "sph_data_types.h"
#include "sph_linked_list.h"

int safe_free_box(linkedListBox *box){
	kh_destroy(0, box->hbegin);
	kh_destroy(1, box->hend);
	free(box);

	return 0;
}

int compare_int64_t(const void *p,const void *q){
	int64_t *data1,*data2;
	data1 = (int64_t*)p;
	data2 = (int64_t*)q;

	if(data1[0] < data2[0])
		return -1;
	else if(data1[0] == data2[0])
		return 0;
	else
		return 1;
}

#define safe_check_alloc(ptr,N,dtype) {                                          \
                                       (ptr) = (dtype*)malloc((N)*sizeof(dtype));\
                                       if((ptr)==NULL){                          \
                                         success=0;                              \
                                         goto finishlabel;                       \
                                       }                                         \
                                      }

#define safe_check__aligned_alloc(ptr,alignment,N,dtype) {                                        \
                                       (ptr) = (dtype*)aligned_alloc(alignment,(N)*sizeof(dtype));\
                                       if((ptr)==NULL){                                           \
                                         success=0;                                               \
                                         goto finishlabel;                                        \
                                       }                                                          \
                                      }

#define safe_free(ptr) {               \
                        if(ptr != NULL)\
                          free(ptr);   \
                       }

int SPHparticle_SoA_malloc(int N,SPHparticle **lsph){
  int success=1;

  (*lsph) = (SPHparticle*)malloc(1*sizeof(SPHparticle));
  if(lsph==NULL){
    success = 0;
    goto finishlabel;
  }
  (*lsph)->x  = NULL; (*lsph)->y  = NULL; (*lsph)->z  = NULL;
  (*lsph)->ux = NULL; (*lsph)->uy = NULL; (*lsph)->uz = NULL;
  (*lsph)->Fx = NULL; (*lsph)->Fy = NULL; (*lsph)->Fz = NULL;
  (*lsph)->nu = NULL; (*lsph)->rho= NULL;
  (*lsph)->id = NULL; (*lsph)->hash= NULL;

  const int alignment = 32;
  safe_check__aligned_alloc((*lsph)->x   , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->y   , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->z   , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->ux  , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->uy  , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->uz  , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->Fx  , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->Fy  , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->Fz  , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->nu  , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->rho , alignment, N ,double);
  safe_check__aligned_alloc((*lsph)->id  , alignment, N ,int64_t);
  safe_check__aligned_alloc((*lsph)->hash, alignment,2*N,int64_t);

  /*
  safe_check_alloc((*lsph)->x   , N ,double);
  safe_check_alloc((*lsph)->y   , N ,double);
  safe_check_alloc((*lsph)->z   , N ,double);
  safe_check_alloc((*lsph)->ux  , N ,double);
  safe_check_alloc((*lsph)->uy  , N ,double);
  safe_check_alloc((*lsph)->uz  , N ,double);
  safe_check_alloc((*lsph)->Fx  , N ,double);
  safe_check_alloc((*lsph)->Fy  , N ,double);
  safe_check_alloc((*lsph)->Fz  , N ,double);
  safe_check_alloc((*lsph)->nu  , N ,double);
  safe_check_alloc((*lsph)->rho , N ,double);
  safe_check_alloc((*lsph)->id  , N ,int64_t);
  safe_check_alloc((*lsph)->hash,2*N,int64_t);*/

finishlabel:

  if(success)
    return 0;
  else{
    if(*lsph==NULL)
      return 1;

    safe_free((*lsph)->x);  safe_free((*lsph)->y);  safe_free((*lsph)->z);
    safe_free((*lsph)->ux); safe_free((*lsph)->uy); safe_free((*lsph)->uz);
    safe_free((*lsph)->Fx); safe_free((*lsph)->Fy); safe_free((*lsph)->Fz);
    safe_free((*lsph)->nu); safe_free((*lsph)->rho);
    safe_free((*lsph)->id); safe_free((*lsph)->hash);

    return 1;
  }
}

int SPHparticleSOA_safe_free(int N,SPHparticle **lsph){

  if(*lsph==NULL)
    return 1;

  safe_free((*lsph)->x);  safe_free((*lsph)->y);  safe_free((*lsph)->z);
  safe_free((*lsph)->ux); safe_free((*lsph)->uy); safe_free((*lsph)->uz);
  safe_free((*lsph)->Fx); safe_free((*lsph)->Fy); safe_free((*lsph)->Fz);
  safe_free((*lsph)->nu); safe_free((*lsph)->rho);
  safe_free((*lsph)->id); safe_free((*lsph)->hash);

  free((*lsph));

  return 0;
}

int arg_parse(int argc, char **argv, int64_t *N, double *h,
              linkedListBox *box){
  bool intern_h = true;

  box->Xmin = -1.0; box->Ymin = -1.0; box->Zmin = -1.0;
  box->Xmax =  2.0; box->Ymax =  2.0; box->Zmax =  2.0;

  if(argc%2==0){
    printf("wrong number of arguments!\n");
    printf("Maybe an option is missing a value?\n");
  }

  for(int i=1;i<argc;i+=2){
    if( strcmp(argv[i],"-N") == 0 ){
      *N = (int64_t) atol(argv[i+1]);
      printf("N particles = %ld\n",*N);
    }
    else if( strcmp(argv[i],"-h") == 0 ){
      *h = atof(argv[i+1]);
      printf("h = %lf\n",*h);
      intern_h = false;
    }
    else if( strcmp(argv[i],"-Xmin") == 0 ){
      box->Xmin = atof(argv[i+1]);
      printf("Xmin = %lf\n",box->Xmin);
    }
    else if( strcmp(argv[i],"-Ymin") == 0 ){
      box->Ymin = atof(argv[i+1]);
      printf("Ymin = %lf\n",box->Ymin);
    }
    else if( strcmp(argv[i],"-Zmin") == 0 ){
      box->Zmin = atof(argv[i+1]);
      printf("Zmin = %lf\n",box->Zmin);
    }
    else if( strcmp(argv[i],"-Xmax") == 0 ){
      box->Xmax = atof(argv[i+1]);
      printf("Xmax = %lf\n",box->Xmax);
    }
    else if( strcmp(argv[i],"-Ymax") == 0 ){
      box->Ymax = atof(argv[i+1]);
      printf("Ymax = %lf\n",box->Ymax);
    }
    else if( strcmp(argv[i],"-Zmax") == 0 ){
      box->Zmax = atof(argv[i+1]);
      printf("Zmax = %lf\n",box->Zmax);
    }
    else if( strcmp(argv[i],"-Nx") == 0 ){
      box->Nx   = atol(argv[i+1]);
      printf("Nx = %d\n",box->Nx);
    }
    else if( strcmp(argv[i],"-Ny") == 0 ){
      box->Ny   = atol(argv[i+1]);
      printf("Ny = %d\n",box->Ny);
    }
    else if( strcmp(argv[i],"-Nz") == 0 ){
      box->Nz   = atol(argv[i+1]);
      printf("Nz = %d\n",box->Nz);
    }
    else{
      printf("unknown option: %s %s\n",argv[i],argv[i+1]);
    }
  }

  if(intern_h){
    box->Nx = (int)( (box->Xmax-box->Xmin)/(2*(*h)) );
    box->Ny = (int)( (box->Ymax-box->Ymin)/(2*(*h)) );
    box->Nz = (int)( (box->Zmax-box->Zmin)/(2*(*h)) );
  }

  box->N  = (box->Nx)*(box->Ny)*(box->Nz);

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

  return 0;
}

int gen_unif_rdn_pos(int64_t N, int seed, SPHparticle *lsph){

	const gsl_rng_type *T=NULL;
	gsl_rng *r=NULL;

	if(lsph==NULL)
		return 1;

	gsl_rng_env_setup();

	T = gsl_rng_default;
	r = gsl_rng_alloc(T);
	gsl_rng_set(r,seed);

	for(int64_t i=0;i<N;i+=1){
		lsph->x[i] = gsl_rng_uniform(r);
		lsph->y[i] = gsl_rng_uniform(r);
		lsph->z[i] = gsl_rng_uniform(r);

		lsph->ux[i] = 0.0; lsph->Fx[i] = 0.0;
		lsph->uy[i] = 0.0; lsph->Fy[i] = 0.0;
    lsph->uz[i] = 0.0; lsph->Fz[i] = 0.0;

		lsph->nu[i]   = 1.0/N;
		lsph->rho[i]  = 0.0;
		lsph->id[i]   = (int64_t) i;
		lsph->hash[2*i+0] = (int64_t) 0;
		lsph->hash[2*i+1] = (int64_t) i;
	}

	gsl_rng_free(r);

	return 0;
}

int gen_unif_rdn_pos_box(int64_t N, int seed, linkedListBox *box,SPHparticle *lsph){

  const gsl_rng_type *T=NULL;
  gsl_rng *r=NULL;

  if(lsph==NULL)
    return 1;

  gsl_rng_env_setup();

  T = gsl_rng_default;
  r = gsl_rng_alloc(T);
  gsl_rng_set(r,seed);

  for(int64_t i=0;i<N;i+=1){
    lsph->x[i] = gsl_rng_uniform(r)*(box->Xmax-box->Xmin) + box->Xmin;
    lsph->y[i] = gsl_rng_uniform(r)*(box->Ymax-box->Ymin) + box->Ymin;
    lsph->z[i] = gsl_rng_uniform(r)*(box->Zmax-box->Zmin) + box->Zmin;

    lsph->ux[i] = 0.0; lsph->Fx[i] = 0.0;
    lsph->uy[i] = 0.0; lsph->Fy[i] = 0.0;
    lsph->uz[i] = 0.0; lsph->Fz[i] = 0.0;

    lsph->nu[i]   = 1.0/N;
    lsph->rho[i]  = 0.0;
    lsph->id[i]   = (int64_t) i;
    lsph->hash[2*i+0] = (int64_t) 0;
    lsph->hash[2*i+1] = (int64_t) i;
  }

  gsl_rng_free(r);

  return 0;
}

int print_time_stats(int runs, double *times){
  double t[5], dt[5], total_time, dtotal_time;

  printf("fast neighbour search / SoA / outer-openMP / symmetric load balanced\n");

  total_time = 0.;
  for(int k=0;k<5;k+=1){
    t[k]=0.; dt[k]=0.;
    for(int run=0;run<runs;run+=1)
      t[k] += times[5*run+k];
    t[k] /= runs;
    for(int run=0;run<runs;run+=1)
      dt[k] += (times[5*run+k]-t[k])*(times[5*run+k]-t[k]);
    dt[k] /= runs;
    dt[k] = sqrt(dt[k]);

    total_time += t[k];
  }

  dtotal_time = 0.;
  for(int run=0;run<runs;run+=1){
    double rgm = 0.;
    for(int k=0;k<5;k+=1)
      rgm += times[5*run+k];

    dtotal_time += (rgm-total_time)*(rgm-total_time);
  }
  dtotal_time /= runs;
  dtotal_time = sqrt(dtotal_time);

  printf("compute_hash_MC3D calc time                 : %.5lf +- %.6lf s : %.3lg%% +- %.3lg%%\n",t[0],dt[0],100*t[0]/total_time,100*dt[0]/total_time);
  printf("qsort calc time                             : %.5lf +- %.6lf s : %.3lg%% +- %.3lg%%\n",t[1],dt[1],100*t[1]/total_time,100*dt[1]/total_time);
  printf("reorder_lsph_SoA calc time                  : %.5lf +- %.6lf s : %.3lg%% +- %.3lg%%\n",t[2],dt[2],100*t[2]/total_time,100*dt[2]/total_time);
  printf("setup_interval_hashtables calc time         : %.5lf +- %.6lf s : %.3lg%% +- %.3lg%%\n",t[3],dt[3],100*t[3]/total_time,100*dt[3]/total_time);
  printf("compute_density_3d load balanced calc time  : %.5lf +- %.6lf s : %.3lg%% +- %.3lg%%\n",t[4],dt[4],100*t[4]/total_time,100*dt[4]/total_time);
  printf("compute_density_3d load balanced total time : %.5lf +- %.6lf s : %.3lf%%\n",total_time,dtotal_time,100.);

  return 0;
}

int compute_hash_MC3D(int64_t N, SPHparticle *lsph, linkedListBox *box){

	if(lsph==NULL)
		return 1;

	if(box==NULL)
		return 1;

	const double etax = box->Nx/(box->Xmax - box->Xmin);
	const double etay = box->Ny/(box->Ymax - box->Ymin);
	const double etaz = box->Nz/(box->Zmax - box->Zmin);

	for(int64_t i=0;i<N;i+=1){
		uint32_t kx,ky,kz;
		kx = (uint32_t)(etax*(lsph->x[i] - box->Xmin));
		ky = (uint32_t)(etay*(lsph->y[i] - box->Ymin));
		kz = (uint32_t)(etaz*(lsph->z[i] - box->Zmin));

		if((kx<0)||(ky<0)||(kz<0))
			return 1;
		else if((kx>=box->Nx)||(ky>=box->Nx)||(kz>=box->Nx))
			return 1;
		else{
			lsph->hash[2*i] = ullMC3Dencode(kx,ky,kz);
		}
	}

	return 0;
}

int compute_hash_MC2D(int64_t N, SPHparticle *lsph, linkedListBox *box){

	if(lsph==NULL)
		return 1;

	if(box==NULL)
		return 1;

	const double etax = box->Nx/(box->Xmax - box->Xmin);
	const double etay = box->Ny/(box->Ymax - box->Ymin);

	for(int64_t i=0;i<N;i+=1){
		uint32_t kx,ky;
		kx = (uint32_t)(etax*(lsph->x[i] - box->Xmin));
		ky = (uint32_t)(etay*(lsph->y[i] - box->Ymin));

		if((kx<0)||(ky<0))
			return 1;
		else if((kx>=box->Nx)||(ky>=box->Nx))
			return 1;
		else
			lsph->hash[2*i] = ullMC2Dencode(kx,ky);
	}

	return 0;
}

#define swap_loop(N,lsph,temp_swap,member,type) for(int64_t i=0;i<(N);i+=1)                            \
																	 	              (temp_swap)[i] = (lsph)->member[(lsph)->hash[2*i+1]];\
																		            memcpy((lsph)->member,temp_swap,(N)*sizeof(type))

int reorder_lsph_SoA(int N, SPHparticle *lsph, void *swap_arr){

	int64_t *int64_temp_swap = (int64_t *)swap_arr;
	swap_loop(N,lsph,int64_temp_swap,id ,int64_t);
	double *double_temp_swap = (double *)swap_arr;
	swap_loop(N,lsph,double_temp_swap,nu ,double);
	swap_loop(N,lsph,double_temp_swap,rho,double);
	swap_loop(N,lsph,double_temp_swap,x  ,double);
	swap_loop(N,lsph,double_temp_swap,y  ,double);
	swap_loop(N,lsph,double_temp_swap,z  ,double);
	swap_loop(N,lsph,double_temp_swap,ux ,double);
	swap_loop(N,lsph,double_temp_swap,uy ,double);
	swap_loop(N,lsph,double_temp_swap,uz ,double);
	swap_loop(N,lsph,double_temp_swap,Fx ,double);
	swap_loop(N,lsph,double_temp_swap,Fy ,double);
	swap_loop(N,lsph,double_temp_swap,Fz ,double);

	return 0;
}

int setup_interval_hashtables(int64_t N,SPHparticle *lsph,linkedListBox *box){

	int ret;
	int64_t hash0 = lsph->hash[2*0];
	khiter_t kbegin,kend;

	if(lsph==NULL)
		return 1;

	if(box==NULL)
		return 1;

	kbegin = kh_put(0, box->hbegin, lsph->hash[2*0], &ret); kh_value(box->hbegin, kbegin) = (int64_t)0;
	for(int64_t i=0;i<N;i+=1){
		lsph->hash[i] = lsph->hash[2*i];
		if(lsph->hash[i] == hash0)
			continue;
		hash0 = lsph->hash[i];
		kend   = kh_put(1, box->hend  , lsph->hash[i-1], &ret); kh_value(box->hend  , kend)   = i;
		kbegin = kh_put(0, box->hbegin, lsph->hash[i  ], &ret); kh_value(box->hbegin, kbegin) = i;
	}
	// TODO: possible bug? verify 2*(N-1) instead of N-1
	kend   = kh_put(1, box->hend  , lsph->hash[2*(N-1)], &ret); kh_value(box->hend  , kend)   = N;

	return 0;
}

int neighbour_hash_3d(int64_t hash,int64_t *nblist,int width, linkedListBox *box){
	int idx=0,kx=0,ky=0,kz=0;

	kx = ullMC3DdecodeX(hash);
	ky = ullMC3DdecodeY(hash);
	kz = ullMC3DdecodeZ(hash);

	for(int ix=-width;ix<=width;ix+=1){
		for(int iy=-width;iy<=width;iy+=1){
			for(int iz=-width;iz<=width;iz+=1){
				if((kx+ix<0)||(ky+iy<0)||(kz+iz<0))
					nblist[idx++] = -1;
				else if( (kx+ix>=box->Nx)||(ky+iy>=box->Ny)||(kz+iz>=box->Nz) )
					nblist[idx++] = -1;
				else if( kh_get(0, box->hbegin, ullMC3Dencode(kx+ix,ky+iy,kz+iz)) == kh_end(box->hbegin) )
					nblist[idx++] = -1;
				else
					nblist[idx++] = ullMC3Dencode(kx+ix,ky+iy,kz+iz);
			}
		}
	}
	return 0;
}

int neighbour_hash_2d(int64_t hash,int64_t *nblist,int width, linkedListBox *box){
	int idx=0,kx=0,ky=0;

	kx = ullMC2DdecodeX(hash);
	ky = ullMC2DdecodeY(hash);

	for(int ix=-width;ix<=width;ix+=1){
		for(int iy=-width;iy<=width;iy+=1){
				if((kx+ix<0)||(ky+iy<0))
					nblist[idx++] = -1;
				else if((kx+ix>=box->Nx)||(ky+iy>=box->Ny))
					nblist[idx++] = -1;
				else if( kh_get(0, box->hbegin, ullMC2Dencode(kx+ix,ky+iy)) == kh_end(box->hbegin) )
					nblist[idx++] = -1;
				else
					nblist[idx++] = ullMC2Dencode(kx+ix,ky+iy);
		}
	}
	return 0;
}

int print_boxes_populations(linkedListBox *box){
	khiter_t kbegin,kend;

	for (kbegin = kh_begin(box->hbegin); kbegin != kh_end(box->hbegin); kbegin++){
		if (kh_exist(box->hbegin, kbegin)){
			kend = kh_get(1, box->hend, kh_key(box->hbegin, kbegin));
			printf("hash - (begin,end) = %lu - (%lu,%lu): %ld\n",
				kh_key(box->hbegin, kbegin),
				kh_value(box->hbegin, kbegin),
				kh_value(box->hend, kend),
				((int64_t)kh_value(box->hend, kend))-((int64_t)kh_value(box->hbegin, kbegin)));
		}
	}

	return 0;
}

int print_neighbour_list_MC3D(int64_t Nmax,unsigned int width,unsigned int stride,SPHparticle *lsph,linkedListBox *box){

	int64_t nblist[(2*width+1)*(2*width+1)*(2*width+1)];

	for(int64_t i=0;i<Nmax;i+=(int64_t)stride){
		int res = neighbour_hash_3d(lsph->hash[i],nblist,width,box);
		if(res!=0)
			printf("invalid neighbour_hash_3d calculation\n");
		printf("origin hash %lu : (%d,%d,%d) \n",lsph->hash[i],ullMC3DdecodeX(lsph->hash[i]),
															  ullMC3DdecodeY(lsph->hash[i]),
															  ullMC3DdecodeZ(lsph->hash[i]));
		for(unsigned int j=0;j<(2*width+1)*(2*width+1)*(2*width+1);j+=1){
			if(nblist[j]>=0)
				printf("    neighbour hash %lu : (%d,%d,%d) \n",nblist[j],ullMC3DdecodeX(nblist[j]),
																		  ullMC3DdecodeY(nblist[j]),
																		  ullMC3DdecodeZ(nblist[j]));
			else
				printf("no neighbour here\n");
		}
		printf("\n\n");
	}

	return 0;
}

int print_neighbour_list_MC2D(int64_t Nmax,unsigned int width,unsigned int stride,SPHparticle *lsph,linkedListBox *box){

	int64_t nblist[(2*width+1)*(2*width+1)];

	for(int64_t i=0;i<Nmax;i+=(int64_t)stride){
		int res = neighbour_hash_2d(lsph->hash[i],nblist,width,box);
		if(res!=0)
			printf("invalid neighbour_hash_3d calculation\n");

		printf("origin hash %lu : (%d,%d) \n",lsph->hash[i],ullMC2DdecodeX(lsph->hash[i]),
															  											  ullMC2DdecodeY(lsph->hash[i]));
		for(unsigned int j=0;j<(2*width+1)*(2*width+1);j+=1){
			if(nblist[j]>=0)
				printf("    neighbour hash %lu : (%d,%d) \n",nblist[j],ullMC3DdecodeX(lsph->hash[i]),
																		  												 ullMC3DdecodeY(lsph->hash[i]));
			else
				printf("no neighbour here\n");
		}
		printf("\n\n");
	}

	return 0;
}

int print_neighbour_list_MC3D_lsph_file(int64_t Nmax,unsigned int width,unsigned int stride,SPHparticle *lsph,linkedListBox *box){
	FILE *fp;
	int64_t nblist[(2*width+1)*(2*width+1)*(2*width+1)];

	fp = fopen("data/nblist_MC3D.csv","w");

	const double etax = box->Nx/(box->Xmax - box->Xmin);
	const double etay = box->Ny/(box->Ymax - box->Ymin);
	const double etaz = box->Nz/(box->Zmax - box->Zmin);

	for(int64_t i=0;i<Nmax;i+=(int64_t)stride){
		uint32_t kx,ky,kz;
		int res = neighbour_hash_3d(lsph->hash[i],nblist,width,box);
		if(res!=0)
			printf("invalid neighbour_hash_3d calculation\n");

		kx = (uint32_t)(etax*(lsph->x[i] - box->Xmin));
		ky = (uint32_t)(etay*(lsph->y[i] - box->Ymin));
		kz = (uint32_t)(etaz*(lsph->z[i] - box->Zmin));

		fprintf(fp,"base   hash %lu : (%d,%d,%d) \n",lsph->hash[i],kx,ky,kz);
		fprintf(fp,"origin hash %lu : (%d,%d,%d) \n",lsph->hash[i],ullMC3DdecodeX(lsph->hash[i]),
					         									  												 ullMC3DdecodeY(lsph->hash[i]),
							    								  													 ullMC3DdecodeZ(lsph->hash[i]));

		for(unsigned int j=0;j<(2*width+1)*(2*width+1)*(2*width+1);j+=1)
			if(nblist[j]>=0){
				fprintf(fp,"  %lu:(%d,%d,%d)",nblist[j],ullMC3DdecodeX(nblist[j]),
																								ullMC3DdecodeY(nblist[j]),
																								ullMC3DdecodeZ(nblist[j]));
				if(j<(2*width+1)*(2*width+1)*(2*width+1)-1)
					fprintf(fp,",");
			}

		fprintf(fp,"\n\n");
	}

	fclose(fp);

	return 0;
}

int print_neighbour_list_MC3D_lsph_ids_file(int N, SPHparticle *lsph, linkedListBox *box){
	int res;
  khiter_t kbegin,kend;
  int64_t node_hash=-1,node_begin=0, node_end=0;
  int64_t nb_begin= 0, nb_end = 0;
  int64_t nblist[(2*box->width+1)*(2*box->width+1)*(2*box->width+1)];

  FILE *fp = fopen("data/nblist_ll.json","w");
  fprintf(fp,"{\n");
  for (kbegin = kh_begin(box->hbegin); kbegin != kh_end(box->hbegin); kbegin++){

    if (kh_exist(box->hbegin, kbegin)){

      kend = kh_get(1, box->hend, kh_key(box->hbegin, kbegin));
      node_hash = kh_key(box->hbegin, kbegin);
      node_begin = kh_value(box->hbegin, kbegin);
      node_end   = kh_value(box->hend, kend);

      res = neighbour_hash_3d(node_hash,nblist,box->width,box);
			if(res!=0)
				printf("invalid neighbour_hash_3d calculation\n");
      for(int64_t ii=node_begin;ii<node_end;ii+=1){

      	fprintf(fp,"\"%ld\":[",lsph->id[ii]);
      	for(int j=0;j<(2*box->width+1)*(2*box->width+1)*(2*box->width+1);j+=1){
      		if(nblist[j]>=0){
      			//nb_hash  = nblist[j];
          	nb_begin = kh_value(box->hbegin, kh_get(0, box->hbegin, nblist[j]) );
          	nb_end   = kh_value(box->hend  , kh_get(1, box->hend  , nblist[j]) );

          	for(int64_t jj=nb_begin;jj<nb_end;jj+=1)
          		fprintf(fp,"%ld, ",lsph->id[jj]);
      		}
      	}
      	if((kbegin==kh_end(box->hbegin)-1)&&(ii==node_end-1))
      		fprintf(fp,"-1]\n");
    		else
    			fprintf(fp,"-1],\n");
      }
    }
  }
  fprintf(fp,"}");

  fflush(fp);
  fclose(fp);

	return 0;
}

int print_neighbour_list_MC2D_lsph_file(int64_t Nmax,unsigned int width,unsigned int stride,SPHparticle *lsph,linkedListBox *box){
	FILE *fp;
	int64_t nblist[(2*width+1)*(2*width+1)];

	fp = fopen("data/nblist_MC2D.csv","w");

	const double etax = box->Nx/(box->Xmax - box->Xmin);
	const double etay = box->Ny/(box->Ymax - box->Ymin);

	for(int64_t i=0;i<Nmax;i+=(int64_t)stride){
		uint32_t kx,ky;
		int res = neighbour_hash_2d(lsph->hash[i],nblist,width,box);
		if(res!=0)
			printf("invalid neighbour_hash_3d calculation\n");

		kx = (uint32_t)(etax*(lsph->x[i] - box->Xmin));
		ky = (uint32_t)(etay*(lsph->y[i] - box->Ymin));

		fprintf(fp,"base   hash %lu : (%d,%d) \n",lsph->hash[i],kx,ky);
		fprintf(fp,"origin hash %lu : (%d,%d) \n",lsph->hash[i],ullMC2DdecodeX(lsph->hash[i]),
					         									  											ullMC2DdecodeY(lsph->hash[i]));

		for(unsigned int j=0;j<(2*width+1)*(2*width+1);j+=1)
			if(nblist[j]>=0){
				fprintf(fp,"  %lu:(%d,%d)",nblist[j], ullMC2DdecodeX(nblist[j]),
																    					ullMC2DdecodeY(nblist[j]));
				if(j<(2*width+1)*(2*width+1)-1)
					fprintf(fp,",");
			}

		fprintf(fp,"\n\n");
	}

	fclose(fp);

	return 0;
}

/*
int main(){

  int j=0,numThreads=6,err;
  int64_t N = 100000;
  double S=0.,S2=0.;
  linkedListBox *box;
  SPHparticle *lsph;

  lsph = (SPHparticle*)malloc(N*sizeof(SPHparticle));

  err = gen_unif_rdn_pos( N,123123123,lsph);

  box = (linkedListBox*)malloc(1*sizeof(linkedListBox));

  box->Nx = box->Ny = box->Nz = 10;
  box->N  = (box->Nx)*(box->Ny)*(box->Nz);
  box->Xmin.x = 0.0; box->Xmin.y = 0.0; box->Xmin.z = 0.0;
  box->Xmax.x = 1.0; box->Xmax.y = 1.0; box->Xmax.z = 1.0;
  box->hbegin = kh_init(0);
  box->hend = kh_init(1);

  err = compute_hash_MC3D(N,lsph,box);

  qsort(lsph,N,sizeof(SPHparticle),compare_SPHparticle);

  err = setup_interval_hashtables(N,lsph,box);

  //print_boxes_populations(box);
  //print_neighbour_list_MC3D(N,1,N/13,lsph,box);
  print_neighbour_list_MC3D_lsph_file(N,1,1,lsph,box);

  free(lsph);
  safe_free_box(box);

  return 0;
}*/

////////////////////////////////////////////////////////////////////
// https://stackoverflow.com/questions/16007640/openmp-parallel-quicksort

#define TASK_SIZE 24

int64_t partition(int64_t * a, int64_t p, int64_t r)
{
  int64_t lt[2*(r-p)];
  int64_t gt[2*(r-p)];
  int64_t i;
  int64_t j;
  int64_t key0 = a[2*r+0];
  int64_t key1 = a[2*r+1];
  int64_t lt_n = 0;
  int64_t gt_n = 0;

  for(i = p; i < r; i++){
    if(a[2*i] < a[2*r]){
      lt[2*lt_n+0] = a[2*i+0];
      lt[2*lt_n+1] = a[2*i+1];
      lt_n++;
    }else{
      gt[2*gt_n+0] = a[2*i+0];
      gt[2*gt_n+1] = a[2*i+1];
      gt_n++;
    }
  }

  for(i = 0; i < lt_n; i++){
    a[2*(p + i)+0] = lt[2*i+0];
    a[2*(p + i)+1] = lt[2*i+1];
  }

  a[2*(p + lt_n)+0] = key0;
  a[2*(p + lt_n)+1] = key1;

  for(j = 0; j < gt_n; j++){
    a[2*(p + lt_n + j + 1)+0] = gt[2*j+0];
    a[2*(p + lt_n + j + 1)+1] = gt[2*j+1];
  }

  return p + lt_n;
}

void quicksort_omp(int64_t * a, int64_t p, int64_t r)
{
	int64_t div;

  if(p < r){
    div = partition(a, p, r);
    #pragma omp task shared(a) if(r - p > TASK_SIZE)
    quicksort_omp(a, p, div - 1);
    #pragma omp task shared(a) if(r - p > TASK_SIZE)
    quicksort_omp(a, div + 1, r);
  }
}


#ifndef KGS_GSLQR_H
#define KGS_GSLQR_H

#include "math/QR.h"
#include <gsl/gsl_matrix.h>

class QRGSL: public QR {
 public:
  QRGSL(gsl_matrix* M): QR(M){}

 protected:

  void updateFromMatrix() override;
};

#endif //KGS_GSLQR_H


/*
 * This file is part of the GROMACS molecular simulation package.
 *
 * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
 * Copyright (c) 2001-2004, The GROMACS development team,
 * check out http://www.gromacs.org for more information.
 * Copyright (c) 2012,2013, by the GROMACS development team, led by
 * David van der Spoel, Berk Hess, Erik Lindahl, and including many
 * others, as listed in the AUTHORS file in the top-level source
 * directory and at http://www.gromacs.org.
 *
 * GROMACS is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2.1
 * of the License, or (at your option) any later version.
 *
 * GROMACS 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with GROMACS; if not, see
 * http://www.gnu.org/licenses, or write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
 *
 * If you want to redistribute modifications to GROMACS, please
 * consider that scientific software is very special. Version
 * control is crucial - bugs must be traceable. We will be happy to
 * consider code for inclusion in the official distribution, but
 * derived work must not be called official GROMACS. Details are found
 * in the README & COPYING files - if they are missing, get the
 * official version at http://www.gromacs.org.
 *
 * To help us fund GROMACS development, we humbly ask that you cite
 * the research papers on the package. Check out http://www.gromacs.org.
 */
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#ifdef HAVE_LIBGSL
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_version.h>
#endif

#include <math.h>
#include "typedefs.h"
#include "smalloc.h"
#include "vec.h"
#include "geminate.h"
#include "gmx_omp.h"

/* The first few sections of this file contain functions that were adopted,
 * and to some extent modified, by Erik Marklund (erikm[aT]xray.bmc.uu.se,
 * http://folding.bmc.uu.se) from code written by Omer Markovitch (email, url).
 * This is also the case with the function eq10v2().
 *
 * The parts menetioned in the previous paragraph were contributed under the BSD license.
 */

/* This first part is from complex.c which I recieved from Omer Markowitch.
 * - Erik Marklund
 *
 * ------------- from complex.c ------------- */

/* Complexation of a paired number (r,i)                                     */
static gem_complex gem_cmplx(double r, double i)
{
    gem_complex value;
    value.r = r;
    value.i = i;
    return value;
}

/* Complexation of a real number, x */
static gem_complex gem_c(double x)
{
    gem_complex value;
    value.r = x;
    value.i = 0;
    return value;
}

/* Magnitude of a complex number z                                           */
static double gem_cx_abs(gem_complex z)
{
    return (sqrt(z.r*z.r+z.i*z.i));
}

/* Addition of two complex numbers z1 and z2                                 */
static gem_complex gem_cxadd(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    value.r = z1.r+z2.r;
    value.i = z1.i+z2.i;
    return value;
}

/* Addition of a complex number z1 and a real number r */
static gem_complex gem_cxradd(gem_complex z, double r)
{
    gem_complex value;
    value.r = z.r + r;
    value.i = z.i;
    return value;
}

/* Subtraction of two complex numbers z1 and z2                              */
static gem_complex gem_cxsub(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    value.r = z1.r-z2.r;
    value.i = z1.i-z2.i;
    return value;
}

/* Multiplication of two complex numbers z1 and z2                           */
static gem_complex gem_cxmul(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    value.r = z1.r*z2.r-z1.i*z2.i;
    value.i = z1.r*z2.i+z1.i*z2.r;
    return value;
}

/* Square of a complex number z                                              */
static gem_complex gem_cxsq(gem_complex z)
{
    gem_complex value;
    value.r = z.r*z.r-z.i*z.i;
    value.i = z.r*z.i*2.;
    return value;
}

/* multiplication of a complex number z and a real number r */
static gem_complex gem_cxrmul(gem_complex z, double r)
{
    gem_complex value;
    value.r = z.r*r;
    value.i = z.i*r;
    return value;
}

/* Division of two complex numbers z1 and z2                                 */
static gem_complex gem_cxdiv(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    double      num;
    num = z2.r*z2.r+z2.i*z2.i;
    if (num == 0.)
    {
        fprintf(stderr, "ERROR in gem_cxdiv function\n");
    }
    value.r = (z1.r*z2.r+z1.i*z2.i)/num; value.i = (z1.i*z2.r-z1.r*z2.i)/num;
    return value;
}

/* division of a complex z number by a real number x */
static gem_complex gem_cxrdiv(gem_complex z, double r)
{
    gem_complex value;
    value.r = z.r/r;
    value.i = z.i/r;
    return value;
}

/* division of a real number r by a complex number x */
static gem_complex gem_rxcdiv(double r, gem_complex z)
{
    gem_complex value;
    double      f;
    f       = r/(z.r*z.r+z.i*z.i);
    value.r = f*z.r;
    value.i = -f*z.i;
    return value;
}

/* Exponential of a complex number-- exp (z)=|exp(z.r)|*{cos(z.i)+I*sin(z.i)}*/
static gem_complex gem_cxdexp(gem_complex z)
{
    gem_complex value;
    double      exp_z_r;
    exp_z_r = exp(z.r);
    value.r = exp_z_r*cos(z.i);
    value.i = exp_z_r*sin(z.i);
    return value;
}

/* Logarithm of a complex number -- log(z)=log|z|+I*Arg(z),                  */
/*                                  where -PI < Arg(z) < PI                  */
static gem_complex gem_cxlog(gem_complex z)
{
    gem_complex value;
    double      mag2;
    mag2 = z.r*z.r+z.i*z.i;
    if (mag2 < 0.)
    {
        fprintf(stderr, "ERROR in gem_cxlog func\n");
    }
    value.r = log(sqrt(mag2));
    if (z.r == 0.)
    {
        value.i = PI/2.;
        if (z.i < 0.)
        {
            value.i = -value.i;
        }
    }
    else
    {
        value.i = atan2(z.i, z.r);
    }
    return value;
}

/* Square root of a complex number z = |z| exp(I*the) -- z^(1/2)             */
/*                               z^(1/2)=|z|^(1/2)*[cos(the/2)+I*sin(the/2)] */
/*                               where 0 < the < 2*PI                        */
static gem_complex gem_cxdsqrt(gem_complex z)
{
    gem_complex value;
    double      sq;
    sq      = gem_cx_abs(z);
    value.r = sqrt(fabs((sq+z.r)*0.5)); /* z'.r={|z|*[1+cos(the)]/2}^(1/2) */
    value.i = sqrt(fabs((sq-z.r)*0.5)); /* z'.i={|z|*[1-cos(the)]/2}^(1/2) */
    if (z.i < 0.)
    {
        value.r = -value.r;
    }
    return value;
}

/* Complex power of a complex number z1^z2                                   */
static gem_complex gem_cxdpow(gem_complex z1, gem_complex z2)
{
    gem_complex value;
    value = gem_cxdexp(gem_cxmul(gem_cxlog(z1), z2));
    return value;
}

/* ------------ end of complex.c ------------ */

/* This next part was derived from cubic.c, also received from Omer Markovitch.
 * ------------- from cubic.c ------------- */

/* Solver for a cubic equation: x^3-a*x^2+b*x-c=0                            */
static void gem_solve(gem_complex *al, gem_complex *be, gem_complex *gam,
                      double a, double b, double c)
{
    double      t1, t2, two_3, temp;
    gem_complex ctemp, ct3;

    two_3 = pow(2., 1./3.); t1 = -a*a+3.*b; t2 = 2.*a*a*a-9.*a*b+27.*c;
    temp  = 4.*t1*t1*t1+t2*t2;

    ctemp = gem_cmplx(temp, 0.);   ctemp = gem_cxadd(gem_cmplx(t2, 0.), gem_cxdsqrt(ctemp));
    ct3   = gem_cxdpow(ctemp, gem_cmplx(1./3., 0.));

    ctemp = gem_rxcdiv(-two_3*t1/3., ct3);
    ctemp = gem_cxadd(ctemp, gem_cxrdiv(ct3, 3.*two_3));

    *gam = gem_cxadd(gem_cmplx(a/3., 0.), ctemp);

    ctemp = gem_cxmul(gem_cxsq(*gam), gem_cxsq(gem_cxsub(*gam, gem_cmplx(a, 0.))));
    ctemp = gem_cxadd(ctemp, gem_cxmul(gem_cmplx(-4.*c, 0.), *gam));
    ctemp = gem_cxdiv(gem_cxdsqrt(ctemp), *gam);
    *al   = gem_cxrmul(gem_cxsub(gem_cxsub(gem_cmplx(a, 0.), *gam), ctemp), 0.5);
    *be   = gem_cxrmul(gem_cxadd(gem_cxsub(gem_cmplx(a, 0.), *gam), ctemp), 0.5);
}

/* ------------ end of cubic.c ------------ */

/* This next part was derived from cerror.c and rerror.c, also received from Omer Markovitch.
 * ------------- from [cr]error.c ------------- */

/************************************************************/
/* Real valued error function and related functions         */
/* x, y     : real variables                                */
/* erf(x)   : error function                                */
/* erfc(x)  : complementary error function                  */
/* omega(x) : exp(x*x)*erfc(x)                              */
/* W(x,y)   : exp(-x*x)*omega(x+y)=exp(2*x*y+y^2)*erfc(x+y) */
/************************************************************/

/*---------------------------------------------------------------------------*/
/* Utilzed the series approximation of erf(x)                                */
/* Relative error=|err(x)|/erf(x)<EPS                                        */
/* Handbook of Mathematical functions, Abramowitz, p 297                     */
/* Note: When x>=6 series sum deteriorates -> Asymptotic series used instead */
/*---------------------------------------------------------------------------*/
/*  This gives erfc(z) correctly only upto >10-15 */

static double gem_erf(double x)
{
    double n, sum, temp, exp2, xm, x2, x4, x6, x8, x10, x12;
    temp = x;
    sum  = temp;
    xm   = 26.;
    x2   = x*x;
    x4   = x2*x2;
    x6   = x4*x2;
    x8   = x6*x2;
    x10  = x8*x2;
    x12  = x10*x2;
    exp2 = exp(-x2);
    if (x <= xm)
    {
        for (n = 1.; n <= 2000.; n += 1.)
        {
            temp *= 2.*x2/(2.*n+1.);
            sum  += temp;
            if (fabs(temp/sum) < 1.E-16)
            {
                break;
            }
        }

        if (n >= 2000.)
        {
            fprintf(stderr, "In Erf calc - iteration exceeds %lg\n", n);
        }
        sum *= 2./sPI*exp2;
    }
    else
    {
        /* from the asymptotic expansion of experfc(x) */
        sum = (1. - 0.5/x2 + 0.75/x4
               - 1.875/x6 + 6.5625/x8
               - 29.53125/x10 + 162.421875/x12)
            / sPI/x;
        sum *= exp2; /* now sum is erfc(x) */
        sum  = -sum+1.;
    }
    return x >= 0.0 ? sum : -sum;
}

/* Result --> Alex's code for erfc and experfc behaves better than this      */
/* complementray error function.                Returns 1.-erf(x)            */
static double gem_erfc(double x)
{
    double t, z, ans;
    z = fabs(x);
    t = 1.0/(1.0+0.5*z);

    ans = t * exp(-z*z - 1.26551223 +
                  t*(1.00002368 +
                     t*(0.37409196 +
                        t*(0.09678418 +
                           t*(-0.18628806 +
                              t*(0.27886807 +
                                 t*(-1.13520398 +
                                    t*(1.48851587 +
                                       t*(-0.82215223 +
                                          t*0.17087277)))))))));

    return x >= 0.0 ? ans : 2.0-ans;
}

/* omega(x)=exp(x^2)erfc(x)                                                  */
static double gem_omega(double x)
{
    double xm, ans, xx, x4, x6, x8, x10, x12;
    xm  = 26;
    xx  = x*x;
    x4  = xx*xx;
    x6  = x4*xx;
    x8  = x6*xx;
    x10 = x8*xx;
    x12 = x10*xx;

    if (x <= xm)
    {
        ans = exp(xx)*gem_erfc(x);
    }
    else
    {
        /* Asymptotic expansion */
        ans = (1. - 0.5/xx + 0.75/x4 - 1.875/x6 + 6.5625/x8 - 29.53125/x10 + 162.421875/x12) / sPI/x;
    }
    return ans;
}

/*---------------------------------------------------------------------------*/
/* Utilzed the series approximation of erf(z=x+iy)                           */
/* Relative error=|err(z)|/|erf(z)|<EPS                                      */
/* Handbook of Mathematical functions, Abramowitz, p 299                     */
/* comega(z=x+iy)=cexp(z^2)*cerfc(z)                                         */
/*---------------------------------------------------------------------------*/
static gem_complex gem_comega(gem_complex z)
{
    gem_complex value;
    double      x, y;
    double      sumr, sumi, n, n2, f, temp, temp1;
    double      x2, y2, cos_2xy, sin_2xy, cosh_2xy, sinh_2xy, cosh_ny, sinh_ny, exp_y2;

    x        = z.r;
    y        = z.i;
    x2       = x*x;
    y2       = y*y;
    sumr     = 0.;
    sumi     = 0.;
    cos_2xy  = cos(2.*x*y);
    sin_2xy  = sin(2.*x*y);
    cosh_2xy = cosh(2.*x*y);
    sinh_2xy = sinh(2.*x*y);
    exp_y2   = exp(-y2);

    for (n = 1.0, temp = 0.; n <= 2000.; n += 1.0)
    {
        n2      = n*n;
        cosh_ny = cosh(n*y);
        sinh_ny = sinh(n*y);
        f       = exp(-n2/4.)/(n2+4.*x2);
        /* if(f<1.E-200) break; */
        sumr += (2.*x - 2.*x*cosh_ny*cos_2xy + n*sinh_ny*sin_2xy)*f;
        sumi += (2.*x*cosh_ny*sin_2xy + n*sinh_ny*cos_2xy)*f;
        temp1 = sqrt(sumr*sumr+sumi*sumi);
        if (fabs((temp1-temp)/temp1) < 1.E-16)
        {
            break;
        }
        temp = temp1;
    }
    if (n == 2000.)
    {
        fprintf(stderr, "iteration exceeds %lg\n", n);
    }
    sumr *= 2./PI;
    sumi *= 2./PI;

    if (x != 0.)
    {
        f = 1./2./PI/x;
    }
    else
    {
        f = 0.;
    }
    value.r = gem_omega(x)-(f*(1.-cos_2xy)+sumr);
    value.i = -(f*sin_2xy+sumi);
    value   = gem_cxmul(value, gem_cmplx(exp_y2*cos_2xy, exp_y2*sin_2xy));
    return (value);
}

/* ------------ end of [cr]error.c ------------ */

/*_ REVERSIBLE GEMINATE RECOMBINATION
 *
 * Here are the functions for reversible geminate recombination. */

/* Changes the unit from square cm per s to square Ångström per ps,
 * since Omers code uses the latter units while g_mds outputs the former.
 * g_hbond expects a diffusion coefficent given in square cm per s. */
static double sqcm_per_s_to_sqA_per_ps (real D)
{
    fprintf(stdout, "Diffusion coefficient is %f A^2/ps\n", D*1e4);
    return (double)(D*1e4);
}

static double eq10v2(double theoryCt[], double time[], int manytimes,
                     double ka, double kd, t_gemParams *params)
{
    /* Finding the 3 roots */
    int    i;
    double kD, D, r, a, b, c, tsqrt, sumimaginary;
    gem_complex
           alpha, beta, gamma,
        c1, c2, c3, c4,
        oma, omb, omc,
        part1, part2, part3, part4;

    kD = params->kD;
    D  = params->D;
    r  = params->sigma;
    a  = (1.0 + ka/kD) * sqrt(D)/r;
    b  = kd;
    c  = kd * sqrt(D)/r;

    gem_solve(&alpha, &beta, &gamma, a, b, c);
    /* Finding the 3 roots */

    sumimaginary = 0;
    part1        = gem_cxmul(alpha, gem_cxmul(gem_cxadd(beta,  gamma), gem_cxsub(beta,  gamma)));                 /* 1(2+3)(2-3) */
    part2        = gem_cxmul(beta,  gem_cxmul(gem_cxadd(gamma, alpha), gem_cxsub(gamma, alpha)));                 /* 2(3+1)(3-1) */
    part3        = gem_cxmul(gamma, gem_cxmul(gem_cxadd(alpha, beta), gem_cxsub(alpha, beta)));                   /* 3(1+2)(1-2) */
    part4        = gem_cxmul(gem_cxsub(gamma, alpha), gem_cxmul(gem_cxsub(alpha, beta), gem_cxsub(beta, gamma))); /* (3-1)(1-2)(2-3) */

#pragma omp parallel for \
    private(i, tsqrt, oma, omb, omc, c1, c2, c3, c4) \
    reduction(+:sumimaginary) \
    default(shared) \
    schedule(guided)
    for (i = 0; i < manytimes; i++)
    {
        tsqrt         = sqrt(time[i]);
        oma           = gem_comega(gem_cxrmul(alpha, tsqrt));
        c1            = gem_cxmul(oma, gem_cxdiv(part1, part4));
        omb           = gem_comega(gem_cxrmul(beta, tsqrt));
        c2            = gem_cxmul(omb, gem_cxdiv(part2, part4));
        omc           = gem_comega(gem_cxrmul(gamma, tsqrt));
        c3            = gem_cxmul(omc, gem_cxdiv(part3, part4));
        c4.r          = c1.r+c2.r+c3.r;
        c4.i          = c1.i+c2.i+c3.i;
        theoryCt[i]   = c4.r;
        sumimaginary += c4.i * c4.i;
    }

    return sumimaginary;

} /* eq10v2 */

/* This returns the real-valued index(!) to an ACF, equidistant on a log scale. */
static double getLogIndex(const int i, const t_gemParams *params)
{
    return (exp(((double)(i)) * params->logQuota) -1);
}

extern t_gemParams *init_gemParams(const double sigma, const double D,
                                   const real *t, const int len, const int nFitPoints,
                                   const real begFit, const real endFit,
                                   const real ballistic, const int nBalExp, const gmx_bool bDt)
{
    double       tDelta;
    t_gemParams *p;

    snew(p, 1);

    /* A few hardcoded things here. For now anyway. */
/*   p->ka_min   = 0; */
/*   p->ka_max   = 100; */
/*   p->dka      = 10; */
/*   p->kd_min   = 0; */
/*   p->kd_max   = 2; */
/*   p->dkd      = 0.2; */
    p->ka       = 0;
    p->kd       = 0;
/*   p->lsq      = -1; */
/*   p->lifetime = 0; */
    p->sigma    = sigma*10; /* Omer uses Å, not nm */
/*   p->lsq_old  = 99999; */
    p->D        = sqcm_per_s_to_sqA_per_ps(D);
    p->kD       = 4*acos(-1.0)*sigma*p->D;

    /* Parameters used by calcsquare(). Better to calculate them
     * here than in calcsquare every time it's called. */
    p->len = len;
/*   p->logAfterTime = logAfterTime; */
    tDelta       = (t[len-1]-t[0]) / len;
    if (tDelta <= 0)
    {
        gmx_fatal(FARGS, "Time between frames is non-positive!");
    }
    else
    {
        p->tDelta = tDelta;
    }

    p->nExpFit      = nBalExp;
/*   p->nLin         = logAfterTime / tDelta; */
    p->nFitPoints   = nFitPoints;
    p->begFit       = begFit;
    p->endFit       = endFit;
    p->logQuota     = (double)(log(p->len))/(p->nFitPoints-1);
/*   if (p->nLin <= 0) { */
/*     fprintf(stderr, "Number of data points in the linear regime is non-positive!\n"); */
/*     sfree(p); */
/*     return NULL; */
/*   } */
/* We want the same number of data points in the log regime. Not crucial, but seems like a good idea. */
/* p->logDelta = log(((float)len)/p->nFitPoints) / p->nFitPoints;/\* log(((float)len)/p->nLin) / p->nLin; *\/ */
/*   p->logPF    = p->nFitPoints*p->nFitPoints/(float)len; /\* p->nLin*p->nLin/(float)len; *\/ */
/* logPF and logDelta are stitched together with the macro GETLOGINDEX defined in geminate.h */

/*   p->logMult      = pow((float)len, 1.0/nLin);/\* pow(t[len-1]-t[0], 1.0/p->nLin); *\/ */
    p->ballistic    =  ballistic;
    return p;
}

/* There was a misunderstanding regarding the fitting. From our
 * recent correspondence it appears that Omer's code require
 * the ACF data on a log-scale and does not operate on the raw data.
 * This needs to be redone in gemFunc_residual() as well as in the
 * t_gemParams structure. */
#ifdef HAVE_LIBGSL
static double gemFunc_residual2(const gsl_vector *p, void *data)
{
    gemFitData *GD;
    int         i, iLog, nLin, nFitPoints, nData;
    double      r, residual2, *ctTheory, *y;

    GD         = (gemFitData *)data;
    nLin       = GD->params->nLin;
    nFitPoints = GD->params->nFitPoints;
    nData      = GD->nData;
    residual2  = 0;
    ctTheory   = GD->ctTheory;
    y          = GD->y;

    /* Now, we'd save a lot of time by not calculating eq10v2 for all
     * time points, but only those that we sample to calculate the mse.
     */

    eq10v2(GD->ctTheory, GD->doubleLogTime /* GD->time */, nFitPoints /* GD->nData */,
           gsl_vector_get(p, 0), gsl_vector_get(p, 1),
           GD->params);

    fixGemACF(GD->ctTheory, nFitPoints);

    /* Removing a bunch of points from the log-part. */
#pragma omp parallel for schedule(dynamic) \
    firstprivate(nData, ctTheory, y, nFitPoints) \
    private (i, iLog, r) \
    reduction(+:residual2) \
    default(shared)
    for (i = 0; i < nFitPoints; i++)
    {
        iLog       = GD->logtime[i];
        r          = log(ctTheory[i /* iLog */]);
        residual2 += sqr(r-log(y[iLog]));
    }
    residual2 /= nFitPoints; /* Not really necessary for the fitting, but gives more meaning to the returned data. */
    /* printf("ka=%3.5f\tkd=%3.5f\trmse=%3.5f\n", gsl_vector_get(p,0), gsl_vector_get(p,1), residual2); */
    return residual2;

/*   for (i=0; i<nLin; i++) { */
/*     /\* Linear part ----------*\/ */
/*     r = ctTheory[i]; */
/*     residual2 += sqr(r-y[i]); */
/*     /\* Log part -------------*\/ */
/*     iLog = GETLOGINDEX(i, GD->params); */
/*     if (iLog >= nData) */
/*       gmx_fatal(FARGS, "log index out of bounds: %i", iLog); */
/*     r = ctTheory[iLog]; */
/*     residual2 += sqr(r-y[iLog]); */

/*   } */
/*   residual2 /= GD->n; */
/*   return residual2; */
}
#endif

static real* d2r(const double *d, const int nn);

extern real fitGemRecomb(double *ct, double *time, double **ctFit,
                         const int nData, t_gemParams *params)
{

    int         nThreads, i, iter, status, maxiter;
    real        size, d2, tol, *dumpdata;
    size_t      p, n;
    gemFitData *GD;
    char       *dumpstr, dumpname[128];

    /* nmsimplex2 had convergence problems prior to gsl v1.14,
     * but it's O(N) instead of O(N) operations, so let's use it if v >= 1.14 */
#ifdef HAVE_LIBGSL
    gsl_multimin_fminimizer *s;
    gsl_vector              *x, *dx; /* parameters and initial step size */
    gsl_multimin_function    fitFunc;
#ifdef GSL_MAJOR_VERSION
#ifdef GSL_MINOR_VERSION
#if ((GSL_MAJOR_VERSION == 1 && GSL_MINOR_VERSION >= 14) || \
    (GSL_MAJOR_VERSION > 1))
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
#else
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
#endif /* #if ... */
#endif /* GSL_MINOR_VERSION */
#else
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
#endif /* GSL_MAJOR_VERSION */
    fprintf(stdout, "Will fit ka and kd to the ACF according to the reversible geminate recombination model.\n");
#else  /* HAVE_LIBGSL */
    fprintf(stderr, "Sorry, can't do reversible geminate recombination without gsl. "
            "Recompile using --with-gsl.\n");
    return -1;
#endif /* HAVE_LIBGSL */

#ifdef HAVE_LIBGSL
#ifdef GMX_OPENMP
    nThreads = gmx_omp_get_max_threads();
    gmx_omp_set_num_threads(nThreads);
    fprintf(stdout, "We will be using %i threads.\n", nThreads);
#endif

    iter    = 0;
    status  = 0;
    maxiter = 100;
    tol     = 1e-10;

    p = 2;                                        /* Number of parameters to fit. ka and kd.  */
    n = params->nFitPoints; /* params->nLin*2 */; /* Number of points in the reduced dataset  */

    if (params->D <= 0)
    {
        fprintf(stderr, "Fitting of D is not implemented yet. It must be provided on the command line.\n");
        return -1;
    }

/*   if (nData<n) { */
/*     fprintf(stderr, "Reduced data set larger than the complete data set!\n"); */
/*     n=nData; */
/*   } */
    snew(dumpdata, nData);
    snew(GD, 1);

    GD->n        = n;
    GD->y        = ct;
    GD->ctTheory = NULL;
    snew(GD->ctTheory, nData);
    GD->LinLog = NULL;
    snew(GD->LinLog, n);
    GD->time   = time;
    GD->ka     = 0;
    GD->kd     = 0;
    GD->tDelta = time[1]-time[0];
    GD->nData  = nData;
    GD->params = params;
    snew(GD->logtime, params->nFitPoints);
    snew(GD->doubleLogTime, params->nFitPoints);

    for (i = 0; i < params->nFitPoints; i++)
    {
        GD->doubleLogTime[i]  = (double)(getLogIndex(i, params));
        GD->logtime[i]        = (int)(GD->doubleLogTime[i]);
        GD->doubleLogTime[i] *= GD->tDelta;

        if (GD->logtime[i] >= nData)
        {
            fprintf(stderr, "Ayay. It seems we're indexing out of bounds.\n");
            params->nFitPoints = i;
        }
    }

    fitFunc.f      = &gemFunc_residual2;
    fitFunc.n      = 2;
    fitFunc.params = (void*)GD;

    x  = gsl_vector_alloc (fitFunc.n);
    dx = gsl_vector_alloc (fitFunc.n);
    gsl_vector_set (x,  0, 25);
    gsl_vector_set (x,  1, 0.5);
    gsl_vector_set (dx, 0, 0.1);
    gsl_vector_set (dx, 1, 0.01);

    s = gsl_multimin_fminimizer_alloc (T, fitFunc.n);
    gsl_multimin_fminimizer_set (s, &fitFunc, x, dx);
    gsl_vector_free (x);
    gsl_vector_free (dx);

    do
    {
        iter++;
        status = gsl_multimin_fminimizer_iterate (s);

        if (status != 0)
        {
            gmx_fatal(FARGS, "Something went wrong in the iteration in minimizer %s:\n \"%s\"\n",
                      gsl_multimin_fminimizer_name(s), gsl_strerror(status));
        }

        d2         = gsl_multimin_fminimizer_minimum(s);
        size       = gsl_multimin_fminimizer_size(s);
        params->ka = gsl_vector_get (s->x, 0);
        params->kd = gsl_vector_get (s->x, 1);

        if (status)
        {
            fprintf(stderr, "%s\n", gsl_strerror(status));
            break;
        }

        status = gsl_multimin_test_size(size, tol);

        if (status == GSL_SUCCESS)
        {
            fprintf(stdout, "Converged to minimum at\n");
        }

        printf ("iter %5d: ka = %2.5f  kd = %2.5f  f() = %7.3f  size = %.3f  chi2 = %2.5f\n",
                iter,
                params->ka,
                params->kd,
                s->fval, size, d2);

        if (iter%1 == 0)
        {
            eq10v2(GD->ctTheory, time, nData, params->ka, params->kd, params);
            /* fixGemACF(GD->ctTheory, nFitPoints); */
            sprintf(dumpname, "Iter_%i.xvg", iter);
            for (i = 0; i < GD->nData; i++)
            {
                dumpdata[i] = (real)(GD->ctTheory[i]);
                if (!gmx_isfinite(dumpdata[i]))
                {
                    gmx_fatal(FARGS, "Non-finite value in acf.");
                }
            }
            dumpN(dumpdata, GD->nData, dumpname);
        }
    }
    while ((status == GSL_CONTINUE) && (iter < maxiter));

    /*   /\* Calculate the theoretical ACF from the parameters one last time. *\/ */
    eq10v2(GD->ctTheory, time, nData, params->ka, params->kd, params);
    *ctFit = GD->ctTheory;

    sfree(GD);
    gsl_multimin_fminimizer_free (s);

    return d2;

#endif /* HAVE_LIBGSL */
}

#ifdef HAVE_LIBGSL
static int balFunc_f(const gsl_vector *x, void *data, gsl_vector *f)
{
    /* C + sum{ A_i * exp(-B_i * t) }*/

    balData *BD;
    int      n, i, j, nexp;
    double  *y, *A, *B, C,     /* There are the parameters to be optimized. */
             t, ct;

    BD   = (balData *)data;
    n    = BD->n;
    nexp = BD->nexp;
    y    = BD->y,
    snew(A, nexp);
    snew(B, nexp);

    for (i = 0; i < nexp; i++)
    {
        A[i] = gsl_vector_get(x, i*2);
        B[i] = gsl_vector_get(x, i*2+1);
    }
    C = gsl_vector_get(x, nexp*2);

    for (i = 0; i < n; i++)
    {
        t  = i*BD->tDelta;
        ct = 0;
        for (j = 0; j < nexp; j++)
        {
            ct += A[j] * exp(B[j] * t);
        }
        ct += C;
        gsl_vector_set (f, i, ct - y[i]);
    }
    return GSL_SUCCESS;
}

/* The derivative stored in jacobian form (J)*/
static int balFunc_df(const gsl_vector *params, void *data, gsl_matrix *J)
{
    balData *BD;
    size_t   n, i, j;
    double  *y, *A, *B, C, /* There are the parameters. */
             t;
    int      nexp;

    BD   = (balData*)data;
    n    = BD->n;
    y    = BD->y;
    nexp = BD->nexp;

    snew(A, nexp);
    snew(B, nexp);

    for (i = 0; i < nexp; i++)
    {
        A[i] = gsl_vector_get(params, i*2);
        B[i] = gsl_vector_get(params, i*2+1);
    }
    C = gsl_vector_get(params, nexp*2);
    for (i = 0; i < n; i++)
    {
        t = i*BD->tDelta;
        for (j = 0; j < nexp; j++)
        {
            gsl_matrix_set (J, i, j*2,   exp(B[j]*t));        /* df(t)/dA_j */
            gsl_matrix_set (J, i, j*2+1, A[j]*t*exp(B[j]*t)); /* df(t)/dB_j */
        }
        gsl_matrix_set (J, i, nexp*2, 1);                     /* df(t)/dC */
    }
    return GSL_SUCCESS;
}

/* Calculation of the function and its derivative */
static int balFunc_fdf(const gsl_vector *params, void *data,
                       gsl_vector *f, gsl_matrix *J)
{
    balFunc_f(params, data, f);
    balFunc_df(params, data, J);
    return GSL_SUCCESS;
}
#endif /* HAVE_LIBGSL */

/* Removes the ballistic term from the beginning of the ACF,
 * just like in Omer's paper.
 */
extern void takeAwayBallistic(double *ct, double *t, int len, real tMax, int nexp, gmx_bool bDerivative)
{

    /* Use nonlinear regression with GSL instead.
     * Fit with 4 exponentials and one constant term,
     * subtract the fatest exponential. */

    int      nData, i, status, iter;
    balData *BD;
    double  *guess,           /* Initial guess. */
    *A,                       /* The fitted parameters. (A1, B1, A2, B2,... C) */
             a[2],
             ddt[2];
    gmx_bool sorted;
    size_t   n;
    size_t   p;

    nData = 0;
    do
    {
        nData++;
    }
    while (t[nData] < tMax+t[0] && nData < len);

    p = nexp*2+1;            /* Number of parameters. */

#ifdef HAVE_LIBGSL
    const gsl_multifit_fdfsolver_type *T
        = gsl_multifit_fdfsolver_lmsder;

    gsl_multifit_fdfsolver   *s;           /* The solver itself. */
    gsl_multifit_function_fdf fitFunction; /* The function to be fitted. */
    gsl_matrix               *covar;       /* Covariance matrix for the parameters.
                                            * We'll not use the result, though. */
    gsl_vector_view           theParams;

    nData = 0;
    do
    {
        nData++;
    }
    while (t[nData] < tMax+t[0] && nData < len);

    guess = NULL;
    n     = nData;

    snew(guess, p);
    snew(A, p);
    covar = gsl_matrix_alloc (p, p);

    /* Set up an initial gess for the parameters.
     * The solver is somewhat sensitive to the initial guess,
     * but this worked fine for a TIP3P box with -geminate dd
     * EDIT: In fact, this seems like a good starting pont for other watermodels too. */
    for (i = 0; i < nexp; i++)
    {
        guess[i*2]   = 0.1;
        guess[i*2+1] = -0.5 + (((double)i)/nexp - 0.5)*0.3;
    }
    guess[nexp * 2] = 0.01;

    theParams = gsl_vector_view_array(guess, p);

    snew(BD, 1);
    BD->n      = n;
    BD->y      = ct;
    BD->tDelta = t[1]-t[0];
    BD->nexp   = nexp;

    fitFunction.f      =  &balFunc_f;
    fitFunction.df     =  &balFunc_df;
    fitFunction.fdf    =  &balFunc_fdf;
    fitFunction.n      =  nData;
    fitFunction.p      =  p;
    fitFunction.params =  BD;

    s = gsl_multifit_fdfsolver_alloc (T, nData, p);
    if (s == NULL)
    {
        gmx_fatal(FARGS, "Could not set up the nonlinear solver.");
    }

    gsl_multifit_fdfsolver_set(s, &fitFunction, &theParams.vector);

    /* \=============================================/ */

    iter = 0;
    do
    {
        iter++;
        status = gsl_multifit_fdfsolver_iterate (s);

        if (status)
        {
            break;
        }
        status = gsl_multifit_test_delta (s->dx, s->x, 1e-4, 1e-4);
    }
    while (iter < 5000 && status == GSL_CONTINUE);

    if (iter == 5000)
    {
        fprintf(stderr, "The non-linear fitting did not converge in 5000 steps.\n"
                "Check the quality of the fit!\n");
    }
    else
    {
        fprintf(stderr, "Non-linear fitting of ballistic term converged in %d steps.\n\n", (int)iter);
    }
    for (i = 0; i < nexp; i++)
    {
        fprintf(stdout, "%c * exp(%c * t) + ", 'A'+(char)i*2, 'B'+(char)i*2);
    }

    fprintf(stdout, "%c\n", 'A'+(char)nexp*2);
    fprintf(stdout, "Here are the actual numbers for A-%c:\n", 'A'+nexp*2);

    for (i = 0; i < nexp; i++)
    {
        A[i*2]   = gsl_vector_get(s->x, i*2);
        A[i*2+1] = gsl_vector_get(s->x, i*2+1);
        fprintf(stdout, " %g*exp(%g * x) +", A[i*2], A[i*2+1]);
    }
    A[i*2] = gsl_vector_get(s->x, i*2);        /* The last and constant term */
    fprintf(stdout, " %g\n", A[i*2]);

    fflush(stdout);

    /* Implement some check for parameter quality */
    for (i = 0; i < nexp; i++)
    {
        if (A[i*2] < 0 || A[i*2] > 1)
        {
            fprintf(stderr, "WARNING: ----------------------------------\n"
                    " | A coefficient does not lie within [0,1].\n"
                    " | This may or may not be a problem.\n"
                    " | Double check the quality of the fit!\n");
        }
        if (A[i*2+1] > 0)
        {
            fprintf(stderr, "WARNING: ----------------------------------\n"
                    " | One factor in the exponent is positive.\n"
                    " | This could be a problem if the coefficient\n"
                    " | is large. Double check the quality of the fit!\n");
        }
    }
    if (A[i*2] < 0 || A[i*2] > 1)
    {
        fprintf(stderr, "WARNING: ----------------------------------\n"
                " | The constant term does not lie within [0,1].\n"
                " | This may or may not be a problem.\n"
                " | Double check the quality of the fit!\n");
    }

    /* Sort the terms */
    sorted = (nexp > 1) ?  FALSE : TRUE;
    while (!sorted)
    {
        sorted = TRUE;
        for (i = 0; i < nexp-1; i++)
        {
            ddt[0] = A[i*2] * A[i*2+1];
            ddt[1] = A[i*2+2] * A[i*2+3];

            if ((bDerivative && (ddt[0] < 0 && ddt[1] < 0 && ddt[0] > ddt[1])) || /* Compare derivative at t=0... */
                (!bDerivative && (A[i*2+1] > A[i*2+3])))                          /* Or just the coefficient in the exponent */
            {
                sorted = FALSE;
                a[0]   = A[i*2];   /* coefficient */
                a[1]   = A[i*2+1]; /* parameter in the exponent */

                A[i*2]   = A[i*2+2];
                A[i*2+1] = A[i*2+3];

                A[i*2+2] = a[0];
                A[i*2+3] = a[1];
            }
        }
    }

    /* Subtract the fastest component */
    fprintf(stdout, "Fastest component is %g * exp(%g * t)\n"
            "Subtracting fastest component from ACF.\n", A[0], A[1]);

    for (i = 0; i < len; i++)
    {
        ct[i] = (ct[i] - A[0] * exp(A[1] * i*BD->tDelta)) / (1-A[0]);
    }

    sfree(guess);
    sfree(A);

    gsl_multifit_fdfsolver_free(s);
    gsl_matrix_free(covar);
    fflush(stdout);

#else
    /* We have no gsl. */
    fprintf(stderr, "Sorry, can't take away ballistic component without gsl. "
            "Recompile using --with-gsl.\n");
    return;
#endif /* HAVE_LIBGSL */

}

extern void dumpN(const real *e, const int nn, char *fn)
{
    /* For debugging only */
    int   i;
    FILE *f;
    char  standardName[] = "Nt.xvg";
    if (fn == NULL)
    {
        fn = standardName;
    }

    f = fopen(fn, "w");
    fprintf(f,
            "@ type XY\n"
            "@ xaxis label \"Frame\"\n"
            "@ yaxis label \"N\"\n"
            "@ s0 line type 3\n");

    for (i = 0; i < nn; i++)
    {
        fprintf(f, "%-10i %-g\n", i, e[i]);
    }

    fclose(f);
}

static real* d2r(const double *d, const int nn)
{
    real *r;
    int   i;

    snew(r, nn);
    for (i = 0; i < nn; i++)
    {
        r[i] = (real)d[i];
    }

    return r;
}

static void _patchBad(double *ct, int n, double dy)
{
    /* Just do lin. interpolation for now. */
    int i;

    for (i = 1; i < n; i++)
    {
        ct[i] = ct[0]+i*dy;
    }
}

static void patchBadPart(double *ct, int n)
{
    _patchBad(ct, n, (ct[n] - ct[0])/n);
}

static void patchBadTail(double *ct, int n)
{
    _patchBad(ct+1, n-1, ct[1]-ct[0]);

}

extern void fixGemACF(double *ct, int len)
{
    int      i, j, b, e;
    gmx_bool bBad;

    /* Let's separate two things:
     * - identification of bad parts
     * - patching of bad parts.
     */

    b    = 0; /* Start of a bad stretch */
    e    = 0; /* End of a bad stretch */
    bBad = FALSE;

    /* An acf of binary data must be one at t=0. */
    if (abs(ct[0]-1.0) > 1e-6)
    {
        ct[0] = 1.0;
        fprintf(stderr, "|ct[0]-1.0| = %1.6d. Setting ct[0] to 1.0.\n", abs(ct[0]-1.0));
    }

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

#ifdef HAS_ISFINITE
        if (isfinite(ct[i]))
#elif defined(HAS__ISFINITE)
        if (_isfinite(ct[i]))
#else
        if (1)
#endif
        {
            if (!bBad)
            {
                /* Still on a good stretch. Proceed.*/
                continue;
            }

            /* Patch up preceding bad stretch. */
            if (i == (len-1))
            {
                /* It's the tail */
                if (b <= 1)
                {
                    gmx_fatal(FARGS, "The ACF is mostly NaN or Inf. Aborting.");
                }
                patchBadTail(&(ct[b-2]), (len-b)+1);
            }

            e = i;
            patchBadPart(&(ct[b-1]), (e-b)+1);
            bBad = FALSE;
        }
        else
        {
            if (!bBad)
            {
                b = i;

                bBad = TRUE;
            }
        }
    }
}


#include <stdio.h>
#include <cblas.h>

int
main ()
{
  int i;
  double x[] = { 1, 1, 1 };

  cblas_dscal(3, 4.323, x, 1);

  for (i = 0; i < 3; ++i)
    printf ("%f\n", x[i]);

  return 0;
}


/* C functions for running SampleTau from Cython*/

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

/*GSL includes*/
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

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

static gsl_rng *ptGSLRNG;

void c_initRNG()
{
    //const gsl_rng_type * T;

    gsl_rng_env_setup();

    //T = gsl_rng_default;
    ptGSLRNG = gsl_rng_alloc (gsl_rng_mt19937);
}

void c_setRNG(unsigned long int seed)
{
    //printf("GSL RNG initialise %lu\n",seed);
    gsl_rng_set (ptGSLRNG, seed);
}

void c_freeRNG()
{
    gsl_rng_free (ptGSLRNG);
}

void normaliseLog4(double *adLogProb)
{
    double dMax = adLogProb[0], dSum = 0.0;
    int b = 0;

    for(b = 1; b < 4; b++){
        if(adLogProb[b] > dMax){
            dMax = adLogProb[b];
        }
    }

    for(b = 0; b < 4; b++){
        adLogProb[b] = adLogProb[b] - dMax;
        dSum += exp(adLogProb[b]);
    }

    for(b = 0; b < 4; b++){
        adLogProb[b] = exp(adLogProb[b])/dSum;
        //printf("b=%d,p=%f\n",b,adLogProb[b]);
    }

    return;
}

int sample4(double *adProb, double dU){
    double adCProb[4];

    adCProb[0] = adProb[0];
    adCProb[1] = adProb[1] + adCProb[0];
    adCProb[2] = adProb[2] + adCProb[1];
    adCProb[3] = 1.0;

    if(dU < adCProb[0]){
        return 0;
    }
    else if(dU < adCProb[1])
        return 1;
    else if(dU < adCProb[2]){
        return 2;
    }
    else{
        return 3;
    }
}

int c_sample_tau (long *anTau, double* adPi, double *adEta, long* anVariants, int nV, int nG, int nS)
{
    int a = 0, b = 0;
    int g = 0, h = 0, v = 0, s = 0, t = 0;
    int nchange = 0;
    double adPSB[nS][4];
    double adPSBStore[nS][4];
    double dLogProb[4];
    int** anTauIndex = NULL;
    double u = 0.0;

    anTauIndex = (int **) malloc(nV*sizeof(int*));
    if(!anTauIndex)
        goto memoryError;

    for(v = 0; v < nV; v++){
        anTauIndex[v] = (int *) malloc(nG*sizeof(int));
        if(!anTauIndex[v])
            goto memoryError;
        for(g = 0; g < nG; g++){
            for(b = 0; b < 4; b++){
                int vIndex = v*4*nG + 4*g + b;

                if(anTau[vIndex] == 1){
                    anTauIndex[v][g] = b;
                    break;
                }
            }
            //printf("%d,%d,%d\n",v,g,anTauIndex[v][g]);
        }

    }

    //loop V positions
    for(v = 0; v < nV; v++){

        //loop G strains
        for(g = 0; g < nG; g++){

            //calc contribution from all other strains
            for(s = 0; s < nS; s++){
                for(b = 0; b < 4; b++){
                    adPSBStore[s][b] = 0.0;

                    for(h = 0; h < nG; h++){
                        if(h != g){
                            int pIndex = s*nG + h;
                            int eIndex = anTauIndex[v][h]*4 + b;

                            adPSBStore[s][b] += adEta[eIndex]*adPi[pIndex];
                        }
                    }

                }
            }

            for(a = 0; a < 4; a++){
                for(s = 0; s < nS; s++){
                    for(b = 0; b < 4; b++){
                        adPSB[s][b] = adPSBStore[s][b];

                        adPSB[s][b] += adEta[a*4 + b]*adPi[s*nG + g];
                    }
                }
                dLogProb[a] = 0.0;
                for(s = 0; s < nS; s++){
                    for(b = 0; b < 4; b++){
                        int vIndex = v*4*nS + 4*s + b;
                        double temp = ((float) anVariants[vIndex])*log(adPSB[s][b]);
                        //printf("s=%d,b=%d,cont=%f\n",s,b,temp);
                        dLogProb[a] += temp;
                    }
                }
            }
  //          printf("u=%f,p1=%f,p2=%f,p3=%f,p4 =%f\n",u,dLogProb[0],dLogProb[1],dLogProb[2],dLogProb[3]);

            normaliseLog4(dLogProb);

            u = gsl_rng_uniform (ptGSLRNG);
       //     printf("u=%f,p1=%f,p2=%f,p3=%f,p4 =%f\n",u,dLogProb[0],dLogProb[1],dLogProb[2],dLogProb[3]);
            t = sample4(dLogProb, u);
         //   printf("v=%d,g=%d,tnew=%d,told=%d\n",v,g,t,anTauIndex[v][g]);
            if(t != anTauIndex[v][g]){
                int vIndex = v*4*nG + 4*g;
                anTau[vIndex + anTauIndex[v][g]] = 0;
                anTau[vIndex + t] = 1;

                nchange++;
                anTauIndex[v][g] = t;
            }
        } //finish sampling strain g

    }//finish sampling position v

    //Free up tau indices
    for(v = 0; v < nV; v++){
        free(anTauIndex[v]);
    }
    free(anTauIndex);

    return nchange;

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


#ifndef SYNAPSE_H_
#define SYNAPSE_H_

#include "spike_train.h"
#include <gsl/gsl_rng.h>

namespace neurophys {

class DynamicSynapse {
public:
	virtual SpikeTrain filter(const SpikeTrain& in) = 0;
};

class FDSynapse: public DynamicSynapse {
public:
	static const int STATIC = 0;
	static const int DEPRESSING = 1;
	static const int FACILITATING = 2;
	static const int GENERAL = DEPRESSING | FACILITATING;
	static const int KEEP_STATE_VARIABLES = 4; // no reset between trials

	FDSynapse(double F0, double tau_f, double tau_d, double delta, int flags):
		F0_(F0), tau_f_(tau_f), tau_d_(tau_d), delta_(delta), flags_(flags),
        FC0_(0), t_(0) {};

protected:
	const double F0_;
	const double tau_f_;
	const double tau_d_;
	const double delta_;
	const int flags_;
    double FC0_;
    double t_;
};

class DeterministicFDSynapse: public FDSynapse {
public:
	DeterministicFDSynapse(double F0, double tau_f, double tau_d, double delta,
           int flags):
		FDSynapse(F0, tau_f, tau_d, delta, flags), D0_(0) {};

	virtual SpikeTrain filter(const SpikeTrain& in);
private:
    double D0_;
};

// rng in constructor is not so nice -- make explicit as param of filter and
// screw poplymorphism?
class StochasticFDSynapse: public FDSynapse {
public:
	StochasticFDSynapse(double F0, double tau_f, double tau_d, double delta,
            int flags, gsl_rng* rng, unsigned int n_release_sites):
		FDSynapse(F0, tau_f, tau_d, delta, flags), rng_(rng), n_release_sites_(n_release_sites), n_ves_present_(n_release_sites) {};

	virtual SpikeTrain filter(const SpikeTrain& in);
private:
	gsl_rng* rng_;
	const unsigned int n_release_sites_;
	unsigned int n_ves_present_;
};

}

#endif // SYNAPSE_H_


#ifndef _RMSD_H_
#define _RMSD_H_

#include <algorithm>
#include <map>
#include <cmath>
#include <iostream>
#include <vector>
#include <gsl/gsl_matrix.h>
#include "pdb.h"
#include "atom.h"
#include "align.h"
#include "contact.h"

// Indicates that, when calculating the ligand rmsd between a receptor/ligand pair
// only atoms (in the ligand) closer than 10 angstroms to any receptor atom should
// be considered
#define LIGAND_RMSD_THRESHOLD 10

/*
 * This version of the RMSD calculation method assumes that both vectors
 * are the same size and that each position in each array corresponds to
 * the actual alignment of atoms
 */
double calculate_allatom_rmsd(vector<atom>& S, vector<atom>& T);
double calculate_lrms(vector<atom>&, vector<atom>&, map<int, int>&);
double calculate_irmsd(vector<atom>&, vector<atom>&, vector<atom>&,
    vector<atom>&, map<int, int>&, map<int, int>&, int, int);
void get_fnat(vector<string>& target, vector<string>& predicted, double& fnat, double& fnon_nat);
// overloaded version that not only calculates fnat fot a target/prediction, but for each of the chains too
// according to the per-chain information given to it
void get_fnat(vector<string>& target, vector<string>& predicted, vector< vector<string> >& target_per_chain, vector< vector<string> >& predicted_per_chain,
                         double& fnat, double& fnon_nat, double* fnat_per_chain, double* fnon_nat_per_chain);
// This function takes a multiple chain target and its prediction and calculates the global c-alpha rmsd
// between them. It directly returns the total rmsd but it also returns the specific rmsd for each chain
// in the output parameter named rmsd_by_chain. The memory for this parameter should have been allocated previously
double calculate_multiple_rmsd(vector< vector<atom> >& target, vector< vector<atom> >& predicted, double* rmsd_by_chain, bool interface_only);
#endif