//
// Created by Dominik Krupke, http://krupke.cc on 10/7/17.
//

#include <gtest/gtest.h>
#include <boost/graph/adjacency_list.hpp>
#include "MatchingSolver.h"

TEST(MatchingSolver, MatchWithIsolatedVertex){
  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  Graph graph{7};
  auto e1 = boost::add_edge(0,3, graph).first;
  boost::add_edge(0,4, graph);
  boost::add_edge(0,5, graph);
  auto e2 = boost::add_edge(1,4, graph).first;
  auto e3 = boost::add_edge(2,5, graph).first;

  bipartvc::details::MatchingSolver<Graph> matchingSolver{graph};
  ASSERT_TRUE(matchingSolver.solve());
  for(unsigned int i=0; i<6; ++i){

    ASSERT_TRUE(matchingSolver.isMatched(i));
  }
  ASSERT_FALSE(matchingSolver.isMatched(6));

  ASSERT_EQ(matchingSolver.getPartner(0), 3);
  ASSERT_EQ(matchingSolver.getPartner(1), 4);
  ASSERT_EQ(matchingSolver.getPartner(2), 5);
  ASSERT_EQ(matchingSolver.getPartner(3), 0);
  ASSERT_EQ(matchingSolver.getPartner(4), 1);
  ASSERT_EQ(matchingSolver.getPartner(5), 2);
  ASSERT_EQ(matchingSolver.getPartner(6), boost::graph_traits<Graph>::null_vertex());
}


// Boost.Geometry Index
//
// boxes union/sum area/volume
//
// Copyright (c) 2008 Federico J. Fernandez.
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_UNION_CONTENT_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_UNION_CONTENT_HPP

#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/index/detail/algorithms/content.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

/**
 * \brief Compute the area of the union of b1 and b2
 */
template <typename Box, typename Geometry>
inline typename default_content_result<Box>::type union_content(Box const& b, Geometry const& g)
{
    Box expanded_box(b);
    geometry::expand(expanded_box, g);
    return detail::content(expanded_box);
}

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_UNION_CONTENT_HPP


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

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2009 StatPro Italia srl
 Copyright (C) 2004 Ferdinando Ametrano

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

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

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

/*! \file array.hpp
    \brief 1-D array used in linear algebra.
*/

#ifndef quantlib_array_hpp
#define quantlib_array_hpp

#include <ql/types.hpp>
#include <ql/errors.hpp>
#include <ql/utilities/disposable.hpp>
#include <ql/utilities/null.hpp>
#include <boost/iterator/reverse_iterator.hpp>
#include <boost/scoped_array.hpp>
#include <boost/type_traits.hpp>
#include <functional>
#include <numeric>
#include <vector>
#include <iomanip>

namespace QuantLib {

    //! 1-D array used in linear algebra.
    /*! This class implements the concept of vector as used in linear
        algebra.
        As such, it is <b>not</b> meant to be used as a container -
        <tt>std::vector</tt> should be used instead.

        \test construction of arrays is checked in a number of cases
    */
    class Array {
      public:
        //! \name Constructors, destructor, and assignment
        //@{
        //! creates the array with the given dimension
        explicit Array(Size size = 0);
        //! creates the array and fills it with <tt>value</tt>
        Array(Size size, Real value);
        /*! \brief creates the array and fills it according to
            \f$ a_{0} = value, a_{i}=a_{i-1}+increment \f$
        */
        Array(Size size, Real value, Real increment);
        Array(const Array&);
        Array(const Disposable<Array>&);
        //! creates the array from an iterable sequence
        template <class ForwardIterator>
        Array(ForwardIterator begin, ForwardIterator end);

        Array& operator=(const Array&);
        Array& operator=(const Disposable<Array>&);
        bool operator==(const Array&) const;
        bool operator!=(const Array&) const;
        //@}
        /*! \name Vector algebra

            <tt>v += x</tt> and similar operation involving a scalar value
            are shortcuts for \f$ \forall i : v_i = v_i + x \f$

            <tt>v *= w</tt> and similar operation involving two vectors are
            shortcuts for \f$ \forall i : v_i = v_i \times w_i \f$

            \pre all arrays involved in an algebraic expression must have
            the same size.
        */
        //@{
        const Array& operator+=(const Array&);
        const Array& operator+=(Real);
        const Array& operator-=(const Array&);
        const Array& operator-=(Real);
        const Array& operator*=(const Array&);
        const Array& operator*=(Real);
        const Array& operator/=(const Array&);
        const Array& operator/=(Real);
        //@}
        //! \name Element access
        //@{
        //! read-only
        Real operator[](Size) const;
        Real at(Size) const;
        Real front() const;
        Real back() const;
        //! read-write
        Real& operator[](Size);
        Real& at(Size);
        Real& front();
        Real& back();
        //@}
        //! \name Inspectors
        //@{
        //! dimension of the array
        Size size() const;
        //! whether the array is empty
        bool empty() const;
        //@}
        typedef Size size_type;
        typedef Real value_type;
        typedef Real* iterator;
        typedef const Real* const_iterator;
        typedef boost::reverse_iterator<iterator> reverse_iterator;
        typedef boost::reverse_iterator<const_iterator> const_reverse_iterator;
        //! \name Iterator access
        //@{
        const_iterator begin() const;
        iterator begin();
        const_iterator end() const;
        iterator end();
        const_reverse_iterator rbegin() const;
        reverse_iterator rbegin();
        const_reverse_iterator rend() const;
        reverse_iterator rend();
        //@}
        //! \name Utilities
        //@{
        void swap(Array&);  // never throws
        //@}

      private:
        boost::scoped_array<Real> data_;
        Size n_;
    };

    //! specialization of null template for this class
    template <>
    class Null<Array> {
      public:
        Null() {}
        operator Array() const { return Array(); }
    };

    /*! \relates Array */
    Real DotProduct(const Array&, const Array&);

    /*! \relates Array */
    Real Norm2(const Array&);

    // unary operators
    /*! \relates Array */
    const Disposable<Array> operator+(const Array& v);
    /*! \relates Array */
    const Disposable<Array> operator-(const Array& v);

    // binary operators
    /*! \relates Array */
    const Disposable<Array> operator+(const Array&, const Array&);
    /*! \relates Array */
    const Disposable<Array> operator+(const Array&, Real);
    /*! \relates Array */
    const Disposable<Array> operator+(Real, const Array&);
    /*! \relates Array */
    const Disposable<Array> operator-(const Array&, const Array&);
    /*! \relates Array */
    const Disposable<Array> operator-(const Array&, Real);
    /*! \relates Array */
    const Disposable<Array> operator-(Real, const Array&);
    /*! \relates Array */
    const Disposable<Array> operator*(const Array&, const Array&);
    /*! \relates Array */
    const Disposable<Array> operator*(const Array&, Real);
    /*! \relates Array */
    const Disposable<Array> operator*(Real, const Array&);
    /*! \relates Array */
    const Disposable<Array> operator/(const Array&, const Array&);
    /*! \relates Array */
    const Disposable<Array> operator/(const Array&, Real);
    /*! \relates Array */
    const Disposable<Array> operator/(Real, const Array&);

    // math functions
    /*! \relates Array */
    const Disposable<Array> Abs(const Array&);
    /*! \relates Array */
    const Disposable<Array> Sqrt(const Array&);
    /*! \relates Array */
    const Disposable<Array> Log(const Array&);
    /*! \relates Array */
    const Disposable<Array> Exp(const Array&);
    /*! \relates Array */
    const Disposable<Array> Pow(const Array&, Real);

    // utilities
    /*! \relates Array */
    void swap(Array&, Array&);

    // format
    /*! \relates Array */
    std::ostream& operator<<(std::ostream&, const Array&);

    // inline definitions

    inline Array::Array(Size size)
    : data_(size ? new Real[size] : (Real*)(0)), n_(size) {}

    inline Array::Array(Size size, Real value)
    : data_(size ? new Real[size] : (Real*)(0)), n_(size) {
        std::fill(begin(),end(),value);
    }

    inline Array::Array(Size size, Real value, Real increment)
    : data_(size ? new Real[size] : (Real*)(0)), n_(size) {
        for (iterator i=begin(); i!=end(); ++i, value+=increment)
            *i = value;
    }

    inline Array::Array(const Array& from)
    : data_(from.n_ ? new Real[from.n_] : (Real*)(0)), n_(from.n_) {
        #if defined(QL_PATCH_MSVC) && defined(QL_DEBUG)
        if (n_)
        #endif
        std::copy(from.begin(),from.end(),begin());
    }

    inline Array::Array(const Disposable<Array>& from)
    : data_((Real*)(0)), n_(0) {
        swap(const_cast<Disposable<Array>&>(from));
    }

    namespace detail {

        template <class I>
        inline void _fill_array_(Array& a,
                                 boost::scoped_array<Real>& data_,
                                 Size& n_,
                                 I begin, I end,
                                 const boost::true_type&) {
            // we got redirected here from a call like Array(3, 4)
            // because it matched the constructor below exactly with
            // ForwardIterator = int.  What we wanted was fill an
            // Array with a given value, which we do here.
            Size n = begin;
            Real value = end;
            data_.reset(n ? new Real[n] : (Real*)(0));
            n_ = n;
            std::fill(a.begin(),a.end(),value);
        }

        template <class I>
        inline void _fill_array_(Array& a,
                                 boost::scoped_array<Real>& data_,
                                 Size& n_,
                                 I begin, I end,
                                 const boost::false_type&) {
            // true iterators
            Size n = std::distance(begin, end);
            data_.reset(n ? new Real[n] : (Real*)(0));
            n_ = n;
            #if defined(QL_PATCH_MSVC) && defined(QL_DEBUG)
            if (n_)
            #endif
            std::copy(begin, end, a.begin());
        }

    }

    template <class ForwardIterator>
    inline Array::Array(ForwardIterator begin, ForwardIterator end) {
        // Unfortunately, calls such as Array(3, 4) match this constructor.
        // We have to detect integral types and dispatch.
        detail::_fill_array_(*this, data_, n_, begin, end,
                             boost::is_integral<ForwardIterator>());
    }

    inline Array& Array::operator=(const Array& from) {
        // strong guarantee
        Array temp(from);
        swap(temp);
        return *this;
    }

    inline bool Array::operator==(const Array& to) const {
        return (n_ == to.n_) && std::equal(begin(), end(), to.begin());
    }

    inline bool Array::operator!=(const Array& to) const {
        return !(this->operator==(to));
    }

    inline Array& Array::operator=(const Disposable<Array>& from) {
        swap(const_cast<Disposable<Array>&>(from));
        return *this;
    }

    inline const Array& Array::operator+=(const Array& v) {
        QL_REQUIRE(n_ == v.n_,
                   "arrays with different sizes (" << n_ << ", "
                   << v.n_ << ") cannot be added");
        std::transform(begin(),end(),v.begin(),begin(),
                       std::plus<Real>());
        return *this;
    }

    inline const Array& Array::operator+=(Real x) {
        std::transform(begin(),end(),begin(),
                       std::bind2nd(std::plus<Real>(),x));
        return *this;
    }

    inline const Array& Array::operator-=(const Array& v) {
        QL_REQUIRE(n_ == v.n_,
                   "arrays with different sizes (" << n_ << ", "
                   << v.n_ << ") cannot be subtracted");
        std::transform(begin(),end(),v.begin(),begin(),
                       std::minus<Real>());
        return *this;
    }

    inline const Array& Array::operator-=(Real x) {
        std::transform(begin(),end(),begin(),
                       std::bind2nd(std::minus<Real>(),x));
        return *this;
    }

    inline const Array& Array::operator*=(const Array& v) {
        QL_REQUIRE(n_ == v.n_,
                   "arrays with different sizes (" << n_ << ", "
                   << v.n_ << ") cannot be multiplied");
        std::transform(begin(),end(),v.begin(),begin(),
                       std::multiplies<Real>());
        return *this;
    }

