#if !defined(PETIGA_H)
#define PETIGA_H

/*
#include <petscconf.h>
#undef  static inline
#define static inline static __inline
*/

#include <petsc.h>

#if PETSC_VERSION_LT(3,7,0)
#error "PetIGA requires PETSc 3.7 or higher"
#endif

#include <petsc/private/petscimpl.h>

#if PETSC_VERSION_LT(3,8,0)
#  ifdef PETSC_FUNCTION_NAME
#    undef  __FUNCT__
#    define __FUNCT__ PETSC_FUNCTION_NAME
#  endif
#endif

#if PETSC_VERSION_LT(3,17,0)
#undef  SETERRQ
#define SETERRQ(comm,ierr,...) return PetscError(comm,__LINE__,PETSC_FUNCTION_NAME,__FILE__,ierr,PETSC_ERROR_INITIAL,__VA_ARGS__)
#endif

typedef ISLocalToGlobalMapping LGMap;
#define LGMap LGMap

/* ---------------------------------------------------------------- */

typedef struct _p_IGA         *IGA;
typedef struct _n_IGAAxis     *IGAAxis;
typedef struct _n_IGARule     *IGARule;
typedef struct _n_IGABasis    *IGABasis;
typedef struct _n_IGAForm     *IGAForm;
typedef struct _n_IGAElement  *IGAElement;
typedef struct _n_IGAPoint    *IGAPoint;
typedef struct _n_IGAProbe    *IGAProbe;

/* ---------------------------------------------------------------- */

struct _n_IGAAxis {
  PetscInt refct;
  /**/
  PetscInt   p; /* polynomial order    */
  PetscInt   m; /* last knot index     */
  PetscReal *U; /* knot vector         */
  /**/
  PetscBool  periodic; /* periodicity  */
  PetscInt   nnp,nel;  /* bases, spans */
  PetscInt   *span;    /* span indices */
};

PETSC_EXTERN PetscErrorCode IGAAxisCreate(IGAAxis *axis);
PETSC_EXTERN PetscErrorCode IGAAxisDestroy(IGAAxis *axis);
PETSC_EXTERN PetscErrorCode IGAAxisReset(IGAAxis axis);
PETSC_EXTERN PetscErrorCode IGAAxisReference(IGAAxis axis);
PETSC_EXTERN PetscErrorCode IGAAxisCopy(IGAAxis base,IGAAxis axis);
PETSC_EXTERN PetscErrorCode IGAAxisDuplicate(IGAAxis base,IGAAxis *axis);
PETSC_EXTERN PetscErrorCode IGAAxisSetPeriodic(IGAAxis axis,PetscBool periodic);
PETSC_EXTERN PetscErrorCode IGAAxisGetPeriodic(IGAAxis axis,PetscBool *periodic);
PETSC_EXTERN PetscErrorCode IGAAxisSetDegree(IGAAxis axis,PetscInt p);
PETSC_EXTERN PetscErrorCode IGAAxisGetDegree(IGAAxis axis,PetscInt *p);
PETSC_EXTERN PetscErrorCode IGAAxisSetKnots(IGAAxis axis,PetscInt m,const PetscReal U[]);
PETSC_EXTERN PetscErrorCode IGAAxisGetKnots(IGAAxis axis,PetscInt *m,PetscReal *U[]);
PETSC_EXTERN PetscErrorCode IGAAxisGetLimits(IGAAxis axis,PetscReal *Ui,PetscReal *Uf);
PETSC_EXTERN PetscErrorCode IGAAxisGetSizes(IGAAxis axis,PetscInt *nel,PetscInt *nnp);
PETSC_EXTERN PetscErrorCode IGAAxisGetSpans(IGAAxis axis,PetscInt *nel,PetscInt *spans[]);
PETSC_EXTERN PetscErrorCode IGAAxisInit(IGAAxis axis,PetscInt p,PetscInt m,const PetscReal U[]);
PETSC_EXTERN PetscErrorCode IGAAxisInitBreaks(IGAAxis axis,PetscInt nu,const PetscReal u[],PetscInt C);
PETSC_EXTERN PetscErrorCode IGAAxisInitUniform(IGAAxis axis,PetscInt N,PetscReal Ui,PetscReal Uf,PetscInt C);
PETSC_EXTERN PetscErrorCode IGAAxisSetUp(IGAAxis axis);

typedef enum {
  IGA_RULE_LEGENDRE=0, /* Gauss-Legendre         */
  IGA_RULE_LOBATTO,    /* Gauss-Lobatto          */
  IGA_RULE_REDUCED,    /* Reduced Gauss-Legendre */
  IGA_RULE_USER        /* User-defined           */
} IGARuleType;

PETSC_EXTERN const char *const IGARuleTypes[];

struct _n_IGARule {
  PetscInt refct;
  /**/
  IGARuleType type;   /* rule type */
  /**/
  PetscInt  nqp;      /* number of quadrature points */
  PetscReal *point;   /* [nqp] quadrature points  */
  PetscReal *weight;  /* [nqp] quadrature weights */
};

PETSC_EXTERN PetscErrorCode IGARuleCreate(IGARule *rule);
PETSC_EXTERN PetscErrorCode IGARuleDestroy(IGARule *rule);
PETSC_EXTERN PetscErrorCode IGARuleReset(IGARule rule);
PETSC_EXTERN PetscErrorCode IGARuleReference(IGARule rule);
PETSC_EXTERN PetscErrorCode IGARuleCopy(IGARule base,IGARule rule);
PETSC_EXTERN PetscErrorCode IGARuleDuplicate(IGARule base,IGARule *rule);
PETSC_EXTERN PetscErrorCode IGARuleSetType(IGARule rule,IGARuleType type);
PETSC_EXTERN PetscErrorCode IGARuleSetSize(IGARule rule,PetscInt nqp);
PETSC_EXTERN PetscErrorCode IGARuleSetUp(IGARule rule);
PETSC_EXTERN PetscErrorCode IGARuleSetRule(IGARule rule,PetscInt q,const PetscReal x[],const PetscReal w[]);
PETSC_EXTERN PetscErrorCode IGARuleGetRule(IGARule rule,PetscInt *q,PetscReal *x[],PetscReal *w[]);

typedef enum {
  IGA_BASIS_BSPLINE=0, /* B-Spline basis functions */
  IGA_BASIS_BERNSTEIN, /* Bernstein polynomials (C^0 B-Spline basis) */
  IGA_BASIS_LAGRANGE,  /* Lagrange polynomials on Newton-Cotes points */
  IGA_BASIS_SPECTRAL   /* Lagrange polynomials on Gauss-Lobatto points */
} IGABasisType;

PETSC_EXTERN const char *const IGABasisTypes[];

struct _n_IGABasis {
  PetscInt refct;
  /**/
  IGABasisType type;  /* basis type */
  /**/
  PetscInt  nel;      /* number of elements */
  PetscInt  nqp;      /* number of quadrature points */
  PetscInt  nen;      /* number of local basis functions */

  PetscInt  *offset;  /* [nel] basis offset */
  PetscReal *detJac;  /* [nel] element length */
  PetscReal *weight;  /* [nel][nqp] quadrature weight */
  PetscReal *point;   /* [nel][nqp] quadrature point */
  PetscReal *value;   /* [nel][nqp][nen][5] basis derivatives */

  PetscReal  bnd_detJac;
  PetscReal  bnd_weight;
  PetscReal  bnd_point[2];
  PetscReal *bnd_value[2]; /* [nen][5] */
};

PETSC_EXTERN PetscErrorCode IGABasisCreate(IGABasis *basis);
PETSC_EXTERN PetscErrorCode IGABasisDestroy(IGABasis *basis);
PETSC_EXTERN PetscErrorCode IGABasisReset(IGABasis basis);
PETSC_EXTERN PetscErrorCode IGABasisReference(IGABasis basis);
PETSC_EXTERN PetscErrorCode IGABasisSetType(IGABasis basis,IGABasisType type);
PETSC_EXTERN PetscErrorCode IGABasisInitQuadrature (IGABasis basis,IGAAxis axis,IGARule rule);
PETSC_EXTERN PetscErrorCode IGABasisInitCollocation(IGABasis basis,IGAAxis axis);

/* ---------------------------------------------------------------- */

typedef PetscErrorCode (*IGAFormExact)(IGAPoint point,PetscInt k,PetscScalar V[],void *ctx);
typedef PetscErrorCode (*IGAFormScalar)(IGAPoint point,const PetscScalar U[],PetscInt n,PetscScalar S[],void *ctx);
typedef PetscErrorCode (*IGAFormVector)(IGAPoint point,PetscScalar F[],void *ctx);
typedef PetscErrorCode (*IGAFormMatrix)(IGAPoint point,PetscScalar K[],void *ctx);
typedef PetscErrorCode (*IGAFormSystem)(IGAPoint point,PetscScalar K[],PetscScalar F[],void *ctx);
typedef PetscErrorCode (*IGAFormFunction)(IGAPoint point,
                                          const PetscScalar U[],
                                          PetscScalar F[],void *ctx);
typedef PetscErrorCode (*IGAFormJacobian)(IGAPoint point,
                                          const PetscScalar U[],
                                          PetscScalar J[],void *ctx);
typedef PetscErrorCode (*IGAFormIFunction)(IGAPoint point,
                                           PetscReal a,const PetscScalar V[],
                                           PetscReal t,const PetscScalar U[],
                                           PetscScalar F[],void *ctx);
typedef PetscErrorCode (*IGAFormIJacobian)(IGAPoint point,
                                           PetscReal a,const PetscScalar V[],
                                           PetscReal t,const PetscScalar U[],
                                           PetscScalar J[],void *ctx);
typedef PetscErrorCode (*IGAFormI2Function)(IGAPoint point,
                                            PetscReal a,const PetscScalar A[],
                                            PetscReal v,const PetscScalar V[],
                                            PetscReal t,const PetscScalar U[],
                                            PetscScalar F[],void *ctx);
typedef PetscErrorCode (*IGAFormI2Jacobian)(IGAPoint point,
                                            PetscReal a,const PetscScalar A[],
                                            PetscReal v,const PetscScalar V[],
                                            PetscReal t,const PetscScalar U[],
                                            PetscScalar J[],void *ctx);
typedef PetscErrorCode (*IGAFormIEFunction)(IGAPoint point,
                                            PetscReal a,const PetscScalar V[],
                                            PetscReal t,const PetscScalar U[],
                                            PetscReal t0,const PetscScalar U0[],
                                            PetscScalar F[],void *ctx);
typedef PetscErrorCode (*IGAFormIEJacobian)(IGAPoint point,
                                            PetscReal a,const PetscScalar V[],
                                            PetscReal t,const PetscScalar U[],
                                            PetscReal t0,const PetscScalar U0[],
                                            PetscScalar J[],void *ctx);
typedef PetscErrorCode (*IGAFormRHSFunction)(IGAPoint point,
                                             PetscReal t,const PetscScalar U[],
                                             PetscScalar F[],void *ctx);
typedef PetscErrorCode (*IGAFormRHSJacobian)(IGAPoint point,
                                             PetscReal t,const PetscScalar U[],
                                             PetscScalar J[],void *ctx);

