text:
// 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 setup_h
#define setup_h

#include <stdbool.h>
#include <string.h>
#include <petsc.h>
#include <petscdmplex.h>
#include <petscfe.h>
#include <ceed.h>
#include "qfunctions/bps/common.h"
#include "qfunctions/bps/bp1.h"
#include "qfunctions/bps/bp2.h"
#include "qfunctions/bps/bp3.h"
#include "qfunctions/bps/bp4.h"

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

// Data for PETSc Matshell
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 Matshells
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, Erestrictxi, 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
                                           };

// -----------------------------------------------------------------------------
// Boundary Conditions
// -----------------------------------------------------------------------------

// Diff boundary condition function
PetscErrorCode BCsDiff(PetscInt dim, PetscReal time, const PetscReal x[],
                       PetscInt ncompu, PetscScalar *u, void *ctx) {
  // *INDENT-OFF*
  #ifndef M_PI
  #define M_PI    3.14159265358979323846
  #endif
  // *INDENT-ON*
  const CeedScalar c[3] = { 0, 1., 2. };
  const CeedScalar k[3] = { 1., 2., 3. };

  PetscFunctionBeginUser;

  for (PetscInt i = 0; i < ncompu; i++)
    u[i] = sin(M_PI*(c[0] + k[0]*x[0])) *
           sin(M_PI*(c[1] + k[1]*x[1])) *
           sin(M_PI*(c[2] + k[2]*x[2]));

  PetscFunctionReturn(0);
}

// Mass boundary condition function
PetscErrorCode BCsMass(PetscInt dim, PetscReal time, const PetscReal x[],
                       PetscInt ncompu, PetscScalar *u, void *ctx) {
  PetscFunctionBeginUser;

  for (PetscInt i = 0; i < ncompu; i++)
    u[i] = PetscSqrtScalar(PetscSqr(x[0]) + PetscSqr(x[1]) +
                           PetscSqr(x[2]));

  PetscFunctionReturn(0);
}

// Create BC label
static PetscErrorCode CreateBCLabel(DM dm, const char name[]) {
  int ierr;
  DMLabel label;

  PetscFunctionBeginUser;

  ierr = DMCreateLabel(dm, name); CHKERRQ(ierr);
  ierr = DMGetLabel(dm, name, &label); CHKERRQ(ierr);
  ierr = DMPlexMarkBoundaryFaces(dm, 1, label); CHKERRQ(ierr);
  ierr = DMPlexLabelComplete(dm, label); CHKERRQ(ierr);

  PetscFunctionReturn(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;

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,
    .enforce_bc = false,
    .bcs_func = BCsMass
  },
  [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,
    .enforce_bc = false,
    .bcs_func = BCsMass
  },
  [CEED_BP3] = {
    .ncompu = 1,
    .qdatasize = 6,
    .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,
    .enforce_bc = true,
    .bcs_func = BCsDiff
  },
  [CEED_BP4] = {
    .ncompu = 3,
    .qdatasize = 6,
    .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,
    .enforce_bc = true,
    .bcs_func = BCsDiff
  },
  [CEED_BP5] = {
    .ncompu = 1,
    .qdatasize = 6,
    .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,
    .enforce_bc = true,
    .bcs_func = BCsDiff
  },
  [CEED_BP6] = {
    .ncompu = 3,
    .qdatasize = 6,
    .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,
    .enforce_bc = true,
    .bcs_func = BCsDiff
  }
};

// -----------------------------------------------------------------------------
// 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 = PetscDTGaussJacobiQuadrature(dim,   1, quadPointsPerEdge, -1.0, 1.0,
                                        &q); CHKERRQ(ierr);
    ierr = PetscDTGaussJacobiQuadrature(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,
                           bpType bpChoice) {
  PetscInt ierr, dim, marker_ids[1] = {1};
  PetscFE fe;

  PetscFunctionBeginUser;

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

  // Setup DM
  ierr = DMCreateDS(dm); CHKERRQ(ierr);
  if (bpOptions[bpChoice].enforce_bc) {
    PetscBool hasLabel;
    DMHasLabel(dm, "marker", &hasLabel);
    if (!hasLabel) {CreateBCLabel(dm, "marker");}
    ierr = DMAddBoundary(dm, DM_BC_ESSENTIAL, "wall", "marker", 0, 0, NULL,
                         (void(*)(void))bpOptions[bpChoice].bcs_func,
                         1, marker_ids, NULL);
    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->Erestrictxi);
  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;

  PetscFunctionBeginUser;

  // 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*P, &erestrict); CHKERRQ(ierr);
  for (c=cStart, eoffset=0; c<cEnd; c++) {
    PetscInt numindices, *indices, i;
    ierr = DMPlexGetClosureIndices(dm, section, section, c, &numindices,
                                   &indices, NULL); CHKERRQ(ierr);
    for (i=0; i<numindices; i+=ncomp) {
      for (PetscInt j=0; j<ncomp; j++) {
        if (indices[i+j] != indices[i] + (PetscInt)(copysign(j, indices[i])))
          SETERRQ1(PETSC_COMM_SELF, PETSC_ERR_ARG_INCOMP,
                   "Cell %D closure indices not interlaced", c);
      }
      // Essential boundary conditions are encoded as -(loc+1)
      PetscInt loc = indices[i] >= 0 ? indices[i] : -(indices[i] + 1);
      erestrict[eoffset++] = loc/ncomp;
    }
    ierr = DMPlexRestoreClosureIndices(dm, section, section, c, &numindices,
                                       &indices, 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*P, nnodes/ncomp, ncomp,
                            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 dim,
                                CeedInt qextra, PetscInt ncompu, PetscInt gsize,
                                PetscInt xlsize, bpType bpChoice, CeedData data,
                                PetscBool setup_rhs, CeedVector rhsceed,
                                CeedVector *target) {
  int ierr;
  DM dmcoord;
  PetscSection section;
  Vec coords;
  const PetscScalar *coordArray;
  CeedBasis basisx, basisu;
  CeedElemRestriction Erestrictx, Erestrictu, Erestrictxi,
                      Erestrictui, Erestrictqdi;
  CeedQFunction qf_setupgeo, qf_apply;
  CeedOperator op_setupgeo, op_apply;
  CeedVector xcoord, qdata, xceed, yceed;
  CeedInt qdatasize = bpOptions[bpChoice].qdatasize, ncompx = dim, P, Q,
          cStart, cEnd, nelem;

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

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

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

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

  CeedElemRestrictionCreateIdentity(ceed, nelem, Q*Q*Q, nelem*Q*Q*Q, ncompu,
                                    &Erestrictui); CHKERRQ(ierr);
  CeedElemRestrictionCreateIdentity(ceed, nelem, Q*Q*Q, nelem*Q*Q*Q,
                                    qdatasize, &Erestrictqdi); CHKERRQ(ierr);
  CeedElemRestrictionCreateIdentity(ceed, nelem, Q*Q*Q, nelem*Q*Q*Q, ncompx,
                                    &Erestrictxi); CHKERRQ(ierr);

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

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

  // 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, "dx", ncompx*dim, 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 ? dim : 1;
  CeedInt outscale = bpOptions[bpChoice].outmode==CEED_EVAL_GRAD ? dim : 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, CEED_QFUNCTION_NONE,
                     CEED_QFUNCTION_NONE, &op_setupgeo);
  CeedOperatorSetField(op_setupgeo, "dx", Erestrictx, CEED_TRANSPOSE,
                       basisx, CEED_VECTOR_ACTIVE);
  CeedOperatorSetField(op_setupgeo, "weight", Erestrictxi, CEED_NOTRANSPOSE,
                       basisx, CEED_VECTOR_NONE);
  CeedOperatorSetField(op_setupgeo, "qdata", Erestrictqdi, CEED_NOTRANSPOSE,
                       CEED_BASIS_COLLOCATED, CEED_VECTOR_ACTIVE);

  // Create the operator
  CeedOperatorCreate(ceed, qf_apply, CEED_QFUNCTION_NONE, CEED_QFUNCTION_NONE,
                     &op_apply);
  CeedOperatorSetField(op_apply, "u", Erestrictu, CEED_TRANSPOSE,
                       basisu, CEED_VECTOR_ACTIVE);
  CeedOperatorSetField(op_apply, "qdata", Erestrictqdi, CEED_NOTRANSPOSE,
                       CEED_BASIS_COLLOCATED, qdata);
  CeedOperatorSetField(op_apply, "v", Erestrictu, CEED_TRANSPOSE,
                       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", dim, CEED_EVAL_INTERP);
    CeedQFunctionAddInput(qf_setuprhs, "dx", ncompx*dim, CEED_EVAL_GRAD);
    CeedQFunctionAddInput(qf_setuprhs, "weight", 1, CEED_EVAL_WEIGHT);
    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, CEED_QFUNCTION_NONE,
                       CEED_QFUNCTION_NONE, &op_setuprhs);
    CeedOperatorSetField(op_setuprhs, "x", Erestrictx, CEED_TRANSPOSE,
                         basisx, CEED_VECTOR_ACTIVE);
    CeedOperatorSetField(op_setuprhs, "dx", Erestrictx, CEED_TRANSPOSE,
                         basisx, CEED_VECTOR_ACTIVE);
    CeedOperatorSetField(op_setuprhs, "weight", Erestrictxi, CEED_NOTRANSPOSE,
                         basisx, CEED_VECTOR_NONE);
    CeedOperatorSetField(op_setuprhs, "true_soln", Erestrictui, CEED_NOTRANSPOSE,
                         CEED_BASIS_COLLOCATED, *target);
    CeedOperatorSetField(op_setuprhs, "rhs", Erestrictu, CEED_TRANSPOSE,
                         basisu, CEED_VECTOR_ACTIVE);

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

    // 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->Erestrictxi = Erestrictxi;
  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, CEED_QFUNCTION_NONE,
                       CEED_QFUNCTION_NONE, &op_restrict);
    CeedOperatorSetField(op_restrict, "input", data[i]->Erestrictu,
                         CEED_NOTRANSPOSE, CEED_BASIS_COLLOCATED,
                         CEED_VECTOR_ACTIVE);
    CeedOperatorSetField(op_restrict, "output", data[i-1]->Erestrictu,
                         CEED_TRANSPOSE, 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, CEED_QFUNCTION_NONE,
                       CEED_QFUNCTION_NONE, &op_interp);
    CeedOperatorSetField(op_interp, "input", data[i-1]->Erestrictu,
                         CEED_NOTRANSPOSE, basisctof, CEED_VECTOR_ACTIVE);
    CeedOperatorSetField(op_interp, "output", data[i]->Erestrictu,
                         CEED_TRANSPOSE, 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);
  CeedVectorSyncArray(user->yceed, CEED_MEM_HOST);

  // Restore PETSc vectors
  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);
  CeedVectorSyncArray(user->ceedvecf, CEED_MEM_HOST);

  // Restore PETSc vectors
  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);
  CeedVectorSyncArray(user->ceedvecc, CEED_MEM_HOST);

  // Restore PETSc vectors
  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

