#include "metrics.hpp"
#include <mutex>
#include <atomic>
#include <thread>
#include <optional>
#include <cmath>
#include <cfloat>

// PCL
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include "voxel_lite.cpp"

#include <pcl/filters/crop_box.h>
#include <Eigen/Dense>

#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include "status_snapshot.hpp"
using namespace std::chrono;


namespace {

    std::mutex g_mtx_params;
    MetricsParams gP;

    // ---- fila latest-only para nuvens ----
    std::mutex g_mtx_queue;
    std::optional<CloudRGB> g_last_cloud;
    uint64_t g_last_cloud_ms = 0;

    std::atomic<bool> g_run{ false };
    std::thread g_thr;

    pcl::PointCloud<pcl::PointXYZ>::Ptr toPclXYZ(const CloudRGB& merged) {
        auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
        out->points.reserve(merged.pts.size());
        for (const auto& q : merged.pts) {
            // Se tiver NaN/Inf, pule
            if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z)) continue;
            out->points.push_back(pcl::PointXYZ{ q.x, q.y, q.z });
        }
        out->width = static_cast<uint32_t>(out->points.size());
        out->height = 1;                     // unorganized
        out->is_dense = false;               // conservador
        return out;
    }

    inline Distance6 computeDistances6(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& pc, const MetricsParams& P, uint64_t t_ms) {
        Distance6 d{}; d.t_ms = t_ms;
        auto upd_min = [](float v, float& cur) { if (cur == 0.f || v < cur) cur = v; };
        for (const auto& p : pc->points) {
            if (p.x > 0) upd_min(p.x, d.front); else if (p.x < 0) upd_min(-p.x, d.back);
            if (p.y > 0) upd_min(p.y, d.left); else if (p.y < 0) upd_min(-p.y, d.right);
            if (p.z > 0) upd_min(p.z, d.up); else if (p.z < 0) upd_min(-p.z, d.down);
        }
        if (d.front > 0 && d.front > P.x_max) d.front = 0;
        if (d.back > 0 && d.back > std::abs(P.x_min)) d.back = 0;
        if (d.left > 0 && d.left > P.y_max) d.left = 0;
        if (d.right > 0 && d.right > std::abs(P.y_min)) d.right = 0;
        if (d.up > 0 && d.up > P.z_max) d.up = 0;
        if (d.down > 0 && d.down > std::abs(P.z_min)) d.down = 0;
        return d;
    }

    // pipeline síncrona (usada pela thread)
    static void update_from_cloud_sync(const CloudRGB& merged, uint64_t now_ms) {
        MetricsParams P; { std::lock_guard<std::mutex> lk(g_mtx_params); P = gP; }
        
        // 1) ROI (ping-pong, sem in-place)
        auto A = toPclXYZ(merged);
        if (A->empty()) { Core::set_distances(Distance6{ .t_ms = now_ms }); Core::set_bboxes({}); return; }

        // Sanitize: remova NaNs e ajuste header (evita PassThrough iterar fora do vetor)
        {
            std::vector<int> idxmap;
            pcl::removeNaNFromPointCloud(*A, *A, idxmap);
            A->width = static_cast<uint32_t>(A->points.size());
            A->height = 1;
            A->is_dense = false;
            if (A->empty()) { Core::set_distances(Distance6{ .t_ms = now_ms }); Core::set_bboxes({}); return; }
        }

        /*auto B = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::PassThrough<pcl::PointXYZ> pass;

        auto pass_axis = [&](const char* axis, float lo, float hi) {
            pass.setInputCloud(A);
            pass.setFilterFieldName(axis);
            pass.setFilterLimits(lo, hi);

            // RESET COMPLETO do buffer de saída (clear() não zera header)
            B->points.clear();
            B->width = 0; B->height = 1; B->is_dense = false;
            pass.filter(*B);

            // corrige header após o filtro
            B->width = static_cast<uint32_t>(B->points.size());
            B->height = 1;
            B->is_dense = false;

            A.swap(B);
            };
        
        try {
            pass_axis("x", P.x_min, P.x_max);
            pass_axis("y", P.y_min, P.y_max);
            pass_axis("z", P.z_min, P.z_max);
        }
        catch (const std::exception& e) {
            std::cerr << "[ROI] PassThrough exception: " << e.what() << std::endl;
            Core::set_distances(Distance6{ .t_ms = now_ms });
            Core::set_bboxes({});
            return;
        }
        catch (...) {
            std::cerr << "[ROI] PassThrough unknown exception" << std::endl;
            Core::set_distances(Distance6{ .t_ms = now_ms });
            Core::set_bboxes({});
            return;
        }*/

        /*auto roi_manual = [&](const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& in,
            float xmin, float xmax, float ymin, float ymax, float zmin, float zmax)
            -> pcl::PointCloud<pcl::PointXYZ>::Ptr
            {
                auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
                out->points.reserve(in->points.size());

                for (const auto& p : in->points) {
                    if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue;
                    if (p.x < xmin || p.x > xmax) continue;
                    if (p.y < ymin || p.y > ymax) continue;
                    if (p.z < zmin || p.z > zmax) continue;
                    out->points.push_back(p);
                }
                out->width = static_cast<uint32_t>(out->points.size());
                out->height = 1;
                out->is_dense = false;
                return out;
            };

        // uso no teu código (substitui os 3 PassThrough):
        A = roi_manual(A, P.x_min, P.x_max, P.y_min, P.y_max, P.z_min, P.z_max);*/

        /*pcl::CropBox<pcl::PointXYZ> cb;
        cb.setMin(Eigen::Vector4f(P.x_min, P.y_min, P.z_min, 1.0f));
        cb.setMax(Eigen::Vector4f(P.x_max, P.y_max, P.z_max, 1.0f));
        cb.setInputCloud(A);
        auto B = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
        cb.filter(*B);
        B->width = static_cast<uint32_t>(B->points.size());
        B->height = 1;
        B->is_dense = false;
        A.swap(B);*/

        /*auto filter_axis = [&](const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& in,
            const char* axis, float lo, float hi)
            -> pcl::PointCloud<pcl::PointXYZ>::Ptr
            {
                std::vector<int> keep;
                keep.reserve(in->points.size());

                pcl::PassThrough<pcl::PointXYZ> pass;
                pass.setInputCloud(in);
                pass.setFilterFieldName(axis);
                pass.setFilterLimits(lo, hi);
                pass.filter(keep);                       // ← pega só os índices válidos

                auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
                out->points.reserve(keep.size());
                pcl::copyPointCloud(*in, keep, *out);

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

        // encadeia os 3 eixos:
        A = filter_axis(A, "x", P.x_min, P.x_max);
        A = filter_axis(A, "y", P.y_min, P.y_max);
        A = filter_axis(A, "z", P.z_min, P.z_max);*/

        /*struct VoxelKey {
            int ix, iy, iz;
            bool operator==(const VoxelKey& o) const {
                return ix == o.ix && iy == o.iy && iz == o.iz;
            }
        };
        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);
            }
        };
        auto _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;
            };
        auto CastXYZRGBtoXYZ = [](const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& in) {
            auto out = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            if (!in) return out;
            out->points.reserve(in->points.size());
            for (const auto& p : in->points) out->points.emplace_back(p.x, p.y, p.z);
            out->width = static_cast<uint32_t>(out->points.size());
            out->height = 1; out->is_dense = false;
            return out;
            };
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZRGB>);
        cloud_in->points.reserve(merged.pts.size());
        for (const auto& q : merged.pts) {
            // supondo que CloudRGB usa metros em x/y/z
            if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z)) continue;
            pcl::PointXYZRGB p;
            p.x = q.x; p.y = q.y; p.z = q.z;
            p.r = q.r; p.g = q.g; p.b = q.b; // se não tiver RGB, deixe 0
            cloud_in->points.push_back(p);
        }
        cloud_in->width = static_cast<uint32_t>(cloud_in->points.size());
        cloud_in->height = 1;
        cloud_in->is_dense = false;
        if (cloud_in->points.size() < 20) {
            Core::set_distances(Distance6{ .t_ms = now_ms }); Core::set_bboxes({}); return;
        }
        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;
            if (dist2 > 9.0f) continue;
            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;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr coarse = _DownsampleVoxelLike(roi, 0.05f);
        if (coarse->points.size() < 20) {
            return;
        }
        auto pc_vx = CastXYZRGBtoXYZ(coarse);*/


        if (A->empty()) { Core::set_distances(Distance6{ .t_ms = now_ms }); Core::set_bboxes({}); return; }

        // 2) Voxel (guard de leaf)
        /*auto pc_vx = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::VoxelGrid<pcl::PointXYZ> vg; vg.setInputCloud(A);
        const float leaf = (P.voxel_leaf > 1e-5f) ? P.voxel_leaf : 0.05f;
        vg.setLeafSize(leaf, leaf, leaf);
        vg.filter(*pc_vx);*/

        // 2) Downsample voxel-like (sem PCL::VoxelGrid)
        auto pc_vx = VoxelLite::DownsampleVoxelLikeXYZ(A, P.voxel_leaf);

        // 3) Distâncias
        auto d6 = computeDistances6(pc_vx, P, now_ms);
        Core::set_distances(d6);

        // 4) Clusters
        if (pc_vx->size() < (size_t)P.min_cluster) { 
            Core::set_bboxes({}); 
            return; 
        }
        
        struct VKey { int x, y, z; };
        struct VKeyHash {
            size_t operator()(const VKey& k) const noexcept {
                // hash 3D simples
                uint64_t h = (uint64_t)(uint32_t)k.x * 73856093u
                    ^ (uint64_t)(uint32_t)k.y * 19349663u
                    ^ (uint64_t)(uint32_t)k.z * 83492791u;
                return (size_t)h;
            }
        };
        struct VKeyEq {
            bool operator()(const VKey& a, const VKey& b) const noexcept {
                return a.x == b.x && a.y == b.y && a.z == b.z;
            }
        };

        // vizinhança 26-conexa
        static const int NBR[26][3] = {
            {-1,-1,-1},{-1,-1,0},{-1,-1,1},{-1,0,-1},{-1,0,0},{-1,0,1},{-1,1,-1},{-1,1,0},{-1,1,1},
            {0,-1,-1},{0,-1,0},{0,-1,1},{0,0,-1},            {0,0,1},{0,1,-1},{0,1,0},{0,1,1},
            {1,-1,-1},{1,-1,0},{1,-1,1},{1,0,-1},{1,0,0},{1,0,1},{1,1,-1},{1,1,0},{1,1,1}
        };

        if (!pc_vx || pc_vx->empty()) {
            Core::set_bboxes({});
            return;
        }

        const float vox = std::max(0.01f, P.voxel_leaf); // segurança
        // 1) marca voxels ocupados (usando a nuvem já voxelizada pc_vx)
        std::unordered_set<VKey, VKeyHash, VKeyEq> occ;
        occ.reserve(pc_vx->size());

        // ROI rápida (aproveita seus limites já definidos)
        for (const auto& p : pc_vx->points) {
            if (!std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) continue;
            if (p.x < P.x_min || p.x > P.x_max ||
                p.y < P.y_min || p.y > P.y_max ||
                p.z < P.z_min || p.z > P.z_max) continue;

            VKey k{ (int)std::floor(p.x / vox), (int)std::floor(p.y / vox), (int)std::floor(p.z / vox) };
            occ.insert(k);
        }

        if (occ.empty()) {
            Core::set_bboxes({});
            return;
        }

        // 2) componentes conexos em voxels ocupados
        std::unordered_map<VKey, uint8_t, VKeyHash, VKeyEq> vis; // 0/1 visitado
        vis.reserve(occ.size());

        std::vector<VKey> stack; stack.reserve(4096);
        std::vector<BBox3D> boxes; boxes.reserve(256);

        // limite opcional de saída (evita overlays gigantes)
        constexpr size_t MAX_BOXES_OUT = 200;

        for (const auto& k0 : occ) {
            if (vis[k0]) continue;
            vis[k0] = 1;
            stack.clear();
            stack.push_back(k0);

            // AABB em coord reais
            float min_x = FLT_MAX, min_y = FLT_MAX, min_z = FLT_MAX;
            float max_x = -FLT_MAX, max_y = -FLT_MAX, max_z = -FLT_MAX;
            size_t vox_count = 0;

            while (!stack.empty()) {
                VKey k = stack.back(); stack.pop_back();
                ++vox_count;

                // bounds do voxel → atualiza AABB
                const float x0 = k.x * vox, x1 = (k.x + 1) * vox;
                const float y0 = k.y * vox, y1 = (k.y + 1) * vox;
                const float z0 = k.z * vox, z1 = (k.z + 1) * vox;
                if (x0 < min_x) min_x = x0; if (x1 > max_x) max_x = x1;
                if (y0 < min_y) min_y = y0; if (y1 > max_y) max_y = y1;
                if (z0 < min_z) min_z = z0; if (z1 > max_z) max_z = z1;

                // expande 26-vizinhos
                for (int n = 0; n < 26; ++n) {
                    VKey nb{ k.x + NBR[n][0], k.y + NBR[n][1], k.z + NBR[n][2] };
                    if (occ.find(nb) == occ.end()) continue;
                    auto it = vis.find(nb);
                    if (it != vis.end() && it->second) continue;
                    vis[nb] = 1;
                    stack.push_back(nb);
                }
            }

            // 3) filtros de tamanho com SEUS campos já existentes
            // usamos min/max em "conta de voxels", reaproveitando P.min_cluster / P.max_cluster
            if (vox_count >= (size_t)std::max(1, P.min_cluster) &&
                vox_count <= (size_t)std::max(P.min_cluster, P.max_cluster)) {

                // centro e dimensões
                const float cx = 0.5f * (min_x + max_x);
                const float cy = 0.5f * (min_y + max_y);
                const float cz = 0.5f * (min_z + max_z);

                const float w = std::fabs(max_y - min_y);
                const float h = std::fabs(max_z - min_z);
                const float d = std::fabs(max_x - min_x);

                // distâncias
                const float dist_m = std::sqrt(cx * cx + cy * cy + cz * cz); // 3D
                const float dist_xy_m = std::sqrt(cx * cx + cy * cy);         // plano chão
                const float frente_m = cx;                               // eixo X é "frente"

                BBox3D out{
                    min_x, min_y, min_z,
                    max_x, max_y, max_z,
                    now_ms
                };
                out.voxels = static_cast<uint32_t>(vox_count);
                out.cx = cx; out.cy = cy; out.cz = cz;
                out.w = w;  out.h = h;  out.d = d;
                out.dist_m = dist_m;
                out.dist_xy_m = dist_xy_m;
                out.frente_m = frente_m;

                boxes.push_back(std::move(out));
                if (boxes.size() >= MAX_BOXES_OUT) break;
            }
        }

        Core::set_bboxes(boxes);
    }

    void worker_loop() {
        using namespace std::chrono;
        const uint64_t intervalo_ms = 100;  // processa a cada 100 ms (ajuste conforme quiser)

        uint64_t ultimo_processamento = 0;

        while (g_run.load()) {
            CloudRGB c;
            uint64_t t = 0;

            {
                std::lock_guard<std::mutex> lk(g_mtx_queue);
                if (g_last_cloud.has_value()) {
                    c = std::move(*g_last_cloud);
                    t = g_last_cloud_ms;
                    g_last_cloud.reset();
                }
            }

            // tempo atual
            uint64_t agora_ms = duration_cast<milliseconds>(
                steady_clock::now().time_since_epoch()).count();

            if (t != 0 && (agora_ms - ultimo_processamento >= intervalo_ms)) {
                ultimo_processamento = agora_ms;
                update_from_cloud_sync(c, t);
            }
            else {
                // dorme curto pra não travar a thread
                std::this_thread::sleep_for(std::chrono::milliseconds(2));
            }
        }
    }

} // anon

