Utils/CloudUtils/Src/LaserDataLoader.cpp

#include "LaserDataLoader.h"
#include <algorithm>
#include <cctype>
#include <cstdarg>
#include <cstdio>
#include <cstdint>
#include <cstring>
#include <cstdlib>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <memory>
#include <new>
#include <sstream>
#include <stdexcept>
#include <regex>

#include "VrLog.h"

// 辅助函数：去除字符串末尾的 \r 字符（处理 Windows 格式的 CR LF 换行符）
static inline void TrimCarriageReturn(std::string& str)
{
    if (!str.empty() && str.back() == '\r') {
        str.pop_back();
    }
}

namespace
{
struct DatValue2DFloat
{
    double x;
    double y;
    double value;
};

struct Dat3DPoint
{
    double X;
    double Y;
    double Z;
};

struct Dat3DFeaturePoint
{
    int index;
    int flag;
    DatValue2DFloat stereo;
    DatValue2DFloat stereoRight;
    Dat3DPoint eye;
    Dat3DPoint world;
    std::uint64_t next;
};

struct DatLaserScan3DLinesHeader
{
    char dataCalibrated;
    char adjusted;
    int scanLineNum;
    int vr1;
    int vr2;
    double lineV;
    int clock;
    std::uint32_t maxTimeStamp;
    std::uint64_t scanLineList;
};

struct DatLineScan3DData
{
    int lineIndex;
    std::uint32_t timeStamp;
    double lineStepping;
};

static_assert(sizeof(DatLaserScan3DLinesHeader) == 40, "Unexpected dat header layout");
static_assert(sizeof(Dat3DFeaturePoint) == 112, "Unexpected dat feature point layout");

template <typename T>
bool ReadBinary(std::ifstream& inputFile, T& value)
{
    inputFile.read(reinterpret_cast<char*>(&value), sizeof(T));
    return static_cast<bool>(inputFile);
}

bool HasFileExtension(const std::string& fileName, const char* extension)
{
    const size_t dotPos = fileName.find_last_of('.');
    if (dotPos == std::string::npos) {
        return false;
    }

    const size_t slashPos = fileName.find_last_of("/\\");
    if (slashPos != std::string::npos && dotPos < slashPos) {
        return false;
    }

    std::string fileExt = fileName.substr(dotPos);
    std::transform(fileExt.begin(), fileExt.end(), fileExt.begin(),
                   [](unsigned char c) { return static_cast<char>(std::tolower(c)); });
    return fileExt == extension;
}

bool LooksLikeTextLaserFile(const std::string& fileName)
{
    std::ifstream inputFile(fileName, std::ios::binary);
    if (!inputFile.is_open()) {
        return false;
    }

    char header[8] = {};
    inputFile.read(header, sizeof(header));
    return inputFile.gcount() == sizeof(header) && std::memcmp(header, "LineNum:", sizeof(header)) == 0;
}

bool ReadDatLineRecord(std::ifstream& inputFile, int recordSize, DatLineScan3DData& lineData)
{
    char buffer[40] = {};
    if (recordSize <= 0 || recordSize > static_cast<int>(sizeof(buffer))) {
        return false;
    }

    inputFile.read(buffer, recordSize);
    if (!inputFile) {
        return false;
    }

    std::memcpy(&lineData.lineIndex, buffer, sizeof(lineData.lineIndex));
    std::memcpy(&lineData.timeStamp, buffer + 4, sizeof(lineData.timeStamp));
    std::memcpy(&lineData.lineStepping, buffer + 8, sizeof(lineData.lineStepping));
    return true;
}

void AppendFormattedLine(std::string& output, const char* format, ...)
{
    char stackBuffer[256];

    va_list args;
    va_start(args, format);
    va_list argsCopy;
    va_copy(argsCopy, args);
    int written = vsnprintf(stackBuffer, sizeof(stackBuffer), format, args);
    va_end(args);

    if (written < 0) {
        va_end(argsCopy);
        return;
    }

    if (static_cast<size_t>(written) < sizeof(stackBuffer)) {
        output.append(stackBuffer, static_cast<size_t>(written));
        va_end(argsCopy);
        return;
    }

    std::vector<char> dynamicBuffer(static_cast<size_t>(written) + 1);
    vsnprintf(dynamicBuffer.data(), dynamicBuffer.size(), format, argsCopy);
    va_end(argsCopy);
    output.append(dynamicBuffer.data(), static_cast<size_t>(written));
}
}

LaserDataLoader::LaserDataLoader()
{
    m_lastError.clear();
}

LaserDataLoader::~LaserDataLoader()
{
}