max_stars_repo_name: jrwrigh/libCEED

max_issues_repo_name: jrwrigh/libCEED

max_forks_repo_name: jrwrigh/libCEED


text:
#pragma once

#include <nextalign/nextalign.h>

#include <gsl/string_span>

template<typename Letter>
using SequenceSpan = gsl::basic_string_span<Letter, gsl::dynamic_extent>;

template<typename Letter>
struct InsertionInternal {
  int begin;
  int end;
  Sequence<Letter> seq;
};

struct PeptideInternal {
  std::string name;
  AminoacidSequence seq;
  std::vector<InsertionInternal<Aminoacid>> insertions;
};

ext: h

max_stars_repo_name: davidcroll/nextclade

max_issues_repo_name: davidcroll/nextclade

max_forks_repo_name: davidcroll/nextclade


text:
#ifndef IMAT_MATRIX_H
#define IMAT_MATRIX_H

#include <cstdarg>
#include <cstdlib>

#include <fstream>
#include <sstream>

#ifdef GSL
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort_double.h>
#endif // GSL

//#include "split_str.h"
#include "vector.h"
#include "base.h"

namespace imat {

enum Dim { kByRow = 0, kByCol = 1 };  // Choose dimention for Min, Max, Mean
                                      // Sum, Covariance and Sort

// Matrix Class:
// Provide a C++ Matrix class, whoes data can be exposed to C numeric libraries
// (eg. GSL lib).
template <class T>
class Matrix {
  // Following functions are not as efficient as operate_and_assign (+=, -= and
  // *=). Try to use operate_and_assign instead of these functions whenever
  // possible.
  template <class TT>
  friend Matrix<TT> operator- (const Matrix<TT> &lfs, const Matrix<TT> &rhs);
  template <class TT>
  friend Matrix<TT> operator+ (const Matrix<TT> &lfs, const Matrix<TT> &rhs);
  template <class TT>
  friend Matrix<TT> operator* (const Matrix<TT> &lfs, const Matrix<TT> &rhs);
  template <class TT>
  friend Matrix<TT> operator* (const Matrix<TT> &mat, const TT &x);
  template <class TT>
  friend Matrix<TT> operator* (const TT &x, const Matrix<TT> &mat);
  //
  // Cross Product
  template <class TT>
  friend void Cross(Matrix<TT> *res, const Matrix<TT> &a, const Matrix<TT> &b);

 public:
  Matrix() : rows_(0), cols_(0), tda_(0), data_(NULL), owner_(false) {}
  ~Matrix()  { if (owner_) delete []data_; }
  Matrix(isize_t rows, isize_t cols);
  Matrix(const Matrix <T> &source);
  Matrix<T> & operator= (const Matrix<T> &source);
  ListInitializer<T> operator= (const T &value);
  void Allocate(isize_t rows, isize_t cols); // for empty matrix only
  // Matrix views: can only be created by mat.View(...)
  // View raw data
  void View(T *src, isize_t rows, isize_t cols, isize_t tda=kDefault);
  // View submatrix: const and non-const version
  void View(Matrix<T> *src,
            isize_t row_0,         isize_t col_0,
            isize_t rows=kDefault, isize_t cols=kDefault);
  void View(const Matrix<T> &src,
            isize_t row_0,         isize_t col_0,
            isize_t rows=kDefault, isize_t cols=kDefault);
  // Copy elements from a submatrix of src
  void SubCopy(const Matrix<T> &src,
               isize_t row_0=kDefault, isize_t col_0=kDefault,
               isize_t rows=kDefault,  isize_t cols=kDefault);

  T &       operator() (isize_t i, isize_t j);
  const T & operator() (isize_t i, isize_t j) const;
  Matrix<T> operator-() const;
  bool operator== (const Matrix<T> &other) const;
  void operator+= (const Matrix<T> &other);
  void operator-= (const Matrix<T> &other);
  void operator*= (const Matrix<T> &other);
  void operator+= (const T &x);
  void operator-= (const T &x);
  void operator*= (const T &x);

  void SetZero();           // Set all elements as 0
  void SetAll(const T &x);  // Set all elements as x
  void SetUnit();           // Set square matrix as unit matrix
  T Sum() const;            // Sumation of all elements
  T Min() const;            // Minimum of all elements
  T Max() const;            // Maximum of all elements
  double Mean() const;      // Mean value of all elements
  void GetRow(Vector<T> *row_view, isize_t i);
  void GetCol(Vector<T> *col_view, isize_t i);
  void Trans();

  // Following functions are designed to operate on rows or cols:
  // eg. A = [[ 1,  2,  3],
  //          [ 4,  5,  6],
  //          [ 7,  8,  9],
  //          [10, 11, 12]]
  // A.Sum(kByRow) ==> [6, 15, 24, 33]
  // A.Sum(kByCol) ==> [22, 26, 30]
  //
  Vector<T> Sum(Dim dimention) const;
  Vector<T> Min(Dim dimention) const;
  Vector<T> Max(Dim dimention) const;
  Vector<double> Mean(Dim dimention) const;
  void Norm(Dim dimention=kByRow);            // Only for T==double

#ifdef GSL
  // Get covariance matrix of data seris stored in cols or rows
  Matrix<T> Cov(Dim dimention=kByCol) const;  // Only for T==double
  void Sort(Dim dimention=kByRow, int key=0); // Only for T==double
  void Eigen(Vector<T> *eigen_value, Matrix<T> *eigen_vector); // Only for T==double
#endif // GSL

  void Print() const;
  void Readf(const char *filename);
  void Printf(const char *filename) const;

  bool     empty() const { return (rows_ == 0 or cols_ == 0); }
  isize_t  rows()  const { return rows_;                      }
  isize_t  cols()  const { return cols_;                      }
  isize_t  tda()   const { return tda_;                       }
  T*       data()        { return data_;                      }
  const T* data()  const { return data_;                      }
  void     clear();

 protected:
  bool is_view_() const { return (data_ != NULL and owner_ == false); }
  void copy_(const Matrix<T> &source);
  void allocate_(isize_t rows, isize_t cols);

  isize_t rows_;
  isize_t cols_;
  isize_t tda_;
  T *data_;
  bool owner_;
};

typedef Matrix<int> MatrixI;
typedef Matrix<double> MatrixD;

#include "matrix_inl.h"

} // namespace imat
#endif // IMAT_MATRIX_H

max_stars_repo_name: hongjunbai/imat

max_issues_repo_name: hongjunbai/imat

max_forks_repo_name: hongjunbai/imat


text:
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "ccl.h"

ccl_cl_tracer_collection_t *ccl_cl_tracer_collection_t_new(int *status) {
  ccl_cl_tracer_collection_t *trc = NULL;
  trc = malloc(sizeof(ccl_cl_tracer_collection_t));
  if (trc == NULL)
    *status = CCL_ERROR_MEMORY;

  if (*status == 0) {
    trc->n_tracers = 0;
    // Currently CCL_MAX_TRACERS_PER_COLLECTION is hard-coded to 100.
    // It should be enough for any practical application with minimal memory overhead
    trc->ts = malloc(CCL_MAX_TRACERS_PER_COLLECTION*sizeof(ccl_cl_tracer_t *));
    if (trc->ts == NULL) {
      *status = CCL_ERROR_MEMORY;
      free(trc);
      trc = NULL;
    }
  }

  return trc;
}

void ccl_cl_tracer_collection_t_free(ccl_cl_tracer_collection_t *trc) {
  if (trc != NULL) {
    if (trc->ts != NULL)
      free(trc->ts);
    free(trc);
  }
}

void ccl_add_cl_tracer_to_collection(ccl_cl_tracer_collection_t *trc,
                                     ccl_cl_tracer_t *tr, int *status) {
  if (trc->n_tracers >= CCL_MAX_TRACERS_PER_COLLECTION) {
    *status = CCL_ERROR_MEMORY;
    return;
  }
  trc->ts[trc->n_tracers] = tr;
  trc->n_tracers++;
}

