#pragma once
#include <string>
#include <vector>
#include <atomic>
#include <algorithm>
#include <sstream>
#include <iomanip>
#include <chrono>
#include <iostream>

// ===== Depende do teu projeto =====
#include "core.hpp"
#include "redis_publisher.hpp"

// Se quiser habilitar nlohmann/json, remova o comentário:
// #include <nlohmann/json.hpp>

// ======= Estruturas =======
struct SnapshotFreq {
    double pcl{ 0.0 };
    double imu{ 0.0 };
};

struct SnapshotIMU {
    uint64_t timestamp = 0;
    double roll_deg{ 0.0 };
    double pitch_deg{ 0.0 };
    double yaw_deg{ 0.0 };
};

struct SnapshotStatus {
    double temperatura_c{ 0.0 };
    uint32_t vezes_ligado{ 0 };
};

struct SnapshotDist {
    double frente{ 0.0 };
    double tras{ 0.0 };
    double esquerda{ 0.0 };
    double direita{ 0.0 };
    double sup{ 0.0 };
    double inf{ 0.0 };
};

struct StatusSnapshot {
    uint64_t timestamp_ms{ 0 };
    bool conectado{ true }; // placeholder por enquanto
    bool executando{ false };
    bool sensor_ativo{ false };
    std::string lidar_ip;
    std::string host_ip;
    std::string firmware_version;
    std::string dev_type;
    uint32_t handle{ 0 };
    SnapshotFreq freq;
    SnapshotIMU imu;
    SnapshotStatus status;
    SnapshotDist dist;
    std::vector<BBox3D> bboxes;

    static std::unique_ptr<RedisPublisher> g_pub;

    static void start_publisher() {
        const std::string uri = "tcp://localhost:6379";
        StatusSnapshot::g_pub = std::make_unique<RedisPublisher>(uri);
    }

    // ===== Coletor: puxa dados do sistema e retorna um Snapshot pronto =====
    static StatusSnapshot collect_now() {
        StatusSnapshot s{};

        // tempo
        using namespace std::chrono;
        s.timestamp_ms = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

        // flags globais/atômicos do teu runtime
        s.conectado = Core::connected.load(std::memory_order_relaxed);
        s.executando = Core::running.load(std::memory_order_relaxed);
        s.sensor_ativo = Core::streaming.load(std::memory_order_relaxed);
        s.handle = Core::handle.load(std::memory_order_relaxed);
        s.host_ip = Core::get_host_ip();
        s.lidar_ip = Core::get_lidar_ip();
        s.firmware_version = Core::get_fw_version();
        s.dev_type = Core::get_dev_type();

        // freq (Hz). Se teus g_freq_* guardam Hz, só converte p/ double:
        s.freq.pcl = static_cast<double>(Core::freq_pcl.load(std::memory_order_relaxed));
        s.freq.imu = static_cast<double>(Core::freq_imu.load(std::memory_order_relaxed));

        // IMU (em graus)
        const auto att = Core::get_attitude();
        s.imu.timestamp = att.timestamp;
        s.imu.roll_deg = Core::rad2deg(att.roll);
        s.imu.pitch_deg = Core::rad2deg(att.pitch);
        s.imu.yaw_deg = Core::rad2deg(att.yaw);

        // status (temperatura e contagem de ligadas)
        s.status.temperatura_c = Core::get_temperature_c();
        s.status.vezes_ligado = Core::get_power_count();

        // distâncias (ajusta os nomes conforme teu Distance6)
        const auto d = Core::get_distances();
        s.dist.frente = d.front;
        s.dist.tras = d.back;
        s.dist.esquerda = d.left;
        s.dist.direita = d.right;
        s.dist.sup = d.up;
        s.dist.inf = d.down;

        // bboxes — adapta para tua fonte. Exemplo genérico:
        // Supondo que exista Core::get_bboxes() -> std::vector<Deteccao>
        // com campos equivalentes.
        for (const auto& b : Core::get_bboxes()) {
            s.bboxes.push_back(std::move(b));
        }

        return s;
    }

