#include "viewer_debug.hpp"
#include "core.hpp"

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/extract_clusters.h>
#include <algorithm>
#include <sstream>
#include <iomanip>
#include <chrono>
#include <deque>
#include <boost/make_shared.hpp>

#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkPoints.h>
#include <vtkCellArray.h>
#include <vtkLine.h>

namespace {
    std::string degC(reinterpret_cast<const char*>(u8"°C"));
    std::string deg(reinterpret_cast<const char*>(u8"°"));

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

    std::atomic<bool> g_draw_boxes{ false };
    std::atomic<bool> g_color_by_distance{ true };
    std::atomic<bool> g_mode_map{ false };
    std::atomic<bool> g_use_slam{ false };
    std::atomic<bool> g_sensor_invertido{ false };
    std::atomic<bool> g_show_axis{ false };


    // colormap tipo "jet" (0..1 -> RGB)
    static inline void ColorMapJet(float v, uint8_t& r, uint8_t& g, uint8_t& b) {
        if (v < 0) v = 0; if (v > 1) v = 1;
        float x = 4.0f * v;
        float rf = std::clamp(std::min(x - 1.5f, -x + 4.5f), 0.0f, 1.0f);
        float gf = std::clamp(std::min(x - 0.5f, -x + 3.5f), 0.0f, 1.0f);
        float bf = std::clamp(std::min(x + 0.5f, -x + 2.5f), 0.0f, 1.0f);
        r = (uint8_t)(rf * 255.f);
        g = (uint8_t)(gf * 255.f);
        b = (uint8_t)(bf * 255.f);
    }

    // recolore de acordo com o modo: distância radial (true) ou altura Z (false)
    static inline void colorizePoint(pcl::PointXYZRGB& p, bool by_dist) {
        uint8_t r, g, b;
        if (by_dist) {
            float dist = std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
            ColorMapJet(dist / 5.0f, r, g, b); // 5 m = referência; ajuste se quiser
        }
        else {
            // normaliza Z entre [-0.2, 1.5] como no código antigo
            float zmin = -0.2f, zmax = 1.5f;
            float v = (p.z - zmin) / std::max(1e-3f, (zmax - zmin));
            ColorMapJet(v, r, g, b);
        }
        p.r = r; p.g = g; p.b = b;
    }

    // ---- helpers HUD ----
    static void addHudTop(pcl::visualization::PCLVisualizer::Ptr& viewer) {
        viewer->removeShape("hud_top");
        auto rw = viewer->getRenderWindow();
        int* size = rw->GetSize();
        int win_w = size[0], win_h = size[1];

        const int margin = 10;
        const int font_px = 14;
        const int line_h = font_px + 4;
        const int x = margin;
        const int y = win_h - margin - line_h;

        auto temp = Core::get_temperature_c();
        auto rpy = Core::get_attitude();

        std::stringstream ss;
        ss.setf(std::ios::fixed);
        ss << std::setprecision(1);
        ss << "TEMP: " << temp << degC
            << "  |" 
            << "  R:" << Core::rad2deg(rpy.roll) << deg
            << "  P:" << Core::rad2deg(rpy.pitch) << deg
            << "  Y:" << Core::rad2deg(rpy.yaw) << deg;

        viewer->addText(ss.str(), x, y, font_px, 1.0, 1.0, 1.0, "hud_top");
    }

    static void addHudBottom(pcl::visualization::PCLVisualizer::Ptr& viewer) {
        viewer->removeShape("hud_bottom");
        std::stringstream ss;
        ss << (g_mode_map.load() ? "MODE: MAP  " : "MODE: LIVE  ")
            << (g_color_by_distance.load() ? "DISTANCIA  " : "ALTURA  ")
            << "| SLAM: " << (g_use_slam.load() ? "ON" : "OFF")
            << " | BOX: " << (g_draw_boxes.load() ? "ON" : "OFF");
        viewer->addText(ss.str(), 10, 10, 14, 1.0, 1.0, 1.0, "hud_bottom");
    }

