#include "PointCloudConverter.h"
#include "LaserDataLoader.h"
#include "VZNL_Types.h"
#include "VrLog.h"

#include <fstream>
#include <sstream>
#include <algorithm>
#include <cctype>
#include <cmath>
#include <cstring>

PointCloudConverter::PointCloudConverter()
    : m_loadedPointCount(0)
    , m_loadedLineCount(0)
    , m_lastLoadHadColor(false)
{
}

PointCloudConverter::~PointCloudConverter()
{
}

std::string PointCloudConverter::getFileExtension(const std::string& fileName)
{
    size_t pos = fileName.rfind('.');
    if (pos == std::string::npos) {
        return "";
    }
    std::string ext = fileName.substr(pos + 1);
    std::transform(ext.begin(), ext.end(), ext.begin(),
                   [](unsigned char c) { return std::tolower(c); });
    return ext;
}

int PointCloudConverter::loadFromTxt(const std::string& fileName, PointCloudXYZ& cloud)
{
    LaserDataLoader loader;

    // 使用 CloudUtils 加载数据
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> laserLines;
    int lineNum = 0;
    float scanSpeed = 0.0f;
    int maxTimeStamp = 0;
    int clockPerSecond = 0;

    int result = loader.LoadLaserScanData(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
    if (result != 0) {
        m_lastError = "加载文件失败: " + loader.GetLastError();
        return result;
    }

    LOG_INFO("[CloudView] LoadLaserScanData success, laserLines size: %zu, lineNum: %d\n",
           laserLines.size(), lineNum);

    // 转换为 SVzNL3DPosition 格式
    std::vector<std::vector<SVzNL3DPosition>> scanLines;
    result = loader.ConvertToSVzNL3DPosition(laserLines, scanLines);
    if (result != 0) {
        m_lastError = "转换数据失败";
        loader.FreeLaserScanData(laserLines);
        return result;
    }

    LOG_INFO("[CloudView] ConvertToSVzNL3DPosition success, scanLines size: %zu\n", scanLines.size());

    // 转换为自定义点云格式，保留线索引（保留所有点包括0,0,0用于旋转）
    cloud.clear();
    size_t totalCount = 0;
    int lineIndex = 0;

    for (const auto& line : scanLines) {
        for (const auto& pos : line) {
            totalCount++;
            Point3D point;
            point.x = static_cast<float>(pos.pt3D.x);
            point.y = static_cast<float>(pos.pt3D.y);
            point.z = static_cast<float>(pos.pt3D.z);
            cloud.push_back(point, lineIndex);
        }
        lineIndex++;
    }

    LOG_INFO("[CloudView] Total points: %zu, Lines: %d\n", totalCount, lineIndex);

    loader.FreeLaserScanData(laserLines);
    m_loadedPointCount = totalCount;
    m_loadedLineCount = lineIndex;
    return 0;
}

int PointCloudConverter::loadFromTxt(const std::string& fileName, PointCloudXYZRGB& cloud)
{
    LaserDataLoader loader;

    // 使用 CloudUtils 加载数据
    std::vector<std::pair<EVzResultDataType, SVzLaserLineData>> laserLines;
    int lineNum = 0;
    float scanSpeed = 0.0f;
    int maxTimeStamp = 0;
    int clockPerSecond = 0;

    int result = loader.LoadLaserScanData(fileName, laserLines, lineNum, scanSpeed, maxTimeStamp, clockPerSecond);
    if (result != 0) {
        m_lastError = "加载文件失败，loasreuslt: " + std::to_string(result);
        return result;
    }

    LOG_INFO("[CloudView] LoadLaserScanData(XYZRGB) success, laserLines size: %zu, lineNum: %d\n",
           laserLines.size(), lineNum);

    // 检查数据类型是否包含 RGBA
    bool hasRGBA = false;
    for (const auto& linePair : laserLines) {
        if (linePair.first == keResultDataType_PointXYZRGBA) {
            hasRGBA = true;
            break;
        }
    }

    cloud.clear();
    size_t totalCount = 0;
    int lineIndex = 0;

    if (hasRGBA) {
        // RGBA 路径：使用 ConvertToSVzNLXYZRGBDLaserLine 转换
        std::vector<SVzNLXYZRGBDLaserLine> rgbdData;
        result = loader.ConvertToSVzNLXYZRGBDLaserLine(laserLines, rgbdData);
        if (result != 0) {
            m_lastError = "转换RGBA数据失败";
            loader.FreeLaserScanData(laserLines);
            return result;
        }

        LOG_INFO("[CloudView] ConvertToSVzNLXYZRGBDLaserLine success, rgbdData size: %zu\n", rgbdData.size());

        for (const auto& line : rgbdData) {
            for (int i = 0; i < line.nPointCnt; ++i) {
                const SVzNLPointXYZRGBA& pt = line.p3DPoint[i];
                // 解包颜色：nRGB 格式为 (A << 24) | (B << 16) | (G << 8) | R
                uint8_t r = static_cast<uint8_t>(pt.nRGB & 0xFF);
                uint8_t g = static_cast<uint8_t>((pt.nRGB >> 8) & 0xFF);
                uint8_t b = static_cast<uint8_t>((pt.nRGB >> 16) & 0xFF);
                uint8_t a = static_cast<uint8_t>((pt.nRGB >> 24) & 0xFF);
                // A > 1 时作为点大小使用
                float pointSize = (a > 1) ? static_cast<float>(a) : 0.0f;

                Point3DRGB point(pt.x, pt.y, pt.z, r, g, b, pointSize);
                cloud.push_back(point, lineIndex);
                totalCount++;
            }
            lineIndex++;
        }

        loader.FreeConvertedData(rgbdData);
        m_lastLoadHadColor = true;
    } else {
        // 非 RGBA 路径：回退到 SVzNL3DPosition + 白色
        std::vector<std::vector<SVzNL3DPosition>> scanLines;
        result = loader.ConvertToSVzNL3DPosition(laserLines, scanLines);
        if (result != 0) {
            m_lastError = "转换数据失败";
            loader.FreeLaserScanData(laserLines);
            return result;
        }

        LOG_INFO("[CloudView] ConvertToSVzNL3DPosition success, scanLines size: %zu\n", scanLines.size());

        for (const auto& line : scanLines) {
            for (const auto& pos : line) {
                Point3DRGB point(
                    static_cast<float>(pos.pt3D.x),
                    static_cast<float>(pos.pt3D.y),
                    static_cast<float>(pos.pt3D.z),
                    255, 255, 255);
                cloud.push_back(point, lineIndex);
                totalCount++;
            }
            lineIndex++;
        }

        m_lastLoadHadColor = false;
    }

    LOG_INFO("[CloudView] Total points(XYZRGB): %zu, Lines: %d, hasRGBA: %d\n", totalCount, lineIndex, hasRGBA);

    loader.FreeLaserScanData(laserLines);
    m_loadedPointCount = totalCount;
    m_loadedLineCount = lineIndex;
    return 0;
}

bool PointCloudConverter::parsePcdHeader(std::ifstream& file, PcdHeader& header)
{
    std::string line;
    while (std::getline(file, line)) {
        if (line.empty() || line[0] == '#') {
            continue;
        }

        std::istringstream iss(line);
        std::string key;
        iss >> key;

        if (key == "VERSION") {
            // 忽略版本
        } else if (key == "FIELDS") {
            std::string field;
            while (iss >> field) {
                header.fields.push_back(field);
                if (field == "rgb" || field == "rgba") {
                    header.hasRgb = true;
                }
            }
        } else if (key == "SIZE") {
            int size;
            while (iss >> size) {
                header.fieldSizes.push_back(size);
                header.pointSize += size;
            }
        } else if (key == "TYPE") {
            char type;
            while (iss >> type) {
                header.fieldTypes.push_back(type);
            }
        } else if (key == "COUNT") {
            // 忽略 COUNT
        } else if (key == "WIDTH") {
            iss >> header.width;
        } else if (key == "HEIGHT") {
            iss >> header.height;
        } else if (key == "VIEWPOINT") {
            // 忽略 VIEWPOINT
        } else if (key == "POINTS") {
            iss >> header.points;
        } else if (key == "DATA") {
            std::string dataType;
            iss >> dataType;
            header.isBinary = (dataType == "binary" || dataType == "binary_compressed");
            return true;  // 头部解析完成
        }
    }
    return false;
}

int PointCloudConverter::loadFromPcd(const std::string& fileName, PointCloudXYZ& cloud)
{
    std::ifstream file(fileName, std::ios::binary);
    if (!file.is_open()) {
        m_lastError = "无法打开文件: " + fileName;
        return -1;
    }

    PcdHeader header;
    if (!parsePcdHeader(file, header)) {
        m_lastError = "无法解析 PCD 文件头";
        return -1;
    }

    int numPoints = header.points > 0 ? header.points : header.width * header.height;
    LOG_INFO("[CloudView] PCD header: points=%d, width=%d, height=%d, isBinary=%d\n",
           numPoints, header.width, header.height, header.isBinary);

    cloud.clear();
    cloud.reserve(numPoints);

    // 查找 x, y, z 字段的索引
    int xIdx = -1, yIdx = -1, zIdx = -1;
    for (size_t i = 0; i < header.fields.size(); ++i) {
        if (header.fields[i] == "x") xIdx = static_cast<int>(i);
        else if (header.fields[i] == "y") yIdx = static_cast<int>(i);
        else if (header.fields[i] == "z") zIdx = static_cast<int>(i);
    }

    if (xIdx < 0 || yIdx < 0 || zIdx < 0) {
        m_lastError = "PCD 文件缺少 x, y, z 字段";
        return -1;
    }

    if (header.isBinary) {
        // 二进制格式
        std::vector<char> buffer(header.pointSize);
        for (int i = 0; i < numPoints; ++i) {
            file.read(buffer.data(), header.pointSize);
            if (!file) break;

            Point3D pt;
            int offset = 0;
            for (size_t j = 0; j < header.fields.size(); ++j) {
                if (j == static_cast<size_t>(xIdx)) {
                    memcpy(&pt.x, buffer.data() + offset, sizeof(float));
                } else if (j == static_cast<size_t>(yIdx)) {
                    memcpy(&pt.y, buffer.data() + offset, sizeof(float));
                } else if (j == static_cast<size_t>(zIdx)) {
                    memcpy(&pt.z, buffer.data() + offset, sizeof(float));
                }
                offset += header.fieldSizes[j];
            }

            if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
                cloud.push_back(pt);
            }
        }
    } else {
        // ASCII 格式
        std::string line;
        while (std::getline(file, line) && cloud.size() < static_cast<size_t>(numPoints)) {
            std::istringstream iss(line);
            std::vector<float> values;
            float val;
            while (iss >> val) {
                values.push_back(val);
            }

            if (values.size() >= 3 &&
                static_cast<size_t>(xIdx) < values.size() &&
                static_cast<size_t>(yIdx) < values.size() &&
                static_cast<size_t>(zIdx) < values.size()) {
                Point3D pt;
                pt.x = values[xIdx];
                pt.y = values[yIdx];
                pt.z = values[zIdx];

                if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
                    cloud.push_back(pt);
                }
            }
        }
    }

    LOG_INFO("[CloudView] Loaded %zu points from PCD\n", cloud.size());
    m_loadedPointCount = cloud.size();
    m_loadedLineCount = 0;  // PCD 文件没有线信息
    return 0;
}

