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

#pragma hdrstop

#include <boost/geometry.hpp>

#include "PolynomialAreaCalc.h"

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

#pragma package(smart_init)

namespace AreaPrj {

double PolynomialAreaCalc::DoCompute( IModel const & Model ) const
{
    return boost::geometry::area( Model.GetPolygons() );
}

} // End of namespace AreaPrj


#include <boost/accumulators/statistics/rolling_variance.hpp>


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file SteamCATrajInterface.hpp
///
/// \author Tim Tang, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_CA_TRAJECTORY_INTERFACE_HPP
#define STEAM_CA_TRAJECTORY_INTERFACE_HPP

#include <Eigen/Core>

#include <steam/common/Time.hpp>

#include <steam/trajectory/SteamCATrajVar.hpp>
#include <steam/trajectory/SteamTrajInterface.hpp>

#include <steam/problem/WeightedLeastSqCostTerm.hpp>
#include <steam/problem/ParallelizedCostTermCollection.hpp>

namespace steam {
namespace se3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief The trajectory class wraps a set of state variables to provide an interface
///        that allows for continuous-time pose interpolation.
//////////////////////////////////////////////////////////////////////////////////////////////
class SteamCATrajInterface : public SteamTrajInterface
{
 public:

  using SteamTrajInterface::SteamTrajInterface;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Add a new knot
  //////////////////////////////////////////////////////////////////////////////////////////////
  void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
           const VectorSpaceStateVar::Ptr& velocity,
           const VectorSpaceStateVar::Ptr& acceleration);

  void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
           const VectorSpaceStateVar::Ptr& velocity,
           const VectorSpaceStateVar::Ptr& acceleration,
           const Eigen::Matrix<double,18,18> cov);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get velocity evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::VectorXd getVelocity(const steam::Time& time);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get acceleration evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::VectorXd getAcceleration(const steam::Time& time);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration
  ///        prior should exist on a trajectory, adding a second will overwrite the first.
  //////////////////////////////////////////////////////////////////////////////////////////////
  void addAccelerationPrior(const steam::Time& time, const Eigen::Matrix<double,6,1>& acceleration,
    const Eigen::Matrix<double,6,6>& cov);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get binary cost terms associated with the prior for active parts of the trajectory
  //////////////////////////////////////////////////////////////////////////////////////////////
  void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get interpolated/extrapolated covariance at given time
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::MatrixXd getCovariance(const steam::Time& time) const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Acceleration prior
  //////////////////////////////////////////////////////////////////////////////////////////////
  steam::WeightedLeastSqCostTerm<6,6>::Ptr accelerationPriorFactor_;

};

} // se3
} // steam

#endif // STEAM_CA_TRAJECTORY_INTERFACE_HPP


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <stdlib.h>
#include <vector>
#include <string>
#include <functional>

#include <random>

// ML-Data Utils
#include <ml_data/ml_data.hpp>

// Models
#include <toolkits/supervised_learning/random_forest.hpp>

#include <sframe/testing_utils.hpp>
#include <util/testing_utils.hpp>

#include <cfenv>

using namespace turi;
using namespace turi::supervised;
using namespace turi::supervised::xgboost;

void run_random_forest_classifier_test(std::map<std::string, flexible_type> opts) {

  size_t examples = opts.at("examples");
  size_t features = opts.at("features");
  std::string target_column_name = "target";
  std::vector<std::string> column_names = {"user", "item"};

  // Answers
  // -----------------------------------------------------------------------
  DenseVector coefs(features+1);
  coefs.randn();

  // Feature names
  std::vector<std::string> feature_names;
  std::vector<flex_type_enum> feature_types;
  for(size_t i=0; i < features; i++){
    feature_names.push_back(std::to_string(i));
    feature_types.push_back(flex_type_enum::FLOAT);
  }

  // Data
  std::vector<std::vector<flexible_type>> y_data;
  std::vector<std::vector<flexible_type>> X_data;
  for(size_t i=0; i < examples; i++){
    DenseVector x(features);
    x.randn();
    std::vector<flexible_type> x_tmp;
    for(size_t k=0; k < features; k++){
      x_tmp.push_back(x(k));
    }

    // Compute the prediction for this
    double t = dot(x, coefs.subvec(0, features-1)) + coefs(features);
    t = 1.0/(1.0+exp(-1.0*t));
    int c = turi::random::bernoulli(t);
    std::vector<flexible_type> y_tmp;
    y_tmp.push_back(c);

    X_data.push_back(x_tmp);
    y_data.push_back(y_tmp);
  }

  // Options
  std::map<std::string, flexible_type> options = {
    {"max_iterations", 10},
    {"column_subsample", 1.0}
  };

  // Make the data
  sframe X = make_testing_sframe(feature_names, feature_types, X_data);
  sframe y = make_testing_sframe({"target"}, {flex_type_enum::INTEGER}, y_data);
  std::shared_ptr<random_forest_classifier> model;
  model.reset(new random_forest_classifier);
  model->init(X,y);
  model->init_options(options);
  model->train();

  // Check options
  // ----------------------------------------------------------------------
  std::map<std::string, flexible_type> _options;
  _options = model->get_current_options();
  for (auto& kvp: options){
    TS_ASSERT(_options[kvp.first] == kvp.second);
    if (kvp.first == "max_iterations") {
      logprogress_stream << "Max iterations should be 10: "
                         << kvp.second
                         << std::endl;
    }
  }
  TS_ASSERT(model->is_trained() == true);

  // Construct the ml_data
  ml_data data = model->construct_ml_data_using_current_metadata(X, y);

  // Check predictions
  // ----------------------------------------------------------------------
  std::vector<flexible_type> pred_class;
  std::shared_ptr<sarray<flexible_type>> _pred_class
    = model->predict(data, "class");

  // Check that we can train a model when providing a validation set
  std::vector<std::vector<flexible_type>> y_v;
  std::vector<std::vector<flexible_type>> X_v;
  for(size_t i=0; i < 5; i++){
    DenseVector x(features);
    x.randn();
    std::vector<flexible_type> x_tmp;
    for(size_t k=0; k < features; k++){
      x_tmp.push_back(x(k));
    }

    // Compute the prediction for this
    double t = dot(x, coefs.subvec(0, features-1)) + coefs(features);
    t = 1.0/(1.0+exp(-1.0*t));
    int c = turi::random::bernoulli(t);
    std::vector<flexible_type> y_tmp;
    y_tmp.push_back(1 + c);

    X_v.push_back(x_tmp);
    y_v.push_back(y_tmp);
  }

  sframe Xv = make_testing_sframe(feature_names, feature_types, X_v);
  sframe yv = make_testing_sframe({"target"}, {flex_type_enum::INTEGER}, y_v);

  model.reset(new random_forest_classifier);
  logprogress_stream << "Training with a validation set" << std::endl;
  model->init(X, y, Xv, yv);
  model->init_options(options);
  model->train();

}

struct random_forest_classifier_test  {

  public:

  void test_random_forest_classifier_basic_2d() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 100},
      {"features", 1}};
    run_random_forest_classifier_test(opts);
  }

  void test_random_forest_classifier_small() {
    std::map<std::string, flexible_type> opts = {
      {"examples", 1000},
      {"features", 10}};
    run_random_forest_classifier_test(opts);
  }

};

BOOST_FIXTURE_TEST_SUITE(_random_forest_classifier_test, random_forest_classifier_test)
BOOST_AUTO_TEST_CASE(test_random_forest_classifier_basic_2d) {
  random_forest_classifier_test::test_random_forest_classifier_basic_2d();
}
BOOST_AUTO_TEST_CASE(test_random_forest_classifier_small) {
  random_forest_classifier_test::test_random_forest_classifier_small();
}
BOOST_AUTO_TEST_SUITE_END()


/*

Copyright (c) 2016, Project OSRM contributors
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef COORDINATE_HPP_
#define COORDINATE_HPP_

#include "util/strong_typedef.hpp"

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

#include <cstddef>
#include <iosfwd> //for std::ostream
#include <string>
#include <type_traits>

namespace osrm
{

constexpr const double COORDINATE_PRECISION = 1e6;

namespace util
{

OSRM_STRONG_TYPEDEF(int32_t, FixedLatitude)
OSRM_STRONG_TYPEDEF(int32_t, FixedLongitude)
OSRM_STRONG_TYPEDEF(double, FloatLatitude)
OSRM_STRONG_TYPEDEF(double, FloatLongitude)

/**
 * Converts a typed latitude from floating to fixed representation.
 *
 * \param floating typed latitude in floating representation.
 * \return typed latitude in fixed representation
 * \see Coordinate, toFloating
 */
inline FixedLatitude toFixed(const FloatLatitude floating)
{
    const auto latitude = static_cast<double>(floating);
    const auto fixed =
        boost::numeric_cast<std::int32_t>(std::round(latitude * COORDINATE_PRECISION));
    return FixedLatitude{fixed};
}

/**
 * Converts a typed longitude from floating to fixed representation.
 *
 * \param floating typed longitude in floating representation.
 * \return typed latitude in fixed representation
 * \see Coordinate, toFloating
 */
inline FixedLongitude toFixed(const FloatLongitude floating)
{
    const auto longitude = static_cast<double>(floating);
    const auto fixed =
        boost::numeric_cast<std::int32_t>(std::round(longitude * COORDINATE_PRECISION));
    return FixedLongitude{fixed};
}

/**
 * Converts a typed latitude from fixed to floating representation.
 *
 * \param fixed typed latitude in fixed representation.
 * \return typed latitude in floating representation
 * \see Coordinate, toFixed
 */
inline FloatLatitude toFloating(const FixedLatitude fixed)
{
    const auto latitude = static_cast<std::int32_t>(fixed);
    const auto floating = boost::numeric_cast<double>(latitude / COORDINATE_PRECISION);
    return FloatLatitude{floating};
}

/**
 * Converts a typed longitude from fixed to floating representation.
 *
 * \param fixed typed longitude in fixed representation.
 * \return typed longitude in floating representation
 * \see Coordinate, toFixed
 */
inline FloatLongitude toFloating(const FixedLongitude fixed)
{
    const auto longitude = static_cast<std::int32_t>(fixed);
    const auto floating = boost::numeric_cast<double>(longitude / COORDINATE_PRECISION);
    return FloatLongitude{floating};
}

// fwd. decl.
struct FloatCoordinate;

/**
 * Represents a coordinate based on longitude and latitude in fixed representation.
 *
 * To prevent accidental longitude and latitude flips, we provide typed longitude and latitude
 * wrappers. You can cast these wrappers back to their underlying representation or convert them
 * from one representation to the other.
 *
 * The two representation we provide are:
 *  - Fixed point
 *  - Floating point
 *
 * \see FloatCoordinate, toFixed, toFloating
 */
struct Coordinate
{
    FixedLongitude lon;
    FixedLatitude lat;

    Coordinate() : lon{std::numeric_limits<int>::min()}, lat{std::numeric_limits<int>::min()} {}

    Coordinate(const FloatCoordinate &other);

    Coordinate(const FloatLongitude lon_, const FloatLatitude lat_)
        : Coordinate(toFixed(lon_), toFixed(lat_))
    {
    }

    Coordinate(const FixedLongitude lon_, const FixedLatitude lat_) : lon(lon_), lat(lat_) {}

    template <class T> Coordinate(const T &coordinate) : lon(coordinate.lon), lat(coordinate.lat)
    {
        static_assert(!std::is_same<T, Coordinate>::value,
                      "This constructor should not be used for Coordinates");
        static_assert(std::is_same<decltype(lon), decltype(coordinate.lon)>::value,
                      "coordinate types incompatible");
        static_assert(std::is_same<decltype(lat), decltype(coordinate.lat)>::value,
                      "coordinate types incompatible");
    }

    bool IsValid() const;
    friend bool operator==(const Coordinate lhs, const Coordinate rhs);
    friend bool operator!=(const Coordinate lhs, const Coordinate rhs);
    friend std::ostream &operator<<(std::ostream &out, const Coordinate coordinate);
};

/**
 * Represents a coordinate based on longitude and latitude in floating representation.
 *
 * To prevent accidental longitude and latitude flips, we provide typed longitude and latitude
 * wrappers. You can cast these wrappers back to their underlying representation or convert them
 * from one representation to the other.
 *
 * The two representation we provide are:
 *  - Fixed point
 *  - Floating point
 *
 * \see Coordinate, toFixed, toFloating
 */
