<commit_before>#pragma once
#include <map>

#include <Core/Containers/VectorArray.hpp>
#include <Core/RaCore.hpp>
#include <Core/Utils/Index.hpp>
#include <Core/Utils/Observable.hpp>

namespace Ra {
namespace Core {

namespace Geometry {
// need forward declarations for friend classes outside of Utils namespace
class TopologicalMesh;
class TriangleMesh;
} // namespace Geometry

namespace Utils {

template <typename T>
class Attrib;

/**
 * AttribBase is the base class for attributes of all type.
 */
class RA_CORE_API AttribBase : public ObservableVoid
{
  public:
    inline explicit AttribBase( const std::string& name );
    virtual ~AttribBase();
    AttribBase( const AttribBase& ) = delete;
    AttribBase& operator=( const AttribBase& ) = delete;

    /// Return the attribute's name.
    inline std::string getName() const;

    ///    Set the attribute's name.
    inline void setName( const std::string& name );

    /// Resize the attribute's array.
    virtual void resize( size_t s ) = 0;

    /// Return the number of elements in the attribute array
    virtual size_t getSize() const = 0;

    /// Return the number of components of one element (i.e. for
    /// glVertexAttribPointer size value)
    /// \see https://www.khronos.org/registry/OpenGL-Refpages/gl4/html/glVertexAttribPointer.xhtml
    /// \todo rename getNumberOfComponent ?
    virtual size_t getElementSize() const = 0;

    /// return the size in byte of the container
    virtual size_t getBufferSize() const = 0;

    /// Return the stride, in bytes, from one attribute address to the next one.
    /// This is use for glVertexAttribPointer.
    /// \warning it's meaningful only if the attrib do not contain heap
    /// allocated data. Such attrib could not be easily sent to the GPU, and
    /// getStride is meaningless.
    virtual int getStride() const = 0;

    /// Return true if *this and \p rhs have the same name.
    bool inline operator==( const AttribBase& rhs );

    /// Downcast from AttribBase to Attrib<T>.
    template <typename T>
    inline Attrib<T>& cast();

    /// Downcast from AttribBase to Attrib<T>.
    template <typename T>
    inline const Attrib<T>& cast() const;

    /// Return true if the attribute content is of float type, false otherwise.
    virtual bool isFloat() const = 0;

    /// Return true if the attribute content is of Vector2 type, false otherwise.
    virtual bool isVector2() const = 0;

    /// Return true if the attribute content is of Vector3 type, false otherwise.
    virtual bool isVector3() const = 0;

    /// Return true if the attribute content is of Vector4 type, false otherwise.
    virtual bool isVector4() const = 0;

    /// Return a void * on the attrib data
    virtual const void* dataPtr() const = 0;

    /// Return true if data is locked, i.e. has been locked for write access with
    /// getDataWithlock() (defined in subclass Attrib). Double lock is prohebited, so when finished,
    /// call unlock();
    bool inline isLocked() const;

    /// Unlock data so another one can gain write access.
    void inline unlock();

  protected:
    void inline lock( bool isLocked = true );

  private:
    /// The attribute's name.
    std::string m_name;

    /// Is data access locked by a user ?
    bool m_isLocked {false};
};

/**
 * An Attrib stores an element of type \p T for each entry.
 */
template <typename T>
class Attrib : public AttribBase
{
  public:
    using value_type = T;
    using Container  = VectorArray<T>;

    explicit Attrib( const std::string& name );
    virtual ~Attrib();

    /// Resize the container (value_type must have a default ctor).
    void resize( size_t s ) override;

    /// Read-write access to the attribute content.
    /// lock the content, when done call unlock()
    inline Container& getDataWithLock();

    const void* dataPtr() const override;

    ///@{
    /// setAttribData, attrib mustn't be locked (it's asserted).
    void setData( const Container& data );
    void setData( Container&& data );
    ///@}

    /// Read-only acccess to the attribute content.
    inline const Container& data() const;

    size_t getSize() const override;

    size_t getElementSize() const override;
    int getStride() const override;
    size_t getBufferSize() const override;
    bool isFloat() const override;
    bool isVector2() const override;
    bool isVector3() const override;
    bool isVector4() const override;

    /// check if attrib is a given type, as in attr.isType<MyMatrix>()
    template <typename U>
    bool isType();

  private:
    Container m_data;
};

/// An attrib handle basically store an Index and a name.
template <typename T>
class AttribHandle
{
  public:
    using value_type = T;
    using AttribType = Attrib<T>;
    using Container  = typename Attrib<T>::Container;
    /// compare two handle, there are the same if they both represent the same
    /// attrib (type and value).
    template <typename U>
    bool operator==( const AttribHandle<U>& lhs ) const {
        return std::is_same<T, U>::value && m_idx == lhs.m_idx;
    }

    /// return the index of the attrib.
    Index idx() const { return m_idx; }

    /// return the name of the attrib.
    /// attrib name are unique in a given attribManager.
    std::string attribName() const { return m_name; }

  private:
    Index m_idx        = Index::Invalid();
    std::string m_name = "";

    friend class AttribManager;
};

/*!
 * \brief The AttribManager provides attributes management by handles.
 *
 * The AttribManager stores a container of AttribBase *, which can
 * be accessed (#getAttrib) and deleted (#removeAttrib) using a AttribHandle.
 * Handles are created from an attribute name using #addAttrib, and retrieved
 * using #findAttrib.
 *
 * Example of typical use case:
 * \code
 * // somewhere: creation
 * AttribManager mng;
 * auto inputfattrib = mng.addAttrib<float>("MyAttrib");
 *
 * ...
 *
 * // somewhere else: access
 * auto iattribhandler = mng.findAttrib<float>("MyAttrib"); //  iattribhandler == inputfattrib
 * if ( mng.isValid( iattribhandler ) ) { // true
 *     auto &attrib = mng.getAttrib( iattribhandler );
 *     ...
 * }
 * auto& iattribhandler = mng.findAttrib<float>("InvalidAttrib"); // invalid
 * if ( mng.isValid( iattribhandler ) ) { // false
 *    ...
 * }
 * \endcode
 *
 * \warning There is no error check on the handles attribute type.
 *
 */
class RA_CORE_API AttribManager : public Observable<const std::string&>
{
  public:
    using value_type         = AttribBase;
    using pointer_type       = value_type*;
    using smart_pointer_type = std::unique_ptr<value_type>;
    using Container          = std::vector<smart_pointer_type>;

    inline AttribManager();

    /// Copy constructor and assignment operator are forbidden.
    AttribManager( const AttribManager& m ) = delete;
    AttribManager& operator=( const AttribManager& m ) = delete;

    inline AttribManager( AttribManager&& m );

    inline AttribManager& operator=( AttribManager&& m );
    ~AttribManager() override;

    /// Base copy, does nothing.
    inline void copyAttributes( const AttribManager& m );

    /// Copy the given attributes from \p m.
    /// \note If some attrib already exists, it will be replaced.
    /// \note Invalid handles are ignored.
    template <class T, class... Handle>
    void copyAttributes( const AttribManager& m, const AttribHandle<T>& attr, Handle... attribs );

    /// Copy all attributes from \p m.
    /// \note If some attrib already exists, it will be replaced.
    void copyAllAttributes( const AttribManager& m );

    /// clear all attribs, invalidate handles.
    void clear();

    /// Return true if \p h correspond to an existing attribute in *this.
    template <typename T>
    bool isValid( const AttribHandle<T>& h ) const;

    /*!
     * \brief contains Check if an attribute with the given \p name exists.
     * \param name Name of the attribute.
     * \warning There is no error check on the attribute type.
     * \complexity \f$ O(\log(n)) \f$
     */
    inline bool contains( const std::string& name ) const;

    /*!
     * \brief findAttrib Grab an attribute handler by \p name.
     * \param name Name of the attribute.
     * \return Attribute handler if found, an invalid handler otherwise.
     * \warning There is no error check on the attribute type.
     * \complexity \f$ O(\log(n)) \f$
     */
    template <typename T>
    inline AttribHandle<T> findAttrib( const std::string& name ) const;

    ///@{
    /// Get attribute by handle.
    /// \complexity \f$ O(1) \f$
    /// \warning There is no check on the handle validity.
    template <typename T>
    inline Attrib<T>& getAttrib( const AttribHandle<T>& h );

    template <typename T>
    inline Attrib<T>* getAttribPtr( const AttribHandle<T>& h );

    /// Get attribute by handle (const).
    /// \complexity \f$ O(1) \f$
    /// \warning There is no check on the handle validity.
    template <typename T>
    inline const Attrib<T>& getAttrib( const AttribHandle<T>& h ) const;

    /// Return a AttribBase ptr to the attrib identified by name.
    /// to give access to AttribBase method, regardless of the type of element
    /// stored in the attrib.
    inline AttribBase* getAttribBase( const std::string& name );

    /// \see getAttribBase( const std::string& name );
    inline AttribBase* getAttribBase( const Index& idx );
    ///@}

    ///@{
    /// Set the data of the attrib h.
    /// \warning \p h has to be a valid attrib handle, this is not checked.
    template <typename T>
    inline void setAttrib( const AttribHandle<T>& h,
                           const typename AttribHandle<T>::Container& data );

    /// \see #setAttrib
    template <typename T>
    inline void setAttrib( const AttribHandle<T>& h, typename AttribHandle<T>::Container&& data );
    ///@}

    /// Add attribute by name.
    /// notify(name) is called to trigger the observers
    /// \note If an attribute with the same name already exists,
    ///       just returns a AttribHandle to it.
    /// \complexity \f$ O(n) \f$
    template <typename T>
    AttribHandle<T> addAttrib( const std::string& name );

    /// Remove attribute by handle, invalidates the handle.
    /// notify(name) is called to trigger the observers
    /// \warning If a new attribute is added, old invalidated handles may lead to
    ///          the new attribute.
    /// \complexity \f$ O(\log(n)) \f$
    template <typename T>
    void removeAttrib( AttribHandle<T>& h );
    /// Return true if *this and \p other have the same attributes, same amount
    /// and same names.
    /// \warning There is no check on the attribute type nor data.
    bool hasSameAttribs( const AttribManager& other );

    ///@{
    /// Perform \p fun on each attribute.
    // This is needed by the user to avoid caring about removed attributes (nullptr)
    // \todo reimplement as range for
    template <typename F>
    void for_each_attrib( const F& func ) const;

    // \todo keep non const version private
    template <typename F>
    void for_each_attrib( const F& func );
    ///@}

    /// Return the number of attributes
    inline int getNumAttribs() const;

