#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;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include <time.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>

typedef struct parameters{
	double Xo;
	double Ko;
	double SIGMA2_K;
	double SIGMA2_C;
	double R;
	double MU;
	double SIGMA2_MU;
}PAR;

/****** Function Prototypes ******/
/* Model */

double Nhat(double x, double Ko, double SIGMA2_K);
double C(double x, double y, double SIGMA2_C);

/* Jump moment equations */
double alpha1(double phi, void *params);
double alpha1_prime(double phi, void *params);

/* Useful functions */
double round( double x);               // rounding

/*Main function */
void canonical
(
	PAR parameters,
	double MAX_TIME,
	double ENSEMBLES,
	double Dt,
	double dt
);

void ratescalc
(
	gsl_rng *rng,
	double X, double Y,
	void *params,
	double *prob, double *wait_time
);

int func (double t, const double y[], double f[], void *params);
void gslode(void *params, double MAX_TIME, FILE *theory);
void euler(void *params, double MAX_TIME, FILE *theory);


/*
 * qubit.h
 *
 *  Created on: 30 Jan 2019
 *      Author: jobinrjohnson
 */

#ifndef IMPLEMENTATION_QUBIT_H_
#define IMPLEMENTATION_QUBIT_H_

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_rng.h>
#include <iostream>

class Qubit
{

  protected:
	gsl_vector_complex *v_state;

  public:
	Qubit();
	Qubit(double, double, double, double);
	virtual ~Qubit();
	void print_state();
	gsl_vector_complex *get_state();
	bool set_state(gsl_vector_complex *);
};

#endif /* IMPLEMENTATION_QUBIT_H_ */


/**
 *
 * @generated s Tue Jan  7 11:45:26 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include <plasma.h>
#include <core_blas.h>
#include "auxiliary.h"

/*-------------------------------------------------------------------
 * Check the orthogonality of Q
 */

int s_check_orthogonality(int M, int N, int LDQ, float *Q)
{
    float alpha, beta;
    float normQ;
    int info_ortho;
    int i;
    int minMN = min(M, N);
    float eps;
    float *work = (float *)malloc(minMN*sizeof(float));

    eps = LAPACKE_slamch_work('e');
    alpha = 1.0;
    beta  = -1.0;

    /* Build the idendity matrix USE DLASET?*/
    float *Id = (float *) malloc(minMN*minMN*sizeof(float));
    memset((void*)Id, 0, minMN*minMN*sizeof(float));
    for (i = 0; i < minMN; i++)
        Id[i*minMN+i] = (float)1.0;

    /* Perform Id - Q'Q */
    if (M >= N)
        cblas_ssyrk(CblasColMajor, CblasUpper, CblasTrans, N, M, alpha, Q, LDQ, beta, Id, N);
    else
        cblas_ssyrk(CblasColMajor, CblasUpper, CblasNoTrans, M, N, alpha, Q, LDQ, beta, Id, M);

    normQ = LAPACKE_slansy_work(LAPACK_COL_MAJOR, 'i', 'u', minMN, Id, minMN, work);

    printf("============\n");
    printf("Checking the orthogonality of Q \n");
    printf("||Id-Q'*Q||_oo / (N*eps) = %e \n",normQ/(minMN*eps));

    if ( isnan(normQ / (minMN * eps)) || (normQ / (minMN * eps) > 10.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }

    free(work); free(Id);

    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the factorization QR
 */

int s_check_QRfactorization(int M, int N, float *A1, float *A2, int LDA, float *Q)
{
    float Anorm, Rnorm;
    float alpha, beta;
    int info_factorization;
    int i,j;
    float eps;

    eps = LAPACKE_slamch_work('e');

    float *Ql       = (float *)malloc(M*N*sizeof(float));
    float *Residual = (float *)malloc(M*N*sizeof(float));
    float *work              = (float *)malloc(max(M,N)*sizeof(float));

    alpha=1.0;
    beta=0.0;

    if (M >= N) {
        /* Extract the R */
        float *R = (float *)malloc(N*N*sizeof(float));
        memset((void*)R, 0, N*N*sizeof(float));
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'u', M, N, A2, LDA, R, N);

        /* Perform Ql=Q*R */
        memset((void*)Ql, 0, M*N*sizeof(float));
        cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, N, (alpha), Q, LDA, R, N, (beta), Ql, M);
        free(R);
    }
    else {
        /* Extract the L */
        float *L = (float *)malloc(M*M*sizeof(float));
        memset((void*)L, 0, M*M*sizeof(float));
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'l', M, N, A2, LDA, L, M);

    /* Perform Ql=LQ */
        memset((void*)Ql, 0, M*N*sizeof(float));
        cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, M, (alpha), L, M, Q, LDA, (beta), Ql, M);
        free(L);
    }

    /* Compute the Residual */
    for (i = 0; i < M; i++)
        for (j = 0 ; j < N; j++)
            Residual[j*M+i] = A1[j*LDA+i]-Ql[j*M+i];

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Residual, M, work);
    Anorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, A2, LDA, work);

    if (M >= N) {
        printf("============\n");
        printf("Checking the QR Factorization \n");
        printf("-- ||A-QR||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }
    else {
        printf("============\n");
        printf("Checking the LQ Factorization \n");
        printf("-- ||A-LQ||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }

    if (isnan(Rnorm / (Anorm * N *eps)) || (Rnorm / (Anorm * N * eps) > 10.0) ) {
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else {
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(work); free(Ql); free(Residual);

    return info_factorization;
}

/*------------------------------------------------------------------------
 *  Check the factorization of the matrix A2
 */

int s_check_LLTfactorization(int N, float *A1, float *A2, int LDA, int uplo)
{
    float Anorm, Rnorm;
    float alpha;
    int info_factorization;
    int i,j;
    float eps;

    eps = LAPACKE_slamch_work('e');

    float *Residual = (float *)malloc(N*N*sizeof(float));
    float *L1       = (float *)malloc(N*N*sizeof(float));
    float *L2       = (float *)malloc(N*N*sizeof(float));
    float *work              = (float *)malloc(N*sizeof(float));

    memset((void*)L1, 0, N*N*sizeof(float));
    memset((void*)L2, 0, N*N*sizeof(float));

    alpha= 1.0;

    LAPACKE_slacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_strmm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_slacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_strmm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', N, N, Residual, N, work);
    Anorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', N, N, A1, LDA, work);

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 10.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

/*--------------------------------------------------------------
 * Check the gemm
 */
float s_check_gemm(PLASMA_enum transA, PLASMA_enum transB, int M, int N, int K,
                   float alpha, float *A, int LDA,
                   float *B, int LDB,
                   float beta, float *Cplasma,
                   float *Cref, int LDC,
                   float *Cinitnorm, float *Cplasmanorm, float *Clapacknorm )
{
    float beta_const = -1.0;
    float Rnorm;
    float *work = (float *)malloc(max(K,max(M, N))* sizeof(float));

    *Cinitnorm   = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref,    LDC, work);
    *Cplasmanorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Cplasma, LDC, work);

    cblas_sgemm(CblasColMajor, (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB, M, N, K,
                (alpha), A, LDA, B, LDB, (beta), Cref, LDC);

    *Clapacknorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    cblas_saxpy(LDC * N, (beta_const), Cplasma, 1, Cref, 1);

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the trsm
 */
float s_check_trsm(PLASMA_enum side, PLASMA_enum uplo, PLASMA_enum trans, PLASMA_enum diag,
                   int M, int NRHS, float alpha,
                   float *A, int LDA,
                   float *Bplasma, float *Bref, int LDB,
                   float *Binitnorm, float *Bplasmanorm, float *Blapacknorm )
{
    float beta_const = -1.0;
    float Rnorm;
    float *work = (float *)malloc(max(M, NRHS)* sizeof(float));
    /*float eps = LAPACKE_slamch_work('e');*/

    *Binitnorm   = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, Bref,    LDB, work);
    *Bplasmanorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bplasma, LDB, work);

    cblas_strsm(CblasColMajor, (CBLAS_SIDE)side, (CBLAS_UPLO)uplo,
                (CBLAS_TRANSPOSE)trans, (CBLAS_DIAG)diag, M, NRHS,
                (alpha), A, LDA, Bref, LDB);

    *Blapacknorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);

    cblas_saxpy(LDB * NRHS, (beta_const), Bplasma, 1, Bref, 1);

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);
    Rnorm = Rnorm / *Blapacknorm;
    /* max(M,NRHS) * eps);*/

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the solution
 */

float s_check_solution(int M, int N, int NRHS, float *A, int LDA,
                      float *B,  float *X, int LDB,
                      float *anorm, float *bnorm, float *xnorm )
{
/*     int info_solution; */
    float Rnorm = -1.00;
    float zone  =  1.0;
    float mzone = -1.0;
    float *work = (float *)malloc(max(M, N)* sizeof(float));

    *anorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, N,    A, LDA, work);
    *xnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, X, LDB, work);
    *bnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, NRHS, N, (zone), A, LDA, X, LDB, (mzone), B, LDB);

    Rnorm = LAPACKE_slange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    free(work);

    return Rnorm;
}