int PointCloudConverter::loadFromPcd(const std::string& fileName, PointCloudXYZRGB& cloud)
{
    std::ifstream file(fileName, std::ios::binary);
    if (!file.is_open()) {
        m_lastError = "无法打开文件: " + fileName;
        return -1;
    }

    PcdHeader header;
    if (!parsePcdHeader(file, header)) {
        m_lastError = "无法解析 PCD 文件头";
        return -1;
    }

    int numPoints = header.points > 0 ? header.points : header.width * header.height;
    cloud.clear();
    cloud.reserve(numPoints);

    // 查找字段索引
    int xIdx = -1, yIdx = -1, zIdx = -1, rgbIdx = -1;
    for (size_t i = 0; i < header.fields.size(); ++i) {
        if (header.fields[i] == "x") xIdx = static_cast<int>(i);
        else if (header.fields[i] == "y") yIdx = static_cast<int>(i);
        else if (header.fields[i] == "z") zIdx = static_cast<int>(i);
        else if (header.fields[i] == "rgb" || header.fields[i] == "rgba") rgbIdx = static_cast<int>(i);
    }

    if (xIdx < 0 || yIdx < 0 || zIdx < 0) {
        m_lastError = "PCD 文件缺少 x, y, z 字段";
        return -1;
    }

    if (header.isBinary) {
        std::vector<char> buffer(header.pointSize);
        for (int i = 0; i < numPoints; ++i) {
            file.read(buffer.data(), header.pointSize);
            if (!file) break;

            Point3DRGB pt;
            int offset = 0;
            for (size_t j = 0; j < header.fields.size(); ++j) {
                if (j == static_cast<size_t>(xIdx)) {
                    memcpy(&pt.x, buffer.data() + offset, sizeof(float));
                } else if (j == static_cast<size_t>(yIdx)) {
                    memcpy(&pt.y, buffer.data() + offset, sizeof(float));
                } else if (j == static_cast<size_t>(zIdx)) {
                    memcpy(&pt.z, buffer.data() + offset, sizeof(float));
                } else if (j == static_cast<size_t>(rgbIdx)) {
                    // RGB 通常存储为 packed float
                    float rgbFloat;
                    memcpy(&rgbFloat, buffer.data() + offset, sizeof(float));
                    uint32_t rgb = *reinterpret_cast<uint32_t*>(&rgbFloat);
                    pt.r = (rgb >> 16) & 0xFF;
                    pt.g = (rgb >> 8) & 0xFF;
                    pt.b = rgb & 0xFF;
                }
                offset += header.fieldSizes[j];
            }

            if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
                cloud.push_back(pt);
            }
        }
    } else {
        std::string line;
        while (std::getline(file, line) && cloud.size() < static_cast<size_t>(numPoints)) {
            std::istringstream iss(line);
            std::vector<float> values;
            float val;
            while (iss >> val) {
                values.push_back(val);
            }

            if (values.size() >= 3 &&
                static_cast<size_t>(xIdx) < values.size() &&
                static_cast<size_t>(yIdx) < values.size() &&
                static_cast<size_t>(zIdx) < values.size()) {
                Point3DRGB pt;
                pt.x = values[xIdx];
                pt.y = values[yIdx];
                pt.z = values[zIdx];

                if (rgbIdx >= 0 && static_cast<size_t>(rgbIdx) < values.size()) {
                    float rgbFloat = values[rgbIdx];
                    uint32_t rgb = *reinterpret_cast<uint32_t*>(&rgbFloat);
                    pt.r = (rgb >> 16) & 0xFF;
                    pt.g = (rgb >> 8) & 0xFF;
                    pt.b = rgb & 0xFF;
                }

                if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
                    cloud.push_back(pt);
                }
            }
        }
    }

    m_loadedPointCount = cloud.size();
    m_loadedLineCount = 0;  // PCD 文件没有线信息
    m_lastLoadHadColor = (rgbIdx >= 0);
    return 0;
}

