static char help[] =
"Solves structured-grid Poisson problem in 1D, 2D, 3D.  Option prefix fsh_.\n"
"Equation is\n"
"    - cx u_xx - cy u_yy - cz u_zz = f,\n"
"subject to Dirichlet boundary conditions.  Solves three different problems\n"
"where exact solution is known.  Uses DMDA and SNES.  Equation is put in form\n"
"F(u) = - grad^2 u - f.  Call-backs fully-rediscretize for the supplied grid.\n"
"Defaults to 2D, a SNESType of KSPONLY, and a KSPType of CG.\n\n";

#include <petsc.h>
#include "poissonfunctions.h"

// exact solutions  u(x,y),  for boundary condition and error calculation

static double u_exact_1Dmanupoly(double x, double y, double z, void *ctx) {
    return x*x * (1.0 - x*x);
}

static double u_exact_2Dmanupoly(double x, double y, double z, void *ctx) {
    return x*x * (1.0 - x*x) * y*y *(y*y - 1.0);
}

static double u_exact_3Dmanupoly(double x, double y, double z, void *ctx) {
    return x*x * (1.0 - x*x) * y*y * (y*y - 1.0) * z*z * (z*z - 1.0);
}

static double u_exact_1Dmanuexp(double x, double y, double z, void *ctx) {
    return - exp(x);
}

static double u_exact_2Dmanuexp(double x, double y, double z, void *ctx) {
    return - x * exp(y);
}

static double u_exact_3Dmanuexp(double x, double y, double z, void *ctx) {
    return - x * exp(y + z);
}

static double zero(double x, double y, double z, void *ctx) {
    return 0.0;
}

// right-hand-side functions  f(x,y) = - laplacian u

static double f_rhs_1Dmanupoly(double x, double y, double z, void *ctx) {
    PoissonCtx* user = (PoissonCtx*)ctx;
    return user->cx * 12.0 * x*x - 2.0;
}

static double f_rhs_2Dmanupoly(double x, double y, double z, void *ctx) {
    PoissonCtx* user = (PoissonCtx*)ctx;
    double aa, bb, ddaa, ddbb;
    aa = x*x * (1.0 - x*x);
    bb = y*y * (y*y - 1.0);
    ddaa = 2.0 * (1.0 - 6.0 * x*x);
    ddbb = 2.0 * (6.0 * y*y - 1.0);
    return - (user->cx * ddaa * bb + user->cy * aa * ddbb);
}

static double f_rhs_3Dmanupoly(double x, double y, double z, void *ctx) {
    PoissonCtx* user = (PoissonCtx*)ctx;
    double aa, bb, cc, ddaa, ddbb, ddcc;
    aa = x*x * (1.0 - x*x);
    bb = y*y * (y*y - 1.0);
    cc = z*z * (z*z - 1.0);
    ddaa = 2.0 * (1.0 - 6.0 * x*x);
    ddbb = 2.0 * (6.0 * y*y - 1.0);
    ddcc = 2.0 * (6.0 * z*z - 1.0);
    return - (user->cx * ddaa * bb * cc + user->cy * aa * ddbb * cc + user->cz * aa * bb * ddcc);
}

static double f_rhs_1Dmanuexp(double x, double y, double z, void *ctx) {
    return exp(x);
}

static double f_rhs_2Dmanuexp(double x, double y, double z, void *ctx) {
    return x * exp(y);  // note  f = - (u_xx + u_yy) = - u
}

static double f_rhs_3Dmanuexp(double x, double y, double z, void *ctx) {
    return 2.0 * x * exp(y + z);  // note  f = - laplacian u = - 2 u
}

// functions simply to put u_exact()=g_bdry() into a grid
// these are irritatingly-dimension-dependent inside ...
extern PetscErrorCode Form1DUExact(DMDALocalInfo*, Vec, PoissonCtx*);
extern PetscErrorCode Form2DUExact(DMDALocalInfo*, Vec, PoissonCtx*);
extern PetscErrorCode Form3DUExact(DMDALocalInfo*, Vec, PoissonCtx*);

//STARTPTRARRAYS
// arrays of pointers to functions:   ..._ptr[DIMS]
static void* residual_ptr[3]
    = {&Poisson1DFunctionLocal, &Poisson2DFunctionLocal, &Poisson3DFunctionLocal};

static void* jacobian_ptr[3]
    = {&Poisson1DJacobianLocal, &Poisson2DJacobianLocal, &Poisson3DJacobianLocal};

static void* getuexact_ptr[3]
    = {&Form1DUExact, &Form2DUExact, &Form3DUExact};
//ENDPTRARRAYS

typedef enum {MANUPOLY, MANUEXP, ZERO} ProblemType;
static const char* ProblemTypes[] = {"manupoly","manuexp","zero",
                                     "ProblemType", "", NULL};

// more arrays of pointers to functions:   ..._ptr[DIMS][PROBLEMS]
static void* g_bdry_ptr[3][3]
    = {{&u_exact_1Dmanupoly, &u_exact_1Dmanuexp, &zero},
       {&u_exact_2Dmanupoly, &u_exact_2Dmanuexp, &zero},
       {&u_exact_3Dmanupoly, &u_exact_3Dmanuexp, &zero}};

static void* f_rhs_ptr[3][3]
    = {{&f_rhs_1Dmanupoly, &f_rhs_1Dmanuexp, &zero},
       {&f_rhs_2Dmanupoly, &f_rhs_2Dmanuexp, &zero},
       {&f_rhs_3Dmanupoly, &f_rhs_3Dmanuexp, &zero}};

static const char* InitialTypes[] = {"zeros","random",
                                     "InitialType", "", NULL};

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    DM             da, da_after;
    SNES           snes;
    KSP            ksp;
    Vec            u_initial, u, u_exact;
    PoissonCtx     user;
    DMDALocalInfo  info;
    double         errinf, normconst2h, err2h;
    char           gridstr[99];
    PetscErrorCode (*getuexact)(DMDALocalInfo*,Vec,PoissonCtx*);

    // fish defaults:
    int            dim = 2;                  // 2D
    ProblemType    problem = MANUEXP;        // manufactured problem using exp()
    InitialType    initial = ZEROS;          // set u=0 for initial iterate
    PetscBool      gonboundary = PETSC_TRUE; // initial iterate has u=g on boundary

    // get options and configure context
    user.Lx = 1.0;
    user.Ly = 1.0;
    user.Lz = 1.0;
    user.cx = 1.0;
    user.cy = 1.0;
    user.cz = 1.0;
    PetscInitialize(&argc,&argv,NULL,help);
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"fsh_", "options for fish.c", ""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-cx",
         "set coefficient of x term u_xx in equation",
         "fish.c",user.cx,&user.cx,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsReal("-cy",
         "set coefficient of y term u_yy in equation",
         "fish.c",user.cy,&user.cy,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsReal("-cz",
         "set coefficient of z term u_zz in equation",
         "fish.c",user.cz,&user.cz,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsInt("-dim",
         "dimension of problem (=1,2,3 only)",
         "fish.c",dim,&dim,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsBool("-initial_gonboundary",
         "set initial iterate to have correct boundary values",
         "fish.c",gonboundary,&gonboundary,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-initial_type",
         "type of initial iterate",
         "fish.c",InitialTypes,(PetscEnum)initial,(PetscEnum*)&initial,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-Lx",
         "set Lx in domain ([0,Lx] x [0,Ly] x [0,Lz], etc.)",
         "fish.c",user.Lx,&user.Lx,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsReal("-Ly",
         "set Ly in domain ([0,Lx] x [0,Ly] x [0,Lz], etc.)",
         "fish.c",user.Ly,&user.Ly,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsReal("-Lz",
         "set Ly in domain ([0,Lx] x [0,Ly] x [0,Lz], etc.)",
         "fish.c",user.Lz,&user.Lz,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-problem",
         "problem type; determines exact solution and RHS",
         "fish.c",ProblemTypes,(PetscEnum)problem,(PetscEnum*)&problem,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);
    user.g_bdry = g_bdry_ptr[dim-1][problem];
    user.f_rhs = f_rhs_ptr[dim-1][problem];
    if ( user.cx <= 0.0 || user.cy <= 0.0 || user.cz <= 0.0 ) {
        SETERRQ(PETSC_COMM_WORLD,2,"positivity required for coefficients cx,cy,cz\n");
    }
    if ((problem == MANUEXP) && ( user.cx != 1.0 || user.cy != 1.0 || user.cz != 1.0)) {
        SETERRQ(PETSC_COMM_WORLD,3,"cx=cy=cz=1 required for problem MANUEXP\n");
    }

//STARTCREATE
    // DMDA in chosen dimension
    switch (dim) {
        case 1:
            ierr = DMDACreate1d(PETSC_COMM_WORLD,
                DM_BOUNDARY_NONE,3,1,1, NULL, &da); CHKERRQ(ierr);
            break;
        case 2:
            ierr = DMDACreate2d(PETSC_COMM_WORLD,
                DM_BOUNDARY_NONE,DM_BOUNDARY_NONE,DMDA_STENCIL_STAR,
                3,3,PETSC_DECIDE,PETSC_DECIDE,1,1,NULL,NULL,&da); CHKERRQ(ierr);
            break;
        case 3:
            ierr = DMDACreate3d(PETSC_COMM_WORLD,
                DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DM_BOUNDARY_NONE,
                DMDA_STENCIL_STAR,
                3,3,3,PETSC_DECIDE,PETSC_DECIDE,PETSC_DECIDE,
                1,1,NULL,NULL,NULL,&da); CHKERRQ(ierr);
            break;
        default:
            SETERRQ(PETSC_COMM_WORLD,1,"invalid dim for DMDA creation\n");
    }
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);  // call BEFORE SetUniformCoordinates
    ierr = DMDASetUniformCoordinates(da,0.0,user.Lx,0.0,user.Ly,0.0,user.Lz); CHKERRQ(ierr);

    // create SNES supply call-backs
    ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
    ierr = SNESSetDM(snes,da); CHKERRQ(ierr);
    ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
             (DMDASNESFunction)(residual_ptr[dim-1]),&user); CHKERRQ(ierr);
    ierr = DMDASNESSetJacobianLocal(da,
             (DMDASNESJacobian)(jacobian_ptr[dim-1]),&user); CHKERRQ(ierr);

    // default to KSPONLY+CG because problem is linear and SPD
    ierr = SNESSetType(snes,SNESKSPONLY); CHKERRQ(ierr);
    ierr = SNESGetKSP(snes,&ksp); CHKERRQ(ierr);
    ierr = KSPSetType(ksp,KSPCG); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

    // initial iterate and solve
    ierr = DMGetGlobalVector(da,&u_initial); CHKERRQ(ierr);
    ierr = InitialState(da, initial, gonboundary, u_initial, &user); CHKERRQ(ierr);
    ierr = SNESSolve(snes,NULL,u_initial); CHKERRQ(ierr);
//ENDCREATE

//STARTGETSOLUTION
    // -snes_grid_sequence could change grid resolution
    ierr = DMRestoreGlobalVector(da,&u_initial); CHKERRQ(ierr);
    ierr = DMDestroy(&da); CHKERRQ(ierr);

    // evaluate error and report
    ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr);  // SNES owns u; do not destroy it
    ierr = VecDuplicate(u,&u_exact); CHKERRQ(ierr);
    ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr); // SNES owns da_after; do not destroy it
    ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
    getuexact = getuexact_ptr[dim-1];
    ierr = (*getuexact)(&info,u_exact,&user); CHKERRQ(ierr);
    ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr);   // u <- u + (-1.0) uexact
    ierr = VecDestroy(&u_exact); CHKERRQ(ierr);      // no longer needed
    ierr = VecNorm(u,NORM_INFINITY,&errinf); CHKERRQ(ierr);
    ierr = VecNorm(u,NORM_2,&err2h); CHKERRQ(ierr);