/*
* -----------------------------------------------------------------
*  Ellipsoid Library --- ell_lib.h
*  Version: 1.6180
*  Date: Feb 19, 2010
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010 by Americo Barbosa da Cunha Junior
*
*  This program is free software: you can redistribute it and/or
*  modify it under the terms of the GNU General Public License as
*  published by the Free Software Foundation, either version 3 of
*  the License, or (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  A copy of the GNU General Public License is available in
*  LICENSE.txt or http://www.gnu.org/licenses/.
* -----------------------------------------------------------------
*  This is the header file for a library that operates an
*  n-dimensional ellipsoid.
* -----------------------------------------------------------------
*/

#ifndef __ELL_LIB_H__
#define __ELL_LIB_H__

#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

#undef  TOL
#define TOL 1.0e-7

#undef  TRUE
#define TRUE 1

#undef  FALSE
#define FALSE 0

/*
*------------------------------------------------------------
* function prototypes
*------------------------------------------------------------
*/

void ell_psd2eig(gsl_matrix *B,
                 gsl_matrix *V,
                 gsl_vector *lam);

void ell_psd2chol(gsl_matrix *B,
                  gsl_matrix *L);

void ell_eig2chol(gsl_matrix *V,
                  gsl_vector *sig,
                  gsl_matrix *L);

double ell_pt_in(gsl_vector *x,
                 gsl_vector *c,
                 gsl_matrix *L);

void ell_map2uhs(gsl_vector *x,
                 gsl_vector *c,
                 gsl_matrix *L,
                 gsl_vector *y);

void ell_pt_modify(gsl_vector *p,
                   gsl_vector *c,
                   gsl_matrix *L);

#endif /* __ELL_LIB_H__ */


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

#include <config.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "err.c"
#include "qng.h"