int PointCloudConverter::loadFromFile(const std::string& fileName, PointCloudXYZ& cloud)
{
    std::string ext = getFileExtension(fileName);

    if (ext == "pcd") {
        return loadFromPcd(fileName, cloud);
    } else if (ext == "txt") {
        return loadFromTxt(fileName, cloud);
    } else if (ext == "ply") {
        return loadFromPly(fileName, cloud);
    } else {
        m_lastError = "不支持的文件格式: " + ext;
        return -1;
    }
}

int PointCloudConverter::loadFromFile(const std::string& fileName, PointCloudXYZRGB& cloud)
{
    std::string ext = getFileExtension(fileName);

    if (ext == "pcd") {
        return loadFromPcd(fileName, cloud);
    } else if (ext == "txt") {
        return loadFromTxt(fileName, cloud);
    } else if (ext == "ply") {
        return loadFromPly(fileName, cloud);
    } else {
        m_lastError = "不支持的文件格式: " + ext;
        return -1;
    }
}

int PointCloudConverter::saveToTxt(const std::string& fileName, const PointCloudXYZ& cloud, int lineNum, int linePtNum)
{
    if (cloud.empty()) {
        m_lastError = "点云数据为空";
        return -1;
    }

    // 转换为 std::vector<std::vector<SVzNL3DPosition>> 格式
    std::vector<std::vector<SVzNL3DPosition>> xyzData;
    xyzData.resize(lineNum);

    for (int line = 0; line < lineNum; ++line) {
        xyzData[line].resize(linePtNum);
    }

    // 填充数据
    size_t ptIdx = 0;
    for (int line = 0; line < lineNum; ++line) {
        for (int j = 0; j < linePtNum; ++j) {
            if (ptIdx < cloud.points.size()) {
                xyzData[line][j].pt3D.x = cloud.points[ptIdx].x;
                xyzData[line][j].pt3D.y = cloud.points[ptIdx].y;
                xyzData[line][j].pt3D.z = cloud.points[ptIdx].z;
                xyzData[line][j].nPointIdx = j;
                ptIdx++;
            } else {
                // 填充零点
                xyzData[line][j].pt3D.x = 0;
                xyzData[line][j].pt3D.y = 0;
                xyzData[line][j].pt3D.z = 0;
                xyzData[line][j].nPointIdx = j;
            }
        }
    }

    // 使用 LaserDataLoader 保存
    LaserDataLoader loader;
    int result = loader.DebugSaveLaser(fileName, xyzData);
    if (result != 0) {
        m_lastError = "保存文件失败: " + loader.GetLastError();
        return result;
    }

    LOG_INFO("[CloudView] Saved %zu points to %s (lineNum=%d, linePtNum=%d)\n",
             cloud.points.size(), fileName.c_str(), lineNum, linePtNum);
    return 0;
}

