#pragma once
#include <cstdint>
#include <vector>
#include <mutex>
#include <atomic>
#include <deque>
#include <algorithm>
#include <Eigen/Dense>

// Estruturas principais (estado “de operação”)

struct AttitudeRPY {
    uint64_t timestamp = 0;
    float roll = 0.f;   // rad
    float pitch = 0.f;  // rad
    float yaw = 0.f;    // rad
    float latencia = 0.f;
    float frequencia = 0.f;
};

struct ImuRaw {
    float acc_x = 0.f;  // em g (ou m/s^2, conforme seu pipeline)
    float acc_y = 0.f;
    float acc_z = 0.f;
    float gyro_x = 0.f; // rad/s
    float gyro_y = 0.f;
    float gyro_z = 0.f;
    uint64_t t_ms = 0;
};

struct Distance6 {
    // distâncias médias (m) à frente/trás/esq/dir/cima/baixo (0 = sem obstáculo)
    float front = 0.f;
    float back = 0.f;
    float left = 0.f;
    float right = 0.f;
    float up = 0.f;
    float down = 0.f;
    uint64_t t_ms = 0;
};

struct BBox3D {
    // caixa alinhada aos eixos do frame do sensor
    float min_x, min_y, min_z;
    float max_x, max_y, max_z;
    uint64_t t_ms = 0;

    // ---- enriquecimentos (novos) ----
    uint32_t voxels = 0;   // quantos voxels no componente
    // centro (m)
    float cx = 0.f, cy = 0.f, cz = 0.f;
    // dimensões (m)
    float w = 0.f, h = 0.f, d = 0.f;   // w = |y|, h = |z|, d = |x|
    // distâncias úteis (m)
    float dist_m = 0.f; // euclidiana 3D até o centro
    float dist_xy_m = 0.f; // projeção no plano chão
    float frente_m = 0.f; // “range” à frente do sensor (eixo X positivo)
};

struct RGBPoint {
    float x, y, z;
    uint8_t r, g, b;
};

struct CloudRGB {
    std::vector<RGBPoint> pts;
    uint64_t t_ms = 0;
};

struct TemporalCloudAccumulator {
    // janela de tempo (ms) e limite máximo de pontos para a nuvem mesclada
    std::atomic<uint64_t> window_ms{ 200 };        // ex.: 150 ms de cauda
    std::atomic<size_t>   max_points{ 200000 };    // limite de pontos

    struct Block {
        std::vector<RGBPoint> pts;  // usamos RGBPoint do Core (x,y,z,r,g,b)
        uint64_t t_ms{ 0 };
    };

    std::mutex mtx;
    std::deque<Block> ring;

    // insere um pacote; assume CloudRGB.x/y/z em metros
    void push(const CloudRGB& c) {
        if (c.pts.empty()) return;
        Block b;
        b.t_ms = c.t_ms;
        b.pts = c.pts; // cópia (rápida): RGBPoint é pequeno
        std::lock_guard<std::mutex> lk(mtx);
        ring.push_back(std::move(b));
        // poda imediata pelo tempo atual do pacote
        prune_unlocked(c.t_ms);
    }

    // gera nuvem mesclada (aplica janela e max_points)
    CloudRGB buildMerged(uint64_t now_ms) {
        std::lock_guard<std::mutex> lk(mtx);
        prune_unlocked(now_ms);

        CloudRGB out;
        if (ring.empty()) return out;

        // estime total e corte pelo max_points (começando dos mais recentes)
        size_t total = 0;
        for (auto it = ring.rbegin(); it != ring.rend(); ++it) {
            total += it->pts.size();
            if (total > max_points.load()) break;
        }
        out.pts.reserve(std::min(total, max_points.load()));
        out.t_ms = ring.back().t_ms; // timestamp do mais recente

        size_t budget = max_points.load();
        for (auto it = ring.rbegin(); it != ring.rend() && budget > 0; ++it) {
            const auto& v = it->pts;
            size_t take = std::min(budget, v.size());
            out.pts.insert(out.pts.end(), v.end() - take, v.end());
            budget -= take;
        }
        return out;
    }

