//---------------------------------------------------------------------------//
//!
//! \file   Utility_GaussRadauQuadratureSet.cpp
//! \author Luke Kersting
//! \brief  Gauss-Radau quadrature set
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "Utility_GaussRadauQuadratureSet.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_ContractException.hpp"

namespace Utility{

// Constructor
GaussRadauQuadratureSet::GaussRadauQuadratureSet(
                              boost::function<double (double, int)>
                                polynomial_expansion_function,
                              const double error_tol,
                              const int polynomial_order )
  : d_polynomial_expansion_function( polynomial_expansion_function ),
    d_error_tol( error_tol ),
    d_polynomial_order( polynomial_order )
{
  // Make sure the error tolerances are valid
  testPrecondition( error_tol >= 0.0 );

  // Make sure the work space size is valid
  testPrecondition( polynomial_order > 0 );
}

// Caluclate the nth order Jacobi Polynomial at x
/* \details The Jacobi Polynomials can be calculated by the following recursion
 * relationship:
 * a1_n P_{n+1}^{\alpha,\beta}(x) = (a2_n + a3_n x)P_n^{\alpha,\beta}(x) - a4_n P_{n-1}^{\alpha,\beta}(x)
 * where:
 * a1_n = 2(n+1)(n+\alpha+\beta+1)(2n+\alpha+\beta)
 * a2_n = (2n+\alpha+\beta+1)(\alpha^2-\beta^2)
 * a3_n = (2n + \alpha + \beta)(2n + \alpha + \beta + 1)(2n + \alpha +\beta + 2)
 * a4_n = 2(n+\alpha)(n+\beta)(2n+\alpha+\beta+2)
 */
double GaussRadauQuadratureSet::getJacobiPolynomial(
                              double x,
                              int n,
                              int alpha,
                              int beta ) const
{
  // Calculate the first two polynomials
  double P_0 = 1.0;
  double P_1 = 0.5*( alpha - beta + ( alpha + beta + 2 )* x );

  if (n==0)
  {
    return P_0;
  }
  if (n==1)
  {
    return P_1;
  }
  else
  {
    int a_b = alpha + beta;
    int a_b_1 = a_b + 1;
    int a_b_2 = a_b + 2;

    double p_i_minus_2 = P_0;
    double p_i_minus_1 = P_1;
    double p_i;

    // Use recursion relation to calculate higher order Jacobi polynomials
    for (int i = 1; i < n; ++i)
    {
      int two_i = 2*i;

      // Calculate the Jacobi coefficients
      double a1_i = 2.0*( i + 1.0 )*( i + a_b_1 )*( two_i + a_b );
      double a2_i = ( two_i + a_b_1 )*( alpha*alpha - beta*beta );
      double a3_i = ( two_i + a_b )*( two_i + a_b_1 )*( two_i + a_b_2 );
      double a4_i = 2.0*( i + alpha )*( i + beta )*( two_i + a_b_2 );

      // Calculate new P_n value
      p_i = 1.0/a1_i*( ( a2_i + a3_i*x )*p_i_minus_1 - a4_i*p_i_minus_2 );

      // Update P_n_minus_1 and P_n_minus_2 for next value on n
      p_i_minus_2 = p_i_minus_1;
      p_i_minus_1 = p_i;
    }

  return p_i;
  }
}

double GaussRadauQuadratureSet::getLegendrePolynomial(
                                        double x,
                                        int n ) const
{
  return getJacobiPolynomial( x, n, 0, 0 );
}

// Calculate the derivative of the nth order Jacobi Polynomial at x
/* \details The Jacobi Polynomials can be calculated by the following recursion
 * relationship:
 * 1/2( n + \alpha + \beta + 1 )P_{n-1}^{\alpha + 1,\beta + 1}(x)
 */
double GaussRadauQuadratureSet::getJacobiPolynomialDerivative(
                              double x,
                              int n,
                              int alpha,
                              int beta ) const
{

  if (n==0)
  {
    return 0.0;
  }
  if (n==1)
  {
    return 0.5*( alpha + beta + 2.0 );
  }
  else
  {
    return 0.5 * (alpha + beta + n + 1.0) * getJacobiPolynomial( x,
                                                                 n-1,
                                                                 alpha + 1,
                                                                 beta + 1 );
  }
}

// Estimate the roots of the Jacobi Polynomial
/* \details The roots of the Jacobi Polynomials can be estimated by the roots of
 * the Chebyshev Polynomials which are given by the relationship:
 * x_k = cos( (2k - 1)\pi/2n ) , k = 1, ... , n
 */
void GaussRadauQuadratureSet::getJacobiPolynomialRoots(
                                 Teuchos::Array<double>& roots,
                                 const int n,
                                 int alpha,
                                 int beta ) const
{
  // Max number of allowed iterations
  int max_iterations = 200;

  int iteration;
  double root_k, s, jacobi, jacobi_derivative, delta_root;

  // Iterate through all n roots ( 0 < k < n )
  for (int k = 0; k < n; k++)
  {
    // Make an initial guess that the roots are equal to the roots of the Chebyshev Polynomial
    root_k = -cos( ( 2.0*k + 1.0 )/( 2.0 * n )* PhysicalConstants::pi );

    // Actual root is known to be inbetween roots of Chebyshev Polynomial
    if (k > 0)
    {
      root_k = ( root_k + roots[k-1] )/2.0;
    }

    iteration = 0;

    // Iterate until you converge on root
    do
    {
      s = 0;

      for (int i = 0; i < k; i++)
      {
        s += 1.0/( root_k - roots[i] );
      }

      // Get error
      jacobi = getJacobiPolynomial( root_k, n, alpha, beta );
      jacobi_derivative =
               getJacobiPolynomialDerivative( root_k, n, alpha, beta );\

      delta_root = -jacobi/( jacobi_derivative - jacobi*s );

      // Update root value
      root_k += delta_root;

      // Update iteration
      ++iteration;

      if ( iteration > max_iterations )
         break;
/*
      TEST_FOR_EXCEPTION( iteration > max_iterations,
		          RadauQuadratueError,
		          "Error: the root of the Jacobi Polynomial "
                          "did not converge" );
*/
    }
    while ( fabs(delta_root) > d_error_tol );

    roots[k] = root_k;
  }
}

// Find the Radau nodes and wieghts including at end point -1 or 1
void GaussRadauQuadratureSet::findNodesAndWeights(
                            double end_point,
                            Teuchos::Array<double>& nodes,
                            Teuchos::Array<double>& weights ) const
{
  // Make sure end_point is either -1 or 1
  testPrecondition( fabs(end_point) == 1.0 );

  int n = d_polynomial_order;
  int alpha = 0;
  int beta = 1;
  double jacobi_derivative;

  Teuchos::Array<double> roots( n );

  // Calculate the roots of the Jacobi Polynomial
  getJacobiPolynomialRoots( roots, n, alpha, beta );

  // Check to see if the end point is the first node
  if ( end_point < roots[0] )
  {
    nodes[0] = end_point;
    weights[0] = findWeightAtEndPoint( end_point, n );

    // Iterate through the all nodes and weights
    for ( int i = 0; i < n; ++i )
    {
      nodes[i+1] = roots[i];

      weights[i+1] = findWeightAtNode( roots[i], end_point, n );
    }
  }
  else
  {
    nodes[n] = end_point;
    weights[n] = findWeightAtEndPoint( end_point, n );

    // Iterate through all other nodes and weights
    for ( int i = 0; i < n; ++i )
    {
      nodes[i] = roots[i];

      weights[i] = findWeightAtNode( roots[i], end_point, n );
    }
  }
}

// Find the Radau nodes and wieghts including at end point -1 or 1
void GaussRadauQuadratureSet::findNodesAndPositiveWeights(
                            double end_point,
                            Teuchos::Array<double>& nodes,
                            Teuchos::Array<double>& weights ) const
{
  // Make sure end_point is either -1 or 1
  testPrecondition( fabs(end_point) == 1.0 );

  int n = d_polynomial_order;
  int alpha = 0;
  int beta = 1;
  double jacobi_derivative;

  Teuchos::Array<double> roots( n );

  // Calculate the roots of the Jacobi Polynomial
  getJacobiPolynomialRoots( roots, n, alpha, beta );

  // Check to see if the end point is the first node
  if ( end_point < roots[0] )
  {
    nodes[0] = end_point;
    weights[0] = findWeightAtEndPoint( end_point, n );

    // Iterate through the all nodes and weights
    for ( int i = 0; i < n; ++i )
    {
      nodes[i+1] = roots[i];

      jacobi_derivative =
              getJacobiPolynomialDerivative( nodes[i+1], n, alpha, beta );

      weights[i+1] = findWeightAtNode( roots[i], end_point, n );
    }
  }
  else
  {
    nodes[n] = end_point;
    weights[n] = findWeightAtEndPoint( -end_point, n );

    // Iterate through all other nodes and weights
    for ( int i = 0; i < n; ++i )
    {
      nodes[n-1-i] = -roots[i];

      // Get the derivative of the Jacobi Polynomial of order n
      jacobi_derivative =
              getJacobiPolynomialDerivative( roots[i],
                                             n,
                                             alpha, beta );

      weights[n-1-i] = findWeightAtNode( roots[i], -end_point, n );
    }
  }
}

// Create the integrand function for the weight at the end point -1 or 1
double GaussRadauQuadratureSet::getFixedWeightIntegrand(
                               double end_point,
                               int n ) const
{
  return d_polynomial_expansion_function( end_point, n )*getJacobiPolynomial( end_point, n );
}

// Create the integrand function for the weight at a given node with end point -1 or 1
double GaussRadauQuadratureSet::getWeightIntegrand(
                               double x,
                               double node,
                               double end_point,
                               int n ) const
{
  return getFixedWeightIntegrand( x, n )*( x - end_point )/( x - node );
}

// Find the Radau wieght for the function at the end point -1 or 1
double GaussRadauQuadratureSet::findWeightAtEndPoint( double end_point,
                                                         int n ) const
{
  // Make sure end_point is either -1 or 1
  testPrecondition( fabs(end_point) == 1.0 );
  // Make sure n is positive
  testPrecondition( n > 0.0 );

  boost::function<double (double x)> weight_function =
    boost::bind<double>( &GaussRadauQuadratureSet::getFixedWeightIntegrand,
			 boost::cref( *this ),
			 _1,
			 n );

  double abs_error, result;
  double precision = 1e-12;

  Utility::GaussKronrodIntegrator integrator( precision );

  integrator.integrateAdaptively<15>(
					weight_function,
					-1.0,
					1.0,
					result,
					abs_error );

  return result/getJacobiPolynomial( end_point, n );

}

// Find the Radau wieght for the function at a given node
double GaussRadauQuadratureSet::findWeightAtNode( double node,
                                                     double end_point,
                                                     int n ) const
{
  // Make sure end_point is either -1 or 1
  testPrecondition( fabs(end_point) == 1.0 );
  // Make sure node is between -1 and 1
  testPrecondition( node < 1.0 );
  testPrecondition( node > -1.0 );
  // Make sure n is positive
  testPrecondition( n > 0.0 );

  boost::function<double (double x)> weight_function =
    boost::bind<double>( &GaussRadauQuadratureSet::getWeightIntegrand,
			 boost::cref( *this ),
			 _1,
                         node,
                         end_point,
			 n );

  double abs_error, result;
  double precision = 1e-12;

  Utility::GaussKronrodIntegrator integrator( precision );

  integrator.integrateAdaptively<15>(
					weight_function,
					-1.0,
					1.0,
					result,
					abs_error );
  return result/
           ( getJacobiPolynomialDerivative( node, n ) * ( node - end_point) );

}

} // end Utility namespace

//---------------------------------------------------------------------------//
// end Utility_GaussRadauQuadratureSet.cpp
//---------------------------------------------------------------------------//


/* Implementation of the algorithm
 *
  D. Conte, L. Ixaru, B. Paternoster, G. Santomauro:
    Exponentially-fitted Gauss–Laguerre quadrature rule for integrals over an unbounded interval.

  https://hpcquantlib.wordpress.com/2020/05/17/optimized-heston-model-integration-exponentially-fitted-gauss-laguerre-quadrature-rule/

    Copyright (c) 2020, Klaus Spanderen
    All rights reserved.

    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.

    3. Neither the names of the copyright holders nor the names of the QuantLib
    Group and its contributors may be used to endorse or promote products
    derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT HOLDER 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.
*/

#include <ql/math/integrals/gaussianquadratures.hpp>

