//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2005-2009. 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/interprocess for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_INTERPROCESS_INTERSEGMENT_PTR_HPP
#define BOOST_INTERPROCESS_INTERSEGMENT_PTR_HPP

#if (defined _MSC_VER) && (_MSC_VER >= 1200)
#  pragma once
#endif

#include <boost/interprocess/detail/config_begin.hpp>
#include <boost/interprocess/detail/workaround.hpp>

#include <boost/interprocess/interprocess_fwd.hpp>
#include <boost/interprocess/detail/utilities.hpp>
#include <boost/interprocess/detail/math_functions.hpp>
#include <boost/interprocess/detail/cast_tags.hpp>
#include <boost/assert.hpp>
#include <boost/interprocess/sync/scoped_lock.hpp>
#include <boost/interprocess/sync/interprocess_mutex.hpp>
#include <boost/interprocess/containers/flat_map.hpp>
#include <boost/interprocess/containers/vector.hpp>   //vector
#include <boost/interprocess/containers/set.hpp>      //set
#include <boost/detail/no_exceptions_support.hpp>
#include <boost/interprocess/detail/mpl.hpp>
#include <climits>
#include <iterator>
#include <boost/static_assert.hpp>  //BOOST_STATIC_ASSERT
#include <climits>   //CHAR_BIT
#include <boost/integer/static_log2.hpp>
#include <boost/assert.hpp>   //BOOST_ASSERT
#include <boost/interprocess/detail/multi_segment_services.hpp>

//!\file
//!
namespace boost {

//Predeclarations
template <class T>
struct has_trivial_constructor;

template <class T>
struct has_trivial_destructor;

namespace interprocess {

template <class T>
struct is_multisegment_ptr;

struct intersegment_base
{
   typedef intersegment_base  self_t;
   BOOST_STATIC_ASSERT((sizeof(std::size_t) == sizeof(void*)));
   BOOST_STATIC_ASSERT((sizeof(void*)*CHAR_BIT == 32 || sizeof(void*)*CHAR_BIT == 64));
   static const std::size_t size_t_bits = (sizeof(void*)*CHAR_BIT == 32) ? 32 : 64;
   static const std::size_t ctrl_bits = 2;
   static const std::size_t align_bits = 12;
   static const std::size_t align      = std::size_t(1) << align_bits;
   static const std::size_t max_segment_size_bits = size_t_bits - 2;
   static const std::size_t max_segment_size = std::size_t(1) << max_segment_size_bits;

   static const std::size_t begin_bits             = max_segment_size_bits - align_bits;
   static const std::size_t pow_size_bits_helper = static_log2<max_segment_size_bits>::value;
   static const std::size_t pow_size_bits =
      (max_segment_size_bits == (std::size_t(1) << pow_size_bits_helper)) ?
      pow_size_bits_helper : pow_size_bits_helper + 1;
   static const std::size_t frc_size_bits =
      size_t_bits - ctrl_bits - begin_bits - pow_size_bits;

   BOOST_STATIC_ASSERT(((size_t_bits - pow_size_bits - frc_size_bits) >= ctrl_bits ));

   static const std::size_t relative_size_bits =
      size_t_bits - max_segment_size_bits - ctrl_bits;

   static const std::size_t is_pointee_outside  = 0;
   static const std::size_t is_in_stack         = 1;
   static const std::size_t is_relative         = 2;
   static const std::size_t is_segmented        = 3;
   static const std::size_t is_max_mode         = 4;

   intersegment_base()
   {
      this->set_mode(is_pointee_outside);
      this->set_null();
   }

   struct relative_addressing
   {
      std::size_t ctrl     :  2;
      std::size_t pow      :  pow_size_bits;
      std::size_t frc      :  frc_size_bits;
      std::size_t beg      :  begin_bits;
      std::ptrdiff_t off   :  sizeof(std::ptrdiff_t)*CHAR_BIT - 2;
      std::ptrdiff_t bits  :  2;
   };

   struct direct_addressing
   {
      std::size_t ctrl     :  2;
      std::size_t dummy    :  sizeof(std::size_t)*CHAR_BIT - 2;
      void * addr;
   };

   struct segmented_addressing
   {
      std::size_t ctrl     :  2;
      std::size_t segment  :  sizeof(std::size_t)*CHAR_BIT - 2;
      std::size_t off      :  sizeof(std::size_t)*CHAR_BIT - 2;
      std::size_t bits     :  2;
   };

   union members_t{
      relative_addressing  relative;
      direct_addressing    direct;
      segmented_addressing segmented;
   } members;

   BOOST_STATIC_ASSERT(sizeof(members_t) == 2*sizeof(std::size_t));

   void *relative_calculate_begin_addr() const
   {
      const std::size_t mask = ~(align - 1);
      std::size_t beg = this->members.relative.beg;
      return reinterpret_cast<void*>((((std::size_t)this) & mask) - (beg << align_bits));
   }

   void relative_set_begin_from_base(void *addr)
   {
      BOOST_ASSERT(addr < static_cast<void*>(this));
      std::size_t off = reinterpret_cast<char*>(this) - reinterpret_cast<char*>(addr);
      members.relative.beg = off >> align_bits;
   }

   //!Obtains the address pointed by the
   //!object
   std::size_t relative_size() const
   {
      std::size_t pow  = members.relative.pow;
      std::size_t size = (std::size_t(1u) << pow);
      BOOST_ASSERT(pow >= frc_size_bits);
      size |= members.relative.frc << (pow - frc_size_bits);
      return size;
   }

   static std::size_t calculate_size(std::size_t orig_size, std::size_t &pow, std::size_t &frc)
   {
      if(orig_size < align)
         orig_size = align;
      orig_size = ipcdetail::get_rounded_size_po2(orig_size, align);
      pow = ipcdetail::floor_log2(orig_size);
      std::size_t low_size = (std::size_t(1) << pow);
      std::size_t diff = orig_size - low_size;
      BOOST_ASSERT(pow >= frc_size_bits);
      std::size_t rounded = ipcdetail::get_rounded_size_po2
                              (diff, (std::size_t)(1u << (pow - frc_size_bits)));
      if(rounded == low_size){
         ++pow;
         frc = 0;
         rounded  = 0;
      }
      else{
         frc = rounded >> (pow - frc_size_bits);
      }
      BOOST_ASSERT(((frc << (pow - frc_size_bits)) & (align-1))==0);
      return low_size + rounded;
   }

   std::size_t get_mode()const
   {  return members.direct.ctrl;   }

   void set_mode(std::size_t mode)
   {
      BOOST_ASSERT(mode < is_max_mode);
      members.direct.ctrl = mode;
   }

   //!Returns true if object represents
   //!null pointer
   bool is_null() const
   {
      return   (this->get_mode() < is_relative) &&
               !members.direct.dummy &&
               !members.direct.addr;
   }

   //!Sets the object to represent
   //!the null pointer
   void set_null()
   {
      if(this->get_mode() >= is_relative){
         this->set_mode(is_pointee_outside);
      }
      members.direct.dummy = 0;
      members.direct.addr  = 0;
   }