    inline const Array& Array::operator*=(Real x) {
        std::transform(begin(),end(),begin(),
                       std::bind2nd(std::multiplies<Real>(),x));
        return *this;
    }

    inline const Array& Array::operator/=(const Array& v) {
        QL_REQUIRE(n_ == v.n_,
                   "arrays with different sizes (" << n_ << ", "
                   << v.n_ << ") cannot be divided");
        std::transform(begin(),end(),v.begin(),begin(),
                       std::divides<Real>());
        return *this;
    }

    inline const Array& Array::operator/=(Real x) {
        std::transform(begin(),end(),begin(),
                       std::bind2nd(std::divides<Real>(),x));
        return *this;
    }

    inline Real Array::operator[](Size i) const {
        #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(i<n_,
                   "index (" << i << ") must be less than " << n_ <<
                   ": array access out of range");
        #endif
        return data_.get()[i];
    }

    inline Real Array::at(Size i) const {
        QL_REQUIRE(i<n_,
                   "index (" << i << ") must be less than " << n_ <<
                   ": array access out of range");
        return data_.get()[i];
    }

    inline Real Array::front() const {
        #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(n_>0, "null Array: array access out of range");
        #endif
        return data_.get()[0];
    }

    inline Real Array::back() const {
        #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(n_>0, "null Array: array access out of range");
        #endif
        return data_.get()[n_-1];
    }

    inline Real& Array::operator[](Size i) {
        #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(i<n_,
                   "index (" << i << ") must be less than " << n_ <<
                   ": array access out of range");
        #endif
        return data_.get()[i];
    }

    inline Real& Array::at(Size i) {
        QL_REQUIRE(i<n_,
                   "index (" << i << ") must be less than " << n_ <<
                   ": array access out of range");
        return data_.get()[i];
    }

    inline Real& Array::front() {
        #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(n_>0, "null Array: array access out of range");
        #endif
        return data_.get()[0];
    }

    inline Real& Array::back() {
        #if defined(QL_EXTRA_SAFETY_CHECKS)
        QL_REQUIRE(n_>0, "null Array: array access out of range");
        #endif
        return data_.get()[n_-1];
    }

    inline Size Array::size() const {
        return n_;
    }

    inline bool Array::empty() const {
        return n_ == 0;
    }

    inline Array::const_iterator Array::begin() const {
        return data_.get();
    }

    inline Array::iterator Array::begin() {
        return data_.get();
    }

    inline Array::const_iterator Array::end() const {
        return data_.get()+n_;
    }

    inline Array::iterator Array::end() {
        return data_.get()+n_;
    }

    inline Array::const_reverse_iterator Array::rbegin() const {
        return const_reverse_iterator(end());
    }

    inline Array::reverse_iterator Array::rbegin() {
        return reverse_iterator(end());
    }

    inline Array::const_reverse_iterator Array::rend() const {
        return const_reverse_iterator(begin());
    }

    inline Array::reverse_iterator Array::rend() {
        return reverse_iterator(begin());
    }

    inline void Array::swap(Array& from) {
        using std::swap;
        data_.swap(from.data_);
        swap(n_,from.n_);
    }

    // dot product and norm

    inline Real DotProduct(const Array& v1, const Array& v2) {
        QL_REQUIRE(v1.size() == v2.size(),
                   "arrays with different sizes (" << v1.size() << ", "
                   << v2.size() << ") cannot be multiplied");
        return std::inner_product(v1.begin(),v1.end(),v2.begin(),0.0);
    }

    inline Real Norm2(const Array& v) {
        return std::sqrt(DotProduct(v, v));
    }

    // overloaded operators

    // unary

    inline const Disposable<Array> operator+(const Array& v) {
        Array result = v;
        return result;
    }

    inline const Disposable<Array> operator-(const Array& v) {
        Array result(v.size());
        std::transform(v.begin(),v.end(),result.begin(),
                       std::negate<Real>());
        return result;
    }

    // binary operators

    inline const Disposable<Array> operator+(const Array& v1,
                                             const Array& v2) {
        QL_REQUIRE(v1.size() == v2.size(),
                   "arrays with different sizes (" << v1.size() << ", "
                   << v2.size() << ") cannot be added");
        Array result(v1.size());
        std::transform(v1.begin(),v1.end(),v2.begin(),result.begin(),
                       std::plus<Real>());
        return result;
    }

    inline const Disposable<Array> operator+(const Array& v1, Real a) {
        Array result(v1.size());
        std::transform(v1.begin(),v1.end(),result.begin(),
                       std::bind2nd(std::plus<Real>(),a));
        return result;
    }

    inline const Disposable<Array> operator+(Real a, const Array& v2) {
        Array result(v2.size());
        std::transform(v2.begin(),v2.end(),result.begin(),
                       std::bind1st(std::plus<Real>(),a));
        return result;
    }

    inline const Disposable<Array> operator-(const Array& v1,
                                             const Array& v2) {
        QL_REQUIRE(v1.size() == v2.size(),
                   "arrays with different sizes (" << v1.size() << ", "
                   << v2.size() << ") cannot be subtracted");
        Array result(v1.size());
        std::transform(v1.begin(),v1.end(),v2.begin(),result.begin(),
                       std::minus<Real>());
        return result;
    }

    inline const Disposable<Array> operator-(const Array& v1, Real a) {
        Array result(v1.size());
        std::transform(v1.begin(),v1.end(),result.begin(),
                       std::bind2nd(std::minus<Real>(),a));
        return result;
    }

    inline const Disposable<Array> operator-(Real a, const Array& v2) {
        Array result(v2.size());
        std::transform(v2.begin(),v2.end(),result.begin(),
                       std::bind1st(std::minus<Real>(),a));
        return result;
    }

    inline const Disposable<Array> operator*(const Array& v1,
                                             const Array& v2) {
        QL_REQUIRE(v1.size() == v2.size(),
                   "arrays with different sizes (" << v1.size() << ", "
                   << v2.size() << ") cannot be multiplied");
        Array result(v1.size());
        std::transform(v1.begin(),v1.end(),v2.begin(),result.begin(),
                       std::multiplies<Real>());
        return result;
    }

    inline const Disposable<Array> operator*(const Array& v1, Real a) {
        Array result(v1.size());
        std::transform(v1.begin(),v1.end(),result.begin(),
                       std::bind2nd(std::multiplies<Real>(),a));
        return result;
    }

    inline const Disposable<Array> operator*(Real a, const Array& v2) {
        Array result(v2.size());
        std::transform(v2.begin(),v2.end(),result.begin(),
                       std::bind1st(std::multiplies<Real>(),a));
        return result;
    }

    inline const Disposable<Array> operator/(const Array& v1,
                                             const Array& v2) {
        QL_REQUIRE(v1.size() == v2.size(),
                   "arrays with different sizes (" << v1.size() << ", "
                   << v2.size() << ") cannot be divided");
        Array result(v1.size());
        std::transform(v1.begin(),v1.end(),v2.begin(),result.begin(),
                       std::divides<Real>());
        return result;
    }

    inline const Disposable<Array> operator/(const Array& v1, Real a) {
        Array result(v1.size());
        std::transform(v1.begin(),v1.end(),result.begin(),
                       std::bind2nd(std::divides<Real>(),a));
        return result;
    }

    inline const Disposable<Array> operator/(Real a, const Array& v2) {
        Array result(v2.size());
        std::transform(v2.begin(),v2.end(),result.begin(),
                       std::bind1st(std::divides<Real>(),a));
        return result;
    }

    // functions

    inline const Disposable<Array> Abs(const Array& v) {
        Array result(v.size());
        std::transform(v.begin(),v.end(),result.begin(),
                       std::ptr_fun<Real,Real>(std::fabs));
        return result;
    }

    inline const Disposable<Array> Sqrt(const Array& v) {
        Array result(v.size());
        std::transform(v.begin(),v.end(),result.begin(),
                       std::ptr_fun<Real,Real>(std::sqrt));
        return result;
    }

    inline const Disposable<Array> Log(const Array& v) {
        Array result(v.size());
        std::transform(v.begin(),v.end(),result.begin(),
                       std::ptr_fun<Real,Real>(std::log));
        return result;
    }

    inline const Disposable<Array> Exp(const Array& v) {
        Array result(v.size());
        std::transform(v.begin(),v.end(),result.begin(),
                       std::ptr_fun<Real,Real>(std::exp));
        return result;
    }

    inline const Disposable<Array> Pow(const Array& v, Real alpha) {
        Array result(v.size());
        std::transform(v.begin(), v.end(), result.begin(),
            std::bind2nd(std::ptr_fun<Real, Real, Real>(std::pow), alpha));

        return result;
    }

    inline void swap(Array& v, Array& w) {
        v.swap(w);
    }

    inline std::ostream& operator<<(std::ostream& out, const Array& a) {
        std::streamsize width = out.width();
        out << "[ ";
        if (!a.empty()) {
            for (Size n=0; n<a.size()-1; ++n)
                out << std::setw(int(width)) << a[n] << "; ";
            out << std::setw(int(width)) << a.back();
        }
        out << " ]";
        return out;
    }

}

#endif


#include <boost/test/unit_test.hpp>

#include <boost/format.hpp>

#include <set>
#include <iostream>

// using namespace std;
using namespace crave;

using boost::format;

BOOST_FIXTURE_TEST_SUITE(Context_t, Context_Fixture)

struct s_multiple_solver_instances : public crv_sequence_item {
  s_multiple_solver_instances(crv_object_name) {}
  crv_variable<int> r;
  crv_constraint con = {r() < 6};
};

BOOST_AUTO_TEST_CASE(multiple_solver_instances) {
  s_multiple_solver_instances r1("r1");
  s_multiple_solver_instances r2("r2");
  BOOST_REQUIRE(r1.randomize());
  BOOST_REQUIRE(r2.randomize());
}

struct s_constants : public crv_sequence_item {
  s_constants(crv_object_name) {}
  crv_variable<unsigned> x;
  crv_constraint con = {x() == 135421};
};

BOOST_AUTO_TEST_CASE(constants) {
  s_constants item("item");
  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.x, 135421);
}

struct s_boolean : public crv_sequence_item {
  s_boolean(crv_object_name) {}
  crv_variable<bool> b;
  crv_constraint con = {b() == b()};
};

BOOST_AUTO_TEST_CASE(boolean) {
  s_boolean item("item");
  BOOST_REQUIRE(item.randomize());
  bool b1 = item.b;
  item.con &= {item.b() != b1};
  BOOST_REQUIRE(item.randomize());
  bool b2 = item.b;
  item.con &= {item.b() != b2};
  BOOST_REQUIRE(!item.randomize());
}

struct s_by_reference : public crv_sequence_item {
  s_by_reference(crv_object_name) {}
  crv_variable<unsigned> a;
  crv_constraint con;
};

BOOST_AUTO_TEST_CASE(by_reference) {
  s_by_reference item("item");
  unsigned b = 0;
  item.con &= {item.a() == reference(b)};

  while (item.randomize()) {
    unsigned av = item.a;
    BOOST_REQUIRE_EQUAL(av, b);
    ++b;
    if (b > 10) break;
  }

  BOOST_REQUIRE_GT(b, 10);
}

struct s_named_reference : public crv_sequence_item {
  s_named_reference(crv_object_name) {}
  crv_variable<unsigned> a, c;
  crv_constraint con;
};

// temporaly fix a variable to a certain value using the assign operator
BOOST_AUTO_TEST_CASE(named_reference) {
  unsigned bv = 0;
  s_named_reference item("item");
  ReadReference<unsigned> b(bv);
  item.con &= {item.a() == b};
  item.con &= {item.c() != b};

  while (item.randomize()) {
    unsigned av = item.a;
    unsigned cv = item.c;
    BOOST_REQUIRE_EQUAL(av, bv);
    BOOST_REQUIRE_NE(cv, bv);
    ++bv;
    if (bv > 10) break;
  }

  BOOST_REQUIRE_GT(bv, 10);
}