//ENDGETSOLUTION

    switch (dim) {
        case 1:
            normconst2h = PetscSqrtReal((double)(info.mx-1));
            snprintf(gridstr,99,"%d point 1D",info.mx);
            break;
        case 2:
            normconst2h = PetscSqrtReal((double)(info.mx-1)*(info.my-1));
            snprintf(gridstr,99,"%d x %d point 2D",info.mx,info.my);
            break;
        case 3:
            normconst2h = PetscSqrtReal((double)(info.mx-1)*(info.my-1)*(info.mz-1));
            snprintf(gridstr,99,"%d x %d x %d point 3D",info.mx,info.my,info.mz);
            break;
        default:
            SETERRQ(PETSC_COMM_WORLD,4,"invalid dim value in final report\n");
    }
    err2h /= normconst2h; // like continuous L2
    ierr = PetscPrintf(PETSC_COMM_WORLD,
                "problem %s on %s grid:\n"
                "  error |u-uexact|_inf = %.3e, |u-uexact|_h = %.3e\n",
                ProblemTypes[problem],gridstr,errinf,err2h); CHKERRQ(ierr);

    // destroy what we explicitly Created
    ierr = SNESDestroy(&snes); CHKERRQ(ierr);
    return PetscFinalize();
}

PetscErrorCode Form1DUExact(DMDALocalInfo *info, Vec u, PoissonCtx* user) {
  PetscErrorCode ierr;
  int          i;
  double       xmax[1], xmin[1], hx, x, *au;
  ierr = DMDAGetBoundingBox(info->da,xmin,xmax); CHKERRQ(ierr);
  hx = (xmax[0] - xmin[0]) / (info->mx - 1);
  ierr = DMDAVecGetArray(info->da, u, &au);CHKERRQ(ierr);
  for (i=info->xs; i<info->xs+info->xm; i++) {
      x = xmin[0] + i * hx;
      au[i] = user->g_bdry(x,0.0,0.0,user);
  }
  ierr = DMDAVecRestoreArray(info->da, u, &au);CHKERRQ(ierr);
  return 0;
}

PetscErrorCode Form2DUExact(DMDALocalInfo *info, Vec u, PoissonCtx* user) {
    PetscErrorCode ierr;
    int     i, j;
    double  xymin[2], xymax[2], hx, hy, x, y, **au;
    ierr = DMDAGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    ierr = DMDAVecGetArray(info->da, u, &au);CHKERRQ(ierr);
    for (j=info->ys; j<info->ys+info->ym; j++) {
        y = xymin[1] + j * hy;
        for (i=info->xs; i<info->xs+info->xm; i++) {
            x = xymin[0] + i * hx;
            au[j][i] = user->g_bdry(x,y,0.0,user);
        }
    }
    ierr = DMDAVecRestoreArray(info->da, u, &au);CHKERRQ(ierr);
    return 0;
}

PetscErrorCode Form3DUExact(DMDALocalInfo *info, Vec u, PoissonCtx* user) {
    PetscErrorCode ierr;
    int    i, j, k;
    double xyzmin[3], xyzmax[3], hx, hy, hz, x, y, z, ***au;
    ierr = DMDAGetBoundingBox(info->da,xyzmin,xyzmax); CHKERRQ(ierr);
    hx = (xyzmax[0] - xyzmin[0]) / (info->mx - 1);
    hy = (xyzmax[1] - xyzmin[1]) / (info->my - 1);
    hz = (xyzmax[2] - xyzmin[2]) / (info->mz - 1);
    ierr = DMDAVecGetArray(info->da, u, &au);CHKERRQ(ierr);
    for (k=info->zs; k<info->zs+info->zm; k++) {
        z = xyzmin[2] + k * hz;
        for (j=info->ys; j<info->ys+info->ym; j++) {
            y = xyzmin[1] + j * hy;
            for (i=info->xs; i<info->xs+info->xm; i++) {
                x = xyzmin[0] + i * hx;
                au[k][j][i] = user->g_bdry(x,y,z,user);
            }
        }
    }
    ierr = DMDAVecRestoreArray(info->da, u, &au);CHKERRQ(ierr);
    return 0;
}


// Use the geodesic solver code to create a gsl implementation so you can
// compare the speeds.  The template needs to get close to ,ideally better than,
// exactly to the gsl run times

#include <stdio.h>
#include <iostream>
#include <chrono>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>

const double alpha = 1.32754125e20;
int odefunc (double x, const double y[], double f[], void *params)
{
    f[0] = y[3];
    f[1] = y[4];
    f[2] = y[5];
    double r = sqrt(y[0]*y[0]+y[1]*y[1]+y[2]*y[2]);

    f[3] = -(alpha*y[0]) / (r*r*r);
    f[4] = -(alpha*y[1]) / (r*r*r);
    f[5] = -(alpha*y[2]) / (r*r*r);
    return GSL_SUCCESS;
}

int main ()
{
    double AU = 1.496e11; // astronomical unit
    double V = 30000.0; // initial velocity
    // initial conditions
    double y0[] = {0.0,AU,0.0,-V,0.0,0.0};
    double T0 = 0.0;
    double Tf = 7.2e6;
    double hi = 7.2e3/3;
    int dim = 6;
    gsl_odeiv2_system sys = {odefunc, NULL, dim, NULL};

    gsl_odeiv2_driver * d = gsl_odeiv2_driver_alloc_y_new (&sys, gsl_odeiv2_step_rkf45, hi, 1e-5, 0.0);
    auto Nruns = 5000;
    auto totalDuration = 0;
    auto t1 = std::chrono::high_resolution_clock::now();
    double T;
    for (int i = 1; i <= Nruns; i++)
    {
        T0 = 0.0;
        T = Tf;
        y0[0]=0.0;
        y0[1] = AU;
        y0[2] = 0.0;
        y0[3] = -V;
        y0[4] = 0.0;
        y0[5] = 0.0;
        gsl_odeiv2_driver_apply (d, &T0, T, y0);
    }

    auto t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>( t2 - t1 ).count();
    totalDuration += duration;
    std::cout << "GSL runs" << std::endl;
    std::cout << totalDuration/Nruns << std::endl;
    for(int j = 0 ; j < 6 ; j++)
    {
        std::cout << y0[j] << ",";
    }
    std::cout << std::endl;
    gsl_odeiv2_driver_free (d);
    return 0;
}


//
//  trsm.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 1/5/12.
//  Copyright (c) 2012 University of Colorado Denver. All rights reserved.
//

#ifndef _trsm_h
#define _trsm_h