   static std::size_t round_size(std::size_t orig_size)
   {
      std::size_t pow, frc;
      return calculate_size(orig_size, pow, frc);
   }
};

//!Configures intersegment_ptr with the capability to address:
//!2^(sizeof(std::size_t)*CHAR_BIT/2) segment groups
//!2^(sizeof(std::size_t)*CHAR_BIT/2) segments per group.
//!2^(sizeof(std::size_t)*CHAR_BIT/2)-1 bytes maximum per segment.
//!The mapping is implemented through flat_maps synchronized with mutexes.
template <class Mutex>
struct flat_map_intersegment
   :  public intersegment_base
{
   typedef flat_map_intersegment<Mutex>   self_t;

   void set_from_pointer(const volatile void *ptr)
   {  this->set_from_pointer(const_cast<const void *>(ptr));  }

   //!Obtains the address pointed
   //!by the object
   void *get_pointer() const
   {
      if(is_null()){
         return 0;
      }
      switch(this->get_mode()){
         case is_relative:
            return const_cast<char*>(reinterpret_cast<const char*>(this)) + members.relative.off;
         break;
         case is_segmented:
            {
            segment_info_t segment_info;
            std::size_t offset;
            void *this_base;
            get_segment_info_and_offset(this, segment_info, offset, this_base);
            char *base  = static_cast<char*>(segment_info.group->address_of(this->members.segmented.segment));
            return base + this->members.segmented.off;
            }
         break;
         case is_in_stack:
         case is_pointee_outside:
            return members.direct.addr;
         break;
         default:
         return 0;
         break;
      }
   }

   //!Calculates the distance between two basic_intersegment_ptr-s.
   //!This only works with two basic_intersegment_ptr pointing
   //!to the same segment. Otherwise undefined
   std::ptrdiff_t diff(const self_t &other) const
   {  return static_cast<char*>(this->get_pointer()) - static_cast<char*>(other.get_pointer());   }

   //!Returns true if both point to
   //!the same object
   bool equal(const self_t &y) const
   {  return this->get_pointer() == y.get_pointer();  }

   //!Returns true if *this is less than other.
   //!This only works with two basic_intersegment_ptr pointing
   //!to the same segment group. Otherwise undefined. Never throws
   bool less(const self_t &y) const
   {  return this->get_pointer() < y.get_pointer(); }

   void swap(self_t &other)
   {
      void *ptr_this  = this->get_pointer();
      void *ptr_other = other.get_pointer();
      other.set_from_pointer(ptr_this);
      this->set_from_pointer(ptr_other);
   }

   //!Sets the object internals to represent the
   //!address pointed by ptr
   void set_from_pointer(const void *ptr)
   {
      if(!ptr){
         this->set_null();
         return;
      }

      std::size_t mode = this->get_mode();
      if(mode == is_in_stack){
         members.direct.addr = const_cast<void*>(ptr);
         return;
      }
      if(mode == is_relative){
         char *beg_addr = static_cast<char*>(this->relative_calculate_begin_addr());
         std::size_t seg_size = this->relative_size();
         if(ptr >= beg_addr && ptr < (beg_addr + seg_size)){
            members.relative.off = static_cast<const char*>(ptr) - reinterpret_cast<const char*>(this);
            return;
         }
      }
      std::size_t ptr_offset;
      std::size_t this_offset;
      segment_info_t ptr_info;
      segment_info_t this_info;
      void *ptr_base;
      void *this_base;
      get_segment_info_and_offset(this, this_info, this_offset, this_base);

      if(!this_info.group){
         this->set_mode(is_in_stack);
         this->members.direct.addr = const_cast<void*>(ptr);
      }
      else{
         get_segment_info_and_offset(ptr, ptr_info, ptr_offset, ptr_base);

         if(ptr_info.group != this_info.group){
            this->set_mode(is_pointee_outside);
            this->members.direct.addr =  const_cast<void*>(ptr);
         }
         else if(ptr_info.id == this_info.id){
            this->set_mode(is_relative);
            members.relative.off = (static_cast<const char*>(ptr) - reinterpret_cast<const char*>(this));
            this->relative_set_begin_from_base(this_base);
            std::size_t pow, frc;
            std::size_t s = calculate_size(this_info.size, pow, frc);
            (void)s;
            BOOST_ASSERT(this_info.size == s);
            this->members.relative.pow = pow;
            this->members.relative.frc = frc;
         }
         else{
            this->set_mode(is_segmented);
            this->members.segmented.segment = ptr_info.id;
            this->members.segmented.off     = ptr_offset;
         }
      }
   }

   //!Sets the object internals to represent the address pointed
   //!by another flat_map_intersegment
   void set_from_other(const self_t &other)
   {
      this->set_from_pointer(other.get_pointer());
   }

   //!Increments internal
   //!offset
   void inc_offset(std::ptrdiff_t bytes)
   {
      this->set_from_pointer(static_cast<char*>(this->get_pointer()) + bytes);
   }

   //!Decrements internal
   //!offset
   void dec_offset(std::ptrdiff_t bytes)
   {
      this->set_from_pointer(static_cast<char*>(this->get_pointer()) - bytes);
   }

   //////////////////////////////////////
   //////////////////////////////////////
   //////////////////////////////////////

   flat_map_intersegment()
      :  intersegment_base()
   {}

   ~flat_map_intersegment()
   {}

   private:

   class segment_group_t
   {
      struct segment_data
      {
         void *addr;
         std::size_t size;
      };
      vector<segment_data> m_segments;
      multi_segment_services &m_ms_services;

      public:
      segment_group_t(multi_segment_services &ms_services)
         :  m_ms_services(ms_services)
      {}

      void push_back(void *addr, std::size_t size)
      {
         segment_data d = { addr, size };
         m_segments.push_back(d);
      }

      void pop_back()
      {
         BOOST_ASSERT(!m_segments.empty());
         m_segments.erase(--m_segments.end());
      }

      void *address_of(std::size_t segment_id)
      {
         BOOST_ASSERT(segment_id < (std::size_t)m_segments.size());
         return m_segments[segment_id].addr;
      }

      void clear_segments()
      {  m_segments.clear();  }

      std::size_t get_size() const
      {  return m_segments.size();  }

      multi_segment_services &get_multi_segment_services() const
      {  return m_ms_services;   }

      friend bool operator< (const segment_group_t&l, const segment_group_t &r)
      {  return &l.m_ms_services < &r.m_ms_services;   }
   };

   struct segment_info_t
   {
      std::size_t size;
      std::size_t id;
      segment_group_t *group;
      segment_info_t()
         :  size(0), id(0), group(0)
      {}
   };

   typedef set<segment_group_t>  segment_groups_t;

   typedef boost::interprocess::flat_map
      <const void *
      ,segment_info_t
      ,std::less<const void *> >          ptr_to_segment_info_t;

   struct mappings_t : Mutex
   {
      //!Mutex to preserve integrity in multi-threaded
      //!enviroments
      typedef Mutex        mutex_type;
      //!Maps base addresses and segment information
      //!(size and segment group and id)*

      ptr_to_segment_info_t      m_ptr_to_segment_info;

      ~mappings_t()
      {
         //Check that all mappings have been erased
         BOOST_ASSERT(m_ptr_to_segment_info.empty());
      }
   };

   //Static members
   static mappings_t       s_map;
   static segment_groups_t s_groups;
   public:

   typedef segment_group_t*      segment_group_id;

   //!Returns the segment and offset
   //!of an address
   static void get_segment_info_and_offset(const void *ptr, segment_info_t &segment, std::size_t &offset, void *&base)
   {
      //------------------------------------------------------------------
      boost::interprocess::scoped_lock<typename mappings_t::mutex_type> lock(s_map);
      //------------------------------------------------------------------
      base = 0;
      if(s_map.m_ptr_to_segment_info.empty()){
         segment = segment_info_t();
         offset  = reinterpret_cast<const char*>(ptr) - static_cast<const char*>(0);
         return;
      }
      //Find the first base address greater than ptr
      typename ptr_to_segment_info_t::iterator it
         = s_map.m_ptr_to_segment_info.upper_bound(ptr);
      if(it == s_map.m_ptr_to_segment_info.begin()){
         segment = segment_info_t();
         offset  = reinterpret_cast<const char*>(ptr) - static_cast<const char *>(0);
      }
      //Go to the previous one
      --it;
      char *      segment_base = const_cast<char*>(reinterpret_cast<const char*>(it->first));
      std::size_t segment_size = it->second.size;

      if(segment_base <= reinterpret_cast<const char*>(ptr) &&
         (segment_base + segment_size) >= reinterpret_cast<const char*>(ptr)){
         segment = it->second;
         offset  = reinterpret_cast<const char*>(ptr) - segment_base;
         base = segment_base;
      }
      else{
         segment = segment_info_t();
         offset  = reinterpret_cast<const char*>(ptr) - static_cast<const char*>(0);
      }
   }

   //!Associates a segment defined by group/id with a base address and size.
   //!Returns false if the group is not found or there is an error
   static void insert_mapping(segment_group_id group_id, void *ptr, std::size_t size)
   {
      //------------------------------------------------------------------
      boost::interprocess::scoped_lock<typename mappings_t::mutex_type> lock(s_map);
      //------------------------------------------------------------------

      typedef typename ptr_to_segment_info_t::value_type value_type;
      typedef typename ptr_to_segment_info_t::iterator   iterator;
      typedef std::pair<iterator, bool>                  it_b_t;

      segment_info_t info;
      info.group = group_id;
      info.size  = size;
      info.id    = group_id->get_size();

      it_b_t ret = s_map.m_ptr_to_segment_info.insert(value_type(ptr, info));
      BOOST_ASSERT(ret.second);

      value_eraser<ptr_to_segment_info_t> v_eraser(s_map.m_ptr_to_segment_info, ret.first);
      group_id->push_back(ptr, size);
      v_eraser.release();
   }

   static bool erase_last_mapping(segment_group_id group_id)
   {
      //------------------------------------------------------------------
      boost::interprocess::scoped_lock<typename mappings_t::mutex_type> lock(s_map);
      //------------------------------------------------------------------
      if(!group_id->get_size()){
         return false;
      }
      else{
         void *addr = group_id->address_of(group_id->get_size()-1);
         group_id->pop_back();
         std::size_t erased = s_map.m_ptr_to_segment_info.erase(addr);
         (void)erased;
         BOOST_ASSERT(erased);
         return true;
      }
   }

   static segment_group_id new_segment_group(multi_segment_services *services)
   {
      {  //------------------------------------------------------------------
         boost::interprocess::scoped_lock<typename mappings_t::mutex_type> lock(s_map);
         //------------------------------------------------------------------
         typedef typename segment_groups_t::iterator iterator;
         std::pair<iterator, bool> ret =
            s_groups.insert(segment_group_t(*services));
         BOOST_ASSERT(ret.second);
         return &*ret.first;
      }
   }

   static bool delete_group(segment_group_id id)
   {
      {  //------------------------------------------------------------------
         boost::interprocess::scoped_lock<typename mappings_t::mutex_type> lock(s_map);
         //------------------------------------------------------------------
         bool success = 1u == s_groups.erase(segment_group_t(*id));
         if(success){
            typedef typename ptr_to_segment_info_t::iterator ptr_to_segment_info_it;
            ptr_to_segment_info_it it(s_map.m_ptr_to_segment_info.begin());
            while(it != s_map.m_ptr_to_segment_info.end()){
               if(it->second.group == id){
                  it = s_map.m_ptr_to_segment_info.erase(it);
               }
               else{
                  ++it;
               }
            }
         }
         return success;
      }
   }
};

//!Static map-segment_info associated with
//!flat_map_intersegment<>
template <class Mutex>
typename flat_map_intersegment<Mutex>::mappings_t
   flat_map_intersegment<Mutex>::s_map;

//!Static segment group container associated with
//!flat_map_intersegment<>
template <class Mutex>
typename flat_map_intersegment<Mutex>::segment_groups_t
   flat_map_intersegment<Mutex>::s_groups;

//!A smart pointer that can point to a pointee that resides in another memory
//!memory mapped or shared memory segment.
template <class T>
class intersegment_ptr : public flat_map_intersegment<interprocess_mutex>
{
   typedef flat_map_intersegment<interprocess_mutex> PT;
   typedef intersegment_ptr<T>                  self_t;
   typedef PT                                      base_t;

   void unspecified_bool_type_func() const {}
   typedef void (self_t::*unspecified_bool_type)() const;

   public:
   typedef T *                                     pointer;
   typedef typename ipcdetail::add_reference<T>::type reference;
   typedef T                                       value_type;
   typedef std::ptrdiff_t                          difference_type;
   typedef std::random_access_iterator_tag         iterator_category;

   public:   //Public Functions

   //!Constructor from raw pointer (allows "0" pointer conversion).
   //!Never throws.
   intersegment_ptr(pointer ptr = 0)
   {  base_t::set_from_pointer(ptr);   }

   //!Constructor from other pointer.
   //!Never throws.
   template <class U>
   intersegment_ptr(U *ptr){  base_t::set_from_pointer(pointer(ptr)); }

   //!Constructor from other intersegment_ptr
   //!Never throws
   intersegment_ptr(const intersegment_ptr& ptr)
   {  base_t::set_from_other(ptr);   }

   //!Constructor from other intersegment_ptr. If pointers of pointee types are
   //!convertible, intersegment_ptrs will be convertibles. Never throws.
   template<class T2>
   intersegment_ptr(const intersegment_ptr<T2> &ptr)
   {  pointer p(ptr.get());   (void)p; base_t::set_from_other(ptr); }

   //!Emulates static_cast operator.
   //!Never throws.
   template<class U>
   intersegment_ptr(const intersegment_ptr<U> &r, ipcdetail::static_cast_tag)
   {  base_t::set_from_pointer(static_cast<T*>(r.get())); }

   //!Emulates const_cast operator.
   //!Never throws.
   template<class U>
   intersegment_ptr(const intersegment_ptr<U> &r, ipcdetail::const_cast_tag)
   {  base_t::set_from_pointer(const_cast<T*>(r.get())); }

   //!Emulates dynamic_cast operator.
   //!Never throws.
   template<class U>
   intersegment_ptr(const intersegment_ptr<U> &r, ipcdetail::dynamic_cast_tag)
   {  base_t::set_from_pointer(dynamic_cast<T*>(r.get())); }