// Takes an array of z-dependent numbers and the corresponding z values
// and returns an array of a values and the corresponding a-dependent values.
// The order of the original arrays is assumed to be ascending in z, and
// the order of the returned arrays is swapped (so it has ascending a).
static void from_z_to_a(ccl_cosmology *cosmo,
                        int nz, double *z_arr, double *fz_arr,
                        double **a_arr, double **fa_arr, int *status) {
  *a_arr = NULL;
  *fa_arr = NULL;
  *a_arr = malloc(nz*sizeof(double));
  *fa_arr = malloc(nz*sizeof(double));

  if ((a_arr == NULL) || (fa_arr == NULL)) {
    *status = CCL_ERROR_MEMORY;
    free(*a_arr);
    free(*fa_arr);
    ccl_cosmology_set_status_message(
      cosmo, "ccl_tracers.c: from_z_to_a(): memory allocation error\n");
  }

  if (*status == 0) {
    int ia;
    // Populate array of scale factors in reverse order
    for (ia=0; ia < nz; ia++) {
      (*a_arr)[ia] = 1./(1+z_arr[nz-1-ia]);
      (*fa_arr)[ia] = fz_arr[nz-1-ia];
    }
  }
}

//Integrand for N(z) integrator
static double nz_integrand(double z, void *pars) {
  ccl_f1d_t *nz_f = (ccl_f1d_t *)pars;

  return ccl_f1d_t_eval(nz_f,z);
}

// Gets area of N(z) curve
static double get_nz_norm(ccl_cosmology *cosmo, ccl_f1d_t *nz_f,
                          double z0, double zf, int *status) {
  double nz_norm = -1, nz_enorm;

  // Get N(z) norm
  gsl_function F;
  gsl_integration_workspace *w = NULL;
  F.function = &nz_integrand;
  F.params = nz_f;

  w = gsl_integration_workspace_alloc(cosmo->gsl_params.N_ITERATION);

  if (w == NULL) {
    *status = CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(
      cosmo, "ccl_tracers.c: get_nz_norm(): out of memory");
  }
  else {
    int gslstatus = gsl_integration_qag(
      &F, z0, zf, 0,
      cosmo->gsl_params.INTEGRATION_EPSREL,
      cosmo->gsl_params.N_ITERATION,
      cosmo->gsl_params.INTEGRATION_GAUSS_KRONROD_POINTS,
      w, &nz_norm, &nz_enorm);

    if (gslstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_tracers.c: get_nz_norm():");
      *status = CCL_ERROR_INTEG;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_tracers.c: get_nz_norm(): "
        "integration error when normalizing N(z)\n");
    }
  }

  gsl_integration_workspace_free(w);

  return nz_norm;
}

static void from_z_to_chi(ccl_cosmology *cosmo, int nz, double *z_arr,
                          double *chi_arr, int *status) {
  for (int ichi=0; ichi < nz; ichi++)
    chi_arr[ichi] = ccl_comoving_radial_distance(cosmo, 1./(1+z_arr[ichi]), status);
}

void ccl_get_number_counts_kernel(ccl_cosmology *cosmo,
                                  int nz, double *z_arr, double *nz_arr,
                                  int normalize_nz,
                                  double *pchi_arr, int *status) {
  // Returns dn/dchi normalized to unit area from an unnormalized dn/dz.
  // Prepare N(z) spline
  ccl_f1d_t *nz_f = NULL;

  nz_f = ccl_f1d_t_new(nz, z_arr, nz_arr, 0, 0);
  if (nz_f == NULL) {
    *status = CCL_ERROR_SPLINE;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_tracers.c: ccl_get_number_counts_kernel: "
      "error initializing spline\n");
  }

  // Get N(z) normalization
  double i_nz_norm = -1;
  if (*status == 0) {
    if (normalize_nz)
      i_nz_norm = 1./get_nz_norm(cosmo, nz_f, z_arr[0], z_arr[nz-1], status);
    else
      i_nz_norm = 1;
  }

  if (*status == 0) {
    // Populate arrays
    for(int ichi=0; ichi < nz; ichi++) {
      double a = 1./(1+z_arr[ichi]);
      double h = cosmo->params.h*ccl_h_over_h0(cosmo,a,status)/ccl_constants.CLIGHT_HMPC;
      // H(z) * dN/dz * 1/Ngal
      pchi_arr[ichi] = h*nz_arr[ichi]*i_nz_norm;
    }
  }

  ccl_f1d_t_free(nz_f);
}

//3 H0^2 Omega_M / 2
static double get_lensing_prefactor(ccl_cosmology *cosmo,int *status) {
  double hub = cosmo->params.h/ccl_constants.CLIGHT_HMPC;
  return 1.5*hub*hub*cosmo->params.Omega_m;
}

typedef struct {
  ccl_cosmology *cosmo;
  double z_max;
  double z_end;
  double chi_end;
  double i_nz_norm;
  ccl_f1d_t *nz_f;
  ccl_f1d_t *sz_f;
  int *status;
} integ_lensing_pars;

// Integrand for lensing kernel.
// Returns N(z) * (1 - 5*s(z)/2) * (chi(z)-chi) / chi(z)
static double lensing_kernel_integrand(double z, void *pars) {
  integ_lensing_pars *p = (integ_lensing_pars *)pars;
  double pz = ccl_f1d_t_eval(p->nz_f, z);
  double qz;
  if (p->sz_f == NULL) // No magnification factor
    qz = 1;
  else // With magnification factor
    qz = (1 - 2.5*ccl_f1d_t_eval(p->sz_f, z));

  if (z == 0)
    return pz * qz;
  else {
    double chi = ccl_comoving_radial_distance(p->cosmo, 1./(1+z), p->status);
    return (
      pz * qz *
      ccl_sinn(p->cosmo, chi-p->chi_end, p->status) /
      ccl_sinn(p->cosmo, chi, p->status));
  }
}

// Returns
// Integral[ p(z) * (1-5s(z)/2) * chi_end * (chi(z)-chi_end)/chi(z) , {z',z_end,z_max} ]
static double lensing_kernel_integrate(ccl_cosmology *cosmo,
                                       integ_lensing_pars *pars,
                                       gsl_integration_workspace *w) {
  int gslstatus = 0;
  double result, eresult;
  gsl_function F;
  F.function = &lensing_kernel_integrand;
  F.params = pars;
  gslstatus = gsl_integration_qag(
    &F, pars->z_end, pars->z_max, 0,
    cosmo->gsl_params.INTEGRATION_EPSREL,
    cosmo->gsl_params.N_ITERATION,
    cosmo->gsl_params.INTEGRATION_GAUSS_KRONROD_POINTS,
    w, &result, &eresult);

  if ((gslstatus != GSL_SUCCESS) || (*(pars->status))) {
    ccl_raise_gsl_warning(gslstatus, "ccl_tracers.c: lensing_kernel_integrate():");
    return -1;
  }

  return result * pars->i_nz_norm * pars->chi_end;
}

//Returns number of divisions on which
//the lensing kernel should be calculated
int ccl_get_nchi_lensing_kernel(int nz, double *z_arr, int *status) {
  int nchi;
  double dz = -1, z_max = -1;
  //Compute redshift step
  dz = (z_arr[nz-1]-z_arr[0])/(nz-1);

  //How many steps to z=0?
  return (int)(z_arr[nz-1]/dz+0.5);
}

//Return array with the values of chi at
//the which the lensing kernel will be
//calculated.
void ccl_get_chis_lensing_kernel(ccl_cosmology *cosmo,
                                 int nchi, double z_max,
                                 double *chis, int *status) {
  double dz = z_max/nchi;
  for(int ichi=0; ichi < nchi; ichi++) {
    double z = dz*ichi+1E-15;
    double a = 1./(1+z);
    chis[ichi] = ccl_comoving_radial_distance(cosmo, a, status);
  }
}

//Returns array with lensing kernel:
//3 * H0^2 * Omega_M / 2 / a *
// Integral[ p(z) * (1-5s(z)/2) * chi_end * (chi(z)-chi_end)/chi(z) ,
//          {z',z_end,z_max} ]
void ccl_get_lensing_mag_kernel(ccl_cosmology *cosmo,
                                int nz, double *z_arr, double *nz_arr,
                                int normalize_nz, double z_max,
                                int nz_s, double *zs_arr, double *sz_arr,
                                int nchi, double *chi_arr, double *wL_arr,
                                int *status) {
  ccl_f1d_t *nz_f = NULL;
  ccl_f1d_t *sz_f = NULL;
  gsl_integration_workspace *w;
  integ_lensing_pars *ipar;
  int local_status;

  // Prepare N(z) spline
  nz_f = ccl_f1d_t_new(nz, z_arr, nz_arr, 0, 0);
  if (nz_f == NULL) {
    *status = CCL_ERROR_SPLINE;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_tracers.c: get_lensing_mag_kernel: error initializing spline\n");
  }

  // Get N(z) normalization
  double i_nz_norm = -1;
  if (*status == 0) {
    if (normalize_nz)
      i_nz_norm = 1./get_nz_norm(cosmo, nz_f, z_arr[0], z_arr[nz-1], status);
    else
      i_nz_norm = 1.;
  }

  // Prepare magnification bias spline if needed
  if (*status == 0) {
    if ((nz_s > 0) && (zs_arr != NULL) && (sz_arr != NULL)) {
      sz_f = ccl_f1d_t_new(nz_s, zs_arr, sz_arr, sz_arr[0], sz_arr[nz_s-1]);
      if (sz_f == NULL) {
        *status = CCL_ERROR_SPLINE;
        ccl_cosmology_set_status_message(
          cosmo,
          "ccl_tracers.c: get_lensing_mag_kernel: error initializing spline\n");
      }
    }
  }

  double lens_prefac = get_lensing_prefactor(cosmo, status);
  double chi, a, z, mgfac;
  int ichi;

  w = NULL;
  ipar = NULL;
  local_status = *status;

  if (local_status == 0) {
    ipar = malloc(sizeof(integ_lensing_pars));
    w = gsl_integration_workspace_alloc(cosmo->gsl_params.N_ITERATION);

    if ((ipar == NULL) || (w == NULL)) {
      local_status = CCL_ERROR_MEMORY;
    }
  }

  if (local_status == 0) {
    ipar->cosmo = cosmo;
    ipar->z_max = z_max;
    ipar->i_nz_norm = i_nz_norm;
    ipar->sz_f = sz_f;
    ipar->nz_f = nz_f;
    ipar->status = &local_status;
  }

  //Populate arrays
  for (ichi=0; ichi < nchi; ichi++) {
    if (local_status == 0) {
      chi = chi_arr[ichi];
      a = ccl_scale_factor_of_chi(cosmo, chi, &local_status);
      z = 1./a-1;
      // Add MG correction if needed
      mgfac = 1.0;
      if (fabs(cosmo->params.sigma_0))
        mgfac += ccl_Sig_MG(cosmo, a, &local_status);
      ipar->z_end = z;
      ipar->chi_end = chi;

      wL_arr[ichi] = lensing_kernel_integrate(cosmo, ipar, w)*(1+z)*lens_prefac*mgfac;
    } else {
      wL_arr[ichi] = NAN;
    }
  }

  gsl_integration_workspace_free(w);
  free(ipar);

  if (local_status != 0) {
    *status = CCL_ERROR_INTEG;
  }

  ccl_f1d_t_free(nz_f);
  ccl_f1d_t_free(sz_f);
}