#include <cmath>
#include <iostream>
#include <fstream>
#include <queue>

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/unordered_map.hpp>
#include <boost/multiprecision/gmp.hpp>
#include <boost/thread.hpp>
#include <boost/bind.hpp>

using namespace boost::numeric::ublas;

//typedef boost::multiprecision::number<
//            boost::multiprecision::cpp_dec_float<400> > Float;

typedef boost::multiprecision::number<
            boost::multiprecision::gmp_float<600> > Float;

template <class real>
boost::numeric::ublas::vector<real> lu(
    const boost::numeric::ublas::matrix<real>& A,
    const boost::numeric::ublas::vector<real>& b) {

    matrix<real> A_fac = A;

    permutation_matrix<std::size_t> piv(b.size());
    int singular = lu_factorize(A_fac, piv);

    if (singular)
        throw std::runtime_error("lu: A is singular.");

    vector<real> x = b;
    lu_substitute(A_fac, piv, x);

    return x;
}

template <class real>
boost::numeric::ublas::matrix<real> inv(
    const boost::numeric::ublas::matrix<real>& A) {

    using namespace boost::numeric::ublas;

    matrix<real> A_fac = A;

    permutation_matrix<std::size_t> piv(A.size1());
    int singular = lu_factorize(A_fac, piv);

    if (singular)
        throw std::runtime_error("lu: A is singular.");

    matrix<real> inverse = identity_matrix<real>(A.size1());

    lu_substitute(A_fac, piv, inverse);

    return inverse;
}

Float pow_c(const Float& x, int n) {

    if (n == 0)
        return Float(1.0);

    if (n == 1)
        return x;

    typedef std::pair<int, Float> key_type;

    typedef boost::unordered_map<key_type, Float> ResultMap;

    static boost::mutex mutex;

    static std::size_t maxCacheSize = 16384;

    static ResultMap results;
    static std::queue<key_type> fifo;

    const key_type key(n, x);
    {
        boost::lock_guard<boost::mutex> lock(mutex);

        const typename ResultMap::const_iterator iter = results.find(key);

        if (iter != results.end())
            return iter->second;
    }

    const Float result = (n < 0)
                ? Float(1.0)/pow_c(x, -n)
                : Float(pow_c(x, n/2) * pow_c(x, n/2) * pow_c(x, n - 2*(n/2)));

    boost::lock_guard<boost::mutex> lock(mutex);

    results.emplace(key, result);
    fifo.push(key);

    while(fifo.size() > maxCacheSize) {
        results.erase(fifo.front());
        fifo.pop();
    }

    return result;
}

Float pow_i(long n) {
    if (n < 0)
        return pow_c(Float(2), n);
    else if (n < 8*sizeof(unsigned long))
        return Float(1uL << n);
    else
        return pow_c(Float(2), n);
}

template <class result_type, class float_type>
result_type eta(int m, float_type Z) {

    typedef std::pair<int, float_type> key_type;
    typedef boost::unordered_map<key_type, result_type> ResultMap;

    static boost::mutex mutex;

    static std::size_t maxCacheSize = 16384;

    static ResultMap results;
    static std::queue<key_type> fifo;

    const key_type key(m, Z);
    {
        boost::lock_guard<boost::mutex> lock(mutex);
        const typename ResultMap::const_iterator iter = results.find(key);

        if (iter != results.end()) {
            return iter->second;
        }
    }

    result_type result;

    if (m == 0) {
        if (Z < 0) {
            const result_type sz = sqrt(-Z);
            result = sin(sz)/sz;
        }
        else if (Z > 0) {
            const result_type sz = sqrt(Z);
            result = sinh(sz)/sz;
        }
        else
            result = result_type(1.0);
    }
    else if (m == -1)
        if (Z <= 0)
            result = cos(sqrt(-Z));
        else
            result = cosh(sqrt(Z));
    else
        result = (eta<result_type, float_type>(m-2, Z)
            - (2*m-1)*eta<result_type, float_type>(m-1, Z))/Z;

    boost::lock_guard<boost::mutex> lock(mutex);
    results.emplace(key, result);
    fifo.push(key);

    while(fifo.size() > maxCacheSize) {
        results.erase(fifo.front());
        fifo.pop();
    }

    return result;
}

template <class real>
real factorial(std::size_t n) {
    static std::vector<real> cache(1, real(1));

    if (cache.size() > n)
        return cache[n];

    const real val = real(n) * factorial<real>(n-1);
    cache.resize(n+1);

    return cache[n] = val;
}

template <class real>
vector<real> w_lin(const vector<real>& x, const real& Z) {
    const int N = x.size();

    const int s = N/2;
    const int r = N-s;

    matrix<real> A(N, N);
    int row = 0;

    vector<real> b(x.size());

    for (int n=r+1; n <= N; ++n) {
        for (int k=0; k < N; ++k)
            A(row, k) = pow_c(x(k), 2*n-2)*eta<real, real>(n-2, x(k)*x(k)*Z);

        b(row) = pow_i(n-1)*factorial<real>(n-1)/pow_c(1.0-Z, n);
        ++row;
    }

    for (int n=s+1; n <= N; ++n) {
        for (int k=0; k < N; ++k)
            A(row, k) = pow_c(x(k), 2*n-1)*eta<real, real>(n-1, x(k)*x(k)*Z);

        b(row) = pow_i(n-1)*factorial<real>(n-1)/pow_c(1.0-Z, n);
        ++row;
    }

    return lu(A,b);
}

class WorkerJxA {
  public:
    WorkerJxA(
        matrix<Float>& JxA, const vector<Float>& w,
        const vector<Float>& x, const Float& Z)
    : JxA_(JxA), w_(w), x_(x), Z_(Z) {}

    void run() const {
        const int N = JxA_.size1();
        const int s = N/2;
        const int r = N-s;

        for (int j=0; j < N; ++j) {
            const Float xxZ = x_(j)*x_(j)*Z_;

            for (int i=1; i <= N; ++i) {
                JxA_(i-1, j) = (i <= s)
                    ? pow_c(x_(j), 2*(i+r)-3)*(
                        2*(i+r-1)*eta<Float, Float>(i+r-2, xxZ)
                        + xxZ*eta<Float, Float>(i+r-1, xxZ))
                    : pow_c(x_(j), 2*(i-1))*(
                        (2*i-1)*eta<Float, Float>(i-1, xxZ)
                        + xxZ*eta<Float, Float>(i, xxZ));
                JxA_(i-1, j) *= -w_(j);
            }
        }
    }

  private:
    matrix<Float>& JxA_;
    const vector<Float>& w_, x_;
    const Float& Z_;
};

class WorkerInvA {
  public:
    WorkerInvA(matrix<Float>& A, const vector<Float>& x, const Float& Z)
    : A_(A), x_(x), Z_(Z) { }

    void run() const {
        const int N = A_.size1();
        const int s = N/2;
        const int r = N-s;

        for (int j=0; j < N; ++j) {
            const Float xxZ = x_(j)*x_(j)*Z_;
            for (int i=1; i <= N; ++i)
                A_(i-1, j) = (i <= s)
                   ? pow_c(x_(j), 2*(i+r-1))*eta<Float, Float>(i+r-2, xxZ)
                   : pow_c(x_(j), 2*i-1    )*eta<Float, Float>(i-1,   xxZ);
        }

        A_ = inv(A_);
    }

  private:
    matrix<Float>& A_;
    const vector<Float>& x_;
    const Float& Z_;
};

class WorkerC {
  public:
    WorkerC(matrix<Float>& C, const vector<Float>& w,
            const vector<Float>& x, const Float& Z)
    : C_(C), w_(w), x_(x), Z_(Z) {}

    void run() const {
        const int N = C_.size1();
        const int s = N/2;
        const int r = N-s;

        for (int k=0; k < N; ++k) {
            const Float xxZ = x_(k)*x_(k)*Z_;
            for (int i=1; i <= N; ++i) {
                C_(i-1, k) = (i <= r)
                    ? pow_c(x_(k), 2*i-3)*( (2*i-2)*eta<Float, Float>(i-2, xxZ)
                        + xxZ*eta<Float, Float>(i-1, xxZ) )
                    : pow_c(x_(k), 2*(i-r-1))*( (2*(i-r)-1)*eta<Float, Float>(i-r-1, xxZ)
                        + xxZ*eta<Float, Float>(i-r, xxZ) );
                C_(i-1, k) *= w_(k);
            }
        }
    }
  private:
    matrix<Float>& C_;
    const vector<Float>& w_, x_;
    const Float& Z_;
};

class WorkerD {
  public:
    WorkerD(matrix<Float>& D, vector<Float>& dZ, const vector<Float>& x, const Float& Z)
    : D_(D), dZ_(dZ), x_(x), Z_(Z) {}

    void run() const {
        const int N = D_.size1();
        const int s = N/2;
        const int r = N-s;

        for (int k=0; k < N; ++k) {
            const Float xxZ = x_(k)*x_(k)*Z_;
            for (int i=1; i <= N; ++i)
                D_(i-1, k) = ( i <= r)
                    ? pow_c(x_(k), 2*i-2)    *eta<Float, Float>(i-2, xxZ)
                    : pow_c(x_(k), 2*(i-r)-1)*eta<Float, Float>(i-r-1, xxZ);
        }

        const Float omz(1-Z_);

        for (int i=1; i <= N; ++i)
            dZ_(i-1) = (i <= r)
                ? pow_i(i-1)*factorial<Float>(i-1) / pow_c(omz, i)
                : pow_i(i-r-1)*factorial<Float>(i-r-1) / pow_c(omz, i-r);
    }

  private:
    matrix<Float>& D_;
    vector<Float>& dZ_;
    const vector<Float>& x_;
    const Float& Z_;
};

template <class real>
vector<real> newton_iter(const vector<real>& w, const vector<real>& x, const real& Z) {
    const int N = x.size();
    const int s = N/2;
    const int r = N-s;

    matrix<real> invA(N, N);
    WorkerInvA workerInvA(invA, x, Z);

    boost::thread invA_thread(&WorkerInvA::run, &workerInvA);

    matrix<real> JxA(N, N);

    WorkerJxA workerJxA(JxA, w, x, Z);
    boost::thread JxA_thread(&WorkerJxA::run, &workerJxA);

    matrix<real> C(N, N);
    WorkerC workerC(C, w, x, Z);
    boost::thread C_thread(&WorkerC::run, &workerC);

    matrix<real> D(N, N);
    vector<real> dZ(N);
    WorkerD workerD(D, dZ, x, Z);
    boost::thread D_thread(&WorkerD::run, &workerD);

    JxA_thread.join();
    invA_thread.join();
    const matrix<real> JxW = prod(invA, JxA);

    C_thread.join();
    D_thread.join();

    const matrix<real> B = C + prod(D, JxW);

    return lu(B, vector<real>(prod(D, w) - dZ));
}

template <class real>
vector<real> newton(vector<real>& x, real Z) {
    const static real eps = Float(1e-300);

    const std::size_t N = x.size();

    vector<real> w(N), dx;

    do {
        w = w_lin(x, Z);

        dx = newton_iter(w, x, Z);

        x = x - dx;

        std::cout << norm_2(dx) << std::endl;

        for (std::size_t i=0; i < N; ++i)
            if (x(i) < 0.0) {
                return vector<real>();
            }
    }
    while (norm_2(dx) > eps);

    return w;
}

bool greaterThan(vector<Float>& x, vector<Float>& y) {
    bool f = false;

    for (std::size_t i=0; i < x.size(); ++i) {
        if (x[i] >= y[i]) {
            f = true;
        }
    }
    return f;
}

