#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <thread>
#include <chrono>
#include <mutex>
#include <atomic>
#include <iostream>
#include <iomanip>      // <- pra formatar tabela bonitinha
#include <filesystem>
#include <cstring>

#include <deque>
#include <chrono>
#include <cmath>        // sqrt etc.

#include <Eigen/Dense>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>  // downsample pra ajudar ICP

#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

#include "livox_lidar_api.h"
#include "livox_lidar_def.h"

namespace fs = std::filesystem;

// =====================================================
// ESTRUTURAS / GLOBAIS
// =====================================================

struct TimedPoint {
    pcl::PointXYZRGB p;
    uint64_t t_ms;
};

static std::deque<TimedPoint> g_buffer_temporal;
static const uint64_t WINDOW_MS = 100; // janela visível: últimos 300 ms

// nuvem atual pra render
static std::mutex g_cloud_mutex;
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
static std::atomic<bool> g_running(true);

// escolher modo de cor (true = distância radial, false = altura Z)
static bool ver_distancia = true;
static bool modo_map = false;
static bool usar_slam = false;
static bool desenhar_caixas = true;
static bool sensor_invertido = true;          // true = montado de cabeça pra baixo

// nuvem global acumulada (mapão)
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_global_map(new pcl::PointCloud<pcl::PointXYZRGB>);

// ---- IMU globals ----
// vamos guardar o último pacote de IMU recebido
static std::mutex g_imu_mutex;

// vamos manter valores "mais recentes"
static float g_imu_acc_x = 0.0f;
static float g_imu_acc_y = 0.0f;
static float g_imu_acc_z = 0.0f;
static float g_imu_gyro_x = 0.0f;
static float g_imu_gyro_y = 0.0f;
static float g_imu_gyro_z = 0.0f;
static uint64_t g_imu_last_ms = 0; // timestamp local que recebemos

// Pose estimada do LiDAR no mundo (por enquanto só rotação).
static std::mutex g_pose_mutex;
static float g_roll = 0.0f;   // rotação em torno de X
static float g_pitch = 0.0f;   // rotação em torno de Y
static float g_yaw = 0.0f;   // rotação em torno de Z
static uint64_t g_last_imu_ms = 0;

// posição global estimada (em metros)
static float g_pos_x = 0.0f;
static float g_pos_y = 0.0f;
static float g_pos_z = 0.0f;

// velocidade global estimada (m/s)
static float g_vel_x = 0.0f;
static float g_vel_y = 0.0f;
static float g_vel_z = 0.0f;

// Pose global estimada via ICP+IMU
// OBS: agora quem manda em posição é ICP, e não mais a integração da aceleração.
// A gente NÃO vai mais usar g_pos_x/g_pos_y/g_pos_z que vinha da IMU que driftava infinito.
// Em vez disso, vamos manter uma matriz 4x4 acumulada que representa a pose do LiDAR no "mapa global".

static Eigen::Matrix4f g_pose_Twm = Eigen::Matrix4f::Identity();
// Twm = Transform from sensor(local frame "m") to world "w".
// (world é o mapa global onde estamos acumulando pontos)

// nuvem 'keyframe' anterior para ICP
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_prev_cloud_raw(new pcl::PointCloud<pcl::PointXYZRGB>);

// flag pra saber se já temos prev_cloud inicializada
static bool g_has_prev_frame = false;

// acumula pontos estabilizados (rotacionados pela IMU) até formar um "keyframe"
static pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_keyframe_accum(new pcl::PointCloud<pcl::PointXYZRGB>);

// timestamp de quando começamos a acumular esse keyframe
static uint64_t g_keyframe_start_ms = 0;

static const uint64_t KEYFRAME_WINDOW_MS = 150; // ~150ms de varredura por keyframe
static const size_t   KEYFRAME_MIN_POINTS = 2000; // se ainda não bateu tempo mas já tem bastante ponto
static const size_t   KEYFRAME_MAX_POINTS = 20000; // se passou disso também fecha logo

static uint64_t g_last_detection_ms = 0;
static const uint64_t DETECTION_PERIOD_MS = 100; // roda cluster a cada ~300ms

static float g_temp_c = 0.0f;
static uint32_t g_power_count = 0;

std::atomic<uint32_t> g_handle{ 0 };




inline float _rad2deg(float r) { return r * 180.0f / static_cast<float>(M_PI); }

// value em [0,1] -> cor estilo "jet"
static inline void _ColorMapJet(float value, uint8_t& r, uint8_t& g, uint8_t& b) {
    if (value < 0.0f) value = 0.0f;
    if (value > 1.0f) value = 1.0f;

    float fourValue = 4.0f * value;

    float r_f = std::min(std::max(std::min(fourValue - 1.5f, -fourValue + 4.5f), 0.0f), 1.0f);
    float g_f = std::min(std::max(std::min(fourValue - 0.5f, -fourValue + 3.5f), 0.0f), 1.0f);
    float b_f = std::min(std::max(std::min(fourValue + 0.5f, -fourValue + 2.5f), 0.0f), 1.0f);

    r = static_cast<uint8_t>(r_f * 255.0f);
    g = static_cast<uint8_t>(g_f * 255.0f);
    b = static_cast<uint8_t>(b_f * 255.0f);
}

static Eigen::Matrix3f _R_from_rpy(float roll, float pitch, float yaw) {
    float cr = std::cos(roll);
    float sr = std::sin(roll);
    float cp = std::cos(pitch);
    float sp = std::sin(pitch);
    float cy = std::cos(yaw);
    float sy = std::sin(yaw);

    Eigen::Matrix3f R;
    R << cy * cp, cy* sp* sr - sy * cr, cy* sp* cr + sy * sr,
        sy* cp, sy* sp* sr + cy * cr, sy* sp* cr - cy * sr,
        -sp, cp* sr, cp* cr;
    return R;
}

