/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */
#ifndef opengl_COctreePointRenderer_H
#define opengl_COctreePointRenderer_H

#include <mrpt/img/TPixelCoord.h>
#include <mrpt/opengl/CBox.h>
#include <mrpt/opengl/CRenderizable.h>
#include <mrpt/opengl/CSetOfObjects.h>

#include <atomic>
#include <deque>
#include <shared_mutex>

namespace mrpt
{
namespace global_settings
{
/** Default value = 0.01 points/px^2. Affects to these classes (read their docs
 *for further details):
 *		- mrpt::opengl::CPointCloud
 *		- mrpt::opengl::CPointCloudColoured
 * \ingroup mrpt_opengl_grp
 */
void OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL(float value);
float OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL();

/** Default value = 1e5. Maximum number of elements in each octree node before
 *splitting. Affects to these classes (read their docs for further details):
 *		- mrpt::opengl::CPointCloud
 *		- mrpt::opengl::CPointCloudColoured
 * \ingroup mrpt_opengl_grp
 */
size_t OCTREE_RENDER_MAX_POINTS_PER_NODE();
void OCTREE_RENDER_MAX_POINTS_PER_NODE(size_t value);

}  // namespace global_settings

namespace opengl
{
/** Template class that implements the data structure and algorithms for
 * Octree-based efficient rendering.
 *  \sa mrpt::opengl::CPointCloud, mrpt::opengl::CPointCloudColoured,
 * https://www.mrpt.org/Efficiently_rendering_point_clouds_of_millions_of_points
 * \ingroup mrpt_opengl_grp
 */
template <class Derived>
class COctreePointRenderer
{
 public:
  /** Default ctor */
  COctreePointRenderer() = default;

  /** Copy ctor */
  COctreePointRenderer(const COctreePointRenderer&) :
      m_octree_has_to_rebuild_all(true),
      m_visible_octree_nodes(0),
      m_visible_octree_nodes_ongoing(0)
  {
  }

  enum
  {
    OCTREE_ROOT_NODE = 0
  };

 protected:
  // Helper methods in any CRTP template
  inline Derived& octree_derived() { return *static_cast<Derived*>(this); }
  inline const Derived& octree_derived() const { return *static_cast<const Derived*>(this); }

  /** Must be called at children class' render() previously to \a
   * octree_render() */
  inline void octree_assure_uptodate() const
  {
    const_cast<COctreePointRenderer<Derived>*>(this)->internal_octree_assure_uptodate();
  }

  /** Render the entire octree recursively.
   * Should be called from children's render() method.
   */
  void octree_render(const mrpt::opengl::TRenderMatrices& ri) const
  {
    m_visible_octree_nodes_ongoing = 0;

    // Stage 1: Build list of visible octrees
    m_render_queue.clear();
    m_render_queue.reserve(m_octree_nodes.size());

    mrpt::img::TPixelCoordf cr_px[8];
    float cr_z[8];
    octree_recursive_render(
        OCTREE_ROOT_NODE, ri, cr_px, cr_z,
        false /* corners are not computed for this first iteration */);

    m_visible_octree_nodes = m_visible_octree_nodes_ongoing;

    // Stage 2: Render them all
    for (size_t i = 0; i < m_render_queue.size(); i++)
    {
      const TNode& node = m_octree_nodes[m_render_queue[i].node_id];
      octree_derived().render_subset(node.all, node.pts, m_render_queue[i].render_area_sqpixels);
    }
  }

  std::optional<mrpt::math::TBoundingBox> octree_getBoundingBox() const
  {
    octree_assure_uptodate();
    if (m_octree_nodes.size() > 1 ||
        (m_octree_nodes.size() == 1 && m_octree_nodes.at(0).is_leaf &&
         (!m_octree_nodes.at(0).pts.empty() || m_octree_nodes.at(0).all)))
      return {
          {mrpt::math::TPoint3D(m_octree_nodes[0].bb_min),
           mrpt::math::TPoint3D(m_octree_nodes[0].bb_max)}
      };
    else
      return {};
  }

