Utils/CloudView/Src/PointCloudGLWidget.cpp

#include "PointCloudGLWidget.h"
#include <QDebug>
#include <QtMath>
#include <cfloat>
#include "VrLog.h"

// OpenGL/GLU 头文件
#ifdef _WIN32
#include <windows.h>
#include <GL/gl.h>
#include <GL/glu.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#endif

PointCloudGLWidget::PointCloudGLWidget(QWidget* parent)
    : QOpenGLWidget(parent)
    , m_distance(100.0f)
    , m_rotationX(0.0f)
    , m_rotationY(0.0f)
    , m_rotationZ(0.0f)
    , m_center(0, 0, 0)
    , m_pan(0, 0, 0)
    , m_minBound(-50, -50, -50)
    , m_maxBound(50, 50, 50)
    , m_leftButtonPressed(false)
    , m_rightButtonPressed(false)
    , m_middleButtonPressed(false)
    , m_currentColor(PointCloudColor::White)
    , m_pointSize(1.0f)
    , m_lineSelectMode(LineSelectMode::Vertical)
    , m_eulerRotationOrder(EulerRotationOrder::ZYX)
    , m_measureDistanceEnabled(false)
    , m_hasListHighlightPoint(false)
    , m_colorIndex(0)
{
    setFocusPolicy(Qt::StrongFocus);
    setMouseTracking(true);

    // 确保 m_model 是单位矩阵
    m_model.setToIdentity();
}

PointCloudGLWidget::~PointCloudGLWidget()
{
    // 在 GL 上下文中释放所有 VBO
    makeCurrent();
    for (auto& cloudData : m_pointClouds) {
        releaseVBO(cloudData);
    }
    doneCurrent();
}

void PointCloudGLWidget::uploadToVBO(PointCloudData& data)
{
    // 先释放旧的 VBO
    releaseVBO(data);

    if (data.vertices.empty()) {
        return;
    }

    // 创建并上传顶点 VBO
    data.vertexBuffer.create();
    data.vertexBuffer.bind();
    data.vertexBuffer.setUsagePattern(QOpenGLBuffer::StaticDraw);
    data.vertexBuffer.allocate(data.vertices.data(),
                               static_cast<int>(data.vertices.size() * sizeof(float)));
    data.vertexBuffer.release();

    // 如果有颜色数据，创建并上传颜色 VBO
    if (data.hasColor && !data.colors.empty()) {
        data.colorBuffer.create();
        data.colorBuffer.bind();
        data.colorBuffer.setUsagePattern(QOpenGLBuffer::StaticDraw);
        data.colorBuffer.allocate(data.colors.data(),
                                   static_cast<int>(data.colors.size() * sizeof(float)));
        data.colorBuffer.release();
    }

    data.vboCreated = true;
}

void PointCloudGLWidget::releaseVBO(PointCloudData& data)
{
    if (!data.vboCreated) {
        return;
    }

    if (data.vertexBuffer.isCreated()) {
        data.vertexBuffer.destroy();
    }
    if (data.colorBuffer.isCreated()) {
        data.colorBuffer.destroy();
    }

    data.vboCreated = false;
}

void PointCloudGLWidget::initializeGL()
{
    initializeOpenGLFunctions();

    LOG_INFO("[CloudView] OpenGL initialized, version: %s\n", reinterpret_cast<const char*>(glGetString(GL_VERSION)));

    glClearColor(0.15f, 0.15f, 0.15f, 1.0f);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_POINT_SMOOTH);
    glHint(GL_POINT_SMOOTH_HINT, GL_NICEST);
}

void PointCloudGLWidget::resizeGL(int w, int h)
{
    glViewport(0, 0, w, h);

    m_projection.setToIdentity();
    float aspect = static_cast<float>(w) / static_cast<float>(h > 0 ? h : 1);
    m_projection.perspective(45.0f, aspect, 0.1f, 10000.0f);
}

