// pcl_wrapper/src/wrapper.cpp
#include "filter.hpp"
#include "point_types.hpp"
#include "pcl_wrapper_api.hpp"
#include <vector>
#include <cstring>

using namespace pcl_wrapper;

using CloudPtr = pcl::PointCloud<PointXYZ>::Ptr;

class VoxelGridXYZBridge {
public:
    VoxelGridXYZBridge(float leaf) {
        filter.setLeafSize(leaf);
    }

    void setInput(const float* points, int count) {
        CloudPtr cloud(new pcl::PointCloud<PointXYZ>());
        cloud->resize(count);
        for (int i = 0; i < count; ++i) {
            (*cloud)[i].x = points[3 * i];
            (*cloud)[i].y = points[3 * i + 1];
            (*cloud)[i].z = points[3 * i + 2];
        }
        filter.setInputCloud(cloud);
    }

    void getFiltered(float** outPts, int* outCount) {
        auto cloud = filter.getFilteredCloud();
        *outCount = cloud->size();
        *outPts = new float[*outCount * 3];
        for (int i = 0; i < *outCount; ++i) {
            (*outPts)[3 * i + 0] = (*cloud)[i].x;
            (*outPts)[3 * i + 1] = (*cloud)[i].y;
            (*outPts)[3 * i + 2] = (*cloud)[i].z;
        }
    }

private:
    VoxelGridFilterImpl<PointXYZ> filter;
};


extern "C" {

void* CreateVoxelGridXYZ(float leaf) {
    return new VoxelGridXYZBridge(leaf);
}

void SetInputCloudXYZ(void* obj, const float* points, int count) {
    static_cast<VoxelGridXYZBridge*>(obj)->setInput(points, count);
}

void ApplyVoxelGridXYZ(void* obj, float** outPoints, int* outCount) {
    static_cast<VoxelGridXYZBridge*>(obj)->getFiltered(outPoints, outCount);
}

void FreeVoxelGridXYZ(void* obj) {
    delete static_cast<VoxelGridXYZBridge*>(obj);
}

} // extern C