//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ROUND_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ROUND_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/round.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/bitofsign.hpp>
#include <boost/simd/include/constants/twotonmb.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::round_, tag::cpu_, (A0)(X)
                            , ((simd_<integer_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return a0; }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::round_, boost::simd::tag::simd_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const result_type v   = boost::simd::abs(a0);
      const result_type t2n = boost::simd::Twotonmb<A0>();
      const result_type d0  = v+t2n;
      const result_type d   = d0-t2n;
      const result_type d1  = if_else(lt(v,t2n),d,v);
      return (d1^bitofsign(a0));
    }
  };
} } }

#endif


/*
@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/bool.hpp>
#include <boost/hana/integral.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

struct T; struct E;
constexpr struct { } invalid{};

auto test_eval_if = [](auto true_, auto false_) {
    auto x = eval_if(true_,
        [](auto) { return type<T>; },
        [](auto id) { id(invalid)("would fail"); return type<E>; }
    );
    BOOST_HANA_CONSTANT_ASSERT(x == type<T>);

    auto y = eval_if(false_,
        [](auto id) { id(invalid)("would fail"); return type<T>; },
        [](auto) { return type<E>; }
    );
    BOOST_HANA_CONSTANT_ASSERT(y == type<E>);
};

auto test_if = [](auto true_, auto false_) {
    BOOST_HANA_CONSTANT_ASSERT(if_(true_, type<T>, type<E>) == type<T>);
    BOOST_HANA_CONSTANT_ASSERT(if_(false_, type<T>, type<E>) == type<E>);
};

auto test_and = [](auto true_, auto false_) {
    BOOST_HANA_CONSTANT_ASSERT(!or_(false_));
    BOOST_HANA_CONSTANT_ASSERT(or_(true_));
    BOOST_HANA_CONSTANT_ASSERT(!or_(false_, false_));
    BOOST_HANA_CONSTANT_ASSERT(or_(false_, true_));
    BOOST_HANA_CONSTANT_ASSERT(or_(false_, false_, true_));
    // BOOST_HANA_CONSTANT_ASSERT(or_(true_, invalid, invalid));
};

auto test_or = [](auto true_, auto false_) {
    BOOST_HANA_CONSTANT_ASSERT(and_(true_));
    BOOST_HANA_CONSTANT_ASSERT(!and_(false_));
    BOOST_HANA_CONSTANT_ASSERT(and_(true_, true_));
    BOOST_HANA_CONSTANT_ASSERT(!and_(true_, false_));
    BOOST_HANA_CONSTANT_ASSERT(!and_(true_, true_, false_));
    // BOOST_HANA_CONSTANT_ASSERT(!and_(false_, invalid, invalid));
};

auto test_not = [](auto true_, auto false_) {
    BOOST_HANA_CONSTANT_ASSERT(not_(false_));
    BOOST_HANA_CONSTANT_ASSERT(not_(not_(true_)));
};

auto test = [](auto true_, auto false_) {
    test_eval_if(true_, false_);
    test_if(true_, false_);
    test_and(true_, false_);
    test_or(true_, false_);
    test_not(true_, false_);
};

int main() {
    test(true_, false_);
    test(int_<2>, int_<0>); // make sure int_<2> can be treated as true_
}


//
// Created by Alberto Bailoni on 07/01/2021.
//

#pragma once

#include <functional>
#include <set>
#include <vector>
#include <unordered_set>
#include <boost/container/flat_set.hpp>
#include <string>
#include <cmath>        // std::abs

// #include "nifty/tools/changable_priority_queue.hxx"
// #include "nifty/graph/edge_contraction_graph.hxx"
// #include "nifty/graph/agglo/cluster_policies/cluster_policies_common.hxx"
#include <iostream>
#include "xtensor/xarray.hpp"

#include "nifty/parallel/threadpool.hxx"
#include <ctime> // Random seed for shuffle
#include <xtensor/xrandom.hpp>

namespace nifty{
    namespace graph {
        template<class GRAPH, class LABELS, class SIGNED_WEIGHTS, class IS_LOCAL_EDGE>
        void runLabelPropagation(const GRAPH & graph,
                                 LABELS & nodeLabels,
                                 const SIGNED_WEIGHTS & signedWeights,
                                 const IS_LOCAL_EDGE & localEdges,
                                 const uint64_t nb_iter=1,
                                 const int64_t size_constr=-1,
                                 const int64_t nbThreads=-1){
            typedef std::set<uint64_t> uintSetType;
            const auto nbNodes = graph.nodeIdUpperBound()+1;
            xt::xtensor<uint64_t, 1> clusterSizes = xt::ones<uint64_t>({nbNodes});
            xt::xtensor<uint64_t, 1> shuffled_nodes = xt::arange<uint64_t>(0, nbNodes, 1);
            xt::random::seed(time(NULL));
            // Shuffle the order of the nodes:
            auto engine = xt::random::get_default_random_engine();

            const parallel::ParallelOptions pOpts(nbThreads);
            const std::size_t actualNumberOfThreads = pOpts.getActualNumThreads();

            xt::xtensor<double, 2> neighValues = xt::zeros<double>({actualNumberOfThreads, (std::size_t) graph.nodeIdUpperBound()+1});
            // xt::xtensor<double, 1> neighSizes = xt::zeros<double>({actualNumberOfThreads, (std::size_t) graph.nodeIdUpperBound()+1});
            xt::xtensor<bool, 2> neighLocality = xt::zeros<bool>({actualNumberOfThreads, (std::size_t) graph.nodeIdUpperBound()+1});

            for (uint64_t iter = 0; iter < nb_iter; ++iter) {
                xt::random::shuffle(shuffled_nodes, engine);

                parallel::ThreadPool threadpool(pOpts);
                nifty::parallel::parallel_foreach(threadpool,
                                           shuffled_nodes.begin(),
                                           shuffled_nodes.end(),
                                           [&](const int tid, const uint64_t node){
                                               const auto oldLabel = nodeLabels(node);
                                               uintSetType neighStats;

                                               for (auto adj : graph.adjacency(node)) {
                                                   const auto neighNode = adj.node();
                                                   const auto neighEdge = adj.edge();
                                                   const auto neighLabel = nodeLabels(neighNode);
                                                   auto neighSize = clusterSizes(neighLabel);
                                                   if (neighLabel == oldLabel) {
                                                       neighSize--;
                                                   }
                                                   if (size_constr > 0) {
                                                       if (neighSize >= size_constr) {
                                                           continue;
                                                       }
                                                   }
                                                   // Update stats:
                                                   neighStats.insert(neighLabel);
                                                   neighValues(tid, neighLabel) = neighValues(tid, neighLabel) + signedWeights(neighEdge);
                                                   // neighSizes(neighLabel) = neighSizes(neighLabel) + 1;
                                                   auto edge_is_local = localEdges(neighEdge);
                                                   neighLocality(tid, neighLabel) = neighLocality(tid, neighLabel) || edge_is_local;
                                               }
                                               // Find max label:
                                               uintSetType maxLabels;
                                               // TODO: update and add new label if all are repulsive!
                                               maxLabels.insert(oldLabel);
                                               double max = 0.;
                                               for (auto neighborCluster : neighStats) {
                                                   if (neighLocality(tid, neighborCluster)) {
                                                       auto prio = neighValues(tid, neighborCluster);
                                                       if (prio > max) {
                                                           max = prio;
                                                           maxLabels = uintSetType();
                                                           maxLabels.insert(neighborCluster);
                                                       } else if (prio == max) {
                                                           maxLabels.insert(neighborCluster);
                                                       }
                                                   }
                                                   // Reset stats for next node:
                                                   neighValues(tid, neighborCluster) = 0;
                                                   neighLocality(tid, neighborCluster) = false;
                                               }

                                               // If more than one, extract one randomly:
                                               uint64_t selectedCluster;
                                               if (maxLabels.size() > 1) {
                                                   const uint64_t nb_labels = maxLabels.size();
                                                   xt::xtensor<uint64_t, 1> randomLabel = xt::random::randint({1}, (uint64_t) 0, nb_labels, engine);
                                                   selectedCluster = *std::next(maxLabels.begin(), randomLabel(0));
                                               } else {
                                                   selectedCluster = *maxLabels.begin();
                                               }

                                               nodeLabels(node) = selectedCluster;
                                               clusterSizes(oldLabel)--;
                                               clusterSizes(selectedCluster)++;
                                           });

                // for (const uint64_t node : shuffled_nodes) {
                //
                // }
            }
        }
    }
}


#include "stdafx.h"

#include <boost/gil/gil_all.hpp>
#include <boost/test/unit_test.hpp>

#include "base_types.hpp"

namespace boost { namespace gil {

template< int K >
struct min_value
{
    typedef typename detail::min_fast_uint< K >::type integer_t;

    static integer_t apply() { return 0; }
};

template< int K >
struct max_value
{
    typedef typename detail::min_fast_uint< K >::type integer_t;

    ///@todo why not unsigned_integral_max_value< K > ?
    static integer_t apply() { return detail::unsigned_integral_max_value< packed_channel_value< K > >::value; }
};

} // namespace gil
} // namespace boost

template< int K >
struct make_scoped_channel_value
{
    typedef typename boost::gil::detail::min_fast_uint< K >::type integer_t;

    typedef boost::gil::scoped_channel_value< integer_t, boost::gil::min_value< K >, boost::gil::max_value< K > > type;
};

using namespace boost::gil;

BOOST_AUTO_TEST_SUITE( gil_test )

BOOST_AUTO_TEST_CASE( scoped_channel_test )
{
	typedef make_scoped_channel_value< 12 >::integer_t integer_t;
    typedef make_scoped_channel_value< 12 >::type bits12_t;

    integer_t max =  channel_traits< bits12_t >::max_value();
    BOOST_CHECK_EQUAL( channel_convert<integer_t>(max), max );
}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/**
 * @file matmul.h
 * @brief some matrix / linear algenra stuff
 */