int PointCloudConverter::getPlyTypeByteSize(const std::string& type)
{
    if (type == "char" || type == "int8")    return 1;
    if (type == "uchar" || type == "uint8")  return 1;
    if (type == "short" || type == "int16")  return 2;
    if (type == "ushort" || type == "uint16") return 2;
    if (type == "int" || type == "int32")    return 4;
    if (type == "uint" || type == "uint32")  return 4;
    if (type == "float" || type == "float32") return 4;
    if (type == "double" || type == "float64") return 8;
    return 0;
}

int PointCloudConverter::calcPlyElementStride(const PlyElement& element)
{
    int stride = 0;
    for (const auto& prop : element.properties) {
        if (prop.isList) return -1;  // list 类型无法计算固定 stride
        stride += prop.byteSize;
    }
    return stride;
}

bool PointCloudConverter::parsePlyHeader(std::ifstream& file, PlyHeader& header)
{
    std::string line;

    // 第一行必须是 "ply"
    if (!std::getline(file, line)) return false;
    // 去除行尾可能的 \r
    if (!line.empty() && line.back() == '\r') line.pop_back();
    if (line != "ply") return false;

    PlyElement* currentElement = nullptr;

    while (std::getline(file, line)) {
        // 去除行尾 \r
        if (!line.empty() && line.back() == '\r') line.pop_back();
        if (line.empty()) continue;

        std::istringstream iss(line);
        std::string keyword;
        iss >> keyword;

        if (keyword == "format") {
            std::string fmt;
            iss >> fmt;
            if (fmt == "ascii")                   header.format = PlyHeader::ASCII;
            else if (fmt == "binary_little_endian") header.format = PlyHeader::BINARY_LE;
            else if (fmt == "binary_big_endian")    header.format = PlyHeader::BINARY_BE;
            else return false;
        } else if (keyword == "comment" || keyword == "obj_info") {
            // 跳过注释
        } else if (keyword == "element") {
            PlyElement elem;
            iss >> elem.name >> elem.count;
            header.elements.push_back(elem);
            currentElement = &header.elements.back();
            if (elem.name == "vertex") {
                header.vertexElementIndex = static_cast<int>(header.elements.size()) - 1;
            }
        } else if (keyword == "property") {
            if (!currentElement) return false;
            std::string secondToken;
            iss >> secondToken;

            PlyProperty prop;
            if (secondToken == "list") {
                // property list <count_type> <value_type> <name>
                prop.isList = true;
                iss >> prop.listCountType >> prop.listValueType >> prop.name;
                prop.byteSize = 0;  // list 类型没有固定大小
            } else {
                // property <type> <name>
                prop.type = secondToken;
                iss >> prop.name;
                prop.byteSize = getPlyTypeByteSize(prop.type);
                prop.isList = false;
            }
            currentElement->properties.push_back(prop);
        } else if (keyword == "end_header") {
            return true;
        }
    }
    return false;
}