  private:
    /// Attrib list, better using attribs() to go through.
    Container m_attribs;

    // Map between the attrib's name and its index, used to speedup finding the
    // handle index from the attribute name.
    std::map<std::string, Index> m_attribsIndex;

    // Ease wrapper
    friend class ::Ra::Core::Geometry::TopologicalMesh;
    friend class ::Ra::Core::Geometry::TriangleMesh;

    /// Count number of valid attribs
    int m_numAttribs {0};
};

} // namespace Utils
} // namespace Core
} // namespace Ra

#include <Core/Utils/Attribs.inl>
<commit_msg>[Core] Remove unused friend class<commit_after>#pragma once
#include <map>

#include <Core/Containers/VectorArray.hpp>
#include <Core/RaCore.hpp>
#include <Core/Utils/Index.hpp>
#include <Core/Utils/Observable.hpp>

namespace Ra {
namespace Core {

namespace Geometry {
// need forward declarations for friend classes outside of Utils namespace
class TopologicalMesh;
} // namespace Geometry

namespace Utils {

template <typename T>
class Attrib;

/**
 * AttribBase is the base class for attributes of all type.
 */
class RA_CORE_API AttribBase : public ObservableVoid
{
  public:
    inline explicit AttribBase( const std::string& name );
    virtual ~AttribBase();
    AttribBase( const AttribBase& ) = delete;
    AttribBase& operator=( const AttribBase& ) = delete;

    /// Return the attribute's name.
    inline std::string getName() const;

    ///    Set the attribute's name.
    inline void setName( const std::string& name );

    /// Resize the attribute's array.
    virtual void resize( size_t s ) = 0;

    /// Return the number of elements in the attribute array
    virtual size_t getSize() const = 0;

    /// Return the number of components of one element (i.e. for
    /// glVertexAttribPointer size value)
    /// \see https://www.khronos.org/registry/OpenGL-Refpages/gl4/html/glVertexAttribPointer.xhtml
    /// \todo rename getNumberOfComponent ?
    virtual size_t getElementSize() const = 0;

    /// return the size in byte of the container
    virtual size_t getBufferSize() const = 0;

    /// Return the stride, in bytes, from one attribute address to the next one.
    /// This is use for glVertexAttribPointer.
    /// \warning it's meaningful only if the attrib do not contain heap
    /// allocated data. Such attrib could not be easily sent to the GPU, and
    /// getStride is meaningless.
    virtual int getStride() const = 0;

    /// Return true if *this and \p rhs have the same name.
    bool inline operator==( const AttribBase& rhs );

    /// Downcast from AttribBase to Attrib<T>.
    template <typename T>
    inline Attrib<T>& cast();

    /// Downcast from AttribBase to Attrib<T>.
    template <typename T>
    inline const Attrib<T>& cast() const;

    /// Return true if the attribute content is of float type, false otherwise.
    virtual bool isFloat() const = 0;

    /// Return true if the attribute content is of Vector2 type, false otherwise.
    virtual bool isVector2() const = 0;

    /// Return true if the attribute content is of Vector3 type, false otherwise.
    virtual bool isVector3() const = 0;

    /// Return true if the attribute content is of Vector4 type, false otherwise.
    virtual bool isVector4() const = 0;

    /// Return a void * on the attrib data
    virtual const void* dataPtr() const = 0;

    /// Return true if data is locked, i.e. has been locked for write access with
    /// getDataWithlock() (defined in subclass Attrib). Double lock is prohebited, so when finished,
    /// call unlock();
    bool inline isLocked() const;

    /// Unlock data so another one can gain write access.
    void inline unlock();

  protected:
    void inline lock( bool isLocked = true );

  private:
    /// The attribute's name.
    std::string m_name;

    /// Is data access locked by a user ?
    bool m_isLocked {false};
};

/**
 * An Attrib stores an element of type \p T for each entry.
 */
template <typename T>
class Attrib : public AttribBase
{
  public:
    using value_type = T;
    using Container  = VectorArray<T>;

    explicit Attrib( const std::string& name );
    virtual ~Attrib();

    /// Resize the container (value_type must have a default ctor).
    void resize( size_t s ) override;

    /// Read-write access to the attribute content.
    /// lock the content, when done call unlock()
    inline Container& getDataWithLock();

    const void* dataPtr() const override;

    ///@{
    /// setAttribData, attrib mustn't be locked (it's asserted).
    void setData( const Container& data );
    void setData( Container&& data );
    ///@}

    /// Read-only acccess to the attribute content.
    inline const Container& data() const;

    size_t getSize() const override;

    size_t getElementSize() const override;
    int getStride() const override;
    size_t getBufferSize() const override;
    bool isFloat() const override;
    bool isVector2() const override;
    bool isVector3() const override;
    bool isVector4() const override;

    /// check if attrib is a given type, as in attr.isType<MyMatrix>()
    template <typename U>
    bool isType();

  private:
    Container m_data;
};

/// An attrib handle basically store an Index and a name.
template <typename T>
class AttribHandle
{
  public:
    using value_type = T;
    using AttribType = Attrib<T>;
    using Container  = typename Attrib<T>::Container;
    /// compare two handle, there are the same if they both represent the same
    /// attrib (type and value).
    template <typename U>
    bool operator==( const AttribHandle<U>& lhs ) const {
        return std::is_same<T, U>::value && m_idx == lhs.m_idx;
    }

    /// return the index of the attrib.
    Index idx() const { return m_idx; }

    /// return the name of the attrib.
    /// attrib name are unique in a given attribManager.
    std::string attribName() const { return m_name; }

  private:
    Index m_idx        = Index::Invalid();
    std::string m_name = "";

    friend class AttribManager;
};

/*!
 * \brief The AttribManager provides attributes management by handles.
 *
 * The AttribManager stores a container of AttribBase *, which can
 * be accessed (#getAttrib) and deleted (#removeAttrib) using a AttribHandle.
 * Handles are created from an attribute name using #addAttrib, and retrieved
 * using #findAttrib.
 *
 * Example of typical use case:
 * \code
 * // somewhere: creation
 * AttribManager mng;
 * auto inputfattrib = mng.addAttrib<float>("MyAttrib");
 *
 * ...
 *
 * // somewhere else: access
 * auto iattribhandler = mng.findAttrib<float>("MyAttrib"); //  iattribhandler == inputfattrib
 * if ( mng.isValid( iattribhandler ) ) { // true
 *     auto &attrib = mng.getAttrib( iattribhandler );
 *     ...
 * }
 * auto& iattribhandler = mng.findAttrib<float>("InvalidAttrib"); // invalid
 * if ( mng.isValid( iattribhandler ) ) { // false
 *    ...
 * }
 * \endcode
 *
 * \warning There is no error check on the handles attribute type.
 *
 */
class RA_CORE_API AttribManager : public Observable<const std::string&>
{
  public:
    using value_type         = AttribBase;
    using pointer_type       = value_type*;
    using smart_pointer_type = std::unique_ptr<value_type>;
    using Container          = std::vector<smart_pointer_type>;

    inline AttribManager();

    /// Copy constructor and assignment operator are forbidden.
    AttribManager( const AttribManager& m ) = delete;
    AttribManager& operator=( const AttribManager& m ) = delete;

    inline AttribManager( AttribManager&& m );

    inline AttribManager& operator=( AttribManager&& m );
    ~AttribManager() override;

    /// Base copy, does nothing.
    inline void copyAttributes( const AttribManager& m );

    /// Copy the given attributes from \p m.
    /// \note If some attrib already exists, it will be replaced.
    /// \note Invalid handles are ignored.
    template <class T, class... Handle>
    void copyAttributes( const AttribManager& m, const AttribHandle<T>& attr, Handle... attribs );

    /// Copy all attributes from \p m.
    /// \note If some attrib already exists, it will be replaced.
    void copyAllAttributes( const AttribManager& m );

    /// clear all attribs, invalidate handles.
    void clear();

    /// Return true if \p h correspond to an existing attribute in *this.
    template <typename T>
    bool isValid( const AttribHandle<T>& h ) const;

    /*!
     * \brief contains Check if an attribute with the given \p name exists.
     * \param name Name of the attribute.
     * \warning There is no error check on the attribute type.
     * \complexity \f$ O(\log(n)) \f$
     */
    inline bool contains( const std::string& name ) const;

    /*!
     * \brief findAttrib Grab an attribute handler by \p name.
     * \param name Name of the attribute.
     * \return Attribute handler if found, an invalid handler otherwise.
     * \warning There is no error check on the attribute type.
     * \complexity \f$ O(\log(n)) \f$
     */
    template <typename T>
    inline AttribHandle<T> findAttrib( const std::string& name ) const;

    ///@{
    /// Get attribute by handle.
    /// \complexity \f$ O(1) \f$
    /// \warning There is no check on the handle validity.
    template <typename T>
    inline Attrib<T>& getAttrib( const AttribHandle<T>& h );

    template <typename T>
    inline Attrib<T>* getAttribPtr( const AttribHandle<T>& h );

    /// Get attribute by handle (const).
    /// \complexity \f$ O(1) \f$
    /// \warning There is no check on the handle validity.
    template <typename T>
    inline const Attrib<T>& getAttrib( const AttribHandle<T>& h ) const;

    /// Return a AttribBase ptr to the attrib identified by name.
    /// to give access to AttribBase method, regardless of the type of element
    /// stored in the attrib.
    inline AttribBase* getAttribBase( const std::string& name );

    /// \see getAttribBase( const std::string& name );
    inline AttribBase* getAttribBase( const Index& idx );
    ///@}

    ///@{
    /// Set the data of the attrib h.
    /// \warning \p h has to be a valid attrib handle, this is not checked.
    template <typename T>
    inline void setAttrib( const AttribHandle<T>& h,
                           const typename AttribHandle<T>::Container& data );

    /// \see #setAttrib
    template <typename T>
    inline void setAttrib( const AttribHandle<T>& h, typename AttribHandle<T>::Container&& data );
    ///@}

    /// Add attribute by name.
    /// notify(name) is called to trigger the observers
    /// \note If an attribute with the same name already exists,
    ///       just returns a AttribHandle to it.
    /// \complexity \f$ O(n) \f$
    template <typename T>
    AttribHandle<T> addAttrib( const std::string& name );

    /// Remove attribute by handle, invalidates the handle.
    /// notify(name) is called to trigger the observers
    /// \warning If a new attribute is added, old invalidated handles may lead to
    ///          the new attribute.
    /// \complexity \f$ O(\log(n)) \f$
    template <typename T>
    void removeAttrib( AttribHandle<T>& h );
    /// Return true if *this and \p other have the same attributes, same amount
    /// and same names.
    /// \warning There is no check on the attribute type nor data.
    bool hasSameAttribs( const AttribManager& other );