   //!Emulates reinterpret_cast operator.
   //!Never throws.
   template<class U>
   intersegment_ptr(const intersegment_ptr<U> &r, ipcdetail::reinterpret_cast_tag)
   {  base_t::set_from_pointer(reinterpret_cast<T*>(r.get())); }

   //!Obtains raw pointer from offset.
   //!Never throws.
   pointer get()const
   {  return static_cast<pointer>(base_t::get_pointer());   }

   //!Pointer-like -> operator. It can return 0 pointer.
   //!Never throws.
   pointer operator->() const
   {  return self_t::get(); }

   //!Dereferencing operator, if it is a null intersegment_ptr behavior
   //!is undefined. Never throws.
   reference operator* () const
   {  return *(self_t::get());   }

   //!Indexing operator.
   //!Never throws.
   reference operator[](std::ptrdiff_t idx) const
   {  return self_t::get()[idx];  }

   //!Assignment from pointer (saves extra conversion).
   //!Never throws.
   intersegment_ptr& operator= (pointer from)
   {  base_t::set_from_pointer(from); return *this;  }

   //!Assignment from other intersegment_ptr.
   //!Never throws.
   intersegment_ptr& operator= (const intersegment_ptr &ptr)
   {  base_t::set_from_other(ptr);  return *this;  }

   //!Assignment from related intersegment_ptr. If pointers of pointee types
   //!are assignable, intersegment_ptrs will be assignable. Never throws.
   template <class T2>
   intersegment_ptr& operator= (const intersegment_ptr<T2> & ptr)
   {
      pointer p(ptr.get());   (void)p;
      base_t::set_from_other(ptr); return *this;
   }

   //!intersegment_ptr + std::ptrdiff_t.
   //!Never throws.
   intersegment_ptr operator+ (std::ptrdiff_t idx) const
   {
      intersegment_ptr result (*this);
      result.inc_offset(idx*sizeof(T));
      return result;
   }

   //!intersegment_ptr - std::ptrdiff_t.
   //!Never throws.
   intersegment_ptr operator- (std::ptrdiff_t idx) const
   {
      intersegment_ptr result (*this);
      result.dec_offset(idx*sizeof(T));
      return result;
   }

   //!intersegment_ptr += std::ptrdiff_t.
   //!Never throws.
   intersegment_ptr &operator+= (std::ptrdiff_t offset)
   {  base_t::inc_offset(offset*sizeof(T));  return *this;  }

   //!intersegment_ptr -= std::ptrdiff_t.
   //!Never throws.
   intersegment_ptr &operator-= (std::ptrdiff_t offset)
   {  base_t::dec_offset(offset*sizeof(T));  return *this;  }

   //!++intersegment_ptr.
   //!Never throws.
   intersegment_ptr& operator++ (void)
   {  base_t::inc_offset(sizeof(T));   return *this;  }

   //!intersegment_ptr++.
   //!Never throws.
   intersegment_ptr operator++ (int)
   {  intersegment_ptr temp(*this); ++*this; return temp; }

   //!--intersegment_ptr.
   //!Never throws.
   intersegment_ptr& operator-- (void)
   {  base_t::dec_offset(sizeof(T));   return *this;  }

   //!intersegment_ptr--.
   //!Never throws.
   intersegment_ptr operator-- (int)
   {  intersegment_ptr temp(*this); --*this; return temp; }

   //!Safe bool conversion operator.
   //!Never throws.
   operator unspecified_bool_type() const
   {  return base_t::is_null()? 0 : &self_t::unspecified_bool_type_func;   }

   //!Not operator. Not needed in theory, but improves portability.
   //!Never throws.
   bool operator! () const
   {  return base_t::is_null();   }

   //!Swaps two intersegment_ptr-s. More efficient than std::swap.
   //!Never throws.
   void swap(intersegment_ptr &other)
   {  base_t::swap(other);   }

   //!Calculates the distance between two intersegment_ptr-s.
   //!This only works with two basic_intersegment_ptr pointing
   //!to the same segment. Otherwise undefined
   template <class T2>
   std::ptrdiff_t _diff(const intersegment_ptr<T2> &other) const
   {  return base_t::diff(other);   }

   //!Returns true if both point to the
   //!same object
   template <class T2>
   bool _equal(const intersegment_ptr<T2>&other) const
   {  return base_t::equal(other);   }

   //!Returns true if *this is less than other.
   //!This only works with two basic_intersegment_ptr pointing
   //!to the same segment group. Otherwise undefined. Never throws
   template <class T2>
   bool _less(const intersegment_ptr<T2> &other) const
   {  return base_t::less(other);   }
};

//!Compares the equality of two intersegment_ptr-s.
//!Never throws.
template <class T1, class T2> inline
bool operator ==(const intersegment_ptr<T1> &left,
                 const intersegment_ptr<T2> &right)
{
   //Make sure both pointers can be compared
   bool e = typename intersegment_ptr<T1>::pointer(0) ==
            typename intersegment_ptr<T2>::pointer(0);
   (void)e;
   return left._equal(right);
}

//!Returns true if *this is less than other.
//!This only works with two basic_intersegment_ptr pointing
//!to the same segment group. Otherwise undefined. Never throws
template <class T1, class T2> inline
bool operator <(const intersegment_ptr<T1> &left,
                const intersegment_ptr<T2> &right)
{
   //Make sure both pointers can be compared
   bool e = typename intersegment_ptr<T1>::pointer(0) <
            typename intersegment_ptr<T2>::pointer(0);
   (void)e;
   return left._less(right);
}

template<class T1, class T2> inline
bool operator!= (const intersegment_ptr<T1> &pt1,
                 const intersegment_ptr<T2> &pt2)
{  return !(pt1 ==pt2);  }

//!intersegment_ptr<T1> <= intersegment_ptr<T2>.
//!Never throws.
template<class T1, class T2> inline
bool operator<= (const intersegment_ptr<T1> &pt1,
                 const intersegment_ptr<T2> &pt2)
{  return !(pt1 > pt2);  }

//!intersegment_ptr<T1> > intersegment_ptr<T2>.
//!Never throws.
template<class T1, class T2> inline
bool operator> (const intersegment_ptr<T1> &pt1,
                       const intersegment_ptr<T2> &pt2)
{  return (pt2 < pt1);  }

//!intersegment_ptr<T1> >= intersegment_ptr<T2>.
//!Never throws.
template<class T1, class T2> inline
bool operator>= (const intersegment_ptr<T1> &pt1,
                 const intersegment_ptr<T2> &pt2)
{  return !(pt1 < pt2);  }

//!operator<<
template<class E, class T, class U> inline
std::basic_ostream<E, T> & operator<<
   (std::basic_ostream<E, T> & os, const intersegment_ptr<U> & p)
{  return os << p.get();   }

//!operator>>
template<class E, class T, class U> inline
std::basic_istream<E, T> & operator>>
   (std::basic_istream<E, T> & os, intersegment_ptr<U> & p)
{  U * tmp; return os >> tmp; p = tmp;   }

//!std::ptrdiff_t + intersegment_ptr.
//!The result is another pointer of the same segment
template<class T> inline
intersegment_ptr<T> operator+
   (std::ptrdiff_t diff, const intersegment_ptr<T>& right)
{  return right + diff;  }

//!intersegment_ptr - intersegment_ptr.
//!This only works with two intersegment_ptr-s that point to the
//!same segment
template <class T, class T2> inline
std::ptrdiff_t operator- (const intersegment_ptr<T> &pt,
                          const intersegment_ptr<T2> &pt2)
{  return pt._diff(pt2)/sizeof(T);  }

//! swap specialization
template<class T> inline
void swap (boost::interprocess::intersegment_ptr<T> &pt,
           boost::interprocess::intersegment_ptr<T> &pt2)
{  pt.swap(pt2);  }

//!get_pointer() enables boost::mem_fn to recognize intersegment_ptr.
//!Never throws.
template<class T> inline
T * get_pointer(boost::interprocess::intersegment_ptr<T> const & p)
{  return p.get();   }

//!Simulation of static_cast between pointers.
//!Never throws.
template<class T, class U> inline
boost::interprocess::intersegment_ptr<T> static_pointer_cast(const boost::interprocess::intersegment_ptr<U> &r)
{  return boost::interprocess::intersegment_ptr<T>(r, boost::interprocess::ipcdetail::static_cast_tag());  }

//!Simulation of const_cast between pointers.
//!Never throws.
template<class T, class U> inline
boost::interprocess::intersegment_ptr<T> const_pointer_cast(const boost::interprocess::intersegment_ptr<U> &r)
{  return boost::interprocess::intersegment_ptr<T>(r, boost::interprocess::ipcdetail::const_cast_tag());  }

//!Simulation of dynamic_cast between pointers.
//!Never throws.
template<class T, class U> inline
boost::interprocess::intersegment_ptr<T> dynamic_pointer_cast(const boost::interprocess::intersegment_ptr<U> &r)
{  return boost::interprocess::intersegment_ptr<T>(r, boost::interprocess::ipcdetail::dynamic_cast_tag());  }

//!Simulation of reinterpret_cast between pointers.
//!Never throws.
template<class T, class U> inline
boost::interprocess::intersegment_ptr<T> reinterpret_pointer_cast(const boost::interprocess::intersegment_ptr<U> &r)
{  return boost::interprocess::intersegment_ptr<T>(r, boost::interprocess::ipcdetail::reinterpret_cast_tag());  }

//!Trait class to detect if an smart pointer has
//!multi-segment addressing capabilities.
template <class T>
struct is_multisegment_ptr
   <boost::interprocess::intersegment_ptr<T> >
{
   static const bool value = true;
};

}  //namespace interprocess {

#if defined(_MSC_VER) && (_MSC_VER < 1400)
//!get_pointer() enables boost::mem_fn to recognize intersegment_ptr.
//!Never throws.
template<class T> inline
T * get_pointer(boost::interprocess::intersegment_ptr<T> const & p)
{  return p.get();   }
#endif

//!has_trivial_constructor<> == true_type specialization
//!for optimizations
template <class T>
struct has_trivial_constructor
   < boost::interprocess::intersegment_ptr<T> >
   : public true_type{};

//!has_trivial_destructor<> == true_type specialization
//!for optimizations
template <class T>
struct has_trivial_destructor
   < boost::interprocess::intersegment_ptr<T> >
   : public true_type{};

}  //namespace boost {

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

#if 0

//bits
//-> is_segmented
//-> is_relative
//-> is_in_stack
//-> is_pointee_outside

//Data

//segmented:
//
// std::size_t ctrl    : CTRL_BITS;
// std::size_t segment : MAX_SEGMENT_BITS;
// std::size_t offset;

