// --- utils_voxel.hpp ------------------------------------
#pragma once
#include <unordered_set>
#include <cmath>
#include <cstdint>
#include <limits>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace VoxelLite {

    // 21 bits por eixo -> máscara 0x1FFFFF
    static inline uint64_t pack_key_21b(int ix, int iy, int iz) {
        constexpr uint64_t MASK = (1ull << 21) - 1ull;  // 0x1FFFFF
        constexpr int      BIAS = 1 << 20;              // para lidar com negativos

        uint64_t kx = static_cast<uint64_t>(ix + BIAS) & MASK;
        uint64_t ky = static_cast<uint64_t>(iy + BIAS) & MASK;
        uint64_t kz = static_cast<uint64_t>(iz + BIAS) & MASK;

        // [kx | ky | kz] -> 63 bits usados
        return (kx << 42) | (ky << 21) | kz;
    }

    // Downsample para XYZ (mantém o 1º ponto de cada voxel)
    inline pcl::PointCloud<pcl::PointXYZ>::Ptr
        DownsampleVoxelLikeXYZ(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& in, float leaf) {
        auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        if (!in || in->empty()) return out;

        // guarda “voxels já vistos”
        std::unordered_set<uint64_t> seen;
        seen.reserve(in->points.size());         // evita rehash
        out->points.reserve(in->points.size());  // upper bound; no final cabeçalho ajusta

        const float inv_leaf = 1.0f / std::max(leaf, 1e-5f);

        for (const auto& p : in->points) {
            if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue;

            // voxel index
            const int ix = static_cast<int>(std::floor(p.x * inv_leaf));
            const int iy = static_cast<int>(std::floor(p.y * inv_leaf));
            const int iz = static_cast<int>(std::floor(p.z * inv_leaf));

            const uint64_t key = pack_key_21b(ix, iy, iz);
            auto [it, inserted] = seen.insert(key);
            if (inserted) {
                out->points.push_back(p);  // mantém o primeiro ponto desse voxel
            }
        }

        out->width = static_cast<uint32_t>(out->points.size());
        out->height = 1;
        out->is_dense = false;
        return out;
    }

} // namespace VoxelLite