/**
 * @file BooleanOperation.hpp
 * @author bwu
 * @brief Boolean operation for geometries
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_BOOLEANOPERATION_HPP
#define GENERIC_GEOMETRY_BOOLEANOPERATION_HPP
#include "BoostPolygonRegister.hpp"
#include "GeometryTraits.hpp"
#include <boost/polygon/polygon_set_traits.hpp>
#include <boost/polygon/polygon_set_data.hpp>
namespace generic  {
namespace geometry {
///@brief boolean operations of geometry
namespace boolean  {

template <typename num_type>
using PolygonSet2D = boost::polygon::polygon_set_data<num_type>;

/**
 * @brief Boolean OR (polygon set union) operation of two geometries
 * @param[in] g1 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] g2 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[out] results stl container with Polygon2D that hold the union result of two geometries
 */
template <typename geometry_t1, typename geometry_t2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<geometry_t1>::value &&
                                  traits::is_2d_surf_geom_t<geometry_t2>::value, bool>::type = true>
inline void Unite(const geometry_t1 & g1, const geometry_t2 & g2, container<polygon_type<geometry_t1>, allocator<polygon_type<geometry_t1> > > & results)
{
    using namespace boost::polygon::operators;
    PolygonSet2D<typename geometry_t1::coor_t> polygonSet;
    polygonSet += g1;
    polygonSet += g2;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean OR (polygon set union) operation of a collection of geometries
 * @param[in] begin iterator to the beginning of the geometry collection
 * @param[in] end iterator to the ending of the geometry collection
 * @param[out] results stl container with Polygon2D that hold the union result of the collection of geometries
 */
template <typename iterator,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<
          typename std::iterator_traits<iterator>::value_type>::value, bool>::type = true>
inline void Unite(iterator begin, iterator end,
                  container<polygon_type<typename std::iterator_traits<iterator>::value_type>,
                  allocator<polygon_type<typename std::iterator_traits<iterator>::value_type> > > & results)
{
    using namespace boost::polygon::operators;
    using coor_t = typename std::iterator_traits<iterator>::value_type::coor_t;
    PolygonSet2D<coor_t> polygonSet;
    for(auto iter = begin; iter != end; ++iter)
        polygonSet += *iter;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean AND (polygon set intersection) operation of a two geometries
 * @param[in] g1 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] g2 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[out] results stl container with Polygon2D that hold the intersection result of two geometries
 */
template <typename geometry_t1, typename geometry_t2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<geometry_t1>::value &&
                                  traits::is_2d_surf_geom_t<geometry_t2>::value, bool>::type = true>
inline void Intersect(const geometry_t1 & g1, const geometry_t2 & g2, container<polygon_type<geometry_t1>, allocator<polygon_type<geometry_t1> > > & results)
{
    using namespace boost::polygon::operators;
    PolygonSet2D<typename geometry_t1::coor_t> polygonSet;
    polygonSet += g1;
    polygonSet &= g2;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean AND (polygon set intersection) operation of two geometry sets
 * @param[in] begin1 iterator to the beginning of the geometry collection one
 * @param[in] end1 iterator to the ending of the geometry collection one
 * @param[in] begin2 iterator to the beginning of the geometry collection two
 * @param[in] end2 iterator to the ending of the geometry collection two
 * @param[out] results stl container with Polygon2D that hold the intersection result of two geometry sets
 */
template <typename iterator1, typename iterator2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator1>::value_type>::value &&
                                  traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator2>::value_type>::value, bool>::type = true>
inline void Intersect(iterator1 begin1, iterator1 end1, iterator2 begin2, iterator2 end2,
                      container<polygon_type<typename std::iterator_traits<iterator1>::value_type>,
                      allocator<polygon_type<typename std::iterator_traits<iterator1>::value_type> > > & results)
{
    using namespace boost::polygon::operators;
    using coor_t = typename std::iterator_traits<iterator1>::value_type::coor_t;
    PolygonSet2D<coor_t> polygonSet1, polygonSet2;
    for(auto iter1 = begin1; iter1 != end1; ++iter1)
        polygonSet1 += *iter1;
    for(auto iter2 = begin2; iter2 != end2; ++iter2)
        polygonSet2 += *iter2;
    polygonSet1 *= polygonSet2;
    results.clear();
    polygonSet1.get(results);
}

/**
 * @brief Boolean SUBTRACT operation (polygon set difference) of two geometries
 * @param[in] g1 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] g2 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[out] results stl container with Polygon2D that hold the difference result of two geometries
 */
template <typename geometry_t1, typename geometry_t2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<geometry_t1>::value &&
                                  traits::is_2d_surf_geom_t<geometry_t2>::value, bool>::type = true>
inline void Subtract(const geometry_t1 & g1, const geometry_t2 & g2, container<polygon_type<geometry_t1>, allocator<polygon_type<geometry_t1> > > & results)
{
    using namespace boost::polygon::operators;
    PolygonSet2D<typename geometry_t1::coor_t> polygonSet;
    polygonSet += g1;
    polygonSet -= g2;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean SUBTRACT operation (polygon set difference) of two geometry sets
 * @param[in] begin1 iterator to the beginning of the geometry collection one
 * @param[in] end1 iterator to the ending of the geometry collection one
 * @param[in] begin2 iterator to the beginning of the geometry collection two
 * @param[in] end2 iterator to the ending of the geometry collection two
 * @param[out] results stl container with Polygon2D that hold the difference result of two geometry sets
 */
template <typename iterator1, typename iterator2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator1>::value_type>::value &&
                                  traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator2>::value_type>::value, bool>::type = true>
inline void Subtract(iterator1 begin1, iterator1 end1, iterator2 begin2, iterator2 end2,
                     container<polygon_type<typename std::iterator_traits<iterator1>::value_type>,
                     allocator<polygon_type<typename std::iterator_traits<iterator1>::value_type> > > & results)
{
    using namespace boost::polygon::operators;
    using coor_t = typename std::iterator_traits<iterator1>::value_type::coor_t;
    PolygonSet2D<coor_t> polygonSet;
    for(auto iter = begin1; iter != end1; ++iter)
        polygonSet += *iter;
    for(auto iter = begin2; iter != end2; ++iter)
        polygonSet -= *iter;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean XOR operation (polygon set disjoint-union) of two geometries
 * @param[in] g1 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[in] g2 one of the input geometries, could be one of Triangle2D, Box2D, Polygon2D, PolygonWithHoles2D
 * @param[out] results stl container with Polygon2D that hold the disjoint-union result of two geometries
 */
template <typename geometry_t1, typename geometry_t2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<geometry_t1>::value &&
                                  traits::is_2d_surf_geom_t<geometry_t2>::value, bool>::type = true>
inline void Xor(const geometry_t1 & g1, const geometry_t2 & g2, container<polygon_type<geometry_t1>, allocator<polygon_type<geometry_t1> > > & results)
{
    using namespace boost::polygon::operators;
    PolygonSet2D<typename geometry_t1::coor_t> polygonSet;
    polygonSet += g1;
    polygonSet ^= g2;
    results.clear();
    polygonSet.get(results);
}

/**
 * @brief Boolean XOR operation (polygon set disjoint-union) of two geometry sets
 * @param[in] begin1 iterator to the beginning of the geometry collection one
 * @param[in] end1 iterator to the ending of the geometry collection one
 * @param[in] begin2 iterator to the beginning of the geometry collection two
 * @param[in] end2 iterator to the ending of the geometry collection two
 * @param[out] results stl container with Polygon2D that hold the disjoint-union result of two geometry sets
 */
template <typename iterator1, typename iterator2,
          template <typename, typename> class container,
          template <typename> class allocator = std::allocator,
          typename std::enable_if<traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator1>::value_type>::value &&
                                  traits::is_2d_surf_geom_t<typename std::iterator_traits<iterator2>::value_type>::value, bool>::type = true>
inline void Xor(iterator1 begin1, iterator1 end1, iterator2 begin2, iterator2 end2,
                container<polygon_type<typename std::iterator_traits<iterator1>::value_type>,
                allocator<polygon_type<typename std::iterator_traits<iterator1>::value_type> > > & results)
{
    using namespace boost::polygon::operators;
    using coor_t = typename std::iterator_traits<iterator1>::value_type::coor_t;
    PolygonSet2D<coor_t> polygonSet1, polygonSet2;
    for(auto iter1 = begin1; iter1 != end1; ++iter1)
        polygonSet1 += *iter1;
    for(auto iter2 = begin2; iter2 != end2; ++iter2)
        polygonSet2 += *iter2;
    polygonSet1 ^= polygonSet2;
    results.clear();
    polygonSet1.get(results);
}

} //namespace boolean
} //namespace geometry
} //namespace generic
#endif//GENERIC_GEOMETRY_BOOLEANOPERATION_HPP


//  (C) Copyright John Maddock 2005.
//  (C) Copyright Henry S. Warren 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_TR1_RANDOM_HPP_INCLUDED
#  define BOOST_TR1_RANDOM_HPP_INCLUDED
#  include <boost/tr1/detail/config.hpp>

#ifdef BOOST_HAS_TR1_RANDOM
#  if defined(BOOST_HAS_INCLUDE_NEXT) && !defined(BOOST_TR1_DISABLE_INCLUDE_NEXT)
#     include_next BOOST_TR1_HEADER(random)
#  else
#     include <boost/tr1/detail/config_all.hpp>
#     include BOOST_TR1_STD_HEADER(BOOST_TR1_PATH(random))
#  endif
#else
// Boost.Random:
#include <boost/random.hpp>
#ifndef __SUNPRO_CC
    // Sunpros linker complains if we so much as include this...
#   include <boost/nondet_random.hpp>
#endif
#include <boost/tr1/detail/functor2iterator.hpp>
#include <boost/type_traits/is_fundamental.hpp>
#include <boost/type_traits/is_same.hpp>

namespace std { namespace tr1{

using ::boost::variate_generator;

template<class UIntType, UIntType a, UIntType c, UIntType m>
class linear_congruential
{
private:
   typedef ::boost::random::linear_congruential<UIntType, a, c, m, 0> impl_type;
public:
   // types
   typedef UIntType result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(UIntType, multiplier = a);
   BOOST_STATIC_CONSTANT(UIntType, increment = c);
   BOOST_STATIC_CONSTANT(UIntType, modulus = m);
   // constructors and member function
   explicit linear_congruential(unsigned long x0 = 1)
      : m_gen(x0){}
   linear_congruential(const linear_congruential& that)
      : m_gen(that.m_gen){}
   template<class Gen> linear_congruential(Gen& g)
   {
      init1(g, ::boost::is_same<Gen,linear_congruential>());
   }
   void seed(unsigned long x0 = 1)
   { m_gen.seed(x0); }
   template<class Gen> void seed(Gen& g)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.min)(); }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.max)(); }
   result_type operator()()
   {
      return m_gen();
   }
   bool operator==(const linear_congruential& that)const
   { return m_gen == that.m_gen; }
   bool operator!=(const linear_congruential& that)const
   { return m_gen != that.m_gen; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os,
             const linear_congruential& lcg)
  {
    return os << lcg.m_gen;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is,
             linear_congruential& lcg)
  {
    return is >> lcg.m_gen;
  }
