GrabBag/AppUtils/AppCommon/Src/DebugDataSaver.cpp

#include "DebugDataSaver.h"
#include "LaserDataLoader.h"
#include "VrError.h"
#include "VrLog.h"

#include <QCoreApplication>
#include <QDateTime>
#include <QDir>
#include <QDirIterator>
#include <QFile>
#include <QFileInfo>
#include <QString>

#include <algorithm>
#include <cstring>
#include <utility>

namespace {

QString AppName()
{
    QString name = QFileInfo(QCoreApplication::applicationFilePath()).baseName();
    if (name.isEmpty()) {
        name = QStringLiteral("app");
    }
    return name;
}

QString SubDirPrefix()
{
    return AppName() + QStringLiteral("_PointCloud_");
}

std::string NormalizeBasePath(const std::string& basePath)
{
    return QDir(QString::fromStdString(basePath)).absolutePath().toStdString();
}

std::string NormalizeFilePath(const std::string& filePath)
{
    return QFileInfo(QString::fromStdString(filePath)).absoluteFilePath().toStdString();
}

} // namespace

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] started %d storage threads\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();

    std::lock_guard<std::mutex> lock(m_queueMutex);
    while (!m_taskQueue.empty()) {
        FreeData(m_taskQueue.front().data);
        m_taskQueue.pop();
    }

    LOG_INFO("[DebugDataSaver] stopped\n");
}

void DebugDataSaver::SetMaxQueueSize(int maxTasks)
{
    if (maxTasks < 1) {
        maxTasks = 1;
    }
    m_maxQueueSize = maxTasks;
}