struct s_soft_constraint_t : crv_sequence_item {
  s_soft_constraint_t(crv_object_name) {}
  crv_variable<int> r;
  crv_constraint con = {r() < 6};
  crv_soft_constraint con2;
};

BOOST_AUTO_TEST_CASE(soft_constraint_t) {
  s_soft_constraint_t item("item");
  item.con2 &= {(item.r() == 2)};

  BOOST_REQUIRE(item.randomize());
  BOOST_REQUIRE_EQUAL(item.r, 2);
  item.con2 &= {(item.r() == 3)};

  BOOST_REQUIRE(item.randomize());
}

struct randv_test_s : public crv_sequence_item {
  randv_test_s(crv_object_name) {}
  crv_variable<unsigned int> a;
  crv_variable<unsigned int> b;
  crv_constraint con;
};

BOOST_AUTO_TEST_CASE(randv_test) {
  randv_test_s item("item");
  std::cout << "init: a = " << item.a << ", b = " << item.b << std::endl;
  item.con &=
      {4 <= item.a() && item.a() <= 6, 9 <= item.a() + item.b() && item.a() + item.b() <= 11, item.b() % 2 == 0};
  int count = 0;
  while (item.randomize()) {
    ++count;
    std::cout << "result: a = " << item.a << ", b = " << item.b << std::endl;
    item.con &= {(item.a() != item.a || item.b() != item.b)};
    BOOST_REQUIRE_LE(count, 10);
  }

  BOOST_REQUIRE_EQUAL(count, 4);
}

struct s_randv_var_ref_mixed_test : public crv_sequence_item {
  crv_variable<int> a;
  int b_value;
  WriteReference<int> b{&b_value};
  crv_constraint con = {4 <= a() && a() <= 6, 9 <= a() + b && a() + b <= 11, b % 2 == 0};
  s_randv_var_ref_mixed_test(crv_object_name) {}
};

BOOST_AUTO_TEST_CASE(randv_var_ref_mixed_test) {
  s_randv_var_ref_mixed_test item("item");
  unsigned count = 0;
  while (item.randomize()) {
    ++count;
    std::cout << "result: a = " << item.a << ", b = " << item.b_value;
    std::cout << std::endl;
    item.con &= {(item.a() != item.a || item.b != item.b_value)};
    BOOST_REQUIRE_LE(count, 10);
  }
  BOOST_REQUIRE_EQUAL(count, 4);
}

struct s_alu : public crv_sequence_item {
  s_alu(crv_object_name) {}
  crv_variable<unsigned> op;
  crv_variable<unsigned> a;
  crv_variable<unsigned> b;
  crv_constraint con = {
      a() < 16,                      b() < 16,
      op() < 4,                       // 4 opcodes
      (op() != 0 || a() + b() < 16),  // no add overflow
      (op() != 1 || a() > b()),       // no sub underflow
      (op() != 2 || a() * b() < 16),  // no m overflow
      (op() != 3 || b() != 0)         // div valid
  };
};
BOOST_AUTO_TEST_CASE(alu) {
  s_alu item("item");
  BOOST_REQUIRE(item.randomize());
  std::cout << "result: op=" << item.op << ", a=" << item.a << ", b=" << item.b << std::endl;
}

struct s_alu_enum : public crv_sequence_item {
  s_alu_enum(crv_object_name) {}
  crv_variable<unsigned> op, a, b;
  crv_constraint con = {(a() < 8u),                     (b() < 8u),
                        (op() < 4u),                     // 4 opcodes
                        (op() != 0u || a() + b() < 8u),  // no add overflow
                        (op() != 1u || a() > b()),       // no sub underflow
                        (op() != 2u || a() * b() < 8u),  // no m overflow
                        (op() != 3u || b() != 0u)};
};