#endif

private:
   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_gen = g.m_gen;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_gen.seed(static_cast<unsigned long>(g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      //typedef typename Gen::result_type gen_rt;
      boost::tr1_details::functor2iterator<Gen, unsigned long> f1(g), f2;
      m_gen.seed(f1, f2);
   }
   impl_type m_gen;
};

template<class UIntType, int w, int n, int m, int r,
UIntType a, int u, int s, UIntType b, int t, UIntType c, int l>
class mersenne_twister
{
   typedef ::boost::random::mersenne_twister
      <UIntType, w, n, m, r, a, u, s, b, t, c, l, 0> imp_type;
public:
   // types
   typedef UIntType result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(int, word_size = w);
   BOOST_STATIC_CONSTANT(int, state_size = n);
   BOOST_STATIC_CONSTANT(int, shift_size = m);
   BOOST_STATIC_CONSTANT(int, mask_bits = r);
   BOOST_STATIC_CONSTANT(UIntType, parameter_a = a);
   BOOST_STATIC_CONSTANT(int, output_u = u);
   BOOST_STATIC_CONSTANT(int, output_s = s);
   BOOST_STATIC_CONSTANT(UIntType, output_b = b);
   BOOST_STATIC_CONSTANT(int, output_t = t);
   BOOST_STATIC_CONSTANT(UIntType, output_c = c);
   BOOST_STATIC_CONSTANT(int, output_l = l);
   // constructors and member function
   mersenne_twister(){}
   explicit mersenne_twister(unsigned long value)
      : m_gen(value == 0 ? 5489UL : value){}
   template<class Gen> mersenne_twister(Gen& g)
   {
      init1(g, ::boost::is_same<mersenne_twister,Gen>());
   }
   void seed()
   { m_gen.seed(); }
   void seed(unsigned long value)
   { m_gen.seed(value == 0 ? 5489UL : value); }
   template<class Gen> void seed(Gen& g)
   { init2(g, ::boost::is_fundamental<Gen>()); }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.min)(); }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.max)(); }
   result_type operator()()
   { return m_gen(); }
   bool operator==(const mersenne_twister& that)const
   { return m_gen == that.m_gen; }
   bool operator!=(const mersenne_twister& that)const
   { return m_gen != that.m_gen; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const mersenne_twister& lcg)
   {
      return os << lcg.m_gen;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            mersenne_twister& lcg)
   {
      return is >> lcg.m_gen;
   }
#endif
private:
   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_gen = g.m_gen;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_gen.seed(static_cast<unsigned long>(g == 0 ? 4357UL : g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      m_gen.seed(g);
   }
   imp_type m_gen;
};

template<class IntType, IntType m, int s, int r>
class subtract_with_carry
{
public:
   // types
   typedef IntType result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(IntType, modulus = m);
   BOOST_STATIC_CONSTANT(int, long_lag = r);
   BOOST_STATIC_CONSTANT(int, short_lag = s);

   // constructors and member function
   subtract_with_carry(){}
   explicit subtract_with_carry(unsigned long value)
      : m_gen(value == 0 ? 19780503UL : value){}
   template<class Gen> subtract_with_carry(Gen& g)
   { init1(g, ::boost::is_same<Gen, subtract_with_carry<IntType, m, s, r> >()); }
   void seed(unsigned long value = 19780503ul)
   { m_gen.seed(value == 0 ? 19780503UL : value); }
   template<class Gen> void seed(Gen& g)
   { init2(g, ::boost::is_fundamental<Gen>()); }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.min)(); }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.max)(); }
   result_type operator()()
   { return m_gen(); }
   bool operator==(const subtract_with_carry& that)const
   { return m_gen == that.m_gen; }
   bool operator!=(const subtract_with_carry& that)const
   { return m_gen != that.m_gen; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const subtract_with_carry& lcg)
   {
      return os << lcg.m_gen;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            subtract_with_carry& lcg)
   {
      return is >> lcg.m_gen;
   }
#endif
private:
   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_gen = g.m_gen;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_gen.seed(static_cast<unsigned long>(g == 0 ? 19780503UL : g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      m_gen.seed(g);
   }
   ::boost::random::subtract_with_carry<IntType, m, s, r, 0> m_gen;
};

template<class RealType, int w, int s, int r>
class subtract_with_carry_01
{
public:
   // types
   typedef RealType result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(int, word_size = w);
   BOOST_STATIC_CONSTANT(int, long_lag = r);
   BOOST_STATIC_CONSTANT(int, short_lag = s);

   // constructors and member function
   subtract_with_carry_01(){}
   explicit subtract_with_carry_01(unsigned long value)
      : m_gen(value == 0 ? 19780503UL : value){}
   template<class Gen> subtract_with_carry_01(Gen& g)
   { init1(g, ::boost::is_same<Gen, subtract_with_carry_01<RealType, w, s, r> >()); }
   void seed(unsigned long value = 19780503UL)
   { m_gen.seed(value == 0 ? 19780503UL : value); }
   template<class Gen> void seed(Gen& g)
   { init2(g, ::boost::is_fundamental<Gen>()); }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.min)(); }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return (m_gen.max)(); }
   result_type operator()()
   { return m_gen(); }
   bool operator==(const subtract_with_carry_01& that)const
   { return m_gen == that.m_gen; }
   bool operator!=(const subtract_with_carry_01& that)const
   { return m_gen != that.m_gen; }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const subtract_with_carry_01& lcg)
   {
      return os << lcg.m_gen;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            subtract_with_carry_01& lcg)
   {
      return is >> lcg.m_gen;
   }
#endif
private:
   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_gen = g.m_gen;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_gen.seed(static_cast<unsigned long>(g == 0 ? 19780503UL : g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      //typedef typename Gen::result_type gen_rt;
      boost::tr1_details::functor2iterator<Gen, unsigned long> f1(g), f2;
      m_gen.seed(f1, f2);
   }
   ::boost::random::subtract_with_carry_01<RealType, w, s, r, 0> m_gen;
};

using ::boost::random::discard_block;

template<class UniformRandomNumberGenerator1, int s1, class UniformRandomNumberGenerator2, int s2>
class xor_combine
{
public:
   // types
   typedef UniformRandomNumberGenerator1 base1_type;
   typedef UniformRandomNumberGenerator2 base2_type;
   typedef unsigned long result_type;
   // parameter values
   BOOST_STATIC_CONSTANT(int, shift1 = s1);
   BOOST_STATIC_CONSTANT(int, shift2 = s2);
   // constructors and member function
   xor_combine(){ init_minmax(); }
   xor_combine(const base1_type & rng1, const base2_type & rng2)
      : m_b1(rng1), m_b2(rng2) { init_minmax(); }
   xor_combine(unsigned long s)
      : m_b1(s), m_b2(s+1) { init_minmax(); }
   template<class Gen> xor_combine(Gen& g)
   {
      init_minmax();
      init1(g, ::boost::is_same<Gen, xor_combine<UniformRandomNumberGenerator1, s1, UniformRandomNumberGenerator2, s2> >());
   }
   void seed()
   {
      m_b1.seed();
      m_b2.seed();
   }
   void seed(unsigned long s)
   {
      m_b1.seed(s);
      m_b2.seed(s+1);
   }
   template<class Gen> void seed(Gen& g)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }

   const base1_type& base1() const
   { return m_b1; }
   const base2_type& base2() const
   { return m_b2; }
   result_type min BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return m_min; }
   result_type max BOOST_PREVENT_MACRO_SUBSTITUTION() const
   { return m_max; }
   result_type operator()()
   { return (m_b1() << s1) ^ (m_b2() << s2); }

   bool operator == (const xor_combine& that)const
   { return (m_b1 == that.m_b1) && (m_b2 == that.m_b2); }
   bool operator != (const xor_combine& that)const
   { return !(*this == that); }

#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const xor_combine& lcg)
   {
      return os << lcg.m_b1 << " " << lcg.m_b2;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            xor_combine& lcg)
   {
      return is >> lcg.m_b1 >> lcg.m_b2;
   }
#endif

private:
   void init_minmax();
   base1_type m_b1;
   base2_type m_b2;
   result_type m_min;
   result_type m_max;

   template <class Gen>
   void init1(Gen& g, const ::boost::true_type&)
   {
      m_b1 = g.m_b1;
      m_b2 = g.m_b2;
   }
   template <class Gen>
   void init1(Gen& g, const ::boost::false_type&)
   {
      init2(g, ::boost::is_fundamental<Gen>());
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::true_type&)
   {
      m_b1.seed(static_cast<unsigned long>(g));
      m_b2.seed(static_cast<unsigned long>(g));
   }
   template <class Gen>
   void init2(Gen& g, const ::boost::false_type&)
   {
      m_b1.seed(g);
      m_b2.seed(g);
   }
};