PETSC_EXTERN PetscErrorCode IGAFormJacobianFD(IGAPoint p,
                                              const PetscScalar U[],
                                              PetscScalar J[],void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormIJacobianFD(IGAPoint p,
                                               PetscReal s,const PetscScalar V[],
                                               PetscReal t,const PetscScalar U[],
                                               PetscScalar J[],void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormIEJacobianFD(IGAPoint p,
                                                PetscReal s, const PetscScalar V[],
                                                PetscReal t, const PetscScalar U[],
                                                PetscReal t0,const PetscScalar U0[],
                                                PetscScalar J[],void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormI2JacobianFD(IGAPoint p,
                                                PetscReal a,const PetscScalar A[],
                                                PetscReal v,const PetscScalar V[],
                                                PetscReal t,const PetscScalar U[],
                                                PetscScalar J[],void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormRHSJacobianFD(IGAPoint p,
                                                 PetscReal t,const PetscScalar U[],
                                                 PetscScalar F[],void *ctx);

typedef struct _IGAFormBC *IGAFormBC;
struct _IGAFormBC {
  PetscInt    count;
  PetscInt    field[64];
  PetscScalar value[64];
};

typedef struct _IGAFormOps *IGAFormOps;
struct _IGAFormOps {
  /**/
  IGAFormVector     Vector;
  void              *VecCtx;
  IGAFormMatrix     Matrix;
  void              *MatCtx;
  IGAFormSystem     System;
  void              *SysCtx;
  /**/
  IGAFormFunction   Function;
  void              *FunCtx;
  IGAFormJacobian   Jacobian;
  void              *JacCtx;
  /**/
  IGAFormIFunction  IFunction;
  IGAFormI2Function I2Function;
  void              *IFunCtx;
  IGAFormIJacobian  IJacobian;
  IGAFormI2Jacobian I2Jacobian;
  void              *IJacCtx;
  /**/
  IGAFormIEFunction IEFunction;
  void              *IEFunCtx;
  IGAFormIEJacobian IEJacobian;
  void              *IEJacCtx;
  /**/
  IGAFormRHSFunction RHSFunction;
  void               *RHSFunCtx;
  IGAFormRHSJacobian RHSJacobian;
  void               *RHSJacCtx;
};

struct _n_IGAForm {
  PetscInt refct;
  /**/
  IGAFormOps ops;
  PetscInt   dof;
  IGAFormBC  value[3][2];
  IGAFormBC  load [3][2];
  PetscBool  visit[3][2];
};

PETSC_EXTERN PetscErrorCode IGAGetForm(IGA iga,IGAForm *form);
PETSC_EXTERN PetscErrorCode IGASetForm(IGA iga,IGAForm form);

PETSC_EXTERN PetscErrorCode IGAFormCreate(IGAForm *form);
PETSC_EXTERN PetscErrorCode IGAFormDestroy(IGAForm *form);
PETSC_EXTERN PetscErrorCode IGAFormReset(IGAForm form);
PETSC_EXTERN PetscErrorCode IGAFormReference(IGAForm form);

PETSC_EXTERN PetscErrorCode IGAFormSetBoundaryValue(IGAForm form,PetscInt axis,PetscInt side,PetscInt field,PetscScalar value);
PETSC_EXTERN PetscErrorCode IGAFormSetBoundaryLoad (IGAForm form,PetscInt axis,PetscInt side,PetscInt field,PetscScalar value);
PETSC_EXTERN PetscErrorCode IGAFormSetBoundaryForm (IGAForm form,PetscInt axis,PetscInt side,PetscBool flag);
PETSC_EXTERN PetscErrorCode IGAFormClearBoundary   (IGAForm form,PetscInt axis,PetscInt side);

PETSC_EXTERN PetscErrorCode IGAFormSetVector     (IGAForm form,IGAFormVector      Vector,     void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetMatrix     (IGAForm form,IGAFormMatrix      Matrix,     void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetSystem     (IGAForm form,IGAFormSystem      System,     void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetFunction   (IGAForm form,IGAFormFunction    Function,   void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetJacobian   (IGAForm form,IGAFormJacobian    Jacobian,   void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetIFunction  (IGAForm form,IGAFormIFunction   IFunction,  void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetIJacobian  (IGAForm form,IGAFormIJacobian   IJacobian,  void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetI2Function (IGAForm form,IGAFormI2Function  IFunction,  void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetI2Jacobian (IGAForm form,IGAFormI2Jacobian  IJacobian,  void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetIEFunction (IGAForm form,IGAFormIEFunction  IEFunction, void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetIEJacobian (IGAForm form,IGAFormIEJacobian  IEJacobian, void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetRHSFunction(IGAForm form,IGAFormRHSFunction RHSFunction,void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetRHSJacobian(IGAForm form,IGAFormRHSJacobian RHSJacobian,void *ctx);

PETSC_EXTERN PetscErrorCode IGASetFixTable(IGA iga,Vec table);
PETSC_EXTERN PetscErrorCode IGASetBoundaryValue(IGA iga,PetscInt axis,PetscInt side,PetscInt field,PetscScalar value);
PETSC_EXTERN PetscErrorCode IGASetBoundaryLoad (IGA iga,PetscInt axis,PetscInt side,PetscInt field,PetscScalar value);
PETSC_EXTERN PetscErrorCode IGASetBoundaryForm (IGA iga,PetscInt axis,PetscInt side,PetscBool flag);

PETSC_EXTERN PetscErrorCode IGASetFormVector     (IGA iga,IGAFormVector      Vector,     void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormMatrix     (IGA iga,IGAFormMatrix      Matrix,     void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormSystem     (IGA iga,IGAFormSystem      System,     void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormFunction   (IGA iga,IGAFormFunction    Function,   void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormJacobian   (IGA iga,IGAFormJacobian    Jacobian,   void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormIFunction  (IGA iga,IGAFormIFunction   IFunction,  void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormIJacobian  (IGA iga,IGAFormIJacobian   IJacobian,  void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormI2Function (IGA iga,IGAFormI2Function  IFunction,  void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormI2Jacobian (IGA iga,IGAFormI2Jacobian  IJacobian,  void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormIEFunction (IGA iga,IGAFormIEFunction  IEFunction, void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormIEJacobian (IGA iga,IGAFormIEJacobian  IEJacobian, void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormRHSFunction(IGA iga,IGAFormRHSFunction RHSFunction,void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormRHSJacobian(IGA iga,IGAFormRHSJacobian RHSJacobian,void *ctx);

/* ---------------------------------------------------------------- */

typedef struct _IGAOps *IGAOps;
struct _IGAOps {
  PetscErrorCode (*create)(IGA);
  PetscErrorCode (*destroy)(IGA);
  PetscErrorCode (*view)(IGA);
  PetscErrorCode (*setup)(IGA);
  PetscErrorCode (*setfromoptions)(IGA);
};

struct _p_IGA {
  PETSCHEADER(struct _IGAOps);
  PetscBool  setup;
  PetscInt   setupstage;
  PetscBool  collocation;

  VecType    vectype;
  MatType    mattype;
  char       **fieldname;

  PetscInt  dim;   /* parametric dimension of the function space*/
  PetscInt  dof;   /* number of degrees of freedom per node */
  PetscInt  order; /* maximum derivative order */

  IGAAxis     axis[3];
  IGARule     rule[3];
  IGABasis    basis[3];
  IGAForm     form;

  IGAElement  iterator;

  PetscBool   rational;
  PetscInt    geometry;
  PetscInt    property;
  PetscReal   *rationalW;
  PetscReal   *geometryX;
  PetscScalar *propertyA;

  PetscBool   fixtable;
  PetscScalar *fixtableU;

  PetscInt  proc_sizes[3];
  PetscInt  proc_ranks[3];

  PetscInt  elem_sizes[3];
  PetscInt  elem_start[3];
  PetscInt  elem_width[3];

  PetscInt  geom_sizes[3];
  PetscInt  geom_lstart[3];
  PetscInt  geom_lwidth[3];
  PetscInt  geom_gstart[3];
  PetscInt  geom_gwidth[3];

  PetscInt  node_shift[3];
  PetscInt  node_sizes[3];
  PetscInt  node_lstart[3];
  PetscInt  node_lwidth[3];
  PetscInt  node_gstart[3];
  PetscInt  node_gwidth[3];

  AO          ao;
  LGMap       lgmap;
  PetscLayout map;
  VecScatter  g2l,l2g,l2l;
  PetscInt    nwork;
  Vec         vwork[16];
  Vec         natural;
  VecScatter  n2g,g2n;

  DM elem_dm;
  DM geom_dm;
  DM node_dm;
  DM draw_dm;
};

PETSC_EXTERN PetscClassId IGA_CLASSID;
#define IGA_FILE_CLASSID 1211299

PETSC_EXTERN PetscErrorCode IGAInitializePackage(void);
PETSC_EXTERN PetscErrorCode IGAFinalizePackage(void);
PETSC_EXTERN PetscErrorCode IGARegisterAll(void);

PETSC_EXTERN PetscErrorCode IGACreate(MPI_Comm comm,IGA *iga);
PETSC_EXTERN PetscErrorCode IGADestroy(IGA *iga);
PETSC_EXTERN PetscErrorCode IGAReset(IGA iga);
PETSC_EXTERN PetscErrorCode IGASetUp(IGA iga);
PETSC_EXTERN PetscErrorCode IGAView(IGA iga,PetscViewer viewer);

PETSC_EXTERN PetscErrorCode IGAGetOptionsPrefix(IGA iga,const char *prefix[]);
PETSC_EXTERN PetscErrorCode IGASetOptionsPrefix(IGA iga,const char prefix[]);
PETSC_EXTERN PetscErrorCode IGAPrependOptionsPrefix(IGA iga,const char prefix[]);
PETSC_EXTERN PetscErrorCode IGAAppendOptionsPrefix(IGA iga,const char prefix[]);
PETSC_EXTERN PetscErrorCode IGASetFromOptions(IGA iga);
PETSC_EXTERN PetscErrorCode IGAViewFromOptions(IGA iga,PetscObject obj,const char name[]);
PETSC_EXTERN PetscErrorCode IGAOptionsAlias(const char name[],const char defval[],const char alias[]);

PETSC_EXTERN PetscErrorCode IGALoad(IGA iga,PetscViewer viewer);
PETSC_EXTERN PetscErrorCode IGASave(IGA iga,PetscViewer viewer);
PETSC_EXTERN PetscErrorCode IGARead(IGA iga,const char filename[]);
PETSC_EXTERN PetscErrorCode IGAWrite(IGA iga,const char filename[]);
PETSC_EXTERN PetscErrorCode IGAPrint(IGA iga,PetscViewer viewer);
PETSC_EXTERN PetscErrorCode IGADraw(IGA iga,PetscViewer viewer);

PETSC_EXTERN PetscErrorCode IGASetGeometryDim(IGA iga,PetscInt dim);
PETSC_EXTERN PetscErrorCode IGAGetGeometryDim(IGA iga,PetscInt *dim);
PETSC_EXTERN PetscErrorCode IGALoadGeometry(IGA iga,PetscViewer viewer);
PETSC_EXTERN PetscErrorCode IGASaveGeometry(IGA iga,PetscViewer viewer);

PETSC_EXTERN PetscErrorCode IGASetPropertyDim(IGA iga,PetscInt dim);
PETSC_EXTERN PetscErrorCode IGAGetPropertyDim(IGA iga,PetscInt *dim);
PETSC_EXTERN PetscErrorCode IGALoadProperty(IGA iga,PetscViewer viewer);
PETSC_EXTERN PetscErrorCode IGASaveProperty(IGA iga,PetscViewer viewer);

PETSC_EXTERN PetscErrorCode IGALoadVec(IGA iga,Vec vec,PetscViewer viewer);
PETSC_EXTERN PetscErrorCode IGASaveVec(IGA iga,Vec vec,PetscViewer viewer);
PETSC_EXTERN PetscErrorCode IGADrawVec(IGA iga,Vec vec,PetscViewer viewer);
PETSC_EXTERN PetscErrorCode IGAReadVec(IGA iga,Vec vec,const char filename[]);
PETSC_EXTERN PetscErrorCode IGAWriteVec(IGA iga,Vec vec,const char filename[]);
PETSC_EXTERN PetscErrorCode IGADrawVecVTK(IGA iga,Vec vec,const char filename[]);

PETSC_EXTERN PetscErrorCode IGASetDim(IGA iga,PetscInt dim);
PETSC_EXTERN PetscErrorCode IGAGetDim(IGA iga,PetscInt *dim);
PETSC_EXTERN PetscErrorCode IGASetDof(IGA iga,PetscInt dof);
PETSC_EXTERN PetscErrorCode IGAGetDof(IGA iga,PetscInt *dof);
PETSC_EXTERN PetscErrorCode IGASetName(IGA iga,const char name[]);
PETSC_EXTERN PetscErrorCode IGAGetName(IGA iga,const char *name[]);
PETSC_EXTERN PetscErrorCode IGASetFieldName(IGA iga,PetscInt field,const char name[]);
PETSC_EXTERN PetscErrorCode IGAGetFieldName(IGA iga,PetscInt field,const char *name[]);
PETSC_EXTERN PetscErrorCode IGASetOrder(IGA iga,PetscInt order);
PETSC_EXTERN PetscErrorCode IGAGetOrder(IGA iga,PetscInt *order);
PETSC_EXTERN PetscErrorCode IGASetProcessors(IGA iga,PetscInt i,PetscInt processors);
PETSC_EXTERN PetscErrorCode IGASetBasisType(IGA iga,PetscInt i,IGABasisType type);
PETSC_EXTERN PetscErrorCode IGASetRuleType(IGA iga,PetscInt i,IGARuleType type);
PETSC_EXTERN PetscErrorCode IGASetRuleSize(IGA iga,PetscInt i,PetscInt nqp);
PETSC_EXTERN PetscErrorCode IGASetQuadrature(IGA iga,PetscInt i,PetscInt q);
PETSC_EXTERN PetscErrorCode IGASetUseCollocation(IGA iga,PetscBool collocation);

PETSC_EXTERN PetscErrorCode IGAGetComm(IGA iga,MPI_Comm *comm);
PETSC_EXTERN PetscErrorCode IGAGetAxis(IGA iga,PetscInt i,IGAAxis *axis);
PETSC_EXTERN PetscErrorCode IGAGetRule(IGA iga,PetscInt i,IGARule *rule);
PETSC_EXTERN PetscErrorCode IGAGetBasis(IGA iga,PetscInt i,IGABasis *basis);

PETSC_EXTERN PetscErrorCode IGACreateDMDA(IGA iga,PetscInt bs,
                                          const PetscInt gsizes[],
                                          const PetscInt lsizes[],
                                          const PetscBool periodic[],
                                          PetscBool stencil_box,
                                          PetscInt stencil_width,
                                          DM *dm);
PETSC_EXTERN PetscErrorCode IGACreateElemDM(IGA iga,PetscInt bs,DM *dm);
PETSC_EXTERN PetscErrorCode IGACreateGeomDM(IGA iga,PetscInt bs,DM *dm);
PETSC_EXTERN PetscErrorCode IGACreateNodeDM(IGA iga,PetscInt bs,DM *dm);
PETSC_EXTERN PetscErrorCode IGAGetElemDM(IGA iga,DM *dm);
PETSC_EXTERN PetscErrorCode IGAGetGeomDM(IGA iga,DM *dm);
PETSC_EXTERN PetscErrorCode IGAGetNodeDM(IGA iga,DM *dm);

PETSC_EXTERN PetscErrorCode IGASetVecType(IGA iga,const VecType vectype);
PETSC_EXTERN PetscErrorCode IGASetMatType(IGA iga,const MatType mattype);

PETSC_EXTERN PetscErrorCode IGACreateVec(IGA iga,Vec *vec);
PETSC_EXTERN PetscErrorCode IGACreateMat(IGA iga,Mat *mat);

PETSC_EXTERN PetscErrorCode IGACreateCoordinates(IGA iga,Vec *coords);
PETSC_EXTERN PetscErrorCode IGACreateRigidBody(IGA iga,MatNullSpace *nsp);

PETSC_EXTERN PetscErrorCode IGACreateLocalVec(IGA iga, Vec *lvec);
PETSC_EXTERN PetscErrorCode IGAGetLocalVec(IGA iga,Vec *lvec);
PETSC_EXTERN PetscErrorCode IGARestoreLocalVec(IGA iga,Vec *lvec);
PETSC_EXTERN PetscErrorCode IGAGlobalToLocalBegin(IGA iga,Vec gvec,Vec lvec,InsertMode addv);
PETSC_EXTERN PetscErrorCode IGAGlobalToLocalEnd  (IGA iga,Vec gvec,Vec lvec,InsertMode addv);
PETSC_EXTERN PetscErrorCode IGAGlobalToLocal     (IGA iga,Vec gvec,Vec lvec,InsertMode addv);
PETSC_EXTERN PetscErrorCode IGALocalToGlobalBegin(IGA iga,Vec lvec,Vec gvec,InsertMode addv);
PETSC_EXTERN PetscErrorCode IGALocalToGlobalEnd  (IGA iga,Vec lvec,Vec gvec,InsertMode addv);
PETSC_EXTERN PetscErrorCode IGALocalToGlobal     (IGA iga,Vec lvec,Vec gvec,InsertMode addv);
PETSC_EXTERN PetscErrorCode IGALocalToLocalBegin (IGA iga,Vec gvec,Vec lvec,InsertMode addv);
PETSC_EXTERN PetscErrorCode IGALocalToLocalEnd   (IGA iga,Vec gvec,Vec lvec,InsertMode addv);
PETSC_EXTERN PetscErrorCode IGALocalToLocal      (IGA iga,Vec gvec,Vec lvec,InsertMode addv);

PETSC_EXTERN PetscErrorCode IGAGetNaturalVec(IGA iga,Vec *nvec);
PETSC_EXTERN PetscErrorCode IGANaturalToGlobal(IGA iga,Vec nvec,Vec gvec);
PETSC_EXTERN PetscErrorCode IGAGlobalToNatural(IGA iga,Vec gvec,Vec nvec);

PETSC_EXTERN PetscErrorCode IGAGetLocalVecArray(IGA iga,Vec gvec,Vec *lvec,const PetscScalar *array[]);
PETSC_EXTERN PetscErrorCode IGARestoreLocalVecArray(IGA iga,Vec gvec,Vec *lvec,const PetscScalar *array[]);

PETSC_EXTERN PetscErrorCode IGAClone(IGA iga,PetscInt dof,IGA *newiga);

#define DMIGA "iga"
PETSC_EXTERN PetscErrorCode DMIGACreate(IGA iga,DM *dm);
PETSC_EXTERN PetscErrorCode DMIGASetIGA(DM dm,IGA iga);
PETSC_EXTERN PetscErrorCode DMIGAGetIGA(DM dm,IGA *iga);

/* ---------------------------------------------------------------- */

struct _n_IGAElement {
  PetscInt refct;
  /**/
  PetscInt  start[3];
  PetscInt  width[3];
  PetscInt  sizes[3];
  PetscInt  ID[3];
  /**/
  PetscBool atboundary;
  PetscInt  boundary_id;
  /**/
  PetscInt count;
  PetscInt index;
  /**/
  PetscInt neq;
  PetscInt nen;
  PetscInt dof;
  PetscInt dim;
  PetscInt nsd;
  PetscInt npd;

  PetscInt    *mapping;   /*[nen]      */
  PetscInt    *rowmap;    /*[neq]      */
  PetscInt    *colmap;    /*[nen]      */
  PetscBool   rational;
  PetscReal   *rationalW; /*[nen]      */
  PetscBool   geometry;
  PetscReal   *geometryX; /*[nen][nsd] */
  PetscBool   property;
  PetscScalar *propertyA; /*[nen][npd] */

  PetscInt  nqp;
  PetscInt  sqp[3];

  PetscReal *weight;   /*   [nqp] */
  PetscReal *detJac;   /*   [nqp] */

  PetscReal *basis[5]; /*0: [nqp][nen] */
                       /*1: [nqp][nen][dim] */
                       /*2: [nqp][nen][dim][dim] */
                       /*3: [nqp][nen][dim][dim][dim] */
                       /*4: [nqp][nen][dim][dim][dim][dim] */

  PetscReal *shape[5]; /*0: [nqp][nen] */
                       /*1: [nqp][nen][nsd] */
                       /*2: [nqp][nen][nsd][nsd] */
                       /*3: [nqp][nen][nsd][nsd][nsd] */
                       /*4: [nqp][nen][nsd][nsd][nsd][nsd] */

  PetscReal *mapU[5];  /*0: [nqp][dim] */
                       /*1: [nqp][dim][nsd] */
                       /*2: [nqp][dim][nsd][nsd] */
                       /*3: [nqp][dim][nsd][nsd][nsd] */
                       /*4: [nqp][dim][nsd][nsd][nsd][nsd] */

  PetscReal *mapX[5];  /*0: [nqp][nsd] */
                       /*1: [nqp][nsd][dim] */
                       /*2: [nqp][nsd][dim][dim] */
                       /*3: [nqp][nsd][dim][dim][dim] */
                       /*4: [nqp][nsd][dim][dim][dim][dim] */

  PetscReal *normal;   /*   [nqp][nsd] */
  PetscReal *detX;     /*   [nqp] */
  PetscReal *detS;     /*   [nqp] */

  IGA      parent;
  IGAPoint iterator;

  PetscBool   collocation;

  PetscInt     nfix;
  PetscInt    *ifix;
  PetscScalar *vfix;
  PetscScalar *ufix;

  PetscInt     nflux;
  PetscInt    *iflux;
  PetscScalar *vflux;

  PetscInt    nval;
  PetscScalar *wval[8];
  PetscInt    nvec;
  PetscScalar *wvec[8];
  PetscInt    nmat;
  PetscScalar *wmat[4];

};

PETSC_EXTERN PetscErrorCode IGAElementCreate(IGAElement *element);
PETSC_EXTERN PetscErrorCode IGAElementDestroy(IGAElement *element);
PETSC_EXTERN PetscErrorCode IGAElementReference(IGAElement element);
PETSC_EXTERN PetscErrorCode IGAElementReset(IGAElement element);
PETSC_EXTERN PetscErrorCode IGAElementInit(IGAElement element,IGA iga);

PETSC_EXTERN PetscErrorCode IGAGetElement(IGA iga,IGAElement *element);
PETSC_EXTERN PetscErrorCode IGABeginElement(IGA iga,IGAElement *element);
PETSC_EXTERN PetscBool      IGANextElement(IGA iga,IGAElement element);
PETSC_EXTERN PetscErrorCode IGAEndElement(IGA iga,IGAElement *element);
PETSC_EXTERN PetscBool      IGAElementNextForm(IGAElement element,PetscBool visit[][2]);
PETSC_EXTERN PetscErrorCode IGAElementGetPoint(IGAElement element,IGAPoint *point);
PETSC_EXTERN PetscErrorCode IGAElementBeginPoint(IGAElement element,IGAPoint *point);
PETSC_EXTERN PetscBool      IGAElementNextPoint(IGAElement element,IGAPoint point);
PETSC_EXTERN PetscErrorCode IGAElementEndPoint(IGAElement element,IGAPoint *point);

PETSC_EXTERN PetscErrorCode IGAElementBuildClosure(IGAElement element);
PETSC_EXTERN PetscErrorCode IGAElementBuildTabulation(IGAElement element);

PETSC_EXTERN PetscErrorCode IGAElementGetParent(IGAElement element,IGA *parent);
PETSC_EXTERN PetscErrorCode IGAElementGetIndex(IGAElement element,PetscInt *index);
PETSC_EXTERN PetscErrorCode IGAElementGetCount(IGAElement element,PetscInt *count);
PETSC_EXTERN PetscErrorCode IGAElementGetSizes(IGAElement element,PetscInt *neq,PetscInt *nen,PetscInt *dof);
PETSC_EXTERN PetscErrorCode IGAElementGetClosure(IGAElement element,PetscInt *nen,const PetscInt *mapping[]);
PETSC_EXTERN PetscErrorCode IGAElementGetIndices(IGAElement element,
                                                 PetscInt *neq,const PetscInt *rowmap[],
                                                 PetscInt *nen,const PetscInt *colmap[]);

PETSC_EXTERN PetscErrorCode IGAElementGetWorkVec(IGAElement element,PetscScalar *V[]);
PETSC_EXTERN PetscErrorCode IGAElementGetWorkMat(IGAElement element,PetscScalar *M[]);
PETSC_EXTERN PetscErrorCode IGAElementGetValues(IGAElement element,const PetscScalar arrayU[],PetscScalar *U[]);

PETSC_EXTERN PetscErrorCode IGAElementBuildFix(IGAElement element);
PETSC_EXTERN PetscErrorCode IGAElementDelValues(IGAElement element,PetscScalar V[]);
PETSC_EXTERN PetscErrorCode IGAElementFixValues(IGAElement element,PetscScalar U[]);
PETSC_EXTERN PetscErrorCode IGAElementFixSystem(IGAElement element,PetscScalar K[],PetscScalar F[]);
PETSC_EXTERN PetscErrorCode IGAElementFixFunction(IGAElement element,PetscScalar F[]);
PETSC_EXTERN PetscErrorCode IGAElementFixJacobian(IGAElement element,PetscScalar J[]);

PETSC_EXTERN PetscErrorCode IGAElementAssembleVec(IGAElement element,const PetscScalar F[],Vec vec);
PETSC_EXTERN PetscErrorCode IGAElementAssembleMat(IGAElement element,const PetscScalar K[],Mat mat);

/* ---------------------------------------------------------------- */

struct _n_IGAPoint {
  PetscInt refct;
  /**/
  PetscBool atboundary;
  PetscInt  boundary_id;
  /**/
  PetscInt count;
  PetscInt index;
  /**/
  PetscInt neq;
  PetscInt nen;
  PetscInt dof;
  PetscInt dim;
  PetscInt nsd;
  PetscInt npd;

  PetscReal   *rational;/*  [nen] */
  PetscReal   *geometry;/*  [nen][nsd] */
  PetscScalar *property;/*  [nen][npd] */

  PetscReal *weight;   /*   [1] */
  PetscReal *detJac;   /*   [1] */

  PetscReal *point;    /*   [dim] */
  PetscReal *normal;   /*   [nsd] */

  PetscReal *basis[5]; /*0: [nen] */
                       /*1: [nen][dim] */
                       /*2: [nen][dim][dim] */
                       /*3: [nen][dim][dim][dim] */
                       /*4: [nen][dim][dim][dim][dim] */

  PetscReal *shape[5]; /*0: [nen]  */
                       /*1: [nen][nsd] */
                       /*2: [nen][nsd][nsd] */
                       /*3: [nen][nsd][nsd][nsd] */
                       /*3: [nen][nsd][nsd][nsd][nsd] */

  PetscReal *mapU[5];  /*0: [dim] */
                       /*1: [dim][nsd] */
                       /*2: [dim][nsd][nsd] */
                       /*3: [dim][nsd][nsd][nsd] */
                       /*4: [dim][nsd][nsd][nsd][nsd] */

  PetscReal *mapX[5];  /*0: [nsd] */
                       /*1: [nsd][dim] */
                       /*2: [nsd][dim][dim] */
                       /*3: [nsd][dim][dim][dim] */
                       /*4: [nsd][dim][dim][dim][dim] */

  PetscReal *detX;     /*   [1] */
  PetscReal *detS;     /*   [1] */

  IGAElement parent;

  PetscInt    nvec;
  PetscScalar *wvec[8];
  PetscInt    nmat;
  PetscScalar *wmat[4];
};

PETSC_EXTERN PetscErrorCode IGAPointCreate(IGAPoint *point);
PETSC_EXTERN PetscErrorCode IGAPointDestroy(IGAPoint *point);
PETSC_EXTERN PetscErrorCode IGAPointReference(IGAPoint point);
PETSC_EXTERN PetscErrorCode IGAPointReset(IGAPoint point);
PETSC_EXTERN PetscErrorCode IGAPointInit(IGAPoint point,IGAElement element);

PETSC_EXTERN PetscErrorCode IGAPointGetParent(IGAPoint point,IGAElement *element);
PETSC_EXTERN PetscErrorCode IGAPointGetIndex(IGAPoint point,PetscInt *index);
PETSC_EXTERN PetscErrorCode IGAPointGetCount(IGAPoint point,PetscInt *count);
PETSC_EXTERN PetscErrorCode IGAPointAtBoundary(IGAPoint point,PetscInt *axis,PetscInt *side);
PETSC_EXTERN PetscErrorCode IGAPointGetSizes(IGAPoint point,PetscInt *neq,PetscInt *nen,PetscInt *dof);
PETSC_EXTERN PetscErrorCode IGAPointGetDims(IGAPoint point,PetscInt *dim,PetscInt *nsd,PetscInt *npd);
PETSC_EXTERN PetscErrorCode IGAPointGetQuadrature(IGAPoint point,PetscReal *weigth,PetscReal *detJac);
PETSC_EXTERN PetscErrorCode IGAPointGetBasisFuns(IGAPoint point,PetscInt der,const PetscReal *basisfuns[]);
PETSC_EXTERN PetscErrorCode IGAPointGetShapeFuns(IGAPoint point,PetscInt der,const PetscReal *shapefuns[]);

PETSC_EXTERN PetscErrorCode IGAPointFormPoint(IGAPoint p,PetscReal x[]);
PETSC_EXTERN PetscErrorCode IGAPointFormGeomMap(IGAPoint p,PetscReal x[]);
PETSC_EXTERN PetscErrorCode IGAPointFormGradGeomMap(IGAPoint p,PetscReal F[]);
PETSC_EXTERN PetscErrorCode IGAPointFormInvGradGeomMap(IGAPoint p,PetscReal G[]);

PETSC_EXTERN PetscErrorCode IGAPointEvaluate (IGAPoint p,PetscInt ider,const PetscScalar U[],PetscScalar u[]);
PETSC_EXTERN PetscErrorCode IGAPointFormValue(IGAPoint p,const PetscScalar U[],PetscScalar u[]);
PETSC_EXTERN PetscErrorCode IGAPointFormGrad (IGAPoint p,const PetscScalar U[],PetscScalar u[]);
PETSC_EXTERN PetscErrorCode IGAPointFormHess (IGAPoint p,const PetscScalar U[],PetscScalar u[]);
PETSC_EXTERN PetscErrorCode IGAPointFormDel2 (IGAPoint p,const PetscScalar U[],PetscScalar u[]);
PETSC_EXTERN PetscErrorCode IGAPointFormDer3 (IGAPoint p,const PetscScalar U[],PetscScalar u[]);
PETSC_EXTERN PetscErrorCode IGAPointFormDer4 (IGAPoint p,const PetscScalar U[],PetscScalar u[]);

PETSC_EXTERN PetscErrorCode IGAPointGetWorkVec(IGAPoint point,PetscScalar *V[]);
PETSC_EXTERN PetscErrorCode IGAPointGetWorkMat(IGAPoint point,PetscScalar *M[]);
PETSC_EXTERN PetscErrorCode IGAPointAddArray(IGAPoint point,PetscInt n,const PetscScalar a[],PetscScalar A[]);
PETSC_EXTERN PetscErrorCode IGAPointAddVec(IGAPoint point,const PetscScalar f[],PetscScalar F[]);
PETSC_EXTERN PetscErrorCode IGAPointAddMat(IGAPoint point,const PetscScalar k[],PetscScalar K[]);

/* ---------------------------------------------------------------- */

struct _n_IGAProbe {
  PetscInt refct;
  /**/
  IGA iga;
  Vec gvec;
  Vec lvec;
  /**/
  PetscInt  order;
  PetscBool collective;
  PetscBool offprocess;
  /**/
  PetscInt  dim;
  PetscInt  nsd;
  PetscInt  dof;
  PetscInt  p[3];
  PetscReal *U[3];
  PetscInt  n[3];
  PetscInt  s[3];
  PetscInt  w[3];
  const PetscReal   *arrayW;
  const PetscReal   *arrayX;
  const PetscScalar *arrayA;
  /**/
  PetscInt    nen;
  PetscInt    *map;
  PetscReal   *W;
  PetscReal   *X;
  PetscScalar *A;
  /**/
  PetscReal point[3];
  PetscInt  ID[3];
  PetscReal *BD[3];
  PetscReal *basis[5]; /*0: [nen] */
                       /*1: [nen][dim] */
                       /*2: [nen][dim][dim] */
                       /*3: [nen][dim][dim][dim] */
                       /*4: [nen][dim][dim][dim][dim] */
  PetscReal *shape[5]; /*0: [nen] */
                       /*1: [nen][nsd] */
                       /*2: [nen][nsd][nsd] */
                       /*3: [nen][nsd][nsd][nsd] */
                       /*4: [nen][nsd][nsd][nsd][nsd] */
  PetscReal *mapU[5];  /*0: [dim] */
                       /*1: [dim][nsd] */
                       /*2: [dim][nsd][nsd] */
                       /*3: [dim][nsd][nsd][nsd] */
                       /*4: [dim][nsd][nsd][nsd][nsd] */
  PetscReal *mapX[5];  /*0: [nsd] */
                       /*1: [nsd][dim] */
                       /*2: [nsd][dim][dim] */
                       /*3: [nsd][dim][dim][dim] */
                       /*4: [nsd][dim][dim][dim][dim] */
  PetscReal *detX;
};

PETSC_EXTERN PetscErrorCode IGAProbeCreate(IGA iga,Vec A,IGAProbe *prb);
PETSC_EXTERN PetscErrorCode IGAProbeDestroy(IGAProbe *prb);
PETSC_EXTERN PetscErrorCode IGAProbeReference(IGAProbe prb);

PETSC_EXTERN PetscErrorCode IGAProbeSetOrder(IGAProbe prb,PetscInt order);
PETSC_EXTERN PetscErrorCode IGAProbeSetCollective(IGAProbe prb,PetscBool collective);

PETSC_EXTERN PetscErrorCode IGAProbeSetVec(IGAProbe prb,Vec A);
PETSC_EXTERN PetscErrorCode IGAProbeSetPoint(IGAProbe prb,const PetscReal u[]);

PETSC_EXTERN PetscErrorCode IGAProbeGeomMap  (IGAProbe prb,PetscReal x[]);
PETSC_EXTERN PetscErrorCode IGAProbeEvaluate (IGAProbe prb,PetscInt der,PetscScalar A[]);
PETSC_EXTERN PetscErrorCode IGAProbeFormValue(IGAProbe prb,PetscScalar A[]);
PETSC_EXTERN PetscErrorCode IGAProbeFormGrad (IGAProbe prb,PetscScalar A[]);
PETSC_EXTERN PetscErrorCode IGAProbeFormHess (IGAProbe prb,PetscScalar A[]);
PETSC_EXTERN PetscErrorCode IGAProbeFormDer3 (IGAProbe prb,PetscScalar A[]);
PETSC_EXTERN PetscErrorCode IGAProbeFormDer4 (IGAProbe prb,PetscScalar A[]);

/* ---------------------------------------------------------------- */

PETSC_EXTERN PetscLogEvent IGA_FormScalar;
PETSC_EXTERN PetscLogEvent IGA_FormVector;
PETSC_EXTERN PetscLogEvent IGA_FormMatrix;
PETSC_EXTERN PetscLogEvent IGA_FormSystem;
PETSC_EXTERN PetscLogEvent IGA_FormFunction;
PETSC_EXTERN PetscLogEvent IGA_FormJacobian;
PETSC_EXTERN PetscLogEvent IGA_FormIFunction;
PETSC_EXTERN PetscLogEvent IGA_FormIJacobian;

PETSC_EXTERN PetscErrorCode IGAComputeErrorNorm(IGA iga,PetscInt k,
                                                Vec vecU,IGAFormExact Exact,
                                                PetscReal enorm[],void *ctx);

PETSC_EXTERN PetscErrorCode IGAComputeScalar(IGA iga,Vec U,
                                             PetscInt n,PetscScalar S[],
                                             IGAFormScalar Scalar,void *ctx);

#define PCIGAEBE "igaebe"
#define PCIGABBB "igabbb"

PETSC_EXTERN PetscErrorCode IGAComputeVector(IGA iga,Vec B);
PETSC_EXTERN PetscErrorCode IGAComputeMatrix(IGA iga,Mat A);
PETSC_EXTERN PetscErrorCode IGAComputeSystem(IGA iga,Mat A,Vec B);

PETSC_EXTERN PetscErrorCode IGAComputeFunction(IGA iga,Vec U,Vec F);
PETSC_EXTERN PetscErrorCode IGAComputeJacobian(IGA iga,Vec U,Mat J);

PETSC_EXTERN PetscErrorCode IGAComputeIFunction(IGA iga,
                                                PetscReal a,Vec V,
                                                PetscReal t,Vec U,
                                                Vec F);
PETSC_EXTERN PetscErrorCode IGAComputeIJacobian(IGA iga,
                                                PetscReal a,Vec V,
                                                PetscReal t,Vec U,
                                                Mat J);
PETSC_EXTERN PetscErrorCode IGAComputeIEFunction(IGA iga,
                                                 PetscReal a, Vec V,
                                                 PetscReal t, Vec U,
                                                 PetscReal t0,Vec U0,
                                                 Vec F);
PETSC_EXTERN PetscErrorCode IGAComputeIEJacobian(IGA iga,
                                                 PetscReal a, Vec V,
                                                 PetscReal t, Vec U,
                                                 PetscReal t0,Vec U0,
                                                 Mat J);
PETSC_EXTERN PetscErrorCode IGAComputeRHSFunction(IGA iga,
                                                  PetscReal t,Vec U,
                                                  Vec F);
PETSC_EXTERN PetscErrorCode IGAComputeRHSJacobian(IGA iga,
                                                  PetscReal t,Vec U,
                                                  Mat J);
PETSC_EXTERN PetscErrorCode IGAComputeI2Function(IGA iga,
                                                 PetscReal a,Vec vecA,
                                                 PetscReal v,Vec vecV,
                                                 PetscReal t,Vec vecU,
                                                 Vec vecF);
PETSC_EXTERN PetscErrorCode IGAComputeI2Jacobian(IGA iga,
                                                 PetscReal a,Vec vecA,
                                                 PetscReal v,Vec vecV,
                                                 PetscReal t,Vec vecU,
                                                 Mat matJ);

PETSC_EXTERN PetscErrorCode KSPSetIGA(KSP ksp,IGA iga);
PETSC_EXTERN PetscErrorCode SNESSetIGA(SNES snes,IGA iga);
PETSC_EXTERN PetscErrorCode TSSetIGA(TS ts,IGA iga);

PETSC_EXTERN PetscErrorCode IGACreateKSP(IGA iga,KSP *ksp);
PETSC_EXTERN PetscErrorCode IGACreateSNES(IGA iga,SNES *snes);
PETSC_EXTERN PetscErrorCode IGACreateTS(IGA iga,TS *ts);
PETSC_EXTERN PetscErrorCode IGACreateTS2(IGA iga, TS *ts);

/* ---------------------------------------------------------------- */

PETSC_EXTERN PetscErrorCode IGAKSPFormRHS(KSP,Vec,void*);
PETSC_EXTERN PetscErrorCode IGAKSPFormOperators(KSP,Mat,Mat,void*);
PETSC_EXTERN PetscErrorCode IGASNESFormFunction(SNES,Vec,Vec,void*);
PETSC_EXTERN PetscErrorCode IGASNESFormJacobian(SNES,Vec,Mat,Mat,void*);
PETSC_EXTERN PetscErrorCode IGATSFormIFunction(TS,PetscReal,Vec,Vec,Vec,void*);
PETSC_EXTERN PetscErrorCode IGATSFormIJacobian(TS,PetscReal,Vec,Vec,PetscReal,Mat,Mat,void*);
PETSC_EXTERN PetscErrorCode IGATSFormI2Function(TS,PetscReal,Vec,Vec,Vec,Vec,void*);
PETSC_EXTERN PetscErrorCode IGATSFormI2Jacobian(TS,PetscReal,Vec,Vec,Vec,PetscReal,PetscReal,Mat,Mat,void*);

PETSC_EXTERN PetscErrorCode IGAPreparePCMG(IGA iga,PC pc);
PETSC_EXTERN PetscErrorCode IGAPreparePCBDDC(IGA iga,PC pc);
PETSC_EXTERN PetscErrorCode IGAPreparePCH2OPUS(IGA iga,PC pc);
PETSC_EXTERN PetscErrorCode IGASetOptionsHandlerPC(PC pc);
PETSC_EXTERN PetscErrorCode IGASetOptionsHandlerKSP(KSP ksp);
PETSC_EXTERN PetscErrorCode IGASetOptionsHandlerSNES(SNES snes);
PETSC_EXTERN PetscErrorCode IGASetOptionsHandlerTS(TS ts);

/* ---------------------------------------------------------------- */

#if defined(PETSC_USE_DEBUG)
#define IGACheckSetUp(iga,arg) do {                                     \
    if (PetscUnlikely(!(iga)->setup))                                   \
      SETERRQ(PETSC_COMM_SELF,PETSC_ERR_ARG_WRONGSTATE,                 \
              "Must call IGASetUp() on argument %D \"%s\" before %s()", \
              (arg),#iga,PETSC_FUNCTION_NAME);                          \
    } while (0)
#define IGACheckSetUpStage(iga,arg,stg) do {                            \
    if (PetscUnlikely((iga)->setupstage<(stg))) IGACheckSetUp(iga,arg); \
    } while (0)
#else
#define IGACheckSetUp(iga,arg)          do {} while (0)
#define IGACheckSetUpStage(iga,arg,stg) do {} while (0)
#endif
#define IGACheckSetUpStage1(iga,arg) IGACheckSetUpStage(iga,arg,1)
#define IGACheckSetUpStage2(iga,arg) IGACheckSetUpStage(iga,arg,2)

#if defined(PETSC_USE_DEBUG)
#define IGACheckFormOp(iga,arg,FormOp) do {            \
    if (!iga->form->ops->FormOp)                       \
      SETERRQ(((PetscObject)iga)->comm,PETSC_ERR_USER, \
              "Must call IGASetForm%s() "              \
              "on argument %D \"%s\" before %s()",     \
              #FormOp,(arg),#iga,PETSC_FUNCTION_NAME); \
    } while (0)
#else
#define IGACheckFormOp(iga,arg,FormOp) do {} while (0)
#endif

/* ---------------------------------------------------------------- */

PETSC_EXTERN PetscErrorCode IGAOptionsAlias(const char name[],const char defval[],const char alias[]);
PETSC_EXTERN PetscErrorCode IGAOptionsDefault(const char prefix[],const char name[],const char value[]);
PETSC_EXTERN PetscErrorCode IGAOptionsReject(const char prefix[],const char name[]);

PETSC_EXTERN PetscEnum      IGAGetOptEnum(const char prefix[],const char name[],const char *const elist[],PetscEnum defval);
PETSC_EXTERN const char*    IGAGetOptString(const char prefix[],const char name[],const char defval[]);
PETSC_EXTERN PetscBool      IGAGetOptBool(const char prefix[],const char name[],PetscBool defval);
PETSC_EXTERN PetscInt       IGAGetOptInt(const char prefix[],const char name[],PetscInt defval);
PETSC_EXTERN PetscReal      IGAGetOptReal(const char prefix[],const char name[],PetscReal defval);
PETSC_EXTERN PetscScalar    IGAGetOptScalar(const char prefix[],const char name[],PetscScalar defval);

/* ---------------------------------------------------------------- */

#if PETSC_VERSION_LT(3,8,0)
PETSC_EXTERN PetscErrorCode TSSetMaxSteps(TS,PetscInt);
PETSC_EXTERN PetscErrorCode TSGetMaxSteps(TS,PetscInt*);
PETSC_EXTERN PetscErrorCode TSSetMaxTime(TS,PetscReal);
PETSC_EXTERN PetscErrorCode TSGetMaxTime(TS,PetscReal*);
#endif
#define TSALPHA1 TSALPHA
PETSC_EXTERN PetscErrorCode TSAlphaUseAdapt(TS,PetscBool);
#else

/* ---------------------------------------------------------------- */

#endif/*PETIGA_H*/


// build commmand:
// gcc -c test_fresnel.c -O2 -std=c99 -I../LISAsim -I/opt/local/include
// gcc -o test_fresnel test_fresnel.o struct.o ../LISAsim/LISAnoise.o -lgsl -lgslcblas

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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "splinecoeffs.h"

/* Implementation of the Thomas algorithm to solve a tridiagonal system */
/* Note: assumes numerical stability (e.g. diagonal-dominated matrix) */
static void SolveTridiagThomas(
  gsl_vector* vectx,        /* Output: solution vector, length n - already allocated */
  gsl_vector* vecta,        /* Diagonal of the matrix, length n */
  gsl_vector* vectb,        /* Lower diagonal, length n-1 */
  gsl_vector* vectc,        /* Upper diagonal, length n-1 */
  gsl_vector* vecty,        /* Right-hand-side vector, length n */
  int n)                    /* Length of vectx, vecty */
{
  /* Check lengths */
  if(!(vectx->size==n && vecty->size==n && vecta->size==n && vectb->size==n-1 && vectc->size==n-1)) {
    printf("Error: incompatible lengths in SolveTridiagThomas.\n");
    exit(1);
  }

  /* Note: we modify the vectors c, y in place */
  double* chat = vectc->data;
  double* yhat = vecty->data;
  double* x = vectx->data;
  double* a = vecta->data;
  double* b = vectb->data;

  /* Sweep forward, computing the chat and yhat values */
  chat[0] = chat[0] / a[0];
  yhat[0] = yhat[0] / a[0];
  double factor;
  for(int i=1; i<=n-2; i++) {
    factor = 1./(a[i] - b[i-1]*chat[i-1]);
    chat[i] = chat[i] * factor;
    yhat[i] = (yhat[i] - b[i-1]*yhat[i-1]) * factor;
  }
  factor = 1./(a[n-1] - b[n-2]*chat[n-2]);
  yhat[n-1] = (yhat[n-1] - b[n-2]*yhat[n-2]) * factor;

  /* Solve for x going backward */
  x[n-1] = yhat[n-1];
  for(int i=n-2; i>=0; i--) {
    x[i] = yhat[i] - chat[i] * x[i+1];
  }
}

void BuildNotAKnotSpline(
  gsl_matrix* splinecoeffs,   /* Output: matrix containing all the spline coeffs (already allocated) */
  gsl_vector* vectx,          /* Input: vector x*/
  gsl_vector* vecty,          /* Input: vector y */
  int n)                      /* Size of x, y, and of output matrix */
{
  /* Check lengths */
  if(!(vectx->size==n && vecty->size==n && splinecoeffs->size1==n && splinecoeffs->size2==5)) {
    printf("Error: incompatible lengths in NotAKnotSpline.\n");
    exit(1);
  }
  double* x = vectx->data;
  double* y = vecty->data;

  /* Computing vecth and vectDeltay */
  gsl_vector* vecth = gsl_vector_alloc(n-1);
  gsl_vector* vectDeltay = gsl_vector_alloc(n-1);
  gsl_vector* vectDeltayoverh = gsl_vector_alloc(n-1);
  double* h = vecth->data;
  double* Deltay = vectDeltay->data;
  double* Deltayoverh = vectDeltayoverh->data;
  for(int i=0; i<n-1; i++) {
    h[i] = x[i+1] - x[i];
    Deltay[i] = y[i+1] - y[i];
    Deltayoverh[i] = Deltay[i] / h[i];
  }

  /* Structures for the tridiagonal system */
  gsl_vector* vectY = gsl_vector_alloc(n-2);
  gsl_vector* vecta = gsl_vector_alloc(n-2);
  gsl_vector* vectb = gsl_vector_alloc(n-3);
  gsl_vector* vectc = gsl_vector_alloc(n-3);
  double* Y = vectY->data;
  double* a = vecta->data;
  double* b = vectb->data;
  double* c = vectc->data;
  for(int i=0; i<=n-3; i++) {
    Y[i] = 3.*(Deltayoverh[i+1] - Deltayoverh[i]);
    a[i] = 2.*(h[i+1] + h[i]);
  }
  for(int i=0; i<=n-4; i++) {
    b[i] = h[i+1];
    c[i] = h[i+1];
  }
  /* Adjusting for the not-a-knot condition */
  a[0] += h[0] + h[0]*h[0]/h[1];
  c[0] += -h[0]*h[0]/h[1];
  a[n-3] += h[n-2] + h[n-2]*h[n-2]/h[n-3];
  b[n-4] += -h[n-2]*h[n-2]/h[n-3];

  /* Solving the tridiagonal system */
  gsl_vector* vectp2 = gsl_vector_alloc(n);
  gsl_vector_view viewp2trunc = gsl_vector_subvector(vectp2, 1, n-2);
  SolveTridiagThomas(&viewp2trunc.vector, vecta, vectb, vectc, vectY, n-2);
  double* p2 = vectp2->data;
  p2[0] = p2[1] - h[0]/h[1] * (p2[2] - p2[1]);
  p2[n-1] = p2[n-2] + h[n-2]/h[n-3] * (p2[n-2] - p2[n-3]);

  /* Deducing the p1's and the p3's */
  gsl_vector* vectp1 = gsl_vector_alloc(n);
  gsl_vector* vectp3 = gsl_vector_alloc(n);
  double* p1 = vectp1->data;
  double* p3 = vectp3->data;
  for(int i=0; i<=n-2; i++) {
    p1[i] = Deltayoverh[i] - h[i]/3. * (p2[i+1] + 2.*p2[i]);
    p3[i] = (p2[i+1] - p2[i]) / (3*h[i]);
  }
  /* Note: p1[n-1], p2[n-1], p3[n-1] are set to values coherent with the derivatives of the spline at the last point, but they are not stricly speaking coefficients of the spline. */
  p1[n-1] = p1[n-2] + 2.*p2[n-2]*h[n-2] + 3.*p3[n-2]*h[n-2]*h[n-2];
  p3[n-1] = p3[n-2];

  /* Copying the results in the output matrix */
  gsl_vector_view viewx = gsl_matrix_column(splinecoeffs, 0);
  gsl_vector_view viewp0 = gsl_matrix_column(splinecoeffs, 1);
  gsl_vector_view viewp1 = gsl_matrix_column(splinecoeffs, 2);
  gsl_vector_view viewp2 = gsl_matrix_column(splinecoeffs, 3);
  gsl_vector_view viewp3 = gsl_matrix_column(splinecoeffs, 4);
  gsl_vector_memcpy(&viewx.vector, vectx);
  gsl_vector_memcpy(&viewp0.vector, vecty);
  gsl_vector_memcpy(&viewp1.vector, vectp1);
  gsl_vector_memcpy(&viewp2.vector, vectp2);
  gsl_vector_memcpy(&viewp3.vector, vectp3);

  /* Cleanup*/
  gsl_vector_free(vecth);
  gsl_vector_free(vectDeltay);
  gsl_vector_free(vectDeltayoverh);
  gsl_vector_free(vectY);
  gsl_vector_free(vecta);
  gsl_vector_free(vectb);
  gsl_vector_free(vectc);
  gsl_vector_free(vectp1);
  gsl_vector_free(vectp2);
  gsl_vector_free(vectp3);
}

void BuildQuadSpline(
  gsl_matrix* splinecoeffs,   /* Output: matrix containing all the spline coeffs (already allocated) */
  gsl_vector* vectx,          /* Input: vector x*/
  gsl_vector* vecty,          /* Input: vector y */
  int n)                      /* Size of x, y, and of output matrix */
{
  /* Check lengths */
  if(!(vectx->size==n && vecty->size==n && splinecoeffs->size1==n && splinecoeffs->size2==4)) {
    printf("Error: incompatible lengths in NotAKnotSpline.\n");
    exit(1);
  }
  double* x = vectx->data;
  double* y = vecty->data;

  /* Computing vecth and vectDeltay */
  gsl_vector* vecth = gsl_vector_alloc(n-1);
  gsl_vector* vectDeltay = gsl_vector_alloc(n-1);
  gsl_vector* vectDeltayoverh = gsl_vector_alloc(n-1);
  double* h = vecth->data;
  double* Deltay = vectDeltay->data;
  double* Deltayoverh = vectDeltayoverh->data;
  for(int i=0; i<n-1; i++) {
    h[i] = x[i+1] - x[i];
    Deltay[i] = y[i+1] - y[i];
    Deltayoverh[i] = Deltay[i] / h[i];
  }

  /* Solving for p1 */
  gsl_vector* vectp1 = gsl_vector_alloc(n);
  double* p1 = vectp1->data;
  double ratio = h[n-2] / h[n-3];
  p1[n-3] = ((2. + ratio)*Deltayoverh[n-3] - Deltayoverh[n-2]) / (1. + ratio);
  p1[n-2] = -p1[n-3] + 2.*Deltayoverh[n-3];
  for(int i=n-4; i>=0; i--) {
    p1[i] = -p1[i+1] + 2.*Deltayoverh[i];
  }
  p1[n-1] = (1. + ratio)*p1[n-2] - ratio*p1[n-3];

  /* Deducing the p2's */
  gsl_vector* vectp2 = gsl_vector_alloc(n);
  double* p2 = vectp2->data;
  for(int i=0; i<=n-2; i++) {
    p2[i] = (p1[i+1] - p1[i]) / (2.*h[i]);
  }
  /* Note: p2[n-1] is set to values coherent with the derivatives of the spline at the last point, but not stricly speaking a coefficient of the spline. */
  p2[n-1] = p2[n-2];

  /* Copying the results in the output matrix */
  gsl_vector_view viewx = gsl_matrix_column(splinecoeffs, 0);
  gsl_vector_view viewp0 = gsl_matrix_column(splinecoeffs, 1);
  gsl_vector_view viewp1 = gsl_matrix_column(splinecoeffs, 2);
  gsl_vector_view viewp2 = gsl_matrix_column(splinecoeffs, 3);
  gsl_vector_memcpy(&viewx.vector, vectx);
  gsl_vector_memcpy(&viewp0.vector, vecty);
  gsl_vector_memcpy(&viewp1.vector, vectp1);
  gsl_vector_memcpy(&viewp2.vector, vectp2);

  /* Cleanup*/
  gsl_vector_free(vecth);
  gsl_vector_free(vectDeltay);
  gsl_vector_free(vectDeltayoverh);
  gsl_vector_free(vectp1);
  gsl_vector_free(vectp2);
}

void BuildSplineCoeffs(
  CAmpPhaseSpline** splines,                  /*  */
  CAmpPhaseFrequencySeries* freqseries)       /*  */
{
  /* Initialize output structure */
  int n = (int) freqseries->freq->size;
  CAmpPhaseSpline_Init(splines, n);

  /* Build the splines */
  BuildNotAKnotSpline((*splines)->spline_amp_real, freqseries->freq, freqseries->amp_real, n);
  BuildNotAKnotSpline((*splines)->spline_amp_imag, freqseries->freq, freqseries->amp_imag, n);
  BuildQuadSpline((*splines)->quadspline_phase, freqseries->freq, freqseries->phase, n);
}

void BuildListmodesCAmpPhaseSpline(
  ListmodesCAmpPhaseSpline** listspline,              /* Output: list of modes of splines in matrix form */
  ListmodesCAmpPhaseFrequencySeries* listh)           /* Input: list of modes in amplitude/phase form */
{
    if(*listspline){ /* We don't allow for the case where listspline already points to something */
      printf("Error: Tried to add a mode to an already existing ListmodesCAmpPhaseSpline ");
      exit(1);
    }
    else {
      ListmodesCAmpPhaseFrequencySeries* listelementh = listh;
      while(listelementh) {
	int l = listelementh->l;
	int m = listelementh->m;
	CAmpPhaseSpline* splines = NULL;
	BuildSplineCoeffs(&splines, listelementh->freqseries);
	*listspline = ListmodesCAmpPhaseSpline_AddModeNoCopy(*listspline, splines, l, m);
	listelementh = listelementh->next;
      }
    }
}

/* Note: for the spines in matrix form, the first column contains the x values, so the coeffs start at 1 */
double EvalCubic(
  gsl_vector* coeffs,  /**/
  double eps,          /**/
  double eps2,         /**/
  double eps3)         /**/
{
  double p0 = gsl_vector_get(coeffs, 1);
  double p1 = gsl_vector_get(coeffs, 2);
  double p2 = gsl_vector_get(coeffs, 3);
  double p3 = gsl_vector_get(coeffs, 4);
  return p0 + p1*eps + p2*eps2 + p3*eps3;
}

double EvalQuad(
  gsl_vector* coeffs,  /**/
  double eps,          /**/
  double eps2)         /**/
{
  double p0 = gsl_vector_get(coeffs, 1);
  double p1 = gsl_vector_get(coeffs, 2);
  double p2 = gsl_vector_get(coeffs, 3);
  return p0 + p1*eps + p2*eps2;
}

void EvalCAmpPhaseSpline(
  CAmpPhaseSpline* splines,                    //input
  CAmpPhaseFrequencySeries* freqseries)  //in/out defines CAmpPhase from defined freqs
{
  int ispline=0;
  //printf("Enter: n=%i\n",freqseries->freq->size);
  for(int i=0;i<freqseries->freq->size;i++){
    double f=gsl_vector_get(freqseries->freq,i);
    //printf(" f:%g < %g < %g\n",gsl_matrix_get(splines->quadspline_phase, 0, 0),f,gsl_matrix_get(splines->quadspline_phase, splines->quadspline_phase->size1-1, 0));

    /* Adjust the index in the spline if necessary and compute */
    while(gsl_matrix_get(splines->quadspline_phase, ispline+1, 0)<f)ispline++;

    double eps = f - gsl_matrix_get(splines->quadspline_phase, ispline, 0);
    double eps2 = eps*eps;
    double eps3 = eps2*eps;
    gsl_vector_view coeffsampreal = gsl_matrix_row(splines->spline_amp_real, ispline);
    gsl_vector_view coeffsampimag = gsl_matrix_row(splines->spline_amp_imag, ispline);
    gsl_vector_view coeffsphase = gsl_matrix_row(splines->quadspline_phase, ispline);
    double Ar = EvalCubic(&coeffsampreal.vector, eps, eps2, eps3);
    double Ai = EvalCubic(&coeffsampimag.vector, eps, eps2, eps3);
    double Ph = EvalQuad(&coeffsphase.vector, eps, eps2);

    gsl_vector_set(freqseries->amp_real,i,Ar);
    gsl_vector_set(freqseries->amp_imag,i,Ai);
    gsl_vector_set(freqseries->phase,i,Ph);
  }
};


/* [response orientation nms filterBank] = steerableDetector3D(image, filterOrder, sigma);
 *
 * (c) Francois Aguet, 30/08/2012 (last modified 09/02/2012).
 *
 * Compilation:
 * Mac/Linux: mex -I/usr/local/include -I../../mex/include /usr/local/lib/libgsl.a /usr/local/lib/libgslcblas.a steerableDetector3D.cpp
 * Windows: mex COMPFLAGS="$COMPFLAGS /TP /MT" -I"..\..\..\extern\mex\include\gsl-1.14" -I"..\..\mex\include" "..\..\..\extern\mex\lib\gsl.lib" "..\..\..\extern\mex\lib\cblas.lib" -output steerableDetector3D steerableDetector3D.cpp
 */

#include <cstring>
#include <gsl_include/gsl_poly.h>
#include <gsl_include/gsl_math.h>
#include <gsl_include/gsl_eigen.h>
#include <gsl_include/gsl_cblas.h>
#include <algorithm>
#include <vector>
#include <fstream>
#include <string>
#include <math.h>
#include "stackutil.h"
#include "../../../v3d_main/jba/newmat11/newmatap.h"
#include "../../../v3d_main/jba/newmat11/newmatio.h"

#include "convolver3D.h"

double zhi_abs(double num);
int swapthree(double& dummya, double& dummyb, double& dummyc);

class Filter {

public:
    Filter(const double voxels[], const int nx, const int ny, const int nz, const int M, const double sigma);
    Filter(const double voxels[], const int nx, const int ny, const int nz, const int M, const double sigma, const double zxRatio);
    ~Filter();
    void init();

    double* getResponse();
    double** getOrientation();
    double* getNMS();

private:
    const double* voxels_;
    double* response_;
    double** orientation_;
    double* nms_;
    int nx_, ny_, nz_;
    int M_;
    double sigma_, sigmaZ_;
    int N_;
    double alpha_, sign_, c_;

    double *gxx_, *gxy_, *gxz_, *gyy_, *gyz_, *gzz_;

    void calculateTemplates();
    double interpResponse(const double x, const double y, const double z);
    static int mirror(const int x, const int nx);
    void normalize(double v[], const int k);
    void cross(const double v1[], const double v2[], double r[]);

    void computeCurveNMS();
    void computeSurfaceNMS();
    void run();
};


#define USE_GSL_INTEGRATION 0

#if USE_GSL_INTEGRATION
	#include <gsl/gsl_integration.h>
#endif

struct integrand_params {
	u32 n[2];
	u32 component_count;
	u32 coeff_count;
	f64* coeff;

	nlse_operator_func* op;
	void* op_userdata;

	struct basis basis;
};

static void sbmf_log_integration_result(struct quadgk_result* res) {
	sbmf_log_info("integral: %.10e", res->integral);
	sbmf_log_info("error: %.10e", res->error);
	//sbmf_log_info("performed evals: %d", res->performed_evals);
	sbmf_log_info("converged: %s", (res->converged) ? "yes" : "no");
}

/* functions to handle spatial guesses */

struct guess_integrand_params {
    u32 n;
	nlse_spatial_guess_func* func;
	struct basis b;
};

void guess_integrand(f64* out, f64* in, u32 len, void* data) {
    struct guess_integrand_params* params = data;

    f64 eig[len];
	params->b.eigenfunc(params->n, len, eig, in);

	f64 sample[len];
	params->func(sample, in, len, NULL);

    for (u32 i = 0; i < len; ++i) {
        out[i] = eig[i] * sample[i];
    }
}

/* function to sample linear and non-linear matrix elements */
static void linear_me_integrand(f64* out, f64* in, u32 len, void* data) {
	struct integrand_params* params = data;

	f64 eig1[len];
	f64 eig2[len];
	params->basis.eigenfunc(params->n[0], len, eig1, in);
	params->basis.eigenfunc(params->n[1], len, eig2, in);

	f64 op[len];
	params->op(len, op, in, 0, NULL, params->op_userdata);

	for (u32 i = 0; i < len; ++i) {
		out[i] = eig1[i]*eig2[i]*op[i];
	}
}

static void nonlinear_me_integrand(f64* out, f64* in, u32 len, void* data) {
	struct integrand_params* params = data;

	f64 eig1[len];
	f64 eig2[len];
	params->basis.eigenfunc(params->n[0], len, eig1, in);
	params->basis.eigenfunc(params->n[1], len, eig2, in);

	f64 sample[len*params->component_count];
	for (u32 i = 0; i < params->component_count; ++i) {
		params->basis.sample(params->coeff_count, &params->coeff[i*params->coeff_count], len, &sample[i*len], in);
	}

	f64 op[len];
	params->op(len, op, in, params->component_count, sample, params->op_userdata);

	for (u32 i = 0; i < len; ++i) {
		out[i] = eig1[i]*eig2[i]*op[i];
	}
}

#if USE_GSL_INTEGRATION
static f64 nonlinear_me_integrand_gsl(f64 in, void* data) {
	struct integrand_params* params = data;

	f64 eig1;
	f64 eig2;
	params->basis.eigenfunc(params->n[0], 1, &eig1, &in);
	params->basis.eigenfunc(params->n[1], 1, &eig2, &in);

	f64 sample[params->component_count];
	for (u32 i = 0; i < params->component_count; ++i) {
		params->basis.sample(params->coeff_count, &params->coeff[i*params->coeff_count], 1, &sample[i], &in);
	}

	f64 op;
	params->op(1, &op, &in, params->component_count, sample, params->op_userdata);

	return eig1*eig2*op;
}
#endif

struct nlse_result nlse_solver(struct nlse_settings settings, const u32 component_count, struct nlse_component* component) {
	/* Lazy */
	const u32 N = settings.num_basis_funcs;

	/* Setup results struct */
	struct nlse_result res = {
		.iterations = 0,
		.component_count = component_count,
		.coeff_count = N,
		.coeff 		 = sbmf_stack_push(component_count*(N*sizeof(f64))),
		.abs_error 	 = sbmf_stack_push(component_count*sizeof(f64)),
		.energy 	 = sbmf_stack_push(component_count*sizeof(f64)),
		.residual 	 = sbmf_stack_push(component_count*sizeof(f64)),
		.hamiltonian = sbmf_stack_push(component_count * sizeof(struct symmetric_bandmat)),
		.converged = false,
	};
	for (u32 i = 0; i < component_count; ++i) {
		res.hamiltonian[i] = symmetric_bandmat_new(N,N);
	}

	/* Place to store coeffs from previous iterations */
	f64* old_coeff = sbmf_stack_push(component_count*(N*sizeof(f64)));
	f64* old_energy = sbmf_stack_push(component_count * sizeof(f64));

	struct quadgk_settings int_settings = {
		.gk = (settings.gk.gauss_size > 0) ? settings.gk : gk15,
		.abs_error_tol = 1e-15,
		.rel_error_tol = 1e-8,
		.max_iters = settings.max_quadgk_iters,
	};

	/* Setup intial guess values for coeffs */
	for (u32 i = 0; i < component_count; ++i) {
		switch (component[i].guess.type) {
			case DEFAULT_GUESS: {
				for (u32 j = 0; j < res.coeff_count; ++j) {
					res.coeff[i*res.coeff_count + j] = 0;
				}
				/* Initialize the i:th component to the i:th eigenfunction */
				res.coeff[i*res.coeff_count + i] = 1;
				break;
			}
			case SPATIAL_GUESS: {
				struct guess_integrand_params p = {
					.func = component[i].guess.data.spatial_guess,
					.b = settings.basis,
				};
				int_settings.userdata = &p;
				f64* out = &res.coeff[i*res.coeff_count];
				for (u32 j = 0; j < res.coeff_count; ++j) {
					p.n = j;
					struct quadgk_result r;

					u8 quadgk_memory[quadgk_required_memory_size(&int_settings)];
					quadgk_infinite_interval(guess_integrand, &int_settings, quadgk_memory, &r);
					out[j] = r.integral;
				}
				f64_normalize(out, out, res.coeff_count);

				break;
			}
			case COEFF_GUESS: {
				component[i].guess.data.coeff_guess(&res.coeff[i*res.coeff_count], res.coeff_count, i);
				break;
			}
			case RANDOM_GUESS: {
				struct symmetric_bandmat bm = symmetric_bandmat_new_zero(N,N);
				SYMMETRIC_BANDMAT_FOREACH(bm, r,c) {
					u32 index = symmetric_bandmat_index(bm, r,c);
					bm.data[index] = 2.0 * ((f64)rand()/(f64)RAND_MAX) - 1.0;
				}
				struct eigen_result_real eigres = find_eigenpairs_sparse_real(bm, 1, EV_SMALLEST);
				for (u32 j = 0; j < res.coeff_count; ++j) {
					res.coeff[i*res.coeff_count + j] = eigres.eigenvectors[j];
				}
				break;
			}
			default:
				sbmf_log_error("nlse_solver: component %u has invalid guess!", i);
				return res;
		};
	}

	struct integrand_params params = {
		.component_count = res.component_count,
		.coeff_count = res.coeff_count,
		.coeff = res.coeff,
		.op = NULL,
		.basis = settings.basis,
	};

	/*
	 * Unique number of me's that need to be calculated in
	 * a symmetric matrix
	 */
	const u32 matrix_element_count = symmetric_bandmat_element_count(N);

	/* Precompute indices for matrix elements
	 * 	This way we get a single array looping
	 * 	over the matrix which is easier to
	 * 	parallelize well.
	 * 		Picking the first hamiltonian as a
	 * 	dummny matrix just to get the correct
	 * 	iteration.
	 * */
	u32 matrix_element_rows[matrix_element_count];
	u32 matrix_element_cols[matrix_element_count];
	{
		u32 matrix_element_index = 0;
		SYMMETRIC_BANDMAT_FOREACH(res.hamiltonian[0], r,c) {
			matrix_element_rows[matrix_element_index] = r;
			matrix_element_cols[matrix_element_index] = c;
			matrix_element_index++;
		}
	}

	/* Construct linear hamiltonian */
	struct symmetric_bandmat linear_hamiltonian = symmetric_bandmat_new_zero(N,N);
	{
		if (settings.spatial_pot_perturbation) {
			params.op = settings.spatial_pot_perturbation;
#pragma omp parallel for firstprivate(params, int_settings) shared(res)
			for (u32 i = 0; i < matrix_element_count; ++i) {
				u32 r = matrix_element_rows[i];
				u32 c = matrix_element_cols[i];

				int_settings.userdata = &params;
				params.n[0] = r;
				params.n[1] = c;

				u8 quadgk_memory[quadgk_required_memory_size(&int_settings)];
				struct quadgk_result int_res;
				quadgk_infinite_interval(linear_me_integrand, &int_settings, quadgk_memory, &int_res);

				if (fabs(int_res.integral) <= settings.zero_threshold)
					int_res.integral = 0.0;

				if (!int_res.converged) {
					sbmf_log_error("In construction of linear hamiltonian:");
					sbmf_log_error("\tIntegration failed for %d,%d", r,c);
					sbmf_log_integration_result(&int_res);
				}
				assert(int_res.converged);

				u32 me_index = symmetric_bandmat_index(linear_hamiltonian, r,c);
				linear_hamiltonian.data[me_index] = int_res.integral;
			}
		}

		for (u32 i = 0; i < res.coeff_count; ++i) {
			u32 me_index = symmetric_bandmat_index(linear_hamiltonian, i,i);
			linear_hamiltonian.data[me_index] += settings.basis.eigenval(i);
		}

		assert(symmetric_bandmat_is_valid(linear_hamiltonian));
	}

#if USE_GSL_INTEGRATION
	const u32 num_threads = omp_get_max_threads();
	gsl_integration_workspace* ws[num_threads];
	/* Create gsl workspaces if necessary */
	for (u32 i = 0; i < num_threads; ++i) {
		ws[i] = gsl_integration_workspace_alloc(settings.max_iterations);
	}
#endif

	struct symmetric_bandmat old_Hs[res.component_count];
	struct symmetric_bandmat premix_Hs[res.component_count];
	for (u32 i = 0; i < res.component_count; ++i) {
		old_Hs[i] = symmetric_bandmat_new(N,N);
		premix_Hs[i] = symmetric_bandmat_new(N,N);
	}

	/* Do the actual iterations */
	for (; res.iterations < settings.max_iterations; ++res.iterations) {
		sbmf_log_info("nlse starting iteration %u", res.iterations);
		memcpy(old_energy, res.energy, res.component_count * sizeof(f64));

		/* Call debug callback if requested by user */
		if (settings.measure_every > 0 &&
				settings.debug_callback &&
				res.iterations % settings.measure_every == 0) {
			settings.debug_callback(settings, res);
		}

		/* Should we still apply mixing? */
		if (settings.orbital_mixing > 0 && settings.mix_until_iteration > 0 && res.iterations == settings.mix_until_iteration) {
			settings.orbital_mixing = 0;
		}
		if (settings.hamiltonian_mixing > 0 && settings.mix_until_iteration > 0 && res.iterations == settings.mix_until_iteration) {
			settings.hamiltonian_mixing = 0;
		}

		if (settings.orbital_choice != NLSE_ORBITAL_MAXIMUM_OVERLAP && settings.mom_enable_at_iteration > 0 && res.iterations == settings.mom_enable_at_iteration) {
			settings.orbital_choice = NLSE_ORBITAL_MAXIMUM_OVERLAP;
		}

		/* Apply orbital mixing */
		for (u32 i = 0; i < component_count; ++i) {
			if (settings.orbital_mixing > 0.0) {
				//res.energy[i] = (1.0 - settings.orbital_mixing) * res.energy[i] + settings.orbital_mixing*old_energy[i];

				for (u32 j = 0; j < res.coeff_count; ++j) {
					res.coeff[i*res.coeff_count + j] =
						(1.0 - settings.orbital_mixing) * res.coeff[i*res.coeff_count + j] + settings.orbital_mixing * old_coeff[i*res.coeff_count + j];
				}
				f64_normalize(&res.coeff[i*res.coeff_count], &res.coeff[i*res.coeff_count], res.coeff_count);
			}
		}
		memcpy(old_coeff, res.coeff, res.component_count * N * sizeof(f64));

		/*
		 * Construct all the Hamiltonians
		 */
		for (u32 i = 0; i < component_count; ++i) {
			params.op = component[i].op;
			params.op_userdata = component[i].userdata;

			memcpy(old_Hs[i].data, res.hamiltonian[i].data, N*N*sizeof(f64));

			/* Construct the i:th component's hamiltonian */
#pragma omp parallel for firstprivate(params, int_settings) shared(res, linear_hamiltonian)
			for (u32 j = 0; j < matrix_element_count; ++j) {
				u32 r = matrix_element_rows[j];
				u32 c = matrix_element_cols[j];
				int_settings.userdata = &params;
				params.n[0] = r;
				params.n[1] = c;

#if USE_GSL_INTEGRATION
				integration_result int_res;
				gsl_function F;
				F.function = nonlinear_me_integrand_gsl;
				F.params = &params;
				gsl_integration_qagi(&F, 1e-10, 1e-7, settings.max_iterations, ws[omp_get_thread_num()], &int_res.integral, &int_res.error);
				int_res.converged = true;
#else
				u8 quadgk_memory[quadgk_required_memory_size(&int_settings)];

				struct quadgk_result int_res;
				quadgk_infinite_interval(nonlinear_me_integrand, &int_settings, quadgk_memory, &int_res);
#endif

				/* Check if the resultant integral is less than what we can resolve,
				 * if so, zero it.
				 */
				if (fabs(int_res.integral) <= settings.zero_threshold)
					int_res.integral = 0.0;

				if (!int_res.converged) {
					sbmf_log_error("In construction of component %u's hamiltonian:", i);
					sbmf_log_error("\tIntegration failed for %d,%d", r,c);
					sbmf_log_integration_result(&int_res);
				}
				assert(int_res.converged);

				u32 me_index = symmetric_bandmat_index(res.hamiltonian[i], r,c);
				res.hamiltonian[i].data[me_index] = linear_hamiltonian.data[me_index] + int_res.integral;
				premix_Hs[i].data[me_index] = res.hamiltonian[i].data[me_index];

				if (settings.hamiltonian_mixing > 0) {
					res.hamiltonian[i].data[me_index] = (1.0 - settings.hamiltonian_mixing)*res.hamiltonian[i].data[me_index] + settings.hamiltonian_mixing*old_Hs[i].data[me_index];
				}
			}

			assert(symmetric_bandmat_is_valid(res.hamiltonian[i]));
		}

		/*
		 * Solve and normalize all the Hamiltonian
		 * eigenvalue problems
		 */
		for (u32 i = 0; i < component_count; ++i) {
			struct eigen_result_real eigres;
			u32 new_orbital_index = 0;
			switch (settings.orbital_choice) {
				case NLSE_ORBITAL_LOWEST_ENERGY: {
					eigres = find_eigenpairs_sparse_real(res.hamiltonian[i], 1, EV_SMALLEST);
					f64_normalize(&eigres.eigenvectors[0], &eigres.eigenvectors[0], res.coeff_count);
					break;
				}
				case NLSE_ORBITAL_MAXIMUM_OVERLAP: {
					eigres = find_eigenpairs_sparse_real(res.hamiltonian[i], settings.mom_orbitals_to_consider, EV_SMALLEST);
					f64 maximum_overlap = -INFINITY;
					for (u32 j = 0; j < settings.mom_orbitals_to_consider; ++j) {
						f64_normalize(&eigres.eigenvectors[j*res.coeff_count], &eigres.eigenvectors[j*res.coeff_count], res.coeff_count);
						f64 overlap = 0.0;
						for (u32 k = 0; k < res.coeff_count; ++k) {
							f64 a = eigres.eigenvectors[j*res.coeff_count + k];
							f64 b = old_coeff[i*res.coeff_count + k];
							overlap += a*b;
						}
						overlap = fabs(overlap);

						if (overlap > maximum_overlap) {
							new_orbital_index = j;
							maximum_overlap = overlap;
						}
					}
					break;
				}
				default: {
					sbmf_log_panic("nlse_solver(...): Unspecified energy selection method!");
					assert(0);
				}
			};

			/* Copy energies and coeffs */
			res.energy[i] = eigres.eigenvalues[new_orbital_index];
			for (u32 j = 0; j < res.coeff_count; ++j) {
				res.coeff[i*res.coeff_count + j] = eigres.eigenvectors[res.coeff_count*new_orbital_index + j];
			}
			f64_normalize(&res.coeff[i*res.coeff_count], &res.coeff[i*res.coeff_count], res.coeff_count);

			///* Apply orbital mixing */
			//if (settings.orbital_mixing > 0.0) {
			//	res.energy[i] = (1.0 - settings.orbital_mixing) * res.energy[i] + settings.orbital_mixing*old_energy[i];

			//	for (u32 j = 0; j < res.coeff_count; ++j) {
			//		res.coeff[i*res.coeff_count + j] =
			//			(1.0 - settings.orbital_mixing) * res.coeff[i*res.coeff_count + j] + settings.orbital_mixing * old_coeff[i*res.coeff_count + j];
			//	}
			//	f64_normalize(&res.coeff[i*res.coeff_count], &res.coeff[i*res.coeff_count], res.coeff_count);
			//}

		}

		/* Calculate error */
		for (u32 i = 0; i < component_count; ++i) {
			f64 sum_diff = 0.0;
			f64 sum_prev = 0.0;
			for (u32 j = 0; j < res.coeff_count; ++j) {
				f64 diff = fabs(res.coeff[i*res.coeff_count + j]) - fabs(old_coeff[i*res.coeff_count + j]);
				sum_diff += diff*diff;
				sum_prev += old_coeff[i*res.coeff_count + j]*old_coeff[i*res.coeff_count + j];
			}
			res.abs_error[i] = sqrt(sum_diff);
		}

		/* Break condition */
		bool should_exit = true;
		for (u32 i = 0; i < component_count; ++i) {
			if (res.abs_error[i] > settings.abs_error_tol)
				should_exit = false;
			sbmf_log_info("\t[%u] -- abs error: %.10e energy: %.10e", i, res.abs_error[i], res.energy[i]);
		}

		if (should_exit) {
			res.converged = true;
			break;
		}
	}

#if USE_GSL_INTEGRATION
	/* free gsl workspaces if necessary */
	for (u32 i = 0; i < num_threads; ++i) {
		gsl_integration_workspace_free(ws[i]);
	}
#endif

	/* Compute residuals */
	for (u32 i = 0; i < component_count; ++i) {
		f64 ans1[N], ans2[N];
		symmetric_bandmat_mulv(ans1, premix_Hs[i], &res.coeff[i*N]);

		for (u32 j = 0; j < N; ++j) {
			ans2[j] = res.energy[i] * res.coeff[i*N + j];
		}

		f64 sum = 0.0;
		for (u32 j = 0; j < N; ++j) {
			sum += fabs(ans1[j] - ans2[j]);
		}
		sum = sqrt(sum);

		res.residual[i] = sum;
		sbmf_log_info("\t[%u] residual %e", i, sum);
	}

	return res;
}

/*
 * basic serialization
 */

void nlse_write_to_binary_file(const char* file, struct nlse_result res) {
	FILE* fd = fopen(file, "w");
	fwrite(&res.iterations, 	 sizeof(u32), 1, fd);
	fwrite(&res.component_count, sizeof(u32), 1, fd);
	fwrite(&res.coeff_count, 	 sizeof(u32), 1, fd);

	fwrite(res.coeff,  sizeof(f64), res.coeff_count*res.component_count, fd);
	fwrite(res.abs_error,  sizeof(f64), res.component_count, fd);
	fwrite(res.energy, sizeof(f64), res.component_count, fd);

	for (u32 i = 0; i < res.component_count; ++i) {
		fwrite(&res.hamiltonian[i].size, 	  sizeof(u32), 1, fd);
		fwrite(&res.hamiltonian[i].bandcount, sizeof(u32), 1, fd);
		u32 elements_written = fwrite(res.hamiltonian[i].data, sizeof(f64),
				res.hamiltonian[i].bandcount*res.hamiltonian[i].size,
				fd);
		assert(elements_written == res.hamiltonian[i].bandcount*res.hamiltonian[i].size);
	}
	fwrite(&res.converged, sizeof(bool), 1, fd);
	fclose(fd);
}

struct nlse_result nlse_read_from_binary_file(const char* file) {
	struct nlse_result res;

	FILE* fd = fopen(file, "r");
	fread(&res.iterations, 	 	sizeof(u32), 1, fd);
	fread(&res.component_count, sizeof(u32), 1, fd);
	fread(&res.coeff_count,  	sizeof(u32), 1, fd);

	res.coeff = sbmf_stack_push(sizeof(f64)*res.coeff_count*res.component_count);
	fread(res.coeff,  sizeof(f64), res.coeff_count*res.component_count, fd);

	res.abs_error = sbmf_stack_push(sizeof(f64)*res.component_count);
	fread(res.abs_error,  sizeof(f64), res.component_count, fd);

	res.energy = sbmf_stack_push(sizeof(f64)*res.component_count);
	fread(res.energy, sizeof(f64), res.component_count, fd);

	res.hamiltonian = sbmf_stack_push(res.component_count * sizeof(struct symmetric_bandmat));
	for (u32 i = 0; i < res.component_count; ++i) {
		u32 size, bandcount;
		fread(&size, 	  sizeof(u32), 1, fd);
		fread(&bandcount, sizeof(u32), 1, fd);
		res.hamiltonian[i] = symmetric_bandmat_new(bandcount, size);
		u32 bytes_written = fread(res.hamiltonian[i].data, sizeof(f64),
				bandcount*size,
				fd);
		assert(bytes_written == bandcount*size);
	}
	fread(&res.converged, sizeof(bool), 1, fd);
	fclose(fd);

	sbmf_log_info("loaded:");
	sbmf_log_info("    iterations: %u", res.iterations);
	sbmf_log_info("    component_count: %u", res.component_count);
	sbmf_log_info("    coeff_count: %u", res.coeff_count);
	for (u32 i = 0; i < res.component_count; ++i) {
		sbmf_log_info("    [%u]: size: %u", i, res.hamiltonian[i].size);
		sbmf_log_info("    [%u]: bandcount: %u", i, res.hamiltonian[i].bandcount);
	}

	return res;
}


#pragma once

#include <gsl/gsl_vector.h>

double vs_gsl_Gradient_ForwardDiff (
    const gsl_vector * x, /**< [in] Point at which the gradient is to be evaluated */
    void * data,          /**< [in] Optional parameters passed directly to the function func_f */
    double (*func_f)(const gsl_vector * x, void *data), /**< [in] User-supplied routine that returns the value of the function at x */
    gsl_vector * J,       /**< [out] Gradient vector (same length as x) */
    double dh             /**< [in] Increment in variable for numerical differentiation */
    );


//CELL operation: does output side of Elman RNN layer.
//There is no strict definition of Elman RNN in the literature.
//For example, the Wikipedia article doesn't specify the dims of the hidden or other vectors.
//Here I implement an interpretation where there are N neurons and thus N inputs/outputs.
//The inputs here are from the IN stage, so reduced to N driving input time-series in X.

//Again, this is not an "Elman network", rather a layer of N neurons
//that I have named "Elman" neurons due to their great similarity to an Elman RNN.

//To do: should I allow other output activations other than logistic?

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

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int elman_s (float *Y, const float *X, const float *U, float *H, const float *W, const float *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int elman_d (double *Y, const double *X, const double *U, double *H, const double *W, const double *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int elman_s (float *Y, const float *X, const float *U, float *H, const float *W, const float *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;

    if (N==1u)
    {
        for (size_t t=0u; t<T; ++t)
        {
            H[0] = 1.0f / (1.0f+expf(-X[t]-U[0]*H[0]));
            Y[t] = 1.0f / (1.0f+expf(-B[0]-W[0]*H[0]));
        }
    }
    else
    {
        float *tmp;
        if (!(tmp=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in elman_s: problem with malloc. "); perror("malloc"); return 1; }

        if (dim==0u)
        {
            if (iscolmajor)
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t*N],1,tmp,1);
                    cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-tmp[n])); }
                    //cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t*N],1);
                    //for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-X[t*N+n])); }
                    //for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0f/(1.0f+expf(-cblas_sdot((int)N,&W[n],(int)N,H,1)-B[n])); }
                    cblas_scopy((int)N,B,1,tmp,1);
                    cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0f/(1.0f+expf(-tmp[n])); }
                }
            }
            else
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t],(int)T,tmp,1);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-tmp[n])); }
                    cblas_scopy((int)N,B,1,tmp,1);
                    cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0f/(1.0f+expf(-tmp[n])); }
                }
            }
        }
        else if (dim==1u)
        {
            if (iscolmajor)
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t],(int)T,tmp,1);
                    cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-tmp[n])); }
                    cblas_scopy((int)N,B,1,tmp,1);
                    cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0f/(1.0f+expf(-tmp[n])); }
                }
            }
            else
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_scopy((int)N,&X[t*N],1,tmp,1);
                    cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0f/(1.0f+expf(-tmp[n])); }
                    cblas_scopy((int)N,B,1,tmp,1);
                    cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0f/(1.0f+expf(-tmp[n])); }
                }
            }
        }
        else
        {
            fprintf(stderr,"error in elman_s: dim must be 0 or 1.\n"); return 1;
        }
    }

    return 0;
}

int elman_d (double *Y, const double *X, const double *U, double *H, const double *W, const double *B, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;

    if (N==1u)
    {
        for (size_t t=0u; t<T; ++t)
        {
            H[0] = 1.0 / (1.0+exp(-X[t]-U[0]*H[0]));
            Y[t] = 1.0 / (1.0+exp(-B[0]-W[0]*H[0]));
        }
    }
    else
    {
        double *tmp;
        if (!(tmp=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in elman_d: problem with malloc. "); perror("malloc"); return 1; }

        if (dim==0u)
        {
            if (iscolmajor)
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t*N],1,tmp,1);
                    cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-tmp[n])); }
                    cblas_dcopy((int)N,B,1,tmp,1);
                    cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0/(1.0+exp(-tmp[n])); }
                }
            }
            else
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t],(int)T,tmp,1);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-tmp[n])); }
                    cblas_dcopy((int)N,B,1,tmp,1);
                    cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0/(1.0+exp(-tmp[n])); }
                }
            }
        }
        else if (dim==1u)
        {
            if (iscolmajor)
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t],(int)T,tmp,1);
                    cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-tmp[n])); }
                    cblas_dcopy((int)N,B,1,tmp,1);
                    cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t+n*T] = 1.0/(1.0+exp(-tmp[n])); }
                }
            }
            else
            {
                for (size_t t=0u; t<T; ++t)
                {
                    cblas_dcopy((int)N,&X[t*N],1,tmp,1);
                    cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { H[n] = 1.0/(1.0+exp(-tmp[n])); }
                    cblas_dcopy((int)N,B,1,tmp,1);
                    cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,W,(int)N,H,1,o,tmp,1);
                    for (size_t n=0u; n<N; ++n) { Y[t*N+n] = 1.0/(1.0+exp(-tmp[n])); }
                }
            }
        }
        else
        {
            fprintf(stderr,"error in elman_d: dim must be 0 or 1.\n"); return 1;
        }
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#ifndef SOURCE_ROBANALYT_H_
#define SOURCE_ROBANALYT_H_