int LaserDataLoader::LoadLaserScanData(const std::string& fileName, 
                                      std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
                                      int& lineNum,
                                      float& scanSpeed,
                                      int& maxTimeStamp,
                                      int& clockPerSecond)
{
    LOG_INFO("Loading from file: %s\n", fileName.c_str());
    
    // 清空输出参数
    laserLines.clear();
    lineNum = 0;
    scanSpeed = 0.0f;
    maxTimeStamp = 0;
    clockPerSecond = 0;

    if (HasFileExtension(fileName, ".dat") && !LooksLikeTextLaserFile(fileName)) {
        return _LoadLaserScanDataFromDatFile(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
    }
    
    // 判断文件类型
    std::ifstream inputFile(fileName);
    if (!inputFile.is_open()) {
        m_lastError = "Cannot open file: " + fileName;
        LOG_ERROR("Cannot open file: %s\n", fileName.c_str());
        return ERR_CODE(FILE_ERR_NOEXIST);
    }
    
    std::string line;
    int result = SUCCESS;

    // 包含DataType字段，检查数据类型
    EVzResultDataType eDataType = keResultDataType_Invalid;
    result = _GetLaserType(fileName, eDataType);
    ERR_CODE_RETURN(result);

    LOG_INFO("Laser data type: %d \n", eDataType);

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

    bool bFindLineNum = true;
    int nLaserPointIdx = 0;

    auto failTextLoad = [&](int code, const std::string& message) -> int {
        m_lastError = message;
        LOG_ERROR("%s\n", message.c_str());
        if (!bFindLineNum) {
            laserLines.push_back(std::make_pair(eDataType, sLaserData));
            bFindLineNum = true;
        }
        FreeLaserScanData(laserLines);
        lineNum = 0;
        return ERR_CODE(code);
    };

    auto commitCurrentLine = [&]() -> int {
        if (bFindLineNum) return SUCCESS;
        // if (nLaserPointIdx != sLaserData.nPointCount) {
        //     std::ostringstream message;
        //     message << "Incomplete laser line: expected " << sLaserData.nPointCount
        //             << " points, got " << nLaserPointIdx;
        //     return failTextLoad(FILE_ERR_FORMAT, message.str());
        // }
        laserLines.push_back(std::make_pair(eDataType, sLaserData));
        bFindLineNum = true;
        return SUCCESS;
    };

    while (std::getline(inputFile, line)) {
        // 去除行末的 \r 字符（处理 Windows 格式的 CR LF 换行符）
        TrimCarriageReturn(line);

        if (line.find("LineNum:") == 0) {
            if (1 != sscanf(line.c_str(), "LineNum:%d", &lineNum) || lineNum <= 0) {
                return failTextLoad(FILE_ERR_FORMAT, "Invalid LineNum header: " + line);
            }
        } else if (line.find("DataType:") == 0) {

        } else if (line.find("Line_") == 0) {

            if(false == bFindLineNum) {
                result = commitCurrentLine();
                if (SUCCESS != result) return result;
            }

            int lineIndex = 0;
            unsigned int timeStamp = 0;
            int ptNum = 0;
            if (3 != sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum) ||
                ptNum < 0) {
                return failTextLoad(FILE_ERR_FORMAT, "Invalid laser line header: " + line);
            }

            sLaserData.llFrameIdx = lineIndex;
            sLaserData.llTimeStamp = timeStamp;
            sLaserData.nPointCount = ptNum;
            if (eDataType == keResultDataType_PointXYZRGBA) {
                sLaserData.p3DPoint = new SVzNLPointXYZRGBA[ptNum];
                sLaserData.p2DPoint = new SVzNL2DLRPoint[ptNum];
                memset(sLaserData.p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * ptNum);
                memset(sLaserData.p2DPoint, 0, sizeof(SVzNL2DLRPoint) * ptNum);
            } else if(eDataType == keResultDataType_Position) {
                sLaserData.p3DPoint = new SVzNL3DPosition[ptNum];
                sLaserData.p2DPoint = new SVzNL2DPosition[ptNum];
                memset(sLaserData.p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
                memset(sLaserData.p2DPoint, 0, sizeof(SVzNL2DPosition) * ptNum);
            }
            nLaserPointIdx = 0;
            bFindLineNum = false;

        } else if (line.find("Poly_") == 0) {
            // 跳过 Poly 折线头及后续点坐标数据
            size_t lastUnderscore = line.rfind('_');
            int polyNum = 0;
            if (lastUnderscore != std::string::npos && lastUnderscore + 1 < line.size()) {
                polyNum = std::atoi(line.c_str() + lastUnderscore + 1);
            }
            for (int skip = 0; skip < polyNum; ) {
                if (!std::getline(inputFile, line)) {
                    return failTextLoad(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT,
                                        "Incomplete Poly block in laser file: " + fileName);
                }
                TrimCarriageReturn(line);
                if (!line.empty()) {
                    ++skip;
                }
            }

        } else if (line.find("{") == 0) {
            // 使用正则表达式判断是XYZ还是RGBD格式
            // XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY}
            // RGBD格式: {x,y,z,r,g,b}-{leftX,leftY}-{rightX,rightY}

            // 更精确的正则表达式，匹配完整的行格式
            if(sLaserData.p3DPoint == nullptr || sLaserData.p2DPoint == nullptr) {
                LOG_ERROR("sLaserData.p3DPoint == nullptr || sLaserData.p2DPoint == nullptr \n");
                return ERR_CODE(DATA_ERR_INVALID);
            }

            // 防止数组越界：跳过超出声明点数的数据行
            if (nLaserPointIdx >= sLaserData.nPointCount) {
                LOG_WARN("nLaserPointIdx(%d) >= nPointCount(%d), skip\n", nLaserPointIdx, sLaserData.nPointCount);
                continue;
            }

            if (eDataType == keResultDataType_PointXYZRGBA) {
                SVzNLPointXYZRGBA* pRGBAPoints = static_cast<SVzNLPointXYZRGBA*>(sLaserData.p3DPoint);
                SVzNL2DLRPoint* p2DPoints = static_cast<SVzNL2DLRPoint*>(sLaserData.p2DPoint);
                result = _ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
                if (SUCCESS != result) {
                    return failTextLoad(FILE_ERR_FORMAT, "Invalid RGBD point: " + line);
                }
                nLaserPointIdx++;
            } else {
                SVzNL3DPosition* p3DPoints = static_cast<SVzNL3DPosition*>(sLaserData.p3DPoint);
                SVzNL2DPosition* p2DPoints = static_cast<SVzNL2DPosition*>(sLaserData.p2DPoint);
                result = _ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
                if (SUCCESS != result) {
                    return failTextLoad(FILE_ERR_FORMAT, "Invalid XYZ point: " + line);
                }
                p3DPoints[nLaserPointIdx].nPointIdx = nLaserPointIdx;
                nLaserPointIdx++;
            }
        }
    }

    if (inputFile.bad()) {
        return failTextLoad(FILE_ERR_READ, "Failed while reading laser file: " + fileName);
    }

    // 添加最后一条扫描线数据
    if (!bFindLineNum) {
        result = commitCurrentLine();
        if (SUCCESS != result) return result;
    }
    if (lineNum <= 0 || laserLines.size() != static_cast<size_t>(lineNum)) {
        std::ostringstream message;
        message << "Laser line count mismatch: expected " << lineNum
                << ", got " << laserLines.size();
        return failTextLoad(FILE_ERR_FORMAT, message.str());
    }
    
    inputFile.close();
    LOG_INFO("Successfully loaded %d laser scan lines from file: %s\n", lineNum, fileName.c_str());
    return SUCCESS;
}

// Read binary .dat laser scan data.
int LaserDataLoader::_LoadLaserScanDataFromDatFile(const std::string& fileName,
                                                   std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
                                                   int& lineNum,
                                                   float& scanSpeed,
                                                   int& maxTimeStamp,
                                                   int& clockPerSecond)
{
    std::ifstream inputFile(fileName, std::ios::binary);
    if (!inputFile.is_open()) {
        m_lastError = "Cannot open file: " + fileName;
        LOG_ERROR("Cannot open dat file: %s\n", fileName.c_str());
        return ERR_CODE(FILE_ERR_NOEXIST);
    }

    auto fail = [&](int code, const std::string& message) -> int {
        m_lastError = message;
        LOG_ERROR("%s\n", message.c_str());
        FreeLaserScanData(laserLines);
        lineNum = 0;
        scanSpeed = 0.0f;
        maxTimeStamp = 0;
        clockPerSecond = 0;
        return ERR_CODE(code);
    };

    inputFile.seekg(0, std::ios::end);
    const std::streamoff fileSize = inputFile.tellg();
    inputFile.seekg(0, std::ios::beg);

    DatLaserScan3DLinesHeader header;
    std::memset(&header, 0, sizeof(header));
    if (!ReadBinary(inputFile, header)) {
        return fail(FILE_ERR_READ, "Cannot read dat header: " + fileName);
    }

    if (header.scanLineNum < 0 || fileSize < static_cast<std::streamoff>(sizeof(header))) {
        return fail(FILE_ERR_FORMAT, "Invalid dat header: " + fileName);
    }

    const int kFeaturePointSize = static_cast<int>(sizeof(Dat3DFeaturePoint));
    auto validateLayout = [&](int lineRecordSize) -> bool {
        std::streamoff pos = static_cast<std::streamoff>(sizeof(header));
        inputFile.clear();
        inputFile.seekg(pos, std::ios::beg);

        for (int i = 0; i < header.scanLineNum; ++i) {
            if (pos + lineRecordSize + static_cast<int>(sizeof(int)) > fileSize) {
                return false;
            }

            inputFile.seekg(lineRecordSize, std::ios::cur);
            pos += lineRecordSize;

            int pointCount = 0;
            if (!ReadBinary(inputFile, pointCount)) {
                return false;
            }
            pos += static_cast<std::streamoff>(sizeof(pointCount));

            if (pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
                return false;
            }

            const std::streamoff pointBytes =
                static_cast<std::streamoff>(pointCount) * static_cast<std::streamoff>(kFeaturePointSize);
            if (pointBytes < 0 || pos + pointBytes > fileSize) {
                return false;
            }

            inputFile.seekg(pointBytes, std::ios::cur);
            pos += pointBytes;
        }

        return pos == fileSize;
    };

    int lineRecordSize = 40;
    if (!validateLayout(lineRecordSize)) {
        lineRecordSize = 32;
        if (!validateLayout(lineRecordSize)) {
            return fail(FILE_ERR_FORMAT, "Invalid dat layout: " + fileName);
        }
    }

    lineNum = header.scanLineNum;
    scanSpeed = static_cast<float>(header.lineV);
    maxTimeStamp = static_cast<int>(header.maxTimeStamp);
    clockPerSecond = header.clock;

    laserLines.clear();
    laserLines.reserve(static_cast<size_t>(lineNum));

    inputFile.clear();
    inputFile.seekg(static_cast<std::streamoff>(sizeof(header)), std::ios::beg);

    for (int i = 0; i < lineNum; ++i) {
        DatLineScan3DData datLine;
        std::memset(&datLine, 0, sizeof(datLine));
        if (!ReadDatLineRecord(inputFile, lineRecordSize, datLine)) {
            return fail(FILE_ERR_READ, "Cannot read dat line header: " + fileName);
        }

        int pointCount = 0;
        if (!ReadBinary(inputFile, pointCount)) {
            return fail(FILE_ERR_READ, "Cannot read dat point count: " + fileName);
        }
        if (pointCount < 0 || pointCount > VZ_LASER_LINE_PT_MAX_NUM) {
            return fail(FILE_ERR_FORMAT, "Invalid dat point count: " + fileName);
        }

        SVzLaserLineData lineData;
        std::memset(&lineData, 0, sizeof(lineData));
        lineData.nPointCount = pointCount;
        lineData.dTotleOffset = datLine.lineStepping;
        lineData.dStep = datLine.lineStepping;
        lineData.llFrameIdx = static_cast<unsigned long long>(datLine.lineIndex);
        lineData.llTimeStamp = static_cast<unsigned long long>(datLine.timeStamp);

        std::unique_ptr<SVzNL3DPosition[]> points3D;
        std::unique_ptr<SVzNL2DPosition[]> points2D;

        if (pointCount > 0) {
            points3D.reset(new (std::nothrow) SVzNL3DPosition[pointCount]);
            points2D.reset(new (std::nothrow) SVzNL2DPosition[pointCount]);
            if (!points3D || !points2D) {
                return fail(DATA_ERR_MEM, "Memory allocation failed while reading dat: " + fileName);
            }

            std::memset(points3D.get(), 0, sizeof(SVzNL3DPosition) * static_cast<size_t>(pointCount));
            std::memset(points2D.get(), 0, sizeof(SVzNL2DPosition) * static_cast<size_t>(pointCount));

            for (int j = 0; j < pointCount; ++j) {
                Dat3DFeaturePoint featurePoint;
                std::memset(&featurePoint, 0, sizeof(featurePoint));
                if (!ReadBinary(inputFile, featurePoint)) {
                    return fail(FILE_ERR_READ, "Cannot read dat feature point: " + fileName);
                }

                points3D[j].nPointIdx = j;
                points3D[j].pt3D.x = featurePoint.eye.X;
                points3D[j].pt3D.y = featurePoint.eye.Y;
                points3D[j].pt3D.z = featurePoint.eye.Z;

                points2D[j].nPointIdx = j;
                points2D[j].ptLeft2D.x = static_cast<int>(featurePoint.stereo.x);
                points2D[j].ptLeft2D.y = static_cast<int>(featurePoint.stereo.y);
                points2D[j].ptRight2D.x = static_cast<int>(featurePoint.stereoRight.x);
                points2D[j].ptRight2D.y = static_cast<int>(featurePoint.stereoRight.y);
            }

            lineData.p3DPoint = points3D.get();
            lineData.p2DPoint = points2D.get();
        }

        laserLines.push_back(std::make_pair(keResultDataType_Position, lineData));
        points3D.release();
        points2D.release();
    }

    LOG_INFO("Successfully loaded %d dat laser scan lines from file: %s\n", lineNum, fileName.c_str());
    return SUCCESS;
}

// 保存激光扫描数据到文件 - 统一接口，支持两种类型的数据
int LaserDataLoader::SaveLaserScanData(const std::string& fileName,
                                      const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines,
                                      int lineNum,
                                      float scanSpeed,
                                      int maxTimeStamp,
                                      int clockPerSecond)
{
    LOG_INFO("Saving  to file: %s\n", fileName.c_str());
    
    if (laserLines.empty() || lineNum <= 0) {
        m_lastError = "Invalid input parameters for saving unified data";
        LOG_ERROR("Invalid parameters for saving unified laser data\n");
        return ERR_CODE(DEV_ARG_INVAILD);
    }
    
    try {
        std::ofstream sw(fileName);
        if (!sw.is_open()) {
            m_lastError = "Cannot open file for writing: " + fileName;
            LOG_ERROR("Cannot open file for writing: %s\n", fileName.c_str());
            return ERR_CODE(FILE_ERR_WRITE);
        }
        
        // 写入文件头
        sw << "LineNum:" << lineNum << '\n';
        sw << "DataType:" << static_cast<int>(fileDataType) << '\n';
        sw << "ScanSpeed:" << scanSpeed << '\n';
        sw << "PointAdjust: 1" << '\n';
        sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << '\n';
        
        // 写入每条扫描线数据
        for (const auto& linePair : laserLines) {
            
            EVzResultDataType dataType = linePair.first;
            const SVzLaserLineData& lineData = linePair.second;
            
            sw << "Line_" << lineData.llFrameIdx << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << '\n';
            
            // 根据数据类型写入点云数据
            if (dataType == keResultDataType_Position && lineData.p3DPoint) {
                // 写入XYZ格式数据
                const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
                const SVzNL2DPosition* points2D = static_cast<const SVzNL2DPosition*>(lineData.p2DPoint);
                constexpr int BATCH = 256;
                constexpr int PER_POINT = 128;
                for (int base = 0; base < lineData.nPointCount; base += BATCH) {
                    int end = (base + BATCH < lineData.nPointCount) ? (base + BATCH) : lineData.nPointCount;
                    std::string buffer;
                    buffer.reserve(BATCH * PER_POINT);
                    for (int i = base; i < end; ++i) {
                        float x = static_cast<float>(points[i].pt3D.x);
                        float y = static_cast<float>(points[i].pt3D.y);
                        float z = static_cast<float>(points[i].pt3D.z);
                        AppendFormattedLine(buffer, "{ %.6f,%.6f,%.6f }-{ %d,%d }-{ %d,%d }\n",
                                            x, y, z,
                                            points2D[i].ptLeft2D.x, points2D[i].ptLeft2D.y,
                                            points2D[i].ptRight2D.x, points2D[i].ptRight2D.y);
                    }
                    sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
                }
            } else if (dataType == keResultDataType_PositionF && lineData.p3DPoint) {
                const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
                const SVzNL2DPositionF* points2D = static_cast<const SVzNL2DPositionF*>(lineData.p2DPoint);
                constexpr int BATCH = 256;
                constexpr int PER_POINT = 160;
                for (int base = 0; base < lineData.nPointCount; base += BATCH) {
                    int end = (base + BATCH < lineData.nPointCount) ? (base + BATCH) : lineData.nPointCount;
                    std::string buffer;
                    buffer.reserve(BATCH * PER_POINT);
                    for (int i = base; i < end; ++i) {
                        AppendFormattedLine(buffer,
                            "{ %.9f, %.9f, %.9f } - { %.6f, %.6f } - { %.6f, %.6f }\n",
                            points[i].pt3D.x, points[i].pt3D.y, points[i].pt3D.z,
                            points2D ? points2D[i].ptLeft2D.x : 0.0f,
                            points2D ? points2D[i].ptLeft2D.y : 0.0f,
                            points2D ? points2D[i].ptRight2D.x : 0.0f,
                            points2D ? points2D[i].ptRight2D.y : 0.0f);
                    }
                    sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
                }
            } else if (dataType == keResultDataType_PointXYZI && lineData.p3DPoint) {
                // LiDAR 点云：snprintf 批量写入，每 256 点 flush
                // 坐标 mm 范围可达 ±200000 → "%.6f" 最长 14 字符 → 整行 ≤73 字符
                const SVzNLPointXYZI* points = static_cast<const SVzNLPointXYZI*>(lineData.p3DPoint);
                const int n = lineData.nPointCount;
                constexpr int BATCH = 256;
                constexpr int PER_POINT = 80;
                char buf[BATCH * PER_POINT];
                for (int base = 0; base < n; base += BATCH)
                {
                    int end = (base + BATCH < n) ? (base + BATCH) : n;
                    char* p = buf;
                    for (int i = base; i < end; ++i)
                    {
                        p += snprintf(p, PER_POINT, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n",
                                      points[i].fData[0], points[i].fData[1], points[i].fData[2]);
                    }
                    sw.write(buf, p - buf);
                }
            } else if (dataType == keResultDataType_PointXYZRGBA && lineData.p3DPoint) {
                // 写入RGBD格式数据
                const SVzNLPointXYZRGBA* points = static_cast<const SVzNLPointXYZRGBA*>(lineData.p3DPoint);
                const SVzNL2DLRPoint* points2D = static_cast<const SVzNL2DLRPoint*>(lineData.p2DPoint);
                constexpr int BATCH = 256;
                constexpr int PER_POINT = 160;
                for (int base = 0; base < lineData.nPointCount; base += BATCH) {
                    int end = (base + BATCH < lineData.nPointCount) ? (base + BATCH) : lineData.nPointCount;
                    std::string buffer;
                    buffer.reserve(BATCH * PER_POINT);
                    for (int i = base; i < end; ++i) {
                        float x = static_cast<float>(points[i].x);
                        float y = static_cast<float>(points[i].y);
                        float z = static_cast<float>(points[i].z);
                        int r = (points[i].nRGB >> 16) & 0xFF;
                        int g = (points[i].nRGB >> 8) & 0xFF;
                        int b = points[i].nRGB & 0xFF;

                        AppendFormattedLine(buffer, "{%.6f,%.6f,%.6f,%.6f,%.6f,%.6f}-{ %d,%d }-{ %d,%d }\n",
                                            x, y, z,
                                            b * 1.0f / 255, g * 1.0f / 255, r * 1.0f / 255,
                                            points2D[i].sLeft.x, points2D[i].sLeft.y,
                                            points2D[i].sRight.x, points2D[i].sRight.y);
                    }
                    sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
                }
            }
        }
        
        sw.flush();
        if (!sw.good()) {
            m_lastError = "Failed while writing file: " + fileName;
            LOG_ERROR("Failed while writing file: %s\n", fileName.c_str());
            return ERR_CODE(FILE_ERR_WRITE);
        }
        sw.close();
        return SUCCESS;
        
    } catch (const std::exception& e) {
        m_lastError = "Error saving unified file: " + std::string(e.what());
        LOG_ERROR("Error saving unified laser data to file: %s\n", e.what());
        return ERR_CODE(FILE_ERR_WRITE);
    }
}

int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition> > xyzData)
{
    return DebugSaveLaser(std::move(fileName), std::move(xyzData), nullptr);
}