struct VoxelKey {
    int ix, iy, iz;
    bool operator==(const VoxelKey& o) const {
        return ix == o.ix && iy == o.iy && iz == o.iz;
    }
};

// hash pra usar VoxelKey como chave em unordered_map
struct VoxelKeyHash {
    std::size_t operator()(const VoxelKey& k) const noexcept {
        // hash combinando 3 ints
        // isso não precisa ser perfeito, só razoável
        std::size_t h1 = std::hash<int>()(k.ix);
        std::size_t h2 = std::hash<int>()(k.iy);
        std::size_t h3 = std::hash<int>()(k.iz);
        return h1 ^ (h2 << 1) ^ (h3 << 2);
    }
};

pcl::PointCloud<pcl::PointXYZRGB>::Ptr _DownsampleVoxelLike(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& in, float leaf)
{
    auto out = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
    out->points.reserve(in->points.size());

    std::unordered_map<VoxelKey, pcl::PointXYZRGB, VoxelKeyHash> voxels;
    voxels.reserve(in->points.size());

    const float inv_leaf = 1.0f / leaf;

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

        VoxelKey key{
            (int)std::floor(p.x * inv_leaf),
            (int)std::floor(p.y * inv_leaf),
            (int)std::floor(p.z * inv_leaf)
        };

        // mantém o primeiro ponto daquele voxel
        if (voxels.find(key) == voxels.end()) {
            voxels[key] = p;
        }
    }

    out->points.reserve(voxels.size());
    for (const auto& kv : voxels) {
        out->points.push_back(kv.second);
    }

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

    return out;
}

void _DesenharBussolaDistancias(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    // --- limpa linhas antigas (mantém só as novas por frame)
    const std::vector<std::string> nomes = {
        "linha_frente", "linha_tras", "linha_esquerda", "linha_direita",
        "linha_cima", "linha_baixo",
        "texto_frente", "texto_tras", "texto_esquerda", "texto_direita",
        "texto_cima", "texto_baixo"
    };
    for (auto& n : nomes) viewer->removeShape(n);

    auto distancia_direcao = [&](auto seletor) {
        std::vector<float> dists;
        dists.reserve(cloud->points.size());
        for (const auto& p : cloud->points) {
            if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue;
            if (seletor(p)) {
                float dist = std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
                dists.push_back(dist);
            }
        }
        if (dists.size() < 5) return 9999.f; // muito poucos pontos = inválido

        std::sort(dists.begin(), dists.end());
        // média dos 10 menores (ou menos se tiver poucos)
        int n = std::min<int>(10, dists.size());
        float soma = 0.f;
        for (int i = 0; i < n; ++i) soma += dists[i];
        return soma / n;
        };

    float frente = distancia_direcao([](const auto& p) { return p.y > 0 && std::fabs(p.x) < 0.5; });
    float tras = distancia_direcao([](const auto& p) { return p.y < 0 && std::fabs(p.x) < 0.5; });
    float direita = distancia_direcao([](const auto& p) { return p.x > 0 && std::fabs(p.y) < 0.5; });
    float esquerda = distancia_direcao([](const auto& p) { return p.x < 0 && std::fabs(p.y) < 0.5; });
    float cima = distancia_direcao([](const auto& p) { return p.z > 0; });
    float baixo = distancia_direcao([](const auto& p) { return p.z < 0; });

    // --- função utilitária
    auto desenhar = [&](const std::string& nome, const Eigen::Vector3f& dir, float dist, const Eigen::Vector3f& cor, const std::string& label) {
        if (dist >= 9990.f) dist = 0.0f; // sem obstáculo
        Eigen::Vector3f origem(0, 0, 0);
        Eigen::Vector3f destino = dir.normalized() * dist;
        viewer->addLine(
            pcl::PointXYZ(origem.x(), origem.y(), origem.z()),
            pcl::PointXYZ(destino.x(), destino.y(), destino.z()),
            cor.x(), cor.y(), cor.z(),
            nome);

        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, nome);

        // adiciona o texto no final da linha
        char txt[64];
        if (dist > 0.001f)
            std::snprintf(txt, sizeof(txt), "%s: %.0f cm", label.c_str(), dist * 100.f);
        else
            std::snprintf(txt, sizeof(txt), "%s: --", label.c_str());

        pcl::PointXYZ pos(destino.x(), destino.y(), destino.z());
        viewer->addText3D(txt, pos, 0.05, cor.x(), cor.y(), cor.z(), "texto_" + label);
        };



    // --- desenha as seis direções
    desenhar("linha_frente", { 1,0,0 }, frente, { 1,0,0 }, "frente");
    desenhar("linha_tras", { -1,0,0 }, tras, { 0.8,0,0 }, "tras");
    desenhar("linha_direita", { 0,1,0 }, direita, { 0,1,0 }, "direita");
    desenhar("linha_esquerda", { 0,-1,0 }, esquerda, { 0,0.8,0 }, "esquerda");
    desenhar("linha_cima", { 0,0,1 }, cima, { 0,0,1 }, "cima");
    desenhar("linha_baixo", { 0,0,-1 }, baixo, { 0,0,0.8 }, "baixo");
}

