/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 *  Copyright (c) 2009-2012, Urban Robotics, 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 the copyright holder(s) 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$
 */

/* \author
 *      Jacob Schloss (jacob.schloss@urbanrobotics.net),
 *      Justin Rosen (jmylesrosen@gmail.com),
 *      Stephen Fox (foxstephend@gmail.com)
 */

#include <pcl/test/gtest.h>

#include <list>
#include <vector>
#include <iostream>
#include <random>

#include <pcl/common/time.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/outofcore/outofcore.h>
#include <pcl/outofcore/outofcore_impl.h>

#include <pcl/PCLPointCloud2.h>

using namespace pcl::outofcore;

#include <boost/foreach.hpp>

/** \brief Unit tests for UR out of core octree code which test public interface of OutofcoreOctreeBase 
 */

// For doing exhaustive checks this is set low remove those, and this can be
// set much higher
constexpr std::uint64_t numPts (10000);

constexpr std::uint32_t rngseed = 0xAAFF33DD;

const static boost::filesystem::path filename_otreeA = "treeA/tree_test.oct_idx";
const static boost::filesystem::path filename_otreeB = "treeB/tree_test.oct_idx";

const static boost::filesystem::path filename_otreeA_LOD = "treeA_LOD/tree_test.oct_idx";
const static boost::filesystem::path filename_otreeB_LOD = "treeB_LOD/tree_test.oct_idx";

const static  boost::filesystem::path outofcore_path ("point_cloud_octree/tree_test.oct_idx");


using PointT = pcl::PointXYZ;

// UR Typedefs
using octree_disk = OutofcoreOctreeBase<OutofcoreOctreeDiskContainer < PointT > , PointT >;
using octree_disk_node = OutofcoreOctreeBaseNode<OutofcoreOctreeDiskContainer < PointT > , PointT >;

using octree_ram = OutofcoreOctreeBase<OutofcoreOctreeRamContainer< PointT> , PointT>;
using octree_ram_node = OutofcoreOctreeBaseNode<OutofcoreOctreeRamContainer<PointT> , PointT>;

using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT> >;

AlignedPointTVector points;


/** \brief helper function to compare two points. is there a templated function in pcl to do this for arbitrary point types?*/
bool 
compPt (const PointT &p1, const PointT &p2)
{
  return (p1.x == p2.x && p1.y == p2.y && p1.z == p2.z);
}

TEST (PCL, Outofcore_Octree_Build)
{

  boost::filesystem::remove_all (filename_otreeA.parent_path ());
  boost::filesystem::remove_all (filename_otreeB.parent_path ());

  Eigen::Vector3d min (-32.0, -32.0, -32.0);
  Eigen::Vector3d max (32.0, 32.0, 32.0);
  
  // Build two trees using each constructor
  // depth of treeA will be same as B because 1/2^3 > .1 and 1/2^4 < .1
  // depth really affects performance
  octree_disk treeA (min, max, .1, filename_otreeA, "ECEF");
  octree_disk treeB (4, min, max, filename_otreeB, "ECEF");

  // Equidistributed uniform pseudo-random number generator
  std::mt19937 rng (rngseed);

  // For testing sparse 
  //std::uniform_real_distribution<double> dist(0.0, 1.0);

  // For testing less sparse
  std::normal_distribution<float> dist (0.5f, .1f);

  // Create a point
  PointT p;
  points.resize (numPts);

  //ignore these fields from the UR point for now
  // p.r = p.g = p.b = 0;
  // p.nx = p.ny = p.nz = 1;
  // p.cameraCount = 0;
  // p.error = 0;
  // p.triadID = 0;

  // Radomize it's position in space
  for (std::size_t i = 0; i < numPts; i++)
  {
    p.x = dist (rng);
    p.y = dist (rng);
    p.z = dist (rng);

    points[i] = p;
  }

  // Add to tree
  treeA.addDataToLeaf (points);

  // Add to tree
  treeB.addDataToLeaf (points);

}