BOOST_AUTO_TEST_CASE(alu_enum) {
  VariableDefaultSolver::bypass_constraint_analysis = true;
  s_alu_enum item("item");

  int count = 0;
  for (int a = 0; a < 8; a++)
    for (int b = 0; b < 8; b++) {
      if (a + b < 8) count++;  // op == 0
      if (a > b) count++;      // op == 1
      if (a * b < 8) count++;  // op == 2
      if (b != 0) count++;     // op == 3;
    }

  while (item.randomize()) {
    --count;
    item.con &= {item.op() != item.op || item.a() != item.a || item.b() != item.b};
    BOOST_REQUIRE_GE(count, 0);
  }
  BOOST_REQUIRE_EQUAL(count, 0);

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

struct s_pythagoras : public crv_sequence_item {
  s_pythagoras(crv_object_name) {}
  crv_variable<unsigned long long> a, b, c;
  crv_constraint con = {a() * a() + b() * b() == c() * c()};
};

BOOST_AUTO_TEST_CASE(pythagoras) {
  s_pythagoras item("pythagoras");
  BOOST_REQUIRE(item.randomize());
  unsigned long long av = item.a;
  unsigned long long bv = item.b;
  unsigned long long cv = item.c;

  BOOST_REQUIRE_EQUAL(av * av + bv * bv, cv * cv);
}

struct s_negative_var : public crv_sequence_item {
  s_negative_var(crv_object_name) {}
  crv_variable<int> a, b;
  crv_constraint con = {a() + b() <= 120, a() > 120};
};

BOOST_AUTO_TEST_CASE(negative_var) {
  s_negative_var item("item");
  BOOST_REQUIRE(item.randomize());

  std::cout << format("result: a=%d, b=%d\n") % item.a % item.b;

  BOOST_REQUIRE(item.a + item.b <= 120);
}

struct s_signed_less_zero : public crv_sequence_item {
  s_signed_less_zero(crv_object_name) {}
  crv_variable<int> a;
  crv_constraint con = {a() < 0};
};
BOOST_AUTO_TEST_CASE(signed_less_zero) {
  s_signed_less_zero item("item");
  BOOST_REQUIRE(item.randomize());

  std::cout << format("result: a=%d\n") % item.a;

  BOOST_REQUIRE(item.a < 0);
}

struct s_mixed_bv_width_1 : crv_sequence_item {
  s_mixed_bv_width_1(crv_object_name) {}
  crv_variable<unsigned long> a;
  crv_variable<unsigned short> b;
  crv_constraint con = {a() + b() >= 120};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_1) {
  s_mixed_bv_width_1 item("item");
  BOOST_REQUIRE(item.randomize());

  std::cout << format("result: a=%d, b=%d\n") % item.a % item.b;

  BOOST_REQUIRE(item.a + item.b >= 120);
}

struct s_mixed_bv_width_2 : crv_sequence_item {
  s_mixed_bv_width_2(crv_object_name) {}
  crv_variable<signed char> a;
  crv_constraint con = {a() < 10};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_2) {
  VariableDefaultSolver::bypass_constraint_analysis = true;
  s_mixed_bv_width_2 item("item");

  std::set<signed char> generated;
  for (int iterations = 0; item.randomize(); ++iterations) {
    generated.insert(item.a);
    item.con &= {item.a() != item.a};

    BOOST_REQUIRE_LT(iterations, 150);
  }

  BOOST_REQUIRE_EQUAL(generated.size(), 138);

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

struct s_mixed_bv_width_3 : crv_sequence_item {
  s_mixed_bv_width_3(crv_object_name) {}
  crv_variable<short> a;
  crv_constraint con = {a()<10, a()> - 10};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_3) {
  s_mixed_bv_width_3 item("item");
  std::set<short> generated;
  for (unsigned iterations = 0; item.randomize(); ++iterations) {
    generated.insert(item.a);
    item.con &= {(item.a() != item.a)};

    BOOST_REQUIRE_LT(iterations, 20);
  }

  BOOST_REQUIRE_EQUAL(generated.size(), 19);
}

struct s_mixed_bv_width_4 : crv_sequence_item {
  s_mixed_bv_width_4(crv_object_name) {}
  crv_variable<int> a;
  crv_constraint con = {a()<(signed char)10, a()>(short)-10};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_4) {
  s_mixed_bv_width_4 item("item");
  std::set<int> generated;
  for (unsigned iterations = 0; item.randomize(); ++iterations) {
    generated.insert(item.a);
    item.con &= {item.a() != item.a};
    BOOST_REQUIRE_LT(iterations, 20);
  }

  BOOST_REQUIRE_EQUAL(generated.size(), 19);
}

struct s_mixed_bv_width_5 : crv_sequence_item {
  s_mixed_bv_width_5(crv_object_name) {}
  crv_variable<short> a;
  crv_variable<signed char> b;
  crv_constraint con = {-3 <= a() && a() <= 3, -3 <= b() && b() <= 3, (-2 <= a() + b()) && (a() + b() <= 2)};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_5) {
  s_mixed_bv_width_5 item("item");
  int cnt = 0;
  while (item.randomize()) {
    cnt++;
    item.con &= {(item.a() != item.a) || (item.b() != item.b)};
    BOOST_REQUIRE_LT(cnt, 300);
  }

  int cnt1 = 0;
  for (short i = -3; i <= 3; i++)
    for (signed char j = -3; j <= 3; j++)
      if ((-2 <= i + j) && (i + j <= 2)) cnt1++;

  BOOST_REQUIRE_EQUAL(cnt, cnt1);
}

struct s_mixed_bv_width_6 : public crv_sequence_item {
  s_mixed_bv_width_6(crv_object_name) {}
  crv_variable<short> a;
  crv_variable<signed char> b;
  crv_constraint con = {-3 <= a() && a() <= 3, -3 <= b() && b() <= 3, a() * b() % 6 == 0};
};

BOOST_AUTO_TEST_CASE(mixed_bv_width_6) {
  s_mixed_bv_width_6 item("item");
  int cnt = 0;
  for (int i = -3; i <= 3; i++)
    for (int j = -3; j <= 3; j++)
      if (i * j % 6 == 0) cnt++;

  int cnt1 = 0;
  while (item.randomize()) {
    cnt1++;
    BOOST_REQUIRE_EQUAL(item.a * item.b % 6, 0);
    item.con &= {item.a() != item.a || item.b() != item.b};
  }

  BOOST_REQUIRE_EQUAL(cnt, cnt1);
}

BOOST_AUTO_TEST_SUITE_END()  // Context

//  vim: ft=cpp:ts=2:sw=2:expandtab


#include "drake/systems/analysis/test_utilities/spring_mass_system.h"

#include <memory>

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

#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/systems/framework/fixed_input_port_value.h"
#include "drake/systems/framework/state.h"
#include "drake/systems/framework/subvector.h"
#include "drake/systems/framework/system.h"
#include "drake/systems/framework/system_output.h"
#include "drake/systems/framework/test_utilities/scalar_conversion.h"
#include "drake/systems/framework/vector_base.h"

using std::make_unique;
using std::unique_ptr;

namespace drake {
namespace systems {
namespace {

const double kSpring = 300.0;  // N/m
const double kMass = 2.0;      // kg

class SpringMassSystemTest : public ::testing::Test {
 public:
  void SetUp() override { Initialize(); }

  void Initialize(bool with_input_force = false) {
    // Construct the system I/O objects.
    system_ = std::make_unique<SpringMassSystem<double>>(kSpring, kMass,
                                                         with_input_force);
    system_->set_name("test_system");
    context_ = system_->CreateDefaultContext();
    system_output_ = system_->AllocateOutput();
    system_derivatives_ = system_->AllocateTimeDerivatives();
    const int nq = system_derivatives_->get_generalized_position().size();
    configuration_derivatives_ = std::make_unique<BasicVector<double>>(nq);

    // Set up some convenience pointers.
    state_ = dynamic_cast<SpringMassStateVector<double>*>(
        &context_->get_mutable_continuous_state_vector());
    output_ = dynamic_cast<const SpringMassStateVector<double>*>(
        system_output_->get_vector_data(0));
    derivatives_ = dynamic_cast<SpringMassStateVector<double>*>(
        &system_derivatives_->get_mutable_vector());
  }

  void InitializeState(const double position, const double velocity) {
    state_->set_position(position);
    state_->set_velocity(velocity);
    state_->set_conservative_work(0);
  }

 protected:
  std::unique_ptr<SpringMassSystem<double>> system_;
  std::unique_ptr<Context<double>> context_;
  std::unique_ptr<SystemOutput<double>> system_output_;
  std::unique_ptr<ContinuousState<double>> system_derivatives_;
  std::unique_ptr<BasicVector<double>> configuration_derivatives_;

  SpringMassStateVector<double>* state_;
  const SpringMassStateVector<double>* output_;
  SpringMassStateVector<double>* derivatives_;

 private:
  std::unique_ptr<VectorBase<double>> erased_derivatives_;
};

TEST_F(SpringMassSystemTest, Construction) {
  // Asserts zero inputs for this case.
  EXPECT_EQ(0, context_->num_input_ports());
  EXPECT_EQ("test_system", system_->get_name());
  EXPECT_EQ(kSpring, system_->get_spring_constant());
  EXPECT_EQ(kMass, system_->get_mass());
  EXPECT_FALSE(system_->get_system_is_forced());
}

TEST_F(SpringMassSystemTest, DirectFeedthrough) {
  EXPECT_FALSE(system_->HasAnyDirectFeedthrough());
}

TEST_F(SpringMassSystemTest, CloneState) {
  InitializeState(1.0, 2.0);
  state_->set_conservative_work(3.0);
  std::unique_ptr<BasicVector<double>> clone = state_->Clone();
  SpringMassStateVector<double>* typed_clone =
      dynamic_cast<SpringMassStateVector<double>*>(clone.get());
  ASSERT_NE(nullptr, typed_clone);
  EXPECT_EQ(1.0, typed_clone->get_position());
  EXPECT_EQ(2.0, typed_clone->get_velocity());
  EXPECT_EQ(3.0, typed_clone->get_conservative_work());
}

TEST_F(SpringMassSystemTest, CloneOutput) {
  InitializeState(1.0, 2.0);
  system_->CalcOutput(*context_, system_output_.get());
  std::unique_ptr<BasicVector<double>> clone = output_->Clone();

  SpringMassStateVector<double>* typed_clone =
      dynamic_cast<SpringMassStateVector<double>*>(clone.get());
  ASSERT_NE(nullptr, typed_clone);
  EXPECT_EQ(1.0, typed_clone->get_position());
  EXPECT_EQ(2.0, typed_clone->get_velocity());
}

// Tests that state is passed through to the output.
TEST_F(SpringMassSystemTest, Output) {
  // Displacement 100cm, vel 250cm/s (.25 is exact in binary).
  InitializeState(0.1, 0.25);
  system_->CalcOutput(*context_, system_output_.get());
  ASSERT_EQ(1, system_output_->num_ports());

  // Check the output through the application-specific interface.
  EXPECT_NEAR(0.1, output_->get_position(), 1e-14);
  EXPECT_EQ(0.25, output_->get_velocity());

  // Check the output through the VectorBase API.
  ASSERT_EQ(3, output_->size());
  EXPECT_NEAR(0.1, output_->get_value()[0], 1e-14);
  EXPECT_EQ(0.25, output_->get_value()[1]);
}

// Tests that second-order structure is exposed in the state.
TEST_F(SpringMassSystemTest, SecondOrderStructure) {
  InitializeState(1.2, 3.4);  // Displacement 1.2m, velocity 3.4m/sec.
  ContinuousState<double>& continuous_state =
      context_->get_mutable_continuous_state();
  ASSERT_EQ(1, continuous_state.get_generalized_position().size());
  ASSERT_EQ(1, continuous_state.get_generalized_velocity().size());
  ASSERT_EQ(1, continuous_state.get_misc_continuous_state().size());
  EXPECT_NEAR(1.2, continuous_state.get_generalized_position().GetAtIndex(0),
              1e-8);
  EXPECT_NEAR(3.4, continuous_state.get_generalized_velocity().GetAtIndex(0),
              1e-8);
}

// Tests that second-order structure can be processed in
// MapVelocityToConfigurationDerivative.
TEST_F(SpringMassSystemTest, MapVelocityToConfigurationDerivative) {
  InitializeState(1.2, 3.4);  // Displacement 1.2m, velocity 3.4m/sec.
  ContinuousState<double>& continuous_state =
      context_->get_mutable_continuous_state();

  // Slice just the configuration derivatives out of the derivative
  // vector.
  Subvector<double> configuration_derivatives(derivatives_, 0, 1);

  system_->MapVelocityToQDot(
      *context_, continuous_state.get_generalized_velocity(),
      &configuration_derivatives);

  EXPECT_NEAR(3.4, derivatives_->get_position(), 1e-8);
  EXPECT_NEAR(3.4, configuration_derivatives.GetAtIndex(0), 1e-8);
}

TEST_F(SpringMassSystemTest, ForcesPositiveDisplacement) {
  InitializeState(0.1, 0.1);  // Displacement 0.1m, velocity 0.1m/sec.
  system_->CalcTimeDerivatives(*context_, system_derivatives_.get());

  ASSERT_EQ(3, derivatives_->size());
  // The derivative of position is velocity.
  EXPECT_NEAR(0.1, derivatives_->get_position(), 1e-8);
  // The derivative of velocity is force over mass.
  EXPECT_NEAR(-kSpring * 0.1 / kMass, derivatives_->get_velocity(), 1e-8);
}

TEST_F(SpringMassSystemTest, ForcesNegativeDisplacement) {
  InitializeState(-0.1, 0.2);  // Displacement -0.1m, velocity 0.2m/sec.
  system_->CalcTimeDerivatives(*context_, system_derivatives_.get());

  ASSERT_EQ(3, derivatives_->size());
  // The derivative of position is velocity.
  EXPECT_NEAR(0.2, derivatives_->get_position(), 1e-8);
  // The derivative of velocity is force over mass.
  EXPECT_NEAR(-kSpring * -0.1 / kMass, derivatives_->get_velocity(), 1e-8);
}

TEST_F(SpringMassSystemTest, DynamicsWithExternalForce) {
  // Initializes a spring mass system with an input port for an external force.
  Initialize(true);
  EXPECT_TRUE(system_->get_system_is_forced());
  EXPECT_FALSE(system_->HasAnyDirectFeedthrough());

  // Asserts exactly one input for this case expecting an external force.
  ASSERT_EQ(1, context_->num_input_ports());

  // Sets the input force.
  const double kExternalForce = 1.0;

  // Creates a free standing input port not actually connected to the output of
  // another system but that has its own data.
  // This is done in order to be able to test this system standalone.
  system_->get_input_port(0).FixValue(context_.get(), kExternalForce);

  InitializeState(0.1, 0.1);  // Displacement 0.1m, velocity 0.1m/sec.
  system_->CalcTimeDerivatives(*context_, system_derivatives_.get());

  ASSERT_EQ(3, derivatives_->size());
  // The derivative of position is velocity.
  EXPECT_NEAR(0.1, derivatives_->get_position(),
              Eigen::NumTraits<double>::epsilon());
  // The derivative of velocity is force over mass.
  EXPECT_NEAR((-kSpring * 0.1 + kExternalForce) / kMass,
              derivatives_->get_velocity(),
              Eigen::NumTraits<double>::epsilon());
}

TEST_F(SpringMassSystemTest, ForceEnergyAndPower) {
  InitializeState(1, 2);
  const double m = system_->get_mass();
  const double k = system_->get_spring_constant();
  const double q0 = 0;  // TODO(david-german-tri) should be a parameter.
  const double q = system_->get_position(*context_);
  const double v = system_->get_velocity(*context_);
  const double w_c = system_->get_conservative_work(*context_);
  const double f = system_->EvalSpringForce(*context_);
  const double pe = system_->CalcPotentialEnergy(*context_);
  const double ke = system_->CalcKineticEnergy(*context_);
  const double power_c = system_->CalcConservativePower(*context_);
  const double power_nc = system_->CalcNonConservativePower(*context_);

  EXPECT_EQ(q, 1.0);
  EXPECT_EQ(v, 2.0);
  EXPECT_EQ(w_c, 0.0);
  EXPECT_NEAR(f, -k * (q - q0), 1e-14);
  EXPECT_NEAR(pe, k * (q - q0) * (q - q0) / 2, 1e-14);
  EXPECT_NEAR(ke, m * v * v / 2, 1e-14);
  EXPECT_NEAR(power_c, f * v, 1e-14);
  EXPECT_EQ(power_nc, 0.0);
}

// These are helper functions for the Integrate test below.

/* Given a System and a Context, calculate the partial derivative matrix
D xdot / D x. Here x has only continuous variables; in general we would have
x=[xc,xd] in which case we'd be working with xc here.

TODO(sherm1) This matrix should be calculated by templatizing the spring-mass
System by AutoDiffScalar and by std::complex (for complex step derivatives).
The numerical routine here should then be used in a separate test to make sure
the autodifferentiated matrices agree with the numerical one. Also, consider
switching to central differences here to get more decimal places. */
MatrixX<double> CalcDxdotDx(const System<double>& system,
                            const Context<double>& context) {
  const double perturb = 1e-7;  // roughly sqrt(precision)
  auto derivs0 = system.AllocateTimeDerivatives();
  system.CalcTimeDerivatives(context, derivs0.get());
  const int nx = derivs0->size();
  const VectorX<double> xdot0 = derivs0->CopyToVector();
  MatrixX<double> d_xdot_dx(nx, nx);

  auto temp_context = context.Clone();
  auto& x = temp_context->get_mutable_continuous_state_vector();

  // This is a temp that holds one column of the result as a ContinuousState.
  auto derivs = system.AllocateTimeDerivatives();

  for (int i = 0; i < x.size(); ++i) {
    const double xi = x.GetAtIndex(i);
    x.SetAtIndex(i, xi + perturb);
    system.CalcTimeDerivatives(*temp_context, derivs.get());
    d_xdot_dx.col(i) = (derivs->CopyToVector() - xdot0) / perturb;
    x.SetAtIndex(i, xi);  // repair Context
  }

  return d_xdot_dx;
}

/* Explicit Euler (unstable): x1 = x0 + h xdot(t0,x0) */
void StepExplicitEuler(
    double h, const ContinuousState<double>& derivs,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    Context<double>& context) {
  const double t = context.get_time();
  // Invalidate all xc-dependent quantities.
  VectorBase<double>& xc =
      context.get_mutable_continuous_state_vector();
  const auto& dxc = derivs.get_vector();
  xc.PlusEqScaled(h, dxc);  // xc += h*dxc
  context.SetTime(t + h);
}

/* Semi-explicit Euler (neutrally stable):
    z1 = z0 + h zdot(t0,x0)
    v1 = v0 + h vdot(t0,x0)
    q1 = q0 + h qdot(q0,v1) */
void StepSemiExplicitEuler(
    double h, const System<double>& system,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    ContinuousState<double>& derivs,  // in/out
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    Context<double>& context) {
  const double t = context.get_time();
  ContinuousState<double>& xc = context.get_mutable_continuous_state();

  // Invalidate z-dependent quantities.
  VectorBase<double>& xz = xc.get_mutable_misc_continuous_state();
  const auto& dxz = derivs.get_misc_continuous_state();
  xz.PlusEqScaled(h, dxz);  // xz += h*dxz

  // Invalidate v-dependent quantities.
  VectorBase<double>& xv = xc.get_mutable_generalized_velocity();
  const auto& dxv = derivs.get_generalized_velocity();
  xv.PlusEqScaled(h, dxv);  // xv += h*dxv

  context.SetTime(t + h);

  // Invalidate q-dependent quantities.
  VectorBase<double>& xq = xc.get_mutable_generalized_position();
  auto& dxq = derivs.get_mutable_generalized_position();
  system.MapVelocityToQDot(context, xv, &dxq);  // qdot = N(q)*v
  xq.PlusEqScaled(h, dxq);                       // xq += h*qdot
}

/* Implicit Euler (unconditionally stable): x1 = x0 + h xdot(t1,x1)
    Define err(x) = x - (x0 + h xdot(t1,x))
          J(x) = D err / D x = 1 - h D xdot / D x
    Guess: x1 = x0 + h xdot(t0,x0)
    do: Solve J(x1) dx = err(x1)
        x1 = x1 - dx
    while (norm(dx)/norm(x0) > tol) */
void StepImplicitEuler(
    double h, const System<double>& system,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    ContinuousState<double>& derivs,  // in/out
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    Context<double>& context) {
  const double t = context.get_time();
  ContinuousState<double>& xc = context.get_mutable_continuous_state();

  // Invalidate all xc-dependent quantities.
  VectorBase<double>& x1 = xc.get_mutable_vector();

  const auto vx0 = x1.CopyToVector();
  const auto& dx0 = derivs.get_vector();
  x1.PlusEqScaled(h, dx0);  // x1 += h*dx0 (initial guess)
  context.SetTime(t + h);   // t=t1
  const int nx = static_cast<int>(vx0.size());
  const auto I = MatrixX<double>::Identity(nx, nx);

  // Would normally iterate until convergence of dx norm as shown above.
  // Here I'm just iterating a fixed number of time that I know is plenty!
  for (int i = 0; i < 6; ++i) {
    system.CalcTimeDerivatives(context, &derivs);
    const auto& dx1 = derivs.get_vector();
    const auto vx1 = x1.CopyToVector();
    const auto vdx1 = dx1.CopyToVector();
    const auto err = vx1 - (vx0 + h * vdx1);
    const auto DxdotDx = CalcDxdotDx(system, context);
    const auto J = I - h * DxdotDx;
    Eigen::PartialPivLU<MatrixX<double>> Jlu(J);
    VectorX<double> dx = Jlu.solve(err);
    x1.SetFromVector(vx1 - dx);
  }
}

/* Calculate the total energy for this system in the given Context.
TODO(sherm1): assuming there is only KE and PE to worry about. */
double CalcEnergy(const SpringMassSystem<double>& system,
                  const Context<double>& context) {
  return system.CalcPotentialEnergy(context) +
         system.CalcKineticEnergy(context);
}

/* This test simulates the spring-mass system simultaneously using three first-
order integrators with different stability properties. The system itself is
conservative since it contains no actuation or damping. All numerical
integrators are approximate, but differing stability properties cause
predictably different effects on total system energy:
  - Explicit Euler: energy should increase
  - Implicit Euler: energy should decrease
  - Semi-explicit Euler: energy should remain constant
The test evaluates the initial energy and then makes sure it goes in the
expected direction for each integration method.

The primary goal of this test is to exercise the System 2.0 API by using it
for solvers that have different API demands. */
TEST_F(SpringMassSystemTest, Integrate) {
  const double h = 0.001, kTfinal = 9, kNumIntegrators = 3;

  // Resource indices for each of the three integrators.
  enum { kXe = 0, kIe = 1, kSxe = 2 };
  std::vector<unique_ptr<Context<double>>> contexts;
  std::vector<unique_ptr<SystemOutput<double>>> outputs;
  std::vector<unique_ptr<ContinuousState<double>>> derivs;

  // Allocate resources.
  for (int i = 0; i < kNumIntegrators; ++i) {
    contexts.push_back(system_->CreateDefaultContext());
    outputs.push_back(system_->AllocateOutput());
    derivs.push_back(system_->AllocateTimeDerivatives());
  }

  // Set initial conditions in each Context.
  for (auto& context : contexts) {
    context->SetTime(0);
    system_->set_position(context.get(), 0.1);  // Displacement 0.1m, vel. 0m/s.
    system_->set_velocity(context.get(), 0.);
  }

  // Save the initial energy (same in all contexts).
  const double initial_energy = CalcEnergy(*system_, *contexts[0]);

  while (true) {
    const auto t = contexts[0]->get_time();

    /* Evaluate derivatives at the beginning of a step, then generate
    Outputs (which may depend on derivatives). Uncomment five lines below if you
    want to get output you can plot.

    TODO(david-german-tri,sherm1) Should have a way to run similar code in
    both regression test form and as friendlier examples. */

    for (int i = 0; i < kNumIntegrators; ++i) {
      system_->CalcTimeDerivatives(*contexts[i], derivs[i].get());
      system_->CalcOutput(*contexts[i], outputs[i].get());
    }

    // Semi-explicit Euler should keep energy roughly constant every step.
    EXPECT_NEAR(CalcEnergy(*system_, *contexts[kSxe]), initial_energy, 1e-2);

    if (t >= kTfinal) break;

    // Integrate three ways.
    StepExplicitEuler(h, *derivs[kXe], *contexts[kXe]);
    StepImplicitEuler(h, *system_, *derivs[kIe], *contexts[kIe]);
    StepSemiExplicitEuler(h, *system_, *derivs[kSxe], *contexts[kSxe]);
  }

  // Now verify that each integrator behaved as expected.
  EXPECT_GT(CalcEnergy(*system_, *contexts[kXe]), 2 * initial_energy);
  EXPECT_LT(CalcEnergy(*system_, *contexts[kIe]), initial_energy / 2);
  EXPECT_NEAR(CalcEnergy(*system_, *contexts[kSxe]), initial_energy, 1e-2);
}

/* Test that EvalConservativePower() correctly measures the transfer of power
from potential energy to kinetic energy. We have to integrate the power to
calculate the net work W(t) done by the spring on the mass. If we set W(0)=0,
then it should always hold that
  PE(t) + KE(t) = PE(0) + KE(0)
  KE(t) = KE(0) + W(t)
  PE(t) = PE(0) - W(t)

This will only be true if the system is integrated with no energy gains or
losses from the numerical method, because those are not conservative changes.
The semi-explicit Euler method we used above is the only method we can use for
this test (unless we run with very small time steps). */
TEST_F(SpringMassSystemTest, IntegrateConservativePower) {
  const double h = 0.00001, kTfinal = 5;

  // Resources.
  unique_ptr<Context<double>> context = system_->CreateDefaultContext();
  unique_ptr<ContinuousState<double>> derivs =
      system_->AllocateTimeDerivatives();

  // Set initial conditions..
  context->SetTime(0);
  system_->set_position(context.get(), 0.1);  // Displacement 0.1m, vel. 0m/s.
  system_->set_velocity(context.get(), 0.);
  system_->set_conservative_work(context.get(), 0.);  // W(0)=0

  // Save the initial energy.
  const double pe0 = system_->CalcPotentialEnergy(*context);
  const double ke0 = system_->CalcKineticEnergy(*context);
  const double e0 = pe0 + ke0;

  while (true) {
    const auto t = context->get_time();

    // Evaluate derivatives at the beginning of a step. We don't need outputs.
    system_->CalcTimeDerivatives(*context, derivs.get());

    if (t >= kTfinal) break;

    const double pe = system_->CalcPotentialEnergy(*context);
    const double ke = system_->CalcKineticEnergy(*context);
    const double e = pe + ke;
    EXPECT_NEAR(e, e0, 1e-3);

    // Due to a quirk of semi-explicit Euler, this integral was evaluated
    // using the previous step's q's rather than the ones current now so
    // there is a larger discrepency than would be expected from accuracy
    // alone.
    const double w = system_->get_conservative_work(*context);
    EXPECT_NEAR(ke, ke0 + w, 1e-2);
    EXPECT_NEAR(pe, pe0 - w, 1e-2);

    // Take a step of size h.
    StepSemiExplicitEuler(h, *system_, *derivs, *context);
  }
}

TEST_F(SpringMassSystemTest, ToAutoDiff) {
  EXPECT_TRUE(is_autodiffxd_convertible(*system_));
}

TEST_F(SpringMassSystemTest, ToSymbolic) {
  EXPECT_TRUE(is_symbolic_convertible(*system_));
}

}  // namespace
}  // namespace systems
}  // namespace drake


/*
 ISC License

 Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder

 Permission to use, copy, modify, and/or distribute this software for any
 purpose with or without fee is hereby granted, provided that the above
 copyright notice and this permission notice appear in all copies.

 THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

 */

#include <iostream>
#include <math.h>
#include <Eigen/Dense>

/*

 Contains various support algorithms related to using the Eigen Library

 */

/*! This function provides a general conversion between an Eigen matrix and
an output C array.  Note that this routine would convert an inbound type
to a MAtrixXd and then transpose the matrix which would be inefficient
in a lot of cases.
@return void
@param inMat The source Eigen matrix that we are converting
@param outArray The destination array (sized by the user!) we copy in to
*/
void eigenMatrixXd2CArray(Eigen::MatrixXd inMat, double *outArray)
{
	Eigen::MatrixXd tempMat = inMat.transpose();
	memcpy(outArray, tempMat.data(), inMat.rows()*inMat.cols()*sizeof(double));
}

/*! This function provides a direct conversion between a 3-vector and an
output C array.  We are providing this function to save on the  inline conversion
and the transpose that would have been performed by the general case.
@return void
@param inMat The source Eigen matrix that we are converting
@param outArray The destination array (sized by the user!) we copy in to
*/
void eigenVector3d2CArray(Eigen::Vector3d & inMat, double *outArray)
{
	memcpy(outArray, inMat.data(), 3 * sizeof(double));
}

/*! This function provides a direct conversion between a 3x3 matrix and an
output C array.  We are providing this function to save on the inline conversion
that would have been performed by the general case.
@return void
@param inMat The source Eigen matrix that we are converting
@param outArray The destination array (sized by the user!) we copy in to
*/
void eigenMatrix3d2CArray(Eigen::Matrix3d & inMat, double *outArray)
{
	Eigen::MatrixXd tempMat = inMat.transpose();
	memcpy(outArray, tempMat.data(), 9 * sizeof(double));
}
/*! This function performs the general conversion between an input C array
and an Eigen matrix.  Note that to use this function the user MUST size
the Eigen matrix ahead of time so that the internal map call has enough
information to ingest the C array.
@return Eigen::MatrixXd
@param inArray The input array (row-major)
@param outMat The output Eigen matrix
*/
Eigen::MatrixXd cArray2EigenMatrixXd(double *inArray, int nRows, int nCols)
{
    Eigen::MatrixXd outMat;
    outMat.resize(nRows, nCols);
	outMat = Eigen::Map<Eigen::MatrixXd>(inArray, outMat.rows(), outMat.cols());
    return outMat;
}
/*! This function performs the conversion between an input C array
3-vector and an output Eigen vector3d.  This function is provided
in order to save an unnecessary conversion between types
@return Eigen::Vector3d
@param inArray The input array (row-major)
@param outMat The output Eigen matrix
*/
Eigen::Vector3d cArray2EigenVector3d(double *inArray)
{
    return Eigen::Map<Eigen::Vector3d>(inArray, 3, 1);
}
/*! This function performs the conversion between an input C array
3x3-matrix and an output Eigen vector3d.  This function is provided
in order to save an unnecessary conversion between types
@return Eigen::Matrix3d
@param inArray The input array (row-major)
@param outMat The output Eigen matrix
*/
Eigen::Matrix3d cArray2EigenMatrix3d(double *inArray)
{
	return Eigen::Map<Eigen::Matrix3d>(inArray, 3, 3);
}

/*! This function returns the Eigen DCM that corresponds to a 1-axis rotation
 by the angle theta.  The DCM is the positive theta rotation from the original
 frame to the final frame.
 @return Eigen::Matrix3d
 @param angle The input rotation angle
 */
Eigen::Matrix3d eigenM1(double angle)
{
    Eigen::Matrix3d mOut;

    mOut.setIdentity();

    mOut(1,1) = cos(angle);
    mOut(1,2) = sin(angle);
    mOut(2,1) = -mOut(1,2);
    mOut(2,2) = mOut(1,1);

    return mOut;
}

/*! This function returns the Eigen DCM that corresponds to a 2-axis rotation
 by the angle theta.  The DCM is the positive theta rotation from the original
 frame to the final frame.
 @return Eigen::Matrix3d
 @param angle The input rotation angle
 */
Eigen::Matrix3d eigenM2(double angle)
{
    Eigen::Matrix3d mOut;

    mOut.setIdentity();

    mOut(0,0) = cos(angle);
    mOut(0,2) = -sin(angle);
    mOut(2,0) = -mOut(0,2);
    mOut(2,2) = mOut(0,0);

    return mOut;
}

/*! This function returns the Eigen DCM that corresponds to a 3-axis rotation
 by the angle theta.  The DCM is the positive theta rotation from the original
 frame to the final frame.
 @return Eigen::Matrix3d
 @param angle The input rotation angle
 */
Eigen::Matrix3d eigenM3(double angle)
{
    Eigen::Matrix3d mOut;

    mOut.setIdentity();

    mOut(0,0) = cos(angle);
    mOut(0,1) = sin(angle);
    mOut(1,0) = -mOut(0,1);
    mOut(1,1) = mOut(0,0);

    return mOut;
}

/*! This function returns the tilde matrix version of a vector. The tilde
 matrix is the matrixi equivalent of a vector cross product, where
 [tilde_a] b == a x b
 @return Eigen::Matrix3d
 @param vec The input vector
 */
Eigen::Matrix3d eigenTilde(Eigen::Vector3d vec)
{
    Eigen::Matrix3d mOut;

    mOut(0,0) = mOut(1,1) = mOut(2,2) = 0.0;

    mOut(0,1) = -vec(2);
    mOut(1,0) =  vec(2);
    mOut(0,2) =  vec(1);
    mOut(2,0) = -vec(1);
    mOut(1,2) = -vec(0);
    mOut(2,1) =  vec(0);

    return mOut;
}

/*! This function solves for the zero of the passed function using the Newton Raphson Method
@return double
@param initialEstimate The initial value to use for newton-raphson
@param accuracy The desired upper bound for the error
@param f Function to find the zero of
@param fPrime First derivative of the function
*/
double newtonRaphsonSolve(double initialEstimate, double accuracy, std::function< double(double) >& f, std::function<
                          double(double) >& fPrime) {
	double currentEstimate = initialEstimate;
	for (int i = 0; i < 100 && std::abs(f(currentEstimate)) > accuracy; i++) {
		double functionVal = f(currentEstimate);
		double functionDeriv = fPrime(currentEstimate);
		currentEstimate = currentEstimate - functionVal/functionDeriv;
	}
	return currentEstimate;
}


/*
@copyright Louis Dionne 2014
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/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto negative = [](auto x) {
        return x < 0;
    };

    BOOST_HANA_CONSTEXPR_ASSERT(
        replace(negative, 0, list(-3, -2, -1, 0, 1, 2, 3))
        ==
        list(0, 0, 0, 0, 1, 2, 3)
    );
    //! [main]
}


// Copyright Aleksey Gurtovoy 2000-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/apply.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/aux_/test.hpp>

template< typename T > struct std_vector
{
#if defined(BOOST_MPL_CFG_NO_IMPLICIT_METAFUNCTIONS)
    typedef std_vector type;
    BOOST_MPL_AUX_LAMBDA_SUPPORT(1, std_vector, (T))
#endif
};

MPL_TEST_CASE()
{
    typedef plus<int_<2>,int_<3> > plus1;
    typedef lambda<plus1>::type plus2;
    MPL_ASSERT(( is_same< plus1,plus2 > ));

    typedef lambda<std_vector<int> >::type v;
    MPL_ASSERT(( is_same< v,std_vector<int> > ));

    typedef lambda<std_vector<_1> >::type make_vector;
    typedef apply_wrap1<make_vector,int>::type v_int;
    MPL_ASSERT(( is_same< v_int,std_vector<int> > ));
}

MPL_TEST_CASE()
{
    typedef plus<_1,_2> plus_fun;
    typedef apply2<plus_fun,int_<2>,int_<3> >::type res;

    MPL_ASSERT_RELATION( res::value, ==, 5 );
}

MPL_TEST_CASE()
{
    typedef apply1<_1, plus<_1,_2> >::type plus_fun;
    MPL_ASSERT(( is_same< plus_fun,plus<_1,_2> > ));

    typedef apply2<plus_fun,int_<2>,int_<3> >::type res;
    MPL_ASSERT_RELATION( res::value, ==, 5 );
}

MPL_TEST_CASE()
{
    typedef lambda< lambda<_1> >::type make_lambda;
    typedef apply_wrap1< make_lambda,std_vector<int> >::type v;
    MPL_ASSERT(( is_same< v,std_vector<int> > ));

    typedef apply_wrap1< make_lambda,std_vector<_1> >::type make_vector;
    typedef apply_wrap1< make_vector,int >::type v_int;
    MPL_ASSERT(( is_same< v_int,std_vector<int> > ));
}

MPL_TEST_CASE()
{
    typedef apply1< _1, std_vector<int> >::type v;
    MPL_ASSERT(( is_same< v,std_vector<int> > ));

    typedef apply1< _1, std_vector<_1> >::type v_lambda;
    typedef apply1<v_lambda,int>::type v_int;
    MPL_ASSERT(( is_same< v_int,std_vector<int> > ));
}

MPL_TEST_CASE()
{
    typedef apply1< lambda<_1>, std_vector<int> >::type v;
    MPL_ASSERT(( is_same< v,std_vector<int> > ));

    typedef apply1< lambda<_1>, std_vector<_1> >::type make_vector;
    typedef apply_wrap1< make_vector,int >::type v_int;
    MPL_ASSERT(( is_same< v_int,std_vector<int> > ));
}

MPL_TEST_CASE()
{
    typedef apply1< lambda<_1>, plus<_1,_2> >::type plus_fun;
    typedef apply_wrap2< plus_fun,int_<2>,int_<3> >::type res;

    MPL_ASSERT_RELATION( res::value, ==, 5 );
}

MPL_TEST_CASE()
{
    typedef bind2<plus<>,_1,_1> b1;
    typedef lambda<b1>::type b2;
    MPL_ASSERT(( is_same< b1,b2 > ));
}

MPL_TEST_CASE()
{
#if !BOOST_WORKAROUND(__MWERKS__, BOOST_TESTED_AT(0x3003))
    typedef lambda< lambda< bind2<plus<>,_1,_1> > >::type make_lambda;
    typedef apply_wrap1< make_lambda::type, int_<5> >::type res;
    MPL_ASSERT_RELATION( res::value, ==, 10 );
#endif
}

MPL_TEST_CASE()
{
    typedef apply1< bind2<plus<>,_1,_1>, int_<5> >::type res;
    MPL_ASSERT_RELATION( res::value, ==, 10 );
}

MPL_TEST_CASE()
{
    typedef apply1<_1, lambda<plus<_1,_2> > >::type plus_fun;
    typedef apply_wrap2< plus_fun::type, int_<2>,int_<3> >::type res;

    MPL_ASSERT_RELATION( res::value, ==, 5 );
}


//
// Created by yongqi on 18-1-3.
//

#ifndef ROBOCAMCAL_CALIBCOMMON_HPP
#define ROBOCAMCAL_CALIBCOMMON_HPP

#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>

namespace Robocamcal {

// supported calibration type
enum class CalibrationType {CameraCalibration, StereoCalibration, HandeyeCalibration};

// supported calibration board type
enum class BoardType {ChessBoard, AcirclesGrid, CharucoBoard, GridBoard};

// camera calibration results
struct CameraCalibResults {
  cv::Mat camera_matrix;
  cv::Mat dist_coeffs;
  cv::Size image_size;
  double rsm_error;
  bool valid;

  cv::Mat map1, map2;

  CameraCalibResults() : image_size(0, 0), valid(false) {}
  void print();
  bool write(std::string s);
  bool read(std::string s);
};

// camera calibration data
struct CameraCalibData {
  std::vector<std::vector<cv::Point2f>> image_points;
  std::vector<std::vector<cv::Point3f>> object_points;
  size_t size() {
    return image_points.size();
  }
};

// stereo calibration results
struct StereoCalibResults {
  CameraCalibResults left_res, right_res;
  cv::Mat R, T;
  cv::Mat E, F;
  cv::Mat R1, P1, R2, P2;
  cv::Mat Q;
  double rsm_error;
  bool valid;

  StereoCalibResults() : valid(false) {}
  void print();
  bool write(std::string s);
  bool read(std::string s);
};

// stereo calibration data
struct StereoCalibData {
  CameraCalibData left_data;
  CameraCalibData right_data;
  size_t size() {
    return left_data.size();
  }
};

// hand eye calibration data
struct HandEyeCalibData {
  std::vector<Eigen::Isometry3d> board_pose;
  std::vector<Eigen::Isometry3d> robot_pose;
  size_t size() {
    return board_pose.size();
  }
};

// hand eye calibration results
struct HandEyeCalibResults {
  Eigen::Isometry3d x;
  bool valid = false;
  CameraCalibResults intrs;
  bool eye_in_hand = false;
  void print();
  bool write(std::string s);
  bool read(std::string s);
};

}

#endif //ROBOCAMCAL_CALIBCOMMON_HPP


/*!
 * @file
 * This file defines `boost::mpl::is_reachable`.
 */

#ifndef BOOST_MPL_IS_REACHABLE_HPP
#define BOOST_MPL_IS_REACHABLE_HPP

#include <boost/mpl/adjacent_vertices_of.hpp>
#include <boost/mpl/any_of.hpp>
#include <boost/mpl/filter_view.hpp>
#include <boost/mpl/has_key.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/is_adjacent.hpp>
#include <boost/mpl/not.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/set.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost { namespace mpl {
    namespace is_reachable_detail {
        template <typename Graph, typename U, typename V, typename Seen>
        struct is_reachable_not_same;

        template <typename Graph, typename U, typename V, typename Seen>
        struct is_reachable_impl
            : or_<
                is_same<U, V>,
                is_adjacent<Graph, U, V>,
                is_reachable_not_same<Graph, U, V, Seen>
            >
        { };

        template <typename Graph, typename U, typename V, typename Seen>
        struct is_reachable_not_same {
            using NewSeen = typename insert<Seen, U>::type;

            using UnseenAdjacentVertices = filter_view<
                typename adjacent_vertices_of<Graph, U>::type,
                not_<has_key<NewSeen, _1>>
            >;

            using type = typename any_of<
                UnseenAdjacentVertices,
                is_reachable_impl<Graph, _1, V, NewSeen>
            >::type;
        };
    }

    //! Return whether `V` is reachable from `U`. A vertex is considered
    //! reachable from itself.
    template <typename Graph, typename U, typename V>
    struct is_reachable
        : is_reachable_detail::is_reachable_impl<
            Graph, U, V, typename set<>::type
        >
    { };
}} // end namespace boost::mpl