void _DetectarClustersEAdicionarCaixas(pcl::visualization::PCLVisualizer::Ptr& viewer, pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud_in)
{
    if (!desenhar_caixas) {
        // Se não vamos desenhar caixas, limpa overlay e sai
        for (int i = 0; i < 100; ++i) {
            std::string box_id = "bbox_" + std::to_string(i);
            viewer->removeShape(box_id);
        }
        return;
    }

    // Limpa overlay anterior (caixas antigas)
    for (int i = 0; i < 100; ++i) {
        std::string box_id = "bbox_" + std::to_string(i);
        viewer->removeShape(box_id);
    }

    if (!cloud_in || cloud_in->empty()) {
        return;
    }

    // ============================================================
    // 1. ROI (Região de Interesse)
    //    - Só objetos até ~5m de raio
    //    - Ignorar muito baixo (abaixo do chão) / muito alto
    //    IMPORTANTE: ajustar limites de z depois que você montar o sensor.
    // ============================================================

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr roi(new pcl::PointCloud<pcl::PointXYZRGB>);
    roi->reserve(cloud_in->points.size());

    for (const auto& p : cloud_in->points) {
        float dist2 = p.x * p.x + p.y * p.y + p.z * p.z;
        // até 5m -> 5 * 5 = 25
        if (dist2 > 9.0f) continue;

        // corta lixo MUITO baixo ou MUITO alto
        // ajuste isso conforme a montagem do LiDAR:
        // Exemplo:
        //   se o LiDAR está a ~0.5m do chão e Z cresce pra cima,
        //   você pode querer ignorar z < -0.2 e z > 2m
        if (p.z < -0.2f) continue;
        if (p.z > 2.0f)  continue;

        roi->points.push_back(p);
    }

    if (roi->points.size() < 20) {
        // quase nada perto -> nada pra detectar
        return;
    }

    roi->width = static_cast<uint32_t>(roi->points.size());
    roi->height = 1;
    roi->is_dense = true;
    
    // ============================================================
    // 2. Downsample (voxel grid)
    //    - A gente não quer cada pontinho, quer blocos de ~7cm
    //    - Isso mata ruído fino tipo perna de tripé fina / cabo fino
    // ============================================================

    //pcl::VoxelGrid<pcl::PointXYZRGB> vg;
    //vg.setInputCloud(roi);
    //vg.setLeafSize(0.05f, 0.05f, 0.05f); // ~7cm
    //pcl::PointCloud<pcl::PointXYZRGB>::Ptr coarse(new pcl::PointCloud<pcl::PointXYZRGB>);
    //vg.filter(*coarse);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr coarse = _DownsampleVoxelLike(roi, 0.05f);

    if (coarse->points.size() < 20) {
        return;
    }

    _DesenharBussolaDistancias(viewer, coarse);

    // ============================================================
    // 3. Remover "chão" rápido via limiar de altura
    //
    // Aqui NÃO estamos fazendo RANSAC (caro).
    // Supondo que o sensor fica a uma altura razoavelmente fixa e o eixo Z
    // está alinhado com "vertical".
    //
    // Ideia: tudo muito baixo (tipo até 10cm do chão local) é chão.
    // E o que está acima disso (0.10m, 0.15m...) é obstáculo.
    //
    // IMPORTANTE: você precisa ajustar esse limite quando souber
    // a altura do sensor no robô. Aqui vou deixar 0.10 m.
    // ============================================================

    const float ALTURA_SENSOR = 0.70f;      // metros
    const float TOLERANCIA = 0.00f;         // metros

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtrada(new pcl::PointCloud<pcl::PointXYZRGB>);
    filtrada->reserve(coarse->points.size());

    for (const auto& p : coarse->points) {
        if (!sensor_invertido) {
            // sensor normal: remove chão
            float limite_chao = -ALTURA_SENSOR + TOLERANCIA;
            if (p.z < limite_chao)
                continue;
        }
        else {
            // sensor invertido: remove teto
            float limite_teto = ALTURA_SENSOR - TOLERANCIA;
            if (p.z > limite_teto)
                continue;
        }
        filtrada->points.push_back(p);
    }

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

    // ============================================================
    // 4. Clusterização Euclidiana
    //    - Agora rodamos cluster só em "acima do chão"
    //    - Com muito menos pontos, bem mais leve
    // ============================================================

    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
    tree->setInputCloud(filtrada);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
    ec.setClusterTolerance(0.20f);    // 20cm de vizinhança pra juntar pontos
    ec.setMinClusterSize(50);         // ignora micro-coisas
    ec.setMaxClusterSize(100000);     // bem grande
    ec.setSearchMethod(tree);
    ec.setInputCloud(filtrada);
    ec.extract(cluster_indices);

    // ============================================================
    // 5. Criar bounding boxes por cluster
    //    - Só desenha clusters grandes o suficiente
    //    - Você falou: queremos ignorar coisa < ~30cm.
    //      Vamos aplicar esse filtro aqui.
    // ============================================================

    const float MIN_SIZE_FOR_BOX = 0.20f; // 30cm mínimo num dos eixos

    int cluster_id = 0;

    for (const auto& indices : cluster_indices) {
        if (indices.indices.size() < 30)
            continue; // redundância com MinClusterSize

        float min_x = std::numeric_limits<float>::max();
        float max_x = -std::numeric_limits<float>::max();
        float min_y = std::numeric_limits<float>::max();
        float max_y = -std::numeric_limits<float>::max();
        float min_z = std::numeric_limits<float>::max();
        float max_z = -std::numeric_limits<float>::max();

        for (int idx : indices.indices) {
            const auto& p = (*filtrada)[idx];

            if (p.x < min_x) min_x = p.x;
            if (p.x > max_x) max_x = p.x;
            if (p.y < min_y) min_y = p.y;
            if (p.y > max_y) max_y = p.y;
            if (p.z < min_z) min_z = p.z;
            if (p.z > max_z) max_z = p.z;
        }

        float size_x = max_x - min_x;
        float size_y = max_y - min_y;
        float size_z = max_z - min_z;

        // filtra clusters muito pequenos
        if (size_x < MIN_SIZE_FOR_BOX &&
            size_y < MIN_SIZE_FOR_BOX &&
            size_z < MIN_SIZE_FOR_BOX) {
            continue;
        }

        float cx = (min_x + max_x) * 0.5f;
        float cy = (min_y + max_y) * 0.5f;
        float cz = (min_z + max_z) * 0.5f;

        std::string box_id = "bbox_" + std::to_string(cluster_id);

        viewer->addCube(
            cx - size_x * 0.5f, cx + size_x * 0.5f,
            cy - size_y * 0.5f, cy + size_y * 0.5f,
            cz - size_z * 0.5f, cz + size_z * 0.5f,
            1.0, 0.0, 0.0, // vermelho (você pode mudar depois)
            box_id
        );

        viewer->setShapeRenderingProperties(
            pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
            pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME,
            box_id);

        viewer->setShapeRenderingProperties(
            pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,
            2.0,
            box_id);

        cluster_id++;
        if (cluster_id >= 100) break; // segurança pra não floodar
    }
}


