/////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2007-2014
//
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/intrusive for documentation.
//
/////////////////////////////////////////////////////////////////////////////
//
// The option that yields to non-floating point 1/sqrt(2) alpha is taken
// from the scapegoat tree implementation of the PSPP library.
//
/////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_INTRUSIVE_SGTREE_HPP
#define BOOST_INTRUSIVE_SGTREE_HPP

#include <boost/intrusive/detail/config_begin.hpp>
#include <boost/intrusive/intrusive_fwd.hpp>
#include <boost/intrusive/detail/assert.hpp>
#include <boost/static_assert.hpp>
#include <boost/intrusive/bs_set_hook.hpp>
#include <boost/intrusive/bstree.hpp>
#include <boost/intrusive/detail/tree_node.hpp>
#include <boost/intrusive/pointer_traits.hpp>
#include <boost/intrusive/detail/mpl.hpp>
#include <boost/intrusive/detail/math.hpp>
#include <boost/intrusive/detail/get_value_traits.hpp>
#include <boost/intrusive/sgtree_algorithms.hpp>
#include <boost/intrusive/detail/key_nodeptr_comp.hpp>
#include <boost/intrusive/link_mode.hpp>

#include <boost/move/utility_core.hpp>
#include <boost/move/adl_move_swap.hpp>

#include <cstddef>
#include <boost/intrusive/detail/minimal_less_equal_header.hpp>
#include <boost/intrusive/detail/minimal_pair_header.hpp>   //std::pair
#include <cmath>
#include <cstddef>

#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

namespace boost {
namespace intrusive {

/// @cond

namespace detail{

/////////////////////////////////////////////////////////////
//
//       Halpha for fixed floating_point<false> option
//
/////////////////////////////////////////////////////////////

//! Returns floor(log2(n)/log2(sqrt(2))) -> floor(2*log2(n))
//! Undefined if N is 0.
//!
//! This function does not use float point operations.
inline std::size_t calculate_h_sqrt2 (std::size_t n)
{
   std::size_t f_log2 = detail::floor_log2(n);
   return (2*f_log2) + static_cast<std::size_t>(n >= detail::sqrt2_pow_2xplus1(f_log2));
}

struct h_alpha_sqrt2_t
{
   h_alpha_sqrt2_t(void){}
   std::size_t operator()(std::size_t n) const
   {  return calculate_h_sqrt2(n);  }
};

struct alpha_0_75_by_max_size_t
{
   alpha_0_75_by_max_size_t(void){}

   std::size_t operator()(std::size_t max_tree_size) const
   {
      const std::size_t max_tree_size_limit = ((~std::size_t(0))/std::size_t(3));
      return max_tree_size > max_tree_size_limit ? max_tree_size/4*3 : max_tree_size*3/4;
   }
};

/////////////////////////////////////////////////////////////
//
//       Halpha for fixed floating_point<true> option
//
/////////////////////////////////////////////////////////////

struct h_alpha_t
{
   explicit h_alpha_t(float inv_minus_logalpha)
      :  inv_minus_logalpha_(inv_minus_logalpha)
   {}

   std::size_t operator()(std::size_t n) const
   {
      ////////////////////////////////////////////////////////////
      // This function must return "floor(log2(1/alpha(n)))" ->
      //    floor(log2(n)/log(1/alpha)) ->
      //    floor(log2(n)/-log2(alpha))
      //    floor(log2(n)*(1/-log2(alpha)))
      ////////////////////////////////////////////////////////////
      return static_cast<std::size_t>(detail::fast_log2(float(n))*inv_minus_logalpha_);
   }

   private:
   //Since the function will be repeatedly called
   //precalculate constant data to avoid repeated
   //calls to log and division.
   //This will store 1/(-std::log2(alpha_))
   float inv_minus_logalpha_;
};

struct alpha_by_max_size_t
{
   explicit alpha_by_max_size_t(float alpha)
      :  alpha_(alpha)
   {}

   float operator()(std::size_t max_tree_size) const
   {  return float(max_tree_size)*alpha_;   }

   private:
   float alpha_;
};

template<bool Activate, class SizeType>
struct alpha_holder
{
   typedef boost::intrusive::detail::h_alpha_t           h_alpha_t;
   typedef boost::intrusive::detail::alpha_by_max_size_t multiply_by_alpha_t;

   alpha_holder()
      : max_tree_size_()
   {  set_alpha(0.70711f);   } // ~1/sqrt(2)

   float get_alpha() const
   {  return alpha_;  }

   void set_alpha(float alpha)
   {
      alpha_ = alpha;
      inv_minus_logalpha_ = 1/(-detail::fast_log2(alpha));
   }

   h_alpha_t get_h_alpha_t() const
   {  return h_alpha_t(inv_minus_logalpha_);  }

   multiply_by_alpha_t get_multiply_by_alpha_t() const
   {  return multiply_by_alpha_t(alpha_);  }

   SizeType &get_max_tree_size()
   {  return max_tree_size_;  }

   protected:
   float alpha_;
   float inv_minus_logalpha_;
   SizeType max_tree_size_;
};

template<class SizeType>
struct alpha_holder<false, SizeType>
{
   //This specialization uses alpha = 1/sqrt(2)
   //without using floating point operations
   //Downside: alpha CAN't be changed.
   typedef boost::intrusive::detail::h_alpha_sqrt2_t           h_alpha_t;
   typedef boost::intrusive::detail::alpha_0_75_by_max_size_t  multiply_by_alpha_t;

   alpha_holder()
      : max_tree_size_()
   {}

   float get_alpha() const
   {  return 0.70710677f;  }

   void set_alpha(float)
   {  //alpha CAN't be changed.
      BOOST_INTRUSIVE_INVARIANT_ASSERT(0);
   }

   h_alpha_t get_h_alpha_t() const
   {  return h_alpha_t();  }

   multiply_by_alpha_t get_multiply_by_alpha_t() const
   {  return multiply_by_alpha_t();  }

   SizeType &get_max_tree_size()
   {  return max_tree_size_;  }