#endif // !BOOST_MPL_IS_REACHABLE_HPP


//  equal_to.hpp
//
//  (C) Copyright 2011 Vicente J. Botet Escriba
//  Use, modification and distribution are subject to the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt).
//

#ifndef BOOST_RATIO_MPL_EQUAL_TO_HPP
#define BOOST_RATIO_MPL_EQUAL_TO_HPP

#include <boost/ratio/ratio.hpp>
#include <boost/ratio/mpl/numeric_cast.hpp>
#include <boost/mpl/equal_to.hpp>

namespace boost {
namespace mpl {

template<>
struct equal_to_impl< rational_c_tag,rational_c_tag >
{
    template< typename R1, typename R2 > struct apply
        : ratio_equal<R1, R2>
    {
    };
};
}
}

#endif  // BOOST_RATIO_MPL_EQUAL_TO_HPP


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object computes the quadrant of its parameter.

    @par Header <boost/simd/function/quadrant.hpp>

    @par semantic:
    For any given positive integral value (possibly flint) @c n of  `quadrant(n)`
    returns the value in the same type 0, 1, 2 or 3 according to the
    two least significant bits of the truncated input.

    for integral types this is similar to n&3.

    @par Example:

      @snippet quadrant.cpp quadrant

    @par Possible output:

      @snippet quadrant.txt quadrant

  **/
  Value quadrant(Value const& n);
} }
#endif

