#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/matrix/row_echelon.h>
#include <scitbx/matrix/row_echelon_full_pivoting.h>
#include <scitbx/array_family/versa.h>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace scitbx { namespace matrix { namespace boost_python {

namespace {

  template <typename ElementType>
  af::ref<ElementType, af::mat_grid>
  flex_as_mat_ref(af::versa<ElementType, af::flex_grid<> >& a)
  {
    SCITBX_ASSERT(a.accessor().nd() == 2);
    SCITBX_ASSERT(a.accessor().is_0_based());
    SCITBX_ASSERT(!a.accessor().is_padded());
    return af::ref<ElementType, af::mat_grid>(
      a.begin(),
      a.accessor().all()[0],
      a.accessor().all()[1]);
  }

  std::size_t
  row_echelon_form_t(
    af::versa<int, af::flex_grid<> >& m,
    af::versa<int, af::flex_grid<> >& t)
  {
    af::ref<int, af::mat_grid> m_ref = flex_as_mat_ref(m);
    af::ref<int, af::mat_grid> t_ref = flex_as_mat_ref(t);
    std::size_t rank = row_echelon::form_t(m_ref, t_ref);
    m.resize(af::flex_grid<>(m_ref.n_rows(), m_ref.n_columns()));
    return rank;
  }

  std::size_t
  row_echelon_form(
    af::versa<int, af::flex_grid<> >& m)
  {
    af::ref<int, af::mat_grid> m_ref = flex_as_mat_ref(m);
    std::size_t rank = row_echelon::form(m_ref);
    m.resize(af::flex_grid<>(m_ref.n_rows(), m_ref.n_columns()));
    return rank;
  }

  int
  row_echelon_back_substitution_int(
    af::versa<int, af::flex_grid<> >& re_mx,
    af::const_ref<int> const& v,
    af::ref<int> const& sol,
    af::ref<bool> const& indep)
  {
    af::ref<int, af::mat_grid> re_mx_ref = flex_as_mat_ref(re_mx);
    const int* v_ptr = 0;
    int* sol_ptr = 0;
    bool* indep_ptr = 0;
    if (v.size()) {
      SCITBX_ASSERT(v.size() == re_mx_ref.n_rows());
      v_ptr = v.begin();
    }
    if (sol.size()) {
      SCITBX_ASSERT(sol.size() == re_mx_ref.n_columns());
      sol_ptr = sol.begin();
    }
    if (indep.size()) {
      SCITBX_ASSERT(indep.size() == re_mx_ref.n_columns());
      indep_ptr = indep.begin();
    }
    return row_echelon::back_substitution_int(
      re_mx_ref, v_ptr, sol_ptr, indep_ptr);
  }

  bool
  row_echelon_back_substitution_float(
    af::versa<int, af::flex_grid<> >& re_mx,
    af::const_ref<double> const& v,
    af::ref<double> const& sol)
  {
    af::ref<int, af::mat_grid> re_mx_ref = flex_as_mat_ref(re_mx);
    const double* v_ptr = 0;
    double* sol_ptr = 0;
    if (v.size()) {
      SCITBX_ASSERT(v.size() == re_mx_ref.n_rows());
      v_ptr = v.begin();
    }
    if (sol.size()) {
      SCITBX_ASSERT(sol.size() == re_mx_ref.n_columns());
      sol_ptr = sol.begin();
    }
    return row_echelon::back_substitution_float(
      re_mx_ref, v_ptr, sol_ptr);
  }

  void wrap_row_echelon()
  {
    using namespace boost::python;
    def("row_echelon_form_t", row_echelon_form_t);
    def("row_echelon_form", row_echelon_form);
    def("row_echelon_back_substitution_int",
      row_echelon_back_substitution_int);
    def("row_echelon_back_substitution_float",
      row_echelon_back_substitution_float);
  }

  struct full_pivoting_wrapper
  {
    typedef row_echelon::full_pivoting<double> wt;

    static void wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<wt>("row_echelon_full_pivoting", no_init)
        .def(init<
          af::versa<double, af::flex_grid<> >,
          double const&,
          int>((
            arg("a_work"),
            arg("min_abs_pivot")=0,
            arg("max_rank")=-1)))
        .def(init<
          af::versa<double, af::flex_grid<> >,
          af::shared<double>,
          double const&,
          int>((
            arg("a_work"),
            arg("b_work"),
            arg("min_abs_pivot")=0,
            arg("max_rank")=-1)))
        .add_property("col_perm", make_getter(&wt::col_perm, rbv()))
        .def_readonly("rank", &wt::rank)
        .def_readonly("nullity", &wt::nullity)
        .def("is_in_row_space", &wt::is_in_row_space, (
          arg("x"),
          arg("epsilon")))
        .def("back_substitution", &wt::back_substitution, (
          arg("free_values"),
          arg("epsilon")=0))
      ;
    }
  };

}}} // namespace matrix::boost_python::<anonymous>

namespace math { namespace boost_python {

  void wrap_row_echelon()
  {
    matrix::boost_python::wrap_row_echelon();
    matrix::boost_python::full_pivoting_wrapper::wrap();
  }

}}} // namespace scitbx::math::boost_python


#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdio>
#include <limits>

#include <harp_test.hpp>

#include <boost/random.hpp>

using namespace std;
using namespace harp;

void harp::test_linalg ( string const & datadir ) {

  cout << "Testing ublas LAPACK bindings..." << endl;

  // matrix-vector multiply

  size_t dim = 100;

  matrix_double mat ( dim, dim );
  vector_double input ( dim );
  vector_double output ( dim );
  vector_double check ( dim );

  typedef boost::ecuyer1988 base_generator_type;
  base_generator_type generator(42u);

  boost::uniform_real < double > dist ( -100.0, 100.0 );
  boost::variate_generator < base_generator_type&, boost::uniform_real < double > > uni ( generator, dist );

  for ( size_t i = 0; i < dim; ++i ) {
    input[i] = 10.0;
    for ( size_t j = 0; j < dim; ++j ) {
      mat( j, i ) = uni();
    }
  }

  check.clear();

  for ( size_t i = 0; i < dim; ++i ) {
    for ( size_t j = 0; j < dim; ++j ) {
      check[i] += mat( i, j ) * input[j];
    }
  }

  boost::numeric::bindings::blas::gemv ( 1.0, mat, input, 0.0, output );

  for ( size_t i = 0; i < dim; ++i ) {
    if ( fabs ( ( output[i] - check[i] ) / check[i] ) > std::numeric_limits < float > :: epsilon() ) {
      cerr << "FAIL:  blas::gemv output element " << i << " is wrong (" << output[i]
 << " != " << check[i] << ")" << endl;
      exit(1);
    }
  }

  cout << "  (PASSED)" << endl;

  cout << "Testing eigen-decomposition..." << endl;

  // construct random matrices

  double rngmax = 1000.0;

  matrix_double a1 ( dim, dim );
  matrix_double a2 ( dim, dim );

  typedef boost::ecuyer1988 base_generator_type;
  typedef boost::uniform_01<> distribution_type;
  typedef boost::variate_generator < base_generator_type&, distribution_type > gen_type;

  gen_type gen ( generator, distribution_type() );

  for ( size_t i = 0; i < dim; ++i ) {
    for ( size_t j = 0; j < dim; ++j ) {
      a1 ( i, j ) = rngmax * gen();
      a2 ( i, j ) = rngmax * gen();
    }
  }

  // construct symmetric test matrix

  matrix_double sym ( dim, dim );

  boost::numeric::bindings::blas::gemm ( 1.0, boost::numeric::bindings::trans ( a1 ), a1, 0.0, sym );
  boost::numeric::bindings::blas::gemm ( 1.0, boost::numeric::bindings::trans ( a2 ), a2, 1.0, sym );

  // get eigenvectors and eigenvalues

  vector_double w;
  matrix_double Z;

  eigen_decompose ( sym, w, Z, false );

  matrix_double symprod ( dim, dim );
  matrix_double eprod ( dim, dim );
  matrix_double wdiag ( dim, dim );

  wdiag.clear();

  for ( size_t i = 0; i < dim; ++i ) {
    wdiag ( i, i ) = w[i];
  }

  boost::numeric::bindings::blas::gemm ( 1.0, sym, Z, 0.0, symprod );
  boost::numeric::bindings::blas::gemm ( 1.0, Z, wdiag, 0.0, eprod );

  double relerr;
  double eval, sval;

  for ( size_t i = 0; i < dim; ++i ) {
    for ( size_t j = 0; j < dim; ++j ) {
      eval = eprod ( j, i );
      sval = symprod ( j, i );
      relerr = fabs ( ( eval - sval ) / sval );
      if ( relerr > std::numeric_limits < float > :: epsilon() ) {
        cerr << "FAIL on matrix element (" << j << ", " << i << ") Av = " << sval << ", ev = " << eval << " rel err = " << relerr << endl;
        exit(1);
      }
    }
  }

  cout << "  (PASSED)" << endl;

  cout << "Testing re-composition..." << endl;

  matrix_double outcomp;

  eigen_compose ( EIG_NONE, w, Z, outcomp );

  double inval;
  double outval;

  for ( size_t i = 0; i < dim; ++i ) {
    for ( size_t j = 0; j < dim; ++j ) {
      inval = sym ( j, i );
      outval = outcomp ( j, i );
      relerr = fabs ( ( outval - inval ) / inval );
      if ( relerr > std::numeric_limits < float > :: epsilon() ) {
        cerr << "FAIL on matrix element (" << j << ", " << i << ") input = " << inval << ", output = " << outval << " rel err = " << relerr << endl;
        exit(1);
      }
    }
  }

  matrix_double mat_rt;
  eigen_compose ( EIG_SQRT, w, Z, mat_rt );

  matrix_double mat_invrt;
  eigen_compose ( EIG_INVSQRT, w, Z, mat_invrt );

  vector_double w_inv;
  matrix_double Z_inv;

  eigen_decompose ( mat_invrt, w_inv, Z_inv, false );

  for ( size_t i = 0; i < dim; ++i ) {
    w_inv[i] *= w_inv[i];
  }

  matrix_double comp_rt;

  eigen_compose ( EIG_INVSQRT, w_inv, Z_inv, comp_rt );

  for ( size_t i = 0; i < dim; ++i ) {
    for ( size_t j = 0; j < dim; ++j ) {
      inval = mat_rt ( j, i );
      outval = comp_rt ( j, i );
      relerr = fabs ( ( outval - inval ) / inval );
      if ( relerr > std::numeric_limits < float > :: epsilon() ) {
        cerr << "FAIL on doubly inverted sqrt matrix element (" << j << ", " << i << ") original = " << inval << ", output = " << outval << " rel err = " << relerr << endl;
        exit(1);
      }
    }
  }

  cout << "  (PASSED)" << endl;

  return;
}


// Copyright Nick Thompson, 2020
// 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)

#include <iostream>
#include <unordered_map>
#include <string>
#include <future>
#include <thread>
#include <fstream>
#include <boost/hana/for_each.hpp>
#include <boost/hana/ext/std/integer_sequence.hpp>
#include <boost/math/special_functions/daubechies_scaling.hpp>
#include <boost/math/special_functions/detail/daubechies_scaling_integer_grid.hpp>
#include <boost/math/interpolators/cubic_hermite.hpp>
#include <boost/math/interpolators/quintic_hermite.hpp>
#include <boost/math/interpolators/quintic_hermite.hpp>
#include <boost/math/interpolators/septic_hermite.hpp>
#include <boost/math/interpolators/cardinal_quadratic_b_spline.hpp>
#include <boost/math/interpolators/cardinal_cubic_b_spline.hpp>
#include <boost/math/interpolators/cardinal_quintic_b_spline.hpp>
#include <boost/math/interpolators/whittaker_shannon.hpp>
#include <boost/math/interpolators/cardinal_trigonometric.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/interpolators/makima.hpp>
#include <boost/math/interpolators/pchip.hpp>
#include <boost/multiprecision/float128.hpp>
#include <boost/core/demangle.hpp>

using boost::multiprecision::float128;