//RELATIVE_SIZE_BITS =  SIZE_T_BITS -
//                      MAX_SEGMENT_BITS -
//                      CTRL_BITS                  10    10
//MAX_SEGMENT_SIZE   = SIZE_T_BITS - ALIGN_BITS    20    52

//SIZE_T_BITS - 1 - ALIGN_BITS                     19    51
//POW_SIZE_BITS = upper_log2
// (SIZE_T_BITS - 1 - ALIGN_BITS)                  5     6
//FRC_SIZE_BITS = SIZE_T_BITS - CTRL_BITS
//  MAX_SEGMENT_SIZE_ALIGNBITS - POW_SIZE_BITS     6     5

//relative:
//
// std::size_t ctrl     : CTRL_BITS;               2     2
// std::size_t size_pow : POW_SIZE_BITS            5     6
// std::size_t size_frc : FRC_SIZE_BITS;           6     5
// std::size_t start    : MAX_SEGMENT_SIZE_ALIGNBITS;19    51
// std::ptrdiff_t distance : SIZE_T_BITS;          32    64

//direct:
//
// std::size_t ctrl     : CTRL_BITS;               2     2
// std::size_t dummy    : SIZE_T_BITS - CTRL_BITS  30    62
// void *addr           : SIZE_T_BITS;             32    64

//32 bits systems:
//Page alignment: 2**12
//

//!Obtains the address pointed by the
//!object
void *get_pointer() const
{
   if(this->is_pointee_outside() || this->is_in_stack()){
      return raw_address();
   }
   else if(this->is_relative()){
      return (const_cast<char*>(reinterpret_cast<const char*>(this))) + this->relative_pointee_offset();
   }
   else{
      group_manager *m     = get_segment_group_manager(addr);
      char *base  = static_cast<char*>(m->get_id_address(this->segmented_id()));
      return base + this->segmented_offset();
   }
}

void set_from_pointer(const void *ptr)
{
   if(!ptr){
      this->set_pointee_outside();
      this->raw_address(ptr);
   }
   else if(this->is_in_stack()){
      this->raw_address(ptr);
   }
   else if(this->is_relative() &&
            (  (ptr >= this->relative_start())
            &&(ptr <  this->relative_start() + this->relative_size()))
            ){
      this->relative_offset(ptr - this);
   }
   else{
      segment_info_t ptr_info  = get_id_from_addr(ptr);
      segment_info_t this_info = get_id_from_addr(this);
      if(ptr_info.segment_group != this_info.segment_group){
         if(!ptr_info.segment_group){
            this->set_in_stack();
         }
         else{
            this->set_pointee_outside();
         }
      }
      else if(ptr_info.segment_id == this_info.segment_id){
         set_relative();
         this->relative_size  (ptr_info.size);
         this->relative_offset(static_cast<const char*>(ptr) - reinterpret_cast<const char*>(this));
         this->relative_start (ptr_info.base);
      }
   }
}

void set_from_other(const self_t &other)
{  this->set_from_pointer(other.get_pointer()); }

#endif

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

#endif //#ifndef BOOST_INTERPROCESS_INTERSEGMENT_PTR_HPP


//
// Created by eliezer on 12.12.16.
//

#include <iostream>
#include <chrono>
#include <boost/algorithm/string.hpp>
//#include "BatchPoisson.h"
#include "BatchPoissonPure.h"
#include "datasets.h"
#include "math.h"

using namespace std;
int main(int argc, char* argv[]) {

    /**
    BatchPoissonNewArray(size_t n_ratings,size_t n_wd_entries,size_t n_users, size_t n_items, size_t k_feat, size_t n_words,size_t n_max_neighbors,
                 float a=0.1, float b=0.1, float c=0.1, float d=0.1, float e=0.1, float f=0.1, float g=0.1,
                 float h=0.1, float k=0.1, float l=0.1);
n_words= 11946
n_user= 1892
n_item= 17632
n_neighbors= 119
n_wd_tags_entries= 184941
n_ratings= 278502
     */
    auto t1 = std::chrono::high_resolution_clock::now();

    cout << endl<< "#begin_all :";
    for(int i=0;i<argc;i++){
        cout<< argv[i] << " ";
    }
    cout << endl;

    if (argc < 6) { // We expect 3 arguments: the program name, the source path and the destination path
        std::cout << "runing with default configuration" << std::endl;
        cout << "dataset: /home/eliezer/datasets/hetrec2011/lastfm/p85_train_test_9208/" << endl;
        cout << "k = 100, niter=100, tol=10^6" <<endl;

        experiment exp1("/home/eliezer/datasets/hetrec2011/lastfm/p85_train_test_9208/");
        exp1.run(100,100,6,350,1.0,1.0);
    }
    else if (argc==5){
        size_t k=(size_t)std::stoi(argv[1]);
        size_t niter=(size_t)std::stoi(argv[2]);
        size_t dec=(size_t)std::stoi(argv[3]);
        size_t n_rec=(size_t)std::stoi(argv[4]);
        std::cout << "runing with default dataset" << std::endl;
        cout << "dataset: /home/eliezer/datasets/hetrec2011/lastfm/p85_train_test_9208/" << endl;
        cout << "k = " << k<<", niter=" << niter<<", tol=10^"<< dec <<endl;

        experiment exp1("/home/eliezer/datasets/hetrec2011/lastfm/p85_train_test_9208/");
        exp1.run(k,niter,dec,n_rec,1.0, 1.0);
    }
    else if (argc==8){
        size_t k=(size_t)std::stoi(argv[1]);
        size_t niter=(size_t)std::stoi(argv[2]);
        size_t dec=(size_t)std::stoi(argv[3]);
        size_t n_rec=(size_t)std::stoi(argv[4]);
        string learn(argv[5]);
        bool is_learn;
        double w_content=std::stod(argv[6]);
        double w_social=std::stod(argv[7]);
        if(learn=="const_w")
            is_learn= false;
        else if(learn=="learn_w")
            is_learn=true;
        else
        {
            cout << "invalid is_learn argument, " << learn <<", use 'const_w' for " <<
                    "constant factor and 'learn_w' for factor learnt from the data " << endl;
            return -1;
        }
        std::cout << "runing with default dataset" << std::endl;
        cout << "dataset: /home/eliezer/datasets/hetrec2011/lastfm/p85_train_test_9208/" << endl;
        cout << "k = " << k<<", niter=" << niter<<", tol=10^"<< dec <<endl;

        experiment exp1("/home/eliezer/datasets/hetrec2011/lastfm/p85_train_test_9208/");
        exp1.run(k,niter,dec,n_rec, w_content , w_social,is_learn);
    }
    else if (argc>8){
        size_t k=(size_t)std::stoi(argv[1]);
        size_t niter=(size_t)std::stoi(argv[2]);
        size_t dec=(size_t)std::stoi(argv[3]);
        size_t n_rec=(size_t)std::stoi(argv[4]);
        string learn(argv[5]);
        bool is_learn;
        double w_content=std::stod(argv[6]);
        double w_social=std::stod(argv[7]);
        if(learn=="const_w")
            is_learn= false;
        else if(learn=="learn_w")
            is_learn=true;
        else
        {
            cout << "invalid is_learn argument, " << learn <<", use 'const_w' for " <<
                 "constant factor and 'learn_w' for factor learnt from the data " << endl;
            return -1;
        }
        std::cout << "runing with following configuration" << std::endl;
        cout << "dataset: " << argv[8] << endl;
        cout << "k = " << k<<", niter=" << niter<<", tol=10^"<< dec <<endl;

        experiment exp1(argv[8]);
        if(argc>9){
            string loglevel = argv[9];
            vector<string> strs;
            boost::split(strs,loglevel ,boost::is_any_of("="));
            if(strs.size()>=2){
                int ll = std::stoi(strs[1]);
                exp1.loglevel=ll;
                std::cout<<"loglevel="<<ll;
            }
        }
        exp1.run(k,niter,dec,n_rec, w_content , w_social,is_learn);
    }

    auto t2 = std::chrono::high_resolution_clock::now();

    cout << endl<< "#end_all :";
    for(int i=0;i<argc;i++){
        cout<< argv[i] << " ";
    }
    cout << endl;
    std::cout << "#time_all="
              << std::chrono::duration_cast<std::chrono::seconds>(t2-t1).count()
              << " seconds\n";

    return 0;
}


#include <iostream>
#include <vector>
#include <fstream>
#include <string>

#include "hh_post_det_model.hpp"
//#include "vgcc_det_model.hpp"
//#include "pmca_det_model.hpp"
#include "ca_sensor_gillespie_class.hpp"
#include "vgcc_gillespie_class.hpp"
#include "pmca_gillespie_class.hpp"

#include "gillespie_class.hpp"
#include "physical_constants.hpp"
#include "stl_vector_operation_functions.hpp"

#include <boost/numeric/odeint.hpp> /*  Specifying the file within < > lets c++ search for it at -I path  */
#include <boost/range/iterator_range.hpp>
#include <boost/range/iterator.hpp>

namespace bno=boost::numeric::odeint;
namespace gill=astron::gillespie;
namespace g_vgcc=gill::vgcc;
namespace g_pmca=gill::pmca;
namespace consts=astron::phy_const;

typedef std::vector< double > state_type;
typedef state_type::iterator state_type_it;
typedef bno::runge_kutta4 < state_type > rk4_stepper_type;