#include <boost/simd/function/scalar/quadrant.hpp>
#include <boost/simd/function/simd/quadrant.hpp>

#endif


#include <boost/test/unit_test.hpp>
#include "algorithms/dp/coin_change.hpp"

BOOST_AUTO_TEST_SUITE(TestCoinChange)

BOOST_AUTO_TEST_CASE(test_coinchange_invalid_input) {
    {
        std::vector<int> empty;
        BOOST_CHECK(Algo::DP::CoinChange::ChangeGreedy(empty, 1) == -1);
    }

    {
        std::vector<int> empty;
        BOOST_CHECK(Algo::DP::CoinChange::ChangeDP(empty, 1) == -1);
    }

    {
        std::vector<int> empty;
        BOOST_CHECK(Algo::DP::CoinChange::ChangeGreedy(empty, -12) == -1);
    }

    {
        std::vector<int> empty;
        BOOST_CHECK(Algo::DP::CoinChange::ChangeDP(empty, -12) == -1);
    }
}

BOOST_AUTO_TEST_CASE(test_coinchange_valid_input) {
    {
        std::vector<int> coins = {1};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeGreedy(coins, 0) == 0);
    }

    {
        std::vector<int> coins = {1};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeDP(coins, 0) == 0);
    }

    {
        std::vector<int> coins = {1};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeGreedy(coins, 2) == 2);
    }

    {
        std::vector<int> coins = {1};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeDP(coins, 2) == 2);
    }

    {
        std::vector<int> coins = {1, 5, 10};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeGreedy(coins, 28) == 6);
    }

    {
        std::vector<int> coins = {1, 5, 10};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeDP(coins, 28) == 6);
    }

    {
        std::vector<int> coins = {186, 419, 83, 408};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeDP(coins, 6249) == 20);
    }

    {
        std::vector<int> coins = {2, 5, 10};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeDP(coins, 13) == 5);
    }
}