void PointCloudGLWidget::paintGL()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    m_view.setToIdentity();
    // 平移在相机空间中进行，使鼠标拖动方向与屏幕方向一致
    m_view.translate(-m_pan.x(), -m_pan.y(), -m_distance);

    // 使用轨迹球旋转方式：先Y后X后Z，这样更直观
    m_view.rotate(m_rotationY, 0, 1, 0);
    m_view.rotate(m_rotationX, 1, 0, 0);
    m_view.rotate(m_rotationZ, 0, 0, 1);

    m_view.translate(-m_center);

    // 调试输出：每100帧输出一次当前旋转角度
    static int frameCount = 0;
    if (++frameCount % 100 == 0) {
        LOG_INFO("[CloudView] Current rotation: rotX=%.1f, rotY=%.1f, rotZ=%.1f\n",
                 m_rotationX, m_rotationY, m_rotationZ);
    }

    glMatrixMode(GL_PROJECTION);
    glLoadMatrixf(m_projection.constData());

    glMatrixMode(GL_MODELVIEW);
    QMatrix4x4 modelView = m_view * m_model;
    glLoadMatrixf(modelView.constData());

    // 确保颜色数组禁用
    glDisableClientState(GL_COLOR_ARRAY);

    glPointSize(m_pointSize);

    for (size_t cloudIdx = 0; cloudIdx < m_pointClouds.size(); ++cloudIdx) {
        auto& cloudData = m_pointClouds[cloudIdx];
        if (cloudData.vertices.empty()) {
            continue;
        }

        // 确保 VBO 已创建（延迟上传，因为 addPointCloud 可能在 GL 上下文外调用）
        if (!cloudData.vboCreated) {
            uploadToVBO(cloudData);
        }

        glEnableClientState(GL_VERTEX_ARRAY);

        if (cloudData.vboCreated && cloudData.vertexBuffer.isCreated()) {
            // 使用 VBO 渲染：数据已在 GPU 显存中，无需每帧 CPU→GPU 拷贝
            cloudData.vertexBuffer.bind();
            glVertexPointer(3, GL_FLOAT, 0, nullptr);
            cloudData.vertexBuffer.release();
        } else {
            // 回退到 CPU 指针（VBO 创建失败时）
            glVertexPointer(3, GL_FLOAT, 0, cloudData.vertices.data());
        }

        // 如果有原始颜色且选择显示原始颜色
        if (cloudData.hasColor && !cloudData.colors.empty()) {
            glEnableClientState(GL_COLOR_ARRAY);
            if (cloudData.vboCreated && cloudData.colorBuffer.isCreated()) {
                cloudData.colorBuffer.bind();
                glColorPointer(3, GL_FLOAT, 0, nullptr);
                cloudData.colorBuffer.release();
            } else {
                glColorPointer(3, GL_FLOAT, 0, cloudData.colors.data());
            }
        } else {
            // 使用点云自己的颜色索引
            glDisableClientState(GL_COLOR_ARRAY);
            setColorByIndex(cloudData.colorIndex);
        }

        glDrawArrays(GL_POINTS, 0, static_cast<GLsizei>(cloudData.vertices.size() / 3));

        glDisableClientState(GL_VERTEX_ARRAY);
        glDisableClientState(GL_COLOR_ARRAY);
    }

    drawSelectedPoints();
    drawMeasurementLine();
    drawSelectedLine();
    drawLineSegments();
    drawPosePoints();

    // 最后绘制坐标系指示器（覆盖在所有内容之上）
    drawAxis();
}

void PointCloudGLWidget::addPointCloud(const PointCloudXYZ& cloud, const QString& name)
{
    LOG_INFO("[CloudView] addPointCloud called, cloud size: %zu\n", cloud.size());

    if (cloud.empty()) {
        LOG_WARN("[CloudView] Cloud is empty, returning\n");
        return;
    }

    PointCloudData data;
    data.name = name;
    data.hasColor = false;
    data.hasLineInfo = !cloud.lineIndices.empty();
    data.colorIndex = m_colorIndex;  // 分配颜色索引
    m_colorIndex = (m_colorIndex + 1) % COLOR_COUNT;  // 轮换到下一个颜色
    data.vertices.reserve(cloud.size() * 3);
    data.totalLines = 0;
    data.pointsPerLine = 0;

    const float EPSILON = 1e-6f;
    for (size_t i = 0; i < cloud.points.size(); ++i) {
        const auto& pt = cloud.points[i];
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            continue;
        }
        // 显示时过滤 (0,0,0) 点
        if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
            continue;
        }
        data.vertices.push_back(pt.x);
        data.vertices.push_back(pt.y);
        data.vertices.push_back(pt.z);

        // 保存原始索引
        data.originalIndices.push_back(static_cast<int>(i));

        // 保存线索引
        if (data.hasLineInfo && i < cloud.lineIndices.size()) {
            int lineIdx = cloud.lineIndices[i];
            data.lineIndices.push_back(lineIdx);
            if (lineIdx + 1 > data.totalLines) {
                data.totalLines = lineIdx + 1;
            }
        }
    }

    // 计算每线点数（假设网格化点云）
    if (data.totalLines > 0) {
        data.pointsPerLine = static_cast<int>(cloud.size()) / data.totalLines;
    }

    LOG_INFO("[CloudView] Valid vertices count: %zu, colorIndex: %d, totalLines: %d, pointsPerLine: %d\n",
             data.vertices.size() / 3, data.colorIndex, data.totalLines, data.pointsPerLine);

    m_pointClouds.push_back(std::move(data));

    // 上传 VBO（如果当前在 GL 上下文中）
    makeCurrent();
    uploadToVBO(m_pointClouds.back());
    doneCurrent();

    computeBoundingBox();
    LOG_INFO("[CloudView] BoundingBox min: (%.3f, %.3f, %.3f) max: (%.3f, %.3f, %.3f)\n",
             m_minBound.x(), m_minBound.y(), m_minBound.z(),
             m_maxBound.x(), m_maxBound.y(), m_maxBound.z());
    LOG_INFO("[CloudView] Center: (%.3f, %.3f, %.3f) Distance: %.3f\n",
             m_center.x(), m_center.y(), m_center.z(), m_distance);

    resetView();
    update();
}

void PointCloudGLWidget::addPointCloud(const PointCloudXYZRGB& cloud, const QString& name)
{
    if (cloud.empty()) {
        return;
    }

    PointCloudData data;
    data.name = name;
    data.hasColor = true;
    data.hasLineInfo = !cloud.lineIndices.empty();
    data.colorIndex = m_colorIndex;  // 即使有颜色也分配索引（备用）
    m_colorIndex = (m_colorIndex + 1) % COLOR_COUNT;
    data.vertices.reserve(cloud.size() * 3);
    data.colors.reserve(cloud.size() * 3);
    data.totalLines = 0;
    data.pointsPerLine = 0;

    for (size_t i = 0; i < cloud.points.size(); ++i) {
        const auto& pt = cloud.points[i];
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            continue;
        }
        data.vertices.push_back(pt.x);
        data.vertices.push_back(pt.y);
        data.vertices.push_back(pt.z);

        data.colors.push_back(pt.r / 255.0f);
        data.colors.push_back(pt.g / 255.0f);
        data.colors.push_back(pt.b / 255.0f);

        // 保存原始索引
        data.originalIndices.push_back(static_cast<int>(i));

        // 保存线索引
        if (data.hasLineInfo && i < cloud.lineIndices.size()) {
            int lineIdx = cloud.lineIndices[i];
            data.lineIndices.push_back(lineIdx);
            if (lineIdx + 1 > data.totalLines) {
                data.totalLines = lineIdx + 1;
            }
        }
    }

    // 计算每线点数
    if (data.totalLines > 0) {
        data.pointsPerLine = static_cast<int>(cloud.size()) / data.totalLines;
    }

    m_pointClouds.push_back(std::move(data));

    // 上传 VBO
    makeCurrent();
    uploadToVBO(m_pointClouds.back());
    doneCurrent();

    computeBoundingBox();
    resetView();
    update();
}