struct FloatCoordinate
{
    FloatLongitude lon;
    FloatLatitude lat;

    FloatCoordinate()
        : lon{std::numeric_limits<double>::min()}, lat{std::numeric_limits<double>::min()}
    {
    }

    FloatCoordinate(const Coordinate other)
        : FloatCoordinate(toFloating(other.lon), toFloating(other.lat))
    {
    }

    FloatCoordinate(const FixedLongitude lon_, const FixedLatitude lat_)
        : FloatCoordinate(toFloating(lon_), toFloating(lat_))
    {
    }

    FloatCoordinate(const FloatLongitude lon_, const FloatLatitude lat_) : lon(lon_), lat(lat_) {}

    bool IsValid() const;
    friend bool operator==(const FloatCoordinate lhs, const FloatCoordinate rhs);
    friend bool operator!=(const FloatCoordinate lhs, const FloatCoordinate rhs);
    friend std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate);
};

bool operator==(const Coordinate lhs, const Coordinate rhs);
bool operator==(const FloatCoordinate lhs, const FloatCoordinate rhs);
std::ostream &operator<<(std::ostream &out, const Coordinate coordinate);
std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate);

inline Coordinate::Coordinate(const FloatCoordinate &other)
    : Coordinate(toFixed(other.lon), toFixed(other.lat))
{
}
}
}

#endif /* COORDINATE_HPP_ */


// based on Eigen unsupported bvh KDTree

#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <algorithm>
#include <queue>

#include "rpxdock/bvh/bvh_algo.hpp"
#include "rpxdock/geom/primitive.hpp"
#include "rpxdock/util/types.hpp"

namespace rpxdock {
namespace bvh {

using namespace geom;
using namespace Eigen;

// internal pair class for the BVH--used instead of std::pair because of
// alignment
template <class F, int DIM>
struct VintPair {
  using first_type = Matrix<F, DIM, 1>;
  using secont_type = int;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(F, DIM)
  VintPair(const first_type &v, int i) : first(v), second(i) {}
  first_type first;
  int second;
};

// these templates help the tree initializer get the bounding spheres either in
// from a provided iterator range or using bounding_vol in a unified way
template <typename Objs, typename Vols, typename Iter>
struct get_bvols_helper {
  void operator()(const Objs &objs, Iter sphbeg, Iter sphend, Vols &out) {
    out.insert(out.end(), sphbeg, sphend);
    eigen_assert(out.size() == objs.size());
  }
};

template <typename Objs, typename Vols>
struct get_bvols_helper<Objs, Vols, int> {
  void operator()(const Objs &objs, int, int, Vols &out) {
    out.reserve(objs.size());
    for (int i = 0; i < (int)objs.size(); ++i)
      out.push_back(bounding_vol(objs[i]));
  }
};

template <class RAiter>
struct P1Range {
  using value_type = typename RAiter::value_type::first_type;
  RAiter a, b;
  auto const &operator[](size_t i) const { return (a + i)->first; }
  auto &operator[](size_t i) { return (a + i)->first; }
  size_t size() const { return b - a; }
  auto get_index(size_t i) const { return (a + i)->second; }
};
template <class RAiter>
auto p1range(RAiter a, RAiter b) {
  P1Range<RAiter> r;
  r.a = a;
  r.b = b;
  return r;
}

template <typename F, bool idrange = false>
struct WelzlBoundingSphere {
  static Sphere<F> bound(auto subtree_objs) {
    return welzl_bounding_sphere<true>(subtree_objs);
  }
};

template <typename _Scalar, typename _Object, int _DIM = 3,
          typename _Volume = Sphere<_Scalar>,
          typename BoundingSphere = WelzlBoundingSphere<_Scalar, true>>
class SphereBVH {
 public:
  static int const DIM = _DIM;
  typedef _Object Object;
  typedef std::vector<Object, Eigen::aligned_allocator<Object>> Objs;
  typedef _Scalar F;
  // typedef Eigen::AlignedBox<F, DIM> Volume;
  typedef _Volume Volume;
  typedef std::vector<Volume, Eigen::aligned_allocator<Volume>> Vols;
  typedef int Index;
  typedef const int *VolumeIterator;  // the iterators are just pointers into
                                      // the tree's vectors
  typedef const Object *ObjectIterator;

  std::vector<int> child;  // child of x are child[2x] and
                           // child[2x+1], indices bigger than
                           // vols.size() index into objs.
  Vols vols;
  Objs objs;

  SphereBVH() {}

  template <typename Iter>
  SphereBVH(Iter begin, Iter end) {
    init(begin, end, 0, 0);
  }  // int is recognized by init as not being an iterator type

  template <typename OIter, typename BIter>
  SphereBVH(OIter begin, OIter end, BIter sphbeg, BIter sphend) {
    init(begin, end, sphbeg, sphend);
  }

  size_t size() const { return objs.size(); }

  /** Given an iterator range over \a Object references, constructs the BVH,
   * overwriting whatever is in there currently.
   * Requires that bounding_vol(Object) return a Volume. */
  template <typename Iter>
  void init(Iter begin, Iter end) {
    init(begin, end, 0, 0);
  }

  /** Given an iterator range over \a Object references and an iterator range
   * over their bounding vols,
   * constructs the BVH, overwriting whatever is in there currently. */
  template <typename OIter, typename BIter>
  void init(OIter begin, OIter end, BIter sphbeg, BIter sphend) {
    objs.clear();
    vols.clear();
    child.clear();

    objs.insert(objs.end(), begin, end);
    int n = static_cast<int>(objs.size());

    // if we have at most one object, we don't need any internal nodes
    if (n < 2) return;

    Vols ovol;
    VIPairs ocen;

    // compute the bounding vols depending on BIter type
    get_bvols_helper<Objs, Vols, BIter>()(objs, sphbeg, sphend, ovol);

    ocen.reserve(n);
    vols.reserve(n - 1);
    child.reserve(2 * n - 2);

    for (int i = 0; i < n; ++i) ocen.push_back(VIPair(ovol[i].cen, i));

    // the recursive part of the algorithm
    build(ocen, 0, n, ovol, 0);

    Objs tmp(n);
    tmp.swap(objs);
    for (int i = 0; i < n; ++i) objs[i] = tmp[ocen[i].second];
  }

  /** \returns the index of the root of the hierarchy */
  inline Index getRootIndex() const { return (int)vols.size() - 1; }

  /** Given an \a index of a node, on exit, \a vbeg and \a vend range
   * over the indices of the volume children of the node
   * and \a obeg and \a oend range over the object children of the node
   */
  EIGEN_STRONG_INLINE
  void getChildren(Index index, VolumeIterator &vbeg, VolumeIterator &vend,
                   ObjectIterator &obeg, ObjectIterator &oend) const {
    // inlining this function should open lots of optimization opportunities to
    // the compiler
    if (index < 0) {
      vbeg = vend;
      if (!objs.empty()) obeg = &(objs[0]);
      oend = obeg + objs.size();  // output all objs--necessary
                                  // when the tree has only one
                                  // object
      return;
    }

    int nvol = static_cast<int>(vols.size());

    int idx = index * 2;
    if (child[idx + 1] < nvol) {  // second index is always bigger
      vbeg = &(child[idx]);
      vend = vbeg + 2;
      obeg = oend;
    } else if (child[idx] >= nvol) {  // if both child are objs
      vbeg = vend;
      obeg = &(objs[child[idx] - nvol]);
      oend = obeg + 2;
    } else {  // if the first child is a volume and the second is an object
      vbeg = &(child[idx]);
      vend = vbeg + 1;
      obeg = &(objs[child[idx + 1] - nvol]);
      oend = obeg + 1;
    }
    // std::cout << " gc" << vend - vbeg;
  }

  inline const Volume &getVolume(Index index) const { return vols[index]; }

 private:
  typedef VintPair<F, DIM> VIPair;
  typedef std::vector<VIPair, Eigen::aligned_allocator<VIPair>> VIPairs;
  typedef Eigen::Matrix<F, DIM, 1> VectorType;

  struct AxisComparator {
    int dim;
    AxisComparator(int inDim) : dim(inDim) {}
    inline bool operator()(const VIPair &v1, const VIPair &v2) const {
      return v1.first[dim] < v2.first[dim];
    }
  };
  struct DotComparator {
    Matrix<F, DIM, 1> normal;
    DotComparator(Matrix<F, DIM, 1> n) : normal(n) {}
    template <class Pair>
    DotComparator(Pair p) : normal(p.second - p.first) {}
    inline bool operator()(const VIPair &v1, const VIPair &v2) const {
      return v1.first.dot(normal) < v2.first.dot(normal);
    }
  };

  // Build the part of the tree between objs[from] and objs[to] (not
  // including objs[to]). This routine partitions the ocen in [from, to) along
  // the dimension dim, recursively constructs the two halves, and adds their
  // parent node.  TODO: a cache-friendlier layout
  void build(VIPairs &ocen, int from, int to, Vols const &ovol,
             int dim) noexcept {
    eigen_assert(to - from > 1);
    if (to - from == 2) {
      auto merge = ovol[ocen[from].second].merged(ovol[ocen[from + 1].second]);
      vols.push_back(merge);
      child.push_back(from + (int)objs.size() - 1);
      child.push_back(from + (int)objs.size());
    } else if (to - from == 3) {
      int mid = from + 2;
      auto subtree_objs = p1range(ocen.begin() + from, ocen.begin() + to);
      nth_element(ocen.begin() + from, ocen.begin() + mid, ocen.begin() + to,
                  DotComparator(most_separated_points_on_AABB(subtree_objs)));
      // AxisComparator(dim));
      build(ocen, from, mid, ovol, (dim + 1) % DIM);
      int idx1 = (int)vols.size() - 1;
      Volume bound = BoundingSphere::bound(subtree_objs);
      vols.push_back(bound);
      // Volume merge = vols[idx1].merged(ovol[ocen[mid].second]);
      // if (merge.rad + 0.0001 < bound.rad)
      // std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
      // vols.push_back(bound.rad < merge.rad ? bound : merge);
      child.push_back(idx1);
      child.push_back(mid + (int)objs.size() - 1);
    } else {
      int mid = from + (to - from) / 2;
      auto subtree_objs = p1range(ocen.begin() + from, ocen.begin() + to);
      nth_element(ocen.begin() + from, ocen.begin() + mid, ocen.begin() + to,
                  DotComparator(most_separated_points_on_AABB(subtree_objs)));
      // AxisComparator(dim));
      build(ocen, from, mid, ovol, (dim + 1) % DIM);
      int idx1 = (int)vols.size() - 1;
      build(ocen, mid, to, ovol, (dim + 1) % DIM);
      int idx2 = (int)vols.size() - 1;
      Volume bound = BoundingSphere::bound(subtree_objs);
      vols.push_back(bound);
      // Volume merge = vols[idx1].merged(vols[idx2]);
      // if (merge.rad + 0.0001 < bound.rad)
      // std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
      // vols.push_back(bound.rad < merge.rad ? bound : merge);
      child.push_back(idx1);
      child.push_back(idx2);
    }
  }
};
}  // namespace bvh
}  // namespace rpxdock


// -*- c++ -*-
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>

#include <boost/graph/adjacency_list.hpp>

/*
  Thanks to Dale Gerdemann for this example, which inspired some
  changes to adjacency_list to make this work properly.
 */

/*
  Sample output:

  0  --c--> 1   --j--> 1   --c--> 2   --x--> 2
  1  --c--> 2   --d--> 3
  2  --t--> 4
  3  --h--> 4
  4

  merging vertex 1 into vertex 0

  0  --c--> 0   --j--> 0   --c--> 1   --x--> 1   --d--> 2
  1  --t--> 3
  2  --h--> 3
  3
 */

