using System;
using System.Drawing;
using System.Runtime.InteropServices;

public class PclPointCloudXYZ : IDisposable {
    private IntPtr handle;

    public PclPointCloudXYZ() => handle = create_pointcloud_xyz();
    // 构造函数用于内部封装 handle 对象
    public PclPointCloudXYZ(IntPtr handle) => this.handle = handle;
    public void Dispose() { delete_pointcloud_xyz(handle); handle = IntPtr.Zero; }
    
    public void Resize(int size) => resize_pointcloud_xyz(handle, size);

    public bool Load(string path) => load_pcd_xyz( path,handle);
    public int Save(string path) => save_pcd_xyz(path,handle);
    public IntPtr Handle => handle;
    
    public int Size => get_pointcloud_size_xyz(handle);
    public bool GetPoint(int index, out float x, out float y, out float z) => get_point_xyz(handle, index, out x, out y, out z);
    public void SetPoint(int index, float x, float y, float z) => set_point_xyz(handle, index, x, y, z);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_pointcloud_xyz();
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void resize_pointcloud_xyz(IntPtr ptr, int size);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_pointcloud_xyz(IntPtr ptr);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern bool load_pcd_xyz(string path,IntPtr ptr);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern int save_pcd_xyz(string path,IntPtr ptr);
    [DllImport("./pcl/libpclwrapper.dylib")] static extern int get_pointcloud_size_xyz(IntPtr cloud);
    [DllImport("./pcl/libpclwrapper.dylib")] static extern bool get_point_xyz(IntPtr cloud, int index, out float x, out float y, out float z);
    [DllImport("./pcl/libpclwrapper.dylib")] static extern void set_point_xyz(IntPtr cloud, int index, float x, float y, float z);
}


public class PclPointCloudXYZI : IDisposable {
    private IntPtr handle;

    public PclPointCloudXYZI() => handle = create_pointcloud_xyzi();
    // 构造函数用于内部封装 handle 对象
    public PclPointCloudXYZI(IntPtr handle) => this.handle = handle;
    public void Dispose() { delete_pointcloud_xyzi(handle); handle = IntPtr.Zero; }
    
    public void Resize(int size) => resize_pointcloud_xyzi(handle, size);

    public bool Load(string path) => load_pcd_xyzi( path,handle);
    public int Save(string path) => save_pcd_xyzi(path,handle);
    public IntPtr Handle => handle;
    public int Size => get_pointcloud_size_xyzi(handle);
    public bool GetPoint(int index, out float x, out float y, out float z, out float intensity) => get_point_xyzi(handle, index, out x, out y, out z, out intensity);
    public void SetPoint(int index, float x, float y, float z, float intensity) => set_point_xyzi(handle, index, x, y, z, intensity);

    [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_pointcloud_xyzi();
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_pointcloud_xyzi(IntPtr ptr);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void resize_pointcloud_xyzi(IntPtr ptr, int size);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern bool load_pcd_xyzi(string path,IntPtr ptr);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern int save_pcd_xyzi(string path,IntPtr ptr);
    [DllImport("./pcl/libpclwrapper.dylib")] static extern int get_pointcloud_size_xyzi(IntPtr cloud);
    [DllImport("./pcl/libpclwrapper.dylib")] static extern bool get_point_xyzi(IntPtr cloud, int index, out float x, out float y, out float z, out float intensity);
    [DllImport("./pcl/libpclwrapper.dylib")] static extern void set_point_xyzi(IntPtr cloud, int index, float x, float y, float z, float intensity);

}

public class PclVoxelGridXYZ : IDisposable {
    private IntPtr handle;

    public PclVoxelGridXYZ() => handle = create_voxel_filter_xyz();
    public void Dispose() { delete_voxel_filter(handle); handle = IntPtr.Zero; }

    public void SetLeafSize(float x, float y, float z) => set_voxel_leaf_size(handle, x, y, z);
    public void SetInputCloud(PclPointCloudXYZ cloud) => set_voxel_input_cloud(handle, cloud.Handle);
    public PclPointCloudXYZ Filter() => new PclPointCloudXYZ(apply_voxel_filter_xyz(handle));

    [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_voxel_filter_xyz();
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_voxel_filter(IntPtr ptr);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_voxel_leaf_size(IntPtr ptr, float x, float y, float z);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_voxel_input_cloud(IntPtr filterPtr, IntPtr cloudPtr);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr apply_voxel_filter_xyz(IntPtr filterPtr);

    
}


public class PclVoxelGridXYZI : IDisposable {
    private IntPtr handle;

    public PclVoxelGridXYZI() => handle = create_voxel_filter_xyzi();
    public void Dispose() { delete_voxel_filter(handle); handle = IntPtr.Zero; }

    public void SetLeafSize(float x, float y, float z) => set_voxel_leaf_size(handle, x, y, z);
    public void SetInputCloud(PclPointCloudXYZI cloud) => set_voxel_input_cloud(handle, cloud.Handle);
    public PclPointCloudXYZI Filter() => new PclPointCloudXYZI(apply_voxel_filter_xyzi(handle));

    [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_voxel_filter_xyzi();
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_voxel_filter(IntPtr ptr);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_voxel_leaf_size(IntPtr ptr, float x, float y, float z);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_voxel_input_cloud(IntPtr filterPtr, IntPtr cloudPtr);
    [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr apply_voxel_filter_xyzi(IntPtr filterPtr);

    
}

public class PclKdTreeXYZ : IDisposable {
    private IntPtr handle;

    public PclKdTreeXYZ() => handle = create_kdtree_xyz();
    public void Dispose() {
        if (handle != IntPtr.Zero) {
            delete_kdtree_xyz(handle);
            handle = IntPtr.Zero;
        }
    }

    public void SetInputCloud(PclPointCloudXYZ cloud) => set_kdtree_input_cloud_xyz(handle, cloud.Handle);

    /// 最近邻搜索（KNN）
    public (int[] indices, float[] distances) NearestKSearch(float x, float y, float z, int k) {
        int[] indices = new int[k];
        float[] distances = new float[k];
        int found = knn_search_xyz(handle, x, y, z, k, indices, distances);
        Array.Resize(ref indices, found);
        Array.Resize(ref distances, found);
        return (indices, distances);
    }

    /// 半径搜索
    public (int[] indices, float[] distances) RadiusSearch(float x, float y, float z, float radius, int maxCount = 100) {
        int[] indices = new int[maxCount];
        float[] distances = new float[maxCount];
        int found = radius_search_xyz(handle, x, y, z, radius, indices, distances, maxCount);
        Array.Resize(ref indices, found);
        Array.Resize(ref distances, found);
        return (indices, distances);
    }

    public IntPtr Handle => handle;

    #region DllImport

    [DllImport("./pcl/libpclwrapper.dylib")] private static extern IntPtr create_kdtree_xyz();

    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void delete_kdtree_xyz(IntPtr kd);

    [DllImport("./pcl/libpclwrapper.dylib")] private static extern void set_kdtree_input_cloud_xyz(IntPtr kd, IntPtr cloud);

    [DllImport("./pcl/libpclwrapper.dylib")] private static extern int knn_search_xyz(
        IntPtr kd, float x, float y, float z, int k,
        [Out] int[] indices, [Out] float[] distances);

    [DllImport("./pcl/libpclwrapper.dylib")] private static extern int radius_search_xyz(
        IntPtr kd, float x, float y, float z, float radius,
        [Out] int[] indices, [Out] float[] distances, int maxCount);

    #endregion
}