void PointCloudGLWidget::clearPointClouds()
{
    // 释放所有 VBO
    makeCurrent();
    for (auto& cloudData : m_pointClouds) {
        releaseVBO(cloudData);
    }
    doneCurrent();

    m_pointClouds.clear();
    m_selectedPoints.clear();
    m_selectedLine = SelectedLineInfo();
    m_lineSegments.clear();
    m_posePoints.clear();
    m_minBound = QVector3D(-50, -50, -50);
    m_maxBound = QVector3D(50, 50, 50);
    m_center = QVector3D(0, 0, 0);
    m_colorIndex = 0;  // 重置颜色索引
    resetView();
    update();
}

void PointCloudGLWidget::setPointCloudColor(PointCloudColor color)
{
    m_currentColor = color;
    update();
}

void PointCloudGLWidget::setPointSize(float size)
{
    m_pointSize = qBound(1.0f, size, 10.0f);
    update();
}

void PointCloudGLWidget::resetView()
{
    QVector3D size = m_maxBound - m_minBound;
    float maxSize = qMax(qMax(size.x(), size.y()), size.z());

    // 确保最小视距
    m_distance = qMax(maxSize * 2.0f, 10.0f);
    // 默认视角：右手坐标系，X轴朝右，Y轴朝上，Z轴朝向观察者
    m_rotationX = 0.0f;
    m_rotationY = 0.0f;
    m_rotationZ = 0.0f;
    m_pan = QVector3D(0, 0, 0);

    LOG_INFO("[CloudView] resetView: size=(%.3f, %.3f, %.3f) maxSize=%.3f distance=%.3f\n",
             size.x(), size.y(), size.z(), maxSize, m_distance);

    emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ);
    update();
}

void PointCloudGLWidget::setViewAngles(float rotX, float rotY, float rotZ)
{
    m_rotationX = rotX;
    m_rotationY = rotY;
    m_rotationZ = rotZ;
    m_pan = QVector3D(0, 0, 0);

    LOG_INFO("[CloudView] setViewAngles: rotX=%.1f, rotY=%.1f, rotZ=%.1f\n", rotX, rotY, rotZ);

    emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ);
    update();
}

void PointCloudGLWidget::clearSelectedPoints()
{
    m_selectedPoints.clear();
    update();
}

void PointCloudGLWidget::clearSelectedLine()
{
    m_selectedLine = SelectedLineInfo();
    update();
}

bool PointCloudGLWidget::selectLineByIndex(int lineIndex)
{
    if (m_pointClouds.empty()) {
        return false;
    }

    const auto& cloudData = m_pointClouds[0];
    if (!cloudData.hasLineInfo) {
        return false;
    }

    // 检查线索引是否有效
    if (lineIndex < 0 || lineIndex >= cloudData.totalLines) {
        return false;
    }

    // 计算该线上的点数
    int pointCount = 0;
    for (int idx : cloudData.lineIndices) {
        if (idx == lineIndex) {
            pointCount++;
        }
    }

    if (pointCount == 0) {
        return false;
    }

    // 设置选中的线
    m_selectedLine.valid = true;
    m_selectedLine.cloudIndex = 0;
    m_selectedLine.lineIndex = lineIndex;
    m_selectedLine.pointIndex = -1;
    m_selectedLine.pointCount = pointCount;
    m_selectedLine.mode = LineSelectMode::Vertical;

    emit lineSelected(m_selectedLine);
    update();
    return true;
}

bool PointCloudGLWidget::selectHorizontalLineByIndex(int pointIndex)
{
    if (m_pointClouds.empty()) {
        return false;
    }

    const auto& cloudData = m_pointClouds[0];
    if (!cloudData.hasLineInfo || cloudData.totalLines <= 0 || cloudData.pointsPerLine <= 0) {
        return false;
    }

    // 检查点索引是否有效（使用原始点云的每线点数）
    if (pointIndex < 0 || pointIndex >= cloudData.pointsPerLine) {
        return false;
    }

    // 设置选中的横向线
    m_selectedLine.valid = true;
    m_selectedLine.cloudIndex = 0;
    m_selectedLine.lineIndex = -1;
    m_selectedLine.pointIndex = pointIndex;
    m_selectedLine.pointCount = cloudData.totalLines;  // 横向线的点数等于总线数
    m_selectedLine.mode = LineSelectMode::Horizontal;

    emit lineSelected(m_selectedLine);
    update();
    return true;
}

float PointCloudGLWidget::calculateDistance(const SelectedPointInfo& p1, const SelectedPointInfo& p2)
{
    if (!p1.valid || !p2.valid) {
        return 0.0f;
    }

    float dx = p2.x - p1.x;
    float dy = p2.y - p1.y;
    float dz = p2.z - p1.z;

    return std::sqrt(dx * dx + dy * dy + dz * dz);
}