// *** source/rob_analyt.h ***
// Author: Kevin Wolz, date: 08/2018
//
// Class RobAnalyt: analytical algorithm to calculate distribution in the WPL.

#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <vector>
#include <stdlib.h>
#include <sys/stat.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_integration.h>

#include "utils.h"
#include "print_to_file.h"

class RobAnalyt {
 public:
 	gsl_matrix* gaussian_quadratic_matrix_;		// quadratic contraction with data
	unsigned int num_steps_;		 							// number of PDF evaluation points
	double lower_limit_;				 							// lower limit of PDF calculation
	double upper_limit_;				 							// upper limit of PDF calculation

	RobAnalyt(int num_steps,
	 					double upper_limit,
	 					double lower_limit,
	          unsigned int datdim, 							// number of data points
	          unsigned int pardim, 							// number of models parameters
	          gsl_matrix* g_matrix, 						// model design matrix
	          gsl_matrix* inv_covar_matrix, 		// inverse data covariance matrix
	          gsl_matrix* fisher_matrix_tot, 		// Fisher matrix of total data
						gsl_matrix* fisher_matrix_1, 			// Fisher matrix of first subset
						gsl_matrix* fisher_matrix_2, 			// Fisher matrix of second subset
						gsl_matrix* prior_matrix,
						gsl_vector* prior_param_values, 	// prior fiducial parameter values
						gsl_vector* data_mean_values); 		// mean of data vector