    ///@{
    /// Perform \p fun on each attribute.
    // This is needed by the user to avoid caring about removed attributes (nullptr)
    // \todo reimplement as range for
    template <typename F>
    void for_each_attrib( const F& func ) const;

    // \todo keep non const version private
    template <typename F>
    void for_each_attrib( const F& func );
    ///@}

    /// Return the number of attributes
    inline int getNumAttribs() const;

  private:
    /// Attrib list, better using attribs() to go through.
    Container m_attribs;

    // Map between the attrib's name and its index, used to speedup finding the
    // handle index from the attribute name.
    std::map<std::string, Index> m_attribsIndex;

    // Ease wrapper
    friend class ::Ra::Core::Geometry::TopologicalMesh;

    /// Count number of valid attribs
    int m_numAttribs {0};
};

} // namespace Utils
} // namespace Core
} // namespace Ra

#include <Core/Utils/Attribs.inl>
<|endoftext|>


<commit_before>#ifndef STAN_MATH_PRIM_SCAL_FUN_LOG_MODIFIED_BESSEL_FIRST_KIND_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOG_MODIFIED_BESSEL_FIRST_KIND_HPP

#include <boost/math/special_functions/bessel.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/fun/is_inf.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/log_sum_exp.hpp>
#include <stan/math/prim/scal/fun/log1p.hpp>
#include <limits>

namespace stan {
  namespace math {

    /* Log of the modified Bessel function of the first kind,
     * which is better known as the log of the Bessel I function
     *
     * @param v Order, can be a non-integer but must be at least -1
     * @param z Real non-negative number
     * @throws if either v or z is NaN
     * @throws if z is negative
     * @throws if v is less than -1
     * @return log of Bessel I function
     */
    inline double
    log_modified_bessel_first_kind(const double v, const double z) {
      check_not_nan("log_modified_bessel_first_kind", "v", v);
      check_not_nan("log_modified_bessel_first_kind", "z", z);
      check_nonnegative("log_modified_bessel_first_kind", "z", z);
      check_greater_or_equal("log_modified_bessel_first_kind", "v", v, -1);

      using std::log;
      using stan::math::log_sum_exp;
      using stan::math::log1p;
      using stan::math::lgamma;

      if (z == 0) {
       if (v == 0) return 0.0;
       if (v > 0) return -std::numeric_limits<double>::infinity();
       return std::numeric_limits<double>::quiet_NaN();
      }
      if (stan::math::is_inf(z)) return z;
      if (z > 100) {
        // Boost does something like this in asymptotic_bessel_i_large_x
        double lim = (4 * v * v + 10) / (8 * z);
        lim *= lim;
        lim *= lim;
        lim /= 24;
        if (lim < (std::numeric_limits<double>::epsilon() * 10)) {
          double s = 1;
          double mu = 4 * v * v;
          double ex = 8 * z;
          double num = mu - 1;
          double denom = ex;
          s -= num / denom;
          num *= mu - 9;
          denom *= ex * 2;
          s += num / denom;
          num *= mu - 25;
          denom *= ex * 3;
          s -= num / denom;
          s = z - log(std::sqrt(2 * z * M_PI)) + log(s);
          return s;
        }
      }

      double log_half_z = log(0.5 * z);
      double lgam = lgamma(v + 1);
      double lcons = (2 + v) * log_half_z;
      double out;
      if (v > -1) {
        out = log_sum_exp(v * log_half_z - lgam, lcons - lgamma(v + 2));
        lgam += log(v + 1);
      } else {  // lgam is currently NaN
        out = lcons;
        lgam = 0;
      }

      int m = 2;
      double lfac = 0;
      double old_out = std::numeric_limits<double>::quiet_NaN();
      while (out != old_out) {
        lfac += log(m);
        lgam += log(v + m);
        lcons += 2 * log_half_z;
        old_out = out;
        out = log_sum_exp(out, lcons - lfac - lgam);  // underflows eventually
        m++;
      }
      return out;
    }

  }
}

#endif
<commit_msg>change to templates<commit_after>#ifndef STAN_MATH_PRIM_SCAL_FUN_LOG_MODIFIED_BESSEL_FIRST_KIND_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOG_MODIFIED_BESSEL_FIRST_KIND_HPP

#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/fun/is_inf.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/log_sum_exp.hpp>
#include <stan/math/prim/scal/fun/log1p.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <limits>

namespace stan {
  namespace math {

    /* Log of the modified Bessel function of the first kind,
     * which is better known as the Bessel I function. See
     * modified_bessel_first_kind.hpp for the function definition.
     *
     * @param v Order, can be a non-integer but must be at least -1
     * @param z Real non-negative number
     * @throws if either v or z is NaN
     * @throws if z is negative
     * @throws if v is less than -1
     * @return log of Bessel I function
     */
    template <typename T1, typename T2>
    inline typename boost::math::tools::promote_args<T1, T2, double>::type
    log_modified_bessel_first_kind(const T1 v, const T2 z) {
      check_not_nan("log_modified_bessel_first_kind", "v", v);
      check_not_nan("log_modified_bessel_first_kind", "z", z);
      check_nonnegative("log_modified_bessel_first_kind", "z", z);
      check_greater_or_equal("log_modified_bessel_first_kind", "v", v, -1);

      using std::log;
      using stan::math::log_sum_exp;
      using stan::math::log1p;
      using stan::math::lgamma;
      using stan::math::square;

      typedef typename boost::math::tools::promote_args<T1, T2, double>::type T;

      if (z == 0) {
       if (v == 0) return 0.0;
       if (v > 0) return -std::numeric_limits<T>::infinity();
       return std::numeric_limits<T>::quiet_NaN();
      }
      if (stan::math::is_inf(z)) return z;
      if (z > 100) {
        // Boost does something like this in asymptotic_bessel_i_large_x
        T lim = (4 * square(v) + 10) / (8 * z);
        lim *= lim;
        lim *= lim;
        lim /= 24;
        if (lim < (std::numeric_limits<double>::epsilon() * 10)) {
          T s = 1;
          T mu = 4 * square(v);
          T ex = 8 * z;
          T num = mu - 1;
          T denom = ex;
          s -= num / denom;
          num *= mu - 9;
          denom *= ex * 2;
          s += num / denom;
          num *= mu - 25;
          denom *= ex * 3;
          s -= num / denom;
          s = z - log(std::sqrt(2 * z * M_PI)) + log(s);
          return s;
        }
      }

      T log_half_z = log(0.5 * z);
      T lgam = lgamma(v + 1.0);
      T lcons = (2.0 + v) * log_half_z;
      T out;
      if (v > -1) {
        out = log_sum_exp(v * log_half_z - lgam, lcons - lgamma(v + 2));
        lgam += log(v + 1.0);
      } else {  // lgam is currently NaN
        out = lcons;
        lgam = 0;
      }

      double m = 2;
      T lfac = 0;
      T old_out;
      do {
        lfac += log(m);
        lgam += log(v + m);
        lcons += 2 * log_half_z;
        old_out = out;
        out = log_sum_exp(out, lcons - lfac - lgam);  // underflows eventually
        m++;
      } while (out > old_out || out < old_out);
      return out;
    }

  }
}

#endif
<|endoftext|>


<commit_before>/*

This file is part of VROOM.

Copyright (c) 2015-2018, Julien Coupey.
All rights reserved (see LICENSE).

*/

#include "./input.h"
#include "../../../problems/vrp.h"

input::input(std::unique_ptr<routing_io<cost_t>> routing_wrapper, bool geometry)
  : _start_loading(std::chrono::high_resolution_clock::now()),
    _routing_wrapper(std::move(routing_wrapper)),
    _geometry(geometry),
    _all_locations_have_coords(true) {
}

void input::add_job(const job_t& job) {
  _jobs.push_back(job);

  auto& current_job = _jobs.back();

  // Ensure amount size consistency.
  this->check_amount_size(current_job.amount.size());

  this->store_amount_lower_bound(current_job.amount);

  // Ensure that skills are either always or never provided.
  if (_locations.empty()) {
    _has_skills = !current_job.skills.empty();
  } else {
    if (_has_skills != !current_job.skills.empty()) {
      throw custom_exception("Missing skills.");
    }
  }

  if (!current_job.location.user_index()) {
    // Index of this job in the matrix was not specified upon job
    // creation, using current number of locations.
    current_job.location.set_index(_locations.size());
  }
  _matrix_used_index.insert(current_job.index());
  _all_locations_have_coords &= current_job.location.has_coordinates();

  _locations.push_back(current_job.location);
}

void input::add_vehicle(const vehicle_t& vehicle) {
  _vehicles.push_back(vehicle);

  auto& current_v = _vehicles.back();

  // Ensure amount size consistency.
  this->check_amount_size(current_v.capacity.size());

  // Ensure that skills are either always or never provided.
  if (_locations.empty()) {
    _has_skills = !current_v.skills.empty();
  } else {
    if (_has_skills != !current_v.skills.empty()) {
      throw custom_exception("Missing skills.");
    }
  }

  bool has_start = current_v.has_start();
  bool has_end = current_v.has_end();

  if (has_start) {
    if (!current_v.start.get().user_index()) {
      // Index of this start in the matrix was not specified upon
      // vehicle creation, using current number of locations.
      assert(current_v.start.get().has_coordinates());
      current_v.start.get().set_index(_locations.size());
    }

    _matrix_used_index.insert(current_v.start.get().index());
    _all_locations_have_coords &= current_v.start.get().has_coordinates();

    _locations.push_back(current_v.start.get());
  }

  if (has_end) {
    if (!current_v.end.get().user_index()) {
      // Index of this end in the matrix was not specified upon
      // vehicle creation, using current number of locations.
      assert(current_v.end.get().has_coordinates());
      current_v.end.get().set_index(_locations.size());
    }

    _matrix_used_index.insert(current_v.end.get().index());
    _all_locations_have_coords &= current_v.end.get().has_coordinates();

    _locations.push_back(current_v.end.get());
  }
}

void input::store_amount_lower_bound(const amount_t& amount) {
  if (_amount_lower_bound.empty()) {
    // Create on first call.
    _amount_lower_bound = amount;
  } else {
    for (std::size_t i = 0; i < _amount_size; ++i) {
      _amount_lower_bound[i] = std::min(_amount_lower_bound[i], amount[i]);
    }
  }
}

