#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)
{
}

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)
{
    PointCloudXYZ xyzCloud;
    int result = loadFromTxt(fileName, xyzCloud);
    if (result != 0) {
        return result;
    }

    cloud.clear();
    cloud.reserve(xyzCloud.size());

    for (size_t i = 0; i < xyzCloud.points.size(); ++i) {
        const auto& pt = xyzCloud.points[i];
        Point3DRGB point(pt.x, pt.y, pt.z, 255, 255, 255);
        int lineIdx = (i < xyzCloud.lineIndices.size()) ? xyzCloud.lineIndices[i] : 0;
        cloud.push_back(point, lineIdx);
    }

    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 文件没有线信息
    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 {
        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 {
        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::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;
}