#pragma once

#include <ostream>
#include <vector>

#include <boost/predef/other/endian.h>

#include <pcl/pcl_macros.h>  // for PCL_EXPORTS
#include <pcl/PCLHeader.h>
#include <pcl/PCLPointField.h>
#include <pcl/types.h>

namespace pcl
{

  struct PCL_EXPORTS PCLPointCloud2
  {
    ::pcl::PCLHeader header;

    uindex_t height = 0;
    uindex_t width = 0;

    std::vector<::pcl::PCLPointField>  fields;

    static_assert(BOOST_ENDIAN_BIG_BYTE || BOOST_ENDIAN_LITTLE_BYTE, "unable to determine system endianness");
    std::uint8_t is_bigendian = BOOST_ENDIAN_BIG_BYTE;
    uindex_t point_step = 0;
    uindex_t row_step = 0;

    std::vector<std::uint8_t> data;

    std::uint8_t is_dense = 0;

  public:
    using Ptr = shared_ptr< ::pcl::PCLPointCloud2>;
    using ConstPtr = shared_ptr<const ::pcl::PCLPointCloud2>;

    //////////////////////////////////////////////////////////////////////////
    /** \brief Inplace concatenate two pcl::PCLPointCloud2
      *
      * IFF the layout of all the fields in both the clouds is the same, this command
      * doesn't remove any fields named "_" (aka marked as skip). For comparison of field
      * names, "rgb" and "rgba" are considered equivalent
      * However, if the order and/or number of non-skip fields is different, the skip fields
      * are dropped and non-skip fields copied selectively.
      * This function returns an error if
      *   * the total number of non-skip fields is different
      *   * the non-skip field names are named differently (excluding "rbg{a}") in serial order
      *   * the endian-ness of both clouds is different
      * \param[in,out] cloud1 the first input and output point cloud dataset
      * \param[in] cloud2 the second input point cloud dataset
      * \return true if successful, false if failed (e.g., name/number of fields differs)
      */
    static bool
    concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2);

    /** \brief Concatenate two pcl::PCLPointCloud2
      * \param[in] cloud1 the first input point cloud dataset
      * \param[in] cloud2 the second input point cloud dataset
      * \param[out] cloud_out the resultant output point cloud dataset
      * \return true if successful, false if failed (e.g., name/number of fields differs)
      */
    static bool
    concatenate (const PCLPointCloud2 &cloud1,
                 const PCLPointCloud2 &cloud2,
                 PCLPointCloud2 &cloud_out)
    {
      cloud_out = cloud1;
      return concatenate(cloud_out, cloud2);
    }

    /** \brief Add a point cloud to the current cloud.
      * \param[in] rhs the cloud to add to the current cloud
      * \return the new cloud as a concatenation of the current cloud and the new given cloud
      */
    PCLPointCloud2&
    operator += (const PCLPointCloud2& rhs);

    /** \brief Add a point cloud to another cloud.
      * \param[in] rhs the cloud to add to the current cloud
      * \return the new cloud as a concatenation of the current cloud and the new given cloud
      */
    inline PCLPointCloud2
    operator + (const PCLPointCloud2& rhs)
    {
      return (PCLPointCloud2 (*this) += rhs);
    }
  }; // struct PCLPointCloud2

  using PCLPointCloud2Ptr = PCLPointCloud2::Ptr;
  using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr;

  inline std::ostream& operator<<(std::ostream& s, const  ::pcl::PCLPointCloud2 &v)
  {
    s << "header: " << std::endl;
    s << v.header;
    s << "height: ";
    s << "  " << v.height << std::endl;
    s << "width: ";
    s << "  " << v.width << std::endl;
    s << "fields[]" << std::endl;
    for (std::size_t i = 0; i < v.fields.size (); ++i)
    {
      s << "  fields[" << i << "]: ";
      s << std::endl;
      s << "    " << v.fields[i] << std::endl;
    }
    s << "is_bigendian: ";
    s << "  " << v.is_bigendian << std::endl;
    s << "point_step: ";
    s << "  " << v.point_step << std::endl;
    s << "row_step: ";
    s << "  " << v.row_step << std::endl;
    s << "data[]" << std::endl;
    for (std::size_t i = 0; i < v.data.size (); ++i)
    {
      s << "  data[" << i << "]: ";
      s << "  " << v.data[i] << std::endl;
    }
    s << "is_dense: ";
    s << "  " << v.is_dense << std::endl;

    return (s);
  }

} // namespace pcl