void input::check_amount_size(unsigned size) {
  if (_locations.empty()) {
    // Updating real value on first call.
    _amount_size = size;
  } else {
    // Checking consistency for amount/capacity input lengths.
    if (size != _amount_size) {
      throw custom_exception("Inconsistent amount/capacity lengths: " +
                             std::to_string(size) + " and " +
                             std::to_string(_amount_size) + '.');
    }
  }
}

void input::set_matrix(matrix<cost_t>&& m) {
  _matrix = std::move(m);
}

unsigned input::amount_size() const {
  return _amount_size;
}

amount_t input::get_amount_lower_bound() const {
  return _amount_lower_bound;
}

bool input::vehicle_ok_with_job(index_t v_index, index_t j_index) const {
  return _vehicle_to_job_compatibility[v_index][j_index];
}

const matrix<cost_t>& input::get_matrix() const {
  return _matrix;
}

matrix<cost_t>
input::get_sub_matrix(const std::vector<index_t>& indices) const {
  return _matrix.get_sub_matrix(indices);
}

void input::check_cost_bound() const {
  // Check that we don't have any overflow while computing an upper
  // bound for solution cost.

  std::vector<cost_t> max_cost_per_line(_matrix.size(), 0);
  std::vector<cost_t> max_cost_per_column(_matrix.size(), 0);

  for (const auto i : _matrix_used_index) {
    for (const auto j : _matrix_used_index) {
      max_cost_per_line[i] = std::max(max_cost_per_line[i], _matrix[i][j]);
      max_cost_per_column[j] = std::max(max_cost_per_column[j], _matrix[i][j]);
    }
  }

  cost_t jobs_departure_bound = 0;
  cost_t jobs_arrival_bound = 0;
  for (const auto& j : _jobs) {
    jobs_departure_bound =
      add_without_overflow(jobs_departure_bound, max_cost_per_line[j.index()]);
    jobs_arrival_bound =
      add_without_overflow(jobs_arrival_bound, max_cost_per_column[j.index()]);
  }

  cost_t jobs_bound = std::max(jobs_departure_bound, jobs_arrival_bound);

  cost_t start_bound = 0;
  cost_t end_bound = 0;
  for (const auto& v : _vehicles) {
    if (v.has_start()) {
      start_bound =
        add_without_overflow(start_bound,
                             max_cost_per_line[v.start.get().index()]);
    }
    if (v.has_end()) {
      end_bound =
        add_without_overflow(end_bound,
                             max_cost_per_column[v.end.get().index()]);
    }
  }

  cost_t bound = add_without_overflow(start_bound, jobs_bound);
  bound = add_without_overflow(bound, end_bound);

  BOOST_LOG_TRIVIAL(info) << "[Loading] solution cost upper bound: " << bound
                          << ".";
}

void input::set_vehicle_to_job_compatibility() {
  // Default to no restriction when no skills are provided.
  _vehicle_to_job_compatibility =
    std::vector<std::vector<bool>>(_vehicles.size(),
                                   std::vector<bool>(_jobs.size(), true));
  if (_has_skills) {
    for (std::size_t v = 0; v < _vehicles.size(); ++v) {
      const auto& v_skills = _vehicles[v].skills;
      assert(!v_skills.empty());

      for (std::size_t j = 0; j < _jobs.size(); ++j) {
        bool is_compatible = true;
        assert(!_jobs[j].skills.empty());
        for (const auto& s : _jobs[j].skills) {
          auto search = v_skills.find(s);
          is_compatible &= (search != v_skills.end());
          if (!is_compatible) {
            break;
          }
        }
        _vehicle_to_job_compatibility[v][j] = is_compatible;
      }
    }
  }
}

std::unique_ptr<vrp> input::get_problem() const {
  return std::make_unique<cvrp>(*this);
}

solution input::format_solution(const raw_solution& raw_routes) const {
  std::vector<route_t> routes;
  cost_t total_cost = 0;
  duration_t total_service = 0;
  amount_t total_amount(_amount_size);

  // All job ranks start with unassigned status.
  std::unordered_set<index_t> unassigned_ranks;
  for (unsigned i = 0; i < _jobs.size(); ++i) {
    unassigned_ranks.insert(i);
  }

  for (std::size_t i = 0; i < raw_routes.size(); ++i) {
    const auto& route = raw_routes[i];
    if (route.empty()) {
      continue;
    }
    const auto& v = _vehicles[i];
    cost_t cost = 0;
    duration_t service = 0;
    amount_t amount(_amount_size);

    // Steps for current route.
    std::vector<step> steps;

    // Handle start.
    if (v.has_start()) {
      steps.emplace_back(TYPE::START, v.start.get());
      cost += _matrix[v.start.get().index()][_jobs[route.front()].index()];
    }

    // Handle jobs.
    index_t previous = route.front();
    steps.emplace_back(_jobs[previous]);
    service += steps.back().service;
    amount += steps.back().amount;
    unassigned_ranks.erase(previous);

    for (auto it = ++route.cbegin(); it != route.cend(); ++it) {
      cost += _matrix[_jobs[previous].index()][_jobs[*it].index()];
      steps.emplace_back(_jobs[*it]);
      service += steps.back().service;
      amount += steps.back().amount;
      unassigned_ranks.erase(*it);
      previous = *it;
    }

    // Handle end.
    if (v.has_end()) {
      steps.emplace_back(TYPE::END, v.end.get());
      cost += _matrix[_jobs[route.back()].index()][v.end.get().index()];
    }

    assert(amount <= _vehicles[i].capacity);
    routes.emplace_back(_vehicles[i].id,
                        std::move(steps),
                        cost,
                        service,
                        amount);

    total_cost += cost;
    total_service += service;
    total_amount += amount;
  }

  // Handle unassigned jobs.
  std::vector<job_t> unassigned_jobs;
  std::transform(unassigned_ranks.begin(),
                 unassigned_ranks.end(),
                 std::back_inserter(unassigned_jobs),
                 [&](auto j) { return _jobs[j]; });

  return solution(0,
                  total_cost,
                  std::move(routes),
                  std::move(unassigned_jobs),
                  total_service,
                  std::move(total_amount));
}

solution input::solve(unsigned nb_thread) {
  if (_geometry and !_all_locations_have_coords) {
    // Early abort when info is required with missing coordinates.
    throw custom_exception("Option -g is invalid with missing coordinates.");
  }

  if (_matrix.size() < 2) {
    // OSRM call if matrix not already provided.
    assert(_routing_wrapper);
    BOOST_LOG_TRIVIAL(info) << "[Loading] Start matrix computing.";
    _matrix = _routing_wrapper->get_matrix(_locations);
  }

  // Check for potential overflow in solution cost.
  this->check_cost_bound();

  // Fill vehicle/job compatibility matrix.
  this->set_vehicle_to_job_compatibility();

  // Load relevant problem.
  auto instance = this->get_problem();
  _end_loading = std::chrono::high_resolution_clock::now();

  auto loading = std::chrono::duration_cast<std::chrono::milliseconds>(
                   _end_loading - _start_loading)
                   .count();

  BOOST_LOG_TRIVIAL(info) << "[Loading] Done, took " << loading << " ms.";

  // Solve.
  auto sol = format_solution(instance->solve(nb_thread));

  // Update timing info.
  sol.summary.computing_times.loading = loading;

  _end_solving = std::chrono::high_resolution_clock::now();
  sol.summary.computing_times.solving =
    std::chrono::duration_cast<std::chrono::milliseconds>(_end_solving -
                                                          _end_loading)
      .count();

  if (_geometry) {
    // Routing stuff.
    BOOST_LOG_TRIVIAL(info) << "[Route] Start computing detailed route.";

    for (auto& route : sol.routes) {
      _routing_wrapper->add_route_info(route);
      sol.summary.duration += route.duration;
      sol.summary.distance += route.distance;
    }

    _end_routing = std::chrono::high_resolution_clock::now();
    auto routing = std::chrono::duration_cast<std::chrono::milliseconds>(
                     _end_routing - _end_solving)
                     .count();

    sol.summary.computing_times.routing = routing;

    BOOST_LOG_TRIVIAL(info) << "[Route] Done, took " << routing << " ms.";
  }

  return sol;
}
<commit_msg>Add asserts on vehicle skills.<commit_after>/*

This file is part of VROOM.

Copyright (c) 2015-2018, Julien Coupey.
All rights reserved (see LICENSE).

*/

#include "./input.h"
#include "../../../problems/vrp.h"

input::input(std::unique_ptr<routing_io<cost_t>> routing_wrapper, bool geometry)
  : _start_loading(std::chrono::high_resolution_clock::now()),
    _routing_wrapper(std::move(routing_wrapper)),
    _geometry(geometry),
    _all_locations_have_coords(true) {
}

void input::add_job(const job_t& job) {
  _jobs.push_back(job);

  auto& current_job = _jobs.back();

  // Ensure amount size consistency.
  this->check_amount_size(current_job.amount.size());

  this->store_amount_lower_bound(current_job.amount);

  // Ensure that skills are either always or never provided.
  if (_locations.empty()) {
    _has_skills = !current_job.skills.empty();
  } else {
    if (_has_skills != !current_job.skills.empty()) {
      throw custom_exception("Missing skills.");
    }
  }

  if (!current_job.location.user_index()) {
    // Index of this job in the matrix was not specified upon job
    // creation, using current number of locations.
    current_job.location.set_index(_locations.size());
  }
  _matrix_used_index.insert(current_job.index());
  _all_locations_have_coords &= current_job.location.has_coordinates();

  _locations.push_back(current_job.location);
}

void input::add_vehicle(const vehicle_t& vehicle) {
  _vehicles.push_back(vehicle);

  auto& current_v = _vehicles.back();

  // Ensure amount size consistency.
  this->check_amount_size(current_v.capacity.size());

  // Ensure that skills are either always or never provided.
  if (_locations.empty()) {
    _has_skills = !current_v.skills.empty();
  } else {
    if (_has_skills != !current_v.skills.empty()) {
      throw custom_exception("Missing skills.");
    }
  }

  bool has_start = current_v.has_start();
  bool has_end = current_v.has_end();

  if (has_start) {
    if (!current_v.start.get().user_index()) {
      // Index of this start in the matrix was not specified upon
      // vehicle creation, using current number of locations.
      assert(current_v.start.get().has_coordinates());
      current_v.start.get().set_index(_locations.size());
    }

    _matrix_used_index.insert(current_v.start.get().index());
    _all_locations_have_coords &= current_v.start.get().has_coordinates();

    _locations.push_back(current_v.start.get());
  }

  if (has_end) {
    if (!current_v.end.get().user_index()) {
      // Index of this end in the matrix was not specified upon
      // vehicle creation, using current number of locations.
      assert(current_v.end.get().has_coordinates());
      current_v.end.get().set_index(_locations.size());
    }

    _matrix_used_index.insert(current_v.end.get().index());
    _all_locations_have_coords &= current_v.end.get().has_coordinates();

    _locations.push_back(current_v.end.get());
  }
}