//-----------------
int main(int argc, char *argv[])
{
  //-------------------------------------
  std::string filename;         // Output filename variable
  // decode arguments passed
  if (argc < 2){
    filename = argv[0];
    std::cout << "No filename prefix provided" << std::endl;
    filename = filename.substr(2,filename.length()-2) + "_run";
  }
  else{
    filename = std::string(argv[1]);
  }
  std::ofstream fout(filename);
  if (!fout){
    std::cout << "Error opening file " << filename << "\n";
    return (0);
  }
  std::cout << "File = " << filename << std::endl;
  // ----------------------------------
  const double vol_pre = 1E-18;	// Volume of whole presynaptic terminal
  const double vol_pmca = 1E-18;
  const double vol_vgcc = 1E-18;

  double conCaIn = 100E-09;	// Baseline Calcium concentration
  int numCaIn = conCaIn * consts::Av * vol_pre * 1000; // Molarity in Liters 1L = 1000 * 10^-6 meter^3

  // Initialize deterministic objects
  rk4_stepper_type stepper;

  HH_POST hh_post(500E-12);                             // constructs an hh_pre object with 25 micron radius
  state_type X_hh_post = {0,0,0,-80E-03};                    //{m,h,n,V}

  //PMCA pmca(100E-09);                                   // constructs an PMCA object with 100 nM calcium concentration (intracellular)
  //state_type X_pmca = {100E-09, 17E-03, 0, 0};          //{conCaIn, PMCA0, PMCA1, PMCA2}

  //VGCC vgcc;                                            // constructs VGCCs
  //state_type X_vgcc = {50, 0, 0, 0, 0};                //{C0, C1, C2, C3, O}
  //-------------------------------------

  // Initialize MCMC: PMCA
  const int n_pmca = 1;
  const int n_states_pmca = 5; 	// [NCa, Leak, PMCA0, PMCA1, PMCA2]
  std::vector<int> X_pmca = {62,1530,1530,0,0}; // [NCa, NLeak, NPMCA0, PMCA1, PMCA2]
  std::vector<g_pmca::pmca_gillespie_class> gpmca;
  for(int i=0; i <n_pmca; i++){
    gpmca.push_back(g_pmca::pmca_gillespie_class(1000,X_pmca));
  }
  // Initialize MCMC: VGCC
  const int n_vgcc = 1;
  const int n_states_vgcc = 5;	// [C0 C1 C2 C3 O]
  std::vector<int> X_vgcc = {50,0,0,0,0};
  std::vector<g_vgcc::vgcc_gillespie_class> gvgcc;
  for(int i=0; i <n_vgcc; i++){
    gvgcc.push_back(g_vgcc::vgcc_gillespie_class(vol_vgcc,X_vgcc));
  }
  // Initialize MCMC: ca_sensors
  //const int n_ca_sensors = 1; // number of individual Ca-sensors/vesicles
  //const int n_states_ca_sensor=21; // number of Ca-sensor states
  //std::vector<int> X_cas(n_states_ca_sensor); // State vector for ca_sensor initialization in state X0Y0;
 //for(int i=0;i<n_states_ca_sensor; i++){
 //  X_cas[i] = 0;
 //};
 //X_cas[0] = 7;
 //std::vector<gill::ca_sensor_gillespie_class> ca_sensors;    // ca_sensor object construction
 //for(int i=0; i<n_ca_sensors; i++){
 //  ca_sensors.push_back(gill::ca_sensor_gillespie_class(X_cas)); // Loads state to each ca_sensor
 //}
 ////-------------------------------------
 ////                      time(1) IExt(1) X_hh_post(4) vgcc(5) pmca(4) conCaIn(1) ca_sensor(21) spon_rel(1) syn_rel(1) asyn_rel(1) total_rel(1) =
 //const int n_param_out = (1) + 2+ 21;
 //std::vector< double > Y(11);                           // Ouput vector
  //--------------------------------------
  double t = 0.0;
  const double dt = 10E-06;
  const double tmax = 100E-03;
  // -------------------------------------

  while (t <= tmax){
    //std::cout << "conCaIn = " << conCaIn << " numCaIn = " << numCaIn << std::endl;

    if( (t > 22E-03) && ( t < (24E-03 )) ){
      hh_post.IExt = 20E-02;	// External current to trigger presynaptic spike
    }
    if( (t >= 24E-03) && ( t <= (tmax )) ){
      hh_post.IExt = 0;
    }
    stepper.do_step( hh_post, X_hh_post, t, dt); // update voltage

    //gpmca[0].X[0] = numCaIn;
    for(int i=0; i<n_pmca; i++){
      gillespie(gpmca[i],t,dt);
    }
    numCaIn = gpmca[0].X[0]; //abs(g_pmca::pmca_gillespie_class::numCaIn);
    conCaIn = ((numCaIn) / (consts::Av * vol_pre * 1000));

    g_vgcc::vgcc_gillespie_class::V = X_hh_post[3];
    g_vgcc::vgcc_gillespie_class::conCaIn = conCaIn;
    for(int i=0; i<n_vgcc; i++){
      gillespie(gvgcc[i],t,dt);
    }
    numCaIn = numCaIn + abs(g_vgcc::vgcc_gillespie_class::numCaFlux);
    conCaIn = numCaIn / (consts::Av * vol_pre * 1000);

    fout << t << " " << X_hh_post << " " << gpmca[0].X << " " << gvgcc[0].X << " " << numCaIn << " " << conCaIn << std::endl;
    //std::cout << t << " " << gpmca[0].X << std::endl;
    //std::cin.get();
    t = t + dt;
  }
  fout.close();
  return 0;
}

// Extra codes
  // g_pmca::pmca_gillespie_class::numCaIn = numCaIn;
//g_vgcc::vgcc_gillespie_class::conCaIn = conCaIn;		   // Set vgcc.conCaIn
    //g_vgcc::vgcc_gillespie_class::V = X_hh_post[3];		   // Set vgcc.V
   // for(int i=0; i<n_vgcc; i++){
   //   gillespie(gvgcc[i],t,dt);
   // }
    //conCaIn = conCaIn + (g_pmca::pmca_gillespie_class::numCaIn / (consts::Av * vol_pmca) ); // update conCaIn

    //vgcc.V = X_hh_post[3];	// Feed presynaptic voltage to VGCC
    //stepper.do_step( vgcc, X_vgcc, t,  dt); // update vgcc
    //vgcc.update(dt, X_vgcc[4]);
    //X_pmca[0]  = conCaIn; // update conCaIn of PMCA buffers
    //stepper.do_step( pmca, X_pmca, t, dt);
    //conCaIn = X_pmca[0];	// update conCaIn
    //numCaIn_ca_sensor = round(vgcc.numCaIn/10) + 1;//und((vgcc.numCaIn - 59) / 60 );//round((vgcc.conCaIn - 59) / 6 );
    //gill::ca_sensor_gillespie_class::conCaIn = numCaIn_ca_sensor;
    //for(int i=0; i<n_ca_sensors; i++){
    //   gillespie(ca_sensors[i],t,dt);
    //}

   //Y[7] = gill::ca_sensor_gillespie_class::sponRelcount;
   //Y[8] = gill::ca_sensor_gillespie_class::synRelcount;
   //Y[9] = gill::ca_sensor_gillespie_class::asynRelcount;
   //Y[10] = gill::ca_sensor_gillespie_class::totalRelcount;


#include <stdlib.h>
#include <vector>
#include <string>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/dynamic_spatial_hashed_voxel_grid.hpp>
#include <sdf_tools/collision_map.hpp>

#ifndef DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP
#define DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP

namespace sdf_tools
{
    class DynamicSpatialHashedCollisionMapGrid
    {
    protected:

        VoxelGrid::DynamicSpatialHashedVoxelGrid<COLLISION_CELL> collision_field_;
        uint32_t number_of_components_;
        std::string frame_;
        bool initialized_;
        bool components_valid_;

    public:

        DynamicSpatialHashedCollisionMapGrid(std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value);

        DynamicSpatialHashedCollisionMapGrid(Eigen::Isometry3d origin_transform, std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value);

        DynamicSpatialHashedCollisionMapGrid();

        bool IsInitialized() const;

        bool AreComponentsValid() const;

        std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> Get(const double x, const double y, const double z) const;

        std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> Get(const Eigen::Vector3d& location) const;

        VoxelGrid::SET_STATUS SetCell(const double x, const double y, const double z, COLLISION_CELL value);

        VoxelGrid::SET_STATUS SetCell(const Eigen::Vector3d& location, COLLISION_CELL value);

        VoxelGrid::SET_STATUS SetChunk(const double x, const double y, const double z, COLLISION_CELL value);

        VoxelGrid::SET_STATUS SetChunk(const Eigen::Vector3d& location, COLLISION_CELL value);

        Eigen::Isometry3d GetOriginTransform() const;

        std::vector<visualization_msgs::Marker> ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;
    };
}

#endif // DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP


#ifndef ISO_8601_DURATION_PARSER_HPP
#define ISO_8601_DURATION_PARSER_HPP

#include <boost/bind.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/qi_action.hpp>

namespace osrm
{
namespace util
{

namespace qi = boost::spirit::qi;

template <typename Iterator> struct iso_8601_grammar : qi::grammar<Iterator>
{
    iso_8601_grammar()
        : iso_8601_grammar::base_type(iso_period), temp(0), hours(0), minutes(0), seconds(0)
    {
        iso_period = qi::lit('P') >> qi::lit('T') >>
                     ((value >> hour >> value >> minute >> value >> second) |
                      (value >> hour >> value >> minute) | (value >> hour >> value >> second) |
                      (value >> hour) | (value >> minute >> value >> second) | (value >> minute) |
                      (value >> second));

        value = qi::uint_[boost::bind(&iso_8601_grammar<Iterator>::set_temp, this, ::_1)];
        second = (qi::lit('s') |
                  qi::lit('S'))[boost::bind(&iso_8601_grammar<Iterator>::set_seconds, this)];
        minute = (qi::lit('m') |
                  qi::lit('M'))[boost::bind(&iso_8601_grammar<Iterator>::set_minutes, this)];
        hour = (qi::lit('h') |
                qi::lit('H'))[boost::bind(&iso_8601_grammar<Iterator>::set_hours, this)];
    }

    qi::rule<Iterator> iso_period;
    qi::rule<Iterator, std::string()> value, hour, minute, second;

    unsigned temp;
    unsigned hours;
    unsigned minutes;
    unsigned seconds;

    void set_temp(unsigned number) { temp = number; }

    void set_hours()
    {
        if (temp < 24)
        {
            hours = temp;
        }
    }

    void set_minutes()
    {
        if (temp < 60)
        {
            minutes = temp;
        }
    }

    void set_seconds()
    {
        if (temp < 60)
        {
            seconds = temp;
        }
    }

    unsigned get_duration() const
    {
        unsigned temp = (3600 * hours + 60 * minutes + seconds);
        if (temp == 0)
        {
            temp = std::numeric_limits<unsigned>::max();
        }
        return temp;
    }
};
}
}

#endif // ISO_8601_DURATION_PARSER_HPP


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

	This example program shows how to find frontal human faces in an image and
	estimate their pose.  The pose takes the form of 68 landmarks.  These are
	points on the face such as the corners of the mouth, along the eyebrows, on
	the eyes, and so forth.

	This example is essentially just a version of the face_landmark_detection_ex.cpp
	example modified to use OpenCV's VideoCapture object to read from a camera instead
	of files.

	Finally, note that the face detector is fastest when compiled with at least
	SSE2 instructions enabled.  So if you are using a PC with an Intel or AMD
	chip then you should enable at least SSE2 instructions.  If you are using
	cmake to compile this program you can enable them by using one of the
	following commands when you create the build project:
		cmake path_to_dlib_root/examples -DUSE_SSE2_INSTRUCTIONS=ON
		cmake path_to_dlib_root/examples -DUSE_SSE4_INSTRUCTIONS=ON
		cmake path_to_dlib_root/examples -DUSE_AVX_INSTRUCTIONS=ON
	This will set the appropriate compiler options for GCC, clang, Visual
	Studio, or the Intel compiler.  If you are using another compiler then you
	need to consult your compiler's manual to determine how to enable these
	instructions.  Note that AVX is the fastest but requires a CPU from at least
	2011.  SSE4 is the next fastest and is supported by most current machines.
