#include <iostream>
#include <filesystem>
#include <chrono>
#include <thread>
#include <atomic>
#include <vector>
#include <cstring>

#include "core.hpp"
#include "metrics.hpp"
#include "viewer_debug.hpp"
#include "status_snapshot.hpp"

#include "livox_lidar_api.h"
#include "livox_lidar_def.h"


namespace fs = std::filesystem;

inline void update_freq(std::atomic<double>& freq_hz, std::atomic<uint64_t>& last_ms, uint64_t now_ms, double alpha = 0.2) // 0<alpha<=1 (suavização)
{
    uint64_t prev = last_ms.load(std::memory_order_relaxed);
    last_ms.store(now_ms, std::memory_order_relaxed);

    if (prev == 0 || now_ms <= prev) {
        // primeira amostra ou relógio não avançou
        return;
    }

    const double dt_ms = static_cast<double>(now_ms - prev);
    if (dt_ms <= 0.0) return;

    const double inst_hz = 1000.0 / dt_ms; // ms -> Hz
    double f = freq_hz.load(std::memory_order_relaxed);
    if (f <= 0.0) {
        // inicializa sem EMA na primeira frequência válida
        freq_hz.store(inst_hz, std::memory_order_relaxed);
    }
    else {
        // EMA: f = (1-alpha)*f + alpha*inst
        f = (1.0 - alpha) * f + alpha * inst_hz;
        freq_hz.store(f, std::memory_order_relaxed);
    }
}

static std::string to_hex(const uint8_t* p, uint16_t n) {
    static const char* hex = "0123456789ABCDEF";
    std::string s; s.reserve(n * 2);
    for (uint16_t i = 0; i < n; ++i) {
        uint8_t b = p[i];
        s.push_back(hex[b >> 4]);
        s.push_back(hex[b & 0xF]);
        if (i + 1 < n) s.push_back(' ');
    }
    return s;
}

static std::string ascii_trim_zeros(const uint8_t* p, uint16_t n) {
    while (n && p[n - 1] == 0) --n;
    return std::string(reinterpret_cast<const char*>(p), n);
}



// ===================== Callbacks =====================


void InternalInfoCallback(livox_status status, uint32_t handle, LivoxLidarDiagInternalInfoResponse* response, void*) {
    if (status != kLivoxLidarStatusSuccess || !response) return;

    const uint8_t* ptr = response->data;
    for (uint16_t i = 0; i < response->param_num; ++i) {
        uint16_t id = *(uint16_t*)(ptr + 0);
        uint16_t length = *(uint16_t*)(ptr + 2);
        const uint8_t* val = ptr + 4;

        //std::cout << "Param " << id << " len=" << length << " val=" << to_hex(val, length) << std::endl;

        if (id == 0x0004 && length >= 12) {
            // 0–3: lidar IP, 4–7: subnet, 8–11: host IP
            char lidar_ip[16], host_ip[16];
            snprintf(lidar_ip, sizeof(lidar_ip), "%u.%u.%u.%u",
                val[0], val[1], val[2], val[3]);
            snprintf(host_ip, sizeof(host_ip), "%u.%u.%u.%u",
                val[8], val[9], val[10], val[11]);

            Core::set_lidar_ip(lidar_ip);
            Core::set_host_ip(host_ip);
        }
        else if (id == 0x8001 && length >= 1) {
            auto info = ascii_trim_zeros(val, length);

            // parse simples de FmVer e BuildTime
            auto dev_type = info.find("DevType:");
            if (dev_type != std::string::npos) {
                auto end = info.find(' ', dev_type);
                auto dvtype = info.substr(dev_type + 8, end - (dev_type + 8));
                Core::set_dev_type(dvtype);
            }
            auto fmver_pos = info.find("FmVer:");
            if (fmver_pos != std::string::npos) {
                auto end = info.find(' ', fmver_pos);
                auto fmver = info.substr(fmver_pos + 6, end - (fmver_pos + 6));
                Core::set_fw_version(fmver);
            }
        }
        else if (id == 0x8007 && length >= 4) {
            uint32_t raw = *(uint32_t*)val;
            Core::set_temperature_c(static_cast<float>(raw) / 100.0f);
        }
        else if (id == 0x8008 && length >= 4) {
            Core::set_power_count(*(uint32_t*)val);
        }
        ptr += (4 + length);
    }
}

