/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 */

#ifndef PCL_OCTREE_TREE_2BUF_BASE_H
#define PCL_OCTREE_TREE_2BUF_BASE_H

#include <vector>

#include <pcl/octree/octree_nodes.h>
#include <pcl/octree/octree_container.h>
#include <pcl/octree/octree_key.h>
#include <pcl/octree/octree_iterator.h>


namespace pcl
{
  namespace octree
  {

    template<typename ContainerT>
    class BufferedBranchNode : public OctreeNode
    {

      public:
        /** \brief Empty constructor. */
        BufferedBranchNode () : OctreeNode()
        {
          reset ();
        }

        /** \brief Copy constructor. */
        BufferedBranchNode (const BufferedBranchNode& source) : OctreeNode()
        {
          *this = source;
        }

        /** \brief Copy operator. */
        inline BufferedBranchNode&
        operator = (const BufferedBranchNode &source_arg)
        {

          unsigned char i, b;

          memset (child_node_array_, 0, sizeof(child_node_array_));

          for (b = 0; b < 2; ++b)
            for (i = 0; i < 8; ++i)
              if (source_arg.child_node_array_[b][i])
                child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy ();

          return (*this);

        }

        /** \brief Empty constructor. */
        virtual ~BufferedBranchNode ()
        {
        }

        /** \brief Method to perform a deep copy of the octree */
        virtual BufferedBranchNode*
        deepCopy () const
        {
          return new BufferedBranchNode (*this);
        }

        /** \brief Get child pointer in current branch node
         *  \param buffer_arg: buffer selector
         *  \param index_arg: index of child in node
         *  \return pointer to child node
         * */
        inline OctreeNode*
        getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const
        {
          assert( (buffer_arg<2) && (index_arg<8));
          return child_node_array_[buffer_arg][index_arg];
        }

        /** \brief Set child pointer in current branch node
         *  \param buffer_arg: buffer selector
         *  \param index_arg: index of child in node
         *  \param newNode_arg: pointer to new child node
         * */
        inline void setChildPtr (unsigned char buffer_arg, unsigned char index_arg,
            OctreeNode* newNode_arg)
        {
          assert( (buffer_arg<2) && (index_arg<8));
          child_node_array_[buffer_arg][index_arg] = newNode_arg;
        }

        /** \brief Check if branch is pointing to a particular child node
         *  \param buffer_arg: buffer selector
         *  \param index_arg: index of child in node
         *  \return "true" if pointer to child node exists; "false" otherwise
         * */
        inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const
        {
          assert( (buffer_arg<2) && (index_arg<8));
          return (child_node_array_[buffer_arg][index_arg] != 0);
        }

        /** \brief Get the type of octree node. Returns LEAVE_NODE type */
        virtual node_type_t getNodeType () const
        {
          return BRANCH_NODE;
        }

        /** \brief Reset branch node container for every branch buffer. */
        inline void reset ()
        {
          memset (&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2);
        }

        /** \brief Get const pointer to container */
        const ContainerT*
        operator->() const
        {
          return &container_;
        }

        /** \brief Get pointer to container */
        ContainerT*
        operator-> ()
        {
          return &container_;
        }

        /** \brief Get const reference to container */
        const ContainerT&
        operator* () const
        {
          return container_;
        }

        /** \brief Get reference to container */
        ContainerT&
        operator* ()
        {
          return container_;
        }

        /** \brief Get const reference to container */
        const ContainerT&
        getContainer () const
        {
          return container_;
        }

        /** \brief Get reference to container */
        ContainerT&
        getContainer ()
        {
          return container_;
        }

        /** \brief Get const pointer to container */
        const ContainerT*
        getContainerPtr() const
        {
          return &container_;
        }

        /** \brief Get pointer to container */
        ContainerT*
        getContainerPtr ()
        {
          return &container_;
        }

      protected:
        ContainerT container_;

        OctreeNode* child_node_array_[2][8];
    };