*/

#include <dlib/opencv.h>
#include <opencv2/highgui/highgui.hpp>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <detection.h>
#include <cmath>

using namespace dlib;
using namespace nlohmann;

FacialLandmarkDetector* FacialLandmarkDetector::facialLandmarkDetector = nullptr;

/*
Helper funtions
From https://github.com/HTTdesu/SimpleFacerig
*/
double meter(double A, double B, double C, double x, double y)
{
	double diversion = A * x + B * y + C;
	return diversion / sqrt(pow(A, 2) + pow(B, 2));
}

double eyebrow_move(dlib::point& p1, dlib::point& p2, double slope, double last, double rate)
{
	double bias = p1.y() - slope * p1.x();
	double distance = sqrt(pow(p1.x() - p2.x(), 2) + pow(p1.y() - p2.y(), 2));
	double diversion = meter(slope, -1, bias, p2.x(), p2.y());
	double result = (diversion / distance - 0.45) * 6;
	result = last * (1 - rate) + result * rate;
	return result;
}

double eye_open(dlib::point& p1, dlib::point& p2, dlib::point& v11, dlib::point& v12, dlib::point& v21, dlib::point& v22, double last, double rate)
{
	double distance = sqrt(pow(p1.x() - p2.x(), 2) + pow(p1.y() - p2.y(), 2));
	double slope = (double)(p2.y() - p1.y()) / (double)(p2.x() - p1.x());
	double bias = p2.y() - slope * p2.x();
	double d1 = abs(meter(slope, -1, bias, v11.x(), v11.y()));
	double d2 = abs(meter(slope, -1, bias, v12.x(), v12.y()));
	double diversion = d1 > d2 ? d1 : d2;
	d1 = abs(meter(slope, -1, bias, v21.x(), v21.y()));
	d2 = abs(meter(slope, -1, bias, v22.x(), v22.y()));
	diversion += d1 > d2 ? d1 : d2;
	double ratio = (diversion / distance - 0.18) * 8;
	ratio = ((int)(ratio * 10.0)) / 10.0;
	ratio = last * (1 - rate) + ratio * rate;
	return ratio;
}
/*
Helper funtions end
*/

/**
 * @description: Main detection loop
 * Interpetation of facial landmark point from https://github.com/HTTdesu/SimpleFacerig
 * Modified
 */
void FacialLandmarkDetector::detection()
{
	dlib::frontal_face_detector detector;//facial regonition
	dlib::shape_predictor pose_model;//landmark detection
	cv::CascadeClassifier haarcascade_eye_tree_eyeglasses;//Iris detection
	// live2d parameters
	float x_rotate = 0.0f;
	float y_rotate = 0.0f;
	float z_rotate = 0.0f;

	float left_eye = 1.0f;
	float right_eye = 1.0f;

	float eyebrow_left = 0.0f;
	float eyebrow_right = 0.0f;

	float eyeBallX = 0.0f;
	float eyeBallY = 0.0f;

	float mouth_open = 0.0f;
	//Eyes positions
	int leftEyeMaxX = 0;
	int leftEyeMinX = 0;
	int leftEyeMinY = 0;
	int leftEyeMaxY = 0;
	int rightEyeMaxX = 0;
	int rightEyeMinX = 0;
	int rightEyeMinY = 0;
	int rightEyeMaxY = 0;
	try
	{
		cv::VideoCapture camera(0);
		if (!camera.isOpened())
		{
			std::cerr << "Unable to connect to camera" << std::endl;
			return;
		}

		//image_window win;

		// Load face detection and pose estimation models.
		frontal_face_detector detector = get_frontal_face_detector();
		shape_predictor pose_model;
		haarcascade_eye_tree_eyeglasses.load("..\\..\\res\\haarcascade_eye_tree_eyeglasses.xml");//OpenCV Model
		deserialize("..\\..\\res\\shape_predictor_68_face_landmarks.dat") >> pose_model; //dlib model

		// Grab and process frames until the main window is closed by the user.
		while (camera.isOpened())
		{
			cv::Mat buff, frame;
			camera.read(frame);
			//cv::GaussianBlur(temp, buff, cv::Size(5, 5), 5, 5);
			cv::flip(frame, frame, 1);

//The result of following Process is process facial landmark points and get positions
//Copy from https://github.com/HTTdesu/SimpleFacerig. Authorized by author
			dlib::cv_image<dlib::bgr_pixel> cimg(frame);
			std::vector<dlib::rectangle> faces = detector(cimg);//get faces
			std::vector<dlib::full_object_detection> shapes;

			for (unsigned long i = 0; i < faces.size(); ++i)
				shapes.push_back(pose_model(cimg, faces[i]));

			if (!shapes.empty()) {
				for (int i = 0; i < 68; i++) {
					cv::circle(frame, cvPoint(shapes[0].part(i).x(), shapes[0].part(i).y()), 2, cv::Scalar(0, 0, 255), -1);
				}

				int div_x = shapes[0].part(16).x() - shapes[0].part(0).x();
				int div_y = shapes[0].part(16).y() - shapes[0].part(0).y();
				double center_x = shapes[0].part(0).x() + div_x / 2.0;
				double center_y = shapes[0].part(0).y() + div_y / 2.0;
				double slope = (double)(div_y) / (double)(div_x);
				double bias = center_y - slope * center_x;
				double x_proj = (slope * (shapes[0].part(30).y() - bias) + shapes[0].part(30).x()) / (1 + pow(slope, 2));
				double y_proj = slope * x_proj + bias;
				double diversion = sqrt(pow(x_proj - shapes[0].part(0).x(), 2) + pow(y_proj - shapes[0].part(0).y(), 2));
				double distance = sqrt(pow(shapes[0].part(16).x() - shapes[0].part(0).x(), 2) + pow(shapes[0].part(16).y() - shapes[0].part(0).y(), 2));

				double rate = 0.5;
				// Ax+By+C/sqrt(A^2+B^2)
				x_rotate = x_rotate * (1 - rate) + asin(diversion / distance - 0.5) * 3.14 * 40.0 * rate;

				// nose to eye around 1/6 head
				diversion = meter(slope, -1, bias, shapes[0].part(30).x(), shapes[0].part(30).y());
				diversion = diversion + 1.0 / 6 * distance;
				y_rotate = y_rotate * (1 - rate) + asin(diversion / distance) * 3.14 * 40.0 * rate;

				z_rotate = z_rotate * (1 - rate) + atan(slope) * 3.14 * 40 * rate;

				// eye
				left_eye = eye_open(shapes[0].part(36), shapes[0].part(39), shapes[0].part(37), shapes[0].part(38), shapes[0].part(40), shapes[0].part(41), left_eye, rate);
				right_eye = eye_open(shapes[0].part(42), shapes[0].part(45), shapes[0].part(43), shapes[0].part(44), shapes[0].part(46), shapes[0].part(47), right_eye, rate);

				// eyebrow
				eyebrow_left = eyebrow_move(shapes[0].part(17), shapes[0].part(19), slope, eyebrow_left, rate);
				eyebrow_right = eyebrow_move(shapes[0].part(26), shapes[0].part(24), slope, eyebrow_right, rate);

				// mouth
				diversion = sqrt(pow(shapes[0].part(62).x() - shapes[0].part(66).x(), 2) + pow(shapes[0].part(62).y() - shapes[0].part(66).y(), 2));
				distance = sqrt(pow(shapes[0].part(60).x() - shapes[0].part(64).x(), 2) + pow(shapes[0].part(60).y() - shapes[0].part(64).y(), 2));
				mouth_open = (diversion / distance - 0.15) * 2;

//process facial landmark points end

				dlib::full_object_detection& firstFace = shapes[0];//only proces one face, don't support multiple faces
				// Left eye range
				{

					int minX = INT_MAX, maxX = 0, minY = INT_MAX, maxY = 0;
					for (unsigned long i = 37; i <= 41; ++i) {
						minX = minX < firstFace.part(i).x() ? minX : firstFace.part(i).x();
						maxX = maxX > firstFace.part(i).x() ? maxX : firstFace.part(i).x();
						minY = minY < firstFace.part(i).y() ? minY : firstFace.part(i).y();
						maxY = maxY > firstFace.part(i).y() ? maxY : firstFace.part(i).y();
					}
					leftEyeMaxX = maxX;
					leftEyeMinX = minX;
					leftEyeMinY = minY;
					leftEyeMaxY = maxY;

				}

				// Right eye range
				{
					int minX = INT_MAX, maxX = 0, minY = INT_MAX, maxY = 0;
					for (unsigned long i = 43; i <= 47; ++i) {
						minX = minX < firstFace.part(i).x() ? minX : firstFace.part(i).x();
						maxX = maxX > firstFace.part(i).x() ? maxX : firstFace.part(i).x();
						minY = minY < firstFace.part(i).y() ? minY : firstFace.part(i).y();
						maxY = maxY > firstFace.part(i).y() ? maxY : firstFace.part(i).y();
					}
					rightEyeMaxX = maxX;
					rightEyeMinX = minX;
					rightEyeMinY = minY;
					rightEyeMaxY = maxY;
				}

				try {
					std::vector<cv::Rect> eyes;
					cv::Mat eyesRaw;
					camera.read(eyesRaw);
					cv::flip(eyesRaw, eyesRaw, 1);
					cv::cvtColor(eyesRaw, eyesRaw, cv::COLOR_BGR2GRAY);//灰度
					equalizeHist(eyesRaw, eyesRaw);//直方图均化
					haarcascade_eye_tree_eyeglasses.detectMultiScale(eyesRaw, eyes,
						1.1, 2, 0
						//|CV_HAAR_FIND_BIGGEST_OBJECT
						//|CV_HAAR_DO_ROUGH_SEARCH
						| cv::CASCADE_SCALE_IMAGE
						,
						cv::Size(30, 30));

					for (std::vector<cv::Rect>::const_iterator r = eyes.begin(); r != eyes.end(); r++)
					{
						cv::Point center;
						int radius;
						//center可以作为瞳孔的坐标
						center.x = cvRound(r->x + r->width * 0.5);
						center.y = cvRound(r->y + r->height * 0.5);
						//radius = (int)(cvRound(r->width + r->height)*0.25);
						radius = 2;
						//cv::imshow("eyesRaw", eyesRaw);
						int margin = 10;
						//Calaulate relative position of iris
						if (center.x < rightEyeMaxX + margin && center.x > rightEyeMinX - margin && center.y < rightEyeMaxY + margin && center.y > rightEyeMinY - margin) {
							eyeBallX = (center.x - ((rightEyeMaxX - rightEyeMinX) / 2.0f + rightEyeMinX)) / (rightEyeMaxX - rightEyeMinX) * 2;
							eyeBallY = (center.y - ((rightEyeMaxY - rightEyeMinY) / 2.0f + rightEyeMinY)) / (rightEyeMaxY - rightEyeMinY) * 2;
							cv::circle(frame, center, radius, cv::Scalar(255, 0, 0), 3, 8, 0);

						}
						else if (center.x < leftEyeMaxX + margin && center.x > leftEyeMinX - margin && center.y < leftEyeMaxY + margin && center.y > leftEyeMinY - margin) {
							eyeBallX = (center.x - ((leftEyeMaxX - leftEyeMinX) / 2.0f + leftEyeMinX)) / (leftEyeMaxX - leftEyeMinX) * 2;
							eyeBallY = (center.y - ((leftEyeMaxY - leftEyeMinY) / 2.0f + leftEyeMinY)) / (leftEyeMaxY - leftEyeMinY) * 2;
							cv::circle(frame, center, radius, cv::Scalar(255, 0, 0), 3, 8, 0);

						}
						else {
							cv::circle(frame, center, radius, cv::Scalar(0, 255, 0), 3, 8, 0);
						}

					}
				}
				catch (std::exception e) {
					qDebug() << e.what();//Usually catch fail to load model
				}

				//Idea from
				//版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
				//本文链接：https://blog.csdn.net/computerme/article/details/38142125

				//Finalize and put into Json
				{
					json data;
					data["head"]["ParamAngleZ"] = z_rotate;
					data["head"]["ParamAngleX"] = x_rotate;
					data["head"]["ParamAngleY"] = y_rotate;
					data["eye"]["ParamEyeLOpen"] = left_eye;
					data["eye"]["ParamEyeROpen"] = right_eye;
					data["eye"]["ParamEyeBallX"] = eyeBallX;
					data["eye"]["ParamEyeBallY"] = eyeBallY;
					data["brow"]["ParamBrowLY"] = eyebrow_left;
					data["brow"]["ParamBrowRY"] = eyebrow_right;
					data["ParamMouthOpenY"] = mouth_open;

					Network_QT::getInstance()->updateMotion(data);

					emit NewDetection();
				}

			}
			else
			{
				x_rotate = 0.0f;
				y_rotate = 0.0f;
				z_rotate = 0.0f;
				left_eye = 1.0f;
				right_eye = 1.0f;
				eyebrow_left = 0.0f;
				eyebrow_right = 0.0f;
				mouth_open = 0.0f;
				eyeBallX = 0.0f;
				eyeBallY = 0.0f;
			}

			//double t = (live2d::UtSystem::getUserTimeMSec() / 1000.0) * 2 * 3.14;
			//live2DModel->setParamFloat("PARAM_ANGLE_Z", (float)(30 * sin(t / 3.0)));

			//cv::waitKey(30);
			//Display it all on the screen
			//
			// Display it all on the screen
			if (debug) {
				system("cls");
				qDebug() << "x_rotate" << x_rotate << "\n"
					<< "y_rotate" << y_rotate << "\n"
					<< "z_rotate" << z_rotate << "\n"
					<< "left_eye" << left_eye << "\n"
					<< "right_eye" << right_eye << "\n"
					<< "eyebrow_left" << eyebrow_left << "\n"
					<< "eyebrow_right" << eyebrow_right << "\n"
					<< "mouth_open" << mouth_open << "\n"
					<< "eyeBallX" << eyeBallX << "\n"
					<< "eyeBallY" << eyeBallY << "\n";
			}
			if (Setting::getSetting()->ShowFR) {
				cv::imshow("Feature points", frame);
				cv::waitKey(30);
			}

		}

	}catch (serialization_error & e)
	{//When fail to load dlib model
		std::cerr << "Fail to load model" << std::endl;
		qDebug().noquote() << "Fail to load model";
		this->ModelLoadError = true;
		return;
	}
	catch (std::exception & e)
	{
		std::cerr << e.what() << std::endl;
		return;
	}
}


