/*
* -----------------------------------------------------------------
*  ode_lib.c
*  ODE Solver Library
*  Version: 2.0
*  Last Update: Oct 1, 2019
*
*  This is the implementation of a computational library with
*  numerical tools for Ordinary Differential Equations (ODE).
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2019 by Americo Barbosa da Cunha Junior
* -----------------------------------------------------------------
*/

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

#include <cvode/cvode.h>               /* prototypes for CVODE functions and const */
#include <nvector/nvector_serial.h>    /* access to serial NVector                 */
#include <sunmatrix/sunmatrix_dense.h> /* access to dense SUNMatrix                */
#include <sunlinsol/sunlinsol_dense.h> /* access to dense SUNLinearSolver          */
#include <sundials/sundials_types.h>   /* defs. of realtype, sunindextype          */

#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>

#include "../include/ode_lib.h"

/*
*------------------------------------------------------------
*   uround
*
*   This function computes the machine unit roundoff
*   defined as the smallest u such that 1.0 + u > 1.0
*
*   Output:
*   uround - machine unit roundoff
*
*   last update: May 20, 2009
*------------------------------------------------------------
*/

double uround(void)
{
    double u = 1.0;
    double comp = 0.0;

    while(comp != 1.0)
    {
	u *= 0.5;
	comp = 1.0 + u;
    }

    return 2.0*u;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   wnorm
*
*   This function computes v1 and v2 vector weight norm
*   defined as
*   wnorm = sqrt( (1/n)*(sum (v1[i]*v2[i])^2) )
*
*   Input:
*   n     - vectors dimension
*   v     - vector 1
*   v     - vector 2
*
*   Output:
*   wnorm - weight norm
*
*   last update: May 20, 2009
*------------------------------------------------------------
*/

double wnorm(int n,double *v1,double *v2)
{
    int i;
    double wnorm = 0.0;

    for ( i = 0; i < n; i++ )
	wnorm += v1[i]*v1[i]*v2[i]*v2[i];

    return sqrt( wnorm / (double)n );
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   ewset
*
*   This function creates a error weight vector
*   defined as ewv[i] = rtol*abs(v[i]) + atol
*
*   Input:
*   n    - vector dimension
*   v    - vector
*   atol - absolute tolerance
*   rtol - relative tolerance
*
*   Output:
*   ewv  - error weight vector
*
*   last update: May 22, 2009
*------------------------------------------------------------
*/

void ewtset(int n,double *v,double atol,double rtol,double *ewt)
{
    int i;

    for ( i = 0; i < n; i++ )
        ewt[i] =  rtol*fabs(v[i]) + atol;

    return;
}
/*------------------------------------------------------------*/

/*------------------------------------------------------------
*   jacobian
*
*   This function computes the jacobian matrix of ydot = f(y,t)
*   defiend as  Jij = dfi/dyj
*
*   Input:
*   n      - vector dimension
*   f_data - pointer to external data
*   Fy     - reaction mapping
*   y      - composition vector
*   t      - time step
*   atol   - absolute tolerance
*   rtol   - relative tolerance
*
*   Output:
*   J       - jacobian matrix
*   success or error
*
*   last update: Oct 7, 2009
------------------------------------------------------------*/

int jacobian(CVRhsFn f,void *f_data,gsl_vector *Fy,gsl_vector *y,
                    double t,double atol,double rtol,gsl_matrix *J)
{
    unsigned int i, j, N1, N2;
    double roundoff, min_inc_mult;
    double fnorm, minInc, inc, inc_inv, yjsaved, srur;
    double *yd_data  = NULL;
    double *Fyd_data = NULL;
    N_Vector yd      = NULL;
    N_Vector Fyd     = NULL;
    gsl_vector *ewt  = NULL;

    min_inc_mult = 1.0e3;

    /* vectors dimensions */
    N1 = y->size;
    N2 = Fy->size;

    /* error weight vector */
    ewt = gsl_vector_calloc(N1);

    /* computing the unit roundoff */
    roundoff = uround();

    /* memory allocation and startup of yd and Fyd */
    yd  = N_VNew_Serial(N1);
    Fyd = N_VNew_Serial(N2);
    if ( yd == NULL || Fyd == NULL )
        return GSL_ENOMEM;

    /* obtaining yd and Fyd compoments  */
    yd_data  = NV_DATA_S(yd);
    Fyd_data = NV_DATA_S(Fyd);

    /* setting yd equal y vector */
    for ( i = 0; i < N1; i++ )
        yd_data[i] = y->data[i];

    /* setting Fyd equal the null vector */
    for ( i = 0; i < N2; i++ )
        Fyd_data[i] = 0.0;

    /* defing error weight vector */
    ewtset(N1,y->data,atol,rtol,ewt->data);

    /* computing weight norm */
    fnorm = wnorm(N2,Fy->data,ewt->data);

    /* square root of the machine roundoff */
    srur = sqrt(roundoff);

    /* computing disturbance parameter */
    minInc = (fnorm != 0.0) ?
	    (min_inc_mult*fabs(t)*roundoff*((double)N1)*fnorm) : 1.0;

    for ( j = 0; j < N1; j++ )
    {
        /* saving y[j] value */
        yjsaved = yd_data[j];

        /* disturbance */
        inc = GSL_MAX(srur*fabs(yjsaved),minInc/ewt->data[j]);

    	/* disturbing y */
        yd_data[j] += inc;

        /* computing Fy disturbed */
        f(t,yd,Fyd,f_data);

	/* restoring yd[j] original value */
        yd_data[j] = yjsaved;

	/* computing the step */
        inc_inv = 1.0/inc;

        /* computing jacobian matrix column j */
    	for ( i = 0; i < N2; i++ )
            J->data[i*J->tda+j] = inc_inv*(Fyd_data[i] - Fy->data[i]);
    }

    /* releasing allocated memory */
    N_VDestroy_Serial(yd);
    N_VDestroy_Serial(Fyd);
    gsl_vector_free(ewt);
    yd       = NULL;
    Fyd      = NULL;
    yd_data  = NULL;
    Fyd_data = NULL;
    ewt      = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*------------------------------------------------------------
*   gradient
*
*   This function computes the gradient matrix of R(phi)
*   defiend as: Aij = d R_i/d phi_j
*
*   Input:
*   cvode_mem - ODE solver workspace
*   t0        - initial time
*   delta_t   - time step
*   phi     - composition vector
*   Rphi    - reac tion mapping
*
*   Output:
*   A       - gradient matrix
*   success or error
*
*   last update: Oct 1, 2019
------------------------------------------------------------*/

int gradient(void *cvode_mem,double t0,double delta_t,
             gsl_vector *phi,gsl_vector *Rphi,gsl_matrix *A)
{
    unsigned int i, j, flag;
    double inc, inc_inv, phijsaved, srur;
    gsl_vector *Rphid = NULL;

    /* memory allocation for Rphid */
    Rphid = gsl_vector_calloc(Rphi->size);

    /* square root of 1.0e6 times the machine roundoff */
    srur = sqrt(1.0e6*DBL_EPSILON);

    for ( j = 0; j < phi->size; j++ )
    {
        /* save phi_j value */
        phijsaved = phi->data[j];

        /* disturbance */
        inc = phijsaved*srur + srur;

    	/* disturbe phi_j */
        phi->data[j] += inc;

        /* compute R(phi) disturbed */
	flag = odesolver_reinit(t0,phi,cvode_mem);
        if ( flag != GSL_SUCCESS )
            return flag;

	flag = odesolver(cvode_mem,delta_t,Rphid);
        if ( flag != GSL_SUCCESS )
            return flag;

	/* restore phi_j original value */
        phi->data[j] = phijsaved;

	/* compute the step */
        inc_inv = 1.0/inc;

        /* compute gradient matrix column j */
    	for ( i = 0; i < Rphi->size; i++ )
            A->data[i*A->tda+j] = inc_inv*(Rphid->data[i] - Rphi->data[i]);
    }

    /* release allocated memory */
    gsl_vector_free(Rphid);
    Rphid = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   linear_approx
*
*   This function computes the linear approximation
*   for a vector function F(x) near the point x0.
*
*   F(x) = F(x0) + DF(x0)*(x-x0)
*
*   Input:
*   x    - vector x
*   x0   - vector x0
*   Fx0  - function at x0
*   DFx0 - jacobian matrix at x0
*
*   Output:
*   Fx   - linear approximation for F(x)
*
*   last update: Jun 9, 2009
*------------------------------------------------------------
*/

void linear_approx(gsl_vector *x,gsl_vector *x0,
                gsl_vector *Fx0,gsl_matrix *DFx0,gsl_vector *Fx)
{
    gsl_vector *aux = NULL;

    /* memory allocation */
    aux = gsl_vector_alloc(x0->size);

    /* aux := x */
    gsl_vector_memcpy(aux,x);

    /* aux := x - x0 */
    gsl_vector_sub(aux,x0);

    /* Fx := F(x0) */
    gsl_vector_memcpy(Fx,Fx0);

    /* Fx := F(x0) + DF(x0)*(x-x0) */
    gsl_blas_dgemv(CblasNoTrans,1.0,DFx0,aux,1.0,Fx);

    /* releasing allocated memory */
    gsl_vector_free(aux);
    aux = NULL;

    return;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver_init
*
*   This function initiates the ODE solver workspace.
*
*   Input:
*   f         - right hand side function
*   f_data    - right hand side function data
*   t0        - initial time
*   mxsteps   - max # of solver iterations
*   atol      - absolute tolerance
*   rtol      - relative tolerance
*   x         - initial condition
*   cvode_mem - ODE solver workspace
*
*   Output:
*   success or error
*
*   last update: Oct 1, 2019
*------------------------------------------------------------
*/

int odesolver_init(CVRhsFn f,void *f_data,double t0,int mxsteps,
		   double atol,double rtol,gsl_vector *x,void *cvode_mem)
{
    int         maxnef = 100;           /* max. # of error test failures  */
    int           flag = CV_SUCCESS;    /* return value flag              */
    N_Vector        y0 = NULL;          /* initial condition vector       */
    SUNMatrix        A = NULL;          /* Matrix for linear solver use   */
    SUNLinearSolver LS = NULL;          /* dense linear solver object     */

    /* memory allocation for initial condition y0 */
    y0 = N_VMake_Serial(x->size,x->data);
    if ( y0 == NULL )
	return GSL_ENOMEM;

    /* memory allocation for CVODE workspace */
    flag = CVodeInit(cvode_mem,f,t0,y0);
    if( flag != CV_SUCCESS )
    	return GSL_ENOMEM;

    /* specifies scalar absolute and relative tolerances */
    flag = CVodeSStolerances(cvode_mem,rtol,atol);
    if( flag != CV_SUCCESS )
    	return flag;

    /* set user data for right hand side function */
    flag = CVodeSetUserData(cvode_mem,f_data);
    if( flag != CV_SUCCESS )
        return flag;

    /* Create dense SUNMatrix for use in linear solver */
    A = SUNDenseMatrix(x->size,x->size);
    if ( A == NULL )
	return GSL_ENOMEM;

    /* Create dense SUNLinearSolver object for use by CVode */
    LS = SUNLinSol_Dense(y0, A);
    if ( LS == NULL )
	return GSL_ENOMEM;

    /* Call CVodeSetLinearSolver to attach the matrix and linear solver to CVode */
    flag = CVodeSetLinearSolver(cvode_mem, LS, A);
    if( flag != CVLS_SUCCESS )
    	return flag;

    /* set solver max # of steps */
    flag = CVodeSetMaxNumSteps(cvode_mem,mxsteps);
    if( flag != CV_SUCCESS )
        return flag;

    /* set max # of error test failures permitted */
    flag = CVodeSetMaxErrTestFails(cvode_mem,maxnef);
    if( flag != CV_SUCCESS )
        return flag;

    /* release allocated memory for y0 */
    N_VDestroy_Serial(y0);
    y0 = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver_reinit
*
*   This function reinitiates the ODE solver workspace.
*
*   Input:
*   f         - right hand side function
*   f_data    - right hand side function data
*   t0        - initial time
*   x         - initial condition
*   cvode_mem - ODE solver workspace
*
*   Output:
*   success or error
*
*   last update: Oct 1, 2019
*------------------------------------------------------------
*/

int odesolver_reinit(double t0,gsl_vector *x,void *cvode_mem)
{
    int flag    = CV_SUCCESS;  /* return value flag              */
    N_Vector y0 = NULL;        /* initial condition vector       */

    /* memory allocation for y0 */
    y0 = N_VMake_Serial(x->size,x->data);
    if ( y0 == NULL )
	return GSL_ENOMEM;

    /* restart CVODE workspace */
    flag = CVodeReInit(cvode_mem,t0,y0);
    if( flag != CV_SUCCESS )
    	return flag;

    /* release allocated memory */
    N_VDestroy_Serial(y0);
    y0 = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver
*
*   This function performs the direct integration of the
*   governing equations.
*
*   Input:
*   cvode_mem - ODE solver workspace
*   tf   - final time
*   Fx   - solution vector
*
*   Output:
*   success or error
*
*   last update: Oct 31, 2009
*------------------------------------------------------------
*/

int odesolver(void *cvode_mem,double tf,gsl_vector *Fx)
{
    double t;
    int flag   = CV_SUCCESS;
    N_Vector y = NULL;

    /* memory allocation and startup of y */
    y = N_VMake_Serial(Fx->size,Fx->data);
    if ( y == NULL )
	return GSL_ENOMEM;

    /* calling CVode solver */
    flag = CVode(cvode_mem,tf,y,&t,CV_NORMAL);
    if( flag != CV_SUCCESS )
        return flag;

    /* releasing allocated memory */
    N_VDestroy_Serial(y);
    y = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/


// Copyright (c) 2005  Stanford University (USA).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public License as
// published by the Free Software Foundation; either version 3 of the License,
// or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
//
// Author(s)     : Daniel Russel <drussel@alumni.princeton.edu>

#ifndef CGAL_POLYNOMIAL_INTERNAL_GSL_NUMERIC_SOLVER_H
#define CGAL_POLYNOMIAL_INTERNAL_GSL_NUMERIC_SOLVER_H
#include <CGAL/Polynomial/basic.h>
#include <CGAL/Polynomial/internal/numeric_solvers_support.h>
#include <vector>

#ifdef CGAL_POLYNOMIAL_USE_GSL

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

namespace CGAL { namespace POLYNOMIAL { namespace internal {

/*template <bool CLEAN>
inline double gsl_max_error()
{
    if (CLEAN) return .0000005;
    else return 0;
    }*/

template <bool CLEAN>
inline void gsl_compute_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots_)
{

  //double max_error=gsl_max_error<CLEAN>();

    gsl_poly_complex_workspace *workspace;
    int degree= end-begin-1;
    workspace= gsl_poly_complex_workspace_alloc(degree+1);

//! I can't use auto_ptr because it is an array
    double *roots= new double[2*degree];

/*for ( int i=0; i< 2*fn.degree(); ++i){
  roots[i]=0;
  }*/

    int ret = gsl_poly_complex_solve(begin, degree+1, workspace,
        roots);
    roots_.reserve(ret);

    if (ret!=GSL_SUCCESS) {
        std::cerr << "GSL solver did not converge on ";
        std::copy(begin, end, std::ostream_iterator<double>(std::cerr, " "));
        std::cerr << std::endl;
    }

    double last= -std::numeric_limits<double>::infinity();
    for ( int i=degree-1; i>=0; --i) {
        double r= roots[2*i];
        double c= roots[2*i+1];
        if (root_is_good(r, c, lb, ub)) {
            roots_.push_back(r);
        } else if (CLEAN && root_is_good(r,c, last, ub)) {
	  last= r;
	}
    }

    std::sort(roots_.begin(), roots_.end(), std::greater<double>() );
    delete roots;

    gsl_poly_complex_workspace_free(workspace);

    if (CLEAN) filter_solver_roots(begin,end, lb, ub, last, roots_);
    return;
}

template <bool CLEAN>
inline void gsl_compute_cubic_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots_)
{
    CGAL_Polynomial_precondition(begin[3] != 0);
    //double max_error=gsl_max_error<CLEAN>();

    double r[3];
    int num_roots= gsl_poly_solve_cubic(begin[2]/begin[3],
        begin[1]/begin[3],
        begin[0]/begin[3], &r[0],&r[1],&r[2]);
    roots_.reserve(num_roots);
    double last= -std::numeric_limits<double>::infinity();
// I want reverse sorted roots
    for (int i=num_roots-1; i>=0; --i) {
        if (r[i]>  lb && r[i] < ub) roots_.push_back(r[i]);
	else if (CLEAN && r[i] <lb && r[i] > last){
	  last= r[i];
	}
    }
    if (CLEAN) filter_solver_roots(begin, end, lb, ub, last, roots_);
}

inline void gsl_polynomial_compute_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots)
{
    int degree= end-begin-1;
    switch( degree) {
        case -1:
        case 0:
            break;
        case 1:
            compute_linear_roots(begin,end, lb, ub, roots);
            break;
        case 2:
            compute_quadratic_roots(begin, end, lb, ub, roots);
            break;
        case 3:
            gsl_compute_cubic_roots<false>(begin, end, lb, ub, roots);
            break;
        default:
            gsl_compute_roots<false>(begin, end, lb, ub, roots);
    }
}

inline void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
double lb, double ub,
std::vector<double> &roots)
{
    int degree= end-begin-1;
    switch( degree) {
        case -1:
        case 0:
            break;
        case 1:
            compute_linear_cleaned_roots(begin,end, lb, ub, roots);
            break;
        case 2:
            compute_quadratic_cleaned_roots(begin, end, lb, ub, roots);
            break;
        case 3:
            gsl_compute_cubic_roots<true>(begin, end, lb, ub, roots);
            break;
        default:
            gsl_compute_roots<true>(begin, end, lb, ub, roots);
    }
}

inline double gsl_evaluate_polynomial(const double *b, const double *e, double t)
{
    if (b==e) return 0;
/*double *d= new double[coefs.size()];
for (unsigned int i=0; i< coefs.size(); ++i){
  d[i]= coefs[i];
  }*/
    double v= gsl_poly_eval(b, e-b, t);
    return v;
}

/*

inline void gsl_polynomial_compute_roots(const double *begin, const double *end,
                  double lb, double ub,
                  std::vector<double> &roots){
 CGAL_assertion(0&& no_gsl_support_built);
}

inline void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
                  double lb, double ub,
                  std::vector<double> &roots){
CGAL_assertion(0&& no_gsl_support_built);
}

inline double gsl_evaluate_polynomial(const double *b, const double *e, double t) {
CGAL_assertion(0&& no_gsl_support_built);
return std::numeric_limits<double>::nan();
}
*/

/*// GSL
void gsl_polynomial_compute_roots(const double *begin, const double *end,
                  double lb, double ub, std::vector<double> &roots);

void gsl_polynomial_compute_cleaned_roots(const double *begin, const double *end,
double lb, double ub, std::vector<double> &roots);*/

struct GSL_numeric_solver
{
    void operator()(const double *begin, const double *end,
        double lb, double ub,
        std::vector<double> &roots) const
    {
        gsl_polynomial_compute_roots(begin, end, lb, ub, roots);
    }
};

struct GSL_cleaned_numeric_solver
{
    void operator()(const double *begin, const double *end,
        double lb, double ub,
        std::vector<double> &roots) const
    {
        gsl_polynomial_compute_cleaned_roots(begin, end, lb, ub, roots);
    }
};

} } } //namespace CGAL::POLYNOMIAL::internal

#endif // CGAL_POLYNOMIAL_USE_GSL

#endif


// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz, Conor Williams, Oliver Schonrock
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#pragma once

#include <units/bits/external/hacks.h>
#include <array>
#include <cassert>
#include <cmath>
#include <cstdint>
#include <numeric>
#include <tuple>
#include <type_traits>
#include <gsl/gsl-lite.hpp>

namespace units::detail {

template<typename T>
[[nodiscard]] constexpr T abs(T v) noexcept
{
  return v < 0 ? -v : v;
}

// the following functions enable gcd and related computations on ratios
// with exponents. They avoid overflow. Further information here:
// https://github.com/mpusz/units/issues/62#issuecomment-588152833

// Computes (a * b) mod m relies on unsigned integer arithmetic, should not
// overflow
[[nodiscard]] constexpr std::uint64_t mulmod(std::uint64_t a, std::uint64_t b, std::uint64_t m)
{
  std::uint64_t res = 0;

  if (b >= m) {
    if (m > UINT64_MAX / 2u) {
      b -= m;
    } else {
      b %= m;
    }
  }

  while (a != 0) {
    if (a & 1) {
      if (b >= m - res) {
        res -= m;
      }
      res += b;
    }
    a >>= 1;

    std::uint64_t temp_b = b;
    if (b >= m - b) {
      temp_b -= m;
    }
    b += temp_b;
  }

  return res;
}

// Calculates (a ^ e) mod m , should not overflow.
[[nodiscard]] constexpr std::uint64_t modpow(std::uint64_t a, std::uint64_t e, std::uint64_t m)
{
  a %= m;
  std::uint64_t result = 1;

  while (e > 0) {
    if (e & 1) {
      result = mulmod(result, a, m);
    }
    a = mulmod(a, a, m);
    e >>= 1;
  }
  return result;
}

// gcd(a * 10 ^ e, b), should not overflow
[[nodiscard]] constexpr std::intmax_t gcdpow(std::intmax_t a, std::intmax_t e, std::intmax_t b) noexcept
{
  assert(a > 0);
  assert(e >= 0);
  assert(b > 0);

  // gcd(i, j) = gcd(j, i mod j) for j != 0 Euclid;
  //
  // gcd(a 10^e, b) = gcd(b, a 10^e mod b)
  //
  // (a 10^e) mod b -> [ (a mod b) (10^e mod b) ] mod b

  return std::gcd(
      b, static_cast<std::intmax_t>(mulmod(static_cast<std::uint64_t>(a % b),
                                           modpow(10, static_cast<std::uint64_t>(e), static_cast<std::uint64_t>(b)),
                                           static_cast<std::uint64_t>(b))));
}

constexpr void cwap(std::intmax_t& lhs, std::intmax_t& rhs)
{
  std::intmax_t tmp = lhs;
  lhs = rhs;
  rhs = tmp;
}

// Computes the rational gcd of n1/d1 x 10^e1 and n2/d2 x 10^e2
[[nodiscard]] constexpr auto gcd_frac(std::intmax_t n1, std::intmax_t d1, std::intmax_t e1, std::intmax_t n2, std::intmax_t d2,
                                      std::intmax_t e2) noexcept
{
  // Short cut for equal ratios
  if (n1 == n2 && d1 == d2 && e1 == e2) {
    return std::array{n1, d1, e1};
  }

  if (e2 > e1) {
    detail::cwap(n1, n2);
    detail::cwap(d1, d2);
    detail::cwap(e1, e2);
  }

  std::intmax_t exp = e2;  // minimum

  // gcd(a/b,c/d) = gcd(a⋅d, c⋅b) / b⋅d

  assert(std::numeric_limits<std::intmax_t>::max() / n1 > d2);
  assert(std::numeric_limits<std::intmax_t>::max() / n2 > d1);

  std::intmax_t num = detail::gcdpow(n1 * d2, e1 - e2, n2 * d1);

  assert(std::numeric_limits<std::intmax_t>::max() / d1 > d2);

  std::intmax_t den = d1 * d2;

  std::intmax_t gcd = std::gcd(num, den);

  return std::array{num / gcd, den / gcd, exp};
}

constexpr void normalize(std::intmax_t& num, std::intmax_t& den, std::intmax_t& exp)
{
  if(num == 0) {
    den = 1;
    exp = 0;
    return;
  }

  std::intmax_t gcd = std::gcd(num, den);
  num = num * (den < 0 ? -1 : 1) / gcd;
  den = detail::abs(den) / gcd;

  while (num % 10 == 0) {
    num /= 10;
    ++exp;
  }
  while (den % 10 == 0) {
    den /= 10;
    --exp;
  }
}

[[nodiscard]] constexpr std::intmax_t safe_multiply(std::intmax_t lhs, std::intmax_t rhs)
{
  constexpr std::intmax_t c = std::uintmax_t(1) << (sizeof(std::intmax_t) * 4);

  const std::intmax_t a0 = detail::abs(lhs) % c;
  const std::intmax_t a1 = detail::abs(lhs) / c;
  const std::intmax_t b0 = detail::abs(rhs) % c;
  const std::intmax_t b1 = detail::abs(rhs) / c;

  gsl_Expects(a1 == 0 || b1 == 0);                               // overflow in multiplication
  gsl_Expects(a0 * b1 + b0 * a1 < (c >> 1));                     // overflow in multiplication
  gsl_Expects(b0 * a0 <= INTMAX_MAX);                            // overflow in multiplication
  gsl_Expects((a0 * b1 + b0 * a1) * c <= INTMAX_MAX - b0 * a0);  // overflow in multiplication

  return lhs * rhs;
}

}  // namespace units::detail


/* eigen/gsl_eigen.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#ifndef __GSL_EIGEN_H__
#define __GSL_EIGEN_H__

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

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct {
  size_t size;
  double * d;
  double * sd;
} gsl_eigen_symm_workspace;

gsl_eigen_symm_workspace * gsl_eigen_symm_alloc (const size_t n);
void gsl_eigen_symm_free (gsl_eigen_symm_workspace * w);
int gsl_eigen_symm (gsl_matrix * A, gsl_vector * eval, gsl_eigen_symm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * gc;
  double * gs;
} gsl_eigen_symmv_workspace;

gsl_eigen_symmv_workspace * gsl_eigen_symmv_alloc (const size_t n);
void gsl_eigen_symmv_free (gsl_eigen_symmv_workspace * w);
int gsl_eigen_symmv (gsl_matrix * A, gsl_vector * eval, gsl_matrix * evec, gsl_eigen_symmv_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
} gsl_eigen_herm_workspace;

gsl_eigen_herm_workspace * gsl_eigen_herm_alloc (const size_t n);
void gsl_eigen_herm_free (gsl_eigen_herm_workspace * w);
int gsl_eigen_herm (gsl_matrix_complex * A, gsl_vector * eval,
                         gsl_eigen_herm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
  double * gc;
  double * gs;
} gsl_eigen_hermv_workspace;

gsl_eigen_hermv_workspace * gsl_eigen_hermv_alloc (const size_t n);
void gsl_eigen_hermv_free (gsl_eigen_hermv_workspace * w);
int gsl_eigen_hermv (gsl_matrix_complex * A, gsl_vector * eval,
                           gsl_matrix_complex * evec,
                           gsl_eigen_hermv_workspace * w);

typedef enum {
  GSL_EIGEN_SORT_VAL_ASC,
  GSL_EIGEN_SORT_VAL_DESC,
  GSL_EIGEN_SORT_ABS_ASC,
  GSL_EIGEN_SORT_ABS_DESC
}
gsl_eigen_sort_t;

/* Sort eigensystem results based on eigenvalues.
 * Sorts in order of increasing value or increasing
 * absolute value.
 *
 * exceptions: GSL_EBADLEN
 */

int gsl_eigen_symmv_sort(gsl_vector * eval, gsl_matrix * evec,
                         gsl_eigen_sort_t sort_type);

int gsl_eigen_hermv_sort(gsl_vector * eval, gsl_matrix_complex * evec,
                         gsl_eigen_sort_t sort_type);

/* The following functions are obsolete: */

/* Eigensolve by Jacobi Method
 *
 * The data in the matrix input is destroyed.
 *
 * exceptions:
 */
int
gsl_eigen_jacobi(gsl_matrix * matrix,
                      gsl_vector * eval,
                      gsl_matrix * evec,
                      unsigned int max_rot,
                      unsigned int * nrot);

/* Invert by Jacobi Method
 *
 * exceptions:
 */
int
gsl_eigen_invert_jacobi(const gsl_matrix * matrix,
                             gsl_matrix * ainv,
                             unsigned int max_rot);

__END_DECLS

#endif /* __GSL_EIGEN_H__ */


/*
 * C version of Diffusive Nested Sampling (DNest4) by Brendon J. Brewer
 *
 * Yan-Rong Li, liyanrong@mail.ihep.ac.cn
 * Jun 30, 2016
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include <mpi.h>
#include <gsl/gsl_rng.h>

#include "dnest.h"
#include "model3.h"

int which_level_update;
int num_params;
DNestFptrSet *fptrset_thismodel3;

void model3()
{
  int i, argc=0, narg=6;
  char **argv;

  argv = malloc(narg*sizeof(char *));
  for(i=0; i<narg; i++)
  {
    argv[i] = malloc(200*sizeof(char));
  }
  strcpy(argv[argc++], "dnest");
  strcpy(argv[argc++], "-s");
  strcpy(argv[argc++], "restart_dnest3.txt");
  strcpy(argv[argc++], "-l"); //level-dependnet sampling
  strcpy(argv[argc++], "-g"); // tag
  strcpy(argv[argc++], "3");

  /* setup szie of modeltype, which is used for dnest */
  num_params = 2;
  fptrset_thismodel3 = dnest_malloc_fptrset();

  /* setup functions used for dnest*/
  fptrset_thismodel3->from_prior = from_prior_thismodel3;
  fptrset_thismodel3->log_likelihoods_cal = log_likelihoods_cal_thismodel3;
  fptrset_thismodel3->log_likelihoods_cal_initial = log_likelihoods_cal_thismodel3;
  fptrset_thismodel3->log_likelihoods_cal_restart = log_likelihoods_cal_thismodel3;
  fptrset_thismodel3->perturb = perturb_thismodel3;
  fptrset_thismodel3->print_particle = print_particle_thismodel3;
  fptrset_thismodel3->restart_action = restart_action_model3;

  /* run dnest */
  dnest(argc, argv, fptrset_thismodel3, num_params, NULL, NULL, NULL, "./", "OPTIONS3", NULL, NULL);

  /* free memory */
  dnest_free_fptrset(fptrset_thismodel3);

  for(i=0; i<narg; i++)
    free(argv[i]);
  free(argv);

}

/*====================================================*/
/* users responsible for following struct definitions */

void from_prior_thismodel3(void *model)
{
  int i;
  double *params = (double *)model;
  for(i=0; i<num_params; i++)
  {
    params[i] = -6.0 + 12.0 * dnest_rand();
    dnest_wrap(&params[i], -6.0, 6.0);
  }
}

double log_likelihoods_cal_thismodel3(const void *model)
{
  double *params = (double *)model;
  double logL;
  double logl1;
  double logl2;

  logl1 = -0.5 * pow( (sqrt((params[0] - 3.0)* (params[0] - 3.0) + params[1]*params[1])- 2.0)/0.1,  2.0)
          - 0.5 * log(2.0*M_PI * 0.01);
  logl2 = -0.5 * pow( (sqrt((params[0] + 3.0)* (params[0] + 3.0) + params[1]*params[1])- 2.0)/0.1,  2.0)
          - 0.5 * log(2.0*M_PI * 0.01);

  double max = fmax(logl1, logl2);

  logL = log( exp(logl1-max) + exp(logl2-max) ) + max;

  return logL;
}

double perturb_thismodel3(void *model)
{
  double *params = (double *)model;
  double logH = 0.0, width, limit1, limit2;
  int which = dnest_rand_int(num_params), which_level;
  int size_levels;

  which_level_update = dnest_get_which_level_update();
  size_levels = dnest_get_size_levels();
  which_level = which_level_update > (size_levels - 20)?(size_levels-20):which_level_update;
  which_level = 0;
  if(which_level > 0 )
  {
    limit1 = limits[(which_level_update-1) * num_params *2 + which *2 ];
    limit2 = limits[(which_level_update-1) * num_params *2 + which *2 + 1];
  }
  else
  {
    limit1 = -6.0;
    limit2 = 6.0;
  }
  width = (limit2 - limit1);
  params[which] += width * dnest_randh();
  dnest_wrap(&params[which], -6.0, 6.0);
  return logH;
}
/*=======================================================*/

void print_particle_thismodel3(FILE *fp, const void *model)
{
  int i;
  double *params = (double *)model;
  for(i=0; i<num_params; i++)
  {
    fprintf(fp, "%f ", params[i]);
  }
  fprintf(fp, "\n");
}

/*========================================================*/

void restart_action_model3(int iflag)
{
  return;
}


/*
 * Copyright 2009-2019 The VOTCA-MPIP Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * author: Denis Andrienko
 */

#ifndef __VOTCA_CTP_EIGEN_H
#define __VOTCA_CTP_EIGEN_H

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/numeric/conversion/cast.hpp>

#include <votca/tools/eigen.h>
#include <Eigen/Eigenvalues>

/*
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>
*/

#ifdef DEBUG_LINALG
#include <chrono>
#endif

namespace boost { namespace numeric { namespace ublas {

// specialization for double precision
template<class F, class A>
inline matrix<double,F,A>    // prod( m1, m2 )
prod(const matrix<double,F,A> &m1, const matrix<double,F,A> &m2)
{

    #ifdef DEBUG_LINALG
    auto start = std::chrono::system_clock::now();
    #endif

    boost::numeric::ublas::matrix<double,F,A> AxB( m1.size1(), m2.size2() );

    // Note that a Map<Type> m(&data) gives a view into the data (can change it)
    // while assignments Matrix<Type> m = Map< Matrix<Type> > (&data)
    // lead to a data copy and lost connection between the reference and matrix
    Eigen::Map<const Eigen::MatrixXd> mA_( &(m1.data()[0]), m1.size2(), m1.size1() );
    Eigen::Map<const Eigen::MatrixXd> mB_( &(m2.data()[0]), m2.size2(), m2.size1() );
    Eigen::Map<Eigen::MatrixXd> AxB_( &(AxB.data()[0]), AxB.size2(), AxB.size1() );

    AxB_ = mB_*mA_;

    #ifdef DEBUG_LINALG
    auto end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    std::cout << "\n... ... ... \x1b[0;34mEigen "
              << "[" << m1.size1() << "x" << m1.size2() << "]"
              << "*[" << m2.size1() << "x" << m2.size2() << "]"
              << " time: " << elapsed_seconds.count()
              << "s\x1b[0;39m";
    #endif

    return AxB;

            }

// specialization for single precision
template<class F, class A>
inline matrix<float, F, A> // prod( m1, m2 )
prod(const matrix<float, F, A> &m1, const matrix<float, F, A> &m2) {

    #ifdef DEBUG_LINALG
    auto start = std::chrono::system_clock::now();
    #endif

    boost::numeric::ublas::matrix<float, F, A> AxB(m1.size1(), m2.size2());

    Eigen::Map<const Eigen::MatrixXf> mA_(&(m1.data()[0]), m1.size2(), m1.size1());
    Eigen::Map<const Eigen::MatrixXf> mB_(&(m2.data()[0]), m2.size2(), m2.size1());
    Eigen::Map<Eigen::MatrixXf> AxB_(&(AxB.data()[0]), AxB.size2(), AxB.size1());

    AxB_ = mB_*mA_;

    #ifdef DEBUG_LINALG
    auto end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    std::cout << "\n... ... ... \x1b[0;34mEigen "
            << "[" << m1.size1() << "x" << m1.size2() << "]"
            << "*[" << m2.size1() << "x" << m2.size2() << "]"
            << " time: " << elapsed_seconds.count()
            << "s\x1b[0;39m";
    #endif

    return AxB;
}

}}}

namespace votca { namespace ctp { namespace linalg {

inline void invert_symm( const ub::matrix<double> &A, ub::matrix<double> &V){
    throw "Inversion of a matrix, linalg_invert, is not implemented in eigen.h";
}

/**
 * \brief eigenvalues of a symmetric matrix A*x=E*x
 * @param E vector of eigenvalues
 * @param V input: matrix to diagonalize
 * @param V output: eigenvectors
 *
 * This function wraps Eigen::EigenSolver
 *
 */
inline void eigenvalues_symm(const ub::matrix<double> &A,
                                   ub::vector<double> &E,
                                   ub::matrix<double> &V)
{

    #ifdef DEBUG_LINALG
        auto start = std::chrono::system_clock::now();
    #endif

/*
        gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        V.resize(N, N, false);

        // gsl does not handle conversion of a symmetric_matrix
        ub::matrix<double> _A( N,N );
        //make copy of A as A is destroyed by GSL
        _A = A;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
        assert(status == 0);
	gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_VAL_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

        std::cout << "\nREFERENCE";
        std::cout << "\n A" << A;
        std::cout << "\n E" << E;
        std::cout << "\n V" << V;
        std::cout << "\nREFERENCE\n";

*/
    Eigen::Map<const Eigen::MatrixXd> A_( &(A.data()[0]), A.size2(), A.size1() );

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A_.transpose());
    /*
      std::cout << "\nEIGEN";
      std::cout << "\n E\n" << es.eigenvalues();
      std::cout << "\n V\n" << es.eigenvectors();
      std::cout << "\nEIGEN";
    */

    // copy back to UBLAS objects
    Eigen::Map<Eigen::VectorXd>( &E(0), A.size2() ) = es.eigenvalues();
    Eigen::Map<Eigen::MatrixXd>( &V(0,0), A.size2(), A.size1() ) = es.eigenvectors().transpose();

    #ifdef DEBUG_LINALG
        auto end = std::chrono::system_clock::now();

        std::chrono::duration<double> elapsed_seconds = end - start;

        std::cout << "\n... ... ... \x1b[0;34mEigen Eigenvalues"
                  << "[" << A.size1() << "x" << A.size2() << "]"
                  << " time: " << elapsed_seconds.count()
                  << "s\x1b[0;39m";
    #endif
}

}}}

#endif  // __VOTCA_CTP_EIGEN_H


// Copyright 2018 Jeremy Mason
//
// Licensed under the Apache License, Version 2.0, <LICENSE-APACHE or
// http://apache.org/licenses/LICENSE-2.0> or the MIT license <LICENSE-MIT or
// http://opensource.org/licenses/MIT>, at your option. This file may not be
// copied, modified, or distributed except according to those terms.

//! \file sb_desc.c
//! Contains functions that actually calculate the spherical Bessel descriptors.

#include <cblas.h>      // dscal
#include <math.h>       // pow
#include <stdint.h>     // uint32_t
#include <stdlib.h>     // abort
#include "sbessel.h"    // _sbessel
#include "sb_desc.h"
#include "sb_matrix.h"  // sb_mat_malloc
#include "sb_structs.h" // sb_vec
#include "sb_utility.h" // SB_CHK_ERR
#include "sb_vector.h"  // sb_vec_calloc
#include "safety.h"

// Lookup tables used in get_radial_basis. Built using function in `tables.c`.
static const double _u_data[10152] = {
  #include "unl.tbl"
};
static const size_t _u_n_max = 140;

static const double _c1_data[10011] = {
  #include "c1.tbl"
};
static const size_t _c1_n_max = 140;

static const double _c2_data[10011] = {
  #include "c2.tbl"
};
static const size_t _c2_n_max = 140;

/// Calculates the radial basis functions for the spherical Bessel descriptors.
/// Numerical efficiency depends heavily on the lookup tables defined above,
/// allowing the function to be reduced to evaluating the recursion relations.
///
/// # Parameters
/// - `gnl`: pointer to a matrix to hold the result
/// - `r_data`: radial coordinates of the atoms
/// - `n_max`: defines the number of descriptors calculated
/// - `l`: order of the spherical Bessel functions
/// - `n_atom`: number of atoms in the environment
/// - `rc`: cutoff radius for the environment
///
/// # Returns
/// A copy of `gnl`
static sb_mat * get_radial_basis(
    sb_mat * gnl,
    double * r_data,
    uint32_t l,
    uint32_t n_atom,
    double rc) {
  // access lookup tables directly
  const double * u_data  = _u_data  + l * (2 * _u_n_max  - l + 5) / 2;
  const double * c1_data = _c1_data + l * (2 * _c1_n_max - l + 3) / 2;
  const double * c2_data = _c2_data + l * (2 * _c2_n_max - l + 3) / 2;

  // gnl->n_cols
  const size_t n_cols = gnl->n_cols;

  // forward declaration of variables without initializations
  size_t n, a;
  double u0, u1, u2, d0, d1, e;
  double * g_data;

  // fnl built in gnl
  g_data = gnl->data;
  for (n = 0; n < n_cols; ++n) {
    for (a = 0; a < n_atom; ++a) {
      g_data[a] = c1_data[n] * _sbessel(l, r_data[a] * u_data[n])
        - c2_data[n] * _sbessel(l, r_data[a] * u_data[n + 1]);
    }
    g_data += n_atom;
  }
  sb_mat_smul(gnl, pow(rc, -1.5));

  // initialize quantities used for recursion
  u1 = SB_SQR(u_data[0]);
  u2 = SB_SQR(u_data[1]);
  d1 = 1.;

  // convert to gnl
  g_data = gnl->data;
  for (n = 1; n < n_cols; ++n) {
    u0 = u1;
    u1 = u2;
    u2 = SB_SQR(u_data[n + 1]);

    e = (u0 * u2) / ((u0 + u1) * (u1 + u2));
    d0 = d1;
    d1 = 1. - e / d0;

    g_data += n_atom;
    cblas_dscal(n_atom, 1. / sqrt(d1), g_data, 1);
    cblas_daxpy(n_atom, sqrt(e / (d1 * d0)), g_data - n_atom, 1, g_data, 1);
  }

  return gnl;
}

/// Calculates spherical Bessel descriptors for the given atomic environment.
/// `desc` should contain space for the descriptors, labelled by (n, l) and
/// ordered lexicographically. `disp` should contain the relative Cartesian
/// coordinates of the surrouding atoms in the format `[x_1, y_1, z_1, ...]`.
/// `weights` should contain the weights used in the construction of the
/// neighbor density function (e.g., `[1., ...]`). `restrict` is not used to
/// help with portability.
///
/// # Parameters
/// - `desc`: pointer to an array to hold the result
/// - `disp`: pointer to an array of the relative displacements
/// - `weights`: pointer to an array of the atomic weights
/// - `rc`: cutoff radius for the environment
/// - `n_atom`: number of atoms in the environment
/// - `n_max`: defines the number of descriptors calculated
///
/// # Returns
/// A copy of `desc`
///
/// # Performance
/// The following preprocessor definitions (usually in `safety.h`) enable
/// various safety checks:
/// - `SAFE_MEMORY`: `desc`, `disp`, and `weights` are not `NULL`
/// - `SAFE_FINITE`: `rc` is nonnegative
///
/// # Warning
/// You are reponsible for ensuring that enough memory is allocated for the
/// relevant arrays, and should expect undefined behavior otherwise. The
/// lengths should be:
/// - `desc`: `(n_max + 1) * (n_max + 2) / 2`
/// - `disp`: at least `3 * n_atom`
/// - `weights`: at least `n_atom`
///
/// # Examples
/// ```
/// #include <stddef.h>     // size_t
/// #include <stdio.h>      // printf
/// #include <stdint.h>     // uint32_t
/// #include "sb_desc.h"    // sb_descriptors
///
/// /// An example where `sb_descriptors()` is used to calculate the spherical
/// /// Bessel descriptors for an atomic environment containing four atoms; the
/// /// first and second descriptors should be `0.031870` and `0.138078`.
/// int main(void) {
///   // Sets the number of descriptors returned.
///   uint32_t n_max = 4;
///
///   // Allocate memory for the result.
///   double desc[15] = { 0. };
///
///   // Number of atoms in the environment.
///   uint32_t n_atom = 4;
///
///   // Displacements to the surrounding atoms in Angstroms.
///   // [x_1, y_1, z_1, ...]
///   double disp[12] = {
///      1.3681827, -1.3103517, -1.3131874,
///     -1.5151760,  1.3360077, -1.3477119,
///     -1.3989598, -1.2973683,  1.3679189,
///      1.2279369,  1.3400378,  1.4797429
///   };
///
///   // Weights for the surrounding atoms.
///   double weights[4] = { 1., 1., 1., 1. };
///
///   // Cutoff radius in Angstroms.
///   double rc = 3.7711;
///
///   sb_descriptors(desc, disp, weights, rc, n_atom, n_max);
///
///   // Output the result
///   // Labelled by (n, l), ordered lexicographically
///   printf("SB descriptors:\n");
///   for (size_t a = 0; a < (n_max + 1) * (n_max + 2) / 2; ++a)
///     printf("%.6f\n", desc[a]);
///
///   printf("Completed successfully!\n");
/// }
/// ```
double * sb_descriptors(
    double * desc_arr,
    double * disp_arr,
    const double * weights_arr,
    const double rc,
    const uint32_t n_atom,
    const uint32_t n_max) {
#ifdef SAFE_MEMORY
  SB_CHK_ERR(!desc_arr, abort(), "sb_descriptors: desc cannot be NULL");
  SB_CHK_ERR(!disp_arr, abort(), "sb_descriptors: disp cannot be NULL");
  SB_CHK_ERR(!weights_arr, abort(), "sb_descriptors: weights cannot be NULL");
#endif
#ifdef SAFE_FINITE
  SB_CHK_ERR(rc < 0., abort(), "sb_descriptors: rc cannot be negative");
#endif
  // Check that n_max is within limit defined by tables
  SB_CHK_ERR(n_max > _u_n_max || n_max > _c1_n_max || n_max > _c2_n_max,
      abort(), "sb_descriptors: n_max above limit defined by lookup tables");

  // Convert raw pointers to sb_vec and sb_mat
  sb_mat * disp = malloc(sizeof(sb_mat));
  SB_CHK_ERR(!disp, abort(), "sb_descriptors: failed to allocate disp");

  disp->n_rows = 3;
  disp->n_cols = n_atom;
  disp->n_elem = 3 * n_atom;
  disp->data   = disp_arr;

  double * data1, * data2, * data3;
  size_t a, b;

  // Calculate radial coordinates
  sb_vec * radius = sb_vec_calloc(n_atom, 'r');
  data1 = disp->data;
  data2 = radius->data;
  for (a = 0; a < n_atom; ++a) {
    for (b = 0; b < 3; ++b) {
      *data2 += SB_SQR(data1[b]);
    }
    data1 += 3;
    data2 += 1;
  }
  sb_vec_sqrt(radius);

  // Normalize displacement vectors
  sb_mat_vdiv(disp, radius, 'c');
  sb_vec_smul(radius, 1. / rc);

  // Calculate angle cosines
  sb_mat * gamma = sb_mat_malloc(n_atom, n_atom);
  sb_mat_mm_mul(gamma, disp, disp, "tn");

  // Legendre polynomials
  sb_mat * * lp = malloc((n_max + 1) * sizeof(sb_mat *));
  SB_CHK_ERR(!lp, abort(), "sb_descriptors: failed to allocate lp");
  for (a = 0; a <= n_max; ++a) {
    lp[a] = sb_mat_malloc(n_atom, n_atom);
  }

  sb_mat_set_all(lp[0], 1.);
  sb_mat_memcpy(lp[1], gamma);
  for (a = 2; a <= n_max; ++a) { // l = a
    sb_mat_memcpy(lp[a], gamma);
    sb_mat_pmul(lp[a], lp[a - 1]);
    sb_mat_smul(lp[a], (2. * a - 1.) / (a - 1.));
    sb_mat_psub(lp[a], lp[a - 2]);
    sb_mat_smul(lp[a], (a - 1.) / a);
  }

  // Include multiplier here to simplify calculation below
  for (a = 0; a <= n_max; ++a) { // l = a
    sb_mat_smul(lp[a], (2. * a + 1.) / 12.566370614359172);
  }

  // Radial basis functions
  sb_mat * * gnl = malloc((n_max + 1) * sizeof(sb_mat *));
  SB_CHK_ERR(!gnl, abort(), "sb_descriptors: failed to allocate gnl");
  for (a = 0; a <= n_max; ++a) { // l = a
    gnl[a] = sb_mat_malloc(n_atom, n_max - a + 1);
    get_radial_basis(gnl[a], radius->data, a, n_atom, rc);
    // Scale by the weights
    for (b = 0; b < n_atom; ++b) {
      cblas_dscal(gnl[a]->n_cols, weights_arr[b], gnl[a]->data + b, n_atom);
    }
  }

  // radius can be used for workspace
  data1 = radius->data;
  for (a = 0; a <= n_max; ++a) { // l = a
    data2 = lp[a]->data;
    data3 = gnl[a]->data;
    for (b = a; b <= n_max; ++b) { // n = b
      cblas_dgemv(CblasColMajor, CblasNoTrans, n_atom, n_atom,
          1., data2, n_atom, data3 + (b - a) * n_atom, 1, 0., data1, 1);
      desc_arr[b * (b + 1) / 2 + a] = cblas_ddot(n_atom, data3 + (b - a) * n_atom, 1, data1, 1);
    }
  }

  // Free memory
  for (a = 0; a <= n_max; ++a) {
    SB_MAT_FREE_ALL(lp[a], gnl[a]);
  }
  SB_FREE_ALL(disp, lp, gnl);
  SB_VEC_FREE_ALL(radius);
  SB_MAT_FREE_ALL(gamma);

  return desc_arr;
}


/******************************************************************************
CosmoLike Configuration Space Covariances for Projected Galaxy 2-Point Statistics
https://github.com/CosmoLike/CosmoCov
by CosmoLike developers
******************************************************************************/

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
// #include "../class/include/class.h"

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

//void omega_a(double aa,double *om_m,double *om_v);
double omv_vareos(double a);
static inline double hoverh0(double a);
double growfac(double a);
//int func_for_growfac(double a,const double y[],double f[],void *params);
double Tsqr_EH_wiggle(double khoverMPC);
//double int_for_sigma_r_sqr(double k, void * args);
double sigma_r_sqr();
//double Delta_L_wiggle(double k);
//double Delta_lin_wiggle(double k,double a);
double p_lin(double k,double a);
//double int_sig_R_knl(double logk, void *args);
//double int_neff(double lnk, void *args);
//double int_cur(double lnk, void *args);
//void nonlin_scale(double amp, double *R_NL, double *neff, double *Curv);
//double Halofit(double k, double amp, double omm, double omv,double w_z, double R_NL, double neff,double Curv, double P_delta_Lin);
//void Delta_halofit(double **table_P_NL,double logkmin, double logkmax, double dk, double da);
//double Delta_NL_Halofit(double k_NL, double a); //k in h/Mpc
double Pdelta(double k_NL,double a); //k in coverH0 units

//double int_for_chi(double a,void * args);
double f_K(double chi);
double chi(double a);
double a_chi(double chi1);
// extern void emu(double *xstar, double *ystar, double *kstar);

//c%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//variable Omega_v
//c%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double omv_vareos(double a)
{
  return(cosmology.Omega_v*exp(-3.*((cosmology.w0+cosmology.wa+1.)*log(a)+cosmology.wa*(1.-a))));
}

//c%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//c evolution of omega matter and omega lamda with expansion factor

void omega_a(double aa,double *om_m,double *om_v)
{
  double a2,omega_curv;
  a2=aa*aa;
  omega_curv=1.0-cosmology.Omega_m- cosmology.Omega_v;
  *om_m=cosmology.Omega_m /(cosmology.Omega_m +aa*(omv_vareos(aa) *a2 +omega_curv));
  *om_v=omv_vareos(aa)*a2*aa/(cosmology.Omega_m+aa*(a2*omv_vareos(aa) +omega_curv));
}
//c%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//growth factor including Dark energy parameters w0, wa
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

//function for growfac (DGL)
int func_for_growfac(double a,const double y[],double f[],void *params)
{
  //double *p=(double *)params;
  if (a == 0) {
    printf("a=0 in function 'func_for_growfac'!\n");
    exit(1);
  }
  double aa=a*a;
  double omegam=cosmology.Omega_m/(aa*a);
  double omegav=omv_vareos(a);
  double hub = hoverh0(a);
  double one_plus_mg_mu = 1.;
  hub = hub*hub;
  f[0]=y[1];
  if(cosmology.MGmu != 0){
    one_plus_mg_mu += cosmology.MGmu*omegav/hub/cosmology.Omega_v;
  }
  f[1]=y[0]*3.*cosmology.Omega_m/(2.*hub*aa*aa*a)*one_plus_mg_mu-y[1]/a*(2.-(omegam+(3.*(cosmology.w0+cosmology.wa*(1.-a))+1)*omegav)/(2.*hub));
  return GSL_SUCCESS;
}

static inline double hoverh0(double a){
  return sqrt(cosmology.Omega_m /(a*a*a) + (1.-cosmology.Omega_m -cosmology.Omega_v )/(a*a) + omv_vareos(a) );
}

double growfac(double a)
{
  const double MINA=1.e-8;
  static cosmopara C;
  static double *ai;
  static double *table;
  double res;

  gsl_interp *intf=gsl_interp_alloc(gsl_interp_linear,Ntable.N_a);
  gsl_interp_accel *acc=gsl_interp_accel_alloc();

  if (recompute_expansion(C))
  {

    if(table!=0) free_double_vector(table,0, Ntable.N_a-1);
    if(ai!=0) free_double_vector(ai,0, Ntable.N_a-1);
    ai=create_double_vector(0, Ntable.N_a-1);
    table=create_double_vector(0, Ntable.N_a-1);

    int i;

    const gsl_odeiv_step_type *T=gsl_odeiv_step_rkf45;
    gsl_odeiv_step *s=gsl_odeiv_step_alloc(T,2);
    gsl_odeiv_control *c=gsl_odeiv_control_y_new(1.e-6,0.0);
    gsl_odeiv_evolve *e=gsl_odeiv_evolve_alloc(2);

    double t=MINA;            //start a
    double t1=1.1;                //final a
    double h=1.e-6;              //initial step size
    double y[2]={MINA,MINA};   //initial conditions
    double norm;
    double par[0]={};
    gsl_odeiv_system sys={func_for_growfac,NULL,2,&par};

    for (i=1;i<=Ntable.N_a;i++) {
      ai[i-1]=i*t1/(1.*Ntable.N_a);
      while(t<ai[i-1])
        gsl_odeiv_evolve_apply(e,c,s,&sys,&t,ai[i-1],&h,y);
      if (i==1) norm=y[0]/ai[i-1];
      table[i-1]=y[0]/norm;
    }

    gsl_odeiv_evolve_free(e);
    gsl_odeiv_control_free(c);
    gsl_odeiv_step_free(s);
    update_cosmopara(&C);

  }
  gsl_interp_init(intf,ai,table,Ntable.N_a);
  res=gsl_interp_eval(intf,ai,table,a,acc);
  gsl_interp_accel_free(acc);
  gsl_interp_free(intf);
  return(res);
}

// ---------------------------- Transfer Function from EH98 ----------------------
//Input: k -- Wavenumber at which to calculate transfer function, in Mpc^-1. Output: Returns the value of the full transfer function fitting formula. This is the form given in Section 3 of Eisenstein & Hu (1997). Notes: Units are Mpc, not h^-1 Mpc.

double Tsqr_EH_wiggle(double khoverMPC)
{
  static double omhh=-123.;
  static double obhh=-123.;
  static double OMEGA_V = -123.;
  static double f_baryon;

  static double k_equality;
  static double sound_horizon;
  static double beta_c;
  static double alpha_c;
  static double beta_node;
  static double alpha_b;
  static double beta_b;
  static double k_silk;

  //if (omhh != cosmology.Omega_m*cosmology.h0*cosmology.h0 || obhh != cosmology.omb*cosmology.h0*cosmology.h0|| OMEGA_V != cosmology.Omega_v){
  double theta_cmb,z_equality,z_drag,R_drag,R_equality;
  double z_drag_b1, z_drag_b2;
  double alpha_c_a1, alpha_c_a2, beta_c_b1, beta_c_b2, alpha_b_G, y;

  omhh = cosmology.Omega_m*cosmology.h0*cosmology.h0;
  obhh = cosmology.omb*cosmology.h0*cosmology.h0;
  OMEGA_V = cosmology.Omega_v;
  f_baryon = obhh/omhh;
    //printf("%le\n",f_baryon);

    theta_cmb=2.728/2.7;// Tcmb in units of 2.7 K
    z_equality= 2.50e4*omhh/POW4(theta_cmb);//Redshift of matter-radiation equality, really 1+z

    k_equality=0.0746*omhh/SQR(theta_cmb);//Scale of equality, in Mpc^-1
    z_drag_b1 = 0.313*pow(omhh,-0.419)*(1+0.607*pow(omhh,0.674));
    z_drag_b2 = 0.238*pow(omhh,0.223);
    z_drag=1291*pow(omhh,0.251)/(1+0.659*pow(omhh,0.828))*(1+z_drag_b1*pow(obhh,z_drag_b2));//Redshift of drag epoch
    R_drag=31.5*obhh/POW4(theta_cmb)*(1000/(1+z_drag));//Photon-baryon ratio at drag epoch

    R_equality= 31.5*obhh/POW4(theta_cmb)*(1000/z_equality);//Photon-baryon ratio at equality epoch
    sound_horizon=2./3./k_equality*sqrt(6./R_equality)*log((sqrt(1+R_drag)+sqrt(R_drag+R_equality))/(1+sqrt(R_equality)));//Sound horizon at drag epoch, in Mpc
    k_silk= 1.6*pow(obhh,0.52)*pow(omhh,0.73)*(1+pow(10.4*omhh,-0.95));//Silk damping scale, in Mpc^-1
    alpha_c_a1 = pow(46.9*omhh,0.670)*(1+pow(32.1*omhh,-0.532));
    alpha_c_a2 = pow(12.0*omhh,0.424)*(1+pow(45.0*omhh,-0.582));
    alpha_c=pow(alpha_c_a1,-f_baryon)*pow(alpha_c_a2,-CUBE(f_baryon)); //CDM suppression

    beta_c_b1 = 0.944/(1+pow(458*omhh,-0.708));
    beta_c_b2 = pow(0.395*omhh, -0.0266);
    beta_c=1.0/(1+beta_c_b1*(pow(1-f_baryon, beta_c_b2)-1));//CDM log shift

    y = z_equality/(1+z_drag);
    alpha_b_G = y*(-6.*sqrt(1+y)+(2.+3.*y)*log((sqrt(1+y)+1)/(sqrt(1+y)-1)));
    alpha_b=2.07*k_equality*sound_horizon*pow(1+R_drag,-0.75)*alpha_b_G;//Baryon suppression
    beta_node = 8.41*pow(omhh, 0.435);//Sound horizon shift
    beta_b= 0.5+f_baryon+(3.-2.*f_baryon)*sqrt(pow(17.2*omhh,2.0)+1);//Baryon envelope shift
  //}
  // Start of TFfit_onek from the original tf_fit.c at http://background.uchicago.edu/~whu/transfer/transferpage.html
    double T_c_ln_beta,T_c_ln_nobeta , T_c_C_alpha, T_c_C_noalpha;
    double q,qsqr, xx, xx_tilde;
    double T_c_f, T_c, s_tilde, T_b_T0, T_b;

  double k=khoverMPC*cosmology.h0; //internally this routine uses Mpc^-1 not h/Mpc
  q = k/13.41/k_equality;
  qsqr =SQR(q);
  xx = k*sound_horizon;

  T_c_ln_beta = log(2.718282+1.8*beta_c*q);
  T_c_ln_nobeta = log(2.718282+1.8*q);

  T_c_ln_beta = log(2.718282+1.8*beta_c*q);
  T_c_C_alpha = 14.2/alpha_c + 386.0/(1+69.9*pow(q,1.08));
  T_c_C_noalpha = 14.2 + 386.0/(1+69.9*pow(q,1.08));

  T_c_f = 1.0/(1.0+POW4(xx/5.4));
  T_c = T_c_f*T_c_ln_beta/(T_c_ln_beta+T_c_C_noalpha*qsqr) +(1-T_c_f)*T_c_ln_beta/(T_c_ln_beta+T_c_C_alpha*qsqr);

  s_tilde = sound_horizon*pow(1+CUBE(beta_node/xx),-1./3.);
  xx_tilde = k*s_tilde;

  T_b_T0 = T_c_ln_nobeta/(T_c_ln_nobeta+T_c_C_noalpha*qsqr);

  T_b = sin(xx_tilde)/(xx_tilde)*(T_b_T0/(1+SQR(xx/5.2))+alpha_b/(1+CUBE(beta_b/xx))*exp(-pow(k/k_silk,1.4)));

  return SQR(f_baryon*T_b + (1-f_baryon)*T_c);
}

//Calculate Normalization see Cosmology Notes 8.105
double int_for_sigma_r_sqr(double k, void * args)
{
  double kR, res, x;
  kR = k*8.; // r=8 Mpc/h
  x = (sin(kR) - kR*cos(kR))/(kR*kR*kR);
  res = pow(k,2.+cosmology.n_spec+ 0.5*cosmology.alpha_s*log(k/0.05))*Tsqr_EH_wiggle(k)*x*x;
  return res;
}

double sigma_r_sqr()
{
  static double res = -123.;
  static cosmopara C;
  double integral,array[1];

  if (recompute_Delta(C)) //strictly speaking, this is recomputed unnecessarily if only sigma_8 changes
  {
    integral = int_gsl_integrate_medium_precision(int_for_sigma_r_sqr,(void*)array,1e-4,1e6,NULL,512);
    res = 9.0*integral;   //see Peackock97, eq. 29
    update_cosmopara(&C);

  }
  if (!(res>0.0)){
    fprintf(stderr,"failed with sigma_r_sqr = %le\n", res);
  }
  assert(res>0.0);
  return res;
}

double Delta_L_wiggle(double k)
{
  static cosmopara C;

  static double *table_P;
  static double dk = .0, logkmin = .0, logkmax = .0;

  double klog,f1,norm;
  int i;

  if (k < limits.k_min_mpc || k > limits.k_max_mpc){
    norm=cosmology.sigma_8*cosmology.sigma_8/sigma_r_sqr();

    return norm*pow(k,cosmology.n_spec+ 0.5*cosmology.alpha_s*log(k/0.05)+3.0)*Tsqr_EH_wiggle(k);
    //printf("outside Delta_L_tab\n");
  }
  else{
    if (recompute_Delta(C))
    {
      if (cosmology.M_nu > 0){
        printf("Implementation of EH transfer function does not support massive neutrinos\n EXIT\n");
      }
      update_cosmopara(&C);
      norm=cosmology.sigma_8*cosmology.sigma_8/sigma_r_sqr();

      if(table_P!=0) free_double_vector(table_P,0, Ntable.N_k_lin-1);
      table_P=create_double_vector(0, Ntable.N_k_lin-1);

      logkmin = log(limits.k_min_mpc);
      logkmax = log(limits.k_max_mpc);
      dk = (logkmax - logkmin)/(Ntable.N_k_lin-1.);
      klog = logkmin;

      for (i=0; i<Ntable.N_k_lin; i++, klog += dk) {
       table_P[i]=log(norm*pow(exp(klog),cosmology.n_spec+ 0.5*cosmology.alpha_s*log(k/0.05)+3.0)*Tsqr_EH_wiggle(exp(klog)));
     }
      //printf("finished Delta_L_wiggle\n");
   }
 }
 klog=log(k);
 f1=interpol(table_P, Ntable.N_k_lin, logkmin, logkmax, dk,klog, 1.0,1.0 );
 return exp(f1);
}

// linear power spectrum routine with k in units H_0/c; used in covariances.c for beat coupling and in halo.c
double p_lin(double k,double a)
{
  static cosmopara C;
  static double **table_P_Lz = 0;
  static double logkmin = 0., logkmax = 0., dk = 0., da = 0.;
  int status;
  // if (strcmp(pdeltaparams.runmode,"CLASS")==0 || strcmp(pdeltaparams.runmode,"class")==0) return p_class(k,a,0, &status);

  double amp,ampsqr,grow0,aa,klog,val;

  int i,j;
  if (a >= 0.99999){a =0.99999;}
  if (recompute_cosmo3D(C)){
    update_cosmopara(&C);
    if (table_P_Lz!=0) free_double_matrix(table_P_Lz,0, Ntable.N_a-1, 0, Ntable.N_k_lin-1);
    table_P_Lz = create_double_matrix(0, Ntable.N_a-1, 0, Ntable.N_k_lin-1);
    grow0=growfac(1.);
    da = (1. - limits.a_min)/(Ntable.N_a-1.);
    aa = limits.a_min;
    for (i=0; i<Ntable.N_a; i++, aa +=da) {
      if(aa>1.0) aa=1.0;
      amp=growfac(aa)/grow0;
      ampsqr=amp*amp;

      logkmin = log(limits.k_min_mpc);
      logkmax = log(limits.k_max_mpc);
      dk = (logkmax - logkmin)/(Ntable.N_k_lin-1.);
      klog = logkmin;
      for (j=0; j<Ntable.N_k_lin; j++, klog += dk) {
        table_P_Lz[i][j] = log(ampsqr*Delta_L_wiggle(exp(klog)));
        //printf("%le %le",exp(klog),Delta_L_wiggle(exp(klog));
      }
    }
  }
  if (k/cosmology.coverH0 > exp(logkmax)) return 0.0;
  klog = log(k/cosmology.coverH0);
  val = interpol2d(table_P_Lz, Ntable.N_a, limits.a_min, 1., da, a, Ntable.N_k_lin, logkmin, logkmax, dk, klog, 3.0+cosmology.n_spec, 0.0);
  if(isnan(val) || (k==0)) return 0.0;
  return 2.0*constants.pi_sqr*exp(val)/k/k/k;
}

double int_sig_R_knl(double lnk, void *args) // tak12 A4
{
  double krsqr;

  double *params= (double *) args;
  double Rscale=params[0];
  //printf("Rscale %le k %le\n",Rscale,exp(lnk));
  krsqr= SQR(exp(lnk)*Rscale);
  return Delta_L_wiggle(exp(lnk))*exp(-krsqr);
}

double int_neff(double lnk, void *args) //tak12 A5
{
  double krsqr;
  double *params= (double *) args;
  double Rscale=params[0];
  krsqr= SQR(exp(lnk)*Rscale);
  return Delta_L_wiggle(exp(lnk))*2.0*krsqr*exp(-krsqr); //see S03 eq. 59
}

double int_cur(double lnk, void *args) //tak12 A5
{
  double krsqr;
  double *params= (double *) args;
  double Rscale=params[0];
  krsqr= SQR(exp(lnk)*Rscale);
  return Delta_L_wiggle(exp(lnk))*4.0*krsqr*(1.0-krsqr)*exp(-krsqr); // S03 eq.60
}

//iterative calculation of the nonlinear scale as defined in tak12 A4
void nonlin_scale(double amp, double *R_NL, double *neff, double *Curv)
{
  double sig_R,kmax,logkmax,sig_R_noamp,neffplus3;
  int iterstep;
  const int itermax  = 40;
  int converged=0;
  double array[1];
  double logRmin = -3.0;
  double logRmax =  4.0;

  iterstep=0;
  while(converged==0)
  {
    array[0]=pow(10.,(logRmin+logRmax)/2.0);

    //flexible upper limit of integration depending on drop-off of filter function
    kmax  = sqrt(5.*log(10.))/array[0];
    if (kmax<8000.0) logkmax = log(8000.0);
    sig_R_noamp=sqrt(int_gsl_integrate_medium_precision(int_sig_R_knl,(void*)array,-4.5,logkmax,NULL,512)); //integral goes over ln k exponent correspond to k_min~0.011

    sig_R=amp*sig_R_noamp;
    if (sig_R>1.0)  logRmin=log10(array[0]);
    if (sig_R<1.0)  logRmax=log10(array[0]);
    iterstep=iterstep+1;
    if(fabs(sig_R-1.0) < 0.0001 || iterstep>itermax) converged=1;
  }
  *R_NL=array[0]; //R where sig_R==1
  neffplus3=int_gsl_integrate_medium_precision(int_neff,(void*)array,-4.5,logkmax,NULL,512)/sig_R_noamp/sig_R_noamp;
  *neff= neffplus3 - 3.0;
  *Curv= int_gsl_integrate_medium_precision(int_cur,(void*)array,-4.5,logkmax,NULL,512)/sig_R_noamp/sig_R_noamp + SQR(neffplus3);

  //printf("%d %le\n",iterstep,amp);
}

double Halofit(double k, double amp, double omm, double omv,double w_z, double R_NL, double neff,double Curv, double P_delta_Lin)
{
  double y_scale,n2eff,n3eff,n4eff;
  double a_n,b_n,c_n,gamma_n,alpha_n,beta_n,nu_n,f1,f2,f3;

  double Delta_H,Delta_H_Prime,Delta_Q;
  //determine nonlinear scale, neff and curvature, see tak12 A4, A5
  y_scale=k*R_NL;

  n2eff=neff*neff;
  n3eff=n2eff*neff;
  n4eff=n2eff*n2eff;

  //calculate coefficients
  a_n = pow(10.,1.5222+2.8553*neff + 2.3706*n2eff+0.9903*n3eff+0.2250*n4eff-0.6038*Curv+0.1749*omv*(1.0+w_z));
  b_n = pow(10., -0.5642+0.5864*neff + 0.5716*n2eff-1.5474*Curv +0.2279*omv*(1.0+w_z));
  c_n = pow(10., 0.3698+ 2.0404*neff + 0.8161*n2eff+0.5869*Curv);
  gamma_n = 0.1971-0.0843*neff + 0.8460*Curv;
  alpha_n = fabs(6.0835 + 1.3373*neff - 0.1959*n2eff - 5.5274*Curv);
  beta_n = 2.0379 - 0.7354*neff + 0.3157*n2eff + 1.2490*n3eff + 0.3980*n4eff - 0.1682*Curv;
  nu_n = pow(10,5.2105+3.6902*neff);

  f1 = pow(omm,(-0.0307));
  f2 = pow(omm,(-0.0585));
  f3 = pow(omm,(0.0743));

  //TwoHaloTerm
  Delta_Q=P_delta_Lin*(pow((1.0+P_delta_Lin),beta_n)/(1.0+alpha_n*P_delta_Lin))*exp(-(y_scale/4.0+y_scale*y_scale/8.0));
  //OneHaloterm
  Delta_H_Prime=(a_n*pow(y_scale,3.0*f1))/(1.0+b_n*pow(y_scale,f2)+pow(c_n*f3*y_scale,3.0-gamma_n));
  Delta_H=Delta_H_Prime/(1.0+nu_n*pow(y_scale,-2.0)); // using mu=0.0 Tak A12
  //printf("Delta_Q %le Delta_H %le\n",Delta_Q,Delta_H);
  return Delta_H+Delta_Q;
}

void Delta_halofit(double **table_P_NL,double logkmin, double logkmax, double dk, double da)
{
  double rk,omm,omv,w_z,amp,grow0,aa,klog;
  double R_NL,Curv,neff,P_delta,P_delta_Lin;
  int i,j;

  grow0=growfac(1.);
  aa = limits.a_min;
  //binning in k and a must be the same as in emu
  for (i=0; i<Ntable.N_a; i++, aa +=da) {
    if(aa>1.0) aa=1.0;
    omega_a(aa,&omm,&omv);
    w_z=cosmology.w0+cosmology.wa*(1.-aa);
    amp=growfac(aa)/grow0;
    nonlin_scale(amp, &R_NL, &neff, &Curv);
    //printf("%le %le %le %le\n",aa,R_NL,neff,Curv);
    klog = logkmin;
    for (j=0; j<Ntable.N_k_nlin; j++, klog += dk) {
      rk=exp(klog);
      P_delta_Lin=amp*amp*Delta_L_wiggle(rk);
      P_delta=Halofit(rk, amp, omm, omv, w_z, R_NL, neff, Curv, P_delta_Lin);
      table_P_NL[i][j]=log(P_delta);
    }
  }
}

double Delta_NL_Halofit(double k_NL, double a)
{
  static cosmopara C;
  static double logkmin = 0., logkmax = 0., dk = 0., da = 0.;

  static double **table_P_NL=0;
  double klog,val;

  if (recompute_cosmo3D(C)){
    update_cosmopara(&C);
    if (table_P_NL!=0) free_double_matrix(table_P_NL,0, Ntable.N_a-1, 0, Ntable.N_k_nlin-1);
    table_P_NL = create_double_matrix(0, Ntable.N_a-1, 0, Ntable.N_k_nlin-1);

    da = (1. - limits.a_min)/(Ntable.N_a-1.);
    logkmin = log(limits.k_min_mpc);
    logkmax = log(limits.k_max_mpc);
    dk = (logkmax - logkmin)/(Ntable.N_k_nlin-1.);

    Delta_halofit(table_P_NL,logkmin, logkmax, dk, da);
  }
  klog = log(k_NL);
  val = interpol2d(table_P_NL, Ntable.N_a, limits.a_min, 1., da, a, Ntable.N_k_nlin, logkmin, logkmax, dk, klog, cosmology.n_spec, 0.0);
  return exp(val);
  // returns the dimensionless power spectrum as a function of scale factor a and k
}

double nonlinear_scale_computation(double a)
{
  static cosmopara C;
  static double da = 0.;
  static double *table=0;

  double omm,omv,amp,grow0,aa,res;
  double R_NL,Curv,neff;
  int i;

  if (recompute_cosmo3D(C)){
    update_cosmopara(&C);
    grow0=growfac(1.);
    da = (1.-limits.a_min)/(Ntable.N_a-1.);
    aa = limits.a_min;
    if (table!=0) free_double_vector(table, 0, Ntable.N_a-1);
    table   = create_double_vector(0, Ntable.N_a-1);
    for (i=0; i<Ntable.N_a; i++, aa+=da) {
      if(aa>1.0) aa=1.0;
      omega_a(aa,&omm,&omv);
      amp=growfac(aa)/grow0;
      nonlin_scale(amp, &R_NL, &neff, &Curv);
      table[i] = 1./R_NL;
    }
  }
  res = interpol(table, Ntable.N_a, limits.a_min, 1., da, a, 0.0, 0.0);
  return res;
}

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
//Pdelta is called with k in units H0/c since the comoving distance chi is in units c/H0. Upstream Pdelta all routines are in h/mpc
double Pdelta(double k_NL,double a)
{
  static int P_type = -1;
  if (P_type == -1){
    if (strcmp(pdeltaparams.runmode,"Halofit")==0) P_type = 0;
    if (strcmp(pdeltaparams.runmode,"halofit")==0) P_type = 0;
    // if (strcmp(pdeltaparams.runmode,"emu")==0) P_type = 1;
    // if (strcmp(pdeltaparams.runmode,"emu_only")==0) P_type = 2;
    if (strcmp(pdeltaparams.runmode,"linear")==0) P_type = 3;
    // if (strcmp(pdeltaparams.runmode,"CLASS")==0) P_type = 4;
    // if (strcmp(pdeltaparams.runmode,"class")==0) P_type = 4;
    if (strcmp(pdeltaparams.runmode,"cosmo_sim_test") ==0) P_type = 5;

  }

  //printf("%s set\n",pdeltaparams.runmode);
  double pdelta = 0.,kintern=k_NL/cosmology.coverH0,error,k_nonlin,res;
  int status;
  switch (P_type){
    case 0: pdelta=2.0*constants.pi_sqr*Delta_NL_Halofit(kintern,a)/k_NL/k_NL/k_NL; break;
    // case 1: pdelta=2.0*constants.pi_sqr*Delta_NL_emu(kintern,a)/k_NL/k_NL/k_NL; break;
    // case 2: pdelta=2.0*constants.pi_sqr*Delta_NL_emu_only(kintern,a)/k_NL/k_NL/k_NL; break;
    case 3: pdelta=p_lin(k_NL,a); break;
    // case 4: pdelta=p_class(k_NL,a,1, &status); break;
    case 5: k_nonlin=nonlinear_scale_computation(a);
    if (kintern<0.01) pdelta=2.0*constants.pi_sqr*Delta_NL_Halofit(kintern,a)/k_NL/k_NL/k_NL;
    else{
      error=0.01*pow((pdeltaparams.DIFF_A*kintern/k_nonlin),pdeltaparams.DIFF_n);
      pdelta=2.0*constants.pi_sqr*Delta_NL_Halofit(kintern,a)*(1.0+error)/k_NL/k_NL/k_NL;
    }
    break;
    default:
    printf("cosmo3D:Pdelta: %s Pdelta runmode not defined\n",pdeltaparams.runmode);
    printf("using Halofit (standard)\n");
    pdelta=2.0*constants.pi_sqr*Delta_NL_Halofit(kintern,a)/k_NL/k_NL/k_NL;
    break;
  }

//      double z = 1./a -1 ;
//	  if (z > 4.) {
//		 printf("	z:%lf    a:%lf   	k_Mpc:%lf    PkRatio:%lf \n",z,a,kintern,PkRatio_baryons(kintern, a));
//	  }

  // if (bary.isPkbary==1) pdelta = pdelta*PkRatio_baryons(kintern, a);
  return pdelta;
}

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/*============================================================
 *see BS 2.41 bzw Logbook for detailed calculation of chi from a.*/

double int_for_chi(double a, void * args){
  //double res,asqr;
  //asqr=a*a;
  //res= 1./sqrt(a*cosmology.Omega_m + asqr*(1.-cosmology.Omega_m -cosmology.Omega_v ) + asqr*asqr*omv_vareos(a));
  //return res;
  return 1./(a*a*hoverh0(a)); //changed to call of hoverh0 to be ready for other parametrizations
}

/*for the calculation of chi we have to integrate from a(z2)=a up to a(z1)=1, which means todays expansion factor*/
double chi(double a)
{
  static cosmopara C;
  static double *table;
  static double da = 0.;
  double aa,res;
  int i;
  double array[1];

  if (recompute_expansion(C)){
    update_cosmopara(&C);
    da = (1.-limits.a_min)/(Ntable.N_a-1.);
    aa = limits.a_min;
    if (table!=0) free_double_vector(table, 0, Ntable.N_a-1);
    table   = create_double_vector(0, Ntable.N_a-1);
    for (i=0; i<Ntable.N_a-1; i++, aa+=da) {
      table[i] = int_gsl_integrate_medium_precision(int_for_chi,(void*)array, aa, 1.,NULL,1000);
    }
    table[Ntable.N_a-1] =.0;
  }
  res = interpol(table, Ntable.N_a, limits.a_min, 1., da, a, 0.0, 0.0); // comoving distance in c/H_0
  if (res < 0){printf ("interpolation error in chi(%e)\n",a); res=0.01;}
  return res;
}
//auxilary function to look up a(chi)
double a_chi(double chi1){
  static gsl_spline * a_spline = NULL;
  static gsl_interp_accel * a_accel = NULL;
  static cosmopara C;
  static double chi_max =-1.;
  if (!a_spline){
    a_spline = gsl_spline_alloc(gsl_interp_cspline, Ntable.N_a);
    a_accel = gsl_interp_accel_alloc();
  }
  if (recompute_cosmo3D(C)){
    update_cosmopara(&C);
    double *table_a,*table_chi;
    table_a  = create_double_vector(0, Ntable.N_a-1);
    table_chi  = create_double_vector(0, Ntable.N_a-1);
    for (int i = 0; i < Ntable.N_a; i++){
      table_a[i] = 1.0 - (1.0 - 0.99*limits.a_min)/(Ntable.N_a-1.)*(double)i;
      table_chi[i] = int_gsl_integrate_medium_precision(int_for_chi,NULL, table_a[i], 1.,NULL,1000);
   //   printf("%d %e %e\n",i,table_a[i],table_chi[i]);
    }
    chi_max = int_gsl_integrate_medium_precision(int_for_chi,NULL, limits.a_min, 1.,NULL,1000);
    gsl_spline_init(a_spline, table_chi, table_a, Ntable.N_a);
    free_double_vector(table_a,0, Ntable.N_a-1);
    free_double_vector(table_chi,0, Ntable.N_a-1);
  }
  if (chi1 <=0.0){return 1.0;}
  if (chi1 > chi_max){printf("called a_chi(chi) with chi > chi(limits.a_min\nEXIT\n");exit(1);}
  return gsl_spline_eval(a_spline,chi1,a_accel);
}

/*===============================calculating the angular diameter distance f_K BS01 2.4, 2.30: f_K is a radial function that, depending on the curvature of the Universe, is a trigonometric, linear, or hyperbolic function of chi  */
double f_K(double chi)
{
  double K, K_h, f;
  K = (cosmology.Omega_m   + cosmology.Omega_v  - 1.);
  if (K > precision.medium) {           /* open */
    K_h = sqrt(K); // K in units H0/c see BS eq. 2.30
    f = 1./K_h*sin(K_h*chi);
    //printf("open\n");
  } else if (K < -precision.medium) {   /* closed */
    K_h = sqrt(-K);
    f = 1./K_h*sinh(K_h*chi);
    //printf("closed K=%le %le %le\n",K,cosmology.Omega_m,cosmology.Omega_v);
  } else {                     /* flat */
    f = chi;
    //printf("flatK=%le %le %le\n",K,cosmology.Omega_m,cosmology.Omega_v);
  }
  return f;
}


/* multifit/lmpar.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 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 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <gsl/gsl_permute_vector_double.h>

#include "qrsolv.c"

static size_t
count_nsing (const gsl_matrix * r)
{
  /* Count the number of nonsingular entries. Returns the index of the
     first entry which is singular. */

  size_t n = r->size2;
  size_t i;

  for (i = 0; i < n; i++)
    {
      double rii = gsl_matrix_get (r, i, i);

      if (rii == 0)
        {
          break;
        }
    }

  return i;
}

static void
compute_newton_direction (const gsl_matrix * r, const gsl_permutation * perm,
                          const gsl_vector * qtf, gsl_vector * x)
{

  /* Compute and store in x the Gauss-Newton direction. If the
     Jacobian is rank-deficient then obtain a least squares
     solution. */

  const size_t n = r->size2;
  size_t i, j, nsing;

  for (i = 0 ; i < n ; i++)
    {
      double qtfi = gsl_vector_get (qtf, i);
      gsl_vector_set (x, i,  qtfi);
    }

  nsing = count_nsing (r);

#ifdef DEBUG
  printf("nsing = %d\n", nsing);
  printf("r = "); gsl_matrix_fprintf(stdout, r, "%g"); printf("\n");
  printf("qtf = "); gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
#endif

  for (i = nsing; i < n; i++)
    {
      gsl_vector_set (x, i, 0.0);
    }

  if (nsing > 0)
    {
      for (j = nsing; j > 0 && j--;)
        {
          double rjj = gsl_matrix_get (r, j, j);
          double temp = gsl_vector_get (x, j) / rjj;

          gsl_vector_set (x, j, temp);

          for (i = 0; i < j; i++)
            {
              double rij = gsl_matrix_get (r, i, j);
              double xi = gsl_vector_get (x, i);
              gsl_vector_set (x, i, xi - rij * temp);
            }
        }
    }

  gsl_permute_vector_inverse (perm, x);
}

static void
compute_newton_correction (const gsl_matrix * r, const gsl_vector * sdiag,
                           const gsl_permutation * p, gsl_vector * x,
                           double dxnorm,
                           const gsl_vector * diag, gsl_vector * w)
{
  size_t n = r->size2;
  size_t i, j;

  for (i = 0; i < n; i++)
    {
      size_t pi = gsl_permutation_get (p, i);

      double dpi = gsl_vector_get (diag, pi);
      double xpi = gsl_vector_get (x, pi);

      gsl_vector_set (w, i, dpi * (dpi * xpi) / dxnorm);
    }

  for (j = 0; j < n; j++)
    {
      double sj = gsl_vector_get (sdiag, j);
      double wj = gsl_vector_get (w, j);

      double tj = wj / sj;

      gsl_vector_set (w, j, tj);

      for (i = j + 1; i < n; i++)
        {
          double rij = gsl_matrix_get (r, i, j);
          double wi = gsl_vector_get (w, i);

          gsl_vector_set (w, i, wi - rij * tj);
        }
    }
}

static void
compute_newton_bound (const gsl_matrix * r, const gsl_vector * x,
                      double dxnorm,  const gsl_permutation * perm,
                      const gsl_vector * diag, gsl_vector * w)
{
  /* If the jacobian is not rank-deficient then the Newton step
     provides a lower bound for the zero of the function. Otherwise
     set this bound to zero. */

  size_t n = r->size2;

  size_t i, j;

  size_t nsing = count_nsing (r);

  if (nsing < n)
    {
      gsl_vector_set_zero (w);
      return;
    }

  for (i = 0; i < n; i++)
    {
      size_t pi = gsl_permutation_get (perm, i);

      double dpi = gsl_vector_get (diag, pi);
      double xpi = gsl_vector_get (x, pi);

      gsl_vector_set (w, i, dpi * (dpi * xpi / dxnorm));
    }

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i < j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (w, i);
        }

      {
        double rjj = gsl_matrix_get (r, j, j);
        double wj = gsl_vector_get (w, j);

        gsl_vector_set (w, j, (wj - sum) / rjj);
      }
    }
}