TEST (PCL, Outofcore_Octree_Build_LOD)
{

  boost::filesystem::remove_all (filename_otreeA_LOD.parent_path ());
  boost::filesystem::remove_all (filename_otreeB_LOD.parent_path ());

  Eigen::Vector3d min (0.0, 0.0, 0.0);
  Eigen::Vector3d max (1.0, 1.0, 1.0);
  
  // Build two trees using each constructor
  octree_disk treeA (min, max, .1, filename_otreeA_LOD, "ECEF");
  octree_disk treeB (4, min, max, filename_otreeB_LOD, "ECEF");

  // Equidistributed uniform pseudo-random number generator
  std::mt19937 rng (rngseed);
  // For testing sparse
  //std::uniform_real_distribution<double> dist(0.0, 1.0);
  // For testing less sparse
  std::normal_distribution<float> dist (0.5f, .1f);

  // Create a point
  PointT p;

/*
  p.r = p.g = p.b = 0;
  p.nx = p.ny = p.nz = 1;
  p.cameraCount = 0;
  p.error = 0;
  p.triadID = 0;
*/
  points.resize (numPts);

  // Radomize it's position in space
  for (std::size_t i = 0; i < numPts; i++)
  {
    p.x = dist (rng);
    p.y = dist (rng);
    p.z = dist (rng);
    
    points[i] = p;
  }

  // Add to tree
  treeA.addDataToLeaf_and_genLOD (points);

  // Add to tree
  treeB.addDataToLeaf_and_genLOD (points);
}

TEST(PCL, Outofcore_Bounding_Box)
{

  Eigen::Vector3d min (-32.0,-32.0,-32.0);
  Eigen::Vector3d max (32.0, 32.0, 32.0);
  
  octree_disk treeA (filename_otreeA, false);
  octree_disk treeB (filename_otreeB, false);

  Eigen::Vector3d min_otreeA;
  Eigen::Vector3d max_otreeA;
  treeA.getBoundingBox (min_otreeA, max_otreeA);

  Eigen::Vector3d min_otreeB;
  Eigen::Vector3d max_otreeB;
  treeB.getBoundingBox (min_otreeB, max_otreeB);

  for (int i=0; i<3; i++)
  {
    //octree adds an epsilon to bounding box
    EXPECT_LE (min_otreeA[i], min[i]);
    EXPECT_NEAR (min_otreeA[i], min[i], 1e4);
    
    EXPECT_GE (max_otreeA[i], max[i]);
    EXPECT_NEAR (max_otreeA[i], max[i], 1e4);

    EXPECT_LE (min_otreeB[i] , min[i]);
    EXPECT_NEAR (min_otreeB[i], min[i], 1e4);

    EXPECT_GE (max_otreeB[i] , max[i]);
    EXPECT_NEAR (max_otreeB[i], max[i], 1e4);
  }
}

void 
point_test (octree_disk& t)
{
  std::mt19937 rng (rngseed);
  std::uniform_real_distribution<float> dist(0.0, 1.0);

  Eigen::Vector3d query_box_min;
  Eigen::Vector3d qboxmax;

  for (int i = 0; i < 10; i++)
  {
    //std::cout << "query test round " << i << std::endl;
    for (int j = 0; j < 3; j++)
    {
      query_box_min[j] = dist (rng);
      qboxmax[j] = dist (rng);

      if (qboxmax[j] < query_box_min[j])
      {
        std::swap (query_box_min[j], qboxmax[j]);
        assert (query_box_min[j] < qboxmax[j]);
      }
    }

    //query the trees
    AlignedPointTVector p_ot;

    t.queryBBIncludes (query_box_min, qboxmax, t.getDepth (), p_ot);

    //query the list
    AlignedPointTVector pointsinregion;

    for (const auto &point : points)
    {
      if ((query_box_min[0] <= point.x) && (point.x < qboxmax[0]) && (query_box_min[1] < point.y) && (point.y < qboxmax[1]) && (query_box_min[2] <= point.z) && (point.z < qboxmax[2]))
      {
        pointsinregion.push_back (point);
      }
    }

    EXPECT_EQ (p_ot.size (), pointsinregion.size ());

    //very slow exhaustive comparison
    while(!p_ot.empty ())
    {
      AlignedPointTVector::iterator it;
      it = std::find_first_of (p_ot.begin (), p_ot.end(), pointsinregion.begin (), pointsinregion.end (), compPt);

      if (it != p_ot.end ())
      {
        p_ot.erase (it);
      }
      else
      {
        FAIL () <<  "Dropped Point from tree1!" << std::endl;
        break;
      }
    }

    EXPECT_TRUE (p_ot.empty ());
  }
}

