#if !defined(PETIGA_H)
#define PETIGA_H

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

#include <petsc.h>
#include <petsc-private/petscimpl.h>
#include <petscts2.h>

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;

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

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);

struct _n_IGARule {
  PetscInt refct;
  /**/
  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 IGARuleInit(IGARule rule,PetscInt q);
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,
  IGA_BASIS_BERNSTEIN,
  IGA_BASIS_LAGRANGE,
  IGA_BASIS_HIERARCHICAL
} 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  p,d;      /* polynomial order, last derivative index */

  PetscInt  *offset;  /* [nel] basis offset   */
  PetscReal *detJ;    /* [nel]                */
  PetscReal *weight;  /* [nqp]                */
  PetscReal *point;   /* [nel][nqp]           */
  PetscReal *value;   /* [nel][nqp][nen][d+1] */

  PetscInt   bnd_offset[2];
  PetscReal  bnd_detJ[2];
  PetscReal  bnd_weight[2];
  PetscReal  bnd_point[2];
  PetscReal *bnd_value[2]; /* [nen][d+1] */
};

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,PetscInt order);
PETSC_EXTERN PetscErrorCode IGABasisInitCollocation(IGABasis basis,IGAAxis axis,PetscInt order);

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

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 dt,
                                           PetscReal a,const PetscScalar *V,
                                           PetscReal t,const PetscScalar *U,
                                           PetscScalar *F,void *ctx);
typedef PetscErrorCode (*IGAFormIJacobian)(IGAPoint point,PetscReal dt,
                                           PetscReal a,const PetscScalar *V,
                                           PetscReal t,const PetscScalar *U,
                                           PetscScalar *J,void *ctx);
typedef PetscErrorCode (*IGAFormIFunction2)(IGAPoint point,PetscReal dt,
                                            PetscReal a,const PetscScalar *A,
                                            PetscReal v,const PetscScalar *V,
                                            PetscReal t,const PetscScalar *U,
                                            PetscScalar *F,void *ctx);
typedef PetscErrorCode (*IGAFormIJacobian2)(IGAPoint point,PetscReal dt,
                                            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 dt,
                                            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 dt,
                                            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 dt,
                                             PetscReal t,const PetscScalar *U,
                                             PetscScalar *F,void *ctx);