#include <helib/matmul.h>
#include <NTL/BasicThreadPool.h>
#include <helib/fhe_stats.h>

#if (defined(__unix__) || defined(__unix) || defined(unix))
#include <sys/time.h>
#include <sys/resource.h>
#endif

// Implementation of the various random matrices is found here
#include <helib/randomMatrices.h>
#include <helib/ArgMap.h>

NTL_CLIENT
using namespace helib;

/*
 * Defined in this file are the following class templates:
 *
 *   class RandomMatrix: public MatMul1D_derived<type>
 *   class RandomMultiMatrix: public MatMul1D_derived<type>
 *   class RandomBlockMatrix: public BlockMatMul1D_derived<type>
 *   class RandomMultiBlockMatrix: public BlockMatMul1D_derived<type>
 *   class RandomFullMatrix: public MatMulFull_derived<type>
 *   class RandomFullBlockMatrix : public BlockMatMulFull_derived<type>
 *
 * Each of them has a corresponding build function, namely:
 *
 *   MatMul1D* buildRandomMatrix(const EncryptedArray& ea, long dim);
 *   MatMul1D* buildRandomMultiMatrix(const EncryptedArray& ea, long dim);
 *   BlockMatMul1D* buildRandomBlockMatrix(const EncryptedArray& ea, long dim);
 *   BlockMatMul1D* buildRandomMultiBlockMatrix(const EncryptedArray& ea, long dim);
 *   MatMulFull* buildRandomFullMatrix(const EncryptedArray& ea);
 *   BlockMatMulFull* buildRandomFullBlockMatrix(const EncryptedArray& ea);
 */

template<class Matrix>
bool DoTest(const Matrix& mat, const EncryptedArray& ea,
            const SecKey& secretKey, bool minimal, bool verbose)
{
  HELIB_NTIMER_START(EncodeMartix_MatMul);
  typename Matrix::ExecType mat_exec(mat, minimal);
  mat_exec.upgrade();
  HELIB_NTIMER_STOP(EncodeMartix_MatMul);

  // choose a random plaintext vector and encrypt it
  PlaintextArray v(ea);
  random(ea, v);

  // encrypt the random vector
  Ctxt ctxt(secretKey);
  ea.encrypt(ctxt, secretKey, v);
  Ctxt ctxt2 = ctxt;

  mat_exec.mul(ctxt);

  mul(v, mat);     // multiply the plaintext vector

  PlaintextArray v1(ea);
  ea.decrypt(ctxt, secretKey, v1); // decrypt the ciphertext vector

  return equals(ea, v, v1);        // check that we've got the right answer
}

int ks_strategy = 0;
// 0 == default
// 1 == full
// 2 == BSGS
// 3 == minimal

void TestIt(Context& context, long dim, bool verbose, long full, long block)
{
  resetAllTimers();
  if (verbose) {
    context.zMStar.printout();
    std::cout << "# small primes = " << context.smallPrimes.card() << "\n";
    std::cout << "# ctxt primes = " << context.ctxtPrimes.card() << "\n";
    std::cout << "# bits in ctxt primes = "
	 << long(context.logOfProduct(context.ctxtPrimes)/log(2.0) + 0.5) << "\n";
    std::cout << "# special primes = " << context.specialPrimes.card() << "\n";
    std::cout << "# bits in special primes = "
	 << long(context.logOfProduct(context.specialPrimes)/log(2.0) + 0.5) << "\n";

    fhe_stats = true;
  }

  SecKey secretKey(context);
  const PubKey& publicKey = secretKey;
  secretKey.GenSecKey(); // A Hamming-weight-w secret key

  bool minimal = ks_strategy == 3;

  // we call addSomeFrbMatrices for all strategies except minimal

  switch (ks_strategy) {
  case 0:
    addSome1DMatrices(secretKey);
    addSomeFrbMatrices(secretKey);
    break;
  case 1:
    add1DMatrices(secretKey);
    addSomeFrbMatrices(secretKey);
    break;
  case 2:
    addBSGS1DMatrices(secretKey);
    addSomeFrbMatrices(secretKey);
    break;
  case 3:
    addMinimal1DMatrices(secretKey);
    addMinimalFrbMatrices(secretKey);
    break;

   default:
     Error("bad ks_strategy");
   }

  // encrypted array with "full slots"
  EncryptedArray ea(context, context.alMod);

  if (verbose) {
    {std::stringstream ss;
    ss << publicKey;
    cout << "\n  |pubKey|="<<ss.tellp()<<" bytes";}
    cout << ", security=" << context.securityLevel()<<endl;
    cout << "  #threads="<<NTL::AvailableThreads();
    if (block)
      cout << "block-size="<<(ea.getPAlgebra().getOrdP());
    cout << ", vector-dimension="
         << (full? ea.size() : ea.sizeOfDimension(dim))<<", ";
    if (!full)
      cout << (ea.size()/ea.sizeOfDimension(dim))<<" products in parallel, ";
  }

  bool okSoFar = true;
  for (long i=0; i<5; i++) {
    if (full == 0 && block == 0) {
      std::unique_ptr< MatMul1D > ptr(buildRandomMatrix(ea,dim));
      if (!DoTest(*ptr, ea, secretKey, minimal, verbose))
        okSoFar = false;
    }
    else if (full == 0 && block == 1) {
      std::unique_ptr< BlockMatMul1D > ptr(buildRandomBlockMatrix(ea,dim));
      if (!DoTest(*ptr, ea, secretKey, minimal, verbose))
        okSoFar = false;
    }
    else if (full == 1 && block == 0) {
      std::unique_ptr< MatMulFull > ptr(buildRandomFullMatrix(ea));
      if (!DoTest(*ptr, ea, secretKey, minimal, verbose))
        okSoFar = false;
    }
    else if (full == 1 && block == 1) {
      std::unique_ptr< BlockMatMulFull > ptr(buildRandomFullBlockMatrix(ea));
      if (!DoTest(*ptr, ea, secretKey, minimal, verbose))
        okSoFar = false;
    }
  }
  cout << (okSoFar? "GOOD\n" : "BAD\n");

  if (verbose) {
    printAllTimers(cout);
#if (defined(__unix__) || defined(__unix) || defined(unix))
      struct rusage rusage;
      getrusage( RUSAGE_SELF, &rusage );
      cout << "  rusage.ru_maxrss="<<rusage.ru_maxrss << endl;
#endif
    print_stats(cout);
  }
}