TEST (PCL, Outofcore_Point_Query)
{
  octree_disk treeA(filename_otreeA, false);
  octree_disk treeB(filename_otreeB, false);

  point_test(treeA);
  point_test(treeB);
}

#if 0 //this class will be deprecated soon.
TEST (PCL, Outofcore_Ram_Tree)
{
  Eigen::Vector3d min (0.0,0.0,0.0);
  Eigen::Vector3d max (1.0, 1.0, 1.0);

  const boost::filesystem::path filename_otreeA = "ram_tree/ram_tree.oct_idx";

  octree_ram t (min, max, .1, filename_otreeA, "ECEF");

  std::mt19937 rng (rngseed);
  //std::uniform_real_distribution<double> dist(0.0, 1.0); //for testing sparse
  std::normal_distribution<float> dist (0.5f, .1f); //for testing less sparse
  PointT p;

  points.resize (numPts);
  for (std::size_t i = 0; i < numPts; i++)
  {
    p.x = dist(rng);
    p.y = dist(rng);
    p.z = dist(rng);

    points[i] = p;
  }

  t.addDataToLeaf_and_genLOD (points);
  //t.addDataToLeaf(points);

  Eigen::Vector3d qboxmin;
  Eigen::Vector3d qboxmax;
  for (int i = 0; i < 10; i++)
  {
    //std::cout << "query test round " << i << std::endl;
    for (int j = 0; j < 3; j++)
    {
      qboxmin[j] = dist (rng);
      qboxmax[j] = dist (rng);

      if (qboxmax[j] < qboxmin[j])
      {
        std::swap (qboxmin[j], qboxmax[j]);
      }
    }

    //query the trees
    AlignedPointTVector p_ot1;
    t.queryBBIncludes (qboxmin, qboxmax, t.getDepth (), p_ot1);

    //query the list
    AlignedPointTVector pointsinregion;
    for (const PointT& p : points)
    {
      if ((qboxmin[0] <= p.x) && (p.x <= qboxmax[0]) && (qboxmin[1] <= p.y) && (p.y <= qboxmax[1]) && (qboxmin[2] <= p.z) && (p.z <= qboxmax[2]))
      {
        pointsinregion.push_back (p);
      }
    }

    EXPECT_EQ (p_ot1.size (), pointsinregion.size ());

    //very slow exhaustive comparison
    while (!p_ot1.empty ())
    {
      AlignedPointTVector::iterator it;
      it = std::find_first_of (p_ot1.begin (), p_ot1.end (), pointsinregion.begin (), pointsinregion.end (), compPt);

      if (it != p_ot1.end ())
      {
        p_ot1.erase(it);
      }
      else
      {
        break;
        FAIL () <<  "Dropped Point from tree1!" << std::endl;
      }
    }

    EXPECT_TRUE (p_ot1.empty ());
  }
}
#endif
class OutofcoreTest : public testing::Test
{
  protected:

    OutofcoreTest () = default;

    void SetUp () override
    {
      smallest_voxel_dim = 3.0f;
    }

    void TearDown () override
    {

    }

    void cleanUpFilesystem ()
    {
      //clear existing trees from test path

      boost::filesystem::remove_all (filename_otreeA.parent_path ());
      boost::filesystem::remove_all (filename_otreeB.parent_path ());

      boost::filesystem::remove_all (filename_otreeA_LOD.parent_path ());
      boost::filesystem::remove_all (filename_otreeB_LOD.parent_path ());

      boost::filesystem::remove_all (outofcore_path.parent_path ());

    }

    double smallest_voxel_dim{3.0f};

};