int
gsl_integration_qng (const gsl_function *f,
                     double a, double b,
                     double epsabs, double epsrel,
                     double * result, double * abserr, size_t * neval)
{
  double fv1[5], fv2[5], fv3[5], fv4[5];
  double savfun[21];  /* array of function values which have been computed */
  double res10, res21, res43, res87;    /* 10, 21, 43 and 87 point results */
  double result_kronrod, err ;
  double resabs; /* approximation to the integral of abs(f) */
  double resasc; /* approximation to the integral of abs(f-i/(b-a)) */

  const double half_length =  0.5 * (b - a);
  const double abs_half_length = fabs (half_length);
  const double center = 0.5 * (b + a);
  const double f_center = GSL_FN_EVAL(f, center);

  int k ;

  if (epsabs <= 0 && (epsrel < 50 * GSL_DBL_EPSILON || epsrel < 0.5e-28))
    {
      * result = 0;
      * abserr = 0;
      * neval = 0;
      GSL_ERROR ("tolerance cannot be acheived with given epsabs and epsrel",
                 GSL_EBADTOL);
    };

  /* Compute the integral using the 10- and 21-point formula. */

  res10 = 0;
  res21 = w21b[5] * f_center;
  resabs = w21b[5] * fabs (f_center);

  for (k = 0; k < 5; k++)
    {
      const double abscissa = half_length * x1[k];
      const double fval1 = GSL_FN_EVAL(f, center + abscissa);
      const double fval2 = GSL_FN_EVAL(f, center - abscissa);
      const double fval = fval1 + fval2;
      res10 += w10[k] * fval;
      res21 += w21a[k] * fval;
      resabs += w21a[k] * (fabs (fval1) + fabs (fval2));
      savfun[k] = fval;
      fv1[k] = fval1;
      fv2[k] = fval2;
    }

  for (k = 0; k < 5; k++)
    {
      const double abscissa = half_length * x2[k];
      const double fval1 = GSL_FN_EVAL(f, center + abscissa);
      const double fval2 = GSL_FN_EVAL(f, center - abscissa);
      const double fval = fval1 + fval2;
      res21 += w21b[k] * fval;
      resabs += w21b[k] * (fabs (fval1) + fabs (fval2));
      savfun[k + 5] = fval;
      fv3[k] = fval1;
      fv4[k] = fval2;
    }

  resabs *= abs_half_length ;

  {
    const double mean = 0.5 * res21;

    resasc = w21b[5] * fabs (f_center - mean);

    for (k = 0; k < 5; k++)
      {
        resasc +=
          (w21a[k] * (fabs (fv1[k] - mean) + fabs (fv2[k] - mean))
          + w21b[k] * (fabs (fv3[k] - mean) + fabs (fv4[k] - mean)));
      }
    resasc *= abs_half_length ;
  }

  result_kronrod = res21 * half_length;

  err = rescale_error ((res21 - res10) * half_length, resabs, resasc) ;

  /*   test for convergence. */

  if (err < epsabs || err < epsrel * fabs (result_kronrod))
    {
      * result = result_kronrod ;
      * abserr = err ;
      * neval = 21;
      return GSL_SUCCESS;
    }

  /* compute the integral using the 43-point formula. */

  res43 = w43b[11] * f_center;

  for (k = 0; k < 10; k++)
    {
      res43 += savfun[k] * w43a[k];
    }

  for (k = 0; k < 11; k++)
    {
      const double abscissa = half_length * x3[k];
      const double fval = (GSL_FN_EVAL(f, center + abscissa)
                           + GSL_FN_EVAL(f, center - abscissa));
      res43 += fval * w43b[k];
      savfun[k + 10] = fval;
    }

  /*  test for convergence */

  result_kronrod = res43 * half_length;
  err = rescale_error ((res43 - res21) * half_length, resabs, resasc);

  if (err < epsabs || err < epsrel * fabs (result_kronrod))
    {
      * result = result_kronrod ;
      * abserr = err ;
      * neval = 43;
      return GSL_SUCCESS;
    }

  /* compute the integral using the 87-point formula. */

  res87 = w87b[22] * f_center;

  for (k = 0; k < 21; k++)
    {
      res87 += savfun[k] * w87a[k];
    }

  for (k = 0; k < 22; k++)
    {
      const double abscissa = half_length * x4[k];
      res87 += w87b[k] * (GSL_FN_EVAL(f, center + abscissa)
                          + GSL_FN_EVAL(f, center - abscissa));
    }

  /*  test for convergence */

  result_kronrod = res87 * half_length ;

  err = rescale_error ((res87 - res43) * half_length, resabs, resasc);

  if (err < epsabs || err < epsrel * fabs (result_kronrod))
    {
      * result = result_kronrod ;
      * abserr = err ;
      * neval = 87;
      return GSL_SUCCESS;
    }

  /* failed to converge */

  * result = result_kronrod ;
  * abserr = err ;
  * neval = 87;

  GSL_ERROR("failed to reach tolerance with highest-order rule", GSL_ETOL) ;
}


#ifndef METHODS_HELLER_H
#define METHODS_HELLER_H

#include <gsl/gsl_multimin.h>

namespace method {
// use your method name to create a subspace for your
// implementation of details
namespace heller_cwa {
namespace details {

struct HellerParam {

  // exp(a[0] p^2 + a[1] p + a[2] p q + a[3] q + a[4] q^2 + a[5])
  // alpha = 0.5 / a[0]
  // delta = a[1]
  // epsilon = a[2]