// merge_vertex(u,v,g):
// incoming/outgoing edges for v become incoming/outgoing edges for u
// v is deleted
template <class Graph, class GetEdgeProperties>
void merge_vertex
  (typename boost::graph_traits<Graph>::vertex_descriptor u,
   typename boost::graph_traits<Graph>::vertex_descriptor v,
   Graph& g, GetEdgeProperties getp)
{
  typedef boost::graph_traits<Graph> Traits;
  typename Traits::edge_descriptor e;
  typename Traits::out_edge_iterator out_i, out_end;
  for (boost::tie(out_i, out_end) = out_edges(v, g); out_i != out_end; ++out_i) {
    e = *out_i;
    typename Traits::vertex_descriptor targ = target(e, g);
    add_edge(u, targ, getp(e), g);
  }
  typename Traits::in_edge_iterator in_i, in_end;
  for (boost::tie(in_i, in_end) = in_edges(v, g); in_i != in_end; ++in_i) {
    e = *in_i;
    typename Traits::vertex_descriptor src = source(e, g);
    add_edge(src, u, getp(e), g);
  }
  clear_vertex(v, g);
  remove_vertex(v, g);
}

template <class StoredEdge>
struct order_by_name
  : public std::binary_function<StoredEdge,StoredEdge,bool>
{
  bool operator()(const StoredEdge& e1, const StoredEdge& e2) const {
    // Using std::pair operator< as an easy way to get lexicographical
    // compare over tuples.
    return std::make_pair(e1.get_target(), boost::get(boost::edge_name, e1))
      < std::make_pair(e2.get_target(), boost::get(boost::edge_name, e2));
  }
};
struct ordered_set_by_nameS { };

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
namespace boost {
  template <class ValueType>
  struct container_gen<ordered_set_by_nameS, ValueType> {
    typedef std::set<ValueType, order_by_name<ValueType> > type;
  };
  template <>
  struct parallel_edge_traits<ordered_set_by_nameS> {
    typedef allow_parallel_edge_tag type;
  };
}
#endif

template <class Graph>
struct get_edge_name {
  get_edge_name(const Graph& g_) : g(g_) { }

  template <class Edge>
  boost::property<boost::edge_name_t, char> operator()(Edge e) const {
    return boost::property<boost::edge_name_t, char>(boost::get(boost::edge_name, g, e));
  }
  const Graph& g;
};

int
main()
{
#ifdef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
  std::cout << "This program requires partial specialization." << std::endl;
#else
  using namespace boost;
  typedef property<edge_name_t, char> EdgeProperty;
  typedef adjacency_list<ordered_set_by_nameS, vecS, bidirectionalS,
    no_property, EdgeProperty> graph_type;

  graph_type g;

  add_edge(0, 1, EdgeProperty('j'), g);
  add_edge(0, 2, EdgeProperty('c'), g);
  add_edge(0, 2, EdgeProperty('x'), g);
  add_edge(1, 3, EdgeProperty('d'), g);
  add_edge(1, 2, EdgeProperty('c'), g);
  add_edge(1, 3, EdgeProperty('d'), g);
  add_edge(2, 4, EdgeProperty('t'), g);
  add_edge(3, 4, EdgeProperty('h'), g);
  add_edge(0, 1, EdgeProperty('c'), g);

  property_map<graph_type, vertex_index_t>::type id = get(vertex_index, g);
  property_map<graph_type, edge_name_t>::type name = get(edge_name, g);

  graph_traits<graph_type>::vertex_iterator i, end;
  graph_traits<graph_type>::out_edge_iterator ei, edge_end;

  for (boost::tie(i, end) = vertices(g); i != end; ++i) {
    std::cout << id[*i] << " ";
    for (boost::tie(ei, edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
      std::cout << " --" << name[*ei] << "--> " << id[target(*ei, g)] << "  ";
    std::cout << std::endl;
  }
  std::cout << std::endl;

  std::cout << "merging vertex 1 into vertex 0" << std::endl << std::endl;
  merge_vertex(0, 1, g, get_edge_name<graph_type>(g));

  for (boost::tie(i, end) = vertices(g); i != end; ++i) {
    std::cout << id[*i] << " ";
    for (boost::tie(ei, edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
      std::cout << " --" << name[*ei] << "--> " << id[target(*ei, g)] << "  ";
    std::cout << std::endl;
  }
  std::cout << std::endl;
#endif
  return 0;
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#include "autodiff.h"

#include <iostream>

#include <boost/yap/algorithm.hpp>
#include <boost/polymorphic_cast.hpp>
#include <boost/hana/for_each.hpp>

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

double const Epsilon = 10.0e-6;
#define CHECK_CLOSE(A,B) do { BOOST_CHECK_CLOSE(A,B,Epsilon); } while(0)

using namespace AutoDiff;

//[ autodiff_expr_template_decl
template <boost::yap::expr_kind Kind, typename Tuple>
struct autodiff_expr
{
    static boost::yap::expr_kind const kind = Kind;

    Tuple elements;
};

BOOST_YAP_USER_UNARY_OPERATOR(negate, autodiff_expr, autodiff_expr)

BOOST_YAP_USER_BINARY_OPERATOR(plus, autodiff_expr, autodiff_expr)
BOOST_YAP_USER_BINARY_OPERATOR(minus, autodiff_expr, autodiff_expr)
BOOST_YAP_USER_BINARY_OPERATOR(multiplies, autodiff_expr, autodiff_expr)
BOOST_YAP_USER_BINARY_OPERATOR(divides, autodiff_expr, autodiff_expr)
//]

//[ autodiff_expr_literals_decl
namespace autodiff_placeholders {

    // This defines a placeholder literal operator that creates autodiff_expr
    // placeholders.
    BOOST_YAP_USER_LITERAL_PLACEHOLDER_OPERATOR(autodiff_expr)

}
//]

//[ autodiff_function_terminals
template <OPCODE Opcode>
struct autodiff_fn_expr :
    autodiff_expr<boost::yap::expr_kind::terminal, boost::hana::tuple<OPCODE>>
{
    autodiff_fn_expr () :
        autodiff_expr {boost::hana::tuple<OPCODE>{Opcode}}
    {}

    BOOST_YAP_USER_CALL_OPERATOR_N(::autodiff_expr, 1);
};

// Someone included <math.h>, so we have to add trailing underscores.
autodiff_fn_expr<OP_SIN> const sin_;
autodiff_fn_expr<OP_COS> const cos_;
autodiff_fn_expr<OP_SQRT> const sqrt_;
//]

//[ autodiff_xform
struct xform
{
    // Create a var-node for each placeholder when we see it for the first
    // time.
    template <long long I>
    Node * operator() (boost::yap::expr_tag<boost::yap::expr_kind::terminal>,
                       boost::yap::placeholder<I>)
    {
        if (list_.size() < I)
            list_.resize(I);
        auto & retval = list_[I - 1];
        if (retval == nullptr)
            retval = create_var_node();
        return retval;
    }

    // Create a param-node for every numeric terminal in the expression.
    Node * operator() (boost::yap::expr_tag<boost::yap::expr_kind::terminal>, double x)
    { return create_param_node(x); }

    // Create a "uary" node for each call expression, using its OPCODE.
    template <typename Expr>
    Node * operator() (boost::yap::expr_tag<boost::yap::expr_kind::call>,
                       OPCODE opcode, Expr const & expr)
    {
        return create_uary_op_node(
            opcode,
            boost::yap::transform(boost::yap::as_expr<autodiff_expr>(expr), *this)
        );
    }

    template <typename Expr>
    Node * operator() (boost::yap::expr_tag<boost::yap::expr_kind::negate>,
                       Expr const & expr)
    {
        return create_uary_op_node(
            OP_NEG,
            boost::yap::transform(boost::yap::as_expr<autodiff_expr>(expr), *this)
        );
    }

    // Define a mapping from binary arithmetic expr_kind to OPCODE...
    static OPCODE op_for_kind (boost::yap::expr_kind kind)
    {
        switch (kind) {
        case boost::yap::expr_kind::plus: return OP_PLUS;
        case boost::yap::expr_kind::minus: return OP_MINUS;
        case boost::yap::expr_kind::multiplies: return OP_TIMES;
        case boost::yap::expr_kind::divides: return OP_DIVID;
        default: assert(!"This should never execute"); return OPCODE{};
        }
        assert(!"This should never execute");
        return OPCODE{};
    }

    // ... and use it to handle all the binary arithmetic operators.
    template <boost::yap::expr_kind Kind, typename Expr1, typename Expr2>
    Node * operator() (boost::yap::expr_tag<Kind>, Expr1 const & expr1, Expr2 const & expr2)
    {
        return create_binary_op_node(
            op_for_kind(Kind),
            boost::yap::transform(boost::yap::as_expr<autodiff_expr>(expr1), *this),
            boost::yap::transform(boost::yap::as_expr<autodiff_expr>(expr2), *this)
        );
    }

    vector<Node *> & list_;
};
//]

//[ autodiff_to_node
template <typename Expr, typename ...T>
Node * to_auto_diff_node (Expr const & expr, vector<Node *> & list, T ... args)
{
    Node * retval = nullptr;

    // This fills in list as a side effect.
    retval = boost::yap::transform(expr, xform{list});

    assert(list.size() == sizeof...(args));

    // Fill in the values of the value-nodes in list with the "args"
    // parameter pack.
    auto it = list.begin();
    boost::hana::for_each(
        boost::hana::make_tuple(args ...),
        [&it](auto x) {
            Node * n = *it;
            VNode * v = boost::polymorphic_downcast<VNode *>(n);
            v->val = x;
            ++it;
        }
    );

    return retval;
}
//]

struct F{
	F() {	AutoDiff::autodiff_setup();	}
	~F(){	AutoDiff::autodiff_cleanup();	}
};

BOOST_FIXTURE_TEST_SUITE(all, F)

//[ autodiff_original_node_builder
Node* build_linear_fun1_manually(vector<Node*>& list)
{
	//f(x1,x2,x3) = -5*x1+sin(10)*x1+10*x2-x3/6
	PNode* v5 = create_param_node(-5);
	PNode* v10 = create_param_node(10);
	PNode* v6 = create_param_node(6);
	VNode*	x1 = create_var_node();
	VNode*	x2 = create_var_node();
	VNode*	x3 = create_var_node();

	OPNode* op1 = create_binary_op_node(OP_TIMES,v5,x1);   //op1 = v5*x1
	OPNode* op2 = create_uary_op_node(OP_SIN,v10);         //op2 = sin(v10)
	OPNode* op3 = create_binary_op_node(OP_TIMES,op2,x1);  //op3 = op2*x1
	OPNode* op4 = create_binary_op_node(OP_PLUS,op1,op3);  //op4 = op1 + op3
	OPNode*	op5 = create_binary_op_node(OP_TIMES,v10,x2);  //op5 = v10*x2
	OPNode* op6 = create_binary_op_node(OP_PLUS,op4,op5);  //op6 = op4+op5
	OPNode* op7 = create_binary_op_node(OP_DIVID,x3,v6);   //op7 = x3/v6
	OPNode* op8 = create_binary_op_node(OP_MINUS,op6,op7); //op8 = op6 - op7
	x1->val = -1.9;
	x2->val = 2;
	x3->val = 5./6.;
	list.push_back(x1);
	list.push_back(x2);
	list.push_back(x3);
	return op8;
}
//]

//[ autodiff_yap_node_builder
Node* build_linear_fun1(vector<Node*>& list)
{
    //f(x1,x2,x3) = -5*x1+sin(10)*x1+10*x2-x3/6
    using namespace autodiff_placeholders;
    return to_auto_diff_node(
        -5 * 1_p + sin_(10) * 1_p + 10 * 2_p - 3_p / 6,
        list,
        -1.9,
        2,
        5./6.
    );
}
//]

Node* build_linear_function2_manually(vector<Node*>& list)
{
	//f(x1,x2,x3) = -5*x1+-10*x1+10*x2-x3/6
	PNode* v5 = create_param_node(-5);
	PNode* v10 = create_param_node(10);
	PNode* v6 = create_param_node(6);
	VNode*	x1 = create_var_node();
	VNode*	x2 = create_var_node();
	VNode*	x3 = create_var_node();
	list.push_back(x1);
	list.push_back(x2);
	list.push_back(x3);
	OPNode* op1 = create_binary_op_node(OP_TIMES,v5,x1); //op1 = v5*x1
	OPNode* op2 = create_uary_op_node(OP_NEG,v10);       //op2 = -v10
	OPNode* op3 = create_binary_op_node(OP_TIMES,op2,x1);//op3 = op2*x1
	OPNode* op4 = create_binary_op_node(OP_PLUS,op1,op3);//op4 = op1 + op3
	OPNode*	op5 = create_binary_op_node(OP_TIMES,v10,x2);//op5 = v10*x2
	OPNode* op6 = create_binary_op_node(OP_PLUS,op4,op5);//op6 = op4+op5
	OPNode* op7 = create_binary_op_node(OP_DIVID,x3,v6); //op7 = x3/v6
	OPNode* op8 = create_binary_op_node(OP_MINUS,op6,op7);//op8 = op6 - op7
	x1->val = -1.9;
	x2->val = 2;
	x3->val = 5./6.;
	return op8;
}

Node* build_linear_function2(vector<Node*>& list)
{
    //f(x1,x2,x3) = -5*x1+-10*x1+10*x2-x3/6
    using namespace autodiff_placeholders;
    auto ten = boost::yap::make_terminal<autodiff_expr>(10);
    return to_auto_diff_node(
        -5 * 1_p + -ten * 1_p + 10 * 2_p - 3_p / 6,
        list,
        -1.9,
        2,
        5./6.
    );
}

Node* build_nl_function1_manually(vector<Node*>& list)
{
//	(x1*x2 * sin(x1))/x3 + x2*x4 - x1/x2
	VNode* x1 = create_var_node();
	VNode* x2 = create_var_node();
	VNode* x3 = create_var_node();
	VNode* x4 = create_var_node();
	x1->val = -1.23;
	x2->val = 7.1231;
	x3->val = 2;
	x4->val = -10;
	list.push_back(x1);
	list.push_back(x2);
	list.push_back(x3);
	list.push_back(x4);

	OPNode* op1 = create_binary_op_node(OP_TIMES,x2,x1);
	OPNode* op2 = create_uary_op_node(OP_SIN,x1);
	OPNode* op3 = create_binary_op_node(OP_TIMES,op1,op2);
	OPNode* op4 = create_binary_op_node(OP_DIVID,op3,x3);
	OPNode* op5 = create_binary_op_node(OP_TIMES,x2,x4);
	OPNode* op6 = create_binary_op_node(OP_PLUS,op4,op5);
	OPNode* op7 = create_binary_op_node(OP_DIVID,x1,x2);
	OPNode* op8 = create_binary_op_node(OP_MINUS,op6,op7);
	return op8;
}

Node* build_nl_function1(vector<Node*>& list)
{
    // (x1*x2 * sin(x1))/x3 + x2*x4 - x1/x2
    using namespace autodiff_placeholders;
    return to_auto_diff_node(
        (1_p * 2_p * sin_(1_p)) / 3_p + 2_p * 4_p - 1_p / 2_p,
        list,
	-1.23,
	7.1231,
	2,
	-10
    );
}

BOOST_AUTO_TEST_CASE( test_linear_fun1 )
{
	BOOST_TEST_MESSAGE("test_linear_fun1");
	vector<Node*> list;
	Node* root = build_linear_fun1(list);
	vector<double> grad;
	double val1 = grad_reverse(root,list,grad);
	double val2 = eval_function(root);
	double x1g[] = {-5.5440211108893697744548489936278,10.0,-0.16666666666666666666666666666667};

	for(unsigned int i=0;i<3;i++){
		CHECK_CLOSE(grad[i],x1g[i]);
	}

	double eval = 30.394751221800913;

	CHECK_CLOSE(val1,eval);
	CHECK_CLOSE(val2,eval);

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,0);
}

BOOST_AUTO_TEST_CASE( test_grad_sin )
{
	BOOST_TEST_MESSAGE("test_grad_sin");
	VNode* x1 = create_var_node();
	x1->val = 10;
	OPNode* root = create_uary_op_node(OP_SIN,x1);
	vector<Node*> nodes;
	nodes.push_back(x1);
	vector<double> grad;
	grad_reverse(root,nodes,grad);
	double x1g =  -0.83907152907645244;
	//the matlab give cos(10) = -0.839071529076452

	CHECK_CLOSE(grad[0],x1g);
	BOOST_CHECK_EQUAL(nodes.size(),1);

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,1);
}

BOOST_AUTO_TEST_CASE(test_grad_single_node)
{
	VNode* x1 = create_var_node();
	x1->val = -2;
	vector<Node*> nodes;
	nodes.push_back(x1);
	vector<double> grad;
	double val = grad_reverse(x1,nodes,grad);
	CHECK_CLOSE(grad[0],1);
	CHECK_CLOSE(val,-2);

	EdgeSet s;
	unsigned int n = 0;
	nonlinearEdges(x1,s);
	n = nzHess(s);
	BOOST_CHECK_EQUAL(n,0);

	grad.clear();
	nodes.clear();
	PNode* p = create_param_node(-10);
	//OPNode*	op = create_binary_op_node(TIMES,p,create_param_node(2));
	val = grad_reverse(p,nodes,grad);
	BOOST_CHECK_EQUAL(grad.size(),0);
	CHECK_CLOSE(val,-10);

	s.clear();
	nonlinearEdges(p,s);
	n = nzHess(s);
	BOOST_CHECK_EQUAL(n,0);
}

BOOST_AUTO_TEST_CASE(test_grad_neg)
{
	VNode* x1 = create_var_node();
	x1->val = 10;
	PNode* p2 = create_param_node(-1);
	vector<Node*> nodes;
	vector<double> grad;
	nodes.push_back(x1);
	Node* root = create_binary_op_node(OP_TIMES,x1,p2);
	grad_reverse(root,nodes,grad);
	CHECK_CLOSE(grad[0],-1);
	BOOST_CHECK_EQUAL(nodes.size(),1);
	nodes.clear();
	grad.clear();
	nodes.push_back(x1);
	root = create_uary_op_node(OP_NEG,x1);
	grad_reverse(root,nodes,grad);
	CHECK_CLOSE(grad[0],-1);

	EdgeSet s;
	unsigned int n = 0;
	nonlinearEdges(root,s);
	n = nzHess(s);
	BOOST_CHECK_EQUAL(n,0);
}

BOOST_AUTO_TEST_CASE( test_nl_function)
{
	vector<Node*> list;
	Node* root = build_nl_function1(list);
	double val = eval_function(root);
	vector<double> grad;
	grad_reverse(root,list,grad);
	double eval =-66.929555552886214;
	double gx[] = {-4.961306690356109,-9.444611307649055,-2.064383410399700,7.123100000000000};
	CHECK_CLOSE(val,eval);

	for(unsigned int i=0;i<4;i++)
	{
		CHECK_CLOSE(grad[i],gx[i]);
	}
	unsigned int nzgrad = nzGrad(root);
	unsigned int tol = numTotalNodes(root);
	BOOST_CHECK_EQUAL(nzgrad,4);
	BOOST_CHECK_EQUAL(tol,16);

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,11);
}

BOOST_AUTO_TEST_CASE( test_hess_reverse_1)
{
	vector<Node*> nodes;
	Node* root = build_linear_fun1(nodes);
	vector<double> grad;
	double val = grad_reverse(root,nodes,grad);
	double eval = eval_function(root);
//	cout<<eval<<"\t"<<grad[0]<<"\t"<<grad[1]<<"\t"<<grad[2]<<"\t"<<endl;

	CHECK_CLOSE(val,eval);

	for(unsigned int i=0;i<nodes.size();i++)
	{
		static_cast<VNode*>(nodes[i])->u = 0;
	}

	static_cast<VNode*>(nodes[0])->u = 1;
	double hval = 0;
	vector<double> dhess;
	hval = hess_reverse(root,nodes,dhess);
	CHECK_CLOSE(hval,eval);
	for(unsigned int i=0;i<dhess.size();i++)
	{
		CHECK_CLOSE(dhess[i],0);
	}
}

BOOST_AUTO_TEST_CASE( test_hess_reverse_2)
{
	vector<Node*> nodes;
	Node* root = build_linear_function2(nodes);
	vector<double> grad;
	double val = grad_reverse(root,nodes,grad);
	double eval = eval_function(root);

	CHECK_CLOSE(val,eval);
	for(unsigned int i=0;i<nodes.size();i++)
	{
		static_cast<VNode*>(nodes[i])->u = 0;
	}

	static_cast<VNode*>(nodes[0])->u = 1;
	double hval = 0;
	vector<double> dhess;
	hval = hess_reverse(root,nodes,dhess);
	CHECK_CLOSE(hval,eval);

	for(unsigned int i=0;i<dhess.size();i++)
	{
		CHECK_CLOSE(dhess[i],0);
	}

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,0);
}

BOOST_AUTO_TEST_CASE( test_hess_reverse_4)
{
	vector<Node*> nodes;
//	Node* root = build_nl_function1(nodes);

	VNode* x1 = create_var_node();
	nodes.push_back(x1);
	x1->val = 1;
	x1->u =1;
	Node* op = create_uary_op_node(OP_SIN,x1);
	Node* root = create_uary_op_node(OP_SIN,op);
	vector<double> grad;
	double eval = eval_function(root);
	vector<double> dhess;
	double hval = hess_reverse(root,nodes,dhess);
	CHECK_CLOSE(hval,eval);
	BOOST_CHECK_EQUAL(dhess.size(),1);
	CHECK_CLOSE(dhess[0], -0.778395788418109);

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,1);
}