    /** \brief @b Octree double buffer class
     *
     * \note This octree implementation keeps two separate octree structures
     * in memory.
     *
     * \note This allows for differentially compare the octree structures (change detection, differential encoding).
     * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should be initially defined).
     * \note All leaf nodes are addressed by integer indices.
     * \note Note: The tree depth equates to the bit length of the voxel indices.
     * \ingroup octree
     * \author Julius Kammerl (julius@kammerl.de)
     */
    template<typename LeafContainerT = int,
             typename BranchContainerT = OctreeContainerEmpty >
    class Octree2BufBase
    {

      public:

        typedef Octree2BufBase<LeafContainerT, BranchContainerT> OctreeT;

        // iterators are friends
        friend class OctreeIteratorBase<OctreeT> ;
        friend class OctreeDepthFirstIterator<OctreeT> ;
        friend class OctreeBreadthFirstIterator<OctreeT> ;
        friend class OctreeLeafNodeDepthFirstIterator<OctreeT> ;
        friend class OctreeLeafNodeBreadthFirstIterator<OctreeT> ;

        typedef BufferedBranchNode<BranchContainerT> BranchNode;
        typedef OctreeLeafNode<LeafContainerT> LeafNode;

        typedef BranchContainerT BranchContainer;
        typedef LeafContainerT LeafContainer;

        // Octree default iterators
        typedef OctreeDepthFirstIterator<OctreeT> Iterator;
        typedef const OctreeDepthFirstIterator<OctreeT> ConstIterator;
        Iterator begin(unsigned int max_depth_arg = 0) {return Iterator(this, max_depth_arg);};
        const Iterator end() {return Iterator();};

        // Octree leaf node iterators
        // The previous deprecated names
        // LeafNodeIterator and ConstLeafNodeIterator are deprecated.
        // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead.
        typedef OctreeLeafNodeDepthFirstIterator<OctreeT> LeafNodeIterator;
        typedef const OctreeLeafNodeDepthFirstIterator<OctreeT> ConstLeafNodeIterator;

        PCL_DEPRECATED ("Please use leaf_depth_begin () instead.")
        LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0)
        {
          return LeafNodeIterator (this, max_depth_arg);
        };

        PCL_DEPRECATED ("Please use leaf_depth_end () instead.")
        const LeafNodeIterator leaf_end ()
        {
          return LeafNodeIterator ();
        };

        // The currently valide names
        typedef OctreeLeafNodeDepthFirstIterator<OctreeT> LeafNodeDepthFirstIterator;
        typedef const OctreeLeafNodeDepthFirstIterator<OctreeT> ConstLeafNodeDepthFirstIterator;
        LeafNodeDepthFirstIterator leaf_depth_begin (unsigned int max_depth_arg = 0)
        {
          return LeafNodeDepthFirstIterator (this, max_depth_arg);
        };

        const LeafNodeDepthFirstIterator leaf_depth_end ()
        {
          return LeafNodeDepthFirstIterator();
        };

        // Octree depth-first iterators
        typedef OctreeDepthFirstIterator<OctreeT> DepthFirstIterator;
        typedef const OctreeDepthFirstIterator<OctreeT> ConstDepthFirstIterator;
        DepthFirstIterator depth_begin(unsigned int maxDepth_arg = 0) {return DepthFirstIterator(this, maxDepth_arg);};
        const DepthFirstIterator depth_end() {return DepthFirstIterator();};

        // Octree breadth-first iterators
        typedef OctreeBreadthFirstIterator<OctreeT> BreadthFirstIterator;
        typedef const OctreeBreadthFirstIterator<OctreeT> ConstBreadthFirstIterator;
        BreadthFirstIterator breadth_begin(unsigned int max_depth_arg = 0) {return BreadthFirstIterator(this, max_depth_arg);};
        const BreadthFirstIterator breadth_end() {return BreadthFirstIterator();};

        // Octree leaf node iterators
        typedef OctreeLeafNodeBreadthFirstIterator<OctreeT> LeafNodeBreadthIterator;
        typedef const OctreeLeafNodeBreadthFirstIterator<OctreeT> ConstLeafNodeBreadthIterator;

        LeafNodeBreadthIterator leaf_breadth_begin (unsigned int max_depth_arg = 0u)
        {
          return LeafNodeBreadthIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
        };