 private:
  /** The structure for each octree spatial node. Each node can either be a
   * leaf of has 8 children nodes.
   *  Instead of pointers, children are referenced by their indices in \a
   * m_octree_nodes
   */
  struct TNode
  {
    TNode() :
        bb_min(
            std::numeric_limits<float>::max(),
            std::numeric_limits<float>::max(),
            std::numeric_limits<float>::max()),
        bb_max(
            -std::numeric_limits<float>::max(),
            -std::numeric_limits<float>::max(),
            -std::numeric_limits<float>::max())

    {
    }

    /** true: it's a leaf and \a pts has valid indices; false: \a children
     * is valid. */
    bool is_leaf{true};

    // In all cases, the bounding_box:
    mrpt::math::TPoint3Df bb_min, bb_max;

    // Fields used if is_leaf=true
    /** Point indices in the derived class that fall into this node. */
    std::vector<size_t> pts;
    /** true: All elements in the reference object; false: only those in \a
     * pts */
    bool all{false};

    // Fields used if is_leaf=false
    /** [is_leaf=false] The center of the node, whose coordinates are used
     * to decide between the 8 children nodes. */
    mrpt::math::TPoint3Df center;
    /** [is_leaf=false] The indices in \a m_octree_nodes of the 8 children.
     */
    size_t child_id[8] = {0, 0, 0, 0, 0, 0, 0, 0};

    /** update bounding box with a new point: */
    inline void update_bb(const mrpt::math::TPoint3Df& p)
    {
      mrpt::keep_min(bb_min.x, p.x);
      mrpt::keep_min(bb_min.y, p.y);
      mrpt::keep_min(bb_min.z, p.z);
      mrpt::keep_max(bb_max.x, p.x);
      mrpt::keep_max(bb_max.y, p.y);
      mrpt::keep_max(bb_max.z, p.z);
    }

    inline float getCornerX(int i) const { return (i & 0x01) == 0 ? bb_min.x : bb_max.x; }
    inline float getCornerY(int i) const { return (i & 0x02) == 0 ? bb_min.y : bb_max.y; }
    inline float getCornerZ(int i) const { return (i & 0x04) == 0 ? bb_min.z : bb_max.z; }

    void setBBFromOrderInParent(const TNode& parent, int my_child_index)
    {
      // Coordinate signs are relative to the parent center (split point):
      switch (my_child_index)
      {
        case 0:  // x-, y-, z-
          bb_min = parent.bb_min;
          bb_max = parent.center;
          break;
        case 1:  // x+, y-, z-
          bb_min.x = parent.center.x;
          bb_max.x = parent.bb_max.x;
          bb_min.y = parent.bb_min.y;
          bb_max.y = parent.center.y;
          bb_min.z = parent.bb_min.z;
          bb_max.z = parent.center.z;
          break;
        case 2:  // x-, y+, z-
          bb_min.x = parent.bb_min.x;
          bb_max.x = parent.center.x;
          bb_min.y = parent.center.y;
          bb_max.y = parent.bb_max.y;
          bb_min.z = parent.bb_min.z;
          bb_max.z = parent.center.z;
          break;
        case 3:  // x+, y+, z-
          bb_min.x = parent.center.x;
          bb_max.x = parent.bb_max.x;
          bb_min.y = parent.center.y;
          bb_max.y = parent.bb_max.y;
          bb_min.z = parent.bb_min.z;
          bb_max.z = parent.center.z;
          break;
        case 4:  // x-, y-, z+
          bb_min.x = parent.bb_min.x;
          bb_max.x = parent.center.x;
          bb_min.y = parent.bb_min.y;
          bb_max.y = parent.center.y;
          bb_min.z = parent.center.z;
          bb_max.z = parent.bb_max.z;
          break;
        case 5:  // x+, y-, z+
          bb_min.x = parent.center.x;
          bb_max.x = parent.bb_max.x;
          bb_min.y = parent.bb_min.y;
          bb_max.y = parent.center.y;
          bb_min.z = parent.center.z;
          bb_max.z = parent.bb_max.z;
          break;
        case 6:  // x-, y+, z+
          bb_min.x = parent.bb_min.x;
          bb_max.x = parent.center.x;
          bb_min.y = parent.center.y;
          bb_max.y = parent.bb_max.y;
          bb_min.z = parent.center.z;
          bb_max.z = parent.bb_max.z;
          break;
        case 7:  // x+, y+, z+
          bb_min = parent.center;
          bb_max = parent.bb_max;
          break;
        default:
          throw std::runtime_error("my_child_index!=[0,7]");
      }
    }