BOOST_AUTO_TEST_CASE( test_hess_reverse_3)
{
	vector<Node*> nodes;
	VNode* x1 = create_var_node();
	VNode* x2 = create_var_node();
	nodes.push_back(x1);
	nodes.push_back(x2);
	x1->val = 2.5;
	x2->val = -9;
	Node* op1 = create_binary_op_node(OP_TIMES,x1,x2);
	Node* root = create_binary_op_node(OP_TIMES,x1,op1);
	double eval = eval_function(root);
	for(unsigned int i=0;i<nodes.size();i++)
	{
		static_cast<VNode*>(nodes[i])->u = 0;
	}
	static_cast<VNode*>(nodes[0])->u = 1;

	vector<double> dhess;
	double hval = hess_reverse(root,nodes,dhess);
	BOOST_CHECK_EQUAL(dhess.size(),2);
	CHECK_CLOSE(hval,eval);
	double hx[]={-18,5};
	for(unsigned int i=0;i<dhess.size();i++)
	{
		//Print("\t["<<i<<"]="<<dhess[i]);
		CHECK_CLOSE(dhess[i],hx[i]);
	}

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,3);
}

BOOST_AUTO_TEST_CASE( test_hess_reverse_5)
{
	vector<Node*> nodes;
	VNode* x1 = create_var_node();
	VNode* x2 = create_var_node();
	nodes.push_back(x1);
	nodes.push_back(x2);
	x1->val = 2.5;
	x2->val = -9;
	Node* op1 = create_binary_op_node(OP_TIMES,x1,x1);
	Node* op2 = create_binary_op_node(OP_TIMES,x2,x2);
	Node* op3 = create_binary_op_node(OP_MINUS,op1,op2);
	Node* op4 = create_binary_op_node(OP_PLUS,op1,op2);
	Node* root = create_binary_op_node(OP_TIMES,op3,op4);

	double eval = eval_function(root);

	for(unsigned int i=0;i<nodes.size();i++)
	{
		static_cast<VNode*>(nodes[i])->u = 0;
	}
	static_cast<VNode*>(nodes[0])->u = 1;

	vector<double> dhess;
	double hval = hess_reverse(root,nodes,dhess);
	CHECK_CLOSE(hval,eval);
	double hx[] ={75,0};
	for(unsigned int i=0;i<dhess.size();i++)
	{
		CHECK_CLOSE(dhess[i],hx[i]);
	}

	for(unsigned int i=0;i<nodes.size();i++)
	{
		static_cast<VNode*>(nodes[i])->u = 0;
	}
	static_cast<VNode*>(nodes[1])->u = 1;

	double hx2[] = {0, -972};
	hval = hess_reverse(root,nodes,dhess);
	for(unsigned int i=0;i<dhess.size();i++)
	{
		CHECK_CLOSE(dhess[i],hx2[i]);
	}

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,4);
}
BOOST_AUTO_TEST_CASE( test_hess_reverse_6)
{
	vector<Node*> nodes;
//	Node* root = build_nl_function1(nodes);

	VNode* x1 = create_var_node();
	VNode* x2 = create_var_node();
	nodes.push_back(x1);
	nodes.push_back(x2);
	x1->val = 2.5;
	x2->val = -9;
	Node* root = create_binary_op_node(OP_POW,x1,x2);

	double eval = eval_function(root);

	static_cast<VNode*>(nodes[0])->u=1;static_cast<VNode*>(nodes[1])->u=0;
	vector<double> dhess;
	double hval = hess_reverse(root,nodes,dhess);
	CHECK_CLOSE(hval,eval);
	double hx1[] ={0.003774873600000 , -0.000759862823419};
	double hx2[] ={-0.000759862823419, 0.000220093141567};
	for(unsigned int i=0;i<dhess.size();i++)
	{
		CHECK_CLOSE(dhess[i],hx1[i]);
	}
	static_cast<VNode*>(nodes[0])->u=0;static_cast<VNode*>(nodes[1])->u=1;
	hess_reverse(root,nodes,dhess);
	for(unsigned int i=0;i<dhess.size();i++)
	{
		CHECK_CLOSE(dhess[i],hx2[i]);
	}

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,4);
}