        const LeafNodeBreadthIterator leaf_breadth_end ()
        {
          return LeafNodeBreadthIterator (this, 0, NULL);
        };

        /** \brief Empty constructor. */
        Octree2BufBase ();

        /** \brief Empty deconstructor. */
        virtual
        ~Octree2BufBase ();

        /** \brief Copy constructor. */
        Octree2BufBase (const Octree2BufBase& source) :
            leaf_count_ (source.leaf_count_),
            branch_count_ (source.branch_count_),
            root_node_ (new (BranchNode) (*(source.root_node_))),
            depth_mask_ (source.depth_mask_),
            max_key_ (source.max_key_),
            buffer_selector_ (source.buffer_selector_),
            tree_dirty_flag_ (source.tree_dirty_flag_),
            octree_depth_ (source.octree_depth_),
            dynamic_depth_enabled_(source.dynamic_depth_enabled_)
        {
        }

        /** \brief Copy constructor. */
        inline Octree2BufBase&
        operator = (const Octree2BufBase& source)
        {
          leaf_count_ = source.leaf_count_;
          branch_count_ = source.branch_count_;
          root_node_ = new (BranchNode) (* (source.root_node_));
          depth_mask_ = source.depth_mask_;
          max_key_ = source.max_key_;
          buffer_selector_ = source.buffer_selector_;
          tree_dirty_flag_ = source.tree_dirty_flag_;
          octree_depth_ = source.octree_depth_;
          dynamic_depth_enabled_ = source.dynamic_depth_enabled_;
          return (*this);
        }

        /** \brief Set the maximum amount of voxels per dimension.
         *  \param max_voxel_index_arg: maximum amount of voxels per dimension
         * */
        void
        setMaxVoxelIndex (unsigned int max_voxel_index_arg);

        /** \brief Set the maximum depth of the octree.
         *  \param depth_arg: maximum depth of octree
         * */
        void
        setTreeDepth (unsigned int depth_arg);

        /** \brief Get the maximum depth of the octree.
         *  \return depth_arg: maximum depth of octree
         * */
        inline unsigned int getTreeDepth () const
        {
          return this->octree_depth_;
        }

        /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
         *  \note If leaf node already exist, this method returns the existing node
         *  \param idx_x_arg: index of leaf node in the X axis.
         *  \param idx_y_arg: index of leaf node in the Y axis.
         *  \param idx_z_arg: index of leaf node in the Z axis.
         *  \return pointer to new leaf node container.
         * */
        LeafContainerT*
        createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);

