GrabBag/Device/RsLidarDevice/_Inc/RsLidarDevice.h

#ifndef CRSLIDARDEVICE_H
#define CRSLIDARDEVICE_H

#include "IRsLidarDevice.h"

// SDK 内部以 uint8_t* 调用 recvfrom()，但 C++ 中 char*/unsigned char*/signed char*
// 是三种独立类型，MSVC 拒绝 uint8_t*(=unsigned char*) → char* 的隐式转换。
// 此重载在 SDK 头文件之前定义，利用重载决议的精确匹配规则拦截调用并内部转型。
#ifdef _WIN32
#include <winsock2.h>
#pragma comment(lib, "ws2_32.lib")
inline int recvfrom(SOCKET s, unsigned char* buf, int len, int flags, sockaddr* from, int* fromlen)
{
    return ::recvfrom(s, reinterpret_cast<char*>(buf), len, flags, from, fromlen);
}
#endif

// SDK 依赖 std::array / int16_t / uint16_t，但其内部缺少对应的 #include，
// 在 Linux 上通过 libstdc++ 间接包含可用，MSVC STL 不会间接拉入，需在此处显式引入。
#include <array>
#include <cstdint>

// SDK 头文件全部放在私有层，不污染公共接口
#include <rs_driver/api/lidar_driver.hpp>
#include <rs_driver/msg/point_cloud_msg.hpp>
#include <rs_driver/msg/packet.hpp>
#include <rs_driver/driver/driver_param.hpp>
#include <rs_driver/common/error_code.hpp>
#include <rs_driver/utility/sync_queue.hpp>

#include <thread>
#include <atomic>
#include <mutex>
#include <array>
#include <chrono>
#include <cstddef>

using namespace robosense::lidar;

struct RsSdkDiscardPoint
{
};

// 交付帧：封装一次完整扫描的数据 + 连续点池（p3DPoint 指向此处）
// SDK packet callback 填充 → deliveryThread 消费（调用回调）→ 归还 m_frameFreeQueue
struct DeliveryFrame
{
    std::vector<SVzNLPointXYZI> pointPool;
    RsCloudData cloudData;
    RsFrameInfo  info;

    void reserveLines(size_t lines, size_t pointsPerLine)
    {
        pointPool.resize(lines * pointsPerLine);
        cloudData.reserve(lines);
    }
};
using DeliveryFramePtr = std::shared_ptr<DeliveryFrame>;

class CRsLidarDevice : public IRsLidarDevice
{
public:
    CRsLidarDevice();
    ~CRsLidarDevice();

    static void* operator new(std::size_t size);
    static void operator delete(void* ptr) noexcept;
    static void operator delete(void* ptr, std::size_t) noexcept;

    int InitDevice() override;
    int OpenDevice(const RsLidarConfig& config) override;
    int CloseDevice() override;
    bool IsOpened() const override;

    int Start() override;
    int Stop() override;
    bool IsRunning() const override;

    int SetPointCloudCallback(PointCloudCallback callback) override;
    int SetPacketCallback(PacketCallback callback) override;
    int SetExceptionCallback(ExceptionCallback callback) override;

    std::string GetVersion() override;

    uint64_t GetDroppedFrameCount() const override;
    size_t GetStuffedQueueDepth() const override;
    uint64_t GetExceptionCount(int errCode) const override;
    RsCallbackStats GetCallbackLatencyStats() const override;
    size_t GetStuffedQueuePeak() const override;

private:
    // SDK 内部类型别名
    using SdkCloudMsg = PointCloudT<RsSdkDiscardPoint>;
    using SdkCloudPtr = std::shared_ptr<SdkCloudMsg>;

    /// 交付线程：从交付队列取帧 → 调用用户回调（PushFrame memcpy在此发生）→ 归还交付帧
    void deliveryThread();

    /// 输出 5s 窗口诊断日志并重置窗口字段；仅当距 lastLogTime ≥5s 时实际输出
    void emitDiagnosticLogIfDue(std::chrono::steady_clock::time_point& lastLogTime);

    SdkCloudPtr getFreeCloud();
    void putStuffedCloud(SdkCloudPtr msg);

    /// 将 SDK Packet 转为 RsPacketInfo 后转发给用户
    void onPacket(const Packet& pkt);

    void resetPacketFrameBuilder();
    void initializeRsem4DefaultAngles();
    void updateRsem4PitchTrig();
    bool processRsem4Packet(const Packet& pkt);
    void processRsem4Difop(const uint8_t* data, size_t size);
    bool processRsem4Msop(const uint8_t* data, size_t size);
    bool processRsem4CompleteMsop(const uint8_t* data, size_t size);
    bool processRsem4CompressedMsop(const uint8_t* data, size_t size);
    DeliveryFramePtr acquireFrameForBuild();
    void resetDeliveryFrame(const DeliveryFramePtr& frame);
    void submitActiveFrame();
    void resetActiveFrameTracking();
    bool beginActiveFrame(bool startedAtBoundary);
    bool prepareRsem4FrameForColumn(uint32_t column);
    void markRsem4ColumnSegment(uint32_t column, uint8_t segmentMask, uint8_t completeMask);
    void fillRsem4Point(uint32_t column, uint32_t ring, float distance,
                        uint8_t intensity, uint8_t surfaceIndex,
                        float cosYaw, float sinYaw);

    /// 将 SDK Error 转为 RsExceptionInfo 后转发给用户
    void onException(const Error& code);