// =====================================================
// CALLBACK DE QUERY INTERNAL INFO
// =====================================================
void _OnQueryInternalInfo(livox_status status, uint32_t handle, LivoxLidarDiagInternalInfoResponse* response, void* client_data) {

    std::cout << "[QUERY] InternalInfo callback status=" << status
        << " handle=" << handle << "\n";

    if (status != 0) {
        std::cout << "[QUERY] ainda não pronto\n";
        return;
    }

    // Se chegou aqui, o lidar está pronto pra receber comando.
    // Agora mandamos os passos:

    livox_status wm = SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr);
    std::cout << "[QUERY] SetLivoxLidarWorkMode ret=" << wm << "\n";

    livox_status dt = SetLivoxLidarPclDataType(handle, kLivoxLidarCartesianCoordinateHighData, nullptr, nullptr);
    std::cout << "[QUERY] SetLivoxLidarPclDataType ret=" << dt << "\n";

    livox_status en = EnableLivoxLidarPointSend(handle, nullptr, nullptr);
    std::cout << "[QUERY] EnableLivoxLidarPointSend ret=" << en << "\n";
}

// =====================================================
// CALLBACK DE NUVEM DE PONTOS
// =====================================================
void _LidarPointCloudCallback(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* packet, void* /*client_data*/) {
    g_handle.store(handle, std::memory_order_relaxed);
    using namespace std::chrono;
    uint64_t now_ms = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();

    std::lock_guard<std::mutex> lock(g_cloud_mutex);

    // parâmetros do nosso color map
    const float z_min = -0.2f;
    const float z_max = 1.5f;
    const float z_range = (z_max - z_min);

    // vamos guardar os pontos "novos" desse pacote numa lista temporária,
    // pra poder reutilizar tanto no modo Live quanto no modo Map
    std::vector<TimedPoint> new_points;
    new_points.reserve(packet->dot_num);

    auto make_point = [&](float x, float y, float z) {
        TimedPoint tp;
        tp.p.x = x;
        tp.p.y = y;
        tp.p.z = z;

        uint8_t rr, gg, bb;
        if (ver_distancia) {
            float dist = std::sqrt(x * x + y * y + z * z); // metros
            float norm = dist / 5.0f; // normaliza em ~5m
            _ColorMapJet(norm, rr, gg, bb);
        }
        else {
            float norm = (z - z_min) / (z_range > 0.0001f ? z_range : 1.0f);
            _ColorMapJet(norm, rr, gg, bb);
        }

        tp.p.r = rr;
        tp.p.g = gg;
        tp.p.b = bb;

        tp.t_ms = now_ms;
        new_points.push_back(tp);
        };

    // 1. pegar pontos do pacote e montar new_points
    if (packet->data_type == kLivoxLidarCartesianCoordinateHighData) {
        auto* pts = reinterpret_cast<LivoxLidarCartesianHighRawPoint*>(packet->data);

        for (uint16_t i = 0; i < packet->dot_num; ++i) {
            float x = pts[i].x / 1000.0f;
            float y = pts[i].y / 1000.0f;
            float z = pts[i].z / 1000.0f;
            make_point(x, y, z);
        }
    }
    else if (packet->data_type == kLivoxLidarCartesianCoordinateLowData) {
        auto* pts = reinterpret_cast<LivoxLidarCartesianLowRawPoint*>(packet->data);

        for (uint16_t i = 0; i < packet->dot_num; ++i) {
            float x = pts[i].x / 100.0f;
            float y = pts[i].y / 100.0f;
            float z = pts[i].z / 100.0f;
            make_point(x, y, z);
        }
    }
    else {
        // outros formatos ainda não tratados
    }

    // 2. dependendo do modo, tratamos diferente
    if (!modo_map) {
        // ------ MODO LIVE ------
        // empurrar os pontos novos pro buffer temporal
        for (const auto& tp : new_points) {
            g_buffer_temporal.push_back(tp);
        }

        // remover pontos velhos > WINDOW_MS
        const uint64_t cutoff = now_ms - WINDOW_MS;
        while (!g_buffer_temporal.empty() &&
            g_buffer_temporal.front().t_ms < cutoff) {
            g_buffer_temporal.pop_front();
        }

        // reconstruir g_cloud a partir da janela temporal
        g_cloud->points.clear();
        g_cloud->points.reserve(g_buffer_temporal.size());
        for (const auto& tp : g_buffer_temporal) {
            g_cloud->points.push_back(tp.p);
        }

        g_cloud->width = static_cast<uint32_t>(g_cloud->points.size());
        g_cloud->height = 1;
        g_cloud->is_dense = true;
    }
    else {
        // ------ MODO MAP COM ICP ROBUSTO ------

        using namespace std::chrono;

        // se o SLAM estiver desativado, apenas acumula os pontos diretamente
        if (!usar_slam) {
            Eigen::Matrix3f R_imu = _R_from_rpy(g_roll, g_pitch, g_yaw);

            for (const auto& tp : new_points) {
                Eigen::Vector3f v_local(tp.p.x, tp.p.y, tp.p.z);
                Eigen::Vector3f v_rot = R_imu * v_local;

                pcl::PointXYZRGB p_out;
                p_out.x = v_rot.x();
                p_out.y = v_rot.y();
                p_out.z = v_rot.z();
                p_out.r = tp.p.r;
                p_out.g = tp.p.g;
                p_out.b = tp.p.b;

                g_global_map->points.push_back(p_out);
            }

            g_global_map->width = (uint32_t)g_global_map->points.size();
            g_global_map->height = 1;
            g_global_map->is_dense = true;

            *g_cloud = *g_global_map;
            return;
        }

        // 1. pega atitude atual
        float roll, pitch, yaw;
        {
            std::lock_guard<std::mutex> lock_pose(g_pose_mutex);
            roll = g_roll;
            pitch = g_pitch;
            yaw = g_yaw;
        }

        Eigen::Matrix3f R_imu = _R_from_rpy(roll, pitch, yaw);

        // 2. converte os pontos desse pacote para o frame estabilizado (IMU)
        //    e acumula em g_keyframe_accum
        uint64_t now_ms = duration_cast<milliseconds>(
            steady_clock::now().time_since_epoch()
        ).count();

        if (g_keyframe_start_ms == 0) {
            g_keyframe_start_ms = now_ms;
        }

        for (const auto& tp : new_points) {
            Eigen::Vector3f v_local(tp.p.x, tp.p.y, tp.p.z);
            Eigen::Vector3f v_rot = R_imu * v_local;

            pcl::PointXYZRGB p_out;
            p_out.x = v_rot.x();
            p_out.y = v_rot.y();
            p_out.z = v_rot.z();
            p_out.r = tp.p.r;
            p_out.g = tp.p.g;
            p_out.b = tp.p.b;

            g_keyframe_accum->points.push_back(p_out);
        }

        // 3. decidiu fechar keyframe?
        bool close_keyframe = false;

        uint64_t elapsed = now_ms - g_keyframe_start_ms;
        size_t   npts = g_keyframe_accum->points.size();

        if (elapsed >= KEYFRAME_WINDOW_MS && npts >= KEYFRAME_MIN_POINTS) {
            close_keyframe = true;
        }
        if (npts >= KEYFRAME_MAX_POINTS) {
            close_keyframe = true;
        }

        if (!close_keyframe) {
            // ainda não formou um keyframe completo
            // atualiza visualização só pra você continuar vendo algo (opcional)
            // aqui a gente só mostra o mapa atual sem adicionar nada novo
            *g_cloud = *g_global_map;
            return;
        }

        // ------ FECHANDO KEYFRAME ------

        // monta um ptr pra esse keyframe atual completo
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr curr_keyframe(
            new pcl::PointCloud<pcl::PointXYZRGB>);
        *curr_keyframe = *g_keyframe_accum;
        curr_keyframe->width = (uint32_t)curr_keyframe->points.size();
        curr_keyframe->height = 1;
        curr_keyframe->is_dense = true;

        // limpa o acumulador pro próximo ciclo
        g_keyframe_accum->points.clear();
        g_keyframe_accum->width = 0;
        g_keyframe_accum->height = 1;
        g_keyframe_accum->is_dense = true;
        g_keyframe_start_ms = now_ms;

        // se esse é o primeiro keyframe, só inicializa estado
        if (!g_has_prev_frame) {
            *g_prev_cloud_raw = *curr_keyframe;
            g_has_prev_frame = true;

            // injeta esse primeiro keyframe no mapa global com a pose inicial
            for (const auto& p_in : curr_keyframe->points) {
                Eigen::Vector4f v_local(p_in.x, p_in.y, p_in.z, 1.0f);
                Eigen::Vector4f v_world = g_pose_Twm * v_local;

                pcl::PointXYZRGB p_out;
                p_out.x = v_world.x();
                p_out.y = v_world.y();
                p_out.z = v_world.z();
                p_out.r = p_in.r;
                p_out.g = p_in.g;
                p_out.b = p_in.b;

                g_global_map->points.push_back(p_out);
            }

            g_global_map->width = (uint32_t)g_global_map->points.size();
            g_global_map->height = 1;
            g_global_map->is_dense = true;

            *g_cloud = *g_global_map;
            return;
        }

        // ------ já temos prev frame: ICP entre keyframes ------

        // downsample prev e curr pra robustez
        pcl::VoxelGrid<pcl::PointXYZRGB> voxel;
        voxel.setLeafSize(0.03f, 0.03f, 0.03f);

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr curr_ds(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr prev_ds(new pcl::PointCloud<pcl::PointXYZRGB>);

        voxel.setInputCloud(curr_keyframe);
        voxel.filter(*curr_ds);

        voxel.setInputCloud(g_prev_cloud_raw);
        voxel.filter(*prev_ds);

        // roda ICP
        pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
        icp.setInputSource(curr_ds);   // atual
        icp.setInputTarget(prev_ds);   // anterior
        icp.setMaximumIterations(40);
        icp.setTransformationEpsilon(1e-6);
        icp.setEuclideanFitnessEpsilon(1e-6);
        icp.setMaxCorrespondenceDistance(0.30f); // ~30 cm, porque agora é keyframe vs keyframe

        pcl::PointCloud<pcl::PointXYZRGB> icp_aligned;
        icp.align(icp_aligned);

        bool accept_icp = false;
        Eigen::Matrix4f T_prev_to_curr = Eigen::Matrix4f::Identity();

        if (icp.hasConverged()) {
            double score = icp.getFitnessScore();
            // std::cout << "[ICP KF] fitness=" << score << " points=" << curr_ds->points.size() << "\n";

            if (score < 0.15) {
                // final transformation: curr -> prev
                Eigen::Matrix4f T_curr_to_prev = icp.getFinalTransformation();
                T_prev_to_curr = T_curr_to_prev.inverse();
                accept_icp = true;
            }
        }

        if (accept_icp) {
            g_pose_Twm = g_pose_Twm * T_prev_to_curr;
        }
        else {
            // se não aceitamos o ICP, não mexe a pose (evita teleporte)
            // isso também evita mapear coisa aleatória
        }

        // agora insere o keyframe atual no mapa global usando a pose global atual
        for (const auto& p_in : curr_keyframe->points) {
            Eigen::Vector4f v_local(p_in.x, p_in.y, p_in.z, 1.0f);
            Eigen::Vector4f v_world = g_pose_Twm * v_local;

            pcl::PointXYZRGB p_out;
            p_out.x = v_world.x();
            p_out.y = v_world.y();
            p_out.z = v_world.z();
            p_out.r = p_in.r;
            p_out.g = p_in.g;
            p_out.b = p_in.b;

            g_global_map->points.push_back(p_out);
        }

        g_global_map->width = (uint32_t)g_global_map->points.size();
        g_global_map->height = 1;
        g_global_map->is_dense = true;

        // prepara pro próximo ICP
        *g_prev_cloud_raw = *curr_keyframe;

        // atualiza o viewer
        *g_cloud = *g_global_map;
    }
}

// =====================================================
// CALLBACK DE IMU
// =====================================================
void _LidarImuDataCallback(const uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket* packet, void* /*client_data*/) {

    using namespace std::chrono;
    uint64_t now_ms = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();

    if (!packet || packet->dot_num == 0) {
        return;
    }

    // pega a última amostra do pacote de IMU
    auto* imu_points = reinterpret_cast<LivoxLidarImuRawPoint*>(packet->data);
    uint16_t n = packet->dot_num;
    LivoxLidarImuRawPoint imu = imu_points[n - 1];

    // -------------------------
    // 1. guardar IMU pro HUD
    // -------------------------
    {
        std::lock_guard<std::mutex> lock(g_imu_mutex);

        g_imu_gyro_x = imu.gyro_x;
        g_imu_gyro_y = imu.gyro_y;
        g_imu_gyro_z = imu.gyro_z;

        g_imu_acc_x = imu.acc_x;
        g_imu_acc_y = imu.acc_y;
        g_imu_acc_z = imu.acc_z;

        g_imu_last_ms = now_ms;
    }

    // -------------------------------------------------
    // 2. integrar atitude (roll/pitch/yaw)
    //    + filtro complementar com gravidade
    // -------------------------------------------------
    // assumindo gyro_* em rad/s
    float gx = imu.gyro_x;
    float gy = imu.gyro_y;
    float gz = imu.gyro_z;

    // assumindo acc_* em "g" (1.0 ~= 9.81 m/s^2),
    // vamos converter pra m/s^2
    // se já vier em m/s^2 no seu SDK, coloque scale_acc = 1.0f
    const float g_m_s2 = 9.81f;
    float ax_raw = imu.acc_x * g_m_s2;
    float ay_raw = imu.acc_y * g_m_s2;
    float az_raw = imu.acc_z * g_m_s2;

    std::lock_guard<std::mutex> pose_lock(g_pose_mutex);

    if (g_last_imu_ms == 0) {
        // primeira leitura: só inicializa timestamp
        g_last_imu_ms = now_ms;
        return;
    }

    float dt = (now_ms - g_last_imu_ms) / 1000.0f; // segundos
    g_last_imu_ms = now_ms;

    // integração dos gyros (atitude "livre")
    float roll_gyro = g_roll + gx * dt;
    float pitch_gyro = g_pitch + gy * dt;
    float yaw_gyro = g_yaw + gz * dt;

    // estima roll/pitch absoluto a partir da gravidade
    // assumindo eixos IMU:
    //  X: frente
    //  Y: esquerda
    //  Z: cima
    // pitch = inclinação pra frente/trás
    // roll  = inclinação pros lados
    float denom = std::sqrt(ay_raw * ay_raw + az_raw * az_raw);
    if (denom < 1e-6f) denom = 1e-6f;

    float roll_acc = std::atan2(ay_raw, az_raw);
    float pitch_acc = std::atan2(-ax_raw, denom);

    // filtro complementar
    const float alpha = 0.98f;
    float fused_roll = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
    float fused_pitch = alpha * pitch_gyro + (1.0f - alpha) * pitch_acc;
    float fused_yaw = yaw_gyro; // yaw ainda só do gyro

    // atualiza atitude global
    g_roll = fused_roll;
    g_pitch = fused_pitch;
    g_yaw = fused_yaw;
}

// =====================================================
// CALLBACK DE INFO / ESTADO DO LIDAR
// =====================================================
void _LidarInfoChangeCallback(const uint32_t handle, const LivoxLidarInfo* info, void* /*client_data*/) {

    std::cout << "[INFO] LidarInfoChangeCb CHAMADO!\n";

    if (!info) {
        std::cout << "[INFO] info == NULL\n";
        return;
    }

    g_handle.store(handle, std::memory_order_relaxed);

    std::cout << "[INFO] handle=" << handle
        << " dev_type=" << (int)info->dev_type
        << " sn=" << info->sn
        << " ip=" << info->lidar_ip
        << std::endl;

    // coloca em modo normal
    livox_status wm = SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr);
    std::cout << "[INFO] SetLivoxLidarWorkMode ret=" << wm << std::endl;

    livox_status dt = SetLivoxLidarPclDataType(handle, kLivoxLidarCartesianCoordinateHighData, nullptr, nullptr);
    std::cout << "[INFO] SetLivoxLidarPclDataType ret=" << dt << std::endl;

    // habilita envio de pontos
    livox_status en = EnableLivoxLidarPointSend(handle, nullptr, nullptr);
    std::cout << "[INFO] EnableLivoxLidarPointSend ret=" << en << std::endl;
}