/**
 * @brief 从二进制缓冲区读取指定 PLY 类型的值并转为 float
 */
static float readPlyValueAsFloat(const char* data, const std::string& type, bool swapEndian)
{
    if (type == "float" || type == "float32") {
        float v;
        memcpy(&v, data, sizeof(float));
        if (swapEndian) {
            char* p = reinterpret_cast<char*>(&v);
            std::swap(p[0], p[3]);
            std::swap(p[1], p[2]);
        }
        return v;
    } else if (type == "double" || type == "float64") {
        double v;
        memcpy(&v, data, sizeof(double));
        if (swapEndian) {
            char* p = reinterpret_cast<char*>(&v);
            std::swap(p[0], p[7]);
            std::swap(p[1], p[6]);
            std::swap(p[2], p[5]);
            std::swap(p[3], p[4]);
        }
        return static_cast<float>(v);
    } else if (type == "int" || type == "int32") {
        int32_t v;
        memcpy(&v, data, sizeof(int32_t));
        if (swapEndian) {
            char* p = reinterpret_cast<char*>(&v);
            std::swap(p[0], p[3]);
            std::swap(p[1], p[2]);
        }
        return static_cast<float>(v);
    } else if (type == "uint" || type == "uint32") {
        uint32_t v;
        memcpy(&v, data, sizeof(uint32_t));
        if (swapEndian) {
            char* p = reinterpret_cast<char*>(&v);
            std::swap(p[0], p[3]);
            std::swap(p[1], p[2]);
        }
        return static_cast<float>(v);
    } else if (type == "short" || type == "int16") {
        int16_t v;
        memcpy(&v, data, sizeof(int16_t));
        if (swapEndian) {
            char* p = reinterpret_cast<char*>(&v);
            std::swap(p[0], p[1]);
        }
        return static_cast<float>(v);
    } else if (type == "ushort" || type == "uint16") {
        uint16_t v;
        memcpy(&v, data, sizeof(uint16_t));
        if (swapEndian) {
            char* p = reinterpret_cast<char*>(&v);
            std::swap(p[0], p[1]);
        }
        return static_cast<float>(v);
    } else if (type == "uchar" || type == "uint8") {
        uint8_t v;
        memcpy(&v, data, sizeof(uint8_t));
        return static_cast<float>(v);
    } else if (type == "char" || type == "int8") {
        int8_t v;
        memcpy(&v, data, sizeof(int8_t));
        return static_cast<float>(v);
    }
    return 0.0f;
}

/**
 * @brief 从二进制缓冲区读取指定 PLY 类型的值并转为 uint8_t（用于颜色）
 */
static uint8_t readPlyValueAsUint8(const char* data, const std::string& type, bool swapEndian)
{
    float v = readPlyValueAsFloat(data, type, swapEndian);
    // 如果是 float/double 类型的颜色（0~1范围），需要映射到 0~255
    if (type == "float" || type == "float32" || type == "double" || type == "float64") {
        if (v >= 0.0f && v <= 1.0f) {
            return static_cast<uint8_t>(v * 255.0f + 0.5f);
        }
    }
    // 截断到 0~255
    if (v < 0.0f) return 0;
    if (v > 255.0f) return 255;
    return static_cast<uint8_t>(v);
}