template<typename Real, typename PreciseReal, int p>
void choose_refinement()
{
    std::cout << "Choosing refinement for " << boost::core::demangle(typeid(Real).name()) << " precision Daubechies scaling function with " << p << " vanishing moments.\n";
    using std::abs;
    int rmax = 22;
    auto phi_dense = boost::math::daubechies_scaling_dyadic_grid<PreciseReal, p, 0>(rmax);
    Real dx_dense = (2*p-1)/static_cast<Real>(phi_dense.size()-1);

    for (int r = 2; r <= 18; ++r)
    {
        Real dx = Real(1)/ (1 << r);
        std::cout << "\tdx = 1/" << (1/dx) << " = 1/2^" << r << " = " << dx << "\n";
        auto phi = boost::math::daubechies_scaling<Real, p>(r);
        Real max_flt_distance = 0;
        Real sup  = 0;
        Real rel_sup = 0;
        Real worst_flt_abscissa = 0;
        Real worst_flt_value = 0;
        Real worst_flt_computed = 0;

        Real worst_rel_abscissa = 0;
        Real worst_rel_value = 0;
        Real worst_rel_computed = 0;

        Real worst_abs_abscissa = 0;
        Real worst_abs_computed = 0;
        Real worst_abs_expected = 0;
        for (size_t i = 0; i < phi_dense.size(); ++i)
        {
            Real t = i*dx_dense;
            Real computed = phi(t);
            Real expected = Real(phi_dense[i]);
            Real abs_diff = abs(computed - expected);
            Real rel_diff = abs_diff/abs(expected);
            Real flt_distance = abs(boost::math::float_distance(computed, expected));
            if (flt_distance > max_flt_distance)
            {
                max_flt_distance = flt_distance;
                worst_flt_abscissa = t;
                worst_flt_value = expected;
                worst_flt_computed = computed;
            }
            if (expected != 0 && rel_diff > rel_sup)
            {
                rel_sup = rel_diff;
                worst_rel_abscissa = t;
                worst_rel_value = expected;
                worst_rel_computed = computed;

            }
            if (abs_diff > sup)
            {
                sup = abs_diff;
                worst_abs_abscissa = t;
                worst_abs_computed = computed;
                worst_abs_expected = expected;
            }
        }
        std::cout << "\t\tFloat distance at r = " << r << " is " << max_flt_distance << ", sup distance = " << sup << ", max relative error = " << rel_sup << "\n";
        std::cout << "\t\tWorst flt abscissa = " << worst_flt_abscissa << ", worst expected value = " << worst_flt_value << ", computed = " << worst_flt_computed << "\n";
        std::cout << "\t\tWorst rel abscissa = " << worst_rel_abscissa << ", worst expected value = " << worst_rel_value << ", computed = " << worst_rel_computed << "\n";
        std::cout << "\t\tWorst abs abscissa = " << worst_abs_abscissa << ", worst expected value = " << worst_abs_computed << ", worst abs value (expected) = " << worst_abs_expected << "\n";
    }
    std::cout << "\n\n\n";
}

template<typename Real, typename PreciseReal, int p>
void find_best_interpolator()
{
    std::string filename = "daubechies_" + std::to_string(p) + "_scaling_convergence.csv";
    std::ofstream fs{filename};
    static_assert(sizeof(PreciseReal) >= sizeof(Real), "sizeof(PreciseReal) >= sizeof(Real) is required.");
    using std::abs;
    int rmax = 18;
    std::cout << "Computing phi_dense_precise\n";
    auto phi_dense_precise = boost::math::daubechies_scaling_dyadic_grid<PreciseReal, p, 0>(rmax);
    std::vector<Real> phi_dense(phi_dense_precise.size());
    for (size_t i = 0; i < phi_dense.size(); ++i)
    {
        phi_dense[i] = static_cast<Real>(phi_dense_precise[i]);
    }
    phi_dense_precise.resize(0);
    std::cout << "Done\n";

    Real dx_dense = (2*p-1)/static_cast<Real>(phi_dense.size()-1);
    fs << std::setprecision(std::numeric_limits<Real>::digits10 + 3);
    fs << std::fixed;
    fs << "r, matched_holder, linear, quadratic_b_spline, cubic_b_spline, quintic_b_spline, cubic_hermite, pchip, makima, fo_taylor";
    if (p==2)
    {
        fs << "\n";
    }
    else
    {
        fs << ", quintic_hermite, second_order_taylor";
        if (p > 3)
        {
            fs << ", third_order_taylor, septic_hermite\n";
        }
        else
        {
            fs << "\n";
        }
    }
    for (int r = 2; r < 13; ++r)
    {
        fs << r << ", ";
        std::map<Real, std::string> m;
        auto phi = boost::math::daubechies_scaling_dyadic_grid<Real, p, 0>(r);
        auto phi_prime = boost::math::daubechies_scaling_dyadic_grid<Real, p, 1>(r);

        std::vector<Real> x(phi.size());
        Real dx = (2*p-1)/static_cast<Real>(x.size()-1);
        std::cout << "dx = 1/" << (1 << r) << " = " << dx << "\n";
        for (size_t i = 0; i < x.size(); ++i)
        {
            x[i] = i*dx;
        }

        {
            auto phi_copy = phi;
            auto phi_prime_copy = phi_prime;
            auto mh = boost::math::detail::matched_holder(std::move(phi_copy), std::move(phi_prime_copy), r, Real(0));
            Real sup = 0;
            // call to matched_holder is unchecked, so only go to phi_dense.size() -1.
            for (size_t i = 0; i < phi_dense.size() - 1; ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - mh(x));
                if (diff > sup)
                {
                    sup = diff;
                }
            }
            m.insert({sup, "matched_holder"});
            fs << sup << ", ";
        }

        {
            auto linear = [&phi, &dx, &r](Real x)->Real {
              if (x <= 0 || x >= 2*p-1)
              {
                return Real(0);
              }
              using std::floor;

              Real y = (1<<r)*x;
              Real k = floor(y);

              size_t kk = static_cast<size_t>(k);

              Real t = y - k;
              return (1-t)*phi[kk] + t*phi[kk+1];
            };

            Real linear_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - linear(x));
                if (diff > linear_sup)
                {
                    linear_sup = diff;
                }
            }
            m.insert({linear_sup, "linear interpolation"});
            fs << linear_sup << ", ";
        }

        {
            auto qbs = boost::math::interpolators::cardinal_quadratic_b_spline(phi.data(), phi.size(), Real(0), dx, phi_prime.front(), phi_prime.back());
            Real qbs_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - qbs(x));
                if (diff > qbs_sup) {
                    qbs_sup = diff;
                }
            }
            m.insert({qbs_sup, "quadratic_b_spline"});
            fs << qbs_sup << ", ";
        }

        {
            auto cbs = boost::math::interpolators::cardinal_cubic_b_spline(phi.data(), phi.size(), Real(0), dx, phi_prime.front(), phi_prime.back());
            Real cbs_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - cbs(x));
                if (diff > cbs_sup)
                {
                    cbs_sup = diff;
                }
            }
            m.insert({cbs_sup, "cubic_b_spline"});
            fs << cbs_sup << ", ";
        }

        {
            auto qbs = boost::math::interpolators::cardinal_quintic_b_spline(phi.data(), phi.size(), Real(0), dx, {0,0}, {0,0});
            Real qbs_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - qbs(x));
                if (diff > qbs_sup)
                {
                    qbs_sup = diff;
                }
            }
            m.insert({qbs_sup, "quintic_b_spline"});
            fs << qbs_sup << ", ";
        }

        {
            auto phi_copy = phi;
            auto phi_prime_copy = phi_prime;
            auto ch = boost::math::interpolators::cardinal_cubic_hermite(std::move(phi_copy), std::move(phi_prime_copy), Real(0), dx);
            Real chs_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - ch(x));
                if (diff > chs_sup)
                {
                    chs_sup = diff;
                }
            }
            m.insert({chs_sup, "cubic_hermite_spline"});
            fs << chs_sup << ", ";
        }

        {
            auto phi_copy = phi;
            auto x_copy = x;
            auto phi_prime_copy = phi_prime;
            auto pc = boost::math::interpolators::pchip(std::move(x_copy), std::move(phi_copy));
            Real pchip_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - pc(x));
                if (diff > pchip_sup)
                {
                  pchip_sup = diff;
                }
            }
            m.insert({pchip_sup, "pchip"});
            fs << pchip_sup << ", ";
        }

        {
            auto phi_copy = phi;
            auto x_copy = x;
            auto pc = boost::math::interpolators::makima(std::move(x_copy), std::move(phi_copy));
            Real makima_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i) {
              Real x = i*dx_dense;
              Real diff = abs(phi_dense[i] - pc(x));
              if (diff > makima_sup)
              {
                  makima_sup = diff;
              }
            }
            m.insert({makima_sup, "makima"});
            fs << makima_sup << ", ";
        }

        // Whittaker-Shannon interpolation has linear complexity; test over all points and it's quadratic.
        // I ran this a couple times and found it's not competitive; so comment out for now.
        /*{
            auto phi_copy = phi;
            auto ws = boost::math::interpolators::whittaker_shannon(std::move(phi_copy), Real(0), dx);
            Real sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i) {
              Real x = i*dx_dense;
              using std::abs;
              Real diff = abs(phi_dense[i] - ws(x));
              if (diff > sup) {
                sup = diff;
              }
            }

            m.insert({sup, "whittaker_shannon"});
        }

        // Again, linear complexity of evaluation => quadratic complexity of exhaustive checking.
        {
            auto trig = boost::math::interpolators::cardinal_trigonometric(phi, Real(0), dx);
            Real sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i) {
              Real x = i*dx_dense;
              using std::abs;
              Real diff = abs(phi_dense[i] - trig(x));
              if (diff > sup) {
                sup = diff;
              }
            }
            m.insert({sup, "trig"});
        }*/

        {
            auto fotaylor = [&phi, &phi_prime, &r](Real x)->Real
            {
                if (x <= 0 || x >= 2*p-1)
                {
                    return 0;
                }
                using std::floor;

                Real y = (1<<r)*x;
                Real k = floor(y);

                size_t kk = static_cast<size_t>(k);
                if (y - k < k + 1 - y)
                {
                    Real eps = (y-k)/(1<<r);
                    return phi[kk] + eps*phi_prime[kk];
                }
                else {
                    Real eps = (y-k-1)/(1<<r);
                    return phi[kk+1] + eps*phi_prime[kk+1];
                }
            };
            Real fo_sup = 0;
            for (size_t i = 0; i < phi_dense.size(); ++i)
            {
                Real x = i*dx_dense;
                Real diff = abs(phi_dense[i] - fotaylor(x));
                if (diff > fo_sup)
                {
                  fo_sup = diff;
                }
            }
            m.insert({fo_sup, "First-order Taylor"});
            if (p==2)
            {
                fs << fo_sup << "\n";
            }
            else
            {
                fs << fo_sup << ", ";
            }
        }

        if constexpr (p > 2) {
            auto phi_dbl_prime = boost::math::daubechies_scaling_dyadic_grid<Real, p, 2>(r);

            {
                auto phi_copy = phi;
                auto phi_prime_copy = phi_prime;
                auto phi_dbl_prime_copy = phi_dbl_prime;
                auto qh = boost::math::interpolators::cardinal_quintic_hermite(std::move(phi_copy), std::move(phi_prime_copy), std::move(phi_dbl_prime_copy), Real(0), dx);
                Real qh_sup = 0;
                for (size_t i = 0; i < phi_dense.size(); ++i)
                {
                    Real x = i*dx_dense;
                    Real diff = abs(phi_dense[i] - qh(x));
                    if (diff > qh_sup)
                    {
                        qh_sup = diff;
                    }
                }
                m.insert({qh_sup, "quintic_hermite_spline"});
                fs << qh_sup << ", ";
            }

            {
                auto sotaylor = [&phi, &phi_prime, &phi_dbl_prime, &r](Real x)->Real {
                      if (x <= 0 || x >= 2*p-1)
                      {
                          return 0;
                      }
                      using std::floor;

                      Real y = (1<<r)*x;
                      Real k = floor(y);

                      size_t kk = static_cast<size_t>(k);
                      if (y - k < k + 1 - y)
                      {
                          Real eps = (y-k)/(1<<r);
                          return phi[kk] + eps*phi_prime[kk] + eps*eps*phi_dbl_prime[kk]/2;
                      }
                      else {
                          Real eps = (y-k-1)/(1<<r);
                          return phi[kk+1] + eps*phi_prime[kk+1] + eps*eps*phi_dbl_prime[kk+1]/2;
                      }
                };
                Real so_sup = 0;
                for (size_t i = 0; i < phi_dense.size(); ++i)
                {
                    Real x = i*dx_dense;
                    Real diff = abs(phi_dense[i] - sotaylor(x));
                    if (diff > so_sup)
                    {
                        so_sup = diff;
                    }
                }
                m.insert({so_sup, "Second-order Taylor"});
                if (p > 3)
                {
                    fs << so_sup << ", ";
                }
                else
                {
                    fs << so_sup << "\n";
                }

            }
        }

        if constexpr (p > 3)
        {
            auto phi_dbl_prime = boost::math::daubechies_scaling_dyadic_grid<Real, p, 2>(r);
            auto phi_triple_prime = boost::math::daubechies_scaling_dyadic_grid<Real, p, 3>(r);

            {
                auto totaylor = [&phi, &phi_prime, &phi_dbl_prime, &phi_triple_prime, &r](Real x)->Real {
                      if (x <= 0 || x >= 2*p-1) {
                          return 0;
                      }
                      using std::floor;

                      Real y = (1<<r)*x;
                      Real k = floor(y);

                      size_t kk = static_cast<size_t>(k);
                      if (y - k < k + 1 - y)
                      {
                          Real eps = (y-k)/(1<<r);
                          return phi[kk] + eps*phi_prime[kk] + eps*eps*phi_dbl_prime[kk]/2 + eps*eps*eps*phi_triple_prime[kk]/6;
                      }
                      else {
                          Real eps = (y-k-1)/(1<<r);
                          return phi[kk+1] + eps*phi_prime[kk+1] + eps*eps*phi_dbl_prime[kk+1]/2 + eps*eps*eps*phi_triple_prime[kk]/6;
                      }
                };
                Real to_sup = 0;
                for (size_t i = 0; i < phi_dense.size(); ++i)
                {
                    Real x = i*dx_dense;
                    Real diff = abs(phi_dense[i] - totaylor(x));
                    if (diff > to_sup)
                    {
                        to_sup = diff;
                    }
                }

                m.insert({to_sup, "Third-order Taylor"});
                fs << to_sup << ", ";
            }

            {
                auto phi_copy = phi;
                auto phi_prime_copy = phi_prime;
                auto phi_dbl_prime_copy = phi_dbl_prime;
                auto phi_triple_prime_copy = phi_triple_prime;
                auto sh = boost::math::interpolators::cardinal_septic_hermite(std::move(phi_copy), std::move(phi_prime_copy), std::move(phi_dbl_prime_copy), std::move(phi_triple_prime_copy), Real(0), dx);
                Real septic_sup = 0;
                for (size_t i = 0; i < phi_dense.size(); ++i)
                {
                    Real x = i*dx_dense;
                    Real diff = abs(phi_dense[i] - sh(x));
                    if (diff > septic_sup)
                    {
                        septic_sup = diff;
                    }
                }
                m.insert({septic_sup, "septic_hermite_spline"});
                fs << septic_sup << "\n";
            }

        }
        std::string best = "none";
        Real best_sup = 1000000000;
        std::cout << std::setprecision(std::numeric_limits<Real>::digits10 + 3) << std::fixed;
        for (auto & e : m)
        {
            std::cout << "\t" << e.first << " is error of " << e.second << "\n";
            if (e.first < best_sup)
            {
                best = e.second;
                best_sup = e.first;
            }
        }
        std::cout << "\tThe best method for p = " << p << " is the " << best << "\n";
    }
}