/** \brief Thorough test of the constructors, including exceptions and specified behavior */
TEST_F (OutofcoreTest, Outofcore_Constructors)
{
  //Case 1: create octree on-disk by resolution
  //Case 2: create octree on-disk by depth
  //Case 3: try to create an octree in existing tree and handle exception
  //Case 4: load existing octree from disk
  //Case 5: try to load non-existent octree from disk

  cleanUpFilesystem ();

  //Specify the lower corner of the axis-aligned bounding box
  const Eigen::Vector3d min (-1024.0, -1024.0, -1024.0);
  //Specify the upper corner of the axis-aligned bounding box
  const Eigen::Vector3d max (1024.0, 1024.0, 1024.0);

  AlignedPointTVector some_points;

  for (unsigned int i=0; i< numPts; i++)
    some_points.emplace_back(static_cast<float>(rand () % 1024), static_cast<float>(rand () % 1024), static_cast<float>(rand () % 1024));
  

  //(Case 1)
  //Create Octree based on resolution of smallest voxel, automatically computing depth
  octree_disk octreeA (min, max, smallest_voxel_dim, filename_otreeA, "ECEF");

  EXPECT_EQ (some_points.size (), octreeA.addDataToLeaf (some_points)) << "Dropped points in voxel resolution constructor\n";

  EXPECT_EQ (some_points.size (), octreeA.getNumPointsAtDepth (octreeA.getDepth ()));
  
  //(Case 2)
  //create Octree by prespecified depth in constructor
  int depth = 2;
  octree_disk octreeB (depth, min, max, filename_otreeB, "ECEF");
  EXPECT_EQ (some_points.size (), octreeB.addDataToLeaf (some_points)) << "Dropped points in fixed-depth constructor\n";
  
  EXPECT_EQ (some_points.size (), octreeB.getNumPointsAtDepth (octreeB.getDepth ()));
}

TEST_F (OutofcoreTest, Outofcore_ConstructorSafety)
{
  //Specify the lower corner of the axis-aligned bounding box
  const Eigen::Vector3d min (-1024, -1024, -1024);
  //Specify the upper corner of the axis-aligned bounding box
  const Eigen::Vector3d max (1024, 1024, 1024);
  
  int depth = 2;
  
  //(Case 3) Constructor Safety. These should throw OCT_CHILD_EXISTS exceptions and write an error
  //message of conflicting file path
  ASSERT_TRUE (boost::filesystem::exists (filename_otreeA)) << "No tree detected on disk. This test will fail. Perhaps this test was run out of order.\n";
  ASSERT_TRUE (boost::filesystem::exists (filename_otreeB)) << "No tree detected on disk. This test will fail. Perhaps this test was run out of order.\n";

  EXPECT_ANY_THROW ({ octree_disk octreeC (min, max, smallest_voxel_dim, filename_otreeA, "ECEF"); }) << "Failure to detect existing tree on disk with the same name. Data may be overwritten.\n";
  EXPECT_ANY_THROW ({ octree_disk octreeD (depth, min, max, filename_otreeB, "ECEF"); }) << "Failure to detect existing tree on disk with the same name. Data may be overwritten.\n";

  //(Case 4): Load existing tree from disk
  octree_disk octree_from_disk (filename_otreeB, true);
  EXPECT_EQ (numPts , octree_from_disk.getNumPointsAtDepth (octree_from_disk.getDepth ())) << "Failure to count the number of points in a tree already existing on disk\n";
}

TEST_F (OutofcoreTest, Outofcore_ConstructorBadPaths)
{
  //(Case 5): Try to load non-existent tree from disk
  //root node should be created at this point
  /// \todo Shouldn't these throw an exception for bad path?
  boost::filesystem::path non_existent_path_name ("treeBogus/tree_bogus.oct_idx");
  boost::filesystem::path bad_extension_path ("treeBadExtension/tree_bogus.bad_extension");

  EXPECT_FALSE (boost::filesystem::exists (non_existent_path_name));
  EXPECT_ANY_THROW ({octree_disk octree_bogus_path (non_existent_path_name, true);});

  EXPECT_FALSE (boost::filesystem::exists (bad_extension_path));
  EXPECT_ANY_THROW ({octree_disk octree_bad_extension (bad_extension_path, true);});

}

