// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is 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_GEOMETRY_EXTENSIONS_GIS_LATLONG_DETAIL_GRATICULE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_LATLONG_DETAIL_GRATICULE_HPP

#include <cmath>
#include <sstream>
#include <string>

#include <boost/numeric/conversion/cast.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

/*!
    \brief Cardinal directions.
    \ingroup cs
    \details They are used in the dms-class. When specified by the library user,
    north/east/south/west is, in general, enough. When parsed or received by an algorithm,
    the user knows it it is lat/long but not more
*/
enum cd_selector
{
    /*cd_none, */
    north,
    east,
    south,
    west,
    cd_lat,
    cd_lon
};

/*!
    \brief Utility class to assign poinst with degree,minute,second
    \ingroup cs
    \note Normally combined with latitude and longitude classes
    \tparam CardinalDir selects if it is north/south/west/east
    \tparam coordinate value, double/float
    \par Example:
    Example showing how to use the dms class
    \dontinclude doxygen_1.cpp
    \skip example_dms
    \line {
    \until }
*/
template <cd_selector CardinalDir, typename T = double>
class dms
{
public:

    /// Constructs with a value
    inline explicit dms(T v)
        : m_value(v)
    {}

    /// Constructs with a degree, minute, optional second
    inline explicit dms(int d, int m, T s = 0.0)
    {
        double v = ((CardinalDir == west || CardinalDir == south) ? -1.0 : 1.0)
                    * (double(d) + (m / 60.0) + (s / 3600.0));

        m_value = boost::numeric_cast<T>(v);
    }

    // Prohibit automatic conversion to T
    // because this would enable lon(dms<south>)
    // inline operator T() const { return m_value; }

    /// Explicit conversion to T (double/float)
    inline const T& as_value() const
    {
        return m_value;
    }

    /// Get degrees as integer, minutes as integer, seconds as double.
    inline void get_dms(int& d, int& m, double& s,
                        bool& positive, char& cardinal) const
    {
        double value = m_value;

        // Set to normal earth latlong coordinates
        while (value < -180)
        {
            value += 360;
        }
        while (value > 180)
        {
            value -= 360;
        }
        // Make positive and indicate this
        positive = value > 0;

        // Todo: we might implement template/specializations here
        // Todo: if it is "west" and "positive", make east? or keep minus sign then?

        cardinal = ((CardinalDir == cd_lat && positive) ? 'N'
            :  (CardinalDir == cd_lat && !positive) ? 'S'
            :  (CardinalDir == cd_lon && positive) ? 'E'
            :  (CardinalDir == cd_lon && !positive) ? 'W'
            :  (CardinalDir == east) ? 'E'
            :  (CardinalDir == west) ? 'W'
            :  (CardinalDir == north) ? 'N'
            :  (CardinalDir == south) ? 'S'
            : ' ');

        value = geometry::math::abs(value);

        // Calculate the values
        double fraction = 0;
        double integer = 0;
        fraction = std::modf(value, &integer);
        d = int(integer);
        s = 60.0 * std::modf(fraction * 60.0, &integer);
        m = int(integer);
    }

    /// Get degrees, minutes, seconds as a string, separators can be specified optionally
    inline std::string get_dms(std::string const& ds = " ",
        const std::string& ms = "'",
        const std::string& ss = "\"") const
    {
        double s = 0;
        int d = 0;
        int m = 0;
        bool positive = false;
        char cardinal = 0;
        get_dms(d, m, s, positive, cardinal);
        std::ostringstream out;
        out << d << ds << m << ms << s << ss << " " << cardinal;

        return out.str();
    }

private:

    T m_value;
};

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{
/*!
    \brief internal base class for latitude and longitude classes
    \details The latitude longitude classes define different types for lat and lon. This is convenient
    to construct latlong class without ambiguity.
    \note It is called graticule, after <em>"This latitude/longitude "webbing" is known as the common
    graticule" (http://en.wikipedia.org/wiki/Geographic_coordinate_system)</em>
    \tparam S latitude/longitude
    \tparam T coordinate type, double float or int
*/
template <typename T>
class graticule
{
public:

    // TODO: Pass 'v' by const-ref
    inline explicit graticule(T v) : m_v(v) {}
    inline operator T() const { return m_v; }

private:

    T m_v;
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
    \brief Utility class to assign points with latitude value (north/south)
    \ingroup cs
    \tparam T coordinate type, double / float
    \note Often combined with dms class
*/
template <typename T = double>
class latitude : public detail::graticule<T>
{
public:

    /// Can be constructed with a value
    inline explicit latitude(T v)
        : detail::graticule<T>(v)
    {}

    /// Can be constructed with a NORTH dms-class
    inline explicit latitude(dms<north,T> const& v)
        : detail::graticule<T>(v.as_value())
    {}

    /// Can be constructed with a SOUTH dms-class
    inline explicit latitude(dms<south,T> const& v)
       : detail::graticule<T>(v.as_value())
   {}
};

/*!
\brief Utility class to assign points with longitude value (west/east)
\ingroup cs
\tparam T coordinate type, double / float
\note Often combined with dms class
*/
template <typename T = double>
class longitude : public detail::graticule<T>
{
public:

    /// Can be constructed with a value
    inline explicit longitude(T v)
        : detail::graticule<T>(v)
    {}

    /// Can be constructed with a WEST dms-class
    inline explicit longitude(dms<west, T> const& v)
        : detail::graticule<T>(v.as_value())
    {}

    /// Can be constructed with an EAST dms-class
    inline explicit longitude(dms<east, T> const& v)
        : detail::graticule<T>(v.as_value())
    {}
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_LATLONG_DETAIL_GRATICULE_HPP


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/bitset.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

STF_CASE_TPL (" bitset uit",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using iT =  boost::dispatch::as_integer_t<T, unsigned>;
  using bs::bitset;

  // return type conformity test
  STF_EXPR_IS( bitset(T(), iT()), T );

  // specific values tests
  STF_EQUAL(bitset(T(2),1), T(3));
  STF_EQUAL(bitset(T(3),2), T(3));
  STF_EQUAL(bitset(T(3),3), T(7));
} // end of test for unsigned_int_

STF_CASE_TPL (" bitset si",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using iT =  boost::dispatch::as_integer_t<T, unsigned>;
  using bs::bitset;

  // return type conformity test
  STF_EXPR_IS( bitset(T(), iT()), T );

  // specific values tests
  STF_EQUAL(bitset(T(2),1),T(3));
  STF_EQUAL(bitset(T(2),2), T(2));
  STF_EQUAL(bitset(T(-1), 1), T(-1));
} // end of test for unsigned_int_


// Copyright John Maddock 2008
// Copyright Paul A. Bristow
// Copyright Gautam Sewani

// 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)

#define BOOST_MATH_OVERFLOW_ERROR_POLICY throw_on_error
#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/hypergeometric.hpp>

#include <boost/test/test_exec_monitor.hpp> // Boost.Test
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;

#include <boost/array.hpp>
#include "functor.hpp"
#include "handle_test_result.hpp"

#define BOOST_CHECK_EX(a) \
   {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK(a); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
      {\
         std::cerr << "Failure was with data ";\
         std::cerr << std::setprecision(35); \
         std::cerr << "x = " << x << ", r = " << r << ", n = " << n\
         << ", N = " << N << ", p = " << cp << ", q = " << ccp << std::endl;\
      }\
   }

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double|real_concept";
   }
   else
   {
      largest_type = "long double|real_concept";
   }
#else
   largest_type = "(long\\s+)?double";
#endif
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if((boost::math::tools::digits<long double>() > boost::math::tools::digits<double>())
      && (boost::math::tools::digits<long double>() < 100))
   {
      //
      // Some split of errors from long double into double:
      //
      add_expected_result(
         ".*",                          // compiler
         ".*",                          // stdlib
         ".*",                          // platform
         "double",                      // test type(s)
         "Random.*",                    // test data group
         ".*", 1500, 1500);      // test function
      add_expected_result(
         ".*",                          // compiler
         ".*",                          // stdlib
         ".*",                          // platform
         "double",                      // test type(s)
         ".*",                    // test data group
         ".*", 10, 10);      // test function
   }
#endif

   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                // test type(s)
      "Random.*",                    // test data group
      ".*", 250000000, 25000000);    // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      "Random.*",                    // test data group
      ".*", 10000000, 5000000);      // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      ".*",                          // test data group
      ".*", 50, 20);                 // test function
}

template <class T>
inline unsigned make_unsigned(T x)
{
   return static_cast<unsigned>(x);
}
template<>
inline unsigned make_unsigned(boost::math::concepts::real_concept x)
{
   return static_cast<unsigned>(x.value());
}

template <class T>
T pdf_tester(T r, T n, T N, T x)
{
   boost::math::hypergeometric_distribution<T> d(make_unsigned(r), make_unsigned(n), make_unsigned(N));
   return pdf(d, x);
}

template <class T>
T cdf_tester(T r, T n, T N, T x)
{
   boost::math::hypergeometric_distribution<T> d(make_unsigned(r), make_unsigned(n), make_unsigned(N));
   return cdf(d, x);
}

template <class T>
T ccdf_tester(T r, T n, T N, T x)
{
   boost::math::hypergeometric_distribution<T> d(make_unsigned(r), make_unsigned(n), make_unsigned(N));
   return cdf(complement(d, x));
}

template <class T>
void do_test_hypergeometric(const T& data, const char* type_name, const char* test_name)
{
   // warning suppression:
   (void)data;
   (void)type_name;
   (void)test_name;

#if !defined(TEST_QUANT) || (TEST_QUANT == 0)
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(value_type, value_type, value_type, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = pdf_tester<value_type>;
#else
   pg funcp = pdf_tester;
#endif

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

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

   //
   // test hypergeometric against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1, 2, 3),
      extract_result(4));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "hypergeometric PDF", test_name);

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = cdf_tester<value_type>;
#else
   funcp = cdf_tester;
#endif

   //
   // test hypergeometric against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1, 2, 3),
      extract_result(5));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "hypergeometric CDF", test_name);

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = ccdf_tester<value_type>;
#else
   funcp = ccdf_tester;
#endif

   //
   // test hypergeometric against data:
   //
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1, 2, 3),
      extract_result(6));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "hypergeometric CDF complement", test_name);
   std::cout << std::endl;
#endif
}