int main()
{
    //boost::hana::for_each(std::make_index_sequence<4>(), [&](auto i){ choose_refinement<double, float128, i+16>(); });
    boost::hana::for_each(std::make_index_sequence<12>(), [&](auto i){ find_best_interpolator<double, float128, i+2>(); });
}


#include <armadillo>
#include <bitset>
#include <functional>

using namespace arma;
using namespace std;

namespace genetico {

template <unsigned int nBits>
bitset<nBits> codificar(double fenotipo,
                        double minimoFenotipo,
                        double maximoFenotipo)
{
    double rango = maximoFenotipo - minimoFenotipo;
    double factorConversion = (pow(2, nBits) - 1) / rango;

    unsigned int convertido = round((fenotipo - minimoFenotipo) * factorConversion);

    return bitset<nBits>{convertido};
}

template <unsigned int nBits, unsigned int nVariables>
bitset<nBits * nVariables> codificar(array<double, nVariables> fenotipo,
                                     const array<double, nVariables * 2>& limites)
{
    ostringstream ost;
    for (unsigned int i = 0; i < nVariables; ++i) {
        ost << codificar<nBits>(fenotipo.at(i),
                                limites.at(2 * i),
                                limites.at(2 * i + 1));
    }

    bitset<nBits * nVariables> result;
    istringstream ist{ost.str()};
    ist >> result;

    return result;
}

template <unsigned int nBits>
double decodificar(bitset<nBits> genotipo,
                   double minimoFenotipo,
                   double maximoFenotipo)
{
    double rango = maximoFenotipo - minimoFenotipo;
    double factorConversion = rango / (pow(2, nBits) - 1);

    double convertido = genotipo.to_ulong() * factorConversion + minimoFenotipo;

    return convertido;
}

template <unsigned int nBits, unsigned int nVariables>
array<double, nVariables> decodificar(const bitset<nBits * nVariables>& genotipo,
                                      const array<double, nVariables * 2>& limites)
{
    istringstream ist{genotipo.to_string()};
    array<double, nVariables> result;

    for (unsigned int i = 0; i < nVariables; ++i) {
        bitset<nBits> trozoGenotipo;
        ist >> trozoGenotipo;
        result.at(i) = decodificar<nBits>(trozoGenotipo,
                                          limites.at(2 * i),
                                          limites.at(2 * i + 1));
    }

    return result;
}

// Genotipo con un único cromosoma
template <unsigned int nBits, unsigned int nVariables>
struct Individuo {
    // Individuo inicializado al azar
    Individuo(const array<double, nVariables * 2>& t_limites)
        : genotipo{unsigned(randi(1, distr_param(0.0, pow(2, nBits * nVariables) - 1)).at(0))}
        , limites{t_limites}
    {
    }

    Individuo(bitset<nBits * nVariables> t_genotipo,
              const array<double, nVariables * 2>& t_limites)
        : genotipo{t_genotipo}
        , limites{t_limites}
    {
    }

    array<double, nVariables> fenotipo() const
    {
        return decodificar<nBits, nVariables>(genotipo, limites);
    }

    void mutar()
    {
        genotipo.flip(randi(1, distr_param(0.0, nBits * nVariables - 1)).at(0));
    }

    bitset<nBits * nVariables> genotipo;
    array<double, nVariables * 2> limites;
};

template <unsigned int nBits, unsigned int nVariables>
class Poblacion {
    using I = Individuo<nBits, nVariables>;

public:
    Poblacion(const array<double, nVariables * 2>& limites,
              const std::function<double(array<double, nVariables>)>& fitness,
              int nIndividuos,
              int nGeneraciones,
              int umbral);

    bool evaluarPoblacion();
    void evolucionar(int nGeneraciones);
    vector<I> seleccionarPadres();
    vector<I> hacerCruzas(const vector<I>& padres, int nHijos);

    const vector<I>& individuos() const { return m_individuos; };
    double mejorFitness() const { return m_mejorAptitud; };
    const I& mejorIndividuo() const { return m_mejorIndividuo; }
    bool termino() const { return m_termino; }
    double fitnessPromdedio() const;

    static pair<I, I> cruzar(const I& padre1,
                             const I& padre2,
                             int puntoCruza,
                             const array<double, nVariables * 2>& limites);

private:
    const array<double, nVariables * 2> m_limites;
    const std::function<double(array<double, nVariables>)> m_fitness;
    const int m_nIndividuos;
    vector<I> m_individuos;
    const int m_nGeneraciones;
    const int m_umbral;
    int m_generacion;
    double m_mejorAptitud;
    I m_mejorIndividuo;
    int m_generacionesSinMejora;
    bool m_termino;
};

template <unsigned int nBits, unsigned int nVariables>
Poblacion<nBits, nVariables>::
    Poblacion(const array<double, nVariables * 2>& limites,
              const std::function<double(array<double, nVariables>)>& fitness,
              int nIndividuos,
              int nGeneraciones,
              int umbral)
    : m_limites{limites}
    , m_fitness{fitness}
    , m_nIndividuos{nIndividuos}
    , m_nGeneraciones{nGeneraciones}
    , m_umbral{umbral}
    , m_mejorAptitud{-numeric_limits<double>::max()}
    , m_mejorIndividuo{limites}
    , m_generacionesSinMejora{0}
    , m_termino{false}
{
    for (int i = 0; i < nIndividuos; ++i)
        m_individuos.push_back(I{m_limites});
}

// Devuelve true si se cumple la condición de parada por no mejorar fitness
template <unsigned int nBits, unsigned int nVariables>
bool Poblacion<nBits, nVariables>::
    evaluarPoblacion()
{
    bool mejoro = false;

    for (const I& ind : m_individuos) {
        double aptitud = m_fitness(ind.fenotipo());

        if (aptitud > m_mejorAptitud) {
            m_mejorAptitud = aptitud;
            m_mejorIndividuo = ind;
            mejoro = true;
        }
    }

    return mejoro;
}

template <unsigned int nBits, unsigned int nVariables>
void Poblacion<nBits, nVariables>::
    evolucionar(int nGeneraciones)
{
    // Si nunca se evaluó la población, hacerlo ahora
    if (m_mejorAptitud == numeric_limits<double>::min())
        evaluarPoblacion();

    int generacion;
    for (generacion = 0; generacion < nGeneraciones; ++generacion) {
        vector<I> nuevaGeneracion;
        // 1 - Rescatar el mejor individuo y meterlo en la siguiente generacion
        nuevaGeneracion.push_back(m_mejorIndividuo);

        // 2 - Seleccionar los padres
        vector<I> padres = seleccionarPadres();
        //        copy(padres.begin(), padres.end(), back_inserter(nuevaGeneracion));

        // 3 - Hacer cruzas
        //		vector<I> hijos = hacerCruzas(padres, m_nIndividuos * 0.7);
        vector<I> hijos = hacerCruzas(padres, m_nIndividuos);

        // 4 - Hacer mutaciones
        for (I& h : hijos)
            if (randu(1).at(0, 0) <= 0.1)
                h.mutar();

        //        copy(hijos.begin(), hijos.end(), back_inserter(nuevaGeneracion));
        copy(hijos.begin(), hijos.end() - 1, back_inserter(nuevaGeneracion));
        m_individuos = nuevaGeneracion;

        // 5 - Evaluar la poblacion y ver si se dio el criterio de parada
        if (evaluarPoblacion())
            m_generacionesSinMejora = 0;
        else {
            if (m_generacionesSinMejora == m_umbral) {
                m_termino = true;
                break;
            }

            ++m_generacionesSinMejora;
        }
    }
}

template <unsigned int nBits, unsigned int nVariables>
auto Poblacion<nBits, nVariables>::
    seleccionarPadres() -> vector<I>
{
    vector<I> result;
    const int nPadres = 0.3 * m_nIndividuos - 1;
    const int k = 2;

    for (int i = 0; i < nPadres; ++i) {
        const uvec indices = shuffle(linspace<uvec>(0, m_nIndividuos - 1, m_nIndividuos));

        vector<I> candidatos;
        vec aptitudes(k);
        for (int j = 0; j < k; ++j) {
            candidatos.push_back(m_individuos.at(indices(j)));
            aptitudes(j) = m_fitness(candidatos.at(j).fenotipo());
        }

        I mejor = m_individuos.at(indices(aptitudes.index_max()));
        result.push_back(mejor);
    }

    return result;
}

template <unsigned int nBits, unsigned int nVariables>
auto Poblacion<nBits, nVariables>::
    cruzar(const I& padre1,
           const I& padre2,
           int puntoCruza,
           const array<double, nVariables * 2>& limites)
        -> pair<I, I>
{
    I hijo1{limites}, hijo2{limites};

    for (int i = 0; i < int(padre1.genotipo.size()); ++i) {
        // Nota: El operador de acceso ([]) a un bitset los accede desde
        // el bit menos significativo al más significativo.

        if (i < puntoCruza) { // Parte izquierda del cromosoma
            hijo1.genotipo[i] = padre1.genotipo[i];
            hijo2.genotipo[i] = padre2.genotipo[i];
        }
        else { // Parte derecha del cromosoma
            hijo1.genotipo[i] = padre2.genotipo[i];
            hijo2.genotipo[i] = padre1.genotipo[i];
        }
    }

    return {hijo1, hijo2};
}

template <unsigned int nBits, unsigned int nVariables>
auto Poblacion<nBits, nVariables>::
    hacerCruzas(const vector<I>& padres, int nHijos) -> vector<I>
{
    vector<I> padresAux;
    vector<I> hijos;

    for (int i = 0; i < nHijos; i += 2) {
        if (padresAux.empty()) {
            padresAux = padres;
            random_shuffle(padresAux.begin(), padresAux.end());
        }
        I padre1 = padresAux.back();
        padresAux.pop_back();

        if (padresAux.empty()) {
            padresAux = padres;
            random_shuffle(padresAux.begin(), padresAux.end());
        }
        I padre2 = padresAux.back();
        padresAux.pop_back();

        int puntoCruza = randi(1, distr_param(1, nBits - 1)).at(0);

        I hijo1{m_limites}, hijo2{m_limites};

        if (randu(1).at(0, 0) <= 0.9) // 90% de probabilidad de cruza
            tie(hijo1, hijo2) = cruzar(padre1, padre2, puntoCruza, m_limites);
        else {
            hijo1 = padre1;
            hijo2 = padre2;
        }

        hijos.push_back(hijo1);
        hijos.push_back(hijo2);
    }

    return hijos;
}

template <unsigned int nBits, unsigned int nVariables>
double Poblacion<nBits, nVariables>::
    fitnessPromdedio() const
{
    double suma = 0;

    for (const I& ind : m_individuos) {
        double aptitud = m_fitness(ind.fenotipo());
        suma += aptitud;
    }

    return suma / m_individuos.size();
}
} // namespace genetico


