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

  @copyright 2016 NumScale SAS

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

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_invalid.hpp>
#endif
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/log10.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/tenpower.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( significants_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0>>
                          , bd::scalar_< bd::integer_<A1>>
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( assert_all(is_gtz(a1))
                      , "Number of significant digits must be positive"
                      );
      using i_t = bd::as_integer_t<A0>;
      if (is_eqz(a0)) return a0;
      i_t exp = a1 - iceil(log10(abs(a0)));
      A0 fac = tenpower(exp);
      A0 scaled = bs::nearbyint(a0*fac);
    #ifndef BOOST_SIMD_NO_INVALIDS
      return is_invalid(a0) ? a0 : scaled/fac;
    #else
      return scaled/fac;
    #endif
    }
  };
} } }

#endif


#ifndef WAVE_FIXED_PLANE_HPP
#define WAVE_FIXED_PLANE_HPP

#include <Eigen/Core>
#include <ceres/ceres.h>
#include "wave/odometry/geometry/assign_jacobians.hpp"

namespace wave {

template <typename Scalar, int... states>
class FixedPlaneResidual : public ceres::SizedCostFunction<1, 12, states...> {
 public:
    virtual ~FixedPlaneResidual() {}

    FixedPlaneResidual(const Scalar *pt,
                  VecE<const MatX*> &jacsw1,
                  VecE<const MatX*> &jacsw2,
                  const Scalar &w1,
                  const Scalar &w2,
                  const Vec6 &plane)
            : pt(pt),
              jacsw1(std::move(jacsw1)),
              jacsw2(std::move(jacsw2)),
              w1(w1),
              w2(w2),
              plane(plane) { }

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

    template<typename Derived>
    MatX getDerivativeWpT(const Eigen::MatrixBase<Derived> &normal) const;

    template<typename Derived>
    void setWeight(const Eigen::MatrixBase<Derived> &Weight) {
        this->weight = Weight;
    }

 private:
    // updated by evaluation callback
    const Scalar *pt;
    const VecE<const MatX*> jacsw1, jacsw2;
    const Scalar w1, w2;
    const Vec6 &plane;

    Eigen::Matrix<double, 1, 1> weight = Eigen::Matrix<double, 1, 1>::Identity();
};
}

#include "wave/odometry/geometry/impl/fixed_plane_impl.hpp"

#endif //WAVE_FIXED_PLANE_HPP


//
// Created by saman on 10/29/20.
//

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE my_unit_tests
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include "../includes/Vec3.h"
#include "../includes/ArrayT.h"

BOOST_AUTO_TEST_SUITE(my_testsuite)

    BOOST_AUTO_TEST_CASE(dot_product)
    {
        Vec3 v1(1.0, 2.0, 0.0);
        Vec3 v2(-1.0, -3.0, 1.0);
        BOOST_TEST ( v1.Dot(v2) == -7.0);
    }

    BOOST_AUTO_TEST_CASE(assign_to_scalar)
    {
        Vec3 v1(3, 5, 0);
        double val = 3;
        v1 = val;
        BOOST_TEST (v1.x == 3);
        BOOST_TEST (v1.y == 3);
    }
    BOOST_AUTO_TEST_CASE(multiplying_by_scalar)
    {
        Vec3 v1(5, 4, 0);
        double val = 3;
        v1 *= val;
        BOOST_TEST (v1.x == 15);
        BOOST_TEST (v1.y == 12);
    }
    BOOST_AUTO_TEST_CASE(multiplying_by_scalar_on_the_line)
    {
        Vec3 v1(2.0, 3.0, 0.0);
        double val = 2.0;
        Vec3 v2 = v1 * val;
        BOOST_TEST (v2.x == 4.0);
        BOOST_TEST (v2.y == 6.0);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <boost/numeric/ublas/io.hpp>

#include "geometry/homogeneous_transformation.h"

namespace gca {

  homogeneous_transform
  apply(const point d, const homogeneous_transform& t) {
    return std::make_pair(t.first, t.second + to_vector(d));
  }

  point apply(const homogeneous_transform& t, const point p) {
    return times_3(t.first, p) + from_vector(t.second);
  }

  triangular_mesh apply(const homogeneous_transform& t, const triangular_mesh& m) {
    triangular_mesh rotated =
      m.apply([t](const point p)
	      { return times_3(t.first, p); });
    triangular_mesh shifted =
      rotated.apply_to_vertices([t](const point p)
				{ return p + from_vector(t.second); });
    return shifted;
  }

  boost::optional<homogeneous_transform>
  mate_planes(const plane a, const plane b, const plane c,
	      const plane ap, const plane bp, const plane cp) {

    const ublas::matrix<double> rotation =
      plane_basis_rotation(a.normal(), b.normal(), c.normal(),
			   -1*ap.normal(), -1*bp.normal(), -1*cp.normal());

    if (!within_eps(determinant(rotation), 1.0, 0.001)) {
      return boost::none;
    }

    const ublas::vector<double> displacement =
      plane_basis_displacement(rotation,
			       ap.normal(), bp.normal(), cp.normal(),
			       ap.pt(), bp.pt(), cp.pt(),
			       a.pt(), b.pt(), c.pt());

    std::pair<const ublas::matrix<double>,
	      const ublas::vector<double> > p =
      std::make_pair(rotation, displacement);
    return p;
  }

  std::vector<point> apply(const homogeneous_transform& t,
			   const std::vector<point>& pts) {
    vector<point> rpts;
    for (auto p : pts) {
      rpts.push_back(apply(t, p));
    }
    return rpts;
  }

  labeled_polygon_3 apply(const homogeneous_transform& t,
			  const labeled_polygon_3& p) {
    vector<point> rotated_verts = apply(t, p.vertices());

    vector<vector<point>> holes;
    for (auto h : p.holes()) {
      holes.push_back(apply(t, h));
    }

    polygon_3 transformed = build_clean_polygon_3(rotated_verts, holes);

    transformed.correct_winding_order(times_3(t.first, p.normal()));

    point rnorm = transformed.normal();
    point pnorm = p.normal();
    point rtnorm = times_3(t.first, p.normal());

    // cout << "Original normal             = " << pnorm << endl;
    // cout << "Transformed normal              = " << rnorm << endl;
    // cout << "Rotation of original normal = " << rtnorm << endl;

    double theta = angle_between(transformed.normal(), rtnorm);

    DBG_ASSERT(within_eps(theta, 0.0, 0.1));

    return transformed;
  }

}


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009

//  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 <pch.hpp>

#ifdef _MSC_VER
# pragma warning (disable : 4996) // POSIX name for this item is deprecated
# pragma warning (disable : 4224) // nonstandard extension used : formal parameter 'arg' was previously defined as a type
# pragma warning (disable : 4180) // qualifier applied to function type has no meaning; ignored
#endif

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "test_beta_hooks.hpp"
#include "handle_test_result.hpp"

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the function beta.  There are two sets of tests, spot
// tests which compare our results with selected values computed
// using the online special function calculator at
// functions.wolfram.com, while the bulk of the accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these functions.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
#if LDBL_MANT_DIG == 106
   // Darwin:
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      "Mac OS.*",                    // platform
      "(long\\s+)?double",           // test type(s)
      "Beta Function: Medium.*",     // test data group
      "boost::math::beta", 200, 35); // test function
#endif

   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "(long\\s+)?double",           // test type(s)
      "Beta Function: Small.*",      // test data group
      "boost::math::beta", 8, 5);    // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "(long\\s+)?double",           // test type(s)
      "Beta Function: Medium.*",     // test data group
      "boost::math::beta", 160, 35); // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "(long\\s+)?double",           // test type(s)
      "Beta Function: Divergent.*",  // test data group
      "boost::math::beta", 30, 6);   // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "Beta Function: Small.*",      // test data group
      "boost::math::beta", 15, 15);   // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "Beta Function: Medium.*",     // test data group
      "boost::math::beta", 150, 40); // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "Beta Function: Divergent.*",  // test data group
      "boost::math::beta", 25, 8);   // test function

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class T>
void do_test_beta(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(value_type, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::beta<value_type, value_type>;
#else
   pg funcp = boost::math::beta;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test beta against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1),
      extract_result(2));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::beta", test_name);
#ifdef TEST_OTHER
   if(::boost::is_floating_point<value_type>::value){
      funcp = other::beta;
      result = boost::math::tools::test(
         data,
         bind_func(funcp, 0, 1),
         extract_result(2));
      print_test_result(result, data[result.worst()], result.worst(), type_name, "other::beta");
   }
#endif
   std::cout << std::endl;
}
template <class T>
void test_beta(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // three items, input value a, input value b and beta(a, b):
   //
#  include "beta_small_data.ipp"

   do_test_beta(beta_small_data, name, "Beta Function: Small Values");

#  include "beta_med_data.ipp"

   do_test_beta(beta_med_data, name, "Beta Function: Medium Values");

#  include "beta_exp_data.ipp"

   do_test_beta(beta_exp_data, name, "Beta Function: Divergent Values");
}

#undef small // VC++ #defines small char !!!!!!
template <class T>
void test_spots(T)
{
   //
   // Basic sanity checks, tolerance is 20 epsilon expressed as a percentage:
   //
   T tolerance = boost::math::tools::epsilon<T>() * 20 * 100;
   T small = boost::math::tools::epsilon<T>() / 1024;
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(1), static_cast<T>(1)), static_cast<T>(1), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(1), static_cast<T>(4)), static_cast<T>(0.25), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(4), static_cast<T>(1)), static_cast<T>(0.25), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(small, static_cast<T>(4)), 1/small, tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(4), small), 1/small, tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(4), static_cast<T>(20)), static_cast<T>(0.00002823263692828910220214568040654997176736L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::beta(static_cast<T>(0.0125L), static_cast<T>(0.000023L)), static_cast<T>(43558.24045647538375006349016083320744662L), tolerance);
}