int main(int argc, char *argv[])
{
  ArgMap amap;

  long m=2047;
  amap.arg("m", m, "defines the cyclotomic polynomial Phi_m(X)");
  long p=2;
  amap.arg("p", p, "plaintext base");
  long r=1;
  amap.arg("r", r,  "lifting");
  long L=200;
  amap.arg("L", L, "# of bits in the modulus chain");
  long dim=0;
  amap.arg("dim", dim, "dimension along which to multiply");
  long verbose=0;
  amap.arg("verbose", verbose, "print timing and other info");
  long nt=1;
  amap.arg("nt", nt, "# threads");

  amap.arg("force_bsgs", fhe_test_force_bsgs,
           "1 to force on, -1 to force off");
  amap.arg("force_hoist", fhe_test_force_hoist,
           "-1 to force off");
  amap.arg("ks_strategy", ks_strategy,
           "0: default, 1:full, 2:bsgs, 3:minimal");

  long full = 0;
  amap.arg("full", full, "0: 1D, 1: full");

  long block = 0;
  amap.arg("block", block, "0: normal, 1: block");

  NTL::Vec<long> gens;
  amap.arg("gens", gens, "use specified vector of generators", nullptr);
  amap.note("e.g., gens='[420 1105 1425]'");
  NTL::Vec<long> ords;
  amap.arg("ords", ords, "use specified vector of orders", nullptr);
  amap.note("e.g., ords='[11 8 2]', negative means 'bad'");

  amap.parse(argc, argv);

  if (verbose) {
    cout << "*** Test_MatMul: m=" << m
	 << ", p=" << p
	 << ", r=" << r
	 << ", L=" << L
	 << ", dim=" << dim
	 << ", nt=" << nt
	 << ", full=" << full
	 << ", block=" << block
	 << ", force_bsgs=" << fhe_test_force_bsgs
	 << ", force_hoist=" << fhe_test_force_hoist
	 << ", ks_strategy=" << ks_strategy
	 << endl;

   }

  vector<long> gens1, ords1;
  convert(gens1, gens);
  convert(ords1, ords);

  if (nt > 1) SetNumThreads(nt);

  setTimersOn();

  Context context(m, p, r, gens1, ords1);
  buildModChain(context, L, /*c=*/3);

  TestIt(context, dim, verbose, full, block);
}


//               Copyright Marek Dalewski 2017
// Distributed under the Boost Software License, Version 1.0.
//        (See accompanying file LICENSE.md or copy at
//           http://www.boost.org/LICENSE_1_0.txt)

#include <commander/algorithm.hpp>

#include <boost/test/unit_test.hpp>

#include "arithmetic_helpers.hpp"

#include <type_traits>