    // ===== Serialização manual (sem dependências) =====
    std::string to_json_string() const {
        std::ostringstream os;
        os << std::fixed << std::setprecision(3);
        os << "{";
        os << "\"timestamp\":" << timestamp_ms << ",";
        os << "\"conectado\":" << (conectado ? "true" : "false") << ",";
        os << "\"executando\":" << (executando ? "true" : "false") << ",";
        os << "\"sensor_ativo\":" << (sensor_ativo ? "true" : "false") << ",";
        os << "\"host_ip\":" << host_ip << ",";
        os << "\"lidar_ip\":" << lidar_ip << ",";
        os << "\"firmware_version\":" << firmware_version << ",";
        os << "\"dev_type\":" << dev_type << ",";
        os << "\"handle\":" << handle << ",";

        os << "\"freq\":{"
            << "\"pcl\":" << freq.pcl << ","
            << "\"imu\":" << freq.imu << "},";

        os << "\"imu\":{"
            << "\"timestamp\":" << imu.timestamp << ","
            << "\"roll\":" << imu.roll_deg << ","
            << "\"pitch\":" << imu.pitch_deg << ","
            << "\"yaw\":" << imu.yaw_deg << "},";

        os << "\"status\":{"
            << "\"temperatura\":" << status.temperatura_c << ","
            << "\"vezes_ligado\":" << status.vezes_ligado << "},";

        os << "\"distancias\":{"
            << "\"frente\":" << dist.frente << ","
            << "\"tras\":" << dist.tras << ","
            << "\"esquerda\":" << dist.esquerda << ","
            << "\"direita\":" << dist.direita << ","
            << "\"sup\":" << dist.sup << ","
            << "\"inf\":" << dist.inf << "},";

        os << "\"bboxes\":[";
        for (size_t i = 0; i < bboxes.size(); ++i) {
            const auto& b = bboxes[i];
            os << "{"
                // limites da caixa
                << "\"min_x\":" << b.min_x << ","
                << "\"min_y\":" << b.min_y << ","
                << "\"min_z\":" << b.min_z << ","
                << "\"max_x\":" << b.max_x << ","
                << "\"max_y\":" << b.max_y << ","
                << "\"max_z\":" << b.max_z << ","

                // tempo e voxel info
                << "\"t_ms\":" << b.t_ms << ","
                << "\"voxels\":" << b.voxels << ","

                // centro e dimensões
                << "\"cx\":" << b.cx << ","
                << "\"cy\":" << b.cy << ","
                << "\"cz\":" << b.cz << ","
                << "\"w\":" << b.w << ","
                << "\"h\":" << b.h << ","
                << "\"d\":" << b.d << ","

                // distâncias derivadas
                << "\"dist_m\":" << b.dist_m << ","
                << "\"dist_xy_m\":" << b.dist_xy_m << ","
                << "\"frente_m\":" << b.frente_m

                // fecha objeto
                << "}";

            if (i + 1 < bboxes.size())
                os << ",";
        }
        os << "]";

        os << "}";
        return os.str();
    }

    // --- 2.1 Compactador de BBoxes ---
    std::string bboxes_to_json_compact(std::size_t max_boxes) const {
        if (bboxes.empty()) return "[]";

        // ordena por proximidade frontal (menor frente_m primeiro)
        std::vector<BBox3D> v = bboxes;
        std::sort(v.begin(), v.end(),
            [](const BBox3D& a, const BBox3D& b) { return a.frente_m < b.frente_m; });

        if (v.size() > max_boxes) v.resize(max_boxes);

        std::ostringstream os;
        os.setf(std::ios::fixed); os << std::setprecision(3);
        os << "[";

        for (std::size_t i = 0; i < v.size(); ++i) {
            const auto& b = v[i];
            os << "{"
                << "\"min_x\":" << b.min_x << ","
                << "\"min_y\":" << b.min_y << ","
                << "\"min_z\":" << b.min_z << ","
                << "\"max_x\":" << b.max_x << ","
                << "\"max_y\":" << b.max_y << ","
                << "\"max_z\":" << b.max_z << ","
                << "\"t_ms\":" << b.t_ms << ","
                << "\"voxels\":" << b.voxels << ","
                << "\"cx\":" << b.cx << ","
                << "\"cy\":" << b.cy << ","
                << "\"cz\":" << b.cz << ","
                << "\"w\":" << b.w << ","
                << "\"h\":" << b.h << ","
                << "\"d\":" << b.d << ","
                << "\"dist_m\":" << b.dist_m << ","
                << "\"dist_xy_m\":" << b.dist_xy_m << ","
                << "\"frente_m\":" << b.frente_m
                << "}";
            if (i + 1 < v.size()) os << ",";
        }
        os << "]";
        return os.str();
    }