void input::store_amount_lower_bound(const amount_t& amount) {
  if (_amount_lower_bound.empty()) {
    // Create on first call.
    _amount_lower_bound = amount;
  } else {
    for (std::size_t i = 0; i < _amount_size; ++i) {
      _amount_lower_bound[i] = std::min(_amount_lower_bound[i], amount[i]);
    }
  }
}

void input::check_amount_size(unsigned size) {
  if (_locations.empty()) {
    // Updating real value on first call.
    _amount_size = size;
  } else {
    // Checking consistency for amount/capacity input lengths.
    if (size != _amount_size) {
      throw custom_exception("Inconsistent amount/capacity lengths: " +
                             std::to_string(size) + " and " +
                             std::to_string(_amount_size) + '.');
    }
  }
}

void input::set_matrix(matrix<cost_t>&& m) {
  _matrix = std::move(m);
}

unsigned input::amount_size() const {
  return _amount_size;
}

amount_t input::get_amount_lower_bound() const {
  return _amount_lower_bound;
}

bool input::vehicle_ok_with_job(index_t v_index, index_t j_index) const {
  return _vehicle_to_job_compatibility[v_index][j_index];
}

const matrix<cost_t>& input::get_matrix() const {
  return _matrix;
}

matrix<cost_t>
input::get_sub_matrix(const std::vector<index_t>& indices) const {
  return _matrix.get_sub_matrix(indices);
}

void input::check_cost_bound() const {
  // Check that we don't have any overflow while computing an upper
  // bound for solution cost.

  std::vector<cost_t> max_cost_per_line(_matrix.size(), 0);
  std::vector<cost_t> max_cost_per_column(_matrix.size(), 0);

  for (const auto i : _matrix_used_index) {
    for (const auto j : _matrix_used_index) {
      max_cost_per_line[i] = std::max(max_cost_per_line[i], _matrix[i][j]);
      max_cost_per_column[j] = std::max(max_cost_per_column[j], _matrix[i][j]);
    }
  }

  cost_t jobs_departure_bound = 0;
  cost_t jobs_arrival_bound = 0;
  for (const auto& j : _jobs) {
    jobs_departure_bound =
      add_without_overflow(jobs_departure_bound, max_cost_per_line[j.index()]);
    jobs_arrival_bound =
      add_without_overflow(jobs_arrival_bound, max_cost_per_column[j.index()]);
  }

  cost_t jobs_bound = std::max(jobs_departure_bound, jobs_arrival_bound);

  cost_t start_bound = 0;
  cost_t end_bound = 0;
  for (const auto& v : _vehicles) {
    if (v.has_start()) {
      start_bound =
        add_without_overflow(start_bound,
                             max_cost_per_line[v.start.get().index()]);
    }
    if (v.has_end()) {
      end_bound =
        add_without_overflow(end_bound,
                             max_cost_per_column[v.end.get().index()]);
    }
  }

  cost_t bound = add_without_overflow(start_bound, jobs_bound);
  bound = add_without_overflow(bound, end_bound);

  BOOST_LOG_TRIVIAL(info) << "[Loading] solution cost upper bound: " << bound
                          << ".";
}

void input::set_vehicle_to_job_compatibility() {
  // Default to no restriction when no skills are provided.
  _vehicle_to_job_compatibility =
    std::vector<std::vector<bool>>(_vehicles.size(),
                                   std::vector<bool>(_jobs.size(), true));
  if (_has_skills) {
    for (std::size_t v = 0; v < _vehicles.size(); ++v) {
      const auto& v_skills = _vehicles[v].skills;
      assert(!v_skills.empty());

      for (std::size_t j = 0; j < _jobs.size(); ++j) {
        bool is_compatible = true;
        assert(!_jobs[j].skills.empty());
        for (const auto& s : _jobs[j].skills) {
          auto search = v_skills.find(s);
          is_compatible &= (search != v_skills.end());
          if (!is_compatible) {
            break;
          }
        }
        _vehicle_to_job_compatibility[v][j] = is_compatible;
      }
    }
  }
}

std::unique_ptr<vrp> input::get_problem() const {
  return std::make_unique<cvrp>(*this);
}

solution input::format_solution(const raw_solution& raw_routes) const {
  std::vector<route_t> routes;
  cost_t total_cost = 0;
  duration_t total_service = 0;
  amount_t total_amount(_amount_size);

  // All job ranks start with unassigned status.
  std::unordered_set<index_t> unassigned_ranks;
  for (unsigned i = 0; i < _jobs.size(); ++i) {
    unassigned_ranks.insert(i);
  }

  for (std::size_t i = 0; i < raw_routes.size(); ++i) {
    const auto& route = raw_routes[i];
    if (route.empty()) {
      continue;
    }
    const auto& v = _vehicles[i];
    cost_t cost = 0;
    duration_t service = 0;
    amount_t amount(_amount_size);

    // Steps for current route.
    std::vector<step> steps;

    // Handle start.
    if (v.has_start()) {
      steps.emplace_back(TYPE::START, v.start.get());
      cost += _matrix[v.start.get().index()][_jobs[route.front()].index()];
    }

    // Handle jobs.
    index_t previous = route.front();
    assert(vehicle_ok_with_job(i, previous));
    steps.emplace_back(_jobs[previous]);
    service += steps.back().service;
    amount += steps.back().amount;
    unassigned_ranks.erase(previous);

    for (auto it = ++route.cbegin(); it != route.cend(); ++it) {
      cost += _matrix[_jobs[previous].index()][_jobs[*it].index()];
      assert(vehicle_ok_with_job(i, *it));
      steps.emplace_back(_jobs[*it]);
      service += steps.back().service;
      amount += steps.back().amount;
      unassigned_ranks.erase(*it);
      previous = *it;
    }

    // Handle end.
    if (v.has_end()) {
      steps.emplace_back(TYPE::END, v.end.get());
      cost += _matrix[_jobs[route.back()].index()][v.end.get().index()];
    }

    assert(amount <= _vehicles[i].capacity);
    routes.emplace_back(_vehicles[i].id,
                        std::move(steps),
                        cost,
                        service,
                        amount);

    total_cost += cost;
    total_service += service;
    total_amount += amount;
  }

  // Handle unassigned jobs.
  std::vector<job_t> unassigned_jobs;
  std::transform(unassigned_ranks.begin(),
                 unassigned_ranks.end(),
                 std::back_inserter(unassigned_jobs),
                 [&](auto j) { return _jobs[j]; });

  return solution(0,
                  total_cost,
                  std::move(routes),
                  std::move(unassigned_jobs),
                  total_service,
                  std::move(total_amount));
}

solution input::solve(unsigned nb_thread) {
  if (_geometry and !_all_locations_have_coords) {
    // Early abort when info is required with missing coordinates.
    throw custom_exception("Option -g is invalid with missing coordinates.");
  }

  if (_matrix.size() < 2) {
    // OSRM call if matrix not already provided.
    assert(_routing_wrapper);
    BOOST_LOG_TRIVIAL(info) << "[Loading] Start matrix computing.";
    _matrix = _routing_wrapper->get_matrix(_locations);
  }

  // Check for potential overflow in solution cost.
  this->check_cost_bound();

  // Fill vehicle/job compatibility matrix.
  this->set_vehicle_to_job_compatibility();

  // Load relevant problem.
  auto instance = this->get_problem();
  _end_loading = std::chrono::high_resolution_clock::now();

  auto loading = std::chrono::duration_cast<std::chrono::milliseconds>(
                   _end_loading - _start_loading)
                   .count();

  BOOST_LOG_TRIVIAL(info) << "[Loading] Done, took " << loading << " ms.";

  // Solve.
  auto sol = format_solution(instance->solve(nb_thread));

  // Update timing info.
  sol.summary.computing_times.loading = loading;

  _end_solving = std::chrono::high_resolution_clock::now();
  sol.summary.computing_times.solving =
    std::chrono::duration_cast<std::chrono::milliseconds>(_end_solving -
                                                          _end_loading)
      .count();

  if (_geometry) {
    // Routing stuff.
    BOOST_LOG_TRIVIAL(info) << "[Route] Start computing detailed route.";

    for (auto& route : sol.routes) {
      _routing_wrapper->add_route_info(route);
      sol.summary.duration += route.duration;
      sol.summary.distance += route.distance;
    }

    _end_routing = std::chrono::high_resolution_clock::now();
    auto routing = std::chrono::duration_cast<std::chrono::milliseconds>(
                     _end_routing - _end_solving)
                     .count();

    sol.summary.computing_times.routing = routing;

    BOOST_LOG_TRIVIAL(info) << "[Route] Done, took " << routing << " ms.";
  }

  return sol;
}
<|endoftext|>


<commit_before>//===--- TaskLocal.cpp - Task Local Values --------------------------------===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2014 - 2020 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See https://swift.org/LICENSE.txt for license information
// See https://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//

#include <cstdint>

#include "swift/Runtime/Atomic.h"
#include "swift/Runtime/Casting.h"
#include "swift/Runtime/Portability.h"

#include "Concurrency/Threading/Mutex.h"
#include "Runtime/Threading/ThreadLocal.h"
#include "Runtime/Concurrency.h"

#include "Concurrency/TaskLocal.h"
#include "Concurrency/Task.h"
#include "Concurrency/Actor.h"
/* #include "Concurrency/Metadata.h" */

#include "Concurrency/TaskPrivate.h"

#include "llvm/ADT/PointerIntPair.h"
/* #include "TaskPrivate.h" */
#include <set>

#if defined(__APPLE__)
#include <asl.h>
#elif defined(__ANDROID__)
#include <android/log.h>
#endif

#if HAVE_PTHREAD_H
#include <pthread.h>
#endif

#if defined(_WIN32)
#include <io.h>
#include <handleapi.h>
#include <processthreadsapi.h>
#endif

using namespace swift;

// =============================================================================

/// An extremely silly class which exists to make pointer
/// default-initialization constexpr.
template <class T> struct Pointer {
  T *Value;
  constexpr Pointer() : Value(nullptr) {}
  constexpr Pointer(T *value) : Value(value) {}
  operator T *() const { return Value; }
  T *operator->() const { return Value; }
};