int main() {

    const std::size_t n = 64;
    const std::size_t maxOrder = 45;

    const QuantLib::Array x_laguerre =
        QuantLib::GaussLaguerreIntegration(n).x();
    const QuantLib::Array w_laguerre =
        QuantLib::GaussLaguerreIntegration(n).weights();

    std::vector<vector<Float> > x(maxOrder, vector<Float>(n));
    std::copy(x_laguerre.begin(), x_laguerre.end(), x[0].begin());

    std::ofstream f("values.txt");
    f << std::setprecision(std::numeric_limits<double>::digits10 + 1)
        << "{ 0.0";
    for (std::size_t i = 0; i < n; ++i)
        f << ", " << x_laguerre[i];
    for (std::size_t i = 0; i < n; ++i)
        f << ", " << w_laguerre[i];
    f << " }," << std::endl;
    f.flush();

    vector<Float> xGuess;
    std::vector<Float> o(maxOrder, Float(0.0));
    o[0] = 0.01;

    std::size_t iter = 0;

    vector<Float> w;
    w = newton(x[0], Float(-o[0]*o[0]));

    while (o[0] < 50.0) {

        ++iter;
        const std::size_t order = std::min(maxOrder, iter);

        vector<Float> xTest(n);

        const Float m1 = o[0] + Float(0.01);
        const Float m2 = o[0]*(1 + 0.0075);

        Float nomega = (m1 > m2)? m1 : m2;

        do {
            const Float z = -nomega*nomega;

            for (std::size_t i=0; i < order; ++i) {
                Float l=1.0;
                for (std::size_t j=0; j < order; ++j)
                    if (i != j)
                        l *= (nomega-o[j])/(o[i]-o[j]);

                xTest += x[i]*l;
            }

            xGuess = xTest;

            w = newton(xTest, z);

            if (w.size() == 0) {
                std::cout << "opps, monotocity violation " << nomega;
                nomega = o[0] + 0.5*(nomega - o[0]);
                std::cout << " new " << nomega << std::endl;
            }
        } while (w.size() == 0);

        std::cout << "start norm " << nomega << " " << norm_2(xGuess - xTest) << std::endl;

        if (greaterThan(xTest, x[0]))
            std::cout << "wrong direction" << std::endl;

        for (int i=std::min(maxOrder-1, order); i > 0; --i) {
            x[i] = x[i-1];
            o[i] = o[i-1];
        }

        x[0] = xTest;
        o[0] = nomega;

        Float s=0;
        for (std::size_t i=0; i < n; ++i)
            s+=w(i)*(x[0](i)*cos(o[0]*x[0](i)) + x[0](i)*sin(o[0]*x[0](i)));

        const Float expected = (1+2*o[0]-o[0]*o[0])/(1+o[0]*o[0])/(1+o[0]*o[0]);
        if (abs(s - expected) > 1e-16) {
            std::cout << "integration error " << abs(s - expected) << std::endl;
            exit(-1);
        }

        f << std::setprecision(std::numeric_limits<double>::digits10 + 1)
            << "{ " << o[0];
        for (std::size_t i = 0; i < n; ++i)
            f << ", " << x[0](i);
        for (std::size_t i = 0; i < n; ++i)
            f << ", " << exp(x[0](i))*w(i);
        f << " }," << std::endl;
        f.flush();
    }
    f.close();
}


#pragma once
#include <Eigen/Core>
#include "datatypes.hpp"
#include <Eigen/Eigen>
#include "profile.hpp"
#include "integration.hpp"
#include "transformation.hpp"

namespace gd {

using namespace std;
USING_PART_OF_NAMESPACE_EIGEN

class Mesh1d {
public:
	virtual int findindex(double) { return 0; }
	virtual double indexto_x(int) { return 0; }
	//virtual void indexto_x(int) { return 0; }
	virtual bool inrange(double) { return false; }
	virtual int length() {return 0;}
};

template<class Base=Mesh1d>
class Mesh1dRegular : public Base {
public:
	Mesh1dRegular(double x1, double x2, int length) : x1(x1), x2(x2), _length(length) {}
	int findindex(double x) {
		return x < x1 ? 0 :
			(x >= x2 ? _length-1 : (int)((x-x1)/(x2-x1)*(_length)));
	}
	double indexto_x(int i) {
		return x1 + (x2-x1)/length()*i;
	}
	bool inrange(double r) {
		return (r >= x1) && (r < x2);
	}
	int length() { return _length; }
	double x1, x2;
	int _length;
};

template<class T=double>
class BasisTriLeft {
public:
	T operator()(T x) {
		return x >= -1 ? (x < 0 ? (x+1) : 0) : 0;
	}
};

template<class T=double>
class BasisTriRight {
public:
	T operator()(T x) {
		return x >= 0 ? (x < 1 ? (1-x) : 0) : 0;
	}
};

template<class T=double>
class BasisTriMesh1dRegular {
public:
	BasisTriMesh1dRegular(T _x1, T _x2, int _n_nodes) : x1(_x1), x2(_x2), n_nodes(_n_nodes), mesh(_x1, _x2, _n_nodes), ctrans(_n_nodes+1,_n_nodes+1) {
		MatrixXd m = MatrixXd::Zero(_n_nodes+1,_n_nodes+1);
		//m.setZero();
		m(0,0) = 1./3;
		m(_n_nodes,_n_nodes) = 1./3;
		for(int i = 1; i < (_n_nodes+1); i++) {
			m(i, i-1) = 1./6;
			m(i-1, i) = 1./6;
		}
		for(int i = 1; i < (_n_nodes); i++) {
			m(i, i) = 2./3;
		}
		T scale = (x2-x1)/n_nodes;
		m = m * scale;
		//cout << m << endl;
		ctrans = m.inverse();
		//cout << ctrans << endl;
	}
	void testprofile(Profile* profile, double_vector v, bool dotrans) {
		double* vp = v.data().begin();
		double* array = vp;
		//int size = x.size();
		//double scale = 1./size;
		//cout << "size = " << size << endl;
		for(int i = 0; i < n_nodes; i++) {
			double integral = 0;
			T xleft = mesh.indexto_x(i);
			T xright = mesh.indexto_x(i+1);
			T dx = (xright-xleft);
			{
				BasisTriRight<T> triright;
				auto f = [&](double x) { return triright((x-xleft)/dx) * profile->densityr(x); };
				IntegratorGSL<> integratorGSL(f); // the integrator
				integral = integratorGSL.integrate(xleft, xright);
				array[i] +=  integral;
			}
			{
				BasisTriLeft<T> trileft;
				auto f = [&](double x) { return trileft((x-xleft)/dx-1) * profile->densityr(x); };
				IntegratorGSL<> integratorGSL(f); // the integrator
				integral = integratorGSL.integrate(xleft, xright);
				array[i+1] +=  integral;
			}
		}
		if(dotrans) {
			VectorXd v_alias = VectorXd::Map(v.data().begin(), v.size());
			VectorXd vtrans = ctrans * v_alias;
			VectorXd::Map(v.data().begin(), v.size()) = vtrans;
		}
	}

	void test(double_vector x, double_vector y, double_vector v, bool dotrans) {
		double* xp = x.data().begin();
		double* yp = y.data().begin();
		double* vp = v.data().begin();
		int size = x.size();
		double scale = 1./size * (x2-x1);
		//cout << "size = " << size << endl;
		for(int i = 0; i < size; i++) {
			//cout << "i = " << i << endl;
			this->operator()(xp[i], yp[i]*scale, vp);

		}
		if(dotrans) {
			VectorXd v_alias = VectorXd::Map(v.data().begin(), v.size());
			VectorXd vtrans = ctrans * v_alias;
			VectorXd::Map(v.data().begin(), v.size()) = vtrans;
		}
	}

	template<class A>
	T operator()(T x, T y, A array) {
		if(mesh.inrange(x)) {
			int indexleft = mesh.findindex(x);
			T xleft = mesh.indexto_x(indexleft);
			T xright = mesh.indexto_x(indexleft+1);
			T fraction = (x-xleft)/(xright-xleft);
			//cout << "x = " << x << " xleft = " << xleft << " xright = " << xright << " fraction = " << fraction << endl;
			BasisTriLeft<T> trileft;
			BasisTriRight<T> triright;
			//cout << "y = " << y << " triright(fraction) = " << triright(fraction) << " trileft (fraction-1) = " << trileft (fraction-1) << endl;
			array[indexleft]   +=  triright(fraction) * y;
			array[indexleft+1] +=  trileft (fraction-1) * y;
			// TODO: use trileft(fraction) + triright(fraction) == 1
		}
		return 0; // TODO: not finished.., remove code?
	}

	T x1, x2;
	int n_nodes;
	Mesh1dRegular<> mesh;
	MatrixXd ctrans;
};

template<class Basis, class T=double>
class MeshRegularNodal1d {
public:
	typedef Basis basis_type;
	T x1, x2;
	int n_cells;
	Mesh1dRegular<> mesh;
	MatrixXd ctrans;
	Transformation1d_in_3d* transformation;
	int dof;
	enum { dof_per_cell = Basis::degree+1 };

	int get_dof() { return dof;}
	int get_n_cells() { return n_cells;}
	int dof_index(int cell_index, int local_index) {
		return  cell_index*(dof_per_cell-1)+local_index;
	}

	template<int I, class B=Basis>
	struct util {
		typedef util<I-1, typename B::next_type> next_type;
		next_type next;
		T integrate(int i, double xleft, double xright, double dx, Profile* profile) {
			if(i == I) {
				B basis;
				auto f = [&](double x) { return basis((x-xleft)/dx) * profile->densityr(x); };
				IntegratorGSL<> integratorGSL(f); // the integrator
				return integratorGSL.integrate(xleft, xright);
			} else {
				return next.integrate(i, xleft, xright, dx, profile);
			}
		}
		template<class F>
		T integrate2(int i, double xleft, double xright, double dx, F f) {
			if(i == I) {
				B basis;
				auto f2 = [&](double x) { return basis((x-xleft)/dx) * f(x); };
				IntegratorGSL<> integratorGSL(f2); // the integrator
				return integratorGSL.integrate(xleft, xright);
			} else {
				return next.integrate2(i, xleft, xright, dx, f);
			}
		}
		template<class Array>
		T eval(Array& array, int index, double xleft, double dx, double x) {
			B basis;
			//cout << "eval: " << index << " " << xleft << " " << dx << " " << x << " u=" << ((x-xleft)/dx) << " " << basis((x-xleft)/dx) << " " << array(index) << endl;
			return basis((x-xleft)/dx) * array(index) + next.eval(array, index-1, xleft, dx, x);
		}
		template<class Array>
		T gradient(Array& array, int index, double xleft, double dx, double x) {
			B basis;
			//cout << "grad: " << index << " " << xleft << " " << dx << " " << x << " u=" << ((x-xleft)/dx) << " " << basis((x-xleft)/dx) << " " << basis.dfdx((x-xleft)/dx) << " " << array(index) << endl;
			return basis.dfdx((x-xleft)/dx)/dx * array(index) + next.gradient(array, index-1, xleft, dx, x);
		}
	};
	template<class B>
	struct util<-1, B> {
		T integrate(int, double, double, double, Profile*) {
			return 0;
		}
		template<class F>
		T integrate2(int, double, double, double, F) {
			return 0;
		}
		template<class Array>
		T eval(Array&, int, double, double, double) {
			return 0;
		}
		template<class Array>
		T gradient(Array&, int, double, double, double) {
			return 0;
		}
	};

	template<int I, int J, class B1=Basis, class B2=Basis>
	struct selfintegrator {
		typedef selfintegrator<I-1, J, typename B1::next_type, B2> next_typeI;
		typedef selfintegrator<I, J-1, B1, typename B2::next_type> next_typeJ;
		next_typeI nextI;
		next_typeJ nextJ;
		T integrate(int i, int j) {
			if((i == I)) { // first search for right i
				if(j == J) { // then right j
					B1 basis1;
					B2 basis2;
					auto f = [&](double x) { return basis1(x) * basis2(x); };
					IntegratorGSL<> integratorGSL(f);
					return integratorGSL.integrate(0, 1);
				}  else {
					return nextJ.integrate(i, j);
				}
			} else {
				return nextI.integrate(i, j);
			}
		}
		T integrate_grad(int i, int j, double xleft, double xright, double dx, Transformation1d_in_3d* transformation) {
			if((i == I)) { // first search for right i
				if(j == J) { // then right j
					B1 basis1;
					B2 basis2;
					//auto f = [&](double x) { return basis1.dfdx((x-xleft)/dx) / dx * basis2.dfdx((x-xleft)/dx) / dx *  x * x;}; //*/; };
					auto f = [&](double x) -> double {
						//double r = tan(u*M_PI/2);
						double u = x;
						//double t = tan(u*M_PI/2);
						//double c = cos(u*M_PI/2);
						//return 16./2 * basis1.dfdx((x-xleft)/dx) / dx * basis2.dfdx((x-xleft)/dx) / dx * t * t * c * c;
						return basis1.dfdx((x-xleft)/dx) / dx * basis2.dfdx((x-xleft)/dx) / dx * transformation->laplace_u1_1_times_d3xdu(u) * transformation->laplace_u1_2(u);
					}; //*/; };
					IntegratorGSL<> integratorGSL(f);
					return integratorGSL.integrate(xleft, xright);
				}  else {
					return nextJ.integrate_grad(i, j, xleft, xright, dx, transformation);
				}
			} else {
				return nextI.integrate_grad(i, j, xleft, xright, dx, transformation);
			}
		}
	};
	template<int I, class B1, class B2>
	struct selfintegrator<I, -1, B1, B2> {
		T integrate(int, int) {
			return 0;
		}
		T integrate_grad(int, int, double, double, double, Transformation1d_in_3d*) {
			return 0;
		}
	};
	template<int J, class B1, class B2>
	struct selfintegrator<-1, J, B1, B2> {
		T integrate(int, int) {
			return 0;
		}
		T integrate_grad(int, int, double, double, double, Transformation1d_in_3d* ) {
			return 0;
		}
	};