   public:
  };

  struct TRenderQueueElement
  {
    TRenderQueueElement(size_t id, float area_sq) : node_id(id), render_area_sqpixels(area_sq) {}

    /** The node ID to render */
    size_t node_id;
    /** The approximate size of the octree on the screen (squared pixels).
     */
    float render_area_sqpixels;
  };
  /** The list of elements that really are visible and will be rendered. */
  mutable std::vector<TRenderQueueElement> m_render_queue;

  bool m_octree_has_to_rebuild_all = true;
  /** First one [0] is always the root node */
  std::deque<TNode> m_octree_nodes;

  // Counters of visible octrees for each render:
  mutable std::atomic<size_t> m_visible_octree_nodes = 0;
  mutable size_t m_visible_octree_nodes_ongoing = 0;

  std::shared_mutex m_ptsOctreeMtx;

  /** Render a given node. */
  void octree_recursive_render(
      size_t node_idx,
      const mrpt::opengl::TRenderMatrices& ri,
      mrpt::img::TPixelCoordf cr_px[8],
      float cr_z[8],
      bool corners_are_all_computed = true,
      bool trust_me_youre_visible = false,
      float approx_area_sqpixels = 0) const
  {
    const TNode& node = m_octree_nodes[node_idx];

    if (!corners_are_all_computed)
    {
      for (int i = 0; i < 8; i++)
      {
        // project point:
        ri.projectPointPixels(
            node.getCornerX(i), node.getCornerY(i), node.getCornerZ(i), cr_px[i].x, cr_px[i].y,
            cr_z[i]);
      }
    }

    mrpt::img::TPixelCoordf px_min(
        std::numeric_limits<float>::max(), std::numeric_limits<float>::max()),
        px_max(-std::numeric_limits<float>::max(), -std::numeric_limits<float>::max());
    if (!trust_me_youre_visible)
    {
      // Keep the on-screen bounding box of this node:
      for (int i = 0; i < 8; i++)
      {
        mrpt::keep_min(px_min.x, cr_px[i].x);
        mrpt::keep_min(px_min.y, cr_px[i].y);
        mrpt::keep_max(px_max.x, cr_px[i].x);
        mrpt::keep_max(px_max.y, cr_px[i].y);
      }

      const bool any_cr_zs_neg =
          (cr_z[0] < 0 || cr_z[1] < 0 || cr_z[2] < 0 || cr_z[3] < 0 || cr_z[4] < 0 || cr_z[5] < 0 ||
           cr_z[6] < 0 || cr_z[7] < 0);
      const bool any_cr_zs_pos =
          (cr_z[0] > 0 || cr_z[1] > 0 || cr_z[2] > 0 || cr_z[3] > 0 || cr_z[4] > 0 || cr_z[5] > 0 ||
           cr_z[6] > 0 || cr_z[7] > 0);
      const bool box_crosses_image_plane = any_cr_zs_pos && any_cr_zs_neg;

      // If all 8 corners are way out of the screen (and all "cr_z" have
      // the same sign),
      // this node and all the children are not visible:
      if (!box_crosses_image_plane &&
          (px_min.x >= ri.viewport_width || px_min.y >= ri.viewport_height || px_max.x < 0 ||
           px_max.y < 0))
        return;  // Not visible
    }

    // Check if the node has points and is visible:
    if (node.is_leaf)
    {  // Render this leaf node:
      if (node.all || !node.pts.empty())
      {
        // If we are here, it seems at least a part of the Box is
        // visible:
        m_visible_octree_nodes_ongoing++;

        float render_area_sqpixels =
            trust_me_youre_visible ? approx_area_sqpixels
                                   : std::abs(px_min.x - px_max.x) * std::abs(px_min.y - px_max.y);
        render_area_sqpixels = std::max(1.0f, render_area_sqpixels);

        // OK: Add to list of rendering-pending:
        m_render_queue.push_back(TRenderQueueElement(node_idx, render_area_sqpixels));
      }
    }
    else
    {  // Render children nodes:
      // If ALL my 8 corners are within the screen, tell our children that
      // they
      //  won't need to compute anymore, since all of them and their
      //  children are visible as well:
      bool children_are_all_visible_for_sure = true;

      if (!trust_me_youre_visible)  // Trust my parent... otherwise:
      {
        for (int i = 0; i < 8; i++)
        {
          if (!(cr_px[i].x >= 0 && cr_px[i].y >= 0 && cr_px[i].x < ri.viewport_width &&
                cr_px[i].y < ri.viewport_height))
          {
            children_are_all_visible_for_sure = false;
            break;
          }
        }
      }

      // If all children are visible, it's easy:
      if (children_are_all_visible_for_sure)
      {
        mrpt::img::TPixelCoordf child_cr_px[8];  // No need to initialize
        float child_cr_z[8];                     // No need to initialize

        // Approximate area of the children nodes:
        const float approx_child_area =
            trust_me_youre_visible
                ? approx_area_sqpixels / 8.0f
                : std::abs(px_min.x - px_max.x) * std::abs(px_min.y - px_max.y) / 8.0f;

        for (int i = 0; i < 8; i++)
          this->octree_recursive_render(
              node.child_id[i], ri, child_cr_px, child_cr_z, true, true, approx_child_area);
      }
      else
      {
#ifdef __clang__
#pragma clang diagnostic push  // clang complains about unused vars (becase it
// doesn't realize of the macros?)
#pragma clang diagnostic ignored "-Wunused-variable"
#endif

        // Precompute the 19 (3*9-8) intermediary points so children
        // don't have to compute them several times:
        const mrpt::math::TPoint3Df p_Xm_Ym_Zm(node.bb_min.x, node.bb_min.y,
                                               node.bb_min.z);  // 0
        const mrpt::math::TPoint3Df p_X0_Ym_Zm(node.center.x, node.bb_min.y, node.bb_min.z);
        const mrpt::math::TPoint3Df p_Xp_Ym_Zm(node.bb_max.x, node.bb_min.y,
                                               node.bb_min.z);  // 1
        const mrpt::math::TPoint3Df p_Xm_Y0_Zm(node.bb_min.x, node.center.y, node.bb_min.z);
        const mrpt::math::TPoint3Df p_X0_Y0_Zm(node.center.x, node.center.y, node.bb_min.z);
        const mrpt::math::TPoint3Df p_Xp_Y0_Zm(node.bb_max.x, node.center.y, node.bb_min.z);
        const mrpt::math::TPoint3Df p_Xm_Yp_Zm(node.bb_min.x, node.bb_max.y,
                                               node.bb_min.z);  // 2
        const mrpt::math::TPoint3Df p_X0_Yp_Zm(node.center.x, node.bb_max.y, node.bb_min.z);
        const mrpt::math::TPoint3Df p_Xp_Yp_Zm(node.bb_max.x, node.bb_max.y,
                                               node.bb_min.z);  // 3

        const mrpt::math::TPoint3Df p_Xm_Ym_Z0(node.bb_min.x, node.bb_min.y, node.center.z);
        const mrpt::math::TPoint3Df p_X0_Ym_Z0(node.center.x, node.bb_min.y, node.center.z);
        const mrpt::math::TPoint3Df p_Xp_Ym_Z0(node.bb_max.x, node.bb_min.y, node.center.z);
        const mrpt::math::TPoint3Df p_Xm_Y0_Z0(node.bb_min.x, node.center.y, node.center.z);
        const mrpt::math::TPoint3Df p_X0_Y0_Z0(node.center.x, node.center.y, node.center.z);
        const mrpt::math::TPoint3Df p_Xp_Y0_Z0(node.bb_max.x, node.center.y, node.center.z);
        const mrpt::math::TPoint3Df p_Xm_Yp_Z0(node.bb_min.x, node.bb_max.y, node.center.z);
        const mrpt::math::TPoint3Df p_X0_Yp_Z0(node.center.x, node.bb_max.y, node.center.z);
        const mrpt::math::TPoint3Df p_Xp_Yp_Z0(node.bb_max.x, node.bb_max.y, node.center.z);

        const mrpt::math::TPoint3Df p_Xm_Ym_Zp(node.bb_min.x, node.bb_min.y,
                                               node.bb_max.z);  // 4
        const mrpt::math::TPoint3Df p_X0_Ym_Zp(node.center.x, node.bb_min.y, node.bb_max.z);
        const mrpt::math::TPoint3Df p_Xp_Ym_Zp(node.bb_min.x, node.bb_min.y,
                                               node.bb_max.z);  // 5
        const mrpt::math::TPoint3Df p_Xm_Y0_Zp(node.bb_min.x, node.center.y, node.bb_max.z);
        const mrpt::math::TPoint3Df p_X0_Y0_Zp(node.center.x, node.center.y, node.bb_max.z);
        const mrpt::math::TPoint3Df p_Xp_Y0_Zp(node.bb_max.x, node.center.y, node.bb_max.z);
        const mrpt::math::TPoint3Df p_Xm_Yp_Zp(node.bb_min.x, node.bb_max.y,
                                               node.bb_max.z);  // 6
        const mrpt::math::TPoint3Df p_X0_Yp_Zp(node.center.x, node.bb_max.y, node.bb_max.z);
        const mrpt::math::TPoint3Df p_Xp_Yp_Zp(node.bb_max.x, node.bb_max.y,
                                               node.bb_max.z);  // 7

// Project all these points:
#define PROJ_SUB_NODE(POSTFIX)                                                     \
  mrpt::img::TPixelCoordf px_##POSTFIX;                                            \
  float depth_##POSTFIX;                                                           \
  ri.projectPointPixels(                                                           \
      p_##POSTFIX.x, p_##POSTFIX.y, p_##POSTFIX.z, px_##POSTFIX.x, px_##POSTFIX.y, \
      depth_##POSTFIX);

#define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX)           \
  const mrpt::img::TPixelCoordf px_##POSTFIX = cr_px[INDEX]; \
  float depth_##POSTFIX = cr_z[INDEX];

        PROJ_SUB_NODE_ALREADY_DONE(0, Xm_Ym_Zm)
        PROJ_SUB_NODE(X0_Ym_Zm)
        PROJ_SUB_NODE_ALREADY_DONE(1, Xp_Ym_Zm)

        PROJ_SUB_NODE(Xm_Y0_Zm)
        PROJ_SUB_NODE(X0_Y0_Zm)
        PROJ_SUB_NODE(Xp_Y0_Zm)

        PROJ_SUB_NODE_ALREADY_DONE(2, Xm_Yp_Zm)
        PROJ_SUB_NODE(X0_Yp_Zm)
        PROJ_SUB_NODE_ALREADY_DONE(3, Xp_Yp_Zm)

        PROJ_SUB_NODE(Xm_Ym_Z0)
        PROJ_SUB_NODE(X0_Ym_Z0)
        PROJ_SUB_NODE(Xp_Ym_Z0)
        PROJ_SUB_NODE(Xm_Y0_Z0)
        PROJ_SUB_NODE(X0_Y0_Z0)
        PROJ_SUB_NODE(Xp_Y0_Z0)
        PROJ_SUB_NODE(Xm_Yp_Z0)
        PROJ_SUB_NODE(X0_Yp_Z0)
        PROJ_SUB_NODE(Xp_Yp_Z0)

        PROJ_SUB_NODE_ALREADY_DONE(4, Xm_Ym_Zp)
        PROJ_SUB_NODE(X0_Ym_Zp)
        PROJ_SUB_NODE_ALREADY_DONE(5, Xp_Ym_Zp)

        PROJ_SUB_NODE(Xm_Y0_Zp)
        PROJ_SUB_NODE(X0_Y0_Zp)
        PROJ_SUB_NODE(Xp_Y0_Zp)

        PROJ_SUB_NODE_ALREADY_DONE(6, Xm_Yp_Zp)
        PROJ_SUB_NODE(X0_Yp_Zp)
        PROJ_SUB_NODE_ALREADY_DONE(7, Xp_Yp_Zp)

// Recursive call children nodes:
#define DO_RECURSE_CHILD(INDEX, SEQ0, SEQ1, SEQ2, SEQ3, SEQ4, SEQ5, SEQ6, SEQ7)            \
  {                                                                                        \
    mrpt::img::TPixelCoordf child_cr_px[8] = {px_##SEQ0, px_##SEQ1, px_##SEQ2, px_##SEQ3,  \
                                              px_##SEQ4, px_##SEQ5, px_##SEQ6, px_##SEQ7}; \
    float child_cr_z[8] = {depth_##SEQ0, depth_##SEQ1, depth_##SEQ2, depth_##SEQ3,         \
                           depth_##SEQ4, depth_##SEQ5, depth_##SEQ6, depth_##SEQ7};        \
    this->octree_recursive_render(node.child_id[INDEX], ri, child_cr_px, child_cr_z);      \
  }

        //                     0         1         2         3
        //                     4         5        6         7
        DO_RECURSE_CHILD(
            0, Xm_Ym_Zm, X0_Ym_Zm, Xm_Y0_Zm, X0_Y0_Zm, Xm_Ym_Z0, X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0)
        DO_RECURSE_CHILD(
            1, X0_Ym_Zm, Xp_Ym_Zm, X0_Y0_Zm, Xp_Y0_Zm, X0_Ym_Z0, Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0)
        DO_RECURSE_CHILD(
            2, Xm_Y0_Zm, X0_Y0_Zm, Xm_Yp_Zm, X0_Yp_Zm, Xm_Y0_Z0, X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0)
        DO_RECURSE_CHILD(
            3, X0_Y0_Zm, Xp_Y0_Zm, X0_Yp_Zm, Xp_Yp_Zm, X0_Y0_Z0, Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0)
        DO_RECURSE_CHILD(
            4, Xm_Ym_Z0, X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0, Xm_Ym_Zp, X0_Ym_Zp, Xm_Y0_Zp, X0_Y0_Zp)
        DO_RECURSE_CHILD(
            5, X0_Ym_Z0, Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0, X0_Ym_Zp, Xp_Ym_Zp, X0_Y0_Zp, Xp_Y0_Zp)
        DO_RECURSE_CHILD(
            6, Xm_Y0_Z0, X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0, Xm_Y0_Zp, X0_Y0_Zp, Xm_Yp_Zp, X0_Yp_Zp)
        DO_RECURSE_CHILD(
            7, X0_Y0_Z0, Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0, X0_Y0_Zp, Xp_Y0_Zp, X0_Yp_Zp, Xp_Yp_Zp)
#undef DO_RECURSE_CHILD
#undef PROJ_SUB_NODE
#undef PROJ_SUB_NODE_ALREADY_DONE

#ifdef __clang__
#pragma clang diagnostic pop
#endif
      }  // end "children_are_all_visible_for_sure"=false
    }
  }

  // The actual implementation (and non-const version) of
  // octree_assure_uptodate()
  void internal_octree_assure_uptodate()
  {
    std::shared_lock<std::shared_mutex> readLock(m_ptsOctreeMtx);
    if (!m_octree_has_to_rebuild_all) return;

    readLock.unlock();

    std::unique_lock<std::shared_mutex> writeLock(m_ptsOctreeMtx);
    m_octree_has_to_rebuild_all = false;

    // Reset list of nodes:
    m_octree_nodes.assign(1, TNode());

    // recursive decide:
    internal_recursive_split(OCTREE_ROOT_NODE, true);
  }

  // Check the node "node_id" and create its children if needed, by looking at
  // its list
  //  of elements (or all derived object's elements if "all_pts"=true, which
  //  will only happen
  //  for the root node)
  void internal_recursive_split(const size_t node_id, const bool all_pts = false)
  {
    TNode& node = m_octree_nodes[node_id];
    const size_t N = all_pts ? octree_derived().size_unprotected() : node.pts.size();

    const bool has_to_compute_bb = (node_id == OCTREE_ROOT_NODE);

    if (N <= mrpt::global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE())
    {
      // No need to split this node:
      node.is_leaf = true;
      node.all = all_pts;

      // Update bounding-box:
      if (has_to_compute_bb)
      {
        if (all_pts)
          for (size_t i = 0; i < N; i++) node.update_bb(octree_derived().getPoint3Df(i));
        else
          for (size_t i = 0; i < N; i++) node.update_bb(octree_derived().getPoint3Df(node.pts[i]));
      }
    }
    else
    {
      // We have to split the node.
      // Compute the mean of all elements:
      mrpt::math::TPoint3Df mean(0, 0, 0);
      if (all_pts)
        for (size_t i = 0; i < N; i++)
        {
          const auto& p = octree_derived().getPoint3Df(i);
          mean += p;
          if (has_to_compute_bb) node.update_bb(p);
        }
      else
        for (size_t i = 0; i < N; i++)
        {
          const auto& p = octree_derived().getPoint3Df(node.pts[i]);
          mean += p;
          if (has_to_compute_bb) node.update_bb(p);
        }

      // Save my split point:
      node.is_leaf = false;
      node.center = mean * (1.0f / N);

      // Allocate my 8 children structs
      const size_t children_idx_base = m_octree_nodes.size();
      m_octree_nodes.resize(children_idx_base + 8);
      for (int i = 0; i < 8; i++) node.child_id[i] = children_idx_base + i;

      // Set the bounding-boxes of my children (we already know them):
      for (int i = 0; i < 8; i++)
        m_octree_nodes[children_idx_base + i].setBBFromOrderInParent(node, i);

      // Divide elements among children:
      const mrpt::math::TPoint3Df& c = node.center;  // to make notation clearer
      for (size_t j = 0; j < N; j++)
      {
        const size_t i = all_pts ? j : node.pts[j];
        const mrpt::math::TPoint3Df p = octree_derived().getPoint3Df(i);
        if (p.z < c.z)
        {
          if (p.y < c.y)
          {
            if (p.x < c.x)
              m_octree_nodes[children_idx_base + 0].pts.push_back(i);
            else
              m_octree_nodes[children_idx_base + 1].pts.push_back(i);
          }
          else
          {
            if (p.x < c.x)
              m_octree_nodes[children_idx_base + 2].pts.push_back(i);
            else
              m_octree_nodes[children_idx_base + 3].pts.push_back(i);
          }
        }
        else
        {
          if (p.y < c.y)
          {
            if (p.x < c.x)
              m_octree_nodes[children_idx_base + 4].pts.push_back(i);
            else
              m_octree_nodes[children_idx_base + 5].pts.push_back(i);
          }
          else
          {
            if (p.x < c.x)
              m_octree_nodes[children_idx_base + 6].pts.push_back(i);
            else
              m_octree_nodes[children_idx_base + 7].pts.push_back(i);
          }
        }
      }

      // Clear list of elements (they're now in our children):
      {
        std::vector<size_t> emptyVec;
        node.pts.swap(emptyVec);  // This is THE way of really clearing
                                  // a std::vector
      }

      // Recursive call on children:
      for (int i = 0; i < 8; i++) internal_recursive_split(node.child_id[i]);
    }
  }  // end of internal_recursive_split

 public:
  /** Return the number of octree nodes (all of them, including the empty
   * ones) \sa octree_get_nonempty_node_count */
  size_t octree_get_node_count() const { return m_octree_nodes.size(); }
  /** Return the number of visible octree nodes in the last render event. */
  size_t octree_get_visible_nodes() const { return m_visible_octree_nodes; }
  /** Called from the derived class (or the user) to indicate we have/want to
   * rebuild the entire node tree (for example, after modifying the point
   * cloud or any global octree parameter) */
  inline void octree_mark_as_outdated()
  {
    std::unique_lock<std::shared_mutex> writeLock(m_ptsOctreeMtx);
    m_octree_has_to_rebuild_all = true;
  }

  /** Returns a graphical representation of all the bounding boxes of the
   * octree (leaf) nodes.
   * \param[in] draw_solid_boxes If false, will draw solid boxes of color \a
   * lines_color. Otherwise, wireframe boxes will be drawn.
   */
  void octree_get_graphics_boundingboxes(
      mrpt::opengl::CSetOfObjects& gl_bb,
      const float lines_width = 1,
      const mrpt::img::TColorf& lines_color = mrpt::img::TColorf(1, 1, 1),
      const bool draw_solid_boxes = false) const
  {
    octree_assure_uptodate();
    gl_bb.clear();
    for (size_t i = 0; i < m_octree_nodes.size(); i++)
    {
      const TNode& node = m_octree_nodes[i];
      if (!node.is_leaf) continue;
      mrpt::opengl::CBox::Ptr gl_box = mrpt::opengl::CBox::Create();
      gl_box->setBoxCorners(mrpt::math::TPoint3D(node.bb_min), mrpt::math::TPoint3D(node.bb_max));
      gl_box->setColor(lines_color);
      gl_box->setLineWidth(lines_width);
      gl_box->setWireframe(!draw_solid_boxes);
      gl_bb.insert(gl_box);
    }
  }

  /** Used for debug only */
  void octree_debug_dump_tree(std::ostream& o) const
  {
    o << "Octree nodes: " << m_octree_nodes.size() << std::endl;
    size_t total_elements = 0;
    for (size_t i = 0; i < m_octree_nodes.size(); i++)
    {
      const TNode& node = m_octree_nodes[i];

      o << "Node #" << i << ": ";
      if (node.is_leaf)
      {
        o << "leaf, ";
        if (node.all)
        {
          o << "(all)\n";
          total_elements += octree_derived().size_unprotected();
        }
        else
        {
          o << node.pts.size() << " elements; ";
          total_elements += node.pts.size();
        }
      }
      else
      {
        o << "parent, center=(" << node.center.x << "," << node.center.y << "," << node.center.z
          << "), children: " << node.child_id[0] << "," << node.child_id[1] << ","
          << node.child_id[2] << "," << node.child_id[3] << "," << node.child_id[4] << ","
          << node.child_id[5] << "," << node.child_id[6] << "," << node.child_id[7] << "; ";
      }
      o << " bb: (" << node.bb_min.x << "," << node.bb_min.y << "," << node.bb_min.z << ")-("
        << node.bb_max.x << "," << node.bb_max.y << "," << node.bb_max.z << ")\n";
    }
    o << "Total elements in all nodes: " << total_elements << std::endl;
  }  // end of octree_debug_dump_tree

};  // namespace opengl

}  // namespace opengl
}  // namespace mrpt
#endif