int LaserDataLoader::DebugSaveLaser(std::string fileName, std::vector<std::vector<SVzNL3DPosition>> xyzData,
                                     ISaveProgressCallback* callback)
{
    LOG_INFO("Saving unified laser scan data to file: %s\n", fileName.c_str());

    if (xyzData.empty()) {
        m_lastError = "Invalid input parameters for saving unified data";
        LOG_ERROR("Invalid parameters for saving unified laser data\n");
        return ERR_CODE(DEV_ARG_INVAILD);
    }

    try {
        std::ofstream sw(fileName);
        if (!sw.is_open()) {
            m_lastError = "Cannot open file for writing: " + fileName;
            LOG_ERROR("Cannot open file for writing: %s\n", fileName.c_str());
            return ERR_CODE(FILE_ERR_WRITE);
        }

        // 写入文件头
        sw << "LineNum:" << xyzData.size() << '\n';
        sw << "DataType: 0" << '\n';
        sw << "ScanSpeed:" << 0 << '\n';
        sw << "PointAdjust: 1" << '\n';
        sw << "MaxTimeStamp:" << 0 << "_" << 0 << '\n';

        const size_t totalLines = xyzData.size();
        int index = 0;
        // 写入每条扫描线数据
        for (const auto& linePair : xyzData) {
            if (callback && callback->isSaveCancelled()) {
                sw.close();
                m_lastError = "Save cancelled by user";
                LOG_INFO("Save cancelled by user at line %d/%zu\n", index, totalLines);
                return ERR_CODE(DEV_ARG_INVAILD);
            }

            sw << "Line_" << index << "_" << 0 << "_" << linePair.size() << '\n';
            // 根据数据类型写入点云数据
            constexpr int BATCH = 256;
            constexpr int PER_POINT = 96;
            for (size_t base = 0; base < linePair.size(); base += BATCH) {
                size_t end = std::min(base + static_cast<size_t>(BATCH), linePair.size());
                std::string buffer;
                buffer.reserve(BATCH * PER_POINT);
                for (size_t i = base; i < end; ++i) {
                    const auto& point = linePair[i];

                // 写入XYZ格式数据
                    float x = static_cast<float>(point.pt3D.x);
                    float y = static_cast<float>(point.pt3D.y);
                    float z = static_cast<float>(point.pt3D.z);
                    AppendFormattedLine(buffer, "{ %.6f, %.6f, %.6f } - { 0, 0 } - { 0, 0 }\n",
                                        x, y, z);
                }
                sw.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
            }

            ++index;
            if (callback) {
                callback->onSaveProgress(static_cast<float>(index) / static_cast<float>(totalLines));
            }
        }

        sw.flush();
        if (!sw.good()) {
            m_lastError = "Failed while writing file: " + fileName;
            LOG_ERROR("Failed while writing file: %s\n", fileName.c_str());
            return ERR_CODE(FILE_ERR_WRITE);
        }
        sw.close();
        return SUCCESS;

    } catch (const std::exception& e) {
        m_lastError = "Error saving unified file: " + std::string(e.what());
        LOG_ERROR("Error saving unified laser data to file: %s\n", e.what());
        return ERR_CODE(FILE_ERR_WRITE);
    }
}