void LidarInfoChangeCallback(const uint32_t handle, const LivoxLidarInfo* info, void*) {
    std::cout << "[INFO] InfoChange handle=" << handle << "\n";
    if (!info) return;

    Core::handle.store(handle, std::memory_order_relaxed);

    // WorkMode + DataType + PointSend
    auto wm = SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr);
    auto dt = SetLivoxLidarPclDataType(handle, kLivoxLidarCartesianCoordinateHighData, nullptr, nullptr);
    auto en = EnableLivoxLidarPointSend(handle, nullptr, nullptr);
    std::cout << "  WM=" << wm << " DT=" << dt << " EN=" << en << "\n";
}

void LidarImuDataCallback(const uint32_t handle, const uint8_t, LivoxLidarEthernetPacket* packet, void*) {
    Core::handle.store(handle, std::memory_order_relaxed);
    using namespace std::chrono;
    uint64_t now_ms = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
    if (!packet || packet->dot_num == 0) return;
    update_freq(Core::freq_imu, Core::last_imu_ms, now_ms);
    auto* imu_points = reinterpret_cast<LivoxLidarImuRawPoint*>(packet->data);
    const auto imu = imu_points[packet->dot_num - 1];
    Metrics::update_from_imu(imu.acc_x, imu.acc_y, imu.acc_z, imu.gyro_x, imu.gyro_y, imu.gyro_z, now_ms);
}

void LidarPointCloudCallback(const uint32_t handle, const uint8_t, LivoxLidarEthernetPacket* packet, void*) {
    Core::handle.store(handle, std::memory_order_relaxed);
    using namespace std::chrono;
    uint64_t now_ms = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
    if (!packet || packet->dot_num == 0) return;
    update_freq(Core::freq_pcl, Core::last_pcl_ms, now_ms);
    Core::last_pcl_ms.store(now_ms, std::memory_order_relaxed);
    Core::streaming.store(true, std::memory_order_relaxed);

    // 1) converte o pacote bruto em CloudRGB (sem coloração especial aqui)
    CloudRGB pkt;
    pkt.pts.reserve(packet->dot_num);
    pkt.t_ms = now_ms;

    auto push = [&](float x, float y, float z) {
        RGBPoint p; p.x = x; p.y = y; p.z = z; p.r = 255; p.g = 255; p.b = 255;
        pkt.pts.push_back(p);
        };

    if (packet->data_type == kLivoxLidarCartesianCoordinateHighData) {
        auto* pts = reinterpret_cast<LivoxLidarCartesianHighRawPoint*>(packet->data);
        for (uint16_t i = 0; i < packet->dot_num; ++i)
            push(pts[i].x / 1000.f, pts[i].y / 1000.f, pts[i].z / 1000.f);
    }
    else if (packet->data_type == kLivoxLidarCartesianCoordinateLowData) {
        auto* pts = reinterpret_cast<LivoxLidarCartesianLowRawPoint*>(packet->data);
        for (uint16_t i = 0; i < packet->dot_num; ++i)
            push(pts[i].x / 100.f, pts[i].y / 100.f, pts[i].z / 100.f);
    }

    // 2) empilha no acumulador temporal
    Core::AccPushLive(pkt, now_ms);
    CloudRGB merged = Core::AccBuildLive(now_ms);
    Core::set_cloud_live(merged);
    Core::set_last_points_time_ms(now_ms);
    Metrics::enqueue_cloud(merged, now_ms);

    if (Core::debug.load()) {
        if (ViewerDebug::get_mode_map()) {
            Core::AccPushMap(pkt, now_ms);
            CloudRGB map_merged = Core::AccBuildMap(now_ms);
            Core::set_cloud_map(map_merged);
        }
    }
}

// ===================== Main =====================

struct LidarSupervisor {
    std::thread th;
    std::atomic<bool> alive{ false };

    // thresholds (ajusta à vontade)
    int64_t pcl_timeout_ms = 2000;  // consideramos "sem stream" se >2 s sem nuvem
    int64_t imu_timeout_ms = 3000;  // IMU pode ser um pouco mais folgado
    int64_t diag_period_ms = 3000;  // query de diagnóstico a cada 3 s quando ativo
    int64_t retry_period_ms = 500;  // ciclo de verificação

    void start() {
        alive = true;
        th = std::thread([this] { loop(); });
    }

    void stop() {
        alive = false;
        if (th.joinable()) th.join();
    }