// test CPU direct R solver

#include "test_common_linear_systems.h"

#include <minisam/3rdparty/Catch2/catch.hpp>
#include <minisam/utils/testAssertions.h>

#include <minisam/linear/SquareRootCholesky.h>

#include <Eigen/LU> // rank

using namespace minisam;

// exmaple systems
test::ExampleLinearSystems data;
Eigen::SparseMatrix<double> A, H;

/* ************************************************************************** */
TEST_CASE("SquareRootCholesky_prep_static_values", "[linear]") {

  // use random A to generate test, to test whether R'R - ordering.permute(A'A) = 0
  const int A_row = 40, A_col = 20;
  Eigen::MatrixXd A_dense;
  int A_rank;
  do {
    A_dense = Eigen::MatrixXd::Random(A_row, A_col);
    Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(A_dense);
    A_rank = lu_decomp.rank();
  } while (A_rank < A_col); // make sure full rank

  A = A_dense.sparseView();
  H = A.transpose() * A;
}

/* ************************************************************************** */
TEST_CASE("SquareRootCholesky", "[linear]") {

  Eigen::VectorXd x_act;
  SquareRootSolverStatus status;
  SquareRootSolverCholesky chol(OrderingMethod::NONE);

  Eigen::SparseMatrix<double> R_act(2,2), L_act(2,2), L_exp;

  chol.initialize(data.A1.transpose() * data.A1);
  status = chol.solveR(data.A1.transpose() * data.A1, R_act);
  CHECK(assert_equal(data.R1_exp, R_act));
  CHECK(status == SquareRootSolverStatus::SUCCESS);
  status = chol.solveL(data.A1.transpose() * data.A1, L_act);
  L_exp = data.R1_exp.transpose();
  CHECK(assert_equal(L_exp, L_act));
  CHECK(status == SquareRootSolverStatus::SUCCESS);

  chol.initialize(data.A2.transpose() * data.A2);
  status = chol.solveR(data.A2.transpose() * data.A2, R_act);
  CHECK(assert_equal(data.R2_exp, R_act));
  CHECK(status == SquareRootSolverStatus::SUCCESS);
  status = chol.solveL(data.A2.transpose() * data.A2, L_act);
  L_exp = data.R2_exp.transpose();
  CHECK(assert_equal(L_exp, L_act));
  CHECK(status == SquareRootSolverStatus::SUCCESS);

  chol.initialize(data.A3.transpose() * data.A3);
  status = chol.solveR(data.A3.transpose() * data.A3, R_act);
  CHECK(assert_equal(data.R3_exp, R_act));
  CHECK(status == SquareRootSolverStatus::SUCCESS);
  status = chol.solveL(data.A3.transpose() * data.A3, L_act);
  L_exp = data.R3_exp.transpose();
  CHECK(assert_equal(L_exp, L_act));
  CHECK(status == SquareRootSolverStatus::SUCCESS);

  chol.initialize(data.A4.transpose() * data.A4);
  status = chol.solveR(data.A4.transpose() * data.A4, R_act);
  CHECK(status == SquareRootSolverStatus::RANK_DEFICIENCY);
  status = chol.solveL(data.A4.transpose() * data.A4, R_act);
  CHECK(status == SquareRootSolverStatus::RANK_DEFICIENCY);
}

/* ************************************************************************** */
TEST_CASE("SquareRootCholesky_random", "[linear]") {

  Eigen::SparseMatrix<double> R, L, PHPt_act, PHPt_exp;

  // AMD ordering
  SquareRootSolverStatus status;
  SquareRootSolverCholesky chol;

  status = chol.initialize(H);
  CHECK(status == SquareRootSolverStatus::SUCCESS);
  chol.ordering()->permuteSystemFull(H, PHPt_exp);

  // R'R = ordering.permute(A'A)
  status = chol.solveR(H, R);
  CHECK(status == SquareRootSolverStatus::SUCCESS);
  PHPt_act = R.transpose() * R;
  CHECK(assert_equal(PHPt_exp, PHPt_act));

  // LL' = ordering.permute(A'A)
  status = chol.solveL(H, L);
  CHECK(status == SquareRootSolverStatus::SUCCESS);
  PHPt_act = L * L.transpose();
  CHECK(assert_equal(PHPt_exp, PHPt_act));

  // no ordering
  SquareRootSolverCholesky choln(OrderingMethod::NONE);

  status = choln.initialize(H);
  CHECK(status == SquareRootSolverStatus::SUCCESS);

  // R'R = A'A
  status = choln.solveR(H, R);
  CHECK(status == SquareRootSolverStatus::SUCCESS);
  PHPt_act = R.transpose() * R;
  CHECK(assert_equal(H, PHPt_act));

  // LL' = A'A
  status = choln.solveL(H, L);
  CHECK(status == SquareRootSolverStatus::SUCCESS);
  PHPt_act = L * L.transpose();
  CHECK(assert_equal(H, PHPt_act));
}


#include "pow.h"

#include <cmath>

#include <boost/test/unit_test.hpp>

using namespace std;

namespace
{
	float log2f4to1(float x)
	{
		__declspec(align(16)) float v[4];
		_mm_store_ps(v, log2f4(_mm_set1_ps(x)));
		return v[0];
	}

	float log2f8to1(float x)
	{
		__declspec(align(32)) float v[8];
		_mm256_store_ps(v, log2f8(_mm256_set1_ps(x)));
		return v[0];
	}
}

BOOST_AUTO_TEST_CASE(Log2)
{
	constexpr float tolerance = 0.002f;

	const float args[] = { 0.1f, 0.5f, 1.0f, 2.0f, 5.0f, 100.0f };
	for (float arg : args)
	{
		BOOST_CHECK_CLOSE(log2f4to1(arg), log2(arg), tolerance);
		BOOST_CHECK_CLOSE(log2f8to1(arg), log2(arg), tolerance);
	}
}

BOOST_AUTO_TEST_CASE(Log2f4_Fullness)
{
	constexpr float x = 0.1f;

	__declspec(align(16)) float v[4];
	_mm_store_ps(v, log2f4(_mm_set1_ps(x)));

	for (size_t i = 1; i != 4; ++i)
		BOOST_CHECK_EQUAL(v[0], v[i]);
}

BOOST_AUTO_TEST_CASE(Log2f8_Fullness)
{
	constexpr float x = 0.1f;

	__declspec(align(32)) float v[8];
	_mm256_store_ps(v, log2f8(_mm256_set1_ps(x)));

	for (size_t i = 1; i != 8; ++i)
		BOOST_CHECK_EQUAL(v[0], v[i]);
}


/* ----------------------------------------------------------------------- *//**
 *
 * @file link.hpp
 *
 *//* ----------------------------------------------------------------------- */

#ifndef MADLIB_MODULES_GLM_LINK_HPP
#define MADLIB_MODULES_GLM_LINK_HPP

#include <cmath>
#include <modules/prob/boost.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math/special_functions/erf.hpp>

// ------------------------------------------------------------

using namespace madlib::dbal::eigen_integration;

namespace madlib {

namespace modules {

namespace glm {

using namespace std;

// ------------------------------------------------------------

class Identity {
public:
    static double init(const double &y) { return y + 0.1; }
    static double link_func(const double &mu) { return mu; }
    static double mean_func(const double &ita) { return ita; }
    static double mean_derivative(const double &) { return 1.; }
};

// ------------------------------------------------------------

class Log {
public:
    static double init(const double &y) { return std::max(y, 0.1); }
    static double link_func(const double &mu) { return log(mu); }
    static double mean_func(const double &ita) { return exp(ita); }
    static double mean_derivative(const double &ita) { return exp(ita); }
};

// ------------------------------------------------------------

class Sqrt {
public:
    static double init(const double &y) { return std::max(y, 0.); }
    static double link_func(const double &mu) { return sqrt(mu); }
    static double mean_func(const double &ita) { return ita * ita; }
    static double mean_derivative(const double &ita) { return 2 * ita; }
};

// ------------------------------------------------------------
class Inverse {
public:
    static double init(const double &y) { return y == 0 ? 0.1 : y + 0.1; }
    static double link_func(const double &mu) { return 1./mu; }
    static double mean_func(const double &ita) { return 1./ita; }
    static double mean_derivative(const double &ita) { return -1./(ita*ita); }
};

// ------------------------------------------------------------

class SqrInverse {
public:
    static double init(const double &y) { return y == 0 ? 0.1 : y + 0.1; }
    static double link_func(const double &mu) { return 1./mu/mu; }
    static double mean_func(const double &ita) { return 1./sqrt(ita); }
    static double mean_derivative(const double &ita) { return -1./2/sqrt(ita*ita*ita); }
};

// ------------------------------------------------------------

class Probit
{
public:
    static double init(const double &y) { return (y + 0.5) / 2; }
    static double link_func(const double &mu) {
        double root_2 = sqrt(2);
        return root_2 * boost::math::erf_inv(2*mu-1);
    }

    static double mean_func(const double &ita) {
        return prob::cdf(prob::normal(), ita);
    }