  arma::cx_vec a;
  arma::mat points;
  math::Polynomial<cx_double> V_eff_0;
  double mass;
};

inline
cx_double heller_gaussian(const arma::cx_vec & a,
                          const double q,
                          const double p) {
  return std::exp(a(0) * std::pow(p, 2)
                  + a(1) * p
                  + a(2) * p * q
                  + a(3) * q
                  + a(4) * std::pow(q, 2)
                  + a(5));
}

inline
arma::cx_vec heller_gaussian(const arma::cx_vec & a,
                             const arma::mat & points) {
  if (points.n_rows != 2) {
    throw Error("heller_gaussian only supports one-dimensional phase space");
  }

  arma::cx_vec result(points.n_cols);
#pragma omp parallel for
  for (arma::uword i = 0; i < result.n_elem; i++) {
    result(i) = heller_gaussian(a, points(0, i), points(1, i));
  }

  return result;
}

inline
math::Gaussian<cx_double> heller_gaussian(const arma::cx_vec & a) {

  const arma::cx_mat binomial{{{-a(4) * 2.0, -a(2)}, {-a(2), -a(0) * 2.0}}};
  const arma::cx_mat covariance = arma::inv(binomial);
  const arma::cx_vec center = covariance * arma::cx_vec{a(3),a(1)};

  return math::Gaussian<cx_double>(covariance, center,
      std::exp(a(5)) * std::exp(0.5 * arma::dot(center, arma::cx_vec{a(3),a(1)})));

}

template<typename Potential>
arma::mat force(const Potential & potential,
                const arma::mat & positions) {
  arma::mat result = arma::mat(arma::size(positions));

#pragma omp parallel for
  for (arma::uword i = 0; i < positions.n_cols; i++) {
    const arma::vec position = positions.col(i);
    for (arma::uword j = 0; j < positions.n_rows; j++) {
      result(j, i) = -potential.derivative(j).at(position);
    }
  }

  return result;
}

template<typename Function>
inline
auto effective_potential(const Function & potential,
                         const cx_double alpha,
                         const cx_double delta,
                         const cx_double epsilon,
                         const arma::uword l,
                         const arma::uword cut_off) {

  if (potential.dim() > 1) {
    throw Error("Currently effective potential is only valid "
                "for one dimensional potential");
  }

  const auto functor = [alpha](const Function & function) -> Function {
    return function.derivative(0).derivative(0) / 8.0 / alpha;
  };

  const auto a = exp(functor, derivative(potential, arma::uvec{l}), cut_off) *
                 std::pow(-1, l + 1);
  const auto b = exp(functor, derivative(potential, arma::uvec{l}), cut_off);

  const std::vector<math::Polynomial<cx_double>>
      a_inner =
      {math::Polynomial<cx_double>(
          arma::cx_vec{1.0 + epsilon / 2.0,
                       cx_double{0.0, 1.0} / 2.0 / alpha,
                       0.5 * delta},
          lmat{{1, 0, 0},
               {0, 1, 0}})};

  const std::vector<math::Polynomial<cx_double>>
      b_inner =
      {math::Polynomial<cx_double>(
          arma::cx_vec{1.0 - epsilon / 2.0,
                       -cx_double{0.0, 1.0} / 2.0 / alpha,
                       -0.5 * delta},
          lmat{{1, 0, 0},
               {0, 1, 0}})};

  const auto combined = (a(a_inner) + b(b_inner)) * 0.5;

  return combined;
}

inline
math::Polynomial<cx_double> E_function(const arma::cx_vec & a,
                                       const arma::cx_vec & a_derivatives,
                                       const math::Polynomial<cx_double> & V_eff_0,
                                       const double mass) {

  if (a.n_elem != 6 || a_derivatives.n_elem != 6) {
    throw Error(
        "The number of elements for a's or their derivatives is invalid for E function");
  }

  const math::Polynomial<cx_double> without_derivatives(
      arma::cx_vec{-a(2) / mass, -2.0 * a(4) / mass, -a(3) / mass},
      lmat{{{0, 1, 0}, {2, 1, 1}}}
  );

  const math::Polynomial<cx_double> derivatives(
      arma::cx_vec{-a_derivatives(0),
                   -a_derivatives(1),
                   -a_derivatives(2),
                   -a_derivatives(3),
                   -a_derivatives(4),
                   -a_derivatives(5)},
      lmat{{{0, 0, 1, 1, 2, 0}, {2, 1, 1, 0, 0, 0}}}
  );

  return without_derivatives + derivatives - V_eff_0 * cx_double{0.0, 2.0};

}

inline
double I_function(
    const arma::cx_vec & a,
    const arma::cx_vec & a_derivatives,
    const arma::mat & points,
    const math::Polynomial<cx_double> & V_eff_0,
    const double mass) {

  return arma::sum(arma::pow(arma::abs(
      at(E_function(a, a_derivatives, V_eff_0, mass), points)
      % heller_gaussian(a, points)), 2));

}

inline
arma::vec I_function_derivative(
    const arma::cx_vec & a,
    const arma::cx_vec & a_derivatives,
    const arma::mat & points,
    const math::Polynomial<cx_double> & V_eff_0,
    const double mass
) {

  arma::vec result(12);

  const auto E_func = E_function(a, a_derivatives, V_eff_0, mass);

  const auto polynomial_term_list = lmat{{{0, 0, 1, 1, 2, 0}, {2, 1, 1, 0, 0, 0}}};

#pragma omp parallel for
  for (arma::uword i = 0; i < 6; i++) {

    const lvec E_derivative_term = polynomial_term_list.col(i);
    const auto E_derivative =
        math::polynomial::Term<double>(1.0, E_derivative_term);

    result(i) = -arma::sum(2.0 *
                           arma::real(at(E_func.conj() * E_derivative, points))
                           %
                           arma::pow(arma::abs(heller_gaussian(a, points)), 2));

    result(i + 6) = arma::sum(2.0 *
                              arma::imag(
                                  at(E_func.conj() * E_derivative, points))
                              %
                              arma::pow(arma::abs(heller_gaussian(a, points)),
                                        2));
  }

  return result;
}

inline
double I_function_gsl_wrapper(
    const gsl_vector * a_derivatives,
    void * param
) {
  const HellerParam heller_param = *(HellerParam *) param;
  const arma::cx_vec a = heller_param.a;
  const arma::vec a_derivatives_all = gsl::convert_vec(a_derivatives);
  const arma::cx_vec a_derivatives_arma =
      arma::cx_vec{a_derivatives_all.rows(0, 5),
                   a_derivatives_all.rows(6, 11)};

  return I_function(a,
                    a_derivatives_arma,
                    heller_param.points,
                    heller_param.V_eff_0,
                    heller_param.mass);

}

inline
void I_function_derivative_gsl_wrapper(
    const gsl_vector * a_derivatives,
    void * param,
    gsl_vector * g
) {
  const HellerParam heller_param = *(HellerParam *) param;
  const arma::cx_vec a = heller_param.a;
  const arma::vec a_derivatives_all = gsl::convert_vec(a_derivatives);

  const arma::cx_vec a_derivatives_arma =
      arma::cx_vec{a_derivatives_all.rows(0, 5),
                   a_derivatives_all.rows(6, 11)};

  const auto result_pointer =
      gsl::convert_vec(I_function_derivative(a,
                                             a_derivatives_arma,
                                             heller_param.points,
                                             heller_param.V_eff_0,
                                             heller_param.mass));

  gsl_vector_memcpy(g, result_pointer);

  gsl_vector_free(result_pointer);
}

inline
void I_function_fdf_gsl_wrapper(
    const gsl_vector * a_derivatives,
    void * param,
    double * f,
    gsl_vector * g
) {
  I_function_derivative_gsl_wrapper(a_derivatives, param, g);

  *f = I_function_gsl_wrapper(a_derivatives, param);
}

inline
arma::cx_vec a_derivative(HellerParam input,
                          const double initial_step_size,
                          const double tolerance,
                          const double gradient_tolerance,
                          const size_t total_steps) {

  /* allocate memory for minimization process */
  const auto minimizer_type = gsl_multimin_fdfminimizer_vector_bfgs2;
  auto minimizer_environment = gsl_multimin_fdfminimizer_alloc(minimizer_type,
                                                               12);

  /* assigning function to minimizer object */
  gsl_multimin_function_fdf minimizer_object;
  minimizer_object.f = &I_function_gsl_wrapper;
  minimizer_object.df = &I_function_derivative_gsl_wrapper;
  minimizer_object.fdf = &I_function_fdf_gsl_wrapper;
  minimizer_object.n = 12;
  minimizer_object.params = (void *) &input;

  /* starting point */
  const auto a_derivatives = gsl_vector_calloc(12);

  /* set environment */
  gsl_multimin_fdfminimizer_set(minimizer_environment,
                                &minimizer_object, a_derivatives,
                                initial_step_size, tolerance);

  size_t iter = 0;
  int status = GSL_CONTINUE;
  do {
    iter++;
    status = gsl_multimin_fdfminimizer_iterate(minimizer_environment);

    if (status) {
      throw Error(gsl_strerror(status));
    }

    status = gsl_multimin_test_gradient(minimizer_environment->gradient,
                                        gradient_tolerance);

    if (status == GSL_SUCCESS) {
      const arma::vec result = gsl::convert_vec(minimizer_environment->x);

      gsl_multimin_fdfminimizer_free(minimizer_environment);
      gsl_vector_free(a_derivatives);

      return arma::cx_vec{result.rows(arma::span(0, 5)),
                          result.rows(arma::span(6, 11))};
    }
  } while (status == GSL_CONTINUE && iter < total_steps);

  return arma::ones<arma::cx_vec>(6) * 2.8375;
}

} // namespace details

struct State {
public:
  arma::mat points;
  arma::vec weights;
  arma::vec masses;

