GrabBag/Device/RsLidarDevice/Src/RsLidarDevice.cpp

#include "RsLidarDevice.h"
#include <cmath>
#include <cstdio>
#include <chrono>
#include <cstring>
#include <thread>
#include <cstdlib>
#include <new>
#ifdef _WIN32
#include <malloc.h>
#endif
#include <rs_driver/driver/decoder/compress_algo.hpp>

namespace
{
constexpr uint32_t kRsem4DefaultRings = 520;
constexpr uint32_t kRsem4ReserveColumns = 1200;
constexpr uint32_t kRsem4HalfRings = kRsem4DefaultRings / 2;
constexpr size_t kRsem4VecselsPerColumn = 26;
constexpr size_t kRsem4PixelsPerVcsel = 20;
constexpr size_t kRsem4SurfaceCount = 4;
constexpr size_t kRsem4BlocksPerPacket = 260;
constexpr size_t kRsem4MsopTailLen = 12;
constexpr size_t kDeliveryFramePoolSize = 6;
constexpr float kRsem4DistanceResolution = 0.005f;
constexpr float kRsem4MinDistance = 0.5f;
constexpr float kRsem4MaxDistance = 350.0f;
constexpr double kDeg01ToRad = 3.14159265358979323846 / 18000.0;

#pragma pack(push, 1)
struct Rsem4DifopPkt
{
    uint8_t id[8];
    uint8_t reserved0[106];
    int8_t yawOffset[26];
    int16_t pitchAngle[520];
    int16_t surfacePitchOffset[4];
    uint8_t reserved1[110];
    uint16_t dataLength;
    uint16_t counter;
    uint32_t dataId;
    uint32_t crc32;
};

struct Rsem4Difop2Pkt
{
    uint8_t id[4];
    uint8_t reserved0[63];
    uint8_t surfaceId;
    uint8_t pixelCnt;
    uint8_t vcselCnt;
    int8_t yawOffset[26];
    int16_t pitchAngle[520];
    int16_t surfacePitchOffset[4];
    int16_t rollOffset;
    uint8_t reserved1[4];
    uint16_t dataLength;
    uint16_t counter;
    uint32_t dataId;
    uint32_t crc32;
};

struct Rsem4Difop0624Pkt
{
    uint8_t id[4];
    uint8_t reserved0[306];
    int8_t yawOffset[26];
    int16_t pitchAngle[520];
    int16_t surfacePitchOffset[4];
    uint8_t reserved1[6];
    uint16_t dataLength;
    uint16_t counter;
    uint32_t dataId;
    uint32_t crc32;
};

struct Rsem4Channel
{
    uint16_t distance;
    uint8_t intensity;
    uint8_t pointAttribute;
};

struct Rsem4Block
{
    Rsem4Channel channel[1];
};

struct Rsem4MsopHeader
{
    uint8_t id[4];
    uint16_t pktSeq;
    uint16_t protocolVersion;
    uint8_t returnMode;
    uint8_t timeMode;
    RSTimestampUTC timestamp;
    uint8_t frameSync;
    uint8_t frameRate;
    uint16_t columnNum;
    int16_t yawAngle;
    uint8_t packMode;
    uint8_t surfaceId;
    uint16_t reserved;
    uint8_t lidarType;
    uint8_t temperature;
};

struct Rsem4MsopHeader2
{
    uint8_t id[4];
    uint16_t pktSeq;
    uint8_t reserved[2];
};

struct Rsem4MsopPkt
{
    Rsem4MsopHeader header;
    Rsem4Block blocks[260];
    uint16_t dataLength;
    uint16_t counter;
    uint32_t dataId;
    uint32_t crc32;
};
#pragma pack(pop)

inline void SetZeroPoint(SVzNLPointXYZI& dst)
{
    dst.fData[0] = 0.0f;
    dst.fData[1] = 0.0f;
    dst.fData[2] = 0.0f;
    dst.fData_c[0] = 0.0f;
}

inline int16_t SwapI16(int16_t value)
{
    uint16_t v = static_cast<uint16_t>(value);
    v = static_cast<uint16_t>((v << 8) | (v >> 8));
    return static_cast<int16_t>(v);
}

inline uint16_t ReadU16BE(uint16_t value)
{
    return static_cast<uint16_t>((value << 8) | (value >> 8));
}

inline int32_t NormalizeAngle01(int32_t angle)
{
    angle %= 36000;
    if (angle < 0)
        angle += 36000;
    return angle;
}

struct Deg01TrigTable
{
    std::array<float, 36000> sinValues;
    std::array<float, 36000> cosValues;

    Deg01TrigTable()
    {
        for (size_t i = 0; i < sinValues.size(); ++i)
        {
            const double rad = static_cast<double>(i) * kDeg01ToRad;
            sinValues[i] = static_cast<float>(std::sin(rad));
            cosValues[i] = static_cast<float>(std::cos(rad));
        }
    }
};

inline const Deg01TrigTable& TrigTable()
{
    static const Deg01TrigTable table;
    return table;
}

inline float SinDeg01(int32_t angle)
{
    return TrigTable().sinValues[NormalizeAngle01(angle)];
}

inline float CosDeg01(int32_t angle)
{
    return TrigTable().cosValues[NormalizeAngle01(angle)];
}

inline bool IsCompleteRsem4Packet(const uint8_t* data, size_t size)
{
    static const uint8_t kHeader[] = {0x55, 0xAA, 0x5A, 0xA5};
    return size >= sizeof(kHeader) && std::memcmp(data, kHeader, sizeof(kHeader)) == 0;
}

inline void UpdateMax(std::atomic<uint64_t>& target, uint64_t value)
{
    uint64_t prev = target.load(std::memory_order_relaxed);
    while (value > prev &&
           !target.compare_exchange_weak(prev, value, std::memory_order_relaxed))
    {}
}

inline void UpdateMaxSize(std::atomic<size_t>& target, size_t value)
{
    size_t prev = target.load(std::memory_order_relaxed);
    while (value > prev &&
           !target.compare_exchange_weak(prev, value, std::memory_order_relaxed))
    {}
}

inline uint64_t ElapsedUs(std::chrono::steady_clock::time_point begin,
                          std::chrono::steady_clock::time_point end)
{
    return static_cast<uint64_t>(
        std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count());
}
}