template<class UniformRandomNumberGenerator1, int s1, class UniformRandomNumberGenerator2, int s2>
void xor_combine<UniformRandomNumberGenerator1, s1, UniformRandomNumberGenerator2, s2>::init_minmax()
{
   //
   // The following code is based on that given in "Hacker's Delight"
   // by Henry S. Warren, (Addison-Wesley, 2003), and at
   // http://www.hackersdelight.org/index.htm.
   // Used here by permission.
   //
   // calculation of minimum value:
   //
   result_type a = (m_b1.min)() << s1;
   result_type b = (m_b1.max)() << s1;
   result_type c = (m_b2.min)() << s2;
   result_type d = (m_b2.max)() << s2;
   result_type m, temp;

   m = 0x1uL << ((sizeof(result_type) * CHAR_BIT) - 1);
   while (m != 0) {
      if (~a & c & m) {
         temp = (a | m) & (static_cast<result_type>(0u) - m);
         if (temp <= b) a = temp;
      }
      else if (a & ~c & m) {
         temp = (c | m) & (static_cast<result_type>(0u) - m);
         if (temp <= d) c = temp;
      }
      m >>= 1;
   }
   m_min = a ^ c;

   //
   // calculation of maximum value:
   //
   if((((std::numeric_limits<result_type>::max)() >> s1) < (m_b1.max)())
      || ((((std::numeric_limits<result_type>::max)()) >> s2) < (m_b2.max)()))
   {
      m_max = (std::numeric_limits<result_type>::max)();
      return;
   }
   a = (m_b1.min)() << s1;
   b = (m_b1.max)() << s1;
   c = (m_b2.min)() << s2;
   d = (m_b2.max)() << s2;

   m = 0x1uL << ((sizeof(result_type) * CHAR_BIT) - 1);

   while (m != 0) {
      if (b & d & m) {
         temp = (b - m) | (m - 1);
         if (temp >= a) b = temp;
         else {
            temp = (d - m) | (m - 1);
            if (temp >= c) d = temp;
         }
      }
      m = m >> 1;
   }
   m_max = b ^ d;
}

typedef linear_congruential< ::boost::int32_t, 16807, 0, 2147483647> minstd_rand0;
typedef linear_congruential< ::boost::int32_t, 48271, 0, 2147483647> minstd_rand;
typedef mersenne_twister< ::boost::uint32_t, 32,624,397,31,0x9908b0df,11,7,0x9d2c5680,15,0xefc60000,18> mt19937;
typedef subtract_with_carry_01<float, 24, 10, 24> ranlux_base_01;
typedef subtract_with_carry_01<double, 48, 10, 24> ranlux64_base_01;
typedef discard_block<subtract_with_carry< ::boost::int32_t, (1<<24), 10, 24>, 223, 24> ranlux3;
typedef discard_block<subtract_with_carry< ::boost::int32_t, (1<<24), 10, 24>, 389, 24> ranlux4;
typedef discard_block<subtract_with_carry_01<float, 24, 10, 24>, 223, 24> ranlux3_01;
typedef discard_block<subtract_with_carry_01<float, 24, 10, 24>, 389, 24> ranlux4_01;

#ifndef __SUNPRO_CC
using ::boost::random_device;
#endif
using ::boost::uniform_int;

class bernoulli_distribution
{
public:
   // types
   typedef int input_type;
   typedef bool result_type;
   // constructors and member function
   explicit bernoulli_distribution(double p = 0.5)
      : m_dist(p){}
   double p() const
   { return m_dist.p(); }
   void reset()
   { m_dist.reset(); }
   template<class UniformRandomNumberGenerator>
   result_type operator()(UniformRandomNumberGenerator& urng)
   {
      return m_dist(urng);
   }
#if !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
   template<class CharT, class Traits>
   friend std::basic_ostream<CharT,Traits>&
   operator<<(std::basic_ostream<CharT,Traits>& os,
            const bernoulli_distribution& lcg)
   {
      return os << lcg.m_dist;
   }

   template<class CharT, class Traits>
   friend std::basic_istream<CharT,Traits>&
   operator>>(std::basic_istream<CharT,Traits>& is,
            bernoulli_distribution& lcg)
   {
      return is >> lcg.m_dist;
   }
#endif

private:
   ::boost::bernoulli_distribution<double> m_dist;
};
//using ::boost::bernoulli_distribution;
using ::boost::geometric_distribution;
using ::boost::poisson_distribution;
using ::boost::binomial_distribution;
using ::boost::uniform_real;
using ::boost::exponential_distribution;
using ::boost::normal_distribution;
using ::boost::gamma_distribution;

} }

#endif

#endif


#include <sm/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>

namespace sm {

namespace random {

class Random
{
 public:
  // http://www.boost.org/doc/libs/1_49_0/doc/html/boost_random/reference.html#boost_random.reference.generators
  typedef boost::mt19937 base_generator_type;

  base_generator_type _generator;
  // Define a uniform random number distribution which produces "double"
  // values between 0 and 1 (0 inclusive, 1 exclusive).
  boost::uniform_real<> _uni_dist;
  boost::variate_generator<base_generator_type&, boost::uniform_real<> > _uniform;
  boost::normal_distribution<> _normal_dist;
  boost::variate_generator<base_generator_type&, boost::normal_distribution<> > _normal;

  Random() : _uni_dist(0.0,1.0), _uniform(_generator, _uni_dist), _normal_dist(), _normal(_generator, _normal_dist)
  {
	// 0
  }

  double normal()
  {
	return _normal();
  }

  double uniform()
  {
	return _uniform();
  }

  void seed(boost::uint64_t s)
  {
    _normal.engine().seed(s);
    _normal.distribution().reset();
    _uniform.engine().seed(s);
    _uniform.distribution().reset();
  }

  static Random & instance()
  {
	static Random random;
	return random;
  }
};

double normal()
{
  return Random::instance().normal();
}

double randn()
{
  return Random::instance().normal();
}

double uniform()
{
  return Random::instance().uniform();
}

double rand()
{
  return Random::instance().uniform();
}

double randLU(double lowerBoundInclusive, double upperBoundExclusive)
{
  return Random::instance().uniform() * (upperBoundExclusive - lowerBoundInclusive) + lowerBoundInclusive;
}

int randLUi(int lowerBoundInclusive, int upperBoundExclusive)
{
  return (int)floor(Random::instance().uniform() * (upperBoundExclusive - lowerBoundInclusive)) + lowerBoundInclusive;
}

void seed(boost::uint64_t s)
{
  Random::instance().seed(s);
}

} // namespace random

} // namespace sm


/* compile with g++ ... -l arprec */

#include <iostream>
#include <fstream>
#include <arprec/mp_real.h>
#include <arprec/mp_complex.h>
#include <complex>
#include <boost/numeric/mtl/mtl.hpp>
#include <cassert>
#include <boost/numeric/linear_algebra/identity.hpp>

using namespace std;
typedef mtl::dense2D<mp_complex> mat_c;

ostream& operator<< (ostream& os, const mp_complex& c) {
	return os << "(" << c.real << ", " << c.imag << ")";
}

int main() {

	mp::mp_init(102);
	cout.precision(30);

	mp_real x1,x2,x3,x4,x5,x6;
	x1 = mp_real("2e18");
	x2 = mp_real("-1.34");
	x3 = mp_real("1.6");
	x4 = mp_real(3);
	x5 = mp_real("1.83e7");
	x6 = mp_real("-4.234");
	cout << "x1 = " << x1 << "\n";
	cout << "x2 = " << x2 << "\n";
	cout << "x3 = " << x3 << "\n";
	cout << "x4 = " << x4 << "\n";
	cout << "x5 = " << x5 << "\n";
	cout << "x6 = " << x6 << "\n";
	cout << "---------------\n";

	mp_complex z1,z2,z3,z4,z5,z6;
	z1 = mp_complex("4e30","3e30");
	z2 = mp_complex("3","-3.5");
	z3 = z1+z2;
	z4 = sqrt(mp_complex("-4","0"));
	z5 = z2+z4;
	z6 = z2-z3+z1+z4;
	cout << "z1 = " << z1 << "\n";
	cout << "z2 = " << z2 << "\n";
	cout << "z3 = " << z3 << "\n";
	cout << "z4 = " << z4 << "\n";
	cout << "z5 = " << z5 << "\n";
	cout << "z6 = " << z6 << "\n";
	cout << "---------------\n";
	{
	mtl::dense2D<mp_real> A(2,2),B(2,2),C(2,2),D(2,2);
	mtl::dense_vector<mp_real> b(2),x(2),r(2);
	A = x1,x2,
	    x3,x4;
	b = x5,x6;
	cout << "A = \n" << A;
	cout << "b = " << b << endl;

	// A = A*A;	// Laufzeitfehler
	A*=A;
	cout << "A = \n" << A;
	cout << "A*A = \n" << A*A;
	B =A*A;
	cout << "B = \n" << B;
	C = trans(A);
	cout << "C = \n" << C;
	D = A;

	mtl::dense_vector<int> p(2);
	lu(A,p);
	x = lu_apply(A,p,b);
	r = D*x-b;
	cout << "Ax-b = " << r << endl;
	cout << "---------------\n";
	}

	{
	mtl::dense2D<mp_complex> A(2,2),B(2,2),C(2,2),D(2,2);
	mtl::dense_vector<mp_complex> b(2),x(2),r(2);
 	A = z1,z2,
 	    z3,z4;

	b = z5,z6;
	cout << "A = \n" << A;
	cout << "b = " << b << endl;

	// A = A*A;	// Compilezeitfehler
	// A*=A;	// Compilezeitfehler
	B =A*A;	// Compilezeitfehler
	C = trans(A);	// Compilezeitfehler

	D = A;

	mtl::dense_vector<int> p(2);
	lu(A,p);	// Compilezeitfehler
	x = lu_apply(A,p,b);
	r = D*x-b;
 	cout << "Ax-b = " << r << endl;
	cout << "---------------\n";

	cout << "abs(A[0][0]) is " << abs(A[0][0]) << '\n';
	}

	mp::mp_finalize();

}


#pragma once

