GrabBag/Device/RsLidarDevice/Src/RsLidarDevice.cpp

#include "RsLidarDevice.h"
#include <cmath>
#include <cstdio>
#include <chrono>

// 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>>())
{
}

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

// ============================================================
// 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);

    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_stuffedQueue.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_cbMaxUs.store(0, std::memory_order_relaxed);
    m_cbAccumUs.store(0, std::memory_order_relaxed);
    m_cbCount.store(0, std::memory_order_relaxed);
    m_qPeak.store(0, std::memory_order_relaxed);

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

    for (int i = 0; i < 32; ++i)
    {
        auto msg = std::make_shared<SdkCloudMsg>();
        msg->points.reserve(500000);   // 预分配 SDK 内部缓冲，避免 SDK push_back 反复 realloc
        m_freeQueue.push(std::move(msg));
    }

    m_bStopProcess = false;

    // 预分配交付帧：3 帧 × 600 线 × 1500 点 × 32B ≈ 86MB
    // 帧 0=解析线程填充中  帧 1=交付队列中  帧 2=空闲备用
    m_deliveryQueue.clear();
    m_frameFreeQueue.clear();
    for (int i = 0; i < 3; ++i)
    {
        auto frame = std::make_shared<DeliveryFrame>();
        frame->reserveLines(600, 1500);
        m_frameFreeQueue.push(frame);
    }

    m_pDriver->start();

    m_bRunning = true;
    m_bStopDelivery = false;
    m_deliveryThread = std::thread(&CRsLidarDevice::deliveryThread, this);
    m_processThread  = std::thread(&CRsLidarDevice::processCloudThread, this);

    return 0;
}

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

    // ① 停止解析线程
    m_bStopProcess = true;

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

    if (m_processThread.joinable())
    {
        m_processThread.join();
    }

    // ② 停止交付线程（推入空帧唤醒 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)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_packetCallback = std::move(callback);
    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 时实际输出）
// 在 processCloudThread 内部调用，因此对窗口字段的 exchange 是单写者操作
// ============================================================
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;
    }

    uint64_t cbCnt = m_cbCount.exchange(0);
    uint64_t cbMax = m_cbMaxUs.exchange(0);
    uint64_t cbAcc = m_cbAccumUs.exchange(0);
    uint64_t cbAvg = (cbCnt > 0) ? (cbAcc / cbCnt) : 0;
    uint64_t e0 = m_exceptionCounts[0].exchange(0);
    uint64_t e1 = m_exceptionCounts[1].exchange(0);
    uint64_t e2 = m_exceptionCounts[2].exchange(0);
    uint64_t e3 = m_exceptionCounts[3].exchange(0);
    uint64_t e4 = m_exceptionCounts[4].exchange(0);
    size_t   qPeakSnap = m_qPeak.load();
#if 0
    fprintf(stderr,
        "[RsLidarDevice] qd=%zu qpeak=%zu drop=%llu push=%llu | "
        "cb_us max=%llu avg=%llu n=%llu | "
        "exc msopto=%llu pktof=%llu cldof=%llu wlen=%llu other=%llu\n",
        static_cast<size_t>(m_stuffedQueue.size()),
        qPeakSnap,
        static_cast<unsigned long long>(m_droppedFrameCount.load()),
        static_cast<unsigned long long>(m_totalPushCount.load()),
        static_cast<unsigned long long>(cbMax),
        static_cast<unsigned long long>(cbAvg),
        static_cast<unsigned long long>(cbCnt),
        static_cast<unsigned long long>(e0),
        static_cast<unsigned long long>(e1),
        static_cast<unsigned long long>(e2),
        static_cast<unsigned long long>(e3),
        static_cast<unsigned long long>(e4));
#endif
    lastLogTime = now;
}

