GrabBag/AppUtils/AppCommon/Src/DebugDataSaver.cpp

#include "DebugDataSaver.h"
#include "LaserDataLoader.h"
#include "VrLog.h"
#include "VrError.h"
#include <QDir>
#include <QString>
#include <cstring>

DebugDataSaver::DebugDataSaver(int maxThreads)
    : m_maxThreads(std::max(1, std::min(maxThreads, 4)))
    , m_running(true)
{
    for (int i = 0; i < m_maxThreads; ++i) {
        m_workers.emplace_back(&DebugDataSaver::WorkerThread, this);
    }
    LOG_INFO("[DebugDataSaver] 已启动 %d 个存储线程\n", m_maxThreads);
}

DebugDataSaver::~DebugDataSaver()
{
    Stop();
}

void DebugDataSaver::Stop()
{
    if (!m_running.exchange(false)) {
        return;
    }

    m_queueCondition.notify_all();

    for (auto& worker : m_workers) {
        if (worker.joinable()) {
            worker.join();
        }
    }
    m_workers.clear();

    // 释放队列中未处理的任务
    while (!m_taskQueue.empty()) {
        FreeData(m_taskQueue.front().data);
        m_taskQueue.pop();
    }

    LOG_INFO("[DebugDataSaver] 已停止\n");
}

void DebugDataSaver::SaveAsync(const std::string& basePath, const std::string& timestamp, const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& data, const std::string& deviceName)
{
    if (data.empty() || !m_running) return;

    SaveTask task;
    task.deviceName = deviceName;
    task.filePath = GenerateSavePath(basePath, timestamp, deviceName);
    task.data = DeepCopyData(data);

    {
        std::lock_guard<std::mutex> lock(m_queueMutex);
        m_taskQueue.push(std::move(task));
    }
    m_queueCondition.notify_one();
}

void DebugDataSaver::WorkerThread()
{
    LaserDataLoader dataLoader;

    while (true) {
        SaveTask task;
        {
            std::unique_lock<std::mutex> lock(m_queueMutex);
            m_queueCondition.wait(lock, [this] {
                return !m_taskQueue.empty() || !m_running;
            });

            if (!m_running && m_taskQueue.empty()) {
                break;
            }
            if (m_taskQueue.empty()) {
                continue;
            }

            task = std::move(m_taskQueue.front());
            m_taskQueue.pop();
        }

        int lineNum = static_cast<int>(task.data.size());
        float scanSpeed = 0.0f;
        int maxTimeStamp = 0;
        int clockPerSecond = 0;

        int result = dataLoader.SaveLaserScanData(task.filePath, task.data, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);

        if (result == SUCCESS) {
            LOG_INFO("[DebugDataSaver] 保存成功: %s (%d行)\n", task.filePath.c_str(), lineNum);
        } else {
            LOG_ERROR("[DebugDataSaver] 保存失败: %s, 错误: %s\n", task.filePath.c_str(), dataLoader.GetLastError().c_str());
        }

        FreeData(task.data);
    }
}

std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> DebugDataSaver::DeepCopyData(
    const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& src)
{
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> dst;
    dst.reserve(src.size());

    for (const auto& item : src) {
        EVzResultDataType dataType = item.first;
        const SVzLaserLineData& srcLine = item.second;

        SVzLaserLineData dstLine;
        memset(&dstLine, 0, sizeof(SVzLaserLineData));

        if (srcLine.nPointCount > 0) {
            if (dataType == keResultDataType_Position) {
                dstLine.p3DPoint = new SVzNL3DPosition[srcLine.nPointCount];
                if (srcLine.p3DPoint) {
                    memcpy(dstLine.p3DPoint, srcLine.p3DPoint, sizeof(SVzNL3DPosition) * srcLine.nPointCount);
                }
                dstLine.p2DPoint = new SVzNL2DPosition[srcLine.nPointCount];
                if (srcLine.p2DPoint) {
                    memcpy(dstLine.p2DPoint, srcLine.p2DPoint, sizeof(SVzNL2DPosition) * srcLine.nPointCount);
                }
            } else if (dataType == keResultDataType_PointXYZRGBA) {
                dstLine.p3DPoint = new SVzNLPointXYZRGBA[srcLine.nPointCount];
                if (srcLine.p3DPoint) {
                    memcpy(dstLine.p3DPoint, srcLine.p3DPoint, sizeof(SVzNLPointXYZRGBA) * srcLine.nPointCount);
                }
                dstLine.p2DPoint = new SVzNL2DLRPoint[srcLine.nPointCount];
                if (srcLine.p2DPoint) {
                    memcpy(dstLine.p2DPoint, srcLine.p2DPoint, sizeof(SVzNL2DLRPoint) * srcLine.nPointCount);
                }
            }
        }

        dstLine.nPointCount = srcLine.nPointCount;
        dstLine.llTimeStamp = srcLine.llTimeStamp;
        dstLine.llFrameIdx = srcLine.llFrameIdx;
        dstLine.nEncodeNo = srcLine.nEncodeNo;
        dstLine.fSwingAngle = srcLine.fSwingAngle;
        dstLine.bEndOnceScan = srcLine.bEndOnceScan;

        dst.push_back(std::make_pair(dataType, dstLine));
    }

    return dst;
}

void DebugDataSaver::FreeData(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& data)
{
    LaserDataLoader loader;
    loader.FreeLaserScanData(data);
    data.clear();
}

std::string DebugDataSaver::GenerateSavePath(const std::string& basePath, const std::string& timestamp, const std::string& deviceName)
{
    // 从时间戳提取日期部分作为子文件夹名（前8位：YYYYMMDD）
    std::string dateFolder = timestamp.substr(0, 8);

    QString fullPath = QString::fromStdString(basePath) + "/" + QString::fromStdString(dateFolder);

    QDir dir(fullPath);
    if (!dir.exists()) {
        dir.mkpath(".");
    }

    int taskId = m_taskCounter.fetch_add(1);
    QString fileName;
    if (!deviceName.empty()) {
        fileName = QString("%1/%2_%3_%4.txt").arg(fullPath)
                        .arg(taskId, 4, 10, QChar('0'))
                       .arg(QString::fromStdString(deviceName))
                       .arg(QString::fromStdString(timestamp));
    } else {
        fileName = QString("%1/pointcloud_%2_%3.txt").arg(fullPath)
                       .arg(QString::fromStdString(timestamp))
                       .arg(taskId, 4, 10, QChar('0'));
    }

    return fileName.toStdString();
}