#include <nlohmann/json.hpp>
#include <Mahi/FesExo/Kernels.hpp>
#include <Eigen/Dense>

template <typename T>
class FesGprModel
{
public:
    /// constructor
    FesGprModel(std::string json_filename);
    ~FesGprModel();
    std::string name; // name of the model detailing muscle and joint
    /// prediction using the kernel of the template type. K * alpha + [1 x_new] * beta
    double predict(const std::vector<double>& predict_point);
private:
    T m_kernel;                     // kernel struct to use of type T (template) instantiated with m_theta and m_train_inputs
    std::vector<std::vector<double>> m_train_inputs; // training inputs for prediction
    std::vector<double> m_alpha;        // value of alpha for prediction
    std::vector<double> m_beta;         // value of beta for prediction
    std::vector<double> m_theta;        // value of theta for the kernel
    nlohmann::json m_js;            // json object to read from
    std::string m_json_filename;    // filename of the json with parameters

    /// pulls relevant info from json file m_json_filename and populates member variables parameters
    void populate_parameters();
};


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110111    E. Iorfida        File created.
 *                                  The code is tested with the function: f(x)= x^2 - 3.
 *      110111    K. Kumar          Updated to use address of global  functions instead of
 *                                  pointers for set functions; aligned code as required for
 *                                  namespaces; minor comment changes.
 *      110119    K. Kumar          Updated code to work with adaptor and abstract base
 *                                  implementation so that pointer-to-member functions are not
 *                                  required; filename changed; added cerr statements.
 *      110120    E. Iorfida        Added necessary class that contains functions, and related
 *                                  code, to allow a directly test with adaptor.
 *      110120    K. Kumar          Added global functions test; updated comments; modified layout.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120712    P. Musegaas       Changed absolute tolerance into a safe variant of relative
 *                                  tolerance. Added new unit tests for this.
 *      120318    S. Billemont      Move to new root_finders codebase.
 *      120402    T. Secretin       Code-check.
 *      120810    P. Musegaas       Code-check. Merged two branches. Various edits.
 *      150417    D. Dirkx          Made modifications for templated root finding.
 *
 *    References
 *
 *    Notes
 *      The test cases in this unit test should be more extensive.
 *
 */

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/RootFinders/newtonRaphson.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction1.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction2.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction3.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunctionWithLargeRootDifference.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunctionWithZeroRoot.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( testsuite_rootfinders )

using namespace tudat;
using namespace root_finders;
using namespace root_finders::termination_conditions;

//! Check if Newton-Raphson converges on test function #1 (TestFunction1).
BOOST_AUTO_TEST_CASE( test_newtonRaphson_testFunction1 )
{
    // Create object containing the test functions.
    boost::shared_ptr< TestFunction1 > testFunction = boost::make_shared< TestFunction1 >( 1 );

    // The termination condition.
    NewtonRaphson::TerminationFunction terminationConditionFunction =
            boost::bind( &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                         boost::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                             testFunction->getTrueRootAccuracy( ) ), _1, _2, _3, _4, _5 );

    // Test Newton-Raphson object.
    NewtonRaphson newtonRaphson( terminationConditionFunction );

    // Let Newton-Raphson search for the root.
    const double root = newtonRaphson.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Newton-Raphson converges on test function #2 (TestFunction2).
BOOST_AUTO_TEST_CASE( test_newtonRaphson_testFunction2 )
{
    // Create object containing the test functions.
    boost::shared_ptr< TestFunction2 > testFunction = boost::make_shared< TestFunction2 >( 1 );

    // The termination condition.
    NewtonRaphson::TerminationFunction terminationConditionFunction =
            boost::bind( &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                         boost::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                             testFunction->getTrueRootAccuracy( ) ), _1, _2, _3, _4, _5 );

    // Test Newton-Raphson object.
    NewtonRaphson newtonRaphson( terminationConditionFunction );

    // Let Newton-Raphson search for the root.
    const double root = newtonRaphson.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Newton-Raphson converges on test function #3 (TestFunction3).
BOOST_AUTO_TEST_CASE( test_newtonRaphson_testFunction3 )
{
    // Create object containing the test functions.
    boost::shared_ptr< TestFunction3 > testFunction = boost::make_shared< TestFunction3 >( 1 );

    // The termination condition.
    NewtonRaphson::TerminationFunction terminationConditionFunction =
            boost::bind( &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                         boost::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                             testFunction->getTrueRootAccuracy( ) ), _1, _2, _3, _4, _5 );

    // Test Newton-Raphson object.
    NewtonRaphson newtonRaphson( terminationConditionFunction );

    // Let Newton-Raphson search for the root.
    const double root = newtonRaphson.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Newton-Raphson converges on function with large root difference
//! (testFunctionWithLargeRootDifference).
// Not the best test case. Inheritance from old code.
BOOST_AUTO_TEST_CASE( test_newtonRaphson_testFunctionWithLargeRootDifference )
{
    // Declare tolerance.
    const double tolerance = 1.0e-10;

    // Declare expected roots.
    const double expectedRootLowCase = 1.00000000793634;
    const double expectedRootHighCase = 7937.3386333591;

    // Create objects containing the test functions. Values were obtained during a limit case
    // gravity assist calculation (while evaluating Cassini-1 trajectory).
    boost::shared_ptr< TestFunctionWithLargeRootDifference > testFunctionLowCase =
            boost::make_shared< TestFunctionWithLargeRootDifference >
            ( 1, -3.24859999867635e18, -3248600.0, 1.5707963267949 );
    boost::shared_ptr< TestFunctionWithLargeRootDifference > testFunctionHighCase =
            boost::make_shared< TestFunctionWithLargeRootDifference >
            ( 1, -3248600.0, -3.24859999867635e18, 1.5707963267949 );

    // The termination condition.
    NewtonRaphson::TerminationFunction terminationConditionFunction
            = boost::bind( &RootRelativeToleranceTerminationCondition< >::checkTerminationCondition,
                           boost::make_shared< RootRelativeToleranceTerminationCondition< > >(
                               1.0e-10 ), _1, _2, _3, _4, _5 );

    // Make the Newton-Raphson object.
    NewtonRaphson newtonRaphson( terminationConditionFunction );

    // Let Newton-Raphson search for the root for both cases.
    double rootLowCase = newtonRaphson.execute( testFunctionLowCase, 1.0 + 1.0e-10 );
    double rootHighCase = newtonRaphson.execute( testFunctionHighCase, 1.0 + 1.0e-2 );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( rootLowCase, expectedRootLowCase, tolerance );
    BOOST_CHECK_CLOSE_FRACTION( rootHighCase, expectedRootHighCase, tolerance );
}

//! Check if Newton-Raphson converges on function with zero root (testFunctionWithZeroRoot).
// Not the best test case. Inheritance from old code. Not really relevant anymore. The basic
// idea is that Newton-Raphson should work for both a function that becomes zero, as well as for a
// function that does not become zero. A better case should be written.
BOOST_AUTO_TEST_CASE( test_newtonRaphson_testFunctionWithZeroRoot )
{
    // Create object containing the test functions.
    boost::shared_ptr< TestFunctionWithZeroRoot > testFunction =
            boost::make_shared< TestFunctionWithZeroRoot >( 1 );

    // The termination condition.
    NewtonRaphson::TerminationFunction terminationConditionFunction
            = boost::bind(
                &RootAbsoluteOrRelativeToleranceTerminationCondition< >::
                checkTerminationCondition,
                boost::make_shared< RootAbsoluteOrRelativeToleranceTerminationCondition< > >(
                    1.0e-308, 1.0e-15 ), _1, _2, _3, _4, _5 );

    // Test Newton-Raphson object.
    NewtonRaphson newtonRaphson( terminationConditionFunction );

    // Let Newton-Raphson search for the root.
    const double root = newtonRaphson.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_SMALL( root, 1.0e-150 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

BOOST_AUTO_TEST_SUITE_END( ) // testsuite_rootfinders

} // namespace unit_tests
} // namespace tudat


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/test/unit_test.hpp>
#include <oret/toplevelfixture.hpp>

#include <boost/algorithm/string/replace.hpp>
#include <boost/make_shared.hpp>

#include <ql/currencies/america.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/termstructures/volatility/equityfx/blackvariancecurve.hpp>
#include <ql/termstructures/yield/flatforward.hpp>

#include <qle/termstructures/pricecurve.hpp>

#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/commodityoption.hpp>
#include <ored/portfolio/commodityoption.hpp>
#include <ored/portfolio/portfolio.hpp>

using namespace std;
using namespace boost::unit_test_framework;
using namespace boost::algorithm;
using namespace QuantLib;
using namespace QuantExt;
using namespace ore::data;

namespace {

// testTolerance for Real comparison
Real testTolerance = 1e-10;

// Cached prices of call and put
// Satisfies put call parity: call - put = (1348 - 1340) * 0.990049833749 * 100
Real cachedCallPrice = 5711.6321329012244;
Real cachedPutPrice = 4919.5922659018906;

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        // Reference date and common day counter
        asof_ = Date(19, Feb, 2018);
        Actual365Fixed dayCounter;

        // Add USD discount curve
        Handle<YieldTermStructure> discount(boost::make_shared<FlatForward>(asof_, 0.01, dayCounter));
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "USD")] = discount;

        // Add GOLD_USD price curve
        vector<Date> dates = {asof_, Date(19, Feb, 2019)};
        vector<Real> prices = {1346.0, 1348.0};
        Handle<PriceTermStructure> priceCurve(
            boost::make_shared<InterpolatedPriceCurve<Linear>>(asof_, dates, prices, dayCounter, USDCurrency()));
        commodityCurves_[make_pair(Market::defaultConfiguration, "GOLD_USD")] = priceCurve;

        // Add GOLD_USD volatilities
        vector<Date> volatilityDates = {Date(19, Feb, 2019), Date(19, Feb, 2020)};
        vector<Real> volatilities = {0.10, 0.11};
        Handle<BlackVolTermStructure> volatility(
            boost::make_shared<BlackVarianceCurve>(asof_, volatilityDates, volatilities, dayCounter));
        commodityVols_[make_pair(Market::defaultConfiguration, "GOLD_USD")] = volatility;
    }
};