/// @file trsm.h Solves triangular matrix equations.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief Solves a real triangular matrix equation.
   ///
   /// For a real upper or lower triangular matrix A, real rectangular matrix B and real scalar alpha,
   ///
   ///        A*X=alpha*B  or  A'*X=alpha*B  or  X*A=alpha*B  or  X*A'=alpha*B
   /// is solved for the matrix X.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param side Specifies whether the matrix A appears on the left or right side as follows:
   ///
   ///        if side = 'L' or 'l' then A*X=alpha*B  or  A'*X=alpha*B is solved
   ///        if side = 'R' or 'r' then X*A=alpha*B  or  X*A'=alpha*B is solved
   /// @param uplo Specifies whether A is stored as upper or lower triangular.
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular
   ///        if uplo = 'L' or 'l' then A is lower triangular
   /// @param trans Specifies whether the transpose or conjugate transpose of A is to be used:
   ///
   ///        if trans = 'N' or 'n' then A*X=alpha*B or X*A=alpha*B is solved
   ///        if trans = 'T' or 't' then A'*X=alpha*B or X*A'=alpha*B is solved
   ///        if trans = 'C' or 'c' then A'*X=alpha*B or X*A'=alpha*B is solved
   /// @param diag specifies whether or not A is unit triangular as follows:
   ///
   ///        if diag = 'U' or 'u' then A is assumed to be unit triangular
   ///        if diag = 'N' or 'n' then A is not assumed to be unit triangular.
   /// If A is unit triangular, the diagonal elements are assumed to be unity and are not referenced.
   /// @param m Specifies the number of rows of the matrix B.  m>=0
   /// @param n Specifies the number of columns of the matrix B.  n>=0
   /// @param alpha Real scalar.
   /// @param A Pointer to real triangular matrix A.  The order of A is n if side = 'L' or 'l';
   /// the order of A is m is side = 'R' or 'r'.
   /// If uplo = 'U' or 'u, A is upper triangular and the lower triangular part is not referenced.
   /// If uplo = 'L' or 'l A is lower triangular and the upper triangular part is not referenced.
   /// @param ldA Column length of the matrix A.  If side='L' or 'l' then ldA>=n; if side='R' or 'r' then ldA>=m.
   /// @param B Pointer to real m-by-n matrix B.  On entry, B contains the right hand side matrix B.
   /// On exit, B is overwritten with the solution matrix X.
   /// @param ldB Column length of the matrix B.  ldB>=m.
   /// @ingroup BLAS

   template <typename real_t>
   int TRSM(char side, char uplo, char trans, char diag, int_t m, int_t n, real_t alpha, real_t *A, int_t ldA, real_t *B, int_t ldB)
   {
      using std::toupper;

      const real_t zero(0.0);
      const real_t one(1.0);
      int_t i,j,k;
      real_t *a,*b,*bt;
      real_t t;
      bool nounit;

      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);

      nounit=(diag=='N')?1:0;

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      if(alpha==zero)
      {
         b=B;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               b[i]=zero;
            b+=ldB;
         }
      }
      else
      {
         if(side=='L')
         {
            if(trans=='N')
            {
               if(uplo=='U')
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     a=A+m*ldA;
                     for(k=m-1;k>=0;k--)
                     {
                        a-=ldA;
                        if(nounit)
                           b[k]=b[k]/a[k];
                        for(i=0;i<k;i++)
                           b[i]-=b[k]*a[i];
                     }
                     b+=ldB;
                  }
               }
               else
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     a=A;
                     for(k=0;k<m;k++)
                     {
                        if(nounit)
                           b[k]=b[k]/a[k];
                        for(i=k+1;i<m;i++)
                           b[i]-=b[k]*a[i];
                        a+=ldA;
                     }
                     b+=ldB;
                  }
               }
            }
            else
            {
               if(uplo=='U')
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     a=A;
                     for(i=0;i<m;i++)
                     {
                        t=alpha*b[i];
                        for(k=0;k<i;k++)
                           t-=a[k]*b[k];
                        if(nounit)
                           t=t/a[i];
                        b[i]=t;
                        a+=ldA;
                     }
                     b+=ldB;
                  }
               }
               else
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     a=A+m*ldA;
                     for(i=m-1;i>=0;i--)
                     {
                        a-=ldA;
                        t=alpha*b[i];
                        for(k=i+1;k<m;k++)
                           t-=a[k]*b[k];
                        if(nounit)
                           t=t/a[i];
                        b[i]=t;
                     }
                     b+=ldB;
                  }
               }
            }
         }
         else
         {
            if(trans=='N')
            {
               if(uplo=='U')
               {
                  b=B;
                  a=A;
                  for(j=0;j<n;j++)
                  {
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     bt=B;
                     for(k=0;k<j;k++)
                     {
                        for(i=0;i<m;i++)
                           b[i]-=a[k]*bt[i];
                        bt+=ldB;
                     }
                     if(nounit)
                     {
                        t=one/a[j];
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                     a+=ldA;
                     b+=ldB;
                  }
               }
               else
               {
                  b=B+n*ldB;
                  a=A+n*ldA;
                  for(j=n-1;j>=0;j--)
                  {
                     a-=ldA;
                     b-=ldB;
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     bt=B+(j+1)*ldB;
                     for(k=j+1;k<n;k++)
                     {
                        for(i=0;i<m;i++)
                           b[i]-=a[k]*bt[i];
                        bt+=ldB;
                     }
                     if(nounit)
                     {
                        t=one/a[j];
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                  }
               }
            }
            else
            {
               if(uplo=='U')
               {
                  b=B+n*ldB;
                  a=A+n*ldA;
                  for(k=n-1;k>=0;k--)
                  {
                     a-=ldA;
                     b-=ldB;
                     if(nounit)
                     {
                        t=one/a[k];
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                     bt=B;
                     for(j=0;j<k;j++)
                     {
                        t=a[j];
                        for(i=0;i<m;i++)
                           bt[i]-=t*b[i];
                        bt+=ldB;
                     }
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                  }
               }
               else
               {
                  a=A;
                  b=B;
                  for(k=0;k<n;k++)
                  {
                     if(nounit)
                     {
                        t=one/a[k];
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                     bt=B+(k+1)*ldB;
                     for(j=k+1;j<n;j++)
                     {
                        t=a[j];
                        for(i=0;i<m;i++)
                           bt[i]-=t*b[i];
                        bt+=ldB;
                     }
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     a+=ldA;
                     b+=ldB;
                  }
               }
            }
         }
      }
      return 0;
   }

   /// @brief Solves a complex triangular matrix equation.
   ///
   /// For a complex upper or lower triangular matrix A, complex rectangular matrix B and complex scalar alpha,
   ///
   ///        A*X=alpha*B or A'*X=alpha*B or A.'*X=alpha*B or X*A=alpha*B or X*A'=alpha*B or X*A.'=alpha*B
   /// is solved for the matrix X.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param side Specifies whether the matrix A appears on the left or right side as follows:
   ///
   ///        if side = 'L' or 'l' then A*X=alpha*B  or  A'*X=alpha*B is solved
   ///        if side = 'R' or 'r' then X*A=alpha*B  or  X*A'=alpha*B is solved
   /// @param uplo Specifies whether A is stored as upper or lower triangular.
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular
   ///        if uplo = 'L' or 'l' then A is lower triangular
   /// @param trans Specifies whether the transpose of A is to be used or not:
   ///
   ///        if trans = 'N' or 'n' then A*X=alpha*B or X*A=alpha*B is solved
   ///        if trans = 'T' or 't' then A.'*X=alpha*B or X*A.'=alpha*B is solved
   ///        if trans = 'C' or 'c' then A'*X=alpha*B or X*A'=alpha*B is solved
   /// @param diag specifies whether or not A is unit triangular as follows:
   ///
   ///        if diag = 'U' or 'u' then A is assumed to be unit triangular
   ///        if diag = 'N' or 'n' then A is not assumed to be unit triangular.
   /// If A is unit triangular, the diagonal elements are assumed to be unity and are not referenced.
   /// @param m Specifies the number of rows of the matrix B.  m>=0
   /// @param n Specifies the number of columns of the matrix B.  n>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex triangular matrix A.  The order of A is n if side = 'L' or 'l';
   /// the order of A is m is side = 'R' or 'r'.
   /// If uplo = 'U' or 'u, A is upper triangular and the lower triangular part is not referenced.
   /// If uplo = 'L' or 'l A is lower triangular and the upper triangular part is not referenced.
   /// @param ldA Column length of the matrix A.  If side='L' or 'l' then ldA>=n; if side='R' or 'r' then ldA>=m.
   /// @param B Pointer to complex m-by-n matrix B.  On entry, B contains the right hand side matrix B.
   /// On exit, B is overwritten with the solution matrix X.
   /// @param ldB Column length of the matrix B.  ldB>=m.
   /// @ingroup BLAS

   template <typename real_t>
   int TRSM(char side, char uplo, char trans, char diag, int_t m, int_t n, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *B, int_t ldB)
   {

      using std::conj;
      using std::toupper;
      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,k;
      complex<real_t> *a,*b,*bt;
      complex<real_t> t;
      bool nounit;

      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);

      nounit=(diag=='N')?1:0;

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      if(alpha==zero)
      {
         b=B;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               b[i]=zero;
            b+=ldB;
         }
      }
      else
      {
         if(side=='L')
         {
            if(trans=='N')
            {
               if(uplo=='U')
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     a=A+m*ldA;
                     for(k=m-1;k>=0;k--)
                     {
                        a-=ldA;
                        if(nounit)
                           b[k]=b[k]/a[k];
                        for(i=0;i<k;i++)
                           b[i]-=b[k]*a[i];
                     }
                     b+=ldB;
                  }
               }
               else
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     a=A;
                     for(k=0;k<m;k++)
                     {
                        if(nounit)
                           b[k]=b[k]/a[k];
                        for(i=k+1;i<m;i++)
                           b[i]-=b[k]*a[i];
                        a+=ldA;
                     }
                     b+=ldB;
                  }
               }
            }
            else if(trans=='T')
            {
               if(uplo=='U')
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     a=A;
                     for(i=0;i<m;i++)
                     {
                        t=alpha*b[i];
                        for(k=0;k<i;k++)
                           t-=a[k]*b[k];
                        if(nounit)
                           t=t/a[i];
                        b[i]=t;
                        a+=ldA;
                     }
                     b+=ldB;
                  }
               }
               else
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     a=A+m*ldA;
                     for(i=m-1;i>=0;i--)
                     {
                        a-=ldA;
                        t=alpha*b[i];
                        for(k=i+1;k<m;k++)
                           t-=a[k]*b[k];
                        if(nounit)
                           t=t/a[i];
                        b[i]=t;
                     }
                     b+=ldB;
                  }
               }
            }
            else if(trans=='C')
            {
               if(uplo=='U')
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     a=A;
                     for(i=0;i<m;i++)
                     {
                        t=alpha*b[i];
                        for(k=0;k<i;k++)
                           t-=conj(a[k])*b[k];
                        if(nounit)
                           t=t/conj(a[i]);
                        b[i]=t;
                        a+=ldA;
                     }
                     b+=ldB;
                  }
               }
               else
               {
                  b=B;
                  for(j=0;j<n;j++)
                  {
                     a=A+m*ldA;
                     for(i=m-1;i>=0;i--)
                     {
                        a-=ldA;
                        t=alpha*b[i];
                        for(k=i+1;k<m;k++)
                           t-=conj(a[k])*b[k];
                        if(nounit)
                           t=t/conj(a[i]);
                        b[i]=t;
                     }
                     b+=ldB;
                  }
               }
            }
         }
         else
         {
            if(trans=='N')
            {
               if(uplo=='U')
               {
                  b=B;
                  a=A;
                  for(j=0;j<n;j++)
                  {
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     bt=B;
                     for(k=0;k<j;k++)
                     {
                        for(i=0;i<m;i++)
                           b[i]-=a[k]*bt[i];
                        bt+=ldB;
                     }
                     if(nounit)
                     {
                        t=one/a[j];
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                     a+=ldA;
                     b+=ldB;
                  }
               }
               else
               {
                  b=B+n*ldB;
                  a=A+n*ldA;
                  for(j=n-1;j>=0;j--)
                  {
                     a-=ldA;
                     b-=ldB;
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     bt=B+(j+1)*ldB;
                     for(k=j+1;k<n;k++)
                     {
                        for(i=0;i<m;i++)
                           b[i]-=a[k]*bt[i];
                        bt+=ldB;
                     }
                     if(nounit)
                     {
                        t=one/a[j];
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                  }
               }
            }
            else if(trans=='T')
            {
               if(uplo=='U')
               {
                  b=B+n*ldB;
                  a=A+n*ldA;
                  for(k=n-1;k>=0;k--)
                  {
                     a-=ldA;
                     b-=ldB;
                     if(nounit)
                     {
                        t=one/a[k];
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                     bt=B;
                     for(j=0;j<k;j++)
                     {
                        t=a[j];
                        for(i=0;i<m;i++)
                           bt[i]-=t*b[i];
                        bt+=ldB;
                     }
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                  }
               }
               else
               {
                  a=A;
                  b=B;
                  for(k=0;k<n;k++)
                  {
                     if(nounit)
                     {
                        t=one/a[k];
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                     bt=B+(k+1)*ldB;
                     for(j=k+1;j<n;j++)
                     {
                        t=a[j];
                        for(i=0;i<m;i++)
                           bt[i]-=t*b[i];
                        bt+=ldB;
                     }
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     a+=ldA;
                     b+=ldB;
                  }
               }
            }
            else if(trans=='C')
            {
               if(uplo=='U')
               {
                  b=B+n*ldB;
                  a=A+n*ldA;
                  for(k=n-1;k>=0;k--)
                  {
                     a-=ldA;
                     b-=ldB;
                     if(nounit)
                     {
                        t=one/conj(a[k]);
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                     bt=B;
                     for(j=0;j<k;j++)
                     {
                        t=conj(a[j]);
                        for(i=0;i<m;i++)
                           bt[i]-=t*b[i];
                        bt+=ldB;
                     }
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                  }
               }
               else
               {
                  a=A;
                  b=B;
                  for(k=0;k<n;k++)
                  {
                     if(nounit)
                     {
                        t=one/conj(a[k]);
                        for(i=0;i<m;i++)
                           b[i]*=t;
                     }
                     bt=B+(k+1)*ldB;
                     for(j=k+1;j<n;j++)
                     {
                        t=conj(a[j]);
                        for(i=0;i<m;i++)
                           bt[i]-=t*b[i];
                        bt+=ldB;
                     }
                     for(i=0;i<m;i++)
                        b[i]*=alpha;
                     a+=ldA;
                     b+=ldB;
                  }
               }
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int TRSM<float>(char side, char uplo, char trans, char diag, int_t m, int_t n, float alpha, float *A, int_t ldA, float *B, int_t ldB)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);
      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_DIAG Diag=(diag=='N')?CblasNonUnit:CblasUnit;

      cblas_strsm(CblasColMajor,Side,Uplo,Trans,Diag,m,n,alpha,A,ldA,B,ldB);

      return 0;
   }

   template <> int TRSM<double>(char side, char uplo, char trans, char diag, int_t m, int_t n, double alpha, double *A, int_t ldA, double *B, int_t ldB)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);
      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_DIAG Diag=(diag=='N')?CblasNonUnit:CblasUnit;

      cblas_dtrsm(CblasColMajor,Side,Uplo,Trans,Diag,m,n,alpha,A,ldA,B,ldB);

      return 0;
   }
   template <> int TRSM<float>(char side, char uplo, char trans, char diag, int_t m, int_t n, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> *B, int_t ldB)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);
      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_DIAG Diag=(diag=='N')?CblasNonUnit:CblasUnit;

      cblas_ctrsm(CblasColMajor,Side,Uplo,Trans,Diag,m,n,&alpha,A,ldA,B,ldB);

      return 0;
   }

   template <> int TRSM<double>(char side, char uplo, char trans, char diag, int_t m, int_t n, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> *B, int_t ldB)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      trans=toupper(trans);
      diag=toupper(diag);
      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if((trans!='N')&&(trans!='T')&&(trans!='C'))
         return -3;
      else if((diag!='U')&&(diag!='N'))
         return -4;
      else if(m<0)
         return -5;
      else if(n<0)
         return -6;
      else if(ldA<((side=='L')?m:n))
         return -9;
      else if(ldB<m)
         return -11;
      else if((m==0)||(n==0))
         return 0;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;
      const CBLAS_TRANSPOSE Trans=(trans=='N')?CblasNoTrans:((trans=='T')?CblasTrans:CblasConjTrans);
      const CBLAS_DIAG Diag=(diag=='N')?CblasNonUnit:CblasUnit;

      cblas_ztrsm(CblasColMajor,Side,Uplo,Trans,Diag,m,n,&alpha,A,ldA,B,ldB);

      return 0;
   }
#endif
}
#endif


//STARTWHOLE
static char help[] = "Solve a 4x4 linear system using KSP.\n";

#include <petsc.h>