template <class T>
void do_test_hypergeometric_quantile(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;

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

   if(boost::math::tools::digits<value_type>() > 50)
   {
      for(unsigned i = 0; i < data.size(); ++i)
      {
         using namespace boost::math::policies;

         unsigned r = make_unsigned(data[i][0]);
         unsigned n = make_unsigned(data[i][1]);
         unsigned N = make_unsigned(data[i][2]);
         unsigned x = make_unsigned(data[i][3]);
         value_type cp = data[i][5];
         value_type ccp = data[i][6];
         //
         // A bit of warning supression:
         //
         (void)x;
         (void)n;
         (void)r;
         (void)N;
         (void)cp;
         (void)ccp;

#if !defined(TEST_QUANT) || (TEST_QUANT == 1)
         boost::math::hypergeometric_distribution<value_type,
            policy<discrete_quantile<integer_round_up> > > du(r, n, N);

         if((cp < 0.9) && (cp > boost::math::tools::min_value<value_type>()))
         {
            BOOST_CHECK_EX(quantile(du, cp) >= x);
         }
         if((ccp < 0.9) && (ccp > boost::math::tools::min_value<value_type>()))
         {
            BOOST_CHECK_EX(quantile(complement(du, ccp)) >= x);
         }
#endif
#if !defined(TEST_QUANT) || (TEST_QUANT == 2)
         boost::math::hypergeometric_distribution<value_type,
            policy<discrete_quantile<integer_round_down> > > dl(r, n, N);
         if((cp < 0.9) && (cp > boost::math::tools::min_value<value_type>()))
         {
            BOOST_CHECK_EX(quantile(dl, cp) <= x);
         }
         if((ccp < 0.9) && (ccp > boost::math::tools::min_value<value_type>()))
         {
            BOOST_CHECK_EX(quantile(complement(dl, ccp)) <= x);
         }
#endif
#if !defined(TEST_QUANT) || (TEST_QUANT == 3)
         boost::math::hypergeometric_distribution<value_type,
            policy<discrete_quantile<integer_round_nearest> > > dn(r, n, N);

         if((cp < 0.9) && (cp > boost::math::tools::min_value<value_type>()))
         {
            BOOST_CHECK_EX(quantile(dn, cp) == x);
         }
         if((ccp < 0.9) && (ccp > boost::math::tools::min_value<value_type>()))
         {
            BOOST_CHECK_EX(quantile(complement(dn, ccp)) == x);
         }
#endif
#if !defined(TEST_QUANT) || (TEST_QUANT == 4)
         boost::math::hypergeometric_distribution<value_type,
            policy<discrete_quantile<integer_round_outwards> > > dou(r, n, N);

         if((cp < 0.9) && (cp > boost::math::tools::min_value<value_type>()))
         {
            if(cp < 0.5)
            {
               BOOST_CHECK_EX(quantile(dou, cp) <= x);
            }
            else
            {
               BOOST_CHECK_EX(quantile(dou, cp) >= x);
            }
         }
         if((ccp < 0.9) && (ccp > boost::math::tools::min_value<value_type>()))
         {
            if(ccp < 0.5)
            {
               BOOST_CHECK_EX(quantile(complement(dou, ccp)) >= x);
            }
            else
            {
               BOOST_CHECK_EX(quantile(complement(dou, ccp)) <= x);
            }
         }
#endif
#if !defined(TEST_QUANT) || (TEST_QUANT == 5)
         boost::math::hypergeometric_distribution<value_type,
            policy<discrete_quantile<integer_round_inwards> > > di(r, n, N);

         if((cp < 0.9) && (cp > boost::math::tools::min_value<value_type>()))
         {
            if(cp < 0.5)
            {
               BOOST_CHECK_EX(quantile(di, cp) >= x);
            }
            else
            {
               BOOST_CHECK_EX(quantile(di, cp) <= x);
            }
         }
         if((ccp < 0.9) && (ccp > boost::math::tools::min_value<value_type>()))
         {
            if(ccp < 0.5)
            {
               BOOST_CHECK_EX(quantile(complement(di, ccp)) <= x);
            }
            else
            {
               BOOST_CHECK_EX(quantile(complement(di, ccp)) >= x);
            }
         }
#endif
      }
   }
}

template <class RealType>
void test_spot(unsigned x, unsigned n, unsigned r, unsigned N,
               RealType p, RealType cp, RealType ccp, RealType tol)
{
   //
   // A bit of warning supression:
   //
   (void)x;
   (void)n;
   (void)r;
   (void)N;
   (void)p;
   (void)cp;
   (void)ccp;
   (void)tol;
#if !defined(TEST_QUANT) || (TEST_QUANT == 0)
   boost::math::hypergeometric_distribution<RealType> d(r, n, N);

   std::pair<unsigned, unsigned> extent = range(d);
   // CDF's:
   BOOST_CHECK_CLOSE(pdf(d, x), p, tol);
   BOOST_CHECK_CLOSE(cdf(d, x), cp, tol);
   BOOST_CHECK_CLOSE(cdf(complement(d, x)), ccp, tol);
   // Again with real-value arguments:
   BOOST_CHECK_CLOSE(pdf(d, static_cast<RealType>(x)), p, tol);
   BOOST_CHECK_CLOSE(cdf(d, static_cast<RealType>(x)), cp, tol);
   BOOST_CHECK_CLOSE(cdf(complement(d, static_cast<RealType>(x))), ccp, tol);
   //
   // Quantiles, don't bother checking these for type float
   // as there's not enough precision in the p and q values
   // to get back to where we started:
   //
   if(boost::math::tools::digits<RealType>() > 50)
   {
      using namespace boost::math::policies;

      boost::math::hypergeometric_distribution<RealType,
         policy<discrete_quantile<integer_round_up> > > du(r, n, N);

      BOOST_CHECK_EX(quantile(du, cp) >= x);
      BOOST_CHECK_EX(quantile(complement(du, ccp)) >= x);

      boost::math::hypergeometric_distribution<RealType,
         policy<discrete_quantile<integer_round_down> > > dl(r, n, N);
      BOOST_CHECK_EX(quantile(dl, cp) <= x);
      BOOST_CHECK_EX(quantile(complement(dl, ccp)) <= x);

      boost::math::hypergeometric_distribution<RealType,
         policy<discrete_quantile<integer_round_nearest> > > dn(r, n, N);

      BOOST_CHECK_EX(quantile(dn, cp) == x);
      BOOST_CHECK_EX(quantile(complement(dn, ccp)) == x);
   }
   //
   // Error checking of out of bounds arguments:
   //
   BOOST_CHECK_THROW(pdf(d, extent.second + 1), std::domain_error);
   BOOST_CHECK_THROW(cdf(d, extent.second + 1), std::domain_error);
   BOOST_CHECK_THROW(cdf(complement(d, extent.second + 1)), std::domain_error);
   if(extent.first > 0)
   {
      BOOST_CHECK_THROW(pdf(d, extent.first - 1), std::domain_error);
      BOOST_CHECK_THROW(cdf(d, extent.first - 1), std::domain_error);
      BOOST_CHECK_THROW(cdf(complement(d, extent.first - 1)), std::domain_error);
   }
   BOOST_CHECK_THROW(quantile(d, 1.1f), std::domain_error);
   BOOST_CHECK_THROW(quantile(complement(d, 1.1f)), std::domain_error);
   BOOST_CHECK_THROW(quantile(d, -0.001f), std::domain_error);
   BOOST_CHECK_THROW(quantile(complement(d, -0.001f)), std::domain_error);
   //
   // Checking of extreme values:
   //
   BOOST_CHECK_EQUAL(quantile(d, 0), extent.first);
   BOOST_CHECK_EQUAL(quantile(d, 1), extent.second);
   BOOST_CHECK_EQUAL(quantile(complement(d, 0)), extent.second);
   BOOST_CHECK_EQUAL(quantile(complement(d, 1)), extent.first);
   BOOST_CHECK_EQUAL(cdf(d, extent.second), 1);
   BOOST_CHECK_EQUAL(cdf(complement(d, extent.second)), 0);
#endif
}

template <class RealType>
void test_spots(RealType /*T*/, const char* type_name)
{
   // Basic sanity checks.
   // 50 eps as a percentage, up to a maximum of double precision
   // Test data taken from Mathematica 6
#define T RealType
#include "hypergeometric_test_data.ipp"
   do_test_hypergeometric(hypergeometric_test_data, type_name, "Mathematica data");

#include "hypergeometric_dist_data2.ipp"
   if(boost::is_floating_point<RealType>::value)
   {
      //
      // Don't test this for real_concept: it's too slow!!!
      //
      do_test_hypergeometric(hypergeometric_dist_data2, type_name, "Random large data");
   }

   do_test_hypergeometric_quantile(hypergeometric_test_data, type_name, "Mathematica data");
   if(boost::is_floating_point<RealType>::value)
   {
      //
      // Don't test this for real_concept: it's too slow!!!
      //
      do_test_hypergeometric_quantile(hypergeometric_dist_data2, type_name, "Random large data");
   }

   RealType tolerance = (std::max)(
      static_cast<RealType>(2e-16L),    // limit of test data
      boost::math::tools::epsilon<RealType>());
   cout<<"Absolute tolerance:"<<tolerance<<endl;

   tolerance *= 50 * 100; // 50eps as a persentage
   cout << "Tolerance for type " << typeid(RealType).name()  << " is " << tolerance << " %" << endl;

   //
   // These sanity check values were calculated using the online
   // calculator at http://stattrek.com/Tables/Hypergeometric.aspx
   // It's assumed that the test values are accurate to no more than
   // double precision.
   //
   test_spot(20, 200, 50, 500, static_cast<T>(0.120748236361163), static_cast<T>(0.563532430195156), static_cast<T>(1 - 0.563532430195156), tolerance);
   test_spot(53, 452, 64, 500, static_cast<T>(0.0184749573044286), static_cast<T>(0.0299118078796907), static_cast<T>(1 - 0.0299118078796907), tolerance);
   test_spot(32, 1287, 128, 5000, static_cast<T>(0.0807012167418264), static_cast<T>(0.469768774237742), static_cast<T>(1 - 0.469768774237742), tolerance);
   test_spot(1, 13, 4, 26, static_cast<T>(0.248695652173913), static_cast<T>(0.296521739130435), static_cast<T>(1 - 0.296521739130435), tolerance);
   test_spot(2, 13, 4, 26, static_cast<T>(0.40695652173913), static_cast<T>(0.703478260869565), static_cast<T>(1 - 0.703478260869565), tolerance);
   test_spot(3, 13, 4, 26, static_cast<T>(0.248695652173913), static_cast<T>(0.952173913043478), static_cast<T>(1 - 0.952173913043478), tolerance);

   boost::math::hypergeometric_distribution<RealType> d(50, 200, 500);
   BOOST_CHECK_EQUAL(range(d).first, 0u);
   BOOST_CHECK_EQUAL(range(d).second, 50u);
   BOOST_CHECK_CLOSE(mean(d), static_cast<RealType>(20), tolerance);
   BOOST_CHECK_CLOSE(mode(d), static_cast<RealType>(20), tolerance);
   BOOST_CHECK_CLOSE(variance(d), static_cast<RealType>(10.821643286573146292585170340681L), tolerance);
   BOOST_CHECK_CLOSE(skewness(d), static_cast<RealType>(0.048833071022952084732902910189366L), tolerance);
   BOOST_CHECK_CLOSE(kurtosis_excess(d), static_cast<RealType>(2.5155486690782804816404001878293L), tolerance);
   BOOST_CHECK_CLOSE(kurtosis(d), kurtosis_excess(d) + 3, tolerance);
   BOOST_CHECK_EQUAL(quantile(d, 0.5f), median(d));

   BOOST_CHECK_THROW(d = boost::math::hypergeometric_distribution<RealType>(501, 40, 500), std::domain_error);
   BOOST_CHECK_THROW(d = boost::math::hypergeometric_distribution<RealType>(40, 501, 500), std::domain_error);
}

int test_main(int, char* [])
{
   expected_results();
   // Basic sanity-check spot values.
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
   test_spots(0.0F, "float"); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
   test_spots(0.0, "double"); // Test double. OK at decdigits 7, tolerance = 1e07 %
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L, "long double"); // Test long double.
   test_spots(boost::math::concepts::real_concept(0), "real_concept"); // Test real_concept.
#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;
} // int test_main(int, char* [])


/**
 * Copyright 2014 Facebook
 * @author Misha Denil (mdenil@fb.com)
 */

#include <lua.hpp>
#include <luaT.h>

#include <vector>
#include <queue>
#include <utility>
#include <stdexcept>

// Turn off range checking for multi_array indexing
#define BOOST_DISABLE_ASSERTS
#include <boost/multi_array.hpp>

#include "torch/fb/fbcunn/src/LuaUtils.h"
#include "torch/fb/fbcunn/src/Tensor.h"