BOOST_AUTO_TEST_CASE(test_coinchange_no_change) {
    {
        std::vector<int> coins = {2, 5, 10};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeGreedy(coins, 1) == -1);
        BOOST_CHECK(Algo::DP::CoinChange::ChangeGreedy(coins, 13) == -1);
    }

    {
        std::vector<int> coins = {2, 5, 10};
        BOOST_CHECK(Algo::DP::CoinChange::ChangeDP(coins, 1) == -1);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*=============================================================================
  Copyright (c) 2021 Thinkoid, LLC
  https://github.com/thinkoid/Sprout

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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE libs_math

#include <boost/test/unit_test.hpp>
namespace utf = boost::unit_test;

#include <sprout/math/trunc.hpp>

#include <limits>

BOOST_AUTO_TEST_SUITE(libs_math_trunc)

BOOST_AUTO_TEST_CASE(libs_math_trunc_float)
{
    const auto x = (std::numeric_limits< uintmax_t >::max)();
    const auto result = sprout::math::trunc(static_cast< float >(x));
    BOOST_TEST(result == static_cast< float >(x));
}

BOOST_AUTO_TEST_CASE(libs_math_trunc_double)
{
    const auto x = (std::numeric_limits< uintmax_t >::max)();
    const auto result = sprout::math::trunc(static_cast< double >(x));
    BOOST_TEST(result == static_cast< double >(x));
}

BOOST_AUTO_TEST_CASE(libs_math_trunc_long_double)
{
    const auto x = (std::numeric_limits< uintmax_t >::max)();
    const auto result = sprout::math::trunc(static_cast< long double >(x));
    BOOST_TEST(result == static_cast< long double >(x));
}

BOOST_AUTO_TEST_SUITE_END()


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <algorithm>
#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <functional>
#include <tuple>
#include <type_traits>
#include <utility>  // IWYU pragma: keep // for std::forward

#include "DataStructures/ApplyMatrices.hpp"
#include "DataStructures/DataBox/PrefixHelpers.hpp"
#include "DataStructures/DataBox/Prefixes.hpp"
#include "DataStructures/Matrix.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "NumericalAlgorithms/DiscontinuousGalerkin/LiftFlux.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "NumericalAlgorithms/Spectral/Projection.hpp"
#include "Utilities/Algorithm.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/TMPL.hpp"
/// \cond
template <size_t VolumeDim>
class ElementId;
template <size_t VolumeDim>
class OrientationMap;
// IWYU pragma: no_forward_declare Variables
/// \endcond

namespace dg {

template <size_t VolumeDim>
using MortarId = std::pair<::Direction<VolumeDim>, ElementId<VolumeDim>>;
template <size_t MortarDim>
using MortarSize = std::array<Spectral::MortarSize, MortarDim>;
template <size_t VolumeDim, typename ValueType>
using MortarMap = std::unordered_map<MortarId<VolumeDim>, ValueType,
                                     boost::hash<MortarId<VolumeDim>>>;

/// \ingroup DiscontinuousGalerkinGroup
/// Find a mesh for a mortar capable of representing data from either
/// of two faces.
///
/// \warning Make sure the two face meshes are oriented the same, i.e.
/// their dimensions align. This is facilitated by the `orientation`
/// passed to `domain::Initialization::create_initial_mesh`, for
/// example.
template <size_t Dim>
Mesh<Dim> mortar_mesh(const Mesh<Dim>& face_mesh1, const Mesh<Dim>& face_mesh2);

/// \ingroup DiscontinuousGalerkinGroup
/// Determine the size of the mortar (i.e., the part of the face it
/// covers) for communicating with a neighbor.  This is the size
/// relative to the size of \p self, and will not generally agree with
/// that determined by \p neighbor.
template <size_t Dim>
MortarSize<Dim - 1> mortar_size(const ElementId<Dim>& self,
                                const ElementId<Dim>& neighbor,
                                size_t dimension,
                                const OrientationMap<Dim>& orientation);

/// \ingroup DiscontinuousGalerkinGroup
/// Project variables from a face to a mortar.
template <typename Tags, size_t Dim>
Variables<Tags> project_to_mortar(const Variables<Tags>& vars,
                                  const Mesh<Dim>& face_mesh,
                                  const Mesh<Dim>& mortar_mesh,
                                  const MortarSize<Dim>& mortar_size) {
  const auto projection_matrices = Spectral::projection_matrix_parent_to_child(
      face_mesh, mortar_mesh, mortar_size);
  return apply_matrices(projection_matrices, vars, face_mesh.extents());
}

/// \ingroup DiscontinuousGalerkinGroup
/// Project variables from a mortar to a face.
template <typename Tags, size_t Dim>
Variables<Tags> project_from_mortar(const Variables<Tags>& vars,
                                    const Mesh<Dim>& face_mesh,
                                    const Mesh<Dim>& mortar_mesh,
                                    const MortarSize<Dim>& mortar_size) {
  ASSERT(Spectral::needs_projection(face_mesh, mortar_mesh, mortar_size),
         "project_from_mortar should not be called if the interface mesh and "
         "mortar mesh are identical. Please elide the copy instead.");
  const auto projection_matrices = Spectral::projection_matrix_child_to_parent(
      mortar_mesh, face_mesh, mortar_size);
  return apply_matrices(projection_matrices, vars, mortar_mesh.extents());
}

}  // namespace dg


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing is_not_denormal capabilities

    Returns @ref True or @ref False according x is not denormal.

    @par Semantic:

    @code
    auto r = is_not_denormal(x);
    @endcode

    is similar to:

    @code
    auto r = !(abs(x) <   Smallestposval<T>()) || (x == 0);
    @endcode

    @see Smallestposval

  **/
  as_logical_t<Value> is_not_denormal(Value const& x)
} }
#endif

#include <boost/simd/function/scalar/is_not_denormal.hpp>
#include <boost/simd/function/simd/is_not_denormal.hpp>

#endif


#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <multisense_sensor_model/sensor_model.h>
#include <cylbot_mcl/pose_cloud2d.h>
#include <string>
#include <tf/transform_listener.h>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <pcl/filters/extract_indices.h>
#include <geometry_msgs/Pose.h>

using namespace multisense_sensor_model;
using namespace cylbot_mcl;

ros::Subscriber field_sub;

void laserCallback(const sensor_msgs::PointCloud2::ConstPtr& laser_points,
				   const tf::TransformListener& tf_listener,
				   PoseCloud2D* pose_cloud)
{
	tf::StampedTransform laser_transform;
	if(!tf_listener.waitForTransform(
		   "/base_link",
		   "/head_hokuyo_frame",
		   laser_points->header.stamp,
		   ros::Duration(1.0)))
	{
		ROS_WARN("Transform from /base_link to /head_hokuyo_frame failed");
		return;
	}

	tf_listener.lookupTransform("/base_link", "/head_hokuyo_frame", laser_points->header.stamp, laser_transform);
	tf::Vector3 beam_start = laser_transform.getOrigin();

	pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_raw_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromROSMsg(*laser_points, *pcl_raw_cloud);

	pcl::PointIndices::Ptr invalid_indices(new pcl::PointIndices);
	int i=0;
	for(pcl::PointCloud<pcl::PointXYZ>::const_iterator point = pcl_raw_cloud->points.begin();
		point != pcl_raw_cloud->points.end();
		point++)
	{
		if(point->x != point->x || point->y != point->y || point->z != point->z)
		{
			ROS_WARN("Point has a NaN value");
		}

		// calculate 3D beam length and check for max value
		double x = point->x - beam_start.x();
		double y = point->y - beam_start.y();
		double z = point->z - beam_start.z();
		double beam_length_3d = sqrt(x*x + y*y + z*z);
		if(fabs(beam_length_3d - 30.0) < .1)
		{
			invalid_indices->indices.push_back(i);
			i++;
			continue;
		}

		// check if point is in the plane (or at least close to it)
		if(point->z > .8034 || point->z < .4034)
		{
			invalid_indices->indices.push_back(i);
			i++;
			continue;
		}

		// check if the beam is within the robot's cylinder
		double beam_length_2d = sqrt(x*x + y*y);
		if(fabs(beam_length_2d ) < .5)
		{
			invalid_indices->indices.push_back(i);
			i++;
			continue;
		}
	}

	pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ExtractIndices<pcl::PointXYZ> invalid_filter;
	invalid_filter.setInputCloud(pcl_raw_cloud);
	invalid_filter.setNegative(true);
	invalid_filter.setIndices(invalid_indices);
	invalid_filter.filter(*pcl_cloud);

	tf::StampedTransform map_transform;
	if(!tf_listener.waitForTransform(
		   "/map",
		   "/base_link",
		   laser_points->header.stamp,
		   ros::Duration(1.0)))
	{
		ROS_WARN("Transform from /map to /base_link failed");
		return;
	}
	tf_listener.lookupTransform("/map", "/base_link", laser_points->header.stamp, map_transform);

	geometry_msgs::Pose true_pose;
	tf::pointTFToMsg(map_transform.getOrigin(), true_pose.position);
	tf::quaternionTFToMsg(map_transform.getRotation(), true_pose.orientation);
	true_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, tf::getYaw(true_pose.orientation) + 3.14159/2.0);

	pose_cloud->sensorUpdate(true_pose, *pcl_cloud, laser_points->header.stamp.toSec());

	//ROS_DEBUG("Starting true pose probability calculation");
	//double truePoseProb = pose_cloud->getMeasurementProbability(true_pose, true_pose, *pcl_cloud);
	//ROS_DEBUG_STREAM("true pose probability:" << truePoseProb);

	geometry_msgs::Pose fake_pose = true_pose;
	fake_pose.position.x *= 2.1;
	fake_pose.position.y *= 2.1;
	//ROS_DEBUG("Starting fake pose probability calculation");
	//double fakePoseProb = pose_cloud->getMeasurementProbability(true_pose, fake_pose, *pcl_cloud);
	//ROS_DEBUG_STREAM("fake pose probability:" << fakePoseProb);
}