    static double mean_derivative(const double &ita) {
        return exp(-ita*ita/2.)/sqrt(2*M_PI);
    }
};

// ------------------------------------------------------------

class Logit {
public:
    static double init(const double &y) { return (y + 0.5) / 2; }
    static double link_func(const double &mu) {
        return log(mu / (1 - mu));
    }
    static double mean_func(const double &ita) {
        return 1./(1 + exp(-ita));
    }
    static double mean_derivative(const double &ita) {
        return 1./((1 + exp(-ita)) * (1 + exp(ita)));
    }
};

// ------------------------------------------------------------

class MultiLogit {
public:
    static void init(ColumnVector &mu) {
        mu.fill(1./static_cast<double>(mu.size()+1)); // later we may consider to use y to initialize mu
    }
    static void link_func(const ColumnVector &mu, ColumnVector &ita) {
        for (int i=0;i<mu.size();i++)
          ita(i) = log(mu(i)) - log(1-mu.sum());
    }
    static void mean_func(const ColumnVector &ita, ColumnVector &mu) {
        double temp=0;
        for(int i=0;i<ita.size();i++)
           temp += exp(ita(i));
        temp = temp+1;
        for(int i=0;i<ita.size();i++)
           mu(i) = exp(ita(i))/temp;
    }
    static void mean_derivative(const ColumnVector &ita, Matrix &mu_prime) {
        double temp=0;
        for(int i=0;i<ita.size();i++)
           temp += exp(ita(i));
        temp = temp+1;
        for(int i=0;i<ita.size();i++) {
          for(int j=0;j<ita.size();j++) {
             if(i==j)
                mu_prime(i,j)=exp(ita(i))*(temp - exp(ita(i)))/(temp*temp);
             else
                mu_prime(i,j)=-exp(ita(i))*exp(ita(j))/(temp*temp);
          }
        }
    }
};

// ------------------------------------------------------------

class OrdinalLogit {
public:
    static void init(ColumnVector &mu) {
        mu.fill(1./static_cast<double>(mu.size()+1)); // later we may consider to use y to initialize mu
    }
    static void link_func(const ColumnVector &mu, ColumnVector &ita) {
        ColumnVector sum_vec(mu.size());
        sum_vec(0) = mu(0);
        for (int i=1;i<mu.size();i++)
            sum_vec(i) = sum_vec(i-1) + mu(i);
        for (int i=0;i<mu.size();i++)
            ita(i) = log(sum_vec(i)/(1-sum_vec(i)));
    }
    static void mean_func(const ColumnVector &ita, ColumnVector &mu) {
        mu(0) = exp(ita(0))/(1+exp(ita(0)));
        for(int i=1;i<ita.size();i++)
           mu(i) = exp(ita(i))/(1+exp(ita(i)))- exp(ita(i-1))/(1+exp(ita(i-1)));
    }
    static void mean_derivative(const ColumnVector &ita, Matrix &mu_prime) {
        mu_prime.fill(0);
        for(int i=0;i<ita.size();i++) {
            for(int j=0;j<=i;j++) {
                if(i==j)
                    mu_prime(i,j)=exp(ita(i))/(1+exp(ita(i)))/(1+exp(ita(i)));
                else if(i==(j+1))
                    mu_prime(i,j)=-exp(ita(j))/(1+exp(ita(j)))/(1+exp(ita(j)));
                else
                    mu_prime(i,j)=0;
            }
        }
    }
};

// ------------------------------------------------------------

class OrdinalProbit {
public:
    static void init(ColumnVector &mu) {
        mu.fill(1./static_cast<double>(mu.size()+1)); // later we may consider to use y to initialize mu
    }
    static void link_func(const ColumnVector &mu, ColumnVector &ita) {
        ColumnVector sum_vec(mu.size());
        double root_2 = sqrt(2);
        sum_vec(0) = mu(0);
        for (int i=1;i<mu.size();i++)
            sum_vec(i) = sum_vec(i-1) + mu(i);
        for (int i=0;i<mu.size();i++)
            ita(i) = root_2 * boost::math::erf_inv(2*sum_vec(i)-1);
    }
    static void mean_func(const ColumnVector &ita, ColumnVector &mu) {
        mu(0) = prob::cdf(prob::normal(), ita(0));
        for(int i=1;i<ita.size();i++)
            mu(i) = prob::cdf(prob::normal(), ita(i))-prob::cdf(prob::normal(), ita(i-1));
    }
    static void mean_derivative(const ColumnVector &ita, Matrix &mu_prime) {
        mu_prime.fill(0);
        for(int i=0;i<ita.size();i++) {
            for(int j=0;j<=i;j++) {
                if(i==j)
                    mu_prime(i,j) = exp(-ita(i)*ita(i)/2.)/sqrt(2*M_PI);
                else if(i==(j+1))
                    mu_prime(i,j) = -exp(-ita(j)*ita(j)/2.)/sqrt(2*M_PI);
                else
                    mu_prime(i,j)=0;
            }
        }
    }
};

} // namespace glm

} // namespace modules

} // namespace madlib

#endif // defined(MADLIB_MODULES_GLM_LINK_HPP)


#include <algorithm>
#include <cinttypes>
#include <cmath>
#include <complex>
#include <iostream>
#include <iterator>
#include <vector>

#define png_infopp_NULL nullptr
#define int_p_NULL      nullptr
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_io.hpp>

namespace gil = boost::gil;

// map value in range r1 to r2
auto map_value(double val, double r1_from, double r1_to, double r2_from, double r2_to) -> double {
    return ((val - r1_from) / (r1_to - r1_from)) * (r2_to - r2_from) + r2_from;
}

class mandelbrot_fn {
    struct color_line {
        explicit color_line(int lv) :
            level{ lv }
        {}

        color_line(int lv, gil::bits8 r, gil::bits8 g, gil::bits8 b) :
            level{ lv },
            pixel{ r, g, b }
        {}

        int                 level;
        gil::rgb8_pixel_t   pixel;

        auto operator<(color_line const& rhs) const noexcept -> bool {
            return level < rhs.level;
        }
    };

public:
    using const_t           = mandelbrot_fn;
    using value_type        = gil::rgb8_pixel_t;
    using reference         = value_type;
    using const_reference   = value_type;
    using point_t           = gil::point2<int>;
    using result_type       = value_type;
    using argument_type     = point_t;
    static constexpr bool is_mutable = false;

    explicit mandelbrot_fn(point_t const& size) :
        m_size{ size }
    {
        m_colors.emplace_back( 0,   0,   0,  64);
        m_colors.emplace_back( 8,   0, 255, 255);
        m_colors.emplace_back(16,   0, 255,   0);
        m_colors.emplace_back(24, 255, 128,   0);
        m_colors.emplace_back(32,   0,   0, 255);
        m_colors.emplace_back(40, 255, 255,   0);
        m_colors.emplace_back(48, 255, 255, 255);
        m_colors.emplace_back(56, 255, 255, 255);
        m_colors.emplace_back(64, 255, 255, 255);
        sort(begin(m_colors), end(m_colors));
    }

    auto operator()(point_t const& point) const -> result_type {
        auto level = get_color_level(point);
        if (level < 0) {
            return result_type{ 0, 0, 0 };
        }

        auto uppos = upper_bound(begin(m_colors), end(m_colors), color_line{ level });
        if (uppos == end(m_colors)) {
            --uppos;
        }

        auto const& lower = *(uppos - 1);
        auto const& upper = *uppos;
        result_type pixel;
        for (auto i = 0; i < 3; ++i) {
            pixel[i] = map_value(level, lower.level, upper.level,
                                 lower.pixel[i], upper.pixel[i]);
        }
        return pixel;
    }

private:
    // get color level of given point
    // negative result if the point is in the set
    auto get_color_level(point_t const& point) const -> int {
        // map x to [-2, 1] and y to [1.5, -1.5]
        std::complex<double> c{map_value(point.x, 0, m_size.x, -2, 1),
                               map_value(point.y, 0, m_size.y, 1.5, -1.5)};
        int level = 0;
        for (auto lc = c; level < 64; ++level) {
            if (std::pow(lc.real(), 2) + std::pow(lc.imag(), 2) > 4) {
                return level;
            }
            lc = std::pow(lc, 2) + c;
        }
        return -1;
    }

    point_t m_size;
    std::vector<color_line> m_colors;
};

int main() {
    using point_t = mandelbrot_fn::point_t;
    using locator_t = gil::virtual_2d_locator<mandelbrot_fn, false>;
    using image_view_t = gil::image_view<locator_t>;

    point_t size{ 20'000, 20'000 };
    image_view_t view{ size, locator_t{ point_t{ 0, 0 }, point_t{ 1, 1 }, mandelbrot_fn{ size } } };
    gil::png_write_view("mandelbrot.png", view);
}


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3f m;
m.row(0) << 1, 2, 3;
m.block(1,0,2,2) << 4, 5, 7, 8;
m.col(2).tail(2) << 6, 9;
std::cout << m;

  return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2005, 2016 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include "gaussianquadratures.hpp"
#include "utilities.hpp"

#include <ql/types.hpp>
#include <ql/math/matrix.hpp>
#include <ql/math/functional.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/integrals/gaussianquadratures.hpp>
#include <ql/math/integrals/momentbasedgaussianpolynomial.hpp>
#include <ql/math/integrals/gausslaguerrecosinepolynomial.hpp>
#include <ql/experimental/math/gaussiannoncentralchisquaredpolynomial.hpp>

#include <boost/math/distributions/non_central_chi_squared.hpp>

#ifndef TEST_BOOST_MULTIPRECISION_GAUSSIAN_QUADRATURE
//#define TEST_BOOST_MULTIPRECISION_GAUSSIAN_QUADRATURE
#endif

#ifdef TEST_BOOST_MULTIPRECISION_GAUSSIAN_QUADRATURE
    #if BOOST_VERSION < 105300
        #error This boost version is too old to support boost multi precision
    #endif

    #include <boost/multiprecision/cpp_dec_float.hpp>
#endif

using namespace QuantLib;
using namespace boost::unit_test_framework;

namespace gaussian_quadratures_test {

    template <class T>
    void testSingle(const T& I, const std::string& tag,
                    const boost::function<Real(Real)>& f, Real expected) {
        Real calculated = I(f);
        if (std::fabs(calculated-expected) > 1.0e-4) {
            BOOST_ERROR("integrating" << tag << "\n"
                        << "    calculated: " << calculated << "\n"
                        << "    expected:   " << expected);
        }
    }

    // test functions

    Real inv_exp(Real x) {
        return std::exp(-x);
    }

    Real x_inv_exp(Real x) {
        return x*std::exp(-x);
    }

    Real x_normaldistribution(Real x) {
        return x*NormalDistribution()(x);
    }

    Real x_x_normaldistribution(Real x) {
        return x*x*NormalDistribution()(x);
    }

    Real inv_cosh(Real x) {
        return 1/std::cosh(x);
    }

    Real x_inv_cosh(Real x) {
        return x/std::cosh(x);
    }

    Real x_x_nonCentralChiSquared(Real x) {
        return x * x * boost::math::pdf(
            boost::math::non_central_chi_squared_distribution<Real>(4.0,1.0),x);
    }

    Real x_sin_exp_nonCentralChiSquared(Real x) {
        return x * std::sin(0.1*x) * std::exp(0.3*x) * boost::math::pdf(
            boost::math::non_central_chi_squared_distribution<Real>(1.0,1.0),x);
    }

    template <class T>
    void testSingleJacobi(const T& I) {
        testSingle(I, "f(x) = 1",
                   constant<Real,Real>(1.0), 2.0);
        testSingle(I, "f(x) = x",
                   identity<Real>(),         0.0);
        testSingle(I, "f(x) = x^2",
                   square<Real>(),           2/3.);
        testSingle(I, "f(x) = sin(x)",
                   static_cast<Real(*)(Real)>(std::sin), 0.0);
        testSingle(I, "f(x) = cos(x)",
                   static_cast<Real(*)(Real)>(std::cos),
                   std::sin(1.0)-std::sin(-1.0));
        testSingle(I, "f(x) = Gaussian(x)",
                   NormalDistribution(),
                   CumulativeNormalDistribution()(1.0)
                   -CumulativeNormalDistribution()(-1.0));
    }

    template <class T>
    void testSingleLaguerre(const T& I) {
        testSingle(I, "f(x) = exp(-x)",
                   inv_exp, 1.0);
        testSingle(I, "f(x) = x*exp(-x)",
                   x_inv_exp, 1.0);
        testSingle(I, "f(x) = Gaussian(x)",
                   NormalDistribution(), 0.5);
    }

    void testSingleTabulated(const boost::function<Real(Real)>& f,
                             const std::string& tag,
                             Real expected, Real tolerance) {
        const Size order[] = { 6, 7, 12, 20 };
        TabulatedGaussLegendre quad;
        for (Size i=0; i<LENGTH(order); i++) {
            quad.order(order[i]);
            Real realised = quad(f);
            if (std::fabs(realised-expected) > tolerance) {
                BOOST_ERROR(" integrating " << tag << "\n"
                            << "    order " << order[i] << "\n"
                            << "    realised: " << realised << "\n"
                            << "    expected: " << expected);
            }
        }
    }

}