int PointCloudConverter::loadFromPly(const std::string& fileName, PointCloudXYZ& cloud)
{
    std::ifstream file(fileName, std::ios::binary);
    if (!file.is_open()) {
        m_lastError = "无法打开文件: " + fileName;
        return -1;
    }

    PlyHeader header;
    if (!parsePlyHeader(file, header)) {
        m_lastError = "无法解析 PLY 文件头";
        return -1;
    }

    if (header.vertexElementIndex < 0) {
        m_lastError = "PLY 文件中未找到 vertex 元素";
        return -1;
    }

    const PlyElement& vertexElem = header.elements[header.vertexElementIndex];
    int numPoints = vertexElem.count;

    // 查找 x, y, z 属性索引
    int xIdx = -1, yIdx = -1, zIdx = -1;
    for (size_t i = 0; i < vertexElem.properties.size(); ++i) {
        const std::string& name = vertexElem.properties[i].name;
        if (name == "x") xIdx = static_cast<int>(i);
        else if (name == "y") yIdx = static_cast<int>(i);
        else if (name == "z") zIdx = static_cast<int>(i);
    }

    if (xIdx < 0 || yIdx < 0 || zIdx < 0) {
        m_lastError = "PLY 文件缺少 x, y, z 属性";
        return -1;
    }

    LOG_INFO("[CloudView] PLY header: vertex count=%d, format=%d\n", numPoints, header.format);

    cloud.clear();
    cloud.reserve(numPoints);

    bool swapEndian = (header.format == PlyHeader::BINARY_BE);

    if (header.format == PlyHeader::ASCII) {
        // 跳过 vertex 之前的元素
        for (int ei = 0; ei < header.vertexElementIndex; ++ei) {
            const PlyElement& elem = header.elements[ei];
            for (int j = 0; j < elem.count; ++j) {
                std::string skipLine;
                if (!std::getline(file, skipLine)) {
                    m_lastError = "PLY 文件数据不完整（跳过元素时）";
                    return -1;
                }
            }
        }

        // 读取 vertex 数据
        std::string line;
        for (int i = 0; i < numPoints && std::getline(file, line); ++i) {
            // 去除 \r
            if (!line.empty() && line.back() == '\r') line.pop_back();
            std::istringstream iss(line);
            std::vector<float> values;
            float val;
            while (iss >> val) {
                values.push_back(val);
            }

            if (static_cast<size_t>(xIdx) < values.size() &&
                static_cast<size_t>(yIdx) < values.size() &&
                static_cast<size_t>(zIdx) < values.size()) {
                Point3D pt(values[xIdx], values[yIdx], values[zIdx]);
                if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
                    cloud.push_back(pt);
                }
            }
        }
    } else {
        // Binary 格式（Little-Endian 或 Big-Endian）
        // 跳过 vertex 之前的元素
        for (int ei = 0; ei < header.vertexElementIndex; ++ei) {
            const PlyElement& elem = header.elements[ei];
            int stride = calcPlyElementStride(elem);
            if (stride > 0) {
                // 固定大小元素，直接跳过
                file.seekg(static_cast<std::streamoff>(stride) * elem.count, std::ios::cur);
            } else {
                // 含 list 类型，需要逐条跳过
                for (int j = 0; j < elem.count; ++j) {
                    for (const auto& prop : elem.properties) {
                        if (prop.isList) {
                            int countSize = getPlyTypeByteSize(prop.listCountType);
                            char countBuf[8] = {};
                            file.read(countBuf, countSize);
                            // 读取 list 计数值
                            uint32_t listCount = 0;
                            if (countSize == 1) listCount = static_cast<uint8_t>(countBuf[0]);
                            else if (countSize == 2) { uint16_t v; memcpy(&v, countBuf, 2); listCount = v; }
                            else if (countSize == 4) { uint32_t v; memcpy(&v, countBuf, 4); listCount = v; }
                            int valueSize = getPlyTypeByteSize(prop.listValueType);
                            file.seekg(static_cast<std::streamoff>(valueSize) * listCount, std::ios::cur);
                        } else {
                            file.seekg(prop.byteSize, std::ios::cur);
                        }
                    }
                }
            }
        }

        // 计算 vertex 的 stride
        int vertexStride = calcPlyElementStride(vertexElem);
        if (vertexStride <= 0) {
            m_lastError = "PLY vertex 元素包含 list 属性，不支持";
            return -1;
        }

        // 计算各属性在 vertex 记录中的偏移
        std::vector<int> offsets(vertexElem.properties.size(), 0);
        int offset = 0;
        for (size_t i = 0; i < vertexElem.properties.size(); ++i) {
            offsets[i] = offset;
            offset += vertexElem.properties[i].byteSize;
        }

        // 读取所有 vertex 数据
        std::vector<char> buffer(static_cast<size_t>(vertexStride) * numPoints);
        file.read(buffer.data(), buffer.size());
        if (!file) {
            m_lastError = "PLY 文件数据不完整";
            return -1;
        }

        for (int i = 0; i < numPoints; ++i) {
            const char* record = buffer.data() + static_cast<size_t>(i) * vertexStride;
            float x = readPlyValueAsFloat(record + offsets[xIdx], vertexElem.properties[xIdx].type, swapEndian);
            float y = readPlyValueAsFloat(record + offsets[yIdx], vertexElem.properties[yIdx].type, swapEndian);
            float z = readPlyValueAsFloat(record + offsets[zIdx], vertexElem.properties[zIdx].type, swapEndian);

            if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) {
                cloud.push_back(Point3D(x, y, z));
            }
        }
    }

    LOG_INFO("[CloudView] Loaded %zu points from PLY\n", cloud.size());
    m_loadedPointCount = cloud.size();
    m_loadedLineCount = 0;  // PLY 文件没有线信息
    return 0;
}