// =====================================================
// CALLBACK DE DIAGNOSTICO INTERNO
// =====================================================
void _InternalInfoCallback(livox_status status, uint32_t handle, LivoxLidarDiagInternalInfoResponse* response, void* client_data)
{
    if (status != kLivoxLidarStatusSuccess || !response) {
        std::cerr << "Falha ao obter info interna (status "
            << status << ")" << std::endl;
        return;
    }

    const uint8_t* ptr = response->data;

    for (uint16_t i = 0; i < response->param_num; ++i) {
        uint16_t id = *(uint16_t*)(ptr + 0);
        uint16_t length = *(uint16_t*)(ptr + 2);
        const uint8_t* val = ptr + 4;

        switch (id) {
        case 0x8007: { // temperatura x100
            if (length >= 4) {
                uint32_t raw = *(uint32_t*)val;
                g_temp_c = static_cast<float>(raw) / 100.0f;
                //std::cout << "[Diag] Temperatura interna: "
                //    << g_temp_c << " C" << std::endl;
            }
            break;
        }
        case 0x8008: { // contador de power-on
            if (length >= 4) {
                g_power_count = *(uint32_t*)val;
                //std::cout << "[Diag] Power-on count: "
                //    << g_power_count << std::endl;
            }
            break;
        }
        default:
            break; // ignora outros parâmetros
        }

        ptr += (4 + length);
    }
}