/* compute scaled gradient g = D^{-1} J^T f (see More' eq 7.2) */
static void
compute_gradient_direction (const gsl_matrix * r, const gsl_permutation * p,
                            const gsl_vector * qtf, const gsl_vector * diag,
                            gsl_vector * g)
{
  const size_t n = r->size2;

  size_t i, j;

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i <= j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
        }

      {
        size_t pj = gsl_permutation_get (p, j);
        double dpj = gsl_vector_get (diag, pj);

        gsl_vector_set (g, j, sum / dpj);
      }
    }
}

/* compute gradient g = J^T f */
static void
compute_gradient (const gsl_matrix * r, const gsl_vector * qtf,
                  gsl_vector * g)
{
  const size_t n = r->size2;

  size_t i, j;

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i <= j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
        }

      gsl_vector_set (g, j, sum);
    }
}

static int
lmpar (gsl_matrix * r, const gsl_permutation * perm, const gsl_vector * qtf,
       const gsl_vector * diag, double delta, double * par_inout,
       gsl_vector * newton, gsl_vector * gradient, gsl_vector * sdiag,
       gsl_vector * x, gsl_vector * w)
{
  double dxnorm, gnorm, fp, fp_old, par_lower, par_upper, par_c;

  double par = *par_inout;

  size_t iter = 0;

#ifdef DEBUG
  printf("ENTERING lmpar\n");
#endif

  compute_newton_direction (r, perm, qtf, newton);

#ifdef DEBUG
  printf ("newton = ");
  gsl_vector_fprintf (stdout, newton, "%g");
  printf ("\n");

  printf ("diag = ");
  gsl_vector_fprintf (stdout, diag, "%g");
  printf ("\n");
#endif

  /* Evaluate the function at the origin and test for acceptance of
     the Gauss-Newton direction. */

  dxnorm = scaled_enorm (diag, newton);

  fp = dxnorm - delta;

#ifdef DEBUG
  printf ("dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
#endif

  if (fp <= 0.1 * delta)
    {
      gsl_vector_memcpy (x, newton);
#ifdef DEBUG
      printf ("took newton (fp = %g, delta = %g)\n", fp, delta);
#endif

      *par_inout = 0;

      return GSL_SUCCESS;
    }

#ifdef DEBUG
  printf ("r = ");
  gsl_matrix_fprintf (stdout, r, "%g");
  printf ("\n");

  printf ("newton = ");
  gsl_vector_fprintf (stdout, newton, "%g");
  printf ("\n");

  printf ("dxnorm = %g\n", dxnorm);
#endif

  compute_newton_bound (r, newton, dxnorm, perm, diag, w);

#ifdef DEBUG
  printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");

  printf ("diag = ");
  gsl_vector_fprintf (stdout, diag, "%g");
  printf ("\n");

  printf ("w = ");
  gsl_vector_fprintf (stdout, w, "%g");
  printf ("\n");
#endif

  {
    double wnorm = enorm (w);
    double phider = wnorm * wnorm;

    /* w == zero if r rank-deficient,
       then set lower bound to zero form MINPACK, lmder.f
       Hans E. Plesser 2002-02-25 (hans.plesser@itf.nlh.no) */
    if ( wnorm > 0 )
      par_lower = fp / (delta * phider);
    else
      par_lower = 0.0;
  }

#ifdef DEBUG
  printf("par       = %g\n", par      );
  printf("par_lower = %g\n", par_lower);
#endif

  compute_gradient_direction (r, perm, qtf, diag, gradient);

  gnorm = enorm (gradient);

#ifdef DEBUG
  printf("gradient = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
  printf("gnorm = %g\n", gnorm);
#endif

  par_upper =  gnorm / delta;

  if (par_upper == 0)
    {
      par_upper = GSL_DBL_MIN / GSL_MIN_DBL(delta, 0.1);
    }

#ifdef DEBUG
  printf("par_upper = %g\n", par_upper);
#endif

  if (par > par_upper)
    {
#ifdef DEBUG
  printf("set par to par_upper\n");
#endif

      par = par_upper;
    }
  else if (par < par_lower)
    {
#ifdef DEBUG
  printf("set par to par_lower\n");
#endif

      par = par_lower;
    }

  if (par == 0)
    {
      par = gnorm / dxnorm;
#ifdef DEBUG
      printf("set par to gnorm/dxnorm = %g\n", par);
#endif

    }

  /* Beginning of iteration */

iteration:

  iter++;

#ifdef DEBUG
  printf("lmpar iteration = %d\n", iter);
#endif

#ifdef BRIANSFIX
  /* Seems like this is described in the paper but not in the MINPACK code */

  if (par < par_lower || par > par_upper)
    {
      par = GSL_MAX_DBL (0.001 * par_upper, sqrt(par_lower * par_upper));
    }
#endif

  /* Evaluate the function at the current value of par */

  if (par == 0)
    {
      par = GSL_MAX_DBL (0.001 * par_upper, GSL_DBL_MIN);
#ifdef DEBUG
      printf("par = 0, set par to  = %g\n", par);
#endif

    }

  /* Compute the least squares solution of [ R P x - Q^T f, sqrt(par) D x]
     for A = Q R P^T */

#ifdef DEBUG
  printf ("calling qrsolv with par = %g\n", par);
#endif

  {
    double sqrt_par = sqrt(par);

    qrsolv (r, perm, sqrt_par, diag, qtf, x, sdiag, w);
  }

  dxnorm = scaled_enorm (diag, x);

  fp_old = fp;

  fp = dxnorm - delta;

#ifdef DEBUG
  printf ("After qrsolv dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
  printf ("sdiag = ") ; gsl_vector_fprintf(stdout, sdiag, "%g"); printf("\n");
  printf ("x = ") ; gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
  printf ("r = ") ; gsl_matrix_fprintf(stdout, r, "%g"); printf("\nXXX\n");
#endif

  /* If the function is small enough, accept the current value of par */

  if (fabs (fp) <= 0.1 * delta)
    goto line220;

  if (par_lower == 0 && fp <= fp_old && fp_old < 0)
    goto line220;

  /* Check for maximum number of iterations */

  if (iter == 10)
    goto line220;

  /* Compute the Newton correction */

  compute_newton_correction (r, sdiag, perm, x, dxnorm, diag, w);

#ifdef DEBUG
  printf ("newton_correction = ");
  gsl_vector_fprintf(stdout, w, "%g"); printf("\n");
#endif

  {
    double wnorm = enorm (w);
    par_c = fp / (delta * wnorm * wnorm);
  }

#ifdef DEBUG
  printf("fp = %g\n", fp);
  printf("par_lower = %g\n", par_lower);
  printf("par_upper = %g\n", par_upper);
  printf("par_c = %g\n", par_c);
#endif

  /* Depending on the sign of the function, update par_lower or par_upper */

  if (fp > 0)
    {
      if (par > par_lower)
        {
          par_lower = par;
#ifdef DEBUG
      printf("fp > 0: set par_lower = par = %g\n", par);
#endif

        }
    }
  else if (fp < 0)
    {
      if (par < par_upper)
        {
#ifdef DEBUG
      printf("fp < 0: set par_upper = par = %g\n", par);
#endif
          par_upper = par;
        }
    }

  /* Compute an improved estimate for par */

#ifdef DEBUG
      printf("improved estimate par = MAX(%g, %g) \n", par_lower, par+par_c);
#endif

  par = GSL_MAX_DBL (par_lower, par + par_c);

#ifdef DEBUG
      printf("improved estimate par = %g \n", par);
#endif

  goto iteration;

line220:

#ifdef DEBUG
  printf("LEAVING lmpar, par = %g\n", par);
#endif

  *par_inout = par;

  return GSL_SUCCESS;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2018 - Matteo Ragni, Matteo Cocetti - University of Trento
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <cblas.h>
#include "libeuler.h"

/**
 * @brief Internal: Strut that will be passed to euler functions callbacks
 */
typedef struct euler_passthrough
{
  double ts;              /**< Integration steps. Taken from options struct */
  double alpha;           /**< Tustin coefficient. Taken from options struct */
  lapack_int u_offset;    /**< Input offset for \f$t+h\f$ callbacks. Taken from options struct */
  euler_ode_function f;   /**< Vector field to integrate. Taken from input struct */
  euler_ode_jacobian df;  /**< Jacobian of the vector field. Taken from options struct */
  const double *xk;       /**< Current state. Taken from parameters */
  lapack_int x_size;      /**< Ode dimension, taken from the input struct */
  double *work_f;         /**< Working space. Allocated in integration step */
  double *work_df;        /**< Working space. Allocated in integration step */
  void *data;             /**< User supplied data. Taken from parameters */
} euler_passtrough;

/**
 * @brief Euler implicit step wrapper
 *
 * The vector field wrapper implements the following:
 * \f{
 *   g(\cdot) = -x(t+h) + x(t) + (1-\alpha) h f(x(t), u_{1..u_{off}, p} +
 *            \alpha h f(x(t+h), u_{u_{off}..dim(u)}, p)
 * \f}
 * by using user supplied vector field callback. The implicit step search the root
 * of \f$g(\cdot)\f$.
 */
void euler_function_wrapper(double *f, const double t, const double *x, const double *u, const double **p, void *data);

/**
 * @brief Euler implicit step wrapper jacobian
 *
 * The vector field wrapper implements the following jacobian matrix:
 * \f{
 *   \nabla_{x(t+h)} g(\cdot) = -I + \alpha h \nabla_{x(t+h)} f(x(t+h), u_{u_{off}..dim(u)}, p)
 * \f}
 * by using user supplied vector field callback
 */
void euler_jacobian_wrapper(double *f, const double t, const double *x, const double *u, const double **p, void *data);

    euler_ret euler(const euler_options *opt, double *xp, const double t, const double *x, const double *u, const double **p, void *data)
{

  /* EXPLICIT IMPLEMENTATION */
/* Performin a very simple step if alpha == 0 */
if (opt->alpha == 0)
{
  opt->f(xp, t, x, u, p, opt->data);
  cblas_dscal(opt->x_size, opt->ts, xp, 1);
  cblas_daxpy(opt->x_size, 1, x, 1, xp, 1);
  return EULER_SUCCESS;
  }

  /* IMPLICIT IMPLEMENTTION */
  /* Allocating working memory */
  double *work_f = (double *)calloc(2 * opt->x_size, sizeof(double));
  if (!work_f)
    return EULER_EMALLOC;
  double *work_df = (double *)calloc(2 * opt->x_size * opt->x_size, sizeof(double));
  if(!work_df) {
    free(work_f);
    return EULER_EMALLOC;
  }
  /* Setting up the identity matrix in work_df second space, once forever */
  for (lapack_int i = 0; i < opt->x_size; i++)
    work_df[(opt->x_size * opt->x_size) + i + i * opt->x_size] = -1.0;

  /* Setting up options for Euler step */
  newton_options newton_opts = {
    opt->ordering, opt->x_size, opt->x_size,
    opt->s_tol, opt->x_tol, opt->max_iter,
    euler_function_wrapper,
    euler_jacobian_wrapper
  };

  euler_passtrough pt = {
    opt->ts, opt->alpha, opt->u_offset,
    opt->f, opt->df, x, opt->x_size,
    work_f, work_df,
    opt->data
  };

  cblas_dcopy(opt->x_size, x, 1, xp, 1);
  newton_ret nwt = newton_solve(&newton_opts, t, xp, u, p, ((void *)&pt));

  /* Freeing space */
  free(work_f);
  free(work_df);
  if (nwt > NEWTON_MAX_ITER)
    return EULER_GENERIC;
  return EULER_SUCCESS;
}

void euler_function_wrapper(double *f, const double t, const double *x, const double *u, const double **p, void *data) {
  euler_passtrough *_data = ((euler_passtrough *)data);

  /* Evaluating f(x(k), u(k)) and f(x(k+1), u(k+1)), storing result in work_f */
  _data->f(_data->work_f, t, _data->xk, u, p, _data->data);
  _data->f(_data->work_f + _data->x_size, t, x, u + _data->u_offset, p, _data->data);

  /* Computing: x(k) - x(k+1) + (1-alpha) ts f(x(k), u(k)) + alpha ts f(x(k+1), u(k+1)) */
  cblas_dscal(_data->x_size, (1 - _data->alpha) * _data->ts, _data->work_f, 1);
  cblas_dscal(_data->x_size, _data->alpha * _data->ts, _data->work_f + _data->x_size, 1);
  cblas_daxpy(_data->x_size, 1, _data->work_f + _data->x_size, 1, _data->work_f, 1);
  cblas_daxpy(_data->x_size, -1, x, 1, _data->work_f, 1);
  cblas_daxpy(_data->x_size, 1, _data->xk, 1, _data->work_f, 1);

  /* Copying result in output */
  cblas_dcopy(_data->x_size, _data->work_f, 1, f, 1);
}

void euler_jacobian_wrapper(double *df, const double t, const double *x, const double *u, const double **p, void *data) {
  euler_passtrough *_data = ((euler_passtrough *)data);

  /* Evaluating JAC(f)(x(k+1), u(k+1)) */
  _data->df(_data->work_df, t, x, u + _data->u_offset, p, _data->data);

  /* Computing: -I + alpha ts JAC(f)(x(k+1), u(k+1)) (still using level 1 Blas) */
  cblas_dscal(_data->x_size * _data->x_size, _data->alpha * _data->ts, _data->work_df, 1);
  cblas_daxpy(_data->x_size * _data->x_size, 1, _data->work_df + _data->x_size * _data->x_size, 1, _data->work_df, 1);

  /* Copying result in output */
  cblas_dcopy(_data->x_size * _data->x_size, _data->work_df, 1, df, 1);
}


/*
Multisteps: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. Redistributions in binary form must reproduce the above copyright notice,
		this list of conditions and the following disclaimer in the
		documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL Javier Burguete Tolosa OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/**
 * \file rk_4_4.c
 * \brief Source file to optimize Runge-Kutta 5 steps 4th order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_4_4.h"

#define DEBUG_RK_4_4 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 4 steps 4th order Runge-Kutta
 * method.
 */
int
rk_tb_4_4 (Optimize * optimize) ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_4_4
  fprintf (stderr, "rk_tb_4_4: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t4 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  t3 (tb) = 1.L;
  b43 (tb) = (0.25L - 1.L / 3.L * t1 (tb)
              - (1.L / 3.L - 0.5L * t1 (tb)) * t2 (tb))
    / (t3 (tb) * (t3 (tb) - t2 (tb)) * (t3 (tb) - t1 (tb)));
  if (isnan (b43 (tb)))
    return 0;
  b42 (tb) = (1.L / 3.L - 0.5L * t1 (tb)
              - b43 (tb) * t3 (tb) * (t3 (tb) - t1 (tb)))
    / (t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (b42 (tb)))
    return 0;
  b41 (tb) = (0.5L - b42 (tb) * t2 (tb) - b43 (tb) * t3 (tb)) / t1 (tb);
  if (isnan (b41 (tb)))
    return 0;
  b32 (tb) = (1.L / 12.L - 1.L / 6.L * t1 (tb))
    / (b43 (tb) * t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (b32 (tb)))
    return 0;
  b31 (tb) = ((0.125L - 1.L / 6.L * t2 (tb)) / (b43 (tb) * (t3 (tb) - t2 (tb)))
              - b32 (tb) * t2 (tb)) / t1 (tb);
  if (isnan (b31 (tb)))
    return 0;
  b21 (tb) = 1.L / 24.L / (t1 (tb) * b43 (tb) * b32 (tb));
  if (isnan (b21 (tb)))
    return 0;
  rk_b_4 (tb);
#if DEBUG_RK_4_4
  fprintf (stderr, "rk_tb_4_4: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 4th order, 5th order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_4_4t (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_4_4
  fprintf (stderr, "rk_tb_4_4t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t4 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = 0.5L * (t1 (tb) - 0.6L) / (t1 (tb) - 0.5L);
  t3 (tb) = 1.L;
  b43 (tb) = (0.25L - 1.L / 3.L * t1 (tb)
              - (1.L / 3.L - 0.5L * t1 (tb)) * t2 (tb))
    / (t3 (tb) * (t3 (tb) - t2 (tb)) * (t3 (tb) - t1 (tb)));
  if (isnan (b43 (tb)))
    return 0;
  b42 (tb) = (1.L / 3.L - 0.5L * t1 (tb)
              - b43 (tb) * t3 (tb) * (t3 (tb) - t1 (tb)))
    / (t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (b42 (tb)))
    return 0;
  b41 (tb) = (0.5L - b42 (tb) * t2 (tb) - b43 (tb) * t3 (tb)) / t1 (tb);
  if (isnan (b41 (tb)))
    return 0;
  b32 (tb) = (1.L / 12.L - 1.L / 6.L * t1 (tb))
    / (b43 (tb) * t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (b32 (tb)))
    return 0;
  b31 (tb) = ((0.125L - 1.L / 6.L * t2 (tb)) / (b43 (tb) * (t3 (tb) - t2 (tb)))
              - b32 (tb) * t2 (tb)) / t1 (tb);
  if (isnan (b31 (tb)))
    return 0;
  b21 (tb) = 1.L / 24.L / (t1 (tb) * b43 (tb) * b32 (tb));
  if (isnan (b21 (tb)))
    return 0;
  rk_b_4 (tb);
#if DEBUG_RK_4_4
  fprintf (stderr, "rk_tb_4_4t: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 4 steps 4th order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_4 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_4
  fprintf (stderr, "rk_objective_tb_4_4: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b21 (tb) < 0.L)
    o += b21 (tb);
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b31 (tb) < 0.L)
    o += b31 (tb);
  if (b32 (tb) < 0.L)
    o += b32 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_4
  fprintf (stderr, "rk_objective_tb_4_4: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_4: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 4 steps 4th order, 5th
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_4t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_4
  fprintf (stderr, "rk_objective_tb_4_4: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b21 (tb) < 0.L)
    o += b21 (tb);
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b31 (tb) < 0.L)
    o += b31 (tb);
  if (b32 (tb) < 0.L)
    o += b32 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b41 (tb) < 0.L)
    o += b41 (tb);
  if (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_4
  fprintf (stderr, "rk_objective_tb_4_4: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_4: end\n");
#endif
  return o;
}


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

#include <gsl/gsl_spmatrix.h>

int
main()
{
  gsl_spmatrix *A = gsl_spmatrix_alloc(5, 4); /* triplet format */
  gsl_spmatrix *B, *C;
  size_t i, j;

  /* build the sparse matrix */
  gsl_spmatrix_set(A, 0, 2, 3.1);
  gsl_spmatrix_set(A, 0, 3, 4.6);
  gsl_spmatrix_set(A, 1, 0, 1.0);
  gsl_spmatrix_set(A, 1, 2, 7.2);
  gsl_spmatrix_set(A, 3, 0, 2.1);
  gsl_spmatrix_set(A, 3, 1, 2.9);
  gsl_spmatrix_set(A, 3, 3, 8.5);
  gsl_spmatrix_set(A, 4, 0, 4.1);

  printf("printing all matrix elements:\n");
  for (i = 0; i < 5; ++i)
    for (j = 0; j < 4; ++j)
      printf("A(%zu,%zu) = %g\n", i, j,
             gsl_spmatrix_get(A, i, j));

  /* print out elements in triplet format */
  printf("matrix in triplet format (i,j,Aij):\n");
  gsl_spmatrix_fprintf(stdout, A, "%.1f");

  /* convert to compressed column format */
  B = gsl_spmatrix_ccs(A);

  printf("matrix in compressed column format:\n");
  printf("i = [ ");
  for (i = 0; i < B->nz; ++i)
    printf("%d, ", B->i[i]);
  printf("]\n");

  printf("p = [ ");
  for (i = 0; i < B->size2 + 1; ++i)
    printf("%d, ", B->p[i]);
  printf("]\n");

  printf("d = [ ");
  for (i = 0; i < B->nz; ++i)
    printf("%g, ", B->data[i]);
  printf("]\n");

  /* convert to compressed row format */
  C = gsl_spmatrix_crs(A);

  printf("matrix in compressed row format:\n");
  printf("i = [ ");
  for (i = 0; i < C->nz; ++i)
    printf("%d, ", C->i[i]);
  printf("]\n");

  printf("p = [ ");
  for (i = 0; i < C->size1 + 1; ++i)
    printf("%d, ", C->p[i]);
  printf("]\n");

  printf("d = [ ");
  for (i = 0; i < C->nz; ++i)
    printf("%g, ", C->data[i]);
  printf("]\n");

  gsl_spmatrix_free(A);
  gsl_spmatrix_free(B);
  gsl_spmatrix_free(C);

  return 0;
}