#include "core.hpp"
#include <cmath>
#include <deque>
#include <numbers>
using std::numbers::pi_v;

namespace {
    std::mutex g_mtx;

    std::string lidar_ip;
    std::string host_ip;
    std::string firmware_version;
    std::string dev_type;

    ImuRaw       g_imu;
    AttitudeRPY  g_rpy;
    uint64_t     g_last_imu_ms = 0;
    uint64_t     g_last_pts_ms = 0;

    float        g_temp_c = 0.f;
    uint32_t     g_power_count = 0;

    Eigen::Matrix4f g_Twm = Eigen::Matrix4f::Identity();

    CloudRGB     g_cloud_live;
    CloudRGB     g_cloud_map;

    Distance6    g_dist6;
    std::vector<BBox3D> g_boxes;

    std::mutex g_mtx_map;

    static TemporalCloudAccumulator g_live_acc;
    static TemporalCloudAccumulator g_map_acc;
} // anon

namespace Core {

    // ==== DEFINIÇÕES CORRETAS (sem Core::) ====
    std::atomic<uint32_t> handle{ 0 };
    std::atomic<bool>     connected{ false };
    std::atomic<bool>     running{ true };
    std::atomic<bool>     debug{ false };
    std::atomic<uint64_t> last_imu_ms{ 0 };
    std::atomic<double> freq_imu{ 0 };
    std::atomic<uint64_t> last_pcl_ms{ 0 };
    std::atomic<double> freq_pcl{ 0 };
    std::atomic<bool>     sensor_ok{ false };
    std::atomic<bool>     streaming{ false };

    void init() {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_Twm = Eigen::Matrix4f::Identity();
        g_cloud_live = {};
        g_cloud_map = {};
        g_dist6 = {};
        g_boxes.clear();

        g_live_acc.window_ms = 150;         // live com rastro curto (ms)
        g_live_acc.max_points = 200000;     // limite de pontos no live

        g_map_acc.window_ms = 0;            // 0 = sem poda temporal (mapa)
        g_map_acc.max_points = 2000000;     // cap de segurança pro mapa (ajuste à GPU)
    }

    void set_lidar_ip(const std::string& ip) {
        lidar_ip = ip;
    }
    void set_host_ip(const std::string& ip) {
        host_ip = ip;
    }
    const std::string& get_lidar_ip() {
        return lidar_ip;
    }
    const std::string& get_host_ip() {
        return host_ip;
    }

    void set_fw_version(const std::string& v) {
        firmware_version = v;
    }
    const std::string& get_fw_version() {
        return firmware_version;
    }
    
    void set_dev_type(const std::string& t) {
        dev_type = t;
    }
    const std::string& get_dev_type() {
        return dev_type;
    }

    void set_last_imu(const ImuRaw& s) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_imu = s;
        g_last_imu_ms = s.t_ms;
    }
    ImuRaw get_last_imu() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_imu;
    }

    void set_attitude(const AttitudeRPY& rpy) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_rpy = rpy;
    }
    AttitudeRPY get_attitude() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_rpy;
    }

    uint64_t get_last_imu_time_ms() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_last_imu_ms;
    }
    void set_last_points_time_ms(uint64_t t) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_last_pts_ms = t;
    }
    uint64_t get_last_points_time_ms() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_last_pts_ms;
    }

    void set_temperature_c(float t) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_temp_c = t;
    }
    float get_temperature_c() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_temp_c;
    }

    void set_power_count(uint32_t n) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_power_count = n;
    }
    uint32_t get_power_count() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_power_count;
    }

    void set_pose_Twm(const Eigen::Matrix4f& Twm) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_Twm = Twm;
    }
    Eigen::Matrix4f get_pose_Twm() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_Twm;
    }

    void set_cloud_live(const CloudRGB& c) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_cloud_live = c;
    }
    CloudRGB get_cloud_live() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_cloud_live;
    }

    void set_distances(const Distance6& d) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_dist6 = d;
    }
    Distance6 get_distances() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_dist6;
    }

    void set_bboxes(const std::vector<BBox3D>& boxes) {
        std::lock_guard<std::mutex> lk(g_mtx);
        g_boxes = boxes;
    }
    std::vector<BBox3D> get_bboxes() {
        std::lock_guard<std::mutex> lk(g_mtx);
        return g_boxes;
    }

    void set_cloud_map(const CloudRGB& c) {
        std::lock_guard<std::mutex> lk(g_mtx_map);
        g_cloud_map = c;
    }

    CloudRGB get_cloud_map() {
        std::lock_guard<std::mutex> lk(g_mtx_map);
        return g_cloud_map;
    }

    void clear_cloud_map() {
        std::lock_guard<std::mutex> lk(g_mtx_map);
        g_cloud_map = CloudRGB{};
    }

    void ResetGlobalMap()
    {
        clear_cloud_map();
        g_map_acc.clear();
        g_map_acc.window_ms = 0;
        g_map_acc.max_points = 2000000;
    }

    void AccConfigLive(uint64_t window_ms, size_t max_points) {
        g_live_acc.window_ms = window_ms;
        g_live_acc.max_points = max_points;
    }

    void AccConfigMap(uint64_t window_ms, size_t max_points) {
        g_map_acc.window_ms = window_ms;
        g_map_acc.max_points = max_points;
    }

    void AccPushLive(const CloudRGB& pkt, uint64_t t_ms) {
        g_live_acc.push(pkt);
        // opcional: se quiser podar no push por ref_ms de fora:
        (void)t_ms; // não precisa aqui; a prune já ocorre no push interno
    }

    CloudRGB AccBuildLive(uint64_t ref_ms) {
        return g_live_acc.buildMerged(ref_ms);
    }

    void AccPushMap(const CloudRGB& pkt, uint64_t t_ms) {
        g_map_acc.push(pkt);
        (void)t_ms;
    }

    CloudRGB AccBuildMap(uint64_t ref_ms) {
        return g_map_acc.buildMerged(ref_ms);
    }


    void AccClearLive() {
        // se seu acumulador live usa um deque protegido por mutex:
        std::lock_guard<std::mutex> lk(g_live_acc.mtx);  // ajuste ao seu nome real
        g_live_acc.ring.clear();
    }

    void ClearLiveFrame(uint64_t now_ms) {
        // limpa cloud atual
        CloudRGB empty;
        empty.t_ms = now_ms;
        empty.pts.clear();
        set_cloud_live(empty);

        // limpa bboxes
        set_bboxes({});

        // (opcional) invalida distâncias
        InvalidateDistances();

        // (opcional) zera a “idade” dos pontos para o snapshot/HUD ver que não há dados novos
        set_last_points_time_ms(now_ms);
    }

    void InvalidateDistances() {
        Distance6 d;
        d.front = std::numeric_limits<double>::quiet_NaN();
        d.back = std::numeric_limits<double>::quiet_NaN();
        d.left = std::numeric_limits<double>::quiet_NaN();
        d.right = std::numeric_limits<double>::quiet_NaN();
        d.up = std::numeric_limits<double>::quiet_NaN();
        d.down = std::numeric_limits<double>::quiet_NaN();
        set_distances(d);
    }


    // utils
    float Core::rad2deg(float r) { return r * 180.0f / pi_v<float>; }
    float Core::deg2rad(float d) { return d * pi_v<float> / 180.0f; }

    float wrapPi(float a) {
        const float pi = pi_v<float>;
        const float twoPi = 2.f * pi;
        while (a > pi) a -= twoPi;
        while (a < -pi) a += twoPi;
        return a;
    }

} // namespace Core