// Returns kernel for CMB lensing
// 3H0^2Om/2 * chi * (chi_s - chi) / chi_s / a
void ccl_get_kappa_kernel(ccl_cosmology *cosmo, double chi_source,
                          int nchi, double *chi_arr,
                          double *wchi, int *status) {
  double lens_prefac = get_lensing_prefactor(cosmo, status) / ccl_sinn(cosmo, chi_source, status);

  for (int ichi=0; ichi < nchi; ichi++) {
    double chi = chi_arr[ichi];
    double a = ccl_scale_factor_of_chi(cosmo, chi, status);
    double mgfac = 1;
    // Add MG correction if needed
    if (fabs(cosmo->params.sigma_0))
      mgfac += ccl_Sig_MG(cosmo, a, status);
    wchi[ichi] = lens_prefac*(ccl_sinn(cosmo,chi_source-chi,status))*chi*mgfac/a;
  }
}

ccl_cl_tracer_t *ccl_cl_tracer_t_new(ccl_cosmology *cosmo,
                                     int der_bessel,
                                     int der_angles,
                                     int n_w, double *chi_w, double *w_w,
                                     int na_ka, double *a_ka,
                                     int nk_ka, double *lk_ka,
                                     double *fka_arr,
                                     double *fk_arr,
                                     double *fa_arr,
                                     int is_fka_log,
                                     int is_factorizable,
                                     int extrap_order_lok,
                                     int extrap_order_hik,
                                     int *status) {
  ccl_cl_tracer_t *tr = NULL;

  // Check der_bessel and der_angles are sensible
  if ((der_angles < 0) || (der_angles > 2)) {
    *status = CCL_ERROR_INCONSISTENT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_tracers.c: ccl_cl_tracer_new: der_angles must be between 0 and 2\n");
  }
  if ((der_bessel < -1) || (der_bessel > 2)) {
    *status = CCL_ERROR_INCONSISTENT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_tracers.c: ccl_cl_tracer_new: der_bessel must be between -1 and 2\n");
  }

  if (*status == 0) {
    tr = malloc(sizeof(ccl_cl_tracer_t));
    if (tr == NULL)
      *status = CCL_ERROR_MEMORY;
  }

  // Initialize everythin
  if (*status == 0) {
    tr->der_angles = der_angles;
    tr->der_bessel = der_bessel;
    tr->kernel = NULL; // Initialize these to NULL
    tr->transfer = NULL; // Initialize these to NULL
    tr->chi_min = 0;
    tr->chi_max = 1E15;
  }

  if (*status == 0) {
    // Initialize radial kernel
    if ((n_w > 0) && (chi_w != NULL) && (w_w != NULL)) {
      tr->kernel = ccl_f1d_t_new(n_w,chi_w,w_w,0,0);
      if (tr->kernel == NULL)
        *status=CCL_ERROR_MEMORY;
    }
  }

  // Find kernel edges
  if (*status == 0) {
    // If no radial kernel, set limits to zero and maximum distance
    if (tr->kernel == NULL) {
      tr->chi_min = 0;
      tr->chi_max = ccl_comoving_radial_distance(cosmo, cosmo->spline_params.A_SPLINE_MIN, status);
    }
    else {
      int ichi;
      double w_max = fabs(w_w[0]);

      // Find maximum of radial kernel
      for (ichi=0; ichi < n_w; ichi++) {
        if (fabs(w_w[ichi]) >= w_max)
          w_max = fabs(w_w[ichi]);
      }

      // Multiply by fraction
      w_max *= CCL_FRAC_RELEVANT;

      // Initialize as the original edges in case we don't find an interval
      tr->chi_min = chi_w[0];
      tr->chi_max = chi_w[n_w-1];

      // Find minimum
      for (ichi=0; ichi < n_w; ichi++) {
        if (fabs(w_w[ichi]) >= w_max) {
          tr->chi_min = chi_w[ichi];
          break;
        }
      }

      // Find maximum
      for (ichi=n_w-1; ichi >= 0; ichi--) {
        if (fabs(w_w[ichi]) >= w_max) {
          tr->chi_max = chi_w[ichi];
          break;
        }
      }
    }
  }

  if (*status == 0) {
    if ((fka_arr != NULL) || (fk_arr != NULL) || (fa_arr != NULL)) {
      tr->transfer = ccl_f2d_t_new(
        na_ka,a_ka, // na, a_arr
        nk_ka,lk_ka, // nk, lk_arr
        fka_arr, // fka_arr
        fk_arr, // fk_arr
        fa_arr, // fa_arr
        is_factorizable, // is factorizable
        extrap_order_lok, // extrap_order_lok
        extrap_order_hik, // extrap_order_hik
        ccl_f2d_constantgrowth, // extrap_linear_growth
        is_fka_log, // is_fka_log
        NULL, // growth (function)
        1, // growth_factor_0 -> will assume constant transfer function
        0, // growth_exponent
        ccl_f2d_3, // interp_type
        status);
      if (tr->transfer == NULL)
        *status=CCL_ERROR_MEMORY;
    }
  }

  return tr;
}

void ccl_cl_tracer_t_free(ccl_cl_tracer_t *tr) {
  if (tr != NULL) {
    if (tr->transfer != NULL)
      ccl_f2d_t_free(tr->transfer);
    if (tr->kernel != NULL)
      ccl_f1d_t_free(tr->kernel);
    free(tr);
  }
}

double ccl_cl_tracer_t_get_f_ell(ccl_cl_tracer_t *tr, double ell, int *status) {
  if (tr != NULL) {
    if (tr->der_angles == 1)
      return ell*(ell+1.);
    else if (tr->der_angles == 2) {
      if (ell <= 1) // This is identically 0
        return 0;
      else if (ell <= 10) // Use full expression in this case
        return sqrt((ell+2)*(ell+1)*ell*(ell-1));
      else {
        double lp1h = ell+0.5;
        double lp1h2 = lp1h*lp1h;
        if (ell <= 1000)  // This is accurate to 5E-5 for l>10
          return lp1h2*(1-1.25/lp1h2);
        else // This is accurate to 1E-6 for l>1000
          return lp1h2;
      }
    }
    else
      return 1;
  }
  else
    return 1;
}

double ccl_cl_tracer_t_get_kernel(ccl_cl_tracer_t *tr, double chi, int *status) {
  if (tr != NULL) {
    if (tr->kernel != NULL)
      return ccl_f1d_t_eval(tr->kernel, chi);
    else
      return 1;
  }
  else
    return 1;
}

double ccl_cl_tracer_t_get_transfer(ccl_cl_tracer_t *tr,
                                    double lk, double a, int *status) {
  if (tr != NULL) {
    if (tr->transfer != NULL)
      return ccl_f2d_t_eval(tr->transfer, lk, a, NULL, status);
    else
      return 1;
  }
  else
    return 1;
}

ext: c

max_stars_repo_name: benediktdiemer/CCL

max_issues_repo_name: benediktdiemer/CCL

max_forks_repo_name: benediktdiemer/CCL


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

/* A variable-coefficient linear multistep backward differentiation
   formula (BDF) method in Nordsieck form. This stepper uses the
   explicit BDF formula as predictor and implicit BDF formula as
   corrector. A modified Newton iteration method is used to
   solve the system of non-linear equations. Method order varies
   dynamically between 1 and 5.

   References:

   Byrne, G. D., and Hindmarsh, A. C., A Polyalgorithm for the
   Numerical Solution of Ordinary Differential Equations,
   ACM Trans. Math. Software, 1 (1975), pp. 71-96.

   Brown, P. N., Byrne, G. D., and Hindmarsh, A. C., VODE: A
   Variable-coefficient ODE Solver, SIAM J. Sci. Stat. Comput. 10,
   (1989), pp. 1038-1051.

   Hindmarsh, A. C., Brown, P. N., Grant, K. E., Lee, S. L., Serban,
   R., Shumaker, D. E., and Woodward, C. S., SUNDIALS: Suite of
   Nonlinear and Differential/Algebraic Equation Solvers, ACM
   Trans. Math. Software 31 (2005), pp. 363-396.

   Note: The algorithms have been adapted for GSL ode-initval2
   framework.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

#include "odeiv_util.h"

/* Maximum order of BDF methods */
#define MSBDF_MAX_ORD 5