typedef PetscErrorCode (*IGAFormRHSJacobian)(IGAPoint point,PetscReal dt,
                                             PetscReal t,const PetscScalar *U,
                                             PetscScalar *J,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;
  IGAFormIFunction2 IFunction2;
  void              *IFunCtx;
  IGAFormIJacobian  IJacobian;
  IGAFormIJacobian2 IJacobian2;
  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 IGAFormSetIFunction2 (IGAForm form,IGAFormIFunction2  IFunction,  void *ctx);
PETSC_EXTERN PetscErrorCode IGAFormSetIJacobian2 (IGAForm form,IGAFormIJacobian2  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 IGASetFormIFunction2 (IGA iga,IGAFormIFunction2  IFunction,  void *ctx);
PETSC_EXTERN PetscErrorCode IGASetFormIJacobian2 (IGA iga,IGAFormIJacobian2  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;
  PetscBool   fixtable;
  PetscReal   *rationalW;
  PetscReal   *geometryX;
  PetscScalar *propertyA;
  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_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,const char prefix[],const char option[]);
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 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 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 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 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);

#undef  DMIGA
#define DMIGA "iga"
PETSC_EXTERN PetscErrorCode IGACreateWrapperDM(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;

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

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

  PetscReal *detX;     /*   [nqp]                     */
  PetscReal *gradX[2]; /*0: [nqp][nsd][dim]           */
                       /*1: [nqp][dim][nsd]           */
  PetscReal *hessX[2]; /*0: [nqp][nsd][dim][dim]      */
                       /*1: [nqp][dim][nsd][nsd]      */
  PetscReal *der3X[2]; /*0: [nqp][nsd][dim][dim][dim] */
                       /*1: [nqp][dim][nsd][nsd][nsd] */
  PetscReal *detS;     /*   [nqp]                     */
  PetscReal *normal;   /*   [nqp][dim]                */

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

  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 IGAElementBuildQuadrature(IGAElement element);
PETSC_EXTERN PetscErrorCode IGAElementBuildShapeFuns (IGAElement element);
PETSC_EXTERN PetscErrorCode IGAElementBuildQuadratureAtBoundary(IGAElement element,PetscInt dir,PetscInt side);
PETSC_EXTERN PetscErrorCode IGAElementBuildShapeFunsAtBoundary (IGAElement element,PetscInt dir,PetscInt side);

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 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 *point;    /*   [dim] */
  PetscReal *weight;   /*   [1]   */
  PetscReal *detJac;   /*   [1]   */

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

  PetscReal *detX;     /*   [1] */
  PetscReal *gradX[2]; /*0: [nsd][dim] */
                       /*1: [dim][nsd] */
  PetscReal *hessX[2]; /*0: [nsd][dim][dim] */
                       /*1: [dim][nsd][nsd] */
  PetscReal *der3X[2]; /*0: [nsd][dim][dim][dim] */
                       /*1: [dim][nsd][nsd][nsd] */
  PetscReal *detS;     /*   [1] */
  PetscReal *normal;   /*   [dim] */

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

  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 IGAPointInterpolate(IGAPoint point,PetscInt ider,const PetscScalar U[],PetscScalar u[]);

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 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 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[]);

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

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 IGAComputeScalar(IGA iga,Vec U,
                                             PetscInt n,PetscScalar S[],
                                             IGAFormScalar Scalar,void *ctx);
PETSC_EXTERN PetscErrorCode IGAComputeScalarCustom(IGA iga,Vec U,
                                                   PetscInt n,PetscScalar S[],
                                                   IGAFormScalar Scalar,void *ctx,
                                                   PetscBool fix);

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

PETSC_EXTERN PetscErrorCode IGACreateKSP(IGA iga,KSP *ksp);
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 IGACreateSNES(IGA iga,SNES *snes);
PETSC_EXTERN PetscErrorCode IGAComputeFunction(IGA iga,Vec U,Vec F);
PETSC_EXTERN PetscErrorCode IGAComputeJacobian(IGA iga,Vec U,Mat J);

PETSC_EXTERN PetscErrorCode IGACreateTS(IGA iga,TS *ts);
PETSC_EXTERN PetscErrorCode IGAComputeIFunction(IGA iga,PetscReal dt,
                                                PetscReal a,Vec V,
                                                PetscReal t,Vec U,
                                                Vec F);
PETSC_EXTERN PetscErrorCode IGAComputeIJacobian(IGA iga,PetscReal dt,
                                                PetscReal a,Vec V,
                                                PetscReal t,Vec U,
                                                Mat J);
PETSC_EXTERN PetscErrorCode IGAComputeIEFunction(IGA iga,PetscReal dt,
                                                 PetscReal a, Vec V,
                                                 PetscReal t, Vec U,
                                                 PetscReal t0,Vec U0,
                                                 Vec F);
PETSC_EXTERN PetscErrorCode IGAComputeIEJacobian(IGA iga,PetscReal dt,
                                                 PetscReal a, Vec V,
                                                 PetscReal t, Vec U,
                                                 PetscReal t0,Vec U0,
                                                 Mat J);
PETSC_EXTERN PetscErrorCode IGAComputeRHSFunction(IGA iga,PetscReal dt,
                                                  PetscReal t,Vec U,
                                                  Vec F);
PETSC_EXTERN PetscErrorCode IGAComputeRHSJacobian(IGA iga,PetscReal dt,
                                                  PetscReal t,Vec U,
                                                  Mat J);

PETSC_EXTERN PetscErrorCode IGACreateTS2(IGA iga, TS *ts);
PETSC_EXTERN PetscErrorCode IGAComputeIFunction2(IGA iga,PetscReal dt,
                                                 PetscReal a,Vec vecA,
                                                 PetscReal v,Vec vecV,
                                                 PetscReal t,Vec vecU,
                                                 Vec vecF);
PETSC_EXTERN PetscErrorCode IGAComputeIJacobian2(IGA iga,PetscReal dt,
                                                 PetscReal a,Vec vecA,
                                                 PetscReal v,Vec vecV,
                                                 PetscReal t,Vec vecU,
                                                 Mat matJ);

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

PETSC_EXTERN PetscErrorCode IGAPreparePCBDDC(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))                                    \
      SETERRQ3(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)                        \
      SETERRQ4(((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

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

#ifndef PetscValidRealPointer
#define PetscValidRealPointer PetscValidDoublePointer
#endif

#ifndef PetscMalloc1
#define PetscMalloc1(m1,r1) \
  PetscMalloc((m1)*sizeof(**(r1)),r1)
#endif

#ifndef PetscCalloc1
#define PetscCalloc1(m1,r1) \
  (PetscMalloc1((m1),r1) || PetscMemzero(*(r1),(m1)*sizeof(**(r1))))
#endif

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

#if PETSC_VERSION_(3,3,0)

#undef  PETSC_VERSION_LT
#define PETSC_VERSION_LT(MAJOR,MINOR,SUBMINOR)    \
  (PETSC_VERSION_RELEASE == 1 &&                  \
   (PETSC_VERSION_MAJOR < (MAJOR) ||              \
    (PETSC_VERSION_MAJOR == (MAJOR) &&            \
     (PETSC_VERSION_MINOR < (MINOR) ||            \
      (PETSC_VERSION_MINOR == (MINOR) &&          \
       (PETSC_VERSION_SUBMINOR < (SUBMINOR)))))))

#undef  PETSC_VERSION_LE
#define PETSC_VERSION_LE(MAJOR,MINOR,SUBMINOR) \
  (PETSC_VERSION_LT(MAJOR,MINOR,SUBMINOR) ||   \
   PETSC_VERSION_(MAJOR,MINOR,SUBMINOR))

#undef  PETSC_VERSION_GE
#define PETSC_VERSION_GE(MAJOR,MINOR,SUBMINOR) \
  (0 == PETSC_VERSION_LT(MAJOR,MINOR,SUBMINOR))

#undef  PETSC_VERSION_GT
#define PETSC_VERSION_GT(MAJOR,MINOR,SUBMINOR) \
  (0 == PETSC_VERSION_LE(MAJOR,MINOR,SUBMINOR))

#endif

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

#endif/*PETIGA_H*/


/* BRAINS
 * (B)LR (R)everberation-mapping (A)nalysis (I)n AGNs with (N)ested (S)ampling
 * Yan-Rong Li, liyanrong@ihep.ac.cn
 * Thu, Aug 4, 2016
 */

/*!
 * \file user_blr_model.c
 * \brief allow users to define their own BLR models.
 *
 * User should provide: 1) number of parameters in 1D and 2D;
 *                      2) struct for BLR model;
 *                      3) function for setting parameter range
 *                      4) function for calculating clouds' lag and weight in 1D
 *                      5) function for calculating clouds' lag, velocity, and weight in 2D
 *                      6) function for setting the parameter values used in simulation (if flag_dim<0)
 *
 * The following is an example. The users can make modification with their own BLR models.
 *
 */

#include <gsl/gsl_randist.h>
#include "brains.h"

const int num_params_MyBLRmodel1d = 8;   /*!< number of parameters for 1D model */
const int num_params_MyBLRmodel2d = 17;  /*!< number of parameters for 2D model */

/*!
 *  set parameter range
 */
void set_blr_range_mymodel()
{
  int i;

  i = 0;
  //mu
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //beta
  blr_range_model[i][0] = 0.001;
  blr_range_model[i++][1] = 2.0;
  //F
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //inc
  blr_range_model[i][0] = 0.0; //in cosine
  blr_range_model[i++][1] = 1.0;
  //opn
  blr_range_model[i][0] = 0.0;  // in rad
  blr_range_model[i++][1] = 90.0;
  //k
  blr_range_model[i][0] = -0.5;
  blr_range_model[i++][1] = 0.5;
  //gamma
  blr_range_model[i][0] = 1.0;
  blr_range_model[i++][1] = 5.0;
  //xi
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //mbh
  blr_range_model[i][0] = log(0.1);
  blr_range_model[i++][1] = log(1.0e3);
  //fellip
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //fflow
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 1.0;
  //sigr_circ
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);
  //sigthe_circ
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(1.0);
  //sigr_rad
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(0.1);
  //sigthe_rad
  blr_range_model[i][0] = log(0.001);
  blr_range_model[i++][1] = log(1.0);
  //theta_rot
  blr_range_model[i][0] = 0.0;
  blr_range_model[i++][1] = 90.0;
  //sig_turb
  blr_range_model[i][0] = log(0.0001);
  blr_range_model[i++][1] = log(0.1);

  return;
}

/*
 * This function caclulate 2d transfer function.
 *
 * The clouds' lag, velocity, and weight are stored in arrays "clouds_tau", "clouds_vel", "clouds_weight",
 * which must be provided.
 */
void gen_cloud_sample_mymodel(const void *pm, int flag_type, int flag_save)
{
  int i, j, nc;
  double r, phi, cos_phi, sin_phi, dis, Lopn_cos;
  double x, y, z, xb, yb, zb, zb0, vx, vy, vz, vxb, vyb, vzb;
  double V, rhoV, theV, Vr, Vph, Vkep, Rs, g;
  double inc, F, beta, mu, k, gam, xi, a, s, sig, rin;
  double mbh, fellip, fflow, sigr_circ, sigthe_circ, sigr_rad, sigthe_rad, theta_rot, sig_turb;
  double Lphi, Lthe, sin_Lphi, cos_Lphi, sin_Lthe, cos_Lthe, sin_inc_cmp, cos_inc_cmp;
  double weight, rnd, rnd_xi;
  MyBLRmodel *model = (MyBLRmodel *)pm;

  Lopn_cos = cos(model->opn*PI/180.0); /* cosine of openning angle */
  inc = acos(model->inc);         /* inclination angle in rad */
  beta = model->beta;
  F = model->F;
  mu = exp(model->mu);                 /* mean radius */
  k = model->k;
  gam = model-> gam;
  xi = model->xi;

  mbh = exp(model->mbh);
  fellip = model->fellip;
  fflow = model->fflow;
  sigr_circ = exp(model->sigr_circ);
  sigthe_circ = exp(model->sigthe_circ);
  sigr_rad = exp(model->sigr_rad);
  sigthe_rad = exp(model->sigthe_rad);
  theta_rot = model->theta_rot*PI/180.0;
  sig_turb = exp(model->sig_turb);

  Rs = 3.0e11*mbh/CM_PER_LD; // Schwarzchild radius in a unit of light-days

  a = 1.0/beta/beta;
  s = mu/a;
  rin=mu*F + Rs;  // include Scharzschild radius
  sig=(1.0-F)*s;

  sin_inc_cmp = cos(inc); //sin(PI/2.0 - inc);
  cos_inc_cmp = sin(inc); //cos(PI/2.0 - inc);

  for(i=0; i<parset.n_cloud_per_task; i++)
  {
// generate a direction of the angular momentum of the orbit
    Lphi = 2.0*PI * gsl_rng_uniform(gsl_r);
    Lthe = acos(Lopn_cos + (1.0-Lopn_cos) * pow(gsl_rng_uniform(gsl_r), gam));
    sin_Lphi = sin(Lphi);
    cos_Lphi = cos(Lphi);
    sin_Lthe = sin(Lthe);
    cos_Lthe = cos(Lthe);

    nc = 0;
    r = rcloud_max_set+1.0;
    while(r>rcloud_max_set || r<rcloud_min_set)
    {
      if(nc > 1000)
      {
        printf("# Error, too many tries in generating ridial location of clouds.\n");
        exit(0);
      }
      rnd = gsl_ran_gamma(gsl_r, a, 1.0);
//    r = mu * F + (1.0-F) * gsl_ran_gamma(gsl_r, 1.0/beta/beta, beta*beta*mu);
      r = rin + sig * rnd;
      nc++;
    }
    phi = 2.0*PI * gsl_rng_uniform(gsl_r);
    cos_phi = cos(phi);
    sin_phi = sin(phi);

    /* Polar coordinates to Cartesian coordinate */
    x = r * cos_phi;
    y = r * sin_phi;
    z = 0.0;

/* right-handed framework
 * first rotate around y axis by an angle of Lthe, then rotate around z axis
 * by an angle of Lphi
 */
  /*xb = cos(Lthe)*cos(Lphi) * x + sin(Lphi) * y - sin(Lthe)*cos(Lphi) * z;
    yb =-cos(Lthe)*sin(Lphi) * x + cos(Lphi) * y + sin(Lthe)*sin(Lphi) * z;
    zb = sin(Lthe) * x + cos(Lthe) * z; */

    xb = cos_Lthe*cos_Lphi * x + sin_Lphi * y;
    yb =-cos_Lthe*sin_Lphi * x + cos_Lphi * y;
    zb = sin_Lthe * x;

    zb0 = zb;
    rnd_xi = gsl_rng_uniform(gsl_r);
    if( (rnd_xi < 1.0 - xi) && zb0 < 0.0)
      zb = -zb;

// counter-rotate around y, LOS is x-axis
    /* x = xb * cos(PI/2.0-inc) + zb * sin(PI/2.0-inc);
       y = yb;
       z =-xb * sin(PI/2.0-inc) + zb * cos(PI/2.0-inc); */

    x = xb * cos_inc_cmp + zb * sin_inc_cmp;
    y = yb;
    z =-xb * sin_inc_cmp + zb * cos_inc_cmp;

    weight = 0.5 + k*(x/r);
    clouds_weight[i] = weight;

#ifndef SA
    dis = r - x;
    clouds_tau[i] = dis;
    if(flag_type == 1)
    {
      if(flag_save && thistask==roottask)
      {
        if(i%(icr_cloud_save) == 0)
          fprintf(fcloud_out, "%f\t%f\t%f\n", x, y, z);
      }
      continue;
    }
#else
  switch(flag_type)
  {
    case 1:   /* 1D RM */
      dis = r - x;
      clouds_tau[i] = dis;
      if(flag_save && thistask==roottask)
      {
        if(i%(icr_cloud_save) == 0)
          fprintf(fcloud_out, "%f\t%f\t%f\n", x, y, z);
      }
      continue;
      break;

    case 2:  /* 2D RM */
      dis = r - x;
      clouds_tau[i] = dis;
      break;

    case 3: /* SA */
      clouds_alpha[i] = y;
      clouds_beta[i] = z;
      break;

    case 4: /* 1D RM + SA */
      dis = r - x;
      clouds_tau[i] = dis;

      clouds_alpha[i] = y;
      clouds_beta[i] = z;
      break;

    case 5: /* 2D RM + SA */
      dis = r - x;
      clouds_tau[i] = dis;

      clouds_alpha[i] = y;
      clouds_beta[i] = z;
      break;
  }

#endif

    Vkep = sqrt(mbh/r);

    for(j=0; j<parset.n_vel_per_cloud; j++)
    {
      rnd = gsl_rng_uniform(gsl_r);

      if(rnd < fellip)
      {
        rhoV = (gsl_ran_ugaussian(gsl_r) * sigr_circ  + 1.0) * Vkep;
        theV =  (gsl_ran_ugaussian(gsl_r) * sigthe_circ + 0.5)*PI;
      }
      else
      {
        if(fflow <= 0.5) /* inflow */
        {
          rhoV = (gsl_ran_ugaussian(gsl_r) * sigr_rad   + 1.0) * Vkep;
          theV = (gsl_ran_ugaussian(gsl_r) * sigthe_rad + 1.0) * PI + theta_rot;
        }
        else     /* outflow */
        {
          rhoV = (gsl_ran_ugaussian(gsl_r) * sigr_rad  + 1.0) * Vkep;
          theV = (gsl_ran_ugaussian(gsl_r) * sigthe_rad) * PI + theta_rot;
        }
      }

      Vr = sqrt(2.0) * rhoV * cos(theV);
      Vph = rhoV * sin(theV);

      vx = Vr * cos_phi - Vph * sin_phi;
      vy = Vr * sin_phi + Vph * cos_phi;
      vz = 0.0;

    /*vxb = cos(Lthe)*cos(Lphi) * vx + sin(Lphi) * vy - sin(Lthe)*cos(Lphi) * vz;
      vyb =-cos(Lthe)*sin(Lphi) * vx + cos(Lphi) * vy + sin(Lthe)*sin(Lphi) * vz;
      vzb = sin(Lthe) * vx + cos(Lthe) * vz;*/

      vxb = cos_Lthe*cos_Lphi * vx + sin_Lphi * vy;
      vyb =-cos_Lthe*sin_Lphi * vx + cos_Lphi * vy;
      vzb = sin_Lthe * vx;

      if((rnd_xi < 1.0-xi) && zb0 < 0.0)
        vzb = -vzb;

      /*vx = vxb * cos(PI/2.0-inc) + vzb * sin(PI/2.0-inc);
      vy = vyb;
      vz =-vxb * sin(PI/2.0-inc) + vzb * cos(PI/2.0-inc);*/

      vx = vxb * cos_inc_cmp + vzb * sin_inc_cmp;
      vy = vyb;
      vz =-vxb * sin_inc_cmp + vzb * cos_inc_cmp;

      V = -vx;  //note the definition of the line-of-sight velocity. postive means a receding
                // velocity relative to the observer.

      V += gsl_ran_ugaussian(gsl_r) * sig_turb * Vkep; // add turbulence velocity

      if(fabs(V) >= C_Unit) // make sure that the velocity is smaller than speed of light
        V = 0.9999*C_Unit * (V>0.0?1.0:-1.0);

      g = sqrt( (1.0 + V/C_Unit) / (1.0 - V/C_Unit) ) / sqrt(1.0 - Rs/r); //relativistic effects
      V = (g-1.0)*C_Unit;

      clouds_vel[i*parset.n_vel_per_cloud + j] = V;

      if(flag_save && thistask==roottask)
      {
        if(i%(icr_cloud_save) == 0)
          fprintf(fcloud_out, "%f\t%f\t%f\t%f\t%f\t%f\t%f\n", x, y, z, vx*VelUnit, vy*VelUnit, vz*VelUnit, weight);
      }
    }
  }

  return;
}

/*!
 *  set parameter values used in simulation (if flag_dim < 0).
 */
void set_par_value_mymodel_sim(double *pm)
{
  int i;

	i=0;
  pm[i++] = log(4.0);   // mu
  pm[i++] = 1.0;        // beta
  pm[i++] = 0.25;        // F
  pm[i++] = cos(20.0/180.0*PI); // inc
  pm[i++] = 40.0;       // opn
  pm[i++] = -0.4;        // kappa
  pm[i++] = 5.0;        // gamma
  pm[i++] = 0.5;        // obscuration
  pm[i++] = log(2.0);  //mbh
  pm[i++] = 0.5;       //fellip
  pm[i++] = 0.4;       //fflow
  pm[i++] = log(0.01); //
  pm[i++] = log(0.1);  //
  pm[i++] = log(0.01); //
  pm[i++] = log(0.1);  //
  pm[i++] = 0.0;       // theta_rot
  pm[i++] = log(0.001);  // sig_turb
  pm[i++] = 0.0;       // parameter for spectral broadening
}

#ifdef SA
/*!
 *  set parameter range
 */
void set_sa_blr_range_mymodel()
{
  int i;

  i = 0;
  //mu
  sa_blr_range_model[i][0] = log(0.1);
  sa_blr_range_model[i++][1] = log(rcloud_max_set*0.5);
  //beta
  sa_blr_range_model[i][0] = 0.001;
  sa_blr_range_model[i++][1] = 2.0;
  //F
  sa_blr_range_model[i][0] = 0.0;
  sa_blr_range_model[i++][1] = 1.0;
  //inc
  sa_blr_range_model[i][0] = 0.0; //in cosine
  sa_blr_range_model[i++][1] = 1.0;
  //opn
  sa_blr_range_model[i][0] = 0.0;  // in rad
  sa_blr_range_model[i++][1] = 90.0;
  //k
  sa_blr_range_model[i][0] = -0.5;
  sa_blr_range_model[i++][1] = 0.5;
  //gamma
  sa_blr_range_model[i][0] = 1.0;
  sa_blr_range_model[i++][1] = 5.0;
  //xi
  sa_blr_range_model[i][0] = 0.0;
  sa_blr_range_model[i++][1] = 1.0;

  //mbh
  sa_blr_range_model[i][0] = log(0.1);
  sa_blr_range_model[i++][1] = log(1.0e3);
  //fellip
  sa_blr_range_model[i][0] = 0.0;
  sa_blr_range_model[i++][1] = 1.0;
  //fflow
  sa_blr_range_model[i][0] = 0.0;
  sa_blr_range_model[i++][1] = 1.0;
  //sigr_circ
  sa_blr_range_model[i][0] = log(0.001);
  sa_blr_range_model[i++][1] = log(0.1);
  //sigthe_circ
  sa_blr_range_model[i][0] = log(0.001);
  sa_blr_range_model[i++][1] = log(1.0);
  //sigr_rad
  sa_blr_range_model[i][0] = log(0.001);
  sa_blr_range_model[i++][1] = log(0.1);
  //sigthe_rad
  sa_blr_range_model[i][0] = log(0.001);
  sa_blr_range_model[i++][1] = log(1.0);
  //theta_rot
  sa_blr_range_model[i][0] = 0.0;
  sa_blr_range_model[i++][1] = 90.0;
  //sig_turb
  sa_blr_range_model[i][0] = log(0.0001);
  sa_blr_range_model[i++][1] = log(0.1);

  return;
}

/*!
 *  set parameter values used in simulation (if flag_dim < 0).
 */
void set_sa_par_value_mymodel_sim(double *pm)
{
  int i;

	i=0;
  pm[i++] = log(4.0);   // mu
  pm[i++] = 1.0;        // beta
  pm[i++] = 0.25;        // F
  pm[i++] = cos(20.0/180.0*PI); // inc
  pm[i++] = 40.0;       // opn
  pm[i++] = -0.4;        // kappa
  pm[i++] = 5.0;        // gamma
  pm[i++] = 0.5;        // obscuration
  pm[i++] = log(2.0);  //mbh
  pm[i++] = 0.5;       //fellip
  pm[i++] = 0.4;       //fflow
  pm[i++] = log(0.01); //
  pm[i++] = log(0.1);  //
  pm[i++] = log(0.01); //
  pm[i++] = log(0.1);  //
  pm[i++] = 0.0;       // theta_rot
  pm[i++] = log(0.001);  // sig_turb
  pm[i++] = 0.0;       // parameter for spectral broadening
}
#endif


#ifndef ORIENTATION_CONTROLLER_VARIABLE_GAINS_H_
#define ORIENTATION_CONTROLLER_VARIABLE_GAINS_H_
#include <cmath>
#include <barrett/units.h>
#include <barrett/systems.h>
#include <barrett/products/product_manager.h>
#include <libconfig.h++>
#include <barrett/detail/ca_macro.h>
#include <barrett/cdlbt/calgrav.h>
#include <barrett/systems/abstract/system.h>
#include <barrett/systems/abstract/single_io.h>
#include <barrett/systems/kinematics_base.h>
#include <barrett/math/kinematics.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <cstdlib>  // For std::atexit()
#include <barrett/os.h>  // For btsleep()
#include <barrett/math.h>  // For barrett::math::saturate()
#include <Eigen/Dense>
#include <Eigen/Core>
#include <barrett/standard_main_function.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#define BARRETT_SMF_VALIDATE_ARGS
#include <barrett/standard_main_function.h>

using namespace barrett;
using namespace systems;
using systems::connect;
BARRETT_UNITS_FIXED_SIZE_TYPEDEFS;

template<size_t DOF>
class OrientationControllerVariableGains : public systems::System  , public KinematicsInput<DOF>
{
	BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

public:
	Input<Eigen::Quaterniond> ReferenceOrnInput;
	Input<Eigen::Quaterniond> FeedbackOrnInput;
	Input<cp_type> KpGains;
	Input<cp_type> KdGains;

public:
	Output<ct_type> CTOutput;

protected:
	typename Output<ct_type>::Value* ctOutputValue;

public:
	OrientationControllerVariableGains(
//    const units::CartesianPosition::type& ProportionalGains,
//    const units::CartesianPosition::type& DerivativeGains,
    const std::string& sysName = "OrientationControllerVariableGains") :
      System(sysName), KinematicsInput<DOF>(this), ReferenceOrnInput(this), FeedbackOrnInput(this),
      KpGains(this) , KdGains(this),
      CTOutput(this, &ctOutputValue)
//      , ProportionalGains(ProportionalGains), DerivativeGains(DerivativeGains)

{
}
virtual ~OrientationControllerVariableGains()
{
this->mandatoryCleanUp();
}

protected:
	cp_type ProportionalGains;
	cp_type DerivativeGains;

	Eigen::AngleAxisd error;
	ct_type ct;

	cp_type tempVect;
	cp_type tempVect2;
	cp_type errorVect;

	virtual void operate() {

		ProportionalGains = KpGains.getValue();
		DerivativeGains	  = KdGains.getValue();

//		error = this->referenceInput.getValue() * this->feedbackInput.getValue().inverse();  // I think it should be this way
		error = this->FeedbackOrnInput.getValue() * this->ReferenceOrnInput.getValue().inverse();  // but CD's math (that works, BTW) does it this way

		double angle = error.angle();
		// TODO(dc): I looked into Eigen's implementation and noticed that angle will always be between 0 and 2*pi. We should test for this so if Eigen changes, we notice.
		if (angle > M_PI) {
			angle -= 2.0*M_PI;
		}

		if (math::abs(angle) > 3.13) {	// a little dead-zone near the discontinuity at +/-180 degrees
			ct.setZero();
		} else {
		    tempVect = ProportionalGains ;				// copy the ProportiocalGains
		    errorVect = error.axis() * angle ;
		    gsl_vector_mul (tempVect.asGslType() , errorVect.asGslType()  ) ; 	// tempVect <- tempVect * errorVect
		    ct = this->ReferenceOrnInput.getValue().inverse() * ( tempVect );

		}

		tempVect2 = DerivativeGains;
		gsl_vector_mul ( tempVect2.asGslType() , this->kinInput.getValue().impl->tool_velocity_angular ) ;
		ct -= tempVect2 ;
//		ct -= DerivativeGains.dot( this->kinInput.getValue().impl->tool_velocity_angular ) ;
//		gsl_blas_daxpy( -kd, this->kinInput.getValue().impl->tool_velocity_angular, ct.asGslType());

		this->ctOutputValue->setData(&ct);
	}

private:
	DISALLOW_COPY_AND_ASSIGN(OrientationControllerVariableGains);

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#endif /*ORIENTATION_CONTROLLER_VARIABLE_GAINS_H_*/


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

#include <config.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_sys.h>
#include <gsl/gsl_rng.h>

/* This is the Unix rand48() generator. The generator returns the
   upper 32 bits from each term of the sequence,

   x_{n+1} = (a x_n + c) mod m

   using 48-bit unsigned arithmetic, with a = 0x5DEECE66D , c = 0xB
   and m = 2^48. The seed specifies the upper 32 bits of the initial
   value, x_1, with the lower 16 bits set to 0x330E.

   The theoretical value of x_{10001} is 244131582646046.

   The period of this generator is ? FIXME (probably around 2^48). */

static inline void rand48_advance (void *vstate);
static unsigned long int rand48_get (void *vstate);
static double rand48_get_double (void *vstate);
static void rand48_set (void *state, unsigned long int s);

static const unsigned short int a0 = 0xE66D ;
static const unsigned short int a1 = 0xDEEC ;
static const unsigned short int a2 = 0x0005 ;

static const unsigned short int c0 = 0x000B ;

typedef struct
  {
    unsigned short int x0, x1, x2;
  }
rand48_state_t;

static inline void
rand48_advance (void *vstate)
{
  rand48_state_t *state = (rand48_state_t *) vstate;

  /* work with unsigned long ints throughout to get correct integer
     promotions of any unsigned short ints */

  const unsigned long int x0 = (unsigned long int) state->x0 ;
  const unsigned long int x1 = (unsigned long int) state->x1 ;
  const unsigned long int x2 = (unsigned long int) state->x2 ;

  unsigned long int a ;

  a = a0 * x0 + c0 ;
  state->x0 = (a & 0xFFFF) ;

  a >>= 16 ;

  /* although the next line may overflow we only need the top 16 bits
     in the following stage, so it does not matter */

  a += a0 * x1 + a1 * x0 ;
  state->x1 = (a & 0xFFFF) ;

  a >>= 16 ;
  a += a0 * x2 + a1 * x1 + a2 * x0 ;
  state->x2 = (a & 0xFFFF) ;
}

static unsigned long int
rand48_get (void *vstate)
{
  unsigned long int x1, x2;

  rand48_state_t *state = (rand48_state_t *) vstate;
  rand48_advance (state) ;

  x2 = (unsigned long int) state->x2;
  x1 = (unsigned long int) state->x1;

  return (x2 << 16) + x1;
}

static double
rand48_get_double (void * vstate)
{
  rand48_state_t *state = (rand48_state_t *) vstate;

  rand48_advance (state) ;

  return (ldexp((double) state->x2, -16)
          + ldexp((double) state->x1, -32)
          + ldexp((double) state->x0, -48)) ;
}

static void
rand48_set (void *vstate, unsigned long int s)
{
  rand48_state_t *state = (rand48_state_t *) vstate;

  if (s == 0)  /* default seed */
    {
      state->x0 = 0x330E ;
      state->x1 = 0xABCD ;
      state->x2 = 0x1234 ;
    }
  else
    {
      state->x0 = 0x330E ;
      state->x1 = s & 0xFFFF ;
      state->x2 = (s >> 16) & 0xFFFF ;
    }

  return;
}

static const gsl_rng_type rand48_type =
{"rand48",                      /* name */
 0xffffffffUL,                  /* RAND_MAX */
 0,                             /* RAND_MIN */
 sizeof (rand48_state_t),
 &rand48_set,
 &rand48_get,
 &rand48_get_double
};

const gsl_rng_type *gsl_rng_rand48 = &rand48_type;


#include "coneOS.h"
#include <cblas.h>
#include <math.h>

// x = b*a
inline void setAsScaledArray(double *x, const double * a,const double b,int len) {
  int i;
  for( i=0;i<len;++i ) x[i] = b*a[i];
}

// a*= b
inline void scaleArray(double * a,const double b,int len){
  //b_dscal(len,b,a,1);
  cblas_dscal(len,b,a,1);
}

// x'*y
inline double innerProd(const double * x, const double * y, int len){
  //return b_ddot(len, x,1,y,1);
  return cblas_ddot(len, x,1,y,1);
}

// ||v||_2
inline double calcNorm(const double * v,int len){
  //return b_dnrm2(len, v, 1);
  return cblas_dnrm2(len, v, 1);
}

inline double calcNormInf(const double *a, int l){
	double tmp, max = 0.0;
	int i;
	for ( i=0; i<l; ++i){
		tmp = fabs(a[i]);
		if(tmp > max) max = tmp;
	}
	return max;
}

// ||v||_2^2
inline double calcNormSq(const double * v,int len){
  double nrm = calcNorm(v,len);
  return nrm*nrm;
}

// daxpy a += sc*b
inline void addScaledArray(double * a, const double * b, int n, const double sc){
  //b_daxpy(n, sc, b, 1, a, 1);
  cblas_daxpy(n, sc, b, 1, a, 1);
}

inline double calcNormDiff(const double *a,const double *b, int l) {
  double nmDiff = 0.0, tmp;
  int i;
  for ( i=0; i<l; ++i ){
    tmp = (a[i] - b[i]);
    nmDiff += tmp * tmp;
  }
  return sqrt(nmDiff);
}

inline double calcNormInfDiff(const double *a, const double *b, int l) {
	double tmp, max = 0.0;
	int i;
	for ( i=0; i<l; ++i){
		tmp = fabs(a[i] - b[i]);
		if(tmp > max) max = tmp;
	}
	return max;
}

inline void accumByAtrans(Data * d, const double *x, double *y)
{
	cblas_dgemv(CblasColMajor, CblasTrans, d->m, d->n, 1.0, d->Ax, d->m, x, 1, 1.0, y, 1);
}

inline void accumByA(Data * d, const double *x, double *y)
{
 	cblas_dgemv(CblasColMajor, CblasNoTrans, d->m, d->n, 1.0, d->Ax, d->m, x, 1, 1.0, y, 1);
}

/*

inline void b_daxpy(const int n, const double alpha, const double *x, const int incx, double *y, const int incy)
{
  daxpy(&n,&alpha,x,&incx,y,&incy);
  //cblas_daxpy(n,alpha,x,incx,y,incy);
}

inline void b_dsyr(char Uplo, const int N, const double alpha, const double *X, const int incX, double *A, const int lda)
{
  //dsyr(&Uplo,&N,&alpha,X,&incX,A,&lda);
  cblas_dsyr(CblasColMajor, Uplo, N, alpha, X, incX, A, lda);
}

inline void b_dscal(const int N, const double alpha, double *X, const int incX)
{
  //dscal(&N,&alpha,X,&incX);
  cblas_dscal(N,alpha,X,incX);
}

inline double b_ddot(const int n, const double *x, const int incx, const double *y, const int incy)
{
  //return ddot(&n,x,&incx,y,&incy);
  return cblas_ddot(n,x,incx,y,incy);
}

inline double b_dnrm2(const int N, const double *X, const int incX)
{
  return cblas_dnrm2(N,X,incX);
  //return dnrm2(&N,X,&incX);
}

inline void b_dgemm(
              char TransA,
              char TransB,
              const int M,
              const int N,
              const int K,
              const double alpha,
              const double *A,
              const int lda,
              const double *B,
              const int ldb,
              const double beta,
              double *C,
              const int ldc){
  dgemm(&TransA, &TransB, &M, &N, &K,&alpha,A,&lda,B,&ldb,&beta,C,&ldc);
  //cblas_dgemm();
}

inline void b_dgemv(char Trans,
                    const int m,
                    const int n,
                    const double alpha,
                    const double  *a,
                    const int lda,
                    const double  *x,
                    const int incx,
                    const double beta,
                    double  *y,
                    const int incy){
  dgemv(&Trans,&m,&n,&alpha,a,&lda,x,&incx,&beta,y,&incy);
}

inline void b_dtrsv(char Uplo, char TransA, char Diag, const int N, const double *A, const int lda, double *X, const int incX)
{
  dtrsv(&Uplo, &TransA, &Diag, &N, A, &lda, X, &incX);
}

inline void b_dsymv(char Uplo, const int N, const double alpha, const double *A,
                    const int lda, const double *X, const int incX, const double beta,
                    double *Y, const int incY){
  dsymv(&Uplo, &N, &alpha, A, &lda, X, &incX, beta, Y, &incY);
}

*/


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

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cblas.h>

#include "schur.h"

/*
 * This module contains some routines related to manipulating the
 * Schur form of a matrix which are needed by the eigenvalue solvers
 *
 * This file contains routines based on original code from LAPACK
 * which is distributed under the modified BSD license. The LAPACK
 * routine used is DLANV2.
 */

static inline void schur_standard_form(gsl_matrix *A, gsl_complex *eval1,
                                       gsl_complex *eval2, double *cs,
                                       double *sn);

/*
gsl_schur_standardize()
  Wrapper function for schur_standard_form - convert a 2-by-2 eigenvalue
block to standard form and then update the Schur form and
Schur vectors.

Inputs: T        - Schur form
        row      - row of T of 2-by-2 block to be updated
        eval1    - where to store eigenvalue 1
        eval2    - where to store eigenvalue 2
        update_t - 1 = update the entire matrix T with the transformation
                   0 = do not update rest of T
        Z        - (optional) if non-null, accumulate transformation
*/

void
gsl_schur_standardize(gsl_matrix *T, size_t row, gsl_complex *eval1,
                      gsl_complex *eval2, int update_t, gsl_matrix *Z)
{
  const size_t N = T->size1;
  gsl_matrix_view m;
  double cs, sn;

  m = gsl_matrix_submatrix(T, row, row, 2, 2);
  schur_standard_form(&m.matrix, eval1, eval2, &cs, &sn);

  if (update_t)
    {
      gsl_vector_view xv, yv, v;

      /*
       * The above call to schur_standard_form transformed a 2-by-2 block
       * of T into upper triangular form via the transformation
       *
       * U = [ CS -SN ]
       *     [ SN  CS ]
       *
       * The original matrix T was
       *
       * T = [ T_{11} | T_{12} | T_{13} ]
       *     [   0*   |   A    | T_{23} ]
       *     [   0    |   0*   | T_{33} ]
       *
       * where 0* indicates all zeros except for possibly
       * one subdiagonal element next to A.
       *
       * After schur_standard_form, T looks like this:
       *
       * T = [ T_{11} | T_{12}  | T_{13} ]
       *     [   0*   | U^t A U | T_{23} ]
       *     [   0    |    0*   | T_{33} ]
       *
       * since only the 2-by-2 block of A was changed. However,
       * in order to be able to back transform T at the end,
       * we need to apply the U transformation to the rest
       * of the matrix T since there is no way to apply a
       * similarity transformation to T and change only the
       * middle 2-by-2 block. In other words, let
       *
       * M = [ I 0 0 ]
       *     [ 0 U 0 ]
       *     [ 0 0 I ]
       *
       * and compute
       *
       * M^t T M = [ T_{11} | T_{12} U |   T_{13}   ]
       *           [ U^t 0* | U^t A U  | U^t T_{23} ]
       *           [   0    |   0* U   |   T_{33}   ]
       *
       * So basically we need to apply the transformation U
       * to the i x 2 matrix T_{12} and the 2 x (n - i + 2)
       * matrix T_{23}, where i is the index of the top of A
       * in T.
       *
       * The BLAS routine drot() is suited for this.
       */

      if (row < (N - 2))
        {
          /* transform the 2 rows of T_{23} */

          v = gsl_matrix_row(T, row);
          xv = gsl_vector_subvector(&v.vector,
                                    row + 2,
                                    N - row - 2);

          v = gsl_matrix_row(T, row + 1);
          yv = gsl_vector_subvector(&v.vector,
                                    row + 2,
                                    N - row - 2);

          gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
        }

      if (row > 0)
        {
          /* transform the 2 columns of T_{12} */

          v = gsl_matrix_column(T, row);
          xv = gsl_vector_subvector(&v.vector,
                                    0,
                                    row);

          v = gsl_matrix_column(T, row + 1);
          yv = gsl_vector_subvector(&v.vector,
                                    0,
                                    row);

          gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
        }
    } /* if (update_t) */

  if (Z)
    {
      gsl_vector_view xv, yv;

      /*
       * Accumulate the transformation in Z. Here, Z -> Z * M
       *
       * So:
       *
       * Z -> [ Z_{11} | Z_{12} U | Z_{13} ]
       *      [ Z_{21} | Z_{22} U | Z_{23} ]
       *      [ Z_{31} | Z_{32} U | Z_{33} ]
       *
       * So we just need to apply drot() to the 2 columns
       * starting at index 'row'
       */

      xv = gsl_matrix_column(Z, row);
      yv = gsl_matrix_column(Z, row + 1);

      gsl_blas_drot(&xv.vector, &yv.vector, cs, sn);
    } /* if (Z) */
} /* gsl_schur_standardize() */

/*******************************************************
 *            INTERNAL ROUTINES                        *
 *******************************************************/

/*
schur_standard_form()
  Compute the Schur factorization of a real 2-by-2 matrix in
standard form:

[ A B ] = [ CS -SN ] [ T11 T12 ] [ CS SN ]
[ C D ]   [ SN  CS ] [ T21 T22 ] [-SN CS ]

where either:
1) T21 = 0 so that T11 and T22 are real eigenvalues of the matrix, or
2) T11 = T22 and T21*T12 < 0, so that T11 +/- sqrt(|T21*T12|) are
   complex conjugate eigenvalues

Inputs: A     - 2-by-2 matrix
        eval1 - where to store eigenvalue 1
        eval2 - where to store eigenvalue 2
        cs    - where to store cosine parameter of rotation matrix
        sn    - where to store sine parameter of rotation matrix

Notes: based on LAPACK routine DLANV2
*/

static inline void
schur_standard_form(gsl_matrix *A, gsl_complex *eval1, gsl_complex *eval2,
                    double *cs, double *sn)
{
  double a, b, c, d; /* input matrix values */
  double tmp;
  double p, z;
  double bcmax, bcmis, scale;
  double tau, sigma;
  double cs1, sn1;
  double aa, bb, cc, dd;
  double sab, sac;

  a = gsl_matrix_get(A, 0, 0);
  b = gsl_matrix_get(A, 0, 1);
  c = gsl_matrix_get(A, 1, 0);
  d = gsl_matrix_get(A, 1, 1);

  if (c == 0.0)
    {
      /*
       * matrix is already upper triangular - set rotation matrix
       * to the identity
       */
      *cs = 1.0;
      *sn = 0.0;
    }
  else if (b == 0.0)
    {
      /* swap rows and columns to make it upper triangular */

      *cs = 0.0;
      *sn = 1.0;

      tmp = d;
      d = a;
      a = tmp;
      b = -c;
      c = 0.0;
    }
  else if (((a - d) == 0.0) && (GSL_SIGN(b) != GSL_SIGN(c)))
    {
      /* the matrix has complex eigenvalues with a == d */
      *cs = 1.0;
      *sn = 0.0;
    }
  else
    {
      tmp = a - d;
      p = 0.5 * tmp;
      bcmax = GSL_MAX(fabs(b), fabs(c));
      bcmis = GSL_MIN(fabs(b), fabs(c)) * GSL_SIGN(b) * GSL_SIGN(c);
      scale = GSL_MAX(fabs(p), bcmax);
      z = (p / scale) * p + (bcmax / scale) * bcmis;

      if (z >= 4.0 * GSL_DBL_EPSILON)
        {
          /* real eigenvalues, compute a and d */

          z = p + GSL_SIGN(p) * fabs(sqrt(scale) * sqrt(z));
          a = d + z;
          d -= (bcmax / z) * bcmis;

          /* compute b and the rotation matrix */

          tau = gsl_hypot(c, z);
          *cs = z / tau;
          *sn = c / tau;
          b -= c;
          c = 0.0;
        }
      else
        {
          /*
           * complex eigenvalues, or real (almost) equal eigenvalues -
           * make diagonal elements equal
           */

          sigma = b + c;
          tau = gsl_hypot(sigma, tmp);
          *cs = sqrt(0.5 * (1.0 + fabs(sigma) / tau));
          *sn = -(p / (tau * (*cs))) * GSL_SIGN(sigma);

          /*
           * Compute [ AA BB ] = [ A B ] [ CS -SN ]
           *         [ CC DD ]   [ C D ] [ SN  CS ]
           */
          aa = a * (*cs) + b * (*sn);
          bb = -a * (*sn) + b * (*cs);
          cc = c * (*cs) + d * (*sn);
          dd = -c * (*sn) + d * (*cs);

          /*
           * Compute [ A B ] = [ CS SN ] [ AA BB ]
           *         [ C D ]   [-SN CS ] [ CC DD ]
           */
          a = aa * (*cs) + cc * (*sn);
          b = bb * (*cs) + dd * (*sn);
          c = -aa * (*sn) + cc * (*cs);
          d = -bb * (*sn) + dd * (*cs);

          tmp = 0.5 * (a + d);
          a = d = tmp;

          if (c != 0.0)
            {
              if (b != 0.0)
                {
                  if (GSL_SIGN(b) == GSL_SIGN(c))
                    {
                      /*
                       * real eigenvalues: reduce to upper triangular
                       * form
                       */
                      sab = sqrt(fabs(b));
                      sac = sqrt(fabs(c));
                      p = GSL_SIGN(c) * fabs(sab * sac);
                      tau = 1.0 / sqrt(fabs(b + c));
                      a = tmp + p;
                      d = tmp - p;
                      b -= c;
                      c = 0.0;

                      cs1 = sab * tau;
                      sn1 = sac * tau;
                      tmp = (*cs) * cs1 - (*sn) * sn1;
                      *sn = (*cs) * sn1 + (*sn) * cs1;
                      *cs = tmp;
                    }
                }
              else
                {
                  b = -c;
                  c = 0.0;
                  tmp = *cs;
                  *cs = -(*sn);
                  *sn = tmp;
                }
            }
        }
    }

  /* set eigenvalues */

  GSL_SET_REAL(eval1, a);
  GSL_SET_REAL(eval2, d);
  if (c == 0.0)
    {
      GSL_SET_IMAG(eval1, 0.0);
      GSL_SET_IMAG(eval2, 0.0);
    }
  else
    {
      tmp = sqrt(fabs(b) * fabs(c));
      GSL_SET_IMAG(eval1, tmp);
      GSL_SET_IMAG(eval2, -tmp);
    }

  /* set new matrix elements */

  gsl_matrix_set(A, 0, 0, a);
  gsl_matrix_set(A, 0, 1, b);
  gsl_matrix_set(A, 1, 0, c);
  gsl_matrix_set(A, 1, 1, d);
} /* schur_standard_form() */


// Ref:
// G.P. Lepage, LQCD for Novices
#include "su3_utils.c"
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_fit.h>

// globals
const gsl_rng_type *T;
gsl_rng *r; // random generator
su3_matrix links[4*N*N*N*N]; // link variables
su3_matrix rands[50]; // random unitary matrices

FILE *output;

// prototypes
void setup();
void cleanup();

int
main (int argc, char** argv)
{
  setup();

  int O[4]={0,0,0,0};

  for(int n=0;n<Ntherm;n++) {
    //fprintf(output, "%f \n",plaquette(links,0,1,O));
    hb_update(links,r,rands); //thermalize
  }

  // Confinement phase should have small Polyakov VEV
  if(confined){
    while(polyakov_mean(links) > pol_thres){
      hb_update(links,r,rands); //thermalize
    }
  }

  printf("Thermalized! \n");

  for(int n=0;n<Ncf;n++){
    // Decorrelate
    for(int k=0;k<Ncor;k++){
      hb_update(links,r,rands);
    }

    if(n%5==0){printf("%d \n",n);}
    // Measurement

    //fprintf(output, "%f %f \n", wloop_mean(1,1,links),wloop_mean(1,2,links));
    //fprintf(output, "%f %f %f %f %f %f %f %f\n",twloop2_mean(1,links),twloop2_mean(2,links),twloop2_mean(3,links),twloop2_mean(4,links),twloop2_mean(5,links),twloop2_mean(6,links),twloop2_mean(7,links),twloop2_mean(8,links));
    //fprintf(output, "%f %f %f %f %f %f %f %f\n",plaq_corr(1,links),plaq_corr(2,links),plaq_corr(3,links),plaq_corr(4,links),plaq_corr(5,links),plaq_corr(6,links),plaq_corr(7,links),plaq_corr(8,links));
    //printf("%f \n",polyakov_mean(links));
    printf( "%f \n", plaq_mean_s(0,links));
    printf( "%f %f \n", wloop_mean(1,1,links),wloop_mean(1,2,links) );
  }

  printf("acc=%f\n", (double)acc/tot);

  cleanup();
  return 0;
}

void
setup (){
  acc=0;
  tot=0;
  // ready random num generator
  gsl_rng_env_setup ();
  T = gsl_rng_mt19937;
  r = gsl_rng_alloc (T);
  // fill in table of rand matrices
  for(int i=0;i<25;i++){
    rands[2*i]=su3_rand(r);
    rands[2*i+1]=su3_inv(rands[2*i]);
  }
  //init links to identities
  for(int i=0;i<4*N*N*N*N;i++){
    links[i]=su3_unit();
  }

  output = fopen("out.txt", "w+");
}

void
cleanup (){
  gsl_rng_free(r);
  fclose(output);
}


/*	$Id$ */
/*
 * Copyright (c) 2014--2016 Kristaps Dzonsons <kristaps@kcons.eu>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */
#include <errno.h>
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#ifdef __linux__
#include <bsd/stdlib.h> /* arc4random() */
#endif
#include <string.h>

#include <gtk/gtk.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <kplot.h>

#include "extern.h"

/*
 * For a given point "x" in the domain, fit ourselves to the polynomial
 * coefficients of degree "fitpoly + 1".
 */
static double
fitpoly(const double *fits, size_t poly, double x)
{
	double	 y, v;
	size_t	 i, j;

	for (y = 0.0, i = 0; i < poly; i++) {
		v = fits[i];
		for (j = 0; j < i; j++)
			v *= x;
		y += v;
	}
	return(y);
}

/*
 * Copy the "hotlsb" data into "warm" holding.
 * While here, set our simulation's "work" parameter (if "fitpoly" is
 * set) to contain the necessary dependent variable.
 * We're guaranteed to be the only ones in here, and the only ones with
 * a lock on the LBS data.
 */
static void
snapshot(struct sim *sim, struct simwarm *warm,
	uint64_t truns, uint64_t tgens)
{
	double	 	v, chisq, x, y;
	struct kpair	kp;
	int		rc;
	size_t	 	i, j, k;

	/* Warm copy is already up to date. */
	if (warm->truns == truns) {
		g_assert(warm->tgens == tgens);
		return;
	}

	/* Copy out: we have a lock. */
	simbuf_copy_warm(sim->bufs.times);
	simbuf_copy_warm(sim->bufs.imeans);
	simbuf_copy_warm(sim->bufs.istddevs);
	simbuf_copy_warm(sim->bufs.islandmeans);
	simbuf_copy_warm(sim->bufs.islandstddevs);
	simbuf_copy_warm(sim->bufs.means);
	simbuf_copy_warm(sim->bufs.stddevs);
	simbuf_copy_warm(sim->bufs.mextinct);
	simbuf_copy_warm(sim->bufs.iextinct);
	warm->truns = truns;
	warm->tgens = tgens;

	/*
	 * If we're going to fit to a polynomial, set the dependent
	 * variable within the conditional.
	 */
	if (sim->fitpoly)
		for (i = 0; i < sim->dims; i++) {
			rc = kdata_get(sim->bufs.means->warm, i, &kp);
			g_assert(0 != rc);
			gsl_vector_set(sim->work.y, i, kp.y);
		}

	/*
	 * If we're going to run a weighted polynomial multifit, then
	 * use the variance as the weight.
	 */
	if (sim->fitpoly && sim->weighted)
		for (i = 0; i < sim->dims; i++) {
			rc = kdata_get(sim->bufs.stddevs->warm, i, &kp);
			g_assert(0 != rc);
			gsl_vector_set(sim->work.w, i, kp.y);
		}

	/*
	 * If we're not fitting to a polynomial, simply notify that
	 * we've copied out and continue on our way.
	 */
	if (0 == sim->fitpoly)
		return;

	/*
	 * If we're fitting to a polynomial, initialise our independent
	 * variables now.
	 */
	for (i = 0; i < sim->dims; i++) {
		gsl_matrix_set(sim->work.X, i, 0, 1.0);
		for (j = 0; j < sim->fitpoly; j++) {
			v = sim->xmin +
				(sim->xmax -
				 sim->xmin) *
				(i / (double)sim->dims);
			for (k = 0; k < j; k++)
				v *= v;
			gsl_matrix_set(sim->work.X, i, j + 1, v);
		}
	}

	/*
	 * Now perform the actual fitting.
	 * We either use weighted (weighted with the variance) or
	 * non-weighted fitting algorithms.
	 */
	if (sim->weighted)
		gsl_multifit_wlinear(sim->work.X, sim->work.w,
			sim->work.y, sim->work.c, sim->work.cov,
			&chisq, sim->work.work);
	else
		gsl_multifit_linear(sim->work.X, sim->work.y,
			sim->work.c, sim->work.cov, &chisq,
			sim->work.work);

	/*
	 * Now snapshot the fitted polynomial coefficients and compute
	 * the fitted approximation for all incumbents (along with the
	 * minimum value).
	 */
	for (i = 0; i < sim->fitpoly + 1; i++)
		sim->work.coeffs[i] = gsl_vector_get(sim->work.c, i);

	for (i = 0; i < sim->dims; i++) {
		x = sim->xmin + (sim->xmax - sim->xmin) *
			i / (double)sim->dims;
		y = fitpoly(sim->work.coeffs,
			sim->fitpoly + 1, x);
		kdata_array_set(sim->bufs.fitpoly, i, x, y);
	}
}

/*
 * In a given simulation, compute the next mutant/incumbent pair.
 * We make sure that incumbents are striped evenly in any given
 * simulation but that mutants are randomly selected from within the
 * strategy domain.
 */
static int
on_sim_next(struct sim *sim, const gsl_rng *rng,
	size_t *islandidx, double *mutantp, double *incumbentp,
	size_t *incumbentidx, double *vp, const size_t *islands,
	size_t gen)
{
	int		 dosnap, rc;
	size_t		 mutant;
	uint64_t	 truns, tgens;

	if (sim->terminate)
		return(0);

	truns = tgens = 0; /* Silence compiler. */

	/* This is set if we "own" the LSB->warm process. */
	dosnap = 0;

	g_assert(*incumbentidx < sim->dims);
	g_assert(*islandidx < sim->islands);
	g_mutex_lock(&sim->hot.mux);

	/*
	 * If we're entering this with a result value, then plug it into
	 * the index associated with the run and increment our count.
	 * This prevents us from overwriting others' results.
	 */
	if (NULL != vp) {
		rc = kdata_array_add
			(sim->bufs.times->hot, gen, 1.0);
		g_assert(0 != rc);
		rc = kdata_array_fill_ysizes
			(sim->bufs.islands, islands);
		g_assert(0 != rc);
		rc = kdata_array_set(sim->bufs.fractions,
			*incumbentidx, *incumbentp, *vp);
		g_assert(0 != rc);
		rc = kdata_array_set(sim->bufs.ifractions,
			*islandidx, *islandidx, *vp);
		g_assert(0 != rc);
		rc = kdata_array_set(sim->bufs.mutants,
			*incumbentidx, *incumbentp, 0.0 == *vp);
		g_assert(0 != rc);
		rc = kdata_array_set(sim->bufs.incumbents,
			*incumbentidx, *incumbentp, 1.0 == *vp);
		g_assert(0 != rc);
		sim->hot.tgens += gen;
		sim->hot.truns++;
	}

	/*
	 * Check if we've been requested to pause.
	 * If so, wait for a broadcast on our condition.
	 * This will unlock the mutex for others to process.
	 */
	if (1 == sim->hot.pause)
		g_cond_wait(&sim->hot.cond, &sim->hot.mux);

	/*
	 * Check if we've been instructed by the main thread of
	 * execution to snapshot hot data into warm storage.
	 * We do this in two parts: first, we register that we're in a
	 * copyout (it's now 2) and then actually do the copyout outside
	 * of the hot mutex.
	 */
	if (1 == sim->hot.copyout) {
		simbuf_copy_hotlsb(sim->bufs.times);
		simbuf_copy_hotlsb(sim->bufs.islandmeans);
		simbuf_copy_hotlsb(sim->bufs.islandstddevs);
		simbuf_copy_hotlsb(sim->bufs.imeans);
		simbuf_copy_hotlsb(sim->bufs.istddevs);
		simbuf_copy_hotlsb(sim->bufs.means);
		simbuf_copy_hotlsb(sim->bufs.stddevs);
		simbuf_copy_hotlsb(sim->bufs.mextinct);
		simbuf_copy_hotlsb(sim->bufs.iextinct);
		truns = sim->hot.truns;
		tgens = sim->hot.tgens;
		sim->hot.copyout = dosnap = 2;
	}

	/*
	 * Reassign our mutant and incumbent from the ring sized by the
	 * configured lattice dimensions.
	 * These both increment in single movements until the end of the
	 * lattice, then wrap around.
	 */
	*islandidx = sim->hot.island;
	mutant = sim->hot.mutant;
	*incumbentidx = sim->hot.incumbent;
	sim->hot.mutant++;
	if (sim->hot.mutant == sim->dims) {
		sim->hot.incumbent++;
		if (sim->hot.incumbent == sim->dims) {
			sim->hot.incumbent = 0;
			if (MAPINDEX_STRIPED == sim->mapindex)
				sim->hot.island =
					(sim->hot.island + 1) %
					sim->islands;
		}
		sim->hot.mutant = 0;
	}

	g_mutex_unlock(&sim->hot.mux);

	/*
	 * Assign our incumbent and mutant.
	 * The incumbent just gets the current lattice position,
	 * ensuring an even propogation.
	 * The mutant is either assigned the same way or from a Gaussian
	 * distribution around the current incumbent.
	 */
	*incumbentp = sim->xmin + (sim->xmax - sim->xmin) *
		(*incumbentidx / (double)sim->dims);

	if (MUTANTS_GAUSSIAN == sim->mutants) {
		do {
			*mutantp = *incumbentp +
				gsl_ran_gaussian
				(rng, sim->mutantsigma);
		} while (*mutantp < sim->ymin ||
			 *mutantp >= sim->ymax);
	} else
		*mutantp = sim->xmin +
			(sim->xmax -
			 sim->xmin) *
			(mutant / (double)sim->dims);

	/*
	 * If we were the ones to set the copyout bit, then do the
	 * copyout right now.
	 * When we're finished, lower the copyout semaphor.
	 */
	if (dosnap) {
		snapshot(sim, &sim->warm, truns, tgens);
		g_mutex_lock(&sim->hot.mux);
		g_assert(2 == sim->hot.copyout);
		sim->hot.copyout = 0;
		g_mutex_unlock(&sim->hot.mux);
	}

	return(1);
}

/*
 * For a given island (size "pop") player's strategy "x" where mutants
 * (numbering "mutants") have strategy "mutant" and incumbents
 * (numbering "incumbents") have strategy "incumbent", compute the
 * a(1 + delta(pi(x, X)) function.
 */
static double
reproduce(const struct sim *sim, double x,
	double mutant, double incumbent, size_t mutants, size_t pop)
{
	double	 v;

	if (0 == pop)
		return(0.0);

	v = hnode_exec
		((const struct hnode *const *)
		 sim->exp, x,
		 (mutants * mutant) + ((pop - mutants) * incumbent),
		 pop);
	g_assert( ! (isnan(v) || isinf(v)));
	return(sim->alpha * (1.0 + sim->delta * v));
}

static size_t
migrate(const struct sim *sim, const gsl_rng *rng, size_t cur)
{
	double	 v;
	size_t	 i;

again:
	while (0.0 == (v = gsl_rng_uniform(rng)))
		/* Loop. */ ;

	for (i = 0; i < sim->islands - 1; i++)
		if ((v -= sim->ms[cur][i]) <= 0.0)
			break;

	/*
	 * This can occur due to floating-point rounding.
	 * If it does, re-run the algorithm.
	 */
	if (i == sim->islands - 1 && i == cur) {
		g_debug("Degenerate probability: re-running");
		goto again;
	}

	g_assert(cur != i);
	return(i);
}

/*
 * Run a simulation.
 * This can be one thread of many within the same simulation.
 */
void *
simulation(void *arg)
{
	struct simthr	  *thr = arg;
	struct sim	  *sim = thr->sim;
	double		   mutant, incumbent, v, lambda, prob;
	unsigned int	   offs;
	unsigned long	   seed;
	double		  *vp, *icache, *mcache;
	double		***icaches, ***mcaches;
	size_t		  *kids[2], *migrants[2], *imutants, *npops,
			  *ndeaths;
	size_t		   t, i, j, k, new, mutants, incumbents,
			   len1, len2, incumbentidx, islandidx,
			   ntotalpop;
	int		   mutant_old, mutant_new;
	gsl_rng		  *rng;

	rng = gsl_rng_alloc(gsl_rng_default);
	seed = arc4random();
	gsl_rng_set(rng, seed);

	g_debug("%p: Thread (simulation %p) "
		"start", g_thread_self(), sim);

	icache = mcache = NULL;
	icaches = mcaches = NULL;
	kids[0] = g_malloc0_n(sim->islands, sizeof(size_t));
	kids[1] = g_malloc0_n(sim->islands, sizeof(size_t));
	ndeaths = g_malloc0_n(sim->islands, sizeof(size_t));
	migrants[0] = g_malloc0_n(sim->islands, sizeof(size_t));
	migrants[1] = g_malloc0_n(sim->islands, sizeof(size_t));
	imutants = g_malloc0_n(sim->islands, sizeof(size_t));
	vp = NULL;
	npops = NULL;
	incumbentidx = 0;
	islandidx = MAPINDEX_FIXED == sim->mapindex ?
		sim->mapindexfix : 0;
	mutant = incumbent = 0.0;
	t = 0;

	/*
	 * Set up our mutant and incumbent payoff caches.
	 * These consist of all possible payoffs with a given number of
	 * mutants and incumbents on an island.
	 * We have two ways of doing this: with non-uniform island sizes
	 * (icaches and mcaches) and uniform sizes (icache and mcache,
	 * notice the singular).
	 * The non-uniform island size can also change, so we precompute
	 * for all possible populations as well.
	 */
	if (NULL != sim->pops) {
		g_assert(0 == sim->pop);
		npops = g_malloc0_n(sim->islands, sizeof(size_t));
		for (i = 0; i < sim->islands; i++)
			npops[i] = sim->pops[i];
		icaches = g_malloc0_n(sim->islands, sizeof(double **));
		mcaches = g_malloc0_n(sim->islands, sizeof(double **));
		for (i = 0; i < sim->islands; i++) {
			icaches[i] = g_malloc0_n
				(sim->pops[i] + 1, sizeof(double *));
			mcaches[i] = g_malloc0_n
				(sim->pops[i] + 1, sizeof(double *));
			for (j = 0; j <= sim->pops[i]; j++) {
				icaches[i][j] = g_malloc0_n(j + 1, sizeof(double));
				mcaches[i][j] = g_malloc0_n(j + 1, sizeof(double));
			}
		}
	} else {
		g_assert(sim->pop > 0);
		icache = g_malloc0_n(sim->pop + 1, sizeof(double));
		mcache = g_malloc0_n(sim->pop + 1, sizeof(double));
	}
again:
	/*
	 * Repeat til we're instructed to terminate.
	 * We also pass in our last result for processing.
	 */
	if ( ! on_sim_next(sim, rng, &islandidx, &mutant,
		&incumbent, &incumbentidx, vp, imutants, t)) {
		g_debug("%p: Thread (simulation %p) exiting",
			g_thread_self(), sim);
		/*
		 * Upon termination, free up all of the memory
		 * associated with our simulation.
		 */
		g_free(ndeaths);
		g_free(imutants);
		g_free(kids[0]);
		g_free(kids[1]);
		g_free(migrants[0]);
		g_free(migrants[1]);
		if (NULL != sim->pops)
			for (i = 0; i < sim->islands; i++) {
				for (j = 0; j <= sim->pops[i]; j++) {
					g_free(icaches[i][j]);
					g_free(mcaches[i][j]);
				}
				g_free(icaches[i]);
				g_free(mcaches[i]);
			}
		g_free(icaches);
		g_free(mcaches);
		g_free(icache);
		g_free(mcache);
		return(NULL);
	}

	/*
	 * Initialise a random island to have one mutant.
	 * The rest are all incumbents.
	 */
	memset(imutants, 0, sim->islands * sizeof(size_t));
	imutants[islandidx] = 1;
	mutants = 1;
	incumbents = sim->totalpop - mutants;
	ntotalpop = sim->totalpop;

	/*
	 * Precompute all possible payoffs.
	 * This allows us not to re-run the lambda calculation for each
	 * individual.
	 * If we have only a single island size, then avoid allocating
	 * for each island by using only the first mcaches index.
	 */
	if (NULL != sim->pops)
		for (i = 0; i < sim->islands; i++) {
			for (j = 0; j <= sim->pops[i]; j++) {
				for (k = 0; k <= j; k++) {
					mcaches[i][j][k] = reproduce
						(sim, mutant, mutant,
						 incumbent, k, j);
					icaches[i][j][k] = reproduce
						(sim, incumbent, mutant,
						 incumbent, k, j);
				}
			}
		}
	else
		for (i = 0; i <= sim->pop; i++) {
			mcache[i] = reproduce(sim, mutant,
				mutant, incumbent, i, sim->pop);
			icache[i] = reproduce(sim, incumbent,
				mutant, incumbent, i, sim->pop);
		}

	for (t = 0; t < sim->stop; t++) {
		if (NULL != sim->pops && sim->ideathmean > 0) {
			/*
			 * If we're a non-uniform population and have an
			 * island death mean, then see if we're supposed
			 * to kill off islands, then re-set the shot
			 * clock.
			 */
			for (i = 0; i < sim->islands; i++) {
				/*
				 * If the shot-clock has not been
				 * started OR the island is already
				 * dead, set it again.
				 */
				if (0 == ndeaths[i] || 0 == npops[i]) {
					ndeaths[i] = t + 1 +
						gsl_ran_poisson
						(rng, sim->ideathmean);
					continue;
				} else if (t != ndeaths[i])
					continue;

				ndeaths[i] = t + 1 +
					gsl_ran_poisson
					(rng, sim->ideathmean);

				/*
				 * What's the sum of our payoffs?
				 * Compute the probability that we're
				 * going to be killed from that and our
				 * coefficient.
				 */
				g_assert(npops[i] > 0);
				v = mcaches[i][npops[i]][imutants[i]] *
					imutants[i] +
				    icaches[i][npops[i]][imutants[i]] *
				    	(npops[i] - imutants[i]);
				prob = sim->ideathcoef * exp(-v);
				if (gsl_rng_uniform(rng) >= prob)
					continue;
				mutants -= imutants[i];
				incumbents -= (npops[i] - imutants[i]);
				ntotalpop -= npops[i];
				npops[i] = 0;
				imutants[i] = 0;
			}
		}

		/*
		 * Birth process: have each individual (first mutants,
		 * then incumbents) give birth.
		 * Use a Poisson process with the given mean in order to
		 * properly compute this.
		 * We need two separate versions for whichever type of
		 * mcache we decide to use.
		 */
		if (NULL != sim->pops)
			for (j = 0; j < sim->islands; j++) {
				if (0 == npops[j])
					continue;
				g_assert(0 == kids[0][j]);
				g_assert(0 == kids[1][j]);
				g_assert(0 == migrants[0][j]);
				g_assert(0 == migrants[1][j]);
				g_assert(imutants[j] <= npops[j]);
				lambda = mcaches[j]
					[npops[j]][imutants[j]];
				for (k = 0; k < imutants[j]; k++) {
					offs = gsl_ran_poisson
						(rng, lambda);
					kids[0][j] += offs;
				}
				lambda = icaches[j]
					[npops[j]][imutants[j]];
				for ( ; k < npops[j]; k++) {
					offs = gsl_ran_poisson
						(rng, lambda);
					kids[1][j] += offs;
				}
			}
		else
			for (j = 0; j < sim->islands; j++) {
				g_assert(0 == kids[0][j]);
				g_assert(0 == kids[1][j]);
				g_assert(0 == migrants[0][j]);
				g_assert(0 == migrants[1][j]);
				lambda = mcache[imutants[j]];
				for (k = 0; k < imutants[j]; k++) {
					offs = gsl_ran_poisson
						(rng, lambda);
					kids[0][j] += offs;
				}
				lambda = icache[imutants[j]];
				for ( ; k < sim->pop; k++) {
					offs = gsl_ran_poisson
						(rng, lambda);
					kids[1][j] += offs;
				}
			}

		/*
		 * Determine whether we're going to migrate and, if
		 * migration is stipulated, to where.
		 */
		if (NULL != sim->ms)
			for (j = 0; j < sim->islands; j++) {
				for (k = 0; k < kids[0][j]; k++) {
					new = j;
					if (gsl_rng_uniform(rng) < sim->m)
						new = migrate(sim, rng, j);
					migrants[0][new]++;
				}
				for (k = 0; k < kids[1][j]; k++) {
					new = j;
					if (gsl_rng_uniform(rng) < sim->m)
						new = migrate(sim, rng, j);
					migrants[1][new]++;
				}
				kids[0][j] = kids[1][j] = 0;
			}
		else
			for (j = 0; j < sim->islands; j++) {
				for (k = 0; k < kids[0][j]; k++) {
					new = j;
					if (gsl_rng_uniform(rng) < sim->m) do
						new = gsl_rng_uniform_int
							(rng, sim->islands);
					while (new == j);
					migrants[0][new]++;
				}
				for (k = 0; k < kids[1][j]; k++) {
					new = j;
					if (gsl_rng_uniform(rng) < sim->m) do
						new = gsl_rng_uniform_int
							(rng, sim->islands);
					while (new == j);
					migrants[1][new]++;
				}
				kids[0][j] = kids[1][j] = 0;
			}

		/*
		 * Perform the migration itself.
		 * We randomly select an individual on the destination
		 * island as well as one from the migrant queue.
		 * We then replace one with the other.
		 */
		if (NULL != sim->pops)
			for (j = 0; j < sim->islands; j++) {
				if (npops[j] < sim->pops[j]) {
					/*
					 * This is the case where a
					 * given island has had its
					 * population killed off.
					 * Try to fill it up: don't
					 * replace anybody.
					 */
					while (npops[j] < sim->pops[j]) {
						len1 = migrants[0][j] +
							migrants[1][j];
						if (0 == len1)
							break;
						len2 = gsl_rng_uniform_int
							(rng, len1);
						if (len2 < migrants[0][j]) {
							migrants[0][j]--;
							imutants[j]++;
							mutants++;
						} else {
							migrants[1][j]--;
							incumbents++;
						}
						npops[j]++;
						ntotalpop++;
					}
				} else {
					len1 = migrants[0][j] +
						migrants[1][j];
					if (0 == len1)
						continue;
					len2 = gsl_rng_uniform_int
						(rng, npops[j]);
					mutant_old = len2 < imutants[j];
					len2 = gsl_rng_uniform_int(rng, len1);
					mutant_new = len2 < migrants[0][j];
					if (mutant_old && ! mutant_new) {
						imutants[j]--;
						mutants--;
						incumbents++;
					} else if ( ! mutant_old && mutant_new) {
						imutants[j]++;
						mutants++;
						incumbents--;
					}
				}
				migrants[0][j] = migrants[1][j] = 0;
			}
		else
			for (j = 0; j < sim->islands; j++) {
				len1 = migrants[0][j] + migrants[1][j];
				if (0 == len1)
					continue;

				len2 = gsl_rng_uniform_int(rng, sim->pop);
				mutant_old = len2 < imutants[j];
				len2 = gsl_rng_uniform_int(rng, len1);
				mutant_new = len2 < migrants[0][j];

				if (mutant_old && ! mutant_new) {
					imutants[j]--;
					mutants--;
					incumbents++;
				} else if ( ! mutant_old && mutant_new) {
					imutants[j]++;
					mutants++;
					incumbents--;
				}

				migrants[0][j] = migrants[1][j] = 0;
			}

		/* Stop when a population goes extinct. */
		if (0 == mutants || 0 == incumbents)
			break;
	}

	/*
	 * Assign the result pointer to the last population fraction.
	 * This will be processed by on_sim_next().
	 */
	if (incumbents == 0) {
		g_assert(mutants == ntotalpop);
		v = 1.0;
	} else if (mutants == 0) {
		g_assert(incumbents == ntotalpop);
		v = 0.0;
	} else
		v = mutants / (double)ntotalpop;

	vp = &v;
	goto again;
}