///////////////////////////////////////////////////////////////////////////////
//  Copyright 2013 John Maddock
//  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_MATH_BERNOULLI_DETAIL_HPP_
#define BOOST_MATH_BERNOULLI_DETAIL_HPP_

#include "../math_fwd.hpp"
#include <boost/config.hpp>
#include <boost/detail/lightweight_mutex.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/math/tools/toms748_solve.hpp>

#ifdef BOOST_HAS_THREADS

#ifndef BOOST_NO_CXX11_HDR_ATOMIC
#  include <atomic>
#  define BOOST_MATH_ATOMIC_NS std
#if ATOMIC_INT_LOCK_FREE == 2
typedef std::atomic<int> atomic_counter_type;
typedef int atomic_integer_type;
#elif ATOMIC_SHORT_LOCK_FREE == 2
typedef std::atomic<short> atomic_counter_type;
typedef short atomic_integer_type;
#elif ATOMIC_LONG_LOCK_FREE == 2
typedef std::atomic<long> atomic_counter_type;
typedef long atomic_integer_type;
#elif ATOMIC_LLONG_LOCK_FREE == 2
typedef std::atomic<long long> atomic_counter_type;
typedef long long atomic_integer_type;
#else
#  define BOOST_MATH_NO_ATOMIC_INT
#endif

#else // BOOST_NO_CXX11_HDR_ATOMIC
//
// We need Boost.Atomic, but on any platform that supports auto-linking we do
// not need to link against a separate library:
//
#define BOOST_ATOMIC_NO_LIB
#include <boost/atomic.hpp>
#  define BOOST_MATH_ATOMIC_NS boost

namespace boost{ namespace math{ namespace detail{

//
// We need a type to use as an atomic counter:
//
#if BOOST_ATOMIC_INT_LOCK_FREE == 2
typedef boost::atomic<int> atomic_counter_type;
typedef int atomic_integer_type;
#elif BOOST_ATOMIC_SHORT_LOCK_FREE == 2
typedef boost::atomic<short> atomic_counter_type;
typedef short atomic_integer_type;
#elif BOOST_ATOMIC_LONG_LOCK_FREE == 2
typedef boost::atomic<long> atomic_counter_type;
typedef long atomic_integer_type;
#elif BOOST_ATOMIC_LLONG_LOCK_FREE == 2
typedef boost::atomic<long long> atomic_counter_type;
typedef long long atomic_integer_type;
#else
#  define BOOST_MATH_NO_ATOMIC_INT
#endif

}}} // namespaces

#endif  // BOOST_NO_CXX11_HDR_ATOMIC

#endif // BOOST_HAS_THREADS

namespace boost{ namespace math{ namespace detail{
//
// Asymptotic expansion for B2n due to
// Luschny LogB3 formula (http://www.luschny.de/math/primes/bernincl.html)
//
template <class T, class Policy>
T b2n_asymptotic(int n)
{
   BOOST_MATH_STD_USING
   const T nx = static_cast<T>(n);
   const T nx2(nx * nx);

   const T approximate_log_of_bernoulli_bn =
        ((boost::math::constants::half<T>() + nx) * log(nx))
        + ((boost::math::constants::half<T>() - nx) * log(boost::math::constants::pi<T>()))
        + (((T(3) / 2) - nx) * boost::math::constants::ln_two<T>())
        + ((nx * (T(2) - (nx2 * 7) * (1 + ((nx2 * 30) * ((nx2 * 12) - 1))))) / (((nx2 * nx2) * nx2) * 2520));
   return ((n / 2) & 1 ? 1 : -1) * (approximate_log_of_bernoulli_bn > tools::log_max_value<T>()
      ? policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, nx, Policy())
      : static_cast<T>(exp(approximate_log_of_bernoulli_bn)));
}

template <class T, class Policy>
T t2n_asymptotic(int n)
{
   BOOST_MATH_STD_USING
   // Just get B2n and convert to a Tangent number:
   T t2n = fabs(b2n_asymptotic<T, Policy>(2 * n)) / (2 * n);
   T p2 = ldexp(T(1), n);
   if(tools::max_value<T>() / p2 < t2n)
      return policies::raise_overflow_error<T>("boost::math::tangent_t2n<%1%>(std::size_t)", 0, T(n), Policy());
   t2n *= p2;
   p2 -= 1;
   if(tools::max_value<T>() / p2 < t2n)
      return policies::raise_overflow_error<T>("boost::math::tangent_t2n<%1%>(std::size_t)", 0, Policy());
   t2n *= p2;
   return t2n;
}
//
// We need to know the approximate value of /n/ which will
// cause bernoulli_b2n<T>(n) to return infinity - this allows
// us to elude a great deal of runtime checking for values below
// n, and only perform the full overflow checks when we know that we're
// getting close to the point where our calculations will overflow.
// We use Luschny's LogB3 formula (http://www.luschny.de/math/primes/bernincl.html)
// to find the limit, and since we're dealing with the log of the Bernoulli numbers
// we need only perform the calculation at double precision and not with T
// (which may be a multiprecision type).  The limit returned is within 1 of the true
// limit for all the types tested.  Note that although the code below is basically
// the same as b2n_asymptotic above, it has been recast as a continuous real-valued
// function as this makes the root finding go smoother/faster.  It also omits the
// sign of the Bernoulli number.
//
struct max_bernoulli_root_functor
{
   max_bernoulli_root_functor(long long t) : target(static_cast<double>(t)) {}
   double operator()(double n)
   {
      BOOST_MATH_STD_USING

      // Luschny LogB3(n) formula.

      const double nx2(n * n);

      const double approximate_log_of_bernoulli_bn
         =   ((boost::math::constants::half<double>() + n) * log(n))
           + ((boost::math::constants::half<double>() - n) * log(boost::math::constants::pi<double>()))
           + (((double(3) / 2) - n) * boost::math::constants::ln_two<double>())
           + ((n * (2 - (nx2 * 7) * (1 + ((nx2 * 30) * ((nx2 * 12) - 1))))) / (((nx2 * nx2) * nx2) * 2520));

      return approximate_log_of_bernoulli_bn - target;
   }
private:
   double target;
};

template <class T, class Policy>
inline std::size_t find_bernoulli_overflow_limit(const mpl::false_&)
{
   long long t = lltrunc(boost::math::tools::log_max_value<T>());
   max_bernoulli_root_functor fun(t);
   boost::math::tools::equal_floor tol;
   boost::uintmax_t max_iter = boost::math::policies::get_max_root_iterations<Policy>();
   return static_cast<std::size_t>(boost::math::tools::toms748_solve(fun, sqrt(double(t)), double(t), tol, max_iter).first) / 2;
}

template <class T, class Policy>
inline std::size_t find_bernoulli_overflow_limit(const mpl::true_&)
{
   return max_bernoulli_index<bernoulli_imp_variant<T>::value>::value;
}

template <class T, class Policy>
std::size_t b2n_overflow_limit()
{
   // This routine is called at program startup if it's called at all:
   // that guarantees safe initialization of the static variable.
   typedef mpl::bool_<(bernoulli_imp_variant<T>::value >= 1) && (bernoulli_imp_variant<T>::value <= 3)> tag_type;
   static const std::size_t lim = find_bernoulli_overflow_limit<T, Policy>(tag_type());
   return lim;
}

//
// The tangent numbers grow larger much more rapidly than the Bernoulli numbers do....
// so to compute the Bernoulli numbers from the tangent numbers, we need to avoid spurious
// overflow in the calculation, we can do this by scaling all the tangent number by some scale factor:
//
template <class T>
inline typename enable_if_c<std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::radix == 2), T>::type tangent_scale_factor()
{
   BOOST_MATH_STD_USING
   return ldexp(T(1), std::numeric_limits<T>::min_exponent + 5);
}
template <class T>
inline typename disable_if_c<std::numeric_limits<T>::is_specialized && (std::numeric_limits<T>::radix == 2), T>::type tangent_scale_factor()
{
   return tools::min_value<T>() * 16;
}
//
// Initializer: ensure all our constants are initialized prior to the first call of main:
//
template <class T, class Policy>
struct bernoulli_initializer
{
   struct init
   {
      init()
      {
         //
         // We call twice, once to initialize our static table, and once to
         // initialize our dymanic table:
         //
         boost::math::bernoulli_b2n<T>(2, Policy());
#ifndef BOOST_NO_EXCEPTIONS
         try{
#endif
            boost::math::bernoulli_b2n<T>(max_bernoulli_b2n<T>::value + 1, Policy());
#ifndef BOOST_NO_EXCEPTIONS
         } catch(const std::overflow_error&){}
#endif
         boost::math::tangent_t2n<T>(2, Policy());
      }
      void force_instantiate()const{}
   };
   static const init initializer;
   static void force_instantiate()
   {
      initializer.force_instantiate();
   }
};

template <class T, class Policy>
const typename bernoulli_initializer<T, Policy>::init bernoulli_initializer<T, Policy>::initializer;

//
// We need something to act as a cache for our calculated Bernoulli numbers.  In order to
// ensure both fast access and thread safety, we need a stable table which may be extended
// in size, but which never reallocates: that way values already calculated may be accessed
// concurrently with another thread extending the table with new values.
//
// Very very simple vector class that will never allocate more than once, we could use
// boost::container::static_vector here, but that allocates on the stack, which may well
// cause issues for the amount of memory we want in the extreme case...
//
template <class T>
struct fixed_vector : private std::allocator<T>
{
   typedef unsigned size_type;
   typedef T* iterator;
   typedef const T* const_iterator;
   fixed_vector() : m_used(0)
   {
      std::size_t overflow_limit = 5 + b2n_overflow_limit<T, policies::policy<> >();
      m_capacity = static_cast<unsigned>((std::min)(overflow_limit, static_cast<std::size_t>(100000u)));
      m_data = this->allocate(m_capacity);
   }
   ~fixed_vector()
   {
      for(unsigned i = 0; i < m_used; ++i)
         this->destroy(&m_data[i]);
      this->deallocate(m_data, m_capacity);
   }
   T& operator[](unsigned n) { BOOST_ASSERT(n < m_used); return m_data[n]; }
   const T& operator[](unsigned n)const { BOOST_ASSERT(n < m_used); return m_data[n]; }
   unsigned size()const { return m_used; }
   unsigned size() { return m_used; }
   void resize(unsigned n, const T& val)
   {
      if(n > m_capacity)
      {
         BOOST_THROW_EXCEPTION(std::runtime_error("Exhausted storage for Bernoulli numbers."));
      }
      for(unsigned i = m_used; i < n; ++i)
         new (m_data + i) T(val);
      m_used = n;
   }
   void resize(unsigned n) { resize(n, T()); }
   T* begin() { return m_data; }
   T* end() { return m_data + m_used; }
   T* begin()const { return m_data; }
   T* end()const { return m_data + m_used; }
   unsigned capacity()const { return m_capacity; }
private:
   T* m_data;
   unsigned m_used, m_capacity;
};

template <class T, class Policy>
class bernoulli_numbers_cache
{
public:
   bernoulli_numbers_cache() : m_overflow_limit((std::numeric_limits<std::size_t>::max)())
#if defined(BOOST_HAS_THREADS) && !defined(BOOST_MATH_NO_ATOMIC_INT)
      , m_counter(0)
#endif
   {}

   typedef fixed_vector<T> container_type;