int PointCloudConverter::loadFromPly(const std::string& fileName, PointCloudXYZRGB& cloud)
{
    std::ifstream file(fileName, std::ios::binary);
    if (!file.is_open()) {
        m_lastError = "无法打开文件: " + fileName;
        return -1;
    }

    PlyHeader header;
    if (!parsePlyHeader(file, header)) {
        m_lastError = "无法解析 PLY 文件头";
        return -1;
    }

    if (header.vertexElementIndex < 0) {
        m_lastError = "PLY 文件中未找到 vertex 元素";
        return -1;
    }

    const PlyElement& vertexElem = header.elements[header.vertexElementIndex];
    int numPoints = vertexElem.count;

    // 查找属性索引
    int xIdx = -1, yIdx = -1, zIdx = -1;
    int rIdx = -1, gIdx = -1, bIdx = -1;
    for (size_t i = 0; i < vertexElem.properties.size(); ++i) {
        const std::string& name = vertexElem.properties[i].name;
        if (name == "x") xIdx = static_cast<int>(i);
        else if (name == "y") yIdx = static_cast<int>(i);
        else if (name == "z") zIdx = static_cast<int>(i);
        else if (name == "red" || name == "r") rIdx = static_cast<int>(i);
        else if (name == "green" || name == "g") gIdx = static_cast<int>(i);
        else if (name == "blue" || name == "b") bIdx = static_cast<int>(i);
    }

    if (xIdx < 0 || yIdx < 0 || zIdx < 0) {
        m_lastError = "PLY 文件缺少 x, y, z 属性";
        return -1;
    }

    bool hasColor = (rIdx >= 0 && gIdx >= 0 && bIdx >= 0);

    LOG_INFO("[CloudView] PLY header(XYZRGB): vertex count=%d, format=%d, hasColor=%d\n",
             numPoints, header.format, hasColor);

    cloud.clear();
    cloud.reserve(numPoints);

    bool swapEndian = (header.format == PlyHeader::BINARY_BE);

    if (header.format == PlyHeader::ASCII) {
        // 跳过 vertex 之前的元素
        for (int ei = 0; ei < header.vertexElementIndex; ++ei) {
            const PlyElement& elem = header.elements[ei];
            for (int j = 0; j < elem.count; ++j) {
                std::string skipLine;
                if (!std::getline(file, skipLine)) {
                    m_lastError = "PLY 文件数据不完整（跳过元素时）";
                    return -1;
                }
            }
        }

        // 读取 vertex 数据
        std::string line;
        for (int i = 0; i < numPoints && std::getline(file, line); ++i) {
            if (!line.empty() && line.back() == '\r') line.pop_back();
            std::istringstream iss(line);
            std::vector<float> values;
            float val;
            while (iss >> val) {
                values.push_back(val);
            }

            if (static_cast<size_t>(xIdx) < values.size() &&
                static_cast<size_t>(yIdx) < values.size() &&
                static_cast<size_t>(zIdx) < values.size()) {
                Point3DRGB pt;
                pt.x = values[xIdx];
                pt.y = values[yIdx];
                pt.z = values[zIdx];

                if (hasColor &&
                    static_cast<size_t>(rIdx) < values.size() &&
                    static_cast<size_t>(gIdx) < values.size() &&
                    static_cast<size_t>(bIdx) < values.size()) {
                    pt.r = static_cast<uint8_t>(std::min(255.0f, std::max(0.0f, values[rIdx])));
                    pt.g = static_cast<uint8_t>(std::min(255.0f, std::max(0.0f, values[gIdx])));
                    pt.b = static_cast<uint8_t>(std::min(255.0f, std::max(0.0f, values[bIdx])));
                }

                if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
                    cloud.push_back(pt);
                }
            }
        }
    } else {
        // Binary 格式
        // 跳过 vertex 之前的元素
        for (int ei = 0; ei < header.vertexElementIndex; ++ei) {
            const PlyElement& elem = header.elements[ei];
            int stride = calcPlyElementStride(elem);
            if (stride > 0) {
                file.seekg(static_cast<std::streamoff>(stride) * elem.count, std::ios::cur);
            } else {
                for (int j = 0; j < elem.count; ++j) {
                    for (const auto& prop : elem.properties) {
                        if (prop.isList) {
                            int countSize = getPlyTypeByteSize(prop.listCountType);
                            char countBuf[8] = {};
                            file.read(countBuf, countSize);
                            uint32_t listCount = 0;
                            if (countSize == 1) listCount = static_cast<uint8_t>(countBuf[0]);
                            else if (countSize == 2) { uint16_t v; memcpy(&v, countBuf, 2); listCount = v; }
                            else if (countSize == 4) { uint32_t v; memcpy(&v, countBuf, 4); listCount = v; }
                            int valueSize = getPlyTypeByteSize(prop.listValueType);
                            file.seekg(static_cast<std::streamoff>(valueSize) * listCount, std::ios::cur);
                        } else {
                            file.seekg(prop.byteSize, std::ios::cur);
                        }
                    }
                }
            }
        }

        // 计算 vertex 的 stride
        int vertexStride = calcPlyElementStride(vertexElem);
        if (vertexStride <= 0) {
            m_lastError = "PLY vertex 元素包含 list 属性，不支持";
            return -1;
        }

        // 计算各属性偏移
        std::vector<int> offsets(vertexElem.properties.size(), 0);
        int offset = 0;
        for (size_t i = 0; i < vertexElem.properties.size(); ++i) {
            offsets[i] = offset;
            offset += vertexElem.properties[i].byteSize;
        }

        // 读取所有 vertex 数据
        std::vector<char> buffer(static_cast<size_t>(vertexStride) * numPoints);
        file.read(buffer.data(), buffer.size());
        if (!file) {
            m_lastError = "PLY 文件数据不完整";
            return -1;
        }

        for (int i = 0; i < numPoints; ++i) {
            const char* record = buffer.data() + static_cast<size_t>(i) * vertexStride;

            Point3DRGB pt;
            pt.x = readPlyValueAsFloat(record + offsets[xIdx], vertexElem.properties[xIdx].type, swapEndian);
            pt.y = readPlyValueAsFloat(record + offsets[yIdx], vertexElem.properties[yIdx].type, swapEndian);
            pt.z = readPlyValueAsFloat(record + offsets[zIdx], vertexElem.properties[zIdx].type, swapEndian);

            if (hasColor) {
                pt.r = readPlyValueAsUint8(record + offsets[rIdx], vertexElem.properties[rIdx].type, swapEndian);
                pt.g = readPlyValueAsUint8(record + offsets[gIdx], vertexElem.properties[gIdx].type, swapEndian);
                pt.b = readPlyValueAsUint8(record + offsets[bIdx], vertexElem.properties[bIdx].type, swapEndian);
            }

            if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) {
                cloud.push_back(pt);
            }
        }
    }

    LOG_INFO("[CloudView] Loaded %zu points from PLY (hasColor=%d)\n", cloud.size(), hasColor);
    m_loadedPointCount = cloud.size();
    m_loadedLineCount = 0;  // PLY 文件没有线信息
    m_lastLoadHadColor = hasColor;
    return 0;
}