QVector<QVector3D> PointCloudGLWidget::getSelectedLinePoints() const
{
    QVector<QVector3D> points;

    if (!m_selectedLine.valid || m_selectedLine.cloudIndex < 0) {
        return points;
    }

    if (m_selectedLine.cloudIndex >= static_cast<int>(m_pointClouds.size())) {
        return points;
    }

    const auto& cloudData = m_pointClouds[m_selectedLine.cloudIndex];
    if (!cloudData.hasLineInfo) {
        return points;
    }

    if (m_selectedLine.mode == LineSelectMode::Vertical) {
        // 纵向选线：获取同一条扫描线上的所有点
        for (size_t i = 0; i < cloudData.lineIndices.size(); ++i) {
            if (cloudData.lineIndices[i] == m_selectedLine.lineIndex) {
                size_t vertIdx = i * 3;
                if (vertIdx + 2 < cloudData.vertices.size()) {
                    points.append(QVector3D(
                        cloudData.vertices[vertIdx],
                        cloudData.vertices[vertIdx + 1],
                        cloudData.vertices[vertIdx + 2]));
                }
            }
        }
    } else {
        // 横向选线：获取所有线的相同索引点
        if (cloudData.pointsPerLine > 0 && m_selectedLine.pointIndex >= 0) {
            for (size_t i = 0; i < cloudData.originalIndices.size(); ++i) {
                int originalIdx = cloudData.originalIndices[i];
                if (originalIdx % cloudData.pointsPerLine == m_selectedLine.pointIndex) {
                    size_t vertIdx = i * 3;
                    if (vertIdx + 2 < cloudData.vertices.size()) {
                        points.append(QVector3D(
                            cloudData.vertices[vertIdx],
                            cloudData.vertices[vertIdx + 1],
                            cloudData.vertices[vertIdx + 2]));
                    }
                }
            }
        }
    }

    return points;
}

void PointCloudGLWidget::setListHighlightPoint(const QVector3D& point)
{
    m_hasListHighlightPoint = true;
    m_listHighlightPoint = point;
    update();
}

void PointCloudGLWidget::clearListHighlightPoint()
{
    m_hasListHighlightPoint = false;
    update();
}

void PointCloudGLWidget::transformAllClouds(const QMatrix4x4& matrix)
{
    for (auto& cloudData : m_pointClouds) {
        for (size_t i = 0; i + 2 < cloudData.vertices.size(); i += 3) {
            QVector3D pt(cloudData.vertices[i], cloudData.vertices[i + 1], cloudData.vertices[i + 2]);
            QVector3D transformed = matrix.map(pt);
            cloudData.vertices[i] = transformed.x();
            cloudData.vertices[i + 1] = transformed.y();
            cloudData.vertices[i + 2] = transformed.z();
        }
    }

    // 重新上传 VBO（顶点数据已变更）
    makeCurrent();
    for (auto& cloudData : m_pointClouds) {
        uploadToVBO(cloudData);
    }
    doneCurrent();

    computeBoundingBox();
    resetView();
    update();
}

void PointCloudGLWidget::computeBoundingBox()
{
    if (m_pointClouds.empty()) {
        m_minBound = QVector3D(-50, -50, -50);
        m_maxBound = QVector3D(50, 50, 50);
        m_center = QVector3D(0, 0, 0);
        return;
    }

    float minX = FLT_MAX, minY = FLT_MAX, minZ = FLT_MAX;
    float maxX = -FLT_MAX, maxY = -FLT_MAX, maxZ = -FLT_MAX;

    for (const auto& cloudData : m_pointClouds) {
        for (size_t i = 0; i < cloudData.vertices.size(); i += 3) {
            float x = cloudData.vertices[i];
            float y = cloudData.vertices[i + 1];
            float z = cloudData.vertices[i + 2];

            minX = qMin(minX, x);
            minY = qMin(minY, y);
            minZ = qMin(minZ, z);
            maxX = qMax(maxX, x);
            maxY = qMax(maxY, y);
            maxZ = qMax(maxZ, z);
        }
    }

    m_minBound = QVector3D(minX, minY, minZ);
    m_maxBound = QVector3D(maxX, maxY, maxZ);
    m_center = (m_minBound + m_maxBound) / 2.0f;
}

void PointCloudGLWidget::setCurrentColor(PointCloudColor color)
{
    switch (color) {
    case PointCloudColor::White:   glColor3f(1.0f, 1.0f, 1.0f); break;
    case PointCloudColor::Red:     glColor3f(1.0f, 0.0f, 0.0f); break;
    case PointCloudColor::Green:   glColor3f(0.0f, 1.0f, 0.0f); break;
    case PointCloudColor::Blue:    glColor3f(0.0f, 0.0f, 1.0f); break;
    case PointCloudColor::Yellow:  glColor3f(1.0f, 1.0f, 0.0f); break;
    case PointCloudColor::Cyan:    glColor3f(0.0f, 1.0f, 1.0f); break;
    case PointCloudColor::Magenta: glColor3f(1.0f, 0.0f, 1.0f); break;
    default: glColor3f(1.0f, 1.0f, 1.0f); break;
    }
}