int main(int argc,char **args) {
    PetscErrorCode ierr;
    Vec        x, b;
    Mat        A;
    KSP        ksp;
    PetscInt   i, j[4] = {0, 1, 2, 3};                // j = column index
    PetscReal  ab[4] = {7.0, 1.0, 1.0, 3.0},          // vector entries
               aA[4][4] = {{ 1.0,  2.0,  3.0,  0.0},  // matrix entries
                           { 2.0,  1.0, -2.0, -3.0},
                           {-1.0,  1.0,  1.0,  0.0},
                           { 0.0,  1.0,  1.0, -1.0}};

    ierr = PetscInitialize(&argc,&args,NULL,help); if (ierr) return ierr;

    ierr = VecCreate(PETSC_COMM_WORLD,&b); CHKERRQ(ierr);
    ierr = VecSetSizes(b,PETSC_DECIDE,4); CHKERRQ(ierr);
    ierr = VecSetFromOptions(b); CHKERRQ(ierr);
    ierr = VecSetValues(b,4,j,ab,INSERT_VALUES); CHKERRQ(ierr);
    ierr = VecAssemblyBegin(b); CHKERRQ(ierr);
    ierr = VecAssemblyEnd(b); CHKERRQ(ierr);

    ierr = MatCreate(PETSC_COMM_WORLD,&A); CHKERRQ(ierr);
    ierr = MatSetSizes(A,PETSC_DECIDE,PETSC_DECIDE,4,4); CHKERRQ(ierr);
    ierr = MatSetFromOptions(A); CHKERRQ(ierr);
    ierr = MatSetUp(A); CHKERRQ(ierr);
    for (i=0; i<4; i++) {   // set entries one row at a time
        ierr = MatSetValues(A,1,&i,4,j,aA[i],INSERT_VALUES); CHKERRQ(ierr);
    }
    ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);

    ierr = KSPCreate(PETSC_COMM_WORLD,&ksp); CHKERRQ(ierr);
    ierr = KSPSetOperators(ksp,A,A); CHKERRQ(ierr);
    ierr = KSPSetFromOptions(ksp); CHKERRQ(ierr);
    ierr = VecDuplicate(b,&x); CHKERRQ(ierr);
    ierr = KSPSolve(ksp,b,x); CHKERRQ(ierr);
    ierr = VecView(x,PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);

    KSPDestroy(&ksp);  MatDestroy(&A);
    VecDestroy(&x);  VecDestroy(&b);
    return PetscFinalize();
}
//ENDWHOLE


/*
* C functions for the NIG marginal distribution.
*
* This file is part of Fieldosophy, a toolkit for random fields.
*
* Copyright (C) 2021 Anders Gunnar Felix Hildeman <fieldosophySPDEC@gmail.com>
*
* This Source Code is subject to the terms of the BSD 3-Clause License.
* If a copy of the license was not distributed with this file, you can obtain one at https://opensource.org/licenses/BSD-3-Clause.
*
*
* Author: Anders Gunnar Felix Hildeman
* Date: 2020-04
*/

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <omp.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>

extern "C"
{

    // Make sure that alpha tilde is not too large
    void NIG_filterParams( double *pAlphaTilde, double *pBetaTilde)
    {
        if (*pAlphaTilde > 100.0d)
        {
            *pBetaTilde = *pBetaTilde * ( 100.0d / (*pAlphaTilde) );
            *pAlphaTilde = 100.0d;
        }

        return;
    }

    // Modified Bessel function of the second kind
    double Kv(const int v, const double x)
    {
        double lOut = 0.0d;
        int status;

        gsl_sf_result result;

        switch(v)
        {
        case 0:
            status = gsl_sf_bessel_K0_e(x, &result);
            break;
        case 1:
            status = gsl_sf_bessel_K1_e(x, &result);
            break;
        default:
            status = gsl_sf_bessel_Kn_e(v, x, &result);
            break;
        }

        // If some kind of error
        if (status != 0)
        {
            //printf("Error code: %d in Kv", status);
            lOut = GSL_NAN;
        }
        else
        {
            lOut = result.val;
        }

        return lOut;
    }

    // Logarithm of modified Bessel function of second kind
    double lKv(const int v, const double x)
    {
        double lOut = Kv( v, x );
        // Logarithmize
        lOut = log( lOut );

        return lOut;
    }

    // logarithm of PDF for NIG distribution
    unsigned int NIG_lpdf( double * const pX, const unsigned int pN, const double alphaTilde, const double betaTilde, const double mu, const double delta )
    {
        // turn off gsl error handler
        gsl_set_error_handler_off();

        // Loop through each element in vector
        #pragma omp parallel for
        for ( unsigned int iter = 0; iter < pN; iter++)
        {
            // Get normalized data value
            const double lCur = (pX[iter] - mu) / delta;
            // Compute q function
            const double lQ = sqrt( (1.0d + lCur * lCur) );
            // Compute bessel function
            const double lLogK = lKv(1, alphaTilde * lQ);

            // compute log pdf
            double lCurOut = lLogK;
            lCurOut -= log(lQ);
            lCurOut += betaTilde * lCur;
            // Add constants
            lCurOut += log( alphaTilde / M_PI ) + sqrt( alphaTilde * alphaTilde - betaTilde * betaTilde );
            lCurOut -= log(delta);

            // Store full log pdf
            pX[iter] = lCurOut;
        }

        return pN;
    }

    // Compute log-likelihood
    double NIG_logLikelihood( double * lOut, const double * const pX, const unsigned int pN, const double alphaTilde, const double betaTilde, const double mu, const double delta )
    {
        // Copy content
        memcpy( lOut, pX, sizeof(double) * pN );

        // Compute log-pdf
        NIG_lpdf( lOut, pN, alphaTilde, betaTilde, mu, delta );
        // Sum
        double lSum = 0.0d;
        for (unsigned int iter = 0; iter < pN; iter++)
        {
            lSum += lOut[iter];
        }

        return lSum;
    }

    // EM-algorithm for finding MLE of NIG
    unsigned int NIG_MLE( double * const pLogLik, double * const pX, const unsigned int pN,
        double * const pAlphaTilde, double * const pBetaTilde, double * const pMu, double * const pDelta,
        const double pXBar, const unsigned int pM, const double pTol )
    {

        // turn off gsl error handler
        gsl_set_error_handler_off();

        // Init
        double lAlphaTilde = *pAlphaTilde;
        double lBetaTilde = *pBetaTilde;
        double lMu = *pMu;
        double lDelta = *pDelta;
        // Filter tilde parameters
        NIG_filterParams( &lAlphaTilde, &lBetaTilde);

        // Preallocate arrays
        double * lTempArray1 = (double*) malloc(sizeof(double) * pN);
        double * lTempArray2 = (double*) malloc(sizeof(double) * pN);
        if (lTempArray1 == NULL || lTempArray2 == NULL)
            return 1;

        // Compute log-likelihood
        *pLogLik = NIG_logLikelihood( lTempArray1, pX, pN, lAlphaTilde, lBetaTilde, lMu, lDelta );

        // If likelihood is bad
        if (isnan(*pLogLik))
            return 1;

        // Iterate EM-algorithm
        for (unsigned int iter = 0; iter < pM; iter++)
        {
            // --- E-step ---

            double lSBar = 0.0d;
            double lWBar = 0.0d;
            #pragma omp parallel for reduction(+:lSBar) reduction(+:lWBar)
            for (unsigned int iter2 = 0; iter2 < pN; iter2++)
            {
                const double lPhiSqrt = sqrt( 1.0d + ( (pX[iter2]-lMu)/lDelta ) * ( (pX[iter2]-lMu)/lDelta )  );
                // compute besselk function values
                const double lK0 = Kv( 0, lAlphaTilde * lPhiSqrt );
                const double lK1 = Kv( 1, lAlphaTilde * lPhiSqrt );
                const double lK2 = Kv( 2, lAlphaTilde * lPhiSqrt );
                // Compute pseudo values
                lTempArray1[iter2] = ( lDelta * lPhiSqrt * lK0 ) / ( (lAlphaTilde * lK1) / lDelta ); // S
                lTempArray2[iter2] = ( (lAlphaTilde * lK2) / lDelta ) / ( lDelta * lPhiSqrt * lK1 ); // W
                lSBar += lTempArray1[iter2];
                lWBar += lTempArray2[iter2];
            }
            lSBar /= ((double)pN);
            lWBar /= ((double)pN);

            // --- M-step ---

            // Compute delta
            lDelta = sqrt( 1.0d / ( lWBar - (1.0d / lSBar) ) );
            double lGamma = lDelta / lSBar;
            // Compute beta
            double lBeta = 0.0d;
            for (unsigned int iter2 = 0; iter2 < pN; iter2++)
            {
                lBeta += pX[iter2] * lTempArray2[iter2];
            }
            lBeta = lBeta - pXBar * lWBar * ((double)pN);
            lBeta /= ( (double)pN - lSBar * lWBar * ((double)pN) );
            lBetaTilde = lBeta * lDelta;
            // Compute alpha
            double lAlpha = sqrt( lGamma * lGamma + lBeta * lBeta );
            lAlphaTilde = lAlpha * lDelta;
            // Filter tilde values
            NIG_filterParams( &lAlphaTilde, &lBetaTilde);
            lGamma = sqrt( lAlphaTilde * lAlphaTilde - lBetaTilde * lBetaTilde ) / lDelta;
            // Compute mu
            lMu = pXBar - lBetaTilde / lGamma;

            // Compute loglik
            const double llLikNew = NIG_logLikelihood( lTempArray1, pX, pN, lAlphaTilde, lBetaTilde, lMu, lDelta );

            // If likelihood is bad
            if (isnan(llLikNew))
                return(iter+2);    // Mark that likelihood went bad during iterations

            // Compute likelihood difference
            const double lLLikDiff = llLikNew - *pLogLik;

            // If new likelihood is higher
            if (lLLikDiff > 0)
            {
                // Set new log-likelihood as old one
                *pLogLik = llLikNew;
                // Update output parameters
                *pAlphaTilde = lAlphaTilde;
                *pBetaTilde = lBetaTilde;
                *pMu = lMu;
                *pDelta = lDelta;

                // If should break due to tolerance
                if ( lLLikDiff < pTol )
                    break;
            }

        }   // end of EM-iterations

        // Free allocated arrays
        free(lTempArray1);
        free(lTempArray2);

         // Return no problem
         return( 0 );
    }

}


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_randist.h>

#include "allvars.h"
#include "proto.h"

int mcmc_sampling(char *fname_mcmc, double (* prob_fun)(double *theta))
{
  const int n_cov_update=10000;
  double prob, prob_new, scale, ratio, u;
  double theta[ntheta], theta_new[ntheta];
  double var_cov_mat[ntheta*ntheta], Pmatrix[ntheta*ntheta], Prandvec[ntheta], Py[ntheta];
  double *theta_mcmc;
  int istep, iaccpt, i, j, info, flag;

  FILE *fmcmc;
  fmcmc = fopen(fname_mcmc, "w");
  theta_mcmc = array_malloc(ntheta*n_cov_update);

/*
 * sampling in logrithm space.
 */
  memcpy(theta, theta_input, ntheta*sizeof(double));

  for(i=0; i<ntheta; i++)
  {
    var_cov_mat[i*ntheta+i] = sigma_input[i] * sigma_input[i];
    for(j=0; j<i; j++)
      var_cov_mat[i*ntheta+j] = var_cov_mat[j*ntheta + i] = 0.0;
  }
  memcpy(Pmatrix, var_cov_mat, ntheta*ntheta*sizeof(double));
  Chol_decomp_U(Pmatrix, ntheta, &info);

  scale = 1.0;

  prob = prob_fun(theta);
  istep = 0;
  iaccpt = 0;

  printf("step:%6d", 0);
  while(istep < nmcmc)
  {
    flag = 1;
    while(flag)
    {
      for(i=0; i<ntheta; i++)
      {
        Prandvec[i]  = gsl_ran_gaussian(gsl_r, 1.0);
      }
      multiply_matvec(Pmatrix, Prandvec, ntheta, Py);
      flag = 0;
      for(i=0; i<ntheta; i++)
      {
        if(theta_fixed[i]==1)
        {
          theta_new[i] = theta[i];
        }
        else
        {
          theta_new[i] = theta[i] + scale * Py[i];
          if(theta_new[i]<theta_range[i][0] || theta_new[i]>theta_range[i][1])
          {
            flag = 1;
            break;
          }
        }
      }
    }

    prob_new = prob_fun(theta_new);

    ratio = prob_new - prob;
    if(ratio > 0.0)
    {
      memcpy(theta, theta_new, ntheta*sizeof(double));
      prob = prob_new;
      iaccpt++;
    }
    else
    {
      u = gsl_rng_uniform(gsl_r);
      if(log(u) < ratio)
      {
        memcpy(theta, theta_new, ntheta*sizeof(double));
        prob = prob_new;
        iaccpt++;
      }
    }

    for(i=0; i<ntheta; i++)
      theta_mcmc[i*n_cov_update + istep%n_cov_update] = theta[i];
    if(istep%10==0)printf("\b\b\b\b\b\b%6d", istep);

    fprintf(fmcmc, "%d", istep);
    for(i=0;i<ntheta;i++)
    {
      fprintf(fmcmc,"\t%f", theta[i]);
    }
    fprintf(fmcmc, "\t%e\n", prob);

    istep++;

    if(istep%n_cov_update == 0)
    {
      get_cov_matrix_diag(theta_mcmc, n_cov_update, ntheta);
      memcpy(var_cov_mat, cov_matrix, ntheta*ntheta*sizeof(double));
      //display_mat(var_cov_mat, ntheta, ntheta);
      memcpy(Pmatrix, var_cov_mat, ntheta*ntheta*sizeof(double));
      Chol_decomp_U(Pmatrix, ntheta, &info);
    }
  }

  printf("\n");
  printf("Accept rate: %f\n", 1.0*iaccpt/istep);

  fclose(fmcmc);
  free(theta_mcmc);
  return 0;
}


