#include "cloud_handle.hpp"
#include "voxel_filter.hpp"
#include "kdtree.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 resize_pointcloud_xyz(void* cloud, int count) {
    auto* pc = static_cast<PointCloudXYZ*>(cloud);
    pc->cloud->resize(count);
}

void resize_pointcloud_xyzi(void* cloud, int count) {
    auto* pc = static_cast<PointCloudXYZI*>(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_xyz(void* cloud) {
    delete static_cast<PointCloudXYZ*>(cloud);
}

void delete_pointcloud_xyzi(void* cloud) {
    delete static_cast<PointCloudXYZI*>(cloud);
}

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

int get_pointcloud_size_xyzi(void* cloud) {
    auto* pc = static_cast<PointCloudXYZI*>(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); // 可再细分
}

// === KdTree相关 ===

void* create_kdtree_xyz() {
    return new KdTreeXYZ();
}

void set_kdtree_input_cloud_xyz(void* kd, void* cloud) {
    auto* kdtree = static_cast<KdTreeXYZ*>(kd);
    auto* pc = static_cast<PointCloudXYZ*>(cloud);
    kdtree->setInputCloud(pc->cloud);
}

int knn_search_xyz(void* kd, float x, float y, float z, int k,
                   int* out_indices, float* out_distances) {
    auto* kdtree = static_cast<KdTreeXYZ*>(kd);
    pcl::PointXYZ pt(x, y, z);
    std::vector<int> indices;
    std::vector<float> distances;
    bool success = kdtree->nearestKSearch(pt, k, indices, distances);
    if (!success) return 0;
    for (int i = 0; i < k; ++i) {
        out_indices[i] = indices[i];
        out_distances[i] = distances[i];
    }
    return static_cast<int>(indices.size());
}

int radius_search_xyz(void* kd, float x, float y, float z, float radius,
                      int* out_indices, float* out_distances, int max_count) {
    auto* kdtree = static_cast<KdTreeXYZ*>(kd);
    pcl::PointXYZ pt(x, y, z);
    std::vector<int> indices;
    std::vector<float> distances;
    indices.reserve(max_count);
    distances.reserve(max_count);
    bool success = kdtree->radiusSearch(pt, radius, indices, distances);
    int count = std::min<int>(indices.size(), max_count);
    for (int i = 0; i < count; ++i) {
        out_indices[i] = indices[i];
        out_distances[i] = distances[i];
    }
    return count;
}

void delete_kdtree_xyz(void* kd) {
    delete static_cast<KdTreeXYZ*>(kd);
}


// === 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);
}

}