#include <ros/ros.h>
#include <fstream>

#include "recon_surface/MeshFromPointCloud2.h"

#include <iostream>
#include "recon_surface/surface_recon.hpp"
#include "recon_surface/pset.hpp"
#include "recon_surface/hole_filling.hpp"
#include "recon_surface/rm_artifacts.hpp"
#include "recon_surface/parser.hpp"
#include "recon_surface/texture.hpp"

#include <dynamic_reconfigure/server.h>
#include <recon_surface/ReconSurfaceConfig.h>

#include <map>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>

#include <iostream>
#include <iomanip>
#include <ctime>
#include <CGAL/Vector_3.h>

#include <CGAL/Kernel_traits.h>
#include <CGAL/boost/graph/properties.h>

#include <boost/cstdint.hpp>
#include <boost/graph/graph_traits.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>
#include <iostream>
#include <chrono>
#include <visualization_msgs/Marker.h>

double gridm = 0.0;
double holemaxsize = 0.0;
string output_folder = "";
double sample = 0.0;
double trim = 0.0;
int texturesize = 0;
int remove_borders_iterations = 0;

ros::Publisher marker_pub_normal;
ros::Publisher marker_pub_inverted;

void publishSTLMarker(ros::Publisher marker_pub, string stl_filepath, string frame_id)
{
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "basic_shapes";
    marker.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = visualization_msgs::Marker::MESH_RESOURCE;
    marker.mesh_resource = "file://" + stl_filepath;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 0.5;

    marker.lifetime = ros::Duration();
    marker.mesh_use_embedded_materials = true;

    marker_pub.publish(marker);
    ROS_INFO_STREAM("Publishing frame_id: " << frame_id << "stl file:" << stl_filepath);
}

void writeSTLfromMesh(Mesh mesh_obj, string output_stl_filepath, bool flip_normal, bool is_binary_format){
    /*
     * Binary STL format
     * https://github.com/CGAL/cgal/blob/master/Polyhedron_IO/include/CGAL/IO/STL_writer.h
     */

    ofstream out(output_stl_filepath);

    if(is_binary_format) {
        // is binary
        string header = "FileType: Binary                                                                ";
        out.write(header.c_str(), 80);

        const boost::uint32_t N32 = static_cast<boost::uint32_t>(faces(mesh_obj).size());
        out.write(reinterpret_cast<const char *>(&N32), sizeof(N32));

        BOOST_FOREACH(Mesh::Face_index
                              face_index, mesh_obj.faces()) {
                        Mesh::Halfedge_index he = mesh_obj.halfedge(face_index);

                        // Get the x y z position of the 3 verfices of every face
                        vertex_descriptor v0 = mesh_obj.target(he);
                        vertex_descriptor v1 = mesh_obj.target(mesh_obj.next(he));
                        vertex_descriptor v2 = mesh_obj.source(he);

                        CPoint3 p = mesh_obj.point(v0);
                        CPoint3 q = mesh_obj.point(v1);
                        CPoint3 r = mesh_obj.point(v2);

                        if (flip_normal) {
                            q = mesh_obj.point(v0);
                            p = mesh_obj.point(v1);
                        }

                        CGAL::Vector_3<CGAL::Simple_cartesian<double>> n = CGAL::Vector_3<CGAL::Simple_cartesian<double>>(
                                1, 0, 0);
                        if (!CGAL::collinear(p, q, r)) {
                            n = CGAL::unit_normal(p, q, r);
                        }

                        const float coords[12] = {
                                static_cast<float>(n.x()), static_cast<float>(n.y()), static_cast<float>(n.z()),
                                static_cast<float>(p.x()), static_cast<float>(p.y()), static_cast<float>(p.z()),
                                static_cast<float>(q.x()), static_cast<float>(q.y()), static_cast<float>(q.z()),
                                static_cast<float>(r.x()), static_cast<float>(r.y()), static_cast<float>(r.z())};

                        for (int i = 0; i < 12; ++i)
                            out.write(reinterpret_cast<const char *>(&coords[i]), sizeof(coords[i]));

                        out << "  ";
                    }
    }
    else{
        // is ASCII
        out << "solid STL generated by MeshLab\n";
        BOOST_FOREACH(Mesh::Face_index
                              face_index, mesh_obj.faces()) {
                        Mesh::Halfedge_index he = mesh_obj.halfedge(face_index);

                        // Get the x y z position of the 3 verfices of every face
                        vertex_descriptor v0 = mesh_obj.target(he);
                        vertex_descriptor v1 = mesh_obj.target(mesh_obj.next(he));
                        vertex_descriptor v2 = mesh_obj.source(he);

                        CPoint3 p = mesh_obj.point(v0);
                        CPoint3 q = mesh_obj.point(v1);
                        CPoint3 r = mesh_obj.point(v2);

                        if (flip_normal) {
                            q = mesh_obj.point(v0);
                            p = mesh_obj.point(v1);
                        }

                        CGAL::Vector_3<CGAL::Simple_cartesian<double>> n = CGAL::Vector_3<CGAL::Simple_cartesian<double>>(
                                1, 0, 0);
                        if(!CGAL::collinear(p, q, r)){
                            n = CGAL::unit_normal(p, q, r);
                        }

                        out << "facet normal " << n << "\nouter loop\n";
                        out << "vertex " << p << "\n";
                        out << "vertex " << q << "\n";
                        out << "vertex " << r << "\n";
                        out << "endloop\nendfacet\n";
                    }
        out << "endsolid vcg\n";
    }
}

