#include "cloud_handle.hpp"
#include "voxel_filter.hpp"
#include "io.hpp"
#include "pcl_wrapper_api.hpp"

using namespace pcl_wrapper;

// === 点云相关 ===
extern "C" {

void* create_pointcloud_xyz() {
    return new PointCloudXYZ();
}

void* create_pointcloud_xyzi() {
    return new PointCloudXYZI();
}

void reserve_pointcloud_xyz(void* cloud, int count) {
    auto* pc = static_cast<PointCloudXYZ*>(cloud);
    pc->cloud->resize(count);
}

void set_point_xyz(void* cloud, int index, float x, float y, float z) {
    auto* pc = static_cast<PointCloudXYZ*>(cloud);
    (*pc->cloud)[index].x = x;
    (*pc->cloud)[index].y = y;
    (*pc->cloud)[index].z = z;
}

void set_point_xyzi(void* cloud, int index, float x, float y, float z, float intensity) {
    auto* pc = static_cast<PointCloudXYZI*>(cloud);
    (*pc->cloud)[index].x = x;
    (*pc->cloud)[index].y = y;
    (*pc->cloud)[index].z = z;
    (*pc->cloud)[index].intensity = intensity;
}

void delete_pointcloud(void* cloud) {
    delete static_cast<PointCloudXYZ*>(cloud); // 可根据类型再区分
}

int get_pointcloud_size(void* cloud) {
    auto* pc = static_cast<PointCloudXYZ*>(cloud);
    return static_cast<int>(pc->cloud->size());
}

bool get_point_xyz(void* cloud, int index, float* x, float* y, float* z) {
    auto* pc = static_cast<PointCloudXYZ*>(cloud);
    if (index < 0 || index >= pc->cloud->size()) return false;
    const auto& pt = (*pc->cloud)[index];
    *x = pt.x;
    *y = pt.y;
    *z = pt.z;
    return true;
}

bool get_point_xyzi(void* cloud, int index, float* x, float* y, float* z, float* intensity) {
    auto* pc = static_cast<PointCloudXYZI*>(cloud);
    if (index < 0 || index >= pc->cloud->size()) return false;
    const auto& pt = (*pc->cloud)[index];
    *x = pt.x;
    *y = pt.y;
    *z = pt.z;
    *intensity = pt.intensity;
    return true;
}

// === 滤波器相关 ===

void* create_voxel_filter_xyz() {
    return new VoxelGridXYZ();
}

void* create_voxel_filter_xyzi() {
    return new VoxelGridXYZI();
}

void set_voxel_leaf_size(void* filter, float lx, float ly, float lz) {
    auto* vf_xyz = static_cast<VoxelGridXYZ*>(filter);
    vf_xyz->setLeafSize(lx, ly, lz); // 默认按XYZ处理
}

void set_voxel_input_cloud(void* filter, void* cloud) {
    auto* vf = static_cast<VoxelGridXYZ*>(filter);
    auto* pc = static_cast<PointCloudXYZ*>(cloud);
    vf->setInputCloud(pc->cloud);
}

void* apply_voxel_filter_xyz(void* filter) {
    auto* vf = static_cast<VoxelGridXYZ*>(filter);
    auto* out = new PointCloudXYZ();
    out->cloud = vf->filterCloud();
    return out;
}

void* apply_voxel_filter_xyzi(void* filter) {
    auto* vf = static_cast<VoxelGridXYZI*>(filter);
    auto* out = new PointCloudXYZI();
    out->cloud = vf->filterCloud();
    return out;
}

void delete_voxel_filter(void* ptr) {
    delete static_cast<VoxelGridXYZ*>(ptr); // 可再细分
}

// === IO相关 ===

bool load_pcd_xyz(const char* filename, void* out) {
    auto* pc = static_cast<PointCloudXYZ*>(out);
    return loadPCD_XYZ(filename, *pc);
}

bool load_pcd_xyzi(const char* filename, void* out) {
    auto* pc = static_cast<PointCloudXYZI*>(out);
    return loadPCD_XYZI(filename, *pc);
}

bool save_pcd_xyz(const char* filename, void* cloud) {
    auto* pc = static_cast<PointCloudXYZ*>(cloud);
    return savePCD_XYZ(filename, *pc);
}

bool save_pcd_xyzi(const char* filename, void* cloud) {
    auto* pc = static_cast<PointCloudXYZI*>(cloud);
    return savePCD_XYZI(filename, *pc);
}

}