void PointCloudGLWidget::setColorByIndex(int colorIndex)
{
    // 颜色表：浅灰、浅绿、浅蓝、黄、青、品红、白
    static const float colorTable[7][3] = {
        {0.75f, 0.75f, 0.75f},  // 浅灰色
        {0.3f, 1.0f, 0.3f},  // 浅绿
        {0.3f, 0.3f, 1.0f},  // 浅蓝
        {1.0f, 1.0f, 0.3f},  // 黄
        {0.3f, 1.0f, 1.0f},  // 青
        {1.0f, 0.3f, 1.0f},  // 品红
        {1.0f, 1.0f, 1.0f},  // 白
    };

    int idx = colorIndex % COLOR_COUNT;
    glColor3f(colorTable[idx][0], colorTable[idx][1], colorTable[idx][2]);
}

SelectedPointInfo PointCloudGLWidget::pickPoint(int screenX, int screenY)
{
    SelectedPointInfo result;

    if (m_pointClouds.empty()) {
        return result;
    }

    // 获取视口和矩阵
    GLint viewport[4];
    GLdouble modelMatrix[16], projMatrix[16];

    glGetIntegerv(GL_VIEWPORT, viewport);
    glGetDoublev(GL_MODELVIEW_MATRIX, modelMatrix);
    glGetDoublev(GL_PROJECTION_MATRIX, projMatrix);

    // 转换屏幕 Y 坐标（OpenGL Y 轴向上）
    int glScreenY = viewport[3] - screenY;

    float minScreenDist = FLT_MAX;
    size_t bestIndex = 0;
    int bestCloudIndex = -1;
    int bestLineIndex = -1;
    float bestX = 0, bestY = 0, bestZ = 0;

    // 遍历所有点，计算屏幕投影距离
    for (size_t cloudIdx = 0; cloudIdx < m_pointClouds.size(); ++cloudIdx) {
        const auto& cloudData = m_pointClouds[cloudIdx];
        for (size_t i = 0; i < cloudData.vertices.size(); i += 3) {
            float wx = cloudData.vertices[i];
            float wy = cloudData.vertices[i + 1];
            float wz = cloudData.vertices[i + 2];

            // 将世界坐标投影到屏幕
            GLdouble sx, sy, sz;
            gluProject(wx, wy, wz, modelMatrix, projMatrix, viewport, &sx, &sy, &sz);

            // 计算屏幕距离
            float dx = static_cast<float>(sx) - screenX;
            float dy = static_cast<float>(sy) - glScreenY;
            float screenDist = dx * dx + dy * dy;

            if (screenDist < minScreenDist) {
                minScreenDist = screenDist;
                bestIndex = i / 3;
                bestCloudIndex = static_cast<int>(cloudIdx);
                bestX = wx;
                bestY = wy;
                bestZ = wz;

                // 获取线索引
                if (cloudData.hasLineInfo && bestIndex < cloudData.lineIndices.size()) {
                    bestLineIndex = cloudData.lineIndices[bestIndex];
                } else {
                    bestLineIndex = -1;
                }
            }
        }
    }

    // 屏幕距离阈值（像素），20像素内认为选中
    float threshold = 20.0f * 20.0f;
    if (minScreenDist < threshold) {
        result.valid = true;
        result.index = bestIndex;
        result.cloudIndex = bestCloudIndex;
        result.lineIndex = bestLineIndex;
        result.x = bestX;
        result.y = bestY;
        result.z = bestZ;

        // 计算点在线中的原始索引
        if (bestLineIndex >= 0 && bestCloudIndex >= 0) {
            const auto& cloudData = m_pointClouds[bestCloudIndex];
            // 使用原始索引计算：原始索引 % 每线点数
            if (bestIndex < cloudData.originalIndices.size() && cloudData.pointsPerLine > 0) {
                int originalIdx = cloudData.originalIndices[bestIndex];
                result.pointIndexInLine = originalIdx % cloudData.pointsPerLine;
            }
        }

        LOG_INFO("[CloudView] Point selected: (%.3f, %.3f, %.3f) lineIndex=%d pointIndexInLine=%d screenDist=%.1f\n",
                 bestX, bestY, bestZ, bestLineIndex, result.pointIndexInLine, std::sqrt(minScreenDist));
    } else {
        LOG_INFO("[CloudView] No point selected, minScreenDist=%.1f\n", std::sqrt(minScreenDist));
    }

    return result;
}

void PointCloudGLWidget::drawSelectedPoints()
{
    glPointSize(10.0f);
    glDisable(GL_DEPTH_TEST);

    glBegin(GL_POINTS);

    // 绘制选中的点（橙色）
    for (const auto& pt : m_selectedPoints) {
        if (pt.valid) {
            glColor3f(1.0f, 0.5f, 0.0f);  // 橙色
            glVertex3f(pt.x, pt.y, pt.z);
        }
    }

    // 绘制列表高亮点（蓝色，与选点区分）
    if (m_hasListHighlightPoint) {
        glColor3f(0.0f, 0.5f, 1.0f);  // 蓝色
        glVertex3f(m_listHighlightPoint.x(), m_listHighlightPoint.y(), m_listHighlightPoint.z());
    }

    glEnd();

    glEnable(GL_DEPTH_TEST);
    glPointSize(m_pointSize);
}

void PointCloudGLWidget::drawMeasurementLine()
{
    if (m_selectedPoints.size() < 2) {
        return;
    }

    const auto& p1 = m_selectedPoints[0];
    const auto& p2 = m_selectedPoints[1];

    if (!p1.valid || !p2.valid) {
        return;
    }

    glDisable(GL_DEPTH_TEST);
    glLineWidth(2.0f);

    glBegin(GL_LINES);
    glColor3f(0.0f, 1.0f, 0.0f);
    glVertex3f(p1.x, p1.y, p1.z);
    glVertex3f(p2.x, p2.y, p2.z);
    glEnd();

    glLineWidth(1.0f);
    glEnable(GL_DEPTH_TEST);
}