TEST_F (OutofcoreTest, Outofcore_PointcloudConstructor)
{
  cleanUpFilesystem ();
  
  //Specify the lower corner of the axis-aligned bounding box
  const Eigen::Vector3d min (-1,-1,-1);
  //Specify the upper corner of the axis-aligned bounding box
  const Eigen::Vector3d max (1024, 1024, 1024);

  //create a point cloud
  pcl::PointCloud<PointT>::Ptr test_cloud (new pcl::PointCloud<PointT> ());
  
  test_cloud->width = numPts;
  test_cloud->height = 1;
  test_cloud->reserve (numPts);
  
  //generate some random points
  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024));
    
    test_cloud->points.push_back (tmp);
  }

  EXPECT_EQ (numPts, test_cloud->size ());
  
  octree_disk pcl_cloud (4, min, max, outofcore_path, "ECEF");

  pcl_cloud.addPointCloud (test_cloud);
  
  EXPECT_EQ (test_cloud->size (), pcl_cloud.getNumPointsAtDepth (pcl_cloud.getDepth ()));
  
  cleanUpFilesystem ();
}

TEST_F (OutofcoreTest, Outofcore_PointsOnBoundaries)
{
  cleanUpFilesystem ();
  
  const Eigen::Vector3d min (-1,-1,-1);
  const Eigen::Vector3d max (1,1,1);
  
  pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
  cloud->width = 8;
  cloud->height =1;
  cloud->reserve (8);
  
  for (int i=0; i<8; i++)
  {
    PointT tmp;
    tmp.x = static_cast<float> (pow (-1.0, i)) * 1.0f;
    tmp.y = static_cast<float> (pow (-1.0, i+1)) * 1.0f;
    tmp.z = static_cast<float> (pow (-1.0, 3*i)) * 1.0f;
    
    cloud->points.push_back (tmp);
  }

  octree_disk octree (4, min, max, outofcore_path, "ECEF");

  octree.addPointCloud (cloud);

  EXPECT_EQ (8, octree.getNumPointsAtDepth (octree.getDepth ()));

}

/*
TEST_F (OutofcoreTest, Outofcore_PointCloud2Basic)
{
  cleanUpFilesystem ();
  
  const double min[3] = { -1.0, -1.0, -1.0 };
  const double max[3] = { 1.0, 1.0, 1.0 };

  pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());

}
*/

TEST_F (OutofcoreTest, Outofcore_MultiplePointClouds)
{
  cleanUpFilesystem ();

  //Specify the lower corner of the axis-aligned bounding box
  const Eigen::Vector3d min (-1024,-1024,-1024);
  
  //Specify the upper corner of the axis-aligned bounding box
  const Eigen::Vector3d max (1024,1024,1024);
  
  //create a point cloud
  pcl::PointCloud<PointT>::Ptr test_cloud (new pcl::PointCloud<PointT> ());
  pcl::PointCloud<PointT>::Ptr second_cloud (new pcl::PointCloud<PointT> ());

  test_cloud->width = numPts;
  test_cloud->height = 1;
  test_cloud->reserve (numPts);

  second_cloud->width = numPts;
  second_cloud->height = 1;
  second_cloud->reserve (numPts);
  
  //generate some random points
  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024));
    
    test_cloud->points.push_back (tmp);
  }

  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024));
    
    second_cloud->points.push_back (tmp);
  }

  octree_disk pcl_cloud (4, min, max, outofcore_path, "ECEF");

  ASSERT_EQ (test_cloud->size (), pcl_cloud.addPointCloud (test_cloud)) << "Points lost when adding the first cloud to the tree\n";

  ASSERT_EQ (numPts, pcl_cloud.getNumPointsAtDepth (pcl_cloud.getDepth ())) << "Book keeping of number of points at query depth does not match number of points inserted to the leaves\n";

  pcl_cloud.addPointCloud (second_cloud);

  EXPECT_EQ (2*numPts, pcl_cloud.getNumPointsAtDepth (pcl_cloud.getDepth ())) << "Points are lost when two points clouds are added to the outofcore file system\n";

  pcl_cloud.setSamplePercent (0.125);
  pcl_cloud.buildLOD ();
  
  //check that there is at least one point in each LOD
  for (std::size_t i=0; i<pcl_cloud.getDepth (); i++)
    EXPECT_GE (pcl_cloud.getNumPointsAtDepth (i), 1) << "No points in the LOD indicates buildLOD failed\n";

  EXPECT_EQ (2*numPts, pcl_cloud.getNumPointsAtDepth (pcl_cloud.getDepth ())) << "Points in leaves were lost while building LOD!\n";

  cleanUpFilesystem ();
}