    void loop() {
        using namespace std::chrono;
        uint64_t last_diag = 0;

        while (alive.load() && Core::running.load()) {
            const uint64_t now = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();

            const auto last_pcl = Core::last_pcl_ms.load(std::memory_order_relaxed);
            const auto last_imu = Core::last_imu_ms.load(std::memory_order_relaxed);
            const auto h = Core::handle.load(std::memory_order_relaxed);

            bool have_handle = (h != 0);

            // Avalia saúde
            bool pcl_ok = have_handle && (last_pcl != 0) && (now - last_pcl <= (uint64_t)pcl_timeout_ms);
            bool imu_ok = have_handle && (last_imu != 0) && (now - last_imu <= (uint64_t)imu_timeout_ms);
            bool sensor_ok = have_handle && (pcl_ok || imu_ok);

            // Atualiza flag pública
            bool prev_ok = Core::sensor_ok.exchange(sensor_ok, std::memory_order_relaxed);

            // Se temos handle mas stream caiu, tenta reabilitar envio
            if (have_handle && !Core::connected.load(std::memory_order_relaxed)) {
                // força "enable point send" e tipo dos pontos de novo (idempotente)
                auto dt = SetLivoxLidarPclDataType(h, kLivoxLidarCartesianCoordinateHighData, nullptr, nullptr);
                auto en = EnableLivoxLidarPointSend(h, nullptr, nullptr);
                Core::connected.store((dt == kLivoxLidarStatusSuccess || en == kLivoxLidarStatusSuccess), std::memory_order_relaxed);
                if (dt != kLivoxLidarStatusSuccess || en != kLivoxLidarStatusSuccess) {
                    
                }
            }

            // Diagnóstico periódico (temp/power count)
            if (sensor_ok && have_handle && (now - last_diag >= (uint64_t)diag_period_ms)) {
                last_diag = now;
                QueryLivoxLidarInternalInfo(h, InternalInfoCallback, nullptr);
            }

            // Transições de estado úteis pra log
            if (sensor_ok && !prev_ok) {
                std::cout << "[SUP] LiDAR OK (stream ativo)\n";

                if (Core::debug.load()) {
                    ViewerDebug::start(false);
                    std::cout << "[VIEWER] Debug viewer ON. Pressione Ctrl+C para sair.\n";
                }
            }
            else if (!sensor_ok && prev_ok) {
                std::cout << "[SUP] LiDAR OFF/DEGRADED (sem dados recentes)\n";
                Core::connected.store(false, std::memory_order_relaxed);
                Core::streaming.store(false, std::memory_order_relaxed);

                // —— FLUSH de estado ativo ——
                Core::AccClearLive();
                Core::ClearLiveFrame(now);
                Core::freq_pcl.store(0.0, std::memory_order_relaxed);
                Core::freq_imu.store(0.0, std::memory_order_relaxed);
                Metrics::update_from_imu(0, 0, 0, 0, 0, 0, now);

                if (Core::debug.load()) {
                    ViewerDebug::stop();
                    std::cout << "[VIEWER] Debug viewer OFF.\n";
                }
            }

            StatusSnapshot snap = StatusSnapshot::collect_now();
            bool status = snap.publish_state_partial(10, 20);
            bool status_imu = snap.publish_state_imu();
            //std::string json = snap.to_json_string();
            //std::cout << "PUBLICADO: " << status << " - " << json << "\r\n";

            std::this_thread::sleep_for(std::chrono::milliseconds(retry_period_ms));
        }
    }
};

static LidarSupervisor g_sup;

int main(int argc, char** argv) {
    if (argc < 2) {
        std::cout << "Uso: app <config.json> [--debug]\n";
        return -1;
    }
    const std::string cfg = argv[1];
    Core::debug.store((argc >= 3 && std::strcmp(argv[2], "--debug") == 0), std::memory_order_relaxed);

    Core::init();
    Metrics::init(MetricsParams{});
    Metrics::start_worker();
    StatusSnapshot::start_publisher();

    if (!LivoxLidarSdkInit(cfg.c_str())) {
        std::cerr << "[ERR] LivoxLidarSdkInit falhou\n";
        return -1;
    }

    SetLivoxLidarPointCloudCallBack(LidarPointCloudCallback, nullptr);
    SetLivoxLidarImuDataCallback(LidarImuDataCallback, nullptr);
    SetLivoxLidarInfoChangeCallback(LidarInfoChangeCallback, nullptr);

    if (!LivoxLidarSdkStart()) {
        std::cerr << "[ERR] LivoxLidarSdkStart falhou\n";
        LivoxLidarSdkUninit();
        return -1;
    }

    // supervisor ON (faz query inicial e monitora tudo)
    g_sup.start();

    while (Core::running.load()) std::this_thread::sleep_for(std::chrono::milliseconds(100));

    g_sup.stop();
    Metrics::stop_worker();
    LivoxLidarSdkUninit();

    return 0;
}