BOOST_AUTO_TEST_CASE( test_hess_reverse_7)
{
	vector<Node*> nodes;
	Node* root = build_nl_function1(nodes);

	double eval = eval_function(root);

	vector<double> dhess;
	double hx0[] ={-1.747958066718855,
			  -0.657091724418110,
			   2.410459188139686,
			                   0};
	double hx1[] ={ -0.657091724418110,
			   0.006806564792590,
			  -0.289815306593997,
			   1.000000000000000};
	double hx2[] ={  2.410459188139686,
			  -0.289815306593997,
			   2.064383410399700,
			                   0};
	double hx3[] ={0,1,0,0};
	for(unsigned int i=0;i<nodes.size();i++)
	{
		static_cast<VNode*>(nodes[i])->u = 0;
	}
	static_cast<VNode*>(nodes[0])->u = 1;
	double hval = hess_reverse(root,nodes,dhess);
	CHECK_CLOSE(hval,eval);
	for(unsigned int i=0;i<dhess.size();i++)
	{
		CHECK_CLOSE(dhess[i],hx0[i]);
	}

	for (unsigned int i = 0; i < nodes.size(); i++) {
		static_cast<VNode*>(nodes[i])->u = 0;
	}
	static_cast<VNode*>(nodes[1])->u = 1;
	hess_reverse(root, nodes, dhess);
	for (unsigned int i = 0; i < dhess.size(); i++) {
		CHECK_CLOSE(dhess[i], hx1[i]);
	}

	for (unsigned int i = 0; i < nodes.size(); i++) {
		static_cast<VNode*>(nodes[i])->u = 0;
	}
	static_cast<VNode*>(nodes[2])->u = 1;
	hess_reverse(root, nodes, dhess);
	for (unsigned int i = 0; i < dhess.size(); i++) {
		CHECK_CLOSE(dhess[i], hx2[i]);
	}

	for (unsigned int i = 0; i < nodes.size(); i++) {
		static_cast<VNode*>(nodes[i])->u = 0;
	}
	static_cast<VNode*>(nodes[3])->u = 1;
	hess_reverse(root, nodes, dhess);
	for (unsigned i = 0; i < dhess.size(); i++) {
		CHECK_CLOSE(dhess[i], hx3[i]);
	}
}

#if FORWARD_ENABLED
void test_hess_forward(Node* root, unsigned int& nvar)
{
	AutoDiff::num_var = nvar;
	unsigned int len = (nvar+3)*nvar/2;
	double* hess = new double[len];
	hess_forward(root,nvar,&hess);
	for(unsigned int i=0;i<len;i++){
		cout<<"hess["<<i<<"]="<<hess[i]<<endl;
	}
	delete[] hess;
}
#endif

BOOST_AUTO_TEST_CASE( test_hess_reverse_8)
{
	vector<Node*> list;
	vector<double> dhess;

	VNode* x1 = create_var_node();
	list.push_back(x1);
	static_cast<VNode*>(list[0])->val = -10.5;
	static_cast<VNode*>(list[0])->u = 1;
	double deval = hess_reverse(x1,list,dhess);
	CHECK_CLOSE(deval,-10.5);
	BOOST_CHECK_EQUAL(dhess.size(),1);
	BOOST_CHECK(isnan(dhess[0]));

	EdgeSet s;
	nonlinearEdges(x1,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,0);

	PNode* p1 = create_param_node(-1.5);
	list.clear();
	deval = hess_reverse(p1,list,dhess);
	CHECK_CLOSE(deval,-1.5);
	BOOST_CHECK_EQUAL(dhess.size(),0);

	s.clear();
	nonlinearEdges(p1,s);
	n = nzHess(s);
	BOOST_CHECK_EQUAL(n,0);
}

BOOST_AUTO_TEST_CASE( test_hess_revers9)
{
	vector<Node*> list;
	vector<double> dhess;
	VNode* x1 = create_var_node();
	list.push_back(x1);
	static_cast<VNode*>(list[0])->val = 2.5;
	static_cast<VNode*>(list[0])->u =1;
	Node* op1 = create_binary_op_node(OP_TIMES,x1,x1);
	Node* root = create_binary_op_node(OP_TIMES,op1,op1);
	double deval = hess_reverse(root,list,dhess);
	double eval = eval_function(root);
	CHECK_CLOSE(eval,deval);
	BOOST_CHECK_EQUAL(dhess.size(),1);
	CHECK_CLOSE(dhess[0],75);

	EdgeSet s;
	nonlinearEdges(root,s);
	unsigned int n = nzHess(s);
	BOOST_CHECK_EQUAL(n,1);

}

BOOST_AUTO_TEST_CASE( test_hess_revers10)
{
	vector<Node*> list;
	vector<double> dhess;
	VNode* x1 = create_var_node();
	VNode* x2 = create_var_node();
	list.push_back(x1);
	list.push_back(x2);
	Node* op1 = create_binary_op_node(OP_TIMES, x1,x2);
	Node* op2 = create_uary_op_node(OP_SIN,op1);
	Node* op3 = create_uary_op_node(OP_COS,op1);
	Node* root = create_binary_op_node(OP_TIMES, op2, op3);
	static_cast<VNode*>(list[0])->val = 2.1;
	static_cast<VNode*>(list[1])->val = 1.8;
	double eval = eval_function(root);

	//second column
	static_cast<VNode*>(list[0])->u = 0;
	static_cast<VNode*>(list[1])->u = 1;
	double deval = hess_reverse(root,list,dhess);
	CHECK_CLOSE(eval,deval);
	BOOST_CHECK_EQUAL(dhess.size(),2);
	CHECK_CLOSE(dhess[0], -6.945893481707861);
	CHECK_CLOSE(dhess[1],  -8.441601940854081);

	//first column
	static_cast<VNode*>(list[0])->u = 1;
	static_cast<VNode*>(list[1])->u = 0;
	deval = hess_reverse(root,list,dhess);
	CHECK_CLOSE(eval,deval);
	BOOST_CHECK_EQUAL(dhess.size(),2);
	CHECK_CLOSE(dhess[0], -6.201993262668304);
	CHECK_CLOSE(dhess[1], -6.945893481707861);
}

BOOST_AUTO_TEST_CASE( test_grad_reverse11)
{
	vector<Node*> list;
	VNode* x1 = create_var_node();
	Node* p2 = create_param_node(2);
	list.push_back(x1);
	Node* op1 = create_binary_op_node(OP_POW,x1,p2);
	static_cast<VNode*>(x1)->val = 0;
	vector<double> grad;
	grad_reverse(op1,list,grad);
	BOOST_CHECK_EQUAL(grad.size(),1);
	CHECK_CLOSE(grad[0],0);
}

BOOST_AUTO_TEST_CASE( test_hess_reverse12)
{
	vector<Node*> list;
	VNode* x1 = create_var_node();
	Node* p2 = create_param_node(2);
	list.push_back(x1);
	Node* op1 = create_binary_op_node(OP_POW,x1,p2);
	x1->val = 0;
	x1->u = 1;
	vector<double> hess;
	hess_reverse(op1,list,hess);
	BOOST_CHECK_EQUAL(hess.size(),1);
	CHECK_CLOSE(hess[0],2);
}

BOOST_AUTO_TEST_CASE( test_grad_reverse13)
{
	vector<Node*> list;
	VNode* x1 = create_var_node();
	PNode* p1 = create_param_node(0.090901);
	VNode* x2 = create_var_node();
	PNode* p2 = create_param_node(0.090901);
	list.push_back(x1);
	list.push_back(x2);
	Node* op1 = create_binary_op_node(OP_TIMES,x1,p1);
	Node* op2 = create_binary_op_node(OP_TIMES,x2,p2);
	Node* root = create_binary_op_node(OP_PLUS,op1,op2);
	x1->val = 1;
	x2->val = 1;
	vector<double> grad;
	grad_reverse(root,list,grad);
	BOOST_CHECK_EQUAL(grad.size(),2);
	CHECK_CLOSE(grad[0],0.090901);
	CHECK_CLOSE(grad[1],0.090901);
}

BOOST_AUTO_TEST_SUITE_END()


#include "Rational.h"

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

template <typename Archive>
void Rational::serialize(Archive & ar, unsigned int const version) {
  if (version > 0) {
  } else {
  }
  ar & fraction;
}

template void Rational::serialize<boost::archive::text_oarchive>(boost::archive::text_oarchive&, unsigned int const);
template void Rational::serialize<boost::archive::text_iarchive>(boost::archive::text_iarchive&, unsigned int const);

Rational::Rational() : Rational(std::make_pair(0, 1)) { }

Rational::Rational(Fraction const & fraction_set) : fraction(Cancel(fraction_set)){ }

Rational::Rational(int numerator, unsigned int denominator) : Rational(std::make_pair(numerator, denominator)) { }

Rational::Rational(mpq_class q) : Rational(q.get_num().get_si(), q.get_den().get_ui()) { }

Fraction Rational::get_fraction() const { return fraction; }

bool Rational::IsPositive() const { return fraction.first > 0; }
bool Rational::IsNegative() const { return fraction.first < 0; }
bool Rational::IsZero()     const { return fraction.first == 0; }

void Rational::DivideByTwo() {
  if (fraction.first % 2 == 0) {
    fraction.first = fraction.first / 2;
  } else {
    fraction.second = fraction.second * 2;
  }
}

void Rational::Negate() {
  fraction.first = -1 * fraction.first;
}

Fraction Rational::Cancel(Fraction const & fraction_set) {
  unsigned int gcd { GreatestCommonDivisor(fraction_set) };
  return std::make_pair(fraction_set.first / static_cast<int>(gcd), fraction_set.second / gcd);
}

unsigned int Rational::LeastCommonMultiple(Fraction const & fraction) {
  if (fraction.first == 0) {
    return 0;
  }
  unsigned int gcd = GreatestCommonDivisor(fraction);

  if (fraction.first < 0) {
    return (static_cast<unsigned int>(-fraction.first) * fraction.second) / gcd;
  } else {
    return (static_cast<unsigned int>(fraction.first) * fraction.second) / gcd;
  }
}

unsigned int Rational::GreatestCommonDivisor(Fraction const & fraction) {
  return (fraction.first < 0 ? GreatestCommonDivisor(std::make_pair(-fraction.first, fraction.second)) : (fraction.second == 0 ? fraction.first : GreatestCommonDivisor(std::make_pair(fraction.second, fraction.first % fraction.second))));
}

Rational Rational::AddOther (Rational const & other) const {
  return Rational(fraction.first * other.fraction.second + fraction.second * other.fraction.first, fraction.second * other.fraction.second);
}

Rational Rational::SubtractOther (Rational const & other) const {
  return Rational(fraction.first * other.fraction.second - fraction.second * other.fraction.first, fraction.second * other.fraction.second);
}

Rational Rational::DivideOther (Rational const & other) const {
  int numerator = fraction.first * static_cast<int>(other.fraction.second);
  int denominator = static_cast<int>(fraction.second) * other.fraction.first;

  if (numerator * denominator < 0) {
    return Rational(-1 * std::abs(numerator), static_cast<unsigned int>(std::abs(denominator)));
  } else {
    return Rational(std::abs(numerator), static_cast<unsigned int>(std::abs(denominator)));
  }
}