// WSAStartup 引用计数
static std::atomic<int> g_sockRefCount{0};

// ============================================================
// SDK ErrCode → 统计桶映射
// SDK error_code.hpp: MSOPTIMEOUT=0x40 WRONGMSOPLEN=0x42 PKTBUFOVERFLOW=0x48 CLOUDOVERFLOW=0x49
// 桶顺序与日志列对齐：0=msopto 1=pktof 2=cldof 3=wlen 4=other
// ============================================================
size_t CRsLidarDevice::errCodeToBucket(int errCode)
{
    switch (errCode)
    {
        case 0x40: return 0;  // MSOPTIMEOUT
        case 0x48: return 1;  // PKTBUFOVERFLOW
        case 0x49: return 2;  // CLOUDOVERFLOW
        case 0x42: return 3;  // WRONGMSOPLEN
        default:   return 4;  // other
    }
}

// ============================================================
// 工厂方法
// ============================================================
int IRsLidarDevice::CreateObject(IRsLidarDevice** ppDevice)
{
    if (!ppDevice) return -1;
    CRsLidarDevice* p = new CRsLidarDevice();
    *ppDevice = p;
    return 0;
}

// ============================================================
// 构造 / 析构
// ============================================================
CRsLidarDevice::CRsLidarDevice()
    : m_pDriver(std::make_unique<LidarDriver<SdkCloudMsg>>())
{
    initializeRsem4DefaultAngles();
}

CRsLidarDevice::~CRsLidarDevice()
{
    Stop();
    CloseDevice();
}

void* CRsLidarDevice::operator new(std::size_t size)
{
#ifdef _WIN32
    void* ptr = _aligned_malloc(size, 64);
    if (!ptr)
        throw std::bad_alloc();
    return ptr;
#else
    void* ptr = nullptr;
    if (posix_memalign(&ptr, 64, size) != 0)
        throw std::bad_alloc();
    return ptr;
#endif
}

void CRsLidarDevice::operator delete(void* ptr) noexcept
{
#ifdef _WIN32
    _aligned_free(ptr);
#else
    std::free(ptr);
#endif
}

void CRsLidarDevice::operator delete(void* ptr, std::size_t) noexcept
{
    CRsLidarDevice::operator delete(ptr);
}

// ============================================================
// InitDevice
// ============================================================
int CRsLidarDevice::InitDevice()
{
#ifdef _WIN32
    if (g_sockRefCount == 0)
    {
        WSADATA wsaData;
        if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0)
        {
            return -1;
        }
    }
    g_sockRefCount++;
#endif
    return 0;
}

// ============================================================
// 类型转换: RsLidarConfig → RSDriverParam
// ============================================================
RSDriverParam CRsLidarDevice::toDriverParam(const RsLidarConfig& config)
{
    RSDriverParam param;
    param.lidar_type               = strToLidarType(config.lidarType);
    param.input_type               = InputType::ONLINE_LIDAR;
    param.frame_id                 = config.frameId;
    param.input_param.host_address = config.hostAddress;
    param.input_param.msop_port    = config.msopPort;
    param.input_param.difop_port   = config.difopPort;
    param.input_param.imu_port     = config.imuPort;
    param.decoder_param.min_distance  = config.minDistance;
    param.decoder_param.max_distance  = config.maxDistance;
    param.decoder_param.dense_points  = config.densePoints;
    param.decoder_param.use_lidar_clock = config.useLidarClock;
    param.decoder_param.ts_first_point  = config.tsFirstPoint;
    param.decoder_param.wait_for_difop  = config.waitForDifop;
    param.decoder_param.start_angle     = config.startAngle;
    param.decoder_param.end_angle       = config.endAngle;
    param.decoder_param.split_angle     = config.splitAngle;

    param.input_param.socket_recv_buf   = (config.socketRecvBufBytes > 0)
                                          ? config.socketRecvBufBytes
                                          : 33554432;  // 32MB，低配电脑防内核缓冲溢出

    return param;
}

// ============================================================
// OpenDevice
// ============================================================
int CRsLidarDevice::OpenDevice(const RsLidarConfig& config)
{
    if (!m_pDriver)
    {
        return -1;
    }

    CloseDevice();

    m_param = toDriverParam(config);
    if (config.minDistance > 0.0f || config.maxDistance > 0.0f)
    {
        m_rsem4MinDistance = (config.minDistance > 0.0f) ? config.minDistance : 0.0f;
        m_rsem4MaxDistance = (config.maxDistance > 0.0f) ? config.maxDistance : kRsem4MaxDistance;
    }
    else
    {
        m_rsem4MinDistance = kRsem4MinDistance;
        m_rsem4MaxDistance = kRsem4MaxDistance;
    }

    m_pDriver->regPointCloudCallback(
        [this]() -> SdkCloudPtr { return this->getFreeCloud(); },
        [this](SdkCloudPtr msg)  { this->putStuffedCloud(msg); }
    );

    m_pDriver->regPacketCallback(
        [this](const Packet& pkt) { this->onPacket(pkt); }
    );

    m_pDriver->regExceptionCallback(
        [this](const Error& code) { this->onException(code); }
    );

    if (!m_pDriver->init(m_param))
    {
        return -2;
    }

    m_bOpened = true;
    return 0;
}