string generate_mesh(const sensor_msgs::PointCloud2 msg, const geometry_msgs::Point src, string output) {
    std::vector<Point> points;
    std::vector<Vector> normals;
    std::vector<Color> colors;
    std::list<PointVectorPair> estimated_pwn;
    std::vector<Point_3> orig_points;
    std::vector<unsigned int> indices;
    auto src_point = Point_3(src.x, src.y, src.z);

    FT average_spacing = 0.;

    auto start_total_time = chrono::high_resolution_clock::now();

    auto start = chrono::high_resolution_clock::now();
    Pset pointset(points, normals, colors, sample); // init my point cloud
    pointset.read_pointCloud2(msg);

    for (size_t i = 0; i < points.size(); i++) //initialize structure for Kdtree
    {
        orig_points.push_back(Point_3(points[i][0], points[i][1], points[i][2]));
        indices.push_back(i);
    }
    auto stop = chrono::high_resolution_clock::now();

    ROS_INFO_STREAM("Time initialize pointcloud: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    // Initialize a spatial kd-tree for further search on original set of points
    start = chrono::high_resolution_clock::now();
    Tree tree(
            boost::make_zip_iterator(boost::make_tuple(orig_points.begin(), indices.begin())),
            boost::make_zip_iterator(boost::make_tuple(orig_points.end(), indices.end()))
    );

    pointset.write_ply(output + "_full_pcd_rgb.ply"); //write full rgb .ply
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to generate full rgb *.ply file: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    start = chrono::high_resolution_clock::now();
    average_spacing = CGAL::compute_average_spacing<CGAL::Sequential_tag>(orig_points.begin(), orig_points.end(), 6);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to compute_average_spacing: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    double as_ratio = gridm * log(points.size() / 10e4);

    if (as_ratio < 0.6)
        as_ratio = 0.6;

    double cell_size = average_spacing * as_ratio;

    ROS_INFO_STREAM("Estimated weight for grid size w.r.t. avg. point distance: " << as_ratio);

    start = chrono::high_resolution_clock::now();
    pointset.sample_points_cgal(cell_size, 1);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to sample_points_cgal: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    ROS_INFO_STREAM(
            "Sampled point set size: " << points.size() << " | Normals: " << normals.size() << " | Original ply size: "
                                       << colors.size());

    start = chrono::high_resolution_clock::now();
    if (normals.size() == 0){
        //ROS_INFO_STREAM("Estimating normals");
        estimate_normals(points, src_point, estimated_pwn);
    }
    else{
        //ROS_INFO_STREAM("Registering normals normals");
        estimated_pwn = register_normals(points, grab_normals(orig_points, normals));
    }

    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to estimate normals: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    ROS_INFO_STREAM("Points sizes:" << points.size() << " Estimated PWN size:" << estimated_pwn.size());

    start = chrono::high_resolution_clock::now();
    write_ply_wnormals(output + "_simplified_pcd_normals.ply", estimated_pwn, tree, colors);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time to write point cloud .ply file: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    start = chrono::high_resolution_clock::now();
    trim_mesh(reconstruct_surface(estimated_pwn, output), tree, trim * (double) average_spacing, output);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time trim mesh: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    start = chrono::high_resolution_clock::now();
    remove_borders(output + "temp2.off", remove_borders_iterations, output);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time remove_bordersh: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

    // TESTING 11/MAI/21
    start = chrono::high_resolution_clock::now();
    fill_hole(output + "temp3.off", holemaxsize * (double) average_spacing, output);
    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time fill holes: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                            << " seconds");

//    start = chrono::high_resolution_clock::now();
//    texture t(tree, colors, output + "_textured.obj", texturesize);
//    t.build_png();
//    t.save_obj();
//    stop = chrono::high_resolution_clock::now();
//    ROS_INFO_STREAM("Time generate *.obj file: "
//                            << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
//                            << " seconds");

//    ROS_INFO_STREAM("Writing STL");

    // TESTING 11/MAI/21
    // it normally is temp3.off
    std::ifstream input_off3(output + "temp3.off"); // normal is "temp3.off" change after testing
    string output_stl_filepath = output + "_mesh.stl";
    string output_stl_filepath_inverted = output + "_inverted_mesh.stl";
    Mesh mesh_obj;

    stop = chrono::high_resolution_clock::now();
    ROS_INFO_STREAM("Time total reconstruction: "
                            << float(chrono::duration_cast<chrono::microseconds>(stop - start_total_time).count() / 1000000.0)
                            << " seconds");

    if (!input_off3 || !(input_off3 >> mesh_obj)) {
        ROS_ERROR_STREAM("Not a valid off file (generate_mesh): " << output_stl_filepath);
        return "";
    } else {
        start = chrono::high_resolution_clock::now();

        writeSTLfromMesh(mesh_obj, output_stl_filepath, false, true);
        publishSTLMarker(marker_pub_normal, output_stl_filepath, msg.header.frame_id);

        writeSTLfromMesh(mesh_obj, output_stl_filepath_inverted, true, true);
        publishSTLMarker(marker_pub_inverted, output_stl_filepath_inverted, msg.header.frame_id);

        ROS_INFO_STREAM("STL files written to:" << output_stl_filepath << " and " << output_stl_filepath_inverted);
        stop = chrono::high_resolution_clock::now();
        ROS_INFO_STREAM("Time generate *.stl file: "
                                << float(chrono::duration_cast<chrono::microseconds>(stop - start).count() / 1000000.0)
                                << " seconds");
    }

    return output_stl_filepath;
}

void dynReconfigureCallback(recon_surface::ReconSurfaceConfig &config, uint32_t level) {
    if (gridm != config.gridm) {
        gridm = config.gridm;
        ROS_INFO("Reconfigure gridm: %lf", gridm);
    }

    if (holemaxsize != config.holemaxsize) {
        holemaxsize = config.holemaxsize;
        ROS_INFO("Reconfigure holemaxsize: %lf", holemaxsize);
    }

    if (output_folder != config.output_folder) {
        output_folder = config.output_folder;
        ROS_INFO("Reconfigure output_folder: %s", output_folder.c_str());
    }

    if (sample != config.sample) {
        sample = config.sample;
        ROS_INFO("Reconfigure sample: %lf", sample);
    }

    if (texturesize != config.texturesize) {
        texturesize = config.texturesize;
        ROS_INFO("Reconfigure texturesize: %lf", texturesize);
    }

    if (trim != config.trim) {
        trim = config.trim;
        ROS_INFO("Reconfigure trim: %lf", trim);
    }

    if (remove_borders_iterations != config.remove_borders_iterations) {
        remove_borders_iterations = config.remove_borders_iterations;
        ROS_INFO("Reconfigure remove_borders_iterations: %d", remove_borders_iterations);
    }
}

bool genMeshFromPointCloudCallback(recon_surface::MeshFromPointCloud2::Request &req,
                                   recon_surface::MeshFromPointCloud2::Response &res) {
    ROS_INFO("genMeshFromPointCloudCallback called");

    try {
        const sensor_msgs::PointCloud2 ptcloud = req.input;
        const geometry_msgs::Point src = req.src;

        time_t rawtime;
        struct tm *timeinfo;
        char buffer[80];

        time(&rawtime);
        timeinfo = localtime(&rawtime);

        strftime(buffer, sizeof(buffer), "%d-%m-%Y_%H:%M:%S", timeinfo);
        std::string str(buffer);

        string output_file = output_folder;
        char endch = output_file.back();
        if ('/' != endch) {
            output_file += "/";
        }
        output_file += buffer;

        ROS_INFO_STREAM("Output file:" << output_file);

        string output_path = generate_mesh(ptcloud, src, output_file);

        res.success = true;
        res.path = output_path;
    } catch (std::runtime_error &e) {
        ROS_ERROR("genMeshFromPointCloudCallback exception: %s", e.what());
        res.success = false;
        return false;
    }

    return true;
}

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "recon_surface_pointcloud_service_node");
    ros::NodeHandle nh;
    ros::NodeHandle nh_priv("~");

    ROS_INFO("Start recon_surface_service node");

    std::string version(std::to_string(CGAL_VERSION_NR));
    ROS_INFO_STREAM("Using CGAL lib version " << version.substr(1, 2) << "." << version.substr(3, 2));

    // dynamic reconfigure server
    dynamic_reconfigure::Server<recon_surface::ReconSurfaceConfig> server;
    dynamic_reconfigure::Server<recon_surface::ReconSurfaceConfig>::CallbackType f;

    f = boost::bind(&dynReconfigureCallback, _1, _2);
    server.setCallback(f);

    // publishers
    ros::ServiceServer service = nh.advertiseService("/mesh_from_pointclouds", genMeshFromPointCloudCallback);
    marker_pub_normal = nh.advertise<visualization_msgs::Marker>("/reconstructed_mesh_marker_normal", 1, true);
    marker_pub_inverted = nh.advertise<visualization_msgs::Marker>("/reconstructed_mesh_marker_inverted", 1, true);

    ROS_INFO("Spinning recon_surface_service node");

    ros::spin();

    ROS_INFO("recon_surface_service node stopped");

    return 0;
}


// Boost.Geometry

// Copyright (c) 2019, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include "common.hpp"

#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/expand.hpp>

int test_main(int, char*[])
{
    geom g;

    bg::expand(g.b, g.pt, bg::strategy::expand::cartesian_point());
    bg::expand(g.b, g.pt, bg::strategy::expand::spherical_point());
    bg::expand(g.b, g.b, bg::strategy::expand::cartesian_box());
    bg::expand(g.b, g.b, bg::strategy::expand::spherical_box());
    bg::expand(g.b, g.s, bg::strategy::expand::cartesian_segment());
    bg::expand(g.b, g.s, bg::strategy::expand::spherical_segment<>());
    bg::expand(g.b, g.s, bg::strategy::expand::geographic_segment<>());

    bg::envelope(g.pt, g.b, bg::strategy::envelope::cartesian_point());
    bg::envelope(g.pt, g.b, bg::strategy::envelope::spherical_point());
    bg::envelope(g.b, g.b, bg::strategy::envelope::cartesian_box());
    bg::envelope(g.b, g.b, bg::strategy::envelope::spherical_box());
    bg::envelope(g.s, g.b, bg::strategy::envelope::cartesian_segment<>());
    bg::envelope(g.s, g.b, bg::strategy::envelope::spherical_segment<>());
    bg::envelope(g.s, g.b, bg::strategy::envelope::geographic_segment<>());
    bg::envelope(g.ls, g.b, bg::strategy::envelope::cartesian<>());
    bg::envelope(g.ls, g.b, bg::strategy::envelope::spherical<>());
    bg::envelope(g.ls, g.b, bg::strategy::envelope::geographic<>());
    bg::envelope(g.r, g.b, bg::strategy::envelope::cartesian<>());
    bg::envelope(g.r, g.b, bg::strategy::envelope::spherical<>());
    bg::envelope(g.r, g.b, bg::strategy::envelope::geographic<>());
    bg::envelope(g.po, g.b, bg::strategy::envelope::cartesian<>());
    bg::envelope(g.po, g.b, bg::strategy::envelope::spherical<>());
    bg::envelope(g.po, g.b, bg::strategy::envelope::geographic<>());
    bg::envelope(g.mls, g.b, bg::strategy::envelope::cartesian<>());
    bg::envelope(g.mls, g.b, bg::strategy::envelope::spherical<>());
    bg::envelope(g.mls, g.b, bg::strategy::envelope::geographic<>());
    bg::envelope(g.mpo, g.b, bg::strategy::envelope::cartesian<>());
    bg::envelope(g.mpo, g.b, bg::strategy::envelope::spherical<>());
    bg::envelope(g.mpo, g.b, bg::strategy::envelope::geographic<>());
    bg::envelope(g.mpt, g.b, bg::strategy::envelope::cartesian_multipoint());
    bg::envelope(g.mpt, g.b, bg::strategy::envelope::spherical_multipoint());

    return 0;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/arch/common/detail/scalar/expo_base.hpp>

#endif