/*
 * 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_2BUF_BASE_HPP
#define PCL_OCTREE_2BUF_BASE_HPP

namespace pcl {
namespace octree {
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
Octree2BufBase<LeafContainerT, BranchContainerT>::Octree2BufBase()
: leaf_count_(0)
, branch_count_(1)
, root_node_(new BranchNode())
, depth_mask_(0)
, buffer_selector_(0)
, tree_dirty_flag_(false)
, octree_depth_(0)
, dynamic_depth_enabled_(false)
{}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
Octree2BufBase<LeafContainerT, BranchContainerT>::~Octree2BufBase()
{
  // deallocate tree structure
  deleteTree();
  delete (root_node_);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
    uindex_t max_voxel_index_arg)
{
  uindex_t treeDepth;

  assert(max_voxel_index_arg > 0);

  // tree depth == amount of bits of maxVoxels
  treeDepth =
      std::max<uindex_t>(std::min<uindex_t>(OctreeKey::maxDepth,
                                            std::ceil(std::log2(max_voxel_index_arg))),
                         0);

  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
  depth_mask_ = (1 << (treeDepth - 1));
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::setTreeDepth(uindex_t depth_arg)
{
  assert(depth_arg > 0);

  // set octree depth
  octree_depth_ = depth_arg;

  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
  depth_mask_ = (1 << (depth_arg - 1));

  // define max. keys
  max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
LeafContainerT*
Octree2BufBase<LeafContainerT, BranchContainerT>::findLeaf(uindex_t idx_x_arg,
                                                           uindex_t idx_y_arg,
                                                           uindex_t idx_z_arg)
{
  // generate key
  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);

  // check if key exist in octree
  return (findLeaf(key));
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
LeafContainerT*
Octree2BufBase<LeafContainerT, BranchContainerT>::createLeaf(uindex_t idx_x_arg,
                                                             uindex_t idx_y_arg,
                                                             uindex_t idx_z_arg)
{
  // generate key
  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);

  // check if key exist in octree
  return (createLeaf(key));
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
bool
Octree2BufBase<LeafContainerT, BranchContainerT>::existLeaf(uindex_t idx_x_arg,
                                                            uindex_t idx_y_arg,
                                                            uindex_t idx_z_arg) const
{
  // generate key
  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);

  // check if key exist in octree
  return existLeaf(key);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::removeLeaf(uindex_t idx_x_arg,
                                                             uindex_t idx_y_arg,
                                                             uindex_t idx_z_arg)
{
  // generate key
  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);

  // free voxel at key
  return (this->removeLeaf(key));
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::deleteTree()
{
  if (root_node_) {
    // reset octree
    deleteBranch(*root_node_);
    leaf_count_ = 0;
    branch_count_ = 1;

    tree_dirty_flag_ = false;
    depth_mask_ = 0;
    octree_depth_ = 0;
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::switchBuffers()
{
  if (tree_dirty_flag_) {
    // make sure that all unused branch nodes from previous buffer are deleted
    treeCleanUpRecursive(root_node_);
  }

  // switch butter selector
  buffer_selector_ = !buffer_selector_;

  // reset flags
  tree_dirty_flag_ = true;
  leaf_count_ = 0;
  branch_count_ = 1;

  // we can safely remove children references of root node
  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
    root_node_->setChildPtr(buffer_selector_, child_idx, nullptr);
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTree(
    std::vector<char>& binary_tree_out_arg, bool do_XOR_encoding_arg)
{
  OctreeKey new_key;

  // clear binary vector
  binary_tree_out_arg.clear();
  binary_tree_out_arg.reserve(this->branch_count_);

  serializeTreeRecursive(
      root_node_, new_key, &binary_tree_out_arg, nullptr, do_XOR_encoding_arg, false);

  // serializeTreeRecursive cleans-up unused octree nodes in previous octree
  tree_dirty_flag_ = false;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTree(
    std::vector<char>& binary_tree_out_arg,
    std::vector<LeafContainerT*>& leaf_container_vector_arg,
    bool do_XOR_encoding_arg)
{
  OctreeKey new_key;

  // clear output vectors
  binary_tree_out_arg.clear();
  leaf_container_vector_arg.clear();

  leaf_container_vector_arg.reserve(leaf_count_);
  binary_tree_out_arg.reserve(this->branch_count_);

  serializeTreeRecursive(root_node_,
                         new_key,
                         &binary_tree_out_arg,
                         &leaf_container_vector_arg,
                         do_XOR_encoding_arg,
                         false);

  // serializeTreeRecursive cleans-up unused octree nodes in previous octree
  tree_dirty_flag_ = false;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::serializeLeafs(
    std::vector<LeafContainerT*>& leaf_container_vector_arg)
{
  OctreeKey new_key;

  // clear output vector
  leaf_container_vector_arg.clear();

  leaf_container_vector_arg.reserve(leaf_count_);

  serializeTreeRecursive(
      root_node_, new_key, nullptr, &leaf_container_vector_arg, false, false);

  // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
  tree_dirty_flag_ = false;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTree(
    std::vector<char>& binary_tree_in_arg, bool do_XOR_decoding_arg)
{
  OctreeKey new_key;

  // we will rebuild an octree -> reset leafCount
  leaf_count_ = 0;

  // iterator for binary tree structure vector
  std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
  std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();

  deserializeTreeRecursive(root_node_,
                           depth_mask_,
                           new_key,
                           binary_tree_in_it,
                           binary_tree_in_it_end,
                           nullptr,
                           nullptr,
                           false,
                           do_XOR_decoding_arg);

  // we modified the octree structure -> clean-up/tree-reset might be required
  tree_dirty_flag_ = false;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTree(
    std::vector<char>& binary_tree_in_arg,
    std::vector<LeafContainerT*>& leaf_container_vector_arg,
    bool do_XOR_decoding_arg)
{
  OctreeKey new_key;

  // set data iterator to first element
  typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it =
      leaf_container_vector_arg.begin();

  // set data iterator to last element
  typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end =
      leaf_container_vector_arg.end();

  // we will rebuild an octree -> reset leafCount
  leaf_count_ = 0;

  // iterator for binary tree structure vector
  std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
  std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();

  deserializeTreeRecursive(root_node_,
                           depth_mask_,
                           new_key,
                           binary_tree_in_it,
                           binary_tree_in_it_end,
                           &leaf_container_vector_it,
                           &leaf_container_vector_it_end,
                           false,
                           do_XOR_decoding_arg);

  // we modified the octree structure -> clean-up/tree-reset might be required
  tree_dirty_flag_ = false;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::serializeNewLeafs(
    std::vector<LeafContainerT*>& leaf_container_vector_arg)
{
  OctreeKey new_key;

  // clear output vector
  leaf_container_vector_arg.clear();
  leaf_container_vector_arg.reserve(leaf_count_);

  serializeTreeRecursive(
      root_node_, new_key, nullptr, &leaf_container_vector_arg, false, true);

  // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer
  tree_dirty_flag_ = false;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
uindex_t
Octree2BufBase<LeafContainerT, BranchContainerT>::createLeafRecursive(
    const OctreeKey& key_arg,
    uindex_t depth_mask_arg,
    BranchNode* branch_arg,
    LeafNode*& return_leaf_arg,
    BranchNode*& parent_of_leaf_arg,
    bool branch_reset_arg)
{
  // branch reset -> this branch has been taken from previous buffer
  if (branch_reset_arg) {
    // we can safely remove children references
    for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
      branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
    }
  }

  // find branch child from key
  unsigned char child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);

  if (depth_mask_arg > 1) {
    // we have not reached maximum tree depth
    BranchNode* child_branch;
    bool doNodeReset;

    doNodeReset = false;

    // if required branch does not exist
    if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
      // check if we find a branch node reference in previous buffer

      if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
        OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);

        if (child_node->getNodeType() == BRANCH_NODE) {
          child_branch = static_cast<BranchNode*>(child_node);
          branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
        }
        else {
          // depth has changed.. child in preceding buffer is a leaf node.
          deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
          child_branch = createBranchChild(*branch_arg, child_idx);
        }

        // take child branch from previous buffer
        doNodeReset = true; // reset the branch pointer array of stolen child node
      }
      else {
        // if required branch does not exist -> create it
        child_branch = createBranchChild(*branch_arg, child_idx);
      }

      branch_count_++;
    }
    // required branch node already exists - use it
    else
      child_branch = static_cast<BranchNode*>(
          branch_arg->getChildPtr(buffer_selector_, child_idx));

    // recursively proceed with indexed child branch
    return createLeafRecursive(key_arg,
                               depth_mask_arg / 2,
                               child_branch,
                               return_leaf_arg,
                               parent_of_leaf_arg,
                               doNodeReset);
  }

  // branch childs are leaf nodes
  LeafNode* child_leaf;
  if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
    // leaf node at child_idx does not exist

    // check if we can take copy a reference from previous buffer
    if (branch_arg->hasChild(!buffer_selector_, child_idx)) {

      OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
      if (child_node->getNodeType() == LEAF_NODE) {
        child_leaf = static_cast<LeafNode*>(child_node);
        child_leaf->getContainer() = LeafContainer(); // Clear contents of leaf
        branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
      }
      else {
        // depth has changed.. child in preceding buffer is a leaf node.
        deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
        child_leaf = createLeafChild(*branch_arg, child_idx);
      }
      leaf_count_++;
    }
    else {
      // if required leaf does not exist -> create it
      child_leaf = createLeafChild(*branch_arg, child_idx);
      leaf_count_++;
    }

    // return leaf node
    return_leaf_arg = child_leaf;
    parent_of_leaf_arg = branch_arg;
  }
  else {
    // leaf node already exist
    return_leaf_arg =
        static_cast<LeafNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
    parent_of_leaf_arg = branch_arg;
  }

  return depth_mask_arg;
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
    const OctreeKey& key_arg,
    uindex_t depth_mask_arg,
    BranchNode* branch_arg,
    LeafContainerT*& result_arg) const
{
  // return leaf node
  unsigned char child_idx;

  // find branch child from key
  child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);

  if (depth_mask_arg > 1) {
    // we have not reached maximum tree depth
    BranchNode* child_branch;
    child_branch =
        static_cast<BranchNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));

    if (child_branch)
      // recursively proceed with indexed child branch
      findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
  }
  else {
    // we reached leaf node level
    if (branch_arg->hasChild(buffer_selector_, child_idx)) {
      // return existing leaf node
      LeafNode* leaf_node =
          static_cast<LeafNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
      result_arg = leaf_node->getContainerPtr();
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
bool
Octree2BufBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive(
    const OctreeKey& key_arg, uindex_t depth_mask_arg, BranchNode* branch_arg)
{
  // index to branch child
  unsigned char child_idx;
  // indicates if branch is empty and can be safely removed
  bool bNoChilds;

  // find branch child from key
  child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);

  if (depth_mask_arg > 1) {
    // we have not reached maximum tree depth
    BranchNode* child_branch;

    // next branch child on our path through the tree
    child_branch =
        static_cast<BranchNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));

    if (child_branch) {
      // recursively explore the indexed child branch
      bool bBranchOccupied =
          deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);

      if (!bBranchOccupied) {
        // child branch does not own any sub-child nodes anymore -> delete child branch
        deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
        branch_count_--;
      }
    }
  }
  else {
    // our child is a leaf node -> delete it
    deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
    leaf_count_--;
  }

  // check if current branch still owns childs
  bNoChilds = false;
  for (child_idx = 0; child_idx < 8; child_idx++) {
    bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx);
    if (bNoChilds)
      break;
  }

  // return true if current branch can be deleted
  return (bNoChilds);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::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,
    bool new_leafs_filter_arg)
{
  if (binary_tree_out_arg) {
    // occupancy bit patterns of branch node  (current octree buffer)
    const char branch_bit_pattern_curr_buffer =
        getBranchBitPattern(*branch_arg, buffer_selector_);
    if (do_XOR_encoding_arg) {
      // occupancy bit patterns of branch node  (previous octree buffer)
      const char branch_bit_pattern_prev_buffer =
          getBranchBitPattern(*branch_arg, !buffer_selector_);
      // XOR of current and previous occupancy bit patterns
      const char node_XOR_bit_pattern =
          branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
      // write XOR bit pattern to output vector
      binary_tree_out_arg->push_back(node_XOR_bit_pattern);
    }
    else {
      // write bit pattern of current buffer to output vector
      binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer);
    }
  }

  // iterate over all children
  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
    if (branch_arg->hasChild(buffer_selector_, child_idx)) {
      // add current branch voxel to key
      key_arg.pushBranch(child_idx);

      OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx);

      switch (child_node->getNodeType()) {
      case BRANCH_NODE: {
        // recursively proceed with indexed child branch
        serializeTreeRecursive(static_cast<BranchNode*>(child_node),
                               key_arg,
                               binary_tree_out_arg,
                               leaf_container_vector_arg,
                               do_XOR_encoding_arg,
                               new_leafs_filter_arg);
        break;
      }
      case LEAF_NODE: {
        LeafNode* child_leaf = static_cast<LeafNode*>(child_node);

        if (new_leafs_filter_arg) {
          if (!branch_arg->hasChild(!buffer_selector_, child_idx)) {
            if (leaf_container_vector_arg)
              leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());

            serializeTreeCallback(**child_leaf, key_arg);
          }
        }
        else {

          if (leaf_container_vector_arg)
            leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());

          serializeTreeCallback(**child_leaf, key_arg);
        }

        break;
      }
      default:
        break;
      }

      // pop current branch voxel from key
      key_arg.popBranch();
    }
    else if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
      // delete branch, free memory
      deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive(
    BranchNode* branch_arg,
    uindex_t depth_mask_arg,
    OctreeKey& key_arg,
    typename std::vector<char>::const_iterator& binaryTreeIT_arg,
    typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
    typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
    typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
    bool branch_reset_arg,
    bool do_XOR_decoding_arg)
{

  // branch reset -> this branch has been taken from previous buffer
  if (branch_reset_arg) {
    // we can safely remove children references
    for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
      branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
    }
  }

  if (binaryTreeIT_arg != binaryTreeIT_End_arg) {
    // read branch occupancy bit pattern from vector
    char nodeBits = *binaryTreeIT_arg++;

    // recover branch occupancy bit pattern
    char recoveredNodeBits;
    if (do_XOR_decoding_arg) {
      recoveredNodeBits =
          getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits;
    }
    else {
      recoveredNodeBits = nodeBits;
    }

    // iterate over all children
    for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
      // if occupancy bit for child_idx is set..
      if (recoveredNodeBits & (1 << child_idx)) {
        // add current branch voxel to key
        key_arg.pushBranch(child_idx);

        if (depth_mask_arg > 1) {
          // we have not reached maximum tree depth

          bool doNodeReset = false;

          BranchNode* child_branch;

          // check if we find a branch node reference in previous buffer
          if (!branch_arg->hasChild(buffer_selector_, child_idx)) {

            if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
              OctreeNode* child_node =
                  branch_arg->getChildPtr(!buffer_selector_, child_idx);

              if (child_node->getNodeType() == BRANCH_NODE) {
                child_branch = static_cast<BranchNode*>(child_node);
                branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
              }
              else {
                // depth has changed.. child in preceding buffer is a leaf node.
                deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
                child_branch = createBranchChild(*branch_arg, child_idx);
              }

              // take child branch from previous buffer
              doNodeReset = true; // reset the branch pointer array of stolen child node
            }
            else {
              // if required branch does not exist -> create it
              child_branch = createBranchChild(*branch_arg, child_idx);
            }

            branch_count_++;
          }
          else {
            // required branch node already exists - use it
            child_branch = static_cast<BranchNode*>(
                branch_arg->getChildPtr(buffer_selector_, child_idx));
          }

          // recursively proceed with indexed child branch
          deserializeTreeRecursive(child_branch,
                                   depth_mask_arg / 2,
                                   key_arg,
                                   binaryTreeIT_arg,
                                   binaryTreeIT_End_arg,
                                   dataVectorIterator_arg,
                                   dataVectorEndIterator_arg,
                                   doNodeReset,
                                   do_XOR_decoding_arg);
        }
        else {
          // branch childs are leaf nodes
          LeafNode* child_leaf;

          // check if we can take copy a reference pointer from previous buffer
          if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
            // take child leaf node from previous buffer
            OctreeNode* child_node =
                branch_arg->getChildPtr(!buffer_selector_, child_idx);
            if (child_node->getNodeType() == LEAF_NODE) {
              child_leaf = static_cast<LeafNode*>(child_node);
              branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
            }
            else {
              // depth has changed.. child in preceding buffer is a leaf node.
              deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
              child_leaf = createLeafChild(*branch_arg, child_idx);
            }
          }
          else {
            // if required leaf does not exist -> create it
            child_leaf = createLeafChild(*branch_arg, child_idx);
          }

          // we reached leaf node level

          if (dataVectorIterator_arg &&
              (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) {
            LeafContainerT& container = **child_leaf;
            container = ***dataVectorIterator_arg;
            ++*dataVectorIterator_arg;
          }

          leaf_count_++;

          // execute deserialization callback
          deserializeTreeCallback(**child_leaf, key_arg);
        }

        // pop current branch voxel from key
        key_arg.popBranch();
      }
      else if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
        // remove old branch pointer information in current branch
        branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);

        // remove unused branches in previous buffer
        deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
      }
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename LeafContainerT, typename BranchContainerT>
void
Octree2BufBase<LeafContainerT, BranchContainerT>::treeCleanUpRecursive(
    BranchNode* branch_arg)
{
  // occupancy bit pattern of branch node  (previous octree buffer)
  char occupied_children_bit_pattern_prev_buffer =
      getBranchBitPattern(*branch_arg, !buffer_selector_);

  // XOR of current and previous occupancy bit patterns
  char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg);

  // bit pattern indicating unused octree nodes in previous branch
  char unused_branches_bit_pattern =
      node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;

  // iterate over all children
  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
    if (branch_arg->hasChild(buffer_selector_, child_idx)) {
      OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx);

      switch (child_node->getNodeType()) {
      case BRANCH_NODE: {
        // recursively proceed with indexed child branch
        treeCleanUpRecursive(static_cast<BranchNode*>(child_node));
        break;
      }
      case LEAF_NODE:
        // leaf level - nothing to do..
        break;
      default:
        break;
      }
    }

    // check for unused branches in previous buffer
    if (unused_branches_bit_pattern & (1 << child_idx)) {
      // delete branch, free memory
      deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
    }
  }
}
} // namespace octree
} // namespace pcl

#define PCL_INSTANTIATE_Octree2BufBase(T)                                              \
  template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;

#endif