int LaserDataLoader::LoadPolySegments(const std::string& fileName,
                                      std::vector<std::vector<SVzNLPointXYZRGBA>>& polyLines)
{
    LOG_INFO("Loading poly segments from file: %s\n", fileName.c_str());

    polyLines.clear();

    std::ifstream inputFile(fileName);
    if (!inputFile.is_open()) {
        m_lastError = "Cannot open file: " + fileName;
        LOG_ERROR("Cannot open file: %s\n", fileName.c_str());
        return ERR_CODE(FILE_ERR_NOEXIST);
    }

    std::string line;

    while (std::getline(inputFile, line)) {
        TrimCarriageReturn(line);

        // 跳过空行和注释
        if (line.empty() || line[0] == '#') {
            continue;
        }

        // 查找 Poly_index_num 头
        if (line.find("Poly_") != 0) {
            continue;
        }

        // 解析点数量（最后一个下划线后的数字）
        size_t lastUnderscore = line.rfind('_');
        if (lastUnderscore == std::string::npos || lastUnderscore + 1 >= line.size()) {
            LOG_WARN("[LaserDataLoader] Invalid Poly header: %s\n", line.c_str());
            continue;
        }

        int num = std::atoi(line.c_str() + lastUnderscore + 1);
        if (num < 2) {
            LOG_WARN("[LaserDataLoader] Invalid point count in Poly header: %s\n", line.c_str());
            continue;
        }

        // 读取 num 个点坐标
        std::vector<SVzNLPointXYZRGBA> points;
        bool incompleteBlock = false;
        for (int i = 0; i < num; ) {
            if (!std::getline(inputFile, line)) {
                incompleteBlock = true;
                break;
            }
            TrimCarriageReturn(line);
            if (line.empty()) {
                continue;  // 跳过空行，不计数
            }

            SVzNLPointXYZRGBA pt;
            memset(&pt, 0, sizeof(pt));
            pt.nRGB = 0x00FFFFFF;  // 默认白色
            bool parsed = false;

            // 尝试花括号格式
            if (line.find('{') != std::string::npos) {
                float fx, fy, fz;
                int R = 255, G = 255, B = 255, A = 255;

                // 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
                int cnt = sscanf(line.c_str(),
                    " { %f , %f , %f } - { %*f , %*f } - { %*f , %*f } - { %d , %d , %d , %d }",
                    &fx, &fy, &fz, &R, &G, &B, &A);
                if (cnt >= 3) {
                    pt.x = fx;
                    pt.y = fy;
                    pt.z = fz;
                    if (cnt >= 7) {
                        // 解析到了颜色 R,G,B,A（0-255），Alpha 存储在高 8 位
                        unsigned int nRGB = static_cast<unsigned int>(A) << 24;
                        nRGB |= static_cast<unsigned int>(B) << 16;
                        nRGB |= static_cast<unsigned int>(G) << 8;
                        nRGB |= static_cast<unsigned int>(R);
                        pt.nRGB = nRGB;
                    }
                    parsed = true;
                }

                // 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry}
                if (!parsed) {
                    float r, g, b;
                    if (sscanf(line.c_str(), " { %f , %f , %f , %f , %f , %f }",
                               &fx, &fy, &fz, &r, &g, &b) == 6) {
                        pt.x = fx;
                        pt.y = fy;
                        pt.z = fz;
                        int nRGB = (int)(b * 255); nRGB <<= 8;
                        nRGB += (int)(g * 255); nRGB <<= 8;
                        nRGB += (int)(r * 255);
                        pt.nRGB = nRGB;
                        parsed = true;
                    }
                }

                // XYZ格式: {x,y,z}-{...}-{...}
                if (!parsed) {
                    if (sscanf(line.c_str(), " { %f , %f , %f }", &fx, &fy, &fz) == 3) {
                        pt.x = fx;
                        pt.y = fy;
                        pt.z = fz;
                        parsed = true;
                    }
                }
            }

            // 尝试纯数字格式: x y z（空格/Tab/逗号分隔）
            if (!parsed) {
                float fx, fy, fz;
                std::string lineCopy = line;
                for (char& c : lineCopy) {
                    if (c == ',' || c == '\t') c = ' ';
                }
                if (sscanf(lineCopy.c_str(), "%f %f %f", &fx, &fy, &fz) == 3) {
                    pt.x = fx;
                    pt.y = fy;
                    pt.z = fz;
                    parsed = true;
                }
            }

            if (parsed) {
                points.push_back(pt);
            }
            ++i;
        }

        if (incompleteBlock || points.size() != static_cast<size_t>(num)) {
            m_lastError = "Incomplete or invalid Poly block in file: " + fileName;
            LOG_ERROR("%s\n", m_lastError.c_str());
            polyLines.clear();
            return ERR_CODE(inputFile.bad() ? FILE_ERR_READ : FILE_ERR_FORMAT);
        }
        polyLines.push_back(std::move(points));
    }

    if (inputFile.bad()) {
        m_lastError = "Failed while reading Poly file: " + fileName;
        LOG_ERROR("%s\n", m_lastError.c_str());
        polyLines.clear();
        return ERR_CODE(FILE_ERR_READ);
    }

    inputFile.close();
    LOG_INFO("Loaded %zu poly lines from file: %s\n", polyLines.size(), fileName.c_str());
    return SUCCESS;
}

