#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>

using namespace std;

class PointCloudInfo
{
public:
    typedef boost::shared_ptr<PointCloudInfo> Ptr;            /*!< Point cloud information shared point */
    typedef boost::shared_ptr<const PointCloudInfo> ConstPtr; /*!< Point cloud information shared point (const)*/

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; /*!< Pcl point cloud */
    double octo_min_x{ 0 };                    /*!< Minimum x-axis value of octomap */
    double octo_min_y{ 0 };                    /*!< Minimum y-axis value of octomap */
    double octo_min_z{ 0 };                    /*!< Minimum z-axis value of octomap */
    double octo_max_x{ 0 };                    /*!< Maximum x-axis value of octomap */
    double octo_max_y{ 0 };                    /*!< Maximum y-axis value of octomap */
    double octo_max_z{ 0 };                    /*!< Maximum z-axis value of octomap */
    double octo_resol{ 0 };                    /*!< Octomap Resolution */
};

struct xyz
{
    float x;
    float y;
    float z;
};

int main()
{

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZ> ( "rabbit.pcd", cloud );    

    octomap::OcTree tree( 0.001 );
    vector<xyz> vp;
    xyz m_p;
    FILE *fp = fopen("rabbit.txt","w");        

    for (auto p:cloud.points)
    {
        m_p.x = p.x;
        m_p.y = p.y;
        m_p.z = p.z;
        vp.emplace_back(m_p);

        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
        fprintf(fp,"%f %f %f\n",p.x,p.y,p.z);
    }
    fclose(fp);

    fp = fopen("rabbit.bin","wb");
    fwrite(&vp[0],vp.size()*sizeof(xyz),1,fp);
    fclose(fp);

    tree.updateInnerOccupancy();
    tree.write("rabbit.ot");
    tree.writeBinary("rabbit.bt");

    ///////////////////////////read bt///////////////////////////////////////
    octomap::OcTree octo_tree(0.001);
    octo_tree.readBinary("rabbit.bt");
    
    PointCloudInfo::Ptr pc_info(new PointCloudInfo());
    octo_tree.getMetricMin(pc_info->octo_min_x, pc_info->octo_min_y, pc_info->octo_min_z);
    octo_tree.getMetricMax(pc_info->octo_max_x, pc_info->octo_max_y, pc_info->octo_max_z);
    pc_info->octo_resol = octo_tree.getResolution();


    pc_info->cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointXYZ point;
    for (octomap::OcTree::leaf_iterator it = octo_tree.begin_leafs(); it != octo_tree.end_leafs(); ++it)
    {
        if (it != nullptr && octo_tree.isNodeOccupied(*it))
        {
            point.x = static_cast<float>(it.getX());
            point.y = static_cast<float>(it.getY());
            point.z = static_cast<float>(it.getZ());
            pc_info->cloud->push_back(point);
        }
    }

    return 0;
}