#ifndef TRIANGLES_H_XWPDRVKT
#define TRIANGLES_H_XWPDRVKT

#include <cmath>
#include <gsl/gsl>
#include <iostream>
#include <limits>
#include <sens_loc/math/constants.h>
#include <type_traits>

namespace sens_loc::math {

/// This function calculates the bearing angle between two neighbouring range
/// measurements.
/// \param b reference depth and > 0
/// \param c prior depth and > 0
/// \param cos_alpha == std::cos(alpha) of the angle between two measurements
/// \returns bearing angle in radians
/// \pre b and c and positive values
/// \pre \f$0 < \cos \alpha < 1\f$
/// \post \f$0 < result < \pi\f$
///
/// The bearing angle is angle between the ray to 'b' and the connecting line
/// from 'c' to 'b'.
/// It is bigger 0° and smaller 180° due to triangle constraints.
template <typename Real>
inline Real
bearing_angle(const Real b, const Real c, const Real cos_alpha) noexcept {
    static_assert(std::is_arithmetic_v<Real>);

    Expects(b > 0.);
    Expects(c > 0.);
    // => alpha is smaller 90°
    Expects(cos_alpha > 0.);
    // => alpha is bigger 0°
    Expects(cos_alpha < 1.);

    // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
    const Real delta = Real(10.) * std::numeric_limits<Real>::epsilon();

    const Real nom = b - c * cos_alpha;
    const Real den = [&]() {
        const Real d = std::sqrt(b * b + c * c - Real(2.) * b * c * cos_alpha);
        return d == Real(0.) ? d + delta : d;
    }();
    Ensures(den != 0.);

    // Note: Because of inaccuracy of floating point operations it is possible
    // to get abs(ratio) == 1. This is not an error in the implementation
    // but rather an numerical artifact. To not hit the post-conditions
    // it is ok to subtract a tiny epsilon.
    const Real ratio =
        std::clamp(nom / den, Real(-1.) + delta, Real(1.) - delta);

    Ensures(ratio > -1.);
    Ensures(ratio < +1.);

    const Real result = std::acos(ratio);

    Ensures(result > 0.);
    Ensures(result < math::pi<Real>);

    return result;
}

/// This function implements the bearing angle with formula from Lin, which is
/// incorrect.
/// This implementation is here to verify the difference in the results
/// and testing covers that as well.
template <typename Real = float>
inline Real reference_lin_bearing_angle(const Real b,
                                        const Real c,
                                        const Real cos_alpha) noexcept {
    static_assert(std::is_arithmetic_v<Real>);

    Expects(b > 0.);
    Expects(c > 0.);
    // => alpha is smaller 90°
    Expects(cos_alpha > 0.);
    // => alpha is bigger 0°
    Expects(cos_alpha < 1.);

    const Real nom   = (b - c * cos_alpha);
    const Real denom = (b * b + c * c - 2. * b * c * cos_alpha);

    const Real ratio = nom / denom;

    Ensures(ratio > -1.);
    Ensures(ratio < +1.);

    const Real result = std::acos(ratio);

    Ensures(result > 0.);
    Ensures(result < math::pi<Real>);

    return result;
}

}  // namespace sens_loc::math

#endif /* end of include guard: TRIANGLES_H_XWPDRVKT */


static char help[] =
"Coupled reaction-diffusion equations (Pearson 1993).  Option prefix -ptn_.\n"
"Demonstrates form  F(t,Y,dot Y) = G(t,Y)  where F() is IFunction and G() is\n"
"RHSFunction().  Implements IJacobian() and RHSJacobian().  Defaults to\n"
"ARKIMEX (= adaptive Runge-Kutta implicit-explicit) TS type.\n\n";

#include <petsc.h>

typedef struct {
  PetscReal u, v;
} Field;

typedef struct {
  PetscReal  L,     // domain side length
             Du,    // diffusion coefficient: u equation
             Dv,    //                        v equation
             phi,   // "dimensionless feed rate" (F in Pearson 1993)
             kappa; // "dimensionless rate constant" (k in Pearson 1993)
  PetscBool  IFcn_called, IJac_called, RHSFcn_called, RHSJac_called;
} PatternCtx;

extern PetscErrorCode InitialState(DM, Vec, PetscReal, PatternCtx*);
extern PetscErrorCode FormRHSFunctionLocal(DMDALocalInfo*, PetscReal, Field**,
                                           Field**, PatternCtx*);
extern PetscErrorCode FormRHSJacobianLocal(DMDALocalInfo*, PetscReal, Field**,
                                           Mat, Mat, PatternCtx*);
extern PetscErrorCode FormIFunctionLocal(DMDALocalInfo*, PetscReal, Field**, Field**,
                                         Field **, PatternCtx*);
extern PetscErrorCode FormIJacobianLocal(DMDALocalInfo*, PetscReal, Field**, Field**,
                                         PetscReal, Mat, Mat, PatternCtx*);