void LaserDataLoader::FreeLaserScanData(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& laserLines)
{
    LOG_DEBUG("Freeing unified laser scan data, line count: %zu\n", laserLines.size());
    
    if (!laserLines.empty()) {
        for (auto& linePair : laserLines) {
            EVzResultDataType dataType = linePair.first;
            SVzLaserLineData& lineData = linePair.second;
            
            if (lineData.p3DPoint) {
                switch (dataType) {
                case keResultDataType_Position:
                    delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
                    break;
                case keResultDataType_PositionF:
                    delete[] static_cast<SVzNL3DPosition*>(lineData.p3DPoint);
                    break;
                case keResultDataType_PointF:
                    delete[] static_cast<SVzNL3DPointF*>(lineData.p3DPoint);
                    break;
                case keResultDataType_PointXYZ:
                    delete[] static_cast<SVzNLPointXYZ*>(lineData.p3DPoint);
                    break;
                case keResultDataType_PointXYZRGBA:
                    delete[] static_cast<SVzNLPointXYZRGBA*>(lineData.p3DPoint);
                    break;
                case keResultDataType_PointGray:
                    delete[] static_cast<SVzNLPointXYZGray*>(lineData.p3DPoint);
                    break;
                case keResultDataType_PointXYZI:
                    delete[] static_cast<SVzNLPointXYZI*>(lineData.p3DPoint);
                    break;
                case keResultDataType_PointRGBA_D:
                    delete[] static_cast<SVzNLPointRGBA_D*>(lineData.p3DPoint);
                    break;
                default:
                    LOG_WARN("Unknown laser data type when freeing p3DPoint: %d\n", dataType);
                    break;
                }
                lineData.p3DPoint = nullptr;
            }

            if (lineData.p2DPoint) {
                switch (dataType) {
                case keResultDataType_Position:
                    delete[] static_cast<SVzNL2DPosition*>(lineData.p2DPoint);
                    break;
                case keResultDataType_PositionF:
                    delete[] static_cast<SVzNL2DPositionF*>(lineData.p2DPoint);
                    break;
                case keResultDataType_PointF:
                case keResultDataType_PointXYZ:
                case keResultDataType_PointXYZRGBA:
                case keResultDataType_PointGray:
                case keResultDataType_PointXYZI:
                case keResultDataType_PointRGBA_D:
                    delete[] static_cast<SVzNL2DLRPoint*>(lineData.p2DPoint);
                    break;
                default:
                    LOG_WARN("Unknown laser data type when freeing p2DPoint: %d\n", dataType);
                    break;
                }
                lineData.p2DPoint = nullptr;
            }
        }
        laserLines.clear();
    }
    
    LOG_DEBUG("Unified laser scan data freed successfully\n");
}