	double integrate_gradshape(int cell_index, int i, int j) {
		typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
		selfintegrator_type si;
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		T dx = (xright-xleft);
		//cout << "integrate: " << cell_index << " i " << i << " "  << xleft << " to " << xright << endl;
		return si.integrate_grad(i, j, xleft, xright, dx, transformation);
	}

	template<class F>
	double integrate_shape(int cell_index, int i, F f) {
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		T dx = (xright-xleft);
		util<Basis::degree, Basis> integrator;
		//cout << "integrate shape: " << cell_index << " i " << i << " " << xleft << " to " << xright << endl;
		return integrator.integrate2(i, xleft, xright, dx, f);
		//return -integrator.integrate2(dof_per_cell-1-i, xright, xleft, dx, f);
	}

	double eval(VectorXd& solution, double x) {
		int cell_index = mesh.findindex(x);
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		T dx = xright-xleft;
		double v = 0;
		util<Basis::degree, Basis> util;
		if(mesh.inrange(x))
			v = util.eval(solution, dof_index(cell_index, dof_per_cell-1), xleft, dx, x);
		//cout << "eval: " << v << endl;
		return v;
		/*for(int i = 0; i < dof_per_cell; i++) {
			ev
		}*/
	}

	double gradient(VectorXd& solution, double x) {
		int cell_index = mesh.findindex(x);
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		T dx = xright-xleft;
		double v = 0;
		util<Basis::degree, Basis> util;
		if(mesh.inrange(x))
			v = util.gradient(solution, dof_index(cell_index, dof_per_cell-1), xleft, dx, x);
		//cout << "grad: " << " " << cell_index << " " << n_cells << " " << v << endl;
		return v;
		/*for(int i = 0; i < dof_per_cell; i++) {
			ev
		}*/
	}

	MeshRegularNodal1d(T _x1, T _x2, int n_cells, Transformation1d_in_3d* transformation) : x1(_x1), x2(_x2), n_cells(n_cells), mesh(_x1, _x2, n_cells), ctrans(1, 1), transformation(transformation) {
		if(Basis::degree == 0) {
			//dof_per_cell = 1;
			dof = n_cells;
		} else {
			dof = 1 + n_cells + (dof_per_cell-2)*n_cells; // 1 dof per border + dofs inside the cel
		}
		//cout << "n_cells = " << n_cells << " dof = " << dof << " dof_per_cell = " << dof_per_cell << endl;
		MatrixXd m = MatrixXd::Zero(dof, dof);
		ctrans.resize(dof, dof);
		T scale = (x2-x1)/n_cells;

		T integrals[dof_per_cell][dof_per_cell];
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < (j+1); k++) {
				typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
				selfintegrator_type si;
				double integral = si.integrate(j,k);
				//cout << j << " " << k << " " << integral << endl;
				integrals[j][k] = integral;
				integrals[k][j] = integral;
			}
		}
		for(int i = 0; i < n_cells; i++) {
			for(int j = 0; j < dof_per_cell; j++) {
				for(int k = 0; k < dof_per_cell; k++) {
					int i1 = i*(dof_per_cell-1)+j;
					int i2 = i*(dof_per_cell-1)+k;
					m(i1, i2) = m(i1, i2) + integrals[j][k];
				}
			}
		}
		//cout << m << endl;
		m = m * scale;
		ctrans = m.inverse();
	}
	void testprofile(Profile* profile, double_vector v, bool dotrans) {
		double* vp = v.data().begin();
		assert((int)v.size() == dof);
		double* array = vp;
		//int size = x.size();
		//double scale = 1./size;
		//cout << "size = " << size << endl;
		for(int i = 0; i < n_cells; i++) {
			for(int j = 0; j < dof_per_cell; j++) {
				T xleft = mesh.indexto_x(i);
				T xright = mesh.indexto_x(i+1);
				T dx = (xright-xleft);
				util<Basis::degree, Basis> integrator;
				double integral = integrator.integrate(j, xleft, xright, dx, profile);
				/**/
				cout << "> " << i << " " << j << " " << (i*(dof_per_cell-1)+j) << " " << integral << endl;
				//array[i*(dof_per_cell-1)+(dof_per_cell-1-j)] += integral;
				array[i*(dof_per_cell-1)+j] += integral;

			}
		}
		if(dotrans) {
			VectorXd v_alias = VectorXd::Map(v.data().begin(), v.size());
			VectorXd vtrans = ctrans * v_alias;
			VectorXd::Map(v.data().begin(), v.size()) = vtrans;
		}
	}

	void test(double_vector x, double_vector y, double_vector v, bool dotrans) {
		/*double* xp = x.data().begin();
		double* yp = y.data().begin();
		double* vp = v.data().begin();
		int size = x.size();
		double scale = 1./size * (x2-x1);
		//cout << "size = " << size << endl;
		for(int i = 0; i < size; i++) {
			//cout << "i = " << i << endl;
			this->operator()(xp[i], yp[i]*scale, vp);

		}
		if(dotrans) {
			VectorXd v_alias = VectorXd::Map(v.data().begin(), v.size());
			VectorXd vtrans = ctrans * v_alias;
			VectorXd::Map(v.data().begin(), v.size()) = vtrans;
		}*/
	}

	template<class A>
	T operator()(T x, T y, A array) {
		/*if(mesh.inrange(x)) {
			int indexleft = mesh.findindex(x);
			T xleft = mesh.indexto_x(indexleft);
			T xright = mesh.indexto_x(indexleft+1);
			T fraction = (x-xleft)/(xright-xleft);
			//cout << "x = " << x << " xleft = " << xleft << " xright = " << xright << " fraction = " << fraction << endl;
			BasisTriLeft<T> trileft;
			BasisTriRight<T> triright;
			//cout << "y = " << y << " triright(fraction) = " << triright(fraction) << " trileft (fraction-1) = " << trileft (fraction-1) << endl;
			array[indexleft]   +=  triright(fraction) * y;
			array[indexleft+1] +=  trileft (fraction-1) * y;
			// TODO: use trileft(fraction) + triright(fraction) == 1
		}*/
	}

};

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ATAN2D_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ATAN2D_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing atan2d capabilities

    atan2d function : atan2 in degrees.

    @par Semantic:

    For every parameters of same floating type

    @code
    auto r = atan2d(y, x);
    @endcode

    is similar  to:

    @code
    T r =  indeg(atan2(y, x));
    @endcode

    For any real arguments @c x and @c y not both equal to zero, <tt>atan2d(y, x)</tt>
    is the angle in degrees between the positive x-axis of a plane and the point
    given by the coordinates  <tt>(x, yx)</tt>.

    It is also the angle in \f$[-180,180[\f$ for which
    \f$x/\sqrt{x^2+y^2}\f$ and \f$y/\sqrt{x^2+y^2}\f$
    are respectively the sine and the cosine.

    @see atand, atan2, atan

  **/
  Value atan2d(Value const& x, Value const& y );
} }
#endif

#include <boost/simd/function/scalar/atan2d.hpp>
#include <boost/simd/function/simd/atan2d.hpp>

#endif


/** @file SymmetricBetaDist.cpp
 * @author Mark J. Olah (mjo\@cs.unm DOT edu)
 * @date 2017-2019
 * @brief SymmetricBetaDist class definition
 *
 */
#include "PriorHessian/SymmetricBetaDist.h"
#include "PriorHessian/PriorHessianError.h"

#include <sstream>
#include <cmath>
#include <limits>

#include <boost/math/special_functions/beta.hpp>