// ============================================================
// CloseDevice
// ============================================================
int CRsLidarDevice::CloseDevice()
{
    if (m_bRunning)
    {
        Stop();
    }

    m_bOpened = false;

    m_freeQueue.clear();
    m_frameFreeQueue.clear();
    m_deliveryQueue.clear();

#ifdef _WIN32
    if (--g_sockRefCount == 0)
    {
        WSACleanup();
    }
#endif

    return 0;
}

bool CRsLidarDevice::IsOpened() const
{
    return m_bOpened;
}

// ============================================================
// Start / Stop
// ============================================================
int CRsLidarDevice::Start()
{
    if (!m_bOpened || !m_pDriver)
    {
        return -1;
    }

    if (m_bRunning)
    {
        return 0;
    }

    // 重置诊断计数（每次 Start 开始新会话统计）
    m_droppedFrameCount.store(0, std::memory_order_relaxed);
    m_totalPushCount.store(0, std::memory_order_relaxed);
    for (auto& c : m_exceptionCounts) c.store(0, std::memory_order_relaxed);
    m_parseAccumUs.store(0, std::memory_order_relaxed);
    m_parseMaxUs.store(0, std::memory_order_relaxed);
    m_parseCount.store(0, std::memory_order_relaxed);
    m_frameWaitAccumUs.store(0, std::memory_order_relaxed);
    m_frameWaitMaxUs.store(0, std::memory_order_relaxed);
    m_frameWaitCount.store(0, std::memory_order_relaxed);
    m_frameWaitTimeoutCount.store(0, std::memory_order_relaxed);
    m_deliveryDropCount.store(0, std::memory_order_relaxed);
    m_deliveredFrameCount.store(0, std::memory_order_relaxed);
    m_deliveryQueuePeak.store(0, std::memory_order_relaxed);
    m_cbMaxUs.store(0, std::memory_order_relaxed);
    m_cbAccumUs.store(0, std::memory_order_relaxed);
    m_cbCount.store(0, std::memory_order_relaxed);
    m_diagLastPushCount = 0;
    m_diagLastQueueDropCount = 0;
    m_diagLastParseCount = 0;
    m_diagLastParseAccumUs = 0;
    m_diagLastFrameWaitCount = 0;
    m_diagLastFrameWaitAccumUs = 0;
    m_diagLastFrameWaitTimeoutCount = 0;
    m_diagLastDeliveryDropCount = 0;
    m_diagLastDeliveredFrameCount = 0;
    m_diagLastCbCount = 0;
    m_diagLastCbAccumUs = 0;
    m_diagLastExceptionCounts.fill(0);
    m_diagLastLogTime = std::chrono::steady_clock::now();
    resetPacketFrameBuilder();

    // 清空可能残留的预热缓冲（防止 Stop→Start 循环累积膨胀）
    m_freeQueue.clear();

    for (int i = 0; i < 4; ++i)
    {
        auto msg = std::make_shared<SdkCloudMsg>();
        m_freeQueue.push(std::move(msg));
    }

    m_bStopProcess = false;

    // 预分配交付帧：6 帧 × 1200 线 × 520 点 × 32B ≈ 120MB
    // 帧 0=SDK packet callback 填充中，其余用于交付队列和短时回调抖动缓冲
    m_deliveryQueue.clear();
    m_frameFreeQueue.clear();
    for (size_t i = 0; i < kDeliveryFramePoolSize; ++i)
    {
        auto frame = std::make_shared<DeliveryFrame>();
        frame->reserveLines(kRsem4ReserveColumns, kRsem4DefaultRings);
        m_frameFreeQueue.push(frame);
    }

    m_bRunning = true;
    m_bStopDelivery = false;
    m_deliveryThread = std::thread(&CRsLidarDevice::deliveryThread, this);
    m_pDriver->start();

    return 0;
}

int CRsLidarDevice::Stop()
{
    if (!m_bRunning)
    {
        return 0;
    }

    // ① 停止包级解析
    m_bStopProcess = true;

    if (m_pDriver)
    {
        m_pDriver->stop();
    }
    resetPacketFrameBuilder();

    // ② 停止交付线程（推入空帧唤醒 popWait，避免等超时）
    m_bStopDelivery = true;
    m_deliveryQueue.push(std::make_shared<DeliveryFrame>());
    if (m_deliveryThread.joinable())
    {
        m_deliveryThread.join();
    }

    // ③ 释放所有队列内存
    m_deliveryQueue.clear();
    m_frameFreeQueue.clear();

    m_bRunning = false;
    return 0;
}

bool CRsLidarDevice::IsRunning() const
{
    return m_bRunning;
}

// ============================================================
// 回调注册
// ============================================================
int CRsLidarDevice::SetPointCloudCallback(PointCloudCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_pointCloudCallback = std::move(callback);
    return 0;
}

int CRsLidarDevice::SetPacketCallback(PacketCallback callback)
{
    bool hasCallback = false;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        m_packetCallback = std::move(callback);
        hasCallback = static_cast<bool>(m_packetCallback);
    }
    m_hasPacketCallback.store(hasCallback, std::memory_order_release);
    return 0;
}

int CRsLidarDevice::SetExceptionCallback(ExceptionCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_exceptionCallback = std::move(callback);
    return 0;
}

std::string CRsLidarDevice::GetVersion()
{
    return getDriverVersion();
}