// 转换统一格式数据为std::vector<std::vector<SVzNL3DPosition>>格式
int LaserDataLoader::ConvertToSVzNL3DPosition(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
                                       std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                       size_t* validPointCount)
{
    LOG_DEBUG("Converting unified data to scan lines format\n");

    scanLines.clear();

    // 初始化有效点计数
    size_t validCount = 0;

    // 浮点数零值判断阈值
    const double EPSILON = 1e-6;

    for (const auto& linePair : unifiedData) {
        EVzResultDataType dataType = linePair.first;
        const SVzLaserLineData& lineData = linePair.second;

        // 只处理Position类型的数据
        if (dataType == keResultDataType_Position && lineData.p3DPoint) {
            std::vector<SVzNL3DPosition> scanLine;

            // 为当前扫描线分配空间
            scanLine.reserve(lineData.nPointCount);

            // 复制点数据并统计有效点
            const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
            for (int i = 0; i < lineData.nPointCount; i++) {
                scanLine.push_back(points[i]);

                // 统计有效点（xyz全非0，使用epsilon阈值判断）
                if (std::fabs(points[i].pt3D.x) > EPSILON &&
                    std::fabs(points[i].pt3D.y) > EPSILON &&
                    std::fabs(points[i].pt3D.z) > EPSILON) {
                    validCount++;
                }
            }

            scanLines.push_back(scanLine);
        } else if (dataType == keResultDataType_PositionF && lineData.p3DPoint) {
            std::vector<SVzNL3DPosition> scanLine;
            scanLine.reserve(lineData.nPointCount);

            const SVzNL3DPosition* points = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
            for (int i = 0; i < lineData.nPointCount; i++) {
                scanLine.push_back(points[i]);

                if (std::fabs(points[i].pt3D.x) > EPSILON &&
                    std::fabs(points[i].pt3D.y) > EPSILON &&
                    std::fabs(points[i].pt3D.z) > EPSILON) {
                    validCount++;
                }
            }

            scanLines.push_back(scanLine);
        }
    }

    // 返回有效点计数（如果调用者需要）
    if (validPointCount) {
        *validPointCount = validCount;
    }

    LOG_DEBUG("Converted %zu lines to scan lines format, valid points: %zu\n", scanLines.size(), validCount);
    return SUCCESS;
}