namespace prior_hessian {

const StringVecT SymmetricBetaDist::_param_names = { "beta" };
const SymmetricBetaDist::NparamsVecT SymmetricBetaDist::_param_lbound = { 0 }; //Lower bound on valid parameter values
const SymmetricBetaDist::NparamsVecT SymmetricBetaDist::_param_ubound = { INFINITY }; //Upper bound on valid parameter values

/* Constructors */
SymmetricBetaDist::SymmetricBetaDist(double beta)
    : UnivariateDist(),
      _beta(checked_beta(beta)),
      llh_const_initialized(false)
{ }

/* Non-static member functions */
void  SymmetricBetaDist::set_beta(double val)
{
    _beta = checked_beta(val);
    llh_const_initialized = false;
}

double SymmetricBetaDist::cdf(double x) const
{
    if(x==0) return 0;
    if(x==1) return 1;
    return boost::math::ibeta(_beta, _beta, x);
}

double SymmetricBetaDist::icdf(double u) const
{
    if(u==0) return 0;
    if(u==1) return 1;
    return boost::math::ibeta_inv(_beta, _beta, u);
}

double SymmetricBetaDist::pdf(double x) const
{
   return boost::math::ibeta_derivative(_beta, _beta, x);
}

double SymmetricBetaDist::llh(double x) const
{
    if(!llh_const_initialized) initialize_llh_const();
    return rllh(x) + llh_const;
}

void SymmetricBetaDist::initialize_llh_const() const
{
    llh_const = compute_llh_const(beta());
    llh_const_initialized = true;
}

double SymmetricBetaDist::compute_llh_const(double beta)
{
    return -2*lgamma(beta) - lgamma(2*beta);//log(1/Beta(beta,beta))
}

double SymmetricBetaDist::checked_beta(double val)
{
    if(val<=0 || !std::isfinite(val)) {
        std::ostringstream msg;
        msg<<"SymmetricBetaDist: got bad beta value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

} /* namespace prior_hessian */


#ifndef IGLP_HPP
#define IGLP_HPP

#include <glpk.h>
#include <Eigen/Eigen>

#include <cstdio>
#include <cstdlib>
#include <string>

namespace iglp
{

inline double linprog(const Eigen::VectorXd &c,
                      const Eigen::MatrixXd &A,
                      const Eigen::VectorXd &b,
                      Eigen::VectorXd &x,
                      bool ipm = false,
                      bool verbose = false)
// linprog:
//         min cTx s.t. Ax<=b
// input:
//         c: d*1 objective coeffs
//         A: m*d constraint matrix
//         b: m*1 constraint bound
//         ipm: use interior point method
//              or simplex method
//         verbose: show details
// output:
//         x: d*1 decision variables
// return:
//         inf: No feasible solution or fail
//        -inf: Unbounded problem
//         real: minimum objective function
{
    int d = c.size();
    int m = b.size();
    int dm = d * m;
    x = Eigen::VectorXd::Zero(d);

    glp_prob *lp;
    int *ia = new int[dm + 1];
    int *ja = new int[dm + 1];
    double *ar = new double[dm + 1];
    int s;
    double z;

    lp = glp_create_prob();
    glp_set_prob_name(lp, "lp");
    glp_set_obj_dir(lp, GLP_MIN);

    glp_add_rows(lp, m);
    for (int i = 1; i <= m; i++)
    {
        glp_set_row_name(lp, i, (std::to_string(i) + "y").c_str());
        glp_set_row_bnds(lp, i, GLP_UP, 0.0, b(i - 1));
    }

    glp_add_cols(lp, d);
    for (int i = 1; i <= d; i++)
    {
        glp_set_col_name(lp, i, (std::to_string(i) + "x").c_str());
        glp_set_col_bnds(lp, i, GLP_FR, 0.0, 0.0);
        glp_set_obj_coef(lp, i, c(i - 1));
    }

    int k = 1;
    for (int i = 1; i <= m; i++)
    {
        for (int j = 1; j <= d; j++)
        {
            ia[k] = i;
            ja[k] = j;
            ar[k] = A(i - 1, j - 1);
            k++;
        }
    }
    glp_load_matrix(lp, dm, ia, ja, ar);

    if (!ipm)
    {
        glp_smcp param;
        glp_init_smcp(&param);
        param.msg_lev = verbose ? GLP_MSG_ALL : GLP_MSG_OFF;
        glp_simplex(lp, &param);
        s = glp_get_status(lp);
        z = INFINITY;
        if (s == GLP_OPT || s == GLP_UNBND)
        {
            z = (s == GLP_UNBND) ? -INFINITY : glp_get_obj_val(lp);
            for (int i = 1; i <= d; i++)
            {
                x(i - 1) = glp_get_col_prim(lp, i);
            }
        }
    }
    else
    {
        glp_iptcp param;
        glp_init_iptcp(&param);
        param.msg_lev = verbose ? GLP_MSG_ALL : GLP_MSG_OFF;
        glp_interior(lp, &param);
        s = glp_ipt_status(lp);
        z = INFINITY;
        if (s == GLP_OPT)
        {
            z = glp_ipt_obj_val(lp);
            for (int i = 1; i <= d; i++)
            {
                x(i - 1) = glp_ipt_col_prim(lp, i);
            }
        }
    }

    glp_delete_prob(lp);
    delete[] ia;
    delete[] ja;
    delete[] ar;

    return z;
}

} // namespace iglp

#endif


//  (C) Copyright Christopher Kormanyos 1999 - 2021.
//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_CCMATH_FREXP_HPP
#define BOOST_MATH_CCMATH_FREXP_HPP

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isfinite.hpp>

namespace boost::math::ccmath {

namespace detail
{

template <typename Real>
inline constexpr Real frexp_zero_impl(Real arg, int* exp)
{
    *exp = 0;
    return arg;
}

template <typename Real>
inline constexpr Real frexp_impl(Real arg, int* exp)
{
    const bool negative_arg = (arg < Real(0));

    Real f = negative_arg ? -arg : arg;
    int e2 = 0;
    constexpr Real two_pow_32 = Real(4294967296);

    while (f >= two_pow_32)
    {
        f = f / two_pow_32;
        e2 += 32;
    }

    while(f >= Real(1))
    {
        f = f / Real(2);
        ++e2;
    }

    if(exp != nullptr)
    {
        *exp = e2;
    }

    return !negative_arg ? f : -f;
}

} // namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real frexp(Real arg, int* exp)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return arg == Real(0)  ? detail::frexp_zero_impl(arg, exp) :
               arg == Real(-0) ? detail::frexp_zero_impl(arg, exp) :
               boost::math::ccmath::isinf(arg) ? detail::frexp_zero_impl(arg, exp) :
               boost::math::ccmath::isnan(arg) ? detail::frexp_zero_impl(arg, exp) :
               boost::math::ccmath::detail::frexp_impl(arg, exp);
    }
    else
    {
        using std::frexp;
        return frexp(arg, exp);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr double frexp(Z arg, int* exp)
{
    return boost::math::ccmath::frexp(static_cast<double>(arg), exp);
}

inline constexpr float frexpf(float arg, int* exp)
{
    return boost::math::ccmath::frexp(arg, exp);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double frexpl(long double arg, int* exp)
{
    return boost::math::ccmath::frexp(arg, exp);
}
#endif

}

#endif // BOOST_MATH_CCMATH_FREXP_HPP


#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <algorithm>
#include <cstddef>
#include <cassert>
#include <limits>
#include <Eigen/Sparse>

using namespace std;

class Array2D
{
private:
	vector<double> m_data;
	size_t m_Nx, m_Ny;

public:
	Array2D(size_t nx, size_t ny, double val = 0.0) : m_Nx(nx), m_Ny(ny), m_data(nx*ny, val) {}

	// 0-based indexing
	double &at(int i, int j)
	{
		int idx = i + m_Nx * j;
		return m_data[idx];
	}

	double at(int i, int j) const
	{
		int idx = i + m_Nx * j;
		return m_data[idx];
	}

	// 1-based indexing
	double &operator()(int i, int j)
	{
		return at(i - 1, j - 1);
	}

	double operator()(int i, int j) const
	{
		return at(i - 1, j - 1);
	}
};

const size_t WIDTH = 16;
const size_t DIGITS = 7;

const double L = 0.5; // m
const double D = 0.01; // m
const double Ue = 1.0; // m/s
const double Pe = 0.0;
const double rho = 1.225; // Kg/m3
const double mu = 3.737e-5; // Kg/m/s

const int Nx = 21, Ny = 11;
const double dx = L / (Nx - 1), dy = D / (Ny - 1);
const double dx2 = 2 * dx, dy2 = 2 * dy;
const double dxdx = dx * dx, dydy = dy * dy;
vector<double> x(Nx, 0.0), y(Ny, 0.0);

const double dt = 0.001;
double t = 0.0;
int iter_cnt = 0;
const int MAX_ITER_NUM = 2000;

const double a = 2 * (dt / dxdx + dt / dydy);
const double b = -dt / dxdx;
const double c = -dt / dydy;
double d_min = numeric_limits<double>::max(), d_max = numeric_limits<double>::min(), d_15_5 = 0.0;

Array2D p(Nx, Ny, Pe), p_star(Nx, Ny, Pe), p_prime(Nx, Ny, 0.0);
Array2D u(Nx + 1, Ny, 0.0), u_wedge(Nx + 1, Ny, 0.0), u_star(Nx + 1, Ny, 0.0), u_prime(Nx + 1, Ny, 0.0);
Array2D v(Nx + 2, Ny + 1, 0.0), v_wedge(Nx + 2, Ny + 1, 0.0), v_star(Nx + 2, Ny + 1, 0.0), v_prime(Nx + 2, Ny + 1, 0.0);

// Full flowfield in TECPLOT ASCII Format.
void output1(void)
{
	Array2D u_interp(Nx, Ny, 0.0);
	for (int i = 1; i <= Nx; ++i)
		u_interp(i, 1) = 0.0; // Bottom
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 1; i <= Nx; ++i)
			u_interp(i, j) = (u(i, j) + u(i + 1, j)) / 2; // Inner
	for (int i = 1; i <= Nx; ++i)
		u_interp(i, Ny) = Ue; // Top

	Array2D v_interp(Nx, Ny, 0.0);
	for (int i = 1; i <= Nx; ++i)
		v_interp(i, 1) = 0.0; // Bottom
	for (int j = 2; j <= Ny - 1; ++j)
	{
		v_interp(1, j) = 0.0; // Left
		for (int i = 3; i <= Nx + 1; ++i)
			v_interp(i - 1, j) = (v(i, j) + v(i, j + 1)) / 2; // Inner and Right
	}
	for (int i = 1; i <= Nx; ++i)
		v_interp(i, Ny) = 0.0; // Top

	// Create Tecplot data file.
	ofstream result("flow" + to_string(iter_cnt) + ".dat");
	if (!result)
		throw("Failed to create data file!");

	// Header
	result << "TITLE = \"t=" << t << "\"" << endl;
	result << "VARIABLES = \"X\", \"Y\", \"P\", \"U\", \"V\"" << endl;
	result << "ZONE I=" << Nx << ", J=" << Ny << ", F=POINT" << endl;

	// Flowfield data
	for (int j = 1; j <= Ny; ++j)
		for (int i = 1; i <= Nx; ++i)
		{
			result << setw(WIDTH) << setprecision(DIGITS) << x[i - 1];
			result << setw(WIDTH) << setprecision(DIGITS) << y[j - 1];
			result << setw(WIDTH) << setprecision(DIGITS) << p(i, j);
			result << setw(WIDTH) << setprecision(DIGITS) << u_interp(i, j);
			result << setw(WIDTH) << setprecision(DIGITS) << v_interp(i, j);
			result << endl;
		}

	// Finalize
	result.close();
}

// Statistics at (15, 5) and i=15
void output2(int iter)
{
	static const string fn("history_at_15_5.txt");

	ofstream fout;
	if (iter == 0)
	{
		fout.open(fn, ios::out);
		if (!fout)
			throw("Failed to open history file.");

		for (int j = 0; j < Ny; ++j)
			fout << setw(WIDTH) << setprecision(DIGITS) << y[j];
		fout << endl;
	}
	else
	{
		fout.open(fn, ios::app);
		if (!fout)
			throw("Failed to open history file.");
	}

	for (int j = 1; j <= Ny; ++j)
		fout << setw(WIDTH) << setprecision(DIGITS) << u(15, j);
	fout << endl;
	for (int j = 1; j <= Ny; ++j)
		fout << setw(WIDTH) << setprecision(DIGITS) << v(15, j);
	fout << endl;
	fout << d_15_5 << endl;

	fout.close();
}

void init(void)
{
	cout << "mu=" << mu << endl;
	cout << "dt=" << dt << endl;

	// Init
	for (int i = 1; i < Nx; ++i)
		x[i] = L * i / (Nx - 1); // X-Coordinates
	for (int j = 1; j < Ny; ++j)
		y[j] = D * j / (Ny - 1); // Y-Coordinates

	for (int i = 1; i <= Nx + 1; ++i)
		u(i, Ny) = u_wedge(i, Ny) = u_star(i, Ny) = Ue; // U at top
	v(15, 5) = v_wedge(15, 5) = v_star(15, 5) = 0.5; // Initial peak to ensure 2D flow structure
}

// Solve the pressure equation.
void ImplicitMethod1()
{
	typedef Eigen::SparseMatrix<double> SpMat;
	typedef Eigen::Triplet<double> T;

	const int m = Nx * Ny;
	vector<T> coef;
	Eigen::VectorXd rhs(m);
	SpMat A(m, m);

	// Calculating coefficients
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			const int id_w = id - 1;
			const int id_e = id + 1;
			const int id_n = id + Nx;
			const int id_s = id - Nx;

			if (i == 0 || i == Nx - 1) // Inlet and Outlet
			{
				coef.push_back(T(id, id, 1.0));
				rhs(id) = Pe;
			}
			else if (j == 0) // Bottom
			{
				coef.push_back(T(id, id_n, 1.0));
				coef.push_back(T(id, id, -1.0));
				const double ddvddx = 0.0;
				//const double ddvddy = 4.0 / 3 * (v_wedge.at(i + 1, j + 2) - 3 * v_wedge.at(i + 1, j + 1)) / dydy;
				const double ddvddy = 0.0;
				rhs(id) = mu * (ddvddx + ddvddy) * dy;
			}
			else if (j == Ny - 1) // Top
			{
				coef.push_back(T(id, id, 1.0));
				coef.push_back(T(id, id_s, -1.0));
				const double ddvddx = 0.0;
				//const double ddvddy = 4.0 / 3 * (v_wedge.at(i + 1, j - 1) - 3 * v_wedge.at(i + 1, j)) / dydy;
				const double ddvddy = 0.0;
				rhs(id) = mu * (ddvddx + ddvddy) * dy;
			}
			else // Inner
			{
				// Use 0-based interface
				const double d = (rho*u_wedge.at(i + 1, j) - rho * u_wedge.at(i, j)) / dx + (rho*v_wedge.at(i + 1, j + 1) - rho * v_wedge.at(i + 1, j)) / dy;

				coef.push_back(T(id, id, a));
				coef.push_back(T(id, id_w, b));
				coef.push_back(T(id, id_e, b));
				coef.push_back(T(id, id_n, c));
				coef.push_back(T(id, id_s, c));
				rhs(id) = -d;
			}
		}

	// Construct sparse matrix
	A.setFromTriplets(coef.begin(), coef.end());

	// Solve the linear system: Ax = rhs
	Eigen::SimplicialCholesky<SpMat> chol(A);
	Eigen::VectorXd x = chol.solve(rhs);

	// Update p
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			p.at(i, j) = x(id);
		}
}

// Solve the pressure-correction equation
void ImplicitMethod2()
{
	typedef Eigen::SparseMatrix<double> SpMat;
	typedef Eigen::Triplet<double> T;

	const int m = Nx * Ny;
	vector<T> coef;
	Eigen::VectorXd rhs(m);
	SpMat A(m, m);

	// Calculating coefficients
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			const int id_w = id - 1;
			const int id_e = id + 1;
			const int id_n = id + Nx;
			const int id_s = id - Nx;

			if (i == 0 || i == Nx - 1) // Inlet and Outlet
			{
				coef.push_back(T(id, id, 1.0));
				rhs(id) = 0.0;
			}
			else if (j == 0) // Bottom
			{
				coef.push_back(T(id, id, 1.0));
				coef.push_back(T(id, id_n, -1.0));
				rhs(id) = 0.0;
			}
			else if (j == Ny - 1) // Top
			{
				coef.push_back(T(id, id, 1.0));
				coef.push_back(T(id, id_s, -1.0));
				rhs(id) = 0.0;
			}
			else // Inner
			{
				// Use 0-based interface
				const double d = (rho*u_star.at(i + 1, j) - rho * u_star.at(i, j)) / dx + (rho * v_star.at(i + 1, j + 1) - rho * v_star.at(i + 1, j)) / dy;
				if (d > d_max)
					d_max = d;
				if (d < d_min)
					d_min = d;
				if (i == 15 && j == 5)
					d_15_5 = d;

				coef.push_back(T(id, id, a));
				coef.push_back(T(id, id_w, b));
				coef.push_back(T(id, id_e, b));
				coef.push_back(T(id, id_n, c));
				coef.push_back(T(id, id_s, c));
				rhs(id) = -d;
			}
		}

	// Construct sparse matrix
	A.setFromTriplets(coef.begin(), coef.end());

	// Solve the linear system: Ax = rhs
	Eigen::SimplicialCholesky<SpMat> chol(A);
	Eigen::VectorXd x = chol.solve(rhs);

	// Update p_prime
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			p_prime.at(i, j) = x(id);
		}
}