        /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
         *  \note If leaf node already exist, this method returns the existing node
         *  \param idx_x_arg: index of leaf node in the X axis.
         *  \param idx_y_arg: index of leaf node in the Y axis.
         *  \param idx_z_arg: index of leaf node in the Z axis.
         *  \return pointer to leaf node container if found, null pointer otherwise.
         * */
        LeafContainerT*
        findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);

        /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
         *  \param idx_x_arg: index of leaf node in the X axis.
         *  \param idx_y_arg: index of leaf node in the Y axis.
         *  \param idx_z_arg: index of leaf node in the Z axis.
         *  \return "true" if leaf node search is successful, otherwise it returns "false".
         * */
        bool
        existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const;

        /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
         *  \param idx_x_arg: index of leaf node in the X axis.
         *  \param idx_y_arg: index of leaf node in the Y axis.
         *  \param idx_z_arg: index of leaf node in the Z axis.
         * */
        void
        removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);

        /** \brief Return the amount of existing leafs in the octree.
         *  \return amount of registered leaf nodes.
         * */
        inline std::size_t getLeafCount () const
        {
          return (leaf_count_);
        }

        /** \brief Return the amount of existing branches in the octree.
         *  \return amount of branch nodes.
         * */
        inline std::size_t getBranchCount () const
        {
          return (branch_count_);
        }

        /** \brief Delete the octree structure and its leaf nodes.
         * */
        void
        deleteTree ();

        /** \brief Delete octree structure of previous buffer. */
        inline void deletePreviousBuffer ()
        {
          treeCleanUpRecursive (root_node_);
        }

        /** \brief Delete the octree structure in the current buffer. */
        inline void deleteCurrentBuffer ()
        {
          buffer_selector_ = !buffer_selector_;
          treeCleanUpRecursive (root_node_);
          leaf_count_ = 0;
        }

        /** \brief Switch buffers and reset current octree structure. */
        void
        switchBuffers ();

        /** \brief Serialize octree into a binary output vector describing its branch node structure.
         *  \param binary_tree_out_arg: reference to output vector for writing binary tree structure.
         *  \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
         * */
        void
        serializeTree (std::vector<char>& binary_tree_out_arg,
                       bool do_XOR_encoding_arg = false);

        /** \brief Serialize octree into a binary output vector describing its branch node structure and and push all DataT elements stored in the octree to a vector.
         * \param binary_tree_out_arg: reference to output vector for writing binary tree structure.
         * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the octree
         * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
         * */
        void
        serializeTree (std::vector<char>& binary_tree_out_arg,
                       std::vector<LeafContainerT*>& leaf_container_vector_arg,
                       bool do_XOR_encoding_arg = false);

        /** \brief Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
         *  \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
         * */
        void
        serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);

        /** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist in the previous octree buffer.
         *  \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
         * */
        void
        serializeNewLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);

        /** \brief Deserialize a binary octree description vector and create a corresponding octree structure. Leaf nodes are initialized with getDataTByKey(..).
         *  \param binary_tree_in_arg: reference to input vector for reading binary tree structure.
         *  \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
         * */
        void
        deserializeTree (std::vector<char>& binary_tree_in_arg,
                         bool do_XOR_decoding_arg = false);

        /** \brief Deserialize a binary octree description and create a corresponding octree structure. Leaf nodes are initialized with DataT elements from the dataVector.
         *  \param binary_tree_in_arg: reference to inpvectoream for reading binary tree structure.
         *  \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
         *  \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
         * */
        void
        deserializeTree (std::vector<char>& binary_tree_in_arg,
                         std::vector<LeafContainerT*>& leaf_container_vector_arg,
                         bool do_XOR_decoding_arg = false);

      protected:

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Protected octree methods based on octree keys
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        /** \brief Retrieve root node */
        OctreeNode*
        getRootNode () const
        {
          return (this->root_node_);
        }

        /** \brief Find leaf node
         *  \param key_arg: octree key addressing a leaf node.
         *  \return pointer to leaf container. If leaf node is not found, this pointer returns 0.
         * */
        inline LeafContainerT*
        findLeaf (const OctreeKey& key_arg) const
        {
          LeafContainerT* result = 0;
          findLeafRecursive (key_arg, depth_mask_, root_node_, result);
          return result;
        }

        /** \brief Create a leaf node.
         *  \note If the leaf node at the given octree node does not exist, it will be created and added to the tree.
         *  \param key_arg: octree key addressing a leaf node.
         *  \return pointer to an existing or created leaf container.
         * */
        inline LeafContainerT*
        createLeaf (const OctreeKey& key_arg)
        {
          LeafNode* leaf_node;
          BranchNode* leaf_node_parent;

          createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent, false);

          LeafContainerT* ret = leaf_node->getContainerPtr();

          return ret;
        }

        /** \brief Check if leaf doesn't exist in the octree
         *  \param key_arg: octree key addressing a leaf node.
         *  \return "true" if leaf node is found; "false" otherwise
         * */
        inline bool existLeaf (const OctreeKey& key_arg) const
        {
          return (findLeaf(key_arg) != 0);
        }

        /** \brief Remove leaf node from octree
         *  \param key_arg: octree key addressing a leaf node.
         * */
        inline void removeLeaf (const OctreeKey& key_arg)
        {
          if (key_arg <= max_key_)
          {
            deleteLeafRecursive (key_arg, depth_mask_, root_node_);

            // we changed the octree structure -> dirty
            tree_dirty_flag_ = true;
          }
        }

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Branch node accessor inline functions
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        /** \brief Check if branch is pointing to a particular child node
         *  \param branch_arg: reference to octree branch class
         *  \param child_idx_arg: index to child node
         *  \return "true" if pointer to child node exists; "false" otherwise
         * */
        inline bool
        branchHasChild (const BranchNode& branch_arg, unsigned char child_idx_arg) const
        {
          // test occupancyByte for child existence
          return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != 0);
        }

        /** \brief Retrieve a child node pointer for child node at child_idx.
         * \param branch_arg: reference to octree branch class
         * \param child_idx_arg: index to child node
         * \return pointer to octree child node class
         */
        inline OctreeNode*
        getBranchChildPtr (const BranchNode& branch_arg,
            unsigned char child_idx_arg) const
        {
          return branch_arg.getChildPtr(buffer_selector_, child_idx_arg);
        }

        /** \brief Assign new child node to branch
         *  \param branch_arg: reference to octree branch class
         *  \param child_idx_arg: index to child node
         *  \param new_child_arg: pointer to new child node
         * */
        inline void
        setBranchChildPtr (BranchNode& branch_arg, unsigned char child_idx_arg, OctreeNode* new_child_arg)
        {
          branch_arg.setChildPtr (buffer_selector_, child_idx_arg, new_child_arg);
        }

        /** \brief Generate bit pattern reflecting the existence of child node pointers for current buffer
         *  \param branch_arg: reference to octree branch class
         *  \return a single byte with 8 bits of child node information
         * */
        inline char getBranchBitPattern (const BranchNode& branch_arg) const
        {
          unsigned char i;
          char node_bits;

          // create bit pattern
          node_bits = 0;
          for (i = 0; i < 8; i++)
          {
            const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i);
            node_bits |= static_cast<char> ( (!!child) << i);
          }

          return (node_bits);
        }

        /** \brief Generate bit pattern reflecting the existence of child node pointers in specific buffer
         *  \param branch_arg: reference to octree branch class
         *  \param bufferSelector_arg: buffer selector
         *  \return a single byte with 8 bits of child node information
         * */
        inline char getBranchBitPattern (const BranchNode& branch_arg,
            unsigned char bufferSelector_arg) const
        {
          unsigned char i;
          char node_bits;

          // create bit pattern
          node_bits = 0;
          for (i = 0; i < 8; i++)
          {
            const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i);
            node_bits |= static_cast<char> ( (!!child) << i);
          }

          return (node_bits);
        }

        /** \brief Generate XOR bit pattern reflecting differences between the two octree buffers
         *  \param branch_arg: reference to octree branch class
         *  \return a single byte with 8 bits of child node XOR difference information
         * */
        inline char getBranchXORBitPattern (
            const BranchNode& branch_arg) const
        {
          unsigned char i;
          char node_bits[2];

          // create bit pattern for both buffers
          node_bits[0] = node_bits[1] = 0;

          for (i = 0; i < 8; i++)
          {
            const OctreeNode* childA = branch_arg.getChildPtr(0, i);
            const OctreeNode* childB = branch_arg.getChildPtr(1, i);

            node_bits[0] |= static_cast<char> ( (!!childA) << i);
            node_bits[1] |= static_cast<char> ( (!!childB) << i);
          }

          return node_bits[0] ^ node_bits[1];
        }

        /** \brief Test if branch changed between previous and current buffer
         *  \param branch_arg: reference to octree branch class
         *  \return "true", if child node information differs between current and previous octree buffer
         * */
        inline bool hasBranchChanges (const BranchNode& branch_arg) const
        {
          return (getBranchXORBitPattern (branch_arg) > 0);
        }

        /** \brief Delete child node and all its subchilds from octree in specific buffer
         *  \param branch_arg: reference to octree branch class
         *  \param buffer_selector_arg: buffer selector
         *  \param child_idx_arg: index to child node
         * */
        inline void deleteBranchChild (BranchNode& branch_arg,
            unsigned char buffer_selector_arg,
            unsigned char child_idx_arg)
        {
          if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg))
          {
            OctreeNode* branchChild = branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg);

            switch (branchChild->getNodeType ())
            {
              case BRANCH_NODE:
              {
                // free child branch recursively
                deleteBranch (*static_cast<BranchNode*> (branchChild));

                // delete unused branch
                delete (branchChild);
                break;
              }

              case LEAF_NODE:
              {
                // push unused leaf to branch pool
                delete (branchChild);
                break;
              }
              default:
                break;
            }

            // set branch child pointer to 0
            branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, 0);
          }
        }

        /** \brief Delete child node and all its subchilds from octree in current buffer
         *  \param branch_arg: reference to octree branch class
         *  \param child_idx_arg: index to child node
         * */
        inline void deleteBranchChild (BranchNode& branch_arg,  unsigned char child_idx_arg)
        {
          deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg);
        }

        /** \brief Delete branch and all its subchilds from octree (both buffers)
         *  \param branch_arg: reference to octree branch class
         * */
        inline void deleteBranch (BranchNode& branch_arg)
        {
          char i;

          // delete all branch node children
          for (i = 0; i < 8; i++)
          {

            if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i))
            {
              // reference was copied - there is only one child instance to be deleted
              deleteBranchChild (branch_arg, 0, i);

              // remove pointers from both buffers
              branch_arg.setChildPtr(0, i, 0);
              branch_arg.setChildPtr(1, i, 0);
            }
            else
            {
              deleteBranchChild (branch_arg, 0, i);
              deleteBranchChild (branch_arg, 1, i);
            }
          }
        }

        /** \brief Fetch and add a new branch child to a branch class in current buffer
         *  \param branch_arg: reference to octree branch class
         *  \param child_idx_arg: index to child node
         *  \return pointer of new branch child to this reference
         * */
        inline  BranchNode* createBranchChild (BranchNode& branch_arg,
            unsigned char child_idx_arg)
        {
          BranchNode* new_branch_child = new BranchNode();

          branch_arg.setChildPtr (buffer_selector_, child_idx_arg,
              static_cast<OctreeNode*> (new_branch_child));

          return new_branch_child;
        }

        /** \brief Fetch and add a new leaf child to a branch class
         *  \param branch_arg: reference to octree branch class
         *  \param child_idx_arg: index to child node
         *  \return pointer of new leaf child to this reference
         * */
        inline LeafNode*
        createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg)
        {
          LeafNode* new_leaf_child = new LeafNode();

          branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child);

          return new_leaf_child;
        }

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Recursive octree methods
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        /** \brief Create a leaf node at octree key. If leaf node does already exist, it is returned.
         *  \param key_arg: reference to an octree key
         *  \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator
         *  \param branch_arg: current branch node
         *  \param return_leaf_arg: return pointer to leaf container
         *  \param parent_of_leaf_arg: return pointer to parent of leaf node
         *  \param branch_reset_arg: Reset pointer array of current branch
         *  \return depth mask at which leaf node was created/found
         **/
        unsigned int
        createLeafRecursive (const OctreeKey& key_arg,
                             unsigned int depth_mask_arg,
                             BranchNode* branch_arg,
                             LeafNode*& return_leaf_arg,
                             BranchNode*& parent_of_leaf_arg,
                             bool branch_reset_arg = false);


        /** \brief Recursively search for a given leaf node and return a pointer.
         *  \note  If leaf node does not exist, a 0 pointer is returned.
         *  \param key_arg: reference to an octree key
         *  \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator
         *  \param branch_arg: current branch node
         *  \param result_arg: pointer to leaf container class
         **/
        void
        findLeafRecursive (const OctreeKey& key_arg,
                           unsigned int depth_mask_arg,
                           BranchNode* branch_arg,
                           LeafContainerT*& result_arg) const;


        /** \brief Recursively search and delete leaf node
         *  \param key_arg: reference to an octree key
         *  \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
         *  \param branch_arg: current branch node
         *  \return "true" if branch does not contain any childs; "false" otherwise. This indicates if current branch can be deleted.
         **/
        bool
        deleteLeafRecursive (const OctreeKey& key_arg,
                             unsigned int depth_mask_arg,
                             BranchNode* branch_arg);

        /** \brief Recursively explore the octree and output binary octree description together with a vector of leaf node DataT content.
         *  \param branch_arg: current branch node
         *  \param key_arg: reference to an octree key
         *  \param binary_tree_out_arg: binary output vector
         *  \param leaf_container_vector_arg: vector to return pointers to all leaf container in the tree.
         *  \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
         *  \param new_leafs_filter_arg: execute callback only for leaf nodes that did not exist in preceding buffer
         **/
        void
        serializeTreeRecursive (BranchNode* branch_arg,
                                OctreeKey& key_arg,
                                std::vector<char>* binary_tree_out_arg,
                                typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
                                bool do_XOR_encoding_arg = false,
                                bool new_leafs_filter_arg = false);

        /** \brief Rebuild an octree based on binary XOR octree description and DataT objects for leaf node initialization.
         *  \param branch_arg: current branch node
         *  \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
         *  \param key_arg: reference to an octree key
         *  \param binary_tree_in_it_arg iterator of binary input data
         *  \param binary_tree_in_it_end_arg
         *  \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers to be added to a leaf node
         *  \param leaf_container_vector_it_end_arg: iterator pointing to leaf container pointers pointing to last object in input container.
         *  \param branch_reset_arg: Reset pointer array of current branch
         *  \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
         **/
        void
        deserializeTreeRecursive (BranchNode* branch_arg,
                                  unsigned int depth_mask_arg,
                                  OctreeKey& key_arg,
                                  typename std::vector<char>::const_iterator& binary_tree_in_it_arg,
                                  typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg,
                                  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
                                  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg,
                                  bool branch_reset_arg = false,
                                  bool do_XOR_decoding_arg = false);


        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Serialization callbacks
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        /** \brief Callback executed for every leaf node data during serialization
         **/
        virtual void serializeTreeCallback (LeafContainerT &, const OctreeKey &)
        {

        }

        /** \brief Callback executed for every leaf node data during deserialization
         **/
        virtual void deserializeTreeCallback (LeafContainerT&, const OctreeKey&)
        {

        }

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Helpers
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        /** \brief Recursively explore the octree and remove unused branch and leaf nodes
         *  \param branch_arg: current branch node
         **/
        void
        treeCleanUpRecursive (BranchNode* branch_arg);

        /** \brief Helper function to calculate the binary logarithm
         * \param n_arg: some value
         * \return binary logarithm (log2) of argument n_arg
         */
        inline double Log2 (double n_arg)
        {
          return log (n_arg) / log (2.0);
        }

        /** \brief Test if octree is able to dynamically change its depth. This is required for adaptive bounding box adjustment.
         *  \return "false" - not resizeable due to XOR serialization
         **/
        inline bool octreeCanResize ()
        {
          return (false);
        }

        /** \brief Prints binary representation of a byte - used for debugging
         *  \param data_arg - byte to be printed to stdout
         **/
        inline void printBinary (char data_arg)
        {
          unsigned char mask = 1;  // Bit mask

          // Extract the bits
          for (int i = 0; i < 8; i++)
          {
            // Mask each bit in the byte and print it
            std::cout << ((data_arg & (mask << i)) ? "1" : "0");
          }
          std::cout << std::endl;
        }

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Globals
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        /** \brief Amount of leaf nodes   **/
        std::size_t leaf_count_;

        /** \brief Amount of branch nodes   **/
        std::size_t branch_count_;

        /** \brief Pointer to root branch node of octree   **/
        BranchNode* root_node_;

        /** \brief Depth mask based on octree depth   **/
        unsigned int depth_mask_;

        /** \brief key range */
        OctreeKey max_key_;

        /** \brief Currently active octree buffer  **/
        unsigned char buffer_selector_;

        // flags indicating if unused branches and leafs might exist in previous buffer
        bool tree_dirty_flag_;

        /** \brief Octree depth */
        unsigned int octree_depth_;

        /** \brief Enable dynamic_depth
         *  \note Note that this parameter is ignored in octree2buf! */
        bool dynamic_depth_enabled_;

    };
  }
}

#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree2buf_base.hpp>
#endif

#endif