void PointCloudGLWidget::drawSelectedLine()
{
    if (!m_selectedLine.valid || m_selectedLine.cloudIndex < 0) {
        return;
    }

    if (m_selectedLine.cloudIndex >= static_cast<int>(m_pointClouds.size())) {
        return;
    }

    const auto& cloudData = m_pointClouds[m_selectedLine.cloudIndex];
    if (!cloudData.hasLineInfo) {
        return;
    }

    // 高亮显示选中线上的所有点
    glPointSize(3.0f);
    glDisable(GL_DEPTH_TEST);

    glBegin(GL_POINTS);
    glColor3f(1.0f, 1.0f, 0.0f);  // 黄色高亮

    if (m_selectedLine.mode == LineSelectMode::Vertical) {
        // 纵向选线：显示同一条扫描线上的所有点
        for (size_t i = 0; i < cloudData.lineIndices.size(); ++i) {
            if (cloudData.lineIndices[i] == m_selectedLine.lineIndex) {
                size_t vertIdx = i * 3;
                if (vertIdx + 2 < cloudData.vertices.size()) {
                    glVertex3f(cloudData.vertices[vertIdx],
                              cloudData.vertices[vertIdx + 1],
                              cloudData.vertices[vertIdx + 2]);
                }
            }
        }
    } else {
        // 横向选线：显示所有线的相同原始索引点
        if (cloudData.pointsPerLine > 0 && m_selectedLine.pointIndex >= 0) {
            for (size_t i = 0; i < cloudData.originalIndices.size(); ++i) {
                int originalIdx = cloudData.originalIndices[i];
                // 原始索引 % 每线点数 == 选中的点索引
                if (originalIdx % cloudData.pointsPerLine == m_selectedLine.pointIndex) {
                    size_t vertIdx = i * 3;
                    if (vertIdx + 2 < cloudData.vertices.size()) {
                        glVertex3f(cloudData.vertices[vertIdx],
                                  cloudData.vertices[vertIdx + 1],
                                  cloudData.vertices[vertIdx + 2]);
                    }
                }
            }
        }
    }
    glEnd();

    glEnable(GL_DEPTH_TEST);
    glPointSize(m_pointSize);
}

void PointCloudGLWidget::drawAxis()
{
    // 在右下角绘制坐标系指示器（右手坐标系）
    // X轴：红色，指向右
    // Y轴：绿色，指向上
    // Z轴：蓝色，指向观察者（屏幕外）
    GLint viewport[4];
    glGetIntegerv(GL_VIEWPORT, viewport);

    int axisSize = 60;  // 坐标系区域大小（像素）
    int margin = 10;    // 距离边缘的边距
    int axisX = viewport[2] - axisSize - margin;  // 右下角 X
    int axisY = margin;                            // 右下角 Y（OpenGL Y 轴向上）

    // 保存当前矩阵状态
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    // 设置正交投影，覆盖整个视口
    glOrtho(0, viewport[2], 0, viewport[3], -100, 100);

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();

    // 移动到右下角中心位置
    glTranslatef(axisX + axisSize / 2.0f, axisY + axisSize / 2.0f, 0.0f);

    // 应用当前视图的旋转（与主视图同步）
    glRotatef(m_rotationY, 0, 1, 0);
    glRotatef(m_rotationX, 1, 0, 0);
    glRotatef(m_rotationZ, 0, 0, 1);

    // 关闭深度测试，确保坐标系始终可见
    glDisable(GL_DEPTH_TEST);

    float axisLength = axisSize * 0.4f;  // 坐标轴长度
    glLineWidth(2.0f);
    glBegin(GL_LINES);

    // X 轴 - 红色（右手坐标系：向右）
    glColor3f(1.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(axisLength, 0.0f, 0.0f);

    // Y 轴 - 绿色（右手坐标系：向上）
    glColor3f(0.0f, 1.0f, 0.0f);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, axisLength, 0.0f);

    // Z 轴 - 蓝色（右手坐标系：向观察者）
    glColor3f(0.0f, 0.0f, 1.0f);
    glVertex3f(0.0f, 0.0f, 0.0f);
    glVertex3f(0.0f, 0.0f, axisLength);

    glEnd();
    glLineWidth(1.0f);

    // 恢复深度测试
    glEnable(GL_DEPTH_TEST);

    // 恢复矩阵状态
    glPopMatrix();
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
}