void SIMPLER(void)
{
	// u_wedge at inner points
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			double v_bar1 = 0.5*(v(i, j + 1) + v(i + 1, j + 1));
			double v_bar2 = 0.5*(v(i, j) + v(i + 1, j));

			double t11 = rho * pow(u(i + 1, j), 2) - rho * pow(u(i - 1, j), 2);
			double t12 = rho * u(i, j + 1)*v_bar1 - rho * u(i, j - 1)*v_bar2;
			double t21 = u(i + 1, j) - 2 * u(i, j) + u(i - 1, j);
			double t22 = u(i, j + 1) - 2 * u(i, j) + u(i, j - 1);
			double A = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			u_wedge(i, j) = (rho * u(i, j) + A * dt) / rho;
		}

	// v_wedge at inner points
	for (int i = 3; i <= Nx; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			double u_bar1 = 0.5 *(u(i, j - 1) + u(i, j));
			double u_bar2 = 0.5 *(u(i - 1, j - 1) + u(i - 1, j));

			double t11 = rho * v(i + 1, j) * u_bar1 - rho * v(i - 1, j) * u_bar2;
			double t12 = rho * pow(v(i, j + 1), 2) - rho * pow(v(i, j - 1), 2);
			double t21 = v(i + 1, j) - 2 * v(i, j) + v(i - 1, j);
			double t22 = v(i, j + 1) - 2 * v(i, j) + v(i, j - 1);
			double B = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			v_wedge(i, j) = (rho * v(i, j) + B * dt) / rho;
		}

	// Solve p
	ImplicitMethod1();

	// Set p_star to p
	for (int j = 1; j <= Ny; ++j)
		for (int i = 1; i <= Nx; ++i)
			p_star(i, j) = p(i, j);

	// u_star at inner points
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			double v_bar1 = 0.5*(v(i, j + 1) + v(i + 1, j + 1));
			double v_bar2 = 0.5*(v(i, j) + v(i + 1, j));

			double t11 = rho * pow(u(i + 1, j), 2) - rho * pow(u(i - 1, j), 2);
			double t12 = rho * u(i, j + 1)*v_bar1 - rho * u(i, j - 1)*v_bar2;
			double t21 = u(i + 1, j) - 2 * u(i, j) + u(i - 1, j);
			double t22 = u(i, j + 1) - 2 * u(i, j) + u(i, j - 1);
			double A = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			u_star(i, j) = (rho * u(i, j) + A * dt - dt / dx * (p_star(i, j) - p_star(i - 1, j))) / rho;
		}

	// v_star at inner points
	for (int i = 3; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			double u_bar1 = 0.5 *(u(i, j - 1) + u(i, j));
			double u_bar2 = 0.5 *(u(i - 1, j - 1) + u(i - 1, j));

			double t11 = rho * v(i + 1, j) * u_bar1 - rho * v(i - 1, j) * u_bar2;
			double t12 = rho * pow(v(i, j + 1), 2) - rho * pow(v(i, j - 1), 2);
			double t21 = v(i + 1, j) - 2 * v(i, j) + v(i - 1, j);
			double t22 = v(i, j + 1) - 2 * v(i, j) + v(i, j - 1);
			double B = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			v_star(i, j) = (rho * v(i, j) + B * dt - dy / dy * (p_star(i - 1, j) - p_star(i - 1, j - 1))) / rho;
		}

	d_min = numeric_limits<double>::max();
	d_max = numeric_limits<double>::min();
	ImplicitMethod2();

	// Correct u at inner nodes
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			u_prime(i, j) = -dt / dx * (p_prime(i, j) - p_prime(i - 1, j)) / rho; // u_prime
			u(i, j) = u_star(i, j) + u_prime(i, j);
		}

	// Linear extrapolation of u at virtual nodes
	for (int j = 2; j <= Ny - 1; ++j)
	{
		u(1, j) = 2 * u(2, j) - u(3, j);
		u(Nx + 1, j) = 2 * u(Nx, j) - u(Nx - 1, j);
	}

	// Correct v at inner nodes
	for (int i = 3; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			v_prime(i, j) = -dt / dy * (p_prime(i - 1, j) - p_prime(i - 1, j - 1)) / rho; // v_prime
			v(i, j) = v_star(i, j) + v_prime(i, j);
		}

	// Linear extrapolation of v at both top and bottom virtual nodes
	// No-Penetration at both top and bottom
	for (int i = 2; i <= Nx + 1; ++i)
	{
		v(i, 1) = -v(i, 2);
		v(i, Ny + 1) = -v(i, Ny);
	}

	// Linear extrapolation of v at right virtual nodes
	for (int j = 2; j <= Ny; ++j)
		v(Nx + 2, j) = 2 * v(Nx + 1, j) - v(Nx, j);
}

bool check_convergence(void)
{
	// Statistics of the mass flux residue
	cout << "Max(d)=" << d_max << " Min(d)=" << d_min << endl;

	// Statistics of u
	double u_max = numeric_limits<double>::min();
	double u_min = numeric_limits<double>::max();
	for (int i = 2; i <= Nx; ++i)
		for (int j = 1; j <= Ny; ++j)
		{
			u_max = max(u_max, u(i, j));
			u_min = min(u_min, u(i, j));
		}
	cout << "Max(u)=" << u_max << " Min(u)=" << u_min << endl;

	// Statistics of v
	double v_max = numeric_limits<double>::min();
	double v_min = numeric_limits<double>::max();
	for (int i = 2; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			v_max = max(v_max, v(i, j));
			v_min = min(v_min, v(i, j));
		}
	cout << "Max(v)=" << v_max << " Min(v)=" << v_min << endl;

	// Statistics of p
	double p_max = numeric_limits<double>::min();
	double p_min = numeric_limits<double>::max();
	for (int i = 1; i <= Nx; ++i)
		for (int j = 1; j <= Ny; ++j)
		{
			p_max = max(p_max, p(i, j));
			p_min = min(p_min, p(i, j));
		}
	cout << "Max(p)=" << p_max << " Min(p)=" << p_min << endl;

	return iter_cnt > MAX_ITER_NUM || max(abs(d_max), abs(d_min)) < 1e-4;
}

void loop(void)
{
	bool converged = false;
	while (!converged)
	{
		++iter_cnt;
		cout << "Iter" << iter_cnt << ":" << endl;

		SIMPLER();
		t += dt;

		output1();
		output2(iter_cnt);

		converged = check_convergence();
	}
}

int main(int argc, char *argv[])
{
	// Initialize
	init();

	// Output I.C.
	output1();
	output2(0);

	// Solve
	loop();

	return 0;
}


#include <bits/stdc++.h>
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using namespace std;
using namespace boost;

namespace GaussianElimination
{
    ///------------------------------------------------------------
    constexpr int MAX_EQ = 500;
    constexpr int MAX_VAR = 500;

    using T = boost::multiprecision::cpp_int;

    rational<T> A[MAX_EQ + 1][MAX_VAR + 1];
    rational<T> B[MAX_EQ + 1][MAX_VAR + 1];

    rational<T> solutions[MAX_VAR + 1];

    int N, M;
    ///------------------------------------------------------------

    ///------------------------------------------------------------
    void clear()
    {
        for (int i = 0; i <= MAX_EQ; ++i)
            for (int j = 0; j <= MAX_VAR; ++j)
                A[i][j] = B[i][j] = 0;

        for (int j = 0; j <= MAX_VAR; ++j)
            solutions[j] = 0;
    }

    void makeIdentity()
    {
        for (int i = 1; i <= N; ++i)
            B[i][i] = 1;
    }

    bool checkIfInvertible()
    {
        assert(N == M);

        for (int i = 1; i <= N; ++i)
            if (A[i][i] != 1)
                return false;

        return true;
    }
    ///------------------------------------------------------------

    ///------------------------------------------------------------
    void swapLines(int x, int y) /// swap(X, Y)
    {
        swap(A[x], A[y]);
        swap(B[x], B[y]);

        cerr << "SWAP LINES : " << x << " " << y << endl;
    }

    void divideLine(int line, rational<T> rat) /// X = X / rat
    {
        assert(rat != 0);

        for (int j = 1; j <= M; ++j)
            A[line][j] /= rat;

        for (int j = 1; j <= M; ++j)
            B[line][j] /= rat;

        cerr << "DIVIDE LINE_" << line << " BY " << rat << endl;
    }

    void changeLines(int x, int y, rational<T> rat) /// X = X - rat * Y
    {
        for (int i = 1; i <= M; ++i)
            A[x][i] -= rat * A[y][i];

        for (int i = 1; i <= M; ++i)
            B[x][i] -= rat * B[y][i];

        cerr << "ADD TO LINE_" << x << " => LINE_" << y << " x " << -rat << endl;
    }
    ///------------------------------------------------------------

    void rowEchelonForm()
    {
        cerr << "START ROW ECHELON" << endl;

        int i = 1, j = 1;

        while (i <= N && j <= M)
        {
            int k = i;

            while (k <= N && A[k][j] == 0)
                k++;

            if (k == N + 1)
            {
                j++;
                continue;
            }

            if (k != i)
                swapLines(k, i);

            assert(A[i][j] != 0);
            divideLine(i, A[i][j]);

            for (int l = i + 1; l <= N; ++l)
                changeLines(l, i, A[l][j]);

            i++;
            j++;
        }

        cerr << "FINISH ROW ECHELON" << endl << endl;
    }

    void reducedRowEchelonForm()
    {
        cerr << "START REDUCED ROW ECHELON" << endl;

        for (int i = N; i >= 1; i--)
        {
            int j = 1;

            while (j <= M && A[i][j] == 0)
                j++;

            if (j <= M)
            {
                for (int k = i - 1; k >= 1; k--)
                    changeLines(k, i, A[k][j]);
            }
        }

        cerr << "FINISH REDUCED ROW ECHELON" << endl << endl;
    }

    vector<vector<double>> getInverse(const vector<vector<int>> &coef, ostream &out = cout)
    {
        assert(coef.size() >= 1);
        assert(coef[0].size() >= 1);

        GaussianElimination::N = coef.size();
        GaussianElimination::M = coef[0].size();

        if (N != M)
        {
            out << "Matrix not invertible : N != M (non-square)\n";
            return {};
        }

        clear();

        for (int i = 0; i < N; ++i)
            for (int j = 0; j < M; ++j)
                A[i + 1][j + 1] = coef[i][j];

        makeIdentity();
        rowEchelonForm();

        if (checkIfInvertible() == false)
        {
            out << "Matrix not invertible\n";
            return {};
        }

        reducedRowEchelonForm();

        vector<vector<double>> inverse(N + 2);

        for (int i = 1; i <= N; ++i)
        {
            inverse[i - 1].resize(M + 1);

            for (int j = 1; j <= M; ++j)
            {
                inverse[i - 1][j - 1] = boost::rational_cast<double>(B[i][j]);
                out << B[i][j] << " ";
            }

            out << endl;
        }

        return inverse;
    }

    vector<double> solveSystemEquations(const vector<vector<int>> &coef, const vector<int> &bs, ostream &out = cout)
    {
        assert(coef.size() >= 1);
        assert(coef[0].size() >= 1);

        GaussianElimination::N = coef.size();
        GaussianElimination::M = coef[0].size();

        assert(static_cast<int>(bs.size()) == GaussianElimination::N);

        clear();

        for (int i = 0; i < N; ++i)
            for (int j = 0; j < M; ++j)
                A[i + 1][j + 1] = coef[i][j];

        for (int i = 0; i < N; ++i)
            B[i + 1][1] = bs[i];

        rowEchelonForm();
        reducedRowEchelonForm();

        for (int i = N; i >= 1; i--)
        {
            int j = 1;

            while (j <= M && A[i][j] == 0)
                j++;

            if (j == M + 1)
            {
                if (B[i][1] != 0)
                {
                    out << "Impossible!" << endl;
                    return {};
                }
            }
            else
            {
                solutions[i] = B[i][1];

                for (int p = j + 1; p <= M; ++p)
                    solutions[i] -= A[i][p] * solutions[p];
            }
        }

        vector<double> solution(M);

        for (int j = 0; j < M; ++j)
            solution[j] = boost::rational_cast<double>(solutions[j + 1]);

        for (int j = 1; j <= M; ++j)
            out << solutions[j] << " ";

        out << endl;

        return solution;
    }
}