namespace facebook { namespace deeplearning { namespace torch {

namespace {

template<typename T>
struct index_value : public std::pair<long, T> {
  index_value(long const& i, T const& v): std::pair<long, T>(i, v) {}
  long const& index() const { return this->first; }
  T const& value() const { return this->second; }
};

template<typename T>
inline index_value<T> make_index_value(long const& i, T const& v) {
  return index_value<T>(i, v);
}

template<typename T>
struct value_order {
  bool operator()(index_value<T> const& a, index_value<T> const& b) {
    return a.value() > b.value();
  }
};

template<typename T>
struct index_order {
  bool operator()(index_value<T> const& a, index_value<T> const& b) {
    return a.index() < b.index();
  }
};

template<typename T>
using array_view = boost::multi_array_ref<T, 2>;

template<typename T>
using array_col_view = typename array_view<T>::template array_view<1>::type;

template<typename T>
boost::multi_array_ref<T, 2> tensor_array_view(Tensor<T>& tensor) {
  return boost::multi_array_ref<T, 2>(
    tensor.data(),
    boost::extents[tensor.size(0)][tensor.size(1)]);
}

template<typename T>
boost::const_multi_array_ref<T, 2> tensor_array_view(Tensor<T> const& tensor) {
  return boost::const_multi_array_ref<T, 2>(
    tensor.data(),
    boost::extents[tensor.size(0)][tensor.size(1)]);
}

template<typename T>
boost::multi_array_ref<T, 1> tensor_vector_view(Tensor<T>& tensor) {
  return boost::multi_array_ref<T, 1>(
    tensor.data(),
    boost::extents[tensor.size(0)]);
}

template<typename T>
boost::const_multi_array_ref<T, 1> tensor_vector_view(Tensor<T> const& tensor) {
  return boost::const_multi_array_ref<T, 1>(
    tensor.data(),
    boost::extents[tensor.size(0)]);
}

template<typename T>
using value_priority_queue = std::priority_queue<
  index_value<T>,
  std::vector<index_value<T>>,
  value_order<T>>;

// Forward pass
template <class T>
int updateOutput(lua_State* L) {
  auto const input_tensor = luaGetTensorChecked<T>(L, 2);
  auto input_length_tensor
    = luaGetFieldIfTensorChecked<long>(L, 1, "input_length");
  auto output_length_tensor
    = luaGetFieldIfTensorChecked<long>(L, 1, "output_length");
  auto switches_tensor = luaGetFieldIfTensorChecked<long>(L, 1, "switches");
  auto k = luaGetFieldIfNumberChecked<long>(L, 1, "k");
  auto k_dynamic = luaGetFieldIfNumberChecked<double>(L, 1, "k_dynamic");
  auto output_tensor = luaGetFieldIfTensorChecked<T>(L, 1, "output");
  int outputIdx = lua_gettop(L);

  luaL_argcheck(L, input_tensor.ndims() == 2, 1,
    "input must have exactly 2 dimensions.");
  luaL_argcheck(L, input_length_tensor.ndims() == 1, 1,
    "input_length must have exactly 1 dimension");
  luaL_argcheck(L, input_length_tensor.size() == 1, 1,
    "input_length must have exactly 1 element");

  // Set k for dynamic k-max pooling
  if (k_dynamic > 0) {
    int sentence_dim = input_tensor.ndims() - 2;
    k = std::max(k, static_cast<long>(k_dynamic * input_tensor.size(sentence_dim)));
  }

  output_tensor.resize(LongStorage{k, input_tensor.size(1)});
  switches_tensor.resize(LongStorage{k, input_tensor.size(1)});
  output_tensor.fill(0.0);
  switches_tensor.fill(0);

  auto input_data = tensor_array_view(input_tensor);
  auto output_data = tensor_array_view(output_tensor);
  auto switches_data = tensor_array_view(switches_tensor);
  auto input_length_data = tensor_vector_view(input_length_tensor);
  auto output_length_data = tensor_vector_view(output_length_tensor);

  for (int col = 0; col < input_tensor.size(1); ++col) {
    value_priority_queue<T> kmax;

    // extract the k-max values with indices
    // for (int row = 0; row < input_tensor.size(0); ++row) {
    for (int row = 0; row < input_length_data[0]; ++row) {
      T value = input_data[row][col];

      if (kmax.size() < k || value > kmax.top().value()) {
        kmax.push(make_index_value(row, value));

        if (kmax.size() > k) {
          kmax.pop();
        }
      }
    }

    // re-order k-max into input order
    std::vector<index_value<T>> kmax_sorted;
    while (!kmax.empty()) {
      kmax_sorted.push_back(kmax.top());
      kmax.pop();
    }
    std::sort(
      kmax_sorted.begin(),
      kmax_sorted.end(),
      index_order<T>());

    // copy the k-max values to the output
    array_col_view<T> output_col = output_data[
      boost::indices[typename array_view<T>::index_range()][col]];
    std::transform(
      kmax_sorted.begin(),
      kmax_sorted.end(),
      output_col.begin(),
      [](index_value<T> const& v) -> T {
        return v.value();
      });

    // copy the k-max indices into switches
    array_col_view<long> switches_col = switches_data[
      boost::indices[typename array_view<long>::index_range()][col]];
    std::transform(
      kmax_sorted.begin(),
      kmax_sorted.end(),
      switches_col.begin(),
      [](index_value<T> const& v) -> long {
        return v.index();
      });
  }

  // update length after pooling
  output_length_data[0] = std::min(input_length_data[0], k);

  lua_pushvalue(L, outputIdx);
  return 1;
}

// Backprop
template <class T>
int updateGradInput(lua_State* L) {
  auto const input = luaGetTensorChecked<T>(L, 2);
  auto const gradOutput_tensor = luaGetTensorChecked<T>(L, 3);
  auto input_length_tensor
    = luaGetFieldIfTensorChecked<long>(L, 1, "input_length");
  auto output_length_tensor
    = luaGetFieldIfTensorChecked<long>(L, 1, "output_length");
  auto const switches_tensor =
    luaGetFieldIfTensorChecked<long>(L, 1, "switches");
  auto gradInput_tensor = luaGetFieldIfTensorChecked<T>(L, 1, "gradInput");
  int gradInputIdx = lua_gettop(L);

  luaL_argcheck(L, gradOutput_tensor.ndims() == 2, 1,
    "gradOutput must have exactly 2 dimensions.");
  luaL_argcheck(L, input_length_tensor.ndims() == 1, 1,
    "input_length must have exactly 1 dimension");
  luaL_argcheck(L, input_length_tensor.size() == 1, 1,
    "input_length must have exactly 1 element");

  long output_length = output_length_tensor.front();

  gradInput_tensor.resizeAs(input);
  gradInput_tensor.fill(0.0);

  auto gradInput_data = tensor_array_view(gradInput_tensor);
  auto const gradOutput_data = tensor_array_view(gradOutput_tensor);
  auto const switches_data = tensor_array_view(switches_tensor);

  long row_limit = std::min(output_length, gradOutput_tensor.size(0));
  for (int row = 0; row < row_limit; ++row) {
    for (int col = 0; col < gradOutput_tensor.size(1); ++col) {
      long input_row = switches_data[row][col];
      gradInput_data[input_row][col] = gradOutput_data[row][col];
    }
  }

  lua_pushvalue(L, gradInputIdx);
  return 1;
}

template <class T>
class Registerer {
 private:
  static const luaL_Reg functions_[];

 public:
  static void registerFunctions(lua_State* L);
};

template <class T>
const luaL_Reg Registerer<T>::functions_[] = {
  {"KMaxPooling_updateOutput", updateOutput<T>},
  {"KMaxPooling_updateGradInput", updateGradInput<T>},
  {nullptr, nullptr},
};

template <class T>
void Registerer<T>::registerFunctions(lua_State* L) {
  luaT_pushmetatable(L, Tensor<T>::kLuaTypeName);
  luaT_registeratname(L, functions_, "nn");
  lua_pop(L, 1);
}

}  // namespace

void initKMaxPooling(lua_State* L) {
  Registerer<float>::registerFunctions(L);
  Registerer<double>::registerFunctions(L);
}

}}}  // namespaces


//////////////////////////////////////////////////////////////////////////////
// random::poisson_ext::devroye::sampler::meta_int_mean.hpp          		//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  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_RANDOM_POISSON_EXT_DEVROYE_SAMPLER_META_INT_MEAN_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_SAMPLER_META_INT_MEAN_HPP_ER_2010
#include <boost/math/policies/policy.hpp>
#include <boost/random/poisson_ext/devroye/detail/step4/choose.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace sampler{

	// Selects one of two versions, standard or squeeze, of Devroye's
    // algorithm for sampling from the poisson with an integer mean.
	//
	// Example :
    // #include <boost/random/poisson_ext/detail/step4/squeeze.hpp
    // meta_int_mean<tag::squeeze>::type poisson(mean);
    //
    // Requirements : see comments inside /step4/*.hpp.
    template<typename Tag,typename Int = int,typename T = double,
    	typename P = boost::math::policies::policy<> >
    struct meta_int_mean : detail::step4::choose<Tag,Int,T,P>{};

}// sampler
}// devroye
}// poisson
}// random
}// boost

#endif


/*
 * Author: Johannes M Dieterich
 */

#ifndef STRESSTENSOR_HPP
#define	STRESSTENSOR_HPP

#include <armadillo>
using namespace std;
using namespace arma;

class StressTensor {

public:
    StressTensor();
    ~StressTensor();

    mat* getTensor();
private:
    mat* _tensor;
};

#endif	/* STRESSTENSOR_HPP */


#include "pch.hpp"
#include <string>
#include <boost/algorithm/string/replace.hpp>
#include "opstruct.h"