// ============================================================
// 内部：点云解析线程（只做数据处理，不调回调）
// 处理完一帧 → 推入 m_deliveryQueue → 立刻取下一帧解析
// 回调（PushFrame memcpy）在 deliveryThread 中并行执行
// ============================================================
void CRsLidarDevice::processCloudThread()
{
    auto lastLogTime = std::chrono::steady_clock::now();
    while (!m_bStopProcess)
    {
        SdkCloudPtr sdkCloud = m_stuffedQueue.popWait(500000);
        if (!sdkCloud || sdkCloud->points.empty())
        {
            emitDiagnosticLogIfDue(lastLogTime);
            continue;
        }

        emitDiagnosticLogIfDue(lastLogTime);

        // 获取空闲交付帧（超时 100ms 允许检查 m_bStopProcess）
        auto frame = m_frameFreeQueue.popWait(100000);
        if (!frame)
        {
            if (m_bStopProcess) break;
            continue;
        }

        // ① NaN→0 + ② 坐标系修正(X=-SDK_Y,Y=-SDK_Z,Z=SDK_X) + ③ 米→毫米(×1000) + ④ 拆线
        const auto& src = sdkCloud->points;
        const uint32_t N = static_cast<uint32_t>(src.size());
        const uint32_t H = sdkCloud->height > 0 ? sdkCloud->height : 1;
        const uint32_t W = (H > 1 && sdkCloud->width > 0) ? sdkCloud->width : N;

        if (frame->linePool.size() < H)
            frame->linePool.resize(H);

        auto& cloudData = frame->cloudData;
        cloudData.clear();
        cloudData.reserve(H);
        bool allValid = sdkCloud->is_dense;

        for (uint32_t line = 0; line < H; ++line)
        {
            uint32_t start = line * W;
            uint32_t end   = (line + 1 == H) ? N : (start + W);
            if (end <= start) continue;

            uint32_t cnt = end - start;
            auto& linePts = frame->linePool[line];
            linePts.resize(cnt);

            for (uint32_t j = 0; j < cnt; ++j)
            {
                uint32_t i = start + j;
                float sx = std::isnan(src[i].x) ? 0.0f : src[i].x;
                float sy = std::isnan(src[i].y) ? 0.0f : src[i].y;
                float sz = std::isnan(src[i].z) ? 0.0f : src[i].z;
                linePts[j].fData[0]   = -sy * 1000.0f;
                linePts[j].fData[1]   = -sz * 1000.0f;
                linePts[j].fData[2]   =  sx * 1000.0f;
                linePts[j].fData_c[0] = static_cast<float>(src[i].intensity);
                if (std::isnan(src[i].x) || std::isnan(src[i].y) || std::isnan(src[i].z))
                    allValid = false;
            }

            SVzLaserLineData ld{};
            ld.p3DPoint     = linePts.data();
            ld.nPointCount  = static_cast<int>(cnt);
            ld.llFrameIdx   = sdkCloud->seq;
            ld.llTimeStamp  = static_cast<unsigned long long>(sdkCloud->timestamp * 1e6);
            ld.nEncodeNo    = static_cast<int>(line);
            ld.bEndOnceScan = (line + 1 == H) ? VzTrue : VzFalse;

            cloudData.emplace_back(keResultDataType_PointXYZI, ld);
        }

        frame->info.height  = H;
        frame->info.width   = W;
        frame->info.isDense = allValid;

        // 送入交付队列（可能短暂阻塞等 deliveryThread 消费，提供反压）
        m_deliveryQueue.push(frame);

        // 归还 SDK 缓冲到 freeQueue（复用其预留 capacity）
        m_freeQueue.push(sdkCloud);
    }
}

// ============================================================
// 内部：交付线程（只调回调，不做数据处理）
// 从 m_deliveryQueue 取帧 → 调 PointCloudCallback（PushFrame memcpy 在此发生）
// 与 processCloudThread 并行：交付帧 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 (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 元数据，保留 linePool capacity）
        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)
{
    m_totalPushCount++;
    size_t depth = m_stuffedQueue.push(msg);
    if (depth == 0)
    {
        m_droppedFrameCount++;
        // 丢帧时把 SdkCloudMsg 回收到 freeQueue，复用其 points vector 容量
        // (单帧最多 reserve MAX_POINT_CLOUD_SIZE × 13B ≈ 22MB，避免反复分配)
        m_freeQueue.push(msg);
    }
    else
    {
        // CAS 采样队列峰值（Open 以来）
        size_t prevPeak = m_qPeak.load(std::memory_order_relaxed);
        while (depth > prevPeak &&
               !m_qPeak.compare_exchange_weak(prevPeak, depth, std::memory_order_relaxed))
        {
            // prevPeak 已被自动更新，循环重试
        }
    }
}

// ============================================================
// 内部：Packet → RsPacketInfo
// ============================================================
void CRsLidarDevice::onPacket(const Packet& pkt)
{
    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_stuffedQueue.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_qPeak.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);
    }
}