    /// RsLidarConfig → RSDriverParam
    static RSDriverParam toDriverParam(const RsLidarConfig& config);

    bool                        m_bOpened = false;
    std::atomic<bool>           m_bRunning{false};
    std::atomic<bool>           m_bStopProcess{false};
    std::atomic<bool>           m_bStopDelivery{false};

    PointCloudCallback          m_pointCloudCallback;
    PacketCallback              m_packetCallback;
    ExceptionCallback           m_exceptionCallback;
    std::atomic<bool>           m_hasPacketCallback{false};
    std::mutex                  m_callbackMutex;

    std::unique_ptr<LidarDriver<SdkCloudMsg>> m_pDriver;
    std::thread                 m_deliveryThread;

    SyncQueue<SdkCloudPtr, 4096>  m_freeQueue;

    // 交付帧流水线：SDK packet callback 填充 → deliveryThread 消费
    SyncQueue<DeliveryFramePtr, 16>  m_deliveryQueue;
    SyncQueue<DeliveryFramePtr, 16>  m_frameFreeQueue;

    RSDriverParam               m_param;

    DeliveryFramePtr            m_activeFrame;
    bool                        m_activeFrameHasData = false;
    bool                        m_activeFrameAllValid = true;
    bool                        m_activeFrameStartedAtBoundary = false;
    bool                        m_activeFramePacketLoss = false;
    uint32_t                    m_activeCompleteColumns = 0;
    uint8_t                     m_activeColumnCompleteMask = 0;
    std::array<uint8_t, 1200>   m_activeColumnMask{};
    bool                        m_hasRsem4PrevColumn = false;
    uint32_t                    m_prevRsem4Column = 0;
    bool                        m_rsem4FrameSynced = false;
    bool                        m_hasRsem4PrevSeq = false;
    uint16_t                    m_prevRsem4Seq = 0;
    std::chrono::steady_clock::time_point m_activeFrameStartTime{};
    std::chrono::steady_clock::time_point m_diagLastLogTime{};

    std::array<int8_t, 26>      m_rsem4YawOffset{};
    std::array<int16_t, 520>    m_rsem4PitchAngle{};
    std::array<std::array<float, 520>, 4> m_rsem4PitchSin{};
    std::array<std::array<float, 520>, 4> m_rsem4PitchCos{};
    std::array<int16_t, 4>      m_rsem4SurfacePitchOffset{};
    bool                        m_rsem4AnglesReady = false;
    bool                        m_rsem4HasPartialPacket = false;
    uint16_t                    m_rsem4PartialSeq = 0;
    size_t                      m_rsem4PartialLen = 0;
    std::array<uint8_t, 3000>   m_rsem4PartialPacket{};
    uint32_t                    m_diagPacketTick = 0;
    float                       m_rsem4MinDistance = 0.5f;
    float                       m_rsem4MaxDistance = 350.0f;

    // 丢帧监控
    std::atomic<uint64_t>       m_droppedFrameCount{0};
    std::atomic<uint64_t>       m_totalPushCount{0};
    std::atomic<uint64_t>       m_parseAccumUs{0};
    std::atomic<uint64_t>       m_parseMaxUs{0};
    std::atomic<uint64_t>       m_parseCount{0};
    std::atomic<uint64_t>       m_frameWaitAccumUs{0};
    std::atomic<uint64_t>       m_frameWaitMaxUs{0};
    std::atomic<uint64_t>       m_frameWaitCount{0};
    std::atomic<uint64_t>       m_frameWaitTimeoutCount{0};
    std::atomic<uint64_t>       m_deliveryDropCount{0};
    std::atomic<uint64_t>       m_deliveredFrameCount{0};
    std::atomic<size_t>         m_deliveryQueuePeak{0};

    // 诊断统计（每 5s 滚动窗口）
    // bucket: 0=MSOPTIMEOUT(0x40) 1=PKTBUFOVERFLOW(0x48) 2=CLOUDOVERFLOW(0x49) 3=WRONGMSOPLEN(0x42) 4=other
    static constexpr size_t     EXC_BUCKET_COUNT = 5;
    std::array<std::atomic<uint64_t>, EXC_BUCKET_COUNT> m_exceptionCounts{};
    std::atomic<uint64_t>       m_cbMaxUs{0};
    std::atomic<uint64_t>       m_cbAccumUs{0};
    std::atomic<uint64_t>       m_cbCount{0};

    uint64_t                    m_diagLastPushCount = 0;
    uint64_t                    m_diagLastQueueDropCount = 0;
    uint64_t                    m_diagLastParseCount = 0;
    uint64_t                    m_diagLastParseAccumUs = 0;
    uint64_t                    m_diagLastFrameWaitCount = 0;
    uint64_t                    m_diagLastFrameWaitAccumUs = 0;
    uint64_t                    m_diagLastFrameWaitTimeoutCount = 0;
    uint64_t                    m_diagLastDeliveryDropCount = 0;
    uint64_t                    m_diagLastDeliveredFrameCount = 0;
    uint64_t                    m_diagLastCbCount = 0;
    uint64_t                    m_diagLastCbAccumUs = 0;
    std::array<uint64_t, EXC_BUCKET_COUNT> m_diagLastExceptionCounts{};

    /// 将 SDK ErrCode 映射到统计桶下标，未知返回 4 (other)
    static size_t errCodeToBucket(int errCode);
};

#endif // CRSLIDARDEVICE_H