void GaussianQuadraturesTest::testJacobi() {
    BOOST_TEST_MESSAGE("Testing Gauss-Jacobi integration...");

    using namespace gaussian_quadratures_test;

    testSingleJacobi(GaussLegendreIntegration(16));
    testSingleJacobi(GaussChebyshevIntegration(130));
    testSingleJacobi(GaussChebyshev2ndIntegration(130));
    testSingleJacobi(GaussGegenbauerIntegration(50,0.55));
}

void GaussianQuadraturesTest::testLaguerre() {
     BOOST_TEST_MESSAGE("Testing Gauss-Laguerre integration...");

     using namespace gaussian_quadratures_test;

     testSingleLaguerre(GaussLaguerreIntegration(16));
     testSingleLaguerre(GaussLaguerreIntegration(150,0.01));

     testSingle(GaussLaguerreIntegration(16, 1.0), "f(x) = x*exp(-x)",
                x_inv_exp, 1.0);
     testSingle(GaussLaguerreIntegration(32, 0.9), "f(x) = x*exp(-x)",
                x_inv_exp, 1.0);
}

void GaussianQuadraturesTest::testHermite() {
     BOOST_TEST_MESSAGE("Testing Gauss-Hermite integration...");

     using namespace gaussian_quadratures_test;

     testSingle(GaussHermiteIntegration(16), "f(x) = Gaussian(x)",
                NormalDistribution(), 1.0);
     testSingle(GaussHermiteIntegration(16,0.5), "f(x) = x*Gaussian(x)",
                x_normaldistribution, 0.0);
     testSingle(GaussHermiteIntegration(64,0.9), "f(x) = x*x*Gaussian(x)",
                x_x_normaldistribution, 1.0);
}

void GaussianQuadraturesTest::testHyperbolic() {
     BOOST_TEST_MESSAGE("Testing Gauss hyperbolic integration...");

     using namespace gaussian_quadratures_test;

     testSingle(GaussHyperbolicIntegration(16), "f(x) = 1/cosh(x)",
                inv_cosh, M_PI);
     testSingle(GaussHyperbolicIntegration(16), "f(x) = x/cosh(x)",
                x_inv_cosh, 0.0);
}

void GaussianQuadraturesTest::testTabulated() {
     BOOST_TEST_MESSAGE("Testing tabulated Gauss-Laguerre integration...");

     using namespace gaussian_quadratures_test;

     testSingleTabulated(constant<Real,Real>(1.0), "f(x) = 1",
                         2.0,       1.0e-13);
     testSingleTabulated(identity<Real>(), "f(x) = x",
                         0.0,       1.0e-13);
     testSingleTabulated(square<Real>(), "f(x) = x^2",
                         (2.0/3.0), 1.0e-13);
     testSingleTabulated(cube<Real>(), "f(x) = x^3",
                         0.0,       1.0e-13);
     testSingleTabulated(fourth_power<Real>(), "f(x) = x^4",
                         (2.0/5.0), 1.0e-13);
}

void GaussianQuadraturesTest::testNonCentralChiSquared() {
     BOOST_TEST_MESSAGE(
         "Testing Gauss non-central chi-squared integration...");

     using namespace gaussian_quadratures_test;

     testSingle(
        GaussianQuadrature(2, GaussNonCentralChiSquaredPolynomial(4.0, 1.0)),
        "f(x) = x^2 * nonCentralChiSquared(4, 1)(x)",
        x_x_nonCentralChiSquared, 37.0);

     testSingle(
        GaussianQuadrature(14, GaussNonCentralChiSquaredPolynomial(1.0, 1.0)),
        "f(x) = x * sin(0.1*x)*exp(0.3*x)*nonCentralChiSquared(1, 1)(x)",
        x_sin_exp_nonCentralChiSquared, 17.408092);
}

void GaussianQuadraturesTest::testNonCentralChiSquaredSumOfNodes() {
     BOOST_TEST_MESSAGE(
         "Testing Gauss non-central chi-squared sum of nodes...");

     using namespace gaussian_quadratures_test;

     // Walter Gautschi, How and How not to check Gaussian Quadrature Formulae
     // https://www.cs.purdue.edu/homes/wxg/selected_works/section_08/084.pdf

     // Expected results have been calculated with a multi precision library
     // following the description of test #4 in the paper above.
     // Using QuantLib's own determinant function will not work here
     // as it supports only double precision.

     const Real expected[] = {
         47.53491786730293,
         70.6103295419633383,
         98.0593406849441607,
         129.853401537905341,
         165.96963582663912,
         206.389183233992043
     };

     const Real nu=4.0;
     const Real lambda=1.0;
     const GaussNonCentralChiSquaredPolynomial orthPoly(nu, lambda);

     const Real tol = 1e-5;

	 for (Size n = 4; n < 10; ++n) {
		 const Array x = GaussianQuadrature(n, orthPoly).x();
         const Real calculated = std::accumulate(x.begin(), x.end(), 0.0);

         if (std::fabs(calculated - expected[n-4]) > tol) {
             BOOST_ERROR("failed to reproduce rule of sum"
                         << "\n    calculated: " << calculated
                         << "\n    expected:   " << expected[n-4]
                         << "\n    diff    :   " << calculated - expected[n-4]);
         }
     }
}

namespace gaussian_quadratures_test {
    template <class mp_float>
    class MomentBasedGaussLaguerrePolynomial
            : public MomentBasedGaussianPolynomial<mp_float> {
      public:
        mp_float moment(Size i) const {
            if (i == 0)
                return mp_float(1.0);
            else
                return mp_float(i)*moment(i-1);
        }

        Real w(Real x) const {
            return std::exp(-x);
        }
    };
}

void GaussianQuadraturesTest::testMomentBasedGaussianPolynomial() {
     BOOST_TEST_MESSAGE("Testing moment based Gaussian polynomials...");

     using namespace gaussian_quadratures_test;

     GaussLaguerrePolynomial g;

     std::vector<ext::shared_ptr<GaussianOrthogonalPolynomial> > ml;
     ml.push_back(
         ext::make_shared<MomentBasedGaussLaguerrePolynomial<Real> >());

#ifdef TEST_BOOST_MULTIPRECISION_GAUSSIAN_QUADRATURE
     ml.push_back(
         ext::make_shared<MomentBasedGaussLaguerrePolynomial<
             boost::multiprecision::number<
                 boost::multiprecision::cpp_dec_float<20> > > >());
#endif

     const Real tol = 1e-12;
     for (Size k=0; k < ml.size(); ++k) {

         for (Size i=0; i < 10; ++i) {
             const Real diffAlpha = std::fabs(ml[k]->alpha(i)-g.alpha(i));
             const Real diffBeta = std::fabs(ml[k]->beta(i)-g.beta(i));

             if (diffAlpha > tol) {
                 BOOST_ERROR("failed to reproduce alpha for Laguerre quadrature"
                             << "\n    calculated: " << ml[k]->alpha(i)
                             << "\n    expected  : " << g.alpha(i)
                             << "\n    diff      : " << diffAlpha);
             }
             if (i > 0 && diffBeta > tol) {
                 BOOST_ERROR("failed to reproduce beta for Laguerre quadrature"
                             << "\n    calculated: " << ml[k]->beta(i)
                             << "\n    expected  : " << g.beta(i)
                             << "\n    diff      : " << diffBeta);
             }
         }
     }
}

void GaussianQuadraturesTest::testGaussLaguerreCosinePolynomial() {
    BOOST_TEST_MESSAGE("Testing Gauss-Laguerre-Cosine quadrature...");

    using namespace gaussian_quadratures_test;

    const GaussianQuadrature quadCosine(
            16, GaussLaguerreCosinePolynomial<Real>(0.2));

    testSingle(quadCosine, "f(x) = exp(-x)",
               inv_exp, 1.0);
    testSingle(quadCosine, "f(x) = x*exp(-x)",
               x_inv_exp, 1.0);

    const GaussianQuadrature quadSine(
            16, GaussLaguerreSinePolynomial<Real>(0.2));

    testSingle(quadSine, "f(x) = exp(-x)",
               inv_exp, 1.0);
    testSingle(quadSine, "f(x) = x*exp(-x)",
               x_inv_exp, 1.0);
}

test_suite* GaussianQuadraturesTest::suite() {
    test_suite* suite = BOOST_TEST_SUITE("Gaussian quadratures tests");
    suite->add(QUANTLIB_TEST_CASE(&GaussianQuadraturesTest::testJacobi));
    suite->add(QUANTLIB_TEST_CASE(&GaussianQuadraturesTest::testLaguerre));
    suite->add(QUANTLIB_TEST_CASE(&GaussianQuadraturesTest::testHermite));
    suite->add(QUANTLIB_TEST_CASE(&GaussianQuadraturesTest::testHyperbolic));
    suite->add(QUANTLIB_TEST_CASE(&GaussianQuadraturesTest::testTabulated));
    suite->add(QUANTLIB_TEST_CASE(
        &GaussianQuadraturesTest::testMomentBasedGaussianPolynomial));
    suite->add(QUANTLIB_TEST_CASE(
        &GaussianQuadraturesTest::testGaussLaguerreCosinePolynomial));

    return suite;
}

test_suite* GaussianQuadraturesTest::experimental() {
    test_suite* suite = BOOST_TEST_SUITE(
        "Gaussian quadratures experimental tests");

    suite->add(QUANTLIB_TEST_CASE(
        &GaussianQuadraturesTest::testNonCentralChiSquared));
    suite->add(QUANTLIB_TEST_CASE(
        &GaussianQuadraturesTest::testNonCentralChiSquaredSumOfNodes));

    return suite;
}


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/erf.hpp>
#include <math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdErfc, Fvar) {
  using stan::math::erfc;
  using stan::math::fvar;
  using std::exp;
  using std::sqrt;

  fvar<double> x(0.5, 1.0);

  fvar<double> a = erfc(x);
  EXPECT_FLOAT_EQ(erfc(0.5), a.val_);
  EXPECT_FLOAT_EQ(
      -2 * exp(-0.5 * 0.5) / sqrt(boost::math::constants::pi<double>()), a.d_);

  fvar<double> b = erfc(-x);
  EXPECT_FLOAT_EQ(erfc(-0.5), b.val_);
  EXPECT_FLOAT_EQ(
      2 * exp(-0.5 * 0.5) / sqrt(boost::math::constants::pi<double>()), b.d_);
}

TEST(AgradFwdErfc, FvarFvarDouble) {
  using stan::math::erfc;
  using stan::math::fvar;
  using std::exp;
  using std::sqrt;

  fvar<fvar<double> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > a = erfc(x);

  EXPECT_FLOAT_EQ(erfc(0.5), a.val_.val_);
  EXPECT_FLOAT_EQ(
      -2 * exp(-0.5 * 0.5) / sqrt(boost::math::constants::pi<double>()),
      a.val_.d_);
  EXPECT_FLOAT_EQ(0, a.d_.val_);
  EXPECT_FLOAT_EQ(0, a.d_.d_);

  fvar<fvar<double> > y;
  y.val_.val_ = 0.5;
  y.d_.val_ = 1.0;

  a = erfc(y);
  EXPECT_FLOAT_EQ(erfc(0.5), a.val_.val_);
  EXPECT_FLOAT_EQ(0, a.val_.d_);
  EXPECT_FLOAT_EQ(
      -2 * exp(-0.5 * 0.5) / sqrt(boost::math::constants::pi<double>()),
      a.d_.val_);
  EXPECT_FLOAT_EQ(0, a.d_.d_);
}

struct erfc_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return erfc(arg1);
  }
};

TEST(AgradFwdErfc, erfc_NaN_0) {
  erfc_fun erfc_;
  test_nan_fwd(erfc_, false);
}


#include <opencv2/opencv.hpp>
#include "openbr_internal.h"
#include "openbr/core/qtutils.h"
#include "openbr/core/opencvutils.h"
#include "openbr/core/eigenutils.h"
#include <QString>
#include <Eigen/SVD>
#include <Eigen/Dense>

using namespace std;
using namespace cv;
using namespace Eigen;

namespace br
{

/*!
 * \ingroup transforms
 * \brief Procrustes alignment of points
 * \author Scott Klum \cite sklum
 */
class ProcrustesTransform : public Transform
{
    Q_OBJECT

    Q_PROPERTY(bool warp READ get_warp WRITE set_warp RESET reset_warp STORED false)
    BR_PROPERTY(bool, warp, true)

