#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>

using PointType = pcl::PointXYZ;

class PclPointCloud {
public:
    pcl::PointCloud<PointType>::Ptr cloud;

    PclPointCloud() : cloud(new pcl::PointCloud<PointType>) {}
};

class PclVoxelGrid {
public:
    pcl::VoxelGrid<PointType> filter;
};

extern "C" {

void* create_point_cloud() {
    return new PclPointCloud();
}

void delete_point_cloud(void* ptr) {
    delete static_cast<PclPointCloud*>(ptr);
}

int load_point_cloud(void* ptr, const char* path) {
    auto* cloud = static_cast<PclPointCloud*>(ptr);
    return pcl::io::loadPCDFile<PointType>(path, *cloud->cloud);
}

int save_point_cloud(void* ptr, const char* path) {
    auto* cloud = static_cast<PclPointCloud*>(ptr);
    return pcl::io::savePCDFileBinary(path, *cloud->cloud);
}

void* create_voxel_filter() {
    return new PclVoxelGrid();
}

void delete_voxel_filter(void* ptr) {
    delete static_cast<PclVoxelGrid*>(ptr);
}

void set_voxel_leaf_size(void* ptr, float lx, float ly, float lz) {
    auto* vf = static_cast<PclVoxelGrid*>(ptr);
    vf->filter.setLeafSize(lx, ly, lz);
}

void set_voxel_input_cloud(void* vf_ptr, void* cloud_ptr) {
    auto* vf = static_cast<PclVoxelGrid*>(vf_ptr);
    auto* cloud = static_cast<PclPointCloud*>(cloud_ptr);
    vf->filter.setInputCloud(cloud->cloud);
}

void* apply_voxel_filter(void* vf_ptr) {
    auto* vf = static_cast<PclVoxelGrid*>(vf_ptr);
    auto* result = new PclPointCloud();
    vf->filter.filter(*result->cloud);
    return result;
}

}