class CommonData {
public:
    Envelope envelope;
    string commodityName;
    string currency;
    Real quantity;
    Real strike;
    bool payOffAtExpiry;
    vector<string> expiry;
    Date expiryDate;

    boost::shared_ptr<Market> market;
    boost::shared_ptr<EngineFactory> engineFactory;

    CommonData()
        : commodityName("GOLD_USD"), currency("USD"), quantity(100), strike(1340.0), payOffAtExpiry(false),
          expiry(1, "2019-02-19"), expiryDate(19, Feb, 2019) {

        // Create engine factory
        market = boost::make_shared<TestMarket>();
        boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
        engineData->model("CommodityOption") = "BlackScholes";
        engineData->engine("CommodityOption") = "AnalyticEuropeanEngine";
        engineFactory = boost::make_shared<EngineFactory>(engineData, market);

        // Set evaluation date
        Settings::instance().evaluationDate() = market->asofDate();
    }
};

} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CommodityOptionTests)

BOOST_AUTO_TEST_CASE(testCommodityOptionTradeBuilding) {

    BOOST_TEST_MESSAGE("Testing commodity option trade building");

    // Common test data and setup
    CommonData td;

    // Test the building of a commodity option doesn't throw
    OptionData optionData("Long", "Call", "European", td.payOffAtExpiry, td.expiry);
    boost::shared_ptr<CommodityOption> option = boost::make_shared<CommodityOption>(
        td.envelope, optionData, td.commodityName, td.currency, td.strike, td.quantity);
    BOOST_CHECK_NO_THROW(option->build(td.engineFactory));

    // Check the underlying instrument was built as expected
    boost::shared_ptr<Instrument> qlInstrument = option->instrument()->qlInstrument();

    boost::shared_ptr<VanillaOption> vanillaOption = boost::dynamic_pointer_cast<VanillaOption>(qlInstrument);
    BOOST_CHECK(vanillaOption);
    BOOST_CHECK_EQUAL(vanillaOption->exercise()->type(), Exercise::Type::European);
    BOOST_CHECK_EQUAL(vanillaOption->exercise()->dates().size(), 1);
    BOOST_CHECK_EQUAL(vanillaOption->exercise()->dates()[0], td.expiryDate);

    boost::shared_ptr<TypePayoff> payoff = boost::dynamic_pointer_cast<TypePayoff>(vanillaOption->payoff());
    BOOST_CHECK(payoff);
    BOOST_CHECK_EQUAL(payoff->optionType(), Option::Type::Call);

    // Calculate the expected price and check against cached price
    // This is an extra check of the market etc.
    // Know it is then safe to use the cached price elsewhere in this suite
    Real forwardPrice = td.market->commodityPriceCurve(td.commodityName)->price(td.expiryDate);
    DiscountFactor discount = td.market->discountCurve(td.currency)->discount(td.expiryDate);
    Real variance = td.market->commodityVolatility(td.commodityName)->blackVariance(td.expiryDate, td.strike);
    Real expectedPrice =
        td.quantity * blackFormula(Option::Type::Call, td.strike, forwardPrice, sqrt(variance), discount);
    BOOST_CHECK_CLOSE(expectedPrice, cachedCallPrice, testTolerance);

    // Check the price
    BOOST_CHECK_CLOSE(option->instrument()->NPV(), expectedPrice, testTolerance);
}

BOOST_AUTO_TEST_CASE(testCommodityOptionFromXml) {

    BOOST_TEST_MESSAGE("Testing parsing of commodity option trade from XML");

    // Common test data and setup
    CommonData td;

    // Create an XML string representation of the trade
    string tradeXml;
    tradeXml.append("<Portfolio>");
    tradeXml.append("  <Trade id=\"CommodityOption_Gold\">");
    tradeXml.append("    <TradeType>CommodityOption</TradeType>");
    tradeXml.append("    <Envelope>");
    tradeXml.append("      <CounterParty>CPTY_A</CounterParty>");
    tradeXml.append("      <NettingSetId>CPTY_A</NettingSetId>");
    tradeXml.append("      <AdditionalFields/>");
    tradeXml.append("    </Envelope>");
    tradeXml.append("    <CommodityOptionData>");
    tradeXml.append("      <OptionData>");
    tradeXml.append("        <LongShort>Long</LongShort>");
    tradeXml.append("        <OptionType>Call</OptionType>");
    tradeXml.append("        <Style>European</Style>");
    tradeXml.append("        <Settlement>Cash</Settlement>");
    tradeXml.append("        <PayOffAtExpiry>false</PayOffAtExpiry>");
    tradeXml.append("        <ExerciseDates>");
    tradeXml.append("          <ExerciseDate>2019-02-19</ExerciseDate>");
    tradeXml.append("        </ExerciseDates>");
    tradeXml.append("      </OptionData>");
    tradeXml.append("      <Name>GOLD_USD</Name>");
    tradeXml.append("      <Currency>USD</Currency>");
    tradeXml.append("      <Strike>1340</Strike>");
    tradeXml.append("      <Quantity>100</Quantity>");
    tradeXml.append("    </CommodityOptionData>");
    tradeXml.append("  </Trade>");
    tradeXml.append("</Portfolio>");

    // Load portfolio from XML string
    Portfolio portfolio;
    portfolio.loadFromXMLString(tradeXml);

    // Extract CommodityOption trade from portfolio
    boost::shared_ptr<Trade> trade = portfolio.trades()[0];
    boost::shared_ptr<CommodityOption> option = boost::dynamic_pointer_cast<ore::data::CommodityOption>(trade);

    // Check fields after checking that the cast was successful
    BOOST_CHECK(option);
    BOOST_CHECK_EQUAL(option->tradeType(), "CommodityOption");
    BOOST_CHECK_EQUAL(option->id(), "CommodityOption_Gold");
    BOOST_CHECK_EQUAL(option->asset(), "GOLD_USD");
    BOOST_CHECK_EQUAL(option->currency(), "USD");
    BOOST_CHECK_CLOSE(option->strike(), 1340, testTolerance);
    BOOST_CHECK_CLOSE(option->quantity(), 100, testTolerance);
    BOOST_CHECK_EQUAL(option->option().longShort(), "Long");
    BOOST_CHECK_EQUAL(option->option().callPut(), "Call");
    BOOST_CHECK_EQUAL(option->option().style(), "European");
    BOOST_CHECK_EQUAL(option->option().exerciseDates().size(), 1);
    BOOST_CHECK_EQUAL(option->option().exerciseDates()[0], "2019-02-19");

    // Build the option and check the price
    BOOST_CHECK_NO_THROW(option->build(td.engineFactory));
    BOOST_CHECK_CLOSE(option->instrument()->NPV(), cachedCallPrice, testTolerance);

    // Make trade short call and test price is negated
    replace_all(tradeXml, ">Long<", ">Short<");
    portfolio.clear();
    portfolio.loadFromXMLString(tradeXml);
    trade = portfolio.trades()[0];
    BOOST_CHECK_NO_THROW(trade->build(td.engineFactory));
    BOOST_CHECK_CLOSE(trade->instrument()->NPV(), -cachedCallPrice, testTolerance);

    // Make trade short put and test price
    replace_all(tradeXml, ">Call<", ">Put<");
    portfolio.clear();
    portfolio.loadFromXMLString(tradeXml);
    trade = portfolio.trades()[0];
    BOOST_CHECK_NO_THROW(trade->build(td.engineFactory));
    BOOST_CHECK_CLOSE(trade->instrument()->NPV(), -cachedPutPrice, testTolerance);

    // Make trade long put and test price
    replace_all(tradeXml, ">Short<", ">Long<");
    portfolio.clear();
    portfolio.loadFromXMLString(tradeXml);
    trade = portfolio.trades()[0];
    BOOST_CHECK_NO_THROW(trade->build(td.engineFactory));
    BOOST_CHECK_CLOSE(trade->instrument()->NPV(), cachedPutPrice, testTolerance);
}

BOOST_AUTO_TEST_CASE(testLongShortCallPutPrices) {

    BOOST_TEST_MESSAGE("Testing commodity option prices");

    // Common test data and setup
    CommonData td;

    // Option
    boost::shared_ptr<CommodityOption> option;

    // Long call
    OptionData optionData("Long", "Call", "European", td.payOffAtExpiry, td.expiry);
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, td.commodityName, td.currency, td.strike,
                                                 td.quantity);
    option->build(td.engineFactory);
    BOOST_CHECK_CLOSE(option->instrument()->NPV(), cachedCallPrice, testTolerance);

    // Short call
    optionData = OptionData("Short", "Call", "European", td.payOffAtExpiry, td.expiry);
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, td.commodityName, td.currency, td.strike,
                                                 td.quantity);
    option->build(td.engineFactory);
    BOOST_CHECK_CLOSE(option->instrument()->NPV(), -cachedCallPrice, testTolerance);

    // Long put
    optionData = OptionData("Long", "Put", "European", td.payOffAtExpiry, td.expiry);
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, td.commodityName, td.currency, td.strike,
                                                 td.quantity);
    option->build(td.engineFactory);
    BOOST_CHECK_CLOSE(option->instrument()->NPV(), cachedPutPrice, testTolerance);

    // Short put
    optionData = OptionData("Short", "Put", "European", td.payOffAtExpiry, td.expiry);
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, td.commodityName, td.currency, td.strike,
                                                 td.quantity);
    option->build(td.engineFactory);
    BOOST_CHECK_CLOSE(option->instrument()->NPV(), -cachedPutPrice, testTolerance);
}