   void tangent(std::size_t m)
   {
      static const std::size_t min_overflow_index = b2n_overflow_limit<T, Policy>() - 1;
      tn.resize(static_cast<typename container_type::size_type>(m), T(0U));

      BOOST_MATH_INSTRUMENT_VARIABLE(min_overflow_index);

      std::size_t prev_size = m_intermediates.size();
      m_intermediates.resize(m, T(0U));

      if(prev_size == 0)
      {
         m_intermediates[1] = tangent_scale_factor<T>() /*T(1U)*/;
         tn[0U] = T(0U);
         tn[1U] = tangent_scale_factor<T>()/* T(1U)*/;
         BOOST_MATH_INSTRUMENT_VARIABLE(tn[0]);
         BOOST_MATH_INSTRUMENT_VARIABLE(tn[1]);
      }

      for(std::size_t i = std::max<size_t>(2, prev_size); i < m; ++i)
      {
         bool overflow_check = false;
         if(i >= min_overflow_index && (boost::math::tools::max_value<T>() / (i-1) < m_intermediates[1]) )
         {
            std::fill(tn.begin() + i, tn.end(), boost::math::tools::max_value<T>());
            break;
         }
         m_intermediates[1] = m_intermediates[1] * (i-1);
         for(std::size_t j = 2; j <= i; ++j)
         {
            overflow_check =
                  (i >= min_overflow_index) && (
                  (boost::math::tools::max_value<T>() / (i - j) < m_intermediates[j])
                  || (boost::math::tools::max_value<T>() / (i - j + 2) < m_intermediates[j-1])
                  || (boost::math::tools::max_value<T>() - m_intermediates[j] * (i - j) < m_intermediates[j-1] * (i - j + 2))
                  || ((boost::math::isinf)(m_intermediates[j]))
                );

            if(overflow_check)
            {
               std::fill(tn.begin() + i, tn.end(), boost::math::tools::max_value<T>());
               break;
            }
            m_intermediates[j] = m_intermediates[j] * (i - j) + m_intermediates[j-1] * (i - j + 2);
         }
         if(overflow_check)
            break; // already filled the tn...
         tn[static_cast<typename container_type::size_type>(i)] = m_intermediates[i];
         BOOST_MATH_INSTRUMENT_VARIABLE(i);
         BOOST_MATH_INSTRUMENT_VARIABLE(tn[static_cast<typename container_type::size_type>(i)]);
      }
   }

   void tangent_numbers_series(const std::size_t m)
   {
      BOOST_MATH_STD_USING
      static const std::size_t min_overflow_index = b2n_overflow_limit<T, Policy>() - 1;

      typename container_type::size_type old_size = bn.size();

      tangent(m);
      bn.resize(static_cast<typename container_type::size_type>(m));

      if(!old_size)
      {
         bn[0] = 1;
         old_size = 1;
      }

      T power_two(ldexp(T(1), static_cast<int>(2 * old_size)));

      for(std::size_t i = old_size; i < m; ++i)
      {
         T b(static_cast<T>(i * 2));
         //
         // Not only do we need to take care to avoid spurious over/under flow in
         // the calculation, but we also need to avoid overflow altogether in case
         // we're calculating with a type where "bad things" happen in that case:
         //
         b  = b / (power_two * tangent_scale_factor<T>());
         b /= (power_two - 1);
         bool overflow_check = (i >= min_overflow_index) && (tools::max_value<T>() / tn[static_cast<typename container_type::size_type>(i)] < b);
         if(overflow_check)
         {
            m_overflow_limit = i;
            while(i < m)
            {
               b = std::numeric_limits<T>::has_infinity ? std::numeric_limits<T>::infinity() : tools::max_value<T>();
               bn[static_cast<typename container_type::size_type>(i)] = ((i % 2U) ? b : T(-b));
               ++i;
            }
            break;
         }
         else
         {
            b *= tn[static_cast<typename container_type::size_type>(i)];
         }

         power_two = ldexp(power_two, 2);

         const bool b_neg = i % 2 == 0;

         bn[static_cast<typename container_type::size_type>(i)] = ((!b_neg) ? b : T(-b));
      }
   }