  // Establish an easy way to construct your State
  template<typename PhaseSpaceDistribution>
  State(const PhaseSpaceDistribution & initial,
        const arma::uvec & grid,
        const arma::mat & range,
        const arma::vec & masses) :
      points(math::space::points_generate(grid, range)),
      weights(arma::real(at(initial, points))),
      masses(masses) {
    if (grid.n_rows != range.n_rows) {
      throw Error("Different dimension between the grid and the range");
    }
    if (grid.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the grid and the masses");
    }
  }

  template<typename PhaseSpaceDistribution>
  State(const PhaseSpaceDistribution & initial,
        const arma::uvec & grid,
        const arma::mat & range) :
      points(math::space::points_generate(grid, range)),
      weights(arma::real(at(initial, points))),
      masses(arma::ones<arma::vec>(grid.n_elem / 2)) {
    if (grid.n_elem % 2 != 0) {
      throw Error("Odd number of dimension - it is not likely a phase space");
    }
    if (grid.n_rows != range.n_rows) {
      throw Error("Different dimension between the grid and the range");
    }
    if (grid.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the grid and the masses");
    }
  }

  inline
  State(const arma::mat & points,
        const arma::vec & weights,
        const arma::vec & masses) :
      points(points),
      weights(weights),
      masses(masses) {
    if (points.n_cols != weights.n_elem) {
      throw Error("Different number of points and corresponding weights");
    }

    if (points.n_rows != 2 * masses.n_rows) {
      throw Error("Different dimension between the points and the masses");
    }
  }

  inline
  arma::uword dim() const {
    return points.n_rows / 2;
  }

  inline
  State normalise() const {
    return State(this->points, this->weights / arma::sum(this->weights),
                 this->masses);
  }

  template<typename Function>
  arma::vec expectation(const std::vector<Function> & function) const {
    arma::vec result(function.size());

#pragma omp parallel for
    for (arma::uword i = 0; i < result.n_elem; i++) {

      if (function[i].dim() != this->dim() * 2) {
        throw Error(
            "The dimension of the function is invalid for the calculation of expectation");
      }
      result(i) = arma::dot(at(function[i], this->points), weights) /
                  arma::sum(weights);
    }

    return result;
  }

  template<typename Function>
  double expectation(const Function & function) const {
    const arma::vec result = at(function, this->points);

    return arma::dot(result, weights) / arma::sum(weights);
  }

  inline
  arma::vec positional_expectation() const {
    arma::uword dim = this->dim();

    return this->points.rows(0, dim - 1) * this->weights /
           arma::sum(this->weights);
  }

  inline
  arma::vec momentum_expectation() const {
    arma::uword dim = this->dim();

    return this->points.rows(dim, 2 * dim - 1) * this->weights /
           arma::sum(this->weights);
  }

  State operator+(const State & B) const {
    if (!arma::approx_equal(this->weights, B.weights, "abs_diff", 1e-16) ||
        !arma::approx_equal(this->masses, B.masses, "abs_diff", 1e-16)) {
      throw Error("Different cwa states are being added");
    }

    return State(this->points + B.points, this->weights, this->masses);
  }

  State operator*(const double B) const {
    return State(this->points * B, this->weights, this->masses);
  }
};

template<typename Potential>
struct Operator {

private:
  PropagationType type = Classic;

public:
  Potential potential;

  Operator(const State & state,
           const Potential & potential) :
      potential(potential) {}

  inline
  PropagationType propagation_type() const {
    return Classic;
  }

  State operator()(const State & state) const {

    arma::mat p_submatrix = state.points.rows(state.dim(), 2 * state.dim() - 1);
    p_submatrix.each_col() /= state.masses;

    const arma::mat change_list =
        arma::join_cols(p_submatrix,
                        details::force(potential,
                                       state.points.rows(0, state.dim() - 1)));

    return State(change_list, state.weights, state.masses);
  }

};

} // namespace heller
}

#endif //METHODS_HELLER_H


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <time.h>
#include <gsl/gsl_math.h>

#include "cosmocalc.h"