    Eigen::MatrixXf meanShape;

    void train(const TemplateList &data)
    {
        QList< QList<QPointF> > normalizedPoints;

        // Normalize all sets of points
        foreach (br::Template datum, data) {
            QList<QPointF> points = datum.file.points();
            QList<QRectF> rects = datum.file.rects();

            if (points.empty() || rects.empty()) continue;

            // Assume rect appended last was bounding box
            points.append(rects.last().topLeft());
            points.append(rects.last().topRight());
            points.append(rects.last().bottomLeft());
            points.append(rects.last().bottomRight());

            // Center shape at origin
            Scalar mean = cv::mean(OpenCVUtils::toPoints(points).toVector().toStdVector());
            for (int i = 0; i < points.size(); i++) points[i] -= QPointF(mean[0],mean[1]);

            // Remove scale component
            float norm = cv::norm(OpenCVUtils::toPoints(points).toVector().toStdVector());
            for (int i = 0; i < points.size(); i++) points[i] /= norm;

            normalizedPoints.append(points);
        }

        if (normalizedPoints.empty()) qFatal("Unable to calculate normalized points");

        // Determine mean shape, assuming all shapes contain the same number of points
        meanShape = Eigen::MatrixXf(normalizedPoints[0].size(), 2);

        for (int i = 0; i < normalizedPoints[0].size(); i++) {
            double x = 0;
            double y = 0;

            for (int j = 0; j < normalizedPoints.size(); j++) {
                x += normalizedPoints[j][i].x();
                y += normalizedPoints[j][i].y();
            }

            x /= (double)normalizedPoints.size();
            y /= (double)normalizedPoints.size();

            meanShape(i,0) = x;
            meanShape(i,1) = y;
        }
    }

    void project(const Template &src, Template &dst) const
    {
        QList<QPointF> points = src.file.points();
        QList<QRectF> rects = src.file.rects();

        if (points.empty() || rects.empty()) {
            dst = src;
            if (Globals->verbose) qWarning("Procrustes alignment failed because points or rects are empty.");
            return;
        }

        // Assume rect appended last was bounding box
        points.append(rects.last().topLeft());
        points.append(rects.last().topRight());
        points.append(rects.last().bottomLeft());
        points.append(rects.last().bottomRight());

        Scalar mean = cv::mean(OpenCVUtils::toPoints(points).toVector().toStdVector());
        for (int i = 0; i < points.size(); i++) points[i] -= QPointF(mean[0],mean[1]);

        Eigen::MatrixXf srcMat(points.size(), 2);
        float norm = cv::norm(OpenCVUtils::toPoints(points).toVector().toStdVector());
        for (int i = 0; i < points.size(); i++) {
            points[i] /= norm;
            srcMat(i,0) = points[i].x();
            srcMat(i,1) = points[i].y();
        }

        Eigen::JacobiSVD<Eigen::MatrixXf> svd(srcMat.transpose()*meanShape, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::MatrixXf R = svd.matrixU()*svd.matrixV().transpose();

        dst = src;

        // Store procrustes stats in the order:
        // R(0,0), R(1,0), R(1,1), R(0,1), mean_x, mean_y, norm
        QList<float> procrustesStats;
        procrustesStats << R(0,0) << R(1,0) << R(1,1) << R(0,1) << mean[0] << mean[1] << norm;
        dst.file.setList<float>("ProcrustesStats",procrustesStats);

        if (warp) {
            Eigen::MatrixXf dstMat = srcMat*R;
            for (int i = 0; i < dstMat.rows(); i++) {
                dst.file.appendPoint(QPointF(dstMat(i,0),dstMat(i,1)));
            }
        }
    }

    void store(QDataStream &stream) const
    {
        stream << meanShape;
    }

    void load(QDataStream &stream)
    {
        stream >> meanShape;
    }

};

BR_REGISTER(Transform, ProcrustesTransform)

/*!
 * \ingroup transforms
 * \brief Creates a Delaunay triangulation based on a set of points
 * \author Scott Klum \cite sklum
 */
class DelaunayTransform : public Transform
{
    Q_OBJECT

    Q_PROPERTY(float scaleFactor READ get_scaleFactor WRITE set_scaleFactor RESET reset_scaleFactor STORED false)
    Q_PROPERTY(bool warp READ get_warp WRITE set_warp RESET reset_warp STORED false)
    BR_PROPERTY(float, scaleFactor, 1)
    BR_PROPERTY(bool, warp, true)

    void project(const Template &src, Template &dst) const
    {
        QList<QPointF> points = src.file.points();
        QList<QRectF> rects = src.file.rects();

        if (points.empty() || rects.empty()) {
            dst = src;
            if (Globals->verbose) qWarning("Delauney triangulation failed because points or rects are empty.");
            return;
        }

        int cols = src.m().cols;
        int rows = src.m().rows;

        // Assume rect appended last was bounding box
        points.append(rects.last().topLeft());
        points.append(rects.last().topRight());
        points.append(rects.last().bottomLeft());
        points.append(rects.last().bottomRight());

        Subdiv2D subdiv(Rect(0,0,cols,rows));
        // Make sure points are valid for Subdiv2D
        // TODO: Modify points to make them valid
        for (int i = 0; i < points.size(); i++) {
            if (points[i].x() < 0 || points[i].y() < 0 || points[i].y() >= rows || points[i].x() >= cols) {
                dst = src;
                if (Globals->verbose) qWarning("Delauney triangulation failed because points lie on boundary.");
                return;
            }
            subdiv.insert(OpenCVUtils::toPoint(points[i]));
        }

        vector<Vec6f> triangleList;
        subdiv.getTriangleList(triangleList);

        QList<QPointF> validTriangles;

        for (size_t i = 0; i < triangleList.size(); i++) {
            // Check the triangle to make sure it's falls within the matrix
            bool valid = true;

            QList<QPointF> vertices;
            vertices.append(QPointF(triangleList[i][0],triangleList[i][1]));
            vertices.append(QPointF(triangleList[i][2],triangleList[i][3]));
            vertices.append(QPointF(triangleList[i][4],triangleList[i][5]));
            for (int j = 0; j < 3; j++) if (vertices[j].x() > cols || vertices[j].y() > rows || vertices[j].x() < 0 || vertices[j].y() < 0) valid = false;

            if (valid) validTriangles.append(vertices);
        }

        if (warp) {
            dst.m() = Mat::zeros(rows,cols,src.m().type());

            QList<float> procrustesStats = src.file.getList<float>("ProcrustesStats");

            Eigen::MatrixXf R(2,2);
            R(0,0) = procrustesStats.at(0);
            R(1,0) = procrustesStats.at(1);
            R(1,1) = procrustesStats.at(2);
            R(0,1) = procrustesStats.at(3);

            cv::Scalar mean(2);
            mean[0] = procrustesStats.at(4);
            mean[1] = procrustesStats.at(5);

            float norm = procrustesStats.at(6);

            QList<Point2f> mappedPoints;

            for (int i = 0; i < validTriangles.size(); i+=3) {
                // Matrix to store original (pre-transformed) triangle vertices
                Eigen::MatrixXf srcMat(3, 2);

                for (int j = 0; j < 3; j++) {
                    srcMat(j,0) = (validTriangles[i+j].x()-mean[0])/norm;
                    srcMat(j,1) = (validTriangles[i+j].y()-mean[1])/norm;
                }

                Eigen::MatrixXf dstMat = srcMat*R;

                Point2f srcPoints[3];
                for (int j = 0; j < 3; j++) srcPoints[j] = OpenCVUtils::toPoint(validTriangles[i+j]);

                Point2f dstPoints[3];
                for (int j = 0; j < 3; j++) {
                    // Scale and shift destination points
                    Point2f warpedPoint = Point2f(dstMat(j,0)*scaleFactor+cols/2,dstMat(j,1)*scaleFactor+rows/2);
                    dstPoints[j] = warpedPoint;
                    mappedPoints.append(warpedPoint);
                }

                Mat buffer(rows,cols,src.m().type());

                warpAffine(src.m(), buffer, getAffineTransform(srcPoints, dstPoints), Size(cols,rows));

                Mat mask = Mat::zeros(rows, cols, CV_8UC1);
                Point maskPoints[1][3];
                maskPoints[0][0] = dstPoints[0];
                maskPoints[0][1] = dstPoints[1];
                maskPoints[0][2] = dstPoints[2];
                const Point* ppt = { maskPoints[0] };

                fillConvexPoly(mask, ppt, 3, Scalar(255,255,255), 8);

                Mat output(rows,cols,src.m().type());

                if (i > 0) {
                    Mat overlap;
                    bitwise_and(dst.m(),mask,overlap);
                    mask.setTo(0, overlap!=0);
                }

                bitwise_and(buffer,mask,output);

                dst.m() += output;
            }

            // Overwrite any rects
            Rect boundingBox = boundingRect(mappedPoints.toVector().toStdVector());
            dst.file.setRects(QList<QRectF>() << OpenCVUtils::fromRect(boundingBox));
        } else dst = src;

        dst.file.setList<QPointF>("DelaunayTriangles", validTriangles);
    }
};

BR_REGISTER(Transform, DelaunayTransform)

/*!
 * \ingroup transforms
 * \brief Creates a Delaunay triangulation based on a set of points
 * \author Scott Klum \cite sklum
 */
class DrawDelaunayTransform : public UntrainableTransform
{
    Q_OBJECT

    void project(const Template &src, Template &dst) const
    {
        dst = src;

        if (src.file.contains("DelaunayTriangles")) {
            QList<Point2f> validTriangles = OpenCVUtils::toPoints(src.file.getList<QPointF>("DelaunayTriangles"));

            // Clone the matrix do draw on it
            for (int i = 0; i < validTriangles.size(); i+=3) {
                line(dst, validTriangles[i], validTriangles[i+1], Scalar(0,0,0), 1);
                line(dst, validTriangles[i+1], validTriangles[i+2], Scalar(0,0,0), 1);
                line(dst, validTriangles[i+2], validTriangles[i], Scalar(0,0,0), 1);
            }
        } else qWarning("Template does not contain Delaunay triangulation.");
    }
};

BR_REGISTER(Transform, DrawDelaunayTransform)

/*!
 * \ingroup transforms
 * \brief Read landmarks from a file and associate them with the correct templates.
 * \author Scott Klum \cite sklum
 *
 * Example of the format:
 * \code
 * image_001.jpg:146.000000,190.000000,227.000000,186.000000,202.000000,256.000000
 * image_002.jpg:75.000000,235.000000,140.000000,225.000000,91.000000,300.000000
 * image_003.jpg:158.000000,186.000000,246.000000,188.000000,208.000000,233.000000
 * \endcode
 */
class ReadLandmarksTransform : public UntrainableTransform
{
    Q_OBJECT

    Q_PROPERTY(QString file READ get_file WRITE set_file RESET reset_file STORED false)
    Q_PROPERTY(QString imageDelimiter READ get_imageDelimiter WRITE set_imageDelimiter RESET reset_imageDelimiter STORED false)
    Q_PROPERTY(QString landmarkDelimiter READ get_landmarkDelimiter WRITE set_landmarkDelimiter RESET reset_landmarkDelimiter STORED false)
    BR_PROPERTY(QString, file, QString())
    BR_PROPERTY(QString, imageDelimiter, ":")
    BR_PROPERTY(QString, landmarkDelimiter, ",")

    QHash<QString, QList<QPointF> > landmarks;

    void init()
    {
        if (file.isEmpty())
            return;

        QFile f(file);
        if (!f.open(QFile::ReadOnly | QFile::Text))
            qFatal("Failed to open %s for reading.", qPrintable(f.fileName()));

        while (!f.atEnd()) {
            const QStringList words = QString(f.readLine()).split(imageDelimiter);
            const QStringList lm = words[1].split(landmarkDelimiter);

            QList<QPointF> points;
            bool ok;
            for (int i=0; i<lm.size(); i+=2)
                points.append(QPointF(lm[i].toFloat(&ok),lm[i+1].toFloat(&ok)));
            if (!ok) qFatal("Failed to read landmark.");

            landmarks.insert(words[0],points);
        }
    }