void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& initial_pose,
						 PoseCloud2D* pose_cloud,
						 const int num_particles)
{
	pose_cloud->resetCloud(*initial_pose, num_particles);
}

void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg,
					  PoseCloud2D* pose_cloud,
					  geometry_msgs::TwistStamped::ConstPtr& last_cmd)
{
	if(last_cmd == NULL)
	{
		last_cmd = twist_msg;
		return;
	}
	ros::Duration dt = twist_msg->header.stamp - last_cmd->header.stamp;
	last_cmd = twist_msg;

	pose_cloud->motionUpdate(twist_msg->twist, dt.toSec());
}

void fieldCallback(const cylbot_map_creator::LikelihoodField::ConstPtr& field,
				 PoseCloud2D* pose_cloud)
{
	pose_cloud->fieldUpdate(*field);

	field_sub.shutdown();
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "localization_2d");

	ros::NodeHandle nh;

	tf::TransformListener tf_listener;

	ros::NodeHandle priv_nh("~");
	std::string sensor_params_file = "";
	priv_nh.getParam("sensor_params_file", sensor_params_file);
	int num_global_samples;
	if(!priv_nh.getParam("num_global_samples", num_global_samples))
		num_global_samples = 10000;

	int num_local_samples;
	if(!priv_nh.getParam("num_local_samples", num_local_samples))
		num_local_samples = 1000;

	IntrinsicParams sensor_params;
	if(sensor_params_file.compare("") == 0)
	{
		ROS_WARN("Sensor params file not provided or not valid");
	}
	else
	{
		readIntrinsicParamsFromFile(sensor_params_file, &sensor_params);
	}

	RobotModel model(sensor_params);

	for(int i=0; i<model.alpha.size(); i++)
	{
		model.alpha[i] = .01;
	}

	ROS_INFO("num_global_samples: %d num_local_samples %d", num_global_samples, num_local_samples);
	PoseCloud2D pose_cloud(model, num_global_samples);

	ros::Subscriber laser_sub = nh.subscribe<sensor_msgs::PointCloud2>("/laser/points", 1,
																	   boost::bind(laserCallback,
																				   _1,
																				   boost::ref(tf_listener),
																				   &pose_cloud));
	ros::Subscriber initial_pose_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1,
																							  boost::bind(initialPoseCallback,
																										  _1,
																										  &pose_cloud,
																										  num_local_samples));

	geometry_msgs::TwistStamped::ConstPtr last_velocity;
	ros::Subscriber vel_sub = nh.subscribe<geometry_msgs::TwistStamped>("/cylbot/velocity", 1000,
																		boost::bind(velocityCallback,
																					_1,
																					&pose_cloud,
																					boost::ref(last_velocity)));

	field_sub = nh.subscribe<cylbot_map_creator::LikelihoodField>("/likelihood_field", 1, boost::bind(fieldCallback, _1, &pose_cloud));

	ros::Publisher pose_array_pub = nh.advertise<geometry_msgs::PoseArray>("/pose_cloud", 1);

	ros::Rate loop_rate(100);
	while(ros::ok())
	{
		geometry_msgs::PoseArray pose_array = pose_cloud.getPoses();
		pose_array.header.stamp = ros::Time::now();
		pose_array.header.frame_id = "/map";
		pose_array_pub.publish(pose_array);

		ros::spinOnce();
		loop_rate.sleep();
	}
}


#include <boost/heap/fibonacci_heap.hpp>


// Copyright 2010 The Trustees of Indiana University.

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

//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_GRAPH_RANDOM_SPANNING_TREE_HPP
#define BOOST_GRAPH_RANDOM_SPANNING_TREE_HPP

#include <vector>
#include <boost/assert.hpp>
#include <boost/graph/loop_erased_random_walk.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/named_function_params.hpp>

namespace boost
{

namespace detail
{
    // Use Wilson's algorithm (based on loop-free random walks) to generate a
    // random spanning tree.  The distribution of edges used is controlled by
    // the next_edge() function, so this version allows either weighted or
    // unweighted selection of trees.
    // Algorithm is from http://en.wikipedia.org/wiki/Uniform_spanning_tree
    template < typename Graph, typename PredMap, typename ColorMap,
        typename NextEdge >
    void random_spanning_tree_internal(const Graph& g,
        typename graph_traits< Graph >::vertex_descriptor s, PredMap pred,
        ColorMap color, NextEdge next_edge)
    {
        typedef
            typename graph_traits< Graph >::vertex_descriptor vertex_descriptor;

        BOOST_ASSERT(num_vertices(g)
            >= 1); // g must also be undirected (or symmetric) and connected

        typedef color_traits< typename property_traits< ColorMap >::value_type >
            color_gen;
        BGL_FORALL_VERTICES_T(v, g, Graph) put(color, v, color_gen::white());

        std::vector< vertex_descriptor > path;

        put(color, s, color_gen::black());
        put(pred, s, graph_traits< Graph >::null_vertex());

        BGL_FORALL_VERTICES_T(v, g, Graph)
        {
            if (get(color, v) != color_gen::white())
                continue;
            loop_erased_random_walk(g, v, next_edge, color, path);
            for (typename std::vector<
                     vertex_descriptor >::const_reverse_iterator i
                 = path.rbegin();
                 boost::next(i)
                 != (typename std::vector<
                     vertex_descriptor >::const_reverse_iterator)path.rend();
                 ++i)
            {
                typename std::vector<
                    vertex_descriptor >::const_reverse_iterator j
                    = i;
                ++j;
                BOOST_ASSERT(get(color, *j) == color_gen::gray());
                put(color, *j, color_gen::black());
                put(pred, *j, *i);
            }
        }
    }
}

// Compute a uniformly-distributed spanning tree on a graph.  Use Wilson's
// algorithm:
// @inproceedings{wilson96generating,
//    author = {Wilson, David Bruce},
//    title = {Generating random spanning trees more quickly than the cover
//    time}, booktitle = {STOC '96: Proceedings of the twenty-eighth annual ACM
//    symposium on Theory of computing}, year = {1996}, isbn = {0-89791-785-5},
//    pages = {296--303},
//    location = {Philadelphia, Pennsylvania, United States},
//    doi = {http://doi.acm.org/10.1145/237814.237880},
//    publisher = {ACM},
//    address = {New York, NY, USA},
//  }
//
template < typename Graph, typename Gen, typename PredMap, typename ColorMap >
void random_spanning_tree(const Graph& g, Gen& gen,
    typename graph_traits< Graph >::vertex_descriptor root, PredMap pred,
    static_property_map< double >, ColorMap color)
{
    unweighted_random_out_edge_gen< Graph, Gen > random_oe(gen);
    detail::random_spanning_tree_internal(g, root, pred, color, random_oe);
}

// Compute a weight-distributed spanning tree on a graph.
template < typename Graph, typename Gen, typename PredMap, typename WeightMap,
    typename ColorMap >
void random_spanning_tree(const Graph& g, Gen& gen,
    typename graph_traits< Graph >::vertex_descriptor root, PredMap pred,
    WeightMap weight, ColorMap color)
{
    weighted_random_out_edge_gen< Graph, WeightMap, Gen > random_oe(
        weight, gen);
    detail::random_spanning_tree_internal(g, root, pred, color, random_oe);
}

template < typename Graph, typename Gen, typename P, typename T, typename R >
void random_spanning_tree(
    const Graph& g, Gen& gen, const bgl_named_params< P, T, R >& params)
{
    using namespace boost::graph::keywords;
    typedef bgl_named_params< P, T, R > params_type;
    BOOST_GRAPH_DECLARE_CONVERTED_PARAMETERS(params_type, params)
    typedef typename graph_traits< Graph >::vertex_descriptor vertex_descriptor;
    vertex_descriptor default_vertex = *vertices(g).first;
    vertex_descriptor start_vertex = arg_pack[_root_vertex | default_vertex];
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::predecessor_map >::type pred_map
        = arg_pack[_predecessor_map];
    static_property_map< double > default_weight_map(1.);
    typename boost::parameter::value_type< arg_pack_type,
        boost::graph::keywords::tag::weight_map,
        static_property_map< double > >::type e_w_map
        = arg_pack[_weight_map | default_weight_map];
    typename boost::detail::map_maker< Graph, arg_pack_type,
        boost::graph::keywords::tag::color_map,
        boost::default_color_type >::map_type c_map
        = boost::detail::make_color_map_from_arg_pack(g, arg_pack);
    random_spanning_tree(g, gen, start_vertex, pred_map, e_w_map, c_map);
}
}

#include <boost/graph/iteration_macros_undef.hpp>

#endif // BOOST_GRAPH_RANDOM_SPANNING_TREE_HPP


// Copyright (c) Dewetron 2017
#include "otfft.h"

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <vector>

BOOST_AUTO_TEST_SUITE(otfft_dct_test)

BOOST_AUTO_TEST_CASE(TestAlternatingOnes)
{
    const std::size_t SIZE = 4;
    const double testArray[SIZE] = {1.0, -1.0, 1.0, -1.0};

    std::vector<double> workspace(testArray, testArray + SIZE);
    {
        auto dct = OTFFT::Factory::createDCT(static_cast<int>(SIZE));
        dct->fwd0(workspace.data());
        dct->invn(workspace.data());
    }

    for (std::size_t idx{0}; idx < SIZE; ++idx)
    {
        BOOST_CHECK_CLOSE(testArray[idx], workspace[idx], .1);
    }
}

BOOST_AUTO_TEST_CASE(TestConstantInput)
{
    const std::size_t SIZE = 4;
    const double testArray[SIZE] = {1.0, 1.0, 1.0, 1.0};

    std::vector<double> workspace(testArray, testArray + SIZE);
    {
        auto dct = OTFFT::Factory::createDCT(static_cast<int>(SIZE));
        dct->fwd0(workspace.data());
        dct->invn(workspace.data());
    }

    for (std::size_t idx{0}; idx < SIZE; ++idx)
    {
        BOOST_CHECK_CLOSE(testArray[idx], workspace[idx], .1);
    }
}

BOOST_AUTO_TEST_CASE(TestCachedCosines)
{
    const std::size_t SIZE = 4;
    const double testArray[SIZE] = {1.0, 1.0, 1.0, 1.0};

    std::vector<double> workspace(testArray, testArray + SIZE);
    {
        auto dct = OTFFT::Factory::createDCT(static_cast<int>(SIZE));
        dct->fwd0(workspace.data());
        dct->fwd0(workspace.data());
        dct->invn(workspace.data());
        dct->invn(workspace.data());
    }

    for (std::size_t idx{0}; idx < SIZE; ++idx)
    {
        BOOST_CHECK_CLOSE(testArray[idx], workspace[idx], .1);
    }
}

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_SIMD_LOGICAL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IS_SIMD_LOGICAL_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#include <boost/simd/function/simd/all.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD(is_simd_logical_
                         , (typename A0,typename X)
                         , bd::cpu_
                         , bs::pack_<bd::arithmetic_<A0>,X>
                         )

  {
    using result = bs::as_logical_t<bd::scalar_of_t<A0>>;
    BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0, signed>;
      iA0 tmp = bitwise_cast<iA0>(a0);
      return {bs::all(logical_or(is_equal(tmp, Mone<iA0>()), is_eqz(tmp)))};
    }
  };

  BOOST_DISPATCH_OVERLOAD(is_simd_logical_
                         , (typename A0,typename X)
                         , bd::cpu_
                         , bs::pack_<bs::logical_<A0>,X>
                         )

  {
    using sA0 = bd::scalar_of_t<A0>;
    using result = bs::as_logical_t<sA0>;
    BOOST_FORCEINLINE result operator()(A0 const&) const
    {
      return {true};
    }
  };

} } }

#endif