int main()
{
    ifstream in("data.in");

    assert(in.is_open());

    vector<vector<int>> A;
    int n, m;

    in >> n >> m;
    A.resize(n);
    vector<int> bs(n);

    for (int i = 0; i < n; ++i)
    {
        A[i].resize(m);

        for (int j = 0; j < m; ++j)
            in >> A[i][j];

        in >> bs[i];
    }

    vector<double> xs = GaussianElimination::solveSystemEquations(A, bs);

    return 0;
}


#include "Gaussian.h"
#include <boost/math/distributions/normal.hpp>

BaseDistributionGaussian::BaseDistributionGaussian(const Options& iOptions, const Data& iData) : BaseDistribution(iOptions, iData) {

}
float BaseDistributionGaussian::getCdf(float iX, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   boost::math::normal dist(mean, sqrt(variance));
   return boost::math::cdf(dist, iX);
}
float BaseDistributionGaussian::getPdf(float iX, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   boost::math::normal dist(mean, sqrt(variance));
   return boost::math::pdf(dist, iX);
}
float BaseDistributionGaussian::getInv(float iCdf, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   boost::math::normal dist(mean, sqrt(variance));
   return boost::math::quantile(dist, iCdf);

}
int BaseDistributionGaussian::getNumMoments() const {
   return 2;
}


/**
 * ravg.hpp
 *
 * 2021 Gabriel A. Moreira
 *
 * gmoreira at isr.tecnico.ulisboa.pt
 * https://github.com/gabmoreira/maks
 *
 * This software and the related documents  are provided as  is,  with no express
 * or implied  warranties,  other  than those  that are  expressly stated  in the
 * License.
 */

#ifndef RAVG_HPP
#define RAVG_HPP

#include <Eigen/Dense>
#include <Eigen/Sparse>

/**
 * Cycle Solver in SO(3).
 *
 * Closed-form cycle graph rotation averaging solver in SO(3).
 *
 * @param pairwise (input) Eigen::MatrixXd - 3 x 3n Eigen matrix with SO(3) blocks.
 * @param num_nodes (input) int - number of variables.
 * @param R (output) Eigen::MatrixXd - 3n x 3 Eigen matrix containing optimal solution.
 */
void cycleSolverSO3(const Eigen::Ref<Eigen::MatrixXd> pairwise,
                    int num_nodes,
                    Eigen::Ref<Eigen::MatrixXd> R);

/**
 * Rotation averaging Primal-Dual method in SO(3).
 *
 * Primal-dual update method for averaging rotations in SO(3).
 *
 * @param Rtilde (input) Eigen::SparseMatrix<double> - 3n x 3n sparse rotation adjacency matrix.
 * @param R (output) Eigen::MatrixXd - 3n x 3 solution.
 * @param A (input/output) Eigen::SparseMatrix<double> - 3n x 3n symmetric and sparse graph adjacency matrix.
 * @param num_nodes (input) Int - number of variables.
 * @param maxiter (input) int - maxiter.
 * @param dual (output) Double - dual problem.
 * @param eta (input) Double - minimum eigenvalue stopping criterion.
 * @param sigma (input) Double - spectral shift.
 */
void primalDualSO3(const Eigen::SparseMatrix<double>& Rtilde,
                   const Eigen::SparseMatrix<double>& A,
                   Eigen::Ref<Eigen::MatrixXd> R,
                   int num_nodes,
                   int maxiter,
                   double& dual,
                   double eta,
                   double sigma);

#endif /* RAVG_HPP */


// MIT License
//
// Copyright (c) 2020 Lennart Braun
//
// 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 "linear_algebra.h"

#include <cassert>

#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>

#include "tensor/tensor_op.h"

namespace MOTION {

template <typename T>
void matrix_multiply(std::size_t dim_l, std::size_t dim_m, std::size_t dim_n, const T* A,
                     const T* B, T* output) {
  using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  Eigen::Map<MatrixType> matrix_output(output, dim_l, dim_n);
  Eigen::Map<const MatrixType> matrix_A(A, dim_l, dim_m);
  Eigen::Map<const MatrixType> matrix_B(B, dim_m, dim_n);
  matrix_output = matrix_A * matrix_B;
}

template <typename T>
std::vector<T> matrix_multiply(std::size_t dim_l, std::size_t dim_m, std::size_t dim_n,
                               const std::vector<T>& A, const std::vector<T>& B) {
  assert(A.size() == dim_l * dim_m);
  assert(B.size() == dim_m * dim_n);
  std::vector<T> output(dim_l * dim_n);
  matrix_multiply(dim_l, dim_m, dim_n, A.data(), B.data(), output.data());
  return output;
}

template <typename T>
void matrix_multiply(const tensor::GemmOp& gemm_op, const T* A, const T* B, T* output) {
  using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  assert(gemm_op.verify());
  Eigen::Map<MatrixType> matrix_output(output, gemm_op.output_shape_[0], gemm_op.output_shape_[1]);
  Eigen::Map<const MatrixType> matrix_A(A, gemm_op.input_A_shape_[0], gemm_op.input_A_shape_[1]);
  Eigen::Map<const MatrixType> matrix_B(B, gemm_op.input_B_shape_[0], gemm_op.input_B_shape_[1]);

  if (gemm_op.transA_ && gemm_op.transB_) {
    matrix_output = matrix_A.transpose() * matrix_B.transpose();
  } else if (gemm_op.transA_) {
    matrix_output = matrix_A.transpose() * matrix_B;
  } else if (gemm_op.transB_) {
    matrix_output = matrix_A * matrix_B.transpose();
  } else {
    matrix_output = matrix_A * matrix_B;
  }
}

template void matrix_multiply(std::size_t, std::size_t, std::size_t, const std::uint8_t*,
                              const std::uint8_t*, std::uint8_t*);
template void matrix_multiply(std::size_t, std::size_t, std::size_t, const std::uint16_t*,
                              const std::uint16_t*, std::uint16_t*);
template void matrix_multiply(std::size_t, std::size_t, std::size_t, const std::uint32_t*,
                              const std::uint32_t*, std::uint32_t*);
template void matrix_multiply(std::size_t, std::size_t, std::size_t, const std::uint64_t*,
                              const std::uint64_t*, std::uint64_t*);
template std::vector<std::uint8_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                   const std::vector<std::uint8_t>&,
                                                   const std::vector<std::uint8_t>&);
template std::vector<std::uint16_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                    const std::vector<std::uint16_t>&,
                                                    const std::vector<std::uint16_t>&);
template std::vector<std::uint32_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                    const std::vector<std::uint32_t>&,
                                                    const std::vector<std::uint32_t>&);
template std::vector<std::uint64_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                    const std::vector<std::uint64_t>&,
                                                    const std::vector<std::uint64_t>&);
template std::vector<__uint128_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                  const std::vector<__uint128_t>&,
                                                  const std::vector<__uint128_t>&);
template void matrix_multiply(const tensor::GemmOp&, const std::uint8_t*, const std::uint8_t*,
                              std::uint8_t*);
template void matrix_multiply(const tensor::GemmOp&, const std::uint16_t*, const std::uint16_t*,
                              std::uint16_t*);
template void matrix_multiply(const tensor::GemmOp&, const std::uint32_t*, const std::uint32_t*,
                              std::uint32_t*);
template void matrix_multiply(const tensor::GemmOp&, const std::uint64_t*, const std::uint64_t*,
                              std::uint64_t*);
template void matrix_multiply(const tensor::GemmOp&, const __uint128_t*, const __uint128_t*,
                              __uint128_t*);

template <typename T>
void convolution(const tensor::Conv2DOp& conv_op, const T* input_buffer, const T* kernel_buffer,
                 T* output_buffer) {
  using TensorType3 = Eigen::Tensor<T, 3, Eigen::RowMajor>;
  using CTensorType3 = Eigen::Tensor<const T, 3, Eigen::RowMajor>;
  using CTensorType4 = Eigen::Tensor<const T, 4, Eigen::RowMajor>;
  assert(conv_op.verify());
  const auto& output_shape = conv_op.output_shape_;
  const auto& input_shape = conv_op.input_shape_;
  const auto& kernel_shape = conv_op.kernel_shape_;

  Eigen::TensorMap<CTensorType3> input(input_buffer, input_shape[0], input_shape[1],
                                       input_shape[2]);
  Eigen::TensorMap<CTensorType4> kernel(kernel_buffer, kernel_shape[0], kernel_shape[1],
                                        kernel_shape[2], kernel_shape[3]);
  Eigen::TensorMap<TensorType3> output(output_buffer, output_shape[0], output_shape[1],
                                       output_shape[2]);
  const std::array<Eigen::Index, 2> kernel_matrix_dimensions = {
      static_cast<Eigen::Index>(kernel_shape[1] * kernel_shape[2] * kernel_shape[3]),
      static_cast<Eigen::Index>(kernel_shape[0])};
  const std::array<Eigen::Index, 2> input_matrix_dimensions = {
      static_cast<Eigen::Index>(output_shape[1] * output_shape[2]),
      static_cast<Eigen::Index>(kernel_shape[1] * kernel_shape[2] * kernel_shape[3])};

  auto kernel_matrix =
      kernel.shuffle(std::array<int, 4>{3, 2, 1, 0}).reshape(kernel_matrix_dimensions);

  auto input_matrix =
      input.shuffle(Eigen::array<Eigen::Index, 3>{2, 1, 0})
          .extract_image_patches(kernel_shape[2], kernel_shape[3], conv_op.strides_[0],
                                 conv_op.strides_[1], conv_op.dilations_[0], conv_op.dilations_[1],
                                 1, 1, conv_op.pads_[0], conv_op.pads_[2], conv_op.pads_[1],
                                 conv_op.pads_[3], 0)
          .reshape(input_matrix_dimensions);

  const std::array<Eigen::IndexPair<Eigen::Index>, 1> contraction_dimensions = {
      Eigen::IndexPair<Eigen::Index>(1, 0)};
  auto output_matrix =
      kernel_matrix.shuffle(std::array<Eigen::Index, 2>{1, 0})
          .contract(input_matrix.shuffle(std::array<Eigen::Index, 2>{1, 0}), contraction_dimensions)
          .shuffle(std::array<Eigen::Index, 2>{1, 0});

  const std::array<Eigen::Index, 3> rev_output_dimensions = {
      output.dimension(2), output.dimension(1), output.dimension(0)};
  output =
      output_matrix.reshape(rev_output_dimensions).shuffle(Eigen::array<Eigen::Index, 3>{2, 1, 0});
}

template <typename T>
std::vector<T> convolution(const tensor::Conv2DOp& conv_op, const std::vector<T>& input_buffer,
                           const std::vector<T>& kernel_buffer) {
  assert(conv_op.verify());
  assert(input_buffer.size() == conv_op.compute_input_size());
  assert(kernel_buffer.size() == conv_op.compute_kernel_size());
  std::vector<T> output_buffer(conv_op.compute_output_size());
  convolution(conv_op, input_buffer.data(), kernel_buffer.data(), output_buffer.data());
  return output_buffer;
}

void convolution(const tensor::Conv2DOp&, const std::uint8_t*, const std::uint8_t*, std::uint8_t*);
void convolution(const tensor::Conv2DOp&, const std::uint16_t*, const std::uint16_t*,
                 std::uint16_t*);
void convolution(const tensor::Conv2DOp&, const std::uint32_t*, const std::uint32_t*,
                 std::uint32_t*);
void convolution(const tensor::Conv2DOp&, const std::uint64_t*, const std::uint64_t*,
                 std::uint64_t*);
template std::vector<std::uint8_t> convolution(const tensor::Conv2DOp&,
                                               const std::vector<std::uint8_t>&,
                                               const std::vector<std::uint8_t>&);
template std::vector<std::uint16_t> convolution(const tensor::Conv2DOp&,
                                                const std::vector<std::uint16_t>&,
                                                const std::vector<std::uint16_t>&);
template std::vector<std::uint32_t> convolution(const tensor::Conv2DOp&,
                                                const std::vector<std::uint32_t>&,
                                                const std::vector<std::uint32_t>&);
template std::vector<std::uint64_t> convolution(const tensor::Conv2DOp&,
                                                const std::vector<std::uint64_t>&,
                                                const std::vector<std::uint64_t>&);
template std::vector<__uint128_t> convolution(const tensor::Conv2DOp&,
                                              const std::vector<__uint128_t>&,
                                              const std::vector<__uint128_t>&);