    void project(const Template &src, Template &dst) const
    {
        dst = src;

        dst.file.appendPoints(landmarks[dst.file.fileName()]);
    }
};

BR_REGISTER(Transform, ReadLandmarksTransform)

/*!
 * \ingroup transforms
 * \brief Name a point/rect
 * \author Scott Klum \cite sklum
 */
class NameLandmarksTransform : public UntrainableMetadataTransform
{
    Q_OBJECT
    Q_PROPERTY(bool point READ get_point WRITE set_point RESET reset_point STORED false)
    BR_PROPERTY(bool, point, true)
    Q_PROPERTY(QList<int> indices READ get_indices WRITE set_indices RESET reset_indices STORED false)
    Q_PROPERTY(QStringList names READ get_names WRITE set_names RESET reset_names STORED false)
    BR_PROPERTY(QList<int>, indices, QList<int>())
    BR_PROPERTY(QStringList, names, QStringList())

    void projectMetadata(const File &src, File &dst) const
    {
        if (indices.size() != names.size()) qFatal("Index/name size mismatch");

        dst = src;

        if (point) {
            QList<QPointF> points = src.points();

            for (int i=0; i<indices.size(); i++) {
                if (indices[i] < points.size()) dst.set(names[i], points[indices[i]]);
                else qFatal("Index out of range.");
            }
        } else {
            QList<QRectF> rects = src.rects();

            for (int i=0; i<indices.size(); i++) {
                if (indices[i] < rects.size()) dst.set(names[i], rects[indices[i]]);
                else qFatal("Index out of range.");
            }
        }
    }
};

BR_REGISTER(Transform, NameLandmarksTransform)

/*!
 * \ingroup transforms
 * \brief Remove a name from a point/rect
 * \author Scott Klum \cite sklum
 */
class AnonymizeLandmarksTransform : public UntrainableMetadataTransform
{
    Q_OBJECT
    Q_PROPERTY(QStringList names READ get_names WRITE set_names RESET reset_names STORED false)
    BR_PROPERTY(QStringList, names, QStringList())

    void projectMetadata(const File &src, File &dst) const
    {
        dst = src;

        foreach (const QString &name, names) {
            if (src.contains(name)) {
                QVariant variant = src.value(name);
                if (variant.canConvert(QMetaType::QPointF)) {
                    dst.appendPoint(variant.toPointF());
                } else if (variant.canConvert(QMetaType::QRectF)) {
                    dst.appendRect(variant.toRectF());
                } else {
                    qFatal("Cannot convert landmark to point or rect.");
                }
            }
        }
    }
};

BR_REGISTER(Transform, AnonymizeLandmarksTransform)

/*!
 * \ingroup transforms
 * \brief Converts either the file::points() list or a QList<QPointF> metadata item to be the template's matrix
 * \author Scott Klum \cite sklum
 */
class PointsToMatrixTransform : public UntrainableTransform
{
    Q_OBJECT

    Q_PROPERTY(QString inputVariable READ get_inputVariable WRITE set_inputVariable RESET reset_inputVariable STORED false)
    BR_PROPERTY(QString, inputVariable, QString())

    void project(const Template &src, Template &dst) const
    {
        dst = src;

        if (inputVariable.isEmpty()) {
            dst.m() = OpenCVUtils::pointsToMatrix(dst.file.points());
        } else {
            if (src.file.contains(inputVariable))
                dst.m() = OpenCVUtils::pointsToMatrix(dst.file.get<QList<QPointF> >(inputVariable));
        }
    }
};

BR_REGISTER(Transform, PointsToMatrixTransform)

/*!
 * \ingroup transforms
 * \brief Normalize points to be relative to a single point
 * \author Scott Klum \cite sklum
 */
class NormalizePointsTransform : public UntrainableTransform
{
    Q_OBJECT

    Q_PROPERTY(int index READ get_index WRITE set_index RESET reset_index STORED false)
    BR_PROPERTY(int, index, 0)

    void project(const Template &src, Template &dst) const
    {
        dst = src;

        QList<QPointF> points = dst.file.points();
        QPointF normPoint = points.at(index);

        QList<QPointF> normalizedPoints;

        for (int i=0; i<points.size(); i++)
            if (i!=index)
                normalizedPoints.append(normPoint-points[i]);

        dst.file.setPoints(normalizedPoints);
    }
};

BR_REGISTER(Transform, NormalizePointsTransform)

/*!
 * \ingroup transforms
 * \brief Normalize points to be relative to a single point
 * \author Scott Klum \cite sklum
 */
class PointDisplacementTransform : public UntrainableTransform
{
    Q_OBJECT

    void project(const Template &src, Template &dst) const
    {
        dst = src;

        QList<QPointF> points = dst.file.points();
        QList<QPointF> normalizedPoints;

        for (int i=0; i<points.size(); i++)
            for (int j=0; j<points.size(); j++)
                // There is redundant information here
                if (j!=i) {
                    QPointF normalizedPoint = points[i]-points[j];
                    normalizedPoint.setX(pow(normalizedPoint.x(),2));
                    normalizedPoint.setY(pow(normalizedPoint.y(),2));
                    normalizedPoints.append(normalizedPoint);
                }

        dst.file.setPoints(normalizedPoints);
    }
};

BR_REGISTER(Transform, PointDisplacementTransform)

} // namespace br

#include "landmarks.moc"


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

#pragma once

#include <stdexcept>
#include <armadillo>

namespace armalogp {

  static inline double square(double x) {
    return x*x;
  }

  static inline int square(int x) {
    return x*x;
  }

  double cholesky_determinant(const arma::mat& R) {
    return arma::prod(square(R.diag()));
  }

  double mahalanobis(const arma::vec& x, const arma::vec& mu, const arma::mat& sigma) {
    const arma::vec err = x - mu;
    return arma::as_scalar(err.t() * sigma.i() * err);
  }

  double mahalanobis(const arma::rowvec& x, const arma::rowvec& mu, const arma::mat& sigma) {
    const arma::rowvec err = x - mu;
    return arma::as_scalar(err * sigma.i() * err.t());
  }

  double mahalanobis_chol(const arma::rowvec& x, const arma::rowvec& mu, const arma::mat& R) {
    const arma::rowvec err = x - mu;
    const arma::mat Rinv(inv(trimatl(R)));
    return arma::as_scalar(err * Rinv * Rinv.t() * err.t());
  }

} // namespace armalogp


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2020 Lew Wei Hao

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/exercise.hpp>
#include <boost/function.hpp>
#include <ql/math/integrals/simpsonintegral.hpp>
#include "analyticeuropeanvasicekengine.hpp"
#include <ql/math/distributions/normaldistribution.hpp>

namespace QuantLib {

    namespace {

        Real g_k(Real t, Real kappa){
            return (1 - std::exp(- kappa * t )) / kappa;
        }

        class integrand_vasicek {
          private:
            const Real sigma_s_;
            const Real sigma_r_;
            const Real correlation_;
            const Real kappa_;
            const Real T_;
          public:
            integrand_vasicek(Real sigma_s, Real sigma_r, Real correlation, Real kappa, Real T)
            : sigma_s_(sigma_s), sigma_r_(sigma_r), correlation_(correlation), kappa_(kappa), T_(T){}
            Real operator()(Real u) const {
                Real g = g_k(T_ - u, kappa_);
                return (sigma_s_ * sigma_s_) + (2 * correlation_ * sigma_s_ * sigma_r_ * g) + (sigma_r_ * sigma_r_ * g * g);
            }
        };

    }

    AnalyticBlackVasicekEngine::AnalyticBlackVasicekEngine(
            const ext::shared_ptr<GeneralizedBlackScholesProcess>& blackProcess,
            const ext::shared_ptr<Vasicek>& vasicekProcess,
            Real correlation)
    : blackProcess_(blackProcess), vasicekProcess_(vasicekProcess), simpsonIntegral_(new SimpsonIntegral(1e-5, 1000)), correlation_(correlation) {
        registerWith(blackProcess_);
        registerWith(vasicekProcess_);
    }

    void AnalyticBlackVasicekEngine::calculate() const {
        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        ext::shared_ptr<StrikedTypePayoff> payoff =
                ext::dynamic_pointer_cast<StrikedTypePayoff>(arguments_.payoff);

        QL_REQUIRE(payoff, "non-striked payoff given");

        CumulativeNormalDistribution f;

        Real t = 0;
        Real T = blackProcess_->riskFreeRate()->dayCounter().yearFraction(blackProcess_->riskFreeRate().currentLink()->referenceDate(),arguments_.exercise->lastDate());
        Real kappa = vasicekProcess_->a();
        Real S_t = blackProcess_->x0();
        Real K = payoff->strike();
        Real sigma_s = blackProcess_->blackVolatility()->blackVol(t, K);
        Real sigma_r = vasicekProcess_->sigma();
        Real r_t = vasicekProcess_->r0();

        Real zcb = vasicekProcess_->discountBond(t, T, r_t);
        Real epsilon = payoff->optionType() == Option::Call ? 1 : -1;
        Real upsilon = (*simpsonIntegral_)(integrand_vasicek(sigma_s, sigma_r, correlation_, kappa, T), t, T);
        Real d_positive = (std::log((S_t / K) / zcb) + upsilon / 2) / std::sqrt(upsilon);
        Real d_negative = (std::log((S_t / K) / zcb) - upsilon / 2) / std::sqrt(upsilon);
        Real n_d1 = f(epsilon * d_positive);
        Real n_d2 = f(epsilon * d_negative);

        results_.value = epsilon * ((S_t * n_d1) - (zcb * K * n_d2));
    }

}


#include "point_to_plane.h"
#include <Eigen/Dense>

using namespace probreg;

Pt2PlResult probreg::computeTwistForPointToPlane(const MatrixX3& model,
                                                 const MatrixX3& target,
                                                 const MatrixX3& target_normal,
                                                 const Vector& weight) {
    Matrix6 ata = Matrix6::Zero();
    Vector6 atb = Vector6::Zero();
    Float r_sum = 0.0;

    #pragma omp declare reduction(+ : Matrix6 : omp_out=omp_out+omp_in) initializer(omp_priv = omp_orig)
    #pragma omp declare reduction(+ : Vector6 : omp_out=omp_out+omp_in) initializer(omp_priv = omp_orig)
    #pragma omp parallel for reduction(+:ata) reduction(+:atb) reduction(+:r_sum)
    for (auto k = 0; k < model.rows(); ++k){
        const auto& vertex_k = model.row(k).transpose();
        const auto& target_k = target.row(k).transpose();
        const auto& normal_k = target_normal.row(k).transpose();
        const auto& weight_k = weight[k];
        const Float residual = normal_k.dot(target_k - vertex_k);
        const Vector6 jac = (Vector6() << vertex_k.cross(normal_k), normal_k).finished();
        const Matrix6 wjjt = weight_k * jac * jac.transpose();
        const Vector6 wrj = weight_k * residual * jac;
        const Float wr = weight_k * weight_k * residual * residual;
        ata.noalias() += wjjt;
        atb.noalias() += wrj;
        r_sum += wr;
    }
    return std::make_pair(ata.selfadjointView<Eigen::Upper>().ldlt().solve(atb), r_sum);
}


/**
 * @file parametricfiniteelements_main.cc
 * @brief NPDE homework ParametricFiniteElements code
 * @author Amélie Loher
 * @date 04.04.2020
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <cmath>
#include <complex>
#include <iostream>
#include <vector>

#include "parametricfiniteelements.h"

using namespace ParametricFiniteElements;

int main() {
  unsigned int n = 3;
  Eigen::VectorXd mu((n + 1) * (n + 1));

  // Psi \in C^1([0,1]), Psi > 0
  auto Psi = [](double x) -> double { return x * x + 1.0; };

  // 1 <= alpha(x) <= 2
  auto alpha = [](Eigen::Vector2d x) -> double { return 3.0 / 2.0; };

  mu = geoThermSolve(n, alpha, Psi);

  // Surface Integral over Gamma_S of expansion coefficient vector mu
  double val = geoThermSurfInt(n, Psi, mu);

  std::cout
      << "Basis Expansion Coefficient vector of solution Variational Problem: "
      << mu << std::endl;
  std::cout << "Value of expansion coefficient vector mu integrated over "
               "Surface Gamma_S : "
            << val << std::endl;

  return 0;
}