BOOST_AUTO_TEST_CASE(testCommodityOptionBuildExceptions) {

    BOOST_TEST_MESSAGE("Testing commodity option exceptions during building");

    // Common test data and setup
    CommonData td;

    // Option
    boost::shared_ptr<CommodityOption> option;

    // Negative strike throws
    OptionData optionData("Long", "Call", "European", td.payOffAtExpiry, td.expiry);
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, td.commodityName, td.currency, -td.strike,
                                                 td.quantity);
    BOOST_CHECK_THROW(option->build(td.engineFactory), Error);

    // Negative quantity throws
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, td.commodityName, td.currency, td.strike,
                                                 -td.quantity);
    BOOST_CHECK_THROW(option->build(td.engineFactory), Error);

    // Name of commodity with no market data throws
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, "GOLD_USD_MISSING", td.currency, td.strike,
                                                 td.quantity);
    BOOST_CHECK_THROW(option->build(td.engineFactory), Error);

    // Non-European OptionData style throws
    optionData = OptionData("Long", "Call", "American", td.payOffAtExpiry, td.expiry);
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, td.commodityName, td.currency, td.strike,
                                                 td.quantity);
    BOOST_CHECK_THROW(option->build(td.engineFactory), Error);

    // More than one expiry date throws
    vector<string> extraExpiries = td.expiry;
    extraExpiries.push_back("2019-08-19");
    optionData = OptionData("Long", "Call", "European", td.payOffAtExpiry, extraExpiries);
    option = boost::make_shared<CommodityOption>(td.envelope, optionData, td.commodityName, td.currency, td.strike,
                                                 td.quantity);
    BOOST_CHECK_THROW(option->build(td.engineFactory), Error);
}

