/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/config.h>
#include <mrpt/opengl/pointcloud_adapters.h>

// NOTE: Only include this file if you have PCL installed in your system
//        and do it only after including MRPT headers...

// Make sure the essential PCL headers are included:
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace mrpt::opengl
{
/** Specialization
 * mrpt::opengl::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZ> > for an XYZ
 * point cloud (without RGB) \ingroup mrpt_adapters_grp */
template <>
class PointCloudAdapter<pcl::PointCloud<pcl::PointXYZ>>
{
 private:
  pcl::PointCloud<pcl::PointXYZ>& m_obj;

 public:
  /** The type of each point XYZ coordinates */
  using coords_t = float;
  /** Has any color RGB info? */
  static constexpr bool HAS_RGB = false;
  /** Has native RGB info (as floats)? */
  static constexpr bool HAS_RGBf = false;
  /** Has native RGB info (as uint8_t)? */
  static constexpr bool HAS_RGBu8 = false;

  /** Constructor (accept a const ref for convenience) */
  inline PointCloudAdapter(const pcl::PointCloud<pcl::PointXYZ>& obj) :
      m_obj(*const_cast<pcl::PointCloud<pcl::PointXYZ>*>(&obj))
  {
  }
  /** Get number of points */
  inline size_t size() const { return m_obj.points.size(); }
  /** Set number of points (to uninitialized values) */
  inline void resize(size_t N) { m_obj.points.resize(N); }
  /** Set height and width (for organized) */
  inline void setDimensions(size_t height, size_t width)
  {
    m_obj.height = height;
    m_obj.width = width;
  }
  /** Get XYZ coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ(size_t idx, T& x, T& y, T& z) const
  {
    const pcl::PointXYZ& p = m_obj.points[idx];
    x = p.x;
    y = p.y;
    z = p.z;
  }
  /** Set XYZ coordinates of i'th point */
  inline void setPointXYZ(size_t idx, const coords_t x, const coords_t y, const coords_t z)
  {
    pcl::PointXYZ& p = m_obj.points[idx];
    p.x = x;
    p.y = y;
    p.z = z;
  }

  /** Set Invalid Point */
  inline void setInvalidPoint(size_t idx)
  {
    pcl::PointXYZ& p = m_obj.points[idx];
    p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
  }
};  // end of mrpt::opengl::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZ> >

/** Specialization
 * mrpt::opengl::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGB> > for an
 * XYZ point cloud with RGB  \ingroup mrpt_adapters_grp */
template <>
class PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGB>>
{
 private:
  pcl::PointCloud<pcl::PointXYZRGB>& m_obj;

 public:
  /** The type of each point XYZ coordinates */
  using coords_t = float;
  /** Has any color RGB info? */
  static constexpr bool HAS_RGB = true;
  /** Has native RGB info (as floats)? */
  static constexpr bool HAS_RGBf = false;
  /** Has native RGB info (as uint8_t)? */
  static constexpr bool HAS_RGBu8 = true;

  /** Constructor (accept a const ref for convenience) */
  inline PointCloudAdapter(const pcl::PointCloud<pcl::PointXYZRGB>& obj) :
      m_obj(*const_cast<pcl::PointCloud<pcl::PointXYZRGB>*>(&obj))
  {
  }
  /** Get number of points */
  inline size_t size() const { return m_obj.points.size(); }
  /** Set number of points (to uninitialized values) */
  inline void resize(size_t N) { m_obj.points.resize(N); }
  /** Set height and width (for organized) */
  inline void setDimensions(size_t height, size_t width)
  {
    m_obj.height = height;
    m_obj.width = width;
  }

  /** Get XYZ coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ(size_t idx, T& x, T& y, T& z) const
  {
    const pcl::PointXYZRGB& p = m_obj.points[idx];
    x = p.x;
    y = p.y;
    z = p.z;
  }
  /** Set XYZ coordinates of i'th point */
  inline void setPointXYZ(size_t idx, const coords_t x, const coords_t y, const coords_t z)
  {
    pcl::PointXYZRGB& p = m_obj.points[idx];
    p.x = x;
    p.y = y;
    p.z = z;
    p.r = p.g = p.b = 255;
  }

  /** Get XYZ_RGBf coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ_RGBf(size_t idx, T& x, T& y, T& z, float& r, float& g, float& b) const
  {
    const pcl::PointXYZRGB& p = m_obj.points[idx];
    x = p.x;
    y = p.y;
    z = p.z;
    r = p.r / 255.f;
    g = p.g / 255.f;
    b = p.b / 255.f;
  }
  /** Set XYZ_RGBf coordinates of i'th point */
  inline void setPointXYZ_RGBf(
      size_t idx,
      const coords_t x,
      const coords_t y,
      const coords_t z,
      const float r,
      const float g,
      const float b)
  {
    pcl::PointXYZRGB& p = m_obj.points[idx];
    p.x = x;
    p.y = y;
    p.z = z;
    p.r = r * 255;
    p.g = g * 255;
    p.b = b * 255;
  }

  /** Get XYZ_RGBu8 coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ_RGBu8(
      size_t idx, T& x, T& y, T& z, uint8_t& r, uint8_t& g, uint8_t& b) const
  {
    const pcl::PointXYZRGB& p = m_obj.points[idx];
    x = p.x;
    y = p.y;
    z = p.z;
    r = p.r;
    g = p.g;
    b = p.b;
  }
  /** Set XYZ_RGBu8 coordinates of i'th point */
  inline void setPointXYZ_RGBu8(
      size_t idx,
      const coords_t x,
      const coords_t y,
      const coords_t z,
      const uint8_t r,
      const uint8_t g,
      const uint8_t b)
  {
    pcl::PointXYZRGB& p = m_obj.points[idx];
    p.x = x;
    p.y = y;
    p.z = z;
    p.r = r;
    p.g = g;
    p.b = b;
  }

  /** Get RGBf color of i'th point */
  inline void getPointRGBf(size_t idx, float& r, float& g, float& b) const
  {
    const pcl::PointXYZRGB& p = m_obj.points[idx];
    r = p.r / 255.f;
    g = p.g / 255.f;
    b = p.b / 255.f;
  }
  /** Set XYZ_RGBf coordinates of i'th point */
  inline void setPointRGBf(size_t idx, const float r, const float g, const float b)
  {
    pcl::PointXYZRGB& p = m_obj.points[idx];
    p.r = r * 255;
    p.g = g * 255;
    p.b = b * 255;
  }

  /** Get RGBu8 color of i'th point */
  inline void getPointRGBu8(size_t idx, uint8_t& r, uint8_t& g, uint8_t& b) const
  {
    const pcl::PointXYZRGB& p = m_obj.points[idx];
    r = p.r;
    g = p.g;
    b = p.b;
  }
  /** Set RGBu8 coordinates of i'th point */
  inline void setPointRGBu8(size_t idx, const uint8_t r, const uint8_t g, const uint8_t b)
  {
    pcl::PointXYZRGB& p = m_obj.points[idx];
    p.r = r;
    p.g = g;
    p.b = b;
  }

};  // end of mrpt::opengl::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGB>
// >

/** Specialization
 * mrpt::opengl::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGBA> > for an
 * XYZ point cloud with RGB  \ingroup mrpt_adapters_grp */
template <>
class PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGBA>>
{
 private:
  pcl::PointCloud<pcl::PointXYZRGBA>& m_obj;

 public:
  /** The type of each point XYZ coordinates */
  using coords_t = float;
  /** Has any color RGB info? */
  static constexpr bool HAS_RGB = true;
  /** Has native RGB info (as floats)? */
  static constexpr bool HAS_RGBf = false;
  /** Has native RGB info (as uint8_t)? */
  static constexpr bool HAS_RGBu8 = true;

  /** Constructor (accept a const ref for convenience) */
  inline PointCloudAdapter(const pcl::PointCloud<pcl::PointXYZRGBA>& obj) :
      m_obj(*const_cast<pcl::PointCloud<pcl::PointXYZRGBA>*>(&obj))
  {
  }
  /** Get number of points */
  inline size_t size() const { return m_obj.points.size(); }
  /** Set number of points (to uninitialized values) */
  inline void resize(size_t N) { m_obj.points.resize(N); }
  /** Set height and width (for organized) */
  inline void setDimensions(size_t height, size_t width)
  {
    m_obj.height = height;
    m_obj.width = width;
  }

  /** Get XYZ coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ(size_t idx, T& x, T& y, T& z) const
  {
    const pcl::PointXYZRGBA& p = m_obj.points[idx];
    x = p.x;
    y = p.y;
    z = p.z;
  }
  /** Set XYZ coordinates of i'th point */
  inline void setPointXYZ(size_t idx, const coords_t x, const coords_t y, const coords_t z)
  {
    pcl::PointXYZRGBA& p = m_obj.points[idx];
    p.x = x;
    p.y = y;
    p.z = z;
    p.r = p.g = p.b = 255;
  }

  /** Set Invalid Point */
  inline void setInvalidPoint(size_t idx)
  {
    pcl::PointXYZRGBA& p = m_obj.points[idx];
    p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
  }

  /** Get XYZ_RGBf coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ_RGBf(size_t idx, T& x, T& y, T& z, float& r, float& g, float& b) const
  {
    const pcl::PointXYZRGBA& p = m_obj.points[idx];
    x = p.x;
    y = p.y;
    z = p.z;
    r = p.r / 255.f;
    g = p.g / 255.f;
    b = p.b / 255.f;
  }
  /** Set XYZ_RGBf coordinates of i'th point */
  inline void setPointXYZ_RGBf(
      size_t idx,
      const coords_t x,
      const coords_t y,
      const coords_t z,
      const float r,
      const float g,
      const float b)
  {
    pcl::PointXYZRGBA& p = m_obj.points[idx];
    p.x = x;
    p.y = y;
    p.z = z;
    p.r = r * 255;
    p.g = g * 255;
    p.b = b * 255;
  }

  /** Get XYZ_RGBu8 coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ_RGBu8(
      size_t idx, T& x, T& y, T& z, uint8_t& r, uint8_t& g, uint8_t& b) const
  {
    const pcl::PointXYZRGBA& p = m_obj.points[idx];
    x = p.x;
    y = p.y;
    z = p.z;
    r = p.r;
    g = p.g;
    b = p.b;
  }
  /** Set XYZ_RGBu8 coordinates of i'th point */
  inline void setPointXYZ_RGBu8(
      size_t idx,
      const coords_t x,
      const coords_t y,
      const coords_t z,
      const uint8_t r,
      const uint8_t g,
      const uint8_t b)
  {
    pcl::PointXYZRGBA& p = m_obj.points[idx];
    p.x = x;
    p.y = y;
    p.z = z;
    p.r = r;
    p.g = g;
    p.b = b;
  }

  /** Get RGBf color of i'th point */
  inline void getPointRGBf(size_t idx, float& r, float& g, float& b) const
  {
    const pcl::PointXYZRGBA& p = m_obj.points[idx];
    r = p.r / 255.f;
    g = p.g / 255.f;
    b = p.b / 255.f;
  }
  /** Set XYZ_RGBf coordinates of i'th point */
  inline void setPointRGBf(size_t idx, const float r, const float g, const float b)
  {
    pcl::PointXYZRGBA& p = m_obj.points[idx];
    p.r = r * 255;
    p.g = g * 255;
    p.b = b * 255;
  }

  /** Get RGBu8 color of i'th point */
  inline void getPointRGBu8(size_t idx, uint8_t& r, uint8_t& g, uint8_t& b) const
  {
    const pcl::PointXYZRGBA& p = m_obj.points[idx];
    r = p.r;
    g = p.g;
    b = p.b;
  }
  /** Set RGBu8 coordinates of i'th point */
  inline void setPointRGBu8(size_t idx, const uint8_t r, const uint8_t g, const uint8_t b)
  {
    pcl::PointXYZRGBA& p = m_obj.points[idx];
    p.r = r;
    p.g = g;
    p.b = b;
  }

};  // end of mrpt::opengl::PointCloudAdapter<pcl::PointCloud<pcl::PointXYZRGBA>
// >
}  // namespace mrpt::opengl