int PointCloudConverter::rotateCloud(const PointCloudXYZ& cloud, PointCloudXYZ& rotatedCloud,
                                      int lineNum, int linePtNum, int& newLineNum, int& newLinePtNum)
{
    if (cloud.empty()) {
        m_lastError = "无效的点云数据";
        return -1;
    }

    if (lineNum <= 0 || linePtNum <= 0) {
        m_lastError = "无效的线信息";
        return -1;
    }

    // 检查点数是否匹配
    size_t expectedPoints = static_cast<size_t>(lineNum) * static_cast<size_t>(linePtNum);
    if (cloud.points.size() != expectedPoints) {
        m_lastError = "点云数据与线信息不匹配，无法旋转";
        return -1;
    }

    // 矩阵转置：原来的行列互换
    // 原来: lineNum 条线，每条线 linePtNum 个点
    // 转置后: linePtNum 条线，每条线 lineNum 个点
    newLineNum = linePtNum;
    newLinePtNum = lineNum;

    rotatedCloud.clear();
    rotatedCloud.reserve(cloud.points.size());

    // 转置操作：
    // 原来第 line 条线的第 col 个点 -> 新的第 col 条线的第 line 个点
    // 原索引: line * linePtNum + col
    // 新索引: col * lineNum + line
    for (int newLine = 0; newLine < newLineNum; ++newLine) {
        for (int newCol = 0; newCol < newLinePtNum; ++newCol) {
            // newLine 对应原来的 col（点在线内的位置）
            // newCol 对应原来的 line（线号）
            int oldLine = newCol;
            int oldCol = newLine;
            size_t oldIdx = static_cast<size_t>(oldLine) * static_cast<size_t>(linePtNum) + static_cast<size_t>(oldCol);

            const Point3D& pt = cloud.points[oldIdx];
            rotatedCloud.push_back(pt, newLine);
        }
    }

    LOG_INFO("[CloudView] Rotated cloud: %zu points, %d lines -> %d lines\n",
             rotatedCloud.points.size(), lineNum, newLineNum);
    return 0;
}