Rational Rational::MultiplyOther (Rational const & other) const {
  return Rational(fraction.first * other.fraction.first, fraction.second * other.fraction.second);
}

std::string const Rational::ToString(bool plus_sign) const {
  std::string ret_string = "";
  if (fraction.first == 0) {
    if (plus_sign) {
      return "+ 0";
    } else {
      return "0";
    }
  } else if (fraction.first > 0) {
    if (plus_sign) {
      ret_string += "+ ";
    }
  } else {
    ret_string += "- ";
  }

  ret_string += std::to_string(std::abs(fraction.first));

  if (fraction.second == 1) {
    return ret_string;
  }

  ret_string += "/";
  ret_string += std::to_string(fraction.second);

  return ret_string;
}

bool Rational::operator== (Rational const & other) const { return (fraction.first == other.fraction.first) && (fraction.second == other.fraction.second); }
bool Rational::operator!= (Rational const & other) const { return !(*this == other); }
bool Rational::operator< (Rational const & other) const { return this->SubtractOther(other).fraction.first < 0; }
bool Rational::operator> (Rational const & other) const { return other < *this; }
bool Rational::operator<= (Rational const & other) const { return !(*this > other); }
bool Rational::operator>= (Rational const & other) const { return !(*this < other); }


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/sqr_abs.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/five.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( sqr_abs_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::sqr_abs;
  using nt2::tag::sqr_abs_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef native<cT,ext_t>                vcT;
  typedef typename nt2::dry<T>             dT;
  typedef native<dT,ext_t>                vcT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(sqr_abs(nt2::Inf<vcT>()), nt2::Inf<vT>());
  NT2_TEST_EQUAL(sqr_abs(nt2::Minf<vcT>()), nt2::Inf<vT>());
  NT2_TEST_EQUAL(sqr_abs(nt2::Nan<vcT>()), nt2::Nan<vT>());
  NT2_TEST_EQUAL(sqr_abs(nt2::Valmax<vcT>()), nt2::Inf<vT>());
  NT2_TEST_EQUAL(sqr_abs(nt2::Valmin<vcT>()), nt2::Inf<vT>());
#endif
  NT2_TEST_EQUAL(sqr_abs(nt2::Mone<vcT>()), nt2::One<vT>());
  NT2_TEST_EQUAL(sqr_abs(nt2::One<vcT>()), nt2::One<vT>());
  NT2_TEST_EQUAL(sqr_abs(nt2::Zero<vcT>()), nt2::Zero<vT>());
  NT2_TEST_EQUAL(sqr_abs(vcT(nt2::One<vT>(),nt2::One<vT>()), nt2::TwoT>());
  NT2_TEST_EQUAL(sqr_abs(vcT(nt2::Three<vT>(),nt2::Four<vT>()) ), nt2::sqr(nt2::Five<vcT>()));
  NT2_TEST_EQUAL(sqr_abs(nt2::Mone<vdT>()), nt2::One<vT>());
  NT2_TEST_EQUAL(sqr_abs(nt2::One<vdT>()), nt2::One<vT>());
  NT2_TEST_EQUAL(sqr_abs(nt2::Zero<vdT>()), nt2::Zero<vT>());
}


#pragma once

//          Copyright Rein Halbersma 2014-2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/container/flat_set.hpp> // flat_set
#include <algorithm>                    // lexicographical_compare_three_way
#include <compare>                      // strong_ordering

namespace boost::container {

template<class Compare, class Allocator>
[[nodiscard]] auto operator<=>(flat_set<int, Compare, Allocator> const& lhs, flat_set<int, Compare, Allocator> const& rhs) noexcept
        -> std::strong_ordering
{
        return std::lexicographical_compare_three_way(lhs.begin(), lhs.end(), rhs.begin(), rhs.end());
}

}       // namespace boost::container


// libgp - Gaussian process library for Machine Learning
// Copyright (c) 2013, Manuel Blum <mblum@informatik.uni-freiburg.de>
// All rights reserved.

#include "cov_factory.h"

#include <Eigen/Dense>
#include <gtest/gtest.h>

TEST(CovFactoryTest, Parser) {

  libgp::CovFactory factory;
  libgp::CovarianceFunction * covf;

  covf = factory.create(4, "CovSum ( CovSum(CovLinearone, CovNoise), CovMatern3iso)");
  ASSERT_EQ(covf->to_string().compare("CovSum(CovSum(CovLinearone, CovNoise), CovMatern3iso)"), 0);
  delete covf;

  covf = factory.create(4, "CovSum(CovMatern3iso, CovSum(CovLinearone, CovNoise))");
  ASSERT_EQ(covf->to_string().compare("CovSum(CovMatern3iso, CovSum(CovLinearone, CovNoise))"), 0);
  delete covf;

  covf = factory.create(4, "CovSum(CovLinearone,CovNoise)");
  ASSERT_EQ(covf->to_string().compare("CovSum(CovLinearone, CovNoise)"), 0);
  delete covf;

  covf = factory.create(4, "CovSum ( CovLinearone , CovNoise )");
  ASSERT_EQ(covf->to_string().compare("CovSum(CovLinearone, CovNoise)"), 0);
  delete covf;

  covf = factory.create(4, "CovLinearone");
  ASSERT_EQ(covf->to_string().compare("CovLinearone"), 0);
  delete covf;

  covf = factory.create(4, "InputDimFilter(2/CovSEiso)");
  ASSERT_EQ(covf->to_string().compare("InputDimFilter(2/CovSEiso)"), 0);
  delete covf;
}


#include <boost/graph/adjacency_list.hpp>
#include <tuple>
#include <algorithm>
#include <iterator>
#include <iostream>

int main()
{
  typedef boost::adjacency_list<boost::setS, boost::vecS,
    boost::undirectedS> graph;
  graph g;

  enum { topLeft, topRight, bottomRight, bottomLeft };

  boost::add_edge(topLeft, topRight, g);
  boost::add_edge(topRight, bottomRight, g);
  boost::add_edge(bottomRight, bottomLeft, g);
  boost::add_edge(bottomLeft, topLeft, g);

  graph::adjacency_iterator vit, vend;
  std::tie(vit, vend) = boost::adjacent_vertices(topLeft, g);
  std::copy(vit, vend,
    std::ostream_iterator<graph::vertex_descriptor>{std::cout, "\n"});

  graph::out_edge_iterator eit, eend;
  std::tie(eit, eend) = boost::out_edges(topLeft, g);
  std::for_each(eit, eend,
    [&g](graph::edge_descriptor it)
      { std::cout << boost::target(it, g) << '\n'; });
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/make_int_range_count.hpp>
#include <fcppt/no_init.hpp>
#include <fcppt/cast/size.hpp>
#include <fcppt/container/grid/object.hpp>
#include <fcppt/container/grid/resize_preserve_init.hpp>
#include <fcppt/container/grid/size_type.hpp>
#include <fcppt/math/dim/comparison.hpp>
#include <fcppt/math/dim/output.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <utility>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	container_grid_resize_preverse_init
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::container::grid::object<
		std::pair<
			fcppt::container::grid::size_type,
			fcppt::container::grid::size_type
		>,
		2
	>
	sz_pair_grid;

	sz_pair_grid test(
		sz_pair_grid::dim(
			5u,
			10u
		),
		fcppt::no_init{}
	);

	for(
		auto const y
		:
		fcppt::make_int_range_count(
			test.size()[1]
		)
	)
		for(
			auto const x
			:
			fcppt::make_int_range_count(
				test.size()[0]
			)
		)
			test[
				sz_pair_grid::pos(
					x,
					y
				)
			] =
				std::make_pair(
					x,
					y
				);

	fcppt::container::grid::resize_preserve_init(
		test,
		sz_pair_grid::dim(
			10u,
			15u
		),
		sz_pair_grid::value_type(
			10u,
			20u
		)
	);

	BOOST_CHECK_EQUAL(
		test.size(),
		sz_pair_grid::dim(
			10u,
			15u
		)
	);

	for(
		auto const y
		:
		fcppt::make_int_range_count(
			test.size()[1]
		)
	)
		for(
			auto const x
			:
			fcppt::make_int_range_count(
				test.size()[0]
			)
		)
		{
			if(
				x < 5 && y < 10
			)
				BOOST_CHECK(
					test[
						sz_pair_grid::pos(
							x,
							y
						)
					]
					==
					std::make_pair(
						fcppt::cast::size<
							fcppt::container::grid::size_type
						>(
							x
						),
						fcppt::cast::size<
							fcppt::container::grid::size_type
						>(
							y
						)
					)
				);
			else
				BOOST_CHECK(
					test[
						sz_pair_grid::pos(
							x,
							y
						)
					]
					==
					sz_pair_grid::value_type(
						10u,
						20u
					)
				);
		}
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120530    M.I. Ganeff       File created.
 *      121018    M.I. Ganeff       Added unit tests for computeSphereOfInfluence().
 *      121123    D. Dirkx          Added unit tests for computeSphereOfInfluence() function taking
 *                                  mass ratio.
 *      130227    D. Dirkx          Added unit test for isOrbitRetrograde() functions.
 *      130305    R.C.A. Boon       Replaced Eigen::VectorXd by basic_mathematics::Vector6d.
 *
 *    References
 *      Montebruck O, Gill E. Satellite Orbits, Corrected Third Printing, Springer, 2005.
 *      Bate R. Fundamentals of Astrodynamics, Courier Dover Publications, 1971.
 *      Wikipedia, Sphere of Influence, accesed 121018.
 *        http://en.wikipedia.org/wiki/Sphere_of_influence_(astrodynamics)
 *      Petit et al., IERS Conventions, IERS, 2010.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

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

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"

namespace tudat
{
namespace unit_tests
{

//! Show the functionality of the unit tests.
BOOST_AUTO_TEST_SUITE( test_Mission_Geometry )

//! Unit test for shadow function (Sun,Earth).
BOOST_AUTO_TEST_CASE( testShadowFunctionForFullShadow )
{
    // Satellite totally blocked from solar radiation by the earth. Sun and satellite located on the
    // x-axis with the Earth in between. Altitude of satellite = 1000 km.

    const Eigen::Vector3d occultedBodyPosition = -149598000.0e3 * Eigen::Vector3d( 1.0, 0.0, 0.0 );
    const Eigen::Vector3d occultingBodyPosition = Eigen::Vector3d::Zero( );
    const double occultedBodyRadius = 6.96e8; // Siedelmann 1992.
    const double occultingBodyRadius = 6378.137e3; // WGS-84.

    const Eigen::Vector3d satellitePosition = ( occultingBodyRadius + 1.0e6 )
            * Eigen::Vector3d( 1.0, 0.0, 0.0 );

    // Compute shadow function.
    const double shadowFunction = mission_geometry::computeShadowFunction(
                occultedBodyPosition,
                occultedBodyRadius,
                occultingBodyPosition,
                occultingBodyRadius,
                satellitePosition );

    // Test values.
    BOOST_CHECK_EQUAL( 0.0, shadowFunction );
}

BOOST_AUTO_TEST_CASE( testShadowFunctionForFullLight )
{
    // Satellite totally subjected to sunlight. Satellite is located on the y-axis and the sun is
    // located on the x-axis. Altitude of satellite = 1000 km.

    const Eigen::Vector3d occultedBodyPosition = -149598000.0e3 * Eigen::Vector3d( 1.0, 0.0, 0.0 );
    const Eigen::Vector3d occultingBodyPosition = Eigen::Vector3d::Zero( );
    const double occultedBodyRadius = 6.96e8; // Siedelmann 1992.
    const double occultingBodyRadius = 6378.137e3; // WGS-84.

    const Eigen::Vector3d satellitePosition = ( occultingBodyRadius + 1.0e6 )
            * Eigen::Vector3d( 0.0, 1.0, 0.0 );

    // Compute shadow function.
    const double shadowFunction = mission_geometry::computeShadowFunction(
                occultedBodyPosition,
                occultedBodyRadius,
                occultingBodyPosition,
                occultingBodyRadius,
                satellitePosition );

    // Test values
    BOOST_CHECK_EQUAL( 1.0, shadowFunction );
}

BOOST_AUTO_TEST_CASE( testShadowFunctionForPartialShadow )
{
    // Satellite partially visible. Satellite is located between the locations of the previous
    // tests in penumbra. According to analytical derivations in Matlab the shadow function should
    // be around 0.4547. Altitude of satellite = 1000 km.

    const Eigen::Vector3d occultedBodyPosition = -149598000.0e3 * Eigen::Vector3d( 1.0, 0.0, 0.0 );
    const Eigen::Vector3d occultingBodyPosition = Eigen::Vector3d::Zero( );
    const double occultedBodyRadius = 6.96e8; // Siedelmann 1992.
    const double occultingBodyRadius = 6378.137e3; // WGS-84.

    Eigen::Vector3d satelliteDirection( 0.018, 1.0, 0.0 );
    satelliteDirection.normalize( );

    const Eigen::Vector3d satellitePosition = ( occultingBodyRadius + 1.0e3 ) * satelliteDirection;

    // Compute shadow function
    const double shadowFunction = mission_geometry::computeShadowFunction(
                occultedBodyPosition,
                occultedBodyRadius,
                occultingBodyPosition,
                occultingBodyRadius,
                satellitePosition );

    // Test values.
    BOOST_CHECK_CLOSE_FRACTION( 0.4547, shadowFunction, 0.001 );
}

//! Unit test for computation of radius of sphere of influence (Earth with respect to Sun).
BOOST_AUTO_TEST_CASE( testSphereOfInfluenceEarth )
{
    const double distanceEarthSun = 1.49597870700e11;  // (IERS, 2010)
    const double massEarth = 5.972186390142457e24;     // (IERS, 2010)
    const double massSun = 1.988415860572227e30;       // (IERS, 2010)

    // Test 1: test function taking masses.
    {
        // Calculate the sphere of influence of the Earth with respect to the Sun.
        const double sphereOfInfluenceEarth
                = mission_geometry::computeSphereOfInfluence(
                    distanceEarthSun, massEarth, massSun );

        // Test values (Wikipedia, Sphere of Influence)
        BOOST_CHECK_CLOSE_FRACTION( 9.25e8, sphereOfInfluenceEarth, 5.0e-4 );
    }

    // Test 2: test function taking mass ratio.
    {
        // Calculate the sphere of influence of the Earth with respect to the Sun.
        const double sphereOfInfluenceEarth
                = mission_geometry::computeSphereOfInfluence(
                    distanceEarthSun, massEarth / massSun );

        // Test values (Wikipedia, Sphere of Influence)
        BOOST_CHECK_CLOSE_FRACTION( 9.25e8, sphereOfInfluenceEarth, 5.0e-4 );
    }
}

//! Unit test for computation of radius of sphere of influence (Moon with respect to Earth).
BOOST_AUTO_TEST_CASE( testSphereOfInfluenceMoon )
{
    const double distanceMoonEarth = 3.84400e8;     // (Horizons, NASA)
    const double massMoon = 7.345811416686730e22;   // (IERS, 2010)
    const double massEarth = 5.972186390142457e24;  // (IERS, 2010)

    // Test 1: test function taking masses.
    {
        // Calculate the sphere of influence of the Moon with respect to the Earth.
        const double sphereOfInfluenceEarth
                = mission_geometry::computeSphereOfInfluence(
                    distanceMoonEarth, massMoon, massEarth );

        // Test values (Wikipedia, Sphere of Influence)
        BOOST_CHECK_CLOSE_FRACTION( 6.61e7, sphereOfInfluenceEarth, 2.0e-3 );
    }

    // Test 2: test function taking mass ratio.
    {
        const double sphereOfInfluenceMoon
                = mission_geometry::computeSphereOfInfluence(
                    distanceMoonEarth, massMoon / massEarth );

        // Test values (Wikipedia, Sphere of Influence)
        BOOST_CHECK_CLOSE_FRACTION( 6.61e7, sphereOfInfluenceMoon, 2.0e-3 );
    }
}

//! Unit test for testing for retrogradeness.
BOOST_AUTO_TEST_CASE( testIsOrbitRetrograde )
{
    using namespace orbital_element_conversions;
    using namespace mission_geometry;
    using namespace mathematical_constants;

    // Initialize test Kepler elements.
    basic_mathematics::Vector6d testKepler = Eigen::VectorXd::Zero( 6 );
    testKepler( semiMajorAxisIndex ) = 1.0e7;
    testKepler( eccentricityIndex ) = 0.1;
    testKepler( inclinationIndex ) = 50.0 / 180.0 * PI;
    testKepler( argumentOfPeriapsisIndex ) = 350.0 / 180.0 * PI;
    testKepler( longitudeOfAscendingNodeIndex ) = 15.0 / 180.0 * PI;
    testKepler( trueAnomalyIndex ) = 170.0 / 180.0 * PI;

    bool expectedIsOrbitRetrograde = false;

    bool calculatedIsOrbitRetrograde = true;

    // Test value for prograde orbit, not near boundary.
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for prograde orbit, near zero boundary.
    {
        testKepler( inclinationIndex ) = std::numeric_limits< double >::epsilon( );
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for prograde orbit, near 90 degrees boundary.
    {
        testKepler( inclinationIndex ) = PI / 2.0 - std::numeric_limits< double >::epsilon( );
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for retrograde orbit, near 90 degrees boundary.
    {
        expectedIsOrbitRetrograde = 1;
        testKepler( inclinationIndex ) = PI / 2.0 + std::numeric_limits< double >::epsilon( );
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for retrograde orbit, not near boundary.
    {
        testKepler( inclinationIndex ) = 2.0;
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for retrograde orbit, near 180 degrees boundary.
    {
        testKepler( inclinationIndex ) = PI - std::numeric_limits< double >::epsilon( );
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Check exception handling for inclinations out of range.
    bool isExceptionFound = false;
    testKepler( inclinationIndex ) = PI + 1.0;

    // Try to calculate retrogradeness.
    try
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
    }
    // Catch the expected runtime error, and set the boolean flag to true.
    catch ( std::runtime_error )
    {
        isExceptionFound = true;
    }
    // Check value of flag.
    BOOST_CHECK( isExceptionFound );

    isExceptionFound = false;
    // Try to calculate retrogradeness.
    try
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
    }
    // Catch the expected runtime error, and set the boolean flag to true.
    catch ( std::runtime_error )
    {
        isExceptionFound = true;
    }
    // Check value of flag.
    BOOST_CHECK( isExceptionFound );

    isExceptionFound = false;
    testKepler( inclinationIndex ) = -1.0;
    // Try to calculate retrogradeness.
    try
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
    }
    // Catch the expected runtime error, and set the boolean flag to true.
    catch ( std::runtime_error )
    {
        isExceptionFound = true;
    }
    // Check value of flag.
    BOOST_CHECK( isExceptionFound );

    isExceptionFound = false;
    // Try to calculate retrogradeness.
    try
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
    }
    // Catch the expected runtime error, and set the boolean flag to true.
    catch ( std::runtime_error )
    {
        isExceptionFound = true;
    }
    // Check value of flag.
    BOOST_CHECK( isExceptionFound );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//
// Created by Gleb Marin on 04.09.2021.
//

#include <random>

#include <boost/compute/device.hpp>

#include <doctest.h>

#include "CooMatrix.hpp"

TEST_SUITE("CooMatrix Stress") {

    using FloatMatrix = floatMatrix::CooMatrix;
    static std::mt19937 rand{std::random_device{}()};

    template <typename T>
    std::size_t getRandInRange(const T& begin, const T& end) {
        return rand() % (end - begin) + begin;
    }

    float getRandFloat() {
        return std::uniform_real_distribution(-20.f, 20.f)(rand);
    }

    FloatMatrix generateMatrix(std::size_t rows, std::size_t cols, std::size_t values) {
        FloatMatrix mtx;
        for (std::size_t i = 0; i < values; ++i) {
            mtx.set(getRandInRange(0UL, rows), getRandInRange(0UL, cols), getRandFloat());
        }
        return mtx;
    }

    struct MatrixPairs {
        FloatMatrix a, b, aCopy, bCopy;
    };

    MatrixPairs generateMatrixPairs(std::size_t rows, std::size_t cols, std::size_t values) {
        FloatMatrix a = generateMatrix(rows, cols, values);
        FloatMatrix b = generateMatrix(rows, cols, values);
        return {a, b, a, b};
    }

    void executeStress(std::size_t rows, std::size_t cols, std::size_t values, std::size_t repeats) {
        for (std::size_t it = 0; it < repeats; ++it) {
            auto [a, b, aCopy, bCopy] = generateMatrixPairs(rows, cols, values);

            boost::compute::device device = boost::compute::system::default_device();
            boost::compute::context context(device);
            boost::compute::command_queue queue(context, device);

            a.add(b);

            try {
                aCopy.add(bCopy, queue);
            } catch (const boost::compute::program_build_failure& e) {
                std::cerr << e.build_log() << std::endl;
                FAIL("OpenCL Program Build failed");
            }

            CHECK_EQ(a, aCopy);
        }
    }

    TEST_CASE("One Cell") {
        executeStress(1, 1, 1, 1);
    }

    TEST_CASE("Tiny Empty") {
        executeStress(5, 5, 10, 1);
    }

    TEST_CASE("Tiny Full") {
        executeStress(5, 5, 5, 5);
    }

    TEST_CASE("Small Empty") {
        executeStress(100, 70, 20, 5);
    }

    TEST_CASE("Small Full") {
        executeStress(100, 70, 6000, 5);
    }

    TEST_CASE("Large Empty") {
        executeStress(500, 200, 2000, 3);
    }

    TEST_CASE("Large Full") {
        executeStress(500, 200, 10000, 3);
    }
}


#include <boost/core/lightweight_test.hpp>
#include <boost/histogram.hpp>
#include <boost/histogram/ostream.hpp>
#include <vector>
#include "throw_exception.hpp"

namespace bh = boost::histogram;
using uogrowth_t = decltype(bh::axis::option::growth | bh::axis::option::underflow |
                            bh::axis::option::overflow);

using arg_t = boost::variant2::variant<std::vector<int>, int>;

int main() {
  using axis_type =
      bh::axis::regular<double, bh::use_default, bh::use_default, uogrowth_t>;
  using axis_variant_type = bh::axis::variant<axis_type>;

  // 1D growing A
  {
    auto axes = std::vector<axis_variant_type>({axis_type(10, 0, 1)});
    auto h = bh::make_histogram_with(bh::dense_storage<double>(), axes);
    auto h2 = h;

    h(-1);

    // used to crash, while growing B did not crash
    std::vector<arg_t> vargs = {-1};
    h2.fill(vargs);

    BOOST_TEST_EQ(h, h2);
  }

  // 1D growing B
  {
    auto axes = std::vector<axis_variant_type>({axis_type(10, 0, 1)});
    auto h = bh::make_histogram_with(bh::dense_storage<double>(), axes);
    auto h2 = h;

    h(2);
    h(-1);

    std::vector<int> f1({2});
    std::vector<int> f2({-1});

    h2.fill(f1);
    h2.fill(f2);

    BOOST_TEST_EQ(h, h2);
  }

  return boost::report_errors();
}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.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>
#include <boost/simd/constant/two.hpp>

template <typename T, int N, typename Env>
void test(Env& runtime)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], a2[N], a3[N], b[N];
  for(int i = 0; i < N; ++i)
  {
     a1[i] = (i%2) ? T(i) : T(-i);
     a2[i] = (i%2) ? T(i+N) : T(-(i+N));
     a3[i] = (i%2) ? T(i+2*N) : T(-(i+2*N));
     b[i] = bs::fma(a1[i], a2[i], a3[i]);
   }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t aa2(&a2[0], &a2[0]+N);
  p_t aa3(&a3[0], &a3[0]+N);
  p_t bb (&b [0], &b [0]+N);
  STF_EQUAL(bs::fma(aa1, aa2, aa3), bb);
}

STF_CASE_TPL("Check fma on pack" , STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL (" fma real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;

  using bs::fma;
  using p_t = bs::pack<T>;

  // return type conformity test
  STF_EXPR_IS(fma(p_t(),p_t(),p_t()), p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(fma(bs::Inf<p_t>(), bs::Inf<p_t>(), bs::Inf<p_t>()), bs::Inf<p_t>());
  STF_IEEE_EQUAL(fma(bs::Minf<p_t>(), bs::Minf<p_t>(), bs::Minf<p_t>()), bs::Nan<p_t>());
  STF_IEEE_EQUAL(fma(bs::Nan<p_t>(), bs::Nan<p_t>(), bs::Nan<p_t>()), bs::Nan<p_t>());
#endif
  STF_EQUAL(fma(bs::Mone<p_t>(), bs::Mone<p_t>(), bs::Mone<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(fma(bs::One<p_t>(), bs::One<p_t>(), bs::One<p_t>()), bs::Two<p_t>());
  STF_EQUAL(fma(bs::Zero<p_t>(), bs::Zero<p_t>(), bs::Zero<p_t>()), bs::Zero<p_t>());
} // end of test for floating_

STF_CASE_TPL (" fma unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;

  using bs::fma;

  // return type conformity test
  STF_EXPR_IS(fma(p_t(),p_t(),p_t()), p_t);

  // specific values tests
  STF_EQUAL(fma(bs::One<p_t>(), bs::One<p_t>(), bs::One<p_t>()), bs::Two<p_t>());
  STF_EQUAL(fma(bs::Zero<p_t>(), bs::Zero<p_t>(), bs::Zero<p_t>()), bs::Zero<p_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" fma signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;

  using bs::fma;

  // return type conformity test
  STF_EXPR_IS(fma(p_t(),p_t(),p_t()), p_t);

  // specific values tests
  STF_EQUAL(fma(bs::Mone<p_t>(), bs::Mone<p_t>(), bs::Mone<p_t>()), bs::Zero<p_t>());
  STF_EQUAL(fma(bs::One<p_t>(), bs::One<p_t>(), bs::One<p_t>()), bs::Two<p_t>());
  STF_EQUAL(fma(bs::Zero<p_t>(), bs::Zero<p_t>(), bs::Zero<p_t>()), bs::Zero<p_t>());
} // end of test for signed_int_


//
//  Copyright (c) 2002-2003
//  Toon Knapen, Kresimir Fresl, Joerg Walter
//
//  Permission to use, copy, modify, distribute and sell this software
//  and its documentation for any purpose is hereby granted without fee,
//  provided that the above copyright notice appear in all copies and
//  that both that copyright notice and this permission notice appear
//  in supporting documentation.  The authors make no representations
//  about the suitability of this software for any purpose.
//  It is provided "as is" without express or implied warranty.
//
//

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_SYMM_HERM_RAW_HPP
#define BOOST_NUMERIC_BINDINGS_TRAITS_SYMM_HERM_RAW_HPP

#include <boost/numeric/bindings/traits/matrix_raw.hpp>
#ifndef BOOST_UBLAS_HAVE_BINDINGS
#  include <boost/numeric/ublas/symmetric.hpp>
#  include <boost/numeric/ublas/hermitian.hpp>
#endif

namespace boost { namespace numeric { namespace bindings { namespace traits {

  namespace ublas = boost::numeric::ublas;

  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::symmetric_adaptor<M, F> &m) {
    return bindings::traits::leading_dimension (m.data());
  }

  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  int leading_dimension (const ublas::hermitian_adaptor<M, F> &m) {
    return bindings::traits::leading_dimension (m.data());
  }

  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::symmetric_adaptor<M, F> &m) {
    return matrix_storage_size (m.data());
  }

  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::hermitian_adaptor<M, F> &m) {
    return matrix_storage_size (m.data());
  }

  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::symmetric_matrix<T,F1,F2,A> &m) {
    return (int) ((m.size1() * (m.size1() + 1)) / 2);
  }

  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  int matrix_storage_size (const ublas::hermitian_matrix<T,F1,F2,A> &m) {
    return (int) ((m.size1() * (m.size1() + 1)) / 2);
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::symmetric_matrix<T,F1,F2,A>::const_pointer
  matrix_storage (const ublas::symmetric_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }
#endif
  // We need data_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::symmetric_matrix<T,F1,F2,A>::const_pointer
  matrix_storage_const (const ublas::symmetric_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::symmetric_matrix<T,F1,F2,A>::pointer
  matrix_storage (ublas::symmetric_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage (const ublas::symmetric_adaptor<M, F> &m) {
    return matrix_storage (m.data());
  }
#endif
  // We need data_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage_const (const ublas::symmetric_adaptor<M, F> &m) {
    return matrix_storage_const (m.data());
  }
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::pointer matrix_storage (ublas::symmetric_adaptor<M, F> &m) {
    return matrix_storage (m.data());
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::hermitian_matrix<T,F1,F2,A>::const_pointer
  matrix_storage (const ublas::hermitian_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }
#endif
  // We need data_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::hermitian_matrix<T,F1,F2,A>::const_pointer
  matrix_storage_const (const ublas::hermitian_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }
  template<typename T, typename F1, typename F2, typename A>
  BOOST_UBLAS_INLINE
  typename ublas::hermitian_matrix<T,F1,F2,A>::pointer
  matrix_storage (ublas::hermitian_matrix<T,F1,F2,A> &m) {
    return &m.data().begin()[0];
  }

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage (const ublas::hermitian_adaptor<M, F> &m) {
    return matrix_storage (m.data());
  }
#endif
  // We need data_const() mostly due to MSVC 6.0.
  // But how shall we write portable code otherwise?
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::const_pointer
  matrix_storage_const (const ublas::hermitian_adaptor<M, F> &m) {
    return matrix_storage_const (m.data());
  }
  template <typename M, typename F>
  BOOST_UBLAS_INLINE
  typename M::pointer matrix_storage (ublas::hermitian_adaptor<M, F> &m) {
    return matrix_storage (m.data());
  }

  namespace detail {

    inline char m_uplo_tag (ublas::upper_tag const&) { return 'U'; }
    inline char m_uplo_tag (ublas::lower_tag const&) { return 'L'; }

  }

  template <typename SymmM>
  inline
  char matrix_uplo_tag (SymmM&) {
      typedef typename SymmM::packed_category uplo_t;
      return detail::m_uplo_tag (uplo_t());
  }

}}}}

#endif


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_FreeGasElasticMarginalAlphaFunction.hpp
//! \author Alex Robinson
//! \brief  Free gas elastic marginal alpha function definition.
//!
//---------------------------------------------------------------------------//

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

// FRENSIE Includes
#include "DataGen_FreeGasElasticMarginalAlphaFunction.hpp"
#include "Utility_SearchAlgorithms.hpp"
#include "Utility_KinematicHelpers.hpp"
#include "Utility_ContractException.hpp"

namespace DataGen{

// Constructor
FreeGasElasticMarginalAlphaFunction::FreeGasElasticMarginalAlphaFunction(
	  const Teuchos::RCP<Utility::OneDDistribution>&
	  zero_temp_elastic_cross_section,
          const Teuchos::RCP<MonteCarlo::NuclearScatteringAngularDistribution>&
	  cm_scattering_distribution,
	  const double A,
	  const double kT,
	  const double beta,
	  const double E )
  : d_gkq_set( 1e-4 ),
    d_sab_function( zero_temp_elastic_cross_section,
		    cm_scattering_distribution,
		    A,
		    kT ),
    d_beta( beta ),
    d_E( E ),
    d_alpha_min( 0.0 ),
    d_alpha_max( 0.0 ),
    d_norm_constant( 1.0 ),
    d_cached_cdf_values()
{
  // Make sure the values are valid
  testPrecondition( A > 0.0 );
  testPrecondition( kT > 0.0 );
  testPrecondition( E > 0.0 );
  testPrecondition( beta > Utility::calculateBetaMin( E, kT ) );

  updateCachedValues();
}

// Set the beta and energy values
void FreeGasElasticMarginalAlphaFunction::setIndependentVariables(
							     const double beta,
							     const double E )
{
  // Make sure beta is valid
  remember( double kT = d_sab_function.getTemperature() );
  testPrecondition( beta > Utility::calculateBetaMin( E, kT ) );

  d_beta = beta;
  d_E = E;

  updateCachedValues();
}

// Get the lower alpha limit
double FreeGasElasticMarginalAlphaFunction::getAlphaMin() const
{
  return d_alpha_min;
}

// Get the upper alpha limit
double FreeGasElasticMarginalAlphaFunction::getAlphaMax() const
{
  return d_alpha_max;
}

// Get the normalization constant
double FreeGasElasticMarginalAlphaFunction::getNormalizationConstant() const
{
  return d_norm_constant;
}

// Evaluate the marginal PDF
double FreeGasElasticMarginalAlphaFunction::operator()( const double alpha )
{
  // Make sure the alpha value is valid
  testPrecondition( alpha >= d_alpha_min );
  testPrecondition( alpha <= d_alpha_max );

  return d_sab_function( alpha, d_beta, d_E )/d_norm_constant;
}

// Evaluate the marginal CDF
double FreeGasElasticMarginalAlphaFunction::evaluateCDF( const double alpha )
{
  // Make sure the alpha value is valid
  testPrecondition( alpha >= d_alpha_min );
  testPrecondition( alpha <= d_alpha_max );

  // Find nearest cached evaluation of cdf
  std::list<Utility::Pair<double,double> >::iterator lower_cdf_point =
    d_cached_cdf_values.begin();

  lower_cdf_point = Utility::Search::binaryLowerBound<Utility::FIRST>(
						     lower_cdf_point,
						     d_cached_cdf_values.end(),
						     alpha );

  // Calculate the cdf value
  double cdf_value, cdf_value_error;

  d_gkq_set.integrateAdaptively<15>( *this,
				    lower_cdf_point->first,
				    alpha,
				    cdf_value,
				    cdf_value_error );

  cdf_value += lower_cdf_point->second;

  // Cache the new cdf value
  Utility::Pair<double,double> new_cdf_point(alpha, cdf_value );

  d_cached_cdf_values.insert( ++lower_cdf_point, new_cdf_point );

  // Return the calculated cdf value
  return cdf_value;
}

// Update cached values
void FreeGasElasticMarginalAlphaFunction::updateCachedValues()
{
  // Calculate the alpha limits
  double A = d_sab_function.getAtomicWeightRatio();
  double kT = d_sab_function.getTemperature();

  d_alpha_min = Utility::calculateAlphaMin( d_E, d_beta, A, kT );

  d_alpha_max = Utility::calculateAlphaMax( d_E, d_beta, A, kT );

  // Calculate the norm constant
  double norm_constant_error;

  boost::function<double (double alpha)> sab_function_wrapper =
    boost::bind<double>( d_sab_function, _1, d_beta, d_E );

  d_gkq_set.integrateAdaptively<15>( sab_function_wrapper,
				    d_alpha_min,
				    d_alpha_max,
				    d_norm_constant,
				    norm_constant_error );

  // Reset the cached cdf values
  d_cached_cdf_values.clear();

  Utility::Pair<double,double> cdf_point( d_alpha_min, 0.0 );
  d_cached_cdf_values.push_back( cdf_point );

  cdf_point( d_alpha_max, 1.0 );

  d_cached_cdf_values.push_back( cdf_point );
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_FreeGasElasticMarginalAlphaFunction.cpp
//---------------------------------------------------------------------------//


#include <assert.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2E.h>
#include <NTL/GF2EX.h>
#include <NTL/GF2EXFactoring.h>

#include "gcm.h"

using namespace std;
using namespace NTL;

int main(int argc, char **argv)
{
    struct slice a1, c1, t1, a2, c2, h;
    struct slice *sp[] = {&a1, &c1, &t1, &a2, &c2, &h};
    int i;
    GF2E x, y, z;
    GF2EX p1, p2;
    char out[2*blocklen + 1];

    argc -= 1;
    argv += 1;

    assert(argc == 6);

    for (i = 0; i < argc; i += 1) {
        hex2bytes(sp[i], argv[i]);
    }

    initfield();

    block2felem(x, &h);
    block2felem(y, &t1);
    buildpoly(p1, &a1, &c1, &t1);
    eval(z, p1, x);

    buildpoly(p2, &a2, &c2, &t1);
    add(p2, p2, y);
    add(p2, p2, z);
    eval(z, p2, x);

    felem2hex(out, z);
    printf("%s\n", out);

    return 0;
}