/* Steps until Jacobian evaluation is forced */
#define MSBDF_JAC_WAIT 50

/* Steps until iteration matrix M evaluation is forced */
#define MSBDF_M_WAIT 20

typedef struct
{
  /* Nordsieck history matrix. Includes concatenated
     Nordsieck vectors [y_n, h*y_n', (h^2/2!)*y_n'', ...,
     (h^ord/ord!)*d^(ord)(y_n)]. Nordsieck vector number i is located
     at z[i*dim] (i=0..ord).
   */
  double *z;

  double *zbackup;              /* backup of Nordsieck matrix */
  double *ytmp;                 /* work area */
  double *ytmp2;                /* work area */
  double *l;                    /* polynomial coefficients */
  double *hprev;                /* previous step sizes */
  double *hprevbackup;          /* backup of hprev */
  size_t *ordprev;              /* orders of previous calls */
  size_t *ordprevbackup;        /* backup of ordprev */
  double *errlev;               /* desired error level of y */
  gsl_vector *abscor;           /* absolute y values for correction */
  gsl_vector *relcor;           /* relative y values for correction */
  gsl_vector *svec;             /* saved abscor & work area */
  gsl_vector *tempvec;          /* work area */
  const gsl_odeiv2_driver *driver;      /* pointer to gsl_odeiv2_driver object */
  gsl_matrix *dfdy;             /* Jacobian */
  double *dfdt;                 /* storage for time derivative of f */
  gsl_matrix *M;                /* Newton iteration matrix */
  gsl_permutation *p;           /* permutation for LU decomposition of M */
  gsl_vector *rhs;              /* right hand side equations (-G) */

  long int ni;                  /* stepper call counter */
  size_t ord;                   /* current order of method */
  double tprev;                 /* t point of previous call */
  size_t ordwait;               /* counter for order change */
  size_t ordwaitbackup;         /* backup of ordwait */
  size_t failord;               /* order of convergence failure */
  double failt;                 /* t point of convergence failure */
  double ordp1coeffprev;        /* saved order coefficient */
  size_t nJ;                    /* step counter for Jacobian evaluation */
  size_t nM;                    /* step counter for update of M */
  double gammaprev;             /* gamma of previous call */
  double gammaprevbackup;       /* backup of gammaprev */
  size_t failcount;             /* counter for rejected steps */
}
msbdf_state_t;

/* Introduce msbdf_reset for use in msbdf_alloc and _apply */

static int msbdf_reset (void *, size_t);

static void *
msbdf_alloc (size_t dim)
{
  msbdf_state_t *state = (msbdf_state_t *) malloc (sizeof (msbdf_state_t));

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

  state->z = (double *) malloc ((MSBDF_MAX_ORD + 1) * dim * sizeof (double));

  if (state->z == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for z", GSL_ENOMEM);
    }

  state->zbackup =
    (double *) malloc ((MSBDF_MAX_ORD + 1) * dim * sizeof (double));

  if (state->zbackup == 0)
    {
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for zbackup", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->ytmp2 = (double *) malloc (dim * sizeof (double));

  if (state->ytmp2 == 0)
    {
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp2", GSL_ENOMEM);
    }

  state->l = (double *) malloc ((MSBDF_MAX_ORD + 1) * sizeof (double));

  if (state->l == 0)
    {
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for l", GSL_ENOMEM);
    }

  state->hprev = (double *) malloc (MSBDF_MAX_ORD * sizeof (double));

  if (state->hprev == 0)
    {
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprev", GSL_ENOMEM);
    }

  state->hprevbackup = (double *) malloc (MSBDF_MAX_ORD * sizeof (double));

  if (state->hprevbackup == 0)
    {
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprevbackup", GSL_ENOMEM);
    }

  state->ordprev = (size_t *) malloc (MSBDF_MAX_ORD * sizeof (size_t));

  if (state->ordprev == 0)
    {
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ordprev", GSL_ENOMEM);
    }

  state->ordprevbackup = (size_t *) malloc (MSBDF_MAX_ORD * sizeof (size_t));

  if (state->ordprevbackup == 0)
    {
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ordprevbackup",
                      GSL_ENOMEM);
    }

  state->errlev = (double *) malloc (dim * sizeof (double));

  if (state->errlev == 0)
    {
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for errlev", GSL_ENOMEM);
    }

  state->abscor = gsl_vector_alloc (dim);

  if (state->abscor == 0)
    {
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for abscor", GSL_ENOMEM);
    }

  state->relcor = gsl_vector_alloc (dim);

  if (state->relcor == 0)
    {
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for relcor", GSL_ENOMEM);
    }

  state->svec = gsl_vector_alloc (dim);

  if (state->svec == 0)
    {
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for svec", GSL_ENOMEM);
    }

  state->tempvec = gsl_vector_alloc (dim);

  if (state->tempvec == 0)
    {
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for tempvec", GSL_ENOMEM);
    }

  state->dfdy = gsl_matrix_alloc (dim, dim);

  if (state->dfdy == 0)
    {
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdy", GSL_ENOMEM);
    }

  state->dfdt = (double *) malloc (dim * sizeof (double));

  if (state->dfdt == 0)
    {
      gsl_matrix_free (state->dfdy);
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdt", GSL_ENOMEM);
    }

  state->M = gsl_matrix_alloc (dim, dim);

  if (state->M == 0)
    {
      free (state->dfdt);
      gsl_matrix_free (state->dfdy);
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for M", GSL_ENOMEM);
    }

  state->p = gsl_permutation_alloc (dim);

  if (state->p == 0)
    {
      gsl_matrix_free (state->M);
      free (state->dfdt);
      gsl_matrix_free (state->dfdy);
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for p", GSL_ENOMEM);
    }

  state->rhs = gsl_vector_alloc (dim);

  if (state->rhs == 0)
    {
      gsl_permutation_free (state->p);
      gsl_matrix_free (state->M);
      free (state->dfdt);
      gsl_matrix_free (state->dfdy);
      gsl_vector_free (state->tempvec);
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->ordprevbackup);
      free (state->ordprev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for rhs", GSL_ENOMEM);
    }

  msbdf_reset ((void *) state, dim);

  state->driver = NULL;

  return state;
}

static int
msbdf_failurehandler (void *vstate, const size_t dim, const double t)
{
  /* Internal failure handler routine for msbdf. Adjusted strategy
     for GSL: Decrease order if this is the second time a failure
     has occurred at this order and point.
   */

  msbdf_state_t *state = (msbdf_state_t *) vstate;

  const size_t ord = state->ord;

  if (ord > 1 && (ord - state->ordprev[0] == 0) &&
      ord == state->failord && t == state->failt)
    {
      state->ord--;
    }

  /* Save information about failure */

  state->failord = ord;
  state->failt = t;
  state->ni++;

  /* Force reinitialization if failure took place at lowest
     order
   */

  if (ord == 1)
    {
      msbdf_reset (vstate, dim);
    }

  return GSL_SUCCESS;
}

static int
msbdf_calccoeffs (const size_t ord, const size_t ordwait,
                  const double h, const double hprev[],
                  double l[],
                  double *errcoeff, double *ordm1coeff,
                  double *ordp1coeff, double *ordp2coeff, double *gamma)
{
  /* Calculates coefficients (l) of polynomial Lambda, error and
     auxiliary order change evaluation coefficients.
   */

  if (ord == 1)
    {
      l[0] = 1.0;
      l[1] = 1.0;
      *errcoeff = 0.5;
      *ordp1coeff = 2.0;

      {
        const double hsum = h + hprev[0];

        const double a5 = -1.5;
        const double a6 = -1.0 - h / hsum;
        const double c2 = 2.0 / (1.0 - a6 + a5);

        *ordp2coeff = fabs (c2 * (h / hsum) * 3.0 * a5);
      }
    }
  else
    {
      size_t i, j;
      double hsum = h;
      double coeff1 = -1.0;
      double x;

      /* Calculate the actual polynomial coefficients (l) */

      DBL_ZERO_MEMSET (l, MSBDF_MAX_ORD + 1);

      l[0] = 1.0;
      l[1] = 1.0;

      for (i = 2; i < ord; i++)
        {
          hsum += hprev[i - 2];
          coeff1 += -1.0 / i;

          for (j = i; j > 0; j--)
            {
              l[j] += h / hsum * l[j - 1];
            }
        }

      coeff1 += -1.0 / ord;

      x = -l[1] - coeff1;

      for (i = ord; i > 0; i--)
        {
          l[i] += l[i - 1] * x;
        }

#ifdef DEBUG
      {
        size_t di;

        printf ("-- calccoeffs l: ");
        for (di = 0; di < ord + 1; di++)
          {
            printf ("%.5e ", l[di]);
          }
        printf ("\n");
      }
#endif

      hsum += hprev[ord - 2];

      {
        const double coeff2 = -l[1] - h / hsum;
        const double a1 = 1.0 - coeff2 + coeff1;
        const double a2 = 1.0 + ord * a1;

        /* Calculate error coefficient */

        *errcoeff = fabs (a1 / (coeff1 * a2));

        /* Calculate auxiliary coefficients used in evaluation of change
           of order
        */

        if (ordwait < 2)
          {
            const double a3 = coeff1 + 1.0 / ord;
            const double a4 = coeff2 + h / hsum;
            const double c1 = a3 / (1.0 - a4 + a3);

            *ordm1coeff = fabs (c1 / (x / l[ord]));

            *ordp1coeff = fabs (a2 / (l[ord] * (h / hsum) / x));

            hsum += hprev[ord - 1];

            {
              const double a5 = coeff1 - 1.0 / (ord + 1.0);
              const double a6 = coeff2 - h / hsum;
              const double c2 = a2 / (1.0 - a6 + a5);

              *ordp2coeff = fabs (c2 * (h / hsum) * (ord + 2) * a5);
            }
          }
      }
    }

  *gamma = h / l[1];

#ifdef DEBUG
  printf ("-- calccoeffs ordm1coeff=%.5e ", *ordm1coeff);
  printf ("ordp1coeff=%.5e ", *ordp1coeff);
  printf ("ordp2coeff=%.5e ", *ordp2coeff);
  printf ("errcoeff=%.5e\n", *errcoeff);
#endif

  return GSL_SUCCESS;
}