/// THIS IS RUNTIME INTERNAL AND NOT ABI.
class FallbackTaskLocalStorage {
  static SWIFT_RUNTIME_DECLARE_THREAD_LOCAL(
      Pointer<TaskLocal::Storage>, Value,
      SWIFT_CONCURRENCY_FALLBACK_TASK_LOCAL_STORAGE_KEY);

public:
  static void set(TaskLocal::Storage *task) { Value.set(task); }
  static TaskLocal::Storage *get() { return Value.get(); }
};

/// Define the thread-locals.
SWIFT_RUNTIME_DECLARE_THREAD_LOCAL(
    Pointer<TaskLocal::Storage>, FallbackTaskLocalStorage::Value,
    SWIFT_CONCURRENCY_FALLBACK_TASK_LOCAL_STORAGE_KEY);

// ==== ABI --------------------------------------------------------------------

SWIFT_CC(swift)
static void swift_task_localValuePushImpl(const HeapObject *key,
                                              /* +1 */ OpaqueValue *value,
                                              const Metadata *valueType) {
  if (AsyncTask *task = swift_task_getCurrent()) {
    task->localValuePush(key, value, valueType);
    return;
  }

  // no AsyncTask available so we must check the fallback
  TaskLocal::Storage *Local = nullptr;
  if (auto storage = FallbackTaskLocalStorage::get()) {
    Local = storage;
  } else {
    void *allocation = malloc(sizeof(TaskLocal::Storage));
    auto *freshStorage = new(allocation) TaskLocal::Storage();

    FallbackTaskLocalStorage::set(freshStorage);
    Local = freshStorage;
  }

  Local->pushValue(/*task=*/nullptr, key, value, valueType);
}

SWIFT_CC(swift)
static OpaqueValue* swift_task_localValueGetImpl(const HeapObject *key) {
  if (AsyncTask *task = swift_task_getCurrent()) {
    // we're in the context of a task and can use the task's storage
    return task->localValueGet(key);
  }

  // no AsyncTask available so we must check the fallback
  if (auto Local = FallbackTaskLocalStorage::get()) {
    return Local->getValue(/*task*/nullptr, key);
  }

  // no value found in task-local or fallback thread-local storage.
  return nullptr;
}

SWIFT_CC(swift)
static void swift_task_localValuePopImpl() {
  if (AsyncTask *task = swift_task_getCurrent()) {
    task->localValuePop();
    return;
  }

  if (TaskLocal::Storage *Local = FallbackTaskLocalStorage::get()) {
    bool hasRemainingBindings = Local->popValue(nullptr);
    if (!hasRemainingBindings) {
      // We clean up eagerly, it may be that this non-swift-concurrency thread
      // never again will use task-locals, and as such we better remove the storage.
      FallbackTaskLocalStorage::set(nullptr);
      free(Local);
    }
    return;
  }

  assert(false && "Attempted to pop value but no task or thread-local storage available!");
}

SWIFT_CC(swift)
static void swift_task_localsCopyToImpl(AsyncTask *task) {
  TaskLocal::Storage *Local = nullptr;

  if (AsyncTask *task = swift_task_getCurrent()) {
    Local = &task->_private().Local;
  } else if (auto *storage = FallbackTaskLocalStorage::get()) {
    Local = storage;
  } else {
    // bail out, there are no values to copy
    return;
  }

  Local->copyTo(task);
}

// =============================================================================
// ==== Initialization ---------------------------------------------------------

void TaskLocal::Storage::initializeLinkParent(AsyncTask* task,
                                              AsyncTask* parent) {
  assert(!head && "initial task local storage was already initialized");
  assert(parent && "parent must be provided to link to it");
  head = TaskLocal::Item::createParentLink(task, parent);
}

TaskLocal::Item*
TaskLocal::Item::createParentLink(AsyncTask *task, AsyncTask *parent) {
  size_t amountToAllocate = Item::itemSize(/*valueType*/nullptr);
  void *allocation = _swift_task_alloc_specific(task, amountToAllocate);
  Item *item = new(allocation) Item();

  auto parentHead = parent->_private().Local.head;
  if (parentHead) {
    if (parentHead->isEmpty()) {
      switch (parentHead->getNextLinkType()) {
        case NextLinkType::IsParent:
          // it has no values, and just points to its parent,
          // therefore skip also skip pointing to that parent and point
          // to whichever parent it was pointing to as well, it may be its
          // immediate parent, or some super-parent.
          item->next = reinterpret_cast<uintptr_t>(parentHead->getNext()) |
                       static_cast<uintptr_t>(NextLinkType::IsParent);
          break;
        case NextLinkType::IsNext:
          if (parentHead->getNext()) {
            assert(false && "empty taskValue head in parent task, yet parent's 'head' is `IsNext`, "
                            "this should not happen, as it implies the parent must have stored some value.");
          } else {
            // is terminal pointer
            item->next = reinterpret_cast<uintptr_t>(parentHead->getNext());
          }
          break;
      }
    } else {
      item->next = reinterpret_cast<uintptr_t>(parentHead) |
                   static_cast<uintptr_t>(NextLinkType::IsParent);
    }
  } else {
    item->next = reinterpret_cast<uintptr_t>(parentHead);
  }

  return item;
}

TaskLocal::Item*
TaskLocal::Item::createLink(AsyncTask *task,
                            const HeapObject *key,
                            const Metadata *valueType) {
  size_t amountToAllocate = Item::itemSize(valueType);
  void *allocation = task ? _swift_task_alloc_specific(task, amountToAllocate)
                          : malloc(amountToAllocate);
  Item *item = new (allocation) Item(key, valueType);

  auto next = task ? task->_private().Local.head
                   : FallbackTaskLocalStorage::get()->head;
  item->next = reinterpret_cast<uintptr_t>(next) |
      static_cast<uintptr_t>(NextLinkType::IsNext);

  return item;
}

void TaskLocal::Item::copyTo(AsyncTask *target) {
  assert(target && "TaskLocal item attempt to copy to null target task!");

  // 'parent' pointers are signified by null valueType.
  // We must not copy parent pointers, but rather perform a deep copy of all values,
  // as such, we skip parent pointers here entirely.
  if (isParentPointer())
    return;

  auto item = Item::createLink(target, key, valueType);
  valueType->vw_initializeWithCopy(item->getStoragePtr(), getStoragePtr());

  /// A `copyTo` may ONLY be invoked BEFORE the task is actually scheduled,
  /// so right now we can safely copy the value into the task without additional
  /// synchronization.
  target->_private().Local.head = item;
}

// =============================================================================
// ==== checks -----------------------------------------------------------------

SWIFT_CC(swift)
static void swift_task_reportIllegalTaskLocalBindingWithinWithTaskGroupImpl(
    const unsigned char *file, uintptr_t fileLength,
    bool fileIsASCII, uintptr_t line) {

  char *message;
  swift_asprintf(
      &message,
      "error: task-local: detected illegal task-local value binding at %.*s:%d.\n"
      "Task-local values must only be set in a structured-context, such as: "
      "around any (synchronous or asynchronous function invocation), "
      "around an 'async let' declaration, or around a 'with(Throwing)TaskGroup(...){ ... }' "
      "invocation. Notably, binding a task-local value is illegal *within the body* "
      "of a withTaskGroup invocation.\n"
      "\n"
      "The following example is illegal:\n\n"
      "    await withTaskGroup(...) { group in \n"
      "        await <task-local>.withValue(1234) {\n"
      "            group.spawn { ... }\n"
      "        }\n"
      "    }\n"
      "\n"
      "And should be replaced by, either: setting the value for the entire group:\n"
      "\n"
      "    // bind task-local for all tasks spawned within the group\n"
      "    await <task-local>.withValue(1234) {\n"
      "        await withTaskGroup(...) { group in\n"
      "            group.spawn { ... }\n"
      "        }\n"
      "    }\n"
      "\n"
      "or, inside the specific task-group child task:\n"
      "\n"
      "    // bind-task-local for only specific child-task\n"
      "    await withTaskGroup(...) { group in\n"
      "        group.spawn {\n"
      "            await <task-local>.withValue(1234) {\n"
      "                ... \n"
      "            }\n"
      "        }\n"
      "\n"
      "        group.spawn { ... }\n"
      "    }\n",
      (int)fileLength, file,
      (int)line);

  if (_swift_shouldReportFatalErrorsToDebugger()) {
    RuntimeErrorDetails details = {
        .version = RuntimeErrorDetails::currentVersion,
        .errorType = "task-local-violation",
        .currentStackDescription = "Task-local bound in illegal context",
        .framesToSkip = 1,
    };
    _swift_reportToDebugger(RuntimeErrorFlagFatal, message, &details);
  }

#if defined(_WIN32)
  #define STDERR_FILENO 2
  _write(STDERR_FILENO, message, strlen(message));
#else
  write(STDERR_FILENO, message, strlen(message));
#endif
#if defined(__APPLE__)
  asl_log(nullptr, nullptr, ASL_LEVEL_ERR, "%s", message);
#elif defined(__ANDROID__)
  __android_log_print(ANDROID_LOG_FATAL, "SwiftRuntime", "%s", message);
#endif

  free(message);
  abort();
}

// =============================================================================
// ==== destroy ----------------------------------------------------------------

void TaskLocal::Item::destroy(AsyncTask *task) {
  // otherwise it was task-local allocated, so we can safely destroy it right away
  if (valueType) {
    valueType->vw_destroy(getStoragePtr());
  }

  // if task is available, we must have used the task allocator to allocate this item,
  // so we must deallocate it using the same. Otherwise, we must have used malloc.
  if (task) _swift_task_dealloc_specific(task, this);
  else free(this);
}

void TaskLocal::Storage::destroy(AsyncTask *task) {
  auto item = head;
  head = nullptr;
  TaskLocal::Item *next;
  while (item) {
    auto linkType = item->getNextLinkType();
    switch (linkType) {
    case TaskLocal::NextLinkType::IsNext:
        next = item->getNext();
        item->destroy(task);
        item = next;
        break;

      case TaskLocal::NextLinkType::IsParent:
        // we're done here; as we must not proceed into the parent owned values.
        // we do have to destroy the item pointing at the parent/edge itself though.
        item->destroy(task);
        return;
    }
  }
}

// =============================================================================
// ==== Task Local Storage: operations -----------------------------------------

void TaskLocal::Storage::pushValue(AsyncTask *task,
                                   const HeapObject *key,
                                   /* +1 */ OpaqueValue *value,
                                   const Metadata *valueType) {
  assert(value && "Task local value must not be nil");

  auto item = Item::createLink(task, key, valueType);
  valueType->vw_initializeWithTake(item->getStoragePtr(), value);
  head = item;
}