BOOST_AUTO_TEST_CASE(testCommodityOptionPremium) {

    BOOST_TEST_MESSAGE("Testing commodity option premium works");

    // Common test data and setup
    CommonData td;

    // Premium
    Real premium = 5000;
    Date premiumDate(21, Feb, 2018);

    // Create option
    OptionData optionData("Long", "Call", "European", td.payOffAtExpiry, td.expiry, "Cash", "", premium, td.currency,
                          "2018-02-21");
    boost::shared_ptr<CommodityOption> option = boost::make_shared<CommodityOption>(
        td.envelope, optionData, td.commodityName, td.currency, td.strike, td.quantity);

    // Test building succeeds
    BOOST_CHECK_NO_THROW(option->build(td.engineFactory));

    // Test that price is correct
    DiscountFactor premiumDiscount = td.market->discountCurve(td.currency)->discount(premiumDate);
    BOOST_CHECK_CLOSE(option->instrument()->NPV(), cachedCallPrice - premiumDiscount * premium, testTolerance);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//#include <dsp++/fixed.h>
//#include <boost/static_assert.hpp>
//#include <climits>
//
//using namespace dsp::fi;
//
//typedef fixed<16, 0> Q15_t;
//BOOST_STATIC_ASSERT(Q15_t::fractional_bits == 15);
//fixed<8, 3> fff = fixed<8,3>::value_of<dsp::rounding::nearest, dsp::overflow::saturate>(-0.33333f);
//Q15_t v = Q15_t::value_of<dsp::rounding::nearest, dsp::overflow::saturate>(0.33333);
//Q15_t v1(2048, raw);
//bool ff = (v == v1);
//
//
//static bool test1() {
//	fixed<32, 12> res = multiply_lossless(fff, v);
//	Q15_t rrr = fixed_cast<Q15_t, dsp::rounding::nearest, dsp::overflow::exception>(res);
//	rrr = fixed_cast<Q15_t, dsp::rounding::nearest>(res);
//	rrr = fixed_cast<16,0,true>(res);
//	rrr = fixed_cast<16,0,true, dsp::rounding::negative>(res);
//
//	rrr -= v;
//
//
//	float ff = float_cast<float>(res);
//	float ffff = float_cast<float>(rrr);
//
//////	fixed<32, 12> r0 = res.round<dsp::rounding::fastest>(5);
////	float f0 = float_cast<float>(r0);
//////	fixed<32, 12> r1 = res.round<dsp::rounding::nearest>(5);
////	float f1 = float_cast<float>(r1);
////	fixed<32, 12> r2 = res.round<dsp::rounding::positive>(5);
////	float f2 = float_cast<float>(r2);
////	fixed<32, 12> r3 = res.round<dsp::rounding::negative>(5);
////	float f3 = float_cast<float>(r3);
//
//	float fv1 = float_cast<float>(v1);
//	fv1 *= 0.33333f;
//
//	fixed<16, 0> q = v * v1;
//	float fq = float_cast<float>(q);
//	q *= fixed<16, 3>::value_of<dsp::rounding::truncated, dsp::overflow::saturate>(-5.f);
//	fv1 *= -5.f;
//	fq = float_cast<float>(q);
//	return true;
//}
//
//bool val = test1();
//
//static bool test2() {
////	for (short i = SHRT_MIN; i < SHRT_MAX; ++i) {
////		short s0 = dsp::round<dsp::rounding::truncated, dsp::overflow::fastest>(i, 5);
////		short s1 = dsp::round<dsp::rounding::nearest, dsp::overflow::exception>(i, 5);
////		short s2 = dsp::round<dsp::rounding::positive, dsp::overflow::saturate>(i, 5);
////		short s3 = dsp::round<dsp::rounding::negative, dsp::overflow::wrap>(i, 5);
////	}
//	return true;
//}
//
//bool vvv = test2();


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_REMEZ_HPP
#define BOOST_MATH_TOOLS_REMEZ_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/solve.hpp>
#include <boost/math/tools/minima.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/function/function1.hpp>
#include <boost/scoped_array.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost{ namespace math{ namespace tools{

namespace detail{

//
// The error function: the difference between F(x) and
// the current approximation.  This is the function
// for which we must find the extema.
//
template <class T>
struct remez_error_function
{
   typedef boost::function1<T, T const &> function_type;
public:
   remez_error_function(
      function_type f_,
      const polynomial<T>& n,
      const polynomial<T>& d,
      bool rel_err)
         : f(f_), numerator(n), denominator(d), rel_error(rel_err) {}

   T operator()(const T& z)const
   {
      T y = f(z);
      T abs = y - (numerator.evaluate(z) / denominator.evaluate(z));
      T err;
      if(rel_error)
      {
         if(y != 0)
            err = abs / fabs(y);
         else if(0 == abs)
         {
            // we must be at a root, or it's not recoverable:
            BOOST_ASSERT(0 == abs);
            err = 0;
         }
         else
         {
            // We have a divide by zero!
            // Lets assume that f(x) is zero as a result of
            // internal cancellation error that occurs as a result
            // of shifting a root at point z to the origin so that
            // the approximation can be "pinned" to pass through
            // the origin: in that case it really
            // won't matter what our approximation calculates here
            // as long as it's a small number, return the absolute error:
            err = abs;
         }
      }
      else
         err = abs;
      return err;
   }
private:
   function_type f;
   polynomial<T> numerator;
   polynomial<T> denominator;
   bool rel_error;
};
//
// This function adapts the error function so that it's minima
// are the extema of the error function.  We can find the minima
// with standard techniques.
//
template <class T>
struct remez_max_error_function
{
   remez_max_error_function(const remez_error_function<T>& f)
      : func(f) {}

   T operator()(const T& x)
   {
      BOOST_MATH_STD_USING
      return -fabs(func(x));
   }
private:
   remez_error_function<T> func;
};

} // detail

template <class T>
class remez_minimax
{
public:
   typedef boost::function1<T, T const &> function_type;
   typedef boost::numeric::ublas::vector<T> vector_type;
   typedef boost::numeric::ublas::matrix<T> matrix_type;

   remez_minimax(function_type f, unsigned oN, unsigned oD, T a, T b, bool pin = true, bool rel_err = false, int sk = 0, int bits = 0);
   remez_minimax(function_type f, unsigned oN, unsigned oD, T a, T b, bool pin, bool rel_err, int sk, int bits, const vector_type& points);

   void reset(unsigned oN, unsigned oD, T a, T b, bool pin = true, bool rel_err = false, int sk = 0, int bits = 0);
   void reset(unsigned oN, unsigned oD, T a, T b, bool pin, bool rel_err, int sk, int bits, const vector_type& points);

   void set_brake(int b)
   {
      BOOST_ASSERT(b < 100);
      BOOST_ASSERT(b >= 0);
      m_brake = b;
   }

   T iterate();

   polynomial<T> denominator()const;
   polynomial<T> numerator()const;

   vector_type const& chebyshev_points()const
   {
      return control_points;
   }

   vector_type const& zero_points()const
   {
      return zeros;
   }

   T error_term()const
   {
      return solution[solution.size() - 1];
   }
   T max_error()const
   {
      return m_max_error;
   }
   T max_change()const
   {
      return m_max_change;
   }
   void rotate()
   {
      --orderN;
      ++orderD;
   }
   void rescale(T a, T b)
   {
      T scale = (b - a) / (max - min);
      for(unsigned i = 0; i < control_points.size(); ++i)
      {
         control_points[i] = (control_points[i] - min) * scale + a;
      }
      min = a;
      max = b;
   }
private:

   void init_chebyshev();

   function_type func;            // The function to approximate.
   vector_type control_points;    // Current control points to be used for the next iteration.
   vector_type solution;          // Solution from the last iteration contains all unknowns including the error term.
   vector_type zeros;             // Location of points of zero error from last iteration, plus the two end points.
   vector_type maxima;            // Location of maxima of the error function, actually contains the control points used for the last iteration.
   T m_max_error;                 // Maximum error found in last approximation.
   T m_max_change;                // Maximum change in location of control points after last iteration.
   unsigned orderN;               // Order of the numerator polynomial.
   unsigned orderD;               // Order of the denominator polynomial.
   T min, max;                    // End points of the range to optimise over.
   bool rel_error;                // If true optimise for relative not absolute error.
   bool pinned;                   // If true the approximation is "pinned" to go through the origin.
   unsigned unknowns;             // Total number of unknowns.
   int m_precision;               // Number of bits precision to which the zeros and maxima are found.
   T m_max_change_history[2];     // Past history of changes to control points.
   int m_brake;                     // amount to break by in percentage points.
   int m_skew;                      // amount to skew starting points by in percentage points: -100-100
};

#ifndef BRAKE
#define BRAKE 0
#endif
#ifndef SKEW
#define SKEW 0
#endif

template <class T>
void remez_minimax<T>::init_chebyshev()
{
   BOOST_MATH_STD_USING
   //
   // Fill in the zeros:
   //
   unsigned terms = pinned ? orderD + orderN : orderD + orderN + 1;

   for(unsigned i = 0; i < terms; ++i)
   {
      T cheb = cos((2 * terms - 1 - 2 * i) * constants::pi<T>() / (2 * terms));
      cheb += 1;
      cheb /= 2;
      if(m_skew != 0)
      {
         T p = static_cast<T>(200 + m_skew) / 200;
         cheb = pow(cheb, p);
      }
      cheb *= (max - min);
      cheb += min;
      zeros[i+1] = cheb;
   }
   zeros[0] = min;
   zeros[unknowns] = max;
   // perform a regular interpolation fit:
   matrix_type A(terms, terms);
   vector_type b(terms);
   // fill in the y values:
   for(unsigned i = 0; i < b.size(); ++i)
   {
      b[i] = func(zeros[i+1]);
   }
   // fill in powers of x evaluated at each of the control points:
   unsigned offsetN = pinned ? 0 : 1;
   unsigned offsetD = offsetN + orderN;
   unsigned maxorder = (std::max)(orderN, orderD);
   for(unsigned i = 0; i < b.size(); ++i)
   {
      T x0 = zeros[i+1];
      T x = x0;
      if(!pinned)
         A(i, 0) = 1;
      for(unsigned j = 0; j < maxorder; ++j)
      {
         if(j < orderN)
            A(i, j + offsetN) = x;
         if(j < orderD)
         {
            A(i, j + offsetD) = -x * b[i];
         }
         x *= x0;
      }
   }
   //
   // Now go ahead and solve the expression to get our solution:
   //
   vector_type l_solution = boost::math::tools::solve(A, b);
   // need to add a "fake" error term:
   l_solution.resize(unknowns);
   l_solution[unknowns-1] = 0;
   solution = l_solution;
   //
   // Now find all the extrema of the error function:
   //
   detail::remez_error_function<T> Err(func, this->numerator(), this->denominator(), rel_error);
   detail::remez_max_error_function<T> Ex(Err);
   m_max_error = 0;
   //int max_err_location = 0;
   for(unsigned i = 0; i < unknowns; ++i)
   {
      std::pair<T, T> r = brent_find_minima(Ex, zeros[i], zeros[i+1], m_precision);
      maxima[i] = r.first;
      T rel_err = fabs(r.second);
      if(rel_err > m_max_error)
      {
         m_max_error = fabs(r.second);
         //max_err_location = i;
      }
   }
   control_points = maxima;
}

template <class T>
void remez_minimax<T>::reset(
         unsigned oN,
         unsigned oD,
         T a,
         T b,
         bool pin,
         bool rel_err,
         int sk,
         int bits)
{
   control_points = vector_type(oN + oD + (pin ? 1 : 2));
   solution = control_points;
   zeros = vector_type(oN + oD + (pin ? 2 : 3));
   maxima = control_points;
   orderN = oN;
   orderD = oD;
   rel_error = rel_err;
   pinned = pin;
   m_skew = sk;
   min = a;
   max = b;
   m_max_error = 0;
   unknowns = orderN + orderD + (pinned ? 1 : 2);
   // guess our initial control points:
   control_points[0] = min;
   control_points[unknowns - 1] = max;
   T interval = (max - min) / (unknowns - 1);
   T spot = min + interval;
   for(unsigned i = 1; i < control_points.size(); ++i)
   {
      control_points[i] = spot;
      spot += interval;
   }
   solution[unknowns - 1] = 0;
   m_max_error = 0;
   if(bits == 0)
   {
      // don't bother about more than float precision:
      m_precision = (std::min)(24, (boost::math::policies::digits<T, boost::math::policies::policy<> >() / 2) - 2);
   }
   else
   {
      // can't be more accurate than half the bits of T:
      m_precision = (std::min)(bits, (boost::math::policies::digits<T, boost::math::policies::policy<> >() / 2) - 2);
   }
   m_max_change_history[0] = m_max_change_history[1] = 1;
   init_chebyshev();
   // do one iteration whatever:
   //iterate();
}

template <class T>
inline remez_minimax<T>::remez_minimax(
         typename remez_minimax<T>::function_type f,
         unsigned oN,
         unsigned oD,
         T a,
         T b,
         bool pin,
         bool rel_err,
         int sk,
         int bits)
   : func(f)
{
   m_brake = 0;
   reset(oN, oD, a, b, pin, rel_err, sk, bits);
}

template <class T>
void remez_minimax<T>::reset(
         unsigned oN,
         unsigned oD,
         T a,
         T b,
         bool pin,
         bool rel_err,
         int sk,
         int bits,
         const vector_type& points)
{
   control_points = vector_type(oN + oD + (pin ? 1 : 2));
   solution = control_points;
   zeros = vector_type(oN + oD + (pin ? 2 : 3));
   maxima = control_points;
   orderN = oN;
   orderD = oD;
   rel_error = rel_err;
   pinned = pin;
   m_skew = sk;
   min = a;
   max = b;
   m_max_error = 0;
   unknowns = orderN + orderD + (pinned ? 1 : 2);
   control_points = points;
   solution[unknowns - 1] = 0;
   m_max_error = 0;
   if(bits == 0)
   {
      // don't bother about more than float precision:
      m_precision = (std::min)(24, (boost::math::policies::digits<T, boost::math::policies::policy<> >() / 2) - 2);
   }
   else
   {
      // can't be more accurate than half the bits of T:
      m_precision = (std::min)(bits, (boost::math::policies::digits<T, boost::math::policies::policy<> >() / 2) - 2);
   }
   m_max_change_history[0] = m_max_change_history[1] = 1;
   // do one iteration whatever:
   //iterate();
}

template <class T>
inline remez_minimax<T>::remez_minimax(
         typename remez_minimax<T>::function_type f,
         unsigned oN,
         unsigned oD,
         T a,
         T b,
         bool pin,
         bool rel_err,
         int sk,
         int bits,
         const vector_type& points)
   : func(f)
{
   m_brake = 0;
   reset(oN, oD, a, b, pin, rel_err, sk, bits, points);
}

template <class T>
T remez_minimax<T>::iterate()
{
   BOOST_MATH_STD_USING
   matrix_type A(unknowns, unknowns);
   vector_type b(unknowns);

   // fill in evaluation of f(x) at each of the control points:
   for(unsigned i = 0; i < b.size(); ++i)
   {
      // take care that none of our control points are at the origin:
      if(pinned && (control_points[i] == 0))
      {
         if(i)
            control_points[i] = control_points[i-1] / 3;
         else
            control_points[i] = control_points[i+1] / 3;
      }
      b[i] = func(control_points[i]);
   }

   T err_err;
   unsigned convergence_count = 0;
   do{
      // fill in powers of x evaluated at each of the control points:
      int sign = 1;
      unsigned offsetN = pinned ? 0 : 1;
      unsigned offsetD = offsetN + orderN;
      unsigned maxorder = (std::max)(orderN, orderD);
      T Elast = solution[unknowns - 1];

      for(unsigned i = 0; i < b.size(); ++i)
      {
         T x0 = control_points[i];
         T x = x0;
         if(!pinned)
            A(i, 0) = 1;
         for(unsigned j = 0; j < maxorder; ++j)
         {
            if(j < orderN)
               A(i, j + offsetN) = x;
            if(j < orderD)
            {
               T mult = rel_error ? T(b[i] - sign * fabs(b[i]) * Elast): T(b[i] - sign * Elast);
               A(i, j + offsetD) = -x * mult;
            }
            x *= x0;
         }
         // The last variable to be solved for is the error term,
         // sign changes with each control point:
         T E = rel_error ? T(sign * fabs(b[i])) : T(sign);
         A(i, unknowns - 1) = E;
         sign = -sign;
      }

   #ifdef BOOST_MATH_INSTRUMENT
      for(unsigned i = 0; i < b.size(); ++i)
         std::cout << b[i] << " ";
      std::cout << "\n\n";
      for(unsigned i = 0; i < b.size(); ++i)
      {
         for(unsigned j = 0; j < b.size(); ++ j)
            std::cout << A(i, j) << " ";
         std::cout << "\n";
      }
      std::cout << std::endl;
   #endif
      //
      // Now go ahead and solve the expression to get our solution:
      //
      solution = boost::math::tools::solve(A, b);

      err_err = (Elast != 0) ? T(fabs((fabs(solution[unknowns-1]) - fabs(Elast)) / fabs(Elast))) : T(1);
   }while(orderD && (convergence_count++ < 80) && (err_err > 0.001));

   //
   // Perform a sanity check to verify that the solution to the equations
   // is not so much in error as to be useless.  The matrix inversion can
   // be very close to singular, so this can be a real problem.
   //
   vector_type sanity = prod(A, solution);
   for(unsigned i = 0; i < b.size(); ++i)
   {
      T err = fabs((b[i] - sanity[i]) / fabs(b[i]));
      if(err > sqrt(epsilon<T>()))
      {
         std::cerr << "Sanity check failed: more than half the digits in the found solution are in error." << std::endl;
      }
   }

   //
   // Next comes another sanity check, we want to verify that all the control
   // points do actually alternate in sign, in practice we may have
   // additional roots in the error function that cause this to fail.
   // Failure here is always fatal: even though this code attempts to correct
   // the problem it usually only postpones the inevitable.
   //
   polynomial<T> num, denom;
   num = this->numerator();
   denom = this->denominator();
   T e1 = b[0] - num.evaluate(control_points[0]) / denom.evaluate(control_points[0]);
#ifdef BOOST_MATH_INSTRUMENT
   std::cout << e1;
#endif
   for(unsigned i = 1; i < b.size(); ++i)
   {
      T e2 = b[i] - num.evaluate(control_points[i]) / denom.evaluate(control_points[i]);
#ifdef BOOST_MATH_INSTRUMENT
      std::cout << " " << e2;
#endif
      if(e2 * e1 > 0)
      {
         std::cerr << std::flush << "Basic sanity check failed: Error term does not alternate in sign, non-recoverable error may follow..." << std::endl;
         T perturbation = 0.05;
         do{
            T point = control_points[i] * (1 - perturbation) + control_points[i-1] * perturbation;
            e2 = func(point) - num.evaluate(point) / denom.evaluate(point);
            if(e2 * e1 < 0)
            {
               control_points[i] = point;
               break;
            }
            perturbation += 0.05;
         }while(perturbation < 0.8);

         if((e2 * e1 > 0) && (i + 1 < b.size()))
         {
            perturbation = 0.05;
            do{
               T point = control_points[i] * (1 - perturbation) + control_points[i+1] * perturbation;
               e2 = func(point) - num.evaluate(point) / denom.evaluate(point);
               if(e2 * e1 < 0)
               {
                  control_points[i] = point;
                  break;
               }
               perturbation += 0.05;
            }while(perturbation < 0.8);
         }

      }
      e1 = e2;
   }

#ifdef BOOST_MATH_INSTRUMENT
   for(unsigned i = 0; i < solution.size(); ++i)
      std::cout << solution[i] << " ";
   std::cout << std::endl << this->numerator() << std::endl;
   std::cout << this->denominator() << std::endl;
   std::cout << std::endl;
#endif

   //
   // The next step is to find all the intervals in which our maxima
   // lie:
   //
   detail::remez_error_function<T> Err(func, this->numerator(), this->denominator(), rel_error);
   zeros[0] = min;
   zeros[unknowns] = max;
   for(unsigned i = 1; i < control_points.size(); ++i)
   {
      eps_tolerance<T> tol(m_precision);
      boost::uintmax_t max_iter = 1000;
      std::pair<T, T> p = toms748_solve(
         Err,
         control_points[i-1],
         control_points[i],
         tol,
         max_iter);
      zeros[i] = (p.first + p.second) / 2;
      //zeros[i] = bisect(Err, control_points[i-1], control_points[i], m_precision);
   }
   //
   // Now find all the extrema of the error function:
   //
   detail::remez_max_error_function<T> Ex(Err);
   m_max_error = 0;
   //int max_err_location = 0;
   for(unsigned i = 0; i < unknowns; ++i)
   {
      std::pair<T, T> r = brent_find_minima(Ex, zeros[i], zeros[i+1], m_precision);
      maxima[i] = r.first;
      T rel_err = fabs(r.second);
      if(rel_err > m_max_error)
      {
         m_max_error = fabs(r.second);
         //max_err_location = i;
      }
   }
   //
   // Almost done now! we just need to set our control points
   // to the extrema, and calculate how much each point has changed
   // (this will be our termination condition):
   //
   swap(control_points, maxima);
   m_max_change = 0;
   //int max_change_location = 0;
   for(unsigned i = 0; i < unknowns; ++i)
   {
      control_points[i] = (control_points[i] * (100 - m_brake) + maxima[i] * m_brake) / 100;
      T change = fabs((control_points[i] - maxima[i]) / control_points[i]);
#if 0
      if(change > m_max_change_history[1])
      {
         // divergence!!! try capping the change:
         std::cerr << "Possible divergent step, change will be capped!!" << std::endl;
         change = m_max_change_history[1];
         if(control_points[i] < maxima[i])
            control_points[i] = maxima[i] - change * maxima[i];
         else
            control_points[i] = maxima[i] + change * maxima[i];
      }
#endif
      if(change > m_max_change)
      {
         m_max_change = change;
         //max_change_location = i;
      }
   }
   //
   // store max change information:
   //
   m_max_change_history[0] = m_max_change_history[1];
   m_max_change_history[1] = fabs(m_max_change);

   return m_max_change;
}

template <class T>
polynomial<T> remez_minimax<T>::numerator()const
{
   boost::scoped_array<T> a(new T[orderN + 1]);
   if(pinned)
      a[0] = 0;
   unsigned terms = pinned ? orderN : orderN + 1;
   for(unsigned i = 0; i < terms; ++i)
      a[pinned ? i+1 : i] = solution[i];
   return boost::math::tools::polynomial<T>(&a[0], orderN);
}

template <class T>
polynomial<T> remez_minimax<T>::denominator()const
{
   unsigned terms = orderD + 1;
   unsigned offsetD = pinned ? orderN : (orderN + 1);
   boost::scoped_array<T> a(new T[terms]);
   a[0] = 1;
   for(unsigned i = 0; i < orderD; ++i)
      a[i+1] = solution[i + offsetD];
   return boost::math::tools::polynomial<T>(&a[0], orderD);
}

}}} // namespaces