    void clear() {
        std::lock_guard<std::mutex> lk(mtx);
        ring.clear();
    }


private:
    void prune_unlocked(uint64_t ref_ms) {
        const uint64_t W = window_ms.load();
        if (W == 0) return; // mapa ilimitado no tempo
        while (!ring.empty() && (ref_ms - ring.front().t_ms) > W) {
            ring.pop_front();
        }
    }
};


namespace Core {

    // ---- estado global acessível pelos módulos ----
    extern std::atomic<uint32_t> handle;
    extern std::atomic<bool>     connected;
    extern std::atomic<bool>     running;
    extern std::atomic<bool>     debug;
    extern std::atomic<uint64_t> last_imu_ms;
    extern std::atomic<double>   freq_imu;
    extern std::atomic<uint64_t> last_pcl_ms;
    extern std::atomic<double>   freq_pcl;
    extern std::atomic<bool>     sensor_ok;
    extern std::atomic<bool>     streaming;

    void init();

    // -------- IP --------
    void set_lidar_ip(const std::string& ip);
    void set_host_ip(const std::string& ip);
    const std::string& get_lidar_ip();
    const std::string& get_host_ip();

    // -------- Versao --------
    void set_fw_version(const std::string& v);
    const std::string& get_fw_version();

    // -------- Descricao --------
    void set_dev_type(const std::string& t);
    const std::string& get_dev_type();

    // -------- IMU / atitude --------
    void set_last_imu(const ImuRaw& s);
    ImuRaw get_last_imu();

    void set_attitude(const AttitudeRPY& rpy);
    AttitudeRPY get_attitude();

    // -------- tempo de dados (para watchdog) --------
    uint64_t get_last_imu_time_ms();
    void     set_last_points_time_ms(uint64_t t);
    uint64_t get_last_points_time_ms();

    // -------- temperatura / contagem de power-on --------
    void set_temperature_c(float t);
    float get_temperature_c();

    void set_power_count(uint32_t n);
    uint32_t get_power_count();

    // -------- pose global (ICP/odometria) --------
    void set_pose_Twm(const Eigen::Matrix4f& Twm);
    Eigen::Matrix4f get_pose_Twm();

    // -------- nuvens (live e global/map) --------
    void set_cloud_live(const CloudRGB& c);
    CloudRGB get_cloud_live();

    void set_cloud_map(const CloudRGB& c);
    CloudRGB get_cloud_map();

    // -------- distâncias 6-eixos & bboxes (resultado de percepção) --------
    void set_distances(const Distance6& d);
    Distance6 get_distances();

    void set_bboxes(const std::vector<BBox3D>& boxes);
    std::vector<BBox3D> get_bboxes();

    // -------- utilitários --------
    float rad2deg(float r);
    float deg2rad(float d);
    float wrapPi(float a);

    // Cloud global (MAP)
    void set_cloud_map(const CloudRGB& c);
    CloudRGB get_cloud_map();
    void clear_cloud_map();

    // Configuração
    void AccConfigLive(uint64_t window_ms, size_t max_points);
    void AccConfigMap(uint64_t window_ms, size_t max_points);

    // Push + build
    void     AccPushLive(const CloudRGB& pkt, uint64_t t_ms);
    CloudRGB AccBuildLive(uint64_t ref_ms);

    void     AccPushMap(const CloudRGB& pkt, uint64_t t_ms);
    CloudRGB AccBuildMap(uint64_t ref_ms);

    // Resetar o MAP global (nuvem + acumulador)
    void ResetGlobalMap();

    // Zera acumulador live (ring de pacotes) e limpa frame atual/bboxes
    void AccClearLive();
    void ClearLiveFrame(uint64_t now_ms);

    // (opcional) helper: seta distâncias inválidas/NaN para o snapshot saber que caiu
    void InvalidateDistances();


} // namespace Core