// =====================================================
// CALLBACK DE COMANDOS DO TECLADO
// =====================================================
void _KeyboardCallback(const pcl::visualization::KeyboardEvent& event, void*) {
    if (event.keyDown()) {
        if (event.getKeySym() == "m" || event.getKeySym() == "M") {
            modo_map = !modo_map;
            std::cout << "\n[KEY] Modo alterado para: "
                << (modo_map ? "MAP" : "LIVE") << std::endl;
        }
        if (event.getKeySym() == "s" || event.getKeySym() == "S") {
            usar_slam = !usar_slam;
            std::cout << "\n[KEY] SLAM: "
                << (usar_slam ? "ATIVADO (ICP ON)" : "DESATIVADO (bruto)") << std::endl;
        }
        if (event.getKeySym() == "d" || event.getKeySym() == "D") {
            ver_distancia = !ver_distancia;
            std::cout << "\n[KEY] Modo alterado para: "
                << (ver_distancia ? "DISTANCIA" : "ALTURA") << std::endl;
        }
        if (event.getKeySym() == "b" || event.getKeySym() == "B") {
            desenhar_caixas = !desenhar_caixas;
            std::cout << "\n[KEY] Caixas de obstaculo: "
                << (desenhar_caixas ? "ON" : "OFF") << std::endl;
        }
        if (event.getKeySym() == "i" || event.getKeySym() == "I") {
            sensor_invertido = !sensor_invertido;
        }
    }
}