BOOST_AUTO_TEST_CASE( in_arithmetic_range )
{
    namespace comd = commander::detail;

    {
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<int8_t>::max())      == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<int8_t>::lowest())   == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<uint8_t>::max())     == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<uint8_t>::lowest())  == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<int16_t>::max())     == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<int16_t>::lowest())  == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<uint16_t>::max())    == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<uint16_t>::lowest()) == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<int32_t>::max())     == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<int32_t>::lowest())  == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<uint32_t>::max())    == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<uint32_t>::lowest()) == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<int64_t>::max())     == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<int64_t>::lowest())  == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<uint64_t>::max())    == true);
        BOOST_CHECK(comd::in_arithmetic_range<bool>(nl<uint64_t>::lowest()) == true);

        BOOST_CHECK(comd::in_arithmetic_range<int8_t>(true)   == true);
        BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(true)  == true);
        BOOST_CHECK(comd::in_arithmetic_range<int16_t>(true)  == true);
        BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(true) == true);
        BOOST_CHECK(comd::in_arithmetic_range<int32_t>(true)  == true);
        BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(true) == true);
        BOOST_CHECK(comd::in_arithmetic_range<int64_t>(true)  == true);
        BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(true) == true);

        BOOST_CHECK(comd::in_arithmetic_range<int8_t>(false)   == true);
        BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(false)  == true);
        BOOST_CHECK(comd::in_arithmetic_range<int16_t>(false)  == true);
        BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(false) == true);
        BOOST_CHECK(comd::in_arithmetic_range<int32_t>(false)  == true);
        BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(false) == true);
        BOOST_CHECK(comd::in_arithmetic_range<int64_t>(false)  == true);
        BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(false) == true);
    }

    {
        {
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<int8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<int8_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<uint8_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<uint8_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint8_t>(nl<int8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint8_t>(nl<int8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint8_t>(nl<int8_t>::max(), +1)) == false);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<int16_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<int16_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int16_t>(nl<int8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int16_t>(nl<int8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int16_t>(nl<int8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int16_t>(nl<int8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int16_t>(nl<int8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int16_t>(nl<int8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<uint16_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<uint16_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint16_t>(nl<int8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint16_t>(nl<int8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint16_t>(nl<int8_t>::max(), +1)) == false);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<int32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<int32_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int32_t>(nl<int8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int32_t>(nl<int8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int32_t>(nl<int8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int32_t>(nl<int8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int32_t>(nl<int8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int32_t>(nl<int8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<uint32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<uint32_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint32_t>(nl<int8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint32_t>(nl<int8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint32_t>(nl<int8_t>::max(), +1)) == false);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<int64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<int64_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int64_t>(nl<int8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int64_t>(nl<int8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int64_t>(nl<int8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int64_t>(nl<int8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int64_t>(nl<int8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<int64_t>(nl<int8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<uint64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(nl<uint64_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint64_t>(nl<int8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint64_t>(nl<int8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int8_t>(sc<uint64_t>(nl<int8_t>::max(), +1)) == false);
        }
    }

    {
        {
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<int8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<int8_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int8_t>(nl<uint8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int8_t>(nl<uint8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int8_t>(nl<uint8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<uint8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<uint8_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<int16_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<int16_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int16_t>(nl<uint8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int16_t>(nl<uint8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int16_t>(nl<uint8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int16_t>(nl<uint8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int16_t>(nl<uint8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int16_t>(nl<uint8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<uint16_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<uint16_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint16_t>(nl<uint8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint16_t>(nl<uint8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint16_t>(nl<uint8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint16_t>(nl<uint8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint16_t>(nl<uint8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint16_t>(nl<uint8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<int32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<int32_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int32_t>(nl<uint8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int32_t>(nl<uint8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int32_t>(nl<uint8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int32_t>(nl<uint8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int32_t>(nl<uint8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int32_t>(nl<uint8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<uint32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<uint32_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint32_t>(nl<uint8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint32_t>(nl<uint8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint32_t>(nl<uint8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint32_t>(nl<uint8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint32_t>(nl<uint8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint32_t>(nl<uint8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<int64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<int64_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int64_t>(nl<uint8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int64_t>(nl<uint8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int64_t>(nl<uint8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int64_t>(nl<uint8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int64_t>(nl<uint8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<int64_t>(nl<uint8_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<uint64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(nl<uint64_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint64_t>(nl<uint8_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint64_t>(nl<uint8_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint64_t>(nl<uint8_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint64_t>(nl<uint8_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint64_t>(nl<uint8_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint8_t>(sc<uint64_t>(nl<uint8_t>::lowest(), +1)) == true);
        }
    }

    {
        {
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<int8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<int8_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<uint8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<uint8_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<int16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<int16_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<uint16_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<uint16_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint16_t>(nl<int16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint16_t>(nl<int16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint16_t>(nl<int16_t>::max(), +1)) == false);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<int32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<int32_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int32_t>(nl<int16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int32_t>(nl<int16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int32_t>(nl<int16_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int32_t>(nl<int16_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int32_t>(nl<int16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int32_t>(nl<int16_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<uint32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<uint32_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint32_t>(nl<int16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint32_t>(nl<int16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint32_t>(nl<int16_t>::max(), +1)) == false);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<int64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<int64_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int64_t>(nl<int16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int64_t>(nl<int16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int64_t>(nl<int16_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int64_t>(nl<int16_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int64_t>(nl<int16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<int64_t>(nl<int16_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<uint64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(nl<uint64_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint64_t>(nl<int16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint64_t>(nl<int16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int16_t>(sc<uint64_t>(nl<int16_t>::max(), +1)) == false);
        }
    }

    {
        {
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<int8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<int8_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int8_t>(nl<uint16_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int8_t>(nl<uint16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int8_t>(nl<uint16_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<uint8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<uint8_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint8_t>(nl<uint16_t>::lowest(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint8_t>(nl<uint16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint8_t>(nl<uint16_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<int16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<int16_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int16_t>(nl<uint16_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int16_t>(nl<uint16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int16_t>(nl<uint16_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<uint16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<uint16_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<int32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<int32_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int32_t>(nl<uint16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int32_t>(nl<uint16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int32_t>(nl<uint16_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int32_t>(nl<uint16_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int32_t>(nl<uint16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int32_t>(nl<uint16_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<uint32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<uint32_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint32_t>(nl<uint16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint32_t>(nl<uint16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint32_t>(nl<uint16_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint32_t>(nl<uint16_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint32_t>(nl<uint16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint32_t>(nl<uint16_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<int64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<int64_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int64_t>(nl<uint16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int64_t>(nl<uint16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int64_t>(nl<uint16_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int64_t>(nl<uint16_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int64_t>(nl<uint16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<int64_t>(nl<uint16_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<uint64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(nl<uint64_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint64_t>(nl<uint16_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint64_t>(nl<uint16_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint64_t>(nl<uint16_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint64_t>(nl<uint16_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint64_t>(nl<uint16_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint16_t>(sc<uint64_t>(nl<uint16_t>::lowest(), +1)) == true);
        }
    }

    {
        {
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<int8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<int8_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<uint8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<uint8_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<int16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<int16_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<uint16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<uint16_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<int32_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<int32_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<uint32_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<uint32_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<uint32_t>(nl<int32_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<uint32_t>(nl<int32_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<uint32_t>(nl<int32_t>::max(), +1)) == false);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<int64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<int64_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<int64_t>(nl<int32_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<int64_t>(nl<int32_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<int64_t>(nl<int32_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<int64_t>(nl<int32_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<int64_t>(nl<int32_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<int64_t>(nl<int32_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<uint64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(nl<uint64_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<uint64_t>(nl<int32_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<uint64_t>(nl<int32_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int32_t>(sc<uint64_t>(nl<int32_t>::max(), +1)) == false);
        }
    }

    {
        {
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<int8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<int8_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int8_t>(nl<uint32_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int8_t>(nl<uint32_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int8_t>(nl<uint32_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<uint8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<uint8_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint8_t>(nl<uint32_t>::lowest(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint8_t>(nl<uint32_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint8_t>(nl<uint32_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<int16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<int16_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int16_t>(nl<uint32_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int16_t>(nl<uint32_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int16_t>(nl<uint32_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<uint16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<uint16_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint16_t>(nl<uint32_t>::lowest(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint16_t>(nl<uint32_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint16_t>(nl<uint32_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<int32_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<int32_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int32_t>(nl<uint32_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int32_t>(nl<uint32_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int32_t>(nl<uint32_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<uint32_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<uint32_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<int64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<int64_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int64_t>(nl<uint32_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int64_t>(nl<uint32_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int64_t>(nl<uint32_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int64_t>(nl<uint32_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int64_t>(nl<uint32_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<int64_t>(nl<uint32_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<uint64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(nl<uint64_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint64_t>(nl<uint32_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint64_t>(nl<uint32_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint64_t>(nl<uint32_t>::max(), +1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint64_t>(nl<uint32_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint64_t>(nl<uint32_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint32_t>(sc<uint64_t>(nl<uint32_t>::lowest(), +1)) == true);
        }
    }

    {
        {
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<int8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<int8_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<uint8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<uint8_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<int16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<int16_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<uint16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<uint16_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<int32_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<int32_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<uint32_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<uint32_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<int64_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<int64_t>::lowest()) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<uint64_t>::max()) == false);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(nl<uint64_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(sc<uint64_t>(nl<int64_t>::max(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(sc<uint64_t>(nl<int64_t>::max()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<int64_t>(sc<uint64_t>(nl<int64_t>::max(), +1)) == false);
        }
    }

    {
        {
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<int8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<int8_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int8_t>(nl<uint64_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int8_t>(nl<uint64_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int8_t>(nl<uint64_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<uint8_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<uint8_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint8_t>(nl<uint64_t>::lowest(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint8_t>(nl<uint64_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint8_t>(nl<uint64_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<int16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<int16_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int16_t>(nl<uint64_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int16_t>(nl<uint64_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int16_t>(nl<uint64_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<uint16_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<uint16_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint16_t>(nl<uint64_t>::lowest(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint16_t>(nl<uint64_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint16_t>(nl<uint64_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<int32_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<int32_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int32_t>(nl<uint64_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int32_t>(nl<uint64_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int32_t>(nl<uint64_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<uint32_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<uint32_t>::lowest()) == true);

            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint32_t>(nl<uint64_t>::lowest(), -1)) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint32_t>(nl<uint64_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<uint32_t>(nl<uint64_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<int64_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<int64_t>::lowest()) == false);

            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int64_t>(nl<uint64_t>::lowest(), -1)) == false);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int64_t>(nl<uint64_t>::lowest()))     == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(sc<int64_t>(nl<uint64_t>::lowest(), +1)) == true);
        }

        {
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<uint64_t>::max()) == true);
            BOOST_CHECK(comd::in_arithmetic_range<uint64_t>(nl<uint64_t>::lowest()) == true);
        }
    }
}


/*
 * Copyright (c) Koji Terada
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Koji Terada */

#ifndef TF2_EIGEN__TF2_EIGEN_HPP_
#define TF2_EIGEN__TF2_EIGEN_HPP_

#include <tf2/convert.h>
#include <tf2_ros/buffer_interface.h>
#include <Eigen/Geometry>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <tf2_ros/buffer.h>

namespace tf2
{

/** \brief Convert a timestamped transform to the equivalent Eigen data type.
 * \param t The transform to convert, as a geometry_msgs Transform message.
 * \return The transform message converted to an Eigen Isometry3d transform.
 */
inline
Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::Transform & t)
{
  return Eigen::Isometry3d(
    Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z) *
    Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z));
}

/** \brief Convert a timestamped transform to the equivalent Eigen data type.
 * \param t The transform to convert, as a geometry_msgs TransformedStamped message.
 * \return The transform message converted to an Eigen Isometry3d transform.
 */
inline
Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::TransformStamped & t)
{
  return transformToEigen(t.transform);
}

/** \brief Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type.
 * \param T The transform to convert, as an Eigen Affine3d transform.
 * \return The transform converted to a TransformStamped message.
 */
inline
geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Affine3d & T)
{
  geometry_msgs::msg::TransformStamped t;
  t.transform.translation.x = T.translation().x();
  t.transform.translation.y = T.translation().y();
  t.transform.translation.z = T.translation().z();

  Eigen::Quaterniond q(T.linear());  // assuming that upper 3x3 matrix is orthonormal
  t.transform.rotation.x = q.x();
  t.transform.rotation.y = q.y();
  t.transform.rotation.z = q.z();
  t.transform.rotation.w = q.w();

  return t;
}

/** \brief Convert an Eigen Isometry3d transform to the equivalent geometry_msgs message type.
 * \param T The transform to convert, as an Eigen Isometry3d transform.
 * \return The transform converted to a TransformStamped message.
 */
inline
geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isometry3d & T)
{
  geometry_msgs::msg::TransformStamped t;
  t.transform.translation.x = T.translation().x();
  t.transform.translation.y = T.translation().y();
  t.transform.translation.z = T.translation().z();

  Eigen::Quaterniond q(T.rotation());
  t.transform.rotation.x = q.x();
  t.transform.rotation.y = q.y();
  t.transform.rotation.z = q.z();
  t.transform.rotation.w = q.w();

  return t;
}

/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type.
 * This function is a specialization of the doTransform template defined in tf2/convert.h,
 * although it can not be used in tf2_ros::BufferInterface::transform because this
 * functions rely on the existence of a time stamp and a frame id in the type which should
 * get transformed.
 * \param t_in The vector to transform, as a Eigen Vector3d data type.
 * \param t_out The transformed vector, as a Eigen Vector3d data type.
 * \param transform The timestamped transform to apply, as a TransformStamped message.
 */
template<>
inline
void doTransform(
  const Eigen::Vector3d & t_in,
  Eigen::Vector3d & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out = Eigen::Vector3d(transformToEigen(transform) * t_in);
}

/** \brief Convert a Eigen Vector3d type to a Point message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The timestamped Eigen Vector3d to convert.
 * \return The vector converted to a Point message.
 */
inline
geometry_msgs::msg::Point toMsg(const Eigen::Vector3d & in)
{
  geometry_msgs::msg::Point msg;
  msg.x = in.x();
  msg.y = in.y();
  msg.z = in.z();
  return msg;
}

/** \brief Convert a Point message type to a Eigen-specific Vector3d type.
 * This function is a specialization of the fromMsg template defined in tf2/convert.h
 * \param msg The Point message to convert.
 * \param out The point converted to a Eigen Vector3d.
 */
inline
void fromMsg(const geometry_msgs::msg::Point & msg, Eigen::Vector3d & out)
{
  out.x() = msg.x;
  out.y() = msg.y;
  out.z() = msg.z;
}

/** \brief Convert an Eigen Vector3d type to a Vector3 message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The Eigen Vector3d to convert.
 * \return The vector converted to a Vector3 message.
 */
inline
geometry_msgs::msg::Vector3 & toMsg(const Eigen::Vector3d & in, geometry_msgs::msg::Vector3 & out)
{
  out.x = in.x();
  out.y = in.y();
  out.z = in.z();
  return out;
}

/** \brief Convert a Vector3 message type to a Eigen-specific Vector3d type.
 * This function is a specialization of the fromMsg template defined in tf2/convert.h
 * \param msg The Vector3 message to convert.
 * \param out The vector converted to a Eigen Vector3d.
 */
inline
void fromMsg(const geometry_msgs::msg::Vector3 & msg, Eigen::Vector3d & out)
{
  out.x() = msg.x;
  out.y() = msg.y;
  out.z() = msg.z;
}

/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type.
 * This function is a specialization of the doTransform template defined in tf2/convert.h.
 * \param t_in The vector to transform, as a timestamped Eigen Vector3d data type.
 * \param t_out The transformed vector, as a timestamped Eigen Vector3d data type.
 * \param transform The timestamped transform to apply, as a TransformStamped message.
 */
template<>
inline
void doTransform(
  const tf2::Stamped<Eigen::Vector3d> & t_in,
  tf2::Stamped<Eigen::Vector3d> & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out = tf2::Stamped<Eigen::Vector3d>(
    transformToEigen(transform) * t_in,
    tf2_ros::fromMsg(transform.header.stamp),
    transform.header.frame_id);
}

/** \brief Convert a stamped Eigen Vector3d type to a PointStamped message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The timestamped Eigen Vector3d to convert.
 * \return The vector converted to a PointStamped message.
 */
inline
geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<Eigen::Vector3d> & in)
{
  geometry_msgs::msg::PointStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.point = toMsg(static_cast<const Eigen::Vector3d &>(in));
  return msg;
}

/** \brief Convert a PointStamped message type to a stamped Eigen-specific Vector3d type.
 * This function is a specialization of the fromMsg template defined in tf2/convert.h
 * \param msg The PointStamped message to convert.
 * \param out The point converted to a timestamped Eigen Vector3d.
 */
inline
void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped<Eigen::Vector3d> & out)
{
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  out.frame_id_ = msg.header.frame_id;
  fromMsg(msg.point, static_cast<Eigen::Vector3d &>(out));
}

/** \brief Apply a geometry_msgs Transform to an Eigen Affine3d transform.
 * This function is a specialization of the doTransform template defined in tf2/convert.h,
 * although it can not be used in tf2_ros::BufferInterface::transform because this
 * function relies on the existence of a time stamp and a frame id in the type which should
 * get transformed.
 * \param t_in The frame to transform, as a Eigen Affine3d transform.
 * \param t_out The transformed frame, as a Eigen Affine3d transform.
 * \param transform The timestamped transform to apply, as a TransformStamped message.
 */
template<>
inline
void doTransform(
  const Eigen::Affine3d & t_in,
  Eigen::Affine3d & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out = Eigen::Affine3d(transformToEigen(transform) * t_in);
}

template<>
inline
void doTransform(
  const Eigen::Isometry3d & t_in,
  Eigen::Isometry3d & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out = Eigen::Isometry3d(transformToEigen(transform) * t_in);
}

/** \brief Convert a Eigen Quaterniond type to a Quaternion message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The Eigen Quaterniond to convert.
 * \return The quaternion converted to a Quaterion message.
 */
inline
geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond & in)
{
  geometry_msgs::msg::Quaternion msg;
  msg.w = in.w();
  msg.x = in.x();
  msg.y = in.y();
  msg.z = in.z();
  return msg;
}

/** \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type.
 * This function is a specialization of the fromMsg template defined in tf2/convert.h
 * \param msg The Quaternion message to convert.
 * \param out The quaternion converted to a Eigen Quaterniond.
 */
inline
void fromMsg(const geometry_msgs::msg::Quaternion & msg, Eigen::Quaterniond & out)
{
  out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z);
}

/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type.
 * This function is a specialization of the doTransform template defined in tf2/convert.h,
 * although it can not be used in tf2_ros::BufferInterface::transform because this
 * functions rely on the existence of a time stamp and a frame id in the type which should
 * get transformed.
 * \param t_in The vector to transform, as a Eigen Quaterniond data type.
 * \param t_out The transformed vector, as a Eigen Quaterniond data type.
 * \param transform The timestamped transform to apply, as a TransformStamped message.
 */
template<>
inline
void doTransform(
  const Eigen::Quaterniond & t_in,
  Eigen::Quaterniond & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  Eigen::Quaterniond t;
  fromMsg(transform.transform.rotation, t);
  t_out = t * t_in;
}

/** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The timestamped Eigen Quaterniond to convert.
 * \return The quaternion converted to a QuaternionStamped message.
 */
inline
geometry_msgs::msg::QuaternionStamped toMsg(const Stamped<Eigen::Quaterniond> & in)
{
  geometry_msgs::msg::QuaternionStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.quaternion = toMsg(static_cast<const Eigen::Quaterniond &>(in));
  return msg;
}

/** \brief Convert a QuaternionStamped message type to a stamped Eigen-specific Quaterniond type.
 * This function is a specialization of the fromMsg template defined in tf2/convert.h
 * \param msg The QuaternionStamped message to convert.
 * \param out The quaternion converted to a timestamped Eigen Quaterniond.
 */
inline
void fromMsg(const geometry_msgs::msg::QuaternionStamped & msg, Stamped<Eigen::Quaterniond> & out)
{
  out.frame_id_ = msg.header.frame_id;
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  fromMsg(msg.quaternion, static_cast<Eigen::Quaterniond &>(out));
}

/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type.
 * This function is a specialization of the doTransform template defined in tf2/convert.h.
 * \param t_in The vector to transform, as a timestamped Eigen Quaterniond data type.
 * \param t_out The transformed vector, as a timestamped Eigen Quaterniond data type.
 * \param transform The timestamped transform to apply, as a TransformStamped message.
 */
template<>
inline
void doTransform(
  const tf2::Stamped<Eigen::Quaterniond> & t_in,
  tf2::Stamped<Eigen::Quaterniond> & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out.frame_id_ = transform.header.frame_id;
  t_out.stamp_ = tf2_ros::fromMsg(transform.header.stamp);
  doTransform(
    static_cast<const Eigen::Quaterniond &>(t_in),
    static_cast<Eigen::Quaterniond &>(t_out), transform);
}

/** \brief Convert a Eigen Affine3d transform type to a Pose message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The Eigen Affine3d to convert.
 * \return The Eigen transform converted to a Pose message.
 */
inline
geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d & in)
{
  geometry_msgs::msg::Pose msg;
  msg.position.x = in.translation().x();
  msg.position.y = in.translation().y();
  msg.position.z = in.translation().z();
  Eigen::Quaterniond q(in.linear());
  msg.orientation.x = q.x();
  msg.orientation.y = q.y();
  msg.orientation.z = q.z();
  msg.orientation.w = q.w();
  return msg;
}

/** \brief Convert a Eigen Isometry3d transform type to a Pose message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The Eigen Isometry3d to convert.
 * \return The Eigen transform converted to a Pose message.
 */
inline
geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d & in)
{
  geometry_msgs::msg::Pose msg;
  msg.position.x = in.translation().x();
  msg.position.y = in.translation().y();
  msg.position.z = in.translation().z();
  Eigen::Quaterniond q(in.linear());
  msg.orientation.x = q.x();
  msg.orientation.y = q.y();
  msg.orientation.z = q.z();
  msg.orientation.w = q.w();
  return msg;
}

/** \brief Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * The function was renamed to toMsg2 to avoid overloading conflicts with definitions above.
 *
 * \param in The Eigen::Vector3d to convert.
 * \return The Eigen::Vector3d converted to a geometry_msgs::msg::Vector3 message.
 */
inline
geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in)
{
  geometry_msgs::msg::Vector3 msg;
  msg.x = in(0);
  msg.y = in(1);
  msg.z = in(2);
  return msg;
}

/** \brief Convert a Pose message transform type to a Eigen Affine3d.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param msg The Pose message to convert.
 * \param out The pose converted to a Eigen Affine3d.
 */
inline
void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Affine3d & out)
{
  out = Eigen::Affine3d(
    Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
    Eigen::Quaterniond(
      msg.orientation.w,
      msg.orientation.x,
      msg.orientation.y,
      msg.orientation.z));
}

/** \brief Convert a Pose message transform type to a Eigen Isometry3d.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param msg The Pose message to convert.
 * \param out The pose converted to a Eigen Isometry3d.
 */
inline
void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Isometry3d & out)
{
  out = Eigen::Isometry3d(
    Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
    Eigen::Quaterniond(
      msg.orientation.w,
      msg.orientation.x,
      msg.orientation.y,
      msg.orientation.z));
}

/** \brief Convert a Eigen 6x1 Matrix type to a Twist message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The 6x1 Eigen Matrix to convert.
 * \return The Eigen Matrix converted to a Twist message.
 */
inline
geometry_msgs::msg::Twist toMsg(const Eigen::Matrix<double, 6, 1> & in)
{
  geometry_msgs::msg::Twist msg;
  msg.linear.x = in[0];
  msg.linear.y = in[1];
  msg.linear.z = in[2];
  msg.angular.x = in[3];
  msg.angular.y = in[4];
  msg.angular.z = in[5];
  return msg;
}

/** \brief Convert a Twist message transform type to a Eigen 6x1 Matrix.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param msg The Twist message to convert.
 * \param out The twist converted to a Eigen 6x1 Matrix.
 */
inline
void fromMsg(const geometry_msgs::msg::Twist & msg, Eigen::Matrix<double, 6, 1> & out)
{
  out[0] = msg.linear.x;
  out[1] = msg.linear.y;
  out[2] = msg.linear.z;
  out[3] = msg.angular.x;
  out[4] = msg.angular.y;
  out[5] = msg.angular.z;
}

/** \brief Apply a geometry_msgs TransformStamped to an Eigen Affine3d transform.
 * This function is a specialization of the doTransform template defined in tf2/convert.h,
 * although it can not be used in tf2_ros::BufferInterface::transform because this
 * function relies on the existence of a time stamp and a frame id in the type which should
 * get transformed.
 * \param t_in The frame to transform, as a timestamped Eigen Affine3d transform.
 * \param t_out The transformed frame, as a timestamped Eigen Affine3d transform.
 * \param transform The timestamped transform to apply, as a TransformStamped message.
 */
template<>
inline
void doTransform(
  const tf2::Stamped<Eigen::Affine3d> & t_in,
  tf2::Stamped<Eigen::Affine3d> & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out = tf2::Stamped<Eigen::Affine3d>(
    transformToEigen(transform) * t_in, tf2_ros::fromMsg(
      transform.header.stamp), transform.header.frame_id);
}

/** \brief Apply a geometry_msgs TransformStamped to an Eigen Isometry transform.
 * This function is a specialization of the doTransform template defined in tf2/convert.h,
 * although it can not be used in tf2_ros::BufferInterface::transform because this
 * function relies on the existence of a time stamp and a frame id in the type which should
 * get transformed.
 * \param t_in The frame to transform, as a timestamped Eigen Isometry transform.
 * \param t_out The transformed frame, as a timestamped Eigen Isometry transform.
 * \param transform The timestamped transform to apply, as a TransformStamped message.
 */
template<>
inline
void doTransform(
  const tf2::Stamped<Eigen::Isometry3d> & t_in,
  tf2::Stamped<Eigen::Isometry3d> & t_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  t_out = tf2::Stamped<Eigen::Isometry3d>(
    transformToEigen(transform) * t_in, tf2_ros::fromMsg(
      transform.header.stamp), transform.header.frame_id);
}

/** \brief Convert a stamped Eigen Affine3d transform type to a Pose message.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param in The timestamped Eigen Affine3d to convert.
 * \return The Eigen transform converted to a PoseStamped message.
 */
inline
geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped<Eigen::Affine3d> & in)
{
  geometry_msgs::msg::PoseStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.pose = toMsg(static_cast<const Eigen::Affine3d &>(in));
  return msg;
}

inline
geometry_msgs::msg::PoseStamped toMsg(const tf2::Stamped<Eigen::Isometry3d> & in)
{
  geometry_msgs::msg::PoseStamped msg;
  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
  msg.header.frame_id = in.frame_id_;
  msg.pose = toMsg(static_cast<const Eigen::Isometry3d &>(in));
  return msg;
}

/** \brief Convert a Pose message transform type to a stamped Eigen Affine3d.
 * This function is a specialization of the toMsg template defined in tf2/convert.h.
 * \param msg The PoseStamped message to convert.
 * \param out The pose converted to a timestamped Eigen Affine3d.
 */
inline
void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<Eigen::Affine3d> & out)
{
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  out.frame_id_ = msg.header.frame_id;
  fromMsg(msg.pose, static_cast<Eigen::Affine3d &>(out));
}

inline
void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<Eigen::Isometry3d> & out)
{
  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
  out.frame_id_ = msg.header.frame_id;
  fromMsg(msg.pose, static_cast<Eigen::Isometry3d &>(out));
}

}  // namespace tf2

#ifndef _MSC_VER
namespace Eigen
{
// This is needed to make the usage of the following conversion functions usable in tf2::convert().
// According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or
// in an associated namespace of one of its arguments. The stamped versions of this conversion
// functions work because they have tf2::Stamped as an argument which is the same namespace as
// which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is
// defined in tf2, so it take the following definitions in Eigen namespace to make them usable in
// tf2::convert().

inline
geometry_msgs::msg::Pose toMsg(const Eigen::Affine3d & in)
{
  return tf2::toMsg(in);
}

inline
geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d & in)
{
  return tf2::toMsg(in);
}

inline
void fromMsg(const geometry_msgs::msg::Point & msg, Eigen::Vector3d & out)
{
  tf2::fromMsg(msg, out);
}

inline
geometry_msgs::msg::Point toMsg(const Eigen::Vector3d & in)
{
  return tf2::toMsg(in);
}

inline
void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Affine3d & out)
{
  tf2::fromMsg(msg, out);
}

inline
void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Isometry3d & out)
{
  tf2::fromMsg(msg, out);
}

inline
geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond & in)
{
  return tf2::toMsg(in);
}

inline
geometry_msgs::msg::Twist toMsg(const Eigen::Matrix<double, 6, 1> & in)
{
  return tf2::toMsg(in);
}

inline
void fromMsg(const geometry_msgs::msg::Twist & msg, Eigen::Matrix<double, 6, 1> & out)
{
  tf2::fromMsg(msg, out);
}

}  // namespace Eigen
#endif

#endif  // TF2_EIGEN__TF2_EIGEN_HPP_


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/algorithm/reverse.hpp>
#include <boost/compute/algorithm/reverse_copy.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>

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

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(reverse_int)
{
    bc::vector<int> vector(5);
    bc::iota(vector.begin(), vector.end(), 0);
    CHECK_RANGE_EQUAL(int, 5, vector, (0, 1, 2, 3, 4));

    bc::reverse(vector.begin(), vector.end());
    CHECK_RANGE_EQUAL(int, 5, vector, (4, 3, 2, 1, 0));

    bc::reverse(vector.begin() + 1, vector.end());
    CHECK_RANGE_EQUAL(int, 5, vector, (4, 0, 1, 2, 3));

    bc::reverse(vector.begin() + 1, vector.end() - 1);
    CHECK_RANGE_EQUAL(int, 5, vector, (4, 2, 1, 0, 3));

    bc::reverse(vector.begin(), vector.end() - 2);
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 4, 0, 3));

    vector.resize(6);
    bc::iota(vector.begin(), vector.end(), 10);
    CHECK_RANGE_EQUAL(int, 6, vector, (10, 11, 12, 13, 14, 15));

    bc::reverse(vector.begin(), vector.end());
    CHECK_RANGE_EQUAL(int, 6, vector, (15, 14, 13, 12, 11, 10));

    bc::reverse(vector.begin() + 3, vector.end());
    CHECK_RANGE_EQUAL(int, 6, vector, (15, 14, 13, 10, 11, 12));

    bc::reverse(vector.begin() + 1, vector.end() - 2);
    CHECK_RANGE_EQUAL(int, 6, vector, (15, 10, 13, 14, 11, 12));
}

BOOST_AUTO_TEST_CASE(reverse_copy_int)
{
    bc::vector<int> a(5);
    bc::iota(a.begin(), a.end(), 0);
    CHECK_RANGE_EQUAL(int, 5, a, (0, 1, 2, 3, 4));

    bc::vector<int> b(5);
    bc::vector<int>::iterator iter =
        bc::reverse_copy(a.begin(), a.end(), b.begin());
    BOOST_CHECK(iter == b.end());
    CHECK_RANGE_EQUAL(int, 5, b, (4, 3, 2, 1, 0));

    iter = bc::reverse_copy(b.begin() + 1, b.end(), a.begin() + 1);
    BOOST_CHECK(iter == a.end());
    CHECK_RANGE_EQUAL(int, 5, a, (0, 0, 1, 2, 3));

    iter = bc::reverse_copy(a.begin(), a.end() - 1, b.begin());
    BOOST_CHECK(iter == (b.end() - 1));
    CHECK_RANGE_EQUAL(int, 5, b, (2, 1, 0, 0, 0));
}

BOOST_AUTO_TEST_CASE(reverse_copy_counting_iterator)
{
    bc::vector<int> vector(5);
    bc::reverse_copy(
        bc::make_counting_iterator(1),
        bc::make_counting_iterator(6),
        vector.begin()
    );
    CHECK_RANGE_EQUAL(int, 5, vector, (5, 4, 3, 2, 1));
}

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::uint8_t, 5, Eigen::Dynamic> test_uchar_5_D(const Eigen::Matrix<boost::uint8_t, 5, Eigen::Dynamic> & M)
{
	return M;
}
void export_uchar_5_D()
{
	boost::python::def("test_uchar_5_D",test_uchar_5_D);
}


// Software License Agreement (BSD-3-Clause)
//
// Copyright 2018 The University of North Carolina at Chapel Hill
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.

//! @author William Lee

#ifndef LINK_MANIPULATOR_SCENARIO
#define LINK_MANIPULATOR_SCENARIO

#include "shape_hierarchy.hpp"
#include <mpt/lp_space.hpp>
#include <mpt/so2_space.hpp>
#include <mpt/box_bounds.hpp>
#include <mpt/goal_state.hpp>
#include <Eigen/Dense>
#include <vector>
#include <cmath>

using namespace shape;

template <class T> constexpr T PI = T(3.14159265358979323846264338327950288419716939937510582097494459230781640628620L);

namespace mpt_demo
{
    template <typename Scalar, int dimensions>
    class LinkManipulatorScenario
    {
        static_assert(dimensions > 0, "There must be at least one arm");
    public:
        using Space = unc::robotics::mpt::L1Space<Scalar, dimensions>;
        using Bounds = unc::robotics::mpt::BoxBounds<Scalar, dimensions>;
        using State = typename Space::Type;
        using Distance = typename Space::Distance;
        using Goal = unc::robotics::mpt::GoalState<Space>;
        using Point = Eigen::Matrix<Scalar, 2, 1>;

    private:
        Space space_;
        Bounds bounds_;
        Goal goal_;
        std::vector<Circle<Scalar>> circles_;
        std::vector<Scalar> armLengths_;
        Scalar radius_;

    public:
        LinkManipulatorScenario(
            State goalState,
            std::vector<Circle<Scalar>> &circles,
            std::vector<Scalar> &armLengths,
            Scalar radius
        )
            :
            bounds_(makeBounds()),
            goal_(1e-6, goalState),
            circles_(circles),
            armLengths_(armLengths),
            radius_(radius)
        {
        }

        static Bounds makeBounds()
        {
            State min, max;
            min.fill(-PI<Scalar>);
            max.fill(PI<Scalar>);
            return Bounds(min, max);
        }

        bool valid(const State &q) const
        {
            Point from(0, 0);
            Point to;
            Scalar angle = 0.0;
            for (int i = 0; i < dimensions; i++)
            {
                // compute the two ends of each segment
                angle += q[i];
                to = from + armLengths_[i] * Point(cos(angle), sin(angle));
                // check if the segment(from, to) collides with any circle
                for (const auto &c : circles_)
                    if (!c.segmentIsValid(from, to, radius_))
                        return false;
                from = to;
            }
            return true;
        }

        bool link(const State &a, const State &b) const
        {
            if (!valid(a) || !valid(b))
                return false;
            return bisectLink(a, b);
        }

        bool bisectLink(const State &a, const State &b) const
        {
            State diff = a - b;
            Scalar maxAngleDiff = diff.template lpNorm<Eigen::Infinity>();
            constexpr Scalar tolerance = 0.02; // 0.02 rad ~= 1 degree
            if (maxAngleDiff < tolerance)
                return true;
            State mid = (a + b) / 2;
            if (!valid(mid))
                return false;
            if (!bisectLink(a, mid))
                return false;
            return bisectLink(mid, b);
        }

        const Space &space() const
        {
            return space_;
        }

        const Bounds &bounds() const
        {
            return bounds_;
        }

        const Goal &goal() const
        {
            return goal_;
        }

    };
}
#endif


///////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::meta::is_pseudo_scalar_distribution.hpp             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_META_IS_PSEUDO_SCALAR_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_META_IS_PSEUDO_SCALAR_DISTRIBUTION_HPP_ER_2009
#include <boost/statistics/detail/distribution_toolkit/meta/is_scalar_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

namespace meta{

    // The mapping of a distribution's pdf is not a distribution, but for
    // practical purposes, it may share its interface such as pdf(dist,x)
    template<typename D>
    struct is_pseudo_scalar_distribution
        : is_scalar_distribution<D> {};

}// meta
}// distribution
}// toolkit

}// detail
}// statistics
}// boost

#endif


#define BOOST_TEST_MODULE TestGeom
#include <boost/test/included/unit_test.hpp>
#include "db.h"
#include "helper.cpp"
#include <iostream>
using namespace odb;

BOOST_AUTO_TEST_SUITE( test_suite )

BOOST_AUTO_TEST_CASE( test_oct )
{
    Oct* oct = new Oct;
    oct->init(Point(0,0),Point(400,400),40);
    BOOST_TEST((oct->getCenterHigh() == Point(400,400)));
    BOOST_TEST((oct->getCenterLow() == Point(0,0)));
    BOOST_TEST(oct->getWidth()==40);
    BOOST_TEST(oct->xMin()==-20);
    BOOST_TEST(oct->xMax()==420);
    BOOST_TEST(oct->yMin()==-20);
    BOOST_TEST(oct->yMax()==420);
    BOOST_TEST(oct->dx()==440);
    BOOST_TEST(oct->dy()==440);

    BOOST_TEST(oct->getDir() == Oct::OCT_DIR::RIGHT);
    oct->init(Point(0,0),Point(-400,400),40);
    BOOST_TEST(oct->getDir() == Oct::OCT_DIR::LEFT);
    oct->init(Point(0,0),Point(-400,-400),40);
    BOOST_TEST(oct->getDir() == Oct::OCT_DIR::RIGHT);
    oct->init(Point(0,0),Point(400,-400),40);
    BOOST_TEST(oct->getDir() == Oct::OCT_DIR::LEFT);
}
BOOST_AUTO_TEST_CASE( test_geom_shape )
{
    Oct oct(Point(0,0),Point(400,400),40);
    GeomShape* shape = &oct;
    BOOST_TEST(shape->xMin()==-20);
    BOOST_TEST(shape->xMax()==420);
    BOOST_TEST(shape->yMin()==-20);
    BOOST_TEST(shape->yMax()==420);
    BOOST_TEST(shape->dx()==440);
    BOOST_TEST(shape->dy()==440);
    //OCT POINTS
    std::vector<Point> points = shape->getPoints();
    BOOST_TEST(points.size()==9);
    BOOST_TEST((points[0] == Point(-9,-20)));
    BOOST_TEST((points[1] == Point(9,-20)));
    BOOST_TEST((points[2] == Point(420,391)));
    BOOST_TEST((points[3] == Point(420,409)));
    BOOST_TEST((points[4] == Point(409,420)));
    BOOST_TEST((points[5] == Point(391,420)));
    BOOST_TEST((points[6] == Point(-20,9)));
    BOOST_TEST((points[7] == Point(-20,-9)));
    BOOST_TEST((points[8] == Point(-9,-20)));

    //RECT
    Rect rect(Point(0,0),Point(400,400));
    shape = ▭
    BOOST_TEST(shape->xMin()==0);
    BOOST_TEST(shape->xMax()==400);
    BOOST_TEST(shape->yMin()==0);
    BOOST_TEST(shape->yMax()==400);
    BOOST_TEST(shape->dx()==400);
    BOOST_TEST(shape->dy()==400);
    //RECT POINTS
    points = shape->getPoints();
    BOOST_TEST(points.size()==5);
    BOOST_TEST((points[0] == Point(0,0)));
    BOOST_TEST((points[1] == Point(400,0)));
    BOOST_TEST((points[2] == Point(400,400)));
    BOOST_TEST((points[3] == Point(0,400)));
    BOOST_TEST((points[4] == Point(0,0)));

}
BOOST_AUTO_TEST_CASE( test_rect_merge )
{
    Rect rect(Point(0,0),Point(100,50));
    Oct oct(Point(100,50),Point(200,200),80);
    rect.merge((GeomShape*)&oct);
    BOOST_TEST(rect.xMin()==0);
    BOOST_TEST(rect.xMax()==240);
    BOOST_TEST(rect.yMin()==0);
    BOOST_TEST(rect.yMax()==240);
    BOOST_TEST(rect.dx()==240);
    BOOST_TEST(rect.dy()==240);
}
BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "boost::simd::constants digits"

#include <boost/simd/include/constants/digits.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <boost/simd/sdk/simd/native.hpp>

#ifdef BOOST_MSVC
  #pragma warning(disable: 4309) // truncation of constant value
#endif

////////////////////////////////////////////////////////////////////////////////
// Test value of digit constant for every base types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL( digit_value, BOOST_SIMD_SIMD_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>             dst_t;

  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<dst_t>::value;++i)
  {
    NT2_TEST_EQUAL( (boost::simd::Mten<dst_t>())[i], static_cast<T>(  -10 ) );
    NT2_TEST_EQUAL( (boost::simd::Mnine<dst_t>())[i], static_cast<T>( -9 ) );
    NT2_TEST_EQUAL( (boost::simd::Meight<dst_t>())[i], static_cast<T>( -8 ) );
    NT2_TEST_EQUAL( (boost::simd::Mseven<dst_t>())[i], static_cast<T>( -7 ) );
    NT2_TEST_EQUAL( (boost::simd::Msix<dst_t>())[i], static_cast<T>( -6 ) );
    NT2_TEST_EQUAL( (boost::simd::Mfive<dst_t>())[i], static_cast<T>( -5 ) );
    NT2_TEST_EQUAL( (boost::simd::Mfour<dst_t>())[i], static_cast<T>( -4 ) );
    NT2_TEST_EQUAL( (boost::simd::Mthree<dst_t>())[i], static_cast<T>( -3 ) );
    NT2_TEST_EQUAL( (boost::simd::Mtwo<dst_t>())[i], static_cast<T>( -2 ) );
    NT2_TEST_EQUAL( (boost::simd::Mone<dst_t>())[i], static_cast<T>( -1 ) );
    NT2_TEST_EQUAL( (boost::simd::Zero<dst_t>())[i], static_cast<T>(  0 ) );
    NT2_TEST_EQUAL( (boost::simd::Ten<dst_t>())[i], static_cast<T>(  10 ) );
    NT2_TEST_EQUAL( (boost::simd::Nine<dst_t>())[i], static_cast<T>( 9 ) );
    NT2_TEST_EQUAL( (boost::simd::Eight<dst_t>())[i], static_cast<T>( 8 ) );
    NT2_TEST_EQUAL( (boost::simd::Seven<dst_t>())[i], static_cast<T>( 7 ) );
    NT2_TEST_EQUAL( (boost::simd::Six<dst_t>())[i], static_cast<T>( 6 ) );
    NT2_TEST_EQUAL( (boost::simd::Five<dst_t>())[i], static_cast<T>( 5 ) );
    NT2_TEST_EQUAL( (boost::simd::Four<dst_t>())[i], static_cast<T>( 4 ) );
    NT2_TEST_EQUAL( (boost::simd::Three<dst_t>())[i], static_cast<T>( 3 ) );
    NT2_TEST_EQUAL( (boost::simd::Two<dst_t>())[i], static_cast<T>( 2 ) );
    NT2_TEST_EQUAL( (boost::simd::One<dst_t>())[i], static_cast<T>( 1 ) );
  }
}

////////////////////////////////////////////////////////////////////////////////
// Test value of digit constant for raw char type
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE( char_digit_value )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<char,ext_t>             dst_t;

  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<dst_t>::value;++i)
  {
    NT2_TEST_EQUAL( (boost::simd::Mten<dst_t>())[i], static_cast<char>(  -10 ) );
    NT2_TEST_EQUAL( (boost::simd::Mnine<dst_t>())[i], static_cast<char>( -9 ) );
    NT2_TEST_EQUAL( (boost::simd::Meight<dst_t>())[i], static_cast<char>( -8 ) );
    NT2_TEST_EQUAL( (boost::simd::Mseven<dst_t>())[i], static_cast<char>( -7 ) );
    NT2_TEST_EQUAL( (boost::simd::Msix<dst_t>())[i], static_cast<char>( -6 ) );
    NT2_TEST_EQUAL( (boost::simd::Mfive<dst_t>())[i], static_cast<char>( -5 ) );
    NT2_TEST_EQUAL( (boost::simd::Mfour<dst_t>())[i], static_cast<char>( -4 ) );
    NT2_TEST_EQUAL( (boost::simd::Mthree<dst_t>())[i], static_cast<char>( -3 ) );
    NT2_TEST_EQUAL( (boost::simd::Mtwo<dst_t>())[i], static_cast<char>( -2 ) );
    NT2_TEST_EQUAL( (boost::simd::Mone<dst_t>())[i], static_cast<char>( -1 ) );
    NT2_TEST_EQUAL( (boost::simd::Zero<dst_t>())[i], static_cast<char>(  0 ) );
    NT2_TEST_EQUAL( (boost::simd::Ten<dst_t>())[i], static_cast<char>(  10 ) );
    NT2_TEST_EQUAL( (boost::simd::Nine<dst_t>())[i], static_cast<char>( 9 ) );
    NT2_TEST_EQUAL( (boost::simd::Eight<dst_t>())[i], static_cast<char>( 8 ) );
    NT2_TEST_EQUAL( (boost::simd::Seven<dst_t>())[i], static_cast<char>( 7 ) );
    NT2_TEST_EQUAL( (boost::simd::Six<dst_t>())[i], static_cast<char>( 6 ) );
    NT2_TEST_EQUAL( (boost::simd::Five<dst_t>())[i], static_cast<char>( 5 ) );
    NT2_TEST_EQUAL( (boost::simd::Four<dst_t>())[i], static_cast<char>( 4 ) );
    NT2_TEST_EQUAL( (boost::simd::Three<dst_t>())[i], static_cast<char>( 3 ) );
    NT2_TEST_EQUAL( (boost::simd::Two<dst_t>())[i], static_cast<char>( 2 ) );
    NT2_TEST_EQUAL( (boost::simd::One<dst_t>())[i], static_cast<char>( 1 ) );
  }
}

////////////////////////////////////////////////////////////////////////////////
// Test integral_constant for every base types
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL( integral_constant, BOOST_SIMD_SIMD_INTEGRAL_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>             dst_t;

  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (boost::simd::integral_constant<dst_t,42>())[i], static_cast<T>(42) );
}

////////////////////////////////////////////////////////////////////////////////
// Test integral_constant for raw 'char' type
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE( char_integral_constant )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<char,ext_t>             dst_t;

  for(std::size_t i=0; i< boost::simd::meta::cardinal_of<dst_t>::value;++i)
    NT2_TEST_EQUAL( (boost::simd::integral_constant<dst_t,65>())[i], 'A' );
}


//==================================================================================================
/*!
  @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_SUCCESSOR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SUCCESSOR_HPP_INCLUDED

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

  /*!
    @ingroup group-ieee
    This function object returns the `n`-th least element strictly greater than the parameter

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

    @par Note

     If @c n is null returns @c x else computes the `n`-th least representable value strictly greater
     than x in its type. @c n must be positive or null.

      - For integer it saturate at @ref Valmax.
      - For floating point numbers, all @ref Inf  strict successors are @ref Nan.

    @par Example:

      @snippet successor.cpp successor

    @par Possible output:

      @snippet successor.txt successor

  **/
  ///@{
  Value successor(Value const& x);
  Value successor(Value const& x, IntegerValue const& n);
  ///@}
} }
#endif

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

#endif