void PointCloudGLWidget::mousePressEvent(QMouseEvent* event)
{
    m_lastMousePos = event->pos();

    if (event->button() == Qt::LeftButton) {
        // Ctrl+左键：选点
        if (event->modifiers() & Qt::ControlModifier) {
            makeCurrent();
            SelectedPointInfo point = pickPoint(event->pos().x(), event->pos().y());
            doneCurrent();

            if (point.valid) {
                if (m_measureDistanceEnabled) {
                    // 启用测距：最多保留两个点
                    if (m_selectedPoints.size() >= MAX_SELECTED_POINTS) {
                        m_selectedPoints.clear();
                    }
                    m_selectedPoints.append(point);

                    emit pointSelected(point);

                    if (m_selectedPoints.size() == 2) {
                        float distance = calculateDistance(m_selectedPoints[0], m_selectedPoints[1]);
                        emit twoPointsSelected(m_selectedPoints[0], m_selectedPoints[1], distance);
                    }
                } else {
                    // 未启用测距：只保留一个点
                    m_selectedPoints.clear();
                    m_selectedPoints.append(point);
                    emit pointSelected(point);
                }

                update();
            }
        } else if (event->modifiers() & Qt::ShiftModifier) {
            // Shift+左键：选线
            makeCurrent();
            SelectedPointInfo point = pickPoint(event->pos().x(), event->pos().y());
            doneCurrent();

            if (point.valid && point.lineIndex >= 0) {
                if (m_lineSelectMode == LineSelectMode::Vertical) {
                    // 纵向选线：选中该点所在的线
                    m_selectedLine.valid = true;
                    m_selectedLine.cloudIndex = point.cloudIndex;
                    m_selectedLine.lineIndex = point.lineIndex;
                    m_selectedLine.pointIndex = -1;
                    m_selectedLine.mode = LineSelectMode::Vertical;

                    // 计算该线上的点数
                    int pointCount = 0;
                    if (point.cloudIndex >= 0 && point.cloudIndex < static_cast<int>(m_pointClouds.size())) {
                        const auto& cloudData = m_pointClouds[point.cloudIndex];
                        for (int idx : cloudData.lineIndices) {
                            if (idx == point.lineIndex) {
                                pointCount++;
                            }
                        }
                    }
                    m_selectedLine.pointCount = pointCount;
                } else {
                    // 横向选线：选中所有线的相同索引点
                    m_selectedLine.valid = true;
                    m_selectedLine.cloudIndex = point.cloudIndex;
                    m_selectedLine.lineIndex = -1;
                    m_selectedLine.pointIndex = point.pointIndexInLine;
                    m_selectedLine.mode = LineSelectMode::Horizontal;

                    // 横向线的点数等于总线数
                    if (point.cloudIndex >= 0 && point.cloudIndex < static_cast<int>(m_pointClouds.size())) {
                        m_selectedLine.pointCount = m_pointClouds[point.cloudIndex].totalLines;
                    }
                }

                emit lineSelected(m_selectedLine);
                update();
            }
        } else {
            m_leftButtonPressed = true;
        }
    } else if (event->button() == Qt::RightButton) {
        m_rightButtonPressed = true;
    } else if (event->button() == Qt::MiddleButton) {
        m_middleButtonPressed = true;
    }
}

void PointCloudGLWidget::mouseMoveEvent(QMouseEvent* event)
{
    QPoint delta = event->pos() - m_lastMousePos;
    m_lastMousePos = event->pos();

    if (m_leftButtonPressed) {
        // Alt+左键拖动：旋转rotZ（滚转）
        if (event->modifiers() & Qt::AltModifier) {
            m_rotationZ += delta.x() * 0.5f;
        } else {
            // 普通左键拖动：旋转rotX和rotY
            m_rotationY += delta.x() * 0.5f;
            m_rotationX += delta.y() * 0.5f;
        }
        // 移除角度限制，允许无限旋转
        emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ);
        update();
    } else if (m_middleButtonPressed) {
        float factor = m_distance * 0.002f;
        m_pan.setX(m_pan.x() - delta.x() * factor);
        m_pan.setY(m_pan.y() + delta.y() * factor);
        update();
    } else if (m_rightButtonPressed) {
        m_rotationZ += delta.x() * 0.5f;
        emit viewAnglesChanged(m_rotationX, m_rotationY, m_rotationZ);
        update();
    }
}

void PointCloudGLWidget::mouseReleaseEvent(QMouseEvent* event)
{
    if (event->button() == Qt::LeftButton) {
        m_leftButtonPressed = false;
    } else if (event->button() == Qt::RightButton) {
        m_rightButtonPressed = false;
    } else if (event->button() == Qt::MiddleButton) {
        m_middleButtonPressed = false;
    }
}

void PointCloudGLWidget::wheelEvent(QWheelEvent* event)
{
    float delta = event->angleDelta().y() / 120.0f;
    m_distance *= (1.0f - delta * 0.1f);
    m_distance = qBound(0.1f, m_distance, 10000.0f);
    update();
}

void PointCloudGLWidget::keyPressEvent(QKeyEvent* event)
{
    if (event->key() == Qt::Key_Space) {
        resetView();
    } else {
        QOpenGLWidget::keyPressEvent(event);
    }
}

bool PointCloudGLWidget::getFirstCloudData(PointCloudXYZ& cloud) const
{
    if (m_pointClouds.empty()) {
        return false;
    }

    const auto& cloudData = m_pointClouds[0];
    cloud.clear();
    cloud.reserve(cloudData.vertices.size() / 3);

    for (size_t i = 0; i < cloudData.vertices.size(); i += 3) {
        Point3D pt;
        pt.x = cloudData.vertices[i];
        pt.y = cloudData.vertices[i + 1];
        pt.z = cloudData.vertices[i + 2];

        int lineIdx = 0;
        if (cloudData.hasLineInfo && (i / 3) < cloudData.lineIndices.size()) {
            lineIdx = cloudData.lineIndices[i / 3];
        }
        cloud.push_back(pt, lineIdx);
    }

    return true;
}