#endif // BOOST_MATH_TOOLS_REMEZ_HPP


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

#include "Circuit.hpp"
#include "utils.hpp"

namespace yavque
{
/*
 * @param op must be a hermitian matrix
 * */
std::pair<double, Eigen::VectorXd>
value_and_grad(const Eigen::SparseMatrix<yavque::cx_double>& op, const Circuit& circuit);

} // namespace yavque


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_geometry_second_order_cone_hpp__
#define __multicontact_api_geometry_second_order_cone_hpp__

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

#include "multicontact-api/geometry/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"

namespace multicontact_api {
namespace geometry {

template <typename _Scalar, int _dim, int _Options>
struct SecondOrderCone : public serialization::Serializable<SecondOrderCone<_Scalar, _dim, _Options> > {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  enum { dim = _dim, Options = _Options };
  typedef _Scalar Scalar;
  typedef Eigen::Matrix<Scalar, dim, dim, Options> MatrixD;
  typedef Eigen::Matrix<Scalar, dim, 1, Options> VectorD;
  typedef Eigen::DenseIndex DenseIndex;

  SecondOrderCone()
      : m_Q(MatrixD::Identity()), m_QPo(_dim, _dim), m_direction(VectorD::Zero()), m_Pd(_dim, _dim), m_Po(_dim, _dim) {
    m_direction[_dim - 1] = 1.;
    computeProjectors();
  }

  SecondOrderCone(const MatrixD& Q, const VectorD& direction)
      : m_Q(Q), m_QPo(_dim, _dim), m_direction(direction.normalized()), m_Pd(_dim, _dim), m_Po(_dim, _dim) {
    assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
    assert((Q - Q.transpose()).isMuchSmallerThan(Q));
    computeProjectors();
  }

  ///
  /// \brief Build a regular cone from a given friction coefficient and a direction.
  ///
  /// \param mu Friction coefficient.
  /// \param direction Direction of the cone.
  ///
  /// \returns A second order cone.
  ///
  static SecondOrderCone RegularCone(const Scalar mu, const VectorD& direction) {
    assert(mu > 0 && "The friction coefficient must be non-negative");
    MatrixD Q(MatrixD::Zero());
    Q.diagonal().fill(1. / mu);

    return SecondOrderCone(Q, direction);
  }

  template <typename S2, int O2>
  bool operator==(const SecondOrderCone<S2, dim, O2>& other) const {
    return m_Q == other.m_Q && m_direction == other.m_direction;
  }

  template <typename S2, int O2>
  bool operator!=(const SecondOrderCone<S2, dim, O2>& other) const {
    return !(*this == other);
  }

  /// \returns the value of lhs of the conic inequality
  Scalar lhsValue(const VectorD& point) const {
    //        const VectorD x_Po(m_Po * point);
    return (m_QPo * point).norm();
  }

  /// \returns the value of rhs of the conic inequality
  Scalar rhsValue(const VectorD& point) const { return m_direction.dot(point); }

  /// \returns true if the point is inside the cone
  bool check(const VectorD& point) const { return check(point, 1.); }

  bool check(const VectorD& point, const Scalar factor) const { return lhsValue(point) <= factor * rhsValue(point); }

  /// \returns the direction of the cone.
  const VectorD& direction() const { return m_direction; }
  void setDirection(const VectorD& direction) {
    assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
    m_direction = direction.normalized();
    computeProjectors();
  }

  template <typename S2, int O2>
  bool isApprox(const SecondOrderCone<S2, dim, O2>& other,
                const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
    return m_direction.isApprox(other.m_direction, prec) && m_Q.isApprox(other.m_Q, prec);
  }

  /// \returns the quadratic term of the lhs norm.
  const MatrixD& Q() const { return m_Q; }
  void setQ(const MatrixD& Q) {
    assert((Q - Q.transpose()).isMuchSmallerThan(Q));
    m_Q = Q;
    computeProjectors();
  }

  void disp(std::ostream& os) const {
    os << "Q:\n" << m_Q << std::endl << "direction: " << m_direction.transpose() << std::endl;
  }

  friend std::ostream& operator<<(std::ostream& os, const SecondOrderCone& C) {
    C.disp(os);
    return os;
  }

 protected:
  inline void computeProjectors() {
    m_Pd = m_direction * m_direction.transpose();
    m_Po = MatrixD::Identity() - m_Pd;
    m_QPo.noalias() = m_Q * m_Po;
  }

  /// \brief Cholesky decomposition matrix reprensenting the conic norm
  MatrixD m_Q;
  /// \brief Cholesky decomposition projected on the orthogonal of m_direction
  MatrixD m_QPo;

  /// \brief Direction of the cone
  VectorD m_direction;

  /// \brief Projector along the direction of d
  MatrixD m_Pd;
  /// \brief Projector orthogonal to d
  MatrixD m_Po;

 private:
  // Serialization of the class
  friend class boost::serialization::access;

  template <class Archive>
  void save(Archive& ar, const unsigned int /*version*/) const {
    ar& boost::serialization::make_nvp("quadratic_term", m_Q);
    ar& boost::serialization::make_nvp("direction", m_direction);
  }

  template <class Archive>
  void load(Archive& ar, const unsigned int /*version*/) {
    ar >> boost::serialization::make_nvp("quadratic_term", m_Q);
    ar >> boost::serialization::make_nvp("direction", m_direction);

    computeProjectors();
  }

  BOOST_SERIALIZATION_SPLIT_MEMBER()
};

}  // namespace geometry
}  // namespace multicontact_api

#endif  // ifndef __multicontact_api_geometry_second_order_cone_hpp__