int LaserDataLoader::ConvertToSVzNLPositionD(
    const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
    std::vector<std::vector<SVzNLPositionD>>& scanLines)
{
    LOG_DEBUG("Converting unified data to SVzNLPositionD scan lines format\n");

    scanLines.clear();

    for (const auto& linePair : unifiedData) {
        if (linePair.first != keResultDataType_Position &&
            linePair.first != keResultDataType_PositionF) {
            continue;
        }

        const SVzLaserLineData& lineData = linePair.second;
        if (lineData.nPointCount < 0) {
            continue;
        }

        if (lineData.nPointCount > 0 && (!lineData.p3DPoint || !lineData.p2DPoint)) {
            m_lastError = "Missing 3D or 2D point buffer in Position laser data";
            LOG_ERROR("%s, pointCount=%d, p3D=%p, p2D=%p\n",
                      m_lastError.c_str(),
                      lineData.nPointCount,
                      lineData.p3DPoint,
                      lineData.p2DPoint);
            continue;
        }

        std::vector<SVzNLPositionD> scanLine;
        scanLine.reserve(static_cast<size_t>(lineData.nPointCount));
        if (linePair.first == keResultDataType_Position) {
            const SVzNL3DPosition* points3D = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
            const SVzNL2DPosition* points2D = static_cast<const SVzNL2DPosition*>(lineData.p2DPoint);
            for (int i = 0; i < lineData.nPointCount; ++i) {
                SVzNLPositionD point;
                std::memset(&point, 0, sizeof(point));
                point.nPointIdx  = points3D[i].nPointIdx;
                point.pt3D       = points3D[i].pt3D;
                point.ptLeft2D.x = points2D[i].ptLeft2D.x;
                point.ptLeft2D.y = points2D[i].ptLeft2D.y;
                point.ptRight2D.x = points2D[i].ptRight2D.x;
                point.ptRight2D.y = points2D[i].ptRight2D.y;
                scanLine.push_back(point);
            }
        } else {
            const SVzNL3DPosition* points3D = static_cast<const SVzNL3DPosition*>(lineData.p3DPoint);
            const SVzNL2DPositionF* points2D = static_cast<const SVzNL2DPositionF*>(lineData.p2DPoint);
            for (int i = 0; i < lineData.nPointCount; ++i) {
                SVzNLPositionD point;
                std::memset(&point, 0, sizeof(point));
                point.nPointIdx  = points3D[i].nPointIdx;
                point.pt3D       = points3D[i].pt3D;
                point.ptLeft2D.x = points2D[i].ptLeft2D.x;
                point.ptLeft2D.y = points2D[i].ptLeft2D.y;
                point.ptRight2D.x = points2D[i].ptRight2D.x;
                point.ptRight2D.y = points2D[i].ptRight2D.y;
                scanLine.push_back(point);
            }
        }

        if (!scanLine.empty()) {
            scanLines.push_back(scanLine);
        }
    }

    if (scanLines.empty()) {
        m_lastError = "No Position laser data to convert";
        LOG_ERROR("%s\n", m_lastError.c_str());
        return ERR_CODE(DATA_ERR_INVALID);
    }

    LOG_DEBUG("Converted %zu lines to SVzNLPositionD scan lines format\n", scanLines.size());
    return SUCCESS;
}

// 转换统一格式数据为SVzNL3DLaserLine格式
int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
                                               std::vector<SVzNL3DLaserLine>& xyzData)
{    
    xyzData.clear();
    
    for (const auto& linePair : unifiedData) {
        EVzResultDataType dataType = linePair.first;
        const SVzLaserLineData& lineData = linePair.second;
        
        // PositionF 的 3D buffer 也是 SVzNL3DPosition；仅 2D UV 为浮点。
        if ((dataType == keResultDataType_Position ||
             dataType == keResultDataType_PositionF) && lineData.p3DPoint) {
            SVzNL3DLaserLine xyzLine;
            xyzLine.nTimeStamp = lineData.llTimeStamp;
            xyzLine.nPositionCnt = lineData.nPointCount;
            
            if (lineData.nPointCount > 0) {
                xyzLine.p3DPosition = new SVzNL3DPosition[lineData.nPointCount];
                if (xyzLine.p3DPosition) {
                    memcpy(xyzLine.p3DPosition, lineData.p3DPoint, sizeof(SVzNL3DPosition) * lineData.nPointCount);
                } else {
                    m_lastError = "Memory allocation failed for SVzNL3DPosition";
                    LOG_ERROR("Memory allocation failed for SVzNL3DPosition\n");
                    return ERR_CODE(DATA_ERR_INVALID);
                }
            } else {
                xyzLine.p3DPosition = nullptr;
            }
            
            xyzData.push_back(xyzLine);
        }
    }
    
    LOG_DEBUG("Converted %zu lines to SVzNL3DLaserLine format\n", xyzData.size());
    return SUCCESS;
}

// 转换统一格式数据为SVzNLXYZRGBDLaserLine格式
int LaserDataLoader::ConvertToSVzNLXYZRGBDLaserLine(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
                                                    std::vector<SVzNLXYZRGBDLaserLine>& rgbdData)
{
    LOG_DEBUG("Converting unified data to SVzNLXYZRGBDLaserLine format\n");
    
    rgbdData.clear();
    
    for (const auto& linePair : unifiedData) {
        EVzResultDataType dataType = linePair.first;
        const SVzLaserLineData& lineData = linePair.second;
        
        // 只处理PointXYZRGBA类型的数据
        if (dataType == keResultDataType_PointXYZRGBA && lineData.p3DPoint) {
            SVzNLXYZRGBDLaserLine rgbdLine;
            rgbdLine.nTimeStamp = lineData.llTimeStamp;
            rgbdLine.nPointCnt = lineData.nPointCount;
            
            if (lineData.nPointCount > 0) {
                rgbdLine.p3DPoint = new SVzNLPointXYZRGBA[lineData.nPointCount];
                if (rgbdLine.p3DPoint) {
                    memcpy(rgbdLine.p3DPoint, lineData.p3DPoint, sizeof(SVzNLPointXYZRGBA) * lineData.nPointCount);
                } else {
                    m_lastError = "Memory allocation failed for SVzNLPointXYZRGBA";
                    LOG_ERROR("Memory allocation failed for SVzNLPointXYZRGBA\n");
                    return ERR_CODE(DATA_ERR_INVALID);
                }
            } else {
                rgbdLine.p3DPoint = nullptr;
            }
            
            rgbdData.push_back(rgbdLine);
        }
    }
    
    LOG_DEBUG("Converted %zu lines to SVzNLXYZRGBDLaserLine format\n", rgbdData.size());
    return SUCCESS;
}

// 释放转换后的SVzNL3DLaserLine数据内存
void LaserDataLoader::FreeConvertedData(std::vector<SVzNL3DLaserLine>& xyzData)
{
    LOG_DEBUG("Freeing converted SVzNL3DLaserLine data, line count: %zu\n", xyzData.size());
    
    if (!xyzData.empty()) {
        for (auto& scanLine : xyzData) {
            if (scanLine.p3DPosition) {
                delete[] scanLine.p3DPosition;
                scanLine.p3DPosition = nullptr;
            }
        }
        xyzData.clear();
    }
    
    LOG_DEBUG("Converted SVzNL3DLaserLine data freed successfully\n");
}