	int CalculateAnalytWeakPrior(std::string& filepath);

 private:
	unsigned int datdim_;
	unsigned int pardim_;
	gsl_matrix* g_matrix_tot_;
	gsl_matrix* g_matrix_1_;
	gsl_matrix* g_matrix_2_;
	gsl_matrix* inv_covar_matrix_tot_;
	gsl_matrix* inv_covar_matrix_1_;
	gsl_matrix* inv_covar_matrix_2_;
	gsl_matrix* fisher_matrix_tot_;
	gsl_matrix* fisher_matrix_1_;
	gsl_matrix* fisher_matrix_2_;
	gsl_matrix* prior_matrix_;
	gsl_vector* prior_param_values_;
	gsl_vector* data_mean_values_;

	double delta_y_; 											// constant shift of Robustness (1)
	double delta_y_wpl_;									// constant shift of Robustness (WPL, 1)
	double rob_constant_;									// constant shift of Robustness (2)
	gsl_vector* linear_gaussian_vector_; 	// linear contraction with data
	gsl_matrix* covar_matrix_tot_; 				// covariance matrix of total data
};

#endif // SOURCE_ROBANALYT_H_


#include <lapacke.h>

#include "tasks.h"

void chol_task_seq(void *ptr)
{
    struct chol_task_arg *arg = (struct chol_task_arg*) ptr;

    int n     = arg->n;
    double *A = arg->A;
    int ldA   = arg->ldA;

    LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', n, A, ldA);
}