template <typename T>
void sum_pool(const tensor::AveragePoolOp& avgpool_op, const T* input, T* output) {
  assert(avgpool_op.verify());
  using TensorType3C = Eigen::Tensor<const T, 3, Eigen::RowMajor>;
  using TensorType3 = Eigen::Tensor<T, 3, Eigen::RowMajor>;
  const auto in_channels = static_cast<Eigen::Index>(avgpool_op.input_shape_[0]);
  const auto in_rows = static_cast<Eigen::Index>(avgpool_op.input_shape_[1]);
  const auto in_columns = static_cast<Eigen::Index>(avgpool_op.input_shape_[2]);
  const auto out_channels = static_cast<Eigen::Index>(avgpool_op.output_shape_[0]);
  const auto out_rows = static_cast<Eigen::Index>(avgpool_op.output_shape_[1]);
  const auto out_columns = static_cast<Eigen::Index>(avgpool_op.output_shape_[2]);
  const auto kernel_rows = static_cast<Eigen::Index>(avgpool_op.kernel_shape_[0]);
  const auto kernel_columns = static_cast<Eigen::Index>(avgpool_op.kernel_shape_[1]);
  const auto stride_rows = static_cast<Eigen::Index>(avgpool_op.strides_[0]);
  const auto stride_columns = static_cast<Eigen::Index>(avgpool_op.strides_[1]);

  Eigen::TensorMap<TensorType3C> tensor_src(input, in_channels, in_rows, in_columns);
  Eigen::TensorMap<TensorType3> tensor_dst(output, out_channels, out_rows, out_columns);

  tensor_dst = tensor_src.shuffle(Eigen::array<Eigen::Index, 3>{2, 1, 0})
                   .extract_image_patches(kernel_rows, kernel_columns, stride_rows, stride_columns,
                                          1, 1, 1, 1, 0, 0, 0, 0, T(0))
                   .sum(Eigen::array<Eigen::Index, 2>{1, 2})
                   .reshape(Eigen::array<Eigen::Index, 3>{out_columns, out_rows, out_channels})
                   .shuffle(Eigen::array<Eigen::Index, 3>{2, 1, 0});
}

template void sum_pool(const tensor::AveragePoolOp&, const std::uint8_t*, std::uint8_t*);
template void sum_pool(const tensor::AveragePoolOp&, const std::uint16_t*, std::uint16_t*);
template void sum_pool(const tensor::AveragePoolOp&, const std::uint32_t*, std::uint32_t*);
template void sum_pool(const tensor::AveragePoolOp&, const std::uint64_t*, std::uint64_t*);
template void sum_pool(const tensor::AveragePoolOp&, const __uint128_t*, __uint128_t*);

}  // namespace MOTION


#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/AutoDiff>
#include <iostream>

// Computes the energy terms into costs. The first part contains termes related to edge length variation,
// and the second to angle variations.
template<typename Scalar>
void ikLikeCosts(const Eigen::Matrix<Scalar,Eigen::Dynamic,1>& curve, const Eigen::VectorXd& targetAngles, const Eigen::VectorXd& targetLengths, double beta,
                 Eigen::Matrix<Scalar,Eigen::Dynamic,1>& costs)
{
    using namespace Eigen;
    using std::atan2;

    typedef Matrix<Scalar,2,1> Vec2;
    int nb = curve.size()/2;

    costs.setZero();
    for(int k=1;k<nb-1;++k)
    {
        Vec2 pk0 = curve.template segment<2>(2*(k-1));
        Vec2 pk  = curve.template segment<2>(2*k);
        Vec2 pk1 = curve.template segment<2>(2*(k+1));

        if(k+1<nb-1) {
            costs((nb-2)+(k-1)) = (pk1-pk).norm() - targetLengths(k-1);
        }

        Vec2 v0 = (pk-pk0).normalized();
        Vec2 v1 = (pk1-pk).normalized();

        costs(k-1) = beta * (atan2(-v0.y() * v1.x() + v0.x() * v1.y(), v0.x() * v1.x() + v0.y() * v1.y()) - targetAngles(k-1));
    }
}

// Generic functor
template<typename _Scalar>
struct Functor
{
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = Eigen::Dynamic,
        ValuesAtCompileTime = Eigen::Dynamic
                          };
    typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
    typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
    typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

    const int m_inputs, m_values;

    Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
    Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

    int inputs() const { return m_inputs; } // number of degree of freedom (= 2*nb_vertices)
    int values() const { return m_values; } // number of energy terms (= nb_vertices + nb_edges)

    // you should define that in the subclass :
    //    void operator() (const InputType& x, ValueType* v, JacobianType* _j=0) const;
};

// Specialized functor warping the ikLikeCosts function
struct iklike_functor : Functor<double>
{
    typedef Eigen::AutoDiffScalar<Eigen::Matrix<Scalar,Eigen::Dynamic,1> > ADS;
    typedef Eigen::Matrix<ADS, Eigen::Dynamic, 1> VectorXad;

    // pfirst and plast are the two extremities of the curve
    iklike_functor(const Eigen::VectorXd& targetAngles, const Eigen::VectorXd& targetLengths, double beta, const Eigen::Vector2d pfirst, const Eigen::Vector2d plast)
        :   Functor<double>(targetAngles.size()*2-4,targetAngles.size()*2-1),
            m_targetAngles(targetAngles), m_targetLengths(targetLengths), m_beta(beta),
            m_pfirst(pfirst), m_plast(plast)
    {}

    // input = x = {  ..., x_i, y_i, ....}
    // output = fvec = the value of each term
    int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec)
    {
        using namespace Eigen;
        VectorXd curves(this->inputs()+8);

        curves.segment(4,this->inputs()) = x;

        Vector2d d(1,0);
        curves.segment<2>(0)                   = m_pfirst - d;
        curves.segment<2>(2)                   = m_pfirst;
        curves.segment<2>(this->inputs()+4)    = m_plast;
        curves.segment<2>(this->inputs()+6)    = m_plast + d;

        ikLikeCosts(curves, m_targetAngles, m_targetLengths, m_beta, fvec);
        return 0;
    }

    // Compute the jacobian into fjac for the current solution x
    int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac)
    {
        using namespace Eigen;
        VectorXad curves(this->inputs()+8);

        // Compute the derivatives of each degree of freedom
        // -> grad( x_i ) = (0, ..., 0, 1, 0, ..., 0) ; 1 is in position i
        for(int i=0; i<this->inputs();++i)
            curves(4+i) = ADS(x(i), this->inputs(), i);

        Vector2d d(1,0);
        curves.segment<2>(0)                   = (m_pfirst - d).cast<ADS>();
        curves.segment<2>(2)                   = (m_pfirst).cast<ADS>();
        curves.segment<2>(this->inputs()+4)    = (m_plast).cast<ADS>();
        curves.segment<2>(this->inputs()+6)    = (m_plast + d).cast<ADS>();

        VectorXad v(this->values());

        ikLikeCosts(curves, m_targetAngles, m_targetLengths, m_beta, v);

        // copy the gradient of each energy term into the Jacobian
        for(int i=0; i<this->values();++i)
            fjac.row(i) = v(i).derivatives();

        return 0;
    }

    const Eigen::VectorXd& m_targetAngles;
    const Eigen::VectorXd& m_targetLengths;
    double m_beta;
    Eigen::Vector2d m_pfirst, m_plast;
};

void draw_vecX(const Eigen::VectorXd& res)
{
  for( int i = 0; i < (res.size()/2) ; i++ )
  {
    std::cout << res.segment<2>(2*i).transpose() << "\n";
  }
}

using namespace Eigen;

int main()
{
    Eigen::Vector2d pfirst(-5., 0.);
    Eigen::Vector2d plast ( 5., 0.);

    // rest pose is a straight line starting between first and last point
    const int nb_points = 30;
    Eigen::VectorXd targetAngles (nb_points);
    targetAngles.fill(0);

    Eigen::VectorXd targetLengths(nb_points-1);
    double val = (pfirst-plast).norm() / (double)(nb_points-1);
    targetLengths.fill(val);

    // get initial solution
    Eigen::VectorXd x((nb_points-2)*2);
    for(int i = 1; i < (nb_points - 1); i++)
    {
        double s = (double)i / (double)(nb_points-1);
        x.segment<2>((i-1)*2) = plast * s + pfirst * (1. - s);
    }

    // move last point
    plast = Eigen::Vector2d(4., 1.);

    // Create the functor object
    iklike_functor func(targetAngles, targetLengths, 0.1, pfirst, plast);

    // construct the solver
    Eigen::LevenbergMarquardt<iklike_functor> lm(func);

    // adjust tolerance
    lm.parameters.ftol *= 1e-2;
    lm.parameters.xtol *= 1e-2;
    lm.parameters.maxfev = 2000;

    int a = lm.minimize(x);
    std::cerr << "info = " << a  << " " << lm.nfev << " " << lm.njev << " "  <<  "\n";

    std::cout << pfirst.transpose() << "\n";
    draw_vecX( x);
    std::cout << plast.transpose() << "\n";
}


#ifndef TRIUMF_NMR_HEBEL_SLICHTER_HPP
#define TRIUMF_NMR_HEBEL_SLICHTER_HPP

#include <cmath>
#include <complex>

#include <boost/math/quadrature/exp_sinh.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>

#include <triumf/constants/codata_2018.hpp>
#include <triumf/statistical_mechanics/fermi_dirac.hpp>
#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/dynes.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// Nuclear Magnetic Resonance (NMR)
namespace nmr {

// Hebel-Slichter
namespace hebel_slichter {

template <typename T = double>
T integrand(T energy, T temperature, T critical_temperature, T gap_meV, T alpha,
            T Gamma) {
  // alias some values
  T Delta = triumf::superconductivity::bcs::gap<T>(
      temperature, critical_temperature, gap_meV);
  constexpr T E_0 = 0.0;
  constexpr T E_F = 0.0;
  //
  T E = energy;
  T E_p = E + alpha * gap_meV;

  // calculate the Fermi factors
  // Note: 1e-3 used to convert energyies from meV to eV
  T f_E = triumf::statistical_mechanics::fermi_dirac::distribution<T>(
      temperature, E * 1e-3, E_0, E_F);
  T f_E_p = triumf::statistical_mechanics::fermi_dirac::distribution<T>(
      temperature, E_p * 1e-3, E_0, E_F);

  //
  return (triumf::superconductivity::dynes::N(E, Gamma * gap_meV, Delta) *
              triumf::superconductivity::dynes::N(E_p, Gamma * gap_meV, Delta) +
          triumf::superconductivity::dynes::M(E, Gamma * gap_meV, Delta) *
              triumf::superconductivity::dynes::M(E_p, Gamma * gap_meV,
                                                  Delta)) *
         f_E * (1.0 - f_E_p);
}

// ratio of SLR rates in the superconducting and normal states
template <typename T = double>
T slr_ratio(T temperature, T critical_temperature, T gap_meV, T alpha,
            T Gamma) {
  // define some convenience values
  T reduced_temperature = temperature / critical_temperature;
  constexpr T k_B =
      1e3 *
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  T beta = 1.0 / (k_B * temperature);
  // return limiting values...
  // if (temperature >= critical_temperature) {
  //   return 1.0;
  // } else if (temperature <= 0.0) {
  //  return 0.0;
  if (temperature <= 0.0) {
    return 0.0;
    // ...before attempting to evaluate the intergral!
  } else {
    // define the integrand
    auto hs_integrand = [&](T E) -> T {
      return integrand<T>(E, temperature, critical_temperature, gap_meV, alpha,
                          Gamma);
    };
    // setup values for numeric integration
    // const T tolerance = std::numeric_limits<T>::epsilon();
    // const T tolerance = std::pow(std::numeric_limits<T>::epsilon(), 2.0
    // / 3.0);
    const T tolerance = std::sqrt(std::numeric_limits<T>::epsilon());

    const std::size_t max_refinements = 15;
    static boost::math::quadrature::exp_sinh<T> hs_integrator(max_refinements);
    return 2.0 * beta *
           hs_integrator.integrate(hs_integrand, tolerance, nullptr, nullptr,
                                   nullptr);
    /*
    const unsigned max_depth = 15;
    return 2.0 * beta *
           boost::math::quadrature::gauss_kronrod<T, 61>::integrate(
               hs_integrand, 0.0, std::numeric_limits<T>::infinity(), max_depth,
               tolerance, nullptr, nullptr);
    */
  }
}

} // namespace hebel_slichter

} // namespace nmr

} // namespace triumf

#endif // TRIUMF_NMR_HEBEL_SLICHTER_HPP