GrabBag/GrabBagApp/Utils/Src/LaserDataLoader.cpp

#include "LaserDataLoader.h"
#include <fstream>
#include <sstream>
#include <cstring>
#include <cstdlib>
#include <stdexcept>
#include <regex>
#include "VrLog.h"
#include <iomanip>

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 laser scan data from file: %s\n", fileName.c_str());
    
    // 清空输出参数
    laserLines.clear();
    lineNum = 0;
    scanSpeed = 0.0f;
    maxTimeStamp = 0;
    clockPerSecond = 0;
    
    // 判断文件类型
    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;
    
    while (std::getline(inputFile, line)) {
        if (line.find("LineNum:") == 0) {
            sscanf(line.c_str(), "LineNum:%d", &lineNum);
        } else if (line.find("DataType:") == 0) {
        
        } else if (line.find("Line_") == 0) {

            if(false == bFindLineNum) {
                laserLines.push_back(std::make_pair(eDataType, sLaserData));
            }

            int lineIndex;
            unsigned int timeStamp;
            int ptNum = 0;
            sscanf(line.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);

            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("{") == 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 (eDataType == keResultDataType_PointXYZRGBA) {
                SVzNLPointXYZRGBA* pRGBAPoints = static_cast<SVzNLPointXYZRGBA*>(sLaserData.p3DPoint);
                SVzNL2DLRPoint* p2DPoints = static_cast<SVzNL2DLRPoint*>(sLaserData.p2DPoint);
                _ParseLaserScanPoint(line, pRGBAPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
                nLaserPointIdx++;
            } else {
                SVzNL3DPosition* p3DPoints = static_cast<SVzNL3DPosition*>(sLaserData.p3DPoint);
                SVzNL2DPosition* p2DPoints = static_cast<SVzNL2DPosition*>(sLaserData.p2DPoint);
                _ParseLaserScanPoint(line, p3DPoints[nLaserPointIdx], p2DPoints[nLaserPointIdx]);
                nLaserPointIdx++;
            }
        }
    }
    
    // 添加最后一条扫描线数据
    if (!bFindLineNum) {
        laserLines.push_back(std::make_pair(eDataType, sLaserData));
    }
    
    inputFile.close();
    LOG_INFO("Successfully loaded %d 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 unified laser scan data 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 << std::endl;
        sw << "DataType: 0" << std::endl;
        sw << "ScanSpeed:" << scanSpeed << std::endl;
        sw << "PointAdjust: 1" << std::endl;
        sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl;
        
        // 写入每条扫描线数据
        int lineIndex = 0;
        for (const auto& linePair : laserLines) {
            if (lineIndex >= lineNum) break;
            
            EVzResultDataType dataType = linePair.first;
            const SVzLaserLineData& lineData = linePair.second;
            
            sw << "Line_" << lineIndex << "_" << lineData.llTimeStamp << "_" << lineData.nPointCount << std::endl;
            
            // 根据数据类型写入点云数据
            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);
                for (int i = 0; i < lineData.nPointCount; 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);
                    sw << "{ " << x << "," << y << "," << z << " }-";
                    sw << "{ " << points2D[i].ptLeft2D.x << "," << points2D[i].ptLeft2D.y << " }-";
                    sw << "{ " << points2D[i].ptRight2D.x << "," << points2D[i].ptRight2D.y << " }" << std::endl;
                }
            } 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);
                for (int i = 0; i < lineData.nPointCount; 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;
                    
                    sw << "{" << std::fixed << std::setprecision(6) << x << "," 
                         << std::fixed << std::setprecision(6) << y << "," 
                         << std::fixed << std::setprecision(6) << z << "," 
                         << std::fixed << std::setprecision(6) << b * 1.0f / 255 << "," 
                         << std::fixed << std::setprecision(6) << g * 1.0f / 255 << "," 
                         << std::fixed << std::setprecision(6) << r * 1.0f / 255 << "}-";
                    sw << "{ " << points2D[i].sLeft.x << "," << points2D[i].sLeft.y << " }-";
                    sw << "{ " << points2D[i].sRight.x << "," << points2D[i].sRight.y << " }" << std::endl;
                }
            }
            
            lineIndex++;
        }
        
        sw.close();
        LOG_INFO("Successfully saved unified laser scan data to file: %s\n", fileName.c_str());
        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);
    }
}

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) {
                delete[] lineData.p3DPoint;
                lineData.p3DPoint = nullptr;
            }
            
            if (lineData.p2DPoint) {
                delete[] lineData.p2DPoint;
                lineData.p2DPoint = nullptr;
            }
        }
        laserLines.clear();
    }
    
    LOG_DEBUG("Unified laser scan data freed successfully\n");
}

// 转换统一格式数据为SVzNL3DLaserLine格式
int LaserDataLoader::ConvertToSVzNL3DLaserLine(const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& unifiedData,
                                               std::vector<SVzNL3DLaserLine>& xyzData)
{
    LOG_DEBUG("Converting unified data to SVzNL3DLaserLine format\n");
    
    xyzData.clear();
    
    for (const auto& linePair : unifiedData) {
        EVzResultDataType dataType = linePair.first;
        const SVzLaserLineData& lineData = linePair.second;
        
        // 只处理Position类型的数据
        if (dataType == keResultDataType_Position && 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, Y, Z;
    float leftX, leftY;
    float rightX, rightY;
    sscanf(data.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
    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, Y, Z;
    float r, g, b;
    float leftX, leftY;
    float rightX, rightY;
    sscanf(data.c_str(), "{%f,%f,%f,%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
    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)) {
        if (linedata.find("{") == 0) {
            // 修复正则表达式以匹配实际数据格式
            // XYZ格式: {x,y,z}-{leftX,leftY}-{rightX,rightY}
            // RGBD格式: {x,y,z,r,g,b}-{leftX,leftY}-{rightX,rightY}
            // 更宽松的正则表达式，允许更多的空格变化
            std::regex xyzPattern(R"(\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\})");
            std::regex rgbdPattern(R"(\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\}\s*-\s*\{\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*,\s*[+-]?(?:\d+\.?\d*|\.\d+)\s*\})");

            // 先尝试匹配RGBD格式（6个数字）
            if (std::regex_match(linedata, rgbdPattern)) {
                eDataType = keResultDataType_PointXYZRGBA;
                bFind = true;
            } 
            // 再尝试匹配XYZ格式（3个数字）
            else if (std::regex_match(linedata, xyzPattern)) {
                eDataType = keResultDataType_Position;
                bFind = true;
            }
            break;
        }
    }

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