    // (simplificado) converte CloudRGB ? pcl
    static pcl::PointCloud<pcl::PointXYZRGB>::Ptr toPcl(const CloudRGB& c) {
        auto pc = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>());
        pc->points.reserve(c.pts.size());
        for (auto& q : c.pts) {
            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;
            pc->points.push_back(p);
        }
        pc->width = (uint32_t)pc->points.size();
        pc->height = 1;
        pc->is_dense = true;
        return pc;
    }

    // (debug) desenha “bússola” de distâncias
    static void drawDistances3D(pcl::visualization::PCLVisualizer::Ptr& viewer) {
        using std::string;
        for (auto id : { "linha_frente","linha_tras","linha_esquerda","linha_direita","linha_cima","linha_baixo",
                         "texto_frente","texto_tras","texto_esquerda","texto_direita","texto_cima","texto_baixo" })
            viewer->removeShape(id);

        auto d = Core::get_distances();
        auto draw = [&](const string& idLine, const string& idText,
            float vx, float vy, float vz, float dist, float cr, float cg, float cb, const char* label) {
                if (dist <= 0.001f) dist = 0.0f;
                Eigen::Vector3f dst = Eigen::Vector3f(vx, vy, vz).normalized() * dist;
                viewer->addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(dst.x(), dst.y(), dst.z()), cr, cg, cb, idLine);
                viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, idLine);

                char txt[64];
                if (dist > 0.0f) std::snprintf(txt, sizeof(txt), "%s: %.0f cm", label, dist * 100.f);
                else             std::snprintf(txt, sizeof(txt), "%s: --", label);
                viewer->addText3D(txt, pcl::PointXYZ(dst.x(), dst.y(), dst.z()), 0.05, cr, cg, cb, idText);
            };

        draw("linha_frente", "texto_frente", 1, 0, 0, d.front, 1, 0, 0, "frente");
        draw("linha_tras", "texto_tras", -1, 0, 0, d.back, 0.8, 0, 0, "tras");
        draw("linha_direita", "texto_direita", 0, 1, 0, d.right, 0, 1, 0, "direita");
        draw("linha_esquerda", "texto_esquerda", 0, -1, 0, d.left, 0, 0.8, 0, "esquerda");
        draw("linha_cima", "texto_cima", 0, 0, 1, d.up, 0, 0, 1, "cima");
        draw("linha_baixo", "texto_baixo", 0, 0, -1, d.down, 0, 0, 0.8, "baixo");
    }

    // compara AABB com tolerância
    static inline bool approx_equal(const BBox3D& a, const BBox3D& b, float eps = 1e-4f) {
        auto ae = [&](float x, float y) { return std::fabs(x - y) <= eps; };
        return ae(a.min_x, b.min_x) && ae(a.min_y, b.min_y) && ae(a.min_z, b.min_z) &&
            ae(a.max_x, b.max_x) && ae(a.max_y, b.max_y) && ae(a.max_z, b.max_z);
    }

    static const int BBOX_EDGES[12][2] = {
    {0,1},{1,2},{2,3},{3,0}, {4,5},{5,6},{6,7},{7,4}, {0,4},{1,5},{2,6},{3,7}
    };

    static uint64_t g_last_draw_ms = 0;
    static std::string g_actor_id = "bboxes_all";
    static size_t g_last_n = 0;

    // constrói um único polydata de linhas com TODAS as bboxes
    static vtkSmartPointer<vtkPolyData> build_bbox_poly(const std::vector<BBox3D>& boxes, size_t max_boxes = SIZE_MAX) {
        auto pts = vtkSmartPointer<vtkPoints>::New();
        auto lines = vtkSmartPointer<vtkCellArray>::New();

        const size_t N = std::min(boxes.size(), max_boxes);
        pts->SetDataTypeToFloat();
        pts->Allocate(8 * N);
        lines->AllocateEstimate(12 * N, 2);

        vtkIdType base = 0;
        for (size_t i = 0; i < N; ++i) {
            const auto& b = boxes[i];
            const float x0 = b.min_x, y0 = b.min_y, z0 = b.min_z;
            const float x1 = b.max_x, y1 = b.max_y, z1 = b.max_z;

            // 8 vértices
            vtkIdType id0 = pts->InsertNextPoint(x0, y0, z0);
            vtkIdType id1 = pts->InsertNextPoint(x1, y0, z0);
            vtkIdType id2 = pts->InsertNextPoint(x1, y1, z0);
            vtkIdType id3 = pts->InsertNextPoint(x0, y1, z0);
            vtkIdType id4 = pts->InsertNextPoint(x0, y0, z1);
            vtkIdType id5 = pts->InsertNextPoint(x1, y0, z1);
            vtkIdType id6 = pts->InsertNextPoint(x1, y1, z1);
            vtkIdType id7 = pts->InsertNextPoint(x0, y1, z1);

            const vtkIdType ids[8] = { id0,id1,id2,id3,id4,id5,id6,id7 };

            // 12 arestas
            for (int e = 0; e < 12; ++e) {
                auto ln = vtkSmartPointer<vtkLine>::New();
                ln->GetPointIds()->SetId(0, ids[BBOX_EDGES[e][0]]);
                ln->GetPointIds()->SetId(1, ids[BBOX_EDGES[e][1]]);
                lines->InsertNextCell(ln);
            }

            base += 8;
        }

        auto poly = vtkSmartPointer<vtkPolyData>::New();
        poly->SetPoints(pts);
        poly->SetLines(lines);
        return poly;
    }

    // desenha/atualiza em 1 único ator
    static void draw_bboxes_one_actor(pcl::visualization::PCLVisualizer::Ptr& viewer,
        uint64_t now_ms,
        uint64_t min_interval_ms = 120,   // ~8 Hz
        size_t   max_boxes = 200)   // cota de segurança
    {
        if (now_ms - g_last_draw_ms < min_interval_ms) return;

        auto boxes = Core::get_bboxes();
        if (boxes.empty()) {
            if (g_last_n) { viewer->removeShape(g_actor_id); g_last_n = 0; }
            g_last_draw_ms = now_ms;
            return;
        }

        // (Opcional) cull simples: mantenha só as K mais próximas/do maior tamanho etc.
        // std::partial_sort(...); boxes.resize(std::min(boxes.size(), max_boxes));

        auto poly = build_bbox_poly(boxes, max_boxes);

        // remove e recria SÓ UM ator
        viewer->removeShape(g_actor_id);
        viewer->addModelFromPolyData(poly, g_actor_id);

        // propriedades visuais leves
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, g_actor_id);
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 1.5, g_actor_id);
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, g_actor_id);
        // desliga “efeitos”
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_SHADING, 0.0, g_actor_id);
        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
            pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, g_actor_id);

        g_last_n = std::min(boxes.size(), max_boxes);
        g_last_draw_ms = now_ms;
    }

    void KeyboardCallback(const pcl::visualization::KeyboardEvent& event, void*) {
        if (!event.keyDown()) return;
        const std::string k = event.getKeySym();

        auto toggle = [](std::atomic<bool>& f) {
            // troca atômica: inverte o bit de forma thread-safe
            bool expected = f.load(std::memory_order_relaxed);
            while (!f.compare_exchange_weak(expected, !expected, std::memory_order_relaxed)) {}
            };

        if (k == "m" || k == "M") {
            toggle(g_mode_map);
            std::cout << "\n[KEY] Modo: " << (g_mode_map.load() ? "MAP" : "LIVE") << std::endl;
        }
        else if (k == "s" || k == "S") {
            toggle(g_use_slam);
            std::cout << "\n[KEY] SLAM: " << (g_use_slam.load() ? "ATIVADO (ICP ON)" : "DESATIVADO") << std::endl;
        }
        else if (k == "d" || k == "D") {
            toggle(g_color_by_distance);
            std::cout << "\n[KEY] Color: " << (g_color_by_distance.load() ? "DISTANCIA" : "ALTURA") << std::endl;
        }
        else if (k == "b" || k == "B") {
            toggle(g_draw_boxes);
            std::cout << "\n[KEY] Caixas: " << (g_draw_boxes.load() ? "ON" : "OFF") << std::endl;
        }
        else if (k == "i" || k == "I") {
            toggle(g_sensor_invertido);
            std::cout << "\n[KEY] Sensor invertido: " << (g_sensor_invertido.load() ? "ON" : "OFF") << std::endl;
        }
        else if (k == "a" || k == "A") {
            toggle(g_show_axis);
            std::cout << "\n[KEY] Mostrar eixos: " << (g_show_axis.load() ? "ON" : "OFF") << std::endl;
        }
        else if (k == "c" || k == "C") {
            Core::ResetGlobalMap();
            std::cout << "\n[KEY] MAP: limpo\n";
        }
    }

    void loop() {
        auto viewer = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer("Mid-360 Debug Viewer"));
        viewer->setBackgroundColor(0, 0, 0);
        viewer->addCoordinateSystem(1.0);
        viewer->initCameraParameters();

        auto interactor = viewer->getRenderWindow()->GetInteractor();

        // === Keyboard ===
        viewer->registerKeyboardCallback(&KeyboardCallback, nullptr);

        // nuvem “fonte” (live/map) — aqui vamos desenhar a “live”
        auto live = Core::get_cloud_live();
        auto cloud_pcl = toPcl(live);
        pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_pcl);
        viewer->addPointCloud<pcl::PointXYZRGB>(cloud_pcl, rgb, "cloud");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");

        while (g_run.load() && !viewer->wasStopped()) {
            // 1) pega a nuvem já acumulada (engine)
            CloudRGB src = g_mode_map.load() ? Core::get_cloud_map() : Core::get_cloud_live();

            // 2) converte para PCL
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_now(new pcl::PointCloud<pcl::PointXYZRGB>);
            pc_now->points.reserve(src.pts.size());
            for (const auto& q : src.pts) {
                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; // vamos recolorir abaixo
                pc_now->points.push_back(p);
            }
            pc_now->width = (uint32_t)pc_now->points.size();
            pc_now->height = 1;
            pc_now->is_dense = true;

            // 3) colorir por distância (default) ou por altura (tecla D alterna)
            bool by_dist = g_color_by_distance.load();
            for (auto& p : pc_now->points) {
                colorizePoint(p, by_dist); // mesma função já existente aí
            }

            // 4) atualiza no viewer
            pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb2(pc_now);
            viewer->updatePointCloud<pcl::PointXYZRGB>(pc_now, rgb2, "cloud");

            addHudTop(viewer);
            addHudBottom(viewer);
            
            if (g_show_axis.load()) {
                drawDistances3D(viewer);
            }
            else {
                // remove tudo se estiver OFF
                for (auto id : {
                    "linha_frente","linha_tras","linha_esquerda","linha_direita","linha_cima","linha_baixo",
                    "texto_frente","texto_tras","texto_esquerda","texto_direita","texto_cima","texto_baixo"
                    })
                    viewer->removeShape(id);
            }
            
            if (g_draw_boxes.load()) {
                uint64_t now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
                draw_bboxes_one_actor(viewer, now_ms, /*min_interval_ms=*/10, /*max_boxes=*/20);
            }
            else {
                viewer->removeShape(g_actor_id);
                g_last_n = 0;
            }

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

} // anon

namespace ViewerDebug {
    bool start(bool usar_slam_inicial) {
        if (g_run.exchange(true)) return true;
        g_use_slam = usar_slam_inicial;
        try {
            g_thr = std::thread(loop);
            return true;
        }
        catch (...) {
            g_run.store(false);
            return false;
        }
    }
    void stop() {
        if (!g_run.exchange(false)) return;
        if (g_thr.joinable()) g_thr.join();
    }

    void set_draw_boxes(bool on) { g_draw_boxes = on; }
    void set_color_by_distance(bool on) { g_color_by_distance = on; }
    void set_mode_map(bool on) { g_mode_map = on; }
    void set_use_slam(bool on) { g_use_slam = on; }
    bool get_mode_map() { return g_mode_map.load(); }

} // namespace ViewerDebug