static int
msbdf_update (void *vstate, const size_t dim, gsl_matrix * dfdy, double *dfdt,
              const double t, const double *y, const gsl_odeiv2_system * sys,
              gsl_matrix * M, gsl_permutation * p,
              const size_t iter, size_t * nJ, size_t * nM,
              const double tprev, const double failt,
              const double gamma, const double gammaprev, const double hratio)
{
  /* Evaluates Jacobian dfdy and updates iteration matrix M
     if criteria for update is met.
   */

  /* Jacobian is evaluated
     - at first step
     - if MSBDF_JAC_WAIT steps have been made without re-evaluation
     - in case of a convergence failure if
     --- change in gamma is small, or
     --- convergence failure resulted in step size decrease
   */

  const double c = 0.2;
  const double gammarel = fabs (gamma / gammaprev - 1.0);

  if (*nJ == 0 || *nJ > MSBDF_JAC_WAIT ||
      (t == failt && (gammarel < c || hratio < 1.0)))
    {
#ifdef DEBUG
      printf ("-- evaluate jacobian\n");
#endif
      int s = GSL_ODEIV_JA_EVAL (sys, t, y, dfdy->data, dfdt);

      if (s == GSL_EBADFUNC)
        {
          return s;
        }

      if (s != GSL_SUCCESS)
        {
          msbdf_failurehandler (vstate, dim, t);
#ifdef DEBUG
          printf ("-- FAIL at jacobian function evaluation\n");
#endif
          return s;
        }

      /* Reset counter */

      *nJ = 0;
    }

  /* Iteration matrix M (and it's LU decomposition) is generated
     - at first step
     - if MSBDF_M_WAIT steps have been made without an update
     - if change in gamma is significant (e.g. change in step size)
     - if previous step was rejected
   */

  if (*nM == 0 || *nM > MSBDF_M_WAIT || gammarel >= c ||
      t == tprev || t == failt)
    {
#ifdef DEBUG
      printf ("-- update M, gamma=%.5e\n", gamma);
#endif
      size_t i;
      gsl_matrix_memcpy (M, dfdy);
      gsl_matrix_scale (M, -gamma);

      for (i = 0; i < dim; i++)
        {
          gsl_matrix_set (M, i, i, gsl_matrix_get (M, i, i) + 1.0);
        }

      {
        int signum;
        int s = gsl_linalg_LU_decomp (M, p, &signum);

        if (s != GSL_SUCCESS)
          {
            return GSL_FAILURE;
          }
      }

      /* Reset counter */

      *nM = 0;
    }

  return GSL_SUCCESS;
}

static int
msbdf_corrector (void *vstate, const gsl_odeiv2_system * sys,
                 const double t, const double h, const size_t dim,
                 const double z[], const double errlev[],
                 const double l[], const double errcoeff,
                 gsl_vector * abscor, gsl_vector * relcor,
                 double ytmp[], double ytmp2[],
                 gsl_matrix * dfdy, double dfdt[], gsl_matrix * M,
                 gsl_permutation * p, gsl_vector * rhs,
                 size_t * nJ, size_t * nM,
                 const double tprev, const double failt,
                 const double gamma, const double gammaprev,
                 const double hprev0)
{
  /* Calculates the correction step (abscor). Equation
     system M = I - gamma * dfdy = -G is solved by Newton iteration.
   */

  size_t mi, i;
  const size_t max_iter = 3;    /* Maximum number of iterations */
  double convrate = 1.0;        /* convergence rate */
  double stepnorm = 0.0;        /* norm of correction step */
  double stepnormprev = 0.0;    /* previous norm value */

  /* Evaluate at predicted values */

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, ytmp);

    if (s == GSL_EBADFUNC)
      {
        return s;
      }

    if (s != GSL_SUCCESS)
      {
        msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
        printf ("-- FAIL at user function evaluation\n");
#endif
        return s;
      }
  }

  /* Calculate correction step (abscor) */

  gsl_vector_set_zero (abscor);

  for (mi = 0; mi < max_iter; mi++)
    {
      const double safety = 0.3;
      const double safety2 = 0.1;

      /* Generate or update Jacobian and/or iteration matrix M if needed */

      if (mi == 0)
        {
          int s = msbdf_update (vstate, dim, dfdy, dfdt, t + h, z,
                                sys, M, p, mi,
                                nJ, nM, tprev, failt,
                                gamma, gammaprev,
                                h / hprev0);

          if (s != GSL_SUCCESS)
            {
              return s;
            }
        }

      /* Evaluate the right hand side (-G) */

      for (i = 0; i < dim; i++)
        {
          const double r = -1.0 * gsl_vector_get (abscor, i) -
            z[1 * dim + i] / l[1] + gamma * ytmp[i];

          gsl_vector_set (rhs, i, r);
        }

      /* Solve system of equations */

      {
        int s = gsl_linalg_LU_solve (M, p, rhs, relcor);

        if (s != GSL_SUCCESS)
          {
            msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at LU_solve\n");
#endif
            return GSL_FAILURE;
          }
      }

#ifdef DEBUG
      {
        size_t di;
        printf ("-- dstep: ");
        for (di = 0; di < dim; di++)
          {
            printf ("%.5e ", gsl_vector_get (relcor, di));
          }
        printf ("\n");
      }
#endif

      /* Add iteration results */

      for (i = 0; i < dim; i++)
        {
          const double r =
            gsl_vector_get (abscor, i) + gsl_vector_get (relcor, i);

          gsl_vector_set (abscor, i, r);

          ytmp2[i] = z[i] + r;

          gsl_vector_set (relcor, i, gsl_vector_get (relcor, i) / errlev[i]);
        }

#ifdef DEBUG
      {
        size_t di;
        printf ("-- abscor: ");
        for (di = 0; di < dim; di++)
          {
            printf ("%.5e ", gsl_vector_get (abscor, di));
          }
        printf ("\n");
      }
#endif

      /* Convergence test. Norms used are root-mean-square norms. */

      stepnorm = gsl_blas_dnrm2 (relcor) / sqrt (dim);

      if (mi > 0)
        {
          convrate = GSL_MAX_DBL (safety * convrate, stepnorm / stepnormprev);
        }
      else
        {
          convrate = 1.0;
        }

      {
        const double convtest =
          GSL_MIN_DBL (convrate, 1.0) * stepnorm * errcoeff / safety2;

#ifdef DEBUG
        printf
          ("-- newt iter loop %d, errcoeff=%.5e, stepnorm =%.5e, convrate = %.5e, convtest = %.5e\n",
           (int) mi, errcoeff, stepnorm, convrate, convtest);
#endif
        if (convtest <= 1.0)
          {
            break;
          }
      }

      /* Check for divergence during iteration */

      {
        const double div_const = 2.0;

        if (mi > 1 && stepnorm > div_const * stepnormprev)
          {
            msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL, diverging Newton iteration\n");
#endif
            return GSL_FAILURE;
          }
      }

      /* Evaluate at new y */

      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp2, ytmp);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

      stepnormprev = stepnorm;
    }

#ifdef DEBUG
  printf ("-- Newton iteration exit at mi=%d\n", (int) mi);
#endif

  /* Handle convergence failure */

  if (mi == max_iter)
    {
      msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
      printf ("-- FAIL, max_iter reached\n");
#endif
      return GSL_FAILURE;
    }

  return GSL_SUCCESS;
}

static int
msbdf_eval_order (gsl_vector * abscor, gsl_vector * tempvec,
                  gsl_vector * svec, const double errcoeff,
                  const size_t dim, const double errlev[],
                  const double ordm1coeff, const double ordp1coeff,
                  const double ordp1coeffprev, const double ordp2coeff,
                  const double hprev[],
                  const double h, const double z[],
                  size_t * ord, size_t * ordwait)
{
  /* Evaluates and executes change in method order (current, current-1
     or current+1). Order which maximizes the step length is selected.
   */

  size_t i;

  /* step size estimates at current order, order-1 and order+1 */
  double ordest = 0.0;
  double ordm1est = 0.0;
  double ordp1est = 0.0;

  const double safety = 1e-6;
  const double bias = 6.0;
  const double bias2 = 10.0;
  const double min_incr = 1.5;

  /* Relative step length estimate for current order */

  ordest = 1.0 / (pow (bias * gsl_blas_dnrm2 (abscor) / sqrt (dim)
                       * errcoeff, 1.0 / (*ord + 1)) + safety);

  /* Relative step length estimate for order ord - 1 */

  if (*ord > 1)
    {
      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (tempvec, i, z[*ord * dim + i] / errlev[i]);
        }

      ordm1est = 1.0 / (pow (bias * gsl_blas_dnrm2 (tempvec) / sqrt (dim)
                             / ordm1coeff, 1.0 / (*ord)) + safety);
    }
  else
    {
      ordm1est = 0.0;
    }

  /* Relative step length estimate for order ord + 1 */

  if (*ord < MSBDF_MAX_ORD)
    {
      const double c = -ordp1coeff / ordp1coeffprev *
        pow (h / hprev[1], *ord + 1);

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (svec, i) * c +
                          gsl_vector_get (abscor, i));
        }

      ordp1est = 1.0 / (pow (bias2 * gsl_blas_dnrm2 (svec) / sqrt (dim)
                             / ordp2coeff, 1.0 / (*ord + 2)) + safety);
    }
  else
    {
      ordp1est = 0.0;
    }