// 释放转换后的SVzNLXYZRGBDLaserLine数据内存
void LaserDataLoader::FreeConvertedData(std::vector<SVzNLXYZRGBDLaserLine>& rgbdData)
{
    LOG_DEBUG("Freeing converted SVzNLXYZRGBDLaserLine data, line count: %zu\n", rgbdData.size());
    
    if (!rgbdData.empty()) {
        for (auto& scanLine : rgbdData) {
            if (scanLine.p3DPoint) {
                delete[] scanLine.p3DPoint;
                scanLine.p3DPoint = nullptr;
            }
        }
        rgbdData.clear();
    }
    
    LOG_DEBUG("Converted SVzNLXYZRGBDLaserLine data freed successfully\n");
}


int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPosition& s2DData)
{
    float X = 0.0f, Y = 0.0f, Z = 0.0f;
    float leftX = 0.0f, leftY = 0.0f;
    float rightX = 0.0f, rightY = 0.0f;
    const int parsed = sscanf(data.c_str(),
        " { %f , %f , %f } - { %f , %f } - { %f , %f }",
        &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
    if (parsed != 7) {
        return ERR_CODE(FILE_ERR_FORMAT);
    }
    sData.pt3D.x = X;
    sData.pt3D.y = Y;
    sData.pt3D.z = Z;
    s2DData.ptLeft2D.x = leftX;
    s2DData.ptLeft2D.y = leftY;
    s2DData.ptRight2D.x = rightX;
    s2DData.ptRight2D.y = rightY;
    return SUCCESS;
}

int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNL3DPosition& sData, SVzNL2DPositionF& s2DData)
{
    double X = 0.0, Y = 0.0, Z = 0.0;
    float leftX = 0.0f, leftY = 0.0f;
    float rightX = 0.0f, rightY = 0.0f;
    const int parsed = sscanf(data.c_str(),
        " { %lf , %lf , %lf } - { %f , %f } - { %f , %f }",
        &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
    if (parsed != 7) {
        return ERR_CODE(FILE_ERR_FORMAT);
    }
    sData.pt3D.x = X;
    sData.pt3D.y = Y;
    sData.pt3D.z = Z;
    s2DData.ptLeft2D.x = leftX;
    s2DData.ptLeft2D.y = leftY;
    s2DData.ptRight2D.x = rightX;
    s2DData.ptRight2D.y = rightY;
    return SUCCESS;
}

int LaserDataLoader::_ParseLaserScanPoint(const std::string& data, SVzNLPointXYZRGBA& sData, SVzNL2DLRPoint& s2DData)
{
    float X = 0.0f, Y = 0.0f, Z = 0.0f;
    float leftX = 0.0f, leftY = 0.0f;
    float rightX = 0.0f, rightY = 0.0f;

    // 尝试新格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
    int R, G, B, A;
    int parsed = sscanf(data.c_str(),
        " { %f , %f , %f } - { %f , %f } - { %f , %f } - { %d , %d , %d , %d }",
        &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY, &R, &G, &B, &A);
    if (parsed >= 11) {
        sData.x = X;
        sData.y = Y;
        sData.z = Z;

        // R,G,B,A 是 0-255 整数值，Alpha 存储在高 8 位
        unsigned int nRGB = static_cast<unsigned int>(A) << 24;
        nRGB |= static_cast<unsigned int>(B) << 16;
        nRGB |= static_cast<unsigned int>(G) << 8;
        nRGB |= static_cast<unsigned int>(R);
        sData.nRGB = nRGB;

        s2DData.sLeft.x = leftX;
        s2DData.sLeft.y = leftY;
        s2DData.sRight.x = rightX;
        s2DData.sRight.y = rightY;
        return SUCCESS;
    }

    // 旧格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry}  r,g,b 为 0~1 浮点值
    float r = 0.0f, g = 0.0f, b = 0.0f;
    if (10 != sscanf(data.c_str(),
            " { %f , %f , %f , %f , %f , %f } - { %f , %f } - { %f , %f }",
            &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY)) {
        return ERR_CODE(FILE_ERR_FORMAT);
    }
    sData.x = X;
    sData.y = Y;
    sData.z = Z;

    int nr = (int)(r * 255);
    int ng = (int)(g * 255);
    int nb = (int)(b * 255);
    nb <<= 8;
    nb += ng;
    nb <<= 8;
    nb += nr;
    sData.nRGB = nb;

    s2DData.sLeft.x = leftX;
    s2DData.sLeft.y = leftY;
    s2DData.sRight.x = rightX;
    s2DData.sRight.y = rightY;
    return SUCCESS;
}

// 获取激光数据类型
int LaserDataLoader::_GetLaserType(const std::string& fileName, EVzResultDataType& eDataType)
{
    std::ifstream inputFile(fileName);
    std::string linedata;

    if (!inputFile.is_open()) {
        m_lastError = "Cannot open file: " + fileName;
        return ERR_CODE(FILE_ERR_NOEXIST);
    }

    bool bFind = false;
    while (std::getline(inputFile, linedata)) {
        TrimCarriageReturn(linedata);

        if (linedata.find("{") == 0) {
            // 统计 { 的数量判断格式
            int braceCount = 0;
            for (char c : linedata) {
                if (c == '{') braceCount++;
            }

            if (braceCount == 4) {
                // 新RGBD格式: {x,y,z}-{lx,ly}-{rx,ry}-{R,G,B,A}
                eDataType = keResultDataType_PointXYZRGBA;
                bFind = true;
            } else if (braceCount == 3) {
                // 检查第一组的逗号数量区分XYZ和旧RGBD
                size_t firstClose = linedata.find('}');
                if (firstClose != std::string::npos) {
                    int commaCount = 0;
                    for (size_t i = 0; i < firstClose; ++i) {
                        if (linedata[i] == ',') commaCount++;
                    }
                    if (commaCount >= 5) {
                        // 旧RGBD格式: {x,y,z,r,g,b}-{lx,ly}-{rx,ry}
                        eDataType = keResultDataType_PointXYZRGBA;
                        bFind = true;
                    } else if (commaCount >= 2) {
                        // XYZUV文本格式: {x,y,z}-{lx,ly}-{rx,ry}
                        eDataType = keResultDataType_Position;
                        bFind = true;
                    }
                }
            }
            break;
        }
    }

    inputFile.close();
    return bFind ? SUCCESS : ERR_CODE(FILE_ERR_FORMAT);
}