// ============================================================
// 内部：5s 窗口诊断日志输出 + 字段重置（仅当距上次输出 ≥5s 时实际输出）
// 由 SDK 包处理线程触发，计数器只做 relaxed 采样，避免诊断影响实时路径。
// ============================================================
void CRsLidarDevice::emitDiagnosticLogIfDue(std::chrono::steady_clock::time_point& lastLogTime)
{
    auto now = std::chrono::steady_clock::now();
    if (std::chrono::duration_cast<std::chrono::seconds>(now - lastLogTime).count() < 5)
    {
        return;
    }

    const double sec = std::chrono::duration_cast<std::chrono::duration<double>>(now - lastLogTime).count();

    const uint64_t pushCount = m_totalPushCount.load(std::memory_order_relaxed);
    const uint64_t queueDropCount = m_droppedFrameCount.load(std::memory_order_relaxed);
    const uint64_t frameWaitTimeoutCount = m_frameWaitTimeoutCount.load(std::memory_order_relaxed);
    const uint64_t cbCount = m_cbCount.load(std::memory_order_relaxed);
    const uint64_t cbAccumUs = m_cbAccumUs.load(std::memory_order_relaxed);

    const uint64_t pushDelta = pushCount - m_diagLastPushCount;
    const uint64_t queueDropDelta = queueDropCount - m_diagLastQueueDropCount;
    const uint64_t frameWaitTimeoutDelta = frameWaitTimeoutCount - m_diagLastFrameWaitTimeoutCount;
    const uint64_t cbDelta = cbCount - m_diagLastCbCount;
    const uint64_t cbAccumDelta = cbAccumUs - m_diagLastCbAccumUs;

    uint64_t excDelta[EXC_BUCKET_COUNT] = {};
    for (size_t i = 0; i < EXC_BUCKET_COUNT; ++i)
    {
        const uint64_t current = m_exceptionCounts[i].load(std::memory_order_relaxed);
        excDelta[i] = current - m_diagLastExceptionCounts[i];
        m_diagLastExceptionCounts[i] = current;
    }

    fprintf(stderr,
        "[RsLidarDevice][diag %.1fs] fps=%.1f frames=%llu(+%llu) "
        "drop=%llu(+%llu) q=%zu/%zu waitTO=%llu(+%llu) "
        "cb_us=%llu/%llu n=%llu exc48=%llu exc49=%llu exc42=%llu\n",
        sec,
        (sec > 0.0) ? (static_cast<double>(pushDelta) / sec) : 0.0,
        static_cast<unsigned long long>(pushCount),
        static_cast<unsigned long long>(pushDelta),
        static_cast<unsigned long long>(queueDropCount),
        static_cast<unsigned long long>(queueDropDelta),
        static_cast<size_t>(m_deliveryQueue.size()),
        static_cast<size_t>(m_deliveryQueuePeak.load(std::memory_order_relaxed)),
        static_cast<unsigned long long>(frameWaitTimeoutCount),
        static_cast<unsigned long long>(frameWaitTimeoutDelta),
        static_cast<unsigned long long>((cbDelta > 0) ? (cbAccumDelta / cbDelta) : 0),
        static_cast<unsigned long long>(m_cbMaxUs.load(std::memory_order_relaxed)),
        static_cast<unsigned long long>(cbDelta),
        static_cast<unsigned long long>(excDelta[2]),
        static_cast<unsigned long long>(excDelta[3]),
        static_cast<unsigned long long>(excDelta[4]));

    m_diagLastPushCount = pushCount;
    m_diagLastQueueDropCount = queueDropCount;
    m_diagLastFrameWaitTimeoutCount = frameWaitTimeoutCount;
    m_diagLastCbCount = cbCount;
    m_diagLastCbAccumUs = cbAccumUs;
    lastLogTime = now;
}

// 内部：交付线程（只调回调，不做数据处理）
// 从 m_deliveryQueue 取帧 → 调 PointCloudCallback（PushFrame memcpy 在此发生）
// 与 SDK packet callback 并行：交付帧 N 的同时，解析帧 N+1
// ============================================================
void CRsLidarDevice::deliveryThread()
{
    while (!m_bStopDelivery)
    {
        auto frame = m_deliveryQueue.popWait(500000);
        if (!frame) continue;          // 超时 → 检查 m_bStopDelivery

        // 取回调（持锁时间极短）
        PointCloudCallback cb;
        {
            std::lock_guard<std::mutex> lock(m_callbackMutex);
            cb = m_pointCloudCallback;
        }

        if (frame->cloudData.empty())
        {
            m_frameFreeQueue.push(frame);
            continue;
        }

        if (cb)
        {
            auto t0 = std::chrono::steady_clock::now();
            cb(frame->cloudData, frame->info);
            auto t1 = std::chrono::steady_clock::now();

            // 累计回调耗时统计
            uint64_t us = static_cast<uint64_t>(
                std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count());
            m_cbAccumUs.fetch_add(us, std::memory_order_relaxed);
            m_cbCount.fetch_add(1, std::memory_order_relaxed);

            uint64_t prevMax = m_cbMaxUs.load(std::memory_order_relaxed);
            while (us > prevMax &&
                   !m_cbMaxUs.compare_exchange_weak(prevMax, us, std::memory_order_relaxed))
            {}
        }

        // 归还交付帧到空闲池（清空 cloudData 元数据，保留 pointPool capacity）
        m_deliveredFrameCount.fetch_add(1, std::memory_order_relaxed);
        frame->cloudData.clear();
        m_frameFreeQueue.push(frame);
    }
}

// ============================================================
// 内部：空闲/就绪队列回调（rs_driver 内部线程）
// ============================================================
CRsLidarDevice::SdkCloudPtr CRsLidarDevice::getFreeCloud()
{
    auto msg = m_freeQueue.pop();
    if (msg) return msg;
    return std::make_shared<SdkCloudMsg>();
}

void CRsLidarDevice::putStuffedCloud(SdkCloudPtr msg)
{
    if (msg)
    {
        msg->points.clear();
        m_freeQueue.push(msg);
    }
}