void test_nonlinear_corrfunc(void)
{
  double lnrmin = log(1e-5);
  double lnrmax = log(3e2);
  int N = 10000;
  double r,dlnr = (lnrmax - lnrmin)/N;
  int i;
  FILE *fp;
  double a;

  a = 1.0;
  fp = fopen("xinltest.txt","w");
  for(i=0;i<N;++i)
    {
      r = exp(dlnr*i + lnrmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",r,nonlinear_corrfunc(r,a),linear_corrfunc(r,a));
    }
  fclose(fp);
}

void test_nonlinear_powspec(void)
{
  double lnkmin = log(1e-4);
  double lnkmax = log(1e2);
  double dlnk,k;
  int N = 10000,i;
  FILE *fp;
  double a = 1.0;

  dlnk = (lnkmax - lnkmin)/N;

  a = 1.0;
  fp = fopen("pknltest.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);

  a = 1.0/(1.0 + 0.5);
  fp = fopen("pknltest1.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);

  a = 1.0/(1.0 + 1.0);
  fp = fopen("pknltest2.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);

  a = 1.0/(1.0 + 2.00);
  fp = fopen("pknltest3.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);

  a = 1.0/(1.0 + 3.00);
  fp = fopen("pknltest4.txt","w");
  for(i=0;i<N;++i)
    {
      k = exp(dlnk*i + lnkmin);
      fprintf(fp,"%.20e\t%.20e\t%.20e\n",k,nonlinear_powspec(k,a),linear_powspec(k,a));
    }
  fclose(fp);
}

void test_linxi_fftlog(void)
{
  double rmin;
  long i,N = 2048;
  double dlnr;
  FILE *fp;
  double r;
  double a = 1.0;

  double *pk,*xi;
  double k0,L,q,r0,k;
  double kmin = 1e-7;
  double kmax = 1e7;
  double dlnk = log(kmax/kmin)/N;

  L = log(kmax/kmin);
  k0 = exp(log(kmin) + L/2.0);
  q = 0.0;

  pk = (double*)malloc(sizeof(double)*N);
  assert(pk != NULL);
  xi = (double*)malloc(sizeof(double)*N);
  assert(xi != NULL);

  for(i=0;i<N;++i)
    {
      k = exp(i*dlnk + log(kmin));
      pk[i] = linear_powspec_exact(k,a);
    }

  r0 = 1.0;
  compute_discrete_spherical_fft(pk,N,k0,L,q,xi,&r0);

  if(0)
    {
      for(i=0;i<N;++i)
	xi[i] = xi[i]/pow(2.0*M_PI,3.0);
      compute_discrete_spherical_fft(xi,N,r0,L,q,pk,&k0);

      kmin = exp(log(k0)-L/2.0);
      //fprintf(stderr,"k0 = %e, kmin = %e, L = %f, N = %ld\n",k0,kmin,L,N);
      fp = fopen("linxitest.txt","w");
      for(i=0;i<N;++i)
	{
	  k = exp(i*dlnk + log(kmin));
	  //fprintf(stderr,"k = %e, pk in,out = %e|%e\n",k,linear_powspec(k,a),pk[i]);
	  fprintf(fp,"%.20e \t %.20e \t %.20e \n",k,pk[i],linear_powspec_exact(k,a));
	}
      fclose(fp);

      return;
    }

  rmin = exp(log(r0)-L/2.0);
  dlnr = dlnk;
  fp = fopen("linxitest.txt","w");
  for(i=0;i<N;++i)
    {
      r = exp(i*dlnr + log(rmin));
      xi[i] = xi[i]/pow(2.0*M_PI,3.0);
      //fprintf(stderr,"%.20e \t %.20e \t %.20e \n",r,xi[i],linear_corrfunc_exact(r,a));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",r,xi[i],linear_corrfunc_exact(r,a));
    }
  fclose(fp);
}

void test_linxi(void)
{
  double rmin=1e-9;
  double rmax=1e5;
  long i,Ntest = 1000;
  double dlnr = log(rmax/rmin)/Ntest;
  FILE *fp;
  double r;
  double a = 1.0;

  fp = fopen("linxitest.txt","w");
  for(i=0;i<Ntest;++i)
    {
      r = exp(i*dlnr + log(rmin));
      //fprintf(stderr,"%.20e \t %.20e \t %.20e \n",r,linear_corrfunc_exact(r,a),linear_corrfunc_exact(r,a));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",r,linear_corrfunc(r,a),linear_corrfunc_exact(r,a));
    }
  fclose(fp);
}

void test_biasfunc(void)
{
  FILE *fp;
  double m,lgm;
  double mmin = 6.0;
  double mmax = 16.0;
  double dm = 0.01;
  double a = 1.0;

  fp = fopen("btest.txt","w");
  lgm = mmax;
  while(lgm >= mmin)
    {
      m = pow(10.0,lgm);
      fprintf(fp,"%.20e \t %.20e \n",1.686/sigmaMtophat(m,a),bias_function(m,a));
      lgm -= dm;
    }
  fclose(fp);

}

void test_massfunc(void)
{
  FILE *fp;
  double m,lgm;
  double mmin = 10.0;
  double mmax = 16.0;
  double dm = 0.01;
  double a = 1.0;

  fp = fopen("mftest.txt","w");
  lgm = mmax;
  while(lgm >= mmin)
    {
      m = pow(10.0,lgm);
      fprintf(fp,"%.20e \t %.20e \n",m,mass_function(m,a)*m*m/RHO_CRIT/cosmoData.OmegaM);
      lgm -= dm;
    }
  fclose(fp);
}

void test_peakheight(void)
{
  FILE *fp;
  double r;
  double rmin = 1e-40;
  double rmax = 1e3;
  double dr = 0.99;
  double a = 1.0;
  double numin = 0.15;
  double numax = 6.0;
  double dnu = 0.99;
  double nu;

  double kmin=1e-10;
  double kmax=1e10;
  long i,Ntest = 1000;
  double dlnk = log(kmax/kmin)/Ntest;
  double k,p = 1e-6;

  fp = fopen("phtest_integ.txt","w");
  for(i=0;i<Ntest;++i)
    {
      k = (i*dlnk + log(kmin));
      fprintf(fp,"%.20e \t %.20e \n",exp(k),tophatradnorm_linear_powspec_exact_nonorm_lnk_integ_funct_I0(k,&p));
    }
  fclose(fp);

  fp = fopen("phtest.txt","w");
  r = rmax;
  while(r >= rmin)
    {
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",r,sigmaRtophat(r,a),sigmaRtophat_exact(r,a));
      r *= dr;
    }
  fclose(fp);

  fp = fopen("phtestrev.txt","w");
  nu = numax;
  while(nu >= numin)
    {
      fprintf(fp,"%.20e \t %.20e \n",inverse_nuRtophat(nu,a),DELTAC/nu);
      nu *= dnu;
    }
  fclose(fp);
}

void test_gf(void)
{
  FILE *fp;
  double a;
  double amin = 0.1;
  double amax = 1.0;
  double da = 0.0001;

  fp = fopen("gftest.txt","w");
  a = amax;
  while(a >= amin)
    {
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",a,growth_function(a),growth_function_exact(a));
      a -= da;
    }
  fclose(fp);
}

void test_linpk(void)
{
  double kmin=1e-8;
  double kmax=1e40;
  long i,Ntest = 10000;
  double dlnk = log(kmax/kmin)/Ntest;
  FILE *fp;
  double k;
  double a = 1.0;

  fp = fopen("pktest.txt","w");
  for(i=0;i<Ntest;++i)
    {
      k = exp(i*dlnk + log(kmin));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",k,linear_powspec(k,a),linear_powspec_exact(k,a));
    }
  fclose(fp);

}

void test_transfunct(void)
{
  double kmin=1e-8;
  double kmax=1e30;
  long i,Ntest = 10000;
  double dlnk = log(kmax/kmin)/Ntest;
  FILE *fp;
  double k;

  fp = fopen("tftest.txt","w");
  for(i=0;i<Ntest;++i)
    {
      k = exp(i*dlnk + log(kmin));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",k,transfer_function(k),transfunct_eh98(k));
    }
  fclose(fp);

  cosmoData.OmegaM = 0.2;
  cosmoData.h = 0.5;
  cosmoData.OmegaB = cosmoData.OmegaM*0.5;
  cosmoData.cosmoNum = 2;

  fp = fopen("tftest_highb.txt","w");
  for(i=0;i<Ntest;++i)
    {
      k = exp(i*dlnk + log(kmin));
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",k,transfer_function(k),transfunct_eh98(k));
    }
  fclose(fp);
}

void test_distances(void)
{
  FILE *fp;

  double amin = 0.1;
  double amax = 1.0;
  double da = 0.0001;
  double dmin = 0.0;
  double dmax = 3000.0;
  double dd = 1.0;
  double a,d;

  fp = fopen("disttest_a2d.txt","w");
  a = amax;
  while(a >= amin)
    {
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",a,comvdist(a),comvdist_exact(a));
      a -= da;
    }
  fclose(fp);

  fp = fopen("disttest_d2a.txt","w");
  d = dmin;
  while(d <= dmax)
    {
      a = acomvdist(d);
      fprintf(fp,"%.20e \t %.20e \t %.20e \n",d,a,comvdist_exact(a));
      d += dd;
    }
  fclose(fp);
}


/////////////////
//example25.5.c
/////////////////
#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include <math.h>

typedef struct{
  double g;//gravity
  double M;//mass of the car
  double m;//mass of the load
  double r;//length of the rope
  double C;//damping coefficient

  double F;//input force proportional to torque
  double T;//tension to the rope

  double x,x0;//x position of the load
  double y,y0;//y position of the load
  double X;//X position of the car
  double a;//angle (theta)

  double dx,dx0;
  double dy,dy0;
  double dX,dX0;
  double dr;
  double da,da0;

  double ddx;
  double ddy;
  double ddX;
  double ddr;
  double dda;
  double dXmax;
  double ddXmax;
  double Fmax;//Force

  double h;
  int nh;
  double _h;
  int _dim;
  double *_y;
  double *_y_err;
  double *_dydt_in;
  double *_dydt_out;
  const gsl_odeiv_step_type *_T;
  gsl_odeiv_step *_s;
  gsl_odeiv_system _sys;
  double _t;
} CRANE;

#define dim_crane 4
int cranefunc (double t, const double y[], double f[], void *params)
{
  CRANE *c = (CRANE *)params;
  //y[0]=crane.p, y[1]=crane.da, y[2]=crane.X, y[3]=crane.dX
  f[0] = y[1];
  f[1] = (-(2*c->dr+c->C)*y[1] -c->ddX*cos(y[0]) -c->g*sin(y[0]))/c->r;
  //  f[1] = (-2*c->dr -c->ddX*cos(y[0]) -c->g*sin(y[0]))/c->r;
  f[2] = y[3];
  //  f[3] = c->ddX;
  f[3] = (c->F+c->T*sin(y[0]))/c->M;
  return GSL_SUCCESS;
}
////////////////
#define square(x) ((x)*(x))
CRANE crane;
int initialize()
{

  crane.Fmax=20.0;//check//  crane.Fmax=30;//check
  //  crane.dXmax=1.0;
  crane.dXmax=1.0;
  crane.ddXmax=0.2;
  crane.g=9.8;
  crane.M=100;
  crane.m=20;//10;//10;//20
  crane.r=5;//5;
  crane.C=1;//5;
  crane.dr=crane.ddr=0;
  crane.x0=0;
  crane.y0=crane.r;
  crane.dX0=crane.dx0=crane.dy0=crane.da0=0;
#ifdef CRANESUB
  //  crane.Fmax=_crane_Fmax;//check//  crane.Fmax=30;//check
  crane.m=_crane_m;
  crane.C=_crane_C;
  crane.r=_crane_r;
  crane.h=AP_tS;
  crane.dXmax=_crane_dXmax;
  if(_AP_umax>0) AP_u_max=crane.Fmax=_AP_umax;
  else AP_u_max=crane.Fmax;
  AP_u_min=-AP_u_max;
  //  starttime=0;
  //  totaltime=100;//  totaltime=40;
  //  totaltime=50;//  totaltime=40;
  //  int kmax=totaltime/AP_tS;
  //  _rr=(double*)malloc(kmax*sizeof(double));
  rr=AP_r=_AP_r;//10
  //  int k;for(k=0;k<kmax;k++) _rr[k]=AP_r;//10;
  rr_kyoyou=_rr_kyoyou;
  //  p=(double *)malloc(buffsize*sizeof(double));
  C_MODE=11;
  //  iteration=_iteration;
  //  fprintf(stdout,"?????????iteration=%d\n",_iteration);
#else
  crane.h=0.01;
#endif
  crane.nh=10;
  crane.F=crane.a=crane.da=crane.dda=0;
  crane.T=crane.m*crane.g;
  crane._h=crane.h/crane.nh;//0.001
  crane._dim=4;
  if(crane._y==NULL){
    crane._y=(double*)malloc(crane._dim*sizeof(double));
    crane._y_err=(double*)malloc(crane._dim*sizeof(double));
    crane._dydt_in=(double*)malloc(crane._dim*sizeof(double));
    crane._dydt_out=(double*)malloc(crane._dim*sizeof(double));
  }
  crane._T= gsl_odeiv_step_rk4;
  crane._s= gsl_odeiv_step_alloc (crane._T, crane._dim);
  crane._sys= (gsl_odeiv_system){cranefunc, NULL, crane._dim, &crane};
  crane._t=crane._y[0]=crane._y[1]=crane._y[2]=crane._y[3]=0;
  int i;for(i=0;i<crane._dim;i++) crane._dydt_in[i]=0;
  GSL_ODEIV_FN_EVAL(&(crane._sys), crane._t, crane._y, crane._dydt_in);
  return(0);
}
#ifndef CRANESUB
char *fn="crane2io.dat";
FILE *fp;
#endif//#ifndef CRANESUB

double plant(double uu)
{
  //  crane.F=uu;
  if(uu>crane.Fmax) uu=crane.Fmax;
  else if(uu<-crane.Fmax) uu=-crane.Fmax;
  int n;
  for(n=0;n<crane.nh;n++){
    crane.F=uu;//
    if(crane.dX>=crane.dXmax && crane.F>0) crane.F=0;
    else if(crane.dX<=-crane.dXmax && crane.F<0) crane.F=0;
    int status = gsl_odeiv_step_apply (crane._s, crane._t, crane._h,
				       crane._y, crane._y_err,
				       crane._dydt_in,
				       crane._dydt_out,
				       &crane._sys);
    if (status != GSL_SUCCESS)   break;

    int i;for(i=0;i<dim_crane;i++) crane._dydt_in[i]=crane._dydt_out[i];
    crane._t+=crane._h;

    crane.a  =crane._y[0];
    crane.da =crane._y[1];
    crane.X  =crane._y[2];
    crane.dX =crane._y[3];

    crane.x  =crane.X+crane.r * sin(crane.a);
    crane.y  =crane.r*cos(crane.a);
    crane.dx=(crane.x-crane.x0)/crane._h;
    crane.dy=(crane.y-crane.y0)/crane._h;
    crane.ddx=(crane.dx-crane.dx0)/crane._h;
    crane.ddy=(crane.dy-crane.dy0)/crane._h;
    crane.T=crane.m*sqrt(square(crane.ddx)+square(crane.ddy-crane.g));
    crane.ddX=(crane.dX-crane.dX0)/crane._h;
    //    crane.F=crane.M*crane.ddX-crane.T*sin(crane.a);
    crane.dda=(crane.da-crane.da0)/crane._h;
    /*    crane.dda=(-2*crane.dr*crane.da-crane.g*sin(crane.a)-ddX[n]*cos(crane.a))/crane.r; */
    crane.x0=crane.x;
    crane.y0=crane.y;
    crane.dx0=crane.dx;
    crane.dy0=crane.dy;
    crane.da0=crane.da;
    crane.dX0=crane.dX;
  }
#ifndef CRANESUB
  fprintf(fp,"%.7e %.7e %.7e", crane._t,crane._y[0],crane._y[1]);//crane.a,crane.da
  fprintf(fp," %.7e %.7e %.7e" ,crane.X,crane.dX,crane.ddX);
  fprintf(fp," %.7e %.7e %.7e" ,crane.x,crane.dx,crane.ddx);
  fprintf(fp," %.7e %.7e %.7e" ,crane.y,crane.dy,crane.ddy);
  fprintf(fp," %.7e %.7e" ,crane.T,crane.F);
  fprintf(fp,"\n");
#endif
  return(crane.x);
  //  return(crane.X);
}

#ifndef CRANESUB
int main (void)
{
  //  CRANE crane;
  crane.g=9.8;
  crane.M=100;
  crane.m=20;
  crane.r=5;
  crane.dr=crane.ddr=0;

  ////////////////////////////////////////////////////
  /// method 2                                     ///
  /// input F                                     ///
  /// output p,x,y,X,...                          ///
  ////////////////////////////////////////////////////
  double h;//,hh=0.1;
  //result for different h
  /*gnuplot
   set style data lines;n=1;
   n=n+1;plot "crane1io.dat" using 1:n, "crane2io.dat" using 1:n;print "n=",n
   #strange at n=9(ddx),12(ddy),13(T)
  */
  ////////////////////////////////////////////////////
  ///   read F                                    ///
  ////////////////////////////////////////////////////
  char *fn1="crane1io.dat";
  FILE *fp1=fopen(fn1,"r");
  int n4;
#define buffsize 512
  char buff[buffsize];
  int n0;
  fgets(buff,buffsize,fp1);
  sscanf(buff,"#%lf%d%d%lf%lf%lf%lf",&h,&n0,&n4,&crane.M,&crane.m,&crane.r,&crane.C);
  crane.h=h;
  double t4=n4*h;
  double *F  =(double*)malloc(sizeof(double)*(n4+1));
  //  int M=n4+1;double *F  =(double*)malloc(sizeof(double)*M);
  //  double *ddX=(double*)malloc(sizeof(double)*M);

  int n;
  double t;
  //  double dx0=0,dy0=0,dX0=0,da0=0;
  //  double x0=0,y0=crane.r;

  fgets(buff,buffsize,fp1);
  for(n=0;n<n4;n++){//
    double _x1,_x2,_x3,_x4,_x5,_x6,_x7,_x8,_x9,_x10,_x11,_x12,_x13,_x14;
    fgets(buff,buffsize,fp1);
    sscanf(buff,"%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf",
	   &_x1,&_x2,&_x3,&_x4,&_x5,&_x6,&_x7,&_x8,&_x9,&_x10,&_x11,&_x12,&_x13,&_x14);
    F[n]=_x14;
  }
  ////////////////////////////////////////////////////
  ///   Solve by the Runge Kutta Method of GSL     ///
  ////////////////////////////////////////////////////
  initialize();
  fp=fopen(fn,"w");
  for(t=0;t<t4;t+=crane.h){
    n=t/crane.h;
    plant(F[n]);
  }
  fclose(fp);
  fprintf(stdout,"Results are stored in '%s'.\n",fn);

  gsl_odeiv_step_free (crane._s);
  return 0;
}
#endif


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_fit.h>

#include "cosmocalc.h"

static double linear_corrfunc_integ_funct(double k, void *p);

static double linear_corrfunc_integ_funct(double k, void *p)
{
  double r = ((double*)p)[0];
  double a = ((double*)p)[1];
  return linear_powspec(k,a)*k/r;
}

double linear_corrfunc_exact(double r, double a)
{
  double I0;
  double abserr,p[2];
  gsl_integration_workspace *workspace;
  gsl_function F;
  gsl_integration_qawo_table *wf;
  gsl_integration_workspace *cycle_workspace;

#define WORKSPACE_NUM 70
#define ABSERR 1e-3
  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  cycle_workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
  wf = gsl_integration_qawo_table_alloc(r,1.0,GSL_INTEG_SINE,(size_t) WORKSPACE_NUM);

  F.function = &linear_corrfunc_integ_funct;
  p[0] = r;
  p[1] = a;
  F.params = p;
  gsl_integration_qawf(&F,1e-8,ABSERR,(size_t) WORKSPACE_NUM,workspace,cycle_workspace,wf,&I0,&abserr);

  gsl_integration_qawo_table_free(wf);
  gsl_integration_workspace_free(cycle_workspace);
  gsl_integration_workspace_free(workspace);
#undef ABSERR
#undef WORKSPACE_NUM

  return I0/2.0/M_PI/M_PI;
}

double linear_corrfunc(double r, double a)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static gsl_spline *cosmocalc_linear_corrfunc_spline = NULL;
  static gsl_interp_accel *cosmocalc_linear_corrfunc_acc = NULL;

  double linear_corrfunc_table[COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH];
  double r_table[COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH];
  long i;
  double gf;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      for(i=0;i<COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH;++i)
	{
	  r_table[i] = log(CF_R_MAX/CF_R_MIN)/(COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH-1.0)*((double) i) + log(CF_R_MIN);
	  linear_corrfunc_table[i] = linear_corrfunc_exact(exp(r_table[i]),1.0);
	}

      //init the spline and accelerators
      if(cosmocalc_linear_corrfunc_spline != NULL)
	gsl_spline_free(cosmocalc_linear_corrfunc_spline);
      cosmocalc_linear_corrfunc_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_linear_corrfunc_spline,r_table,linear_corrfunc_table,(size_t) (COSMOCALC_LINEAR_CORRFUNC_TABLE_LENGTH));
      if(cosmocalc_linear_corrfunc_acc != NULL)
	gsl_interp_accel_reset(cosmocalc_linear_corrfunc_acc);
      else
	cosmocalc_linear_corrfunc_acc = gsl_interp_accel_alloc();
    }

  gf = growth_function(a);

  if(r < CF_R_MIN)
    return 0.0;
  else if(r < CF_R_MAX)
    return gsl_spline_eval(cosmocalc_linear_corrfunc_spline,log(r),cosmocalc_linear_corrfunc_acc)*gf*gf;
  else
    return 0.0;
}