// =====================================================
// MAIN
// =====================================================
int _main(int argc, char** argv) {
    if (argc < 2) {
        printf("Params Invalid, must input config path.\n");
        return -1;
    }
    const std::string cfg_str = argv[1];
    fs::path cfg = cfg_str;

    // monta caminho do config.json na mesma pasta do exe
    //fs::path exe_dir = fs::current_path();
    //fs::path cfg = exe_dir / "config.json";

    std::cout << "[MAIN] usando config: " << cfg.string() << std::endl;

    // init SDK
    if (!LivoxLidarSdkInit(cfg.string().c_str())) {
        std::cout << "[ERR] LivoxLidarSdkInit falhou\n";
        LivoxLidarSdkUninit();
        return -1;
    }
    std::cout << "[MAIN] LivoxLidarSdkInit OK\n";

    // registra callbacks principais
    SetLivoxLidarPointCloudCallBack(_LidarPointCloudCallback, nullptr);
    std::cout << "[MAIN] SetLivoxLidarPointCloudCallBack OK\n";

    //SetLivoxLidarInfoChangeCallback(_LidarInfoChangeCallback, nullptr);
    //std::cout << "[MAIN] SetLivoxLidarInfoChangeCallback OK\n";

    SetLivoxLidarImuDataCallback(_LidarImuDataCallback, nullptr);
    std::cout << "[MAIN] SetLivoxLidarImuDataCallback OK\n";

    // inicia sdk
    if (!LivoxLidarSdkStart()) {
        std::cout << "[ERR] LivoxLidarSdkStart falhou\n";
        LivoxLidarSdkUninit();
        return -1;
    }
    std::cout << "[MAIN] LivoxLidarSdkStart OK\n";

    // cria viewer PCL
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Mid-360 Live"));

    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    viewer->registerKeyboardCallback(_KeyboardCallback);

    {
        std::lock_guard<std::mutex> lock(g_cloud_mutex);
        g_cloud->clear();
        g_cloud->width = 1;
        g_cloud->height = 1;
        g_cloud->is_dense = true;
    }

    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(g_cloud);
    viewer->addPointCloud<pcl::PointXYZRGB>(g_cloud, rgb, "livox_cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "livox_cloud");

    std::cout << "[MAIN] Viewer + SDK rodando. Fecha a janela pra sair.\n";

    auto h = g_handle.load(std::memory_order_relaxed);
    std::cout << "[DEBUG] Aguardando lidar ficar pronto no handle " << h << std::endl;
    for (int i = 0; i < 50; ++i) { // tenta ~50 vezes
        if (h) {
            livox_status q = QueryLivoxLidarInternalInfo(h, _OnQueryInternalInfo, nullptr);
            if (q == kLivoxLidarStatusSuccess) break;
            std::cout << "[DEBUG] QueryLivoxLidarInternalInfo ret=" << q << " tentativa=" << i << std::endl;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    // loop principal
    while (!viewer->wasStopped() && g_running.load()) {

        // snapshot nuvem atual que vamos desenhar/usar pra percepção
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_copy(new pcl::PointCloud<pcl::PointXYZRGB>);
        {
            std::lock_guard<std::mutex> lock(g_cloud_mutex);
            *local_copy = *g_cloud;
        }

        // atualiza visualização da nuvem
        {
            pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb2(local_copy);
            viewer->updatePointCloud<pcl::PointXYZRGB>(local_copy, rgb2, "livox_cloud");
        }

        // HUD topo (temperatura + IMU)
        viewer->removeShape("hud_top");
        {
            // Lê tamanho atual da janela para posicionar no topo
            auto rw = viewer->getRenderWindow();
            int* size = rw->GetSize();
            int win_w = size[0];
            int win_h = size[1];

            // Margens e altura da linha
            const int margin = 10;
            const int font_px = 14;
            const int line_h = font_px + 4;

            // Topo-esquerdo: x = margin, y = (altura - margin - line_h)
            const int x = margin;
            const int y = win_h - margin - line_h;

            std::stringstream ss;
            ss.setf(std::ios::fixed);
            ss << std::setprecision(1);

            // Monte a linha: TEMP + Euler (graus)
            ss << "TEMP: " << g_temp_c << " gC"
                << "  |  R: " << _rad2deg(g_roll) << "g"
                << "  P: " << _rad2deg(g_pitch) << "g"
                << "  Y: " << _rad2deg(g_yaw) << "g";

            // texto branco (1,1,1); id único "hud_top"
            viewer->addText(ss.str(), x, y, font_px, 1.0, 1.0, 1.0, "hud_top");
        }

        // HUD bottom
        viewer->removeShape("hud_text");
        {
            std::stringstream ss;
            ss << (modo_map ? "MODE: MAP  " : "MODE: LIVE  ")
                << (ver_distancia ? "DISTANCIA  " : "ALTURA  ")
                << "| SLAM: " << (usar_slam ? "ON" : "OFF")
                << " | BOX: " << (desenhar_caixas ? "ON" : "OFF");
            viewer->addText(ss.str(), 10, 10, 14, 1.0, 1.0, 1.0, "hud_text");
        }

        // detecção de obstáculos a cada DETECTION_PERIOD_MS
        {
            using namespace std::chrono;
            uint64_t now_ms = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();

            if (now_ms - g_last_detection_ms >= DETECTION_PERIOD_MS) {
                g_last_detection_ms = now_ms;
                _DetectarClustersEAdicionarCaixas(viewer, local_copy);

                auto h = g_handle.load(std::memory_order_relaxed);
                if (h) {
                    livox_status query_status = QueryLivoxLidarInternalInfo(h, _InternalInfoCallback, nullptr);
                    if (query_status != kLivoxLidarStatusSuccess) {
                        std::cerr << "Failed to send internal info query command. Status: "
                            << query_status << std::endl;
                    }
                }
            }
        }

        viewer->spinOnce(15);
        std::this_thread::sleep_for(std::chrono::milliseconds(15));

        // 7. Imprime IMU no console, etc, como você já fazia...
        /*{
            std::lock_guard<std::mutex> lock(g_imu_mutex);
            float acc_norm = std::sqrt(
                g_imu_acc_x * g_imu_acc_x +
                g_imu_acc_y * g_imu_acc_y +
                g_imu_acc_z * g_imu_acc_z
            );

            std::cout << "\r"
                << std::fixed << std::setprecision(3)
                << "[IMU] gyro: "
                << " gx=" << std::setw(7) << g_imu_gyro_x
                << " gy=" << std::setw(7) << g_imu_gyro_y
                << " gz=" << std::setw(7) << g_imu_gyro_z
                << " | acc(g?): "
                << " ax=" << std::setw(7) << g_imu_acc_x
                << " ay=" << std::setw(7) << g_imu_acc_y
                << " az=" << std::setw(7) << g_imu_acc_z
                << " | |acc|=" << std::setw(6) << acc_norm
                << " | t_ms=" << g_imu_last_ms
                << "     "
                << std::flush;
        }*/
    }

    g_running.store(false);
    LivoxLidarSdkUninit();
    std::cout << "\n[MAIN] Encerrado.\n";
    return 0;
}