    // --- 2.2 Publisher parcial (Hash + Stream) ---
    bool publish_state_partial(int ttl_sec_hash, std::size_t max_bboxes) const
    {
        if (!g_pub) {
            std::cerr << "[STATUS] RedisPublisher não inicializado\n";
            return false;
        }

        // monta pares para o JSON (paths com '.')
        std::vector<std::pair<std::string, std::string>> J;
        J.reserve(32);

        auto addj_i = [&](std::string path, long long v) {
            J.emplace_back(std::move(path), std::to_string(v));
            };
        auto addj_b = [&](std::string path, bool v) {
            // booleans como true/false (sem aspas) viram bool real no JSON
            J.emplace_back(std::move(path), v ? "true" : "false");
            };
        auto addj_f = [&](std::string path, double v, int prec = 3) {
            std::ostringstream oss;
            oss.setf(std::ios::fixed); oss << std::setprecision(prec) << v;
            J.emplace_back(std::move(path), oss.str()); // número no JSON
            };
        auto addj_s = [&](std::string path, const std::string& v) {
            // adiciona string com aspas no JSON
            std::ostringstream oss;
            oss << "\"" << v << "\"";
            J.emplace_back(std::move(path), oss.str());
            };

        // ---- mesmos campos que você já preencheu no HASH ----
        addj_i("timestamp", static_cast<long long>(timestamp_ms));
        addj_b("conectado", conectado);
        addj_b("executando", executando);
        addj_b("sensor_ativo", sensor_ativo);
        addj_s("host_ip", host_ip);
        addj_s("lidar_ip", lidar_ip);
        addj_s("firmware_version", Core::get_fw_version());
        addj_s("dev_type", Core::get_dev_type());
        addj_i("handle", static_cast<long long>(handle));

        addj_f("freq.pcl", freq.pcl, 2);
        addj_f("freq.imu", freq.imu, 2);

        addj_f("imu.timestamp", imu.timestamp, 2);
        addj_f("imu.roll", imu.roll_deg, 2);
        addj_f("imu.pitch", imu.pitch_deg, 2);
        addj_f("imu.yaw", imu.yaw_deg, 2);

        addj_f("status.temperatura", status.temperatura_c, 2);
        addj_i("status.vezes_ligado", static_cast<long long>(status.vezes_ligado));

        addj_f("dist.frente", dist.frente, 2);
        addj_f("dist.tras", dist.tras, 2);
        addj_f("dist.esquerda", dist.esquerda, 2);
        addj_f("dist.direita", dist.direita, 2);
        addj_f("dist.sup", dist.sup, 2);
        addj_f("dist.inf", dist.inf, 2);

        // opcional: bboxes como array JSON no snapshot (no mesmo documento)
        if (!bboxes.empty()) {
            std::string bb_json = bboxes_to_json_compact(max_bboxes); // deve ser JSON válido
            if (!bb_json.empty()) {
                // como o merge tenta json::parse(value), passar o array/objeto funciona direto
                J.emplace_back("bboxes", bb_json); // ex.: "[{...},{...}]"
            }
        }

        // grava/mescla no snapshot JSON (sem apagar outros campos já existentes)
        bool ok_json = g_pub->set_json_merge_paths("ctx:dados_modulo_Lvx", J, ttl_sec_hash);
        //std::cout << "OK_JSON: " << ok_json << "\n";
        return ok_json;
    }

    bool publish_state_imu() {
        const auto att = Core::get_attitude();

        std::vector<std::pair<std::string, std::string>> J;
        J.reserve(32);

        auto addj_i = [&](std::string path, long long v) {
            J.emplace_back(std::move(path), std::to_string(v));
            };
        auto addj_f = [&](std::string path, double v, int prec = 3) {
            std::ostringstream oss;
            oss.setf(std::ios::fixed); oss << std::setprecision(prec) << v;
            J.emplace_back(std::move(path), oss.str()); // número no JSON
            };

        addj_i("timestamp", static_cast<long long>(att.timestamp));
        addj_f("frequencia", att.frequencia, 4);
        addj_f("latencia", att.latencia, 4);
        addj_f("roll", Core::rad2deg(att.roll), 3);
        addj_f("pitch", Core::rad2deg(att.pitch), 3);
        addj_f("yaw", Core::rad2deg(att.yaw), 3);

        // grava/mescla no snapshot JSON (sem apagar outros campos já existentes)
        bool ok_json = g_pub->set_json_merge_paths("ctx:dados_modulo_Imu", J, 3);
    }

};