namespace Metrics {

    void init(const MetricsParams& p) { std::lock_guard<std::mutex> lk(g_mtx_params); gP = p; }
    void set_params(const MetricsParams& p) { std::lock_guard<std::mutex> lk(g_mtx_params); gP = p; }
    MetricsParams get_params() { std::lock_guard<std::mutex> lk(g_mtx_params); return gP; }

    void start_worker() {
        if (g_run.exchange(true)) return;
        g_thr = std::thread(worker_loop);
    }
    
    void stop_worker() {
        if (!g_run.exchange(false)) return;
        if (g_thr.joinable()) g_thr.join();
    }

    void enqueue_cloud(const CloudRGB& merged, uint64_t now_ms) {
        // guarda só a mais recente
        std::lock_guard<std::mutex> lk(g_mtx_queue);
        g_last_cloud = merged;
        g_last_cloud_ms = now_ms;
    }

    void update_from_imu(float ax, float ay, float az, float gx, float gy, float gz, uint64_t t_ms) {
        auto timestamp_ms_0 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

        Core::set_last_imu(ImuRaw{ ax, ay, az, gx, gy, gz, t_ms });

        static uint64_t last_ms = 0;
        auto rpy = Core::get_attitude();
        if (last_ms == 0) { last_ms = t_ms; return; }
        float dt = (t_ms - last_ms) / 1000.0f; last_ms = t_ms;

        float roll_gyro = rpy.roll + gx * dt;
        float pitch_gyro = rpy.pitch + gy * dt;
        float yaw_gyro = rpy.yaw + gz * dt;

        float denom = std::sqrt(ay * ay + az * az); if (denom < 1e-6f) denom = 1e-6f;
        float roll_acc = std::atan2(ay, az);
        float pitch_acc = std::atan2(-ax, denom);

        const float a = get_params().alpha_complementar;
        AttitudeRPY fused;
        fused.timestamp = timestamp_ms_0;
        fused.roll = Core::wrapPi(a * roll_gyro + (1.f - a) * roll_acc);
        fused.pitch = Core::wrapPi(a * pitch_gyro + (1.f - a) * pitch_acc);
        fused.yaw = Core::wrapPi(yaw_gyro);

        auto timestamp_ms_1 = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
        fused.latencia = timestamp_ms_1 - timestamp_ms_0;
        fused.frequencia = 1.0f / dt;

        Core::set_attitude(fused);
    }

} // namespace Metrics