   protected:
   SizeType max_tree_size_;
};

}  //namespace detail{

struct sgtree_defaults
   : bstree_defaults
{
   static const bool floating_point = true;
};

/// @endcond

//! The class template sgtree is an intrusive scapegoat tree container, that
//! is used to construct intrusive sg_set and sg_multiset containers.
//! The no-throw guarantee holds only, if the value_compare object
//! doesn't throw.
//!
//! The template parameter \c T is the type to be managed by the container.
//! The user can specify additional options and if no options are provided
//! default options are used.
//!
//! The container supports the following options:
//! \c base_hook<>/member_hook<>/value_traits<>,
//! \c floating_point<>, \c size_type<> and
//! \c compare<>.
#if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED)
template<class T, class ...Options>
#else
template<class ValueTraits, class VoidOrKeyOfValue, class VoidOrKeyComp, class SizeType, bool FloatingPoint, typename HeaderHolder>
#endif
class sgtree_impl
   /// @cond
   :  public bstree_impl<ValueTraits, VoidOrKeyOfValue, VoidOrKeyComp, SizeType, true, SgTreeAlgorithms, HeaderHolder>
   ,  public detail::alpha_holder<FloatingPoint, SizeType>
   /// @endcond
{
   public:
   typedef ValueTraits                                               value_traits;
   /// @cond
   typedef bstree_impl< ValueTraits, VoidOrKeyOfValue, VoidOrKeyComp, SizeType
                      , true, SgTreeAlgorithms, HeaderHolder>        tree_type;
   typedef tree_type                                                 implementation_defined;

   /// @endcond

   typedef typename implementation_defined::pointer                  pointer;
   typedef typename implementation_defined::const_pointer            const_pointer;
   typedef typename implementation_defined::value_type               value_type;
   typedef typename implementation_defined::key_type                 key_type;
   typedef typename implementation_defined::key_of_value             key_of_value;
   typedef typename implementation_defined::reference                reference;
   typedef typename implementation_defined::const_reference          const_reference;
   typedef typename implementation_defined::difference_type          difference_type;
   typedef typename implementation_defined::size_type                size_type;
   typedef typename implementation_defined::value_compare            value_compare;
   typedef typename implementation_defined::key_compare              key_compare;
   typedef typename implementation_defined::iterator                 iterator;
   typedef typename implementation_defined::const_iterator           const_iterator;
   typedef typename implementation_defined::reverse_iterator         reverse_iterator;
   typedef typename implementation_defined::const_reverse_iterator   const_reverse_iterator;
   typedef typename implementation_defined::node_traits              node_traits;
   typedef typename implementation_defined::node                     node;
   typedef typename implementation_defined::node_ptr                 node_ptr;
   typedef typename implementation_defined::const_node_ptr           const_node_ptr;
   typedef BOOST_INTRUSIVE_IMPDEF(sgtree_algorithms<node_traits>)    node_algorithms;

   static const bool constant_time_size      = implementation_defined::constant_time_size;
   static const bool floating_point          = FloatingPoint;
   static const bool stateful_value_traits   = implementation_defined::stateful_value_traits;

   /// @cond
   private:

   //noncopyable
   typedef detail::alpha_holder<FloatingPoint, SizeType>    alpha_traits;
   typedef typename alpha_traits::h_alpha_t                 h_alpha_t;
   typedef typename alpha_traits::multiply_by_alpha_t       multiply_by_alpha_t;

   BOOST_MOVABLE_BUT_NOT_COPYABLE(sgtree_impl)
   BOOST_STATIC_ASSERT(((int)value_traits::link_mode != (int)auto_unlink));

   enum { safemode_or_autounlink  =
            (int)value_traits::link_mode == (int)auto_unlink   ||
            (int)value_traits::link_mode == (int)safe_link     };

   /// @endcond

   public:

   typedef BOOST_INTRUSIVE_IMPDEF(typename node_algorithms::insert_commit_data) insert_commit_data;

   //! @copydoc ::boost::intrusive::bstree::bstree()
   sgtree_impl()
      :  tree_type()
   {}

   //! @copydoc ::boost::intrusive::bstree::bstree(const key_compare &,const value_traits &)
   explicit sgtree_impl( const key_compare &cmp, const value_traits &v_traits = value_traits())
      :  tree_type(cmp, v_traits)
   {}

   //! @copydoc ::boost::intrusive::bstree::bstree(bool,Iterator,Iterator,const key_compare &,const value_traits &)
   template<class Iterator>
   sgtree_impl( bool unique, Iterator b, Iterator e
              , const key_compare &cmp     = key_compare()
              , const value_traits &v_traits = value_traits())
      : tree_type(cmp, v_traits)
   {
      if(unique)
         this->insert_unique(b, e);
      else
         this->insert_equal(b, e);
   }

   //! @copydoc ::boost::intrusive::bstree::bstree(bstree &&)
   sgtree_impl(BOOST_RV_REF(sgtree_impl) x)
      :  tree_type(BOOST_MOVE_BASE(tree_type, x)), alpha_traits(x.get_alpha_traits())
   {  ::boost::adl_move_swap(this->get_alpha_traits(), x.get_alpha_traits());   }

   //! @copydoc ::boost::intrusive::bstree::operator=(bstree &&)
   sgtree_impl& operator=(BOOST_RV_REF(sgtree_impl) x)
   {
      this->get_alpha_traits() = x.get_alpha_traits();
      return static_cast<sgtree_impl&>(tree_type::operator=(BOOST_MOVE_BASE(tree_type, x)));
   }

   /// @cond
   private:

   const alpha_traits &get_alpha_traits() const
   {  return *this;  }

   alpha_traits &get_alpha_traits()
   {  return *this;  }

   h_alpha_t get_h_alpha_func() const
   {  return this->get_alpha_traits().get_h_alpha_t();  }

   multiply_by_alpha_t get_alpha_by_max_size_func() const
   {  return this->get_alpha_traits().get_multiply_by_alpha_t(); }

   /// @endcond

   public:

   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree::~bstree()
   ~sgtree_impl();

   //! @copydoc ::boost::intrusive::bstree::begin()
   iterator begin();

   //! @copydoc ::boost::intrusive::bstree::begin()const
   const_iterator begin() const;

   //! @copydoc ::boost::intrusive::bstree::cbegin()const
   const_iterator cbegin() const;

   //! @copydoc ::boost::intrusive::bstree::end()
   iterator end();

   //! @copydoc ::boost::intrusive::bstree::end()const
   const_iterator end() const;

   //! @copydoc ::boost::intrusive::bstree::cend()const
   const_iterator cend() const;

   //! @copydoc ::boost::intrusive::bstree::rbegin()
   reverse_iterator rbegin();

   //! @copydoc ::boost::intrusive::bstree::rbegin()const
   const_reverse_iterator rbegin() const;

   //! @copydoc ::boost::intrusive::bstree::crbegin()const
   const_reverse_iterator crbegin() const;

   //! @copydoc ::boost::intrusive::bstree::rend()
   reverse_iterator rend();

   //! @copydoc ::boost::intrusive::bstree::rend()const
   const_reverse_iterator rend() const;

   //! @copydoc ::boost::intrusive::bstree::crend()const
   const_reverse_iterator crend() const;

   //! @copydoc ::boost::intrusive::bstree::root()
   iterator root();

   //! @copydoc ::boost::intrusive::bstree::root()const
   const_iterator root() const;

   //! @copydoc ::boost::intrusive::bstree::croot()const
   const_iterator croot() const;

   //! @copydoc ::boost::intrusive::bstree::container_from_end_iterator(iterator)
   static sgtree_impl &container_from_end_iterator(iterator end_iterator);

   //! @copydoc ::boost::intrusive::bstree::container_from_end_iterator(const_iterator)
   static const sgtree_impl &container_from_end_iterator(const_iterator end_iterator);

   //! @copydoc ::boost::intrusive::bstree::container_from_iterator(iterator)
   static sgtree_impl &container_from_iterator(iterator it);

   //! @copydoc ::boost::intrusive::bstree::container_from_iterator(const_iterator)
   static const sgtree_impl &container_from_iterator(const_iterator it);

   //! @copydoc ::boost::intrusive::bstree::key_comp()const
   key_compare key_comp() const;

   //! @copydoc ::boost::intrusive::bstree::value_comp()const
   value_compare value_comp() const;

   //! @copydoc ::boost::intrusive::bstree::empty()const
   bool empty() const;

   //! @copydoc ::boost::intrusive::bstree::size()const
   size_type size() const;

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED

   //! @copydoc ::boost::intrusive::bstree::swap
   void swap(sgtree_impl& other)
   {
      //This can throw
      this->tree_type::swap(static_cast<tree_type&>(other));
      ::boost::adl_move_swap(this->get_alpha_traits(), other.get_alpha_traits());
   }

   //! @copydoc ::boost::intrusive::bstree::clone_from(const bstree&,Cloner,Disposer)
   //! Additional notes: it also copies the alpha factor from the source container.
   template <class Cloner, class Disposer>
   void clone_from(const sgtree_impl &src, Cloner cloner, Disposer disposer)
   {
      tree_type::clone_from(src, cloner, disposer);
      this->get_alpha_traits() = src.get_alpha_traits();
   }

   //! @copydoc ::boost::intrusive::bstree::clone_from(bstree&&,Cloner,Disposer)
   //! Additional notes: it also copies the alpha factor from the source container.
   template <class Cloner, class Disposer>
   void clone_from(BOOST_RV_REF(sgtree_impl) src, Cloner cloner, Disposer disposer)
   {
      tree_type::clone_from(BOOST_MOVE_BASE(tree_type, src), cloner, disposer);
      this->get_alpha_traits() = ::boost::move(src.get_alpha_traits());
   }

   //! @copydoc ::boost::intrusive::bstree::insert_equal(reference)
   iterator insert_equal(reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_ptr p = node_algorithms::insert_equal_upper_bound
         (this->tree_type::header_ptr(), to_insert, this->key_node_comp(this->key_comp())
         , (size_type)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
      return iterator(p, this->priv_value_traits_ptr());
   }

   //! @copydoc ::boost::intrusive::bstree::insert_equal(const_iterator,reference)
   iterator insert_equal(const_iterator hint, reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_ptr p = node_algorithms::insert_equal
         ( this->tree_type::header_ptr(), hint.pointed_node(), to_insert, this->key_node_comp(this->key_comp())
         , (std::size_t)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
      return iterator(p, this->priv_value_traits_ptr());
   }

   //! @copydoc ::boost::intrusive::bstree::insert_equal(Iterator,Iterator)
   template<class Iterator>
   void insert_equal(Iterator b, Iterator e)
   {
      iterator iend(this->end());
      for (; b != e; ++b)
         this->insert_equal(iend, *b);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique(reference)
   std::pair<iterator, bool> insert_unique(reference value)
   {
      insert_commit_data commit_data;
      std::pair<iterator, bool> ret = this->insert_unique_check
         (key_of_value()(value), this->key_comp(), commit_data);
      if(!ret.second)
         return ret;
      return std::pair<iterator, bool> (this->insert_unique_commit(value, commit_data), true);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique(const_iterator,reference)
   iterator insert_unique(const_iterator hint, reference value)
   {
      insert_commit_data commit_data;
      std::pair<iterator, bool> ret = this->insert_unique_check
         (hint, key_of_value()(value), this->key_comp(), commit_data);
      if(!ret.second)
         return ret.first;
      return this->insert_unique_commit(value, commit_data);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_check(const KeyType&,KeyTypeKeyCompare,insert_commit_data&)
   template<class KeyType, class KeyTypeKeyCompare>
   BOOST_INTRUSIVE_DOC1ST(std::pair<iterator BOOST_INTRUSIVE_I bool>
      , typename detail::disable_if_convertible
         <KeyType BOOST_INTRUSIVE_I const_iterator BOOST_INTRUSIVE_I
         std::pair<iterator BOOST_INTRUSIVE_I bool> >::type)
      insert_unique_check
      (const KeyType &key, KeyTypeKeyCompare comp, insert_commit_data &commit_data)
   {
      std::pair<node_ptr, bool> ret =
         node_algorithms::insert_unique_check
            (this->tree_type::header_ptr(), key, this->key_node_comp(comp), commit_data);
      return std::pair<iterator, bool>(iterator(ret.first, this->priv_value_traits_ptr()), ret.second);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_check(const_iterator,const KeyType&,KeyTypeKeyCompare,insert_commit_data&)
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<iterator, bool> insert_unique_check
      (const_iterator hint, const KeyType &key
      ,KeyTypeKeyCompare comp, insert_commit_data &commit_data)
   {
      std::pair<node_ptr, bool> ret =
         node_algorithms::insert_unique_check
            (this->tree_type::header_ptr(), hint.pointed_node(), key, this->key_node_comp(comp), commit_data);
      return std::pair<iterator, bool>(iterator(ret.first, this->priv_value_traits_ptr()), ret.second);
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_check(const key_type&,insert_commit_data&)
   std::pair<iterator, bool> insert_unique_check
      (const key_type &key, insert_commit_data &commit_data)
   {  return this->insert_unique_check(key, this->key_comp(), commit_data);   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_check(const_iterator,const key_type&,insert_commit_data&)
   std::pair<iterator, bool> insert_unique_check
      (const_iterator hint, const key_type &key, insert_commit_data &commit_data)
   {  return this->insert_unique_check(hint, key, this->key_comp(), commit_data);   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique_commit
   iterator insert_unique_commit(reference value, const insert_commit_data &commit_data)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_algorithms::insert_unique_commit
         ( this->tree_type::header_ptr(), to_insert, commit_data
         , (std::size_t)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
      return iterator(to_insert, this->priv_value_traits_ptr());
   }

   //! @copydoc ::boost::intrusive::bstree::insert_unique(Iterator,Iterator)
   template<class Iterator>
   void insert_unique(Iterator b, Iterator e)
   {
      if(this->empty()){
         iterator iend(this->end());
         for (; b != e; ++b)
            this->insert_unique(iend, *b);
      }
      else{
         for (; b != e; ++b)
            this->insert_unique(*b);
      }
   }

   //! @copydoc ::boost::intrusive::bstree::insert_before
   iterator insert_before(const_iterator pos, reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_ptr p = node_algorithms::insert_before
         ( this->tree_type::header_ptr(), pos.pointed_node(), to_insert
         , (size_type)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
      return iterator(p, this->priv_value_traits_ptr());
   }

   //! @copydoc ::boost::intrusive::bstree::push_back
   void push_back(reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_algorithms::push_back
         ( this->tree_type::header_ptr(), to_insert
         , (size_type)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
   }

   //! @copydoc ::boost::intrusive::bstree::push_front
   void push_front(reference value)
   {
      node_ptr to_insert(this->get_value_traits().to_node_ptr(value));
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(node_algorithms::unique(to_insert));
      std::size_t max_tree_size = (std::size_t)this->max_tree_size_;
      node_algorithms::push_front
         ( this->tree_type::header_ptr(), to_insert
         , (size_type)this->size(), this->get_h_alpha_func(), max_tree_size);
      this->tree_type::sz_traits().increment();
      this->max_tree_size_ = (size_type)max_tree_size;
   }

   //! @copydoc ::boost::intrusive::bstree::erase(const_iterator)
   iterator erase(const_iterator i)
   {
      const_iterator ret(i);
      ++ret;
      node_ptr to_erase(i.pointed_node());
      if(safemode_or_autounlink)
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(!node_algorithms::unique(to_erase));
      std::size_t max_tree_size = this->max_tree_size_;
      node_algorithms::erase
         ( this->tree_type::header_ptr(), to_erase, (std::size_t)this->size()
         , max_tree_size, this->get_alpha_by_max_size_func());
      this->max_tree_size_ = (size_type)max_tree_size;
      this->tree_type::sz_traits().decrement();
      if(safemode_or_autounlink)
         node_algorithms::init(to_erase);
      return ret.unconst();
   }

   //! @copydoc ::boost::intrusive::bstree::erase(const_iterator,const_iterator)
   iterator erase(const_iterator b, const_iterator e)
   {  size_type n;   return private_erase(b, e, n);   }

   //! @copydoc ::boost::intrusive::bstree::erase(const key_type &)
   size_type erase(const key_type &key)
   {  return this->erase(key, this->key_comp());   }

   //! @copydoc ::boost::intrusive::bstree::erase(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   BOOST_INTRUSIVE_DOC1ST(size_type
      , typename detail::disable_if_convertible<KeyTypeKeyCompare BOOST_INTRUSIVE_I const_iterator BOOST_INTRUSIVE_I size_type>::type)
      erase(const KeyType& key, KeyTypeKeyCompare comp)
   {
      std::pair<iterator,iterator> p = this->equal_range(key, comp);
      size_type n;
      private_erase(p.first, p.second, n);
      return n;
   }

   //! @copydoc ::boost::intrusive::bstree::erase_and_dispose(const_iterator,Disposer)
   template<class Disposer>
   iterator erase_and_dispose(const_iterator i, Disposer disposer)
   {
      node_ptr to_erase(i.pointed_node());
      iterator ret(this->erase(i));
      disposer(this->get_value_traits().to_value_ptr(to_erase));
      return ret;
   }

   #if !defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED)
   template<class Disposer>
   iterator erase_and_dispose(iterator i, Disposer disposer)
   {  return this->erase_and_dispose(const_iterator(i), disposer);   }
   #endif

   //! @copydoc ::boost::intrusive::bstree::erase_and_dispose(const_iterator,const_iterator,Disposer)
   template<class Disposer>
   iterator erase_and_dispose(const_iterator b, const_iterator e, Disposer disposer)
   {  size_type n;   return private_erase(b, e, n, disposer);   }

   //! @copydoc ::boost::intrusive::bstree::erase_and_dispose(const key_type &, Disposer)
   template<class Disposer>
   size_type erase_and_dispose(const key_type &key, Disposer disposer)
   {
      std::pair<iterator,iterator> p = this->equal_range(key);
      size_type n;
      private_erase(p.first, p.second, n, disposer);
      return n;
   }

   //! @copydoc ::boost::intrusive::bstree::erase_and_dispose(const KeyType&,KeyTypeKeyCompare,Disposer)
   template<class KeyType, class KeyTypeKeyCompare, class Disposer>
   BOOST_INTRUSIVE_DOC1ST(size_type
      , typename detail::disable_if_convertible<KeyTypeKeyCompare BOOST_INTRUSIVE_I const_iterator BOOST_INTRUSIVE_I size_type>::type)
      erase_and_dispose(const KeyType& key, KeyTypeKeyCompare comp, Disposer disposer)
   {
      std::pair<iterator,iterator> p = this->equal_range(key, comp);
      size_type n;
      private_erase(p.first, p.second, n, disposer);
      return n;
   }

   //! @copydoc ::boost::intrusive::bstree::clear
   void clear()
   {
      tree_type::clear();
      this->max_tree_size_ = 0;
   }

   //! @copydoc ::boost::intrusive::bstree::clear_and_dispose
   template<class Disposer>
   void clear_and_dispose(Disposer disposer)
   {
      tree_type::clear_and_dispose(disposer);
      this->max_tree_size_ = 0;
   }

   #if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED)
   //! @copydoc ::boost::intrusive::bstree::merge_unique
   template<class T, class ...Options2> void merge_unique(sgtree<T, Options2...> &);
   #else
   template<class Compare2>
   void merge_unique(sgtree_impl
      <ValueTraits, VoidOrKeyOfValue, Compare2, SizeType, FloatingPoint, HeaderHolder> &source)
   #endif
   {
      node_ptr it   (node_algorithms::begin_node(source.header_ptr()))
             , itend(node_algorithms::end_node  (source.header_ptr()));

      while(it != itend){
         node_ptr const p(it);
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(!safemode_or_autounlink || !node_algorithms::unique(p));
         it = node_algorithms::next_node(it);

         std::size_t max_tree1_size = this->max_tree_size_;
         std::size_t max_tree2_size = source.get_max_tree_size();
         if( node_algorithms::transfer_unique
               ( this->header_ptr(), this->key_node_comp(this->key_comp()), this->size(), max_tree1_size
               , source.header_ptr(), p, source.size(), max_tree2_size
               , this->get_h_alpha_func(), this->get_alpha_by_max_size_func()) ){
            this->max_tree_size_  = (size_type)max_tree1_size;
            this->sz_traits().increment();
            source.get_max_tree_size() = (size_type)max_tree2_size;
            source.sz_traits().decrement();
         }
      }
   }

   #if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED)
   //! @copydoc ::boost::intrusive::bstree::merge_equal
   template<class T, class ...Options2> void merge_equal(sgtree<T, Options2...> &);
   #else
   template<class Compare2>
   void merge_equal(sgtree_impl
      <ValueTraits, VoidOrKeyOfValue, Compare2, SizeType, FloatingPoint, HeaderHolder> &source)
   #endif
   {
      node_ptr it   (node_algorithms::begin_node(source.header_ptr()))
             , itend(node_algorithms::end_node  (source.header_ptr()));

      while(it != itend){
         node_ptr const p(it);
         BOOST_INTRUSIVE_SAFE_HOOK_DEFAULT_ASSERT(!safemode_or_autounlink || !node_algorithms::unique(p));
         it = node_algorithms::next_node(it);
         std::size_t max_tree1_size = this->max_tree_size_;
         std::size_t max_tree2_size = source.get_max_tree_size();
         node_algorithms::transfer_equal
            ( this->header_ptr(), this->key_node_comp(this->key_comp()), this->size(), max_tree1_size
            , source.header_ptr(), p, source.size(), max_tree2_size
            , this->get_h_alpha_func(), this->get_alpha_by_max_size_func());
         this->max_tree_size_  = (size_type)max_tree1_size;
         this->sz_traits().increment();
         source.get_max_tree_size() = (size_type)max_tree2_size;
         source.sz_traits().decrement();
      }
   }

   #ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED
   //! @copydoc ::boost::intrusive::bstree::count(const key_type &)const
   size_type count(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::count(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   size_type count(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::lower_bound(const key_type &)
   iterator lower_bound(const key_type &key);

   //! @copydoc ::boost::intrusive::bstree::lower_bound(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   iterator lower_bound(const KeyType& key, KeyTypeKeyCompare comp);

   //! @copydoc ::boost::intrusive::bstree::lower_bound(const key_type &)const
   const_iterator lower_bound(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::lower_bound(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   const_iterator lower_bound(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::upper_bound(const key_type &)
   iterator upper_bound(const key_type &key);

   //! @copydoc ::boost::intrusive::bstree::upper_bound(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   iterator upper_bound(const KeyType& key, KeyTypeKeyCompare comp);

   //! @copydoc ::boost::intrusive::bstree::upper_bound(const key_type &)const
   const_iterator upper_bound(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::upper_bound(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   const_iterator upper_bound(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::find(const key_type &)
   iterator find(const key_type &key);

   //! @copydoc ::boost::intrusive::bstree::find(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   iterator find(const KeyType& key, KeyTypeKeyCompare comp);

   //! @copydoc ::boost::intrusive::bstree::find(const key_type &)const
   const_iterator find(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::find(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   const_iterator find(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::equal_range(const key_type &)
   std::pair<iterator,iterator> equal_range(const key_type &key);

   //! @copydoc ::boost::intrusive::bstree::equal_range(const KeyType&,KeyTypeKeyCompare)
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<iterator,iterator> equal_range(const KeyType& key, KeyTypeKeyCompare comp);

   //! @copydoc ::boost::intrusive::bstree::equal_range(const key_type &)const
   std::pair<const_iterator, const_iterator>
      equal_range(const key_type &key) const;

   //! @copydoc ::boost::intrusive::bstree::equal_range(const KeyType&,KeyTypeKeyCompare)const
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<const_iterator, const_iterator>
      equal_range(const KeyType& key, KeyTypeKeyCompare comp) const;

   //! @copydoc ::boost::intrusive::bstree::bounded_range(const key_type &,const key_type &,bool,bool)
   std::pair<iterator,iterator> bounded_range
      (const key_type &lower_key, const key_type &upper_key, bool left_closed, bool right_closed);

   //! @copydoc ::boost::intrusive::bstree::bounded_range(const KeyType&,const KeyType&,KeyTypeKeyCompare,bool,bool)
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<iterator,iterator> bounded_range
      (const KeyType& lower_key, const KeyType& upper_key, KeyTypeKeyCompare comp, bool left_closed, bool right_closed);

   //! @copydoc ::boost::intrusive::bstree::bounded_range(const key_type &,const key_type &,bool,bool)const
   std::pair<const_iterator, const_iterator>
      bounded_range(const key_type &lower_key, const key_type &upper_key, bool left_closed, bool right_closed) const;

   //! @copydoc ::boost::intrusive::bstree::bounded_range(const KeyType&,const KeyType&,KeyTypeKeyCompare,bool,bool)const
   template<class KeyType, class KeyTypeKeyCompare>
   std::pair<const_iterator, const_iterator> bounded_range
         (const KeyType& lower_key, const KeyType& upper_key, KeyTypeKeyCompare comp, bool left_closed, bool right_closed) const;

   //! @copydoc ::boost::intrusive::bstree::s_iterator_to(reference)
   static iterator s_iterator_to(reference value);

   //! @copydoc ::boost::intrusive::bstree::s_iterator_to(const_reference)
   static const_iterator s_iterator_to(const_reference value);

   //! @copydoc ::boost::intrusive::bstree::iterator_to(reference)
   iterator iterator_to(reference value);

   //! @copydoc ::boost::intrusive::bstree::iterator_to(const_reference)const
   const_iterator iterator_to(const_reference value) const;

   //! @copydoc ::boost::intrusive::bstree::init_node(reference)
   static void init_node(reference value);

   //! @copydoc ::boost::intrusive::bstree::unlink_leftmost_without_rebalance
   pointer unlink_leftmost_without_rebalance();

   //! @copydoc ::boost::intrusive::bstree::replace_node
   void replace_node(iterator replace_this, reference with_this);

   //! @copydoc ::boost::intrusive::bstree::remove_node
   void remove_node(reference value);

   //! @copydoc ::boost::intrusive::bstree::rebalance
   void rebalance();

   //! @copydoc ::boost::intrusive::bstree::rebalance_subtree
   iterator rebalance_subtree(iterator root);

   friend bool operator< (const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator==(const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator!= (const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator>(const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator<=(const sgtree_impl &x, const sgtree_impl &y);

   friend bool operator>=(const sgtree_impl &x, const sgtree_impl &y);

   friend void swap(sgtree_impl &x, sgtree_impl &y);

   #endif   //#ifdef BOOST_INTRUSIVE_DOXYGEN_INVOKED

   //! <b>Returns</b>: The balance factor (alpha) used in this tree
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Complexity</b>: Constant.
   float balance_factor() const
   {  return this->get_alpha_traits().get_alpha(); }

   //! <b>Requires</b>: new_alpha must be a value between 0.5 and 1.0
   //!
   //! <b>Effects</b>: Establishes a new balance factor (alpha) and rebalances
   //!   the tree if the new balance factor is stricter (less) than the old factor.
   //!
   //! <b>Throws</b>: Nothing.
   //!
   //! <b>Complexity</b>: Linear to the elements in the subtree.
   void balance_factor(float new_alpha)
   {
      //The alpha factor CAN't be changed if the fixed, floating operation-less
      //1/sqrt(2) alpha factor option is activated
      BOOST_STATIC_ASSERT((floating_point));
      BOOST_INTRUSIVE_INVARIANT_ASSERT((new_alpha > 0.5f && new_alpha < 1.0f));
      if(new_alpha >= 0.5f && new_alpha < 1.0f){
         float old_alpha = this->get_alpha_traits().get_alpha();
         this->get_alpha_traits().set_alpha(new_alpha);
         if(new_alpha < old_alpha){
            this->max_tree_size_ = this->size();
            this->rebalance();
         }
      }
   }

   /// @cond
   private:
   template<class Disposer>
   iterator private_erase(const_iterator b, const_iterator e, size_type &n, Disposer disposer)
   {
      for(n = 0; b != e; ++n)
        this->erase_and_dispose(b++, disposer);
      return b.unconst();
   }

   iterator private_erase(const_iterator b, const_iterator e, size_type &n)
   {
      for(n = 0; b != e; ++n)
        this->erase(b++);
      return b.unconst();
   }
   /// @endcond
};

//! Helper metafunction to define a \c sgtree that yields to the same type when the
//! same options (either explicitly or implicitly) are used.
#if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED) || defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
template<class T, class ...Options>
#else
template<class T, class O1 = void, class O2 = void
                , class O3 = void, class O4 = void
                , class O5 = void, class O6 = void>
#endif
struct make_sgtree
{
   /// @cond
   typedef typename pack_options
      < sgtree_defaults,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
      O1, O2, O3, O4, O5, O6
      #else
      Options...
      #endif
      >::type packed_options;

   typedef typename detail::get_value_traits
      <T, typename packed_options::proto_value_traits>::type value_traits;

   typedef sgtree_impl
         < value_traits
         , typename packed_options::key_of_value
         , typename packed_options::compare
         , typename packed_options::size_type
         , packed_options::floating_point
         , typename packed_options::header_holder_type
         > implementation_defined;
   /// @endcond
   typedef implementation_defined type;
};

#ifndef BOOST_INTRUSIVE_DOXYGEN_INVOKED

#if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
template<class T, class O1, class O2, class O3, class O4, class O5, class O6>
#else
template<class T, class ...Options>
#endif
class sgtree
   :  public make_sgtree<T,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
      O1, O2, O3, O4, O5, O6
      #else
      Options...
      #endif
      >::type
{
   typedef typename make_sgtree
      <T,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
      O1, O2, O3, O4, O5, O6
      #else
      Options...
      #endif
      >::type   Base;
   BOOST_MOVABLE_BUT_NOT_COPYABLE(sgtree)

   public:
   typedef typename Base::key_compare        key_compare;
   typedef typename Base::value_traits       value_traits;
   typedef typename Base::iterator           iterator;
   typedef typename Base::const_iterator     const_iterator;
   typedef typename Base::reverse_iterator           reverse_iterator;
   typedef typename Base::const_reverse_iterator     const_reverse_iterator;

   //Assert if passed value traits are compatible with the type
   BOOST_STATIC_ASSERT((detail::is_same<typename value_traits::value_type, T>::value));

   sgtree()
      :  Base()
   {}

   explicit sgtree(const key_compare &cmp, const value_traits &v_traits = value_traits())
      :  Base(cmp, v_traits)
   {}

   template<class Iterator>
   sgtree( bool unique, Iterator b, Iterator e
         , const key_compare &cmp = key_compare()
         , const value_traits &v_traits = value_traits())
      :  Base(unique, b, e, cmp, v_traits)
   {}

   sgtree(BOOST_RV_REF(sgtree) x)
      :  Base(BOOST_MOVE_BASE(Base, x))
   {}

   sgtree& operator=(BOOST_RV_REF(sgtree) x)
   {  return static_cast<sgtree &>(this->Base::operator=(BOOST_MOVE_BASE(Base, x)));  }

   template <class Cloner, class Disposer>
   void clone_from(const sgtree &src, Cloner cloner, Disposer disposer)
   {  Base::clone_from(src, cloner, disposer);  }

   template <class Cloner, class Disposer>
   void clone_from(BOOST_RV_REF(sgtree) src, Cloner cloner, Disposer disposer)
   {  Base::clone_from(BOOST_MOVE_BASE(Base, src), cloner, disposer);  }

   static sgtree &container_from_end_iterator(iterator end_iterator)
   {  return static_cast<sgtree &>(Base::container_from_end_iterator(end_iterator));   }

   static const sgtree &container_from_end_iterator(const_iterator end_iterator)
   {  return static_cast<const sgtree &>(Base::container_from_end_iterator(end_iterator));   }

   static sgtree &container_from_iterator(iterator it)
   {  return static_cast<sgtree &>(Base::container_from_iterator(it));   }

   static const sgtree &container_from_iterator(const_iterator it)
   {  return static_cast<const sgtree &>(Base::container_from_iterator(it));   }
};

#endif

} //namespace intrusive
} //namespace boost

#include <boost/intrusive/detail/config_end.hpp>

#endif //BOOST_INTRUSIVE_SGTREE_HPP


// Copyright 2020 Tier IV, Inc.
//
// 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.

#include "detection_by_tracker/utils.hpp"

#include <boost/geometry.hpp>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/utils.h>

#include <algorithm>
#include <vector>

namespace utils
{
void toPolygon2d(
  const autoware_perception_msgs::msg::DynamicObject & object, autoware_utils::Polygon2d & output);
bool isClockWise(const autoware_utils::Polygon2d & polygon);
autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon);

double getArea(const autoware_perception_msgs::msg::Shape & shape)
{
  double area = 0.0;
  if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
    area = getRectangleArea(shape.dimensions);
  } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) {
    area = getCircleArea(shape.dimensions);
  } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) {
    area = getPolygonArea(shape.footprint);
  }
  return area;
}

double getPolygonArea(const geometry_msgs::msg::Polygon & footprint)
{
  double area = 0.0;

  for (size_t i = 0; i < footprint.points.size(); ++i) {
    size_t j = (i + 1) % footprint.points.size();
    area += 0.5 * (footprint.points.at(i).x * footprint.points.at(j).y -
                   footprint.points.at(j).x * footprint.points.at(i).y);
  }

  return area;
}

double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions)
{
  return static_cast<double>(dimensions.x * dimensions.y);
}

double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions)
{
  return static_cast<double>((dimensions.x / 2.0) * (dimensions.x / 2.0) * M_PI);
}

double get2dIoU(
  const autoware_perception_msgs::msg::DynamicObject & object1,
  const autoware_perception_msgs::msg::DynamicObject & object2)
{
  autoware_utils::Polygon2d polygon1, polygon2;
  toPolygon2d(object1, polygon1);
  toPolygon2d(object2, polygon2);

  std::vector<autoware_utils::Polygon2d> union_polygons;
  std::vector<autoware_utils::Polygon2d> intersection_polygons;
  boost::geometry::union_(polygon1, polygon2, union_polygons);
  boost::geometry::intersection(polygon1, polygon2, intersection_polygons);

  double union_area = 0.0;
  double intersection_area = 0.0;
  for (const auto & union_polygon : union_polygons) {
    union_area += boost::geometry::area(union_polygon);
  }
  for (const auto & intersection_polygon : intersection_polygons) {
    intersection_area += boost::geometry::area(intersection_polygon);
  }
  const double iou = union_area < 0.01 ? 0.0 : std::min(1.0, intersection_area / union_area);
  return iou;
}

double get2dPrecision(
  const autoware_perception_msgs::msg::DynamicObject & source_object,
  const autoware_perception_msgs::msg::DynamicObject & target_object)
{
  autoware_utils::Polygon2d source_polygon, target_polygon;
  toPolygon2d(source_object, source_polygon);
  toPolygon2d(target_object, target_polygon);

  std::vector<autoware_utils::Polygon2d> intersection_polygons;
  boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons);

  double intersection_area = 0.0;
  double source_area = 0.0;
  for (const auto & intersection_polygon : intersection_polygons) {
    intersection_area += boost::geometry::area(intersection_polygon);
  }
  source_area = boost::geometry::area(source_polygon);
  const double precision = std::min(1.0, intersection_area / source_area);
  return precision;
}

double get2dRecall(
  const autoware_perception_msgs::msg::DynamicObject & source_object,
  const autoware_perception_msgs::msg::DynamicObject & target_object)
{
  autoware_utils::Polygon2d source_polygon, target_polygon;
  toPolygon2d(source_object, source_polygon);
  toPolygon2d(target_object, target_polygon);

  std::vector<autoware_utils::Polygon2d> intersection_polygons;
  // boost::geometry::union_(source_polygon, target_polygon, intersection_polygons);    // 原来的
  boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); // 我改的

  double intersection_area = 0.0;
  double target_area = 0.0;
  for (const auto & intersection_polygon : intersection_polygons) {
    intersection_area += boost::geometry::area(intersection_polygon);
  }
  target_area += boost::geometry::area(target_polygon);
  const double recall = std::min(1.0, intersection_area / target_area);
  return recall;
}

autoware_utils::Polygon2d inverseClockWise(const autoware_utils::Polygon2d & polygon)
{
  autoware_utils::Polygon2d inverted_polygon;
  for (int i = polygon.outer().size() - 1; 0 <= i; --i) {
    inverted_polygon.outer().push_back(polygon.outer().at(i));
  }
  return inverted_polygon;
}

bool isClockWise(const autoware_utils::Polygon2d & polygon)
{
  const int n = polygon.outer().size();
  const double x_offset = polygon.outer().at(0).x();
  const double y_offset = polygon.outer().at(0).y();
  double sum = 0.0;
  for (std::size_t i = 0; i < polygon.outer().size(); ++i) {
    sum +=
      (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) -
      (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset);
  }

  return sum < 0.0;
}

void toPolygon2d(
  const autoware_perception_msgs::msg::DynamicObject & object, autoware_utils::Polygon2d & output)
{
  if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
    const auto & pose = object.state.pose_covariance.pose;
    const double yaw = autoware_utils::normalizeRadian(tf2::getYaw(pose.orientation));
    Eigen::Matrix2d rotation;
    rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
    Eigen::Vector2d offset0, offset1, offset2, offset3;
    offset0 = rotation *
              Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
    offset1 = rotation *
              Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
    offset2 = rotation *
              Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
    offset3 = rotation *
              Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
    output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
      pose.position.x + offset0.x(), pose.position.y + offset0.y()));
    output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
      pose.position.x + offset1.x(), pose.position.y + offset1.y()));
    output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
      pose.position.x + offset2.x(), pose.position.y + offset2.y()));
    output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
      pose.position.x + offset3.x(), pose.position.y + offset3.y()));
    output.outer().push_back(output.outer().front());
  } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) {
    const auto & center = object.state.pose_covariance.pose.position;
    const auto & radius = object.shape.dimensions.x * 0.5;
    constexpr int n = 6;
    for (int i = 0; i < n; ++i) {
      Eigen::Vector2d point;
      point.x() = std::cos(
                    (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
                    M_PI / static_cast<double>(n)) *
                    radius +
                  center.x;
      point.y() = std::sin(
                    (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
                    M_PI / static_cast<double>(n)) *
                    radius +
                  center.y;
      output.outer().push_back(
        boost::geometry::make<autoware_utils::Point2d>(point.x(), point.y()));
    }
    output.outer().push_back(output.outer().front());
  } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) {
    const auto & pose = object.state.pose_covariance.pose;
    for (const auto & point : object.shape.footprint.points) {
      output.outer().push_back(boost::geometry::make<autoware_utils::Point2d>(
        pose.position.x + point.x, pose.position.y + point.y));
    }
    output.outer().push_back(output.outer().front());
  }
  output = isClockWise(output) ? output : inverseClockWise(output);
}

}  // namespace utils


// std c
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <ctime>

// opencv pcl
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"

// ros
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>

// Eigen
#include <Eigen/Dense>
#include <Eigen/Core>

// ours
#include "detect_3d_cuboid/matrix_utils.h"
#include "detect_3d_cuboid/object_3d_util.h"
#include "tictoc_profiler/profiler.hpp"

using namespace std;
// using namespace cv;
using namespace Eigen;

void detect_3d_cuboid::set_calibration(const Matrix3d &Kalib)
{
	cam_pose.Kalib = Kalib;
	cam_pose.invK = Kalib.inverse();
}

void detect_3d_cuboid::set_cam_pose(const Matrix4d &transToWolrd)
{
	cam_pose.transToWolrd = transToWolrd;
	cam_pose.rotationToWorld = transToWolrd.topLeftCorner<3, 3>();
	Vector3d euler_angles;
	quat_to_euler_zyx(Quaterniond(cam_pose.rotationToWorld), euler_angles(0), euler_angles(1), euler_angles(2));
	cam_pose.euler_angle = euler_angles;
	cam_pose.invR = cam_pose.rotationToWorld.inverse();
	cam_pose.projectionMatrix = cam_pose.Kalib * transToWolrd.inverse().topRows<3>(); // project world coordinate to camera
	cam_pose.KinvR = cam_pose.Kalib * cam_pose.invR;
	cam_pose.camera_yaw = cam_pose.euler_angle(2);
	//TODO relative measure? not good... then need to change transToWolrd.
}

void detect_3d_cuboid::detect_cuboid(const cv::Mat &rgb_img, const Matrix4d &transToWolrd, const MatrixXd &obj_bbox_coors,
									 MatrixXd all_lines_raw, std::vector<ObjectSet> &all_object_cuboids)
{
	set_cam_pose(transToWolrd);
	cam_pose_raw = cam_pose;

	cv::Mat gray_img;
	if (rgb_img.channels() == 3)
		cv::cvtColor(rgb_img, gray_img, cv::COLOR_BGR2GRAY);
	else
		gray_img = rgb_img;

	int img_width = rgb_img.cols;
	int img_height = rgb_img.rows;

	int num_2d_objs = obj_bbox_coors.rows();
	all_object_cuboids.resize(num_2d_objs);

	vector<bool> all_configs;
	all_configs.push_back(consider_config_1);
	all_configs.push_back(consider_config_2);

	// parameters for cuboid generation
	double vp12_edge_angle_thre = 15;
	double vp3_edge_angle_thre = 10;	// 10  10  parameters
	double shorted_edge_thre = 20;		// if box edge are too short. box might be too thin. most possibly wrong.
	bool reweight_edge_distance = true; // if want to compare with all configurations. we need to reweight

	// parameters for proposal scoring
	bool whether_normalize_two_errors = true;
	double weight_vp_angle = 0.8;
	double weight_skew_error = 1.5;
	// if also consider config2, need to weight two erros, in order to compare two configurations

	align_left_right_edges(all_lines_raw); // this should be guaranteed when detecting edges
	if (whether_plot_detail_images)
	{
		cv::Mat output_img;
		plot_image_with_edges(rgb_img, output_img, all_lines_raw, cv::Scalar(255, 0, 0));
		cv::imshow("Raw detected Edges", output_img); //cv::waitKey(0);
	}

	// find ground-wall boundary edges
	Vector4d ground_plane_world(0, 0, 1, 0); // treated as column vector % in my pop-up code, I use [0 0 -1 0]. here I want the normal pointing innerwards, towards the camera to match surface normal prediction
	Vector4d ground_plane_sensor = cam_pose.transToWolrd.transpose() * ground_plane_world;

	//       int object_id=1;
	for (int object_id = 0; object_id < num_2d_objs; object_id++)
	{
		// 	  std::cout<<"object id  "<<object_id<<std::endl;
		ca::Profiler::tictoc("One 3D object total time");
		int left_x_raw = obj_bbox_coors(object_id, 0);
		int top_y_raw = obj_bbox_coors(object_id, 1);
		int obj_width_raw = obj_bbox_coors(object_id, 2);
		int obj_height_raw = obj_bbox_coors(object_id, 3);
		int right_x_raw = left_x_raw + obj_bbox_coors(object_id, 2);
		int down_y_raw = top_y_raw + obj_height_raw;

		std::vector<int> down_expand_sample_all;
		down_expand_sample_all.push_back(0);
		if (whether_sample_bbox_height) // 2D object detection might not be accurate
		{
			int down_expand_sample_ranges = max(min(20, obj_height_raw - 90), 20);
			down_expand_sample_ranges = min(down_expand_sample_ranges, img_height - top_y_raw - obj_height_raw - 1); // should lie inside the image  -1 for c++ index
			if (down_expand_sample_ranges > 10)																		 // if expand large margin, give more samples.
				down_expand_sample_all.push_back(round(down_expand_sample_ranges / 2));
			down_expand_sample_all.push_back(down_expand_sample_ranges);
		}

		// NOTE later if in video, could use previous object yaw..., also reduce search range
		double yaw_init = cam_pose.camera_yaw - 90.0 / 180.0 * M_PI; // yaw init is directly facing the camera, align with camera optical axis
		std::vector<double> obj_yaw_samples;
		linespace<double>(yaw_init - 45.0 / 180.0 * M_PI, yaw_init + 45.0 / 180.0 * M_PI, 6.0 / 180.0 * M_PI, obj_yaw_samples);

		MatrixXd all_configs_errors(400, 9);
		MatrixXd all_box_corners_2ds(800, 8);   // initialize a large eigen matrix
		int valid_config_number_all_height = 0; // all valid objects of all height samples
		ObjectSet raw_obj_proposals;
		raw_obj_proposals.reserve(100);
		// 	    int sample_down_expan_id=1;
		for (int sample_down_expan_id = 0; sample_down_expan_id < down_expand_sample_all.size(); sample_down_expan_id++)
		{
			int down_expand_sample = down_expand_sample_all[sample_down_expan_id];
			int obj_height_expan = obj_height_raw + down_expand_sample;
			int down_y_expan = top_y_raw + obj_height_expan;
			double obj_diaglength_expan = sqrt(obj_width_raw * obj_width_raw + obj_height_expan * obj_height_expan);

			// sample points on the top edges, if edge is too large, give more samples. give at least 10 samples for all edges. for small object, object pose changes lots
			int top_sample_resolution = round(min(20, obj_width_raw / 10)); //  25 pixels
			std::vector<int> top_x_samples;
			linespace<int>(left_x_raw + 5, right_x_raw - 5, top_sample_resolution, top_x_samples);
			MatrixXd sample_top_pts(2, top_x_samples.size());
			for (int ii = 0; ii < top_x_samples.size(); ii++)
			{
				sample_top_pts(0, ii) = top_x_samples[ii];
				sample_top_pts(1, ii) = top_y_raw;
			}

			// expand some small margin for distance map  [10 20]
			int distmap_expand_wid = min(max(min(20, obj_width_raw - 100), 10), max(min(20, obj_height_expan - 100), 10));
			int left_x_expan_distmap = max(0, left_x_raw - distmap_expand_wid);
			int right_x_expan_distmap = min(img_width - 1, right_x_raw + distmap_expand_wid);
			int top_y_expan_distmap = max(0, top_y_raw - distmap_expand_wid);
			int down_y_expan_distmap = min(img_height - 1, down_y_expan + distmap_expand_wid);
			int height_expan_distmap = down_y_expan_distmap - top_y_expan_distmap;
			int width_expan_distmap = right_x_expan_distmap - left_x_expan_distmap;
			Vector2d expan_distmap_lefttop = Vector2d(left_x_expan_distmap, top_y_expan_distmap);
			Vector2d expan_distmap_rightbottom = Vector2d(right_x_expan_distmap, down_y_expan_distmap);

			// find edges inside the object bounding box
			MatrixXd all_lines_inside_object(all_lines_raw.rows(), all_lines_raw.cols()); // first allocate a large matrix, then only use the toprows to avoid copy, alloc
			int inside_obj_edge_num = 0;
			for (int edge_id = 0; edge_id < all_lines_raw.rows(); edge_id++)
				if (check_inside_box(all_lines_raw.row(edge_id).head<2>(), expan_distmap_lefttop, expan_distmap_rightbottom))
					if (check_inside_box(all_lines_raw.row(edge_id).tail<2>(), expan_distmap_lefttop, expan_distmap_rightbottom))
					{
						all_lines_inside_object.row(inside_obj_edge_num) = all_lines_raw.row(edge_id);
						inside_obj_edge_num++;
					}

			// merge edges and remove short lines, after finding object edges.  edge merge in small regions should be faster than all.
			double pre_merge_dist_thre = 20;
			double pre_merge_angle_thre = 5;
			double edge_length_threshold = 30;
			MatrixXd all_lines_merge_inobj;
			merge_break_lines(all_lines_inside_object.topRows(inside_obj_edge_num), all_lines_merge_inobj, pre_merge_dist_thre,
							  pre_merge_angle_thre, edge_length_threshold);

			// compute edge angels and middle points
			VectorXd lines_inobj_angles(all_lines_merge_inobj.rows());
			MatrixXd edge_mid_pts(all_lines_merge_inobj.rows(), 2);
			for (int i = 0; i < all_lines_merge_inobj.rows(); i++)
			{
				lines_inobj_angles(i) = std::atan2(all_lines_merge_inobj(i, 3) - all_lines_merge_inobj(i, 1), all_lines_merge_inobj(i, 2) - all_lines_merge_inobj(i, 0)); // [-pi/2 -pi/2]
				edge_mid_pts.row(i).head<2>() = (all_lines_merge_inobj.row(i).head<2>() + all_lines_merge_inobj.row(i).tail<2>()) / 2;
			}

			// TODO could canny or distance map outside sampling height to speed up!!!!   Then only need to compute canny onces.
			// detect canny edges and compute distance transform  NOTE opencv canny maybe different from matlab. but roughly same
			cv::Rect object_bbox = cv::Rect(left_x_expan_distmap, top_y_expan_distmap, width_expan_distmap, height_expan_distmap); //
			cv::Mat im_canny;
			cv::Canny(gray_img(object_bbox), im_canny, 80, 200); // low thre, high thre    im_canny 0 or 255   [80 200  40 100]
			cv::Mat dist_map;
			cv::distanceTransform(255 - im_canny, dist_map, cv::DIST_L2, 3); // dist_map is float datatype

			if (whether_plot_detail_images)
			{
				cv::imshow("im_canny", im_canny);
				cv::Mat dist_map_img;
				cv::normalize(dist_map, dist_map_img, 0.0, 1.0, cv::NORM_MINMAX);
				cv::imshow("normalized distance map", dist_map_img);
				cv::waitKey();
			}

			// Generate cuboids
			MatrixXd all_configs_error_one_objH(200, 9);
			MatrixXd all_box_corners_2d_one_objH(400, 8);
			int valid_config_number_one_objH = 0;

			std::vector<double> cam_roll_samples;
			std::vector<double> cam_pitch_samples;
			if (whether_sample_cam_roll_pitch)
			{
				linespace<double>(cam_pose_raw.euler_angle(0) - 6.0 / 180.0 * M_PI, cam_pose_raw.euler_angle(0) + 6.0 / 180.0 * M_PI, 3.0 / 180.0 * M_PI, cam_roll_samples);
				linespace<double>(cam_pose_raw.euler_angle(1) - 6.0 / 180.0 * M_PI, cam_pose_raw.euler_angle(1) + 6.0 / 180.0 * M_PI, 3.0 / 180.0 * M_PI, cam_pitch_samples);
			}
			else
			{
				cam_roll_samples.push_back(cam_pose_raw.euler_angle(0));
				cam_pitch_samples.push_back(cam_pose_raw.euler_angle(1));
			}
			// different from matlab. first for loop yaw, then for configurations.
			// 	      int obj_yaw_id=8;
			for (int cam_roll_id = 0; cam_roll_id < cam_roll_samples.size(); cam_roll_id++)
				for (int cam_pitch_id = 0; cam_pitch_id < cam_pitch_samples.size(); cam_pitch_id++)
					for (int obj_yaw_id = 0; obj_yaw_id < obj_yaw_samples.size(); obj_yaw_id++)
					{
						if (whether_sample_cam_roll_pitch)
						{
							Matrix4d transToWolrd_new = transToWolrd;
							transToWolrd_new.topLeftCorner<3, 3>() = euler_zyx_to_rot<double>(cam_roll_samples[cam_roll_id], cam_pitch_samples[cam_pitch_id], cam_pose_raw.euler_angle(2));
							set_cam_pose(transToWolrd_new);
							ground_plane_sensor = cam_pose.transToWolrd.transpose() * ground_plane_world;
						}

						double obj_yaw_esti = obj_yaw_samples[obj_yaw_id];

						Vector2d vp_1, vp_2, vp_3;
						getVanishingPoints(cam_pose.KinvR, obj_yaw_esti, vp_1, vp_2, vp_3); // for object x y z  axis

						MatrixXd all_vps(3, 2);
						all_vps.row(0) = vp_1;
						all_vps.row(1) = vp_2;
						all_vps.row(2) = vp_3;
						// 		  std::cout<<"obj_yaw_esti  "<<obj_yaw_esti<<"  "<<obj_yaw_id<<std::endl;
						MatrixXd all_vp_bound_edge_angles = VP_support_edge_infos(all_vps, edge_mid_pts, lines_inobj_angles,
																				  Vector2d(vp12_edge_angle_thre, vp3_edge_angle_thre));
						// 		  int sample_top_pt_id=15;
						for (int sample_top_pt_id = 0; sample_top_pt_id < sample_top_pts.cols(); sample_top_pt_id++)
						{
							// 		      std::cout<<"sample_top_pt_id "<<sample_top_pt_id<<std::endl;
							Vector2d corner_1_top = sample_top_pts.col(sample_top_pt_id);
							bool config_good = true;
							int vp_1_position = 0; // 0 initial as fail,  1  on left   2 on right
							Vector2d corner_2_top = seg_hit_boundary(vp_1, corner_1_top, Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
							if (corner_2_top(0) == -1)
							{ // vp1-corner1 doesn't hit the right boundary. check whether hit left
								corner_2_top = seg_hit_boundary(vp_1, corner_1_top, Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
								if (corner_2_top(0) != -1) // vp1-corner1 hit the left boundary   vp1 on the right
									vp_1_position = 2;
							}
							else // vp1-corner1 hit the right boundary   vp1 on the left
								vp_1_position = 1;

							config_good = vp_1_position > 0;
							if (!config_good)
							{
								if (print_details)
									printf("Configuration fails at corner 2, outside segment\n");
								continue;
							}
							if ((corner_1_top - corner_2_top).norm() < shorted_edge_thre)
							{
								if (print_details)
									printf("Configuration fails at edge 1-2, too short\n");
								continue;
							}
							// 		      cout<<"corner_1/2   "<<corner_1_top.transpose()<<"   "<<corner_2_top.transpose()<<endl;
							// 		      int config_ind=0; // have to consider config now.
							for (int config_id = 1; config_id < 3; config_id++) // configuration one or two of matlab version
							{
								if (!all_configs[config_id - 1])
									continue;
								Vector2d corner_3_top, corner_4_top;
								if (config_id == 1)
								{
									if (vp_1_position == 1) // then vp2 hit the left boundary
										corner_4_top = seg_hit_boundary(vp_2, corner_1_top, Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
									else // or, then vp2 hit the right boundary
										corner_4_top = seg_hit_boundary(vp_2, corner_1_top, Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
									if (corner_4_top(1) == -1)
									{
										config_good = false;
										if (print_details)
											printf("Configuration %d fails at corner 4, outside segment\n", config_id);
										continue;
									}
									if ((corner_1_top - corner_4_top).norm() < shorted_edge_thre)
									{
										if (print_details)
											printf("Configuration %d fails at edge 1-4, too short\n", config_id);
										continue;
									}
									// compute the last point in the top face
									corner_3_top = lineSegmentIntersect(vp_2, corner_2_top, vp_1, corner_4_top, true);
									if (!check_inside_box(corner_3_top, Vector2d(left_x_raw, top_y_raw), Vector2d(right_x_raw, down_y_expan)))
									{ // check inside boundary. otherwise edge visibility might be wrong
										config_good = false;
										if (print_details)
											printf("Configuration %d fails at corner 3, outside box\n", config_id);
										continue;
									}
									if (((corner_3_top - corner_4_top).norm() < shorted_edge_thre) || ((corner_3_top - corner_2_top).norm() < shorted_edge_thre))
									{
										if (print_details)
											printf("Configuration %d fails at edge 3-4/3-2, too short\n", config_id);
										continue;
									}
									// 			      cout<<"corner_3/4   "<<corner_3_top.transpose()<<"   "<<corner_4_top.transpose()<<endl;
								}
								if (config_id == 2)
								{
									if (vp_1_position == 1) // then vp2 hit the left boundary
										corner_3_top = seg_hit_boundary(vp_2, corner_2_top, Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
									else // or, then vp2 hit the right boundary
										corner_3_top = seg_hit_boundary(vp_2, corner_2_top, Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
									if (corner_3_top(1) == -1)
									{
										config_good = false;
										if (print_details)
											printf("Configuration %d fails at corner 3, outside segment\n", config_id);
										continue;
									}
									if ((corner_2_top - corner_3_top).norm() < shorted_edge_thre)
									{
										if (print_details)
											printf("Configuration %d fails at edge 2-3, too short\n", config_id);
										continue;
									}
									// compute the last point in the top face
									corner_4_top = lineSegmentIntersect(vp_1, corner_3_top, vp_2, corner_1_top, true);
									if (!check_inside_box(corner_4_top, Vector2d(left_x_raw, top_y_expan_distmap), Vector2d(right_x_raw, down_y_expan_distmap)))
									{
										config_good = false;
										if (print_details)
											printf("Configuration %d fails at corner 4, outside box\n", config_id);
										continue;
									}
									if (((corner_3_top - corner_4_top).norm() < shorted_edge_thre) || ((corner_4_top - corner_1_top).norm() < shorted_edge_thre))
									{
										if (print_details)
											printf("Configuration %d fails at edge 3-4/4-1, too short\n", config_id);
										continue;
									}
									// 			      cout<<"corner_3/4   "<<corner_3_top.transpose()<<"   "<<corner_4_top.transpose()<<endl;
								}
								// compute first bottom points    computing bottom points is the same for config 1,2
								Vector2d corner_5_down = seg_hit_boundary(vp_3, corner_3_top, Vector4d(left_x_raw, down_y_expan, right_x_raw, down_y_expan));
								if (corner_5_down(1) == -1)
								{
									config_good = false;
									if (print_details)
										printf("Configuration %d fails at corner 5, outside segment\n", config_id);
									continue;
								}
								if ((corner_3_top - corner_5_down).norm() < shorted_edge_thre)
								{
									if (print_details)
										printf("Configuration %d fails at edge 3-5, too short\n", config_id);
									continue;
								}
								Vector2d corner_6_down = lineSegmentIntersect(vp_2, corner_5_down, vp_3, corner_2_top, true);
								if (!check_inside_box(corner_6_down, expan_distmap_lefttop, expan_distmap_rightbottom))
								{
									config_good = false;
									if (print_details)
										printf("Configuration %d fails at corner 6, outside box\n", config_id);
									continue;
								}
								if (((corner_6_down - corner_2_top).norm() < shorted_edge_thre) || ((corner_6_down - corner_5_down).norm() < shorted_edge_thre))
								{
									if (print_details)
										printf("Configuration %d fails at edge 6-5/6-2, too short\n", config_id);
									continue;
								}
								Vector2d corner_7_down = lineSegmentIntersect(vp_1, corner_6_down, vp_3, corner_1_top, true);
								if (!check_inside_box(corner_7_down, expan_distmap_lefttop, expan_distmap_rightbottom))
								{ // might be slightly different from matlab
									config_good = false;
									if (print_details)
										printf("Configuration %d fails at corner 7, outside box\n", config_id);
									continue;
								}
								if (((corner_7_down - corner_1_top).norm() < shorted_edge_thre) || ((corner_7_down - corner_6_down).norm() < shorted_edge_thre))
								{
									if (print_details)
										printf("Configuration %d fails at edge 7-1/7-6, too short\n", config_id);
									continue;
								}
								Vector2d corner_8_down = lineSegmentIntersect(vp_1, corner_5_down, vp_2, corner_7_down, true);
								if (!check_inside_box(corner_8_down, expan_distmap_lefttop, expan_distmap_rightbottom))
								{
									config_good = false;
									if (print_details)
										printf("Configuration %d fails at corner 8, outside box\n", config_id);
									continue;
								}
								if (((corner_8_down - corner_4_top).norm() < shorted_edge_thre) || ((corner_8_down - corner_5_down).norm() < shorted_edge_thre) || ((corner_8_down - corner_7_down).norm() < shorted_edge_thre))
								{
									if (print_details)
										printf("Configuration %d fails at edge 8-4/8-5/8-7, too short\n", config_id);
									continue;
								}

								MatrixXd box_corners_2d_float(2, 8);
								box_corners_2d_float << corner_1_top, corner_2_top, corner_3_top, corner_4_top, corner_5_down, corner_6_down, corner_7_down, corner_8_down;
								// 			  std::cout<<"box_corners_2d_float \n "<<box_corners_2d_float<<std::endl;
								MatrixXd box_corners_2d_float_shift(2, 8);
								box_corners_2d_float_shift.row(0) = box_corners_2d_float.row(0).array() - left_x_expan_distmap;
								box_corners_2d_float_shift.row(1) = box_corners_2d_float.row(1).array() - top_y_expan_distmap;

								MatrixXi visible_edge_pt_ids, vps_box_edge_pt_ids;
								double sum_dist;
								if (config_id == 1)
								{
									visible_edge_pt_ids.resize(9, 2);
									visible_edge_pt_ids << 1, 2, 2, 3, 3, 4, 4, 1, 2, 6, 3, 5, 4, 8, 5, 8, 5, 6;
									vps_box_edge_pt_ids.resize(3, 4);
									vps_box_edge_pt_ids << 1, 2, 8, 5, 4, 1, 5, 6, 4, 8, 2, 6; // six edges. each row represents two edges [e1_1 e1_2   e2_1 e2_2;...] of one VP
									visible_edge_pt_ids.array() -= 1;
									vps_box_edge_pt_ids.array() -= 1; //change to c++ index
									sum_dist = box_edge_sum_dists(dist_map, box_corners_2d_float_shift, visible_edge_pt_ids);
								}
								else
								{
									visible_edge_pt_ids.resize(7, 2);
									visible_edge_pt_ids << 1, 2, 2, 3, 3, 4, 4, 1, 2, 6, 3, 5, 5, 6;
									vps_box_edge_pt_ids.resize(3, 4);
									vps_box_edge_pt_ids << 1, 2, 3, 4, 4, 1, 5, 6, 3, 5, 2, 6; // six edges. each row represents two edges [e1_1 e1_2   e2_1 e2_2;...] of one VP
									visible_edge_pt_ids.array() -= 1;
									vps_box_edge_pt_ids.array() -= 1;
									sum_dist = box_edge_sum_dists(dist_map, box_corners_2d_float_shift, visible_edge_pt_ids, reweight_edge_distance);
								}
								double total_angle_diff = box_edge_alignment_angle_error(all_vp_bound_edge_angles, vps_box_edge_pt_ids, box_corners_2d_float);
								all_configs_error_one_objH.row(valid_config_number_one_objH).head<4>() = Vector4d(config_id, vp_1_position, obj_yaw_esti, sample_top_pt_id);
								all_configs_error_one_objH.row(valid_config_number_one_objH).segment<3>(4) = Vector3d(sum_dist / obj_diaglength_expan, total_angle_diff, down_expand_sample);
								if (whether_sample_cam_roll_pitch)
									all_configs_error_one_objH.row(valid_config_number_one_objH).segment<2>(7) = Vector2d(cam_roll_samples[cam_roll_id], cam_pitch_samples[cam_pitch_id]);
								else
									all_configs_error_one_objH.row(valid_config_number_one_objH).segment<2>(7) = Vector2d(cam_pose_raw.euler_angle(0), cam_pose_raw.euler_angle(1));
								all_box_corners_2d_one_objH.block(2 * valid_config_number_one_objH, 0, 2, 8) = box_corners_2d_float;
								valid_config_number_one_objH++;
								if (valid_config_number_one_objH >= all_configs_error_one_objH.rows())
								{
									all_configs_error_one_objH.conservativeResize(2 * valid_config_number_one_objH, NoChange);
									all_box_corners_2d_one_objH.conservativeResize(4 * valid_config_number_one_objH, NoChange);
								}
							} //end of config loop
						}	 //end of top id
					}		  //end of yaw

			// 	      std::cout<<"valid_config_number_one_hseight  "<<valid_config_number_one_objH<<std::endl;
			// 	      std::cout<<"all_configs_error_one_objH  \n"<<all_configs_error_one_objH.topRows(valid_config_number_one_objH)<<std::endl;
			// 	      MatrixXd all_corners = all_box_corners_2d_one_objH.topRows(2*valid_config_number_one_objH);
			// 	      std::cout<<"all corners   "<<all_corners<<std::endl;

			VectorXd normalized_score;
			vector<int> good_proposal_ids;
			fuse_normalize_scores_v2(all_configs_error_one_objH.col(4).head(valid_config_number_one_objH), all_configs_error_one_objH.col(5).head(valid_config_number_one_objH),
									 normalized_score, good_proposal_ids, weight_vp_angle, whether_normalize_two_errors);

			for (int box_id = 0; box_id < good_proposal_ids.size(); box_id++)
			{
				int raw_cube_ind = good_proposal_ids[box_id];

				if (whether_sample_cam_roll_pitch)
				{
					Matrix4d transToWolrd_new = transToWolrd;
					transToWolrd_new.topLeftCorner<3, 3>() = euler_zyx_to_rot<double>(all_configs_error_one_objH(raw_cube_ind, 7), all_configs_error_one_objH(raw_cube_ind, 8), cam_pose_raw.euler_angle(2));
					set_cam_pose(transToWolrd_new);
					ground_plane_sensor = cam_pose.transToWolrd.transpose() * ground_plane_world;
				}

				cuboid *sample_obj = new cuboid();
				change_2d_corner_to_3d_object(all_box_corners_2d_one_objH.block(2 * raw_cube_ind, 0, 2, 8), all_configs_error_one_objH.row(raw_cube_ind).head<3>(),
											  ground_plane_sensor, cam_pose.transToWolrd, cam_pose.invK, cam_pose.projectionMatrix, *sample_obj);
				// 		  sample_obj->print_cuboid();
				if ((sample_obj->scale.array() < 0).any())
					continue; // scale should be positive
				sample_obj->rect_detect_2d = Vector4d(left_x_raw, top_y_raw, obj_width_raw, obj_height_raw);
				sample_obj->edge_distance_error = all_configs_error_one_objH(raw_cube_ind, 4); // record the original error
				sample_obj->edge_angle_error = all_configs_error_one_objH(raw_cube_ind, 5);
				sample_obj->normalized_error = normalized_score(box_id);
				double skew_ratio = sample_obj->scale.head(2).maxCoeff() / sample_obj->scale.head(2).minCoeff();
				sample_obj->skew_ratio = skew_ratio;
				sample_obj->down_expand_height = all_configs_error_one_objH(raw_cube_ind, 6);
				if (whether_sample_cam_roll_pitch)
				{
					sample_obj->camera_roll_delta = all_configs_error_one_objH(raw_cube_ind, 7) - cam_pose_raw.euler_angle(0);
					sample_obj->camera_pitch_delta = all_configs_error_one_objH(raw_cube_ind, 8) - cam_pose_raw.euler_angle(1);
				}
				else
				{
					sample_obj->camera_roll_delta = 0;
					sample_obj->camera_pitch_delta = 0;
				}

				raw_obj_proposals.push_back(sample_obj);
			}
		} // end of differnet object height sampling

		// %finally rank all proposals. [normalized_error   skew_error]
		int actual_cuboid_num_small = std::min(max_cuboid_num, (int)raw_obj_proposals.size());
		VectorXd all_combined_score(raw_obj_proposals.size());
		for (int box_id = 0; box_id < raw_obj_proposals.size(); box_id++)
		{
			cuboid *sample_obj = raw_obj_proposals[box_id];
			double skew_error = weight_skew_error * std::max(sample_obj->skew_ratio - nominal_skew_ratio, 0.0);
			if (sample_obj->skew_ratio > max_cut_skew)
				skew_error = 100;
			double new_combined_error = sample_obj->normalized_error + weight_skew_error * skew_error;
			all_combined_score(box_id) = new_combined_error;
		}

		std::vector<int> sort_idx_small(all_combined_score.rows());
		iota(sort_idx_small.begin(), sort_idx_small.end(), 0);
		sort_indexes(all_combined_score, sort_idx_small, actual_cuboid_num_small);
		for (int ii = 0; ii < actual_cuboid_num_small; ii++) // use sorted index
		{
			all_object_cuboids[object_id].push_back(raw_obj_proposals[sort_idx_small[ii]]);
		}

		ca::Profiler::tictoc("One 3D object total time");
	} // end of different objects

	if (whether_plot_final_images || whether_save_final_images)
	{
		cv::Mat frame_all_cubes_img = rgb_img.clone();
		for (int object_id = 0; object_id < all_object_cuboids.size(); object_id++)
			if (all_object_cuboids[object_id].size() > 0)
			{
				plot_image_with_cuboid(frame_all_cubes_img, all_object_cuboids[object_id][0]);
			}
		if (whether_save_final_images)
			cuboids_2d_img = frame_all_cubes_img;
		if (whether_plot_final_images)
		{
			cv::imshow("frame_all_cubes_img", frame_all_cubes_img);
			cv::waitKey(0);
		}
	}
}


///////////////////////////////////////////////////////////////////////////////
// ars::detail::area.hpp                                                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_DETAIL_AREA_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_DETAIL_AREA_HPP_ER_2009
#include <stdexcept>
#include <boost/ars/detail/data.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

// Area of the exponential tangent between two
// 2 intersections (one of which is possibly -inf or inf).

template<typename T>
T area_flat_segment(
    const tangent_intersection<T>& a,
    const tangent_intersection<T>& b,
    const T& offset
){
    typedef constant<T> const_;
    T mid = (a.t() + b.t()) / const_::two_;
    return (b.z() - a.z()) * exp(mid - offset);
}

template<typename T>
T area_left_tail(
    const data<T>& b,
    const T& offset
){
    return exp( b.t() - offset) / b.dy();
}
template<typename T>
T area_right_tail(
    const tangent_intersection<T>& a,
    const point<T>& b,
    const T& offset
){
    return - exp( a.t() - offset) / b.dy();
}

template<typename T>
T area_segment(
    const tangent_intersection<T>& a,
    const data<T>& b,
    const T& offset
){
    typedef constant<T> const_;
    // Note that b.dy() (a.z_-b.z_) == a.t_ - b.t_
    T dt  = a.t() - b.t();
    return exp( b.t() - offset) * (const_::one_ - exp(dt)) / b.dy();
}

template<typename T>
T area_segment_safeguarded(
    const tangent_intersection<T>& a,
    const data<T>& b,
    const T& offset
)
{
    typedef constant<T> const_;
    T area;
    if(fabs(b.dy())<const_::lmin_){
        area = area_flat_segment(a,b,offset);
    }else{
        if(a.t()-b.t()>const_::lmax_){
            // exp(a.t_-iter_.t_) = inf,
            // so 1) not computable and 2) b negligible
            // This alternative is computable because (in principle)
            // exp(a.t_- offset) <inf
            area = area_right_tail(a,b,offset);
        }else{
            area = area_segment(a,b,offset);
        }
    }
    return area;
}

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

#endif


#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/filesystem.hpp>

using namespace cv;
using namespace std;
using namespace boost::filesystem;

/*
 * 原理为张正友相机标定
 */

int main(int argc, char** argv)
{

    if ( argc != 2 )
    {
        cout<<"Parameter input error"<<endl;
        return 1;
    }
    ofstream fout("/home/xc/caliberation_result.txt");  /* 保存标定结果的文件 */

    // 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
    int image_count = 0;  /* 图像数量 */
    Size image_size;      /* 图像的尺寸 */
    Size board_size = Size(6, 4);             /* 标定板上每行、列的角点数 */
    vector<Point2f> image_points_buf;         /* 缓存每幅图像上检测到的角点 */
    vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
    vector<string> filenames;
    //获取图片路径
    path dirPath(argv[1]);
    if(not exists(dirPath) or not is_directory(dirPath))
    {
        cerr<<"不能打开文件路径:"<<argv[1]<<endl;
        return false;
    }

    for(directory_entry& x:directory_iterator(dirPath))
    {
        string extension=x.path().extension().string();
        boost::algorithm::to_lower(extension);
        if(extension==".jpg" or extension==".jpeg" or extension==".png")
        {
            ++image_count;
            cout<<"image_count = "<<image_count<<endl;
            filenames.push_back(x.path().string());
            Mat imageInput=imread(x.path().string());
            if(image_count==1)
            {
                image_size.width=imageInput.cols;
                image_size.height=imageInput.rows;
                cout<<"image_size.width = "<<image_size.width<<endl;
                cout<<"image_size.height = "<<image_size.height<<endl;
            }
            if (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
            {
                cout << "can not find chessboard corners!\n";  // 找不到角点
                exit(1);
            }
            else
            {
                Mat view_gray;
                cvtColor(imageInput, view_gray, CV_RGB2GRAY);  // 转灰度图

                /* 亚像素精确化 */
                // image_points_buf 初始的角点坐标向量，同时作为亚像素坐标位置的输出
                // Size(5,5) 搜索窗口大小
                // （-1，-1）表示没有死区
                // TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
                cornerSubPix(view_gray, image_points_buf, Size(5,5), Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

                image_points_seq.push_back(image_points_buf);  // 保存亚像素角点

                /* 在图像上显示角点位置 */
                drawChessboardCorners(imageInput, board_size, image_points_buf, true); // 用于在图片中标记角点
                namedWindow("Camera Calibration",0);
                imshow("Camera Calibration", imageInput);       // 显示图片
                waitKey(500); //暂停0.5S
            }
        }

    }
    cout<<"end"<<endl;
    int CornerNum = board_size.width * board_size.height;  // 每张图片上总的角点数

    //-------------以下是摄像机标定------------------

    /*棋盘三维信息*/
    Size square_size = Size(10, 10);         /* 实际测量得到的标定板上每个棋盘格的大小 */
    vector<vector<Point3f>> object_points;   /* 保存标定板上角点的三维坐标 */

    /*内外参数*/
    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 摄像机内参数矩阵 */
    vector<int> point_counts;   // 每幅图像中角点的数量
    Mat distCoeffs=Mat(1, 5, CV_32FC1,Scalar::all(0));       /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
    vector<Mat> tvecsMat;      /* 每幅图像的旋转向量 */
    vector<Mat> rvecsMat;      /* 每幅图像的平移向量 */

    /* 初始化标定板上角点的三维坐标 */
    int i, j, t;
    for (t=0; t<image_count; t++)
    {
        vector<Point3f> tempPointSet;
        for (i=0; i<board_size.height; i++)
        {
            for (j=0; j<board_size.width; j++)
            {
                Point3f realPoint;

                /* 假设标定板放在世界坐标系中z=0的平面上 */
                realPoint.x = i * square_size.width;
                realPoint.y = j * square_size.height;
                realPoint.z = 0;
                tempPointSet.push_back(realPoint);
            }
        }
        object_points.push_back(tempPointSet);
    }

    /* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
    for (i=0; i<image_count; i++)
    {
        point_counts.push_back(board_size.width * board_size.height);
    }

    /* 开始标定 */
    // object_points 世界坐标系中的角点的三维坐标
    // image_points_seq 每一个内角点对应的图像坐标点
    // image_size 图像的像素尺寸大小
    // cameraMatrix 输出，内参矩阵
    // distCoeffs 输出，畸变系数
    // rvecsMat 输出，旋转向量
    // tvecsMat 输出，位移向量
    // 0 标定时所采用的算法
    calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);

    //------------------------标定完成------------------------------------

    // -------------------对标定结果进行评价------------------------------

    double total_err = 0.0;         /* 所有图像的平均误差的总和 */
    double err = 0.0;               /* 每幅图像的平均误差 */
    vector<Point2f> image_points2;  /* 保存重新计算得到的投影点 */
    cerr<<"每幅图像的标定误差：\n";

    for (i=0;i<image_count;i++)
    {
        vector<Point3f> tempPointSet = object_points[i];

        /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
        projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);

        /* 计算新的投影点和旧的投影点之间的误差*/
        vector<Point2f> tempImagePoint = image_points_seq[i];
        Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
        Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);

        for (int j = 0 ; j < tempImagePoint.size(); j++)
        {
            image_points2Mat.at<Vec2f>(0,j) = Vec2f(image_points2[j].x, image_points2[j].y);
            tempImagePointMat.at<Vec2f>(0,j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
        }
        err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
        total_err += err/= point_counts[i];
        cerr << "第" << i+1 << "幅图像的平均误差：" << err<< "像素" << endl;
    }
    cerr << "总体平均误差：" << total_err/image_count << "像素" <<endl <<endl;

    //-------------------------评价完成---------------------------------------------

    //-----------------------保存定标结果-------------------------------------------
    Mat rotation_matrix = Mat(3,3,CV_32FC1, Scalar::all(0));  /* 保存每幅图像的旋转矩阵 */
    fout << "相机内参数矩阵：" << endl;
    fout << cameraMatrix << endl << endl;
    fout << "畸变系数：\n";
    fout << distCoeffs << endl << endl << endl;
    for (int i=0; i<image_count; i++)
    {
        cerr << "第" << i+1 << "幅图像的旋转向量：" << endl;
        cerr << tvecsMat[i] << endl;

        /* 将旋转向量转换为相对应的旋转矩阵 */
        Rodrigues(tvecsMat[i], rotation_matrix);
        cerr << "第" << i+1 << "幅图像的旋转矩阵：" << endl;
        cerr << rotation_matrix << endl;
        cerr << "第" << i+1 << "幅图像的平移向量：" << endl;
        cerr << rvecsMat[i] << endl << endl;
    }
    cerr<<endl;

    //--------------------标定结果保存结束-------------------------------

    //----------------------显示定标结果--------------------------------

    Mat mapx = Mat(image_size, CV_32FC1);
    Mat mapy = Mat(image_size, CV_32FC1);
    Mat R = Mat::eye(3, 3, CV_32F);
    string imageFileName;
    std::stringstream StrStm;
    for (int i = 0 ; i != image_count ; i++)
    {
        initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
        Mat imageSource = imread(filenames[i]);
        Mat newimage = imageSource.clone();
        remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
        StrStm.clear();
        imageFileName.clear();
        StrStm << i+1;
        StrStm >> imageFileName;
        imageFileName += "_d.jpg";
        imwrite(imageFileName, newimage);
        imshow("new",newimage);
    }
    fout.close();
    return 0;
}


//
// Created by Samuel Jackson on 11/03/2016.
//

#include <Eigen/Dense>
#include <set>
#include "CellMatrix.h"

using namespace Eigen;
using namespace Molly;

// offset indicies for neighbour cells for each cell
// we only need to look at half the cells at one time.
// includes 13 neighbours (half the total)
const size_t NUM_OFFSETS = 13;
const int OFFSETS[][3] {
    {1, 0, 0},
    {1, 1, 0},
    {0, 1, 0},
    {-1, 1, 0},
    {0, 0, 1},
    {1, 0, 1},
    {1, 1, 1},
    {0, 1, 1},
    {-1, 1, 1},
    {-1, 0, 1},
    {-1, -1, 1},
    {0, -1, 1},
    {1, -1, 1}
};

CellMatrix::CellMatrix(const double cell_width, size_t d1, size_t d2, size_t d3)
        : cell_width(cell_width) {
    resize(d1, d2, d3);
}

void CellMatrix::resize(size_t d1, size_t d2, size_t d3) {
    data.clear();
    data.resize(d1*d2*d3);

    for (auto iter = data.begin(); iter != data.end(); ++iter) {
        *iter = std::make_shared<Cell>();
    }

    // update the dimensions
    this->d1 = d1;
    this->d2 = d2;
    this->d3 = d3;

    // reinitialise neighbours
    create_neighbours();
}

Cell_ptr CellMatrix::operator()(size_t i, size_t j, size_t k) {
    const size_t index = i*d2*d3 + j*d3 + k;

    if (index >= data.size()) {
        std::stringstream ss;
        ss << "Index is out of points for matrix of size " << d1 << ", " << d2 << ", " << d3;
        throw std::runtime_error(ss.str());
    }

    return data[index];
}

Cell_ptr const CellMatrix::operator()(size_t i, size_t j, size_t k) const {
    const size_t index = i*d2*d3 + j*d3 + k;

    if (index >= data.size()) {
        std::stringstream ss;
        ss << "Index is out of points for matrix of size " << d1 << ", " << d2 << ", " << d3;
        throw std::runtime_error(ss.str());
    }

    return data[index];
}

void CellMatrix::create_neighbours() {
    // loop over each dimension of the cell matrix
    for(size_t i = 0; i < d1; ++i) {
        for(size_t j = 0; j < d2; ++j) {
            for(size_t k = 0; k < d3; ++k) {
                // add pointers to each neighbour of a cell in the cell matrix
                Cell_ptr cell = (*this)(i, j, k);
                for (size_t offset_num = 0; offset_num < NUM_OFFSETS; ++offset_num) {
                    // get offset cell indices
                    const int x_offset = OFFSETS[offset_num][0];
                    const int y_offset = OFFSETS[offset_num][1];
                    const int z_offset = OFFSETS[offset_num][2];

                    // correct the offsets to wrap around
                    size_t x = (i+x_offset) % d1;
                    size_t y = (j+y_offset) % d2;
                    size_t z = (k+z_offset) % d3;

                    //set pointer to neighbour cell
                    Cell_ptr neighbour = (*this)(x, y, z);
                    cell->add_neighbour(neighbour);
                }
            }
        }
    }
}

void CellMatrix::add_molecule(Molecule_ptr mol) {
    Eigen::Vector3i index;
    convert_vector_to_index(mol->r, index);
    (*this)(index(0), index(1), index(2))->add_molecule(mol);
}

void CellMatrix::convert_vector_to_index(const Vector3d& vec, Vector3i& index) const {
    index = (vec / cell_width).cast<int>();
    index(0) = index(0) % d1;
    index(1) = index(1) % d2;
    index(2) = index(2) % d3;
}

void CellMatrix::wrap_cells() {
    Vector3i index;
    int i = 0;

    // loop over every cell in the matrix
    for (auto iter = data.begin(); iter != data.end(); ++iter) {
        auto& molecules = (*iter)->get_molecules();

        // check the molecules in each cell to see if they need to move
        // to a different cell
        for(auto mol = molecules.begin(); mol != molecules.end();) {
            convert_vector_to_index((*mol)->r, index);
            // check if this molecule is in the same cell
            if(i != (index(0)*d2*d3 + index(1)*d3 + index(2))) {
                // if it isn't then move it to the correct cell mark for removal from the current.
                (*this)(index(0), index(1), index(2))->add_molecule(*mol);
                // note: must update iterator after removal
                mol = molecules.erase(mol);
            } else {
                // otherwise just skip to next molecule
                ++mol;
            }
        }
        ++i;
    }
}