bool TaskLocal::Storage::popValue(AsyncTask *task) {
  assert(head && "attempted to pop value off empty task-local stack");
  auto old = head;
  head = head->getNext();
  old->destroy(task);

  /// if pointing at not-null next item, there are remaining bindings.
  return head != nullptr;
}

OpaqueValue* TaskLocal::Storage::getValue(AsyncTask *task,
                                          const HeapObject *key) {
  assert(key && "TaskLocal key must not be null.");

  auto item = head;
  while (item) {
    if (item->key == key) {
      return item->getStoragePtr();
    }

    item = item->getNext();
  }

  return nullptr;
}

void TaskLocal::Storage::copyTo(AsyncTask *target) {
  assert(target && "task must not be null when copying values into it");
  assert(!(target->_private().Local.head) &&
      "Task must not have any task-local values bound before copying into it");

  // Set of keys for which we already have copied to the new task.
  // We only ever need to copy the *first* encounter of any given key,
  // because it is the most "specific"/"recent" binding and any other binding
  // of a key does not matter for the target task as it will never be able to
  // observe it.
  std::set<const HeapObject*> copied = {};

  auto item = head;
  while (item) {
    // we only have to copy an item if it is the most recent binding of a key.
    // i.e. if we've already seen an item for this key, we can skip it.
    if (copied.emplace(item->key).second) {
      item->copyTo(target);
    }

    item = item->getNext();
  }
}
<commit_msg>Remove unused functions from 5.6 backdeploy<commit_after>//===--- TaskLocal.cpp - Task Local Values --------------------------------===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2014 - 2020 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See https://swift.org/LICENSE.txt for license information
// See https://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//

#include <cstdint>

#include "swift/Runtime/Atomic.h"
#include "swift/Runtime/Casting.h"
#include "swift/Runtime/Portability.h"

#include "Concurrency/Threading/Mutex.h"
#include "Runtime/Threading/ThreadLocal.h"
#include "Runtime/Concurrency.h"

#include "Concurrency/TaskLocal.h"
#include "Concurrency/Task.h"
#include "Concurrency/Actor.h"
/* #include "Concurrency/Metadata.h" */

#include "Concurrency/TaskPrivate.h"

#include "llvm/ADT/PointerIntPair.h"
/* #include "TaskPrivate.h" */
#include <set>

#if defined(__APPLE__)
#include <asl.h>
#elif defined(__ANDROID__)
#include <android/log.h>
#endif

#if HAVE_PTHREAD_H
#include <pthread.h>
#endif

#if defined(_WIN32)
#include <io.h>
#include <handleapi.h>
#include <processthreadsapi.h>
#endif

using namespace swift;

// =============================================================================

/// An extremely silly class which exists to make pointer
/// default-initialization constexpr.
template <class T> struct Pointer {
  T *Value;
  constexpr Pointer() : Value(nullptr) {}
  constexpr Pointer(T *value) : Value(value) {}
  operator T *() const { return Value; }
  T *operator->() const { return Value; }
};

/// THIS IS RUNTIME INTERNAL AND NOT ABI.
class FallbackTaskLocalStorage {
  static SWIFT_RUNTIME_DECLARE_THREAD_LOCAL(
      Pointer<TaskLocal::Storage>, Value,
      SWIFT_CONCURRENCY_FALLBACK_TASK_LOCAL_STORAGE_KEY);

public:
  static void set(TaskLocal::Storage *task) { Value.set(task); }
  static TaskLocal::Storage *get() { return Value.get(); }
};

/// Define the thread-locals.
SWIFT_RUNTIME_DECLARE_THREAD_LOCAL(
    Pointer<TaskLocal::Storage>, FallbackTaskLocalStorage::Value,
    SWIFT_CONCURRENCY_FALLBACK_TASK_LOCAL_STORAGE_KEY);

// =============================================================================
// ==== Initialization ---------------------------------------------------------

void TaskLocal::Storage::initializeLinkParent(AsyncTask* task,
                                              AsyncTask* parent) {
  assert(!head && "initial task local storage was already initialized");
  assert(parent && "parent must be provided to link to it");
  head = TaskLocal::Item::createParentLink(task, parent);
}

TaskLocal::Item*
TaskLocal::Item::createParentLink(AsyncTask *task, AsyncTask *parent) {
  size_t amountToAllocate = Item::itemSize(/*valueType*/nullptr);
  void *allocation = _swift_task_alloc_specific(task, amountToAllocate);
  Item *item = new(allocation) Item();

  auto parentHead = parent->_private().Local.head;
  if (parentHead) {
    if (parentHead->isEmpty()) {
      switch (parentHead->getNextLinkType()) {
        case NextLinkType::IsParent:
          // it has no values, and just points to its parent,
          // therefore skip also skip pointing to that parent and point
          // to whichever parent it was pointing to as well, it may be its
          // immediate parent, or some super-parent.
          item->next = reinterpret_cast<uintptr_t>(parentHead->getNext()) |
                       static_cast<uintptr_t>(NextLinkType::IsParent);
          break;
        case NextLinkType::IsNext:
          if (parentHead->getNext()) {
            assert(false && "empty taskValue head in parent task, yet parent's 'head' is `IsNext`, "
                            "this should not happen, as it implies the parent must have stored some value.");
          } else {
            // is terminal pointer
            item->next = reinterpret_cast<uintptr_t>(parentHead->getNext());
          }
          break;
      }
    } else {
      item->next = reinterpret_cast<uintptr_t>(parentHead) |
                   static_cast<uintptr_t>(NextLinkType::IsParent);
    }
  } else {
    item->next = reinterpret_cast<uintptr_t>(parentHead);
  }

  return item;
}

TaskLocal::Item*
TaskLocal::Item::createLink(AsyncTask *task,
                            const HeapObject *key,
                            const Metadata *valueType) {
  size_t amountToAllocate = Item::itemSize(valueType);
  void *allocation = task ? _swift_task_alloc_specific(task, amountToAllocate)
                          : malloc(amountToAllocate);
  Item *item = new (allocation) Item(key, valueType);

  auto next = task ? task->_private().Local.head
                   : FallbackTaskLocalStorage::get()->head;
  item->next = reinterpret_cast<uintptr_t>(next) |
      static_cast<uintptr_t>(NextLinkType::IsNext);

  return item;
}

void TaskLocal::Item::copyTo(AsyncTask *target) {
  assert(target && "TaskLocal item attempt to copy to null target task!");

  // 'parent' pointers are signified by null valueType.
  // We must not copy parent pointers, but rather perform a deep copy of all values,
  // as such, we skip parent pointers here entirely.
  if (isParentPointer())
    return;

  auto item = Item::createLink(target, key, valueType);
  valueType->vw_initializeWithCopy(item->getStoragePtr(), getStoragePtr());

  /// A `copyTo` may ONLY be invoked BEFORE the task is actually scheduled,
  /// so right now we can safely copy the value into the task without additional
  /// synchronization.
  target->_private().Local.head = item;
}

// =============================================================================
// ==== destroy ----------------------------------------------------------------

void TaskLocal::Item::destroy(AsyncTask *task) {
  // otherwise it was task-local allocated, so we can safely destroy it right away
  if (valueType) {
    valueType->vw_destroy(getStoragePtr());
  }

  // if task is available, we must have used the task allocator to allocate this item,
  // so we must deallocate it using the same. Otherwise, we must have used malloc.
  if (task) _swift_task_dealloc_specific(task, this);
  else free(this);
}

void TaskLocal::Storage::destroy(AsyncTask *task) {
  auto item = head;
  head = nullptr;
  TaskLocal::Item *next;
  while (item) {
    auto linkType = item->getNextLinkType();
    switch (linkType) {
    case TaskLocal::NextLinkType::IsNext:
        next = item->getNext();
        item->destroy(task);
        item = next;
        break;

      case TaskLocal::NextLinkType::IsParent:
        // we're done here; as we must not proceed into the parent owned values.
        // we do have to destroy the item pointing at the parent/edge itself though.
        item->destroy(task);
        return;
    }
  }
}

// =============================================================================
// ==== Task Local Storage: operations -----------------------------------------

void TaskLocal::Storage::pushValue(AsyncTask *task,
                                   const HeapObject *key,
                                   /* +1 */ OpaqueValue *value,
                                   const Metadata *valueType) {
  assert(value && "Task local value must not be nil");

  auto item = Item::createLink(task, key, valueType);
  valueType->vw_initializeWithTake(item->getStoragePtr(), value);
  head = item;
}

bool TaskLocal::Storage::popValue(AsyncTask *task) {
  assert(head && "attempted to pop value off empty task-local stack");
  auto old = head;
  head = head->getNext();
  old->destroy(task);

  /// if pointing at not-null next item, there are remaining bindings.
  return head != nullptr;
}

OpaqueValue* TaskLocal::Storage::getValue(AsyncTask *task,
                                          const HeapObject *key) {
  assert(key && "TaskLocal key must not be null.");

  auto item = head;
  while (item) {
    if (item->key == key) {
      return item->getStoragePtr();
    }

    item = item->getNext();
  }

  return nullptr;
}

void TaskLocal::Storage::copyTo(AsyncTask *target) {
  assert(target && "task must not be null when copying values into it");
  assert(!(target->_private().Local.head) &&
      "Task must not have any task-local values bound before copying into it");

  // Set of keys for which we already have copied to the new task.
  // We only ever need to copy the *first* encounter of any given key,
  // because it is the most "specific"/"recent" binding and any other binding
  // of a key does not matter for the target task as it will never be able to
  // observe it.
  std::set<const HeapObject*> copied = {};

  auto item = head;
  while (item) {
    // we only have to copy an item if it is the most recent binding of a key.
    // i.e. if we've already seen an item for this key, we can skip it.
    if (copied.emplace(item->key).second) {
      item->copyTo(target);
    }

    item = item->getNext();
  }
}
<|endoftext|>


<commit_before>#include <ctime>
#include <random>
#include <vector>

#include "./../../../lib/easylogging++.h"

#include "./graph_topology.hpp"
#include "./../node/network_node.hpp"
#include "./../node/link.hpp"
#include "./../node/cache/no_cache.hpp"

network::topology::graph_topology::graph_topology() {

}

network::topology::graph_topology::~graph_topology() {

}