std::wstring operation::debugTreeInternal(const std::wstring& curOp) const
{
    std::wstring dbgMsg(L"- ");
    dbgMsg.append(curOp);
    dbgMsg.append(L"\r\n");
    dbgMsg.append(operandA->debugTree());
    dbgMsg.append(L"\r\n");
    dbgMsg.append(operandB->debugTree());
    boost::algorithm::replace_all(dbgMsg, L"\r\n", L"\r\n   ");
    return dbgMsg;
}
void operation::reorderTree()
{
    if (operandA->getPriorityClass() > operandB->getPriorityClass())
        std::swap(operandA,operandB);
    operandA->reorderTree();
    operandB->reorderTree();
};
unsigned __int32 operation::getPriorityClass() const
{
    unsigned __int32 classA, classB;
    classA = operandA->getPriorityClass();
    classB = operandB->getPriorityClass();
    if (classA > classB)
        return classA;
    return classB;

};
operation::operation(std::shared_ptr<criterion> a, std::shared_ptr<criterion> b) : operandA(a), operandB(b)
{}
BOOL andAndClass::include(FileData &file) const
{
    if (this->operandA->include(file) && this->operandB->include(file))
        return true;
    return false;
}
unsigned int andAndClass::directoryCheck(const std::wstring& directory) const
{
    unsigned int resA, resB;
    resA = operandA->directoryCheck(directory);
    resB = operandB->directoryCheck(directory);
    if (resA == DIRECTORY_DONTCARE && resB == DIRECTORY_DONTCARE)
        return DIRECTORY_DONTCARE;
    if (resA == DIRECTORY_DONTCARE)
        return resB;
    if (resB == DIRECTORY_DONTCARE)
        return resA;
    return resA && resB;
}
andAndClass::andAndClass(std::shared_ptr<criterion> a, std::shared_ptr<criterion> b) : operation(a, b)
{}
std::wstring andAndClass::debugTree() const
{
    return debugTreeInternal(L"AND");
}
BOOL orAndClass::include(FileData &file) const
{
    if (this->operandA->include(file))
        return true;
    if (this->operandB->include(file))
        return true;
    return false;
}
unsigned int orAndClass::directoryCheck(const std::wstring& directory) const
{
    unsigned int resA, resB;
    resA = operandA->directoryCheck(directory);
    resB = operandB->directoryCheck(directory);
    if (resA == DIRECTORY_DONTCARE && resB == DIRECTORY_DONTCARE)
        return DIRECTORY_DONTCARE;
    if (resA == DIRECTORY_DONTCARE)
        return resB;
    if (resB == DIRECTORY_DONTCARE)
        return resA;
    return resA || resB;
}
orAndClass::orAndClass(std::shared_ptr<criterion> a, std::shared_ptr<criterion> b) : operation(a, b)
{}
std::wstring orAndClass::debugTree() const
{
    return debugTreeInternal(L"OR");
}
BOOL xorAndClass::include(FileData &file) const
{
    BOOL resA = this->operandA->include(file);
    BOOL resB = this->operandB->include(file);
    return (resA || resB) && !(resA && resB);
}
unsigned int xorAndClass::directoryCheck(const std::wstring& directory) const
{
    unsigned int resA, resB;
    resA = operandA->directoryCheck(directory);
    resB = operandB->directoryCheck(directory);
    if (resA == DIRECTORY_DONTCARE && resB == DIRECTORY_DONTCARE)
        return DIRECTORY_DONTCARE;
    if (resA == DIRECTORY_DONTCARE)
        return resB;
    if (resB == DIRECTORY_DONTCARE)
        return resA;
    return (resA || resB) && !(resA && resB);
}
xorAndClass::xorAndClass(std::shared_ptr<criterion> a, std::shared_ptr<criterion> b) : operation(a, b)
{}
std::wstring xorAndClass::debugTree() const
{
    return debugTreeInternal(L"XOR");
}
void notAndClass::reorderTree()
{
    operand->reorderTree();
}
unsigned __int32 notAndClass::getPriorityClass() const
{
    return operand->getPriorityClass();
}
BOOL notAndClass::include(FileData &file) const
{
    return !(operand->include(file));
}
unsigned int notAndClass::directoryCheck(const std::wstring& directory) const
{
    switch(operand->directoryCheck(directory))
    {
    case DIRECTORY_DONTCARE:
    case DIRECTORY_INCLUDE:
        return DIRECTORY_DONTCARE;
    case DIRECTORY_EXCLUDE:
        return DIRECTORY_INCLUDE;
    }
    return DIRECTORY_DONTCARE;
}
notAndClass::notAndClass(std::shared_ptr<criterion> a) : operand(a)
{}
std::wstring notAndClass::debugTree() const
{
    std::wstring dbgMsg(L"- NOT\r\n");
    dbgMsg.append(operand->debugTree());
    boost::algorithm::replace_all(dbgMsg, L"\r\n", L"\r\n   ");
    return dbgMsg;
}
void bracketClass::reorderTree()
{
    std::sort(expr.begin(), expr.end(),criterionByPriorityClass);
}
unsigned __int32 bracketClass::getPriorityClass() const
{
    //Create a vector with the same size as the expression
    std::vector<__int32> priNumbers(expr.size());
    //Transform the expression items into priority numbers
    std::transform(expr.begin(), expr.end(), priNumbers.begin(), getPriorityFunctor);
    return *(std::max_element(priNumbers.begin(), priNumbers.end()));
}
BOOL bracketClass::include(FileData &file) const
{
    for (register unsigned int idx = 0; idx < expr.size(); idx++)
        if (!expr[idx]->include(file))
            return FALSE;
    return TRUE;
}
unsigned int bracketClass::directoryCheck(const std::wstring& directory)
{
    bool allDontcares = true;
    for (register unsigned int idx = 0; idx < expr.size(); idx++)
    {
        if (!expr[idx]->directoryCheck(directory))
            return DIRECTORY_EXCLUDE;
        if (expr[idx]->directoryCheck(directory) == DIRECTORY_INCLUDE)
            allDontcares = false;
    }
    if (allDontcares)
        return DIRECTORY_DONTCARE;
    else
        return DIRECTORY_INCLUDE;
}
bracketClass::bracketClass(std::vector<std::shared_ptr<criterion> > exprA): expr(exprA)
{
    assert(expr.size() > 1);
}
std::wstring bracketClass::debugTree() const
{
    std::wstring dbgMsg(L"- BRACKET\r\n");
    for (std::vector<std::shared_ptr<criterion> >::const_iterator it = expr.begin(); it != expr.end(); it++)
    {
        dbgMsg.append((*it)->debugTree());
        if (it + 1 != expr.end())
            dbgMsg.append(L"\r\n");
    }
    boost::algorithm::replace_all(dbgMsg, L"\r\n", L"\r\n   ");
    return dbgMsg;
}
void ifClass::reorderTree()
{
    condVal->reorderTree();
    tVal->reorderTree();
    if (fVal)
        fVal->reorderTree();
}
unsigned __int32 ifClass::getPriorityClass() const
{
    unsigned __int32 condValClass, tValClass, fValClass;
    condValClass = condVal->getPriorityClass();
    tValClass = tVal->getPriorityClass();
    if (fVal)
        fValClass = fVal->getPriorityClass();
    else
        fValClass = 0; //Min value means it won't matter in following comparison.
    if (condValClass >= tValClass)
    {
        if (condValClass >= fValClass)
            return condValClass;
        else
            return fValClass;
    } else
    {
        if (tValClass >= fValClass)
            return tValClass;
        else
            return fValClass;
    }
}
BOOL ifClass::include(FileData &file) const
{
    if (condVal->include(file))  //If we have this item
    {
        return tVal->include(file); //Return valueIfTrue
    } else                      //Otherwise
    {
        if (!fVal)              //If there is no ELSE item,
            return true;        //assume true
        return fVal->include(file); //Otherwise return value of the
                                //else part of the node.
    }
}
unsigned int ifClass::directoryCheck(const std::wstring& directory) const
{
    if (condVal->directoryCheck(directory))    //See rationale in include() method
    {
        return tVal->directoryCheck(directory);
    } else
    {
        if (!fVal)
            return DIRECTORY_DONTCARE;
        return fVal->directoryCheck(directory);
    }
}
ifClass::ifClass(std::shared_ptr<criterion> condition,std::shared_ptr<criterion> valueIfTrue,std::shared_ptr<criterion> valueIfFalse)
    : condVal(condition), tVal(valueIfTrue), fVal(valueIfFalse)
{}
std::wstring ifClass::debugTree() const
{
    std::wstring result;

    std::wstring condMsg(L"- IF\r\n");
    condMsg.append(condVal->debugTree());
    boost::algorithm::replace_all(condMsg, L"\r\n", L"\r\n   ");
    result.assign(condMsg);
    result.append(L"\r\n");

    std::wstring thenMsg(L"- THEN\r\n");
    thenMsg.append(tVal->debugTree());
    boost::algorithm::replace_all(thenMsg, L"\r\n", L"\r\n   ");
    result.append(thenMsg);

    std::wstring elseMsg;
    if (fVal)
    {
        result.append(L"\r\n");
        elseMsg.assign(L"- ELSE\r\n");
        elseMsg.append(fVal->debugTree());
        boost::algorithm::replace_all(elseMsg, L"\r\n", L"\r\n   ");
        result.append(elseMsg);
    }

    return result;
}

void operation::makeNonRecursive()
{
    operandA->makeNonRecursive();
    operandB->makeNonRecursive();
}

void notAndClass::makeNonRecursive()
{
    operand->makeNonRecursive();
}

void bracketClass::makeNonRecursive()
{
    for (std::vector<std::shared_ptr<criterion> >::iterator it = expr.begin(); it != expr.end(); it++)
    {
        (*it)->makeNonRecursive();
    }
}

void ifClass::makeNonRecursive()
{
    condVal->makeNonRecursive();
    tVal->makeNonRecursive();
    if (fVal.get())
        fVal->makeNonRecursive();
}