/* This file is auto-generated from weight_functions.h.src */
#ifndef DOUBLE_PREC
#define DOUBLE_PREC
#endif
// # -*- mode: c -*-
#pragma once

#include "defs.h"

#include <stdint.h>

#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_linalg.h>

// Info about a particle pair that we will pass to the weight function
typedef struct
{
    int64_t nprojbins;
    int64_t nsbins;
    double sqr_s;
    double *supp_sqr; //proper way to have array in struct?

} proj_struct_double;

//typedef double (*weight_func_t_double)(const pair_struct_double*);

//////////////////////////////////
// Projection functions
//////////////////////////////////

static inline void tophat_double(const proj_struct_double *proj, double *u){

    int ins = -1;
    for(int p=0;p<proj->nsbins;p++){
        u[p] = 0;
        if (proj->sqr_s >= proj->supp_sqr[p] && proj->sqr_s < proj->supp_sqr[p+1]){
            ins = p;
        }
    }
    if (ins>=0 && ins<proj->nprojbins){
        u[ins] = 1.0;
    }
}

//////////////////////////////////
// Utility functions
//////////////////////////////////

static inline void compute_amplitudes(int nprojbins, int nd1, int nd2, int nr1, int nr2,
            double *dd, double *dr, double *rd, double *rr, double *qq, double *amps){

    printf("Computing amps\n");
    printf("qq:\n");
    for(int i=0;i<nprojbins;i++){
        for (int j=0; j<nprojbins; j++){
            printf(" %f", qq[i*nprojbins+j]);
        }
        printf("\n");
    }

    // Computer numerator of estimator
    double numerator[nprojbins];
    double qqnorm[nprojbins*nprojbins];
    for (int i=0; i<nprojbins; i++){
        double ddnorm = dd[i]/((double)nd1*(double)nd2);
        double drnorm = dr[i]/((double)nd1*(double)nr2);
        double rdnorm = rd[i]/((double)nr1*(double)nd2);
        double rrnorm = rr[i]/((double)nr1*(double)nr2);
        numerator[i] = ddnorm - drnorm - rdnorm + rrnorm;
        for (int j=0; j<nprojbins; j++){
            printf("%f", qq[i*nprojbins+j]);
            qqnorm[i*nprojbins+j] = qq[i*nprojbins+j]/((double)nr1*(double)nr2);
        }
    }

    printf("qqnorm:\n");
    for(int i=0;i<nprojbins;i++){
        for (int j=0; j<nprojbins; j++){
            printf(" %f", qqnorm[i*nprojbins+j]);
        }
        printf("\n");
    }

	int s;
	// Define all the used matrices
	gsl_matrix *qq_mat = gsl_matrix_alloc(nprojbins, nprojbins);
	gsl_matrix *qq_mat_inv = gsl_matrix_alloc(nprojbins, nprojbins);
	gsl_permutation *perm = gsl_permutation_alloc(nprojbins);
	// Fill the matrix m
	for (int i=0; i<nprojbins; i++){
        for (int j=0; j<nprojbins; j++){
            gsl_matrix_set(qq_mat, i, j, qqnorm[i*nprojbins+j]);
        }
    }
	// Make LU decomposition of matrix m
	gsl_linalg_LU_decomp(qq_mat, perm, &s);
	// Invert the matrix m
	gsl_linalg_LU_invert(qq_mat, perm, qq_mat_inv);

    printf("qqinv:\n");
    for(int i=0;i<nprojbins;i++){
        for (int j=0; j<nprojbins; j++){
            printf(" %f", gsl_matrix_get(qq_mat_inv, i, j));
        }
        printf("\n");
    }

    // Take inner product of qqinv * numerator, get amplitude vector
    // TODO: double check
    for (int i=0; i<nprojbins; i++){
        double aval = 0;
        for (int j=0; j<nprojbins; j++){
            aval += gsl_matrix_get(qq_mat_inv, i, j) * numerator[j];
        }
        amps[i] = aval;
    }
}