   template <class OutputIterator>
   OutputIterator copy_bernoulli_numbers(OutputIterator out, std::size_t start, std::size_t n, const Policy& pol)
   {
      //
      // There are basically 3 thread safety options:
      //
      // 1) There are no threads (BOOST_HAS_THREADS is not defined).
      // 2) There are threads, but we do not have a true atomic integer type,
      //    in this case we just use a mutex to guard against race conditions.
      // 3) There are threads, and we have an atomic integer: in this case we can
      //    use the double-checked locking pattern to avoid thread synchronisation
      //    when accessing values already in the cache.
      //
      // First off handle the common case for overflow and/or asymptotic expansion:
      //
      if(start + n > bn.capacity())
      {
         if(start < bn.capacity())
         {
            out = copy_bernoulli_numbers(out, start, bn.capacity() - start, pol);
            n -= bn.capacity() - start;
            start = static_cast<std::size_t>(bn.capacity());
         }
         if(start < b2n_overflow_limit<T, Policy>() + 2u)
         {
            for(; n; ++start, --n)
            {
               *out = b2n_asymptotic<T, Policy>(static_cast<typename container_type::size_type>(start * 2U));
               ++out;
            }
         }
         for(; n; ++start, --n)
         {
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(start), pol);
            ++out;
         }
         return out;
      }
   #if !defined(BOOST_HAS_THREADS)
      //
      // Single threaded code, very simple:
      //
      if(start + n >= bn.size())
      {
         std::size_t new_size = (std::min)((std::max)((std::max)(std::size_t(start + n), std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
         tangent_numbers_series(new_size);
      }

      for(std::size_t i = (std::max)(std::size_t(max_bernoulli_b2n<T>::value + 1), start); i < start + n; ++i)
      {
         *out = (i >= m_overflow_limit) ? policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol) : bn[i];
         ++out;
      }
   #elif defined(BOOST_MATH_NO_ATOMIC_INT)
      //
      // We need to grab a mutex every time we get here, for both readers and writers:
      //
      boost::detail::lightweight_mutex::scoped_lock l(m_mutex);
      if(start + n >= bn.size())
      {
         std::size_t new_size = (std::min)((std::max)((std::max)(std::size_t(start + n), std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
         tangent_numbers_series(new_size);
      }

      for(std::size_t i = (std::max)(std::size_t(max_bernoulli_b2n<T>::value + 1), start); i < start + n; ++i)
      {
         *out = (i >= m_overflow_limit) ? policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol) : bn[i];
         ++out;
      }

   #else
      //
      // Double-checked locking pattern, lets us access cached already cached values
      // without locking:
      //
      // Get the counter and see if we need to calculate more constants:
      //
      if(static_cast<std::size_t>(m_counter.load(BOOST_MATH_ATOMIC_NS::memory_order_consume)) < start + n)
      {
         boost::detail::lightweight_mutex::scoped_lock l(m_mutex);

         if(static_cast<std::size_t>(m_counter.load(BOOST_MATH_ATOMIC_NS::memory_order_consume)) < start + n)
         {
            if(start + n >= bn.size())
            {
               std::size_t new_size = (std::min)((std::max)((std::max)(std::size_t(start + n), std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
               tangent_numbers_series(new_size);
            }
            m_counter.store(static_cast<atomic_integer_type>(bn.size()), BOOST_MATH_ATOMIC_NS::memory_order_release);
         }
      }

      for(std::size_t i = (std::max)(static_cast<std::size_t>(max_bernoulli_b2n<T>::value + 1), start); i < start + n; ++i)
      {
         *out = (i >= m_overflow_limit) ? policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol) : bn[static_cast<typename container_type::size_type>(i)];
         ++out;
      }

   #endif
      return out;
   }

   template <class OutputIterator>
   OutputIterator copy_tangent_numbers(OutputIterator out, std::size_t start, std::size_t n, const Policy& pol)
   {
      //
      // There are basically 3 thread safety options:
      //
      // 1) There are no threads (BOOST_HAS_THREADS is not defined).
      // 2) There are threads, but we do not have a true atomic integer type,
      //    in this case we just use a mutex to guard against race conditions.
      // 3) There are threads, and we have an atomic integer: in this case we can
      //    use the double-checked locking pattern to avoid thread synchronisation
      //    when accessing values already in the cache.
      //
      //
      // First off handle the common case for overflow and/or asymptotic expansion:
      //
      if(start + n > bn.capacity())
      {
         if(start < bn.capacity())
         {
            out = copy_tangent_numbers(out, start, bn.capacity() - start, pol);
            n -= bn.capacity() - start;
            start = static_cast<std::size_t>(bn.capacity());
         }
         if(start < b2n_overflow_limit<T, Policy>() + 2u)
         {
            for(; n; ++start, --n)
            {
               *out = t2n_asymptotic<T, Policy>(static_cast<typename container_type::size_type>(start));
               ++out;
            }
         }
         for(; n; ++start, --n)
         {
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(start), pol);
            ++out;
         }
         return out;
      }
   #if !defined(BOOST_HAS_THREADS)
      //
      // Single threaded code, very simple:
      //
      if(start + n >= bn.size())
      {
         std::size_t new_size = (std::min)((std::max)((std::max)(start + n, std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
         tangent_numbers_series(new_size);
      }

      for(std::size_t i = start; i < start + n; ++i)
      {
         if(i >= m_overflow_limit)
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
         else
         {
            if(tools::max_value<T>() * tangent_scale_factor<T>() < tn[static_cast<typename container_type::size_type>(i)])
               *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
            else
               *out = tn[static_cast<typename container_type::size_type>(i)] / tangent_scale_factor<T>();
         }
         ++out;
      }
   #elif defined(BOOST_MATH_NO_ATOMIC_INT)
      //
      // We need to grab a mutex every time we get here, for both readers and writers:
      //
      boost::detail::lightweight_mutex::scoped_lock l(m_mutex);
      if(start + n >= bn.size())
      {
         std::size_t new_size = (std::min)((std::max)((std::max)(start + n, std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
         tangent_numbers_series(new_size);
      }

      for(std::size_t i = start; i < start + n; ++i)
      {
         if(i >= m_overflow_limit)
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
         else
         {
            if(tools::max_value<T>() * tangent_scale_factor<T>() < tn[static_cast<typename container_type::size_type>(i)])
               *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
            else
               *out = tn[static_cast<typename container_type::size_type>(i)] / tangent_scale_factor<T>();
         }
         ++out;
      }

   #else
      //
      // Double-checked locking pattern, lets us access cached already cached values
      // without locking:
      //
      // Get the counter and see if we need to calculate more constants:
      //
      if(static_cast<std::size_t>(m_counter.load(BOOST_MATH_ATOMIC_NS::memory_order_consume)) < start + n)
      {
         boost::detail::lightweight_mutex::scoped_lock l(m_mutex);

         if(static_cast<std::size_t>(m_counter.load(BOOST_MATH_ATOMIC_NS::memory_order_consume)) < start + n)
         {
            if(start + n >= bn.size())
            {
               std::size_t new_size = (std::min)((std::max)((std::max)(start + n, std::size_t(bn.size() + 20)), std::size_t(50)), std::size_t(bn.capacity()));
               tangent_numbers_series(new_size);
            }
            m_counter.store(static_cast<atomic_integer_type>(bn.size()), BOOST_MATH_ATOMIC_NS::memory_order_release);
         }
      }

      for(std::size_t i = start; i < start + n; ++i)
      {
         if(i >= m_overflow_limit)
            *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
         else
         {
            if(tools::max_value<T>() * tangent_scale_factor<T>() < tn[static_cast<typename container_type::size_type>(i)])
               *out = policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(std::size_t)", 0, T(i), pol);
            else
               *out = tn[static_cast<typename container_type::size_type>(i)] / tangent_scale_factor<T>();
         }
         ++out;
      }

   #endif
      return out;
   }

private:
   //
   // The caches for Bernoulli and tangent numbers, once allocated,
   // these must NEVER EVER reallocate as it breaks our thread
   // safety guarentees:
   //
   fixed_vector<T> bn, tn;
   std::vector<T> m_intermediates;
   // The value at which we know overflow has already occurred for the Bn:
   std::size_t m_overflow_limit;
#if !defined(BOOST_HAS_THREADS)
#elif defined(BOOST_MATH_NO_ATOMIC_INT)
   boost::detail::lightweight_mutex m_mutex;
#else
   boost::detail::lightweight_mutex m_mutex;
   atomic_counter_type m_counter;
#endif
};

template <class T, class Policy>
inline bernoulli_numbers_cache<T, Policy>& get_bernoulli_numbers_cache()
{
   //
   // Force this function to be called at program startup so all the static variables
   // get initailzed then (thread safety).
   //
   bernoulli_initializer<T, Policy>::force_instantiate();
   static bernoulli_numbers_cache<T, Policy> data;
   return data;
}

}}}

#endif // BOOST_MATH_BERNOULLI_DETAIL_HPP


//
// Created by sean on 10/07/16.
//

#ifndef PCL_OBJECT_DETECT_BATCH_MODE_LINALG_HPP
#define PCL_OBJECT_DETECT_BATCH_MODE_LINALG_HPP

#include <Eigen/Core>

#include "pcltools/common.hpp"

using namespace pcltools::literals;

namespace pcltools {

namespace linalg {

/**
 * Converts a 4x4 rigid transformation matrix to a quaternion and translation vector pair
 * @param transformation  A 4x4 matrix representing a rigid transformation.
 * @return                A quaternion and vector pair representing the passed transformation.
 */
template <typename Scalar>
auto getQuaternionAndTranslation (Eigen::Matrix <Scalar, 4, 4> const & transformation)
-> std::pair <Eigen::Quaternion <Scalar>, Eigen::Matrix <Scalar, 3, 1>> {
  auto rotation_matrix =
  Eigen::Matrix <Scalar, 3, 3> {transformation.block (0, 0, 3, 3)};
  auto translation_vector =
      static_cast <Eigen::Matrix <Scalar, 3, 1>> (transformation.block (0, 3, 3, 1));
  return std::make_pair (Eigen::Quaternion <Scalar> (rotation_matrix), translation_vector);
}

/**
 * Converts a quaternion and translation vector representing a rigid transformation to 4x4 matrix
 * @param quaternion  The quaternion representing the rigid rotation.
 * @param translation The translation represented as a vector (a 3x1 matrix).
 * @return            The 4x4 matrix that represents a rigid body transformation.
 */
template <typename Scalar>
auto getTransformationMatrix (Eigen::Quaternion <Scalar> const & quaternion,
                              Eigen::Matrix <Scalar, 3, 1> const & translation)
-> Eigen::Matrix <Scalar, 4, 4> {
  auto rotation = Eigen::Matrix <Scalar, 3, 3> {quaternion.toRotationMatrix ()};
  auto transformation = Eigen::Matrix <Scalar, 4, 4> {Eigen::Matrix <Scalar, 4, 4>::Identity ()};

  for (auto col = 0_sz; col < rotation.cols (); ++col)
    for (auto row = 0_sz; row < rotation.rows (); ++row)
      transformation (row, col) = rotation (row, col);

  for (auto index = 0_sz; index < translation.size (); ++index)
    transformation (index, transformation.cols () - 1) = translation (index);

  return transformation;
}

}
}

#endif //PCL_OBJECT_DETECT_BATCH_MODE_LINALG_HPP


/*

Project: basic icp
Date: 20/08/04
@Author: Yang, Wang.
Detail: align numberous pointclouds
Scenario: VO

*/

#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>

#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/features/normal_3d.h>

#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/registration/transformation_estimation_lm.h>

#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sys/types.h>
#include <string.h>
#include <dirent.h>
#include <stdio.h>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

using namespace std;
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormal;
//typedef Eigen::Matrix<double, 4, 4> Matrix4f;

//#pragma pack(1)

struct PCDMap
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    int index =0;
    PointCloud::Ptr cloud;
    Eigen::Matrix4f global_transform;

    //Eigen::Vector3d move;
    //Eigen::Quaterniond q;

    //Eigen::Vector3d move_after_g2o;
    //Eigen::Quaterniond q_after_g2o;
    bool doing_g2o = false;
    PCDMap():cloud(new PointCloud){};
};

//uniform sampling func
//data acquired: Cloud's ptr for processing.
void uniform_sampling (PointCloud::Ptr cloud)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	//cout << "原始点云个数：" << cloud->points.size() << endl;
	pcl::UniformSampling<pcl::PointXYZ> US;
   	US.setInputCloud(cloud);
    	US.setRadiusSearch(0.2f);
    	US.filter(*cloud_filtered);
	*cloud = *cloud_filtered;
    	//cout << "均匀采样之后点云的个数：" << cloud->points.size() << endl;
}

//remove singel func
//data acquired: Cloud's ptr for processing.
void remove_single (PointCloud::Ptr cloud)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        //cout << "原始点云个数：" << cloud->points.size() << endl;
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> RS;
	RS.setInputCloud(cloud);
	RS.setRadiusSearch(0.5);
	RS.setMinNeighborsInRadius(4);
	RS.filter(*cloud_filtered);
        *cloud = *cloud_filtered;
	//cout << "除去离群点后点云的个数：" << cloud->points.size() << endl;
}

//another new filter to remove the point faraway from the origin
//added on 7/30 by Wang
//data acquired: pcl::PointCloud<PointNormal> PointCloudN
void remove_distant (PointCloud::Ptr cloud)
{
    PointCloud::Ptr cloud_filtered(new PointCloud);
    //cout << "Cloud points number is:" << cloud->points.size() << endl;
    pcl::StatisticalOutlierRemoval<PointT> RD;
    RD.setInputCloud(cloud);
    RD.setMeanK(50);
    RD.setStddevMulThresh(1.0);
    RD.filter(*cloud_filtered);
    *cloud = *cloud_filtered;
    //cout << "Cloud points number after remove distant:" << cloud->points.size() << endl;
}

//icp point align algorithm
//data acquired: 1 & 2 is the Cloud's ptr for aligning, 3 is the Cloud's ptr for output, 4 is transformation matrix, 5 is unclear.
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f&final_transform, int n)
{
	int iterations = 100;
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_src);
        icp.setInputTarget(cloud_tgt);
        icp.setTransformationEpsilon(1e-15);
	icp.setMaxCorrespondenceDistance(2);
        icp.setEuclideanFitnessEpsilon(0.001);
        icp.setMaximumIterations(iterations);

	icp.align(*output);
	//cout<<"\nThe "<<n<< " frame and the "<<n+1<<" frame have converged and score is "<<icp.getFitnessScore()<<endl;
	//cout<<"Transformation is\n"<<icp.getFinalTransformation()<<endl;
	final_transform = icp.getFinalTransformation();
}

//.pcd viewer related
//data acquired: Cloud's ptr for display
void showPCD (const PointCloud::Ptr clouddata)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer_final (new pcl::visualization::PCLVisualizer("result1"));
        viewer_final->setBackgroundColor(0, 0, 0);

        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>clouddatacolor (clouddata, 0, 255, 0);
        viewer_final->addPointCloud<pcl::PointXYZ> (clouddata, clouddatacolor, "cloud data");
        viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud data");
	while (!viewer_final->wasStopped())
        {
                viewer_final->spinOnce(100);
	}

}

int main(int argc, char** argv)
{
    vector<PCDMap> data;
    vector<PCDMap> secdata;
    vector<PCDMap> thidata;
    vector<PCDMap> findata;

    string data_c1 = argv[1];
    string data_c2 = argv[2];
    //string data_c3 = argv[3];
    int data_s = stoi(data_c1);
    int data_e = stoi(data_c2);
    //int data_num = stoi(data_c3);
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for(int i = data_s; i < data_e; i=i+2)
    {
        char namebuf[20];
        sprintf(namebuf, "../%d.pcd", i);
        cout<<namebuf<<endl;
        PCDMap m;
        pcl::io::loadPCDFile (namebuf, *m.cloud); // set the struct.cloud
        m.index = i;                              // set the struct.index
        //m.afterg2o = Eigen::Matrix4d::Identity();
        data.push_back(m);
    }

    PointCloud::Ptr source, target;
    int j = 0;
    int l = 0;
    int n = 0;

    //refinied 20/08/07
    //the first loop of align, for pre-process
    cout<<data.size()<<endl;
    Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
    for(size_t i = 0; i<data.size()-6; i=i+5)
    {
        target = data[i].cloud;
        uniform_sampling(target);
        remove_single(target);

        PointCloud::Ptr mid (new PointCloud);
        for(int fif = 1; fif < 6; fif++)
        {
            source = data[i+fif].cloud;
            uniform_sampling(source);
            remove_single(source);

            PointCloud::Ptr temp (new PointCloud);
            pairAlign(source, target, temp, pairTransform, i);
            pcl::transformPointCloud(*source, *mid, pairTransform);
            uniform_sampling(target);
            remove_single(target);
            remove_distant(target);
            *target = *target + *mid;
        }
        cout<<"1st: "<< j <<endl;
        GlobalTransform = pairTransform*GlobalTransform;
        PointCloud::Ptr target_1st (new PointCloud);
        pcl::transformPointCloud(*target, *target_1st, GlobalTransform);
        j++;
        PCDMap m_1st;
        *m_1st.cloud = *target_1st;
        m_1st.index = j;
        m_1st.global_transform = GlobalTransform;
        secdata.push_back(m_1st);
    }

    //the second loop of align, for the keyframe
    cout<<secdata.size()<<endl;
    PointCloud::Ptr source2, target2;
    Eigen::Matrix4f GlobalTransform2 = Eigen::Matrix4f::Identity (), pairTransform2;
    for(size_t k = 0; k< secdata.size()-4; k=k+3)
    {
        target2 = secdata[k].cloud;
        PointCloud::Ptr mid2 (new PointCloud);
        for(int fif = 1; fif < 4; fif++)
        {
            source2 = secdata[k+fif].cloud;
            PointCloud::Ptr temp (new PointCloud);
            pairAlign(source2, target2, temp, pairTransform2, k);
            pcl::transformPointCloud(*source2, *mid2, pairTransform2);
            *target2 = *target2 + *mid2;
            uniform_sampling(target2);
            remove_single(target2);

        }
        cout<<"2nd: "<< l <<endl;
        GlobalTransform2 = pairTransform2*GlobalTransform2;
        PointCloud::Ptr target_2nd (new PointCloud);
        pcl::transformPointCloud(*target2, *target_2nd, GlobalTransform2);
        l++;
        PCDMap m_2nd;
        *m_2nd.cloud = *target_2nd;
        m_2nd.index = l;
        m_2nd.global_transform = GlobalTransform2;
        thidata.push_back(m_2nd);
    }

    //the third loop of align, for the result
    cout<<thidata.size()<<endl;
    PointCloud::Ptr source3, target3;
    Eigen::Matrix4f GlobalTransform3 = Eigen::Matrix4f::Identity (), pairTransform3;
    PointCloud::Ptr result (new PointCloud);
    result = thidata[0].cloud;
    for(size_t m = 1; m < thidata.size();m++)
    {
        target3 = thidata[m-1].cloud;
        source3 = thidata[m].cloud;
        PointCloud::Ptr temp (new PointCloud);
        PointCloud::Ptr mid3 (new PointCloud);
        pairAlign(source3, target3, temp, pairTransform3, m);
        GlobalTransform3 = pairTransform3*GlobalTransform3;
        pcl::transformPointCloud(*source3, *mid3, GlobalTransform3);
        *result = *result + *mid3;

        PCDMap m_fin;
        m_fin.index = n;
        m_fin.global_transform = GlobalTransform3;
        findata.push_back(m_fin);

    }

    //get the estimated global transform of the No. num keyframe
    //int num = data_num;
    for(size_t o = 0; o < secdata.size()-5;o++)
    {
        Eigen::Matrix4f GlobalTransform_real = Eigen::Matrix4f::Identity ();
        cout << "We would like to have the global transform of the No. " << o << " frame" << endl;
        GlobalTransform_real = secdata[o].global_transform * thidata[o/3].global_transform * findata[o/3].global_transform;
        cout << GlobalTransform_real << endl;
    }

    uniform_sampling(result);
    remove_single(result);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
    cout << "time cost is " << time_used.count() << " seconds." << endl;
    pcl::io::savePCDFileASCII ("../test.pcd", *result);
    showPCD(result);

}


#ifndef _DIMENSIONAL_ANALYSIS_HPP_
#define _DIMENSIONAL_ANALYSIS_HPP_

#include "MathLibDefinitions.h"
#include <boost\units\systems\si\acceleration.hpp>
#include <boost\units\systems\si\force.hpp>
#include <boost\units\systems\si\length.hpp>
#include <boost\units\systems\si\energy.hpp>
#include <boost\units\systems\si\current.hpp>
#include <boost\units\systems\si\io.hpp>

using namespace boost::units;
using namespace boost::units::si;

namespace   mathlib{

