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

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

LaserDataLoader::~LaserDataLoader()
{
}

int LaserDataLoader::LoadLaserScanData(const std::string& fileName, 
                                      std::vector<SVzNL3DLaserLine>& 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;
    
    // 调用内部读取函数
    int dataCalib = 0;
    SVzNL3DLaserLine* rawData = ReadLaserScanPointFromFile_XYZ(fileName.c_str(), 
                                                               &lineNum, 
                                                               &scanSpeed,
                                                               &dataCalib, 
                                                               &maxTimeStamp, 
                                                               &clockPerSecond);
    
    if (rawData == nullptr) {
        m_lastError = "Failed to read laser scan data from file: " + fileName;
        LOG_ERROR("Failed to load laser scan data from file: %s\n", fileName.c_str());
        return ERR_CODE(FILE_ERR_NOEXIST);
    }
    
    // 将原始数据转换为对象向量格式
    try {
        laserLines.reserve(lineNum);
        for (int i = 0; i < lineNum; i++) {
            SVzNL3DLaserLine laserLine;
            laserLine.nTimeStamp = rawData[i].nTimeStamp;
            laserLine.nPositionCnt = rawData[i].nPositionCnt;
            
            // 深拷贝p3DPosition数据
            if (rawData[i].p3DPosition && rawData[i].nPositionCnt > 0) {
                laserLine.p3DPosition = static_cast<SVzNL3DPosition*>(malloc(sizeof(SVzNL3DPosition) * rawData[i].nPositionCnt));
                if (laserLine.p3DPosition) {
                    memcpy(laserLine.p3DPosition, rawData[i].p3DPosition, sizeof(SVzNL3DPosition) * rawData[i].nPositionCnt);
                } else {
                    throw std::bad_alloc();
                }
            } else {
                laserLine.p3DPosition = nullptr;
            }
            
            laserLines.push_back(laserLine);
        }
        
        // 释放原始数据内存
        for (int i = 0; i < lineNum; i++) {
            if (rawData[i].p3DPosition) {
                free(rawData[i].p3DPosition);
            }
        }
        free(rawData);
        
    } catch (const std::exception& e) {
        m_lastError = "Memory allocation error: " + std::string(e.what());
        LOG_ERROR("Memory allocation error when loading laser data: %s\n", e.what());
        
        // 释放已分配的内存
        if (rawData) {
            for (int i = 0; i < lineNum; i++) {
                if (rawData[i].p3DPosition) {
                    free(rawData[i].p3DPosition);
                }
            }
            free(rawData);
        }
        return ERR_CODE(DATA_ERR_INVALID);
    }
    
    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<SVzNL3DLaserLine>& laserLines,
                                      int lineNum,
                                      float scanSpeed,
                                      int maxTimeStamp,
                                      int clockPerSecond)
{
    LOG_INFO("Saving laser scan data to file: %s\n", fileName.c_str());
    
    if (laserLines.empty() || lineNum <= 0) {
        m_lastError = "Invalid input parameters for saving";
        LOG_ERROR("Invalid parameters for saving 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;
        
        // 写入每条扫描线数据
        for (int line = 0; line < lineNum && line < static_cast<int>(laserLines.size()); line++) {
            const SVzNL3DLaserLine& scanLine = laserLines[line];  // 使用对象引用而不是指针
            
            // 计算有效点数（移除零点）
            int realNum = 0;
            for (int i = 0; i < scanLine.nPositionCnt; i++) {
                if (scanLine.p3DPosition[i].pt3D.z > 1e-4) {
                    realNum++;
                }
            }
            
            sw << "Line_" << line << "_" << scanLine.nTimeStamp << "_" << realNum << std::endl;
            
            // 写入有效的3D点
            for (int i = 0; i < scanLine.nPositionCnt; i++) {
                if (scanLine.p3DPosition[i].pt3D.z > 1e-4) {
                    const SVzNL3DPosition* pt3D = &scanLine.p3DPosition[i];
                    float x = static_cast<float>(pt3D->pt3D.x);
                    float y = static_cast<float>(pt3D->pt3D.y);
                    float z = static_cast<float>(pt3D->pt3D.z);
                    sw << "{ " << x << "," << y << "," << z << " }-";
                    sw << "{0,0}-{0,0}" << std::endl;
                }
            }
        }
        
        sw.close();
        LOG_INFO("Successfully saved laser scan data to file: %s\n", fileName.c_str());
        return SUCCESS;
        
    } catch (const std::exception& e) {
        m_lastError = "Error saving file: " + std::string(e.what());
        LOG_ERROR("Error saving laser data to file: %s\n", e.what());
        return ERR_CODE(FILE_ERR_WRITE);
    }
}

void LaserDataLoader::FreeLaserScanData(std::vector<SVzNL3DLaserLine>& laserLines)
{
    LOG_DEBUG("Freeing laser scan data, line count: %zu\n", laserLines.size());
    
    if (!laserLines.empty()) {
        // 释放每个对象中的p3DPosition内存
        for (auto& scanLine : laserLines) {
            if (scanLine.p3DPosition) {
                free(scanLine.p3DPosition);
                scanLine.p3DPosition = nullptr;
            }
        }
        
        laserLines.clear();
    }
    
    LOG_DEBUG("Laser scan data freed successfully\n");
}

SVzNL3DLaserLine* LaserDataLoader::ReadLaserScanPointFromFile_XYZ(const char* fileName, 
                                                                  int* scanLineNum, 
                                                                  float* scanV,
                                                                  int* dataCalib, 
                                                                  int* scanMaxStamp, 
                                                                  int* canClockUnit)
{
    std::ifstream inputFile(fileName);
    std::string linedata;

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

    SVzNL3DLaserLine* _scanLines = nullptr;
    int lines = 0;
    int dataElements = 4;
    int firstIndex = -1;

    // 数据文件版本检测
    enum DataVersion { DATA_VER_OLD = 0, DATA_VER_NEW = 1 };
    DataVersion dataFileVer = DATA_VER_OLD;
    
    std::getline(inputFile, linedata);  // 第一行
    int lineNum = 0;
    
    if (linedata.find("LineNum:") == 0) {
        dataFileVer = DATA_VER_NEW;
        sscanf(linedata.c_str(), "LineNum:%d", &lines);
        if (lines == 0) {
            m_lastError = "Invalid line number in file header";
            return nullptr;
        }
        lineNum = lines;
        _scanLines = static_cast<SVzNL3DLaserLine*>(malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)));
        if (!_scanLines) {
            m_lastError = "Memory allocation failed";
            return nullptr;
        }
        memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
        if (scanLineNum) *scanLineNum = lines;
    }
    else if (linedata.find("LineNum_") == 0) {
        dataFileVer = DATA_VER_OLD;
        sscanf(linedata.c_str(), "LineNum_%d", &lines);
        if (lines == 0) {
            m_lastError = "Invalid line number in file header";
            return nullptr;
        }
        lineNum = lines;
        _scanLines = static_cast<SVzNL3DLaserLine*>(malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)));
        if (!_scanLines) {
            m_lastError = "Memory allocation failed";
            return nullptr;
        }
        memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
        if (scanLineNum) *scanLineNum = lines;
    }
    
    if (_scanLines == nullptr) {
        m_lastError = "Failed to allocate memory for scan lines";
        return nullptr;
    }

    int ptNum = 0;
    int lineIdx = -1;
    SVzNL3DPosition* p3DPoint = nullptr;
    
    if (dataFileVer == DATA_VER_NEW) {
        while (getline(inputFile, linedata)) {
            if (linedata.find("ScanSpeed:") == 0) {
                double lineV = 0;
                sscanf(linedata.c_str(), "ScanSpeed:%lf", &lineV);
                if (scanV) *scanV = static_cast<float>(lineV);
            }
            else if (linedata.find("PointAdjust:") == 0) {
                int ptAdjusted = 0;
                sscanf(linedata.c_str(), "PointAdjust:%d", &ptAdjusted);
                if (dataCalib) *dataCalib = ptAdjusted;
            }
            else if (linedata.find("MaxTimeStamp:") == 0) {
                unsigned int maxTimeStamp = 0;
                unsigned int timePerStamp = 0;
                sscanf(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp);
                if (scanMaxStamp) *scanMaxStamp = maxTimeStamp;
                if (canClockUnit) *canClockUnit = timePerStamp;
            }
            else if (linedata.find("Line_") == 0) {
                int lineIndex;
                unsigned int timeStamp;
                sscanf(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
                if (firstIndex < 0) firstIndex = lineIndex;

                lineIndex = lineIndex - firstIndex;
                if ((lineIndex < 0) || (lineIndex >= lines)) break;

                // 新扫描线
                lineIdx++;
                if (ptNum > 0) {
                    p3DPoint = static_cast<SVzNL3DPosition*>(malloc(sizeof(SVzNL3DPosition) * ptNum));
                    if (!p3DPoint) {
                        m_lastError = "Memory allocation failed for 3D points";
                        // 清理已分配的内存
                        for (int i = 0; i <= lineIdx; i++) {
                            if (_scanLines[i].p3DPosition) {
                                free(_scanLines[i].p3DPosition);
                            }
                        }
                        free(_scanLines);
                        return nullptr;
                    }
                    memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
                } else {
                    p3DPoint = nullptr;
                }
                _scanLines[lineIdx].nPositionCnt = 0;
                _scanLines[lineIdx].nTimeStamp = timeStamp;
                _scanLines[lineIdx].p3DPosition = p3DPoint;
            }
            else if (linedata.find("{") == 0) {
                float X, Y, Z;
                float leftX, leftY;
                float rightX, rightY;
                sscanf(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
                int id = _scanLines[lineIdx].nPositionCnt;
                if (id < ptNum && p3DPoint) {
                    p3DPoint[id].pt3D.x = X;
                    p3DPoint[id].pt3D.y = Y;
                    p3DPoint[id].pt3D.z = Z;
                    _scanLines[lineIdx].nPositionCnt = id + 1;
                }
            }
        }
    }
    else if (dataFileVer == DATA_VER_OLD) {
        while (getline(inputFile, linedata)) {
            if (linedata.find("DataElements_") == 0) {
                sscanf(linedata.c_str(), "DataElements_%d", &dataElements);
                if ((dataElements != 3) && (dataElements != 4)) break;
            }
            if (linedata.find("LineV_") == 0) {
                double lineV = 0;
                sscanf(linedata.c_str(), "LineV_%lf", &lineV);
                if (scanV) *scanV = static_cast<float>(lineV);
            }
            else if (linedata.find("Line_") == 0) {
                int lineIndex;
                unsigned int timeStamp;
                sscanf(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp);
                
                if (firstIndex < 0) firstIndex = lineIndex;
                lineIndex = lineIndex - firstIndex;
                if ((lineIndex < 0) || (lineIndex >= lines)) break;
                
                // 新扫描线
                lineIdx++;
                p3DPoint = static_cast<SVzNL3DPosition*>(malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM));
                if (!p3DPoint) {
                    m_lastError = "Memory allocation failed for 3D points";
                    // 清理已分配的内存
                    for (int i = 0; i <= lineIdx; i++) {
                        if (_scanLines[i].p3DPosition) {
                            free(_scanLines[i].p3DPosition);
                        }
                    }
                    free(_scanLines);
                    return nullptr;
                }
                memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
                _scanLines[lineIdx].nPositionCnt = 0;
                _scanLines[lineIdx].nTimeStamp = timeStamp;
                _scanLines[lineIdx].p3DPosition = p3DPoint;
            }
            else if (linedata.find("(") == 0) {
                float X, Y, Z;
                int imageY = 0;
                if (dataElements == 4) {
                    sscanf(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY);
                } else {
                    sscanf(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z);
                }
                int id = _scanLines[lineIdx].nPositionCnt;
                if (id < VZ_LASER_LINE_PT_MAX_NUM && p3DPoint) {
                    p3DPoint[id].pt3D.x = X;
                    p3DPoint[id].pt3D.y = Y;
                    p3DPoint[id].pt3D.z = Z;
                    _scanLines[lineIdx].nPositionCnt = id + 1;
                }
            }
        }
    }
    
    inputFile.close();
    return _scanLines;
}