network::node::network_node** network::topology::graph_topology::generate(network::topology::topology_config* config) {
    std::srand(std::time(0));

    network::node::network_node** nodes = new network::node::network_node*[config->number_of_nodes + 1];
    int nodes_count = 0;

    std::vector<int> clusters_pos;
    int uuid = 0;

    // position of first cluster in []nodes
    clusters_pos.push_back(0);

    do {
        int nodes_in_cluster = 1 + (std::rand() % config->max_number_of_nodes_in_clusters);
        LOG(INFO) << "[TOPOLOGY_BULD] " << "Nodes in Cluster: " << nodes_in_cluster;

        if ((nodes_count + nodes_in_cluster) > config->number_of_nodes) {
            nodes_in_cluster = config->number_of_nodes - nodes_count;
            LOG(INFO) << "[TOPOLOGY_BULD] " << "Nodes in Cluster (updated):" << nodes_in_cluster;
        }

        int start_pos = *clusters_pos.end();
        // start position of the next cluster
        clusters_pos.push_back(start_pos + nodes_in_cluster);
        LOG(INFO) << "[TOPOLOGY_BULD] " << "AA";

        /**************************************************************
        **************            BUILD CLUSTER          **************
        **************************************************************/
        for (int i = 0; i < nodes_in_cluster; i++) {
            // LOG(INFO) << "[TOPOLOGY_BULD] " << "BB";
            // nodes[start_pos + i] = (network::node::network_node*)malloc(sizeof(network::node::network_node));
            // Segmentation fault is happening here
            nodes[start_pos + i] = new network::node::network_node(++uuid, new network::node::cache::no_cache);
            // LOG(INFO) << "[TOPOLOGY_BULD] " << "CC";
            // LOG(INFO) << "[TOPOLOGY_BULD] " << "DD";
            // LOG(INFO) << "[TOPOLOGY_BULD] " << "EE";
            nodes_count++;

            LOG(INFO) << "[TOPOLOGY_BULD] " << "Created Node ID:" << uuid;
        }

        LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Cluster Nodes";

        /*************************************************************
        **************   LINK MAIN NODE TO ALL CHILDREN   *************
        *************************************************************/
        for (int i = 1; i < nodes_in_cluster; i++) {
            LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Node " << nodes[start_pos]->id()   << " to Node " << nodes[start_pos+i]->id();
            LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Node " << nodes[start_pos+i]->id() << " to Node " << nodes[start_pos]->id();
            nodes[start_pos]->register_forwarding_node(nodes[start_pos+i]);
            nodes[start_pos+i]->register_forwarding_node(nodes[start_pos]);
        }

        LOG(INFO) << "[TOPOLOGY_BULD] " << "Nodes Counter:" << nodes_count;

    } while (nodes_count < config->number_of_nodes);

    LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Clusters";

    for (int i = 0; i < clusters_pos.size(); i++){
        int number_of_edges = 1 + (std::rand() % 2);

        for (int j = 1; j <= number_of_edges; j++) {
            int cluster_start = clusters_pos.at(i);
            int target_cluster = 0;

            if (i == *clusters_pos.end()) {
                // target cluster should be the cluster in position j
                target_cluster = clusters_pos.at(j - 1);
            } else {
                // else, get current plus j
                target_cluster = clusters_pos.at(i + j);
            }

            nodes[cluster_start]->register_forwarding_node(nodes[target_cluster]);
            LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Node " << nodes[cluster_start]->id() << " to Node " << nodes[target_cluster]->id();
        }
    }

    LOG(INFO) << "[TOPOLOGY_BULD] " << "Topology Generated";

    return nodes;
}<commit_msg>Fix Segmentation Fault error. Need to fix out of bounds exception<commit_after>#include <ctime>
#include <random>
#include <vector>

#include "./../../../lib/easylogging++.h"

#include "./graph_topology.hpp"
#include "./../node/network_node.hpp"
#include "./../node/link.hpp"
#include "./../node/cache/no_cache.hpp"

network::topology::graph_topology::graph_topology() {

}

network::topology::graph_topology::~graph_topology() {

}

network::node::network_node** network::topology::graph_topology::generate(network::topology::topology_config* config) {
    std::srand(std::time(0));

    network::node::network_node** nodes = new network::node::network_node*[config->number_of_nodes + 1];
    int nodes_count = 0;

    std::vector<int> clusters_pos;
    int uuid = 0;

    // position of first cluster in []nodes
    clusters_pos.push_back(0);

    do {
        int nodes_in_cluster = 1 + (std::rand() % config->max_number_of_nodes_in_clusters);
        LOG(INFO) << "[TOPOLOGY_BULD] " << "Nodes in Cluster: " << nodes_in_cluster;

        if ((nodes_count + nodes_in_cluster) > config->number_of_nodes) {
            nodes_in_cluster = config->number_of_nodes - nodes_count;
            LOG(INFO) << "[TOPOLOGY_BULD] " << "Nodes in Cluster (updated): " << nodes_in_cluster;
        }

        int start_pos = *(clusters_pos.begin() + (clusters_pos.size() - 1));
        // start position of the next cluster
        clusters_pos.push_back(start_pos + nodes_in_cluster);

        /**************************************************************
        **************            BUILD CLUSTER          **************
        **************************************************************/
        for (int i = 0; i < nodes_in_cluster; i++) {
            nodes[start_pos + i] = new network::node::network_node(++uuid, new network::node::cache::no_cache);
            nodes_count++;

            LOG(INFO) << "[TOPOLOGY_BULD] " << "Created Node ID: " << uuid;
        }

        LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Cluster Nodes";

        /*************************************************************
        **************   LINK MAIN NODE TO ALL CHILDREN   *************
        *************************************************************/
        for (int i = 1; i < nodes_in_cluster; i++) {
            LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Node " << nodes[start_pos]->id()   << " to Node " << nodes[start_pos+i]->id();
            LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Node " << nodes[start_pos+i]->id() << " to Node " << nodes[start_pos]->id();
            nodes[start_pos]->register_forwarding_node(nodes[start_pos+i]);
            nodes[start_pos+i]->register_forwarding_node(nodes[start_pos]);
        }

        LOG(INFO) << "[TOPOLOGY_BULD] " << "Nodes Counter:" << nodes_count;

    } while (nodes_count < config->number_of_nodes);

    LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Clusters";

    for (int i = 0; i < clusters_pos.size(); i++){
        int number_of_edges = 1 + (std::rand() % 2);

        for (int j = 1; j <= number_of_edges; j++) {
            int cluster_start = clusters_pos.at(i);
            int target_cluster = 0;

            if (i == *clusters_pos.end()) {
                // target cluster should be the cluster in position j
                target_cluster = clusters_pos.at(j - 1);
            } else {
                // else, get current plus j
                target_cluster = clusters_pos.at(i + j);
            }

            nodes[cluster_start]->register_forwarding_node(nodes[target_cluster]);
            LOG(INFO) << "[TOPOLOGY_BULD] " << "Connecting Node " << nodes[cluster_start]->id() << " to Node " << nodes[target_cluster]->id();
        }
    }

    LOG(INFO) << "[TOPOLOGY_BULD] " << "Topology Generated";

    return nodes;
}<|endoftext|>


<commit_before>
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud2.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <rtabmap_ros/MsgConversion.h>

namespace rtabmap_ros
{

class PointCloudAggregator : public nodelet::Nodelet
{
public:
	PointCloudAggregator() : sync(NULL)
	{}

	virtual ~PointCloudAggregator()
	{
	    if (sync!=NULL) delete sync;
	}

private:
	void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
	{
		if(cloudPub_.getNumSubscribers())
		{
			pcl::fromROSMsg(*cloudMsg_1, cloud1);
			pcl::fromROSMsg(*cloudMsg_2, cloud2);
			pcl::fromROSMsg(*cloudMsg_3, cloud3);
		    pcl::PointCloud<pcl::PointXYZ> totalCloud;
		    totalCloud = cloud1 + cloud2;
		    totalCloud += cloud3;
		    sensor_msgs::PointCloud2 rosCloud;
		    pcl::toROSMsg(totalCloud, rosCloud);
		    rosCloud.header.stamp = cloudMsg_1->header.stamp;
		    rosCloud.header.frame_id = cloudMsg_1->header.frame_id;
		    cloudPub_.publish(rosCloud);
		}
	}

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> MySyncPolicy;
	virtual void onInit()
	{
		ros::NodeHandle & nh = getNodeHandle();
		ros::NodeHandle & pnh = getPrivateNodeHandle();

		int queueSize = 5;
		pnh.param("queue_size", queueSize, queueSize);

		cloudSub_1_.subscribe(nh, "cloud1", 1);
		cloudSub_2_.subscribe(nh, "cloud2", 1);
		cloudSub_3_.subscribe(nh, "cloud3", 1);

        sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
        sync->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3));

		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
	}

    message_filters::Synchronizer<MySyncPolicy>* sync;
	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3;

	ros::Publisher cloudPub_;
};

PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet);
}

<commit_msg>Queue size for aggregator is set to a higher number, prevent aggregation from hanging<commit_after>#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud2.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <rtabmap_ros/MsgConversion.h>

namespace rtabmap_ros
{

class PointCloudAggregator : public nodelet::Nodelet
{
public:
	PointCloudAggregator() : sync(NULL)
	{}

	virtual ~PointCloudAggregator()
	{
	    if (sync!=NULL) delete sync;
	}

private:
	void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
	{
		if(cloudPub_.getNumSubscribers())
		{
			pcl::fromROSMsg(*cloudMsg_1, cloud1);
			pcl::fromROSMsg(*cloudMsg_2, cloud2);
			pcl::fromROSMsg(*cloudMsg_3, cloud3);
		    pcl::PointCloud<pcl::PointXYZ> totalCloud;
		    totalCloud = cloud1 + cloud2;
		    totalCloud += cloud3;
		    sensor_msgs::PointCloud2 rosCloud;
		    pcl::toROSMsg(totalCloud, rosCloud);
		    rosCloud.header.stamp = cloudMsg_1->header.stamp;
		    rosCloud.header.frame_id = cloudMsg_1->header.frame_id;
		    cloudPub_.publish(rosCloud);
		}
	}

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> MySyncPolicy;
	virtual void onInit()
	{
		ros::NodeHandle & nh = getNodeHandle();
		ros::NodeHandle & pnh = getPrivateNodeHandle();

		int queueSize = 15;
		pnh.param("queue_size", queueSize, queueSize);

		cloudSub_1_.subscribe(nh, "cloud1", 1);
		cloudSub_2_.subscribe(nh, "cloud2", 1);
		cloudSub_3_.subscribe(nh, "cloud3", 1);

        sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
        sync->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3));

		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
	}

    message_filters::Synchronizer<MySyncPolicy>* sync;
	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3;

	ros::Publisher cloudPub_;
};

PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet);
}

<|endoftext|>