int main(int argc,char **argv)
{
  PetscErrorCode ierr;
  PatternCtx     user;
  TS             ts;
  Vec            x;
  DM             da;
  DMDALocalInfo  info;
  PetscReal      noiselevel = -1.0;  // negative value means no initial noise
  PetscBool      no_rhsjacobian = PETSC_FALSE,
                 no_ijacobian = PETSC_FALSE,
                 call_back_report = PETSC_FALSE;
  TSType         type;

  ierr = PetscInitialize(&argc,&argv,NULL,help); if (ierr) return ierr;

  // parameter values from pages 21-22 in Hundsdorfer & Verwer (2003)
  user.L      = 2.5;
  user.Du     = 8.0e-5;
  user.Dv     = 4.0e-5;
  user.phi    = 0.024;
  user.kappa  = 0.06;
  user.IFcn_called   = PETSC_FALSE;
  user.IJac_called   = PETSC_FALSE;
  user.RHSFcn_called = PETSC_FALSE;
  user.RHSJac_called = PETSC_FALSE;
  ierr = PetscOptionsBegin(PETSC_COMM_WORLD, "ptn_", "options for patterns", ""); CHKERRQ(ierr);
  ierr = PetscOptionsBool("-call_back_report","report on which user-supplied call-backs were actually called",
           "pattern.c",call_back_report,&(call_back_report),NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal("-Du","diffusion coefficient of first equation",
           "pattern.c",user.Du,&user.Du,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal("-Dv","diffusion coefficient of second equation",
           "pattern.c",user.Dv,&user.Dv,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal("-kappa","dimensionless rate constant (=k in (Pearson, 1993))",
           "pattern.c",user.kappa,&user.kappa,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal("-L","square domain side length; recommend L >= 0.5",
           "pattern.c",user.L,&user.L,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsBool("-no_ijacobian","do not set call-back DMDATSSetIJacobian()",
           "pattern.c",no_ijacobian,&(no_ijacobian),NULL);CHKERRQ(ierr);
  ierr = PetscOptionsBool("-no_rhsjacobian","do not set call-back DMDATSSetRHSJacobian()",
           "pattern.c",no_rhsjacobian,&(no_rhsjacobian),NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal("-noisy_init",
           "initialize u,v with this much random noise (e.g. 0.2) on top of usual initial values",
           "pattern.c",noiselevel,&noiselevel,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsReal("-phi","dimensionless feed rate (=F in (Pearson, 1993))",
           "pattern.c",user.phi,&user.phi,NULL);CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);

  ierr = DMDACreate2d(PETSC_COMM_WORLD,
               DM_BOUNDARY_PERIODIC, DM_BOUNDARY_PERIODIC,
               DMDA_STENCIL_BOX,  // for 9-point stencil
               3,3,PETSC_DECIDE,PETSC_DECIDE,
               2, 1,              // degrees of freedom, stencil width
               NULL,NULL,&da); CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMDASetFieldName(da,0,"u"); CHKERRQ(ierr);
  ierr = DMDASetFieldName(da,1,"v"); CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  if (info.mx != info.my) {
      SETERRQ(PETSC_COMM_SELF,1,"pattern.c requires mx == my");
  }
  ierr = DMDASetUniformCoordinates(da, 0.0, user.L, 0.0, user.L, -1.0, -1.0); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
           "running on %d x %d grid with square cells of side h = %.6f ...\n",
           info.mx,info.my,user.L/(PetscReal)(info.mx)); CHKERRQ(ierr);

//STARTTSSETUP
  ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
  ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);
  ierr = TSSetDM(ts,da); CHKERRQ(ierr);
  ierr = TSSetApplicationContext(ts,&user); CHKERRQ(ierr);
  ierr = DMDATSSetRHSFunctionLocal(da,INSERT_VALUES,
           (DMDATSRHSFunctionLocal)FormRHSFunctionLocal,&user); CHKERRQ(ierr);
  if (!no_rhsjacobian) {
      ierr = DMDATSSetRHSJacobianLocal(da,
               (DMDATSRHSJacobianLocal)FormRHSJacobianLocal,&user); CHKERRQ(ierr);
  }
  ierr = DMDATSSetIFunctionLocal(da,INSERT_VALUES,
           (DMDATSIFunctionLocal)FormIFunctionLocal,&user); CHKERRQ(ierr);
  if (!no_ijacobian) {
      ierr = DMDATSSetIJacobianLocal(da,
               (DMDATSIJacobianLocal)FormIJacobianLocal,&user); CHKERRQ(ierr);
  }
  ierr = TSSetType(ts,TSARKIMEX); CHKERRQ(ierr);
  ierr = TSSetTime(ts,0.0); CHKERRQ(ierr);
  ierr = TSSetMaxTime(ts,200.0); CHKERRQ(ierr);
  ierr = TSSetTimeStep(ts,5.0); CHKERRQ(ierr);
  ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);
  ierr = TSSetFromOptions(ts);CHKERRQ(ierr);
//ENDTSSETUP

  ierr = DMCreateGlobalVector(da,&x); CHKERRQ(ierr);
  ierr = InitialState(da,x,noiselevel,&user); CHKERRQ(ierr);
  ierr = TSSolve(ts,x); CHKERRQ(ierr);

  // optionally report on call-backs
  if (call_back_report) {
      ierr = TSGetType(ts,&type);CHKERRQ(ierr);
      ierr = PetscPrintf(PETSC_COMM_WORLD,"CALL-BACK REPORT\n  solver type: %s\n",type); CHKERRQ(ierr);
      ierr = PetscPrintf(PETSC_COMM_WORLD,"  IFunction:   %D  | IJacobian:   %D\n",
                                          (int)user.IFcn_called,(int)user.IJac_called); CHKERRQ(ierr);
      ierr = PetscPrintf(PETSC_COMM_WORLD,"  RHSFunction: %D  | RHSJacobian: %D\n",
                                          (int)user.RHSFcn_called,(int)user.RHSJac_called); CHKERRQ(ierr);
  }

  VecDestroy(&x);  TSDestroy(&ts);  DMDestroy(&da);
  return PetscFinalize();
}

// Formulas from page 22 of Hundsdorfer & Verwer (2003).  Interpretation here is
// to always generate 0.5 x 0.5 non-trivial patch in (0,L) x (0,L) domain.
PetscErrorCode InitialState(DM da, Vec Y, PetscReal noiselevel, PatternCtx* user) {
  PetscErrorCode ierr;
  DMDALocalInfo    info;
  PetscInt         i,j;
  PetscReal        sx,sy;
  const PetscReal  ledge = (user->L - 0.5) / 2.0, // nontrivial initial values on
                   redge = user->L - ledge;       //   ledge < x,y < redge
  DMDACoor2d       **aC;
  Field            **aY;

  ierr = VecSet(Y,0.0); CHKERRQ(ierr);
  if (noiselevel > 0.0) {
      // noise added to usual initial condition is uniform on [0,noiselevel],
      //     independently for each location and component
      ierr = VecSetRandom(Y,NULL); CHKERRQ(ierr);
      ierr = VecScale(Y,noiselevel); CHKERRQ(ierr);
  }
  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  ierr = DMDAGetCoordinateArray(da,&aC); CHKERRQ(ierr);
  ierr = DMDAVecGetArray(da,Y,&aY); CHKERRQ(ierr);
  for (j = info.ys; j < info.ys+info.ym; j++) {
    for (i = info.xs; i < info.xs+info.xm; i++) {
      if ((aC[j][i].x >= ledge) && (aC[j][i].x <= redge)
              && (aC[j][i].y >= ledge) && (aC[j][i].y <= redge)) {
          sx = PetscSinReal(4.0 * PETSC_PI * aC[j][i].x);
          sy = PetscSinReal(4.0 * PETSC_PI * aC[j][i].y);
          aY[j][i].v += 0.5 * sx * sx * sy * sy;
      }
      aY[j][i].u += 1.0 - 2.0 * aY[j][i].v;
    }
  }
  ierr = DMDAVecRestoreArray(da,Y,&aY); CHKERRQ(ierr);
  ierr = DMDARestoreCoordinateArray(da,&aC); CHKERRQ(ierr);
  return 0;
}

// in system form  F(t,Y,dot Y) = G(t,Y),  compute G():
//     G^u(t,u,v) = - u v^2 + phi (1 - u)
//     G^v(t,u,v) = + u v^2 - (phi + kappa) v
//STARTRHSFUNCTION
PetscErrorCode FormRHSFunctionLocal(DMDALocalInfo *info,
                   PetscReal t, Field **aY, Field **aG, PatternCtx *user) {
  PetscInt   i, j;
  PetscReal  uv2;

  user->RHSFcn_called = PETSC_TRUE;
  for (j = info->ys; j < info->ys + info->ym; j++) {
      for (i = info->xs; i < info->xs + info->xm; i++) {
          uv2 = aY[j][i].u * aY[j][i].v * aY[j][i].v;
          aG[j][i].u = - uv2 + user->phi * (1.0 - aY[j][i].u);
          aG[j][i].v = + uv2 - (user->phi + user->kappa) * aY[j][i].v;
      }
  }
  return 0;
}
//ENDRHSFUNCTION

PetscErrorCode FormRHSJacobianLocal(DMDALocalInfo *info,
                                    PetscReal t, Field **aY,
                                    Mat J, Mat P, PatternCtx *user) {
    PetscErrorCode ierr;
    PetscInt    i, j;
    PetscReal   v[2], uv, v2;
    MatStencil  col[2],row;

    user->RHSJac_called = PETSC_TRUE;
    for (j = info->ys; j < info->ys+info->ym; j++) {
        row.j = j;  col[0].j = j;  col[1].j = j;
        for (i = info->xs; i < info->xs+info->xm; i++) {
            row.i = i;  col[0].i = i;  col[1].i = i;
            uv = aY[j][i].u * aY[j][i].v;
            v2 = aY[j][i].v * aY[j][i].v;
            // u equation
            row.c = 0;  col[0].c = 0;  col[1].c = 1;
            v[0] = - v2 - user->phi;
            v[1] = - 2.0 * uv;
            ierr = MatSetValuesStencil(P,1,&row,2,col,v,INSERT_VALUES); CHKERRQ(ierr);
            // v equation
            row.c = 1;  col[0].c = 0;  col[1].c = 1;
            v[0] = v2;
            v[1] = 2.0 * uv - (user->phi + user->kappa);
            ierr = MatSetValuesStencil(P,1,&row,2,col,v,INSERT_VALUES); CHKERRQ(ierr);
        }
    }

    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}

// in system form  F(t,Y,dot Y) = G(t,Y),  compute F():
//     F^u(t,u,v,u_t,v_t) = u_t - D_u Laplacian u
//     F^v(t,u,v,u_t,v_t) = v_t - D_v Laplacian v
//STARTIFUNCTION
PetscErrorCode FormIFunctionLocal(DMDALocalInfo *info, PetscReal t,
                                  Field **aY, Field **aYdot, Field **aF,
                                  PatternCtx *user) {
  PetscInt         i, j;
  const PetscReal  h = user->L / (PetscReal)(info->mx),
                   Cu = user->Du / (6.0 * h * h),
                   Cv = user->Dv / (6.0 * h * h);
  PetscReal        u, v, lapu, lapv;

  user->IFcn_called = PETSC_TRUE;
  for (j = info->ys; j < info->ys + info->ym; j++) {
      for (i = info->xs; i < info->xs + info->xm; i++) {
          u = aY[j][i].u;
          v = aY[j][i].v;
          lapu =     aY[j+1][i-1].u + 4.0*aY[j+1][i].u +   aY[j+1][i+1].u
                 + 4.0*aY[j][i-1].u -    20.0*u        + 4.0*aY[j][i+1].u
                 +   aY[j-1][i-1].u + 4.0*aY[j-1][i].u +   aY[j-1][i+1].u;
          lapv =     aY[j+1][i-1].v + 4.0*aY[j+1][i].v +   aY[j+1][i+1].v
                 + 4.0*aY[j][i-1].v -    20.0*v        + 4.0*aY[j][i+1].v
                 +   aY[j-1][i-1].v + 4.0*aY[j-1][i].v +   aY[j-1][i+1].v;
          aF[j][i].u = aYdot[j][i].u - Cu * lapu;
          aF[j][i].v = aYdot[j][i].v - Cv * lapv;
      }
  }
  return 0;
}
//ENDIFUNCTION

// in system form  F(t,Y,dot Y) = G(t,Y),  compute combined/shifted
// Jacobian of F():
//     J = (shift) dF/d(dot Y) + dF/dY
//STARTIJACOBIAN
PetscErrorCode FormIJacobianLocal(DMDALocalInfo *info,
                   PetscReal t, Field **aY, Field **aYdot,
                   PetscReal shift, Mat J, Mat P,
                   PatternCtx *user) {
    PetscErrorCode ierr;
    PetscInt         i, j, s, c;
    const PetscReal  h = user->L / (PetscReal)(info->mx),
                     Cu = user->Du / (6.0 * h * h),
                     Cv = user->Dv / (6.0 * h * h);
    PetscReal        val[9], CC;
    MatStencil       col[9], row;

    ierr = MatZeroEntries(P); CHKERRQ(ierr);  // workaround to address PETSc issue #734
    user->IJac_called = PETSC_TRUE;
    for (j = info->ys; j < info->ys + info->ym; j++) {
        row.j = j;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            row.i = i;
            for (c = 0; c < 2; c++) { // u,v equations are c=0,1
                row.c = c;
                CC = (c == 0) ? Cu : Cv;
                for (s = 0; s < 9; s++)
                    col[s].c = c;
                col[0].i = i;   col[0].j = j;
                val[0] = shift + 20.0 * CC;
                col[1].i = i-1; col[1].j = j;    val[1] = - 4.0 * CC;
                col[2].i = i+1; col[2].j = j;    val[2] = - 4.0 * CC;
                col[3].i = i;   col[3].j = j-1;  val[3] = - 4.0 * CC;
                col[4].i = i;   col[4].j = j+1;  val[4] = - 4.0 * CC;
                col[5].i = i-1; col[5].j = j-1;  val[5] = - CC;
                col[6].i = i-1; col[6].j = j+1;  val[6] = - CC;
                col[7].i = i+1; col[7].j = j-1;  val[7] = - CC;
                col[8].i = i+1; col[8].j = j+1;  val[8] = - CC;
                ierr = MatSetValuesStencil(P,1,&row,9,col,val,INSERT_VALUES); CHKERRQ(ierr);
            }
        }
    }

    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}
//ENDIJACOBIAN


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mersenne.h"
#include <complex.h>
#include <lapacke.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846264338327
#endif

/* Lattice size and dimensions */
#define NT 48
#define NX 6

#define ND 2
#define NDIRS (2*ND)

#define TUP 0
#define XUP 1
#define TDN 2
#define XDN 3

#define VOLUME (NT*NX)

//One staggered fermion
#define Nf 2

//#define ANTISYMMETRIC //Antisymmetric boundaries
//#define SYMMETRIC     //implemented in Thirring_hop
#define OPENX       //open in space, (anti)symmetric in time

#define CG_ACCURACY 1e-30
#define CG_MAX_ITER 10000

/* Simulation parameters */
double m;
double g;
double mu;

/* Neighbour index arrays, to be filled at the beginning
 */
int *tup,*xup,*tdn,*xdn;

double ***A;
int    ***eta;   //Staggered eta matrix

/* Functions for fetching neighboring coordinates */
static inline int tdir(int t, int dir){
  if( dir == TUP ) return tup[t];
  if( dir == TDN ) return tdn[t];
  return(t);
}

static inline int xdir(int x, int dir){
  if( dir == XUP ) return xup[x];
  if( dir == XDN ) return xdn[x];
  return(x);
}

/* Opposite of a direction */
static inline int opp_dir(int dir){
  return ( dir + ND ) % NDIRS;
}

double gauge_action(){
    double S = 0;
    for( int t=0;t<NT;t++) for( int x=0;x<NX;x++) for( int dir=0; dir<ND; dir++ ) {
        S+= 1.-cos(A[t][x][dir]);
    }
    return Nf/g*S;
}

void update_gauge(){
    for( int t=0;t<NT;t++) for( int x=0;x<NX;x++) for( int dir=0; dir<ND; dir++ ) {
        double s1 = -Nf/g*cos(A[t][x][dir]);
        double new = 2*M_PI*mersenne()-M_PI;
        double s2 = -Nf/g*cos(new);
        double edS = exp(-s2+s1);
        //printf("New A=%f at (%d,%d), direction %d, exp(-deltaS)=%g\n", A[t][x][dir],t,x,dir, edS);

        if( mersenne() < edS ) {
            A[t][x][dir] = new;
            //printf("Accepted\n");
        }
    }
}

void fermion_matrix( _Complex double * M ){
  static double expmu, expmmu;
  static int init=1;
  if(init){
    expmu = exp(mu);
    expmmu = exp(-mu);
    init = 0;
  }

  for( int i=0;i<VOLUME*VOLUME;i++)  M[i] = 0;

  for( int t=0;t<NT;t++) for( int x=0;x<NX;x++){
    M[(NX*t + x) + (NX*t+x)*VOLUME] = m;
    int t2 = tup[t];
    int index = (NX*t + x) + (NX*t2+x)*VOLUME;
    double sA = sin(A[t][x][0]);
    double cA = cos(A[t][x][0]);
    if( t2 > t ) M[index] = 0.5 * (cA + sA*I) * eta[t][x][0] * expmu ;
    else M[index] = -0.5 * (cA + sA*I) * eta[t][x][0] * expmu ;

    t2 = tdn[t];
    sA = sin(A[t2][x][0]);
    cA = cos(A[t2][x][0]);
    index = (NX*t + x) + (NX*t2+x)*VOLUME;
    if( t2 > t ) M[index] = 0.5 * (cA - sA*I) * eta[t2][x][0] * expmmu ;
    else M[index] = -0.5 * (cA - sA*I) * eta[t2][x][0] * expmmu ;

    int x2 = xup[x];
    index = (NX*t + x) + (NX*t+x2)*VOLUME;
    sA = sin(A[t][x][1]);
    cA = cos(A[t][x][1]);
    if( x2 > x ) M[index] = 0.5 * (cA + sA*I) * eta[t][x][1] ;
    else M[index] = -0.5 * (cA + sA*I) * eta[t][x][1] ;

    x2 = xdn[x];
    index = (NX*t + x) + (NX*t+x2)*VOLUME;
    sA = sin(A[t][x2][1]);
    cA = cos(A[t][x2][1]);
    if( x2 > x ) M[index] = 0.5 * (cA - sA*I) * eta[t][x2][1] ;
    else M[index] = -0.5 * (cA - sA*I) * eta[t][x2][1] ;
  }
}

double complex determinant(){
    double complex * M; //The matrix
    M = (_Complex double *)malloc( VOLUME*VOLUME*sizeof(_Complex double) );
    fermion_matrix(M);
    int n=VOLUME;
    int info;
    int *ipiv;
    ipiv = malloc( n*sizeof(int) );
    double complex det = 1;

    /*for( int i=0;i<VOLUME;i++) { for( int j=0;j<VOLUME;j++){
        printf(" %4.2f ", creal( M[i+j*VOLUME] ) );
      }
      printf("\n");
    }
    printf("\n");
    */

    LAPACK_zgetrf( &n, &n, M, &n, ipiv, &info );

    if( info > 0 ) printf("error %d\n", info);

    for(int i=0; i<n; i++) {
      det *= M[i*n+i];
      if( ipiv[i] != i+1 ) det*=-1;
    }

    /*
    int lwork=n*n;
    _Complex double *work;
    work = malloc( lwork*sizeof(_Complex double) );

    LAPACK_zgetri(&n, M, &n, ipiv, work, &lwork, &info);
        free(work);

    for( int i=0;i<VOLUME;i++) { for( int j=0;j<VOLUME;j++){
        printf(" %4.2f ", creal( M[i+j*VOLUME] ) );
      }
      printf("\n");
    }
    printf("\n");
    */

    free(M);
    free(ipiv);
    return( det );
}

int n_average;
void measure(){
  /* Measurements */
  static int n=0;
  static double M = 0;
  static double complex det = 0;
  static double sign = 1;

  double complex this_det = determinant();
  det += this_det;
  double this_sign = (creal(this_det) > 0) ? 1 : -1;
  sign += this_sign;

  for( int t=0;t<NT;t++) for( int x=0;x<NX;x++) for( int dir=0; dir<ND; dir++) {
    M += this_sign*A[t][x][dir];
  }

  n++;

  if(n%n_average == 0){
     printf("Magnetisation %g\n",M/(n_average*VOLUME));
     printf("Determinant Re %g Im %g\n",creal(det)/(n_average),cimag(det)/(n_average));
     printf("Sign %g\n",sign/(n_average));
     sign = 0; det = 0; M = 0;
  }

}

int main(void){

  #ifdef DEBUG
  feenableexcept(FE_INVALID | FE_OVERFLOW);
  #endif

  long seed;
  int n_loops, n_measure;

  /* Read in the input */
  printf(" Number of updates : ");
  scanf("%d",&n_loops);

  printf(" Updates / measurement : ");
  scanf("%d",&n_measure);

  printf(" Average over configurations : ");
  scanf("%d",&n_average);

  printf(" m : ");
  scanf("%lf",&m);

  printf(" g : ");
  scanf("%lf",&g);

  printf(" mu : ");
  scanf("%lf",&mu);

  printf(" Random number : ");
  scanf("%ld",&seed);
  seed_mersenne( seed );

  /* "Warm up" the rng generator */
  for ( int i=0; i<543210; i++) mersenne();

  printf(" \n++++++++++++++++++++++++++++++++++++++++++\n");
  printf(" 2D quenched Thirring model, ( %d , %d ) lattice\n", NT, NX );
  printf(" %d updates per measurements\n", n_measure );
  printf(" m %f \n", m);
  printf(" g %f \n", g);
  printf(" mu %f \n", mu);
  printf(" Random seed %ld\n", seed );

  eta = malloc( NT*sizeof(int *) );
  A = malloc( NT*sizeof(double *) );
  for (int t=0; t<NT; t++){
    A[t] = malloc( (NX+1)*sizeof(double *) );
    eta[t] = malloc( (NX+1)*sizeof(int *) );
    for (int x=0; x<NX+1; x++) {
      eta[t][x] = malloc( ND*sizeof(int) );
      A[t][x] = malloc( ND*sizeof(double) );
    }
  }
  xup = malloc( (NX+1)*sizeof(int) );
  xdn = malloc( (NX+1)*sizeof(int) );
  tup = malloc( NT*sizeof(int) );
  tdn = malloc( NT*sizeof(int) );

  /* fill up the index array */
  for ( int i=0; i<NT; i++ ) {
    tup[i] = (i+1) % NT;
    tdn[i] = (i-1+NT) % NT;
  }
  for (int i=0; i<NX+1; i++) {
    xup[i] = (i+1) % NX;
    xdn[i] = (i-1+NX) % NX;
  }

  for (int t=0; t<NT; t++) for (int x=0; x<NX; x++) {
    for( int dir=0; dir<ND; dir++ ) A[t][x][dir] = 0;
    eta[t][x][1] = 1;
    if( x%2 == 0 ){
      eta[t][x][0] = 1;
    } else {
      eta[t][x][0] = -1;
    }
#ifdef OPENX
    eta[t][NX][0] = eta[t][NX][1] = 0;
#endif
  }

  for (int i=1; i<n_loops+1; i++) {
    update_gauge();

    //printf("Updated gauge\n");
    //printf("Action %g\n", gauge_action());

    if((i%n_measure)==0){
      /* Statistics */
      measure();
    }
  }

  return 0;
}


/* eigen/jacobi.c
 *
 * Copyright (C) 2004 Brian Gough, Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>

/* Algorithm 8.4.3 - Cyclic Jacobi.  Golub & Van Loan, Matrix Computations */

static inline double
symschur2 (gsl_matrix * A, size_t p, size_t q, double *c, double *s)
{
  double Apq = gsl_matrix_get (A, p, q);

  if (Apq != 0.0)
    {
      double App = gsl_matrix_get (A, p, p);
      double Aqq = gsl_matrix_get (A, q, q);
      double tau = (Aqq - App) / (2.0 * Apq);
      double t, c1;

      if (tau >= 0.0)
        {
          t = 1.0 / (tau + hypot (1.0, tau));
        }
      else
        {
          t = -1.0 / (-tau + hypot (1.0, tau));
        }

      c1 = 1.0 / hypot (1.0, t);

      *c = c1;
      *s = t * c1;
    }
  else
    {
      *c = 1.0;
      *s = 0.0;
    }

  /* reduction in off(A) is 2*(A_pq)^2 */

  return fabs (Apq);
}

inline static void
apply_jacobi_L (gsl_matrix * A, size_t p, size_t q, double c, double s)
{
  size_t j;
  const size_t N = A->size2;

  /* Apply rotation to matrix A,  A' = J^T A */

  for (j = 0; j < N; j++)
    {
      double Apj = gsl_matrix_get (A, p, j);
      double Aqj = gsl_matrix_get (A, q, j);
      gsl_matrix_set (A, p, j, Apj * c - Aqj * s);
      gsl_matrix_set (A, q, j, Apj * s + Aqj * c);
    }
}

inline static void
apply_jacobi_R (gsl_matrix * A, size_t p, size_t q, double c, double s)
{
  size_t i;
  const size_t M = A->size1;

  /* Apply rotation to matrix A,  A' = A J */

  for (i = 0; i < M; i++)
    {
      double Aip = gsl_matrix_get (A, i, p);
      double Aiq = gsl_matrix_get (A, i, q);
      gsl_matrix_set (A, i, p, Aip * c - Aiq * s);
      gsl_matrix_set (A, i, q, Aip * s + Aiq * c);
    }
}

inline static double
norm (gsl_matrix * A)
{
  size_t i, j, M = A->size1, N = A->size2;
  double sum = 0.0, scale = 0.0, ssq = 1.0;

  for (i = 0; i < M; i++)
    {
      for (j = 0; j < N; j++)
        {
          double Aij = gsl_matrix_get (A, i, j);

          if (Aij != 0.0)
            {
              double ax = fabs (Aij);

              if (scale < ax)
                {
                  ssq = 1.0 + ssq * (scale / ax) * (scale / ax);
                  scale = ax;
                }
              else
                {
                  ssq += (ax / scale) * (ax / scale);
                }
            }

        }
    }

  sum = scale * sqrt (ssq);

  return sum;
}

int
gsl_eigen_jacobi (gsl_matrix * a,
                  gsl_vector * eval,
                  gsl_matrix * evec, unsigned int max_rot, unsigned int *nrot)
{
  size_t i, p, q;
  const size_t M = a->size1, N = a->size2;
  double red, redsum = 0.0;

  if (M != N)
    {
      GSL_ERROR ("eigenproblem requires square matrix", GSL_ENOTSQR);
    }
  else if (M != evec->size1 || M != evec->size2)
    {
      GSL_ERROR ("eigenvector matrix must match input matrix", GSL_EBADLEN);
    }
  else if (M != eval->size)
    {
      GSL_ERROR ("eigenvalue vector must match input matrix", GSL_EBADLEN);
    }

  gsl_vector_set_zero (eval);
  gsl_matrix_set_identity (evec);

  for (i = 0; i < max_rot; i++)
    {
      double nrm = norm (a);

      if (nrm == 0.0)
        break;

      for (p = 0; p < N; p++)
        {
          for (q = p + 1; q < N; q++)
            {
              double c, s;

              red = symschur2 (a, p, q, &c, &s);
              redsum += red;

              /* Compute A <- J^T A J */
              apply_jacobi_L (a, p, q, c, s);
              apply_jacobi_R (a, p, q, c, s);

              /* Compute V <- V J */
              apply_jacobi_R (evec, p, q, c, s);
            }
        }
    }

  *nrot = i;

  for (p = 0; p < N; p++)
    {
      double ep = gsl_matrix_get (a, p, p);
      gsl_vector_set (eval, p, ep);
    }

  if (i == max_rot)
    {
      return GSL_EMAXITER;
    }

  return GSL_SUCCESS;
}

int
gsl_eigen_invert_jacobi (const gsl_matrix * a,
                         gsl_matrix * ainv, unsigned int max_rot)
{
  if (a->size1 != a->size2 || ainv->size1 != ainv->size2)
    {
      GSL_ERROR("jacobi method requires square matrix", GSL_ENOTSQR);
    }
  else if (a->size1 != ainv->size2)
    {
     GSL_ERROR ("inverse matrix must match input matrix", GSL_EBADLEN);
    }

  {
    const size_t n = a->size2;
    size_t i,j,k;
    unsigned int nrot = 0;
    int status;

    gsl_vector * eval = gsl_vector_alloc(n);
    gsl_matrix * evec = gsl_matrix_alloc(n, n);
    gsl_matrix * tmp = gsl_matrix_alloc(n, n);

    gsl_matrix_memcpy (tmp, a);

    status = gsl_eigen_jacobi(tmp, eval, evec, max_rot, &nrot);

    for(i=0; i<n; i++)
      {
        for(j=0; j<n; j++)
          {
            double ainv_ij = 0.0;

            for(k = 0; k<n; k++)
              {
                double f = 1.0 / gsl_vector_get(eval, k);
                double vik = gsl_matrix_get (evec, i, k);
                double vjk = gsl_matrix_get (evec, j, k);
                ainv_ij += vik * vjk * f;
              }
            gsl_matrix_set (ainv, i, j, ainv_ij);
          }
      }

    gsl_vector_free(eval);
    gsl_matrix_free(evec);
    gsl_matrix_free(tmp);

    if (status)
      {
        return status;
      }
    else
      {
        return GSL_SUCCESS;
      }
  }
}


/*
  ferrers.c
    Ferrers bar model.
*/

#include <stddef.h>
#include <stdlib.h>
#include <math.h>

#include "utils.h"
#include "potential.h"

#include <gsl/gsl_sf.h>

#include "ferrers.h"

#define Sq(X) ((X) * (X))

double
_lambda(double x_sq, double y_sq, double z_sq, double a_sq, double b_sq, double c_sq)
{
  #define EQS(u) (x_sq / (a_sq + u) + y_sq / (b_sq + u) + z_sq / (c_sq + u) - 1.)
  double J = 0, K = 0., L = (x_sq + y_sq + z_sq);
  for(;EQS(L) > 0;) L *= 2.;//, printf("doubled!\n");

  // solve the eqs using bisection method.
  for(;L - J > 1.e-8;)
    {
      K = (L + J) / 2.;
      //printf("%f, %f, %f\n", J, K, L);
      if(EQS(K) > 0) J = K;
      else L = K;
    }

  return K;

  #undef EQS
}

int
_orbita_potential_ferrers2_evaluate_wijk(double x, double y, double z,
                                         double a, double b, double c,
                                         double * W, int init,
                                         struct orbita_potential_ferrers2_paramset * ws)
{
  double a_sq = a * a, b_sq = b * b, c_sq = c * c,
         x_sq = x * x, y_sq = y * y, z_sq = z * z;

  // outside the bar region?
  if(x_sq / a_sq + y_sq / b_sq + z_sq / c_sq >= 1 || init)
    {
      // using the root-finding eqs. this is NOT the unique positive root.
      // now use bisection method instead.
      /*
      double A = (a_sq + b_sq + c_sq) - (x_sq + y_sq + z_sq),
             B = (a_sq * b_sq + b_sq * c_sq + c_sq * a_sq) -
                 (x_sq * (b_sq + c_sq) + y_sq * (c_sq + a_sq) + z_sq * (a_sq + b_sq)),
             C = (a_sq * b_sq * c_sq) -
                 (x_sq * b_sq * c_sq + a_sq * y_sq * c_sq + a_sq * b_sq * z_sq);

      double A_sq = A * A, B_sq = B * B, C_sq = C * C;
      double A_cb = A_sq * A, B_cb = B_sq * B;
      double CBR_2 = pow(2., 1. / 3.);

      double lambda =
        pow(-2. * A_cb + 3. * sqrt(3.) * sqrt(4. * A_cb * C - A_sq * B_sq - 18. * A * B * C
          + 4. * B_cb + 27. * C_sq) + 9. * A * B - 27. * C, 1. / 3.) / (3. * CBR_2)
          - (CBR_2 * (2187. * B - 729. * A_sq)) / (2187. * pow(-2. * A_cb + 3. * sqrt(3.)
          * sqrt(4. * A_cb * C - A_sq * B_sq - 18. * A * B * C + 4. * B_cb + 27. * C_sq)
          + 9. * A * B - 27. * C, 1. / 3.)) - A / 3.;
      */

      double lambda = _lambda(x_sq, y_sq, z_sq, a_sq, b_sq, c_sq);
      if(init) lambda = 0.;

      double p = asin(sqrt((a_sq - c_sq) / (a_sq + lambda))),
             k = sqrt((a_sq - b_sq) / (a_sq - c_sq));

      /*
      double F = gsl_sf_ellint_F(p, k, GSL_PREC_DOUBLE),
             E = gsl_sf_ellint_E(p, k, GSL_PREC_DOUBLE);
      */

      double F = gsl_sf_ellint_F(p, k, GSL_PREC_SINGLE),
             E = gsl_sf_ellint_E(p, k, GSL_PREC_SINGLE);

      double delta_lambda = sqrt((a_sq + lambda) * (b_sq + lambda) * (c_sq + lambda));

      /* W_000 */ W[ 0] = 2. * F / sqrt(a_sq - c_sq),
      /* W_100 */ W[ 1] = 2. * (F - E) / ((a_sq - b_sq) * sqrt(a_sq - c_sq)),
      /* W_001 */ W[ 3] = (2. / (b_sq - c_sq)) * sqrt((b_sq + lambda) / ((a_sq + lambda) * (c_sq + lambda)))
                          -2. * E / ((b_sq - c_sq) * sqrt(a_sq - c_sq)),
      /* W_010 */ W[ 2] = 2. / delta_lambda - W[1] - W[3],
      /* W_110 */ W[ 4] = (W[2] - W[1]) / (a_sq - b_sq),
      /* W_011 */ W[ 5] = (W[3] - W[2]) / (b_sq - c_sq),
      /* W_101 */ W[ 6] = (W[1] - W[3]) / (c_sq - a_sq),
      /* W_200 */ W[ 7] = (2. / (delta_lambda * (a_sq + lambda)) - W[4] - W[6]) / 3.,
      /* W_020 */ W[ 8] = (2. / (delta_lambda * (b_sq + lambda)) - W[5] - W[4]) / 3.,
      /* W_002 */ W[ 9] = (2. / (delta_lambda * (c_sq + lambda)) - W[6] - W[5]) / 3.,
      /* W_111 */ W[10] = (W[4] - W[5]) / (c_sq - a_sq),
      /* W_120 */ W[11] = (W[8] - W[4]) / (a_sq - b_sq),
      /* W_012 */ W[12] = (W[9] - W[5]) / (b_sq - c_sq),
      /* W_201 */ W[13] = (W[7] - W[6]) / (c_sq - a_sq),
      /* W_210 */ W[14] = (W[7] - W[4]) / (b_sq - a_sq), // NOTE Fixed. Nov. 15, 2015
      /* W_021 */ W[15] = (W[8] - W[5]) / (c_sq - b_sq), //
      /* W_102 */ W[16] = (W[9] - W[6]) / (a_sq - c_sq), //
      /* W_300 */ W[17] = (2. / (delta_lambda * Sq(a_sq + lambda)) - W[14] - W[13]) / 5.,
      /* W_030 */ W[18] = (2. / (delta_lambda * Sq(b_sq + lambda)) - W[15] - W[11]) / 5.,
      /* W_003 */ W[19] = (2. / (delta_lambda * Sq(c_sq + lambda)) - W[16] - W[12]) / 5.;
    }
  else
    {
      int i_ijk;
      for(i_ijk = 0; i_ijk < 20; ++ i_ijk)
        W[i_ijk] = (ws -> W_i)[i_ijk];
    }

  return 0;
}

double
_orbita_potential_ferrers2_psi(const void * par, const double * pos)
{
  struct orbita_potential_ferrers2_paramset * ws =
    (struct orbita_potential_ferrers2_paramset *) par;

  double W[20];
  _orbita_potential_ferrers2_evaluate_wijk(
      pos[0], pos[1], pos[2], ws -> a, ws -> b, ws -> c, W, 0, ws);

  double x_sq = pos[0] * pos[0], y_sq = pos[1] * pos[1], z_sq = pos[2] * pos[2];

  /*
  return (-(ws -> C) / 6.) *
         (
             W[0] - 6. * x_sq * y_sq * z_sq * W[10]
           + x_sq * (x_sq * (3. * W[7] - x_sq * W[17])
           + 3. * (y_sq * (2 * W[4] - y_sq * W[11] - x_sq * W[14]) - W[1]))
           + y_sq * (y_sq * (3. * W[8] - y_sq * W[18])
           + 3. * (z_sq * (2 * W[5] - z_sq * W[12] - y_sq * W[15]) - W[2]))
           + z_sq * (z_sq * (3. * W[9] - z_sq * W[19])
           + 3. * (x_sq * (2 * W[6] - x_sq * W[13] - z_sq * W[16]) - W[3]))
         );
  */

  // This is to test if Del^2 Psi == 4 Pi G rho. Returns rho from W_jkl.
  return ((ws -> a) * (ws -> b) * (ws -> c) * (ws -> rho) / 2.) *
          (
                  (W[1] + W[2] + W[3])
            - 2. * ((y_sq + x_sq) * W[4] + (z_sq + y_sq) * W[5] + (x_sq + z_sq) * W[6])
            - 6. * (x_sq * W[7] + y_sq * W[8] + z_sq * W[9])
            + 2. * (x_sq * y_sq + y_sq * z_sq + z_sq * x_sq) * W[10]
            + (y_sq * y_sq + 6. * x_sq * y_sq) * W[11]
            + (z_sq * z_sq + 6. * y_sq * z_sq) * W[12]
            + (x_sq * x_sq + 6. * z_sq * x_sq) * W[13]
            + (6. * x_sq * y_sq + x_sq * x_sq) * W[14]
            + (6. * y_sq * z_sq + y_sq * y_sq) * W[15]
            + (6. * z_sq * x_sq + z_sq * z_sq) * W[16]
            + 5. * (x_sq * x_sq * W[17] + y_sq * y_sq * W[18] + z_sq * z_sq * W[19])
          );
}

int
_orbita_potential_ferrers2_f(const void * par,
                             const double * pos, double * F)
{
  struct orbita_potential_ferrers2_paramset * ws =
    (struct orbita_potential_ferrers2_paramset *) par;

  double W[20];
  _orbita_potential_ferrers2_evaluate_wijk(
    pos[0], pos[1], pos[2], ws -> a, ws -> b, ws -> c, W, 0, ws);

  double x_sq = pos[0] * pos[0], y_sq = pos[1] * pos[1], z_sq = pos[2] * pos[2]; //

  F[0] = -pos[0] * (ws -> C) * //
          (
            W[1] + x_sq * (x_sq * W[17] + 2. * (y_sq * W[14] - W[7]))
                 + y_sq * (y_sq * W[11] + 2. * (z_sq * W[10] - W[4]))
                 + z_sq * (z_sq * W[16] + 2. * (x_sq * W[13] - W[6]))
          ),
  F[1] = -pos[1] * (ws -> C) * //
          (
            W[2] + x_sq * (x_sq * W[14] + 2. * (y_sq * W[11] - W[4]))
                 + y_sq * (y_sq * W[18] + 2. * (z_sq * W[15] - W[8]))
                 + z_sq * (z_sq * W[12] + 2. * (x_sq * W[10] - W[5]))
          ),
  F[2] = -pos[2] * (ws -> C) * //
          (
            W[3] + x_sq * (x_sq * W[13] + 2. * (y_sq * W[10] - W[6]))
                 + y_sq * (y_sq * W[15] + 2. * (z_sq * W[12] - W[5]))
                 + z_sq * (z_sq * W[19] + 2. * (x_sq * W[16] - W[9]))
          );

  return 0;
}

double
_orbita_potential_ferrers2_rho(const void * par, const double * pos)
{
  struct orbita_potential_ferrers2_paramset * ws =
    (struct orbita_potential_ferrers2_paramset *) par;

  double m_sq = Sq(pos[0]) / Sq(ws -> a) + Sq(pos[1]) / Sq(ws -> b) + Sq(pos[2]) / Sq(ws -> c);
  if(m_sq < 1.) return (ws -> rho) * Sq(1 - m_sq);
  else return 0.;
}

double
_orbita_potential_ferrers2_Mr(const void * par, const double R)
{
  orbita_err("Method not supported.");
  return 0.;
}

double
_orbita_potential_ferrers2_Mt(const void * par)
{
  struct orbita_potential_ferrers2_paramset * ws =
    (struct orbita_potential_ferrers2_paramset *) par;
  return (ws -> rho) * (ws -> a) * (ws -> b) * (ws -> c) * (32. * Pi / 105.);
}

int
_orbita_potential_ferrers2_kill(struct orbita_potential * psi)
{
  free(psi -> param);
  return 0;
}

struct orbita_potential *
orbita_potential_ferrers2(double rho, double a, double b, double c)
{
  struct orbita_potential * psi =
    (struct orbita_potential *) malloc(sizeof(struct orbita_potential));
  struct orbita_potential_ferrers2_paramset * ws =
    (struct orbita_potential_ferrers2_paramset *)
    malloc(sizeof(struct orbita_potential_ferrers2_paramset));
  psi -> param = (void *)ws;

  // initialize w_i
  _orbita_potential_ferrers2_evaluate_wijk(0., 0., 0., a, b, c, ws -> W_i, 1, ws);
  ws -> C = 2. * Pi * rho * a * b * c;
  ws -> a = a, ws -> b = b, ws -> c = c, ws -> rho = rho;

  // set other parameters
  psi -> symmetry     = 0,
  psi -> coordinates  = ORBITA_COORD_CARTESIAN;

  psi -> rho = & _orbita_potential_ferrers2_rho,
  psi -> psi = & _orbita_potential_ferrers2_psi,
  psi -> f   = & _orbita_potential_ferrers2_f,
  psi -> Mr  = & _orbita_potential_ferrers2_Mr,
  psi -> Mt  = & _orbita_potential_ferrers2_Mt;

  psi -> is_variable = 0,
  psi -> init_param  = NULL,
  psi -> param_func  = NULL;

  psi -> kill = & _orbita_potential_ferrers2_kill;

  psi -> is_composite = 0;

  return psi;
}