#ifdef DEBUG
  printf
    ("-- eval_order ord=%d, ordest=%.5e, ordm1est=%.5e, ordp1est=%.5e\n",
     (int) *ord, ordest, ordm1est, ordp1est);
#endif

  /* Choose order that maximises step size and increases step
     size markedly compared to current step
   */

  if (ordm1est > ordest && ordm1est > ordp1est && ordm1est > min_incr)
    {
      *ord -= 1;
#ifdef DEBUG
      printf ("-- eval_order order DECREASED to %d\n", (int) *ord);
#endif
    }

  else if (ordp1est > ordest && ordp1est > ordm1est && ordp1est > min_incr)
    {
      *ord += 1;
#ifdef DEBUG
      printf ("-- eval_order order INCREASED to %d\n", (int) *ord);
#endif
    }

  *ordwait = *ord + 2;

  return GSL_SUCCESS;
}

static int
msbdf_check_no_order_decrease (size_t const ordprev[])
{
  /* Checks if order has not been decreased according to order history
     array. Used in stability enhancement.
   */

  size_t i;

  for (i = 0; i < MSBDF_MAX_ORD - 1; i++)
    {
      if (ordprev[i + 1] > ordprev[i])
        {
          return 0;
        }
    }

  return 1;
}

static int
msbdf_check_step_size_decrease (double const hprev[])
{
  /* Checks if step size has decreased markedly according to
     step size history array. Used in stability enhancement.
   */

  size_t i;
  double max = fabs (hprev[0]);
  const double min = fabs (hprev[0]);
  const double decrease_limit = 0.5;

  for (i = 1; i < MSBDF_MAX_ORD; i++)
    {
      const double h = fabs (hprev[i]);

      if (h > min && h > max)
        {
          max = h;
        }
    }

  if (min / max < decrease_limit)
    {
      return 1;
    }

  return 0;
}

static int
msbdf_apply (void *vstate, size_t dim, double t, double h,
             double y[], double yerr[],
             const double dydt_in[], double dydt_out[],
             const gsl_odeiv2_system * sys)
{
  /* Carries out a step by BDF linear multistep methods. */

  msbdf_state_t *state = (msbdf_state_t *) vstate;

  double *const z = state->z;
  double *const zbackup = state->zbackup;
  double *const ytmp = state->ytmp;
  double *const ytmp2 = state->ytmp2;
  double *const l = state->l;
  double *const hprev = state->hprev;
  double *const hprevbackup = state->hprevbackup;
  size_t *const ordprev = state->ordprev;
  size_t *const ordprevbackup = state->ordprevbackup;
  double *const errlev = state->errlev;
  gsl_vector *const abscor = state->abscor;
  gsl_vector *const relcor = state->relcor;
  gsl_vector *const svec = state->svec;
  gsl_vector *const tempvec = state->tempvec;

  size_t ord = state->ord;      /* order for this step */
  double ordm1coeff = 0.0;
  double ordp1coeff = 0.0;
  double ordp2coeff = 0.0;
  double errcoeff = 0.0;        /* error coefficient */
  double gamma = 0.0;           /* gamma coefficient */

  const size_t max_failcount = 3;
  size_t i;

#ifdef DEBUG
  {
    size_t di;

    printf ("msbdf_apply: t=%.5e, ord=%d, h=%.5e, y:", t, (int) ord, h);

    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", y[di]);
      }
    printf ("\n");
  }
#endif

  /* Check if t is the same as on previous stepper call (or last
     failed call). This means that calculation of previous step failed
     or the step was rejected, and therefore previous state will be
     restored or the method will be reset.
   */

  if (state->ni > 0 && (t == state->tprev || t == state->failt))
    {
      if (state->ni == 1)
        {
          /* No step has been accepted yet, reset method */

          msbdf_reset (vstate, dim);
#ifdef DEBUG
          printf ("-- first step was REJECTED, msbdf_reset called\n");
#endif
        }
      else
        {
          /* A succesful step has been saved, restore previous state. */

          /* If previous step suggests order increase, but the step was
             rejected, then do not increase order.
           */

          if (ord > ordprev[0])
            {
              state->ord = ordprev[0];
              ord = state->ord;
            }

          /* Restore previous state */

          DBL_MEMCPY (z, zbackup, (MSBDF_MAX_ORD + 1) * dim);
          DBL_MEMCPY (hprev, hprevbackup, MSBDF_MAX_ORD);

          for (i = 0; i < MSBDF_MAX_ORD; i++)
            {
              ordprev[i] = ordprevbackup[i];
            }

          state->ordwait = state->ordwaitbackup;
          state->gammaprev = state->gammaprevbackup;

#ifdef DEBUG
          printf ("-- previous step was REJECTED, state restored\n");
#endif
        }

      /* If step is repeatedly rejected, then reset method */

      state->failcount++;

      if (state->failcount > max_failcount && state->ni > 1)
        {
          msbdf_reset (vstate, dim);
          ord = state->ord;

#ifdef DEBUG
          printf ("-- max_failcount reached, msbdf_reset called\n");
#endif
        }
    }
  else
    {
      /* The previous step was accepted. Backup current state. */

      DBL_MEMCPY (zbackup, z, (MSBDF_MAX_ORD + 1) * dim);
      DBL_MEMCPY (hprevbackup, hprev, MSBDF_MAX_ORD);

      for (i = 0; i < MSBDF_MAX_ORD; i++)
        {
          ordprevbackup[i] = ordprev[i];
        }

      state->ordwaitbackup = state->ordwait;
      state->gammaprevbackup = state->gammaprev;

      state->failcount = 0;

#ifdef DEBUG
      if (state->ni > 0)
        {
          printf ("-- previous step was ACCEPTED, state saved\n");
        }
#endif
    }

#ifdef DEBUG
  printf ("-- ord=%d, ni=%ld, ordwait=%d\n", (int) ord, state->ni,
          (int) state->ordwait);

  size_t di;
  printf ("-- ordprev: ");

  for (di = 0; di < MSBDF_MAX_ORD; di++)
    {
      printf ("%d ", (int) ordprev[di]);
    }

  printf ("\n");
#endif

  /* Get desired error levels via gsl_odeiv2_control object through driver
     object, which is a requirement for this stepper.
   */

  if (state->driver == NULL)
    {
      return GSL_EFAULT;
    }
  else
    {
      size_t i;

      for (i = 0; i < dim; i++)
        {
          if (dydt_in != NULL)
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           dydt_in[i], h, i, &errlev[i]);
            }
          else
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           0.0, h, i, &errlev[i]);
            }
        }
    }

#ifdef DEBUG
  {
    size_t di;
    printf ("-- errlev: ");
    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", errlev[di]);
      }
    printf ("\n");
  }
#endif

  /* On first call initialize Nordsieck matrix */

  if (state->ni == 0)
    {
      size_t i;

      DBL_ZERO_MEMSET (z, (MSBDF_MAX_ORD + 1) * dim);

      if (dydt_in != NULL)
        {
          DBL_MEMCPY (ytmp, dydt_in, dim);
        }
      else
        {
          int s = GSL_ODEIV_FN_EVAL (sys, t, y, ytmp);

          if (s != GSL_SUCCESS)
            {
              return s;
            }
        }

      DBL_MEMCPY (&z[0 * dim], y, dim);
      DBL_MEMCPY (&z[1 * dim], ytmp, dim);

      for (i = 0; i < dim; i++)
        {
          z[1 * dim + i] *= h;
        }
    }

  /* Stability enhancement heuristic for msbdf: If order > 1 and order
     has not been changed, check for decrease in step size, that is
     not accompanied by a decrease in method order. This condition may
     be indication of BDF method stability problems, a change in ODE
     system, or convergence problems in Newton iteration. In all
     cases, the strategy is to decrease method order.
   */

#ifdef DEBUG
  printf ("-- check_no_order_decrease %d, check_step_size_decrease %d\n",
          msbdf_check_no_order_decrease (ordprev),
          msbdf_check_step_size_decrease (hprev));