void DebugDataSaver::PrepareStorageCache(const std::string& basePath)
{
    std::lock_guard<std::mutex> lock(m_diskSpaceMutex);
    StorageCache& cache = m_storageCaches[NormalizeBasePath(basePath)];
    if (!cache.initialized) {
        BuildStorageCacheLocked(basePath, cache);
        LOG_INFO("[DebugDataSaver] storage cache ready: %s, files=%zu, size=%lld\n",
                 basePath.c_str(), cache.files.size(), static_cast<long long>(cache.totalSize));
    }
    EnsureStorageLimitLocked(basePath, cache);
}

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.load()) {
        return;
    }

    const int maxQueueSize = m_maxQueueSize.load();
    {
        std::lock_guard<std::mutex> lock(m_queueMutex);
        if (static_cast<int>(m_taskQueue.size()) >= maxQueueSize) {
            LOG_WARNING("[DebugDataSaver] save queue full (%zu/%d), skip point cloud save: %s\n",
                        m_taskQueue.size(), maxQueueSize, deviceName.c_str());
            return;
        }
    }

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

    {
        std::lock_guard<std::mutex> lock(m_queueMutex);
        if (static_cast<int>(m_taskQueue.size()) >= maxQueueSize) {
            LOG_WARNING("[DebugDataSaver] save queue full after copy (%zu/%d), skip point cloud save: %s\n",
                        m_taskQueue.size(), maxQueueSize, deviceName.c_str());
            FreeData(task.data);
            return;
        }
        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.load();
            });

            if (!m_running.load() && 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) {
            RecordSavedFile(task.basePath, task.filePath);
            LOG_INFO("[DebugDataSaver] saved: %s (%d lines)\n", task.filePath.c_str(), lineNum);
        } else {
            LOG_ERROR("[DebugDataSaver] save failed: %s, error: %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)
{
    QString subDir = SubDirPrefix() + QString::fromStdString(timestamp.substr(0, 8));
    QString fullPath = QString::fromStdString(basePath) + "/" + subDir;

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

    QString name = deviceName.empty() ? QStringLiteral("pointcloud")
                                      : QString::fromStdString(deviceName);
    QString fileName = QString("%1/%2_%3.txt").arg(fullPath)
                           .arg(name)
                           .arg(QString::fromStdString(timestamp));

    return fileName.toStdString();
}

void DebugDataSaver::EnsureStorageLimit(const std::string& basePath)
{
    std::lock_guard<std::mutex> lock(m_diskSpaceMutex);
    StorageCache& cache = m_storageCaches[NormalizeBasePath(basePath)];
    if (!cache.initialized) {
        BuildStorageCacheLocked(basePath, cache);
    }
    EnsureStorageLimitLocked(basePath, cache);
}

void DebugDataSaver::EnsureStorageLimitLocked(const std::string& basePath, StorageCache& cache)
{
    Q_UNUSED(basePath);

    const int64_t limit = m_maxStorageSize.load();
    if (limit <= 0) {
        return;
    }

    const QString prefix = SubDirPrefix();
    while (cache.totalSize > limit && !cache.files.empty()) {
        StorageFileInfo oldest = cache.files.front();
        cache.files.erase(cache.files.begin());

        QString filePath = QString::fromStdString(oldest.path);
        QFileInfo info(filePath);
        QString parentPath = info.absolutePath();
        int64_t fileSize = oldest.size;

        if (QFileInfo::exists(filePath) && !QFile::remove(filePath)) {
            cache.files.insert(cache.files.begin(), oldest);
            LOG_ERROR("[DebugDataSaver] delete failed: %s\n", oldest.path.c_str());
            break;
        }

        cache.totalSize -= fileSize;
        if (cache.totalSize < 0) {
            cache.totalSize = 0;
        }

        LOG_INFO("[DebugDataSaver] storage limit exceeded, delete oldest file: %s (%lld bytes)\n",
                 oldest.path.c_str(), static_cast<long long>(fileSize));

        QDir parentDir(parentPath);
        const QString parentName = parentDir.dirName();
        if (parentName.startsWith(prefix) && parentDir.isEmpty()) {
            parentDir.cdUp();
            if (parentDir.rmdir(parentName)) {
                LOG_INFO("[DebugDataSaver] removed empty directory: %s\n", parentPath.toStdString().c_str());
            }
        }
    }
}

void DebugDataSaver::BuildStorageCacheLocked(const std::string& basePath, StorageCache& cache)
{
    cache.files.clear();
    cache.totalSize = 0;

    QDir rootDir(QString::fromStdString(basePath));
    if (rootDir.exists()) {
        const QString prefix = SubDirPrefix();
        const QStringList subDirs = rootDir.entryList(QDir::Dirs | QDir::NoDotAndDotDot);
        for (const QString& name : subDirs) {
            if (!name.startsWith(prefix)) {
                continue;
            }
            QString subPath = rootDir.absoluteFilePath(name);
            QDirIterator it(subPath, QStringList() << "*.txt", QDir::Files, QDirIterator::Subdirectories);
            while (it.hasNext()) {
                it.next();
                QFileInfo info = it.fileInfo();
                StorageFileInfo fileInfo;
                fileInfo.path = info.absoluteFilePath().toStdString();
                fileInfo.size = info.size();
                fileInfo.lastModifiedMSecs = info.lastModified().toMSecsSinceEpoch();
                cache.totalSize += fileInfo.size;
                cache.files.push_back(fileInfo);
            }
        }
    }

    std::sort(cache.files.begin(), cache.files.end(), DebugDataSaver::IsOlderFile);
    cache.initialized = true;
}

void DebugDataSaver::AddFileToCacheLocked(StorageCache& cache, const StorageFileInfo& fileInfo)
{
    RemoveFileFromCache(cache, fileInfo.path);
    auto it = std::lower_bound(cache.files.begin(), cache.files.end(), fileInfo, DebugDataSaver::IsOlderFile);
    cache.files.insert(it, fileInfo);
    cache.totalSize += fileInfo.size;
}

void DebugDataSaver::RecordSavedFile(const std::string& basePath, const std::string& filePath)
{
    std::lock_guard<std::mutex> lock(m_diskSpaceMutex);
    StorageCache& cache = m_storageCaches[NormalizeBasePath(basePath)];
    if (!cache.initialized) {
        BuildStorageCacheLocked(basePath, cache);
    }

    std::string normalizedFilePath = NormalizeFilePath(filePath);
    QFileInfo info(QString::fromStdString(normalizedFilePath));
    if (info.exists() && info.isFile()) {
        StorageFileInfo fileInfo;
        fileInfo.path = normalizedFilePath;
        fileInfo.size = info.size();
        fileInfo.lastModifiedMSecs = info.lastModified().toMSecsSinceEpoch();
        AddFileToCacheLocked(cache, fileInfo);
    }

    EnsureStorageLimitLocked(basePath, cache);
}

bool DebugDataSaver::RemoveFileFromCache(StorageCache& cache, const std::string& filePath)
{
    auto it = std::find_if(cache.files.begin(), cache.files.end(), [&filePath](const StorageFileInfo& item) {
        return item.path == filePath;
    });
    if (it == cache.files.end()) {
        return false;
    }

    cache.totalSize -= it->size;
    if (cache.totalSize < 0) {
        cache.totalSize = 0;
    }
    cache.files.erase(it);
    return true;
}

bool DebugDataSaver::IsOlderFile(const StorageFileInfo& lhs, const StorageFileInfo& rhs)
{
    if (lhs.lastModifiedMSecs == rhs.lastModifiedMSecs) {
        return lhs.path < rhs.path;
    }
    return lhs.lastModifiedMSecs < rhs.lastModifiedMSecs;
}

int64_t DebugDataSaver::ComputeDirSize(const std::string& rootPath)
{
    QDir rootDir(QString::fromStdString(rootPath));
    if (!rootDir.exists()) {
        return 0;
    }

    const QString prefix = SubDirPrefix();
    int64_t total = 0;
    const QStringList subDirs = rootDir.entryList(QDir::Dirs | QDir::NoDotAndDotDot);
    for (const QString& name : subDirs) {
        if (!name.startsWith(prefix)) {
            continue;
        }
        QString subPath = rootDir.absoluteFilePath(name);
        QDirIterator it(subPath, QStringList() << "*.txt", QDir::Files, QDirIterator::Subdirectories);
        while (it.hasNext()) {
            it.next();
            total += it.fileInfo().size();
        }
    }
    return total;
}

std::string DebugDataSaver::FindOldestFile(const std::string& rootPath)
{
    QDir rootDir(QString::fromStdString(rootPath));
    if (!rootDir.exists()) {
        return std::string();
    }

    const QString prefix = SubDirPrefix();
    QString oldestPath;
    QDateTime oldestTime;
    bool first = true;

    const QStringList subDirs = rootDir.entryList(QDir::Dirs | QDir::NoDotAndDotDot);
    for (const QString& name : subDirs) {
        if (!name.startsWith(prefix)) {
            continue;
        }
        QString subPath = rootDir.absoluteFilePath(name);
        QDirIterator it(subPath, QStringList() << "*.txt", QDir::Files, QDirIterator::Subdirectories);
        while (it.hasNext()) {
            it.next();
            QFileInfo info = it.fileInfo();
            QDateTime mtime = info.lastModified();
            if (first || mtime < oldestTime) {
                oldestTime = mtime;
                oldestPath = info.absoluteFilePath();
                first = false;
            }
        }
    }

    return oldestPath.toStdString();
}