	namespace physics {

		void test(){

			quantity<force, double> Force = quantity<force, double>(2.0 * newton);

		}

		/*

		     *** SIForce   wrapper  class around boost::unit quantity<force,T> ***
		*/

		template<typename T>  class SIForce {

		public:

			/*
			@brief   Zero-force unit.
			*/

		constexpr	SIForce() : m_magnitude{ static_cast<T>(0) } { this->m_oForce = quantity<force, T>(this->m_magnitude * newton); }

			/*
			@brief    Create Force object with user passed magnitude value.
			*/

		    SIForce(_In_ T const mag) : m_magnitude{ mag } { this->m_oForce = quantity<force, T>(this->m_magnitude * newton); }

			/*
			@brief  Copy-Ctor
			*/
			SIForce(_In_ const SIForce &rhs) : m_magnitude{ rhs.m_magnitude }, m_oForce{ rhs.m_oForce }{}

			/*
			@brief   Move-Ctor
			*/
			SIForce(_In_ SIForce &&rhs) : m_magnitude{ std::forward<T>(rhs.m_magnitude) }, m_oForce{ std::forward<T>(rhs.m_oForce) } {}

			/*
			@brief  Dtor == default
			*/
			~SIForce() = default;

			/*

			      ****   Member functions  ****
			*/

			/*  Returns  this->m_magnitude  */
			__forceinline     auto       Magnitude()->const T {

				return (this->m_magnitude);
			}

			/*  Returns  this->m_oForce  */
			__forceinline    auto Force()->const quantity<force,T>{

				return (this->m_oForce);
			}

			/*

			      ***  Member operators  ***
			*/

			/*
			@brief    *this = const &rhs
			*/
			SIForce &     operator=(_In_ const SIForce &rhs) {

				if (this == &rhs) return (*this);

				this->m_magnitude = rhs.m_magnitude;
				this->m_oForce.operator=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief    *this = &&rhs
			*/
			SIForce &     operator=(_In_ SIForce &&rhs) {

				if (this == &rhs) return (*this);

				this->m_magnitude = std::forward<T>(rhs.m_magnitude);
				this->m_oForce.operator=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief    *this +=   const  &rhs
		   */
			SIForce &      operator+=(_In_ const SIForce &rhs) {

				this->m_magnitude += rhs.m_magnitude;
				this->m_oForce.operator+=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief     *this +=    &&rhs
			*/
			SIForce  &      operator+=(_In_ SIForce &&rhs) {

				this->m_magnitude += rhs.m_magnitude;
				this->m_oForce.operator+=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief     *this -=    const  &rhs
			*/
			SIForce  &       operator-=(_In_ const SIForce &rhs) {

				this->m_magnitude -= rhs.m_magnitude;
				this->m_oForce.operator-=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief      *this -=   &&rhs
			*/
			SIForce  &       operator-=(_In_ SIForce &&rhs) {

				this->m_magnitude -= rhs.m_magnitude;
				this->m_oForce.operator-=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief       *this *=  const &rhs
			*/
			SIForce  &       operator*=(_In_  const SIForce &rhs) {

				this->m_magnitude *= rhs.m_magnitude;
				this->m_oForce.operator*=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief       *this *=  &&rhs
			*/
			SIForce  &       operator*=(_In_  SIForce &&rhs) {

				this->m_magnitude *= rhs.m_magnitude;
				this->m_oForce.operator*=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief        *this /=  const &rhs
			*/
			SIForce  &       operator/=(_In_ const SIForce &rhs) {

				this->m_magnitude /= rhs.m_magnitude;
				this->m_oForce.operator/=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief        *this /=   &&rhs
			*/
			SIForce  &       operator/=(_In_ SIForce &&rhs) {

				this->m_magnitude /= rhs.m_magnitude;
				this->m_oForce.operator/=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief         *this == rhs
			*/
			auto             operator==(_In_ const SIForce &rhs)->const bool {

				return (this->m_oForce.value() == rhs.m_oForce.value() &&
					this->m_magnitude == rhs.m_magnitude);
			}

			/*
			@brief           *this != rhs
			*/
			auto              operator!=(_In_ const SIForce &rhs)->const bool {

				return !(this->operator==(rhs));
			}

			/*
			@brief      operator<<
			*/
			friend	std::ostream &      operator<<(_In_ std::ostream &os, _In_   SIForce &rhs) {

				os << "Force = " << rhs.Force().value() << " N" << std::endl;
				return os;
			}

		private:

			/*
			@brief   Amount of force in unit of newton.
			*/
			T   m_magnitude;

			/*
			@brief   unit of Force.
			*/
			quantity<force, T> m_oForce;
		};

		/*
		          ***  Wrapper class for the boost::units quantity of Length.

		*/

		template<typename T>   class SILength {

		public:

			/*
			    Zero-length Default Ctor
			*/
			SILength() : m_magnitude{ static_cast<T>(0) } { this->m_oLength = quantity<length, T>(this->m_magnitude * meter) }

			/*
			    One-arg Ctor.
			*/
			SILength(const T &mag) : m_magnitude{ mag } { this->m_oLength = quantity<length, T>(this->m_magnitude * meter); }

			/*
			    Two-arg Ctor.
			*/
			SILength(const T &rhs, const quantity<length, T> &lhs) : m_magnitude{ rhs }, m_oLength{ lhs } {};
			/*
			    Copy-Ctor
			*/
			SILength(const SILength &rhs) : m_magnitude{ rhs.m_magnitude }, m_oLength{ rhs.m_oLength } {}

			/*
			    Move-Ctor
			*/
			SILength(SILength &&rhs) : m_magnitude{ std::move(rhs.m_magnitude) }, m_oLength{ std::move(rhs.m_oLength) } {}

			/*
			    Dtor = default
			*/
			~SILength() noexcept(true) = default;

			/*

			         ***   Member accessors  ***
			*/

			/*
			     Returns magnitude argument.
			*/
			__forceinline  auto   Magnitude()->const T{

				return (this->m_magnitude);
			}

			/*
			@brief   Returns  m_oLength class variable.
			*/
			__forceinline  auto   Length()->const quantity<length, T> {

				return (this->m_oLength);
			}

			/*

			     ***   Class member and friend operators.  ***
			*/

			/*
			@brief     copy-assignment.
			*/
			auto  operator=(_In_ const SILength &rhs)->SILength<T> & {

				if (this == &rhs) return (*this);

				this->m_magnitude = rhs.m_magnitude;
				this->m_oLength = rhs.m_oLength;
				return (*this);
			}

			/*
			@brief      move-assignment.
			*/
			auto  operator=(_In_ SILength &&rhs)->SILength<T> & {

				if (this == &rhs) return (*this);

				this->m_magnitude = std::forward<T>(rhs.m_magnitude);
				this->m_oLength = std::forward<T>(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief     *this += rhs
			*/
			auto   operator+=(_In_ const SILength &rhs)->SILength<T> & {

				this->m_magnitude += rhs.m_magnitude;
				this->m_oLength.operator+=(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief    *this += rhs (move)
			*/
			auto    operator+=(_In_  SILength<T> &&rhs)->SILength<T> & {

				this->m_magnitude += std::move(rhs.m_magnitude);
				this->m_oLength.operator+=(std::move(rhs.m_oLength));
				return (*this);
			}

			/*
			@brief     *this -= rhs
			*/
			auto   operator-=(_In_ const SILength &rhs)->SILength<T> & {

				this->m_magnitude -= rhs.m_magnitude;
				this->m_oLength.operator-=(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief      *this -= rhs (move)
			*/
			auto    operator-=(_In_ SILength &&rhs)->SILength<T> & {

				this->m_magnitude -= std::move(rhs.m_magnitude);
				this->m_oLength.operator-=(std::move(rhs.m_oLength));
				return (*this);
			}

			/*
			@brief     *this *= rhs
			*/
			auto    operator*=(_In_ const SILength &rhs)->SILength<T> & {

				this->m_magnitude *= rhs.m_magnitude;
				this->m_oLength.operator*=(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief      *this /= rhs
			*/
			auto    operator/=(_In_ SILength &&rhs)->SILength<T> & {

				this->Magnitude = std::move(rhs.m_magnitude);
				this->m_oLength.operator*=(std::move(rhs.m_oLength));
				return (*this);
			}

			/*
			@brief    *this /= rhs
			*/
			auto    operator/=(_In_ const SILength &rhs)->SILength<T> & {

				this->m_magnitude /= rhs.m_magnitude;
				this->m_oLength.operator/=(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief        *this == rhs
		    */
			auto    operator==(_In_ const SILength &rhs)->const bool {

				return (this->m_oLength.value() == rhs.m_oLength.value() &&
					this->m_magnitude == rhs.m_magnitude);
			}

			/*
			@brief        *this != rhs
			*/
			auto    operator!=(_In_ const SILength &rhs)->const bool {

				return (!(this->operator==(rhs)));
			}

			/*
			@brief    operator<<
			*/
			friend  std::ostream &   operator<<(_In_ std::ostream &os, _In_   SILength &rhs) {

				os << "Length = " << rhs.Length().value() << " m" << std::endl;
				return os;
			}

		private:

			/*
			    T m_magnitude class variable
			*/
			T    m_magnitude;

			/*
			    T m_oLength class variable

			*/

			quantity<length, T> m_oLength;

		};

		/*

		        ***  Generic class Work   ***

		*/

		template<class Force, class Distance, typename T>
		class SIWork {

		public:

			/*
			@brief Surpress creation of Default Ctor
			*/
			SIWork() = delete;

			/*
			 @brief    One-arg Ctor
			*/

			SIWork(_In_  Force &f, _In_  Distance &d) : m_oWork{ f.Force() * d.Length() } {}

			/*
			@brief     Copy-Ctor.
			*/
			SIWork(_In_ const SIWork &rhs) : m_oWork{ rhs.m_oWork } {}

			/*
			@brief     Move-Ctor
			*/
			SIWork(_In_ SIWork &&rhs) : m_oWork{ std::move(rhs.m_oWork) } {}

			/*
			@brief   Dtor
			*/
			~SIWork() noexcept(true) = default;

			/*

			      Class member operators, and accessor.

			*/

			auto     Work()->const SIWork<Force, Distance, T> {

				return (this->m_oWork);
			}

			/*
			         *this = rhs (copy)
			*/
			auto     operator=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				if (this == &rhs) return (*this);
				this->m_oWork.operator=(rhs.m_oWork);
				return (*this);
			}

			/*
			         *this = rhs (move)
			*/
			auto      operator=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				if (this == &rhs) return (*this);
				this->m_oWork.operator=(rhs.m_oWork);
				return (*this);
			}

			/*
			             *this += rhs
			 */
			auto      operator+=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator+=(rhs.m_oWork);
				return (*this);
			}

			/*
			            *this += rhs (move)
			*/
			auto       operator+=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator+=(std::move(rhs.m_oWork));
				return (*this);
			}

			/*
			              *this -= rhs
			*/
			auto      operator-=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator-=(rhs.m_oWork);
				return (*this);
			}

			/*
			               *this -= rhs
			*/
			auto       operator-=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator-=(std::move(rhs.m_oWork));
				return (*this);
			}

			/*
			              *this *= rhs
			*/
			auto       operator*=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator*=(rhs.m_oWork);
				return (*this);
			}

			/*
			               *this *= rhs (move)
			 */
			auto       operator*=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator*=(std::move(rhs.m_oWork));
				return (*this);
			}

			/*
			               *this /= rhs
			*/
			auto        operator/=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator/=(rhs.m_oWork);
				return (*this);
			}

			/*
			               *this /= rhs (move)
			*/
			auto        operator/=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator/=(std::move(rhs.m_oWork));
				return (*this);
			}

			/*
			               *this == rhs
		    */
			auto         operator==(_In_ const SIWork &rhs)->const bool {
				return (this->m_oWork.value() == rhs.m_oWork.value());
			}

			/*
			               *this == rhs (move)
			*/
			auto          operator==(_In_ SIWork &&rhs)-> const bool {
				return (std::move(this->m_oWork.value() == std::move(this->m_oWork.value())));
			}
			/*
			               *this != rhs
		    */
			auto         operator!=(_In_ const SIWork &rhs)->const bool {
				return (!(this->operator==(rhs)));
			}

			/*
			               std::cout << *this

			*/
			auto     display()->const void {

				std::printf("Work = %4.16f J\n", this->m_oWork.value());
			}

		private:

			/*
			@brief    Energy(Work) class variable m_oWork
			*/
			quantity<energy, T> m_oWork;

		};

		/*

		             Wrapper class for boost::units::current

		*/

		/*  primary template */

		template<typename T>  class SICurrent {

		public:

			/*
			@brief    Zero-current default Ctor
			*/
			SICurrent() : m_Magnitude{ static_cast<T>(0.0) }, m_oCurrent{ m_Magnitude * amperes } {}

			/*
			@brief    One-arg Ctor , constructs object of type Current.
			*/
			SICurrent(_In_ const T mag) : m_Magnitude{ mag }, m_oCurrent{ m_Magnitude * amperes } {}

			/*
			@brief    Copy-Ctor.
			*/
			SICurrent(_In_ const SICurrent &rhs) : m_Magnitude{ rhs.m_Magnitude }, m_oCurrent{ rhs.m_oCurrent } {}

			/*
			@brief    Move-Ctor
			*/
			SICurrent(_In_ SICurrent &&rhs) : m_Magnitude{ std::move(rhs.m_Magnitude) }, m_oCurrent{ std::move(rhs.m_oCurrent) } {}

			/*
			@brief    Dtor  = default.
			*/
			~SICurrent() noexcept(true) = default;

			/*

			Class member accessors and operators.

			*/

			/*
			@brief     this->m_Magnitude
			*/
			__forceinline  auto         Magnitude()->const T{

				return (this->m_Magnitude);
			}

			/*
			@brief     this->m_oCurrent
			*/
		   __forceinline auto            Current()->const quantity<current, T> {

			  return (this->m_oCurrent);
			}

		    /*
		    @brief      *this = rhs (copy)
		    */
		   auto        operator=(_In_ const SICurrent &rhs)->SICurrent<T> {

			   if (this == &rhs) return (*this);

			   this->m_Magnitude = rhs.m_Magnitude;
			   this->m_oCurrent.operator=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief        *this = rhs (move)
		   */
		   auto         operator=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   if (this == &rhs) return (*this);

			   this->m_Magnitude = std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator=(std::move(rhs.m_oCurrent));
			   return (*this);
		   }

		   /*
		   @brief         *this += rhs
		   */
		   auto          operator+=(_In_ const SICurrent &rhs)->SICurrent<T> & {

			   this->m_Magnitude += rhs.m_Magnitude;
			   this->m_oCurrent.operator+=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief         *this += rhs (move)
		   */
		   auto           operator+=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   this->m_Magnitude += std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator+=(std::move(rhs.m_oCurrent));
			   return (*this);
		   }

		   /*
		   @brief         *this -= rhs
		   */
		   auto            operator-=(_In_ const SICurrent &rhs)->SICurrent<T> & {

			   this->m_Magnitude -= rhs.m_Magnitude;
			   this->m_oCurrent.operator-=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief          *this -= rhs (move)
		   */
		   auto             operator-=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   this->m_Magnitude -= std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator-=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief         *this *= rhs
		   */
		   auto             operator*=(_In_ const SICurrent &rhs)->SICurrent<T> & {

			   this->m_Magnitude *= rhs.m_Magnitude;
			   this->m_oCurrent.operator*=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief          *this *= rhs (move)
		   */
		   auto             operator*=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   this->m_Magnitude *= std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator*=(std::move(rhs.m_oCurrent));
			   return (*this);
		   }

		   /*
		   @brief           *this /= rhs
		   */
		   auto              operator/=(_In_ const SICurrent &rhs)->SICurrent<T> & {

			   this->m_Magnitude /= rhs.m_Magnitude;
			   this->m_oCurrent.operator/=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief            *this /= rhs (move)
		   */
		   auto              operator/=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   this->m_Magnitude /= std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator/=(std::move(rhs.m_oCurrent));
			   return (*this);
		   }

		   /*
		   @brief              *this == rhs
		   */
		   auto              operator==(_In_ const SICurrent &rhs)->const bool {

			   return (this->m_Magnitude == rhs.m_Magnitude &&
				   this->m_oCurrent.value() == rhs.m_oCurrent.value());
		   }

		   /*
		   @brief                *this !=  rhs
		   */
		   auto              operator!=(_In_ const SICurrent &rhs)->const bool {

			   return (!(this->operator==(rhs)));
		   }

		   /*
		   @brief               operator<< , specialized only for std::complex<T> type.
		   */
		   friend  std::enable_if<std::is_class<std::complex<T>>::value,std::ostream &>::type
			   operator<<(_In_ std::ostream &os, _In_ const SICurrent<T> &rhs) {

				       os << "Current=" << rhs.Magnitude().real() << "Re" << "+" <<
					   rhs.Magnitude().imag() << std::endl;
			   }

		   /*
		   @brief         displays the value of this->m_oCurrent in Ampere unit. Displayed value of
		                   Ampere defualted to double-precision.
		   */
		   auto         display()->void {

			   std::printf("Current=%4.16f A/n", this->m_oCurrent.value());
		   }

		private:

			/*
			@brief   quantity magnitude m_oMagnitude.
			*/
			T   m_Magnitude;

			/*
			@brief   quantity of current m_oCurrent
			*/
			quantity<current, T>  m_oCurrent;

		};

		/*

		          Wrapper class for boost::unit::quantity<resistance,T>

		*/

		template<typename T> class SIResistance {

		   public:

			   /*
			         Zero-resistance Ctor.
			   */
			   SIResistance() : m_Magnitude{ static_cast<T>(0.0) }, m_oResistance{ m_Magnitude * ohm } {}

			   /*
			         Variable-resistance Ctor.
			   */
			   SIResistance(_In_ const T &mag) : m_Magnitude{ mag }, m_oResistance{ m_Magnitude * ohm } {}

			   /*
			          Copy-Ctor.
			   */
			   SIResistance(_In_ const SIResistance &rhs) : m_Magnitude{ rhs.m_Magnitude }, m_oResistance{ rhs.m_oResistance } {}

			   /*
			           Move-Ctor.
			   */
			   SIResistance(_In_ SIResistance &&rhs) : m_Magnitude{ std::move(rhs.m_Magnitude) }, m_oResistance{ std::move(rhs.m_oResistance) } {}

			   /*
			           Dtor = default.
			   */
			   ~SIResistance() noexcept(true) = default;

			   /*

			   Accessor and member operators.

			   */

			   /*
			             Returns this->m_Magnitude
			   */
			   __forceinline auto            Magnitude()->const T{

				   return (this->m_Magnitude);
			   }

			   /*
			             Returns this->m_oResistance.
			   */
			   __forceinline  auto           Resistance()->const quantity<resistance, T> {

				   return (this->m_oResistance);
			   }

			   /*
			                 *this = rhs (copy)
			   */
			   auto            operator=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   if (this == &rhs) return (*this);

				   this->m_Magnitude = rhs.m_Magnitude;
				   this->m_oResistance.operator=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                  *this = rhs (move)
			   */
			   auto            operator=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   if (this == &rhs) return (*this);

				   this->m_Magnitude = std::move(rhs.m_Magnitude);
				   this->m_oResistance = std::move(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                    *this += rhs
				*/
			   auto             operator+=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   this->m_Magnitude += rhs.m_Magnitude;
				   this->m_oResistance.operator+=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                     *this += rhs (move)
			   */
			   auto              operator+=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   this->m_Magnitude += std::move(rhs.m_Magnitude);
				   this->m_oResistance.operator+=(std::move(rhs.m_oResistance));
				   return (*this);
			   }

			   /*
			                      *this -= rhs
				*/
			   auto              operator-=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   this->m_Magnitude -= rhs.m_Magnitude;
				   this->m_oResistance.operator-=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                       *this -= rhs (move)
				*/
			   auto              operator-=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   this->m_Magnitude -= std::move(rhs.m_Magnitude);
				   this->m_oResistance.operator-=(std::move(rhs.m_oResistance));
				   return (*this);
			   }

			   /*
			                       *this *= rhs
			   */
			   auto               operator*=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   this->m_Magnitude *= rhs.m_Magnitude;
				   this->m_oResistance.operator*=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                        *this *= rhs  (move)
			   */
			   auto               operator*=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   this->m_Magnitude *= std::move(rhs.m_Magnitude);
				   this->m_oResistance.operator*=(std::move(rhs.m_oResistance));
				   return (*this);
			   }

			   /*
			                         *this /= rhs
			   */
			   auto                operator/=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   this->m_Magnitude /= rhs.m_Magnitude;
				   this->m_oResistance.operator/=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                         *this /= rhs
				*/
			   auto                operator/=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   this->m_Magnitude /= std::move(rhs.m_Magnitude);
				   this->m_oResistance.operator/=(std::move(rhs.m_oResistance));
				   return (*this);
			   }

			   /*
			                         *this == rhs
				*/
		   private:

			   /*
			   @brief       magnitude of quantity<resistance,ohm>
			   */

			   T     m_Magnitude;

			   /*
			   @brief        quantity<resistance,ohm>
			   */
			   quantity<resistance, T> m_oResistance;

		};

		/*
		             mathlib::physics::SIForce namespace operators
		*/

		/*
		     c = a + b
		*/
		template<typename T> __forceinline auto operator+(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			SIForce<T> ret_val = SIForce<T>{lhs.operator+=(rhs)};
			return (ret_val);
		}

		/*
		     c  = a * b
		*/
		template<typename T> __forceinline auto operator*(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			SIForce<T> ret_val = SIForce<T>{lhs.operator*=(rhs)};
			return (ret_val);
		}

		/*
		     c = a - b
		*/
		template<typename T> __forceinline auto operator-(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			SIForce<T> ret_val = SIForce<T>{lhs.operator-=(rhs)};
			return (ret_val);
		}

		/*
		     c  = a / b
		*/
		template<typename T> __forceinline auto operator/(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			SIForce<T> ret_val = SIForce<T>{lhs.operator/=(rhs)};
			return (ret_val);
		}

		/*
		       a == b

		*/
		template<typename T> __forceinline auto operator==(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			return (lhs.operator==(rhs));
		}

		/*
		       a !=  b
		*/
		template<typename T> __forceinline auto operator!=(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			return (lhs.operator==(rhs));
		}

		/*

		                  mathlib::physics::SILength namespace operators

		*/

		/*
		      c = a + b
		*/
		template<typename T> __forceinline auto operator+(_In_  mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->mathlib::physics::SILength<T> {

			SILength<T> ret_val = SILength<T>{lhs.operator+=(rhs)};
			return (ret_val);
		}

		/*
		      c =  a * b
		*/
		template<typename T> __forceinline auto operator*(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->mathlib::physics::SILength<T> {

			SILength<T> ret_val = SILength<T>{lhs.operator*=(rhs)};
			return (ret_val);
		}

		/*
		       c = a - b
		*/
		template<typename T> __forceinline auto operator-(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->mathlib::physics::SILength<T> {

			SILength<T> ret_val = SILength<T>{lhs.operator-=(rhs)};
			return (ret_val);
		}

		/*
		        c =  a / b
		*/
		template<typename T> __forceinline auto operator/(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->mathlib::physics::SILength<T> {

			SILength<T> ret_val = SILength<T>{lhs.operator/=(rhs)};
			return (ret_val);
		}

		/*
		        a == b
		*/
		template<typename T> __forceinline auto operator==(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->bool {

			return (lhs.operator==(rhs));
		}

		/*
		       a != b
		*/
		template<typename T> __forceinline auto operator!=(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->bool {

			return (lhs.operator!=(rhs));
		}

		/*

		           mathlib::physics::SIWork namespace operators.

		*/

		/*
		         c  = lhs + rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator+(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->mathlib::physics::SIWork<Force, Distance, T> {

			SIWork<Force, Distance, T> ret_val = SIWork<Force, Distance, T>{lhs.operator+=(rhs)};
			return (ret_val);
		}

		/*
		         c = lhs - rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator-(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->mathlib::physics::SIWork<Force, Distance, T> {

			SIWork<Force, Distance, T> ret_val = SIWork<Force, Distance, T>{lhs.operator-=(rhs)};
			return (ret_val);
		}

		/*
		        c = lhs * rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator*(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->mathlib::physics::SIWork<Force, Distance, T> {

			SIWork<Force, Distance, T> ret_val = SIWork<Force, Distance, T>{lhs.operator*=(rhs)};
			return (ret_val);
		}

		/*
		         c = lhs / rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator/(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->mathlib::physics::SIWork<Force, Distance, T> {

			SIWork<Force, Distance, T> ret_val = SIWork<Force, Distance, T>{lhs.operator/=(rhs)};
			return (ret_val);
		}

		/*
		        lhs == rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator==(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->bool {

			return (lhs.operator==(rhs));
		}

		/*
		        lhs != rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator!=(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->bool {

			return (lhs.operator!=(rhs));
		}

		/*

		              mathlib::physics::SICurrent namespace operators.

		*/

		/*
		                 c = lhs + rhs
		*/
		template<typename T> __forceinline auto  operator+(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->SICurrent<T> {

			SICurrent<T> ret_val = SICurrent<T>{lhs.operator+=(rhs)};
			return (ret_val);
		}

		/*
		                c = lhs - rhs
		*/
		template<typename T> __forceinline auto operator-(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->SICurrent<T> {

			SICurrent<T> ret_val = SICurrent<T>{lhs.operator-=(rhs)};
			return (ret_val);
		}

		/*
		                c = lhs * rhs
		*/
		template<typename T> __forceinline auto operator*(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->SICurrent<T> {

			SICurrent<T> ret_val = SICurrent<T>{lhs.operator*=(rhs)};
			return (ret_val);
		}

		/*
		                c =  lhs / rhs
		*/
		template<typename T> __forceinline auto operator/(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->SICurrent<T> {

			SICurrent<T> ret_val = SICurrent<T>{lhs.operator/=(rhs)};
			return (ret_val);
		}

		/*
		                 rhs == lhs
		*/
		template<typename T> __forceinline auto operator==(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->bool {

			return (lhs.operator==(rhs));
		}

		/*
		                 rhs != lhs
		*/
		template<typename T> __forceinline auto operator!=(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->bool {

			return (lhs.operator!=(rhs));
		}
	}
}
#endif   /*_DIMENSIONAL_ANALYSIS_HPP_*/


//
//  Copyright (c) 2012, Institue of Cancer Research.
//  All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//     * Neither the name of Institue of Cancer Research.
//       nor the names of its contributors may be used to endorse or promote
//       products derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// For more information on the Plane of Best Fit please see http://pubs.acs.org/doi/abs/10.1021/ci300293f
//
//  If this code has been useful to you, please include the reference
//  in any work which has made use of it:

//  Plane of Best Fit: A Novel Method to Characterize the Three-Dimensionality of Molecules, Nicholas C. Firth, Nathan Brown, and Julian Blagg, Journal of Chemical Information and Modeling 2012 52 (10), 2516-2525

//
//
// Created by Nicholas Firth, November 2011
// Modified by Greg Landrum for inclusion in the RDKit distribution November 2012
//

#include "PBFRDKit.h"
#include <Numerics/Matrix.h>
#include <Numerics/SquareMatrix.h>
#include <Numerics/SymmMatrix.h>
#include <boost/foreach.hpp>

#include <Eigen/Dense>
using namespace RDKit;

void getSmallestEigenVector(double fSumXX,double fSumXY,double fSumXZ,
                            double fSumYY,double fSumYZ,double fSumZZ,
                            double &x,double &y, double &z);

double distanceFromAPlane(const RDGeom::Point3D &pt,const std::vector<double> &plane, double denom){
  double numer=0.0;
  numer = std::fabs(pt.x*plane[0]+pt.y*plane[1]+pt.z*plane[2]+plane[3]);

  return numer/denom;
}

bool getBestFitPlane(const std::vector<RDGeom::Point3D> &points,
                     std::vector<double> &plane,
                     const std::vector<double> *weights) {
  PRECONDITION((!weights || weights->size()>=points.size()),"bad weights vector");
  RDGeom::Point3D origin(0,0,0);
  double wSum=0.0;

  for(unsigned int i=0;i<points.size();++i){
    if(weights){
      double w=(*weights)[i];
      wSum+=w;
      origin+=points[i]*w;
    } else {
      wSum+=1;
      origin+=points[i];
    }
  }
  origin /= wSum;

  double sumXX=0,sumXY=0,sumXZ=0,sumYY=0,sumYZ=0,sumZZ=0;
  for(unsigned int i=0;i<points.size();++i){
    RDGeom::Point3D delta=points[i]-origin;
    if(weights){
      double w=(*weights)[i];
      delta *= w;
    }
    sumXX += delta.x*delta.x;
    sumXY += delta.x*delta.y;
    sumXZ += delta.x*delta.z;
    sumYY += delta.y*delta.y;
    sumYZ += delta.y*delta.z;
    sumZZ += delta.z*delta.z;
  }
  sumXX/=wSum;
  sumXY/=wSum;
  sumXZ/=wSum;
  sumYY/=wSum;
  sumYZ/=wSum;
  sumZZ/=wSum;

  Eigen::Matrix3d mat;
  mat << sumXX, sumXY, sumXZ,
    sumXY, sumYY, sumYZ,
    sumXZ, sumYZ, sumZZ;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(mat);
  if(eigensolver.info()!=Eigen::Success){
    BOOST_LOG(rdErrorLog)<<"eigenvalue calculation did not converge"<<std::endl;
    return 0.0;
  }
  RDGeom::Point3D normal;
  normal.x=eigensolver.eigenvectors()(0,0);
  normal.y=eigensolver.eigenvectors()(1,0);
  normal.z=eigensolver.eigenvectors()(2,0);

  plane[0] = normal.x;
  plane[1] = normal.y;
  plane[2] = normal.z;
  plane[3] = -1*normal.dotProduct(origin);

}

double PBFRD(ROMol& mol,int confId){
  PRECONDITION(mol.getNumConformers()>=1,"molecule has no conformers")
  int numAtoms = mol.getNumAtoms();
  if(numAtoms<4) return 0;

  const Conformer &conf = mol.getConformer(confId);
  if(!conf.is3D()) return 0 ;

  std::vector<RDGeom::Point3D> points;
  points.reserve(numAtoms);
  for(unsigned int i=0; i<numAtoms; ++i){
    points.push_back(conf.getAtomPos(i));
  }

  std::vector<double> plane(4);
  getBestFitPlane(points,plane,0);

  double denom=0.0;
  for(unsigned int i=0; i<3; ++i){
    denom += plane[i]*plane[i];
  }
  denom = pow(denom,0.5);

  double res=0.0;
  for(unsigned int i=0; i<numAtoms; ++i){
    res+= distanceFromAPlane(points[i], plane, denom);
  }
  res /= numAtoms;

  return res;
}


// Copyright (c) 2016 Till Kolditz
//
// 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.

/*
 * File:   Euclidean.hpp
 * Author: Till Kolditz <till.kolditz@gmail.com>
 *
 * Created on 6. Dezember 2016, 00:55
 */

#ifndef EUCLIDEAN_HPP
#define EUCLIDEAN_HPP

#include <iostream>
#include <vector>
#include <cinttypes>
#include <exception>
#include <climits>

#include <boost/multiprecision/cpp_int.hpp>

using boost::multiprecision::uint128_t;

namespace Private {
    template<typename T, typename S>
    struct extractor {
    };

    template<typename T>
    struct extractor<T, uint128_t> {
        static T doIt(
                const uint128_t source) {
            const constexpr unsigned nBitsLimb = sizeof(boost::multiprecision::limb_type) * CHAR_BIT; // size of limb in bits
            boost::multiprecision::limb_type target = 0;
            const unsigned nLimbs = source.backend().size(); // number of limbs
            auto pLimbs = source.backend().limbs();
            for (unsigned i = 0; i < nLimbs && ((i * nBitsLimb) < (sizeof(T) * CHAR_BIT)); ++i) {
                target |= (pLimbs[i]) << (i * nBitsLimb);
            }
            return static_cast<T>(target);
        }
    };
}

/*
 * This algorithm actually computes the modulo inverse of the given first argument
 * b0 in the residual class ring modulo <codewidth> (the second argument). One
 * requirement of this algorithm is, that template type <T> is large enough to
 * store <codewidth>+1 bits!
 */
template<typename T>
T ext_euclidean(
        T b0,
        size_t codewidth) {
    if ((sizeof(T) * CHAR_BIT) <= codewidth) {
        throw std::runtime_error("The template datatype is too small!");
    }
    T a0(1);
    a0 <<= codewidth;
    // T a[20], b[20], q[20], r[20], s[20], t[20];
    std::vector<T> a(32), b(32), q(32), r(32), s(32), t(32);
    // std::vector<uint128_t> a(32), b(32), q(32), r(32), s(32), t(32);
    uint8_t aI = 1, bI = 1, qI = 0, rI = 0, sI = 1, tI = 1;
    a[0] = a0;
    b[0] = b0;
    s[0] = 0;
    t[0] = 0;
    ssize_t i = 0;
    do {
        q[qI++] = a[i] / b[i];
        r[rI++] = a[i] % b[i];
        a[aI++] = b[i];
        b[bI++] = r[i];
        s[sI++] = 0;
        t[tI++] = 0;
    } while (b[++i] > 0);
    s[i] = 1;
    t[i] = 0;

    for (ssize_t j = i; j > 0; --j) {
        s[j - 1] = t[j];
        t[j - 1] = s[j] - q[j - 1] * t[j];
    }
    T result = ((b0 * t[0]) % a0);
    result += result < 0 ? a0 : 0;
    if (result == 1) {
        if constexpr (std::is_fundamental_v<T>) {
            return t[0];
        } else if constexpr (std::is_base_of_v<T, uint128_t>) {
            return Private::extractor<T, uint128_t>::doIt(t[0]);
        } else {
            throw std::runtime_error("ext_euclidean not supported for this type!");
        }
    }
    return 0;
}

#endif /* EUCLIDEAN_HPP */


/**
 * Created by Beck on 3/7/2018
 * Indirect Extended Kalman Filter for the translation of the camera
 * fusing visual information with the accelerometer, given the pose;
 * With imu in 400Hz and camera translation in 30Hz
 * V1: estimation for rotation only
 * V2: give the translation of the shield
 */
#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <queue>
//#include "rm_cv/ArmorRecord.h"

using namespace std;
using namespace Eigen;

ros::Publisher pose_pub, debug_pub, odom_pub, debug_propagate_pub;
string imu_topic, pose_topic, visual_topic;
string debug_topic, publisher_topic;
double acc_weight;
double visual_q_weight, visual_t_weight;
int sleep_time;

/**
 * Define states:
 *      x = [translation_x, y, z; velocity_x, y, z]
 * Define inputs:
 *      u = [acc], raw accelerometer
 * Define noises:
 *      n = [n_acc]
 */
VectorXd x(6);         // state
MatrixXd P = MatrixXd::Identity(6, 6); // covariance
MatrixXd R = MatrixXd::Identity(3, 3); // prediction noise covariance
MatrixXd Q = MatrixXd::Identity(3, 3); // observation noise covariance

// buffers to save gyro and visual reading
queue<sensor_msgs::Imu::ConstPtr> imu_buf;
queue<geometry_msgs::TwistStamped::ConstPtr> visual_buf;
queue<Matrix<double, 6, 1>> x_history;
queue<Matrix<double, 6, 6>> P_history;
Vector3d G = {0, 0, 9.8}; // Consider to add initialization later

double t_prev;          // previous propagated time

// Initialization
const int IMU_INIT_COUNT = 10;
const int MAX_GYRO_QUEUE_SIZE = 400;
const double IMU_UPDATE_TIME = 0.0025; // 1 / 400Hz
int imu_count  = 0;
bool imu_initialized = false;
bool visual_initialized = false;
bool visual_valid = false;
MatrixXd imu_R_camera = MatrixXd::Identity(3, 3); // rotation matrix from camera to imu
Vector3d imu_T_camera = MatrixXd::Zero(3, 1);

//// DEBUG only
double t_prev_update;   // previous update time
Vector3d world_T_shield_prev = MatrixXd::Zero(3, 1); // previous visual translation

void pub_shield_odom(const std_msgs::Header& header)
{
    nav_msgs::Odometry odom;
    odom.header = header;
    odom.child_frame_id = "world";
    odom.pose.pose.position.x = x(0);
    odom.pose.pose.position.y = x(1);
    odom.pose.pose.position.z = x(2);
    odom.twist.twist.linear.x = x(3);
    odom.twist.twist.linear.y = x(4);
    odom.twist.twist.linear.z = x(5);

    odom.pose.covariance[0]  = P(0, 0);
    odom.pose.covariance[7]  = P(1, 1);
    odom.pose.covariance[14] = P(2, 2);
    odom.pose.covariance[21] = P(3, 3);
    odom.pose.covariance[28] = P(4, 4);
    odom.pose.covariance[35] = P(5, 5);
    odom.pose.covariance[3]  = P(0, 3);
    odom.pose.covariance[10] = P(1, 4);
    odom.pose.covariance[17] = P(2, 5);
    odom.pose.covariance[18] = P(3, 0);
    odom.pose.covariance[25] = P(4, 1);
    odom.pose.covariance[32] = P(5, 2);
    odom_pub.publish(odom);
}

void pub_debug_update(const std_msgs::Header& header,
                      const Ref<const Vector3d> p,
                      const Quaterniond& q,
                      double t_update)
{
    nav_msgs::Odometry odom;
    odom.header = header;
    odom.child_frame_id = "world";
    odom.pose.pose.position.x = p[0];
    odom.pose.pose.position.y = p[1];
    odom.pose.pose.position.z = p[2];
    odom.pose.pose.orientation.w = q.w();
    odom.pose.pose.orientation.x = q.x();
    odom.pose.pose.orientation.y = q.y();
    odom.pose.pose.orientation.z = q.z();
    if (visual_initialized) {
        double dt_update = t_update - t_prev_update;
        odom.twist.twist.linear.x = (p[0] - world_T_shield_prev[0]) / dt_update;
        odom.twist.twist.linear.y = (p[1] - world_T_shield_prev[1]) / dt_update;
        odom.twist.twist.linear.z = (p[2] - world_T_shield_prev[2]) / dt_update;

        world_T_shield_prev = p;
        t_prev_update = t_update;
    }
    else {
        odom.twist.twist.linear.x = 0;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.linear.z = 0;
        world_T_shield_prev = p;
        t_prev_update = t_update;
    }

    debug_pub.publish(odom);
}

void pub_debug_propagate(const std_msgs::Header& header,
                         const Ref<const Vector3d> v,
                         const Ref<const Vector3d> a_int)
{
    geometry_msgs::TwistStamped acc_compare;
    acc_compare.header = header;
    acc_compare.twist.linear.x = v[0];
    acc_compare.twist.linear.y = v[1];
    acc_compare.twist.linear.z = v[2];
    acc_compare.twist.angular.x= a_int[0];
    acc_compare.twist.angular.y= a_int[1];
    acc_compare.twist.angular.z= a_int[2];
    debug_propagate_pub.publish(acc_compare);
}

// DEBUG only
Vector3d a_int = MatrixXd::Zero(3, 1);
Vector3d a_int_int = MatrixXd::Zero(3, 1);
void propagate(const sensor_msgs::Imu &imu)
{
    double cur_t = imu.header.stamp.toSec();
    Vector3d a, acc_wo_g;
    a(0) = imu.linear_acceleration.x;
    a(1) = imu.linear_acceleration.y;
    a(2) = imu.linear_acceleration.z;
    Quaterniond world_R_imu(imu.orientation.w, imu.orientation.x, imu.orientation.y, imu.orientation.z);

    double dt = cur_t - t_prev;
    dt = (dt < IMU_UPDATE_TIME * 5) ? dt : IMU_UPDATE_TIME * 5;
    ROS_INFO("dt in propagate is %f, at the cur_t %f", dt, cur_t);
    acc_wo_g = world_R_imu.toRotationMatrix() * a - G;
    x.segment<3>(0) += x.segment<3>(3) * dt + 0.5 * acc_wo_g * dt * dt;
    x.segment<3>(3) += acc_wo_g * dt;

    a_int_int += a_int * dt + 0.5 * acc_wo_g * dt * dt;
    a_int += acc_wo_g * dt;
    pub_debug_propagate(imu.header, acc_wo_g, a_int_int);

    MatrixXd A = MatrixXd::Zero(6, 6);
    A.block<3, 3>(0, 3) = MatrixXd::Identity(3, 3);

    MatrixXd U = -MatrixXd::Zero(6, 3);
    U.block<3, 3>(3, 0) = -world_R_imu.toRotationMatrix();

    MatrixXd F, V;
    F = MatrixXd::Identity(6, 6) + dt * A;
    V = dt * U;
//    cout << "F " << endl << F << endl;
//    cout << "R " << endl << R << endl;
//    cout << "V " << endl << V << endl;
    P = F * P * F.transpose() + V * R * V.transpose();

    t_prev = cur_t;
//    cout << "P " << endl << P << endl;
//    cout << "x " << endl << x.transpose() << endl;
}

static void throwState(double current_time)
{
    while(imu_buf.size() > 1 &&
          imu_buf.front()->header.stamp.toSec() < current_time)
    {
        // trace backward the time to the imu timestamp
        t_prev = imu_buf.front()->header.stamp.toSec();
        ROS_INFO("throw state with time: %f", t_prev);
        imu_buf.pop();
        x_history.pop();
        P_history.pop();
    }
}

static void repropagate()
{
    while (!x_history.empty()) x_history.pop();
    while (!P_history.empty()) P_history.pop();

    queue <sensor_msgs::Imu::ConstPtr> temp_imu_buf;
    while (!imu_buf.empty() && imu_buf.size() > 1)
    {
        propagate(*imu_buf.front());
        temp_imu_buf.push(imu_buf.front());
        x_history.push(x);
        P_history.push(P);
        imu_buf.pop();
    }
    if (!temp_imu_buf.empty()) {
        pub_shield_odom(temp_imu_buf.back()->header);
    }
    swap(imu_buf, temp_imu_buf);
}

static void update(const geometry_msgs::TwistStamped &pnp)
{
    double cur_t = pnp.header.stamp.toSec();
    Vector3d camera_T_shield;

    camera_T_shield[0] = pnp.twist.linear.x;
    camera_T_shield[1] = pnp.twist.linear.y;
    camera_T_shield[2] = pnp.twist.linear.z;
    Vector3d imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;

    imu_T_shield *= 0.001; // Convert millimeter to meter

    throwState(cur_t);

    ROS_INFO("Update, at time %f", cur_t);
    Quaterniond world_R_imu;

    if (imu_buf.empty()) {
        world_R_imu.setIdentity();
    }
    else {
        world_R_imu = Quaterniond(imu_buf.front()->orientation.w,
                                  imu_buf.front()->orientation.x,
                                  imu_buf.front()->orientation.y,
                                  imu_buf.front()->orientation.z);
    }
    Vector3d world_T_shield = world_R_imu.toRotationMatrix() * imu_T_shield;
    pub_debug_update(pnp.header, world_T_shield, world_R_imu, cur_t);

    MatrixXd C = MatrixXd::Zero(3, 6);
    C.block<3, 3>(0, 0) = MatrixXd::Identity(3, 3);

    Matrix3d W = MatrixXd::Identity(3, 3);

    MatrixXd K(6, 3);
    K = P * C.transpose() * (C * P * C.transpose() + W * Q * W.transpose()).inverse();
//    cout << "C " << endl << C << endl;
//    cout << "K " << endl << K << endl;
//    cout << "Q " << endl << Q << endl;
    x = x + K * (world_T_shield - C * x);
    P = P - K * C * P;
//    cout << "P " << endl << P << endl;
//    cout << "x " << endl << x.transpose() << endl;

    if (imu_buf.size() > 1) {
        // repropagate();
    }
}

/**
 * initialize the imu messages
 * @param imu
 */
static void initialize_imu(const sensor_msgs::Imu::ConstPtr &imu)
{
    t_prev = imu->header.stamp.toSec();
    x_history.push(x);
    P_history.push(P);
    imu_buf.push(imu);
    imu_count++;
    if (imu_count == IMU_INIT_COUNT) {
        imu_initialized = true;
    }
}

/**
 * initialization of the state and convariance from visual
 * @param pnp
 */
static void initialize_visual(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    double cur_t = pnp->header.stamp.toSec();
    Vector3d camera_T_shield;
    ROS_INFO("visual init at %f", cur_t);

    camera_T_shield[0] = pnp->twist.linear.x;
    camera_T_shield[1] = pnp->twist.linear.y;
    camera_T_shield[2] = pnp->twist.linear.z;
    Vector3d imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;

    imu_T_shield *= 0.001; // Convert millimeter to meter

    throwState(cur_t);

    Quaterniond world_R_imu;

    if (imu_buf.empty()) {
        world_R_imu.setIdentity();
    }
    else {
        world_R_imu = Quaterniond(imu_buf.front()->orientation.w,
                                  imu_buf.front()->orientation.x,
                                  imu_buf.front()->orientation.y,
                                  imu_buf.front()->orientation.z);
    }
    Vector3d world_T_shield = world_R_imu.toRotationMatrix() * imu_T_shield;
    pub_debug_update(pnp->header, world_T_shield, world_R_imu, cur_t);

    x.setZero();
    x.segment<3>(0) = world_T_shield;
    x.segment<3>(3) << 0, 0, 0;

    cout << "DEBUG: x initialized with " << endl << x.transpose() << endl;

    visual_initialized = true;

    // repropagate();
}

/**
 * handle, save, and process visual messages
 * @param pnp
 */
void visual_callback(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    visual_valid = !(pnp->twist.linear.x == 0 &&
        pnp->twist.linear.y == 0 &&
        pnp->twist.linear.z == 0 );

    if (visual_valid) {
        if (visual_initialized) {
            update(*pnp);
        }
        else {
            initialize_visual(pnp);
        }
    }
}

/**
 * handle 400Hz raw imu data
 * @param imu
 */
void imu_callback(const sensor_msgs::Imu::ConstPtr &imu)
{
    if (!imu_initialized) {
        initialize_imu(imu);
    }
    else {
        propagate(*imu);
        x_history.push(x);
        P_history.push(P);
        imu_buf.push(imu);
		if (imu_buf.size() > MAX_GYRO_QUEUE_SIZE) {
			x_history.pop();
			P_history.pop();
			imu_buf.pop();
		}
        pub_shield_odom(imu->header);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "visual_gyro_fused");
    ros::NodeHandle n("~");

//    n.param("imu_raw", imu_topic, string("/dji_sdk/imu")); // 400Hz
    n.param("imu_pose", pose_topic, string("/attitude_estimator/imu")); // 400Hz
    n.param("visual_topic", visual_topic, string("/pnp_twist"));
    n.param("publisher_topic", publisher_topic, string("/visual_ekf/shield_T_world"));
    n.param("debug_topic", debug_topic, string("/visual_ekf/debug_odom"));
    n.param("accelerometer_noise_weight", acc_weight, 1000.0);
    n.param("visual_pose_weight", visual_q_weight, 10.0);
    n.param("node_sleep_time", sleep_time, 0);

    ros::Duration(sleep_time).sleep();

    // TODO: initalize the R and Q matrix
    R =      acc_weight * MatrixXd::Identity(3, 3); // accelerometer noise
    Q = visual_q_weight * MatrixXd::Identity(3, 3); // observation noise

    x.setZero();

    imu_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;

    imu_T_camera <<  200, 50, 0; // in millimeter

    ros::Subscriber s2 = n.subscribe(visual_topic, 10, visual_callback);
    ros::Subscriber s3 = n.subscribe(pose_topic, 100, imu_callback);
//    pose_pub = n.advertise<geometry_msgs::PoseStamped>(publisher_topic, 100);
    odom_pub = n.advertise<nav_msgs::Odometry>(publisher_topic, 100);
    debug_pub= n.advertise<nav_msgs::Odometry>(debug_topic, 100);
    debug_propagate_pub = n.advertise<geometry_msgs::TwistStamped>(string("/visual_ekf/propagate"), 100);

    ros::Rate r(100);
    ros::spin();
}

/**
 *  0   T_x     translation of the shield in world frame
 *  1   T_y
 *  2   T_z
 *  3   v_x     velocity of the shield in world frame
 *  4   v_y
 *  5   v_z
 */


#include <iostream>
#include <fstream>
#include <ctime>
#include <cmath>
#include <vector>
#include <set>
#include <map>
#include <sstream>
#include <algorithm>

// #define CMG_VERBOSE
#define MEMORYBOOKKEEPING

#include "Model.h"

#include "Map.h"
#include "ChompMap.h"
#include "MorseGraph.h"
#include "Compute_Morse_Graph.h"
#include "RectGeo.h"

#include "SingleOutput.h"
#include "simple_interval.h"

#include "Configuration.h"

#include "libgp/libgp.hpp"

#include <boost/serialization/export.hpp>
#include "SuccinctGrid.h"
BOOST_CLASS_EXPORT_IMPLEMENT(SuccinctGrid);
#include "PointerGrid.h"
BOOST_CLASS_EXPORT_IMPLEMENT(PointerGrid);

std::pair<MorseGraph, MapGraph> ComputeConleyMorseGraph ( Model const& model ) {
  std::shared_ptr<const Map> map = model . map ();
  MorseGraph morsegraph ( model . phaseSpace () );
  std::shared_ptr < Grid > phase_space = morsegraph . phaseSpace ();

  int phase_subdiv_init = model . phase_subdiv_init ();
  int phase_subdiv_min = model . phase_subdiv_min ();
  int phase_subdiv_max = model . phase_subdiv_max ();
  int phase_subdiv_limit = model . phase_subdiv_limit ();

  // Compute Morse graph
  Compute_Morse_Graph ( & morsegraph, phase_space, map, phase_subdiv_init,
                        phase_subdiv_min, phase_subdiv_max, phase_subdiv_limit );

  std::shared_ptr < TreeGrid > phase_space_chomp =
    std::dynamic_pointer_cast<TreeGrid> ( morsegraph . phaseSpace () );

  if ( not phase_space_chomp ) {
    throw std::runtime_error ( "Cannot interface with chomp for this grid type!" );
  }

  typedef std::vector < Grid::GridElement > Subset;
  for ( size_t v = 0; v < morsegraph . NumVertices (); ++ v) {
    Subset subset = phase_space_chomp -> subset ( * morsegraph . grid ( v ) );
    std::shared_ptr<chomp::ConleyIndex_t> conley ( new chomp::ConleyIndex_t );
    morsegraph . conleyIndex ( v ) = conley;
    ChompMap chomp_map ( map );
    chomp::ConleyIndex ( conley . get (), *phase_space_chomp, subset, chomp_map );
  }

  // Compute multi-valued map digraph
  MapGraph map_graph ( phase_space, map );

  return std::make_pair ( morsegraph, map_graph );
}

std::pair<MorseGraph, MapGraph> ComputeMorseGraph ( Model const& model ) {
  std::shared_ptr<const Map> map = model . map ();
  MorseGraph morsegraph ( model . phaseSpace () );
  std::shared_ptr < Grid > phase_space = morsegraph . phaseSpace ();

  int phase_subdiv_init = model . phase_subdiv_init ();
  int phase_subdiv_min = model . phase_subdiv_min ();
  int phase_subdiv_max = model . phase_subdiv_max ();
  int phase_subdiv_limit = model . phase_subdiv_limit ();

  // Compute Morse graph
  Compute_Morse_Graph ( & morsegraph, phase_space, map, phase_subdiv_init,
                        phase_subdiv_min, phase_subdiv_max, phase_subdiv_limit );

  // Compute multi-valued map digraph
  MapGraph map_graph ( phase_space, map );

  return std::make_pair ( morsegraph, map_graph );
}

void computeMorseGraph ( MorseGraph & morsegraph,
                         std::shared_ptr<const Map> map,
                         const int SINGLECMG_INIT_PHASE_SUBDIVISIONS,
                         const int SINGLECMG_MIN_PHASE_SUBDIVISIONS,
                         const int SINGLECMG_MAX_PHASE_SUBDIVISIONS,
                         const int SINGLECMG_COMPLEXITY_LIMIT,
                         const char * outputfile ) {
#ifdef CMG_VERBOSE
  std::cout << "SingleCMG: computeMorseGraph.\n";
#endif
  std::shared_ptr < Grid > phase_space = morsegraph . phaseSpace ();
  clock_t start_time = clock ();
  Compute_Morse_Graph ( & morsegraph,
                        phase_space,
                        map,
                        SINGLECMG_INIT_PHASE_SUBDIVISIONS,
                        SINGLECMG_MIN_PHASE_SUBDIVISIONS,
                        SINGLECMG_MAX_PHASE_SUBDIVISIONS,
                        SINGLECMG_COMPLEXITY_LIMIT );
  clock_t stop_time = clock ();
  if ( outputfile != NULL ) {
    morsegraph . save ( outputfile );
  }
  std::ofstream stats_file ( "SingleCMG_statistics.txt" );
  stats_file << "Morse Graph calculation resource usage statistics.\n";
  stats_file << "The final grid has " << phase_space -> size () << " grid elements.\n";
  stats_file << "The computation took " << ((double)(stop_time-start_time)/(double)CLOCKS_PER_SEC)
             << " seconds.\n";
  stats_file << "All memory figures are in bytes:\n";
  stats_file << "grid_memory_use = " << phase_space -> memory () << "\n";
  stats_file << "max_graph_memory = " << max_graph_memory << "\n";
  stats_file << "max_scc_memory_internal = " << max_scc_memory_internal << "\n";
  stats_file << "max_scc_memory_external = " << max_scc_memory_external << "\n";
  stats_file . close ();
}

MorseGraph MorseGraphIntvalMap ( int phase_subdiv_min, int phase_subdiv_max,
                                 std::vector<double> const& phase_lower_bounds,
                                 std::vector<double> const& phase_upper_bounds,
                                 std::vector<double> const& params,
                                 std::string output_file_name ) {
  std::vector<double> param_lower_bounds = params;
  std::vector<double> param_upper_bounds = params;
  int param_dim = params . size();
  int phase_dim = phase_lower_bounds . size();
  std::vector<bool> phase_periodic ( phase_dim, false );
  int phase_subdiv_init = 0;
  int phase_subdiv_limit = 10000;

  Model model;
  model . initialize ( param_dim, phase_dim,
                       phase_subdiv_min, phase_subdiv_max,
                       phase_subdiv_init, phase_subdiv_limit,
                       param_lower_bounds, param_upper_bounds,
                       phase_lower_bounds, phase_upper_bounds,
                       phase_periodic );
  std::shared_ptr<const Map> map = model . map ();

  MorseGraph morsegraph ( model . phaseSpace () );

  // INITIALIZE THE PHASE SPACE SUBDIVISION PARAMETERS
  int SINGLECMG_INIT_PHASE_SUBDIVISIONS = phase_subdiv_init;
  int SINGLECMG_MIN_PHASE_SUBDIVISIONS = phase_subdiv_min;
  int SINGLECMG_MAX_PHASE_SUBDIVISIONS = phase_subdiv_max;
  int SINGLECMG_COMPLEXITY_LIMIT= phase_subdiv_limit;

  // COMPUTE MORSE GRAPH
  computeMorseGraph ( morsegraph, map,
                      SINGLECMG_INIT_PHASE_SUBDIVISIONS,
                      SINGLECMG_MIN_PHASE_SUBDIVISIONS,
                      SINGLECMG_MAX_PHASE_SUBDIVISIONS,
                      SINGLECMG_COMPLEXITY_LIMIT,
                      output_file_name . c_str () );

  std::cout << "Total Time for Finding Morse Sets ";
  std::cout << "and reachability relation: ";
  std::cout << ": ";

  // Always output the Morse Graph
  // std::cout << "Creating graphviz .dot file...\n";
  // CreateDotFile ( "morsegraph.gv", conleymorsegraph );

  return morsegraph;
}

MorseGraph MorseGraphMap ( int phase_subdiv_min, int phase_subdiv_max,
                           std::vector<double> const& phase_lower_bounds,
                           std::vector<double> const& phase_upper_bounds,
                           std::string output_file_name,
                           std::function<std::vector<double>(std::vector<double>)> const& F ) {
  std::vector<double> params {0.0};
  std::vector<double> param_lower_bounds = params;
  std::vector<double> param_upper_bounds = params;
  int param_dim = params . size();
  int phase_dim = phase_lower_bounds . size();
  std::vector<bool> phase_periodic ( phase_dim, false );
  int phase_subdiv_init = 0;
  int phase_subdiv_limit = 10000;

  Model model;
  model . initialize ( param_dim, phase_dim,
                       phase_subdiv_min, phase_subdiv_max,
                       phase_subdiv_init, phase_subdiv_limit,
                       param_lower_bounds, param_upper_bounds,
                       phase_lower_bounds, phase_upper_bounds,
                       phase_periodic, F );
  std::shared_ptr<const Map> map = model . map ();

  MorseGraph morsegraph ( model . phaseSpace () );

  // INITIALIZE THE PHASE SPACE SUBDIVISION PARAMETERS
  int SINGLECMG_INIT_PHASE_SUBDIVISIONS = phase_subdiv_init;
  int SINGLECMG_MIN_PHASE_SUBDIVISIONS = phase_subdiv_min;
  int SINGLECMG_MAX_PHASE_SUBDIVISIONS = phase_subdiv_max;
  int SINGLECMG_COMPLEXITY_LIMIT= phase_subdiv_limit;

  // COMPUTE MORSE GRAPH
  computeMorseGraph ( morsegraph, map,
                      SINGLECMG_INIT_PHASE_SUBDIVISIONS,
                      SINGLECMG_MIN_PHASE_SUBDIVISIONS,
                      SINGLECMG_MAX_PHASE_SUBDIVISIONS,
                      SINGLECMG_COMPLEXITY_LIMIT,
                      output_file_name . c_str () );

  std::cout << "Total Time for Finding Morse Sets ";
  std::cout << "and reachability relation: ";
  std::cout << ": ";

  // Always output the Morse Graph
  // std::cout << "Creating graphviz .dot file...\n";
  // CreateDotFile ( "morsegraph.gv", conleymorsegraph );

  return morsegraph;
}

/// Python Bindings

#include <pybind11/pybind11.h>
#include <pybind11/functional.h>
#include <pybind11/stl.h>

namespace py = pybind11;

PYBIND11_MODULE(_cmgdb, m) {
  ModelBinding(m);
  GridBinding(m);
  MapGraphBinding(m);
  MorseGraphBinding(m);

  m.doc() = "Conley Morse Graph Database Module";

  m.def("ComputeConleyMorseGraph", &ComputeConleyMorseGraph);
  m.def("ComputeMorseGraph", &ComputeMorseGraph);
  m.def("MorseGraphIntvalMap", &MorseGraphIntvalMap);
  m.def("MorseGraphMap", &MorseGraphMap);
}