// ============================================================
// 内部：Packet → RsPacketInfo
// ============================================================
void CRsLidarDevice::resetPacketFrameBuilder()
{
    if (m_activeFrame)
    {
        m_activeFrame->cloudData.clear();
        m_frameFreeQueue.push(m_activeFrame);
    }
    m_activeFrame.reset();
    m_activeFrameHasData = false;
    m_activeFrameAllValid = true;
    m_activeFrameStartedAtBoundary = false;
    resetActiveFrameTracking();
    m_rsem4FrameSynced = false;
    m_hasRsem4PrevSeq = false;
    m_prevRsem4Seq = 0;
    m_hasRsem4PrevColumn = false;
    m_prevRsem4Column = 0;
    m_rsem4HasPartialPacket = false;
    m_rsem4PartialSeq = 0;
    m_rsem4PartialLen = 0;
    m_diagPacketTick = 0;
}

void CRsLidarDevice::initializeRsem4DefaultAngles()
{
    m_rsem4YawOffset.fill(0);
    m_rsem4SurfacePitchOffset.fill(0);
    for (size_t i = 0; i < m_rsem4PitchAngle.size(); ++i)
        m_rsem4PitchAngle[i] = static_cast<int16_t>(-1300 + static_cast<int>(i) * 5);
    m_rsem4AnglesReady = false;
    updateRsem4PitchTrig();
}

void CRsLidarDevice::updateRsem4PitchTrig()
{
    for (size_t surface = 0; surface < kRsem4SurfaceCount; ++surface)
        for (size_t i = 0; i < m_rsem4PitchAngle.size(); ++i)
        {
            const int pitch = static_cast<int>(m_rsem4PitchAngle[i]) +
                              static_cast<int>(m_rsem4SurfacePitchOffset[surface]);
            m_rsem4PitchSin[surface][i] = SinDeg01(pitch);
            m_rsem4PitchCos[surface][i] = CosDeg01(pitch);
        }
}

DeliveryFramePtr CRsLidarDevice::acquireFrameForBuild()
{
    while (!m_bStopProcess.load(std::memory_order_relaxed))
    {
        const auto t0 = std::chrono::steady_clock::now();
        auto frame = m_frameFreeQueue.popWait(100000);
        const auto t1 = std::chrono::steady_clock::now();
        const uint64_t waitUs = ElapsedUs(t0, t1);
        m_frameWaitAccumUs.fetch_add(waitUs, std::memory_order_relaxed);
        m_frameWaitCount.fetch_add(1, std::memory_order_relaxed);
        UpdateMax(m_frameWaitMaxUs, waitUs);
        if (frame)
        {
            resetDeliveryFrame(frame);
            return frame;
        }
        m_frameWaitTimeoutCount.fetch_add(1, std::memory_order_relaxed);
    }
    return DeliveryFramePtr();
}

void CRsLidarDevice::resetDeliveryFrame(const DeliveryFramePtr& frame)
{
    if (!frame)
        return;

    const size_t pointCount =
        static_cast<size_t>(kRsem4ReserveColumns) * static_cast<size_t>(kRsem4DefaultRings);
    if (frame->pointPool.size() != pointCount)
        frame->pointPool.resize(pointCount);

    frame->cloudData.clear();
    frame->cloudData.reserve(kRsem4ReserveColumns);
    std::memset(frame->pointPool.data(), 0, frame->pointPool.size() * sizeof(SVzNLPointXYZI));

    for (uint32_t col = 0; col < kRsem4ReserveColumns; ++col)
    {
        SVzNLPointXYZI* linePts = frame->pointPool.data() +
            static_cast<size_t>(col) * static_cast<size_t>(kRsem4DefaultRings);

        SVzLaserLineData ld{};
        ld.p3DPoint = linePts;
        ld.nPointCount = static_cast<int>(kRsem4DefaultRings);
        ld.llFrameIdx = col;
        ld.llTimeStamp = col;
        ld.nEncodeNo = static_cast<int>(col);
        ld.bEndOnceScan = (col + 1 == kRsem4ReserveColumns) ? VzTrue : VzFalse;
        frame->cloudData.emplace_back(keResultDataType_PointXYZI, ld);
    }

    frame->info.height = kRsem4ReserveColumns;
    frame->info.width = kRsem4DefaultRings;
    frame->info.isDense = false;
}