#endif

  if (ord > 1 &&
      ord - ordprev[0] == 0 &&
      msbdf_check_no_order_decrease (ordprev) &&
      msbdf_check_step_size_decrease (hprev))
    {
      state->ord--;
      state->ordwait = ord + 2;
      ord = state->ord;

#ifdef DEBUG
      printf ("-- stability enhancement decreased order to %d\n", (int) ord);
#endif
    }

  /* Sanity check */

  {
    const int deltaord = ord - ordprev[0];

  if (deltaord > 1 || deltaord < -1)
    {
      printf ("-- order change %d\n", deltaord);
      GSL_ERROR_NULL ("msbdf_apply too large order change", GSL_ESANITY);
    }

  /* Modify Nordsieck matrix if order or step length has been changed */

  /* If order increased by 1, adjust Nordsieck matrix */

  if (deltaord == 1)
    {
      if (ord > 2)
        {
          size_t i, j;
          double hsum = h;
          double coeff1 = -1.0;
          double coeff2 = 1.0;
          double hrelprev = 1.0;
          double hrelprod = 1.0;
          double hrel = 0.0;

          /* Calculate coefficients used in adjustment to l */

          DBL_ZERO_MEMSET (l, MSBDF_MAX_ORD + 1);

          l[2] = 1.0;

          for (i = 1; i < ord - 1; i++)
            {
              hsum += hprev[i];
              hrel = hsum / h;
              hrelprod *= hrel;
              coeff1 -= 1.0 / (i + 1);
              coeff2 += 1.0 / hrel;

              for (j = i + 2; j > 1; j--)
                {
                  l[j] *= hrelprev;
                  l[j] += l[j - 1];
                }

              hrelprev = hrel;
            }

          /* Scale Nordsieck matrix */

          {
            const double c = (-coeff1 - coeff2) / hrelprod;

            for (i = 0; i < dim; i++)
              {
                z[ord * dim + i] = c * gsl_vector_get (abscor, i);
              }
          }
          for (i = 2; i < ord; i++)
            for (j = 0; j < dim; j++)
              {
                z[i * dim + j] += l[i] * z[ord * dim + j];
              }
        }
      else
        {
          /* zero new vector for order incease from 1 to 2 */

          DBL_ZERO_MEMSET (&z[ord * dim], dim);
        }

#ifdef DEBUG
      printf ("-- order increase detected, Nordsieck modified\n");
#endif
    }

  /* If order decreased by 1, adjust Nordsieck matrix */

  if (deltaord == -1)
    {
      size_t i, j;
      double hsum = 0.0;

      /* Calculate coefficients used in adjustment to l */

      DBL_ZERO_MEMSET (l, MSBDF_MAX_ORD + 1);

      l[2] = 1.0;

      for (i = 1; i < ord; i++)
        {
          hsum += hprev[i - 1];

          for (j = i + 2; j > 1; j--)
            {
              l[j] *= hsum / h;
              l[j] += l[j - 1];
            }
        }

      /* Scale Nordsieck matrix */

      for (i = 2; i < ord + 1; i++)
        for (j = 0; j < dim; j++)
          {
            z[i * dim + j] += -l[i] * z[(ord + 1) * dim + j];
          }

#ifdef DEBUG
      printf ("-- order decrease detected, Nordsieck modified\n");
#endif
    }

  /* Scale Nordsieck vectors if step size has been changed */

  if (state->ni > 0 && h != hprev[0])
    {
      size_t i, j;
      const double hrel = h / hprev[0];
      double coeff = hrel;

      for (i = 1; i < ord + 1; i++)
        {
          for (j = 0; j < dim; j++)
            {
              z[i * dim + j] *= coeff;
            }

          coeff *= hrel;
        }

#ifdef DEBUG
      printf ("-- h != hprev, Nordsieck modified\n");
#endif
    }

  /* Calculate polynomial coefficients (l), error coefficient and
     auxiliary coefficients
   */

  msbdf_calccoeffs (ord, state->ordwait, h, hprev, l, &errcoeff,
                    &ordm1coeff, &ordp1coeff, &ordp2coeff, &gamma);

  /* Carry out the prediction step */

  {
    size_t i, j, k;

    for (i = 1; i < ord + 1; i++)
      for (j = ord; j > i - 1; j--)
        for (k = 0; k < dim; k++)
          {
            z[(j - 1) * dim + k] += z[j * dim + k];
          }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- predicted y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif
  }

  /* Calculate correction step to abscor */
  {
    int s;
    s = msbdf_corrector (vstate, sys, t, h, dim, z, errlev, l, errcoeff,
                         abscor, relcor, ytmp, ytmp2,
                         state->dfdy, state->dfdt, state->M,
                         state->p, state->rhs,
                         &(state->nJ), &(state->nM),
                         state->tprev, state->failt, gamma,
                         state->gammaprev, hprev[0]);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    /* Add accepted final correction step to Nordsieck matrix */

    size_t i, j;

    for (i = 0; i < ord + 1; i++)
      for (j = 0; j < dim; j++)
        {
          z[i * dim + j] += l[i] * gsl_vector_get (abscor, j);
        }

#ifdef DEBUG
    {
      size_t di;
      printf ("---- l: ");
      for (di = 0; di < ord + 1; di++)
        {
          printf ("%.5e ", l[di]);
        }
      printf ("\n");

      printf ("-- corrected y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif

    /* Derivatives at output */

    if (dydt_out != NULL)
      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, dydt_out);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msbdf_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

    /* Calculate error estimate */

    for (i = 0; i < dim; i++)
      {
        yerr[i] = fabs (gsl_vector_get (abscor, i)) * errcoeff;
      }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- yerr: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", yerr[di]);
        }
      printf ("\n");
    }
#endif

    /* Save y values */

    for (i = 0; i < dim; i++)
      {
        y[i] = z[0 * dim + i];
      }
  }

  /* Scale abscor with errlev for later use in norm calculations */
  {
    size_t i;

    for (i = 0; i < dim; i++)
      {
        gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) / errlev[i]);
      }
  }

  /* Save items needed for evaluation of order increase on next
     call, if needed
   */

  if (state->ordwait == 1 && ord < MSBDF_MAX_ORD)
    {
      size_t i;

      state->ordp1coeffprev = ordp1coeff;

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (abscor, i));
        }
    }

  /* Consider and execute order change for next step */

  if (state->ordwait == 0)
    {
      msbdf_eval_order (abscor, tempvec, svec, errcoeff, dim, errlev,
                        ordm1coeff, ordp1coeff,
                        state->ordp1coeffprev, ordp2coeff,
                        hprev, h, z, &(state->ord), &(state->ordwait));
    }

  /* Undo scaling of abscor for possible order increase on next step */
  {
    size_t i;

    for (i = 0; i < dim; i++)
      {
        gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) * errlev[i]);
      }
  }

  /* Save information about current step in state and update counters */
  {
    size_t i;

    for (i = MSBDF_MAX_ORD - 1; i > 0; i--)
      {
        hprev[i] = hprev[i - 1];
        ordprev[i] = ordprev[i - 1];
      }
  }

  hprev[0] = h;
  ordprev[0] = ord;

#ifdef DEBUG
  {
    size_t di;
    printf ("-- hprev: ");
    for (di = 0; di < MSBDF_MAX_ORD; di++)
      {
        printf ("%.5e ", hprev[di]);
      }
    printf ("\n");
  }
#endif

  state->tprev = t;
  state->ordwait--;
  state->ni++;
  state->gammaprev = gamma;

  state->nJ++;
  state->nM++;

#ifdef DEBUG
  printf ("-- nJ=%d, nM=%d\n", (int) state->nJ, (int) state->nM);
#endif
  }

  return GSL_SUCCESS;
}

static int
msbdf_set_driver (void *vstate, const gsl_odeiv2_driver * d)
{
  msbdf_state_t *state = (msbdf_state_t *) vstate;

  state->driver = d;

  return GSL_SUCCESS;
}

static int
msbdf_reset (void *vstate, size_t dim)
{
  msbdf_state_t *state = (msbdf_state_t *) vstate;
  size_t i;

  state->ni = 0;
  state->ord = 1;
  state->ordwait = 2;
  state->ordwaitbackup = 2;
  state->failord = 0;
  state->failt = GSL_NAN;
  state->gammaprev = 1.0;
  state->nJ = 0;
  state->nM = 0;
  state->failcount = 0;

  DBL_ZERO_MEMSET (state->hprev, MSBDF_MAX_ORD);
  DBL_ZERO_MEMSET (state->hprevbackup, MSBDF_MAX_ORD);
  DBL_ZERO_MEMSET (state->z, (MSBDF_MAX_ORD + 1) * dim);
  DBL_ZERO_MEMSET (state->zbackup, (MSBDF_MAX_ORD + 1) * dim);

  for (i = 0; i < MSBDF_MAX_ORD; i++)
    {
      state->ordprev[i] = 1;
      state->ordprevbackup[i] = 1;
    }

#ifdef DEBUG
  printf ("-- msbdf_reset called\n");
#endif

  return GSL_SUCCESS;
}

static unsigned int
msbdf_order (void *vstate)
{
  msbdf_state_t *state = (msbdf_state_t *) vstate;

  return state->ord;
}

static void
msbdf_free (void *vstate)
{
  msbdf_state_t *state = (msbdf_state_t *) vstate;

  gsl_vector_free (state->rhs);
  gsl_permutation_free (state->p);
  gsl_matrix_free (state->M);
  free (state->dfdt);
  gsl_matrix_free (state->dfdy);
  gsl_vector_free (state->tempvec);
  gsl_vector_free (state->svec);
  gsl_vector_free (state->relcor);
  gsl_vector_free (state->abscor);
  free (state->errlev);
  free (state->ordprevbackup);
  free (state->ordprev);
  free (state->hprevbackup);
  free (state->hprev);
  free (state->l);
  free (state->ytmp2);
  free (state->ytmp);
  free (state->zbackup);
  free (state->z);
  free (state);
}

static const gsl_odeiv2_step_type msbdf_type = {
  "msbdf",                      /* name */
  1,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &msbdf_alloc,
  &msbdf_apply,
  &msbdf_set_driver,
  &msbdf_reset,
  &msbdf_order,
  &msbdf_free
};

const gsl_odeiv2_step_type *gsl_odeiv2_step_msbdf = &msbdf_type;

max_stars_repo_name: WikiGaze/Wikipedia-readers-gaze

max_issues_repo_name: WikiGaze/Wikipedia-readers-gaze

max_forks_repo_name: WikiGaze/Wikipedia-readers-gaze