int test_main(int, char* [])
{
   expected_results();
   BOOST_MATH_CONTROL_FP;
   test_spots(0.0F);
   test_spots(0.0);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L);
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   test_spots(boost::math::concepts::real_concept(0.1));
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif

   test_beta(0.1F, "float");
   test_beta(0.1, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_beta(0.1L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   test_beta(boost::math::concepts::real_concept(0.1), "real_concept");
#endif
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif
   return 0;
}


//[ Calc1
//  Copyright 2008 Eric Niebler. Distributed under 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)
//
// This is a simple example of how to build an arithmetic expression
// evaluator with placeholders.

#include <iostream>
#include <boost/proto/core.hpp>
#include <boost/proto/context.hpp>
namespace proto = boost::proto;
using proto::_;

template<int I> struct placeholder {};

// Define some placeholders
proto::terminal< placeholder< 1 > >::type const _1 = {{}};
proto::terminal< placeholder< 2 > >::type const _2 = {{}};

// Define a calculator context, for evaluating arithmetic expressions
struct calculator_context
  : proto::callable_context< calculator_context const >
{
    // The values bound to the placeholders
    double d[2];

    // The result of evaluating arithmetic expressions
    typedef double result_type;

    explicit calculator_context(double d1 = 0., double d2 = 0.)
    {
        d[0] = d1;
        d[1] = d2;
    }

    // Handle the evaluation of the placeholder terminals
    template<int I>
    double operator ()(proto::tag::terminal, placeholder<I>) const
    {
        return d[ I - 1 ];
    }
};

template<typename Expr>
double evaluate( Expr const &expr, double d1 = 0., double d2 = 0. )
{
    // Create a calculator context with d1 and d2 substituted for _1 and _2
    calculator_context const ctx(d1, d2);

    // Evaluate the calculator expression with the calculator_context
    return proto::eval(expr, ctx);
}

int main()
{
    // Displays "5"
    std::cout << evaluate( _1 + 2.0, 3.0 ) << std::endl;

    // Displays "6"
    std::cout << evaluate( _1 * _2, 3.0, 2.0 ) << std::endl;

    // Displays "0.5"
    std::cout << evaluate( (_1 - _2) / _2, 3.0, 2.0 ) << std::endl;

    return 0;
}
//]


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestNthElement
#include <boost/test/unit_test.hpp>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/is_partitioned.hpp>
#include <boost/compute/algorithm/nth_element.hpp>
#include <boost/compute/algorithm/partition_point.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(nth_element_int)
{
    int data[] = { 9, 15, 1, 4, 9, 9, 4, 15, 12, 1 };
    boost::compute::vector<int> vector(10, context);

    boost::compute::copy_n(data, 10, vector.begin(), queue);

    boost::compute::nth_element(
        vector.begin(), vector.begin() + 5, vector.end(), queue
    );

    BOOST_CHECK_EQUAL(vector[5], 9);
    BOOST_VERIFY(boost::compute::is_partitioned(
        vector.begin(), vector.end(), boost::compute::_1 <= 9, queue
    ));
    BOOST_VERIFY(boost::compute::partition_point(
        vector.begin(), vector.end(), boost::compute::_1 <= 9, queue
    ) > vector.begin() + 5);

    boost::compute::copy_n(data, 10, vector.begin(), queue);

    boost::compute::nth_element(
        vector.begin(), vector.end(), vector.end(), queue
    );
    CHECK_RANGE_EQUAL(int, 10, vector, (9, 15, 1, 4, 9, 9, 4, 15, 12, 1));
}

BOOST_AUTO_TEST_CASE(nth_element_median)
{
    int data[] = { 5, 6, 4, 3, 2, 6, 7, 9, 3 };
    boost::compute::vector<int> v(9, context);
    boost::compute::copy_n(data, 9, v.begin(), queue);

    boost::compute::nth_element(v.begin(), v.begin() + 4, v.end(), queue);

    BOOST_CHECK_EQUAL(v[4], 5);
    BOOST_VERIFY(boost::compute::is_partitioned(
        v.begin(), v.end(), boost::compute::_1 <= 5, queue
    ));
    BOOST_VERIFY(boost::compute::partition_point(
        v.begin(), v.end(), boost::compute::_1 <= 5, queue
    ) > v.begin() + 4);
}

BOOST_AUTO_TEST_CASE(nth_element_second_largest)
{
    int data[] = { 5, 6, 4, 3, 2, 6, 7, 9, 3 };
    boost::compute::vector<int> v(9, context);
    boost::compute::copy_n(data, 9, v.begin(), queue);

    boost::compute::nth_element(v.begin(), v.begin() + 1, v.end(), queue);

    BOOST_CHECK_EQUAL(v[1], 3);
    BOOST_VERIFY(boost::compute::is_partitioned(
        v.begin(), v.end(), boost::compute::_1 <= 3, queue
    ));
    BOOST_VERIFY(boost::compute::partition_point(
        v.begin(), v.end(), boost::compute::_1 <= 3, queue
    ) > v.begin() + 1);
}

BOOST_AUTO_TEST_CASE(nth_element_comparator)
{
    int data[] = { 9, 15, 1, 4, 9, 9, 4, 15, 12, 1 };
    boost::compute::vector<int> vector(10, context);

    boost::compute::less<int> less_than;

    boost::compute::copy_n(data, 10, vector.begin(), queue);

    boost::compute::nth_element(
        vector.begin(), vector.begin() + 5, vector.end(), less_than, queue
    );
    BOOST_CHECK_EQUAL(vector[5], 9);
    BOOST_VERIFY(boost::compute::is_partitioned(
        vector.begin(), vector.end(), boost::compute::_1 <= 9, queue
    ));
    BOOST_VERIFY(boost::compute::partition_point(
        vector.begin(), vector.end(), boost::compute::_1 <= 9, queue
    ) > vector.begin() + 5);

    boost::compute::copy_n(data, 10, vector.begin(), queue);

    boost::compute::nth_element(
        vector.begin(), vector.end(), vector.end(), less_than, queue
    );
    CHECK_RANGE_EQUAL(int, 10, vector, (9, 15, 1, 4, 9, 9, 4, 15, 12, 1));
}

BOOST_AUTO_TEST_SUITE_END()


#include <stan/math/prim/prob/hmm_marginal_lpdf.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random.hpp>
#include <test/unit/math/test_ad.hpp>
#include <test/unit/util.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

/**
 * Wrapper around hmm_marginal_density which passes rho and
 * Gamma without the last element of each column. We recover
 * the last element using the fact each column sums to 1.
 * The purpose of this function is to do finite diff benchmarking,
 * without breaking the simplex constraint.
 */
template <typename T_omega, typename T_Gamma, typename T_rho>
inline stan::return_type_t<T_omega, T_Gamma, T_rho> hmm_marginal_test_wrapper(
    const Eigen::Matrix<T_omega, Eigen::Dynamic, Eigen::Dynamic>& log_omegas,
    const Eigen::Matrix<T_Gamma, Eigen::Dynamic, Eigen::Dynamic>&
        Gamma_unconstrained,
    const std::vector<T_rho>& rho_unconstrained) {
  using stan::math::row;
  using stan::math::sum;
  int n_states = log_omegas.rows();

  Eigen::Matrix<T_Gamma, Eigen::Dynamic, Eigen::Dynamic> Gamma(n_states,
                                                               n_states);
  for (int i = 0; i < n_states; i++) {
    Gamma(i, n_states - 1) = 1 - sum(row(Gamma_unconstrained, i + 1));
    for (int j = 0; j < n_states - 1; j++) {
      Gamma(i, j) = Gamma_unconstrained(i, j);
    }
  }

  Eigen::Matrix<T_rho, Eigen::Dynamic, 1> rho(n_states);
  rho(1) = 1 - sum(rho_unconstrained);
  for (int i = 0; i < n_states - 1; i++)
    rho(i) = rho_unconstrained[i];

  return stan::math::hmm_marginal_lpdf(log_omegas, Gamma, rho);
}

/**
 * In the proposed example, the latent state x determines
 * the observational distribution:
 *  0: normal(mu, sigma)
 *  1: normal(-mu, sigma)
 */
double state_lpdf(double y, double abs_mu, double sigma, int state) {
  int x = state == 0 ? 1 : -1;
  double chi = (y - x * abs_mu) / sigma;
  return -0.5 * chi * chi - 0.5 * std::log(2 * M_PI) - std::log(sigma);
}

class hmm_marginal_lpdf_test : public ::testing::Test {
 protected:
  void SetUp() override {
    n_states_ = 2;
    p1_init_ = 0.65;
    gamma1_ = 0.7;
    gamma2_ = 0.45;
    n_transitions_ = 10;
    abs_mu_ = 1;
    sigma_ = 1;

    Eigen::VectorXd rho(n_states_);
    rho << p1_init_, 1 - p1_init_;
    rho_ = rho;

    Eigen::MatrixXd Gamma(n_states_, n_states_);
    Gamma << gamma1_, 1 - gamma1_, gamma2_, 1 - gamma2_;
    Gamma_ = Gamma;

    Eigen::VectorXd obs_data(n_transitions_ + 1);
    obs_data << -0.3315914, -0.1655340, -0.7984021, 0.2364608, -0.4489722,
        2.1831438, -1.4778675, 0.8717423, -1.0370874, 0.1370296, 1.9786208;
    obs_data_ = obs_data;

    Eigen::MatrixXd log_omegas(n_states_, n_transitions_ + 1);
    for (int n = 0; n < n_transitions_ + 1; n++) {
      log_omegas.col(n)[0] = state_lpdf(obs_data[n], abs_mu_, sigma_, 0);
      log_omegas.col(n)[1] = state_lpdf(obs_data[n], abs_mu_, sigma_, 1);
    }
    log_omegas_ = log_omegas;
    log_omegas_zero_ = log_omegas.block(0, 0, n_states_, 1);

    std::vector<double> rho_unconstrained(n_states_ - 1);
    for (int i = 0; i < rho.size() - 1; i++)
      rho_unconstrained[i] = rho(i);
    rho_unconstrained_ = rho_unconstrained;

    Gamma_unconstrained_ = Gamma.block(0, 0, n_states_, n_states_ - 1);
  }

  int n_states_, n_transitions_;
  double abs_mu_, sigma_, p1_init_, gamma1_, gamma2_;

  Eigen::VectorXd rho_;
  Eigen::MatrixXd Gamma_;
  Eigen::VectorXd obs_data_;
  Eigen::MatrixXd log_omegas_;
  Eigen::MatrixXd log_omegas_zero_;

  // Construct "unconstrained" versions of rho and Gamma, without
  // the final element which can be determnied using the fact
  // the columns sum to 1. This allows us to do finite diff tests,
  // without violating the simplex constraint of rho and Gamma.
  std::vector<double> rho_unconstrained_;
  Eigen::MatrixXd Gamma_unconstrained_;
  stan::test::ad_tolerances tols_;
};

// For evaluation of the density, the C++ code is benchmarked against
// a forward algorithm written in R.
// TODO(charlesm93): Add public repo link with R script.
TEST_F(hmm_marginal_lpdf_test, ten_transitions) {
  using stan::math::hmm_marginal_lpdf;

  EXPECT_FLOAT_EQ(-18.37417, hmm_marginal_lpdf(log_omegas_, Gamma_, rho_));

  // Differentiation tests
  auto hmm_functor = [](const auto& log_omegas, const auto& Gamma_unconstrained,
                        const auto& rho_unconstrained) {
    return hmm_marginal_test_wrapper(log_omegas, Gamma_unconstrained,
                                     rho_unconstrained);
  };

  stan::test::expect_ad(tols_, hmm_functor, log_omegas_, Gamma_unconstrained_,
                        rho_unconstrained_);
}

TEST_F(hmm_marginal_lpdf_test, zero_transitions) {
  using stan::math::hmm_marginal_lpdf;

  EXPECT_FLOAT_EQ(-1.520827, hmm_marginal_lpdf(log_omegas_zero_, Gamma_, rho_));

  // Differentiation tests
  auto hmm_functor = [](const auto& log_omegas, const auto& Gamma_unconstrained,
                        const auto& rho_unconstrained) {
    return hmm_marginal_test_wrapper(log_omegas, Gamma_unconstrained,
                                     rho_unconstrained);
  };

  stan::test::expect_ad(tols_, hmm_functor, log_omegas_zero_,
                        Gamma_unconstrained_, rho_unconstrained_);
}

TEST(hmm_marginal_lpdf, one_state) {
  using stan::math::hmm_marginal_lpdf;
  int n_states = 1, p1_init = 1, gamma1 = 1, n_transitions = 10, abs_mu = 1,
      sigma = 1;
  Eigen::VectorXd rho(n_states);
  rho << p1_init;
  Eigen::MatrixXd Gamma(n_states, n_states);
  Gamma << gamma1;
  Eigen::VectorXd obs_data(n_transitions + 1);
  obs_data << -0.9692032, 1.6367754, 1.0339449, 0.9798393, 0.4829358, 2.7508704,
      0.3122448, 1.8316583, 1.6327319, 1.2097332, 0.4087620;
  Eigen::MatrixXd log_omegas(n_states, n_transitions + 1);
  for (int n = 0; n < n_transitions + 1; n++)
    log_omegas.col(n)[0] = state_lpdf(obs_data[n], abs_mu, sigma, 0);

  EXPECT_FLOAT_EQ(-14.89646, hmm_marginal_lpdf(log_omegas, Gamma, rho));

  // Differentiation tests
  // In the case where we have one state, Gamma and rho
  // are fixed (i.e = 1)
  auto hmm_functor = [](const auto& log_omegas) {
    Eigen::MatrixXd Gamma(1, 1);
    Gamma << 1;
    Eigen::VectorXd rho(1);
    rho << 1;

    return hmm_marginal_lpdf(log_omegas, Gamma, rho);
  };

  stan::test::ad_tolerances tols;
  stan::test::expect_ad(tols, hmm_functor, log_omegas);
}

TEST(hmm_marginal_lpdf, exceptions) {
  using Eigen::MatrixXd;
  using Eigen::VectorXd;
  using stan::math::hmm_marginal_lpdf;

  int n_states = 2;
  int n_transitions = 2;
  MatrixXd log_omegas(n_states, n_transitions + 1);
  MatrixXd Gamma(n_states, n_states);
  VectorXd rho(n_states);

  for (int i = 0; i < n_states; i++)
    for (int j = 0; j < n_transitions + 1; j++)
      log_omegas(i, j) = 1;

  rho(0) = 0.65;
  rho(1) = 0.35;
  Gamma << 0.8, 0.2, 0.6, 0.4;

  // Gamma is not square.
  MatrixXd Gamma_rec(n_states, n_states + 1);
  EXPECT_THROW_MSG(
      hmm_marginal_lpdf(log_omegas, Gamma_rec, rho), std::invalid_argument,
      "hmm_marginal_lpdf: Expecting a square matrix; rows of Gamma (2) "
      "and columns of Gamma (3) must match in size");

  // Gamma has a column that is not a simplex.
  MatrixXd Gamma_bad = Gamma;
  Gamma_bad(0, 0) = Gamma(0, 0) + 1;
  EXPECT_THROW_MSG(hmm_marginal_lpdf(log_omegas, Gamma_bad, rho),
                   std::domain_error,
                   "hmm_marginal_lpdf: Gamma[i, ] is not a valid simplex. "
                   "sum(Gamma[i, ]) = 2, but should be 1")

  // The size of Gamma is 0, even though there is at least one transition
  MatrixXd Gamma_empty(0, 0);
  EXPECT_THROW_MSG(
      hmm_marginal_lpdf(log_omegas, Gamma_empty, rho), std::invalid_argument,
      "hmm_marginal_lpdf: Gamma has size 0, but must have a non-zero size")

  // The size of Gamma is inconsistent with that of log_omega
  MatrixXd Gamma_wrong_size(n_states + 1, n_states + 1);

  EXPECT_THROW_MSG(hmm_marginal_lpdf(log_omegas, Gamma_wrong_size, rho),
                   std::invalid_argument,
                   "hmm_marginal_lpdf: Columns of Gamma (3)"
                   " and Rows of log_omegas (2) must match in size")

  // rho is not a simplex.
  VectorXd rho_bad = rho;
  rho_bad(0) = rho(0) + 1;
  EXPECT_THROW_MSG(hmm_marginal_lpdf(log_omegas, Gamma, rho_bad),
                   std::domain_error,
                   "hmm_marginal_lpdf: rho is not a valid simplex. "
                   "sum(rho) = 2, but should be 1")

  // The size of rho is inconsistent with that of log_omega
  VectorXd rho_wrong_size(n_states + 1);
  EXPECT_THROW_MSG(
      hmm_marginal_lpdf(log_omegas, Gamma, rho_wrong_size),
      std::invalid_argument,
      "hmm_marginal_lpdf: rho has dimension = 3, expecting dimension = 2;"
      " a function was called with arguments of different scalar,"
      " array, vector, or matrix types, and they were not consistently sized;"
      "  all arguments must be scalars or multidimensional values of"
      " the same shape.")
}


// Boost.Geometry

// Copyright (c) 2018, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_LINE_INTERPOLATE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_LINE_INTERPOLATE_HPP

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/strategies/line_interpolate.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace line_interpolate
{

/*!
\brief Interpolate point on a cartesian segment.
\ingroup strategies
\tparam CalculationType \tparam_calculation
\tparam DistanceStrategy The underlying point-point distance strategy

\qbk{
[heading See also]
\* [link geometry.reference.algorithms.line_interpolate.line_interpolate_4_with_strategy line_interpolate (with strategy)]
}

*/
template
<
    typename CalculationType = void,
    typename DistanceStrategy = distance::pythagoras<CalculationType>
>
class cartesian
{
public:

    // point-point strategy getters
    struct distance_pp_strategy
    {
        typedef DistanceStrategy type;
    };

    inline typename distance_pp_strategy::type get_distance_pp_strategy() const
    {
        typedef typename distance_pp_strategy::type distance_type;
        return distance_type();
    }

    template <typename Point, typename Fraction, typename Distance>
    inline void apply(Point const& p0,
                      Point const& p1,
                      Fraction const& fraction,
                      Point & p,
                      Distance const&) const
    {
        typedef typename select_calculation_type_alt
            <
                CalculationType,
                Point
            >::type calc_t;

        typedef model::point
            <
                calc_t,
                geometry::dimension<Point>::value,
                cs::cartesian
            > calc_point_t;

        calc_point_t cp0, cp1;
        geometry::detail::conversion::convert_point_to_point(p0, cp0);
        geometry::detail::conversion::convert_point_to_point(p1, cp1);

        //segment convex combination: p0*fraction + p1*(1-fraction)
        Fraction const one_minus_fraction = 1-fraction;
        for_each_coordinate(cp1, detail::value_operation
                                 <
                                    Fraction,
                                    std::multiplies
                                 >(fraction));
        for_each_coordinate(cp0, detail::value_operation
                                 <
                                    Fraction,
                                    std::multiplies
                                 >(one_minus_fraction));
        for_each_coordinate(cp1, detail::point_operation
                                 <
                                    calc_point_t,
                                    std::plus
                                 >(cp0));

        assert_dimension_equal<calc_point_t, Point>();
        geometry::detail::conversion::convert_point_to_point(cp1, p);
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <>
struct default_strategy<cartesian_tag>
{
    typedef strategy::line_interpolate::cartesian<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::line_interpolate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_LINE_INTERPOLATE_HPP


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  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 <boost/math/concepts/real_concept.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/array.hpp>
#include <boost/tr1/random.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "table_type.hpp"

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template <class Real, typename T>
void do_test_ellint_rf(T& data, const char* type_name, const char* test)
{
   typedef typename T::value_type row_type;
   typedef Real                   value_type;

   std::cout << "Testing: " << test << std::endl;

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    value_type (*fp)(value_type, value_type, value_type) = boost::math::ellint_rf<value_type, value_type, value_type>;
#else
    value_type (*fp)(value_type, value_type, value_type) = boost::math::ellint_rf;
#endif
    boost::math::tools::test_result<value_type> result;

    result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(fp, 0, 1, 2),
      extract_result<Real>(3));
   handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "boost::math::ellint_rf", test);

   std::cout << std::endl;

}

template <class Real, typename T>
void do_test_ellint_rc(T& data, const char* type_name, const char* test)
{
   typedef typename T::value_type row_type;
   typedef Real                   value_type;

   std::cout << "Testing: " << test << std::endl;

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    value_type (*fp)(value_type, value_type) = boost::math::ellint_rc<value_type, value_type>;
#else
    value_type (*fp)(value_type, value_type) = boost::math::ellint_rc;
#endif
    boost::math::tools::test_result<value_type> result;

    result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(fp, 0, 1),
      extract_result<Real>(2));
      handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "boost::math::ellint_rc", test);

   std::cout << std::endl;

}

template <class Real, typename T>
void do_test_ellint_rj(T& data, const char* type_name, const char* test)
{
   typedef typename T::value_type row_type;
   typedef Real                   value_type;

   std::cout << "Testing: " << test << std::endl;

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    value_type (*fp)(value_type, value_type, value_type, value_type) = boost::math::ellint_rj<value_type, value_type, value_type, value_type>;
#else
    value_type (*fp)(value_type, value_type, value_type, value_type) = boost::math::ellint_rj;
#endif
    boost::math::tools::test_result<value_type> result;

    result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(fp, 0, 1, 2, 3),
      extract_result<Real>(4));
      handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "boost::math::ellint_rf", test);

   std::cout << std::endl;

}