TEST_F (OutofcoreTest, Outofcore_PointCloudInput_LOD)
{
  cleanUpFilesystem ();

  //Specify the lower corner of the axis-aligned bounding box
  const Eigen::Vector3d min (-1024,-1024,-1024);
  
  //Specify the upper corner of the axis-aligned bounding box
  const Eigen::Vector3d max (1024,1024,1024);
  
  //create a point cloud
  pcl::PointCloud<PointT>::Ptr test_cloud (new pcl::PointCloud<PointT> ());
  pcl::PointCloud<PointT>::Ptr second_cloud (new pcl::PointCloud<PointT> ());

  test_cloud->width = numPts;
  test_cloud->height = 1;
  test_cloud->reserve (numPts);

  second_cloud->width = numPts;
  second_cloud->height = 1;
  second_cloud->reserve (numPts);
  
  //generate some random points
  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024));
    
    test_cloud->points.push_back (tmp);
  }

  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024), 
                 static_cast<float> (i % 1024));
    
    second_cloud->points.push_back (tmp);
  }

  octree_disk pcl_cloud (4, min, max, outofcore_path, "ECEF");

  pcl_cloud.addPointCloud_and_genLOD (second_cloud);

//  EXPECT_EQ (2*numPts, pcl_cloud.getNumPointsAtDepth (pcl_cloud.getDepth ())) << "Points are lost when two points clouds are added to the outofcore file system\n";
  cleanUpFilesystem ();
}

TEST_F (OutofcoreTest, PointCloud2_Constructors)
{
  cleanUpFilesystem ();
  
  //Specify the bounding box of the point clouds
  const Eigen::Vector3d min (-100.1, -100.1, -100.1);
  const Eigen::Vector3d max (100.1, 100.1, 100.1);
  
  constexpr std::uint64_t depth = 2;
  
  //create a point cloud
  pcl::PointCloud<PointT>::Ptr test_cloud (new pcl::PointCloud<PointT> ());

  test_cloud->width = numPts;
  test_cloud->height = 1;
  test_cloud->reserve (numPts);

  //generate some random points
  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 200) - 99 , 
                 static_cast<float> (i % 200) - 99, 
                 static_cast<float> (i % 200) - 99);
    
    test_cloud->points.push_back (tmp);
  }

  pcl::PCLPointCloud2::Ptr point_cloud (new pcl::PCLPointCloud2);
  
  pcl::toPCLPointCloud2 (*test_cloud, *point_cloud);

  octree_disk octreeA (depth, min, max, filename_otreeA, "ECEF");
  octree_disk octreeB (depth, min, max, filename_otreeB, "ECEF");

  EXPECT_EQ (octreeA.addPointCloud (point_cloud, false) , point_cloud->width*point_cloud->height) << "Number of points returned by constructor indicates some points were not properly inserted to the outofcore cloud\n";

  EXPECT_EQ (octreeB.addPointCloud_and_genLOD (point_cloud), point_cloud->width*point_cloud->height) << "Number of points inserted when generating LOD does not match the size of the point cloud\n";
}