static inline void evaluate_xi(int nprojbins, double *amps, int nsvals, double *svals,
                                                int nsbins, double *sbins, double *xi){

    // will need to generalize, projbins won't always be rela   ted to sbins
    double supp_sqr[nsbins];
    //plus 1 because one more edge than number of bins
    for (int i=0; i<nsbins+1; i++){
        supp_sqr[i] = pow(sbins[i], 2);
    }

    // ns: number of s values at which to evaluate xi
    for (int i=0; i<nsvals; i++){
        //get basis function u for given value of s
        double u[nprojbins];
        double sqr_s = pow(svals[i], 2);
        proj_struct_double projdata = {.nprojbins=nprojbins, .nsbins=nsbins, .sqr_s=sqr_s, .supp_sqr=supp_sqr};
        tophat_double(&projdata, u);

        //multiply u by the amplitudes to get xi in that s bin (xi is vector of length ns_toeval)
        double xival = 0;
        for (int j=0; j<nprojbins; j++){
            xival += amps[j]*u[j];

        }
        xi[i] = xival;
    }

}

/* Gives a pointer to the weight function for the given weighting method
 * and instruction set.
 */
//static inline weight_func_t_double get_weight_func_by_method_double(const weight_method_t method){
//    switch(method){
//        case PAIR_PRODUCT:
//            return &pair_product_double;
//        default:
//        case NONE:
//            return NULL;
//    }
//}