void PointCloudGLWidget::replaceFirstCloud(const PointCloudXYZ& cloud, const QString& name)
{
    if (m_pointClouds.empty()) {
        addPointCloud(cloud, name);
        return;
    }

    // 保留第一个点云的颜色索引
    int colorIndex = m_pointClouds[0].colorIndex;

    // 重新构建数据
    PointCloudData data;
    data.name = name;
    data.hasColor = false;
    data.hasLineInfo = !cloud.lineIndices.empty();
    data.colorIndex = colorIndex;
    data.vertices.reserve(cloud.size() * 3);
    data.totalLines = 0;
    data.pointsPerLine = 0;

    const float EPSILON = 1e-6f;
    for (size_t i = 0; i < cloud.points.size(); ++i) {
        const auto& pt = cloud.points[i];
        if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) {
            continue;
        }
        // 显示时过滤 (0,0,0) 点
        if (std::fabs(pt.x) < EPSILON && std::fabs(pt.y) < EPSILON && std::fabs(pt.z) < EPSILON) {
            continue;
        }
        data.vertices.push_back(pt.x);
        data.vertices.push_back(pt.y);
        data.vertices.push_back(pt.z);

        // 保存原始索引
        data.originalIndices.push_back(static_cast<int>(i));

        if (data.hasLineInfo && i < cloud.lineIndices.size()) {
            int lineIdx = cloud.lineIndices[i];
            data.lineIndices.push_back(lineIdx);
            if (lineIdx + 1 > data.totalLines) {
                data.totalLines = lineIdx + 1;
            }
        }
    }

    // 计算每线点数
    if (data.totalLines > 0) {
        data.pointsPerLine = static_cast<int>(cloud.size()) / data.totalLines;
    }

    // 释放旧的 VBO
    makeCurrent();
    releaseVBO(m_pointClouds[0]);

    m_pointClouds[0] = std::move(data);

    // 上传新的 VBO
    uploadToVBO(m_pointClouds[0]);
    doneCurrent();

    computeBoundingBox();
    resetView();
    update();
}

void PointCloudGLWidget::addLineSegments(const QVector<LineSegment>& segments)
{
    m_lineSegments.append(segments);
    update();
}

void PointCloudGLWidget::clearLineSegments()
{
    m_lineSegments.clear();
    update();
}

void PointCloudGLWidget::addPosePoints(const QVector<PosePoint>& poses)
{
    m_posePoints.append(poses);
    update();
}

void PointCloudGLWidget::clearPosePoints()
{
    m_posePoints.clear();
    update();
}

void PointCloudGLWidget::drawLineSegments()
{
    if (m_lineSegments.isEmpty()) {
        return;
    }

    glLineWidth(2.0f);
    glBegin(GL_LINES);

    for (const auto& seg : m_lineSegments) {
        glColor3f(seg.r, seg.g, seg.b);
        glVertex3f(seg.x1, seg.y1, seg.z1);
        glVertex3f(seg.x2, seg.y2, seg.z2);
    }

    glEnd();
    glLineWidth(1.0f);
}

void PointCloudGLWidget::drawPosePoints()
{
    if (m_posePoints.isEmpty()) {
        return;
    }

    glLineWidth(2.0f);

    for (const auto& pose : m_posePoints) {
        glPushMatrix();

        // 移动到姿态点位置
        glTranslatef(pose.x, pose.y, pose.z);

        // 根据选择的欧拉角旋转顺序应用旋转
        // OpenGL的glRotatef是右乘，所以需要反向写
        switch (m_eulerRotationOrder) {
        case EulerRotationOrder::ZYX:  // Yaw-Pitch-Roll（最常用）
            glRotatef(pose.rx, 1, 0, 0);  // X轴旋转（最后应用）
            glRotatef(pose.ry, 0, 1, 0);  // Y轴旋转（第二个应用）
            glRotatef(pose.rz, 0, 0, 1);  // Z轴旋转（第一个应用）
            break;
        case EulerRotationOrder::XYZ:  // Roll-Pitch-Yaw
            glRotatef(pose.rz, 0, 0, 1);
            glRotatef(pose.ry, 0, 1, 0);
            glRotatef(pose.rx, 1, 0, 0);
            break;
        case EulerRotationOrder::ZXY:  // Yaw-Roll-Pitch
            glRotatef(pose.ry, 0, 1, 0);
            glRotatef(pose.rx, 1, 0, 0);
            glRotatef(pose.rz, 0, 0, 1);
            break;
        case EulerRotationOrder::YXZ:  // Pitch-Roll-Yaw
            glRotatef(pose.rz, 0, 0, 1);
            glRotatef(pose.rx, 1, 0, 0);
            glRotatef(pose.ry, 0, 1, 0);
            break;
        case EulerRotationOrder::XZY:  // Roll-Yaw-Pitch
            glRotatef(pose.ry, 0, 1, 0);
            glRotatef(pose.rz, 0, 0, 1);
            glRotatef(pose.rx, 1, 0, 0);
            break;
        case EulerRotationOrder::YZX:  // Pitch-Yaw-Roll
            glRotatef(pose.rx, 1, 0, 0);
            glRotatef(pose.rz, 0, 0, 1);
            glRotatef(pose.ry, 0, 1, 0);
            break;
        }

        // 绘制坐标系（右手坐标系：X红右，Y绿上，Z蓝前）
        glBegin(GL_LINES);

        // X轴 - 红色
        glColor3f(1.0f, 0.0f, 0.0f);
        glVertex3f(0.0f, 0.0f, 0.0f);
        glVertex3f(pose.scale, 0.0f, 0.0f);

        // Y轴 - 绿色
        glColor3f(0.0f, 1.0f, 0.0f);
        glVertex3f(0.0f, 0.0f, 0.0f);
        glVertex3f(0.0f, pose.scale, 0.0f);

        // Z轴 - 蓝色
        glColor3f(0.0f, 0.0f, 1.0f);
        glVertex3f(0.0f, 0.0f, 0.0f);
        glVertex3f(0.0f, 0.0f, pose.scale);

        glEnd();

        glPopMatrix();
    }

    glLineWidth(1.0f);
}