void CRsLidarDevice::submitActiveFrame()
{
    if (!m_activeFrame)
        return;

    auto frame = m_activeFrame;
    m_activeFrame.reset();

    const bool completeFrame =
        m_activeFrameHasData &&
        m_activeFrameStartedAtBoundary &&
        !m_activeFramePacketLoss &&
        m_activeCompleteColumns == kRsem4ReserveColumns;

    if (!completeFrame)
    {
        frame->cloudData.clear();
        m_frameFreeQueue.push(frame);
        if (m_activeFrameStartedAtBoundary && m_activeFrameHasData)
            m_droppedFrameCount.fetch_add(1, std::memory_order_relaxed);
        m_activeFrameStartedAtBoundary = false;
        resetActiveFrameTracking();
        return;
    }

    frame->info.height = kRsem4ReserveColumns;
    frame->info.width = kRsem4DefaultRings;
    frame->info.isDense = m_activeFrameAllValid;

    const uint64_t parseUs = ElapsedUs(m_activeFrameStartTime, std::chrono::steady_clock::now());
    m_parseAccumUs.fetch_add(parseUs, std::memory_order_relaxed);
    m_parseCount.fetch_add(1, std::memory_order_relaxed);
    UpdateMax(m_parseMaxUs, parseUs);

    while (!m_bStopDelivery.load(std::memory_order_relaxed))
    {
        const size_t depth = m_deliveryQueue.push(frame);
        if (depth > 0)
        {
            UpdateMaxSize(m_deliveryQueuePeak, depth);
            m_totalPushCount.fetch_add(1, std::memory_order_relaxed);
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

    if (m_bStopDelivery.load(std::memory_order_relaxed))
        m_frameFreeQueue.push(frame);

    m_activeFrameHasData = false;
    m_activeFrameAllValid = true;
    m_activeFrameStartedAtBoundary = false;
    resetActiveFrameTracking();
}

void CRsLidarDevice::resetActiveFrameTracking()
{
    m_activeFramePacketLoss = false;
    m_activeCompleteColumns = 0;
    m_activeColumnCompleteMask = 0;
    m_activeColumnMask.fill(0);
}

bool CRsLidarDevice::beginActiveFrame(bool startedAtBoundary)
{
    if (m_activeFrame)
        return true;

    m_activeFrame = acquireFrameForBuild();
    if (!m_activeFrame)
        return false;

    m_activeFrameStartTime = std::chrono::steady_clock::now();
    m_activeFrameStartedAtBoundary = startedAtBoundary;
    m_activeFrameHasData = false;
    m_activeFrameAllValid = true;
    resetActiveFrameTracking();
    return true;
}

bool CRsLidarDevice::prepareRsem4FrameForColumn(uint32_t column)
{
    if (column >= kRsem4ReserveColumns)
        return false;

    const bool columnWrap = m_hasRsem4PrevColumn && (column + 10 < m_prevRsem4Column);
    const bool columnStart = (column == 0);

    if (columnWrap && m_activeFrameHasData)
        submitActiveFrame();

    m_hasRsem4PrevColumn = true;
    m_prevRsem4Column = column;

    if (!m_activeFrame)
    {
        if (!columnStart)
            return false;
        return beginActiveFrame(true);
    }

    return true;
}

void CRsLidarDevice::markRsem4ColumnSegment(uint32_t column, uint8_t segmentMask, uint8_t completeMask)
{
    if (!m_activeFrame || column >= kRsem4ReserveColumns || segmentMask == 0)
        return;

    if (m_activeColumnCompleteMask == 0)
    {
        m_activeColumnCompleteMask = completeMask;
    }
    else if (m_activeColumnCompleteMask != completeMask)
    {
        m_activeFramePacketLoss = true;
        return;
    }

    uint8_t& mask = m_activeColumnMask[column];
    const bool wasComplete = ((mask & completeMask) == completeMask);
    mask = static_cast<uint8_t>(mask | segmentMask);
    const bool isComplete = ((mask & completeMask) == completeMask);
    if (!wasComplete && isComplete)
        ++m_activeCompleteColumns;
}

void CRsLidarDevice::fillRsem4Point(uint32_t column, uint32_t ring, float distance,
                                    uint8_t intensity, uint8_t surfaceIndex,
                                    float cosYaw, float sinYaw)
{
    if (!m_activeFrame || column >= kRsem4ReserveColumns || ring >= kRsem4DefaultRings)
        return;

    const size_t pointIndex =
        static_cast<size_t>(column) * static_cast<size_t>(kRsem4DefaultRings) +
        static_cast<size_t>(kRsem4DefaultRings - 1 - ring);
    if (pointIndex >= m_activeFrame->pointPool.size())
        return;

    auto& dst = m_activeFrame->pointPool[pointIndex];

    if (surfaceIndex >= kRsem4SurfaceCount || distance < m_rsem4MinDistance || distance > m_rsem4MaxDistance)
    {
        SetZeroPoint(dst);
        m_activeFrameAllValid = false;
        return;
    }

    const float sinPitch = m_rsem4PitchSin[surfaceIndex][ring];
    const float cosPitch = m_rsem4PitchCos[surfaceIndex][ring];

    const float x = distance * cosPitch * cosYaw;
    const float y = distance * cosPitch * sinYaw;
    const float z = distance * sinPitch;

    dst.fData[0] = -y * 1000.0f;
    dst.fData[1] = -z * 1000.0f;
    dst.fData[2] =  x * 1000.0f;
    dst.fData_c[0] = static_cast<float>(intensity);
}

void CRsLidarDevice::processRsem4Difop(const uint8_t* data, size_t size)
{
    if (!data)
        return;

    auto applyAngles = [this](const int8_t* yaw, const int16_t* pitch, const int16_t* surface) {
        for (size_t i = 0; i < kRsem4VecselsPerColumn; ++i)
            m_rsem4YawOffset[i] = yaw[i];
        for (size_t i = 0; i < kRsem4DefaultRings; ++i)
            m_rsem4PitchAngle[i] = SwapI16(pitch[i]);
        for (size_t i = 0; i < kRsem4SurfaceCount; ++i)
            m_rsem4SurfacePitchOffset[i] = SwapI16(surface[i]);
        m_rsem4AnglesReady = true;
        updateRsem4PitchTrig();
    };

    if (size == sizeof(Rsem4DifopPkt))
    {
        const auto* pkt = reinterpret_cast<const Rsem4DifopPkt*>(data);
        applyAngles(pkt->yawOffset, pkt->pitchAngle, pkt->surfacePitchOffset);
    }
    else if (size == sizeof(Rsem4Difop2Pkt))
    {
        const auto* pkt = reinterpret_cast<const Rsem4Difop2Pkt*>(data);
        applyAngles(pkt->yawOffset, pkt->pitchAngle, pkt->surfacePitchOffset);
    }
    else if (size == sizeof(Rsem4Difop0624Pkt))
    {
        const auto* pkt = reinterpret_cast<const Rsem4Difop0624Pkt*>(data);
        applyAngles(pkt->yawOffset, pkt->pitchAngle, pkt->surfacePitchOffset);
    }
}

bool CRsLidarDevice::processRsem4Packet(const Packet& pkt)
{
    const uint8_t* data = pkt.buf_.empty() ? nullptr : pkt.buf_.data();
    const size_t size = pkt.buf_.size();
    if (!data || size < 4)
        return false;

    if (pkt.is_difop)
    {
        processRsem4Difop(data, size);
        return false;
    }

    return processRsem4Msop(data, size);
}

bool CRsLidarDevice::processRsem4Msop(const uint8_t* data, size_t size)
{
    if (IsCompleteRsem4Packet(data, size))
        return processRsem4CompleteMsop(data, size);

    if (size < sizeof(Rsem4MsopHeader2))
        return false;

    const auto& header2 = *reinterpret_cast<const Rsem4MsopHeader2*>(data);
    const uint16_t seq = ReadU16BE(header2.pktSeq);
    if (!m_rsem4HasPartialPacket || seq != m_rsem4PartialSeq)
    {
        m_rsem4HasPartialPacket = false;
        m_rsem4PartialLen = 0;
        if (m_activeFrame)
            m_activeFramePacketLoss = true;
        return false;
    }

    const size_t payloadOffset = sizeof(Rsem4MsopHeader2);
    const size_t payloadLen = size - payloadOffset;
    if (m_rsem4PartialLen + payloadLen > m_rsem4PartialPacket.size())
    {
        m_rsem4HasPartialPacket = false;
        m_rsem4PartialLen = 0;
        if (m_activeFrame)
            m_activeFramePacketLoss = true;
        return false;
    }

    std::memcpy(m_rsem4PartialPacket.data() + m_rsem4PartialLen, data + payloadOffset, payloadLen);
    m_rsem4PartialLen += payloadLen;
    m_rsem4HasPartialPacket = false;
    return processRsem4CompressedMsop(m_rsem4PartialPacket.data(), m_rsem4PartialLen);
}

bool CRsLidarDevice::processRsem4CompleteMsop(const uint8_t* data, size_t size)
{
    if (size < sizeof(Rsem4MsopHeader))
        return false;

    const auto& header = *reinterpret_cast<const Rsem4MsopHeader*>(data);
    const uint8_t packMode = header.packMode & 0x03;
    if (packMode == 0x03)
    {
        const uint16_t compressedSeq = ReadU16BE(header.pktSeq);

        const uint8_t splitPackNum = header.packMode >> 4;
        if (splitPackNum == 0)
            return processRsem4CompressedMsop(data, size);

        if (size > m_rsem4PartialPacket.size())
        {
            if (m_activeFrame)
                m_activeFramePacketLoss = true;
            return false;
        }
        std::memcpy(m_rsem4PartialPacket.data(), data, size);
        m_rsem4PartialLen = size;
        m_rsem4PartialSeq = compressedSeq;
        m_rsem4HasPartialPacket = true;
        return false;
    }

    if (size < sizeof(Rsem4MsopPkt))
        return false;

    const auto& pkt = *reinterpret_cast<const Rsem4MsopPkt*>(data);
    const uint16_t pktSeqRaw = ReadU16BE(pkt.header.pktSeq);
    const uint16_t pktSeq = (pktSeqRaw > 0) ? static_cast<uint16_t>(pktSeqRaw - 1) : 0;
    if (!m_rsem4FrameSynced)
    {
        m_rsem4FrameSynced = true;
        m_hasRsem4PrevSeq = false;
        m_hasRsem4PrevColumn = false;
    }

    m_prevRsem4Seq = pktSeq;
    m_hasRsem4PrevSeq = true;

    const uint16_t columnRaw = ReadU16BE(pkt.header.columnNum);
    const uint32_t column = (columnRaw < kRsem4ReserveColumns)
        ? static_cast<uint32_t>(columnRaw)
        : static_cast<uint32_t>((pktSeq / 2) % kRsem4ReserveColumns);
    if (!prepareRsem4FrameForColumn(column))
        return false;
    m_activeFrameHasData = true;

    const uint8_t surfaceIndex = (pkt.header.surfaceId > 0) ? static_cast<uint8_t>(pkt.header.surfaceId - 1) : 0;
    const int16_t yawBase = SwapI16(pkt.header.yawAngle);

    float yawCos[kRsem4VecselsPerColumn];
    float yawSin[kRsem4VecselsPerColumn];
    for (size_t i = 0; i < kRsem4VecselsPerColumn; ++i)
    {
        const int yaw = static_cast<int>(yawBase) + static_cast<int>(m_rsem4YawOffset[i]);
        yawCos[i] = CosDeg01(yaw);
        yawSin[i] = SinDeg01(yaw);
    }

    const bool dualReturn = (pkt.header.returnMode == 0x00);
    const uint8_t segmentModulo = dualReturn ? 4 : 2;
    const uint8_t segmentMask = static_cast<uint8_t>(1U << (pktSeq % segmentModulo));
    const uint8_t completeMask = dualReturn ? 0x0F : 0x03;
    for (uint32_t blk = 0; blk < kRsem4BlocksPerPacket; ++blk)
    {
        uint32_t ring = 0;
        if (dualReturn)
        {
            if ((blk & 1U) != 0)
                continue;
            ring = (blk / 2) + (pktSeq % 4) * (kRsem4HalfRings / 2);
        }
        else
        {
            ring = blk + (pktSeq % 2) * kRsem4HalfRings;
        }
        if (ring >= kRsem4DefaultRings)
            continue;

        const auto& channel = pkt.blocks[blk].channel[0];
        const float distance = static_cast<float>(ReadU16BE(channel.distance)) * kRsem4DistanceResolution;
        const size_t vecsel = ring / kRsem4PixelsPerVcsel;
        fillRsem4Point(column, ring, distance, channel.intensity, surfaceIndex, yawCos[vecsel], yawSin[vecsel]);
    }
    markRsem4ColumnSegment(column, segmentMask, completeMask);

    return true;
}

bool CRsLidarDevice::processRsem4CompressedMsop(const uint8_t* data, size_t size)
{
    if (size < sizeof(Rsem4MsopHeader) + kRsem4MsopTailLen)
        return false;

    const auto& header = *reinterpret_cast<const Rsem4MsopHeader*>(data);
    const uint16_t pktSeq = ReadU16BE(header.pktSeq);
    if (!m_rsem4FrameSynced)
    {
        m_rsem4FrameSynced = true;
        m_hasRsem4PrevSeq = false;
        m_hasRsem4PrevColumn = false;
    }

    m_prevRsem4Seq = pktSeq;
    m_hasRsem4PrevSeq = true;

    uint16_t decoded[kRsem4DefaultRings * 2] = {};
    const int dataLen = static_cast<int>((size - sizeof(Rsem4MsopHeader) - kRsem4MsopTailLen) / 2);
    CompressAlgo::RLenc_unpack_optimize(
        reinterpret_cast<const uint16_t*>(data + sizeof(Rsem4MsopHeader)),
        decoded,
        dataLen,
        16);

    const uint16_t* radius = decoded;
    const uint16_t* identity = decoded + kRsem4DefaultRings;

    const uint16_t columnRaw = ReadU16BE(header.columnNum);
    const uint32_t column = (columnRaw < kRsem4ReserveColumns)
        ? static_cast<uint32_t>(columnRaw)
        : static_cast<uint32_t>(pktSeq % kRsem4ReserveColumns);
    if (!prepareRsem4FrameForColumn(column))
        return false;
    m_activeFrameHasData = true;

    const uint8_t surfaceIndex = (header.surfaceId > 0) ? static_cast<uint8_t>(header.surfaceId - 1) : 0;
    const int16_t yawBase = SwapI16(header.yawAngle);

    float yawCos[kRsem4VecselsPerColumn];
    float yawSin[kRsem4VecselsPerColumn];
    for (size_t i = 0; i < kRsem4VecselsPerColumn; ++i)
    {
        const int yaw = static_cast<int>(yawBase) + static_cast<int>(m_rsem4YawOffset[i]);
        yawCos[i] = CosDeg01(yaw);
        yawSin[i] = SinDeg01(yaw);
    }

    for (uint32_t ring = 0; ring < kRsem4DefaultRings; ++ring)
    {
        const float distance = static_cast<float>(radius[ring]) * kRsem4DistanceResolution;
        const uint8_t intensity = static_cast<uint8_t>(identity[ring] & 0xFF);
        const size_t vecsel = ring / kRsem4PixelsPerVcsel;
        fillRsem4Point(column, ring, distance, intensity, surfaceIndex, yawCos[vecsel], yawSin[vecsel]);
    }
    markRsem4ColumnSegment(column, 0x01, 0x01);

    return true;
}

void CRsLidarDevice::onPacket(const Packet& pkt)
{
    if ((++m_diagPacketTick & 0x3FFU) == 0)
        emitDiagnosticLogIfDue(m_diagLastLogTime);
    if (m_param.lidar_type == LidarType::RSEM4)
        processRsem4Packet(pkt);

    if (!m_hasPacketCallback.load(std::memory_order_acquire))
        return;

    PacketCallback cb;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        cb = m_packetCallback;
    }

    if (cb)
    {
        RsPacketInfo info;
        info.timestamp      = pkt.timestamp;
        info.seq            = pkt.seq;
        info.is_difop       = pkt.is_difop;
        info.is_frame_begin = pkt.is_frame_begin;
        info.frameId        = pkt.frame_id;
        info.dataSize       = static_cast<uint32_t>(pkt.buf_.size());
        cb(info);
    }
}

// ============================================================
// 监控接口
// ============================================================
uint64_t CRsLidarDevice::GetDroppedFrameCount() const
{
    return m_droppedFrameCount.load();
}

size_t CRsLidarDevice::GetStuffedQueueDepth() const
{
    return m_deliveryQueue.size();
}

uint64_t CRsLidarDevice::GetExceptionCount(int errCode) const
{
    size_t bucket = errCodeToBucket(errCode);
    return m_exceptionCounts[bucket].load(std::memory_order_relaxed);
}

RsCallbackStats CRsLidarDevice::GetCallbackLatencyStats() const
{
    RsCallbackStats stats;
    stats.count = m_cbCount.load(std::memory_order_relaxed);
    stats.maxUs = m_cbMaxUs.load(std::memory_order_relaxed);
    uint64_t acc = m_cbAccumUs.load(std::memory_order_relaxed);
    stats.avgUs = (stats.count > 0) ? (acc / stats.count) : 0;
    return stats;
}

size_t CRsLidarDevice::GetStuffedQueuePeak() const
{
    return m_deliveryQueuePeak.load(std::memory_order_relaxed);
}

// ============================================================
// 内部：Error → RsExceptionInfo（按 ErrCode 分桶计数后转发）
// ============================================================
void CRsLidarDevice::onException(const Error& code)
{
    // 分桶累加（用于诊断日志和 GetExceptionCount 查询）
    size_t bucket = errCodeToBucket(static_cast<int>(code.error_code));
    m_exceptionCounts[bucket].fetch_add(1, std::memory_order_relaxed);

    ExceptionCallback cb;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        cb = m_exceptionCallback;
    }

    if (cb)
    {
        RsExceptionInfo info;
        info.code    = static_cast<int>(code.error_code);
        info.message = code.toString();
        cb(info);
    }
}