TEST_F (OutofcoreTest, PointCloud2_Insertion)
{
  cleanUpFilesystem ();
  
  const Eigen::Vector3d min (-11, -11, -11);
  const Eigen::Vector3d max (11,11,11);

  pcl::PointCloud<pcl::PointXYZ> point_cloud;

  point_cloud.reserve (numPts);
  point_cloud.width = static_cast<std::uint32_t> (numPts);
  point_cloud.height = 1;

  for (std::size_t i=0; i < numPts; i++)
    point_cloud.emplace_back(static_cast<float>(rand () % 10), static_cast<float>(rand () % 10), static_cast<float>(rand () % 10));


  pcl::PCLPointCloud2::Ptr input_cloud (new pcl::PCLPointCloud2 ());

  pcl::toPCLPointCloud2<pcl::PointXYZ> (point_cloud, *input_cloud);
  ASSERT_EQ (point_cloud.width*point_cloud.height, input_cloud->width*input_cloud->height);

  octree_disk octreeA (min, max, smallest_voxel_dim, filename_otreeA, "ECEF");
  octree_disk octreeB (1, min, max, filename_otreeB, "ECEF");

  //make sure the number of points successfully added are the same as how many we input
  std::uint64_t points_in_input_cloud = input_cloud->width*input_cloud->height;
  EXPECT_EQ (octreeA.addPointCloud (input_cloud, false), points_in_input_cloud) << "Insertion failure. Number of points successfully added does not match size of input cloud\n";
  EXPECT_EQ (octreeB.addPointCloud (input_cloud, false), points_in_input_cloud) << "Insertion failure. Number of points successfully added does not match size of input cloud\n";
}

TEST_F (OutofcoreTest, PointCloud2_MultiplePointCloud)
{

  cleanUpFilesystem ();
  
  //Specify the bounding box of the point clouds
  const Eigen::Vector3d min (-100.1, -100.1, -100.1);
  const Eigen::Vector3d max (100.1, 100.1, 100.1);

  //create a point cloud
  pcl::PointCloud<PointT>::Ptr first_cloud (new pcl::PointCloud<PointT> ());
  pcl::PointCloud<PointT>::Ptr second_cloud (new pcl::PointCloud<PointT> ());

  first_cloud->width = numPts;
  first_cloud->height = 1;
  first_cloud->reserve (numPts);

  second_cloud->width = numPts;
  second_cloud->height = 1;
  second_cloud->reserve (numPts);
  
  //generate some random points
  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 50), 
                static_cast<float> (i % 50),
                 static_cast<float> (i % 50));
    
    first_cloud->points.push_back (tmp);
  }

  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 50), 
                 static_cast<float> (i % 50), 
                 static_cast<float> (i % 50));
    
    second_cloud->points.push_back (tmp);
  }

  pcl::PCLPointCloud2::Ptr first_cloud_ptr (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr second_cloud_ptr (new pcl::PCLPointCloud2 ());
  
  pcl::toPCLPointCloud2<PointT> (*first_cloud, *first_cloud_ptr);
  pcl::toPCLPointCloud2<PointT> (*second_cloud, *second_cloud_ptr);

  //Create an outofcore tree which just concatenates the two clouds into a single PCD in the root node. Check that the number of points is correct.
  octree_disk shallow_outofcore (0/*depth*/, min, max, filename_otreeB, "ECEF");
  
  shallow_outofcore.addPointCloud (first_cloud);
  shallow_outofcore.addPointCloud (second_cloud);
  
  pcl::PCLPointCloud2::Ptr result (new pcl::PCLPointCloud2 ());
  shallow_outofcore.queryBBIncludes (min, max, 0, result);
  
  std::size_t num_points_queried = result->width*result->height;
  std::size_t num_points_inserted = first_cloud->width*first_cloud->height + second_cloud->width*second_cloud->height;

  EXPECT_EQ (num_points_inserted, num_points_queried) << "If num_points_inserted > num_points_on_disk, then points were dropped on insertion of multiple clouds in the outofcore octree";
}