/*
 * Copyright (c) 2020 Johannes Pankert <pankertj@ethz.ch>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of this work 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 OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#include <Eigen/Dense>
#include <perceptive_mpc/kinematics/KinematicsInterface.hpp>

using namespace perceptive_mpc;

template <typename SCALAR_T>
KinematicsInterface<SCALAR_T>::KinematicsInterface(const KinematicInterfaceConfig& config) : config_(config) {}

template <typename SCALAR_T>
void KinematicsInterface<SCALAR_T>::computeState2EndeffectorTransform(Eigen::Matrix<SCALAR_T, 4, 4>& transform,
                                                                      const Eigen::Matrix<SCALAR_T, -1, 1>& state) const {
  if (state.size() != 13) {
    std::stringstream ss;
    ss << "Error: state.size()=" << state.size() << "!=13";
    std::string errorMessage = ss.str();
    std::cerr << std::endl << errorMessage << std::endl << std::endl;
    throw std::runtime_error(ss.str());
  }

  Eigen::Quaternion<SCALAR_T> baseOrientation;
  baseOrientation.coeffs() = state.template head<7>().template head<4>();
  const Eigen::Matrix<SCALAR_T, 3, 1>& basePosition = state.template head<7>().template tail<3>();
  const Eigen::Matrix<SCALAR_T, 6, 1>& armState = state.template tail<6>();

  // homogeneous transform world -> base
  Eigen::Matrix<SCALAR_T, 4, 4> homWorld2Base = Eigen::Matrix<SCALAR_T, 4, 4>::Identity();
  homWorld2Base.template topLeftCorner<3, 3>() = baseOrientation.toRotationMatrix();
  homWorld2Base.template topRightCorner<3, 1>() = basePosition;

  // world -> endeffector
  transform = homWorld2Base * computeBase2EndeffectorTransform(armState);
}

template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 4, 4> KinematicsInterface<SCALAR_T>::computeBase2EndeffectorTransform(
    const Eigen::Matrix<SCALAR_T, 6, 1>& armState) const {
  const Eigen::Matrix<SCALAR_T, 4, 4> armBaseToWrist2Transform = computeArmMountToToolMountTransform(armState);
  return config_.transformBase_X_ArmMount.template cast<SCALAR_T>() * armBaseToWrist2Transform *
         config_.transformToolMount_X_Endeffector.template cast<SCALAR_T>();
}

template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, -1> KinematicsInterface<SCALAR_T>::computeState2MultiplePointsOnRobot(
    const Eigen::Matrix<SCALAR_T, -1, 1>& state, const std::vector<std::vector<double>>& points) const {
  if (state.size() != 13) {
    std::stringstream ss;
    ss << "Error: state.size()=" << state.size() << "!=13";
    std::string errorMessage = ss.str();
    std::cerr << std::endl << errorMessage << std::endl << std::endl;
    throw std::runtime_error(ss.str());
  }

  int dim = 0;
  for (int i = 0; i < points.size(); i++) {
    for (int j = 0; j < points[i].size(); j++) {
      dim++;
    }
  }
  if (dim == 0) {
    return Eigen::Matrix<SCALAR_T, 3, -1>(3, 0);
  }
  Eigen::Matrix<SCALAR_T, 3, -1> result(3, dim);
  int resultIndex = 0;
  int linkIndex;

  Eigen::Quaternion<SCALAR_T> baseOrientation;
  baseOrientation.coeffs() = state.template head<7>().template head<4>();
  const Eigen::Matrix<SCALAR_T, 3, 1>& basePosition = state.template head<7>().template tail<3>();
  const Eigen::Matrix<SCALAR_T, 6, 1>& armState = state.template tail<6>();

  Eigen::Matrix<SCALAR_T, 4, 4> worldXFrBase = Eigen::Matrix<SCALAR_T, 4, 4>::Identity();
  worldXFrBase.template topLeftCorner<3, 3>() = baseOrientation.toRotationMatrix();
  worldXFrBase.template topRightCorner<3, 1>() = basePosition;

  return computeArmState2MultiplePointsOnRobot(armState, points, config_.transformBase_X_ArmMount, config_.transformToolMount_X_Endeffector,
                                               worldXFrBase);
}

template <typename SCALAR_T>
Eigen::Quaternion<SCALAR_T> KinematicsInterface<SCALAR_T>::matrixToQuaternion(const Eigen::Matrix<SCALAR_T, 3, 3>& R) const {
  SCALAR_T t1, t2, t;
  SCALAR_T x1, x2, x;
  SCALAR_T y1, y2, y;
  SCALAR_T z1, z2, z;
  SCALAR_T w1, w2, w;

  t1 = CppAD::CondExpGt(R(0, 0), R(1, 1), 1 + R(0, 0) - R(1, 1) - R(2, 2), 1 - R(0, 0) + R(1, 1) - R(2, 2));
  t2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), 1 - R(0, 0) - R(1, 1) + R(2, 2), 1 + R(0, 0) + R(1, 1) + R(2, 2));
  t = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), t1, t2);

  x1 = CppAD::CondExpGt(R(0, 0), R(1, 1), t, R(1, 0) + R(0, 1));
  x2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), R(0, 2) + R(2, 0), R(2, 1) - R(1, 2));
  x = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), x1, x2);

  y1 = CppAD::CondExpGt(R(0, 0), R(1, 1), R(1, 0) + R(0, 1), t);
  y2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), R(2, 1) + R(1, 2), R(0, 2) - R(2, 0));
  y = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), y1, y2);

  z1 = CppAD::CondExpGt(R(0, 0), R(1, 1), R(0, 2) + R(2, 0), R(2, 1) + R(1, 2));
  z2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), t, R(1, 0) - R(0, 1));
  z = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), z1, z2);

  w1 = CppAD::CondExpGt(R(0, 0), R(1, 1), R(2, 1) - R(1, 2), R(0, 2) - R(2, 0));
  w2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), R(1, 0) - R(0, 1), t);
  w = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), w1, w2);

  Eigen::Matrix<SCALAR_T, 4, 1> q({x, y, z, w});
  q *= 0.5 / sqrt(t);

  Eigen::Quaternion<SCALAR_T> quaternion;
  quaternion.x() = q(0);
  quaternion.y() = q(1);
  quaternion.z() = q(2);
  quaternion.w() = q(3);

  return quaternion;
}
template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, 1> KinematicsInterface<SCALAR_T>::getCOMBaseFrame(const Eigen::Matrix<SCALAR_T, -1, 1>& state) const {
  const Eigen::Matrix<SCALAR_T, 6, 1>& armState = state.template tail<6>();
  Eigen::Matrix<SCALAR_T, 4, 1> armCOM = Eigen::Matrix<SCALAR_T, 4, 1>::Ones();
  armCOM.template head<3>() = getArmCOM(armState);
  Eigen::Matrix<SCALAR_T, 3, 1> armCOMBaseFrame = (config_.transformBase_X_ArmMount.template cast<SCALAR_T>() * armCOM).template head<3>();
  double armMass = getArmMass();
  return ((SCALAR_T)armMass * armCOMBaseFrame + (config_.baseMass * config_.baseCOM).template cast<SCALAR_T>()) /
         (SCALAR_T)(armMass + config_.baseMass);
}
template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, 1> KinematicsInterface<SCALAR_T>::getZMPBaseFrame(
    const Eigen::Matrix<SCALAR_T, -1, 1>& state, const Eigen::Matrix<SCALAR_T, 7, 1>& eePoseReference,
    const Eigen::Matrix<SCALAR_T, 6, 1>& wrenchReferenceEEFrame) const {
  using vector3_t = Eigen::Matrix<SCALAR_T, 3, 1>;
  using hom_transform_t = Eigen::Matrix<SCALAR_T, 4, 4>;
  vector3_t rcog = getCOMBaseFrame(state);
  double robotMass = config_.baseMass + getArmMass();

  // convert reference force from fixed ee frame to base frame
  hom_transform_t worldToEndeffectorReference = hom_transform_t::Zero();
  worldToEndeffectorReference.template block<3, 3>(0, 0) =
      Eigen::Quaternion<SCALAR_T>(eePoseReference.template head<4>()).toRotationMatrix();
  worldToEndeffectorReference.template block<3, 1>(0, 3) = eePoseReference.template tail<3>();
  worldToEndeffectorReference(3, 3) = 1;

  hom_transform_t worldToBase = getWorldToBaseTransform(state);
  hom_transform_t baseToWorld = worldToBase;
  baseToWorld.template block<3, 3>(0, 0) = worldToBase.template block<3, 3>(0, 0).transpose();
  baseToWorld.template block<3, 1>(0, 3) = -baseToWorld.template block<3, 3>(0, 0) * worldToBase.template block<3, 1>(0, 3);

  hom_transform_t baseToEndeffectorReference = baseToWorld * worldToEndeffectorReference;
  vector3_t forceBaseFrame = baseToEndeffectorReference.template block<3, 3>(0, 0) * wrenchReferenceEEFrame.template head<3>();
  vector3_t torqueBaseFrame = baseToEndeffectorReference.template block<3, 3>(0, 0) * wrenchReferenceEEFrame.template tail<3>();

  // compute a virtual mass for the acting end-effector force
  const double g = 9.81;
  vector3_t fg;
  fg << (SCALAR_T)0, (SCALAR_T)0, (SCALAR_T)-robotMass * g;

  // compute translation to the virtual mass in base frame
  hom_transform_t base_X_Endeffector = computeBase2EndeffectorTransform(state.template tail<6>());
  vector3_t ree = base_X_Endeffector.template block<3, 1>(0, 3);

  vector3_t n;
  n << (SCALAR_T)0, (SCALAR_T)0, (SCALAR_T)1;

  return n.template cross(rcog.cross(fg) + ree.cross(forceBaseFrame) + torqueBaseFrame) / n.dot(fg + forceBaseFrame);
}

template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 4, 4> KinematicsInterface<SCALAR_T>::getWorldToBaseTransform(const Eigen::Matrix<SCALAR_T, -1, 1>& state) const {
  Eigen::Quaternion<SCALAR_T> baseOrientation;
  baseOrientation.coeffs() = state.template head<7>().template head<4>();
  const Eigen::Matrix<SCALAR_T, 3, 1>& basePosition = state.template head<7>().template tail<3>();

  // homogeneous transform world -> base
  Eigen::Matrix<SCALAR_T, 4, 4> homWorld2Base = Eigen::Matrix<SCALAR_T, 4, 4>::Identity();
  homWorld2Base.template topLeftCorner<3, 3>() = baseOrientation.toRotationMatrix();
  homWorld2Base.template topRightCorner<3, 1>() = basePosition;
  return homWorld2Base;
}

template class perceptive_mpc::KinematicsInterface<double>;
template class perceptive_mpc::KinematicsInterface<CppAD::AD<CppAD::cg::CG<double>>>;


#include <frovedis.hpp>
#include <frovedis/matrix/sliced_matrix.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>

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

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    auto bm = make_blockcyclic_matrix_load<float> ("./sample_4x4");

    // creating slice blockcyclic matrix from entire matrix
    sliced_blockcyclic_matrix<float> sm (bm);

    // scaling entire slice with 2
    auto col = make_col_vector(sm,0);
    scal<float> (col, 2);

    // scaling entire slice with 2
    auto row = make_row_vector(sm,3);
    scal<float> (row, 5);

    // checking whether operation can be done on sliced matrix correctly
    bm.save("./out_4x4");
    BOOST_CHECK (system("diff ./out_4x4 ./ref_4x4") == 0);
    system("rm -f ./out_4x4");
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is 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_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_EXPAND_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_EXPAND_HPP

#include <cstddef>
#include <functional>

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

#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace expand
{

template
<
    std::size_t Dimension, std::size_t DimensionCount
>
struct nsphere_loop
{
    template <typename Box, typename NSphere>
    static inline void apply(Box& box, NSphere const& source)
    {
        typedef typename select_coordinate_type<NSphere, Box>::type coordinate_type;

        std::less<coordinate_type> const less;
        std::greater<coordinate_type> const greater;

        coordinate_type const min_coord = get<Dimension>(source) - get_radius<0>(source);
        coordinate_type const max_coord = get<Dimension>(source) + get_radius<0>(source);

        if (less(min_coord, get<min_corner, Dimension>(box)))
        {
            set<min_corner, Dimension>(box, min_coord);
        }

        if (greater(max_coord, get<max_corner, Dimension>(box)))
        {
            set<max_corner, Dimension>(box, max_coord);
        }

        nsphere_loop
            <
                Dimension + 1, DimensionCount
            >::apply(box, source);
    }
};

template
<
    std::size_t DimensionCount
>
struct nsphere_loop
    <
        DimensionCount, DimensionCount
    >
{
    template <typename Box, typename NSphere>
    static inline void apply(Box&, NSphere const&) {}
};

}} // namespace detail::expand
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// Box + Nsphere -> new box containing also nsphere
template
<
    typename BoxOut, typename NSphere
>
struct expand<BoxOut, NSphere, box_tag, nsphere_tag>
    : detail::expand::nsphere_loop
        <
            0, dimension<NSphere>::type::value
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_ALGORITHMS_EXPAND_HPP


// Licensed to the Apache Software Foundation (ASF) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  The ASF licenses this file
// to you 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 "kudu/common/key_util.h"

#include <cmath>
#include <cstring>
#include <iterator>
#include <limits>
#include <ostream>
#include <string>
#include <tuple>
#include <type_traits>

#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <glog/logging.h>

#include "kudu/common/column_predicate.h"
#include "kudu/common/common.pb.h"
#include "kudu/common/key_encoder.h"
#include "kudu/common/row.h"
#include "kudu/common/schema.h"
#include "kudu/common/types.h"
#include "kudu/gutil/map-util.h"
#include "kudu/gutil/mathlimits.h"
#include "kudu/gutil/port.h"
#include "kudu/util/memory/arena.h"
#include "kudu/util/slice.h"

using std::nextafter;
using std::numeric_limits;
using std::string;
using std::tuple;
using std::unordered_map;
using std::vector;

namespace kudu {
namespace key_util {

namespace {

bool IncrementBoolCell(void* cell_ptr) {
  bool orig;
  memcpy(&orig, cell_ptr, sizeof(bool));
  if (!orig) {
    bool inc = true;
    memcpy(cell_ptr, &inc, sizeof(bool));
    return true;
  } else {
    return false;
  }
}

bool DecrementBoolCell(void* cell_ptr) {
  bool orig;
  memcpy(&orig, cell_ptr, sizeof(bool));
  if (orig) {
    bool dec = false;
    memcpy(cell_ptr, &dec, sizeof(bool));
    return true;
  }
  return false;
}

template<DataType type>
ATTRIBUTE_NO_SANITIZE_INTEGER
bool IncrementIntCell(void* cell_ptr) {
  typedef DataTypeTraits<type> traits;
  typedef typename traits::cpp_type cpp_type;

  cpp_type orig;
  memcpy(&orig, cell_ptr, sizeof(cpp_type));

  cpp_type inc;
  if (std::is_unsigned<cpp_type>::value) {
    inc = orig + 1;
  } else {
    // Signed overflow is undefined in C. So, we'll use a branch here
    // instead of counting on undefined behavior.
    if (orig == MathLimits<cpp_type>::kMax) {
      inc = MathLimits<cpp_type>::kMin;
    } else {
      inc = orig + 1;
    }
  }
  memcpy(cell_ptr, &inc, sizeof(cpp_type));
  return inc > orig;
}

template<DataType type>
ATTRIBUTE_NO_SANITIZE_INTEGER
bool DecrementIntCell(void* cell_ptr) {
  typedef DataTypeTraits<type> traits;
  typedef typename traits::cpp_type cpp_type;

  cpp_type orig;
  memcpy(&orig, cell_ptr, sizeof(cpp_type));

  cpp_type dec;
  if (std::is_unsigned<cpp_type>::value) {
    dec = orig - 1;
  } else {
    // Signed overflow is undefined in C. So, we'll use a branch here
    // instead of counting on undefined behavior.
    if (orig == MathLimits<cpp_type>::kMin) {
      dec = MathLimits<cpp_type>::kMax;
    } else {
      dec = orig - 1;
    }
  }
  if (dec < orig) {
    memcpy(cell_ptr, &dec, sizeof(cpp_type));
    return true;
  }
  return false;
}

template<DataType type>
bool IncrementFloatingPointCell(void* cell_ptr) {
  typedef DataTypeTraits<type> traits;
  typedef typename traits::cpp_type cpp_type;

  cpp_type orig;
  memcpy(&orig, cell_ptr, sizeof(cpp_type));
  cpp_type inc = nextafter(orig, numeric_limits<cpp_type>::infinity());
  memcpy(cell_ptr, &inc, sizeof(cpp_type));
  return inc != orig;
}

template<DataType type>
bool DecrementFloatingPointCell(void* cell_ptr) {
  typedef DataTypeTraits<type> traits;
  typedef typename traits::cpp_type cpp_type;

  cpp_type orig;
  memcpy(&orig, cell_ptr, sizeof(cpp_type));
  cpp_type dec = nextafter(orig, -numeric_limits<cpp_type>::infinity());
  if (dec != orig) {
    memcpy(cell_ptr, &dec, sizeof(cpp_type));
  }
  return false;
}

bool IncrementStringCell(void* cell_ptr, Arena* arena) {
  Slice orig;
  memcpy(&orig, cell_ptr, sizeof(orig));
  uint8_t* new_buf = CHECK_NOTNULL(
      static_cast<uint8_t*>(arena->AllocateBytes(orig.size() + 1)));
  memcpy(new_buf, orig.data(), orig.size());
  new_buf[orig.size()] = '\0';

  Slice inc(new_buf, orig.size() + 1);
  memcpy(cell_ptr, &inc, sizeof(inc));
  return true;
}

bool DecrementStringCell(void* cell_ptr) {
  Slice orig;
  memcpy(&orig, cell_ptr, sizeof(orig));
  if (orig.size() == 0 || orig[orig.size() - 1] != '\0') {
    return false;
  }
  orig.truncate(orig.size() - 1);
  memcpy(cell_ptr, &orig, sizeof(orig));
  return true;
}

template<typename ColIdxIter>
void SetKeyToMinValues(ColIdxIter first, ColIdxIter last, ContiguousRow* row) {
  for (auto col_idx_it = first; col_idx_it != last; std::advance(col_idx_it, 1)) {
    DCHECK_LE(0, *col_idx_it);
    const ColumnSchema& col = row->schema()->column(*col_idx_it);
    col.type_info()->CopyMinValue(row->mutable_cell_ptr(*col_idx_it));
  }
}

// Increments a key with the provided column indices to the smallest key which
// is greater than the current key.
template<typename ColIdxIter>
bool IncrementKey(ColIdxIter first,
                  ColIdxIter last,
                  ContiguousRow* row,
                  Arena* arena) {
  for (auto col_idx_it = std::prev(last);
       std::distance(first, col_idx_it) >= 0;
       std::advance(col_idx_it, -1)) {
    if (IncrementCell(row->schema()->column(*col_idx_it),
                      row->mutable_cell_ptr(*col_idx_it), arena)) {
      return true;
    }
  }
  return false;
}

template<typename ColIdxIter>
int PushUpperBoundKeyPredicates(ColIdxIter first,
                                ColIdxIter last,
                                const unordered_map<string, ColumnPredicate>& predicates,
                                ContiguousRow* row,
                                Arena* arena) {

  const Schema& schema = *CHECK_NOTNULL(row->schema());
  int pushed_predicates = 0;

  // Step 1: copy predicates into the row in key column order, stopping after the first missing
  // predicate or range predicate which can't be transformed to an inclusive upper bound.
  bool break_loop = false;
  bool is_inclusive_bound = true;
  for (auto col_idx_it = first; !break_loop && col_idx_it < last; std::advance(col_idx_it, 1)) {
    const ColumnSchema& column = schema.column(*col_idx_it);
    const ColumnPredicate* predicate = FindOrNull(predicates, column.name());
    if (predicate == nullptr) break;
    size_t size = column.type_info()->size();
    switch (predicate->predicate_type()) {
      case PredicateType::Equality:
        memcpy(row->mutable_cell_ptr(*col_idx_it), predicate->raw_lower(), size);
        pushed_predicates++;
        break;
      case PredicateType::Range:
        if (predicate->raw_upper() != nullptr) {
          memcpy(row->mutable_cell_ptr(*col_idx_it), predicate->raw_upper(), size);
          pushed_predicates++;
          // Try to decrease because upper bound is exclusive.
          if (!TryDecrementCell(row->schema()->column(*col_idx_it),
                                row->mutable_cell_ptr(*col_idx_it))) {
            is_inclusive_bound = false;
            break_loop = true;
          }
        } else {
          break_loop = true;
        }
        break;
      case PredicateType::IsNotNull: // Fallthrough intended
      case PredicateType::IsNull:
        break_loop = true;
        break;
      case PredicateType::InList:
        // Since the InList predicate is a sorted vector of values, the last
        // value provides an inclusive upper bound that can be pushed.
        DCHECK(!predicate->raw_values().empty());
        memcpy(row->mutable_cell_ptr(*col_idx_it), predicate->raw_values().back(), size);
        pushed_predicates++;
        break;
      case PredicateType::None:
        LOG(FATAL) << "NONE predicate can not be pushed into key";
    }
  }

  // If no predicates were pushed, no need to do any more work.
  if (pushed_predicates == 0) { return 0; }

  // Step 2: If the upper bound is inclusive, increment it to become exclusive.
  if (is_inclusive_bound) {
    if (!IncrementKey(first, std::next(first, pushed_predicates), row, arena)) {
      // If the increment fails then this bound is is not constraining the keyspace.
      return 0;
    }
  }

  // Step 3: Fill the remaining columns without predicates with the min value.
  SetKeyToMinValues(std::next(first, pushed_predicates), last, row);
  return pushed_predicates;
}

template<typename ColIdxIter>
int PushLowerBoundKeyPredicates(ColIdxIter first,
                                ColIdxIter last,
                                const unordered_map<string, ColumnPredicate>& predicates,
                                ContiguousRow* row,
                                Arena* arena) {
  const Schema& schema = *CHECK_NOTNULL(row->schema());
  int pushed_predicates = 0;

  // Step 1: copy predicates into the row in key column order, stopping after
  // the first missing predicate.

  bool break_loop = false;
  for (auto col_idx_it = first; !break_loop && col_idx_it < last; std::advance(col_idx_it, 1)) {
    const ColumnSchema& column = schema.column(*col_idx_it);
    const ColumnPredicate* predicate = FindOrNull(predicates, column.name());
    if (predicate == nullptr) break;
    size_t size = column.type_info()->size();

    switch (predicate->predicate_type()) {
      case PredicateType::Range:
        if (predicate->raw_lower() == nullptr) {
          break_loop = true;
          break;
        }
        // Fall through.
      case PredicateType::Equality:
        memcpy(row->mutable_cell_ptr(*col_idx_it), predicate->raw_lower(), size);
        pushed_predicates++;
        break;
      case PredicateType::IsNotNull: // Fallthrough intended
      case PredicateType::IsNull:
        break_loop = true;
        break;
      case PredicateType::InList:
        // Since the InList predicate is a sorted vector of values, the first
        // value provides an inclusive lower bound that can be pushed.
        DCHECK(!predicate->raw_values().empty());
        memcpy(row->mutable_cell_ptr(*col_idx_it), predicate->raw_values().front(), size);
        pushed_predicates++;
        break;
      case PredicateType::None:
        LOG(FATAL) << "NONE predicate can not be pushed into key";
    }
  }

  // If no predicates were pushed, no need to do any more work.
  if (pushed_predicates == 0) { return 0; }

  // Step 2: Fill the remaining columns without predicates with the min value.
  SetKeyToMinValues(std::next(first, pushed_predicates), last, row);
  return pushed_predicates;
}
} // anonymous namespace

bool IncrementPrimaryKey(ContiguousRow* row, Arena* arena) {
  return IncrementPrimaryKey(row, row->schema()->num_key_columns(), arena);
}

bool IncrementPrimaryKey(ContiguousRow* row, int32_t num_columns, Arena* arena) {
  return IncrementKey(boost::make_counting_iterator(0),
                      boost::make_counting_iterator(num_columns),
                      row,
                      arena);
}

bool IncrementCell(const ColumnSchema& col, void* cell_ptr, Arena* arena) {
  DataType type = col.type_info()->physical_type();
  switch (type) {
    case BOOL:
      return IncrementBoolCell(cell_ptr);
#define HANDLE_TYPE(t) case t: return IncrementIntCell<t>(cell_ptr);
    HANDLE_TYPE(UINT8);
    HANDLE_TYPE(UINT16);
    HANDLE_TYPE(UINT32);
    HANDLE_TYPE(UINT64);
    HANDLE_TYPE(INT8);
    HANDLE_TYPE(INT16);
    HANDLE_TYPE(INT32);
    HANDLE_TYPE(UNIXTIME_MICROS);
    HANDLE_TYPE(INT64);
    HANDLE_TYPE(INT128);
    case FLOAT:
      return IncrementFloatingPointCell<FLOAT>(cell_ptr);
    case DOUBLE:
      return IncrementFloatingPointCell<DOUBLE>(cell_ptr);
    case STRING:
    case BINARY:
      return IncrementStringCell(cell_ptr, arena);
    case UNKNOWN_DATA:
    default: LOG(FATAL) << "Unknown data type: " << type;
  }
  return false; // unreachable
#undef HANDLE_TYPE
}

bool TryDecrementCell(const ColumnSchema &col, void *cell_ptr) {
  DataType type = col.type_info()->physical_type();
  switch (type) {
    case BOOL:
      return DecrementBoolCell(cell_ptr);
#define HANDLE_TYPE(t) case t: return DecrementIntCell<t>(cell_ptr);
    HANDLE_TYPE(UINT8);
    HANDLE_TYPE(UINT16);
    HANDLE_TYPE(UINT32);
    HANDLE_TYPE(UINT64);
    HANDLE_TYPE(INT8);
    HANDLE_TYPE(INT16);
    HANDLE_TYPE(INT32);
    HANDLE_TYPE(UNIXTIME_MICROS);
    HANDLE_TYPE(INT64);
    HANDLE_TYPE(INT128);
    case FLOAT:
      return DecrementFloatingPointCell<FLOAT>(cell_ptr);
    case DOUBLE:
      return DecrementFloatingPointCell<DOUBLE>(cell_ptr);
    case STRING:
    case BINARY:
      return DecrementStringCell(cell_ptr);
    case UNKNOWN_DATA:
    default: LOG(FATAL) << "Unknown data type: " << type;
  }
  return false; // unreachable
#undef HANDLE_TYPE
}

int PushLowerBoundKeyPredicates(const vector<int32_t>& col_idxs,
                                const unordered_map<string, ColumnPredicate>& predicates,
                                ContiguousRow* row,
                                Arena* arena) {
  return PushLowerBoundKeyPredicates(col_idxs.begin(), col_idxs.end(), predicates, row, arena);
}

int PushUpperBoundKeyPredicates(const vector<int32_t>& col_idxs,
                                const unordered_map<string, ColumnPredicate>& predicates,
                                ContiguousRow* row,
                                Arena* arena) {
  return PushUpperBoundKeyPredicates(col_idxs.begin(), col_idxs.end(), predicates, row, arena);
}

int PushLowerBoundPrimaryKeyPredicates(const unordered_map<string, ColumnPredicate>& predicates,
                                       ContiguousRow* row,
                                       Arena* arena) {
  int32_t num_pk_cols = row->schema()->num_key_columns();
  return PushLowerBoundKeyPredicates(boost::make_counting_iterator(0),
                                     boost::make_counting_iterator(num_pk_cols),
                                     predicates,
                                     row,
                                     arena);
}

int PushUpperBoundPrimaryKeyPredicates(const unordered_map<string, ColumnPredicate>& predicates,
                                       ContiguousRow* row,
                                       Arena* arena) {
  int32_t num_pk_cols = row->schema()->num_key_columns();
  return PushUpperBoundKeyPredicates(boost::make_counting_iterator(0),
                                     boost::make_counting_iterator(num_pk_cols),
                                     predicates,
                                     row,
                                     arena);
}

void EncodeKey(const vector<int32_t>& col_idxs, const ContiguousRow& row, string* buffer) {
  for (int i = 0; i < col_idxs.size(); i++) {
    int32_t col_idx = col_idxs[i];
    const auto& encoder = GetKeyEncoder<string>(row.schema()->column(col_idx).type_info());
    encoder.Encode(row.cell_ptr(col_idx), i + 1 == col_idxs.size(), buffer);
  }
}

} // namespace key_util
} // namespace kudu


// Copyright (c) 2020 Bitcoin Association
// Distributed under the Open BSV software license, see the accompanying file
// LICENSE.

#include "test/test_bitcoin.h"
#include "mempool_test_access.h"

#include "txmempool.h"
#include "txmempoolevictioncandidates.h"

#include <boost/test/unit_test.hpp>
#include <deque>

CTxMemPoolEntry MakeEntry(
    double feerate,
    const std::vector<std::tuple<TxId, int, Amount>>& inChainInputs,
    const std::vector<std::tuple<CTransactionRef, int>>& inMempoolInputs,
    size_t nOutputs, size_t opReturnSize=0)
{
    CMutableTransaction tx;
    Amount totalInput;
    for(const auto& [id, ndx, amount]: inChainInputs)
    {
        tx.vin.push_back(CTxIn(id, ndx, CScript()));
        totalInput += amount;
    }

    for(const auto& [txInput, ndx]: inMempoolInputs)
    {
        tx.vin.push_back(CTxIn(txInput->GetId(), ndx, CScript()));
        totalInput += txInput->vout[ndx].nValue;
    }

    for(size_t i = 0; i < nOutputs; i++)
    {
        CScript script;
        script << OP_TRUE;
        tx.vout.push_back(CTxOut(Amount(), script));
    }

    if(opReturnSize > 0)
    {
        CScript script;
        script << OP_RETURN;
        script << std::vector<uint8_t>(opReturnSize);
        tx.vout.push_back(CTxOut(Amount(), script));
    }

    auto txSize = ::GetSerializeSize(tx, SER_NETWORK, PROTOCOL_VERSION);
    auto totalFee = Amount(int64_t( feerate * txSize) / nOutputs * nOutputs);
    auto perOutput = (totalInput - totalFee) / int64_t(nOutputs);

    for(auto& output: tx.vout)
    {
        output.nValue = perOutput;
    }

    auto txRef = MakeTransactionRef(tx);
    CTxMemPoolEntry entry {txRef, totalFee, int64_t{0}, 0, false, LockPoints{}};
    return entry;
}

class MempoolMockup
{
public:
    std::optional<CEvictionCandidateTracker> tracker;
    CTxMemPoolTestAccess::txlinksMap links;
    CTxMemPool::indexed_transaction_set mapTx;

    CTxMemPoolTestAccess::txiter AddTx(CTxMemPoolEntry entry)
    {
        auto[iter, sucess] = mapTx.insert(std::move(entry));
        BOOST_ASSERT(sucess);

        CTxMemPoolTestAccess::TxLinks txlinks;
        for(const auto& input: iter->GetSharedTx()->vin)
        {
            auto inputid = input.prevout.GetTxId();
            auto it = mapTx.find(inputid);
            if(it == mapTx.end())
            {
                continue;
            }
            txlinks.parents.insert(it);
            links[it].children.insert(iter);
        }

        links.insert(std::make_pair(iter,txlinks));

        if(tracker)
        {
            tracker->EntryAdded(iter);
        }
        return iter;
    }

    void AddGroup(std::vector<CTxMemPoolEntry> entries)
    {
        SecondaryMempoolEntryData groupData;
        std::vector<CTxMemPoolTestAccess::txiter> iters;
        CTxMemPoolTestAccess::txiter lastIt;
        for(auto& entry: entries)
        {
            groupData.fee += entry.GetFee();
            groupData.size += entry.GetTxSize();
            auto it = AddTx(entry);
            iters.push_back(it);
            lastIt = it;
        }

        auto group = std::make_shared<CPFPGroup>(groupData, std::vector<CTxMemPoolTestAccess::txiter>(iters));
        for(auto iter: iters)
        {
            mapTx.modify(iter, [&group](CTxMemPoolEntry& entry) {
                                    CTestTxMemPoolEntry(entry).group() = group;
                                    CTestTxMemPoolEntry(entry).groupingData() = std::nullopt;
                                });
        }

        if(tracker)
        {
            tracker->EntryModified(lastIt);
        }
    }

    void RemoveTx(CTxMemPoolTestAccess::txiter entry)
    {
        auto linksIter = links.find(entry);

        assert(linksIter != links.end());
        assert(linksIter->second.children.empty());

        auto parents = linksIter->second.parents;
        auto txId = entry->GetTxId();

        if(entry->IsCPFPGroupMember())
        {
            for(auto it: entry->GetCPFPGroup()->Transactions())
            {
                CTestTxMemPoolEntry(const_cast<CTxMemPoolEntry&>(*it)).group().reset();
            }
        }

        for(const auto& parent: linksIter->second.parents)
        {
            auto parentLinksIter = links.find(parent);
            if(parentLinksIter == links.end())
            {
                continue;
            }
            parentLinksIter->second.children.erase(entry);
        }

        links.erase(linksIter);
        mapTx.erase(entry);

        if(tracker)
        {
            tracker->EntryRemoved(txId, parents);
        }
    }

    void InitializeTracker()
    {
        tracker =
            CEvictionCandidateTracker(
                links,
                [](CTxMemPoolTestAccess::txiter entry)
                {
                    int64_t score = entry->GetFee().GetSatoshis() * 100000 / entry->GetTxSize();
                    if(!entry->IsInPrimaryMempool())
                    {
                        score += std::numeric_limits<int64_t>::min();
                    }
                    return score;
                });
    }

    void RemoveMostWorthless()
    {
        auto iter = tracker->GetMostWorthless();
        RemoveTx(iter);
    }
};

BOOST_FIXTURE_TEST_SUITE(eviction_candidates_tracker_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(single_long_chain) {
    MempoolMockup mempool;
    auto confirmedEntry = std::make_tuple<TxId, int, Amount>(TxId(), 0, Amount(10000000));
    auto entry = MakeEntry(1,{confirmedEntry}, {}, 1, 0);
    mempool.AddTx(entry);

    for(int n = 0; n < 2; n++)
    {
        auto oldEntry = entry;
        std::deque<uint256> addedTransactions;
        for(int i = 0; i < 100; i++)
        {
            auto newEntry = MakeEntry(1, {}, { std::make_tuple<CTransactionRef, int>(oldEntry.GetSharedTx(), 0)}, 1, 0);
            mempool.AddTx(newEntry);
            addedTransactions.push_back(newEntry.GetSharedTx()->GetId());
            oldEntry = newEntry;
            if(mempool.tracker)
            {
                BOOST_ASSERT(mempool.tracker->GetAllCandidates().size() == 1);
            }
        }

        if(!mempool.tracker)
        {
            mempool.InitializeTracker();
        }

        std::deque<uint256> removedTransactions;
        for(int i = 0; i < 100; i++)
        {
            auto txToRemove = mempool.tracker->GetMostWorthless();
            auto txId = txToRemove->GetSharedTx()->GetId();
            removedTransactions.push_front(txId);
            mempool.RemoveTx(txToRemove);
            BOOST_ASSERT(mempool.tracker->GetAllCandidates().size() == 1);
        }

        BOOST_ASSERT(addedTransactions == removedTransactions);
    }

}

BOOST_AUTO_TEST_CASE(broad_tree) {
    MempoolMockup mempool;
    auto confirmedEntry = std::make_tuple<TxId, int, Amount>(TxId(), 0, Amount(100000000));
    auto entry = MakeEntry(1,{confirmedEntry}, {}, 100, 0);
    mempool.AddTx(entry);

    for(int n = 0; n < 2; n++)
    {
        for(size_t i = 0; i < 100; i++)
        {
            auto feerate = 100 + ((i % 2 == 0) ? (i * 0.1) : (i * -0.1));
            auto newEntry = MakeEntry(feerate, {}, { std::make_tuple<CTransactionRef, int>(entry.GetSharedTx(), std::move(i))}, 1, 0);
            mempool.AddTx(newEntry);
            if(mempool.tracker)
            {
                BOOST_ASSERT(mempool.tracker->GetAllCandidates().size() == (i + 1));
            }
        }

        if(!mempool.tracker)
        {
            mempool.InitializeTracker();
        }

        double lastRemovedFeeRate = 0;
        for(size_t i = 0; i < 100; i++)
        {
            BOOST_ASSERT(mempool.tracker->GetAllCandidates().size() == (100-i));
            auto txToRemove = mempool.tracker->GetMostWorthless();
            double feeRate = double(txToRemove->GetFee().GetSatoshis()) / txToRemove->GetTxSize();
            mempool.RemoveTx(txToRemove);
            BOOST_ASSERT(feeRate >= lastRemovedFeeRate);
            lastRemovedFeeRate = feeRate;
        }
    }
}

BOOST_AUTO_TEST_CASE(secondary_mempool_first) {
    MempoolMockup mempool;
    auto confirmedEntry = std::make_tuple<TxId, int, Amount>(TxId(), 0, Amount(10000000));
    auto entry = MakeEntry(1,{confirmedEntry}, {}, 100, 0);
    mempool.AddTx(entry);

    for(int i = 0; i < 100; i++)
    {
        auto newEntry = MakeEntry(100 + (i * 0.1), {}, { std::make_tuple<CTransactionRef, int>(entry.GetSharedTx(), std::move(i))}, 1, 0);
        if(i % 2 == 0)
        {
            CTestTxMemPoolEntry(newEntry).groupingData() =
                SecondaryMempoolEntryData{newEntry.GetFee(), Amount(), newEntry.GetTxSize(), 0};
        }
        mempool.AddTx(newEntry);
    }

    mempool.InitializeTracker();

    double lastRemovedFeeRate = 0;
    bool lastFromSecondary = true;
    for(int i = 0; i < 100; i++)
    {
        auto txToRemove = mempool.tracker->GetMostWorthless();
        double feeRate = double(txToRemove->GetFee().GetSatoshis()) / txToRemove->GetTxSize();
        bool fromSecondary = !txToRemove->IsInPrimaryMempool();
        mempool.RemoveMostWorthless();
        if(fromSecondary)
        {
            BOOST_ASSERT(lastFromSecondary);
            BOOST_ASSERT(feeRate >= lastRemovedFeeRate);
        }
        else
        {
            if(lastFromSecondary)
            {
                // txToRemove is from primary but transaction from last round it is from secondary
                // transaction from last round it is evicted first despite of higher fee
                BOOST_ASSERT(feeRate < lastRemovedFeeRate);
            }
            else
            {
                // txToRemove is from primary as well as tx from last round
                BOOST_ASSERT(feeRate >= lastRemovedFeeRate);
            }
        }
        lastFromSecondary = fromSecondary;
        lastRemovedFeeRate = feeRate;
    }
}

BOOST_AUTO_TEST_CASE(group) {
    MempoolMockup mempool;
    std::vector<std::tuple<TxId, int, Amount>> confirmedInputs = {
        std::make_tuple<TxId, int, Amount>(TxId(), 0, Amount(10000000)),
        std::make_tuple<TxId, int, Amount>(TxId(), 1, Amount(10000000)),
        std::make_tuple<TxId, int, Amount>(TxId(), 2, Amount(10000000)),
        std::make_tuple<TxId, int, Amount>(TxId(), 3, Amount(10000000))
    };

    std::vector<CTxMemPoolEntry> group;
    std::vector<std::tuple<CTransactionRef, int>> inMempoolInputs;
    for(int i = 0; i < 4; i++)
    {
        group.push_back(MakeEntry(0,{confirmedInputs[i]}, {}, 2, 1000));
        inMempoolInputs.push_back(std::make_tuple(group.back().GetSharedTx(), 0));
    }
    group.push_back(MakeEntry(10,{}, inMempoolInputs, 1, 1000));
    mempool.AddGroup(group);
    mempool.InitializeTracker();
    BOOST_ASSERT(mempool.tracker->GetAllCandidates().size() == 1);
    mempool.AddTx(MakeEntry(1000, {}, {std::make_tuple(group[0].GetSharedTx(), 1)}, 1, 1000));
    BOOST_ASSERT(mempool.tracker->GetAllCandidates().size() == 1);
    mempool.RemoveMostWorthless();
    mempool.RemoveMostWorthless();
    BOOST_ASSERT(mempool.tracker->GetAllCandidates().size() == 4);

}

BOOST_AUTO_TEST_CASE(performance, * boost::unit_test::disabled()) {
    MempoolMockup mempool;

    // a tree of 1 million transactions with a single tx at root

    constexpr size_t NUM_OF_TX = 1000000;
    constexpr size_t INPUTS_PER_TX = 1;
    constexpr size_t OUTPUTS_PER_TX = 2;

    std::deque<std::tuple<CTransactionRef, int>> inputs;
    auto input = std::make_tuple<TxId, int, Amount>(TxId(), 0, Amount(10000000000));
    auto rootTx = MakeEntry(0, {input}, {}, OUTPUTS_PER_TX, 300);
    mempool.AddTx(rootTx);

    for(size_t i = 0; i < OUTPUTS_PER_TX; i++)
    {
        inputs.push_back(std::make_tuple(rootTx.GetSharedTx(), static_cast<int>(i)));
    }
    std::vector<std::tuple<CTransactionRef, int>> inMempoolInputs;
    for(size_t n = 1; n < NUM_OF_TX; n++)
    {
        inMempoolInputs.clear();
        for(size_t inp = 0; inp < INPUTS_PER_TX; inp++)
        {
            inMempoolInputs.emplace_back(inputs.front());
            inputs.pop_front();
        }
        auto tx = MakeEntry(double(std::rand()) / double(RAND_MAX) + 0.5, {}, inMempoolInputs, OUTPUTS_PER_TX, 1);
        for(size_t outp = 0; outp < OUTPUTS_PER_TX; outp++)
        {
            inputs.emplace_back(tx.GetSharedTx(), static_cast<int>(outp));
        }
        mempool.AddTx(tx);
    }

    //auto t0 = std::chrono::high_resolution_clock::now();
    mempool.InitializeTracker();
    //auto t1 = std::chrono::high_resolution_clock::now();
    for(size_t i = 0; i < NUM_OF_TX; i++)
    {
        mempool.RemoveMostWorthless();
    }
    //auto t2 = std::chrono::high_resolution_clock::now();

    //std::cout << "InitializeTracker   " << std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0).count() << "ms" << std::endl;
    //std::cout << "RemoveMostWorthless " << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count() << "ms" << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef PARAMETRICLAZY_HPP
#define PARAMETRICLAZY_HPP

#include <cmath>

#include <boost/proto/proto.hpp>

#include <boost/fusion/sequence.hpp>
#include <boost/fusion/container.hpp>

#include "dependant.hh"

namespace proto = boost::proto;
namespace mpl = boost::mpl;
namespace fusion = boost::fusion;

template <typename E>
struct parametric_expr;

struct parametric_generator
  : proto::pod_generator<parametric_expr>
{ };

struct parametric_domain
  : proto::domain<parametric_generator, proto::not_<proto::address_of<proto::_> > >
{
  template <typename T>
  struct as_child: proto_base_domain::as_expr<T>
  { };
};

template <typename T>
struct parametric_domain::as_child<parameter<T> >:
  proto_base_domain::as_child<parameter<T> >
{ };

template <typename T>
struct parametric_domain::as_child<const parameter<T>>:
  proto_base_domain::as_child<const parameter<T> >
{ };

template <typename T>
struct parametric_domain::as_child<variable<T> >:
  proto_base_domain::as_child<variable<T> >
{ };

template <typename T>
struct parametric_domain::as_child<const variable<T> >:
  proto_base_domain::as_child<const variable<T> >
{ };

template <typename E>
struct parametric_expr
{
  BOOST_PROTO_EXTENDS(E, parametric_expr<E>, parametric_domain)
};

namespace ParametricLazyOps {
  template <typename T>
  struct is_terminal : mpl::false_ {};

  template <typename T>
  struct is_terminal<parameter<T> > : mpl::true_ { };

  template <typename T>
  struct is_terminal<variable<T> > : mpl::true_ { };

  template <typename T>
  struct is_terminal<dependant<T> > : mpl::true_ { };

  BOOST_PROTO_DEFINE_OPERATORS(is_terminal, parametric_domain)

  const parametric_expr<proto::terminal<double(*)(double, int)>::type > Pow{{std::pow}};
  const parametric_expr<proto::terminal<double(*)(double)>::type > Sqrt{{std::sqrt}};
  const parametric_expr<proto::terminal<double(*)(double)>::type > Sin{{std::sin}};
  const parametric_expr<proto::terminal<double(*)(double)>::type > Cos{{std::cos}};
  const parametric_expr<proto::terminal<double(*)(double, double)>::type > Atan2{{std::atan2}};
}

#define DEFINE_OUTPUT_OPERATOR(type)					\
template <typename ValueType>						\
inline std::ostream &operator<<(std::ostream &sout, type<ValueType> const &v) { \
  if (v.name()) {							\
    return sout << v.name();						\
  } else {								\
    return sout << #type "<>(" << v.rawdata() << ")";			\
  }									\
}

DEFINE_OUTPUT_OPERATOR(parameter)
DEFINE_OUTPUT_OPERATOR(variable)
DEFINE_OUTPUT_OPERATOR(dependant)
#undef DEFINE_OUTPUT_OPERATOR

struct ParametricExpression
  : proto::or_<proto::terminal<parameter<proto::_> >,
	       proto::terminal<dependant<proto::_> >,
	       proto::terminal<double>,
	       proto::nary_expr<proto::_, proto::vararg<ParametricExpression> >
	       >
{};

struct ParametersTransform
  : proto::or_<proto::when<proto::terminal<parameter<proto::_> >, fusion::cons<proto::_value, proto::_state>(proto::_value, proto::_state)>,
               proto::when<proto::terminal<variable<proto::_> >, fusion::cons<proto::_value, proto::_state>(proto::_value, proto::_state)>,
	       proto::when<proto::terminal<dependant<proto::_> >, fusion::cons<proto::_value, proto::_state>(proto::_value, proto::_state)>,
	       proto::when<proto::terminal<proto::_>, proto::_state>,
	       proto::when<proto::binary_expr<proto::_, proto::_, proto::_>, ParametersTransform(proto::_left, ParametersTransform(proto::_right, proto::_state))>
	       >
{ };

template <typename ReturnType, typename Expr, bool Free>
class DependantEvaluable {
  using evaluable_type = dependant<ReturnType>;

public:
  static evaluable_type get(Expr const &e) {
    static std::vector<std::pair<Expr, evaluable_type*> > cached;
    for (std::pair<Expr, evaluable_type*> dep: cached) {
      if (memcmp(&dep.first, &e, sizeof(Expr)) == 0) {
	  return *dep.second;
	}
    }
    std::vector<changeable> deps;
    fusion::for_each(ParametersTransform()(e, fusion::nil()), InconstantsCollector({deps}) );
    auto d = new evaluable_type(*(new DependantEvaluable<ReturnType, Expr, Free>(e)), deps);
    cached.push_back(std::pair<Expr, evaluable_type *>(e, d));
    return *d;
  }
  ReturnType operator()() const{
    proto::default_context ctx;
    return proto::eval(expr, ctx);
  }
protected:
  struct InconstantsCollector {
    template <typename T>
    void operator()(T const &t) const {
      if (std::find(params.begin(), params.end(), t) != params.end()) {
	return;
      }
      params.push_back(t);
    }
    std::vector<changeable> &params;
  };

  DependantEvaluable(Expr const &e): expr(e) { }
  const Expr expr;
};

template <typename Expr>
dependant<double> mkdep_impl(const Expr &expr) {
  return DependantEvaluable<double, Expr, false>::get(expr);
}

template <typename Expr>
dependant<double> mkdep(const Expr &expr) {
  return mkdep_impl(expr);
}

#define PROTIFY(x) (proto::as_child<parametric_domain>(x))

#endif // PARAMETRICLAZY_HPP


#include <vector>
#include <iostream>
#include <boost/python.hpp>
#include "util.hpp"

using namespace std;

double treegkr(Tree T, vector<double> creation, vector<double> destruction, vector<double> x, vector<double> y);

double treegkr_python_wrapper(boost::python::list vs,
                          boost::python::list us,
                          boost::python::list cost,
                          boost::python::list creation,
                          boost::python::list destruction,
                          boost::python::list x,
                          boost::python::list y){
  int n = boost::python::len(x);
  assert(boost::python::len(vs) == n - 1);
  assert(boost::python::len(us) == n - 1);
  assert(boost::python::len(cost) == n - 1);
  assert(boost::python::len(y) == n);
  assert(boost::python::len(creation) == n);
  assert(boost::python::len(destruction) == n);
  Tree T(n);
  Unionfind uf(n);
  for(int i = 0; i < n - 1; i++){
    int v = boost::python::extract<int>(vs[i]);
    int u = boost::python::extract<int>(us[i]);
    double c = boost::python::extract<double>(cost[i]);
    T.add_edge(v, u, c);
    if(uf.same(u, v)){
      cerr << "Input is not a tree" << endl;
      exit(1);
    }
    uf.unite(u, v);
  }
  if(uf.size[uf.find(0)] != n){
    cerr << "Input is not a tree" << endl;
    exit(1);
  }
  vector<double> xv(n), yv(n), creationv(n), destructionv(n);
  for(int i = 0; i < n; i++){
    xv[i] = boost::python::extract<double>(x[i]);
    yv[i] = boost::python::extract<double>(y[i]);
    creationv[i] = boost::python::extract<double>(creation[i]);
    destructionv[i] = boost::python::extract<double>(destruction[i]);
  }
  return treegkr(T, creationv, destructionv, xv, yv);
}

BOOST_PYTHON_MODULE(treegkr){
  boost::python::def("treegkr",
                     treegkr_python_wrapper,
                     boost::python::args("vs",
                                         "us",
                                         "cost",
                                         "creation",
                                         "destruction",
                                         "x",
                                         "y"),
                     "Fast computation of the generalized Kantorovich Rubinstein (GKR) distance on tree metrics\n"
                     "\n"
                     "Parameters\n"
                     "----------\n"
                     "vs : list of ints\n"
                     "    A list of endpoints of edges\n"
                     "    The node indices should start from zero\n"
                     "    The length of the list should be n-1\n"
                     "us : list of ints\n"
                     "    A list of endpoints of edges\n"
                     "    The node indices should start from zero\n"
                     "    The length of the list should be n-1\n"
                     "cost : list of floats\n"
                     "    A list of costs of edges\n"
                     "    The i-th edge connects node vs[i] and us[i] with cost[i].\n"
                     "    The length of the list should be n-1\n"
                     "creation : list of floats\n"
                     "    A list of creation costs\n"
                     "    A mass on node i can be created by cost creation[i].\n"
                     "    The length of the list should be n\n"
                     "destruction : list of floats\n"
                     "    A list of destruction costs\n"
                     "    A mass on node i can be destructed by cost destruction[i].\n"
                     "    The length of the list should be n\n"
                     "x : list of floats\n"
                     "    A list of source mass\n"
                     "    There exist x[i] target mass on node i.\n"
                     "    The length of the list should be n\n"
                     "y : list of floats\n"
                     "    A list of target mass\n"
                     "    There exist y[i] target mass on node i.\n"
                     "    The length of the list should be n\n"
                     "\n"
                     "Returns\n"
                     "-------\n"
                     "gkr: float\n"
                     "    The GKR distance\n");
}