template <class Real, typename T>
void do_test_ellint_rd(T& data, const char* type_name, const char* test)
{
   typedef typename T::value_type row_type;
   typedef Real                   value_type;

   std::cout << "Testing: " << test << std::endl;

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    value_type (*fp)(value_type, value_type, value_type) = boost::math::ellint_rd<value_type, value_type, value_type>;
#else
    value_type (*fp)(value_type, value_type, value_type) = boost::math::ellint_rd;
#endif
    boost::math::tools::test_result<value_type> result;

    result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(fp, 0, 1, 2),
      extract_result<Real>(3));
    handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "boost::math::ellint_rd", test);

   std::cout << std::endl;

}

template <typename T>
void test_spots(T, const char* type_name)
{
#ifndef TEST_UDT
   using namespace boost::math;
   using namespace std;
   // Spot values from Numerical Computation of Real or Complex
   // Elliptic Integrals, B. C. Carlson: http://arxiv.org/abs/math.CA/9409227
   // RF:
   T tolerance = (std::max)(T(1e-13f), tools::epsilon<T>() * 5) * 100; // Note 5eps expressed as a persentage!!!
   T eps2 = 2 * tools::epsilon<T>();
   BOOST_CHECK_CLOSE(ellint_rf(T(1), T(2), T(0)), T(1.3110287771461), tolerance);
   BOOST_CHECK_CLOSE(ellint_rf(T(0.5), T(1), T(0)), T(1.8540746773014), tolerance);
   BOOST_CHECK_CLOSE(ellint_rf(T(2), T(3), T(4)), T(0.58408284167715), tolerance);
   // RC:
   BOOST_CHECK_CLOSE_FRACTION(ellint_rc(T(0), T(1)/4), boost::math::constants::pi<T>(), eps2);
   BOOST_CHECK_CLOSE_FRACTION(ellint_rc(T(9)/4, T(2)), log(T(2)), eps2);
   BOOST_CHECK_CLOSE_FRACTION(ellint_rc(T(1)/4, T(-2)), log(T(2))/3, eps2);
   // RJ:
   BOOST_CHECK_CLOSE(ellint_rj(T(0), T(1), T(2), T(3)), T(0.77688623778582), tolerance);
   BOOST_CHECK_CLOSE(ellint_rj(T(2), T(3), T(4), T(5)), T(0.14297579667157), tolerance);
   BOOST_CHECK_CLOSE(ellint_rj(T(2), T(3), T(4), T(-0.5)), T(0.24723819703052), tolerance);
   BOOST_CHECK_CLOSE(ellint_rj(T(2), T(3), T(4), T(-5)), T(-0.12711230042964), tolerance);
   // RD:
   BOOST_CHECK_CLOSE(ellint_rd(T(0), T(2), T(1)), T(1.7972103521034), tolerance);
   BOOST_CHECK_CLOSE(ellint_rd(T(2), T(3), T(4)), T(0.16510527294261), tolerance);

   // Sanity/consistency checks from Numerical Computation of Real or Complex
   // Elliptic Integrals, B. C. Carlson: http://arxiv.org/abs/math.CA/9409227
   std::tr1::mt19937 ran;
   std::tr1::uniform_real<float> ur(0, 1000);
   T eps40 = 40 * tools::epsilon<T>();

   for(unsigned i = 0; i < 1000; ++i)
   {
      T x = ur(ran);
      T y = ur(ran);
      T z = ur(ran);
      T lambda = ur(ran);
      T mu = x * y / lambda;
      // RF, eq 49:
      T s1 = ellint_rf(x+lambda, y+lambda, lambda) +
         ellint_rf(x + mu, y + mu, mu);
      T s2 = ellint_rf(x, y, T(0));
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
      // RC is degenerate case of RF:
      s1 = ellint_rc(x, y);
      s2 = ellint_rf(x, y, y);
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
      // RC, eq 50 (Note have to assume y = x):
      T mu2 = x * x / lambda;
      s1 = ellint_rc(lambda, x+lambda)
         + ellint_rc(mu2, x + mu2);
      s2 = ellint_rc(T(0), x);
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
      /*
      T p = ????; // no closed form for a, b and p???
      s1 = ellint_rj(x+lambda, y+lambda, lambda, p+lambda)
         + ellint_rj(x+mu, y+mu, mu, p+mu);
      s2 = ellint_rj(x, y, T(0), p)
         - 3 * ellint_rc(a, b);
      */
      // RD, eq 53:
      s1 = ellint_rd(lambda, x+lambda, y+lambda)
         + ellint_rd(mu, x+mu, y+mu);
      s2 = ellint_rd(T(0), x, y)
         - 3 / (y * sqrt(x+y+lambda+mu));
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
      // RD is degenerate case of RJ:
      s1 = ellint_rd(x, y, z);
      s2 = ellint_rj(x, y, z, z);
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
   }
#endif
   //
   // Now random spot values:
   //
#include "ellint_rf_data.ipp"

   do_test_ellint_rf<T>(ellint_rf_data, type_name, "RF: Random data");

#include "ellint_rc_data.ipp"

   do_test_ellint_rc<T>(ellint_rc_data, type_name, "RC: Random data");

#include "ellint_rj_data.ipp"

   do_test_ellint_rj<T>(ellint_rj_data, type_name, "RJ: Random data");

#include "ellint_rd_data.ipp"

   do_test_ellint_rd<T>(ellint_rd_data, type_name, "RD: Random data");
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
// Copyright 2018 Tom Westerhout
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <iostream>
#include "Utils/random_utils.hpp"
#include <vector>
#include "Utils/json_utils.hpp"
#include "abstract_hilbert.hpp"

#ifndef NETKET_SPIN_HPP
#define NETKET_SPIN_HPP

namespace netket {

/**
  Hilbert space for integer or half-integer spins.
  Notice that here integer values are always used to represent the local quantum
  numbers, such that for example if total spin is S=3/2, the allowed quantum
  numbers are -3,-1,1,3, and if S=1 we have -2,0,2.
*/

class Spin : public AbstractHilbert {
  double S_;
  double totalS_;
  bool constraintSz_;

  std::vector<double> local_;

  int nstates_;

  int nspins_;

 public:
  explicit Spin(const json &pars) {
    const int nspins = FieldVal(pars["Hilbert"], "Nspins", "Hilbert");
    const double S = FieldVal(pars["Hilbert"], "S", "Hilbert");

    Init(nspins, S);

    if (FieldExists(pars["Hilbert"], "TotalSz")) {
      SetConstraint(pars["Hilbert"]["TotalSz"]);
    } else {
      constraintSz_ = false;
    }
  }

  void Init(int nspins, double S) {
    S_ = S;
    nspins_ = nspins;

    if (S <= 0) {
      throw InvalidInputError("Invalid spin value");
    }

    if (std::floor(2. * S) != 2. * S) {
      throw InvalidInputError("Spin value is neither integer nor half integer");
    }

    nstates_ = std::floor(2. * S) + 1;

    local_.resize(nstates_);

    int sp = -std::floor(2. * S);
    for (int i = 0; i < nstates_; i++) {
      local_[i] = sp;
      sp += 2;
    }
  }

  void SetConstraint(double totalS) {
    constraintSz_ = true;
    totalS_ = totalS;
  }

  bool IsDiscrete() const override { return true; }

  int LocalSize() const override { return nstates_; }

  int Size() const override { return nspins_; }

  std::vector<double> LocalStates() const override { return local_; }

  void RandomVals(Eigen::VectorXd &state,
                  netket::default_random_engine &rgen) const override {
    std::uniform_int_distribution<int> distribution(0, nstates_ - 1);

    assert(state.size() == nspins_);

    if (!constraintSz_) {
      // unconstrained random
      for (int i = 0; i < state.size(); i++) {
        state(i) = 2. * (distribution(rgen) - S_);
      }
    } else if (S_ == 0.5) {
      using std::begin;
      using std::end;
      // Magnetisation as a count
      auto const m = static_cast<int>(2 * totalS_);
      if (std::abs(m) > nspins_) {
        throw InvalidInputError(
            "Cannot fix the total magnetization: 2|M| cannot "
            "exceed Nspins.");
      }
      if ((nspins_ + m) % 2 != 0) {
        throw InvalidInputError(
            "Cannot fix the total magnetization: Nspins + "
            "totalSz must be even.");
      }
      auto const nup = (nspins_ + m) / 2;
      auto const ndown = (nspins_ - m) / 2;
      std::fill_n(state.data(), nup, 1.0);
      std::fill_n(state.data() + nup, ndown, -1.0);
      std::shuffle(state.data(), state.data() + nspins_, rgen);
      return;
    } else {
      std::vector<int> sites;
      for (int i = 0; i < nspins_; ++i) sites.push_back(i);

      state.setConstant(-2 * S_);
      int ss = nspins_;

      for (int i = 0; i < S_ * nspins_ + totalS_; ++i) {
        std::uniform_int_distribution<int> distribution_ss(0, ss - 1);
        int s = distribution_ss(rgen);
        state(sites[s]) += 2;
        if (state(sites[s]) > 2 * S_ - 1) {
          sites.erase(sites.begin() + s);
          ss -= 1;
        }
      }
    }
  }

  void UpdateConf(Eigen::VectorXd &v, const std::vector<int> &tochange,
                  const std::vector<double> &newconf) const override {
    assert(v.size() == nspins_);

    int i = 0;
    for (auto sf : tochange) {
      v(sf) = newconf[i];
      i++;
    }
  }
};

}  // namespace netket
#endif


/*
 * snake_brain.hpp
 * Copyright (C) 2020 Lucas Costa Campos <rmk236@gmail.com>
 *
 * Distributed under terms of the MIT license.
 */

#ifndef SNAKE_BRAIN_HPP
#define SNAKE_BRAIN_HPP

#include "moves.hpp"
#include "location.hpp"
#include <Eigen/Eigen>
#include <iostream>
#include <cmath>

class Board;

class NeuralNetwork
{
public:
    NeuralNetwork ();
    NeuralNetwork (const NeuralNetwork& nn);
    virtual ~NeuralNetwork ();

    void compress();

    Move decide(const Eigen::Matrix<float, 24, 1>& input);

    Eigen::Matrix<float, 18, 24> in;
    Eigen::Matrix<float, 18, 18> hidden1, hidden2;
    Eigen::Matrix<float, 4, 18> out;

    Eigen::Matrix<float, 4, 24> final_matrix;

};

class SnakeBrain
{
public:
    SnakeBrain ();
    SnakeBrain (const NeuralNetwork& nn);
    virtual ~SnakeBrain () {};
    Move next_move(const Board& board);

private:
    NeuralNetwork nn;

    // The orders are always Left, Right, Down, Up, Down-left, Down-Right, Up-Left, Up-Right;
    // Each set of three entries will have, in order, (distance, hasFood, hasBody)
    Eigen::Matrix<float, 24, 1> state;
};

#endif /* !SNAKE_BRAIN_HPP */


#include <Eigen/Core>
#include <boost/python.hpp>
#include <numpy/arrayobject.h>

namespace boopy
{
  namespace bp = boost::python;

  template <typename SCALAR>  struct NumpyEquivalentType {};
  template <> struct NumpyEquivalentType<double>  { enum { type_code = NPY_DOUBLE };};
  template <> struct NumpyEquivalentType<int>     { enum { type_code = NPY_INT    };};
  template <> struct NumpyEquivalentType<float>   { enum { type_code = NPY_FLOAT  };};

  /* --- TO PYTHON -------------------------------------------------------------- */
  template< typename MatType >
  struct EigenMatrix_to_python_matrix
  {
    static PyObject* convert(MatType const& mat)
    {
      typedef typename MatType::Scalar T;
      const int R  = mat.rows(), C = mat.cols();

      npy_intp shape[2] = { R,C };
      PyArrayObject* pyArray = (PyArrayObject*)
	PyArray_SimpleNew(2, shape,
			  NumpyEquivalentType<T>::type_code);

      T* pyData = (T*)PyArray_DATA(pyArray);
      Eigen::Map< Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> > pyMatrix(pyData,R,C);
      pyMatrix = mat;

      return (PyObject*)pyArray;
    }
  };

  /* --- FROM PYTHON ------------------------------------------------------------ */
  template<typename MatType>
  struct EigenMatrix_from_python_array
  {

    EigenMatrix_from_python_array()
    {
      bp::converter::registry
	::push_back(&convertible,
		    &construct,
		    bp::type_id<MatType>());
    }

    // Determine if obj_ptr can be converted in a Eigenvec
    static void* convertible(PyObject* obj_ptr)
    {
      typedef typename MatType::Scalar T;

      if (!PyArray_Check(obj_ptr)) return 0;

      std::cout << "Until here ok.   ndim = " << PyArray_NDIM(obj_ptr) << " isvec " << MatType::IsVectorAtCompileTime << std::endl;
      if (PyArray_NDIM(obj_ptr) != 2)
	if ( (PyArray_NDIM(obj_ptr) !=1) || (! MatType::IsVectorAtCompileTime) )
	  return 0;
      std::cout << "Until here ok." << std::endl;

      if (PyArray_ObjectType(obj_ptr, 0) != NumpyEquivalentType<T>::type_code)
	return 0;

      if (!(PyArray_FLAGS(obj_ptr) & NPY_ALIGNED))
	{
	  std::cerr << "NPY non-aligned matrices are not implemented." << std::endl;
	  return 0;
	}

      return obj_ptr;
    }

    // Convert obj_ptr into a Eigenvec
    static void construct(PyObject* pyObj,
			  bp::converter::rvalue_from_python_stage1_data* memory)
    {
      typedef typename MatType::Scalar T;
      using namespace Eigen;

      std::cout << "Until here ok. Constructing..." << std::endl;
      PyArrayObject * pyArray = reinterpret_cast<PyArrayObject*>(pyObj);

      if ( PyArray_NDIM(pyArray) == 2 )
      {
	int R = MatType::RowsAtCompileTime;
	int C = MatType::ColsAtCompileTime;
	if (R == Eigen::Dynamic) R = PyArray_DIMS(pyArray)[0];
	else	               assert(PyArray_DIMS(pyArray)[0]==R);

	if (C == Eigen::Dynamic) C = PyArray_DIMS(pyArray)[1];
	else	               assert(PyArray_DIMS(pyArray)[1]==C);

	T* pyData = reinterpret_cast<T*>(PyArray_DATA(pyArray));

	int itemsize = PyArray_ITEMSIZE(pyArray);
	int stride1 = PyArray_STRIDE(pyArray, 0) / itemsize;
	int stride2 = PyArray_STRIDE(pyArray, 1) / itemsize;
	std::cout << "STRIDE = " << stride1 << " x " << stride2 << std::endl;
	Eigen::Map<MatType,0,Eigen::Stride<Eigen::Dynamic,Eigen::Dynamic> >
	  pyMap( pyData, R,C, Eigen::Stride<Eigen::Dynamic,Eigen::Dynamic>(stride2,stride1) );
	std::cout << "Map = " << pyMap << std::endl;

	void* storage = ((bp::converter::rvalue_from_python_storage<MatType>*)
			 (memory))->storage.bytes;
	MatType & mat = * new (storage) MatType(R,C);
	mat = pyMap;

	memory->convertible = storage;
      }
    else
      {
	int R = MatType::MaxSizeAtCompileTime, C=1;
	if(R==Eigen::Dynamic) R =  PyArray_DIMS(pyArray)[0];
	else                  assert(PyArray_DIMS(pyArray)[0]==R);

	T* pyData = reinterpret_cast<T*>(PyArray_DATA(pyArray));

	int itemsize = PyArray_ITEMSIZE(pyArray);
	int stride = PyArray_STRIDE(pyArray, 0) / itemsize;
	Eigen::Stride<Eigen::Dynamic,Eigen::Dynamic> s(stride,0);
	Eigen::Map<MatType,0,Eigen::InnerStride<Eigen::Dynamic> >
	  pyMap( pyData, R, 1, Eigen::InnerStride<Eigen::Dynamic>(stride) );
	std::cout << "Map = " << pyMap << std::endl;

	void* storage = ((bp::converter::rvalue_from_python_storage<MatType>*)
			 (memory))->storage.bytes;
	MatType & mat = * new (storage) MatType(R,C);
	mat = pyMap;

	memory->convertible = storage;
      }
    }
  };

}

Eigen::MatrixXd test()
{
  Eigen::MatrixXd mat = Eigen::MatrixXd::Random(3,6);
  std::cout << "EigenMAt = " << mat << std::endl;
  return mat;
}
Eigen::VectorXd testVec()
{
  Eigen::VectorXd mat = Eigen::VectorXd::Random(6);
  std::cout << "EigenVec = " << mat << std::endl;
  return mat;
}

void test2( Eigen::MatrixXd mat )
{
  std::cout << "Test2 mat = " << mat << std::endl;
}
void test2Vec( Eigen::VectorXd v )
{
  std::cout << "Test2 vec = " << v << std::endl;
}

BOOST_PYTHON_MODULE(libeigentemplate)
{
  import_array();
  namespace bp = boost::python;
  bp::to_python_converter<Eigen::MatrixXd,
			  boopy::EigenMatrix_to_python_matrix<Eigen::MatrixXd> >();
  boopy::EigenMatrix_from_python_array<Eigen::MatrixXd>();

  bp::to_python_converter<Eigen::VectorXd,
   			  boopy::EigenMatrix_to_python_matrix<Eigen::VectorXd> >();
  boopy::EigenMatrix_from_python_array<Eigen::VectorXd>();

  bp::def("test", test);
  bp::def("testVec", testVec);
  bp::def("test2", test2);
  bp::def("test2Vec", test2Vec);
}


/**
 * \copyright
 * Copyright (c) 2012-2017, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <limits>

#include <gtest/gtest.h>

#include <Eigen/Eigen>

#include "BaseLib/ConfigTree.h"

#include "MaterialLib/FractureModels/LinearElasticIsotropic.h"
#include "MaterialLib/FractureModels/MohrCoulomb.h"

#include "ProcessLib/Parameter/ConstantParameter.h"

using namespace MaterialLib::Fracture;

static const double eps_sigma = 1e6*1e-5;
static const double eps_C = 1e10*1e-5;

TEST(MaterialLib_Fracture, LinearElasticIsotropic)
{
    ProcessLib::ConstantParameter<double> const kn("", 1e11);
    ProcessLib::ConstantParameter<double> const ks("", 1e9);
    LinearElasticIsotropic<2>::MaterialProperties const mp{kn, ks};

    LinearElasticIsotropic<2> fractureModel{mp};
    std::unique_ptr<FractureModelBase<2>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector2d w_prev, w, sigma_prev, sigma;
    Eigen::Matrix2d C;

    ProcessLib::SpatialPosition x;
    w << -1e-5, -1e-5;
    fractureModel.computeConstitutiveRelation(0, x, w_prev, w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(-1e4, sigma[0], eps_sigma);
    ASSERT_NEAR(-1e6, sigma[1], eps_sigma);
    ASSERT_NEAR(1e9, C(0,0), eps_C);
    ASSERT_NEAR(0, C(0,1), eps_C);
    ASSERT_NEAR(0, C(1,0), eps_C);
    ASSERT_NEAR(1e11, C(1,1), eps_C);

    w << -1e-5, 1e-5;
    fractureModel.computeConstitutiveRelation(0, x, w_prev, w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(0, sigma[0], eps_sigma);
    ASSERT_NEAR(0, sigma[1], eps_sigma);
    ASSERT_NEAR(0, C(0,0), eps_C);
    ASSERT_NEAR(0, C(0,1), eps_C);
    ASSERT_NEAR(0, C(1,0), eps_C);
    ASSERT_NEAR(0, C(1,1), eps_C);
}

TEST(MaterialLib_Fracture, MohrCoulomb)
{
    ProcessLib::ConstantParameter<double> const kn("", 50e9);
    ProcessLib::ConstantParameter<double> const ks("", 20e9);
    ProcessLib::ConstantParameter<double> const phi("", 15);
    ProcessLib::ConstantParameter<double> const psi("", 5);
    ProcessLib::ConstantParameter<double> const c("", 3e6);
    MohrCoulomb<2>::MaterialProperties const mp{kn, ks, phi, psi, c};

    MohrCoulomb<2> fractureModel{mp};
    std::unique_ptr<FractureModelBase<2>::MaterialStateVariables> state(
        fractureModel.createMaterialStateVariables());

    Eigen::Vector2d w_prev, w, sigma_prev, sigma;
    Eigen::Matrix2d C;

    ProcessLib::SpatialPosition x;
    sigma_prev << -3.46e6, -2e6;
    w << -1.08e-5, -0.25e-5;
    fractureModel.computeConstitutiveRelation(0, x, w_prev, w, sigma_prev, sigma, C, *state);

    ASSERT_NEAR(-3.50360e6, sigma[0], eps_sigma);
    ASSERT_NEAR(-2.16271e6, sigma[1], eps_sigma);
    ASSERT_NEAR(1.10723e+09, C(0,0), eps_C);
    ASSERT_NEAR(1.26558e+10, C(0,1), eps_C);
    ASSERT_NEAR(4.13226e+09, C(1,0), eps_C);
    ASSERT_NEAR(4.72319e+10, C(1,1), eps_C);
}


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

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2012 Ferdinando Ametrano
 Copyright (C) 2006 Mark Joshi
 Copyright (C) 2006 StatPro Italia srl
 Copyright (C) 2007 Cristina Duminuco
 Copyright (C) 2007 Chiara Fornarola
 Copyright (C) 2013 Gary Kennedy
 Copyright (C) 2015 Peter Caspers
 Copyright (C) 2017 Klaus Spanderen
 Copyright (C) 2019 Wojciech Ślusarski
 Copyright (C) 2020 Marcin Rybacki

 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/pricingengines/blackformula.hpp>
#include <ql/math/functional.hpp>
#include <ql/math/solvers1d/newtonsafe.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/math/special_functions/atanh.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

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

namespace {
    void checkParameters(QuantLib::Real strike,
                         QuantLib::Real forward,
                         QuantLib::Real displacement)
    {
        QL_REQUIRE(displacement >= 0.0, "displacement ("
                                            << displacement
                                            << ") must be non-negative");
        QL_REQUIRE(strike + displacement >= 0.0,
                   "strike + displacement (" << strike << " + " << displacement
                                             << ") must be non-negative");
        QL_REQUIRE(forward + displacement > 0.0, "forward + displacement ("
                                                     << forward << " + "
                                                     << displacement
                                                     << ") must be positive");
    }
}

namespace QuantLib {

    Real blackFormula(Option::Type optionType,
                      Real strike,
                      Real forward,
                      Real stdDev,
                      Real discount,
                      Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        auto sign = Integer(optionType);

        if (stdDev == 0.0)
            return std::max((forward-strike) * sign, Real(0.0)) * discount;

        forward = forward + displacement;
        strike = strike + displacement;

        // since displacement is non-negative strike==0 iff displacement==0
        // so returning forward*discount is OK
        if (strike==0.0)
            return (optionType==Option::Call ? forward*discount : 0.0);

        Real d1 = std::log(forward/strike)/stdDev + 0.5*stdDev;
        Real d2 = d1 - stdDev;
        CumulativeNormalDistribution phi;
        Real nd1 = phi(sign * d1);
        Real nd2 = phi(sign * d2);
        Real result = discount * sign * (forward*nd1 - strike*nd2);
        QL_ENSURE(result>=0.0,
                  "negative value (" << result << ") for " <<
                  stdDev << " stdDev, " <<
                  optionType << " option, " <<
                  strike << " strike , " <<
                  forward << " forward");
        return result;
    }

    Real blackFormula(const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                      Real forward,
                      Real stdDev,
                      Real discount,
                      Real displacement) {
        return blackFormula(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount, displacement);
    }

    Real blackFormulaForwardDerivative(Option::Type optionType,
                                       Real strike,
                                       Real forward,
                                       Real stdDev,
                                       Real discount,
                                       Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        auto sign = Integer(optionType);

        if (stdDev == 0.0)
            return sign * std::max(1.0 * boost::math::sign((forward - strike) * sign), 0.0) * discount;

        forward = forward + displacement;
        strike = strike + displacement;

        if (strike == 0.0)
            return (optionType == Option::Call ? discount : 0.0);

        Real d1 = std::log(forward/strike)/stdDev + 0.5*stdDev;
        CumulativeNormalDistribution phi;
        return sign * phi(sign * d1) * discount;
    }

    Real blackFormulaForwardDerivative(const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                                       Real forward,
                                       Real stdDev,
                                       Real discount,
                                       Real displacement)
    {
        return blackFormulaForwardDerivative(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount, displacement);
    }

    Real blackFormulaImpliedStdDevApproximation(Option::Type optionType,
                                                Real strike,
                                                Real forward,
                                                Real blackPrice,
                                                Real discount,
                                                Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(blackPrice>=0.0,
                   "blackPrice (" << blackPrice << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        Real stdDev;
        forward = forward + displacement;
        strike = strike + displacement;
        if (strike==forward)
            // Brenner-Subrahmanyan (1988) and Feinstein (1988) ATM approx.
            stdDev = blackPrice/discount*std::sqrt(2.0 * M_PI)/forward;
        else {
            // Corrado and Miller extended moneyness approximation
            Real moneynessDelta = Integer(optionType) * (forward-strike);
            Real moneynessDelta_2 = moneynessDelta/2.0;
            Real temp = blackPrice/discount - moneynessDelta_2;
            Real moneynessDelta_PI = moneynessDelta*moneynessDelta/M_PI;
            Real temp2 = temp*temp-moneynessDelta_PI;
            if (temp2<0.0) // approximation breaks down, 2 alternatives:
                // 1. zero it
                temp2=0.0;
                // 2. Manaster-Koehler (1982) efficient Newton-Raphson seed
                //return std::fabs(std::log(forward/strike))*std::sqrt(2.0);
            temp2 = std::sqrt(temp2);
            temp += temp2;
            temp *= std::sqrt(2.0 * M_PI);
            stdDev = temp/(forward+strike);
        }
        QL_ENSURE(stdDev>=0.0,
                  "stdDev (" << stdDev << ") must be non-negative");
        return stdDev;
    }

    Real blackFormulaImpliedStdDevApproximation(
                      const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                      Real forward,
                      Real blackPrice,
                      Real discount,
                      Real displacement) {
        return blackFormulaImpliedStdDevApproximation(payoff->optionType(),
            payoff->strike(), forward, blackPrice, discount, displacement);
    }

    Real blackFormulaImpliedStdDevChambers(Option::Type optionType,
                                                Real strike,
                                                Real forward,
                                                Real blackPrice,
                                                Real blackAtmPrice,
                                                Real discount,
                                                Real displacement) {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(blackPrice >= 0.0,
                   "blackPrice (" << blackPrice << ") must be non-negative");
        QL_REQUIRE(blackAtmPrice >= 0.0, "blackAtmPrice ("
                                             << blackAtmPrice
                                             << ") must be non-negative");
        QL_REQUIRE(discount > 0.0, "discount (" << discount
                                                << ") must be positive");

        Real stdDev;

        forward = forward + displacement;
        strike = strike + displacement;
        blackPrice /= discount;
        blackAtmPrice /= discount;

        Real s0 = M_SQRT2 * M_SQRTPI * blackAtmPrice /
                  forward; // Brenner-Subrahmanyam formula
        Real priceAtmVol =
            blackFormula(optionType, strike, forward, s0, 1.0, 0.0);
        Real dc = blackPrice - priceAtmVol;

        if (close(dc, 0.0)) {
            stdDev = s0;
        } else {
            Real d1 =
                blackFormulaStdDevDerivative(strike, forward, s0, 1.0, 0.0);
            Real d2 = blackFormulaStdDevSecondDerivative(strike, forward, s0,
                                                         1.0, 0.0);
            Real ds = 0.0;
            Real tmp = d1 * d1 + 2.0 * d2 * dc;
            if (std::fabs(d2) > 1E-10 && tmp >= 0.0)
                ds = (-d1 + std::sqrt(tmp)) / d2; // second order approximation
            else
                if(std::fabs(d1) > 1E-10)
                    ds = dc / d1; // first order approximation
            stdDev = s0 + ds;
        }

        QL_ENSURE(stdDev >= 0.0, "stdDev (" << stdDev
                                            << ") must be non-negative");
        return stdDev;
    }

    Real blackFormulaImpliedStdDevChambers(
        const ext::shared_ptr<PlainVanillaPayoff> &payoff,
        Real forward,
        Real blackPrice,
        Real blackAtmPrice,
        Real discount,
        Real displacement) {
        return blackFormulaImpliedStdDevChambers(
            payoff->optionType(), payoff->strike(), forward, blackPrice,
            blackAtmPrice, discount, displacement);
    }

    namespace {
        Real Af(Real x) {
            return 0.5*(1.0+boost::math::sign(x)
                *std::sqrt(1.0-std::exp(-M_2_PI*x*x)));
        }
    }

    Real blackFormulaImpliedStdDevApproximationRS(
        Option::Type type, Real K, Real F,
        Real marketValue, Real df, Real displacement) {

        checkParameters(K, F, displacement);
        QL_REQUIRE(marketValue >= 0.0,
                   "blackPrice (" << marketValue << ") must be non-negative");
        QL_REQUIRE(df > 0.0, "discount (" << df << ") must be positive");

        F = F + displacement;
        K = K + displacement;

        const Real ey = F/K;
        const Real ey2 = ey*ey;
        const Real y = std::log(ey);
        const Real alpha = marketValue/(K*df);
        const Real R = 2*alpha + ((type == Option::Call) ? -ey+1.0 : ey-1.0);
        const Real R2 = R*R;

        const Real a = std::exp((1.0-M_2_PI)*y);
        const Real A = square<Real>()(a - 1.0/a);
        const Real b = std::exp(M_2_PI*y);
        const Real B = 4.0*(b + 1/b)
            - 2*K/F*(a + 1.0/a)*(ey2 + 1 - R2);
        const Real C = (R2-square<Real>()(ey-1))*(square<Real>()(ey+1)-R2)/ey2;

        const Real beta = 2*C/(B+std::sqrt(B*B+4*A*C));
        const Real gamma = -M_PI_2*std::log(beta);

        if (y >= 0.0) {
            const Real M0 = K*df*(
                (type == Option::Call) ? ey*Af(std::sqrt(2*y)) - 0.5
                                       : 0.5-ey*Af(-std::sqrt(2*y)));

            if (marketValue <= M0)
                return std::sqrt(gamma+y)-std::sqrt(gamma-y);
            else
                return std::sqrt(gamma+y)+std::sqrt(gamma-y);
        }
        else {
            const Real M0 = K*df*(
                (type == Option::Call) ? 0.5*ey - Af(-std::sqrt(-2*y))
                                       : Af(std::sqrt(-2*y)) - 0.5*ey);

            if (marketValue <= M0)
                return std::sqrt(gamma-y)-std::sqrt(gamma+y);
            else
                return std::sqrt(gamma+y)+std::sqrt(gamma-y);
        }
    }

    Real blackFormulaImpliedStdDevApproximationRS(
        const ext::shared_ptr<PlainVanillaPayoff> &payoff,
        Real F, Real marketValue,
        Real df, Real displacement) {

        return blackFormulaImpliedStdDevApproximationRS(
            payoff->optionType(), payoff->strike(),
            F, marketValue, df, displacement);
    }

    class BlackImpliedStdDevHelper {
      public:
        BlackImpliedStdDevHelper(Option::Type optionType,
                                 Real strike,
                                 Real forward,
                                 Real undiscountedBlackPrice,
                                 Real displacement = 0.0)
        : halfOptionType_(0.5 * Integer(optionType)),
          signedStrike_(Integer(optionType) * (strike+displacement)),
          signedForward_(Integer(optionType) * (forward+displacement)),
          undiscountedBlackPrice_(undiscountedBlackPrice)
        {
            checkParameters(strike, forward, displacement);
            QL_REQUIRE(undiscountedBlackPrice>=0.0,
                       "undiscounted Black price (" <<
                       undiscountedBlackPrice << ") must be non-negative");
            signedMoneyness_ = Integer(optionType) * std::log((forward+displacement)/(strike+displacement));
        }

        Real operator()(Real stdDev) const {
            #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(stdDev>=0.0,
                       "stdDev (" << stdDev << ") must be non-negative");
            #endif
            if (stdDev==0.0)
                return std::max(signedForward_-signedStrike_, Real(0.0))
                                                   - undiscountedBlackPrice_;
            Real temp = halfOptionType_*stdDev;
            Real d = signedMoneyness_/stdDev;
            Real signedD1 = d + temp;
            Real signedD2 = d - temp;
            Real result = signedForward_ * N_(signedD1)
                - signedStrike_ * N_(signedD2);
            // numerical inaccuracies can yield a negative answer
            return std::max(Real(0.0), result) - undiscountedBlackPrice_;
        }
        Real derivative(Real stdDev) const {
            #if defined(QL_EXTRA_SAFETY_CHECKS)
            QL_REQUIRE(stdDev>=0.0,
                       "stdDev (" << stdDev << ") must be non-negative");
            #endif
            Real signedD1 = signedMoneyness_/stdDev + halfOptionType_*stdDev;
            return signedForward_*N_.derivative(signedD1);
        }
      private:
        Real halfOptionType_;
        Real signedStrike_, signedForward_;
        Real undiscountedBlackPrice_, signedMoneyness_;
        CumulativeNormalDistribution N_;
    };

    Real blackFormulaImpliedStdDev(Option::Type optionType,
                                   Real strike,
                                   Real forward,
                                   Real blackPrice,
                                   Real discount,
                                   Real displacement,
                                   Real guess,
                                   Real accuracy,
                                   Natural maxIterations)
    {
        checkParameters(strike, forward, displacement);

        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        QL_REQUIRE(blackPrice>=0.0,
                   "option price (" << blackPrice << ") must be non-negative");
        // check the price of the "other" option implied by put-call paity
        Real otherOptionPrice = blackPrice - Integer(optionType) * (forward-strike)*discount;
        QL_REQUIRE(otherOptionPrice>=0.0,
                   "negative " << Option::Type(-1*optionType) <<
                   " price (" << otherOptionPrice <<
                   ") implied by put-call parity. No solution exists for " <<
                   optionType << " strike " << strike <<
                   ", forward " << forward <<
                   ", price " << blackPrice <<
                   ", deflator " << discount);

        // solve for the out-of-the-money option which has
        // greater vega/price ratio, i.e.
        // it is numerically more robust for implied vol calculations
        if (optionType==Option::Put && strike>forward) {
            optionType = Option::Call;
            blackPrice = otherOptionPrice;
        }
        if (optionType==Option::Call && strike<forward) {
            optionType = Option::Put;
            blackPrice = otherOptionPrice;
        }

        strike = strike + displacement;
        forward = forward + displacement;

        if (guess==Null<Real>())
            guess = blackFormulaImpliedStdDevApproximation(
                optionType, strike, forward, blackPrice, discount, displacement);
        else
            QL_REQUIRE(guess>=0.0,
                       "stdDev guess (" << guess << ") must be non-negative");
        BlackImpliedStdDevHelper f(optionType, strike, forward,
                                   blackPrice/discount);
        NewtonSafe solver;
        solver.setMaxEvaluations(maxIterations);
        Real minSdtDev = 0.0, maxStdDev = 24.0; // 24 = 300% * sqrt(60)
        Real stdDev = solver.solve(f, accuracy, guess, minSdtDev, maxStdDev);
        QL_ENSURE(stdDev>=0.0,
                  "stdDev (" << stdDev << ") must be non-negative");
        return stdDev;
    }

    Real blackFormulaImpliedStdDev(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real blackPrice,
                        Real discount,
                        Real displacement,
                        Real guess,
                        Real accuracy,
                        Natural maxIterations) {
        return blackFormulaImpliedStdDev(payoff->optionType(), payoff->strike(),
            forward, blackPrice, discount, displacement, guess, accuracy, maxIterations);
    }

    namespace {
        Real Np(Real x, Real v) {
            return CumulativeNormalDistribution()(x/v + 0.5*v);
        }
        Real Nm(Real x, Real v) {
            return std::exp(-x)*CumulativeNormalDistribution()(x/v - 0.5*v);
        }
        Real phi(Real x, Real v) {
            const Real ax = 2*std::fabs(x);
            const Real v2 = v*v;
            return (v2-ax)/(v2+ax);
        }
        Real F(Real v, Real x, Real cs, Real w) {
            return cs+Nm(x,v)+w*Np(x,v);
        }
        Real G(Real v, Real x, Real cs, Real w) {
            const Real q = F(v,x,cs,w)/(1+w);

            // Acklam's inverse w/o Halley's refinement step
            // does not provide enough accuracy. But both together are
            // slower than the boost replacement.
            const Real k = MaddockInverseCumulativeNormal()(q);

            return k + std::sqrt(k*k + 2*std::fabs(x));
        }
    }

    Real blackFormulaImpliedStdDevLiRS(
        Option::Type optionType,
        Real strike,
        Real forward,
        Real blackPrice,
        Real discount,
        Real displacement,
        Real guess,
        Real w,
        Real accuracy,
        Natural maxIterations) {

        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        QL_REQUIRE(blackPrice>=0.0,
                   "option price (" << blackPrice << ") must be non-negative");

        strike = strike + displacement;
        forward = forward + displacement;

        if (guess == Null<Real>()) {
            guess = blackFormulaImpliedStdDevApproximationRS(
                optionType, strike, forward,
                blackPrice, discount, displacement);
        }
        else {
            QL_REQUIRE(guess>=0.0,
                "stdDev guess (" << guess << ") must be non-negative");
        }

        Real x = std::log(forward/strike);
        Real cs = (optionType == Option::Call)
            ? blackPrice / (forward*discount)
            : (blackPrice/ (forward*discount) + 1.0 - strike/forward);

        QL_REQUIRE(cs >= 0.0, "normalized call price (" << cs
                   << ") must be positive");

        if (x > 0) {
            // use in-out duality
            cs = forward/strike*cs + 1.0 - forward/strike;
            QL_REQUIRE(cs >= 0.0, "negative option price from in-out duality");
            x = -x;
        }

        Size nIter = 0;
        Real dv, vk, vkp1 = guess;

        do {
            vk = vkp1;
            const Real alphaK = (1+w)/(1+phi(x,vk));
            vkp1 = alphaK*G(vk,x,cs,w) + (1-alphaK)*vk;
            dv = std::fabs(vkp1 - vk);
        } while (dv > accuracy && ++nIter < maxIterations);

        QL_REQUIRE(dv <= accuracy, "max iterations exceeded");
        QL_REQUIRE(vk >= 0.0, "stdDev (" << vk << ") must be non-negative");

        return vk;
    }

    Real blackFormulaImpliedStdDevLiRS(
        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
        Real forward,
        Real blackPrice,
        Real discount,
        Real displacement,
        Real guess,
        Real omega,
        Real accuracy,
        Natural maxIterations) {

        return blackFormulaImpliedStdDevLiRS(
            payoff->optionType(), payoff->strike(),
            forward, blackPrice, discount, displacement,
            guess, omega, accuracy, maxIterations);
    }

    Real blackFormulaCashItmProbability(Option::Type optionType,
                                        Real strike,
                                        Real forward,
                                        Real stdDev,
                                        Real displacement) {
        checkParameters(strike, forward, displacement);

        auto sign = Integer(optionType);

        if (stdDev==0.0)
            return (forward * sign > strike * sign ? 1.0 : 0.0);

        forward = forward + displacement;
        strike = strike + displacement;
        if (strike==0.0)
            return (optionType == Option::Call ? 1.0 : 0.0);
        Real d2 = std::log(forward/strike)/stdDev - 0.5*stdDev;
        CumulativeNormalDistribution phi;
        return phi(sign * d2);
    }

    Real blackFormulaCashItmProbability(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real displacement) {
        return blackFormulaCashItmProbability(payoff->optionType(),
            payoff->strike(), forward, stdDev , displacement);
    }

    Real blackFormulaAssetItmProbability(
                        Option::Type optionType,
                        Real strike,
                        Real forward,
                        Real stdDev,
                        Real displacement) {
        checkParameters(strike, forward, displacement);

        auto sign = Integer(optionType);

        if (stdDev==0.0)
            return (forward * sign < strike * sign ? 1.0 : 0.0);

        forward = forward + displacement;
        strike = strike + displacement;
        if (strike == 0.0)
            return (optionType == Option::Call ? 1.0 : 0.0);
        Real d1 = std::log(forward/strike)/stdDev + 0.5*stdDev;
        CumulativeNormalDistribution phi;
        return phi(sign * d1);
    }

    Real blackFormulaAssetItmProbability(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real displacement) {
        return blackFormulaAssetItmProbability(payoff->optionType(),
            payoff->strike(), forward, stdDev , displacement);
    }

    Real blackFormulaVolDerivative(Rate strike,
                                      Rate forward,
                                      Real stdDev,
                                      Real expiry,
                                      Real discount,
                                      Real displacement)
    {
        return  blackFormulaStdDevDerivative(strike,
                                     forward,
                                     stdDev,
                                     discount,
                                     displacement)*std::sqrt(expiry);
    }

    Real blackFormulaStdDevDerivative(Rate strike,
                                      Rate forward,
                                      Real stdDev,
                                      Real discount,
                                      Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        forward = forward + displacement;
        strike = strike + displacement;

        if (stdDev==0.0 || strike==0.0)
            return 0.0;

        Real d1 = std::log(forward/strike)/stdDev + .5*stdDev;
        return discount * forward *
            CumulativeNormalDistribution().derivative(d1);
    }

    Real blackFormulaStdDevDerivative(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount,
                        Real displacement) {
        return blackFormulaStdDevDerivative(payoff->strike(), forward,
                                     stdDev, discount, displacement);
    }

    Real blackFormulaStdDevSecondDerivative(Rate strike,
                                            Rate forward,
                                            Real stdDev,
                                            Real discount,
                                            Real displacement)
    {
        checkParameters(strike, forward, displacement);
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        forward = forward + displacement;
        strike = strike + displacement;

        if (stdDev==0.0 || strike==0.0)
            return 0.0;

        Real d1 = std::log(forward/strike)/stdDev + .5*stdDev;
        Real d1p = -std::log(forward/strike)/(stdDev*stdDev) + .5;
        return discount * forward *
            NormalDistribution().derivative(d1) * d1p;
    }

    Real blackFormulaStdDevSecondDerivative(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount,
                        Real displacement) {
        return blackFormulaStdDevSecondDerivative(payoff->strike(), forward,
                                     stdDev, discount, displacement);
    }

    Real bachelierBlackFormula(Option::Type optionType,
                               Real strike,
                               Real forward,
                               Real stdDev,
                               Real discount)
    {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");
        Real d = (forward-strike) * Integer(optionType), h = d / stdDev;
        if (stdDev==0.0)
            return discount*std::max(d, 0.0);
        CumulativeNormalDistribution phi;
        Real result = discount*(stdDev*phi.derivative(h) + d*phi(h));
        QL_ENSURE(result>=0.0,
                  "negative value (" << result << ") for " <<
                  stdDev << " stdDev, " <<
                  optionType << " option, " <<
                  strike << " strike , " <<
                  forward << " forward");
        return result;
    }

    Real bachelierBlackFormula(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount) {
        return bachelierBlackFormula(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount);
    }

    Real bachelierBlackFormulaForwardDerivative(
        Option::Type optionType, Real strike, Real forward, Real stdDev, Real discount)
    {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");
        auto sign = Integer(optionType);
        if (stdDev == 0.0)
            return sign * std::max(1.0 * boost::math::sign((forward - strike) * sign), 0.0) * discount;
        Real d = (forward - strike) * sign, h = d / stdDev;
        CumulativeNormalDistribution phi;
        return sign * phi(h) * discount;
    }

    Real bachelierBlackFormulaForwardDerivative(
        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
        Real forward,
        Real stdDev,
        Real discount)
    {
        return bachelierBlackFormulaForwardDerivative(payoff->optionType(),
            payoff->strike(), forward, stdDev, discount);
    }

    static Real h(Real eta) {

        const static Real  A0          = 3.994961687345134e-1;
        const static Real  A1          = 2.100960795068497e+1;
        const static Real  A2          = 4.980340217855084e+1;
        const static Real  A3          = 5.988761102690991e+2;
        const static Real  A4          = 1.848489695437094e+3;
        const static Real  A5          = 6.106322407867059e+3;
        const static Real  A6          = 2.493415285349361e+4;
        const static Real  A7          = 1.266458051348246e+4;

        const static Real  B0          = 1.000000000000000e+0;
        const static Real  B1          = 4.990534153589422e+1;
        const static Real  B2          = 3.093573936743112e+1;
        const static Real  B3          = 1.495105008310999e+3;
        const static Real  B4          = 1.323614537899738e+3;
        const static Real  B5          = 1.598919697679745e+4;
        const static Real  B6          = 2.392008891720782e+4;
        const static Real  B7          = 3.608817108375034e+3;
        const static Real  B8          = -2.067719486400926e+2;
        const static Real  B9          = 1.174240599306013e+1;

        QL_REQUIRE(eta>=0.0,
                       "eta (" << eta << ") must be non-negative");

        const Real num = A0 + eta * (A1 + eta * (A2 + eta * (A3 + eta * (A4 + eta
                    * (A5 + eta * (A6 + eta * A7))))));

        const Real den = B0 + eta * (B1 + eta * (B2 + eta * (B3 + eta * (B4 + eta
                    * (B5 + eta * (B6 + eta * (B7 + eta * (B8 + eta * B9))))))));

        return std::sqrt(eta) * (num / den);

    }

    Real bachelierBlackFormulaImpliedVol(Option::Type optionType,
                                   Real strike,
                                   Real forward,
                                   Real tte,
                                   Real bachelierPrice,
                                   Real discount) {

        const static Real SQRT_QL_EPSILON = std::sqrt(QL_EPSILON);

        QL_REQUIRE(tte>0.0,
                   "tte (" << tte << ") must be positive");

        Real forwardPremium = bachelierPrice/discount;

        Real straddlePremium;
        if (optionType==Option::Call){
            straddlePremium = 2.0 * forwardPremium - (forward - strike);
        } else {
            straddlePremium = 2.0 * forwardPremium + (forward - strike);
        }

        Real nu = (forward - strike) / straddlePremium;
        QL_REQUIRE(nu<1.0 || close_enough(nu,1.0),
                   "nu (" << nu << ") must be <= 1.0");
        QL_REQUIRE(nu>-1.0 || close_enough(nu,-1.0),
                     "nu (" << nu << ") must be >= -1.0");

        nu = std::max(-1.0 + QL_EPSILON, std::min(nu,1.0 - QL_EPSILON));

        // nu / arctanh(nu) -> 1 as nu -> 0
        Real eta = (std::fabs(nu) < SQRT_QL_EPSILON) ? 1.0 : nu / boost::math::atanh(nu);

        Real heta = h(eta);

        Real impliedBpvol = std::sqrt(M_PI / (2 * tte)) * straddlePremium * heta;

        return impliedBpvol;
    }

        Real bachelierBlackFormulaStdDevDerivative(Rate strike,
                                      Rate forward,
                                      Real stdDev,
                                      Real discount)
    {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,
                   "discount (" << discount << ") must be positive");

        if (stdDev==0.0)
            return 0.0;

        Real d1 = (forward - strike)/stdDev;
        return discount *
            CumulativeNormalDistribution().derivative(d1);
    }

    Real bachelierBlackFormulaStdDevDerivative(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev,
                        Real discount) {
        return bachelierBlackFormulaStdDevDerivative(payoff->strike(), forward,
                                     stdDev, discount);
    }

    Real bachelierBlackFormulaAssetItmProbability(
                        Option::Type optionType,
                        Real strike,
                        Real forward,
                        Real stdDev) {
        QL_REQUIRE(stdDev>=0.0,
                   "stdDev (" << stdDev << ") must be non-negative");
        Real d = (forward - strike) * Integer(optionType), h = d / stdDev;
        if (stdDev==0.0)
            return std::max(d, 0.0);
        CumulativeNormalDistribution phi;
        Real result = phi(h);
        return result;
    }

    Real bachelierBlackFormulaAssetItmProbability(
                        const ext::shared_ptr<PlainVanillaPayoff>& payoff,
                        Real forward,
                        Real stdDev) {
        return bachelierBlackFormulaAssetItmProbability(payoff->optionType(),
            payoff->strike(), forward, stdDev);
    }

    Real bachelierBlackFormulaDelta(Option::Type optionType,
		                            Real         strike,
                                    Real         forward,
                                    Real         stdDev,
                                    Real         discount) {
        QL_REQUIRE(stdDev>=0.0,   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,  "discount (" << discount << ") must be positive");
        Real d = (forward-strike)*Integer(optionType), h = d/stdDev;
        CumulativeNormalDistribution phi;
		return Integer(optionType)*discount*phi(h);
	}

    Real bachelierBlackFormulaGamma(Option::Type optionType,
		                            Real         strike,
                                    Real         forward,
                                    Real         stdDev,
                                    Real         discount) {
        QL_REQUIRE(stdDev>=0.0,   "stdDev (" << stdDev << ") must be non-negative");
        QL_REQUIRE(discount>0.0,  "discount (" << discount << ") must be positive");
        Real d = (forward-strike)*Integer(optionType), h = d/stdDev;
        CumulativeNormalDistribution phi;
		return discount/stdDev*phi.derivative(h);
	}

}