TEST_F (OutofcoreTest, PointCloud2_QueryBoundingBox)
{
  cleanUpFilesystem ();

  //Specify the bounding box of the point clouds
  const Eigen::Vector3d min (-100.1, -100.1, -100.1);
  const Eigen::Vector3d max (100.1, 100.1, 100.1);
  
  constexpr std::uint64_t depth = 2;

  //create a point cloud
  pcl::PointCloud<PointT>::Ptr test_cloud (new pcl::PointCloud<PointT> ());

  test_cloud->width = numPts;
  test_cloud->height = 1;
  test_cloud->reserve (numPts);

  //generate some random points
  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 50) - 50 , 
                 static_cast<float> (i % 50) - 50, 
                 static_cast<float> (i % 50) - 50);
    
    test_cloud->points.push_back (tmp);
  }

  pcl::PCLPointCloud2::Ptr dst_blob (new pcl::PCLPointCloud2 ());
  
  pcl::toPCLPointCloud2 (*test_cloud, *dst_blob);

  octree_disk octreeA (depth, min, max, filename_otreeA, "ECEF");
  octree_disk octreeB (depth, min, max, filename_otreeB, "ECEF");

  std::uint64_t points_added = octreeA.addPointCloud (dst_blob, false);
  EXPECT_EQ (points_added, dst_blob->width*dst_blob->height);
  
  pcl::PCLPointCloud2::Ptr dst_blob2 (new pcl::PCLPointCloud2 ());
  
  octreeA.queryBoundingBox (min, max, 2, dst_blob2);
  std::list<std::string> filenames;
  octreeA.queryBoundingBox (min, max, 2, filenames);
  EXPECT_GE (filenames.size (), 1);
  
  octreeA.queryBoundingBox (min, max, 2, dst_blob2, 0.125);
  EXPECT_GE (dst_blob2->width*dst_blob2->height, 1);
  cleanUpFilesystem ();
}


//test that the PCLPointCloud2 query returns the same points as the templated queries
TEST_F (OutofcoreTest, PointCloud2_Query)
{

  cleanUpFilesystem ();

  //Specify the bounding box of the point clouds
  const Eigen::Vector3d min (-100.1, -100.1, -100.1);
  const Eigen::Vector3d max (100.1, 100.1, 100.1);
  
  constexpr std::uint64_t depth = 2;
  
  //create a point cloud
  pcl::PointCloud<PointT>::Ptr test_cloud (new pcl::PointCloud<PointT> ());

  test_cloud->width = numPts;
  test_cloud->height = 1;
  test_cloud->reserve (numPts);

  //generate some random points
  for (std::size_t i=0; i < numPts; i++)
  {
    PointT tmp (static_cast<float> (i % 50) - 50 , 
                 static_cast<float> (i % 50) - 50, 
                 static_cast<float> (i % 50) - 50);
    
    test_cloud->points.push_back (tmp);
  }

  pcl::PCLPointCloud2::Ptr dst_blob (new pcl::PCLPointCloud2 ());
  
  pcl::toPCLPointCloud2 (*test_cloud, *dst_blob);

  octree_disk octreeA (depth, min, max, filename_otreeA, "ECEF");
  octree_disk octreeB (depth, min, max, filename_otreeB, "ECEF");

  std::uint64_t points_added = octreeA.addPointCloud (dst_blob, false);
  std::uint64_t LOD_points_added = octreeB.addPointCloud_and_genLOD (dst_blob);

  ASSERT_EQ (points_added, dst_blob->width*dst_blob->height) << "Number of points returned by addPointCloud does not match the number of points in the input point cloud\n";
  ASSERT_EQ (LOD_points_added, dst_blob->width*dst_blob->height) << "Number of points returned by addPointCloud_and_genLOD does not match the number of points in the input point cloud\n";

  pcl::PCLPointCloud2::Ptr query_result_a (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr query_result_b (new pcl::PCLPointCloud2 ());

  octreeA.queryBBIncludes (min, max, static_cast<int>(octreeA.getDepth ()), query_result_a);
  
  EXPECT_EQ (test_cloud->width*test_cloud->height, query_result_a->width*query_result_a->height) << "PCLPointCloud2 Query number of points returned failed\n";

  std::uint64_t total_octreeB_LOD_query = 0;
  
  for (std::uint64_t i=0; i <= octreeB.getDepth (); i++)
  {
    octreeB.queryBBIncludes (min, max, i, query_result_b);
    total_octreeB_LOD_query += query_result_b->width*query_result_b->height;
    query_result_b->data.clear ();
    query_result_b->width =0;
    query_result_b->height =0;
  }
  
  EXPECT_EQ (test_cloud->width*test_cloud->height, total_octreeB_LOD_query) << "PCLPointCloud2 Query number of points returned failed\n";

  cleanUpFilesystem ();
}

/* [--- */
int
main (int argc, char** argv)
{
//  pcl::console::setVerbosityLevel (pcl::console::L_VERBOSE);
  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}
/* ]--- */