/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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
 *spliting. 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
